韓月，陳鵬云，沈 鵬

（1. 中北大學(xué)機電工程學(xué)院，太原030051；2. 太原旅游職業(yè)學(xué)院現(xiàn)代教育信息中心，太原030032；3. 國家深海基地管理中心，青島266237）

1 引 言

構(gòu)建精確、可靠、自主的導(dǎo)航系統(tǒng)一直是制約自主式水下無人航行器（Autonomous Underwater Vehicle，AUV）遠(yuǎn)距離深潛與自主作業(yè)的一個瓶頸問題［1］。根據(jù)導(dǎo)航所采用傳感器的不同，AUV 水下導(dǎo)航技術(shù)大致可分為三類：水聲學(xué)導(dǎo)航、慣性基導(dǎo)航、地球物理導(dǎo)航［2］。水聲學(xué)導(dǎo)航需要預(yù)先布置聲學(xué)基陣，不利于AUV 獨立作業(yè)，且導(dǎo)航范圍受限。慣性基導(dǎo)航不需要外部支持，導(dǎo)航推算所需的數(shù)據(jù)全部由自身攜帶的傳感器獲得，但其定位誤差隨時間累積，需要定期上浮接收衛(wèi)星信號修正，不適合AUV 長期、獨立、全天候、高精度的水下導(dǎo)航要求?；诘卮艌?、重力場、水下地形等地球物理信息導(dǎo)航由于具有全天候、無源性且不易受外界干擾的特點，成為各國AUV水下導(dǎo)航方法研究的熱點。地磁場、重力場導(dǎo)航由于受到測量方法和測量精度等因素的限制，在現(xiàn)階段很難應(yīng)用于實踐。隨著多波束測深系統(tǒng)的發(fā)展，高精度的海底地形測量成為可能，因此采用水下地形輔助導(dǎo)航（Underwater Terrain-Aided Navigation，UTAN）可以滿足AUV 水下精確導(dǎo)航定位的需求。UTAN不需要布放外部傳感器，不需要上浮修正，不存在累積誤差，可以長時間水下導(dǎo)航定位［3］。作為一種對慣性導(dǎo)航系統(tǒng)提供精確位置修正的有效手段，UTAN 技術(shù)在近十年得到了快速的發(fā)展，國外多個研究機構(gòu)或組織正致力于相關(guān)技術(shù)研究，并進行了水下地形匹配的海中試驗［4-6］。

現(xiàn)有的地形輔助導(dǎo)航算法大多為地形高度匹配算法，基于地形高度變化信息，可以通過UTAN算法估算載體位置。水下地形具有很強的非線性特征，UTAN 是一個非線性估計問題。因此，系統(tǒng)的后驗概率密度函數(shù)不能直接獲得，只能使用模型近似。一般來說，近似模型可以分為兩類：一類方法是使用一階泰勒展開將非線性模型轉(zhuǎn)換為線性模型，再使用卡爾曼濾波處理線性模型。由于地形的非線性，實際地形不能依靠一階泰勒展開來近似，濾波容易發(fā)散［7］。另一類為基于直接概率準(zhǔn)則的非線性估計方法。對于該類方法，Bergman N 等［8］在貝葉斯框架下提出了飛機的地形輔助導(dǎo)航模型，并指出問題實質(zhì)是非線性貝葉斯后驗概率估計。再引入了粒子濾波（Particle Filtering，PF）求解地形輔助導(dǎo)航的非線性貝葉斯估計［9］，取得了很好的效果。此后，PF 就成為地形輔助導(dǎo)航算法的研究熱點。

PF的主要缺陷是重采樣導(dǎo)致的粒子貧化問題，這將導(dǎo)致PF性能急劇下降。針對這一缺陷，近年來科研人員提出許多用于海底地形輔助導(dǎo)航的改進PF算法［10-12］。這些方法改善并優(yōu)化了重采樣過程，以增加粒子多樣性。本質(zhì)上，這些方法只能推遲顆粒耗盡，而不能完全消除。由于水下地形特征小于陸地地形，因此粒子貧化的影響更為嚴(yán)重。Kotecha J H 等［13］提出一種高斯和粒子濾波器（Gaussian Sum Particle Filtering，GSPF），使用高斯和分布函數(shù)作為PF 的重要密度函數(shù)，沒有重采樣過程，從原理上消除了顆粒消耗，并且無重采樣步驟也減少了濾波的計算量。

