基于TMS320C6678的单历元单基线“北斗”测姿算法的实现*

时间：2024-07-28

(重庆邮电大学重庆市移动通信技术重点实验室，重庆 400065)

李玲霞，黎刚平**

(重庆邮电大学重庆市移动通信技术重点实验室，重庆 400065)

针对姿态测量的高精度问题和实时性，以及基于载波相位的“北斗”测姿算法中最小二乘模糊度去相关平差(LAMBDA)算法的高复杂运算，选择TI公司高端处理器中的C6678多核数字信号处理器 (DSP)作为单历元单基线“北斗”测姿算法实现平台。根据算法的特点采用数据流模式对DSP多核进行任务分配：DSP的CORE0专门负责与“北斗”接收机UDP(User Datagram Protocol)通信，将“北斗”测姿算法中的各个分部算法分配到另外3个DSP CORE中执行并利用基于消息队列方式的核间通信技术进行数据核间通信。利用OEM617D“北斗”接收机采集的实时“北斗”数据，经过在多核DSP算法处理上运行后，将结果与Matlab仿真结果对比，表明本设计可满足“北斗”测姿的精确性和实时性要求。

“北斗”测姿；单历元单基线；多核DSP；用户数据报协议网口传输；Cache技术

1 引言

在基于“北斗”卫星的定位与姿态测量领域，载波相位是高精度定位与姿态测量的主要方式，相对于伪距的观测精度，载波相位的测量精度比其高出上百倍[1-2]。但要利用载波相位进行姿态测量必须保证能正确的解算出整周模糊度。目前，国内外研究人员一直对整周模糊度进行了大量的研究，陆续提出了求解模糊度的算法，如快速模糊度逼近法[3](Fast Ambiguity Resolution Algorithm,FARA)、模糊度估算的优化算法[4](Optimal method for estimating GPS ambiguities,OMEGA)和OTF(On The Fly,OTF)技术[5]等。但这些方法只能在长基线的情况下效果比较明显。而在短基线情况下，由于双天线收到的“北斗”数据存在高度相关性，这种相关性在求解模糊度带来很大困难。而最小二乘模糊度去相关平差(Least-squares Ambiguity Decorrelation Adjustment，LAMBDA)算法首先对浮点解的协方差矩阵进行去相关运算，使得模糊度搜索空间由原来狭长的空间变为球形空间，这将提高整周模糊度的成功率。

随着数字设备的发展，对以DSP为核心的实现平台研究尤为重要。在多数系统的实现中，DSP通常是承载着复杂的核心算法任务的。随着大型算法的运算越来越复杂，传统单核DSP已难以满足算法要求。而如果同时利用多片单核DSP集成系统，将会带来单核DSP间的接口连接过多、硬件设计复杂、系统面积过大等不利因素。TI公司的片上多核DSP TMS320C6678不但具备先进的Keystone架构，而且主频可达1.4 GHz,同时兼顾浮点运算与定点运算[6]。

基于此，本文利用LAMBDA算法求解整周模糊度，同时使用TMS320C6678多核DSP实现”北斗”测姿算法。测试结果表明，采用TMS320C6678多核DSP能实时、精确地解算姿态角。

2 单历元单基线“北斗”测姿算法

单历元单基线“北斗”测姿算法包括“北斗”数据解析算法、伪距定位算法、LAMBDA算法、姿态角解算，其处理流程如图1所示。

图1 单历元单基线“北斗”测姿算法处理流程图
Fig.1 Flow chart of single-baseline Beidou surveying and attitude processing

输入数据是由“北斗”卫星信号经OEM617D”北斗”接收机主副天线接收(由主副天线构成超短基线，长度为1 m)，经射频处理，中频变频和接收机内部程序进行封装后的字符串数据。输入数据包括以#BDSEPHEMERISA字符为数据头的“北斗”星历数据和主副天线的“北斗”数据。配置“北斗”接收机向DSP端发送“北斗”数据的频率为1 Hz。

