基于改進粒子濾波算法的水下地形輔助導航方法

陳睿瑋，車馳東

（上海交通大學 船舶海洋與建筑工程學院，上海 200240）

對于海洋資源的探索一直以來都受到世界各國的高度重視，尤其在極地區(qū)域，蘊含著豐富的自然與航運資源，利用水下航行器（Autonomous Under-water Vehicle，AUV）在極區(qū)進行水下環(huán)境與海洋物理等信息的采集成為了海洋資源開發(fā)的重要一環(huán)。AUV的工作效率與工作質(zhì)量與其導航的性能高度相關，但受水下環(huán)境的制約。如全球定位系統(tǒng)（Global Positioning System，GPS）、基線定位法等傳統(tǒng)的高精度定位方法在AUV水下作業(yè)時無法使用，在有冰層常年覆蓋的極區(qū)，AUV無法浮出水面接收GPS信號修正定位，且基線定位方法所需的外部輔助設備在極地冰雪層上施工困難。AUV只能通過慣性導航提供短距離內(nèi)的定位，傳感器的漂移導致誤差隨AUV航行時間的增加而累積。

高精度的自主導航問題可以通過使用水下地形輔助導航（Terrain Aided Navigation，TAN）技術(shù)解決。TAN方法通過將采集到的地形與地圖信息進行匹配，推算出當前測點的位置。TAN方法根據(jù)匹配形式可以分為地形輪廓匹配（Terrain Contour Matching，TERCOM）和遞歸貝葉斯估計。TERCOM技術(shù)是一種批相關處理的方法，早期用于飛行器和導彈制導，現(xiàn)在技術(shù)已經(jīng)較成熟，并有廣泛的應用，但由于其實時性較差及無法處理過程和量測噪聲，用于AUV導航的效果并不理想?；谶f歸貝葉斯估計的導航方法可以有效地處理過程噪聲，其中具有代表性的有卡爾曼濾波（Kalman Filter，KF）、擴展卡爾曼濾波（Extended Kalman Filter，EKF）和無跡卡爾曼濾波（Unsented Kalman Filter，UKF）。EKF和UKF雖然解決了KF無法用于非線性系統(tǒng)的問題，但二者仍要求系統(tǒng)的觀測是高斯分布的，具有局限性。粒子濾波（Particle Filtering，PF）利用隨機分布的粒子逼近后驗分布，解決了KF及其衍生方法的問題，成為當前TAN技術(shù)研究的主流方向。

PF方法為了解決粒子權(quán)值過低而產(chǎn)生粒子退化的問題，引入了重采樣步驟，但重采樣導致高權(quán)值粒子被大量復制，使粒子的多樣性匱乏，即產(chǎn)生粒子貧化現(xiàn)象。現(xiàn)有研究的主要目標是在剔除低權(quán)值粒子的同時，保持粒子的多樣性。近年來，研究人員通過將相關系數(shù)、高斯和濾波器等引入PF算法，使粒子貧化問題得到改善，并應用于水下地形輔助導航中。研究表明，PF方法用于AUV的長航程、高深度工作也具有良好的性能。

學者們也對TAN的魯棒方法做了大量研究。當前的導航魯棒方法可概括為2種：提高局部定位自適應能力，直接提高局部定位精度；提高逃離局部最優(yōu)定位的能力，間接提高全局定位魯棒性?，F(xiàn)有TAN方法的研究多基于高分辨率的海底地圖，文獻[23]的數(shù)據(jù)表明，導航的精度在分辨率較低時與其呈正相關。當前對極地區(qū)域開發(fā)仍處于起步階段，現(xiàn)有的水下信息并不充分。目前北極地區(qū)的海底地形圖分辨率僅為500 m，且具有分辨率低、噪聲大、特征稀疏、平坦區(qū)域面積大等特點，這些不利因素使得現(xiàn)有的TAN方法作用于低分辨率海圖時，在精度和魯棒性上都表現(xiàn)較差。文獻[25]提出了一種帶粒子抖動的粒子濾波（Jittered Bootsrap Particle Filter，JBPF），通過給重采樣后的粒子添加額外噪聲，來改善粒子貧化，該方法在基于分辨率為500 m的海底地形圖導航仿真中得到較低的導航誤差。JBPF添加的粒子抖動在抖動半徑較大時，將引入較大的額外誤差；而半徑較小時，則魯棒性下降。