本文在建立基于貝葉斯估計的水下地形輔助模型基礎(chǔ)上，采用高斯和粒子濾波方法近似貝葉斯估計問題，并進行回放式仿真試驗。

圖1 基于多波束測深的AUV水下地形輔助導(dǎo)航示意圖Fig.1 Schematic diagram of terrain-aided navigation using multi-beam sounding

2 地形輔助導(dǎo)航的貝葉斯估計模型

由于深度計無累積誤差，因此在地形輔助導(dǎo)航模型中不需要考慮AUV垂向坐標(biāo)的變化，可建立水下地形輔助導(dǎo)航系統(tǒng)模型如下：

其中，式（1）為系統(tǒng)的狀態(tài)方程，式（2）為系統(tǒng)的量測方程。xt是AUV在t時刻的平面坐標(biāo)；ut是提供參考導(dǎo)航信息的慣性導(dǎo)航系統(tǒng)所指示的位移；vt是慣性導(dǎo)航系統(tǒng)的誤差；yt是實時測深數(shù)據(jù)；h（xt）是先驗數(shù)字地圖中xt位置的水深值；et是多波束測深誤差。

為了便于計算，假設(shè)vt、et均為高斯白噪聲，因此，t 時刻的系統(tǒng)的先驗概率密度函數(shù)（Probability Density Function，PDF）可用式（3）表示：

若用實時測深數(shù)據(jù)yt計算地形輔助導(dǎo)航系統(tǒng)誤差，基于貝葉斯原理，需得到系統(tǒng)的后驗PDF。假設(shè)條件獨立，貝葉斯估計的狀態(tài)更新方程為：

式（4）為后驗PDF，其分母是一個歸一化常量，且與xt無關(guān)。根據(jù)系統(tǒng)的狀態(tài)方程可知，由p(xt-1|yt-1)通過位置更新可得到狀態(tài)預(yù)測PDF，假設(shè)位置變量相互獨立，系統(tǒng)狀態(tài)預(yù)測方程如下：

由式（1）、（2）、（4）、（5）可得地形輔助導(dǎo)航的遞推Bayesian濾波估計模型為：

貝葉斯濾波最大困難是后驗PDF 的計算，對于一個非線性隨機系統(tǒng)，很難得到系統(tǒng)后驗PDF解析解，計算難度較大［14］。對此，本文用高斯和濾波近似貝葉斯濾波，并利用粒子濾波對地形輔助導(dǎo)航模型進行數(shù)值求解，提出一種基于高斯和粒子濾波的地形輔助導(dǎo)航方法。

3 水下地形輔助導(dǎo)航的GSPF算法

3.1 高斯和濾波器

通過多個高斯函數(shù)（分量）加強混合，高斯和濾波器（Gaussian Sum Filtering，GSF）可用來近似貝葉斯濾波。從原理上講，如果選取的高斯分量足夠多，GSF可無限逼近貝葉斯濾波的效果。由于式（1）和式（2）所描述的水下地形輔助導(dǎo)航模型中的噪聲均為高斯白噪聲，所以其在t 時刻的先驗PDF 可用高斯和函數(shù)p(·)近似。水下地形輔助導(dǎo)航的GSF模型可描述為：

（1）量測更新

在t 時刻，系統(tǒng)狀態(tài)的預(yù)測分布可用如下高斯和函數(shù)所示［15］：

其中，G 為并行GSF 的數(shù)目，μti，Σti，ωti可利用式（9）計算得到：

在獲取t 時刻的多波束測深數(shù)據(jù)后，后驗PDF如下：

（2）時間更新

由于p(xt|Yt)可用高斯和函數(shù)近似，因此p(xt+1|Yt)也可用高斯和函數(shù)近似，即：

3.2 地形輔助導(dǎo)航算法

基于有限高斯分量組成的高斯和函數(shù)，GSF 可近似系統(tǒng)狀態(tài)的后驗PDF。一般來說，其采用一組并行的擴展Kalman濾波器（Extended Kalman Filter，EKF）實現(xiàn)系統(tǒng)的狀態(tài)估計。而基于一階泰勒展開的EKF舍棄了高階項，對于強非線性系統(tǒng)濾波器存在發(fā)散的可能性。因此，本文用PF代替GSF中的并行EKF，提出一種基于GSPF 的水下地形輔助導(dǎo)航算法，其計算過程如下：

