管 斌，李 濤，吳美平，王荔斌

(1.國防科技大學(xué)機(jī)電工程與自動(dòng)化學(xué)院，湖南長沙410073;2.中國人民解放軍63999部隊(duì)，北京100094)

0 引言

低成本SINS/GPS組合導(dǎo)航系統(tǒng)具有體積小、成本低等優(yōu)點(diǎn)，可以應(yīng)用于智能炮彈、制導(dǎo)炸彈、無人飛機(jī)、機(jī)器人等多種場合［1］。文獻(xiàn)［2］通過理論分析與仿真實(shí)驗(yàn)說明了SINS/GPS組合導(dǎo)航系統(tǒng)中航向角誤差的可觀測(cè)性較差，當(dāng)載體處于靜止或勻速運(yùn)動(dòng)狀態(tài)時(shí)，系統(tǒng)航向角誤差無法有效收斂。文獻(xiàn)［3］通過分段線性定常系統(tǒng)可觀測(cè)性分析理論對(duì)15維狀態(tài)的SINS/GPS組合導(dǎo)航濾波模型進(jìn)行了可觀測(cè)分析，結(jié)果表明:靜止?fàn)顟B(tài)下航向角誤差不可觀測(cè)。

文獻(xiàn)［4］提出了一種通過磁強(qiáng)計(jì)計(jì)算航向角并將之加入至系統(tǒng)的觀測(cè)量中，與三維位置觀測(cè)量構(gòu)成系統(tǒng)四維觀測(cè)量，從而構(gòu)成MIMU/GPS/磁強(qiáng)計(jì)組合系統(tǒng)的方法，并通過可觀測(cè)性分析說明了將磁強(qiáng)計(jì)加入至系統(tǒng)后系統(tǒng)的所有狀態(tài)完全可觀測(cè)。

本文采用磁強(qiáng)計(jì)與低成本SINS/GPS構(gòu)成新的組合導(dǎo)航系統(tǒng)，利用SINS/GPS組合提供的水平姿態(tài)角與磁強(qiáng)計(jì)觀測(cè)量，采用磁羅盤原理計(jì)算得到航向角，并作為組合導(dǎo)航系統(tǒng)新的觀測(cè)量，以此改善航向角的可觀測(cè)性，從而提高組合導(dǎo)航系統(tǒng)的航向精度。首先建立了系統(tǒng)完整的濾波模型，通過多組仿真實(shí)驗(yàn)對(duì)2種組合系統(tǒng)進(jìn)行了對(duì)比，最后通過靜態(tài)實(shí)驗(yàn)驗(yàn)證了本方法的有效性。

1 組合導(dǎo)航系統(tǒng)模型

1.1 捷聯(lián)導(dǎo)航系統(tǒng)誤差模型

在低成本系統(tǒng)的前提條件下，對(duì)文獻(xiàn)［5］中所述捷聯(lián)導(dǎo)航系統(tǒng)誤差模型進(jìn)行簡化，得到姿態(tài)誤差角方程

速度誤差方程

位置誤差方程

1.2 磁航向角的計(jì)算

通過SINS/GPS組合得到載體的橫滾角φ和俯仰角θ，利用磁強(qiáng)計(jì)的測(cè)量信息［X Y Z］可以得到地球的磁場水平分量Xh與，如式(4)所示

航向角可根據(jù)式(5)得到

其中，φM0為磁航向角，經(jīng)過磁偏角的修正后可以得到載體的航向角φM。

1.3 SINS/GPS組合濾波模型

選取量測(cè)向量為

由式(1)～式(3)與式(6)可以得到SINS/GPS組合濾波模型［7］

式中 W為6維狀態(tài)噪聲，由陀螺和加速度計(jì)噪聲構(gòu)成;V為6維觀測(cè)噪聲，由GPS觀測(cè)噪聲構(gòu)成

1.4 SINS/GPS/磁強(qiáng)計(jì)組合濾波模型

將SINS解算得到的航向角φI與磁羅盤所得航向角φM之差增加為濾波器的一維觀測(cè)量，即令量測(cè)向量為

