基于自適應姿態估計的MIMU/GPS緊組合導航算法

單 斌，張復建，楊 波，王躍鋼，薛 亮，騰紅磊

（火箭軍工程大學 導彈工程學院，西安 710025）

由于低成本的微型慣性測量單元（Miniature Inertial Measurement Units,MIMU，簡稱“微慣組”）的漂移和零偏誤差較大，基于MIMU的慣性導航系統誤差隨時間不斷發散。為解決這個問題，許多學者采用衛星與慣導進行組合導航[1-2]，抑制誤差發散。由于松組合模式簡單，因而受到工程上的廣泛使用，然而，當可見衛星數量少于4顆時，松組合模式系統不能正常工作，沒有GPS系統穩定的輸出對微慣導系統輸出進行修正，使得MIMU/GPS松組合導航系統變成純慣導解算，長時間的導航精度難以滿足實際需要。也有不少學者研究了慣性/衛星組合導航系統在缺星情況下的導航方法以及改進的卡爾曼濾波算法[3-4]。

在可見衛星數少于4顆時，MIMU/GPS緊組合系統的可觀性會有所降低，尤其是當載體大角度轉向時，姿態角誤差明顯增大，甚至引起速度誤差的增大，降低組合導航系統的性能[5]。針對此類問題，Qifan Zhou[6]采用高度和航向角信息輔助緊組合導航系統，提出一種新的冗余測量噪聲估計方法應用到自適應卡爾曼濾波中。Jaradat[7]則提出自回歸的神經網絡數據融合用于低成本的慣組/衛星組合導航系統，增加姿態角誤差觀測量，提高組合導航系統的可觀性[8-9]，可以有效地提高組合導航系統性能。然而，采用GPS系統的姿態測量來構造系統姿態誤差觀測量[10]容易受到信號干擾而降低其測姿精度。也可以采用基于加速度計輸出的方式[11]，反解出載體的水平姿態角，達到增加姿態角誤差觀測量而提高系統可觀性的目的，不足之處是當載體處于非重力加速運動狀態時，加速度計輸出包含了非重力加速度項，雖然可以作近似補償修正，但是反解出的姿態角精度隨著載體運動加速度的變化而降低，從而導致了系統的姿態角觀測量不準。而卡爾曼濾波算法或其他濾波算法均是以觀測信息對狀態量實現最小方差估計，當觀測信息不準時，系統濾波精度下降[12-14]。由此可見，增加姿態角誤差觀測信息時，系統導航性能與載體的運動狀態及環境相關。

針對上述存在的問題，本文提出基于多模型的姿態自適應估計，來輔助MIMU/GPS緊組合導航系統，設計智能模糊推理系統，根據微慣組輸出數據信息，識別載體運動狀態，自動分配多模型自適應估計的姿態和濾波算法解算的姿態權重，將二者進行姿態融合，并進行相關實驗驗證。

1 MIMU/GPS緊組合導航系統算法

通常，MIMU慣性導航解算方案是對導航參數的微分方程采取積分方法實現更新，MIMU導航參數微分方程具體如下：

式中，Vn=[VVV]T為載體在導航系的速度，LE、λN、hU分別為載體的緯度、經度、高度，Q為姿態四元數，ωine=[0,ωiec osL,ωiesinLT]，ωibb為陀螺儀的輸出信息，為地球自轉角速率，，g為重力加速度，為加速度計輸出。

很多文獻對 MIMU/GPS緊組合系統狀態空間模型的描述較為詳細[15]，在此不再贅述。MIMU/GPS緊組合導航系統的偽距、偽距率誤差觀測量表示如下：

2 多模型自適應姿態解算方案

2.1 姿態解算

歐拉角微分方程表示如下：