2.1“北斗”数据解析

对从“北斗”接收机传来的“北斗”星历字符数据和主副天线的“北斗”字符数据进行提取，将部分需要转为64 bit double型等其他类型的数据进行转化。

2.2伪距定位算法

设置一个接收机当前位置初始估计值为0或者上次定位的结果，根据伪距双差观测模型建立伪距双差观察方程，将伪距双差方程进行线性化后，利用最小二乘法求解线性化后的伪距观察方程，将求解的线性化后伪距观察方程的解代入初始接收机位置估计值，更新接收机当前的位置；再经过多次迭代后误差小于预先设定的值(本算法中设定norm(delta_x(1:3,1)) <1×10-7)后跳出迭代循环中，得到伪距定位的结果。经多次测试和统计，目前利用本算法解算的伪距定位结果误差在3～5 m内[7]。

2.3LAMBDA算法

本接收设备由两根天线构成一个基线，可看作两个“北斗”接收机，以同个卫星的载波相位观测量为参照，对两个接收机的载波相位观测方程作差，得到单差载波相位观测方程，再在单差的基础上进行两个卫星间的单差载波相位方程作差，就得到了双差载波相位观测方程。将基于双差载波相位[8]的姿态测量方程统一写成如下的一个线性矩阵形式[9]：

y=AΔbur+BN。

(1)

其中：y为接收机给出的双差载波相位观测量，Δbur是基线向量，N为需要被求解的整周模糊度，A和B为常系数矩阵。在姿态测量之所以使用双差载波相位模型作为载波相位观测模型，主要原因是经过双差处理后，能消除卫星钟差、接收机钟差、星历误差等误差。

先不考虑整周模糊度N的整数要求而直接利用加权最小二乘法求解浮点型加权最小二乘解(Δbur，N)，即最优最小二乘解使得残余的加权平方和最小，即

(2)

根据公式(2)求出(Δbur，N)，而(Δbur，N)的协方差矩阵

(3)

LAMBDA算法分为3步[10]：去相关、置换、搜索。

(1)先将浮点解及其协方差矩阵进行Z变换，再对协方差矩阵进行LDL分解，通过一系列的整数高斯变换得到满足一定条件的Z矩阵。

(4)

(5)

2.4姿态角解算

在导航领域，姿态一般是表示载体在某一时刻相对于地面的角位置。大多数姿态都是表示载体坐标系与站心坐标系(东-北-天坐标系)的角位置关系，用航向角、俯仰角、横滚角来表示。假设载体坐标系与站心坐标系原点重合，按照一定的旋转顺序，将站心坐标系旋转3次，使得其与载体坐标系重合。每次旋转所参考的旋转轴均为被旋转坐标系的坐标轴，每次旋转的角度被称欧拉角。设坐标系为a系，站心坐标系为b系。a系在3次旋转过程中，可以得到由欧拉角(俯仰角θ、航向角φ、横滚角γ)表示的3个方向余弦矩阵。

绕x轴旋转：

绕y轴旋转：

绕Z轴旋转：

假设b系的旋转顺序为Z-Y-X，根据每次旋转得到的方向余弦矩阵，可以得到姿态旋转矩阵

(6)

则载体坐标系与ENU坐标系坐标矢量关系如下：

(7)

(8)

由公式(8)可以得到航向角和俯仰角的表达式：

(9)

其中：φ为航向角，θ为俯仰角，[xb,yb,zb]为基线向量在站心坐标系下的坐标。

3 多核DSP实现技术

为了实现快速姿态测量，充分利用了C6678 DSP的多核。将整个“北斗”测姿算法分割为“北斗”数据解析、伪距定位算法、LAMBDA算法、姿态解算，将每个算法根据一定的任务分配模式分配到一个DSP核上。以下是实现过程中用到的关键技术。

3.1多核并行编程技术

