王世臣，張代省，范興民

（1.安徽四創(chuàng)電子股份有限公司，合肥 230031；2.北斗衛(wèi)星導航技術(shù)安徽省重點實驗室，合肥 230031）

1 組合導航處理平臺

慣性導航系統(tǒng)的理論基礎(chǔ)是牛頓力學定律—慣性定律，利用慣性敏感元件、基準方向和初值位置確定載體的位置、速度和姿態(tài)，為自主式導航系統(tǒng)。但慣導系統(tǒng)會隨著時間累積誤差，嚴重時會影響導航精度，需引入外部數(shù)據(jù)對慣導系統(tǒng)進行定時校正，采用兩種或兩種以上的導航設(shè)備組合，構(gòu)建組合導航系統(tǒng)，目前主流的組合方式為衛(wèi)星導航系統(tǒng)（GNSS）和慣性導航系統(tǒng)（INS）相組合[1][2]，組合導航系統(tǒng)中的導航處理平臺一般采用FPGA+DSP 的架構(gòu)實現(xiàn)，缺點是體積大、功耗高。隨著組合導航系統(tǒng)集成化程度的要求越來越高，我們提出一種基于Zynq 處理器構(gòu)建組合導航處理平臺的方案，充分利用平臺優(yōu)勢，有效減小系統(tǒng)體積和功耗。

2 總體設(shè)計方案

組合導航系統(tǒng)平臺系統(tǒng)架構(gòu)如圖1所示：

圖1 組合導航處理平臺系統(tǒng)架構(gòu)圖

該處理平臺由ZYNQ7020，ADC，DDR3，eMMC，UART，網(wǎng)絡(luò)及422/485等接口組成。主要完成信號處理（對陀螺信號、加速度計信號、衛(wèi)導信號等進行處理）、數(shù)據(jù)采集、導航解算、數(shù)據(jù)存儲、數(shù)據(jù)傳輸?shù)裙δ堋?/p>

①陀螺儀和加速度計輸出為TTL 信號，利用ZYNQ7020的PL 端采集兩個TTL 脈沖信號，并進行計數(shù)。

②陀螺控制板與ZYNQ7020 PL 端采用UART TTL 通信，用于實現(xiàn)陀螺儀的抖頻和穩(wěn)頻控制。

③衛(wèi)星導航接收機通過RS422差分輸入，經(jīng)電平轉(zhuǎn)換后與PL 端進行通信，用于衛(wèi)星導航接收機的原始觀測量數(shù)據(jù)和導航信息的采集。

④采用RS422接口輸出導航解算結(jié)果至外部系統(tǒng)，提升通信可靠性和抗干擾能力。

⑤系統(tǒng)預留網(wǎng)絡(luò)和RS485等接口，便于系統(tǒng)調(diào)試和軟件升級。

⑥系統(tǒng)采用大容量的存儲（eMMC）和運行內(nèi)存（DDR3），PL 和PS 端共享內(nèi)存，用于數(shù)據(jù)采集和解算處理。

Zynq7020 作為平臺核心，是基于Xilinx 所有可編程SOC（APSOC）架構(gòu)，由PS 和PL 兩大部分組成，兩者之間通過AXI接口通信，AXI-HP 和AXI-ACP 協(xié)議下，PS 僅能讀數(shù)據(jù)，在AXI-GP 模式下，PS 可以讀寫數(shù)據(jù)。Zynq7020架構(gòu)如圖2所示：

圖2 Zynq7020架構(gòu)

3 軟件設(shè)計方案

基于系統(tǒng)穩(wěn)定性和可擴展性考慮，軟件設(shè)計采用高內(nèi)聚低耦合的設(shè)計思想。劃分為多個模塊，各模塊間接口統(tǒng)一，易于維護和升級。組合導航處理平臺軟件設(shè)計架構(gòu)如圖3所示。

（1）PL 端軟件主要包括溫度信號采集，陀螺信號和加速度計信號采集與處理，衛(wèi)星導航信號的數(shù)據(jù)采集與存儲。

（2）Zynq 處理器的PS 端有兩個ARM 處理器，分別完成不同功能。其中，一個ARM 處理器將PL 端采集和處理的信號，進行慣性導航解算，輸出姿態(tài)和加速度信息。并與衛(wèi)星導航構(gòu)建組合導航系統(tǒng)，實現(xiàn)濾波算法，輸出高精度位置、姿態(tài)和速度信息。同時還完成系統(tǒng)初始對準、桿臂校正及誤差估計等功能，為整個系統(tǒng)的核心。

圖3 系統(tǒng)軟件架構(gòu)圖

①初始對準模塊：根據(jù)加速度計、激光陀螺完成初始對準并確定姿態(tài)矩陣。根據(jù)衛(wèi)星導航模塊或用戶輸入的位置信息確定當?shù)亟?jīng)緯度。

②慣性導航解算：姿態(tài)、速度、位置更新模塊分別用于計算載體的姿態(tài)、速度、位置數(shù)據(jù)。

③組合導航濾波：根據(jù)當前的工作模式，選擇使用慣導系統(tǒng)輸出的姿態(tài)、速度信息，衛(wèi)星導航設(shè)備輸出的位置信息，根據(jù)不同的變量建立卡爾曼濾波方程，然后進行信息融合處理。

④桿臂校正：完成載體坐標系和導航坐標系間的姿態(tài)變換。

⑤誤差估計：完成陀螺及加速度計的零偏估計，進一步提升系統(tǒng)的精度。

（3）Zynq 處理器的ARM 端軟件，主要完成系統(tǒng)接口控制，協(xié)議轉(zhuǎn)換和數(shù)據(jù)存儲等功能，用于完成組合導航系統(tǒng)與外部系統(tǒng)對接。

4 數(shù)學模型及仿真

組合導航處理平臺的算法實現(xiàn)所采用的數(shù)學模型主要包括姿態(tài)矩陣、四元數(shù)法、比利方程、速度更新、位置更新及卡爾曼濾波等[3][4]。

4.1 導航坐標系（n 系）到載體坐標系（b 系）轉(zhuǎn)換

4.2 四元數(shù)法表示姿態(tài)

四元數(shù)法與姿態(tài)矩陣的關(guān)系：

4.3 比利方程

4.4 速度更新

速度更新微分方程：Vk+1=f（Vk，fb（tk）），速度增量：

4.5 位置更新

4.6 組合導航濾波算法

采用卡爾曼濾波器組合導航算法，實現(xiàn)誤差最有估計，具體實現(xiàn)流程如圖4所示。

圖4 組合導航濾波算法框圖

5 結(jié)束語

通過采用Zynq7020處理器平臺，構(gòu)建組合導航系統(tǒng)，可以大大降低系統(tǒng)的設(shè)計復雜度，充分利用該平臺的多核應(yīng)用架構(gòu)，總線內(nèi)部完成數(shù)據(jù)交互，縮短了數(shù)據(jù)緩存延遲。通過算法優(yōu)化，利用雙ARM 處理器協(xié)同工作，提升產(chǎn)品性能，提高系統(tǒng)的穩(wěn)定性和可靠性。