本文針對低分辨率海底地形圖的水下地形輔助導航，提出一種帶有自抖動及修正的粒子濾波方法（Self-jitter and Correction Particle Filter，SJCPF）。通過在重采樣步驟中根據(jù)相關系數(shù)對粒子權(quán)值進行調(diào)整，并引入額外噪聲，增加粒子的多樣性，在保證魯棒性的基礎上，增加了導航精度，且提高了算法的效率。

1 粒子濾波及改進方法

1.1 基本理論

粒子濾波的本質(zhì)是一種貝葉斯估計，采用蒙特卡洛方法，通過隨機生成的粒子來近似概率分布。基于傳統(tǒng)的粒子濾波的輔助導航算法包含以下幾個步驟。

1）起始位置的粒子初始化：在起點處附近以一定的概率分布（如均勻分布、高斯分布等）生成一組離散點，每一個點代表在初始時刻AUV可能存在的位置，此時每個粒子的權(quán)值相等。

2）粒子位置更新與預測值、測量值獲取：從慣導系統(tǒng)獲取數(shù)據(jù)，并推算AUV在2次測量中的位移量，更新所有粒子的位置，并根據(jù)新的粒子位置在數(shù)字地圖上獲取深度的預測值，測量值可由測深傳感器得到。

3）粒子權(quán)值更新及重采樣：根據(jù)測量值和預測值更新每個粒子的權(quán)值，將得到的權(quán)值歸一化后，重新從粒子中采樣，得到新的一組權(quán)值一致的粒子。

4）航行位置預測值更新：將粒子的位置加權(quán)平均后得到AUV航行位置的預測值。

導航過程中，迭代步驟2—4），算法將連續(xù)地提供AUV的預測航跡。

1.2 粒子抖動

雖然重采樣的引入改善了粒子退化的現(xiàn)象，但是隨著迭代次數(shù)的增加，保留的大量高權(quán)值粒子只集中在幾個很小的區(qū)域內(nèi)，尤其在低分辨率海圖的條件下，地形信息的相對匱乏，對數(shù)字海圖插值帶來的誤差更大，傳統(tǒng)算法的重采樣步驟將過早地舍棄部分低權(quán)值但接近AUV實際位置的粒子。為了解決上述問題，引入粒子抖動，即在每次粒子位置更新時，引入額外的過程噪聲，該過程噪聲采用高斯分布。該方法使得傳統(tǒng)算法中過度集中的粒子適當向周圍發(fā)散，改善了算法本身及海圖分辨率低帶來的粒子多樣性匱乏。

1.3 相關系數(shù)修正重采樣權(quán)值

在算法中采用粒子抖動，一方面解決了粒子貧化的問題，改善了算法的魯棒性，但同時也引入了額外的過程誤差?？紤]到AUV航速較慢，并且在長航程工作時采樣頻率低，可以在算法中引入TERCOM方法批相關處理的思想。根據(jù)每個粒子前幾次的預測值序列與測量值序列，得到其對應的相關系數(shù)ρ，重采樣時使用該系數(shù)修正粒子的權(quán)值ω。

在統(tǒng)計學上常用的相關系數(shù)有皮爾遜相關系數(shù)（Pearson Correlation Coefficient，PCC）、斯皮爾曼秩相關系數(shù)（Spearman's Rank Correlation Coefficient，SRCC）及肯達爾秩相關系數(shù)（Kendall Rank Correlation Coefficient，KRCC）。PCC是一種線性相關系數(shù)，通過2個變量、的協(xié)方差除以它們的標準差乘積得到，變量的序列長度為。

PCC本質(zhì)上是2組變量協(xié)方差的歸一化，取值范圍為–1~1，其計算的時間復雜度為()。SRCC和KRCC都是秩相關系數(shù)，其中SRCC可以理解為對數(shù)據(jù)排序后并用排序位置代替原始數(shù)據(jù)得到的PCC。

式中：x、y為X、Y對應的排序位置數(shù)據(jù)。由于使用了排序，SRCC的時間復雜度為(lg)。