將式(5)積分可得到載體的姿態角，但其中含有陀螺儀的漂移誤差，尤其當載體處于靜止狀態時，通過時間積分法得到的姿態角誤差尤為明顯[16]。Tseng[17]對陀螺儀、加速度計和磁強計的頻率響應進行了仿真，結果表明陀螺儀的頻率響應特性與其他兩個傳感器的在本質上是相反的，具有互補的特點。因此，為了補償陀螺儀漂移誤差帶來的影響，將加速度計與磁強計解算得到的姿態角，與式(5)得到的姿態角進行融合，通過加速度計和磁強計輸出計算得到的姿態角方式表示如下：

式中，m=[mx,my,mz]T為磁強計輸出。值得注意的是，在計算載體的姿態角時，考慮到載體前進方向時的非重力加速度對姿態解算產生主要影響，忽略其余次要因素，因此式(6)中的fyb作如下近似補償：

式中，符號 [v](2)表示向量v的第2個元素；表示微慣組采樣頻率；vin表示在GPS接收信號歷元間隔內，微慣組導航解算的第i個三維速度向量值；L表示在相鄰兩個歷元間，經過微慣組采樣值計算載體速度參數的個數。

2.2 線性互補濾波原理

當原始信號分別被高頻和低頻噪聲干擾時，其中一個傳感器輸出為高頻干擾，另一個為低頻干擾，這時分別將兩個傳感器經過低通和高通濾波器，可以得到原始信號較為理想的估計值。這種將具有頻率互補特性的傳感器輸出信息進行融合是一種常用的信息融合技術。

假設y=(θ,γ,ψ)為運動載體的姿態角信息，=(,,)是由陀螺儀計算的姿態角速率，=(θ,γ,ψ)是由加速度計和磁強計計算的姿態角，則由線性互補濾波器原理可以給出載體姿態角的估計值如下：

由于參數α∈[0,1]，且通常保持常值不變，因此為了得到更好的濾波結果，線性互補濾波器一般被非線性互補濾波器或者自適應非線性互補濾波器所取代，尤其當傳感器具有時變偏差或系統具有強非線性時，線性互補濾波算法不能提供可靠的姿態估計。然而，線性互補濾波器的實現結構簡單，在工程上容易實現，具有潛在的較大優勢。

2.3 自適應姿態估計

在很多自適應卡爾曼濾波算法中，基于自適應多模型估計方案廣泛用于修正噪聲矩陣，其主要原理是將多個不同參數的濾波器輸出結果劃分權重，依據每個濾波器輸出的條件概率進行融合。

為充分利用線性互補濾波器的優勢，本文提出一種多模型的自適應方案，其多模型的含義如下：通過設置參數αi(i=1,2,…,m)，由式(8)得到k時刻y?的估計值(αi)，則k時刻由m個不同參數可得到對應的m個姿態估計值，最終k時刻的姿態估計值融合計算式為

式中，(αi)是第i個帶有參數αi的濾波器輸出結果，是由加速度計/磁強計估計的姿態角，m是多模型的濾波器個數，p(αi|zk*)是第i個濾波器在k時刻的姿態估計權重。該權重值(p(αizk*)=wki)通過如下迭代方程更新：

式中，是第i個濾波器在k時刻的姿態估計，wi是k第i個濾波器在k時刻的計算權重。

為便于推導式(9)，定義姿態角殘差為：

式中，是由多模型自適應線性互補濾波器估計的姿態角。每一個濾波器的權重因子通過姿態角殘差更新計算，濾波器的輸出結果為：

式中，y是系統狀態；zk*表示量測值；p(y|zk*)表示在已知zk*的條件下，y的條件概率函數。當條件概率取決于參數α和zk*時，式(13)可以改寫為：

根據貝葉斯理論有：

式中，Ak是殘差rk的協方差矩陣。式(16)即為計算每個子濾波器的權重系數。

將式(15)帶入到式(14)中，得到：

式(17)的離散化后即為式(9)。隨著量測值的更新遞推，當某個濾波器的殘差較小時，對應的濾波器權重系數較大，反之亦然。

3 模糊自適應方案

