改進(jìn)粒子濾波的水下重力匹配導(dǎo)航仿真與實(shí)驗(yàn)

歐陽(yáng)明達(dá)， 楊元喜

(1.信息工程大學(xué) 地理空間信息學(xué)院，河南 鄭州 450052； 2.地理信息工程國(guó)家重點(diǎn)實(shí)驗(yàn)室，陜西 西安 710054)

水下載體導(dǎo)航由于獨(dú)立性、自主性和隱蔽性要求，往往選擇慣性導(dǎo)航手段，但是，慣性導(dǎo)航系統(tǒng)由于累積誤差大，一般不適宜長(zhǎng)時(shí)間、遠(yuǎn)距離的水下航行導(dǎo)航。慣性導(dǎo)航與其他導(dǎo)航手段的組合或融合可以部分校正慣性系統(tǒng)產(chǎn)生的位置誤差積累[1-3]。依靠光學(xué)信號(hào)的天文導(dǎo)航或依賴(lài)無(wú)線(xiàn)電信號(hào)的全球?qū)Ш叫l(wèi)星系統(tǒng)(global navigation satellite system,GNSS)導(dǎo)航是開(kāi)曠地區(qū)最常用的定位方式，但是它們只能在本身具備可用性的情況下才能與慣性系統(tǒng)進(jìn)行組合導(dǎo)航[4]；水聲定位導(dǎo)航技術(shù)是一種通過(guò)測(cè)量聲波傳播的時(shí)間、相位、頻率等信息實(shí)現(xiàn)定位與導(dǎo)航的技術(shù)，受復(fù)雜海洋環(huán)境影響，水聲定位導(dǎo)航觀(guān)測(cè)數(shù)據(jù)受觀(guān)測(cè)有效性低、延遲性高、數(shù)據(jù)率低、觀(guān)測(cè)受水體環(huán)境影響大等特點(diǎn)的制約，水聲/慣性組合導(dǎo)航目前尚處于起步階段，以理論研究為主，實(shí)踐較少，暫時(shí)還未建成海底長(zhǎng)期穩(wěn)定工作的水下大地基準(zhǔn)網(wǎng)[5-7]；地磁/慣性組合導(dǎo)航方式由于缺乏地磁基礎(chǔ)資料，分辨率較低，且地磁變化較快，離實(shí)用化還存在很大差距[8]。

近年來(lái)重力匹配導(dǎo)航方式得到國(guó)內(nèi)外學(xué)者的廣泛關(guān)注，先后研制了重力輔助慣性導(dǎo)航系統(tǒng)和重力匹配導(dǎo)航算法[2,9]，經(jīng)典的算法有地形輪廓匹配(terrain contour matching,TERCOM)算法[10-12]、等值線(xiàn)最近點(diǎn)迭代(iterative closest contour point,ICCP)算法[13-15]、桑迪亞慣性地形輔助導(dǎo)航(sandia intertial terrain contour matching,SITAN)算法[16-17]，這些方法原理相對(duì)簡(jiǎn)單、匹配效率高，早期多用于飛行器地形匹配導(dǎo)航。粒子濾波算法是采用大量隨機(jī)樣本完成貝葉斯濾波的遞推過(guò)程，通過(guò)概率計(jì)算，得到位置最優(yōu)解。與SITAN算法相比，粒子濾波算法無(wú)需地形隨機(jī)線(xiàn)性化過(guò)程，更適用于非線(xiàn)性/非高斯的動(dòng)態(tài)系統(tǒng)，但也存在其自身問(wèn)題，如重要性分布函數(shù)難以選取、重要性函數(shù)與似然函數(shù)不能完好匹配、粒子退化嚴(yán)重等。針對(duì)現(xiàn)實(shí)應(yīng)用問(wèn)題，國(guó)內(nèi)外學(xué)者對(duì)粒子濾波進(jìn)行了一系列改進(jìn)，如：不敏卡爾曼粒子濾波[18-19]、輔助粒子濾波[20]、擴(kuò)展卡爾曼粒子濾波[21]、高斯和粒子濾波[22]、高斯粒子濾波[23]、Rao-Blackwellized粒子濾波[24]、正則化粒子濾波[25]等，這些改進(jìn)算法從不同側(cè)面提高了粒子濾波性能。在匹配導(dǎo)航算法實(shí)踐中，文獻(xiàn)[26]提出一種粒子濾波估計(jì)方法和軌跡相似變換相結(jié)合的二次匹配算法，選擇載體位置為濾波模型的狀態(tài)變量，利用慣導(dǎo)系統(tǒng)短期位移進(jìn)行濾波模型參數(shù)估計(jì)，采用粒子濾波方法對(duì)載體真實(shí)位置進(jìn)行最優(yōu)估計(jì)；文獻(xiàn)[27]針對(duì)貝葉斯濾波后驗(yàn)概率密度函數(shù)的求解問(wèn)題，用高斯混合密度函數(shù)近似狀態(tài)的后驗(yàn)概率密度函數(shù)，提出了基于高斯和粒子濾波的水下地形匹配導(dǎo)航方法。