（1）初始化：t=0，且t0時刻系統(tǒng)狀態(tài)的先驗分布可定義為其中，ω0j=p(x0)中采樣獲得初始粒子集且粒子的權(quán)值

（2）采集多波束實時測深數(shù)據(jù)yt，并從先驗數(shù)字地圖中提取局部地形數(shù)據(jù)h（xt）；

（3）量測更新

（a）更新粒子的重要性權(quán)值：

（b）計算每個高斯分量的均值和協(xié)方差：

（c）計算高斯分量j，j=1，2，…G的權(quán)值：

（d）歸一化高斯分量權(quán)值：

（4）輸出地形輔助導(dǎo)航濾波的狀態(tài)估計和方差估計：

狀態(tài)估計：

方差估計：

（5）預(yù)測更新

（a）對于每一個高斯分量j，j=1，2，…G，在重要性密度函數(shù)中抽樣得到樣本集

（d）更新預(yù)測的均值和方差；

（6）基于式（1）預(yù)測t+1 時刻的參考導(dǎo)航位置xt+1；

（7）t=t+1，轉(zhuǎn)到步驟（2）。

本文提出的水下地形輔助導(dǎo)航算法流程如圖2所示。

圖2 水下地形輔助導(dǎo)航算法流程Fig.2 The Flow chart of UTAN based on GSPF

利用PF代替并行EKF，可避免EKF近似GSF所引入的誤差。與基于重采樣技術(shù)的PF 相比，GSPF利用預(yù)測更新得到粒子集，狀態(tài)的后驗PDF可用粒子和粒子權(quán)重表示。因此GSPF 無粒子退化現(xiàn)象，從原理上避免重采樣所帶來“粒子貧化”現(xiàn)象。同時沒有重采樣過程，GSPF 計算量與標(biāo)準(zhǔn)PF 相比較小，具有更好的實時性。

4 仿真結(jié)果及分析

圖3 所示為AUV 搭載多波束測深系統(tǒng)的某次水下地形測量所獲得的先驗數(shù)字地圖，其范圍的大小為1000m × 900m，最小網(wǎng)格分辨率為1m × 1m。該測量區(qū)域的水深在5～40m 之間，包含了溝壑、坑洼地帶、平坦地形等大部分近海海域所具有的水下地形特征。

圖3 先驗地形數(shù)據(jù)和獨立多波束測線Fig.3 The underwater digital terrain map and independent measuring line

圖3中黑色曲線所示的路線為海試中一次多波束測量所沿路線，用于模擬仿真試驗中實時多波束測深數(shù)據(jù)。該測線由試驗海區(qū)內(nèi)的一次獨立多波束測深試驗獲得，其數(shù)據(jù)不參與先驗地圖的制作，可保證實時測深數(shù)據(jù)和先驗數(shù)字地圖數(shù)據(jù)的相互獨立。

仿真試驗采用回放式仿真的形式，即實時測深數(shù)據(jù)通過不斷讀取獨立測線中的原始多波束數(shù)據(jù)進行模擬。由于實時測深數(shù)據(jù)和先驗數(shù)字地圖的數(shù)據(jù)均來源于海中試驗，因此回放式仿真可以模擬真實海洋環(huán)境條件下的水下地形輔助導(dǎo)航過程，從而驗證算法的可行性。分別基于高斯分量數(shù)目、粒子數(shù)、濾波周期以及初始定位誤差的不同，研究水下地形輔助導(dǎo)航算法精度的影響因素。

（1）初始定位誤差50m，濾波周期2s，粒子數(shù)2000，選取不同的高斯分量數(shù)目，得到的地形輔助導(dǎo)航濾波結(jié)果如圖4所示。

圖4 基于不同高斯分量數(shù)目的導(dǎo)航結(jié)果Fig.4 The navigation results with different Gaussian component number

