秦　嶺，劉晨曦

(武漢輕工大學(xué) 電氣與電子工程學(xué)院，湖北 武漢 430023)

?

反饋粒子濾波在GPS/INS組合導(dǎo)航系統(tǒng)中的應(yīng)用

秦嶺，劉晨曦

(武漢輕工大學(xué) 電氣與電子工程學(xué)院，湖北 武漢 430023)

摘要：GPS/INS組合導(dǎo)航系統(tǒng)的性能主要是由濾波器的精度和實(shí)時(shí)性決定，由于組合導(dǎo)航系統(tǒng)的狀態(tài)維數(shù)較高，導(dǎo)致濾波器的計(jì)算量大，因而制約了粒子濾波與高斯粒子濾波算法在實(shí)際工程中的應(yīng)用。針對慣性組合導(dǎo)航系統(tǒng)的特點(diǎn)，基于反饋粒子濾波算法，對位置/速度誤差進(jìn)行了濾波估計(jì)，同時(shí)利用組合濾波器的輸出對INS系統(tǒng)的誤差進(jìn)行修正，最后對相應(yīng)的算法進(jìn)行了仿真驗(yàn)證。仿真結(jié)果表明，與混合高斯粒子濾波算法相比，反饋粒子濾波算法的濾波精度和實(shí)時(shí)性都有一定的提高。

關(guān)鍵詞：組合導(dǎo)航系統(tǒng)；反饋粒子濾波；高斯粒子濾波

1引言

GPS/INS組合導(dǎo)航系統(tǒng)[1]針對GPS和INS的誤差特性，采用GPS的位置與速度信息對INS進(jìn)行周期性校正，有效的減少了系統(tǒng)誤差的影響，提高了導(dǎo)航系統(tǒng)的整體性能。目前，常用的信息融合算法是Kalman濾波算法[2]，在理想情況下，Kalman濾波效果較好，但是當(dāng)載體作大幅度機(jī)動(dòng)或者GPS信號受到干擾或遮擋時(shí)，Kalman濾波會產(chǎn)生較大的誤差，進(jìn)而導(dǎo)致濾波失敗?；贛onte Carlo采樣方法的粒子濾波(Particle Filter, PF)和高斯粒子濾波(Gaussian Particle Filter, GPF)算法可以有效的解決上述問題[3-6]，然而組合導(dǎo)航系統(tǒng)的狀態(tài)變量維數(shù)高，為了提高粒子的采樣效率，保證濾波收斂及其精度，必須采用更多數(shù)量的粒子來逼近概率密度函數(shù)，這就導(dǎo)致計(jì)算量急劇增加，制約了粒子濾波和高斯粒子濾波算法在組合導(dǎo)航系統(tǒng)中的應(yīng)用。為此，文獻(xiàn)[4]和[6]采用Kalman濾波分別結(jié)合粒子濾波與高斯粒子濾波算法，對組合系統(tǒng)的線性部分采用Kalman濾波進(jìn)行遞推估計(jì)，而對系統(tǒng)非線性部分則分別利用粒子濾波與高斯粒子濾波估計(jì)，有效的減少了濾波的計(jì)算量，加快了濾波的收斂速度。反饋粒子濾波(Feedback Particle Filter, FPF)[7]是一種新型的免重采樣非線性濾波算法，它將每個(gè)粒子都視為一個(gè)能控的隨機(jī)系統(tǒng)，通過求解Euler-Lagrange方程，得到在K-L距離最小意義下的最優(yōu)反饋控制律，進(jìn)而基于每個(gè)粒子的自身狀態(tài)進(jìn)行校正和更新。由于反饋粒子濾波不需要重采樣，因此有效的避免了因估計(jì)狀態(tài)維數(shù)的增加而導(dǎo)致的計(jì)算量增大，同時(shí)反饋粒子濾波實(shí)現(xiàn)簡單，實(shí)時(shí)性好，易于工程實(shí)際應(yīng)用。