本文將粒子濾波算法用于水下重力匹配導(dǎo)航，引入重要性重采樣步驟，克服了濾波過(guò)程中的粒子退化問(wèn)題。在詳細(xì)分析粒子濾波受不同初始偏差、不同蒙特卡羅采樣、不同重力異常觀(guān)測(cè)誤差影響的基礎(chǔ)上，為了控制初始偏差對(duì)匹配精度和計(jì)算時(shí)效的不利影響，引入粒子群優(yōu)化算法削弱初始配準(zhǔn)誤差；通過(guò)算例分析驗(yàn)證，聯(lián)合算法有助于進(jìn)一步提高精度、提升效率、增強(qiáng)可靠性。

1 匹配算法原理

1.1 貝葉斯估計(jì)

非線(xiàn)性隨機(jī)系統(tǒng)可以用系統(tǒng)狀態(tài)方程表示為：

xk+1=fk(xk,wk)

(1)

系統(tǒng)觀(guān)測(cè)方程為：

zk=hk(xk,vk)

(2)

式中：xk為系統(tǒng)狀態(tài)向量；zk為k時(shí)刻的觀(guān)測(cè)向量；wk為過(guò)程噪聲；vk為觀(guān)測(cè)噪聲。

根據(jù)貝葉斯理論，給定后驗(yàn)概率密度函數(shù)或后驗(yàn)密度p(xk|z1:k)以及k時(shí)刻所有可用觀(guān)測(cè)向量z1:k={z1,z2,…,zk}估計(jì)xk出現(xiàn)的概率[28]，這個(gè)概率值即為p(xk|z1:k)。狀態(tài)向量的初始概率為p(x0|z0)，所求概率可以通過(guò)預(yù)測(cè)和更新2步法得到。預(yù)測(cè)階段使用公式為：

(3)

式中：p(xk|z1:k-1)為前一個(gè)后驗(yàn)密度。給定測(cè)量值z(mì)k，更新階段通過(guò)貝葉斯準(zhǔn)則得到后驗(yàn)密度為：

(4)

歸一化常數(shù)：

(5)

從上述遞推過(guò)程可知，計(jì)算過(guò)程涉及大量積分運(yùn)算，由于觀(guān)測(cè)方程式(2)一般為非線(xiàn)性方程，且觀(guān)測(cè)不一定服從正態(tài)分布，所以很難得到后驗(yàn)概率積分值，通常采用蒙特卡羅采樣方法。

1.2 序貫重要性采樣算法

序貫重要性采樣(sequential important sampling,SIS)算法是一種蒙特卡羅(Monte Carlo,MC)方法，通過(guò)一組具有相關(guān)權(quán)重的隨機(jī)樣本表示后驗(yàn)概率密度函數(shù)，并根據(jù)這些樣本和權(quán)重計(jì)算估計(jì)值[29]。當(dāng)樣本數(shù)量變得非常大時(shí)，SIS濾波值接近最佳貝葉斯估計(jì)。

(6)

式中δ為狄拉克函數(shù)。

第i個(gè)粒子的歸一化權(quán)重為：

(7)

(8)

序貫貝葉斯估計(jì)的思路是，在每次迭代中，利用樣本可以構(gòu)成p(x0:k-1|z1:k-1)的近似值，并以一組新的觀(guān)測(cè)樣本得到p(x0:k|z1:k)的近似值。重要性密度表示為：

q(x0:k|z1:k)=q(xk|x0:k-1,z1:k)q(x0:k-1|z1:k-1)

(9)

(10)

根據(jù)式(9)和式(10)，權(quán)重更新方程為：

算法1：SIS粒子濾波

fori=1:N

采樣粒子：

end