由式(1)～式(3)與式(9)可以得到SINS/GPS/磁強(qiáng)計(jì)組合濾波模型

式中 狀態(tài)方程與模型(7)中狀態(tài)方程一致，且

2 仿真實(shí)驗(yàn)分析

為了分析SINS/GPS組合系統(tǒng)的航向角誤差觀測(cè)時(shí)存在的相關(guān)問題，并驗(yàn)證SINS/GPS/磁強(qiáng)計(jì)組合系統(tǒng)的有效性，在Simulink中對(duì)靜止與勻速運(yùn)動(dòng)狀態(tài)、正弦機(jī)動(dòng)狀態(tài)和正弦機(jī)動(dòng)轉(zhuǎn)勻速運(yùn)動(dòng)狀態(tài)下的2種組合系統(tǒng)進(jìn)行了系統(tǒng)仿真和比較分析，仿真參數(shù)如表1所示。

2.1 靜止與勻速運(yùn)動(dòng)狀態(tài)系統(tǒng)仿真

對(duì)靜止與勻速運(yùn)動(dòng)狀態(tài)下的SINS/GPS組合系統(tǒng)與SINS/GPS/磁強(qiáng)計(jì)組合系統(tǒng)進(jìn)行仿真比較，系統(tǒng)姿態(tài)角誤差曲線如圖1所示(2種條件下結(jié)果一致，圖中虛線代表SINS/GPS組合系統(tǒng)，實(shí)線代表SINS/GPS/磁強(qiáng)計(jì)組合系統(tǒng)，下同)。

由圖可得，靜止與勻速運(yùn)動(dòng)狀態(tài)下2種組合系統(tǒng)的水平姿態(tài)角誤差收斂效果一致，而SINS/GPS組合系統(tǒng)航向角誤差不可收斂，與文獻(xiàn)［2，3］中相關(guān)結(jié)論一致，SINS/GPS/磁強(qiáng)計(jì)組合系統(tǒng)的航向角誤差可收斂至較小范圍。仿真結(jié)果表明:在上述運(yùn)動(dòng)狀態(tài)下，將磁強(qiáng)計(jì)加入SINS/GPS組合系統(tǒng)后，系統(tǒng)的航向角誤差由不可收斂轉(zhuǎn)為可有效收斂，說明了該方法的可行性。

表1 仿真實(shí)驗(yàn)參數(shù)設(shè)定表Tab 1 Experiment parameters sheet of simulation

圖1 靜止?fàn)顟B(tài)姿態(tài)角誤差曲線Fig 1 Attitude angle error curve under stationary state

2.2 正弦機(jī)動(dòng)狀態(tài)系統(tǒng)仿真

在正弦機(jī)動(dòng)狀態(tài)下，對(duì)2種組合系統(tǒng)進(jìn)行仿真比較，系統(tǒng)姿態(tài)角誤差曲線如圖2所示。由圖可得，在正弦機(jī)動(dòng)狀態(tài)下SINS/GPS組合系統(tǒng)的航向角誤差可收斂，與文獻(xiàn)［8，9］相關(guān)結(jié)論一致，其收斂效果與機(jī)動(dòng)的效果相關(guān)，本組實(shí)驗(yàn)中航向角誤差在270s時(shí)收斂至1°以內(nèi);SINS/GPS/磁強(qiáng)計(jì)組合系統(tǒng)的航向角誤差收斂效果在正弦機(jī)動(dòng)狀態(tài)下與靜止?fàn)顟B(tài)下一致。仿真結(jié)果進(jìn)一步說明了SINS/GPS組合系統(tǒng)在機(jī)動(dòng)條件下姿態(tài)角誤差可觀測(cè)，而在靜止?fàn)顟B(tài)與勻速運(yùn)動(dòng)狀態(tài)下不可觀測(cè)的問題，說明了SINS/GPS/磁強(qiáng)計(jì)組合系統(tǒng)的有效性。

