基于卡爾曼濾波器的航姿系統測姿算法研究*

苑艷華，李四海，南 江

(西北工業大學自動化學院，西安 710129)

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

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

1 捷聯航姿系統工作原理

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

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

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

2 基于卡爾曼濾波的姿態融合算法

2.1 基于角運動信息的系統方程

根據對姿態角估計的問題可以間接轉化為對重力加速度在機體系的分量gb估計的思想，本文推出了基于角運動信息的系統方程。

對等式兩邊同時微分得:

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

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

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

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

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

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

2.3 系統離散化及姿態融合算法

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

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

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

當載體平穩飛行時，為 0，則

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

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

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

3 載體飛行狀態的判別

3.1 飛行狀態的判別準則

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

由上式可知，當飛機平穩飛行時fb的模值約等于g，因此以利用加速度計輸出信息設計飛行狀態的模值判別準則如下:

當|‖fb‖－g|≤Thlad時，認為載體處于非機動狀態，否則認為載體處于機動狀態，其中Thlad的取值取決于加速度的等效零位偏置和測量噪聲的大小。

3.2 模值判別準則誤判分析

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

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

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

4 仿真結果及分析

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

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

圖1 飛行軌跡的姿態角和加速度參數

圖2 本文算法的姿態角誤差

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

圖4 判別準則對飛行狀態的判別情況

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

5 結論

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

［1］徐玉，李平，韓波.一種面向機動的低成本姿態測量系統［J］.傳感技術學報，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］汪芳，朱少華，雷宏杰.基于卡爾曼濾波器的數字式捷聯航姿系統算法設計［J］.中國慣性技術學報，2008，16(2):208－211.

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

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

［9］秦永元.慣性導航［M］.北京:科學出版社，2006:305－325.

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

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

［12］牛爾卓，任建新，譚劍.捷聯慣導系統測試中仿真軌跡的設計與分析［J］.計算機仿真，2009(08):17－20.