多核DSP的并行编程方式主要有两种，主从模式和数据流模式[11]。主从模式是一种集中控制和分布执行的一个并行编程模式。数据流模式意味着分布式控制和分布执行，该模式适合以流水的方式进行分布式控制和数据处理的应用，而这种应用通常包含几个复杂的算法，单核DSP将无法满足算法性能要求，并且各个算法之间有着频繁的数据交换。

“北斗”测姿算法中含有较为复杂的矩阵运算，同时也算法间有着很强的数据依赖性，故“北斗”测姿算法的多核并行编程模式选用的是数据流模式。

3.2基于消息队列方式的核间通信技术

德州仪器公司(TI)提供了多核开发套件(Multicore Software Development Kit,MCSDK)，包括下列两种核间通信方案：通知(Notify)方式和消息队列(MessageQueue,MessageQ)方式[12]。

基于Notify方式的核间通信是利用核间的通信机制来实施的，而一个Notify携带的信息是非常小的，通常只有32 bit。Notify模块更加侧重于通知，而MessageQ却可以传递变长的消息块，更侧重于传递消息。而实现“北斗”测姿算法的DSP核需要非常频繁地进行不定长的数据传输，故使用MessageQ信息进行核间传递是最佳的方式。

3.3Cache一致性维护

对于C6678多核DSP来说，为多核配置一定的Cache的优势在于多核在处理数据的时候能直接从Cache中快速地读取而不是去直接寻址到实际存储数据的物理单元，因此多核系统中每个核都配置一个属于自己的Cache[13]。多核的Cache模型如图2所示。

在多核情况下，当一个核改变一个存储在共享内存的数据，而这个数据的改变需要被其他核去识别，这时就需要Cache一致性维护。在改变这个数据的时候实际上并没有改变存储在共享存储中的数据，改变的是储存在属于该核的Cache数据。这时其他的核无论是从自己的Cache中还是从共享存储中读取该数据，都不能准确地感受到该数据的变化。在SYS/BIOS的硬件抽象层(hal层)调用Cache模块能完美地解决该问题，即在CORE0改变这个数据之后调用Cache_wbAll()函数或者Cache_wb()函数将该数据回写到真正存着该数据的物理单元中，而在其他核试图读取该数据之前调用Cacha_disable()函数使该核的Cache失效，这样使得读取数据的时候直接从其真正的物理地址读取。为了提高DSP更高效率处理数据，我们一般使用Cache_wb()函数进行部分数据的回写而不是全部数据，使用Cache_disable()函数时使用的不是Cache_Type_All这个参数而是相应的数据类型。

3.4DSP端用户数据报协议网口传输技术

“北斗”接收机以1 Hz的频率向DSP端发送数据且考虑到在将来“北斗”接收机与数字信号处理板卡会集成到一起，故设计多核DSP使用基于网络开发套件(Network Developer’s Kit,NDK)的阻塞式UDP方式与“北斗”接收机进行通信。在SYS/BIOS实时内核的条件下，德州仪器公司提供了NDK，内部已经集成了UDP协议栈[14]。在CCS5.5中，将NDK子系统加入SYS/BIOS工程中，配置好头文件和路径后，即可调用NDK子系统的组件来实现UDP传输。开发流程如图3所示。

图3 基于NDK子系统的阻塞式UDP开发流程图Fig.3 Obstructive UDP development flow chart based on NDK subsystem

在配置目标IP和本地IP时候，需要注意一定要保证目标IP和本地IP在同一网段上，这样才能进行通信。设备间可以直接用网线连接也可以通过交换机进行连接。由于“北斗”接收机以1 Hz频率发送数据，速率相对于千兆网口传输速率比较低，故使用网口传输完全满足数据输入的实时性。此外，在“北斗”接收机与DSP短距离直连的情况下，多核DSP单独使用一个CORE0来运行UDP协议，这使得UDP的丢包几乎可以忽略不计。

