基于SOPC的MEMS組合導(dǎo)航儀設(shè)計(jì)與實(shí)現(xiàn)

梁海波 司文杰 劉志華 肖昌怡

1.北京航天自動(dòng)控制研究所，北京 1008542.中國西南電子設(shè)備研究所，成都 610036

基于SOPC的MEMS組合導(dǎo)航儀設(shè)計(jì)與實(shí)現(xiàn)

梁海波1司文杰1劉志華1肖昌怡2

1.北京航天自動(dòng)控制研究所，北京 100854
2.中國西南電子設(shè)備研究所，成都 610036

為了提高以微機(jī)電系統(tǒng)(Micro Electro Mechanical System，MEMS)慣性器件為核心的慣性導(dǎo)航系統(tǒng)精度，提出了由MEMS慣性測(cè)量單元、GPS接收機(jī)、傾斜儀和電子羅盤構(gòu)成組合導(dǎo)航系統(tǒng)的方案。該系統(tǒng)以FPGA作為中央處理器，采用SOPC(System on Programmable Chip)技術(shù)構(gòu)建導(dǎo)航計(jì)算機(jī)，并開發(fā)多傳感器數(shù)據(jù)融合算法，完成組合導(dǎo)航功能。通過靜態(tài)導(dǎo)航試驗(yàn)，表明該系統(tǒng)能夠?qū)崿F(xiàn)在1000s時(shí)間內(nèi)，位置誤差均值小于2.1m，標(biāo)準(zhǔn)差不大于3.1m。

組合導(dǎo)航;數(shù)據(jù)融合;微機(jī)電系統(tǒng);片上可編程系統(tǒng)

目前，市場(chǎng)上基于微機(jī)電系統(tǒng)(Micro Electro Mechanical System，MEMS)慣性器件的慣性導(dǎo)航產(chǎn)品日益增多，但受制造工藝所限，MEMS慣性器件的精度相比傳統(tǒng)慣性器件還比較低，加之慣性導(dǎo)航誤差隨時(shí)間累積，使得MEMS慣性導(dǎo)航系統(tǒng)還不具備獨(dú)立的長時(shí)間導(dǎo)航定位能力［1-2］。在實(shí)際應(yīng)用中，為減小導(dǎo)航誤差，提高系統(tǒng)精度，引入輔助傳感器和組合導(dǎo)航技術(shù)是行之有效的方案。近年來，基于MEMS慣性器件的多傳感器組合導(dǎo)航方案和算法，國內(nèi)外學(xué)者開展了較深入的研究。例如，文獻(xiàn)［3］對(duì)基于MEMS慣性測(cè)量單元與GPS接收機(jī)組合導(dǎo)航技術(shù)的發(fā)展?fàn)顩r進(jìn)行了總結(jié)與分析;文獻(xiàn)［4］給出了一種采用MEMS慣性器件和GPS接收機(jī)構(gòu)建組合導(dǎo)航系統(tǒng)的方案;文獻(xiàn)［5］將視覺信息引入到組合導(dǎo)航系統(tǒng)中，進(jìn)一步豐富了導(dǎo)航輔助信息的種類，導(dǎo)航算法的設(shè)計(jì)也隨之復(fù)雜化。這里，提出一種新的以SOPC(System on Programmable Chip)技術(shù)為載體，采用MEMS慣性器件、GPS接收機(jī)、傾斜儀和電子羅盤構(gòu)建多傳感器組合導(dǎo)航系統(tǒng)的方案，并研制了原理樣機(jī)，設(shè)計(jì)了相應(yīng)的多傳感器數(shù)據(jù)融合算法(即組合導(dǎo)航算法)，以達(dá)到改善導(dǎo)航精度的目的。

1 硬件設(shè)計(jì)

如圖1所示，組合導(dǎo)航儀由電源、液晶顯示、傳感器和導(dǎo)航計(jì)算機(jī)4大功能模塊構(gòu)成，各功能模塊的具體功能如下:

1)電源模塊將外界輸入的24V直流電源轉(zhuǎn)換為5V二次電源，供導(dǎo)航儀內(nèi)部各子功能模塊使用;