圖2 正弦機(jī)動(dòng)狀態(tài)姿態(tài)角誤差曲線Fig 2 Attitude angle error curve under sinusoidal movement

2.3 正弦機(jī)動(dòng)轉(zhuǎn)勻速運(yùn)動(dòng)系統(tǒng)仿真

為了分析真實(shí)運(yùn)動(dòng)條件下SINS/GPS/磁強(qiáng)計(jì)組合系統(tǒng)的有效性，本文設(shè)計(jì)了600 s時(shí)長的仿真實(shí)驗(yàn)，載體首先做正弦機(jī)動(dòng)300s，然后勻速運(yùn)動(dòng)300s，此運(yùn)動(dòng)狀態(tài)下的姿態(tài)角誤差曲線如圖3所示。

圖3 正弦機(jī)動(dòng)轉(zhuǎn)勻速運(yùn)動(dòng)時(shí)的姿態(tài)角誤差曲線Fig 3 Attitude angle error curve under sinusoidal mobile state turned to uniform motion

由圖可得，對(duì)SINS/GPS組合系統(tǒng)，機(jī)動(dòng)300 s結(jié)束時(shí)的航向角誤差收斂至較小值，而當(dāng)載體開始做勻速運(yùn)動(dòng)時(shí)，航向角誤差開始增大，勻速運(yùn)動(dòng)300 s的過程中，系統(tǒng)航向角誤差由1°變?yōu)?°;而對(duì)SINS/GPS/磁強(qiáng)計(jì)組合系統(tǒng)，系統(tǒng)的航向角誤差始終收斂在0°附近。

仿真結(jié)果表明:對(duì)于SINS/GPS組合系統(tǒng)，大機(jī)動(dòng)條件下系統(tǒng)的航向角誤差可以收斂至小角度，當(dāng)系統(tǒng)重新開始做勻速運(yùn)動(dòng)后，航向角誤差開始發(fā)散;而SINS/GPS/磁強(qiáng)計(jì)組合系統(tǒng)姿態(tài)角誤差一直收斂在較小角度;本實(shí)驗(yàn)說明了，將磁強(qiáng)計(jì)應(yīng)用于低成本SINS/GPS組合系統(tǒng)能夠提高系統(tǒng)航向角精度。

3 靜態(tài)實(shí)驗(yàn)分析

為了進(jìn)一步驗(yàn)證本文所提方法在實(shí)際應(yīng)用中的有效性，在仿真試驗(yàn)的基礎(chǔ)上，利用某型系統(tǒng)進(jìn)行靜態(tài)實(shí)驗(yàn)，該系統(tǒng)所用器件的性能指標(biāo)如表2所示。

表2 系統(tǒng)性能指標(biāo)Tab 2 System property index

實(shí)驗(yàn)方案:在有GPS信號(hào)的空曠環(huán)境下，將系統(tǒng)固定在穩(wěn)定的平臺(tái)上，系統(tǒng)上電并進(jìn)行相關(guān)測(cè)試，確保 MIMU/GPS/磁強(qiáng)計(jì)輸出數(shù)據(jù)有效，待系統(tǒng)捕獲到足夠的GPS信號(hào)后開始記錄數(shù)據(jù)，采集數(shù)據(jù)10 min后系統(tǒng)下電。

利用所記錄數(shù)據(jù)進(jìn)行離線處理，通過模型式(10)解算得到的姿態(tài)角誤差曲線如圖4所示。由圖可得，2種組合系統(tǒng)的水平姿態(tài)角誤差收斂效果一致，而SINS/GPS組合系統(tǒng)的航向角誤差發(fā)散，SINS/GPS/磁強(qiáng)計(jì)組合系統(tǒng)的航向角誤差有效收斂。該靜態(tài)實(shí)驗(yàn)驗(yàn)證了2.1節(jié)所做仿真實(shí)驗(yàn)的正確性，說明了本文所提出應(yīng)用磁強(qiáng)計(jì)提高低成本SINS/GPS組合導(dǎo)航系統(tǒng)航向精度方法的有效性。實(shí)驗(yàn)結(jié)果表明:各狀態(tài)均收斂時(shí)姿態(tài)角誤差標(biāo)準(zhǔn)差為［0.064 6°0.0577°0.1955°］。