當載體處于靜止，勻速或者大角度勻速轉向（或近似勻速轉向）運動狀態時，相比于MIMU/GPS緊組合解算，多模型的姿態解算方法能提供更為精確的姿態信息，此時，多模型的姿態估計數據可以提高 MIMU/GPS系統的導航性能；當載體處于非重力直線加速或近似直線加速狀態時，在多模型的姿態估計中，由于忽略了加速度計所測量的載體非重力次要加速度項，多模型的姿態估計精度下降，此時，MIMU/GPS緊組合解算的姿態精度較高，應當占據更高的權重。為了達到上述目的，合理分配二者的權重大小，載體的運動狀態首先需要明確知道，這是提高MIMU/GPS系統導航性能的關鍵。

本文設計的模糊推理系統主要包含四個過程：定義輸入變量；輸入變量模糊化；通過模糊規則解算輸出變量；輸出變量去模糊化。

基于上述思想，本文通過模糊邏輯原理，構建一種模糊推理算法，根據載體的運動狀態，在MIMU/GPS緊組合導航解算的姿態和多模型自適應解算的姿態之間進行智能決策，確定二者的權重大小，從而達到自適應的提高MIMU/GPS組合導航的性能。其總體方案設計框圖見圖1。

圖1 MIMU/GPS組合導航系統總體方案Fig.1 Overall scheme of MIMU/GPS integrated navigation system

根據微慣組的量測信息，可以判斷載體的運動狀態，則模糊邏輯推理系統的輸入定義為：

為有效評估 MIMU/GPS緊組合導航解算的姿態和多模型自適應估計的姿態信息，定義模糊推理系統的輸出為和，分別用于評估二者的信息融合權重。在上述自適應推理思想下，基于IF-THEN規則設計輸入變量和輸出變量的模糊集，，。在模糊集和中，和分別表示模糊推理系統輸入和輸出參數的數量，表示模糊規則數量，表示一種符號標志，即隸屬函數所對應的模糊集，因此符號和 分別表示輸入和輸出變量所對應的隸屬函數值。

模糊推理系統根據載體運動狀態程度，分別對應三個模糊集：小（S）、中（L）、大（M），并設計九條模糊規則如下：

當模糊系統輸入量接近零時，表明載體處于靜止或者勻速運動狀態；當模糊系統輸入量的大小增加時，表明載體處于變速機動或者非重力加速狀態。相關隸屬函數如圖2所示。

在每條規則中，分別計算輸入和輸出變量所對應模糊集的隸屬函數值：

圖2 輸入、輸出變量隸屬函數Fig. 2 Membership function of input variables

則模糊系統的總體輸出為：

式中，nr表示非零的輸出變量隸屬函數值個數。由質心法得到輸出變量的去模糊化結果（見圖3，部分輸入、輸出變量系數關系見表1和表2）為：

通過模糊推理系統對載體運動狀態的評估，最終得到兩種姿態解算方案所對應的權重值?1*和?2*，則MIMU/GPS組合導航系統的姿態輸出為：

式中，f(Q)表示濾波解算的姿態角，Ori表示MIMU/GPS導航系統最終輸出的姿態角。

圖3 模糊系統輸出曲面Fig.3 Output surface of Fuzzy system

表1 輸出變量?1Tab.1 Output variable ?1

表2 輸出變量?2Tab.2 Input variable ?2

4 仿真驗證

仿真參數設置如下：初始位置為緯度 34.25°、經度108.91°、高度380 m；初始速度和姿態角均為0，初始速度誤差均為0.1 m/s；三個方向的初始失準角均為 1°；陀螺儀常值漂移為 5(°)/h，隨機游走誤差為0.5(°)/h1/2，加速度計常值零偏誤差為1 mg/h，隨機游走誤差為0.1 mg/Hz1/2，GPS接收機時鐘誤差的等效距離誤差為10 m，時鐘等效距離率誤差為0.01 m/s，MIMU采樣頻率為50 Hz，GPS接收機更新頻率為1 Hz，多模型自適應姿態估計方案的濾波器個數為m=10，每個濾波器輸出的初始權重取為0.1，濾波器參數服從均勻分布α～U(0.01,0.99)。