2)液晶顯示模塊作為人機(jī)交互的接口，為用戶實(shí)時(shí)顯示導(dǎo)航結(jié)果;

圖1 系統(tǒng)結(jié)構(gòu)框圖

3)傳感器模塊包括MEMS慣性測(cè)量單元、GPS接收機(jī)、傾斜儀和電子羅盤，各傳感器的性能指標(biāo)如表1所示。MEMS慣性測(cè)量單元結(jié)合捷聯(lián)慣性導(dǎo)航算法可以實(shí)現(xiàn)短時(shí)間、高動(dòng)態(tài)條件下的姿態(tài)、速度和位置解算，而GPS接收機(jī)可以實(shí)時(shí)提供載體的速度和位置信息，傾斜儀能實(shí)現(xiàn)低動(dòng)態(tài)條件下的水平姿態(tài)測(cè)量，電子羅盤能提供地磁北向基準(zhǔn)。通過對(duì)多傳感器數(shù)據(jù)融合算法的設(shè)計(jì)，可取各傳感器之所長，提供較長時(shí)間、較高精度的導(dǎo)航結(jié)果;

4)導(dǎo)航計(jì)算機(jī)作為組合導(dǎo)航儀的核心模塊之一，負(fù)責(zé)各傳感器數(shù)據(jù)的采集，并作為組合導(dǎo)航算法的載體，進(jìn)行組合導(dǎo)航解算。

2 軟件設(shè)計(jì)

2.1 基于SOPC的導(dǎo)航計(jì)算機(jī)定制

導(dǎo)航計(jì)算機(jī)由處理器和外圍功能電路組成，處理器選用Altera公司的FPGA芯片EP1C12Q240I7。該芯片內(nèi)部資源豐富，具有12060個(gè)邏輯單元、239616位RAM、2個(gè)鎖相環(huán)，用戶可用的最大I/O數(shù)為249個(gè)，足以滿足組合導(dǎo)航儀的硬件需求。

SOPC(System On Programmable Chip)技術(shù)是一種特殊的嵌入式處理器技術(shù)，它將CPU和邏輯電路以IP核的形式集成到FPGA芯片上，僅由單個(gè)芯片即可完成整個(gè)系統(tǒng)的主要邏輯功能，具有設(shè)計(jì)方式靈活、功能可裁減、可擴(kuò)充、可升級(jí)的優(yōu)點(diǎn)，并具備軟硬件“在系統(tǒng)可編程”的功能［6-7］。

這里，根據(jù)組合導(dǎo)航儀的功能需求，對(duì)SOPC進(jìn)行了定制，具體包括如下IP核:CPU，定時(shí)器，PIO，SPI，UART，EPCS控制器，SDRAM 控制器等。在Quartus 8.0集成開發(fā)環(huán)境下，通過SOPC Builder工具對(duì)各IP核進(jìn)行如下配置:

1)CPU選用快速型Nios II帶硬件乘法器內(nèi)核，以保證高速的導(dǎo)航解算性能;

2)定制2個(gè)定時(shí)器，其中1個(gè)用于傳感器的定時(shí)采樣，另1個(gè)供μC/OS-II操作系統(tǒng)使用;

3)定制SPI內(nèi)核1個(gè)，用于采集MEMS慣性測(cè)量單元的數(shù)據(jù)。這里，為了與MEMS慣性測(cè)量單元的通訊協(xié)議保持一致，將SPI通信頻率設(shè)為2MHz，奇偶極性和相位極性都設(shè)置為1;

4)定制 UART內(nèi)核6個(gè)，分別用于外部RS422，GPS接收機(jī)、傾斜儀、電子羅盤、顯示模塊，以及底層監(jiān)控程序的通信，并對(duì)各自串行通信所需的波特率、奇偶校驗(yàn)等參數(shù)進(jìn)行設(shè)置;

5)定制EPCS控制器1個(gè)，該控制器一方面能夠保證組合導(dǎo)航儀軟件和試驗(yàn)數(shù)據(jù)能保存到EPCS存儲(chǔ)器中，另一方面在系統(tǒng)上電后，能保證處理器對(duì)EPCS存儲(chǔ)器進(jìn)行主動(dòng)串行配置;