KRCC通過數(shù)據(jù)對(X,Y)、(X,Y)的兩兩比較得到。

式中：為滿足X>X且Y>Y或X<X且Y<Y的數(shù)據(jù)對組數(shù)；為不滿足前述條件的組數(shù)。由于計算過程需要對每2組數(shù)據(jù)分別比較，KRCC的時間復雜度為()。

1.4 帶有自抖動及修正的粒子濾波方法

基于上述討論并針對AUV的航行特點，本文提出了一種基于SJCPF的水下地形輔助導航算法。該算法的計算步驟如圖1所示。

圖1 SJCPF算法流程 Fig.1 SJCPF algorithm flow chart

1）粒子初始化：在起始點的位置附近，采用高斯分布對粒子的位置進行初始化。

2）粒子位置更新及抖動：在每個測量區(qū)間，根據(jù)慣性系統(tǒng)的數(shù)據(jù)，更新粒子的位置，并為更新后的粒子位置添加高斯噪聲。

3）相關系數(shù)修正粒子權(quán)值及重采樣：當測量值及預測值序列長度達到設定的最低值后，計算2組變量的相關系數(shù)，并將其用作核函數(shù)的參數(shù)修正粒子權(quán)值，采用修正后的權(quán)值對粒子進行重采樣。

4）位置更新：將粒子的位置加權(quán)平均后得到AUV航行位置的預測值。

2 仿真與分析

對傳統(tǒng)PF算法及SJCPF算法的導航性能進行仿真，選用北緯75.27°~75.6°、西經(jīng)162.5°~161°區(qū)域的數(shù)字海底地形圖，地圖分辨率為500 m。SJCPF算法中，粒子的抖動方差取225 m，重采樣的相關系數(shù)采用Pearson相關系數(shù)，核函數(shù)取指數(shù)函數(shù)。以75.27°N、162.5°W為坐標原點，仿真中AUV按以上參數(shù)沿規(guī)定航跡行駛，起始位置為北向1 000 m，東向15 000 m。假設起始位置不存在誤差，2種算法的航跡圖及誤差隨位移的變化曲線如圖2及圖3所示。

由圖2和圖3可以看出，PF算法在預測位置偏離航跡后，并沒有及時有效地調(diào)整，在航跡圖中的表現(xiàn)近似于靜差，其導航誤差也維持在500 m以上。這直接體現(xiàn)了傳統(tǒng)PF算法的粒子貧化問題。由于導航 過程中粒子逐漸集中在高權(quán)值粒子附近，在出現(xiàn)較大的導航誤差時，真實位置附近可能不存在粒子，誤差難以消除。

圖2 PF算法及SJCPF算法的地形匹配結(jié)果 Fig.2 Terrain matching results of PF and SJCPF

圖3 PF算法及SJCPF算法的匹配結(jié)果 Fig.3 Matching results of PF and SJCPF

SJCPF算法的預測航跡與實際航跡的重合度更高，導航誤差的均值也顯著低于傳統(tǒng)PF算法。這是由于SJCPF引入的過程噪聲以及在重采樣過程中保留了更多的粒子，并放大了相關系數(shù)高的粒子的權(quán)值，在導航誤差較大的時候也能及時調(diào)整至真實位置附近。

對以上仿真進行200次重復性試驗后，得到傳統(tǒng)PF算法和SJCPF算法的均方根誤差分別為1 767.75 m及1 277.31 m，SJCPF算法控制導航誤差的能力顯著優(yōu)于傳統(tǒng)PF算法。

在表1參數(shù)的基礎上，通過控制SJCPF算法的部分參數(shù)，研究算法對導航精度的影響因素。改變重采樣中相關系數(shù)的求取方法，分別為PCC、SRCC、KRCC。由圖4可以看出，基于KRCC的SJCPF算法的導航誤差相對較小，極值也更低，但與PCC及SRCC并未有明顯區(qū)別。對該仿真進行200次重復性試驗，基于SRCC和KRCC導航算法的均方根誤差分別為1 386.98 m及1 241.01 m，相較于基于PCC的1 277.31 m沒有明顯提升。在算法耗時上，基于PCC的算法執(zhí)行單次循環(huán)體需140.59 ms，SRCC為301.19 ms，KRCC為235.37 ms，并且隨著粒子數(shù)量增加，計算SRCC和KRCC耗時的增速要顯著大于計算PCC的耗時。因此，綜合考慮導航性能，選用PCC更優(yōu)。

