苑艷華，李四海，南 江

(西北工業(yè)大學(xué)自動化學(xué)院，西安 710129)

捷聯(lián)航姿系統(tǒng)是一種自主式低成本的航向姿態(tài)測量系統(tǒng)，此外還可以提供載體的角速率和加速度信息，有著廣泛的應(yīng)用前景。但由于低精度航姿系統(tǒng)陀螺精度較低，漂移較大，載體長時間機(jī)動時不能保證要求的姿態(tài)精度。采用低精度的捷聯(lián)航姿系統(tǒng)與GPS衛(wèi)星定位系統(tǒng)實現(xiàn)組合，是解決長時間飛行過程中姿態(tài)測量問題的良好途徑［1］，但是從軍事應(yīng)用的角度考慮，GPS信號并不可靠，因此必須解決輔助導(dǎo)航系統(tǒng)不可用或大部分不可用時，如何利用捷聯(lián)航姿系統(tǒng)本身的信息來提高精度，以滿足長時間飛行對航姿系統(tǒng)的姿態(tài)要求。目前這方面的研究較少［2－5］，由于航姿系統(tǒng)通常沒有當(dāng)?shù)亟?jīng)緯度等初始位置信息的輸入且沒有位置信息和速度信息的解算，通常采用的導(dǎo)航算法已不再適用［6－7］。

本文設(shè)計了一種基于卡爾曼濾波技術(shù)的姿態(tài)算法，用重力加速度在機(jī)體系的分量gb和陀螺漂移作為待估計的狀態(tài)量，在濾波周期內(nèi)，利用基于角運動信息的狀態(tài)方程進(jìn)行卡爾曼濾波的時間更新過程，同時運用系統(tǒng)自身信息對載體飛行狀態(tài)進(jìn)行判別，在非機(jī)動時利用基于比力信息的量測方程進(jìn)行卡爾曼濾波的量測更新過程，去修正由低精度的陀螺漂移造成的姿態(tài)誤差，仿真結(jié)果表明:此算法可長時間輸出較高精度的姿態(tài)信息。

1 捷聯(lián)航姿系統(tǒng)工作原理

捷聯(lián)航姿系統(tǒng)由低精度光纖陀螺或MEMS陀螺、加速度計、磁傳感器和信息處理電路組成［8］。陀螺敏感載體運動角速度，對于航姿系統(tǒng)中陀螺的輸出，計算時無法區(qū)分載體的角速率、地球表觀角速率和地球的自轉(zhuǎn)角速率，在每個采樣周期內(nèi)采集陀螺的輸出，經(jīng)信號預(yù)處理后，根據(jù)四元素微分方程［9］來更新姿態(tài)角。但由于陀螺漂移較大，姿態(tài)精度較低，同時利用載體在非機(jī)動時的加速度計輸出計算出俯仰角θ、橫滾角γ，然后再利用式(1)計算磁航向角［10］。由于加速度計測量精度較高，其測角精度也很高，因此在某些條件下，可以利用加速度計輸出計算姿態(tài)角，來更新因陀螺漂移造成的姿態(tài)誤差。

其中，Hx，Hy，Hz分別為三軸磁阻傳感器的輸出，β為磁航向角，定義地球表面任一點磁北與地理子午面的夾角為磁偏角α，載體相對于真北的航向角為ψ=β±α。

以上是工程上航姿系統(tǒng)常用的計算姿態(tài)的算法，但由于航姿系統(tǒng)的陀螺精度較低，漂移較大，通常的航姿算法中并未考慮陀螺漂移的影響［11］，因此長時間機(jī)動工作時不能保證要求的姿態(tài)精度，針對此問題，本文提出了基于卡爾曼濾波器的姿態(tài)融合算法。

2 基于卡爾曼濾波的姿態(tài)融合算法

2.1 基于角運動信息的系統(tǒng)方程

根據(jù)對姿態(tài)角估計的問題可以間接轉(zhuǎn)化為對重力加速度在機(jī)體系的分量gb估計的思想，本文推出了基于角運動信息的系統(tǒng)方程。

對等式兩邊同時微分得:

將式(4)代入式(3)得:

同時為了降低陀螺漂移的影響，將陀螺等效漂移列入系統(tǒng)狀態(tài)，本文將陀螺等效漂移考慮為一階馬爾科夫過程。

其中，wε(t)為白噪聲，Tg為相關(guān)時間。

綜合式(5)和式(6)，可得出基于角運動信息的系統(tǒng)方程為:

2.2 基于比力信息的量測方程

加速度計測量的是載體相對于慣性空間的比力信息，由比力方程可知，與載體固連的加速度計測量的真實比力信息fb可以在b系中描為:

2.3 系統(tǒng)離散化及姿態(tài)融合算法