6)定制SDRAM控制器1個(gè)，作為Nios II內(nèi)核與外部SDRAM芯片的通信媒介。

2.2 軟件設(shè)計(jì)步驟

組合導(dǎo)航儀對(duì)軟件的實(shí)時(shí)性和可靠性要求較高，而嵌入式操作系統(tǒng)μC/OS-II是一個(gè)完整的、可移植、固化、剪裁的占先式實(shí)時(shí)多任務(wù)系統(tǒng)［9］，恰能滿足組合導(dǎo)航儀的設(shè)計(jì)需求。因此，選用μC/OS-II作為組合導(dǎo)航儀的軟件運(yùn)行平臺(tái)。在μC/OS-II環(huán)境下，按照如下步驟進(jìn)行軟件設(shè)計(jì):

1)根據(jù)組合導(dǎo)航儀的功能需求，將系統(tǒng)任務(wù)劃分為:慣性測(cè)量單元數(shù)據(jù)采集任務(wù)(SPI接口)、GPS接收機(jī)數(shù)據(jù)采集任務(wù)(UART接口)、傾斜儀數(shù)據(jù)采集任務(wù)(UART接口)、電子羅盤數(shù)據(jù)采集任務(wù)(UART接口)、組合導(dǎo)航解算任務(wù)、底層監(jiān)控任務(wù)(UART接口)、向液晶顯示模塊發(fā)送數(shù)據(jù)任務(wù)和系統(tǒng)運(yùn)行狀態(tài)指示任務(wù)等子任務(wù);

2)為各任務(wù)定義任務(wù)堆棧。μC/OS-II中有1個(gè)專用的OS_STK數(shù)據(jù)類型用于定義任務(wù)堆棧，只需按照μC/OS-II的使用說明，創(chuàng)建長度為TASK_STK_SIZE(根據(jù)需要定義大小)、類型為OS_STK的數(shù)組，即完成了相應(yīng)任務(wù)堆棧空間的創(chuàng)建;

3)定義任務(wù)優(yōu)先級(jí)。注意每個(gè)任務(wù)必須有唯一的優(yōu)先級(jí)，且不能高于指定的最大優(yōu)先級(jí);

4)定義信號(hào)量、消息隊(duì)列、消息郵箱和全局變量，編寫相關(guān)軟件函數(shù)。除系統(tǒng)函數(shù)外，還需編寫本節(jié)第1)條中定義的各子任務(wù)函數(shù);

5)在主函數(shù)中創(chuàng)建初始任務(wù)。在初始任務(wù)中創(chuàng)建消息隊(duì)列和郵箱，以及本節(jié)第1)條中定義的各系統(tǒng)子任務(wù);

6)啟動(dòng)操作系統(tǒng)，按照操作系統(tǒng)的調(diào)度機(jī)制來執(zhí)行各任務(wù)。

2.3 組合導(dǎo)航算法設(shè)計(jì)

在設(shè)計(jì)組合導(dǎo)航算法時(shí)，將組合導(dǎo)航儀內(nèi)部的傳感器分為2類:①慣性測(cè)量單元，用于進(jìn)行捷聯(lián)慣性導(dǎo)航解算，得到載體實(shí)時(shí)的姿態(tài)、速度和位置信息;②GPS接收機(jī)、傾斜儀和電子羅盤，作為慣性導(dǎo)航解算的輔助數(shù)據(jù)。

如圖2所示，組合導(dǎo)航算法的基本思路是:在捷聯(lián)慣性導(dǎo)航系統(tǒng)誤差方程的基礎(chǔ)上，結(jié)合輔助數(shù)據(jù)，建立組合導(dǎo)航系統(tǒng)的狀態(tài)方程和量測(cè)方程;然后采用Kalman濾波對(duì)慣性導(dǎo)航解算的誤差進(jìn)行估計(jì);最后利用估計(jì)值對(duì)慣性導(dǎo)航解算結(jié)果進(jìn)行校正，從而得到穩(wěn)定的、較高精度的姿態(tài)、速度和位置信息，實(shí)現(xiàn)組合導(dǎo)航功能［10］。

圖2 組合導(dǎo)航算法示意圖

(1)系統(tǒng)狀態(tài)方程

