基于卡爾曼濾波器的航姿系統(tǒng)測(cè)姿算法研究*

苑艷華，李四海，南 江

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

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

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

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

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

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

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

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

2.1 基于角運(yùn)動(dòng)信息的系統(tǒng)方程

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

對(duì)等式兩邊同時(shí)微分得:

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

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

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

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

2.2 基于比力信息的量測(cè)方程

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖3 本文算法的陀螺漂移估計(jì)值

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

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

5 結(jié)論

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

［1］徐玉，李平，韓波.一種面向機(jī)動(dòng)的低成本姿態(tài)測(cè)量系統(tǒng)［J］.傳感技術(shù)學(xué)報(bào)，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ì)［J］.中國慣性技術(shù)學(xué)報(bào)，2008，16(2):208－211.

［7］郭鵬飛，任章.機(jī)動(dòng)加速度輔助的航姿系統(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é)報(bào)，2003，6(2):191－194.

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

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