1.3 重要性采樣(SIR)算法

采用SIS 方法進(jìn)行遞歸貝葉斯估計(jì)時(shí)，會(huì)產(chǎn)生嚴(yán)重的粒子退化問(wèn)題。粒子退化問(wèn)題是指通過(guò)若干次遞歸計(jì)算，除了個(gè)別幾個(gè)粒子具有較大權(quán)值外，其余粒子的權(quán)值都很??；極端情況下，只有一個(gè)粒子具有趨近于 1 的較大權(quán)值，所有其他粒子的權(quán)值都趨近于0[30-32]。Gordon 等[33]在其提出的自舉濾波算法中引入重采樣(resampling)步驟，一定程度上解決了粒子退化問(wèn)題。

重采樣的主要思想是，去除那些權(quán)值較小的粒子，保留并復(fù)制那些權(quán)值較大的粒子。重采樣的方法有很多，如分層采樣、殘差采樣、系統(tǒng)采樣等。一般使用有效粒子Neff來(lái)判斷粒子權(quán)重的退化程度：

2 不同工況條件下的結(jié)果討論

將某海域作為研究對(duì)象，重力異常數(shù)據(jù)采用美國(guó)斯克利普斯海洋研究所發(fā)布的SIO grav23.1模型，模型為1′分辨率，精度為2 mGal[34]。圖1示出了該海域重力異常。給出航跡模擬參數(shù)，令真實(shí)航跡起始點(diǎn)坐標(biāo)為22.3°S,105.6°E，航向角45°，載體航行速度10 n mile/h，隨機(jī)誤差均方差為0.06 n mile，指示航跡線(xiàn)性誤差在經(jīng)度方向?yàn)?.3 n mile/h，緯度方向?yàn)?.5 n mile/h，隨機(jī)誤差均方差為0.12 n mile，航跡點(diǎn)重力異常均方根誤差為2 mGal，共有40個(gè)采樣點(diǎn)。

圖1 海域重力異常圖

采用粒子濾波算法進(jìn)行重力匹配導(dǎo)航，設(shè)置如下部分固定參數(shù)：粒子分布半徑6 000 m，粒子數(shù)500個(gè)，重力觀(guān)測(cè)誤差標(biāo)準(zhǔn)差2 mGal。精度誤差評(píng)價(jià)指標(biāo)采用均方根誤差(root mean square error,RMSE)表示為[35]：

本文設(shè)定A、B、C 3種不同工況，即不同初始誤差、不同蒙特卡羅采樣樣本量和不同觀(guān)測(cè)誤差標(biāo)準(zhǔn)差，分別討論不同工況的粒子濾波效果。

1)工況A：不同初始偏差

初始偏差是真實(shí)航跡相對(duì)慣導(dǎo)指示航跡在起始點(diǎn)的偏移量，由于無(wú)法提前預(yù)知其偏離程度，有必要對(duì)不同初始偏差條件下的濾波計(jì)算情況進(jìn)行分析。設(shè)定蒙特卡羅采樣為10次，初始偏差分別為(1 n mile，0 n mile)、(3 n mile，0 n mile)、(5 n mile，0 n mile)、(9 n mile，0 n mile)，圖2示出了不同偏差的粒子濾波計(jì)算結(jié)果，圖3示出了航跡重力異常變化曲線(xiàn)，可以看出：

圖2 不同初始偏差的粒子濾波匹配結(jié)果

圖3 航跡重力異常變化

①航跡點(diǎn)重力異常變化特征明顯，適宜開(kāi)展水下重力匹配導(dǎo)航，相鄰點(diǎn)間的重力異常差異較大，航跡點(diǎn)的重力異常觀(guān)測(cè)誤差不會(huì)淹沒(méi)相鄰2點(diǎn)間的重力異常數(shù)據(jù)變化；

②匹配航跡能夠較好吻合真實(shí)航跡，圖4示出了航跡的初始段差異值，可以看出，采用粒子濾波算法，初始偏移越大，初始段匹配的位置差異越大，倘若初始偏差超過(guò)粒子覆蓋范圍，很容易造成匹配失??；

圖4 不同初始偏差匹配結(jié)果精度

③匹配航跡逐漸向真實(shí)航跡靠攏，中后航段匹配精度高于初始航段。由此考慮，當(dāng)對(duì)航跡整體平移后采用濾波計(jì)算，即若能通過(guò)算法將圖2(d)指示航跡平移變換為近似圖2(a)指示航跡，所得匹配結(jié)果的初始航段精度水平將會(huì)有較大改善。

