INS／GPS組合導(dǎo)航的混合式濾波算法

韓斌子 胡柏青

摘要

本文從分析INS/GPS組合導(dǎo)航常用的直接式和間接式卡爾曼濾波算法的優(yōu)缺點(diǎn)入手，提出了兼有二者優(yōu)點(diǎn)的混合式濾波算法。仿真結(jié)果表明，混合式濾波算法的精度與直接式算法相當(dāng)，計(jì)算量與間接式相當(dāng)，集合了二者的優(yōu)點(diǎn)，具有良好的計(jì)算精度和適用性。

【關(guān)鍵詞】組合導(dǎo)航 卡爾曼濾波 直接式 間接式混合式

IN S/GPS組合是目前常用的組合導(dǎo)航模式，它充分吸收了GPS和INS各自的優(yōu)點(diǎn)，即GPS精度高、INS自主性好不易受到外界環(huán)境的影響;又克服它們各自的缺點(diǎn)，即GPS信號(hào)易受到干擾和遮擋而不穩(wěn)定、INS的誤差隨時(shí)間增加而累計(jì)導(dǎo)致精度下降。在GPS/INS組合導(dǎo)航中，一般使用卡爾曼濾波進(jìn)行GPS與INS的數(shù)據(jù)融合。卡爾曼濾波分為直接式卡爾曼濾波和間接式卡爾曼濾波法。直接式卡爾曼濾波，是以導(dǎo)航系統(tǒng)輸出參數(shù)作為狀態(tài)變量，卡爾曼濾波經(jīng)過(guò)計(jì)算，得到導(dǎo)航參數(shù)的最優(yōu)估計(jì)值;間接式卡爾曼濾波，是以導(dǎo)航系統(tǒng)輸出參數(shù)的誤差作為估計(jì)變量，卡爾曼濾波器經(jīng)過(guò)計(jì)算，得到各導(dǎo)航參數(shù)誤差量的最優(yōu)估計(jì)值。由于直接式狀態(tài)方程是非線性方程，需要進(jìn)行線性化，降低了解算精度，且計(jì)算容易發(fā)散，濾波器的設(shè)計(jì)也困難。而間接式是以導(dǎo)航系統(tǒng)輸出參數(shù)的誤差量作為狀態(tài)，計(jì)算得到誤差量的最優(yōu)估計(jì)，其系統(tǒng)方程是線性方程，濾波器設(shè)計(jì)簡(jiǎn)單且不存在發(fā)散問(wèn)題，但是，計(jì)算結(jié)果不能直接得到導(dǎo)航參數(shù)，需要進(jìn)行推算。

組合導(dǎo)航的濾波算法，關(guān)系到能否發(fā)揮組合導(dǎo)航中各個(gè)傳感器的優(yōu)勢(shì)，提高導(dǎo)航精度，提高導(dǎo)航系統(tǒng)可靠工作時(shí)間，滿足現(xiàn)代大型艦船高精度、長(zhǎng)航時(shí)的導(dǎo)航要求。目前，組合導(dǎo)航的濾波算法，已經(jīng)成為一個(gè)研究熱點(diǎn)，許多學(xué)者開展這方面的工作，包括H2/H。?；旌蠟V波、魯棒濾波、緊組合和深組合等。

本文分析了直接式和間接式卡爾曼濾波的優(yōu)點(diǎn)，提出了將兩者結(jié)合到一起的混合式濾波方法，在保持濾波計(jì)算精度不低于直接式濾波的前提下，降低了計(jì)算工作量，提高了計(jì)算效率。

1理論模型

1.1直接式濾波模型

選“東北天（ E-N-U）”地理坐標(biāo)系作為組合導(dǎo)航系統(tǒng)的導(dǎo)航參考坐標(biāo)系（n系），載體所固連的“右前上（ R-F-U）”坐標(biāo)系作為載體坐標(biāo)系（b系），地球坐標(biāo)系記為c系，慣性坐標(biāo)系記為i系。

則，姿態(tài)微分方程為：

其中：

wix為地球自轉(zhuǎn)角速率，L和h分別為地理緯度和高度。

速度微分方程為：

位置（經(jīng)度、緯度和高度）微分方程為：

姿態(tài)對(duì)應(yīng)的四元數(shù)微分方程為：

直接式濾波算法流程如圖1所示。1.2間接式濾波模型

記姿態(tài)誤差為φ，速度誤差為δvu，位置誤差為δp，則其所對(duì)應(yīng)的誤差方程分別為：

其中：

（16）

量測(cè)模型采用位置作為外界輔助信息的位置松組合，對(duì)應(yīng)的觀測(cè)模型為：