本文主要對松耦合GPS/INS組合導(dǎo)航系統(tǒng)的位置信息融合進(jìn)行仿真研究，利用GPS觀測信息通過反饋粒子濾波算法修正INS的位置誤差信息，以提高導(dǎo)航精度。最后通過算例仿真表明，與混合高斯粒子濾波相比，基于反饋粒子濾波算法的GPS/INS組合導(dǎo)航系統(tǒng)精度較高，實(shí)時(shí)性較好，可以滿足實(shí)際的導(dǎo)航要求。

2GPS/INS松耦合濾波模型

取地理坐標(biāo)系為導(dǎo)航坐標(biāo)系，則慣性導(dǎo)航系統(tǒng)狀態(tài)變量定義為：

X(t)=[φeφnφuδveδvnδvuδL δλ δh

εbxεbyεbzεrxεryεrzbxbybzδt]T.

矩陣各元素分別代表坐標(biāo)系中INS在各方向的平臺角度誤差、速度誤差、位置誤差、陀螺儀漂移、一階馬爾可夫過程以及加速度計(jì)零漂等，共計(jì)18維行向量。同時(shí)，由于采用GPS的量測信息作為INS的修正信息，因此將GPS的接收機(jī)鐘差信息擴(kuò)充到組合導(dǎo)航系統(tǒng)的狀態(tài)向量。進(jìn)而組合導(dǎo)航系統(tǒng)的誤差系統(tǒng)狀態(tài)方程可以表示為：

(1)

其中系統(tǒng)動(dòng)態(tài)矩陣F(t)和過程誤差系數(shù)矩陣G(t)的非零元素詳見文獻(xiàn)[8]。

將INS的輸出位置信息與GPS接收機(jī)計(jì)算出的相應(yīng)位置信息進(jìn)行相減，其差值作為組合導(dǎo)航系統(tǒng)的量測信息，同時(shí)將量測噪聲看為高斯白噪聲，則組合導(dǎo)航系統(tǒng)的量測方程為：

Z(t)=H(t)X(t)+v(t).

(2)

3反饋粒子濾波

假設(shè)系統(tǒng)狀態(tài)方程和量測方程分別為式(1)和式(2)，且估計(jì)狀態(tài)的后驗(yàn)概率密度函數(shù)服從高斯分布，即pt～N(x;μt,Pt)，于是反饋增益矩陣Kt可以由Euler-Lagrange方程解得

(3)

并且滿足以下邊界條件

(4)

(5)

(6)

在算法具體實(shí)現(xiàn)時(shí)，可以采用樣本粒子的均值和方差對其進(jìn)行估計(jì)。即

(7)

(8)

此時(shí)，對第i個(gè)粒子的迭代方程(4)可以改寫為：

(9)

由此，可以給出基于反饋粒子濾波的組合導(dǎo)航濾波算法步驟如下：

Step2. 假設(shè)每次迭代采樣時(shí)間間隔為Δt，則在從t→t+Δt時(shí)，計(jì)算一步預(yù)測均值，即

(10)

Step3.對每一個(gè)樣本粒子分別按下列式子計(jì)算新息誤差和反饋增益；

(11)

(12)

其中樣本粒子的均值和協(xié)方差可以分別按式(7)和式(8)進(jìn)行統(tǒng)計(jì)。

Step4隨機(jī)采樣ΔV～N(0,1)，然后對每一個(gè)樣本粒子按照下式進(jìn)行校正更新。

(13)

然后令t=t+Δt，返回Step2進(jìn)行下一次迭代，更新各個(gè)粒子值。

4算例與分析

取一組GPS/INS導(dǎo)航數(shù)據(jù)進(jìn)行仿真實(shí)驗(yàn)。仿真參數(shù)設(shè)置如下：設(shè)慣性導(dǎo)航系統(tǒng)的陀螺隨機(jī)常值為0.1(°)/h，陀螺儀一階馬爾可夫過程驅(qū)動(dòng)白噪聲為0.1(°)/h，相關(guān)時(shí)間為500 s；加速度計(jì)一階馬爾柯夫過程驅(qū)動(dòng)白噪聲為1×10-3g，相關(guān)時(shí)間常數(shù)1 000 s。GPS接收機(jī)位置白噪聲30 m，速度白噪聲1 m/s，仿真時(shí)間為50 s，采樣時(shí)間為Δt=0.1 s。