配置目标IP、本地IP、网关IP等重要参数代码如下：

char *HostName = “dsp”;

char *LocalIPAddr = “172.24.52.77”;

char *LocalIPMask = “255.255.255.0”;

char *GatewayIP = “172.24.52.1”;

char *DomainName = “receiver”;

char *DNSServer = “8.8.4.4”;

char *DestIPAddr = “172.24.52.209”;

char *DestIPMask = “255.255.255.0”;

NA.IPAddr = inet_addr(LocalIPAddr);

NA.IPMask = inet_addr(LocalIPMask);

strcpy( NA.Domain,DomainName );

端口号设置为60000。图4所示为 PC端的网口调试助手实时接收的数据图。

基于NDK阻塞式的UDP网口传输在速率上完全满足1 Hz。由于”北斗”接收机跟DSP板直连且距离很近，在这种情况下，我们更多考虑的是在可以忽略UDP丢包的情况下，UDP的快速发送接收性能为后续的算法处理提供了高速数据输入，同时也在实时性方面得到了更好的发挥。

4 “北斗”测姿算法的多核DSP实现

4.1基于SYS/BIOS实时内核的开发

SYS/BIOS是由TI公司开发的实时操作系统TI-RTOS的内核组件[15]，也被称为“TI-RTOS Kernel”。在基于SYS/BIOS实时内核进行开发时，借助了TI提供的MCSDK。MCSDK软件框图如图5所示。

图5 MCSDK软件框图Fig.5 Block diagram of MCSDK

在利用C6678 DSP进行实时嵌入式软件开发时，往往需要使用到C6678 DSP的片内外设、网口、运算库函数、多核开发等，这时就可以在SYS/BIOS环境下调用MCSDK的LLDs、NDK、Algorithm Lib等组件。

4.2“北斗”测姿算法的具体实现

单历元单基线“北斗”姿态测量算法的实现硬件平台为TI公司的C6678评估板，设置其主频为1 GHz，软件集成开发环境为CCS5.5。算法实现总共使用了C6678的前4个核。CORE0负责实现网口接收“北斗”接收机的数据，CORE1负责数据分类、数据解析并将各类数据压入对应的Message中，CORE2负责运行单点定位算法，CORE3负责运行LAMBDA算法，解算姿态角。

“北斗”测姿算法实现软件框图如图6所示。

图6 “北斗”测姿算法实现软件框图Fig.6 Software block diagram of Beidou attitude measurement algorithm

在CORE0接收到接收机发来的数据后，将数据立刻转发CORE1，当转发完成后CORE0立刻又在等待下一数据的到来，而CORE1将数据作简单的校验，校验正确后对数据进行解析分类，如果是星历数据，则将星历数据解析后存入一个数据结构中；如果是RANGEA消息，先进行解析，当完成主副天线两条RANGEA消息的解析后，立刻将主副天线的RANGEA消息压入姿态队列，之后再将存有星历数据的数据结构封装为消息压入星历队列，这个过程之后CORE1立刻回到初始状态。而CORE2开始处于接收队列的等待状态，当有姿态消息通过队列传来，CORE1将其接收，之后再接收星历消息。最后将姿态消息与星历消息进行单点定位算法，求得浮点解和浮点解的协方差矩阵，之后通过信号量触发CORE3，将浮点解及其协方差矩阵传输给CORE3，CORE2恢复原来的状态。CORE3则利用接收到的浮点解及其协方差矩阵进行LAMBDA算法、模糊度搜索得到整周模糊度矢量，然后进行坐标转换即可求得姿态角。

5 多核DSP实现测试及结果分析

5.1多核DSP实现测试

测试场地位于重庆邮电大学逸夫科技楼顶楼，测试接收机为OEM617D“北斗”接收机。将双“北斗”天线固定在刚体基线两端，基线方向向东(航向角176°)。在静态条件下，经历连续400历元进行实时性测试后，随机截取其中4次姿态测量数据、姿态角数据与Matlab仿真数据对比，如表1所示。