慣導(dǎo)誤差方程反映了慣性導(dǎo)航系統(tǒng)的誤差傳播特性，是組合導(dǎo)航理論的基礎(chǔ)之一。這里，以東北天地理坐標(biāo)系作為導(dǎo)航坐標(biāo)系，建立如下位移(以開機(jī)時(shí)刻所在位置作為位置零點(diǎn))、速度和姿態(tài)誤差方程［11-12］:

式中，δr·n，δVn，δφ·n分別為導(dǎo)航系下的位移誤差、速度誤差和姿態(tài)誤差;fn為加速度計(jì)在導(dǎo)航系下測(cè)量的比力為由載體系到導(dǎo)航系的坐標(biāo)轉(zhuǎn)換矩陣(即捷聯(lián)矩陣);▽b和εb分別為載體系下的加速度計(jì)零偏和陀螺常值漂移，這里認(rèn)為二者是常值［13］。=［0 ΩcosLΩsinL］T(Ω和L分別為地球自轉(zhuǎn)角速度和緯度)(RM和RN分別為地球子午曲率半徑和卯酉曲率半徑，h為當(dāng)前位置的高度);=

將系統(tǒng)噪聲考慮在內(nèi)，得到組合導(dǎo)航系統(tǒng)狀態(tài)方程為

(2)系統(tǒng)量測(cè)方程

系統(tǒng)量測(cè)方程為

以GPS接收機(jī)提供的位置和速度、以傾斜儀提供的水平姿態(tài)、以電子羅盤提供的航向作為輔助數(shù)據(jù)，將捷聯(lián)慣性解算的位置、速度和姿態(tài)與上述數(shù)據(jù)相減，得到兩類數(shù)據(jù)的差值。這里，取東向和北向位移差值、東向和北向速度差值、3個(gè)姿態(tài)差值作為組合導(dǎo)航系統(tǒng)的量測(cè)值，即

通過對(duì)各輔助傳感器的數(shù)據(jù)進(jìn)行統(tǒng)計(jì)學(xué)分析，可以得到組合導(dǎo)航系統(tǒng)量測(cè)噪聲的統(tǒng)計(jì)特性。至此，便建立了組合導(dǎo)航系統(tǒng)的狀態(tài)方程和量測(cè)方程，可以利用Kalman濾波進(jìn)行組合導(dǎo)航解算。需要指出的是，前面所建立的組合導(dǎo)航系統(tǒng)方程針對(duì)的是時(shí)間連續(xù)系統(tǒng)，但在實(shí)際操作中需將其離散化，然后在導(dǎo)航計(jì)算機(jī)中按照離散系統(tǒng)方程進(jìn)行計(jì)算。時(shí)間連續(xù)系統(tǒng)離散化的實(shí)質(zhì)是根據(jù)連續(xù)系統(tǒng)的系統(tǒng)矩陣Ft計(jì)算出離散系統(tǒng)的轉(zhuǎn)移矩陣，以及根據(jù)連續(xù)系統(tǒng)的噪聲方差陣Qt計(jì)算出離散系統(tǒng)噪聲方差陣，限于文章篇幅，具體的離散化方法可以參見文獻(xiàn)［14］，這里不再贅述。

3 試驗(yàn)與結(jié)果分析

靜態(tài)組合導(dǎo)航試驗(yàn)具有操作簡便、現(xiàn)象直觀的特點(diǎn)，可以用于驗(yàn)證組合導(dǎo)航算法的有效性。將組合導(dǎo)航儀裝配于三軸速率轉(zhuǎn)臺(tái)上，使導(dǎo)航儀坐標(biāo)系與轉(zhuǎn)臺(tái)坐標(biāo)系重合，將GPS接收機(jī)的天線置于戶外，控制轉(zhuǎn)臺(tái)給定某一恒定姿態(tài)。開啟組合導(dǎo)航儀，待GPS信息有效后，將轉(zhuǎn)臺(tái)的姿態(tài)、GPS接收機(jī)提供的位置作為組合導(dǎo)航儀的初始姿態(tài)和初始位置，并將初始速度置0，開始組合導(dǎo)航解算。