2)工況B：不同次數(shù)蒙特卡羅采樣

采用多次蒙特卡羅采樣，粒子濾波算法在獲得高精度匹配結(jié)果的同時(shí)會(huì)犧牲實(shí)時(shí)性[36]。匹配精度與計(jì)算效率之間需要合理平衡，對(duì)采用1次、10次、30次、60次蒙特卡羅采樣的匹配結(jié)果進(jìn)行分析，圖5示出了不同初始偏差的蒙特卡羅采樣后匹配結(jié)果，可以看出：

圖5 不同蒙特卡羅采樣匹配結(jié)果精度

①當(dāng)偏差為(1 n mile，0 n mile)，進(jìn)行1次蒙特卡羅采樣后的結(jié)果和多次采樣后結(jié)果未呈現(xiàn)顯著差異；當(dāng)偏差為(9 n mile，0 n mile)時(shí)，進(jìn)行1次蒙特卡羅采樣后的結(jié)果在初始段精度略低于多次采樣后結(jié)果；由此可見(jiàn)，初始偏差越小，1次蒙特卡羅采樣的穩(wěn)定性和可靠性更優(yōu)；

②多次采樣后的匹配結(jié)果，對(duì)精度改善和提升作用不大，甚至有個(gè)別點(diǎn)位在多次蒙特卡羅采樣后，精度低于較低次數(shù)的蒙特卡羅采樣結(jié)果。計(jì)算時(shí)效分別為1次，耗時(shí)約50 s；10次，耗時(shí)約5 min；30次，耗時(shí)約14 min；60次，耗時(shí)約28 min。進(jìn)行1次蒙特卡羅采樣意味著能夠?qū)崟r(shí)性開(kāi)展水下重力匹配導(dǎo)航，倘若對(duì)指示航跡整體平移以縮小初始偏差，則能夠得到更高效、更準(zhǔn)確、更穩(wěn)妥的匹配結(jié)果。

3)工況C：不同觀(guān)測(cè)誤差標(biāo)準(zhǔn)差

設(shè)定蒙特卡羅采樣次數(shù)為10次，對(duì)航跡點(diǎn)重力異常分別加入2 mGal、5 mGal、9 mGal、13 mGal、20 mGal的隨機(jī)誤差。圖6示出了初始偏差為(1 n mile，0 n mile)和(3 n mile，0 n mile)的不同觀(guān)測(cè)誤差下匹配導(dǎo)航結(jié)果。圖6中，σ′為觀(guān)測(cè)誤差標(biāo)準(zhǔn)差。可以看出：

圖6 不同觀(guān)測(cè)誤差標(biāo)準(zhǔn)差的匹配結(jié)果精度

①當(dāng)初始偏差為(3 n mile，0 n mile)時(shí)，匹配導(dǎo)航精度受σ′值變動(dòng)影響較大，σ′在5 mGal以?xún)?nèi)時(shí)，匹配航跡與真實(shí)航跡吻合程度較好，穩(wěn)定性和精確度較高；當(dāng)σ′值過(guò)大，匹配結(jié)果呈現(xiàn)為發(fā)散狀態(tài)，偏離真實(shí)航跡越來(lái)越遠(yuǎn)；當(dāng)初始偏差為(5 n mile，0 n mile)、(9 n mile，0 n mile)時(shí)，σ′增大，匹配失敗。

②當(dāng)初始偏差為(1 n mile，0 n mile)時(shí)，導(dǎo)航結(jié)果精度受σ′值影響程度減弱，當(dāng)σ′較大時(shí)，匹配結(jié)果發(fā)散程度較偏差較大時(shí)的情況有所減緩，整體精度水平更優(yōu)。由此可見(jiàn)，縮小初始偏差，能夠在一定程度上克服實(shí)際重力異常觀(guān)測(cè)誤差較大帶來(lái)的負(fù)面影響。

3 聯(lián)合粒子群優(yōu)化的改進(jìn)算法

