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

（火箭軍工程大學(xué) 導(dǎo)彈工程學(xué)院，西安 710025）

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

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

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

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

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

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

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

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

2.1 姿態(tài)解算

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

將式(5)積分可得到載體的姿態(tài)角，但其中含有陀螺儀的漂移誤差，尤其當(dāng)載體處于靜止?fàn)顟B(tài)時(shí)，通過(guò)時(shí)間積分法得到的姿態(tài)角誤差尤為明顯[16]。Tseng[17]對(duì)陀螺儀、加速度計(jì)和磁強(qiáng)計(jì)的頻率響應(yīng)進(jìn)行了仿真，結(jié)果表明陀螺儀的頻率響應(yīng)特性與其他兩個(gè)傳感器的在本質(zhì)上是相反的，具有互補(bǔ)的特點(diǎn)。因此，為了補(bǔ)償陀螺儀漂移誤差帶來(lái)的影響，將加速度計(jì)與磁強(qiáng)計(jì)解算得到的姿態(tài)角，與式(5)得到的姿態(tài)角進(jìn)行融合，通過(guò)加速度計(jì)和磁強(qiáng)計(jì)輸出計(jì)算得到的姿態(tài)角方式表示如下：

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

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

2.2 線性互補(bǔ)濾波原理

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

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

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

2.3 自適應(yīng)姿態(tài)估計(jì)

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

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

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

式中，是第i個(gè)濾波器在k時(shí)刻的姿態(tài)估計(jì)，wi是k第i個(gè)濾波器在k時(shí)刻的計(jì)算權(quán)重。

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

式中，是由多模型自適應(yīng)線性互補(bǔ)濾波器估計(jì)的姿態(tài)角。每一個(gè)濾波器的權(quán)重因子通過(guò)姿態(tài)角殘差更新計(jì)算，濾波器的輸出結(jié)果為：

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

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

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

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

式(17)的離散化后即為式(9)。隨著量測(cè)值的更新遞推，當(dāng)某個(gè)濾波器的殘差較小時(shí)，對(duì)應(yīng)的濾波器權(quán)重系數(shù)較大，反之亦然。

3 模糊自適應(yīng)方案

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

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

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

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

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

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

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

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

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

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

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

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

通過(guò)模糊推理系統(tǒng)對(duì)載體運(yùn)動(dòng)狀態(tài)的評(píng)估，最終得到兩種姿態(tài)解算方案所對(duì)應(yīng)的權(quán)重值?1*和?2*，則MIMU/GPS組合導(dǎo)航系統(tǒng)的姿態(tài)輸出為：

式中，f(Q)表示濾波解算的姿態(tài)角，Ori表示MIMU/GPS導(dǎo)航系統(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 仿真驗(yàn)證

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

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

載體運(yùn)動(dòng)參照無(wú)人機(jī)等飛行器的實(shí)際飛行機(jī)動(dòng)形式，主要考慮靜止、加減速、爬升、平飛、轉(zhuǎn)彎、高度機(jī)動(dòng)、俯沖，其運(yùn)動(dòng)軌跡見(jiàn)圖4。

圖4 載體運(yùn)動(dòng)軌跡Fig.4 Vehicle plane motion trajectory

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

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

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

表3 速度和位置誤差統(tǒng)計(jì)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 結(jié) 論

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