試驗(yàn)過程中，對(duì)組合導(dǎo)航儀輸出的姿態(tài)、速度和位置等導(dǎo)航數(shù)據(jù)進(jìn)行記錄，并存儲(chǔ)在組合導(dǎo)航儀內(nèi)部的EPCS存儲(chǔ)器中。試驗(yàn)共進(jìn)行了3次，每次持續(xù)時(shí)間1000s。通過與試驗(yàn)真值進(jìn)行對(duì)比，得到了組合導(dǎo)航誤差。通過分析，3次試驗(yàn)結(jié)果的重復(fù)性較好，圖3給出了第2次試驗(yàn)的誤差曲線。

由圖3可以看出，組合導(dǎo)航算法克服了純慣性導(dǎo)航解算誤差隨時(shí)間積累的缺點(diǎn)，姿態(tài)、速度和位置誤差不再發(fā)散，而是在某一范圍內(nèi)波動(dòng)并且有收斂趨勢(shì)，導(dǎo)航定位精度有了明顯改善。由于慣性導(dǎo)航誤差按照姿態(tài)、速度、位置的順序傳播，位置誤差的大小可在一定程度上反映導(dǎo)航儀性能的優(yōu)劣，因此對(duì)位置誤差進(jìn)行了如表2所示的統(tǒng)計(jì)分析。從表中數(shù)據(jù)可以看出，在1000s時(shí)間內(nèi)，位置誤差均值被控制在2.1m以內(nèi)，且誤差標(biāo)準(zhǔn)差不大于3.1m。

表2 組合導(dǎo)航位置誤差

圖3 靜態(tài)試驗(yàn)結(jié)果

4 結(jié)論與展望

實(shí)踐證明，組合導(dǎo)航技術(shù)是提高M(jìn)EMS慣性導(dǎo)航精度的有效手段，SOPC系統(tǒng)開發(fā)的靈活性特點(diǎn)在組合導(dǎo)航儀的設(shè)計(jì)中得到了充分的體現(xiàn)。在后續(xù)的研究中，將根據(jù)導(dǎo)航儀的實(shí)際應(yīng)用需求，從GPS接收機(jī)信號(hào)有/無、導(dǎo)航儀高/低動(dòng)態(tài)等方面對(duì)組合導(dǎo)航算法進(jìn)行更深入的研究。

通過進(jìn)一步改進(jìn)，該組合導(dǎo)航儀將有望在攝像穩(wěn)定平臺(tái)、船舶/車輛的穩(wěn)定控制和制導(dǎo)彈藥等領(lǐng)域發(fā)揮重要的作用。

［1］張英，戚紅向，李德強(qiáng)，等.MEMS航天慣導(dǎo)產(chǎn)品及技術(shù)發(fā)展簡介［J］.航天標(biāo)準(zhǔn)化，2010，(3):40-43.(Zhang Ying，Qi Hongxiang，Li Deqiang，et al.Development of MEMS Inertial Navigation System in Aerospace Field ［J］.Aerospace Standardization，2010，(3):40-43.)

［2］王永海，劉佳琪，張力，杜潤樂.基于小型一體化MEMS慣性組合單元的應(yīng)用研究［J］.航天標(biāo)準(zhǔn)化，2010，(3):36-40.(Wang Yonghai，Liu Jiaqi，Zhang Li，Du Runle.Application of Small Integrated MEMS-based Inertial Unit［J］.Aerospace Standardization，2010，(3):36-40.)

［3］康岳林，高楊，高立.基于MEMS的GPS/SINS組合導(dǎo)航技術(shù)發(fā)展現(xiàn)狀［J］.船電技術(shù)，2011，31(5):26-29.(Kang Yuelin，Gao Yang，Gao Li.Trend of MEMS-based GPS/SINS Integrated Navigation System Based on MEMS［J］.Marine Electric ＆ Electronic Engineering，2011，31(5):26-29.)

［4］El-Sheimy N，Niu X.An Expert Knowledge GPS/INS System for Mobile Mapping and GIS Applications［C］∥Proceedings of the 2000 National Technical Meeting of the Satellite Division of the Institute of Navigation，2005，1:26-28.

