劉繁明，唐英麗

哈爾濱工程大學自動化學院，黑龍江哈爾濱150001

差分進化粒子濾波在慣性/重力組合導航中的應用研究

劉繁明，唐英麗

哈爾濱工程大學自動化學院，黑龍江哈爾濱150001

針對常規(guī)粒子濾波在慣性/重力組合導航中的粒子退化和樣本貧化問題，將差分進化引入到粒子濾波的重采樣過程，通過群體差異進行全局搜索實現(xiàn)粒子分布優(yōu)化，使粒子集提高對系統(tǒng)狀態(tài)變量后驗概率的逼近程度。算法應用于慣性/重力組合導航中，采用慣性導航系統(tǒng)海上試驗數(shù)據(jù)進行數(shù)值實驗，對比分析不同算法在相同條件下的導航誤差，證明了算法能夠提高組合導航的定位精度。

慣性/重力組合導航;重力匹配;粒子濾波;差分進化;重采樣;粒子退化

網(wǎng)絡出版地址: http://www.cnki.net/kcms/detail/23.1191.U.20150727.1037.009.html

水下潛器在長時間航行過程中，由于慣性導航系統(tǒng)(inertial navigation system，INS)的位置誤差隨時間累積，需要定期校正或重調(diào)才能保證載體的定位精度。傳統(tǒng)意義上的方法，如利用外部的天文導航信息、無線電導航信息、衛(wèi)星導航信息等定期對INS的漂移進行校正要求潛器接近或浮出水面，降低了潛器的隱身性能［1］。地球重力場是地球固有的物理特性之一，其空間分布不均，存在一個變化的拓撲。鑒于重力場信息的無源性、全天候和基本不受外界干擾等優(yōu)點，采用重力場信息實現(xiàn)組合導航時，潛器無需浮出水面發(fā)射或接收信號，具有良好隱蔽性［2-3］。與此同時，衛(wèi)星測高、航空重力測量等技術(shù)的快速發(fā)展也給高精度慣性/重力組合導航提供了可能性［4］。

慣性/重力組合導航的基本原理是通過某種策略實現(xiàn)由慣性導航系統(tǒng)指示的位置向真實位置的逼近。對于粒子濾波在定位［5-7］、目標跟蹤［8］等非線性、非高斯問題的最優(yōu)狀態(tài)估計中的應用，國內(nèi)外專家學者進行了大量的探索、研究。對于傳統(tǒng)粒子濾波，引入了重采樣過程以解決經(jīng)典蒙特卡洛方法存在的粒子退化問題，但同時也由于粒子多樣性的降低，使得現(xiàn)有粒子不能有效表達后驗概率密度函數(shù)［9］。將群智能方法與粒子濾波相結(jié)合可以優(yōu)化搜索過程，增加粒子多樣性，達到提高粒子濾波性能的目的［10-12］。差分進化算法是一種基于群體差異實現(xiàn)全局優(yōu)化的進化計算方法，其原理相對簡單，易于理解和實現(xiàn)，且其表現(xiàn)出來的高可靠性和良好優(yōu)化性能，已成為進化計算領(lǐng)域一個重要研究方向［13-14］。文中提出一種基于差分進化的粒子濾波算法，通過引入差分進化，使各粒子間的信息形成共享，利用個體粒子間的差分向量對粒子集進行擾動實現(xiàn)全局搜索，改善樣本貧化問題，提高傳統(tǒng)粒子濾波的精度。將算法用于慣性/重力組合導航中，數(shù)值實驗證明，算法能夠獲得較高的定位精度。

1　慣性重力匹配導航基本原理

慣性/重力匹配導航是一種利用高分辨率的重力數(shù)據(jù)庫所提供的重力場數(shù)據(jù)對慣性導航系統(tǒng)進行校正，從而獲取載體位置信息的導航技術(shù)。其基本原理如圖1所示。