綜上，減小初始偏差有助于提升匹配導(dǎo)航定位精度、提高計(jì)算效率，有必要對(duì)其進(jìn)行預(yù)處理。引入粒子群優(yōu)化算法，該方法的優(yōu)點(diǎn)在于不拘泥格網(wǎng)分辨率的限制，通過(guò)設(shè)定適用性函數(shù)引導(dǎo)粒子向最優(yōu)位置逼近，得到初始段航跡最優(yōu)匹配解，其與指示航跡起始點(diǎn)間差異即為平移參數(shù)。程向紅等[37]給出了粒子群優(yōu)化算法的基本原理。

聯(lián)合粒子群優(yōu)化改進(jìn)算法計(jì)算步驟如下：首先，利用粒子群優(yōu)化算法對(duì)初始段航跡進(jìn)行匹配計(jì)算，實(shí)現(xiàn)初始點(diǎn)的精確定位；其次，對(duì)初始點(diǎn)匹配位置進(jìn)行標(biāo)記，得到與慣導(dǎo)指示航跡初始點(diǎn)的位置差異，設(shè)置為平移量，將慣導(dǎo)指示航跡整體平移；然后，將平移后航跡作為待匹配航跡，其上的重力異常值保持不變；最后，采用粒子濾波算法，得到匹配航跡，由于初始偏差得到改善，粒子濾波過(guò)程采用1次蒙特卡羅采樣計(jì)算，即可實(shí)現(xiàn)實(shí)時(shí)匹配導(dǎo)航，而后每隔一定航距進(jìn)行多次蒙特卡羅計(jì)算驗(yàn)證結(jié)果。

沿用上文算例，設(shè)定初始偏移量為(9 n mile，0 n mile)，蒙特卡羅采樣1次，重力異常觀(guān)測(cè)誤差標(biāo)準(zhǔn)差為2 mGal，采用粒子群優(yōu)化算法后，將慣導(dǎo)指示航跡整體平移，采用粒子濾波算法，得到匹配結(jié)果如圖7所示。圖8示出了粒子濾波算法和聯(lián)合算法的匹配航跡結(jié)果差異，可見(jiàn)，聯(lián)合算法對(duì)初始段航跡具有明顯改善作用；當(dāng)重力異常觀(guān)測(cè)誤差標(biāo)準(zhǔn)差為5 mGal時(shí)，得到匹配結(jié)果如圖9所示，圖10示出了兩者間差異，可見(jiàn)，聯(lián)合算法能夠在一定程度上降低實(shí)測(cè)重力異常誤差對(duì)匹配導(dǎo)航結(jié)果精度的負(fù)面影響。

圖7 觀(guān)測(cè)誤差標(biāo)準(zhǔn)差2 mGal時(shí)，聯(lián)合方法匹配航跡結(jié)果

圖8 觀(guān)測(cè)誤差標(biāo)準(zhǔn)差2 mGal時(shí)匹配導(dǎo)航結(jié)果精度比較

圖9 觀(guān)測(cè)誤差標(biāo)準(zhǔn)差5 mGal時(shí)匹配航跡結(jié)果

圖10 觀(guān)測(cè)誤差標(biāo)準(zhǔn)差5 mGal匹配導(dǎo)航結(jié)果精度比較

4 結(jié)論

1)本文將粒子濾波算法應(yīng)用于水下重力匹配導(dǎo)航，首先分析了不同蒙特卡羅采樣樣本量、不同重力異常觀(guān)測(cè)誤差標(biāo)準(zhǔn)差、以及不同初始偏差3種工況對(duì)匹配導(dǎo)航精度的影響。

2)初始偏差較大時(shí)，對(duì)其進(jìn)行改善有助于提升初始段匹配精度；初始偏差較小時(shí)，采用多次蒙特卡羅計(jì)算對(duì)提高精度的幫助作用不大，初始偏差較大時(shí)，單次蒙特卡羅采樣的計(jì)算精度會(huì)有所下降，多次蒙特卡羅采樣提升精度的同時(shí)降低了匹配導(dǎo)航工作效率。

3)實(shí)測(cè)重力異常觀(guān)測(cè)誤差標(biāo)準(zhǔn)差越小越好，然而，當(dāng)誤差標(biāo)準(zhǔn)差增大，縮小初始偏差有助于減小其負(fù)面影響。

4)綜上可以看出，采用粒子群優(yōu)化和粒子濾波的聯(lián)合算法能夠?qū)Τ跏计钸M(jìn)行改善，有助于進(jìn)一步提高精度、提升效率、增強(qiáng)可靠性，通過(guò)算例，進(jìn)一步驗(yàn)證了該方法的可行性和有效性。