馬 騰，丁碩碩，范佳佳，李 曄

（1.哈爾濱工程大學(xué) 水下機(jī)器人技術(shù)國家級(jí)重點(diǎn)實(shí)驗(yàn)室，黑龍江 哈爾濱 150001；2.哈爾濱理工大學(xué) 自動(dòng)化學(xué)院，黑龍江 哈爾濱 150001）

0 引言

自主水下機(jī)器人（Autonomous Underwater Vehicle，AUV）是執(zhí)行海底礦物勘探、水下管道檢測(cè)、海洋環(huán)境調(diào)查等任務(wù)的重要載體平臺(tái)[1]。AUV能否高效、精確地執(zhí)行海底勘探等任務(wù)取決于其導(dǎo)航精度[2]。在傳統(tǒng)導(dǎo)航方法中，由于無線電訊號(hào)在水下衰減嚴(yán)重，全球定位系統(tǒng)（Global Positioning System，GPS）無法在AUV水下航行時(shí)為其提供精確定位信息。慣性導(dǎo)航系統(tǒng)（Inertial Navigation System，INS）不僅需要價(jià)格昂貴的多普勒計(jì)程儀（Doppler Velocity Log，DVL）和航姿參考系統(tǒng)（Attitude and Heading Reference System，AHRS）輔助，其導(dǎo)航誤差累積特性也導(dǎo)致其難以作為AUV長時(shí)間水下航行的精確導(dǎo)航手段[3]。長基線（Long Baseline，LBL）、超短基線（Ultrashort Baseline，USBL）等聲學(xué)導(dǎo)航手段可在AUV水下航行過程中提供具備有界誤差的精確導(dǎo)航結(jié)果。但LBL聲學(xué)導(dǎo)航需要提前于海底布設(shè)多個(gè)聲信標(biāo)，使用USBL實(shí)現(xiàn)精確導(dǎo)航則需要AUV保持同支持母船的通訊，兩者均使AUV作業(yè)范圍受限嚴(yán)重[4]。海底地形匹配導(dǎo)航等地球物理導(dǎo)航手段，不依賴于衛(wèi)星、聲學(xué)信標(biāo)或支持母船等外部輔助，不存在累積誤差，可實(shí)現(xiàn)AUV長時(shí)間、全天候、遠(yuǎn)距離精確導(dǎo)航，已經(jīng)得到了世界各國研究人員的廣泛重視[5]。

目前，國內(nèi)外研究人員已利用多波束測(cè)深探測(cè)儀（Multibeam Echosounder，MBES）針對(duì)海底地形匹配導(dǎo)航方法展開大量理論和試驗(yàn)研究[6-7]。海底地形匹配導(dǎo)航最早是通過卡爾曼濾波（Kalman Filter，KF）[8]、擴(kuò)展卡爾曼濾波（Extended Kalman Filter，EKF）[9]、無跡卡爾曼濾波（Unscented Kalman Filter，UKF）[10]等參數(shù)濾波方法展開的。與傳統(tǒng)KF方法中以觀測(cè)到的水深值為測(cè)量更新不同，Li Peijua[8]等提出將迭代最近輪廓點(diǎn)（Iterated Closest Contour Point，ICCP）匹配結(jié)果與INS定位結(jié)果差值作為KF測(cè)量值，然后通過濾波器反饋校正及時(shí)校正SINS的累積誤差。

隨著計(jì)算機(jī)技術(shù)和非線性優(yōu)化技術(shù)的不斷進(jìn)步，粒子濾波器（Particle Filter，PF）、質(zhì)點(diǎn)濾波器（Point-Mass Filter，PMF）逐漸成為海底地形匹配導(dǎo)航問題的重要解決方案。Jind?ich Duník[11]等提出通過 RaoBlackwellized質(zhì)點(diǎn)濾波器解決海底地形匹配導(dǎo)航問題，其通過回放式仿真試驗(yàn)驗(yàn)證質(zhì)點(diǎn)濾波器在解決地形匹配導(dǎo)航問題時(shí)具備較高精度，對(duì)質(zhì)點(diǎn)初始化結(jié)果有極強(qiáng)魯棒性，計(jì)算復(fù)雜度較小且可通過理論推導(dǎo)預(yù)測(cè)。Georgios Salavasidis[12]等提出以聲學(xué)高度計(jì)為地形獲取手段，通過PF方法實(shí)現(xiàn)為期77小時(shí)的AUV長航程精確水下導(dǎo)航。周天[13]等在用于海底地形匹配導(dǎo)航的PF 中引入了變柵格尺寸的KLD（Kullback–Leibler distance）方法，極大降低了算法運(yùn)行所需的資源消耗。針對(duì)MBES測(cè)量過程中存在的地形畸變現(xiàn)象，Peng Dongdong[14]等提出利用Huber損失函數(shù)修改粒子權(quán)重計(jì)算方程，從而在重采樣過程中減少離群值對(duì)估計(jì)值的影響。