為了驗證本文算法的有效性，將引言部分提到的方法作為實驗方案的對照試驗，假設可見衛星數量為 3顆的情況下，基于以下 4種實驗方案，對MIMU/GPS組合導航系統進行驗證：1）方案一，偽距+偽距率的方案；2）方案二，航向角+偽距+偽距率的方案；3）方案三，全姿態+偽距+偽距率的方案；4）方案四，本文提出的方案。

載體運動參照無人機等飛行器的實際飛行機動形式，主要考慮靜止、加減速、爬升、平飛、轉彎、高度機動、俯沖，其運動軌跡見圖4。

圖4 載體運動軌跡Fig.4 Vehicle plane motion trajectory

基于上述實驗方案，當有效衛星數為 3顆時，4種方案均能有效解算載體的位置，表明緊組合方案在可見衛星數低于4顆時，依然能實現導航解算。當載體處于水平機動轉向時，方案一導航解算時會出現較大的誤差，尤其是姿態角誤差受影響變化較大，對此，增加航向角和全姿態的觀測信息，分別見方案二和方案三，有效改善了姿態角誤差大的問題。然而，無論是增加航向角信息觀測，還是增加全姿態信息觀測，在一定程度上都能提高 MIMU/GPS組合導航系統的可觀測性，但是當載體處于大角度變向運動時，系統姿態觀測信息的準確性出現下滑，導致方案二和方案三解算的姿態角誤差出現較大波動，系統魯棒性不強，

見圖5～7，且載體速度誤差也出現較大波動，最大速度誤差在1 m/s以上，見圖8～10，載體位置誤差波動變化不顯著，見圖11～13。在此，按照式(24)定義載體速度誤差和位置誤差，其誤差統計結果見表3。

從方案二和方案三的導航誤差估計結果來看，增加航向角或全姿態角的觀測信息，在可見衛星數少于4顆時，可以有效提高MIMU/GPS系統的導航性能，但是當載體處于大角度變向運動時，方案三不如方案二，尤其是載體的水平速度誤差受影響較大，因此基于加速度計輸出解算的水平姿態角精度與載體運動狀態直接相關，進而直接決定水平姿態觀測信息的可靠程度。針對此問題，本文所提出的方案是通過建立多模型的自適應姿態估計方案，根據微慣組輸出信息來預判載體的運動狀態，通過模糊規則，自適應的分配相應權重，進一步實現載體的高精度位置和速度輸出，提高MIMU/GPS組合導航系統的總體性能。本文提出方法解算的姿態角誤差在15′之內，速度誤差在0.1 m/s之內，位置誤差在5 m之內。

表3 速度和位置誤差統計Tab.3 Error statistics of velocity and position

圖5 俯仰角誤差Fig.5 Error of pitch angle

圖6 橫滾角誤差Fig.6 Error of roll angle

圖7 航向角誤差Fig.7 Error of yaw angle

圖8 東向速度誤差Fig.8 Velocity error of eastward

圖9 北向速度誤差Fig.9 Velocity error of northward

圖10 天向速度誤差Fig.10 Velocity error of upward

圖11 緯度誤差Fig.11 Error of latitude

圖12 經度誤差Fig.12 Error of longitude

圖13 高度誤差Fig.13 Error of height

5 結 論

本文針對 MIMU/GPS組合導航過程中衛星數低于4顆時的情況，為了提高組合導航系統性能，保證系統持續長時間輸出高精度的導航結果，分析對比了基于姿態觀測信息輔助的緊組合導航方法的局限性，提出了基于多模型自適應姿態估計輔助的模糊智能緊組合導航方法，通過實驗驗證了本文算法的有效性，并且該方法不受載體大角度轉向運動狀態的影響，能夠持續輸出高精度的導航信息。