基于Monte Carlo方法的粒子濾波和高斯粒子濾波算法的濾波精度是隨著粒子數(shù)目的增加而增大的，估計(jì)狀態(tài)的維數(shù)越高，需要的粒子數(shù)目就越多，濾波算法的計(jì)算量也就越大，而反饋粒子濾波算法由于不需要在每次迭代時(shí)對粒子進(jìn)行采樣。只需要計(jì)算粒子的反饋增益，因此有效的減少了算法的復(fù)雜度，在保證濾波收斂的同時(shí)也提高了算法的精度。因此，為了評價(jià)反饋粒子濾波算法的性能，在仿真中對同一組測試數(shù)據(jù)分別利用高斯粒子混合濾波算法和反饋粒子濾波算法進(jìn)行組合導(dǎo)航系統(tǒng)的信息融合，粒子數(shù)目分別選取為N=1 000和N=10 000兩種情況進(jìn)行比較。

圖1—2給出了1 000個(gè)粒子情況下，GPF和FPF算法的位置和速度在各方向的誤差曲線圖。由圖可以看出，GPF和FPF算法的位置誤差曲線均能收斂，其中FPF濾波后各個(gè)方向的位置誤差均減小到約20 m以內(nèi)，濾波精度明顯均優(yōu)于GPF濾波后各個(gè)方向的位置誤差。兩種算法對于垂直方向的速度誤差都是發(fā)散的，但是FPF濾波后水平方向的速度誤差均減小至1 m/s以內(nèi)，濾波精度也較GPF更高。同時(shí)，表1給出了兩種算法在不同情況下的運(yùn)行時(shí)間，由此看出，F(xiàn)PF的實(shí)時(shí)性也優(yōu)于GPF，表明FPF在粒子數(shù)目較少的情況下，不僅保證了濾波精度，而且提高了導(dǎo)航系統(tǒng)的實(shí)時(shí)性。

圖1　位置誤差曲線圖(N=1 000)

圖2　速度誤差曲線圖(N=1 000)

圖3　位置誤差曲線圖(N=10 000)

圖4　速度誤差曲線圖(N=10 000)

圖3—4給出了10 000個(gè)粒子情況下，GPF和FPF的位置和速度在各方向的誤差曲線圖。由圖可以看出，對于GPF，水平方向的位置誤差雖然可以收斂至20 m范圍以內(nèi)，但是波動(dòng)大，精度較差，速度誤差也呈現(xiàn)發(fā)散趨勢。而經(jīng)過反FPF融合后，組合導(dǎo)航系統(tǒng)的位置誤差均能收斂在20 m以內(nèi)，同時(shí)水平方向的速度誤差均能減小至1 m/s以內(nèi)，與GPF相比，曲線變化更為平緩。由此可見，在相同粒子數(shù)目的情況下，采用FPF實(shí)現(xiàn)GPS/INS組合導(dǎo)航系統(tǒng)的位置和速度信息融合，其誤差精度均優(yōu)于采用GPF濾波算法。并且，當(dāng)粒子數(shù)量減少后，F(xiàn)PF濾波精度雖然有所降低，但是提高了組合系統(tǒng)的實(shí)時(shí)性。

表1FPF和GPF運(yùn)行時(shí)間比較

數(shù)目100010000算法FPFGPFFPFGPF時(shí)間/s5.94599918.16729246.891639178.200659

5結(jié)論

本文基于反饋粒子濾波算法，對GPS/INS松組合導(dǎo)航系統(tǒng)進(jìn)行了信息融合。并將濾波結(jié)果進(jìn)行對比分析，仿真結(jié)果表明，該方法可以在粒子數(shù)目較少的情況下，較為準(zhǔn)確的估計(jì)慣性導(dǎo)航系統(tǒng)的誤差信息，并且隨著粒子數(shù)目的增加，濾波精度會有所提高，但是實(shí)時(shí)性降低。與高斯粒子濾波混合算法相比，反饋粒子濾波不需要進(jìn)行重采樣，計(jì)算量小，易