由于具備對(duì)非高斯噪聲和非線性運(yùn)動(dòng)模型的出色擬合能力，粒子濾波和質(zhì)點(diǎn)濾波技術(shù)逐漸成為海底地形匹配導(dǎo)航方法研究的主流，而考慮 AUV機(jī)載計(jì)算機(jī)數(shù)據(jù)處理能力受限，質(zhì)點(diǎn)濾波在海底地形匹配導(dǎo)航中具備更強(qiáng)的實(shí)用性。但如圖1所示，MBES測(cè)繪得到的海底地形測(cè)點(diǎn)空間分布不規(guī)則，而先驗(yàn)地圖則往往是網(wǎng)格化柵格數(shù)據(jù)，導(dǎo)致在質(zhì)點(diǎn)權(quán)重計(jì)算過程中，實(shí)測(cè)地形測(cè)點(diǎn)與先驗(yàn)數(shù)據(jù)對(duì)應(yīng)關(guān)系計(jì)算中存在大量插值過程，造成極大計(jì)算消耗。

圖1 先驗(yàn)地形圖與MBES實(shí)測(cè)地形數(shù)據(jù)Fig.1 A priori bathymetric map and bathymetric data measured using a MBES

為實(shí)現(xiàn)AUV的實(shí)時(shí)海底地形匹配精確導(dǎo)航，本文設(shè)計(jì)了一種海底地形匹配高效質(zhì)點(diǎn)濾波導(dǎo)航方法。本文提出通過構(gòu)建偽輸入高斯過程（Sparse Pseudo-input Gaussian Processes，SPGPs）模型實(shí)現(xiàn)對(duì)海底地形深度的高效、準(zhǔn)確估計(jì)，從而將MBES實(shí)時(shí)測(cè)繪地形數(shù)據(jù)網(wǎng)格化形成概率地圖；提出適用于概率地圖的質(zhì)點(diǎn)權(quán)重計(jì)算模型，并完成了基于概率地圖的海底地形匹配質(zhì)點(diǎn)濾波算法框架設(shè)計(jì)。最后，通過船載MBES采集的海試數(shù)據(jù)，對(duì)所提出的基于 SPGPs的測(cè)深數(shù)據(jù)網(wǎng)格化方法和海底地形匹配質(zhì)點(diǎn)濾波方法進(jìn)行了回放式仿真數(shù)據(jù)驗(yàn)證。

1 測(cè)深數(shù)據(jù)網(wǎng)格化概率地圖構(gòu)建方法

實(shí)測(cè)海底地形數(shù)據(jù)的網(wǎng)格化是質(zhì)點(diǎn)濾波器中最為耗時(shí)的過程。傳統(tǒng)網(wǎng)格化方法包括反距離加權(quán)（Inverse Distance Weighted，IDW）插值[15]、克里金（Kriging）插值法[16]等，其中，IDW 插值忽略了地形測(cè)點(diǎn)之間的相關(guān)性關(guān)系，導(dǎo)致插值結(jié)果誤差嚴(yán)重，而Kriging插值法雖然考慮了地形測(cè)點(diǎn)相關(guān)性，但其計(jì)算結(jié)構(gòu)較IDW插值法更為復(fù)雜，計(jì)算效率較低。

本文提出通過 SPGPs回歸方法實(shí)現(xiàn)網(wǎng)格化插值。SPGPs回歸方法其實(shí)質(zhì)是利用少量偽輸入點(diǎn)代替全部歷史數(shù)據(jù)實(shí)現(xiàn)高斯過程（Gaussian Process，GP）回歸中的 GP模型訓(xùn)練[17]。因而本文首先對(duì)GP回歸在網(wǎng)格化插值中的應(yīng)用進(jìn)行介紹。