潛器在航行過程中，根據(jù)慣性導航系統(tǒng)指示的載體位置在數(shù)據(jù)庫中找到相應的重力場參量(重力異?；蛑亓μ荻?。與此同時，重力儀(或重力梯度儀)實時測量當前位置的重力場數(shù)據(jù)，將實時測量數(shù)據(jù)與數(shù)據(jù)庫數(shù)據(jù)經(jīng)濾波算法解算求得最佳匹配位置，從而獲得載體的精確位置信息，并利用該位置作為反饋信息實現(xiàn)慣導系統(tǒng)誤差校正。根據(jù)慣導誤差方程，取系統(tǒng)狀態(tài)為

圖1　慣性/重力匹配組合導航系統(tǒng)原理示意

式中:δφ，δλ為緯度和經(jīng)度誤差;δvx，δvy為東西和北向速度誤差;α，β，γ為水平和方位誤差;ΔAx，ΔAy為加速度計零位誤差;εx，εy，εz為陀螺漂移。則慣性/重力組合導航的系統(tǒng)狀態(tài)方程如式(1) :

式中: I為單位陣; 0為零矩陣; A2×4、A5×4和A5×3參見文獻［15］，在此不再贅述。矩陣B=I12×12為單位陣，系統(tǒng)噪聲ω均為零均值高斯白噪聲。

對于上述系統(tǒng)，直接取重力異常值作為系統(tǒng)觀測，由于重力異常觀測值與載體真實位置具有對應關(guān)系，因此慣性/重力組合導航的觀測方程為

y(t)=Γ(φ，λ )+ v(t)

式中: y(t)為重力異常觀測值;(φ，λ )為此時載體真實位置，Γ(φ，λ )為真實位置處重力異常值。v(t)為觀測噪聲。

2　基于差分進化的粒子濾波算法

2.1基本粒子濾波算法

粒子濾波算法通過使用蒙特卡洛方法實現(xiàn)遞推貝葉斯濾波，通過尋找一組在狀態(tài)空間中傳播的帶權(quán)值的隨機樣本(粒子)逼近狀態(tài)變量的后驗概率密度，每個樣本代表系統(tǒng)的一個可能狀態(tài)。在k時刻，粒子濾波通過預測采樣獲得新粒子集:

則可以用粒子集中帶權(quán)值的粒子來近似表示該時刻的后驗概率密度:

在當前時刻粒子權(quán)重被更新后，通過引入重采樣過程改善粒子退化現(xiàn)象，但是，重采樣過程降低了粒子的多樣性。因此，需要確定有效的粒子數(shù)為

2.2差分進化算法

差分進化(differntial evolution，DE)算法是由Rainer Storn和Kenneth Price為求解切比雪夫多項式于1996年提出的一種基于群進化采用浮點矢量編碼在連續(xù)空間進行全局搜索的優(yōu)化算法［13-14］。DE算法借助于種群個體之間的差分信息對個體形式擾動進而完成對整個種群的搜索，實現(xiàn)種群內(nèi)信息共享并利用貪婪競爭機制選擇下一代個體，尋找最優(yōu)解。DE算法的基本原理與遺傳算法類似，差別在于DE算法是利用種群個體間的差分向量進行變異，從種群的角度模擬生物進化機制，充分利用了不可行解所攜帶的互補信息，減小了陷入局部最優(yōu)解的可能性。DE算法的具體流程如下:

1)種群初始化

2)變異操作

變異操作是將經(jīng)過縮放的差分向量疊加到基向量上來實現(xiàn)的，差分進化通過變異操作來生成新的個體，變異操作決定了差分進化算法的搜索能力?；蛄颗c差分向量的選擇方式?jīng)Q定了進化策略。根據(jù)慣性/重力匹配組合導航問題的特點，文中選取DE/rand-to-best/1的進化策略，其表達式形式為

變異尺度因子F對差分向量進行縮放，確定當前個體的搜索范圍。當個體適應值較優(yōu)時，該個體所攜帶的信息應該被更多的利用，此時F應該取較小的值，使算法在此較優(yōu)個體附近繼續(xù)進化，加強算法的局部搜索能力;當個體的適應值較差時，個體的信息應該較少地被利用，此時F應取較大的值，使算法在解空間內(nèi)進行搜索，加強算法的全局搜索能力［13］。因此，需要根據(jù)個體適應值的變化情況自適應地調(diào)整各個體對應的Fi，調(diào)節(jié)策略如式(4)所示。