z（f）=Hx（t）+V（t）（17）

其中：

Z（t）=pINS（t）-PGNSS（t）

（18）

H=[03x6 I3x3 03x6]

（19）

V（t）表示量測(cè)噪聲，其與狀態(tài)噪聲Wb是相互獨(dú)立的。

由于上述模型是線性的，因此可直接采用卡爾曼濾波算法，具體流程如下。

狀態(tài)預(yù)測(cè)：

量測(cè)更新：

間接式濾波算法流程如圖2所示。1.3混合式濾波模型

混合式組合導(dǎo)航方案的核心在于姿態(tài)估計(jì)采用直接式濾波方案，速度、位置估計(jì)采用間接式濾波方案。

其狀態(tài)傳遞模型為：

式（25）所對(duì)應(yīng)的誤差傳遞矩陣為：

（27）

混合式濾波算法框架如圖3所示。

2仿真驗(yàn)證

采用一組仿真實(shí)驗(yàn)數(shù)據(jù)來(lái)比較不同組合方案的濾波效果。仿真運(yùn)動(dòng)軌跡如圖4所示。三種濾波方式的導(dǎo)航估計(jì)結(jié)果如圖5.圖7所示。

從圖中可以看出直接式濾波方案和混合式濾波方案估計(jì)效果非常接近，同時(shí)都優(yōu)于間接式濾波方案，尤其是在姿態(tài)估計(jì)結(jié)果上。在算法計(jì)算量上，間接式方案所需計(jì)算量最小，直接式方案所需計(jì)算量最大，接近于間接式方案的10倍。這是因?yàn)樵诮M合導(dǎo)航中計(jì)算量最大的部分在于導(dǎo)航解算，間接式方案只需一次導(dǎo)航解算，而直接式方案需要24次（每次需要采樣24個(gè)sigma點(diǎn)）。本文提出的混合式方案計(jì)算量是間接式的3倍，顯著低于直接式方案。因此，綜合考慮算法精度和計(jì)算量，本章所提出的混合式方案更具應(yīng)用優(yōu)勢(shì)。

3結(jié)語(yǔ)

本文針對(duì)INS/GPS組合導(dǎo)航中常用的兩種濾波方案：直接式和間接式卡爾曼濾波存在的問(wèn)題，提出了混合式卡爾曼濾波。仿真實(shí)驗(yàn)表明，混合式的估計(jì)效果與直接式接近，明顯優(yōu)于間接式，并且計(jì)算量適中，顯著低于直接式，因此綜合考慮算法精度和計(jì)算量，本文所提出的混合式組合導(dǎo)航方案更具應(yīng)用優(yōu)勢(shì)。

參考文獻(xiàn)

[1]秦永元，張洪鉞，汪叔華，卡爾曼濾波與組合導(dǎo)航原理[M].西安：西北工業(yè)大學(xué)出版社，1998： 238-275.

[2]胡建宇，侯書銘.UKF在INS/GPS組合導(dǎo)航直接法濾波中的應(yīng)用[J].計(jì)算機(jī)與數(shù)字工程，2 01 5，304 （02）： 252-255.

[3]王磊，張懷周.間接Kalman濾波器在航天器CNS/SINS組合導(dǎo)航中的應(yīng)用[J].測(cè)控技術(shù)，2 008，27 （03）： 88-90.

[4]易大江，王浩，賈坤.H一2/H一。?；旌蠟V波在GPS/DR組合導(dǎo)航中的應(yīng)用研究I[J].信號(hào)處理，2009，Vol. 25 （01）：104-108.

[5]易大江，王浩，賈坤.H_2/H_∞混合濾波在GPS/DR組合導(dǎo)航中的應(yīng)用研究II[J].信號(hào)處理，2 009，Vol. 25 （02）： 280-284.

[6]王融，熊智，劉建業(yè)等.高超音速飛行器組合導(dǎo)航魯棒濾波算法[J]，航空計(jì)算技術(shù)，2 01 3，Vol. 43 （05）：11-14.

[7]鐘麗娜，劉建業(yè)，李榮冰，高關(guān)根.INS/SFGPS-PPP緊組合系統(tǒng)動(dòng)態(tài)補(bǔ)償濾波算法[J]，儀器儀表學(xué)報(bào)，2016，Vol. 37 （06）：1283-1289.

[8]周衛(wèi)東，蔡佳楠，孫龍.GPS/SINS超緊組合導(dǎo)航系統(tǒng)自適應(yīng)混合濾波算法[J]，哈爾濱工業(yè)大學(xué)學(xué)報(bào)，2014，V01.46（07）：47-52.