GP模型可以看作一個(gè)輸入為歷史地形測(cè)深數(shù)據(jù)、輸出為多元高斯分布均值和方差的函數(shù)。換而言之，在包含N個(gè)地形測(cè)點(diǎn)的海底地形測(cè)深數(shù)據(jù)集中，假設(shè)N個(gè)測(cè)點(diǎn)的平面坐標(biāo)和深度分別為和，則可將其視為無限維度中從1個(gè)N變量高斯分布中取樣的單點(diǎn)，即N個(gè)測(cè)點(diǎn)的深度總是滿足1個(gè)聯(lián)合高斯分布N(μ,Σ)，其中μ和 Σ 分別為高斯分布的均值和方差。在GP模型中，由于地形測(cè)深數(shù)據(jù)輸入前會(huì)減去其地形深度均值，通常取μ=0，而Σ則由N個(gè)測(cè)點(diǎn)間相關(guān)性計(jì)算得到，且與測(cè)點(diǎn)水平位置的差值成正相關(guān)。

實(shí)現(xiàn)，該計(jì)算可以通過Gauss-Newton方法實(shí)現(xiàn)。

完成GP模型構(gòu)建后，利用該模型實(shí)現(xiàn)網(wǎng)格點(diǎn)地形高程插值。假設(shè)網(wǎng)格點(diǎn)水平坐標(biāo)為x*，其水深值z(mì)*滿足以下多元高斯分布：

圖2 高斯過程回歸的地形水深估計(jì)流程圖Fig.2 Flowchart of terrain depth estimate using the GP regression method

考慮采用全部歷史MBES測(cè)深數(shù)據(jù)訓(xùn)練GP模型會(huì)造成巨量計(jì)算消耗，在GP回歸算法的基礎(chǔ)上，本文提出在全部歷史 MBES測(cè)深數(shù)據(jù)中提取能夠保留大部分信息的少量偽輸入點(diǎn)，通過偽輸入點(diǎn)訓(xùn)練 SPGPs模型，從而實(shí)現(xiàn)測(cè)深數(shù)據(jù)的高效網(wǎng)格化操作。

假設(shè)M個(gè)偽輸入點(diǎn)（M遠(yuǎn)小于N）水平面坐標(biāo)為，則通過最大化邊緣概率：

即可同時(shí)求取X′和超參數(shù)θ。式（9）中，j?[1,M]；IN為大小為N×N的單位矩陣；Λ=diag(λ)且λ={λ1,λ2,...,λN}，其中，。

在通過Gauss-Newton方法求解式（9）后，水平坐標(biāo)為x*的網(wǎng)格插值點(diǎn)水深值z(mì)*滿足均值為*μ，均值為Σ*的高斯分布，且：

式（10）和式（11）中：

2 質(zhì)點(diǎn)濾波算法

本文所設(shè)計(jì)的海底地形匹配質(zhì)點(diǎn)濾波算法框架如圖3所示。

圖3 質(zhì)點(diǎn)濾波算法框架Fig.3 Framework of point-mass filter algorithm

傳統(tǒng)質(zhì)點(diǎn)濾波中通過計(jì)算MSD（Mean Square Displacement）算子實(shí)現(xiàn)質(zhì)點(diǎn)權(quán)重計(jì)算。假設(shè)質(zhì)點(diǎn)i在t時(shí)刻大小為N1×N2的網(wǎng)格化地圖實(shí)測(cè)水深表示為h′(i,j)(i=1,2,...,N1,j=1,2,...,N2)，對(duì)應(yīng)位置的先驗(yàn)地形圖水深值為h(i,j)，則其質(zhì)點(diǎn)i權(quán)重wi為

但是，以上方法忽略了實(shí)測(cè)地圖網(wǎng)格化過程中不同網(wǎng)格節(jié)點(diǎn)的置信度差別。如式（11）所示，SPGPs回歸方法的一大優(yōu)勢(shì)在于其可以在網(wǎng)格化插值過程中構(gòu)建概率地圖，即對(duì)各個(gè)插值節(jié)點(diǎn)的方差Σi,j進(jìn)行估計(jì)。因而本文提出一種適用于概率地圖的質(zhì)點(diǎn)權(quán)重計(jì)算方法，即將式（14）改寫為

式中，克羅內(nèi)克函數(shù)δ(i,j)為

3 回放式仿真試驗(yàn)

如圖4所示，于青島中沙礁海域開展了船載MBES的數(shù)據(jù)獲取海上試驗(yàn)。試驗(yàn)載體船與設(shè)備布置如圖5所示。

圖4 海試試驗(yàn)區(qū)域Fig.4 Sea trial area

圖5 試驗(yàn)船只與設(shè)備布置Fig.5 Experimental vessel and equipment layout