式(7)建立的系統(tǒng)方程是非線性的，可以用擴(kuò)展卡爾曼濾波，也可以用上一步的估值來構(gòu)造當(dāng)前步的系數(shù)矩陣將其近似為線性系統(tǒng)。

對式(7)進(jìn)行離散化得:

一步轉(zhuǎn)移矩陣和等效離散噪聲方差陣計算如下:

當(dāng)載體平穩(wěn)飛行時，為 0，則

忽略加速度計等效零偏，則在載體非機(jī)動情況下時，基于比力信息的量測方程為:

對系統(tǒng)方程進(jìn)行離散化后，根據(jù)離散卡爾曼濾波基本方程即可完成姿態(tài)融合算法。由于式(9)表示的是載體在非機(jī)動情況下的量測方程，因此本文的姿態(tài)融合算法中，首先利用載體自身信息對運動狀態(tài)進(jìn)行判別，當(dāng)載體處于機(jī)動狀態(tài)時只進(jìn)行系統(tǒng)方程的時間更新過程，當(dāng)載體處于非機(jī)動狀態(tài)時，才進(jìn)行爾曼濾波的量測更新過程，即利用加速度計輸出信息來修正由于陀螺儀漂移而產(chǎn)生的估計誤差，同時用估計的陀螺漂移實時地修正陀螺儀的零位漂移。設(shè)狀態(tài)量gb的估計值為，則

計算出俯仰角和橫滾角后，再根據(jù)三軸磁阻傳感器的輸出利用式(1)來計算磁航向角。

3 載體飛行狀態(tài)的判別

3.1 飛行狀態(tài)的判別準(zhǔn)則

根據(jù)加速度計的輸出方程式(8)可知當(dāng)載體處于非機(jī)動時

由上式可知，當(dāng)飛機(jī)平穩(wěn)飛行時fb的模值約等于g，因此以利用加速度計輸出信息設(shè)計飛行狀態(tài)的模值判別準(zhǔn)則如下:

當(dāng)|‖fb‖－g|≤Thlad時，認(rèn)為載體處于非機(jī)動狀態(tài)，否則認(rèn)為載體處于機(jī)動狀態(tài)，其中Thlad的取值取決于加速度的等效零位偏置和測量噪聲的大小。

3.2 模值判別準(zhǔn)則誤判分析

以上所述的模值判別的理論依據(jù)是當(dāng)忽略等效加計偏置和測量噪聲時，飛機(jī)平穩(wěn)飛行時，加計輸出的模值等于重力加速度值。在忽略加速度計等效偏置和測量噪聲條件下，式(8)描述的加速度計輸出模型在n系中可以簡化為:

式(12)描述的加速度軌跡是一個球心在(0，0，－g)，半徑為g的球。即當(dāng)載體機(jī)動加速度在地理坐標(biāo)系的投影an的取值位于這個球面上或附近時，就會得出載體平穩(wěn)飛行的結(jié)論。因此只用模值判別會存在原理性的誤判。

對于上述問題可以加入卡爾曼濾波新息來判別，卡爾曼濾波新息指:

4 仿真結(jié)果及分析

仿真參數(shù)設(shè)置:馬爾科夫陀螺漂移均值為20°/h;相關(guān)時間為3 600 s;陀螺初始零偏誤差為10°/h;陀螺的系統(tǒng)噪聲均方差為10°/h;加速度計的零位偏置為0.001gn;加速度的測量噪聲均方差為0.001gn;卡爾曼濾波的時間更新周期為10 ms;卡爾曼濾波的量測更新周期為100 ms，仿真總時間為3 600 s。

本文模擬了殲擊機(jī)的對地攻擊過程來設(shè)計飛行軌跡［12］，包括各種典型的機(jī)動動作，同時為了驗證本文的算法，延長了飛機(jī)連續(xù)機(jī)動的時間。設(shè)計軌跡的姿態(tài)和加速度計參數(shù)如圖1所示，用上述提出的姿態(tài)融合算法計算的俯仰角和橫滾角誤差如圖2所示，估計的等效陀螺漂移如圖3所示，本文判別準(zhǔn)則對飛行狀態(tài)的判別情況如圖4所示。

圖1 飛行軌跡的姿態(tài)角和加速度參數(shù)

圖2 本文算法的姿態(tài)角誤差

圖3 本文算法的陀螺漂移估計值

圖4 判別準(zhǔn)則對飛行狀態(tài)的判別情況