圖4 靜止?fàn)顟B(tài)姿態(tài)角誤差曲線Fig 4 Attitude error under static state

4 結(jié)論

本文提出了一種應(yīng)用磁強(qiáng)計(jì)提高低成本SINS/GPS組合導(dǎo)航系統(tǒng)航向精度的方法。給出了詳細(xì)的系統(tǒng)濾波模型的同時(shí)，對(duì)低成本設(shè)備參數(shù)條件下的SINS/GPS組合導(dǎo)航系統(tǒng)與SINS/GPS/磁強(qiáng)計(jì)組合導(dǎo)航系統(tǒng)進(jìn)行了數(shù)字仿真實(shí)驗(yàn)，仿真實(shí)驗(yàn)表明:由于慣性器件精度較低，在靜止與勻速運(yùn)動(dòng)狀態(tài)下，SINS/GPS組合導(dǎo)航系統(tǒng)航向角誤差發(fā)散，而SINS/GPS/磁強(qiáng)計(jì)組合導(dǎo)航系統(tǒng)的航向角有效收斂。利用某型系統(tǒng)對(duì)本文方法在實(shí)際應(yīng)用中有效性進(jìn)行了驗(yàn)證，靜態(tài)實(shí)驗(yàn)表明:在傳感器精度較差的條件下，系統(tǒng)航向角誤差仍可以有效收斂;長時(shí)間實(shí)驗(yàn)表明:系統(tǒng)收斂時(shí)的姿態(tài)角誤差和誤差的標(biāo)準(zhǔn)差均較小，說明了應(yīng)用磁強(qiáng)計(jì)能夠提高低成本SINS/GPS組合導(dǎo)航系統(tǒng)航向精度。

［1］祝燕華，蔡體菁，楊卓鵬.MEMS-IMU/GPS組合導(dǎo)航系統(tǒng)的實(shí)現(xiàn)［J］.中國慣性技術(shù)學(xué)報(bào)，2009，17(5):552－561.

［2］李淵濤，陳 哲，劉 劍.SINS/GPS組合系統(tǒng)姿態(tài)角誤差可觀測(cè)性研究［J］.北京航空航天大學(xué)學(xué)報(bào)，1999，25(3):367－370.

［3］孔星煒，郭美鳳，董景新.MINS/GPS組合導(dǎo)航系統(tǒng)車載試驗(yàn)誤差特性分析［J］.中國慣性技術(shù)學(xué)報(bào)，2009，17(3):297－301.

［4］Laurent Wenger，Demoz Gebre Egziabher.System concepts and performance analysis of multi-sensor navigation systems for UAV applications［C］∥Proceedings of 2nd AIAA Unmanned Unlimited Systems，Technologies，and Operations.AIAA:San Diego，California，2003.

［5］Titterton D H，Weston JL.Strapdown inertial navigantion technology［M］.Second Edition.Lexington，Massachusetts，USA:American Institute of Aeronautics and Astronautics，Inc，2004.

［6］Caruso M J.Applications of magnetic sensors for low cost compass systems［C］∥Proceedings of 2000 IEEE Position Location and Navigation Symposium.San Diego，California，2000.

［7］秦永元，張洪鉞，汪叔華.卡爾曼濾波與組合導(dǎo)航原理［M］.西安:西北工業(yè)大學(xué)出版社，1998.

［8］劉 準(zhǔn)，陳 哲.局部可觀測(cè)理論在INS/GPS機(jī)動(dòng)對(duì)準(zhǔn)中的應(yīng)用［J］.北京航空航天大學(xué)學(xué)報(bào)，2001，27(6):702－705.

［9］耿延睿，郭 偉，崔中興，等.GPS/SINS系統(tǒng)空中對(duì)準(zhǔn)姿態(tài)角誤差可觀測(cè)性研究［J］.中國慣性技術(shù)學(xué)報(bào)，2004，12(1):37－42.