于實(shí)現(xiàn)，在保證濾波精度的同時(shí)，也能有效的克服由于組合導(dǎo)航系統(tǒng)維數(shù)過高而導(dǎo)致粒子數(shù)目增加濾波計(jì)算量增大的缺點(diǎn)，更加適用于組合導(dǎo)航系統(tǒng)信息的實(shí)時(shí)融合。

參考文獻(xiàn):

[1]Casper E S. INS and GPSintegration[D]. Lyngby: Technical University of Denmark, 2006: 60-72.

[2]鮑其蓮,周媛嬡. 基于UKF的GPS/SINS偽距(偽距率)組合導(dǎo)航系統(tǒng)設(shè)計(jì)[J]. 中國慣性技術(shù)學(xué)報(bào), 2008, 16(1): 78-82.

[3]周翟和,劉建業(yè),賴際舟,等. Rao-Blackwellized粒子濾波在SINS/GPS深組合導(dǎo)航系統(tǒng)中的應(yīng)用研究[J]. 宇航學(xué)報(bào), 2009(2): 515-520.

[4]袁俊剛,范勝林,劉建業(yè),等. 卡爾曼/粒子組合濾波在GPS/INS組合導(dǎo)航中的應(yīng)用研究[J].導(dǎo)航與控制, 2010,09(4): 37-40．

[5]周翟和,劉建業(yè),賴際舟,等．混合高斯粒子濾波在組合導(dǎo)航中應(yīng)用的計(jì)算量分析[J]．中國慣性技術(shù)學(xué)報(bào), 2010, 18(5): 595-599．

[6]熊劍,郭杭,熊智,等. GPS/INS組合導(dǎo)航系統(tǒng)中的高斯粒子濾波混和算法[J]. 中國慣性技術(shù)學(xué)報(bào),2012,20(2): 225-229．

[7]Yang T, Mehta P G,Meyn S P.Feedback particle filter[J]. IEEE Trans. onAutomatic Control, 2013, 58(10): 2465-2480.

[8]王惠南. GPS導(dǎo)航原理與應(yīng)用[M]. 北京: 科學(xué)出版社, 2003．

Usage of Feedback Particle Filter algorithm in

GPS/INS integrated navigation system

QINLing,LIUChen-Xi

(School of Electrical and Electronic Engineering, Wuhan Polytechnic University, Wuhan 430023,China)

Abstract：The performance of GPS/INS integrated navigation system will directly be determinedby the precision and real-time performance of the filters.The dimension of the integrated navigation system is so high that the filter needs the large amount of computation. The result is that the application of particle filter and the Gaussian particle filter algorithms is restricted in practice. According to the characteristics of integrated navigation system, the errors of position and velocity are estimated based on feedback particle filter.And the use of the integration method allows the integration navigation system correcting the errors of INS. At last the filter algorithm is verified by simulation. The simulation results show that the precision and real-time performance of feedback particle filter can be improved, compared with those of Gaussian particle hybrid filter.

Key words：integrated navigation system; Feedback Particle Filter; Gaussian Particle Filter

基金項(xiàng)目：國家自然科學(xué)基金資助項(xiàng)目(61179032).

DOI:10.3969/j.issn.2095-7386.2015.04.016 10.3969/j.issn.2095-7386.2015.04.015

文章編號：2095-7386(2015)04-0065-03 2095-7386(2015)04-0060-05

通信作者：郭宏磊(1964-)，男，博士，教授，E-mail:ghl15186@qliyun.com.

作者簡介：徐偉(1990-)，男，碩士研究生，E-mail:673407190@qq.com. 王防修(1973-)，男，副教授，E-mail:wfx323@126.com.

收稿日期：2014-12-05.修回日期：2015-04-16. 2015-11-12.

中圖分類號：U 666.1

文獻(xiàn)標(biāo)識碼：A