3)交叉操作

差分進化算法采用離散交叉算子。交叉算子把通過變異操作得到的變異向量與目標向量進行離散交叉生成試驗向量。二項式交叉操作如式(5) :

式中: CR∈［0，1］為交叉率; rand［0，1］是［0，1］區(qū)間內(nèi)均勻分布的隨機數(shù)。

CR決定了試驗向量取自變異向量還是目標向量，當變異向量的適應值f()較優(yōu)時，CR應取較大的值使得試驗向量以較大概率取自變異向量，反之，CR應取較小的值［13］。因此CR的自適應調(diào)節(jié)機制可以表示為

4)選擇操作

在差分進化算法中，在變異操作與交叉操作完成后進行選擇操作，選擇操作采取貪婪競爭機制，即只有在新產(chǎn)生的子代個體優(yōu)于父代個體時才會將其保留下來;否則，父代個體將被保留到下一代中。即

由上述算法流程可以看出，DE算法原理簡單，易于實現(xiàn)。從DE算法產(chǎn)生新個體的能力方面來說，基于種群進化的變異操作是它生成新個體的主要方法。由于DE算法具有保持種群多樣性的機制和搜索能力，DE算法所具有比它進化算法更優(yōu)越的性能。F和CR的取值對于算法性能有著重要影響，在算法進行搜索的不同階段自適應地選取合適的F和CR能夠加速算法的優(yōu)化過程。

2.3差分進化粒子濾波算法

粒子濾波算法的重采樣過程是在對粒子的權(quán)值進行評估后，在維持粒子總數(shù)不變的情況下，對權(quán)值大的粒子進行賦值并刪除權(quán)值較小的粒子。這就導致了用于逼近系統(tǒng)狀態(tài)變量后驗概率密度函數(shù)的粒子由大量重復的粒子組成，使得描述后驗概率密度的樣本點不充分。

差分進化算法是從整個種群的角度來模擬生物進化機制的，強調(diào)的是整個群體的進化作為差分進化產(chǎn)生新個體的主要方法，變異操作充分地利用了種群內(nèi)個體之間的互補信息，能夠有效地實現(xiàn)在整個種群內(nèi)的搜索。因此，差分進化算法具有較強的全局尋優(yōu)能力，優(yōu)化結(jié)果具有較好的分布性。差分進化算法反映的是進化思想，而影響粒子濾波算法性能的主要問題是粒子退化，用差分進化思想來改善粒子的分布特性，可以提高粒子濾波的精度。在粒子濾波完成一步預測后，差分進化算法利用實數(shù)編碼根據(jù)候選粒子集生成初始化種群，然后進行變異、交叉和選擇操作，生成新的建議分布，再對新粒子集進行權(quán)值更新和重采樣。實現(xiàn)過程為:

步驟1初始化

步驟2預測

步驟3優(yōu)化粒子分布

while gen＜genmaxdo

2) for i=1: NP do

(a)變異操作。按式(4)計算變異尺度因子Fi，并根據(jù)式(3)得到變異向量(gen)。

(c)選擇操作。根據(jù)式(7)得到優(yōu)化后的粒子。

end for

end while

步驟4重采樣

步驟5狀態(tài)估計

基于差分進化的粒子濾波算法主要是將DE算法中的變異、交叉和選擇操作引入到粒子濾波中。由于DE算法具有較強的尋優(yōu)能力，在提高粒子使用效率的同時，使逼近系統(tǒng)狀態(tài)變量的后驗概率密度所需的粒子數(shù)明顯減少;由于DE算法可以有效增加粒子多樣性，可防止出現(xiàn)濾波發(fā)散現(xiàn)象，提高狀態(tài)估計精度。

3　仿真研究

將算法應用于慣性/重力組合導航系統(tǒng)中對比粒子群優(yōu)化粒子濾波［10］(PSO-PF)算法、人工物理優(yōu)化粒子濾波［16］(APO-PF)算法和基于差分進化粒子濾波(DE-PF)算法匹配輔助導航的效果，驗證DE-PF算法的匹配有效性。