如圖6所示，試驗(yàn)使用T–sea CMBS200在線MBES作為海底地形數(shù)據(jù)測(cè)繪設(shè)備。T–sea CMBS200每次發(fā)射192個(gè)聲學(xué)脈沖信號(hào)，測(cè)線寬度約為MBES距海底距離的10倍，水平波束寬度優(yōu)于1°，垂直波束開角優(yōu)于 2°，水平視場(chǎng)角≥140°，測(cè)距離≥300 m，距離分辨率優(yōu)于2 cm，作業(yè)頻率為4 Hz。試驗(yàn)過程中載體運(yùn)動(dòng)狀態(tài)信息由圖 7所示的StarNeto XW–GI5651導(dǎo)航系統(tǒng)給出，在GPS信號(hào)輔助下，該系統(tǒng)艏向角測(cè)量誤差優(yōu)于0.1°，橫傾、縱傾角測(cè)量誤差優(yōu)于 0.01°，水平面定位誤差小于1 m，作業(yè)頻率為10 Hz。

圖6 T–sea CMBS200在線多波束聲吶Fig.6 T-sea CMBS200 online MBES

圖7 StarNeto XW–GI5651導(dǎo)航系統(tǒng)Fig.7 StarNeto XW-GI5651 navigation system

采用海上試驗(yàn)數(shù)據(jù)開展回放式仿真試驗(yàn)。軌跡長度約為1 800 m，載體航速約為4 kn，數(shù)據(jù)更新頻率為4 Hz，以GPS軌跡作為真實(shí)軌跡，慣性導(dǎo)航軌跡通過式（17）進(jìn)行模擬。

在軌跡上每隔25 s選取1個(gè)地形導(dǎo)航點(diǎn)，并進(jìn)行1次質(zhì)點(diǎn)權(quán)重計(jì)算和重采樣，輸出海底地形匹配質(zhì)點(diǎn)濾波導(dǎo)航結(jié)果。軌跡中共存在47個(gè)地形導(dǎo)航點(diǎn)，粒子數(shù)設(shè)定為500個(gè)，慣導(dǎo)系統(tǒng)初始誤差約為9.5 m。

如圖8所示，在第5個(gè)地形導(dǎo)航點(diǎn)后濾波器中質(zhì)點(diǎn)分布高度收斂于載體實(shí)際位置處，質(zhì)點(diǎn)濾波器輸出結(jié)果保持穩(wěn)定。如圖9和圖10所示，海底地形匹配質(zhì)點(diǎn)濾波導(dǎo)航結(jié)果與 GPS結(jié)果高度吻合，在任務(wù)過程當(dāng)中的47個(gè)地形導(dǎo)航點(diǎn)上，慣性導(dǎo)航系統(tǒng)和本文提出的地形匹配導(dǎo)航系統(tǒng)提供的 AUV定位誤差均值分別為25.39 m和1.46 m，中位數(shù)分別為26.38 m和1.26 m。算法在MATLAB平臺(tái)下執(zhí)行耗時(shí)為69.63 s，且其中53.14 s用于概率地圖構(gòu)建，總耗時(shí)占任務(wù)時(shí)間比例少于8%。

圖8 任務(wù)過程中質(zhì)點(diǎn)分布變化Fig.8 Changes in mass point distribution during the mission

圖9 導(dǎo)航系統(tǒng)輸出軌跡結(jié)果Fig.9 Trajectories yielded by navigation systems

圖10 任務(wù)過程中載體定位結(jié)果Fig.10 Location errors of the vessel during the mission

試驗(yàn)結(jié)果證明：所提出海底地形匹配高效質(zhì)點(diǎn)濾波導(dǎo)航方法可以在低成本慣性導(dǎo)航系統(tǒng)輔助下為AUV提供在線的精確導(dǎo)航信息。

4 結(jié)論

本文提出了一種海底地形匹配高效質(zhì)點(diǎn)濾波導(dǎo)航算法，并通過海測(cè)數(shù)據(jù)回放式仿真試驗(yàn)驗(yàn)證，得到以下結(jié)論：

1）相較于IDW插值和Kriging方法，SPGPs回歸用于網(wǎng)格化插值可同時(shí)達(dá)到插值精度和計(jì)算效率的最優(yōu)；

2）所提出海底地形匹配質(zhì)點(diǎn)濾波導(dǎo)航方法可獲得精確定位結(jié)果，其實(shí)時(shí)導(dǎo)航圓概率誤差小于2 m；

3）所提出海底地形匹配高效質(zhì)點(diǎn)濾波導(dǎo)航方法耗時(shí)遠(yuǎn)小于任務(wù)時(shí)常，可實(shí)現(xiàn) AUV在線精確導(dǎo)航。