陳睿瑋，車馳東

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

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

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

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

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

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

1 粒子濾波及改進(jìn)方法

1.1 基本理論

粒子濾波的本質(zhì)是一種貝葉斯估計(jì)，采用蒙特卡洛方法，通過隨機(jī)生成的粒子來近似概率分布?；趥鹘y(tǒng)的粒子濾波的輔助導(dǎo)航算法包含以下幾個(gè)步驟。

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

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

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

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

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

1.2 粒子抖動(dòng)

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

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

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

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

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

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

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

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

1.4 帶有自抖動(dòng)及修正的粒子濾波方法

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

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

1）粒子初始化：在起始點(diǎn)的位置附近，采用高斯分布對(duì)粒子的位置進(jìn)行初始化。

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

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

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

2 仿真與分析

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

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

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

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

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

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

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

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

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

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

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

3 結(jié)論

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

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