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

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

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

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

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

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

1 MIMU/GPS緊組合導航系統(tǒng)算法

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

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

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

2 多模型自適應姿態(tài)解算方案

2.1 姿態(tài)解算

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

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

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

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

2.2 線性互補濾波原理

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

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

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

2.3 自適應姿態(tài)估計

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

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

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

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

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

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

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

根據(jù)貝葉斯理論有：

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

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

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

3 模糊自適應方案

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

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

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

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

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

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

模糊推理系統(tǒng)根據(jù)載體運動狀態(tài)程度，分別對應三個模糊集：?。⊿）、中（L）、大（M），并設計九條模糊規(guī)則如下：

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

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

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

則模糊系統(tǒng)的總體輸出為：

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

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

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

圖3 模糊系統(tǒng)輸出曲面Fig.3 Output surface of Fuzzy system

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

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

4 仿真驗證

仿真參數(shù)設置如下：初始位置為緯度 34.25°、經(jīng)度108.91°、高度380 m；初始速度和姿態(tài)角均為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，多模型自適應姿態(tài)估計方案的濾波器個數(shù)為m=10，每個濾波器輸出的初始權重取為0.1，濾波器參數(shù)服從均勻分布α～U(0.01,0.99)。

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

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

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

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

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

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

表3 速度和位置誤差統(tǒng)計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 經(jīng)度誤差Fig.12 Error of longitude

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

5 結 論

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