在整個飛行過程中，載體從601 s到645 s的44 s時間以2°/s的角速率進(jìn)行了右盤旋運動，從901 s到945 s的44 s時間以2°/s的角速率進(jìn)行了左盤旋運動;載體從1 861 s到1 981 s的120 s的時間內(nèi)以1.5°/s的角速率左盤旋半周，從2 101 s到2 341 s的240 s的時間內(nèi)以1.5°/s的角速率右盤旋一周。從圖2可以看出，姿態(tài)誤差角出現(xiàn)了幾個峰值，姿態(tài)誤差峰值的出現(xiàn)是陀螺等效漂移在盤旋時間段內(nèi)積分的結(jié)果，后兩次盤旋的時間比較長，姿態(tài)角誤差峰值也較大，但載體從2 101 s到2 341 s的240 s連續(xù)機(jī)動的時間內(nèi)姿態(tài)角誤差最大值為0.7°，而用通常的導(dǎo)航算法姿態(tài)誤差將達(dá)到2°(由于篇幅限制，通常導(dǎo)航算法仿真誤差圖未加入)，因此連續(xù)機(jī)動的姿態(tài)精度提高了65%，且載體從2 500 s到2 560 s的時間內(nèi)一直做減速運動，姿態(tài)誤差角也很小，這是由于反饋了陀螺漂移估計值的結(jié)果。在3 600 s的整個飛行過程中的姿態(tài)角誤差都在1°以內(nèi)，因此本文設(shè)計的基于卡爾曼濾波的姿態(tài)融合算法比較穩(wěn)定，且提高了載體長時間機(jī)動的姿態(tài)精度，同時從圖4和載體的機(jī)動情況對比可以看出，本文設(shè)計的判別準(zhǔn)則準(zhǔn)確地對飛行狀態(tài)進(jìn)行了識別。

5 結(jié)論

本文提出了一種基于卡爾曼濾波技術(shù)的姿態(tài)融合算法，用重力加速度在機(jī)體系的分量gb和陀螺漂移作為待估計的狀態(tài)量，在濾波周期內(nèi)，利用基于角運動信息的系統(tǒng)方程進(jìn)行卡爾曼濾波的時間更新過程，在量測更新周期內(nèi)，先利用判別準(zhǔn)則對載體飛行狀態(tài)進(jìn)行判別，若載體處于非機(jī)動狀態(tài)，則利用基于比力信息的量測方程進(jìn)行卡爾曼濾波的量測更新過程，去修正由低精度的陀螺漂移造成的姿態(tài)誤差。仿真結(jié)果表明:本文所設(shè)計的姿態(tài)融合算法比較穩(wěn)定，計算簡單，且明顯提高了長期機(jī)動的動態(tài)精度，為下一步的航姿系統(tǒng)研發(fā)奠定了理論基礎(chǔ)。

［1］徐玉，李平，韓波.一種面向機(jī)動的低成本姿態(tài)測量系統(tǒng)［J］.傳感技術(shù)學(xué)報，2007，20(10):2272－2275.

［2］Zhu Rong，Sun Dong，Zhou Zhaoying.A Linear Fusion Algorithm for Attitude Determination Using Low Cost MEMS-Based Sensors［J］.Measurement，2007，40(3):3222－328.

［3］Gao Zhongyu，Niu Xiaoji，Guo Meifeng.Quaternion-Based Kalman Filter for Micro-Machined Strapdown Attitude Heading Reference system［J］.ChineseJournal of Aeronautics，2002，15(3):171－175.

［4］Gebre Egziabher D，HayWard R C，Powell J D.Design of Multi-Sensor Attitude Determination Systems［J］.IEEE Transactions on Aerospace and Electronic System，2004，40(2):627－649.

［5］Zhou Hongren，Kumar KSP.A Current Statistical Model and Adaptive Algorithm for Estimating Maneuvering Targets［J］.AIAA，Journal of Guidance，Control and Dynamics，1984，7(5):596－602.

［6］汪芳，朱少華，雷宏杰.基于卡爾曼濾波器的數(shù)字式捷聯(lián)航姿系統(tǒng)算法設(shè)計［J］.中國慣性技術(shù)學(xué)報，2008，16(2):208－211.

［7］郭鵬飛，任章.機(jī)動加速度輔助的航姿系統(tǒng)擴(kuò)展卡爾曼濾波［J］.系統(tǒng)工程與電子技術(shù)，2009，31(3):625－629.

［8］魏萍.基于ARM的嵌入式航向姿態(tài)參考系統(tǒng)的研究［D］.浙江大學(xué)碩士學(xué)位論文，2006:23－45.

［9］秦永元.慣性導(dǎo)航［M］.北京:科學(xué)出版社，2006:305－325.

［10］李秉璽，趙忠，孫照鑫.磁阻傳感器的捷聯(lián)式磁航向儀及誤差補(bǔ)償［J］.傳感技術(shù)學(xué)報，2003，6(2):191－194.

［11］傅建國，王孝通，馬野，等.自主姿態(tài)測量最優(yōu)算法研究［J］.航空學(xué)報，2005，26(5):594－597.

［12］牛爾卓，任建新，譚劍.捷聯(lián)慣導(dǎo)系統(tǒng)測試中仿真軌跡的設(shè)計與分析［J］.計算機(jī)仿真，2009(08):17－20.