表1 实现与仿真数据对比Tab.1 Comparison between implementation and simulation data

经过连续400历元姿态测量结果与Matlab算法仿真结果取差值，如图7所示。

图7 姿态结果与Matlab算法仿真结果取差值Fig.7 Comparison between DSP results and Matlab algorithm simulation results

在DSP核主频配置为1 GHz的情况下，对连续400历元各个部分算法速度进行测试，结果如表2所示。

表2 各个部分算法速度测试Tab.2 Speed test of each part algorithm

根据1 GHz的主频，时钟周期数为89 496 991，其对应的运行时间为0.089 496 991 s，小于数据接收的时间间隔1 s，说明本实现设计方案满足实时性要求。

5.2结果分析

在连续400历元分别在DSP平台和Matlab算法仿真平台分别进行姿态测量后，实现解算出的姿态数据与Matlab仿真解算出姿态角差值在<-4×10-7,5×10-7>。同时，对各个算法模块进行速度测试，其总的运行时间小于数据接收的时间间隔1 s，符合姿态测量的精度要求和实时性要求，也同时说明C6678多核DSP为单历元单基线“北斗”测姿算法提供了很稳定的算法实现平台。在姿态测量领域，利用多核DSP实现姿态测量算法能进行实时、精确的姿态解算。

6 结束语

本文以TMS320C6678多核DSP作为算法实现平台，以SYS/BIOS实时内核为软件系统平台，调用SYS/BIOS的IPC模块、Cache模块以及基于NDK的UDP模块。在算法实现上，将算法分割为多个任务，利用多核进行并行处理，实现了“北斗”姿态测量算法。在姿态测量领域，利用多核DSP进行姿态解算是一个新的思路。而在程序优化上，由于TI公司为多核DSP匹配了很多已经优化的运算模块如MATHLIB C66x等，故下一步研究的问题是如何调用多核DSP丰富的运算库来优化移植在DSP平台上的算法代码，从而实现更高效的姿态测量。

[1] 李晨阳. 载波相位在提高GNSS接收机精度中的应用研究[D]. 北京:北京邮电大学，2014.

LI Chenyang. Study on the application of carrier phase in improving GNSS receiver precision[D].Beijing:Beijing University of Posts and Telecommunications，2014.(in Chinese)

[2] LI Y,DUSHA D,KELLAR W,et al. Calibrated MEMS inertial sensors with GPS for a precise attitude heading reference system on autonomous farming tractors[C]// Proceedings of ION-GNSS2009.Savannah,Georgia:ION,2009:22-25.

[3] CHEN D S. Development of a fast ambiguity search filtering (FASF) method for GPS carrier phase ambiguity resolution[D]. Calgary,AB,Canada:Department of GeomaticsEngineering,University of Calgary,1994:31-41.

[4] 柴洪洲. 模糊度搜索的优化算法[J]. 全球定位系统,2002,27(1):10-12.

CHAI Hongzhou. Optimized algorithm of ambiguity search[J].GNSS World of China,2002,27(1):10-12. (in Chinese)

[5] TEUNISSEN P J G,VERHAGEN S. GNSS ambiguity resolution:when and how to fix or not to fix[C]//Proceedings of VI Hotine-Marussi Symposium on Theoretical and Computational Geodesy. Heidelberg，Berlin:IEEE Press,2008:143-148.

[6] Texas Instrument. TI TMS320C6678 multicore fixed and floating-point digital processor[M].Texas:TI,2014:13-15.

[7] 周巍.北斗卫星导航系统精密定位理论方法研究与实现[D]. 郑州:解放军信息工程大学,2013.

ZHOU Wei.Theoretical study and realization of precise positioning of Beidou satellite navigation system[D].Zhengzhou:Information Engineering University,2013.(in Chinese)

