基于TMS320C6678的單歷元單基線“北斗”測姿算法的實現*

(重慶郵電大學 重慶市移動通信技術重點實驗室，重慶 400065)

基于TMS320C6678的單歷元單基線“北斗”測姿算法的實現*

李玲霞，黎剛平**

(重慶郵電大學 重慶市移動通信技術重點實驗室，重慶 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

A

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