作為對比，IPF 地形輔助導(dǎo)航算法［16］也被同時執(zhí)行。從圖4 可以看出，高斯分量數(shù)目的變化對濾波收斂速度無影響，但是高斯分量數(shù)目較少時，GSPF導(dǎo)航精度比IPF導(dǎo)航精度低；此時若增加高斯分量數(shù)目，GSPF 導(dǎo)航精度可達(dá)到IPF 精度；但若進一步增加高斯分量數(shù)目（＞10），GSPF 導(dǎo)航精度并未增加。如果粒子數(shù)相同，高斯分量的數(shù)目的增加將導(dǎo)致計算量的增加，因此在保證地形輔助導(dǎo)航精度的前提下，高斯分量數(shù)目的取值越小越好，本文中高斯分量數(shù)目的取值為10。

（2）初始定位誤差50m，濾波周期2s，高斯分量數(shù)目10，選取不同的粒子數(shù)，得到的地形輔助導(dǎo)航濾波結(jié)果如圖5所示。

從圖5 可以看出，不同粒子數(shù)的對導(dǎo)航濾波性能并不產(chǎn)生明顯影響，增加粒子數(shù)不能有效提高濾波收斂速度和改善濾波收斂后誤差區(qū)間的穩(wěn)態(tài)，而粒子數(shù)過多會影響地形輔助導(dǎo)航的實時性，因此粒子數(shù)的數(shù)目不能過多。

（3）初始定位誤差50m，高斯分量數(shù)目10，粒子數(shù)1000，選取不同的濾波周期，得到的地形輔助導(dǎo)航濾波結(jié)果如圖6所示。

從圖6 可以看出，濾波周期和濾波收斂所需的航行距離成反比：當(dāng)濾波周期為1s時，在經(jīng)過100m的航行后濾波已收斂；當(dāng)濾波周期為32s時，穿越整個匹配區(qū)域濾波仍發(fā)散。

圖5 基于不同粒子數(shù)的導(dǎo)航結(jié)果Fig.5 The navigation results with different particle number

圖6 基于不同濾波周期的導(dǎo)航結(jié)果Fig.6 The navigation results with different filtering period

（4）高斯分量數(shù)目10，粒子數(shù)1000，濾波周期2s，選取不同的初始定位誤差，得到的地形輔助導(dǎo)航濾波結(jié)果如圖7所示。

由圖7 可以看出，濾波收斂所需時間受初始定位誤差的影響較大。若初始定位誤差較小，在航行很短距離后濾波就可收斂；反之，若初始定位誤差較大，需要較長時間航行濾波才能收斂。

基于上述仿真試驗可知，在高斯分量數(shù)目、粒子數(shù)、濾波周期以及初始定位誤差等影響因素中，濾波周期和初始定位誤差對GSPF算法性能的影響較大。濾波周期和初始定位誤差越小，濾波收斂時間越短，地形輔助導(dǎo)航實時性越好。這是因為濾波周期短，迭代后粒子的權(quán)值與真實情況更為接近，初始定位誤差小，粒子的分布范圍更接近貝葉斯濾波的PDF分布。

圖7 基于不同初始誤差的導(dǎo)航結(jié)果Fig.7 The navigation results with different initial positioning error

5 結(jié) 論

本文提出了一種基于貝葉斯濾波模型的水下輔助匹配導(dǎo)航模型，并針對貝葉斯濾波后驗概率求解困難的問題，用高斯混合函數(shù)近似貝葉斯濾波的后驗概PDF，提出一種基于高斯和粒子濾波的水下地形輔助導(dǎo)航方法，并通過仿真試驗對地形輔助導(dǎo)航中的影響因素進行了研究，得到如下結(jié)論：

（1）與基于重采樣技術(shù)的粒子濾波算法相比，本文提出的方法通過濾波更新和預(yù)測獲得粒子，計算量較低。

（2）高斯分量數(shù)目較少時，隨著數(shù)目的增多，GSPF結(jié)果也更加接近重采樣粒子濾波結(jié)果，但當(dāng)高斯分量達(dá)到一定數(shù)量時，進一步增加其數(shù)目并不能有效改善濾波性能。

（3）基于GSPF 的地形輔助導(dǎo)航方法對初始定位誤差較為敏感，因此用其進行地形輔助導(dǎo)航時，應(yīng)盡可能減小導(dǎo)航系統(tǒng)的初始定位誤差。