表1 仿真參數(shù) Tab.1 Parameters of Simulation

圖4 SJCPF算法基于不同相關系數(shù)的匹配結(jié)果 Fig.4 Terrain matching results of SJCPF based on different correlation coefficients

改變SJCPF算法中粒子的數(shù)量，分別取50、500、1 000。從圖5中可以看出，隨著粒子數(shù)量的增加，導航的精度及穩(wěn)定性都有所提高。但當粒子數(shù)量較大時，這一提升便開始減小，并且粒子數(shù)量的增多將增加計算負擔，給功耗和耗時都帶來負面影響。在粒子數(shù)量只有50時，由于粒子數(shù)量過低，放大了引入的過程噪聲帶來的影響，導航穩(wěn)定性大大降低；粒子數(shù)量提升至1 000時，由于粒子密度已經(jīng)較高，對導航精度的提升有限。

圖5 SJCPF算法基于不同粒子數(shù)量的匹配結(jié)果 Fig.5 Terrain matching results of SJCPF based on different number of particles

改變SJCPF算法中的測量頻率，分別取0.05、0.01、0.001 Hz。隨著測量頻率的提升，導航誤差的均值及方差都有明顯的降低，如圖6所示。這一結(jié)果表明，工程中需要避免使用過低的測量頻率，但并不意味著需要大幅提升測量頻率。一方面，更加頻繁地使用測深儀器會增大AUV的功耗，降低其續(xù)航能力；另一方面，仿真所使用的插值地圖并不能很好地模擬網(wǎng)格節(jié)點間的真實地形，實際上這部分的誤差并不能通過提高測量頻率來消除。因此，綜合考慮，只需要選用能使測量間距略低于網(wǎng)格分辨率的測量頻率即可。

圖6 SJCPF算法基于不同測量頻率的匹配結(jié)果 Fig.6 Terrain matching results of SJCPF based on differ-ent measuring frequency

3 結(jié)論

1）SJCPF算法較好地解決了傳統(tǒng)PF算法的粒子貧化問題。在低分辨率海圖的條件下，粒子貧化所帶來的誤差影響尤為明顯，傳統(tǒng)PF算法極易在誤差較大時脫離原航跡，產(chǎn)生導航靜差。SJCPF算法增加的過程噪聲使得誤差較大時粒子仍有機會回到真實位置附近，并且運用粒子先前時刻測量值求取的相關系數(shù)修正權(quán)值，避免了部分貼近真實航跡的粒子只因為一次測量值偏差較大而被剔除，增加了算法的魯棒性。相較于傳統(tǒng)PF算法，導航的均方根誤差降低了27.7%。

2）通過比較不同參數(shù)對SJCPF算法導航性能的影響，發(fā)現(xiàn)PCC、SRCC、KRCC這3種相關系數(shù)用于算法中都可以得到較低的導航誤差，其中KRCC表現(xiàn)最佳，但是提升程度有限。在耗時方面，PCC由于擁有線性的時間復雜度而遠低于SRCC和KRCC。導航誤差和粒子數(shù)量呈負相關，在粒子數(shù)量較小的時候，粒子數(shù)量的提升對導航誤差的控制效果顯著；粒子密度較高時，粒子數(shù)量增加并不能明顯降低誤差，反而增加了算法耗時。測量頻率對導航誤差的影響與粒子數(shù)量類似，在頻率較低時提升明顯，頻率較高時提升測量頻率對導航誤差的影響有限，且受制于仿真使用的地圖精度，高測量頻率帶來的誤差減小可能只適用于仿真中，而無法在實際工程中實現(xiàn)。因此，在實際工程中，使用SJCPF算法進行水下地形輔助導航時，可選用PCC作為相關系數(shù)，使用較高的粒子數(shù)量及較高的測量頻率，可以兼顧AUV的續(xù)航及導航精度。