［5］邵瑋，祝小平，趙剛.基于視覺/GPS/MEMS-INS數(shù)據(jù)融合的MUAV導(dǎo)航方案［J］.系統(tǒng)仿真技術(shù)，2011，7(1):77-81.(Shao Wei，Zhu Xiaoping，Zhao Gang.Integrated Navigation Scheme for MUAV Based on Vision/GPS/MEMS-INS ［J］.System Simulation Technology，2011，7(1):77-81.)

［6］Altera Corp.Nios ll Processor Reference Handbook［EB/OL］.2008.

［7］Altera Corp.Nios ll Software Developer's Handbook［EB/OL］.2008.

［8］周立功.SOPC嵌入式系統(tǒng)基礎(chǔ)教程［M］.北京:北京航空航天大學(xué)出版社，2006.(Zhou Ligong.SOPC Embedded System Tutorial［M］.Beijing:Beijing University of Aeronautics and Astronautics Press，2006.

［9］任哲.嵌入式實(shí)時(shí)操作系統(tǒng) μC/OS-II原理及應(yīng)用(第二版)［M］.北京:北京航空航天大學(xué)出版社，2009.(Ren Zhe.Embedded Real-time Operating System μC/OS-II Principles and Applications［M］.2nd ed.Beijing:Beijing University of Aeronautics and Astronautics Press，2006.)

［10］Robert M.Rogers.Applied Mathematics in Integrated Navigation Systems［M］.3rd ed.American Institute of Aeronautics and Astronautics，2007.

［11］秦永元.慣性導(dǎo)航［M］.北京:科學(xué)出版社，2006:355-360.(Qin Yongyuan.Inertial Navigation［M］.Beijing:Science Press，2006:355-360.)

［12］王惠南.GPS導(dǎo)航原理與應(yīng)用［M］.北京:科學(xué)出版社，2003:224-229.(Wang Huinan.Principles and Applications of GPS Navigation［M］.Beijing:Science Press，2003:224-229.)

［13］孫暢，富立，劉文麗.微陀螺零偏在線補(bǔ)償方法及仿真［J］.彈箭與制導(dǎo)學(xué)報(bào)，2008，28(3):52-54.(Sun Chang，F(xiàn)u Li，Liu Wenli.On-line Compensation for Zero Bias of MEMS Gyro［J］.Journal of Projectiles，Rockets，Missiles and Guidance，2008，28(3):52-54.)

［14］張國良，曾靜.組合導(dǎo)航原理與技術(shù)［M］.西安:西安交通大學(xué)出版社，2008:124-125.(Zhang Guoliang，Zeng Jing.Navigation Principles and Techniques［M］.Xi’an:Xi’an Jiaotong University Press，2008:124-125.)

Design and Realization of MEMS Integrated Navigator Based on SOPC

LIANG Haibo1SI Wenjie1LIU Zhihua1XIAO Changyi2
1.Beijing Aerospace Automatic Control Institute，Beijing 100854，China
2.Southwest China Research Institute of Electronic Equipment，Chengdu 610036，China

In order to enhance the accuracy of navigation system based onMEMS(Micro Electro Mechanical System)inertial devices，an integrated navigation system，including inertial measurement unit，magnetometer，inclinometer andGPS，is presented．In this system，theFPGAchip is adopted as the navigation computer，supported by theSOPC(System on Programmable Chip)technique．Furthermore，the software of multiple sensors fusion is developed．The static navigation test indicates that the integrated navigator can improve the precision of navigation system effectively．

Integrated navigation;Data fusion;MEMS;SOPC

V241.6

A

1006-3242(2014)02-0003-06

2012-10-18

梁海波(1984-)，男，天津人，博士，工程師，主要研究方向?yàn)镸EMS慣性導(dǎo)航與組合導(dǎo)航技術(shù);司文杰(1982-)，男，浙江人，博士，工程師，主要研究方向?yàn)榭刂葡到y(tǒng)電氣設(shè)計(jì)技術(shù);劉志華(1978-)，女，湖南人，碩士，高級(jí)工程師，主要研究方向?yàn)榭刂葡到y(tǒng)電氣設(shè)計(jì)技術(shù);肖昌怡(1986-)，男，湖南人，碩士，工程師，主要研究方向?yàn)榻M合導(dǎo)航技術(shù)。