王世臣，張代省，范興民

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

1 組合導(dǎo)航處理平臺(tái)

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

2 總體設(shè)計(jì)方案

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

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

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

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

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

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

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

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

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

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

圖2 Zynq7020架構(gòu)

3 軟件設(shè)計(jì)方案

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

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

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

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

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

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

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

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

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

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

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

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

4.1 導(dǎo)航坐標(biāo)系（n 系）到載體坐標(biāo)系（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 組合導(dǎo)航濾波算法

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

圖4 組合導(dǎo)航濾波算法框圖

5 結(jié)束語(yǔ)

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