[8] 马煦,常青,侯俊. 综合利用电离层残差法和载波相位变化率法探测并修复周跳[J]. 电讯技术,2005,45(1):115-119.

MA Xu,CHANG Qing,HOU Jun. Detection and reparation for cycle slips using ionosphere residual error method and carrier phase rate method[J].Telecommunication Engineering,2005,45(1):115-119.(in Chinese)

[9] 张贤达,矩阵分析与应用[M].北京：清华大学出版社,2004:548-561.

[10] TEUNISSEN P. The LAMBDA method for the GNSS compass[J]. Artificial Satellites,2006,41(3):89-103.

[11] Texas Instrument. TI multicore programming guide [M]. Texas:TI,2012：1-13.

[12] Texas Instrument. TI SYS/BIOS inter-processor communication (IPC) 1.25 user′s guide [M]. Texas:TI,2012：2-6.

[13] Texas Instrument. TI TMS320C66x DSP cache user guide [M]. Texas:TI,2010：10-20.

[14] Texas Instrument. TI network developer′s kit (NDK) v2.24 user′s guide [M]. Texas:TI,2014:6-10.

[15] Texas Instrument. TISYS/BIOS (TI-RTOS kernel) v6.45 user′s guide[M]. Texas:TI,2015：10-14.

ImplementationofSingle-epochSingle-baselineBeidouAttitudeMeasurementBasedonTMS320C6678

LI Lingxia，LI Gangping
(Chongqing Key Laboratory of Mobile Communications Technology,Chongqing University of Postsand Telecommunications,Chongqing 400065,China)

In order to realize high accuracy and real-time of attitude measurement and solve the problem of high complexity of Least-squares Ambiguity Decorrelation Adjustment(LAMBDA) algorithm in Beidou attitude measurement algorithm beased on carrier phase,the C6678 multi-core digital signal processor (DSP) with TI’s high-end processor is chosen as the single-apoch single-line Beidou attitude measurement platform. According to the characteristics of algorithm,the data stream mode is adopted to allocate tasks to multi-core:DSP’s CORE0 is responsible for the User Datagram Protocol (UDP)communication with the Beidou receiver,the various branches of Beidou attitude measurement algorithm are assigned to other three DSP COREs implementation. Running result of the real-time data collected by OEM617D Beidou receiver on the multi-core DSP algorithm is compared with the Matlab simulation result,which shows that the design can meet the real-time and accuracy requirements of the Beidou attitude measurement.

Beidou attitude measurement；single-epoch single-baseline;multi-core DSP；user datagram protocol network port transmission；Cache technology

date:2016-12-22；Revised date：2017-05-23

国家自然科学基金资助项目(61301126);重庆市基础与前沿研究计划项目(cstc2013jcyjA40032)；重庆邮电大学博士启动基金(A2012-33)；重庆邮电大学青年科学研究项目(A2013-31)

**通信作者：1528709062@qq.com Corresponding author：1528709062@qq.com

TN965；P228.1

1001-893X(2017)10-1198-07

李玲霞(1976—)，女，湖北武穴人，2010年于重庆邮电大学获硕士学位，现为高级工程师，主要研究方向为未来移动通信理论与技术、宽度无线接入技术；

Email:lilx@cqupt.edu.cn

黎刚平(1993—),男，江西抚州人，硕士研究生，主要研究方向为DSP嵌入式开发、“北斗”姿态测量。

Email:1528709062@qq.com

10.3969/j.issn.1001-893x.2017.10.016

李玲霞，黎刚平.基于TMS320C6678的单历元单基线“北斗”测姿算法的实现[J].电讯技术，2017,57(10):1198-1204.[LI Lingxia，LI Gangping.Implementation of single-epoch single-baseline Beidou attitude measurement based on TMS320C6678[J].Telecommunication Engineering,2017,57(10):1198-1204.]

2016-12-22；

2017-05-26