文中采用慣性導航系統(tǒng)海上試驗實測數(shù)據(jù)，數(shù)據(jù)輸出周期為1 s，數(shù)據(jù)記錄時間共8 h。從慣導運行2 h后開始進行組合導航，系統(tǒng)噪聲按表1選取。重力異常如圖2所示，其分辨率為30″×30″，重力異常實時測量值是根據(jù)載體真實位置(由GPS提供)、以重力場背景數(shù)據(jù)庫為基礎(chǔ)，經(jīng)過雙線性內(nèi)插并疊加均值為0 mGal、標準差為1 mGal的高斯白噪聲得到的。

表1　陀螺和加速度計主要性能參數(shù)

圖2　重力異常場與航跡

DE-PF算法的參數(shù)設(shè)置為:變異尺度因子上限Fmax=1、下限Fmin=0.4，交叉率上限CRmax=1、下限CRmin=0。取PSO-PF、APO-PF和DE-PF的迭代次數(shù)均為20，初始位置誤差均為2 000 m，粒子數(shù)均為100，濾波周期為10 s。

利用APO-PF和DE-PF算法對組合導航進行M=200次Monte-Carlo實驗。

圖3給出了載體的真實航跡、INS航跡以及APO-PF、DE-PF算法仿真的航跡。濾波開始時，慣導經(jīng)度誤差約為0.004 2°(471 m)，緯度誤差約為0.011 3°(1 260 m)。

圖3 APO-PF和DE-PF算法匹配航跡

從圖3中可看出INS的位置誤差較大，但經(jīng)過組合后APO-PF和DE-PF算法能夠顯著的抑制INS的誤差。為了更直觀地體現(xiàn)DE-PF算法組合導航的效果。

圖4給出了200次Monte-Carlo實驗得到的PSO-PF、APO-PF和DE-PF算法對載體經(jīng)度、緯度誤差估計的均方根誤差曲線。

圖4 APO-PF算法和DE-PF算法的定位誤差

k時刻，RMSE值的表達式如下所示:

計算結(jié)果表明，在濾波穩(wěn)定后的3～8 h內(nèi)，PSO-PF、APO-PF和DE-PF算法估計結(jié)果的均方根誤差分別為370.7、151.2和101.5 m。在4 h左右，由于載體轉(zhuǎn)彎的影響，PSO-PF、APO-PF和DE-PF算法對載體的經(jīng)、緯度估計出現(xiàn)較大偏差。從圖4中可以看出，PSO-PF、APO-PF算法的收斂速度更快，但DE-PF算法的定位精度要高于前2種算法。其主要原因在于DE-PF算法的變異策略中差分向量的選取方式在一定程度上影響了算法的收斂速度和種群多樣性，文中采用的變異策略的差分向量中的個體是隨機選擇的，一定程度上保證了種群的多樣性，但是降低了收斂速度。另外，DE-PF算法的多樣性增強策略是通過綜合2部分粒子的綜合信息實現(xiàn)的:一部分是通過變異操作得到的候選粒子集，這一部分粒子蘊含提升粒子多樣性的信息;另一部分則是重采樣粒子蘊含的對于真實系統(tǒng)狀態(tài)的有效逼近信息。因此，DE-PF算法能夠獲得更高的定位精度。

4　結(jié)束語

在慣性/重力組合導航過程中，采用粒子濾波可以避免常規(guī)濾波方法的線性化過程，但傳統(tǒng)粒子濾波存在的粒子退化和樣本貧化問題。差分進化算法反映了進化的思想，將差分進化算法引入粒子濾波的重采樣過程，在增強粒子多樣性的同時可以實現(xiàn)粒子的優(yōu)選。實驗結(jié)果表明，與人工物理優(yōu)化粒子濾波相比，利用基于差分進化的粒子濾波算法可以改善狀態(tài)估計精度并提高系統(tǒng)穩(wěn)定性，將其用于慣性/重力組合導航可以獲得較高的定位精度。該方法目前還存在控制參數(shù)較多、收斂速度慢的弊端，未來的工作可以針對變異策略的改進和算法收斂性的驗證進行深入的研究。

［1］劉繁明，成怡.重力/慣性匹配導航系統(tǒng)的仿真研究［J］.中國慣性技術(shù)學報，2005，13(3) : 22-25.

［2］RICE H，KELMENSON S，MENDELSOHN L.Geophysical navigation technologies and applications［C］//IEEE Position Location and Navigation Symposium.Monterey，USA，2004: 618-624.

［3］王躍鋼，文超斌，左朝陽，等.自適應混沌蟻群徑向分析算法求解重力輔助導航匹配問題［J］.物理學報，2014，63(8) : 454-459.

［4］王虎彪，王勇，方劍，等.“最小均方誤差旋轉(zhuǎn)擬合法”重力輔助導航仿真研究［J］.中國科學:地球科學，2012，42(7) : 1055-1062.

［5］GUSTAFSSON F，GUNNARSSON F，BERGMAN N，et al.Particle filters for positioning，navigation，and tracking［J］.IEEE Trans on Signal Processing，2002，50(2) : 425-437.

［6］NORDLUND P J，GUSTAFSSON F.Marginalized particle filter for accurate and reliable terrain-aided navigation［J］.IEEE Trans on Aerospace and Electronic Systems，2009， 45(4) : 1385-1399.

［7］GUSTAFSSON F.Particle filter theory and practice with positioning applications［J］.IEEE Aerospace and Electronic Systems Magazine，2010，25(7) : 53-82.

［8］HOU S Y，HUNG H S，CHANG S H，et al.Novel algorithm for tracking multiple targets［J］.Journal of Marine Science and Technology，2010，18(2) : 259-267.

［9］楊小軍，潘泉，王睿，等.粒子濾波進展與展望［J］.控制理論與應用，2006，23(2) : 261-267.

［10］方正，佟國峰，徐心和.基于粒子群優(yōu)化的粒子濾波定位方法［J］.控制理論與應用，2008，23(3) : 533-537.

［11］ZHAO Jing，LI Zhiyuan.Particle filter based on particle swarm optimization resampling for vision tracking［J］.Expert Systems with Applications，2010，37 (12) : 8910-8914.

［12］XU Benlian，ZHU Jihong，XU Huigang.An ant stochastic decision based particle filter and its convergence［J］.Signal Processing，2010，90(9) : 2731-2748.

［13］張春美，陳杰，辛斌.參數(shù)適應性分布式差分進化算法［J］.控制與決策，2014，29(3) : 701-706.

［14］吳亮紅.差分進化算法及應用研究［D］.長沙:湖南大學，2007: 9-14.

［15］王文晶.基于重力和環(huán)境特征的水下導航定位方法研究［D］.哈爾濱:哈爾濱工程大學，2009: 71-75.

［16］劉繁明，錢東，劉超華.一種人工物理優(yōu)化的粒子濾波算法［J］.控制與決策，2012，27(8) : 1145-1156.

Application of the particle filter in INS/gravity integrated navigation based on differential evolution

LIU Fanming，TANG Yingli

College of Automation，Harbin Engineering University，Harbin 150001，China

In this paper，aiming at the sample degeneration and impoverishment problem of the conventional particle filter in INS/gravity integrated navigation，a differential evolution algorithm is incorporated into the resampling process of the particle filter.The distribution of particles is optimized by the differential evolution algorithm which realizes the global search by the differences among the population，and the posterior probability density of the state value is approximated closely by the candidate particle set.The proposed method is applied to INS/gravity integrated navigation，the numerical experiment is accomplished by the sea experiment data of an inertial navigation system，and the navigation errors of different algorithms are compared and analysed under the same condition.The proposed algorithm can improve the positioning accuracy of integrated navigation.

INS/gravity integrated navigation; gravity matching; particle filter; differential evolution; resampling; degeneracy

TP29

A

1009-671X(2015) 04-015-06

10.3969/j.issn.1009-671X.201411001

2014-11-03.網(wǎng)絡出版日期: 2015-07-27.

總裝備部十二五預研資助項目(5130301).

劉繁明(1963-)，男，教授，博士生導師;唐英麗(1987-)，女，碩士研究生.

劉繁明，E-mail: hrblfm407@hrbeu.edu.cn.