朱益賢，馬騰*，范佳佳，漆池，李曄

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

1 引 言

目前，智能水下機(jī)器人（Autonomous Underwater Vehicle，AUV）已成為實(shí)現(xiàn)大規(guī)模高分辨率地形測(cè)繪的重要載體平臺(tái)，但AUV獲取的海底地圖精度取決于AUV導(dǎo)航系統(tǒng)的誤差。慣性導(dǎo)航系統(tǒng)（Inertial Navigation System，INS）、長(zhǎng)基線（Long Baseline，LBL）聲學(xué)定位系統(tǒng)、超短基線（Ultrashort Baseline，USBL）聲學(xué)定位系統(tǒng)都能為AUV提供不同精度的定位服務(wù)。但現(xiàn)有方法中，即使有多普勒測(cè)速儀（Doppler Velocity Log，DVL）和航姿參考系統(tǒng)（Attitude and Heading Reference System，AHRS）的輔助，INS導(dǎo)航誤差仍然隨著時(shí)間無限增長(zhǎng)。LBL聲學(xué)定位系統(tǒng)的定位精度很高，誤差只有幾厘米，但是將基線應(yīng)答器布置在海床上是一項(xiàng)復(fù)雜的工作，時(shí)間和資金花費(fèi)較大。USBL聲學(xué)定位系統(tǒng)的應(yīng)答器安裝在支持母船上，其定位精度取決于AUV與母船之間的距離，誤差大約是母船和AUV間距離的0.5%，這些缺點(diǎn)使其不適用于深海導(dǎo)航。地球物理導(dǎo)航系統(tǒng)諸如地形匹配導(dǎo)航系統(tǒng)、地磁場(chǎng)導(dǎo)航系統(tǒng)的導(dǎo)航誤差收斂且有界，但是地球物理導(dǎo)航系統(tǒng)順利作業(yè)的前提是提前獲得工作海域的地形/地磁先驗(yàn)海圖。

近來，測(cè)深信息同步定位與建圖（Bathymetric Simultaneous Localization and Mapping，BSLAM）已經(jīng)成為了為AUV提供高精度導(dǎo)航和高一致性海圖測(cè)繪結(jié)果的有效手段。在測(cè)深信息SLAM中，AUV首先同時(shí)使用多波束測(cè)深聲吶產(chǎn)生的測(cè)深數(shù)據(jù)和慣性導(dǎo)航系統(tǒng)測(cè)得的位姿信息來構(gòu)建海底地形圖，然后根據(jù)當(dāng)前測(cè)深信息和歷史測(cè)深信息的相似性進(jìn)行閉環(huán)檢測(cè)，最后根據(jù)閉環(huán)檢測(cè)結(jié)果實(shí)現(xiàn)AUV導(dǎo)航和建圖誤差約束。

使用濾波技術(shù)或者圖優(yōu)化方法的測(cè)深信息SLAM技術(shù)已經(jīng)取得了許多突破，但是通過地形匹配過程進(jìn)行的閉環(huán)檢測(cè)（本文中也稱為測(cè)深數(shù)據(jù)關(guān)聯(lián)）依然是約束AUV導(dǎo)航誤差的唯一有效數(shù)據(jù)關(guān)聯(lián)。然而，只有當(dāng)AUV回到曾經(jīng)到達(dá)過的位置時(shí)才能進(jìn)行閉環(huán)檢測(cè)。在深海地形測(cè)繪任務(wù)中，位置回訪會(huì)造成大量時(shí)間和能源浪費(fèi)，因此，AUV通常會(huì)在任務(wù)進(jìn)行一段時(shí)間后執(zhí)行位置回訪和閉環(huán)檢測(cè)，這也意味著AUV在任務(wù)開始之后的很長(zhǎng)一段時(shí)間內(nèi)只能依靠慣性導(dǎo)航系統(tǒng)進(jìn)行定位，其導(dǎo)航誤差隨時(shí)間不斷累積。

然而，事實(shí)上對(duì)AUV位置的實(shí)時(shí)估計(jì)是不可或缺的。首先，實(shí)時(shí)位置估計(jì)有助于AUV沿著預(yù)定軌跡航行。在AUV預(yù)設(shè)路徑上準(zhǔn)確執(zhí)行路徑跟蹤對(duì)于完整有效地繪制全覆蓋海底地形圖是必要的。其次，實(shí)時(shí)位置估計(jì)有助于減少無效的測(cè)深數(shù)據(jù)關(guān)聯(lián)。在測(cè)深信息SLAM中，有效測(cè)深數(shù)據(jù)關(guān)聯(lián)被檢測(cè)出來之前，慣性導(dǎo)航系統(tǒng)誤差無界增長(zhǎng)，AUV位置估計(jì)置信區(qū)間不斷擴(kuò)大并最終覆蓋全部歷史測(cè)深數(shù)據(jù)，導(dǎo)致需要通過在當(dāng)前實(shí)測(cè)地形和所有歷史測(cè)深數(shù)據(jù)之間開展相似性計(jì)算來檢測(cè)測(cè)深數(shù)據(jù)關(guān)聯(lián)［1-2］。由于海底過于平坦的地形，導(dǎo)致大部分的測(cè)深數(shù)據(jù)關(guān)聯(lián)都是無效的。如果可以實(shí)現(xiàn)AUV位置實(shí)時(shí)估計(jì)，測(cè)深數(shù)據(jù)關(guān)聯(lián)檢測(cè)中僅需要考慮小部分在AUV估計(jì)位置附近的量測(cè)，這將使得無效測(cè)深數(shù)據(jù)關(guān)聯(lián)的數(shù)量大大減少［3-4］。

通過融合多波束測(cè)深聲吶的測(cè)深數(shù)據(jù)和其他視覺/聲學(xué)傳感器數(shù)據(jù)可以為執(zhí)行深海測(cè)繪任務(wù)的AUV提供實(shí)時(shí)位置估計(jì)［5］。例如，Kunz等［1］提出了一種融合多波束測(cè)深聲吶測(cè)深信息和水下相機(jī)視覺信息的SLAM技術(shù)，試驗(yàn)結(jié)果表明：融合了兩種傳感器的SLAM算法要比只使用任意單一傳感器的算法效果更好。Lagudi等［2］、Campos等［4］、Bodenmann等［6］也做了相似的工作。然而，水下照相機(jī)和多波束測(cè)深聲吶對(duì)AUV距底距離的需求截然不同，即水下照相機(jī)只能在靠近海床15 m或以內(nèi)的地方工作（若水質(zhì)混濁，距離將更短），而多波束測(cè)深聲吶的聲帶寬度是和AUV與海床的距離成正比，通常需要AUV距底30 m左右以獲取較大范圍地形測(cè)繪結(jié)果。

因此，本文提出了一種融合測(cè)深/測(cè)距信息的水下SLAM方法。在該方法中，AUV通過多波束測(cè)深聲吶獲取海底地形測(cè)深數(shù)據(jù)，并同時(shí)獲取與位置未知的聲信標(biāo)間測(cè)距信息。AUV通過測(cè)深數(shù)據(jù)與測(cè)距信息，構(gòu)建包含測(cè)深數(shù)據(jù)關(guān)聯(lián)、測(cè)距數(shù)據(jù)關(guān)聯(lián)、里程計(jì)約束、AUV位姿和信標(biāo)位置的位姿圖，通過最小化位姿圖中所有數(shù)據(jù)關(guān)聯(lián)/約束之間的不一致性來估計(jì)AUV的實(shí)際軌跡和信標(biāo)的位置。為了解決測(cè)距異常值問題，本文提出了一種基于概率圖形分割算法的測(cè)距異常值拒絕方法。此外，本文還提出了一種通過在位姿圖里刪除節(jié)點(diǎn)來降低本文所提出的SLAM計(jì)算復(fù)雜度的應(yīng)用信息論節(jié)點(diǎn)約簡(jiǎn)算法。

本文組織如下：第2節(jié)首先概述國內(nèi)外相關(guān)工作，第3節(jié)描述了融合測(cè)深/測(cè)距信息的水下SLAM工作機(jī)制，第4節(jié)討論了仿真試驗(yàn)結(jié)果，第5節(jié)給出了本研究的結(jié)論和對(duì)未來工作的簡(jiǎn)要討論。

2 國內(nèi)外相關(guān)研究

2.1 測(cè)深信息SLAM技術(shù)

與激光SLAM相似，大多數(shù)水下測(cè)深SLAM算法都是基于擴(kuò)展卡爾曼濾波（Extended Kalman Filter，EKF）和粒子濾波（Particle Filter，PF）等濾波方法設(shè)計(jì)的。Fairfield等［7］引入了Rao-Blackwellized粒子濾波器（Rao-Blackwellized Particle Filter，RBPF）來處理測(cè)深SLAM問題，并使用配備有單波束測(cè)深聲吶的AUV進(jìn)行了現(xiàn)場(chǎng)試驗(yàn)。在Fairfield工作的基礎(chǔ)上，Norgren等［8］應(yīng)用測(cè)深分布式粒子濾波SLAM（Bathymetric Distributed Particle Filter SLAM，BPSLAM）繪制冰蓋地圖。Norgren等將多波束測(cè)深聲吶在AUV中向上安裝指向冰面，收集冰山底部冰形數(shù)據(jù)；使用EKF算法預(yù)測(cè)冰山的漂移速度，同時(shí)使用BPSLAM估計(jì)AUV在冰山固定坐標(biāo)系中的位姿。Jang等［9］提出了一種基于EKF算法的測(cè)深SLAM方法，算法中考慮到AUV位置的不確定性，使用動(dòng)態(tài)網(wǎng)格方法表示地形表面，以避免AUV導(dǎo)航誤差導(dǎo)致的無效測(cè)量更新。Palomer等［10］使用激光掃描儀代替多波束測(cè)深聲吶測(cè)量海底地形，提出了一種基于位姿的EKF-SLAM方法，以解決測(cè)深SLAM中的全SLAM（Full SLAM）問題。

使用當(dāng)前測(cè)深數(shù)據(jù)和歷史測(cè)深數(shù)據(jù)之間的地形匹配過程來檢測(cè)測(cè)深SLAM中的測(cè)深數(shù)據(jù)關(guān)聯(lián)時(shí)，由于海床地形趨于平坦，將檢測(cè)到大量無效的測(cè)深數(shù)據(jù)關(guān)聯(lián)，并且當(dāng)新檢測(cè)到這些關(guān)聯(lián)時(shí)，很難對(duì)其關(guān)聯(lián)有效性進(jìn)行實(shí)時(shí)判別，往往需要在之后某一時(shí)刻結(jié)合新的數(shù)據(jù)關(guān)聯(lián)才能實(shí)現(xiàn)對(duì)其有效性的判別［11］。基于濾波方法的測(cè)深SLAM算法無法處理無效的歷史測(cè)深數(shù)據(jù)關(guān)聯(lián)，即一旦濾波測(cè)深SLAM算法將某一關(guān)聯(lián)識(shí)別為有效，該關(guān)聯(lián)對(duì)AUV軌跡估計(jì)的影響將再也無法消除。圖優(yōu)化算法在每次檢測(cè)到新的數(shù)據(jù)關(guān)聯(lián)后，都使用所有數(shù)據(jù)關(guān)聯(lián)重新優(yōu)化軌跡，即圖優(yōu)化算法可以在任意時(shí)刻剔除無效數(shù)據(jù)關(guān)聯(lián)對(duì)AUV軌跡估計(jì)的影響，這使得圖優(yōu)化方法更適合于解決測(cè)深SLAM問題［12］。

Torroba等［13］提出了一種圖優(yōu)化測(cè)深SLAM算法，該算法融合了測(cè)深數(shù)據(jù)關(guān)聯(lián)和航位推算約束，以優(yōu)化航行器的整個(gè)軌跡，還通過最大化地圖的自身一致性估計(jì)車輛的當(dāng)前位姿。Torroba等已經(jīng)通過仿真和現(xiàn)場(chǎng)試驗(yàn)測(cè)試了所提出的算法。Massot-Campos等［14］使用基于激光的結(jié)構(gòu)光進(jìn)行了基于圖的測(cè)深SLAM試驗(yàn)，并引入了重建三維子地圖的成對(duì)配準(zhǔn)，用于基于圖的測(cè)深SLAM中的無效測(cè)深數(shù)據(jù)關(guān)聯(lián)檢測(cè)。Ma等［15］提出了一種穩(wěn)健的測(cè)深SLAM，其中提出了多窗口一致性方法（Multi-windows Consistency Method，MCM），以根據(jù)地圖一致性識(shí)別無效的測(cè)深數(shù)據(jù)關(guān)聯(lián)。為了減少基于圖優(yōu)化的測(cè)深SLAM的存儲(chǔ)需求［16］，Bore等［17］提出了高斯過程圖優(yōu)化SLAM，該方法應(yīng)用稀疏高斯過程來存儲(chǔ)局部測(cè)深圖。

2.2 僅測(cè)距SLAM技術(shù)

在測(cè)距信息（Range-Only，RO）SLAM中，AUV可以根據(jù)自身和信標(biāo)之間的距離測(cè)量來估計(jì)其位置和信標(biāo)位置。大多數(shù)RO-SLAM算法基于濾波方法，如EKF［18-19］、RBPF［20-21］、快速SLAM［22］和稀疏擴(kuò)展信息濾波方法（Sparse Extended Information Filter，SEIF）［23-25］。Fabresse等［19］提出了一種基于EKF的RO-SLAM算法，在該算法中，應(yīng)用單方程、僅測(cè)距觀測(cè)模型來更新所有假設(shè)的信標(biāo)位置。Fabresse等引入了AUV與路標(biāo)間測(cè)距信息和路標(biāo)間測(cè)距模型，以減少計(jì)算要求并縮短算法定位誤差收斂時(shí)間。Torres-González等［23］提出了一種可擴(kuò)展的分布式RO-SLAM方案，用于資源受限的航行器運(yùn)行。他們?cè)诨赟EIF的RO-SLAM中引入了分布式信息驅(qū)動(dòng)的測(cè)量分配工具，用于測(cè)距值選擇，以平衡AUV精確定位需求和算法計(jì)算消耗。Kim等［26］提出了一種高效的協(xié)同動(dòng)態(tài)RO-SLAM（Efficient Cooperative Dynamic Range-Only SLAM，eCDRO- SLAM）算法，該算法通過引入一些技巧（如簡(jiǎn)化歸一化、對(duì)稱高斯分布和、去除低概率點(diǎn)），大幅提高了協(xié)作動(dòng)態(tài)RO-SLAM的計(jì)算效率。

圖優(yōu)化也被證明是解決RO-SLAM問題的可行方法。Herranz等［27］在處理RO-SLAM問題時(shí)對(duì)EKF算法與平滑和建圖算法（Smoothing And Mapping，SAM）進(jìn)行了比較，他們證明SAM方法比EKF方法在RO-SLAM中更加準(zhǔn)確高效。

3 融合測(cè)深/測(cè)距信息的水下同步定位與建圖技術(shù)

3.1 系統(tǒng)定義與數(shù)據(jù)關(guān)聯(lián)

融合了測(cè)深/測(cè)距信息的水下同步定位與建圖技術(shù)原理如圖1所示，AUV使用多波束測(cè)深聲吶連續(xù)收集包含M個(gè)測(cè)點(diǎn)的測(cè)深條帶，同時(shí)AUV和信標(biāo)之間進(jìn)行聲學(xué)信號(hào)交換來獲取距離信息，該距離信息代表了AUV和信標(biāo)之間的距離［28］。例如，距離量測(cè)代表i時(shí)刻AUV位置（X）i和信標(biāo)k位置（Xb，k）距離。

圖1 融合測(cè)深/測(cè)距SLAM系統(tǒng)原理Fig.1 System definition of underwater SLAM fusing bathymetric and range information

結(jié)合測(cè)深和測(cè)距數(shù)據(jù)，可構(gòu)建圖優(yōu)化SLAM中的位姿圖。如圖2所示，位姿圖中的節(jié)點(diǎn)表示AUV的位姿。連接節(jié)點(diǎn)的邊則表示AUV在不同時(shí)刻位姿間的數(shù)據(jù)關(guān)聯(lián)，包括里程計(jì)約束、測(cè)深數(shù)據(jù)關(guān)聯(lián)和測(cè)距數(shù)據(jù)關(guān)聯(lián)。

圖2 融合測(cè)深/測(cè)距SLAM構(gòu)建的位姿圖Fig.2 Pose-graph in underwater SLAM fusing bathymetric and range information

里程計(jì)約束通常由AUV的離散狀態(tài)方程得出

式中，Xi={xi，yi，hi，φi，θi，ψi}是AUV在i時(shí)刻的位姿，xi，yi，hi是AUV在大地坐標(biāo)系下分別對(duì)應(yīng)于東、北、地三軸的位置坐標(biāo)，φi，θi，ψi分別代表AUV繞著東、北、地三軸的旋轉(zhuǎn)角度。ui={vi，ωi}是包含了線速度vi和角速度ωi的控制向量，可以通過螺旋槳和舵控制。wi是服從高斯分布N(0，Q)的過程噪聲。由于光纖陀螺儀和深度計(jì)的測(cè)量精度比慣導(dǎo)系統(tǒng)的測(cè)量精度高得多，所以本文只考慮AUV東向位置xi和北向位置yi的估計(jì)誤差。

測(cè)深數(shù)據(jù)關(guān)聯(lián)中，i時(shí)刻通過多波束測(cè)深聲吶獲取的第j個(gè)測(cè)點(diǎn)在大地坐標(biāo)系下位置為

式中，αj和βj分別是多波束測(cè)深聲吶發(fā)出第j束波的回波角和波長(zhǎng)，vMBES代表服從高斯分布的多波束測(cè)深聲吶量測(cè)噪聲，Rxi矩陣為

在測(cè)深SLAM中，所有測(cè)點(diǎn)將會(huì)被儲(chǔ)存為子地圖，當(dāng)前子地圖和歷史子地圖的地形匹配過程是獲得測(cè)深數(shù)據(jù)關(guān)聯(lián)的有效方法［29］。因?yàn)樽拥貓D中有大量的測(cè)點(diǎn)存在，多波束測(cè)深聲吶的量測(cè)噪聲可視為高斯白噪聲，在地形匹配過程通常可以忽略。因此，本文中的測(cè)深數(shù)據(jù)關(guān)聯(lián)為

式中，l是通過地形匹配過程得到的T1與T2時(shí)刻AUV的相對(duì)位姿，vb是服從高斯分布Ν(0，Qb)的地形匹配噪聲。

AUV和信標(biāo)之間的測(cè)距模型為

式中，vr～N(0，Qr)是i時(shí)刻AUV位置和信標(biāo)k位置間距的量測(cè)噪聲。不同于深度值已知的AUV位置坐標(biāo)估計(jì)，信標(biāo)位置估計(jì)需要考慮東軸、北軸和深度軸三個(gè)維度上的坐標(biāo)。

因?yàn)闇y(cè)距信息不能為信標(biāo)位置估計(jì)提供足夠的自由度（Degree of Freedom，DOF），新量測(cè)到的信標(biāo)位置需要進(jìn)行初始化。第k個(gè)信標(biāo)的初始位置計(jì)算公式為

式中，υi，ωi，xi，yi和θi已經(jīng)在式（1）中闡述。不滿足式（7）的測(cè)距信息將不會(huì)代入式（6）中進(jìn)行AUV初始位置估計(jì)。AUV真實(shí)運(yùn)動(dòng)軌跡和信標(biāo)位置可以通過圖優(yōu)化算法得到。為了減少位姿圖中的節(jié)點(diǎn)數(shù)目，僅當(dāng)AUV位姿包含測(cè)距數(shù)據(jù)關(guān)聯(lián)或測(cè)深數(shù)據(jù)關(guān)聯(lián)時(shí)，才認(rèn)為其為位姿圖中的節(jié)點(diǎn)，對(duì)應(yīng)的時(shí)刻稱為關(guān)鍵幀。融合了里程計(jì)約束，k個(gè)測(cè)深數(shù)據(jù)關(guān)聯(lián)，m個(gè)距離量測(cè)數(shù)據(jù)關(guān)聯(lián)，AUV關(guān)鍵幀位姿和信標(biāo)位置估算公式為

式中，X*={X1，…，Xn，Xb，1，…，Xb，N}，n表示關(guān)鍵幀的數(shù)目，m表示信標(biāo)的數(shù)目。AUV非關(guān)鍵幀位置估計(jì)可以通過臨近關(guān)鍵幀位置線性插值得到。式（8）可以通過高斯-牛頓法來解，t時(shí)刻X*迭代的初始值是t-1時(shí)刻式（8）的最優(yōu)解。

3.2 測(cè)距和測(cè)深中的野值拒絕機(jī)制

3.2.1 測(cè)距野值拒絕機(jī)制

測(cè)距野值會(huì)導(dǎo)致信標(biāo)位置的錯(cuò)誤估計(jì)，進(jìn)而導(dǎo)致本文提出的SLAM算法失效。根據(jù)式（6）得到信標(biāo)位置估計(jì)Xb，k，距離量測(cè)滿足不等式（9）時(shí)，被認(rèn)為是野值

式（9）中，Rt是一個(gè)給定的臨界值，Qi是i時(shí)刻AUV位姿估計(jì)的協(xié)方差矩陣。如果i時(shí)刻是關(guān)鍵幀，則Qi直接可以通過關(guān)鍵幀位姿估計(jì)協(xié)方差矩陣的求法［31］計(jì)算，否則可以由式（10）計(jì)算

式中，Qnode是從0到i時(shí)刻和i時(shí)刻最鄰近關(guān)鍵幀的協(xié)方差矩陣。tnode，i代表最鄰近時(shí)刻關(guān)鍵幀和i時(shí)刻之間的時(shí)間間隔，Q是式（1）中過程噪聲矩陣。

式（6）中的測(cè)距野值拒絕機(jī)制與前述方法截然不同。因?yàn)樵跊]有信標(biāo)位置初始估計(jì)值的情況下，無法運(yùn)用式（9）來判斷距離量測(cè)Zb=是否為野值。圖分割算法是判斷距離量測(cè)是否連續(xù)的有效方法，并被廣泛應(yīng)用于測(cè)距導(dǎo)航。圖分割算法中，每一個(gè)節(jié)點(diǎn)代表一次量測(cè)，兩個(gè)節(jié)點(diǎn)之間的連線稱為邊，表示對(duì)應(yīng)的兩次量測(cè)是連續(xù)的。N次量測(cè)中連續(xù)的一套量測(cè)為

式中，A是一個(gè)N×N的矩陣，并且

式中，V是一個(gè)N×1的向量，并且

在測(cè)距導(dǎo)航中，以i時(shí)刻和j時(shí)刻AUV位置為圓心，分別以距離量測(cè)zi和zj為半徑畫圓，如果圓i和圓j在信標(biāo)位置附近有交點(diǎn)，則認(rèn)為這兩次量測(cè)是連續(xù)的。然而在SLAM算法中，信標(biāo)的位置是未知的。如圖3所示，以i時(shí)刻和j時(shí)刻AUV位置為圓心，分別以距離量測(cè)和為半徑畫圓，兩圓有交點(diǎn)不能成為和連續(xù)的證據(jù)。因此本文提出了一種概率圖分割算法。

圖3 測(cè)距圓交點(diǎn)法不適用于融合測(cè)深/測(cè)距SLAMFig.3 Range measurement circle intersection method is unsuitable for SLAM fusing bathymetric and range information

如圖4所示，在概率圖分割算法中，一對(duì)數(shù)據(jù)關(guān)聯(lián)沒有被標(biāo)記為連續(xù)或者不連續(xù)，而是存在一種概率標(biāo)簽。更具體來說，N次距離量測(cè)中，如果和有交點(diǎn)，則Ai，j為

圖4 圖分割問題的普通圖和概率圖Fig.4 Graph and probabilistic graph representing the graph partitioning problem

式中，N是AUV和信標(biāo)k之間的量測(cè)數(shù)目，代表距離量測(cè)和之間連續(xù)程度，有

式中，代表距離量測(cè)和的交點(diǎn)，Xl代表l時(shí)刻AUV的位置，代表l時(shí)刻AUV和信標(biāo)k間距。因此，圖4（b）中和都比和要大。因此距離量測(cè)2可以被剔除掉。

應(yīng)注意到，只有當(dāng)長(zhǎng)時(shí)間通信中斷后AUV恢復(fù)與信標(biāo)通信時(shí)采用概率圖分割算法，其他情況下都采用式（9）表示的測(cè)距野值拒絕方法。

3.2.2 測(cè)深野值拒絕機(jī)制

沒有AUV位置實(shí)時(shí)估計(jì)的情況下，難以判斷新檢測(cè)到的測(cè)深數(shù)據(jù)關(guān)聯(lián)是否有效，地圖一致性檢測(cè)是解決該問題有效的方法，但這也導(dǎo)致了巨大的運(yùn)算量。不同于測(cè)深SLAM，通過測(cè)距信息得到的AUV位置實(shí)時(shí)估計(jì)能夠判斷新檢測(cè)到的測(cè)深數(shù)據(jù)關(guān)聯(lián)是否有效。

與已知信標(biāo)位置的測(cè)距野值拒絕機(jī)制類似，測(cè)深數(shù)據(jù)關(guān)聯(lián)的野值判斷式為

式中，T1時(shí)刻AUV位置估計(jì)協(xié)方差矩陣QT1，T2時(shí)刻AUV位置估計(jì)協(xié)方差矩陣QT2可以通過式（10）計(jì)算得到，并且

應(yīng)當(dāng)指出，式（16）提出的異常值拒絕機(jī)制能夠辨別出大多數(shù)但是并非全部無效測(cè)深數(shù)據(jù)關(guān)聯(lián)，MCM方法［15］也被應(yīng)用到了融合測(cè)深/測(cè)距信息的水下SLAM技術(shù)中。但是很顯然，只有小部分無效數(shù)據(jù)關(guān)聯(lián)需要通過MCM方法才能辨識(shí)出。

3.3 信息論節(jié)點(diǎn)約簡(jiǎn)算法

不同于基于濾波的SLAM算法，當(dāng)一個(gè)新的數(shù)據(jù)關(guān)聯(lián)被檢測(cè)出來時(shí)，圖優(yōu)化SLAM算法會(huì)重新優(yōu)化AUV所有歷史位姿和信標(biāo)位置。隨著位姿圖中節(jié)點(diǎn)和邊的數(shù)目無限增長(zhǎng)，每一次軌跡優(yōu)化需要消耗的計(jì)算資源也越來越大。雖然融合測(cè)深/測(cè)距信息的水下SLAM技術(shù)能夠?qū)UV實(shí)時(shí)位置進(jìn)行準(zhǔn)確估計(jì)，但也導(dǎo)致位姿圖里出現(xiàn)了大量的節(jié)點(diǎn)，如圖5所示。

圖5 應(yīng)用信息論節(jié)點(diǎn)約簡(jiǎn)算法的位姿圖Fig.5 Pose-graph using information-theoretic node reduction

對(duì)信標(biāo)位置有相似估計(jì)的節(jié)點(diǎn)被認(rèn)為是重復(fù)的，可剔除的。理想信息增長(zhǎng)通常可以降低由于距離量測(cè)帶來的信標(biāo)位置不確定性，并且可以通過熵來估計(jì)。

在有N次距離量測(cè)的情況下，熵的計(jì)算公式為

在式（19）中，U={u0，u1，…，ut-1}，并且

式中，由于可以認(rèn)為INS測(cè)得的AUV幀間運(yùn)動(dòng)的不確定性相同，故=tQ，這也意味著式（20）可以被簡(jiǎn)化為時(shí)間t的函數(shù)。因此，考慮一個(gè)時(shí)間臨界值T，式（19）可以簡(jiǎn)化為

根據(jù)貝葉斯公式得

式中，代表信標(biāo)k的位置估計(jì)。式（22）中Xt取決于Xb，k，取決于Xb，k和Xt的共同狀態(tài)。因此，式（22）可以被簡(jiǎn)化為

式中，η是一個(gè)歸一化因子

以上公式中

故有

根據(jù)式（26），本文提出了一種應(yīng)用信息論節(jié)點(diǎn)約簡(jiǎn)算法（Information-Theoretic Node Reduction，ITNR）。如圖6所示，信標(biāo)k對(duì)應(yīng)一個(gè)長(zhǎng)度為T（圖6中T=3）的移動(dòng)窗口，將當(dāng)前窗口內(nèi)所有AUV位置（圖6中是從Xi到Xi+2）代入式（26）計(jì)算熵。如果計(jì)算出來的熵H大于臨界熵值HT，則移動(dòng)窗口的起點(diǎn)由Xi前進(jìn)到Xi+3（如圖6（a）所示），否則移動(dòng)窗口的起點(diǎn)只向前移動(dòng)一個(gè)時(shí)間步長(zhǎng)，從Xi前進(jìn)到Xi+1（如圖6（b）所示）。

圖6 應(yīng)用信息論節(jié)點(diǎn)約簡(jiǎn)算法中移動(dòng)窗口的運(yùn)動(dòng)Fig.6 Movement of sliding windows in node reduction method

在圖6中，如果使用移動(dòng)窗口中所有AUV位姿Xi、Xi+1和Xi+2計(jì)算得到的熵H小于HT，則窗口中除了保留滿足下式的Xj，其他節(jié)點(diǎn)都會(huì)被剔除掉。

4 仿真試驗(yàn)

仿真試驗(yàn)中的AUV運(yùn)動(dòng)軌跡和測(cè)深數(shù)據(jù)都是在青島市中沙礁附近的海上試驗(yàn)中采集得到。海上試驗(yàn)用T-SEA CMBS200船載多波束測(cè)深聲吶采集測(cè)深數(shù)據(jù)，用量角精度為O（0.1deg）的StarNeto XW-GI5651 INS/GNSS系統(tǒng)中的陀螺儀和慣性測(cè)量單元來測(cè)量AUV的位置坐標(biāo)和姿態(tài)角。T-SEA CMBS200船載多波束測(cè)深聲吶能夠以4 Hz的頻率在120°的扇形范圍內(nèi)發(fā)射192道聲波用于傳遞與接收信息。對(duì)于軌跡誤差計(jì)算十分重要的AUV真實(shí)運(yùn)動(dòng)軌跡使用StarNeto XW-GI5651 INS/GNSS中的GNNS接收機(jī)提供的GPS數(shù)據(jù)，定位精度為O（0.1 m）。AUV工作深度設(shè)置為水下5 m，并且認(rèn)為在任務(wù)過程中，其工作面深度不變。進(jìn)行AUV和信標(biāo)之間距離量測(cè)仿真［32］，AUV位置Xi和信標(biāo)位置Xb，k之間的距離量測(cè)為

式中，τi，k～N(0，1)是距離量測(cè)噪聲，Γi，k為

仿真試驗(yàn)的參數(shù)設(shè)置如表1所示。

表1 仿真試驗(yàn)參數(shù)設(shè)置Table 1 Setup in simulation experiments

AUV和信標(biāo)的定位誤差、絕對(duì)軌跡誤差（Absolute Trajectory Error，ATE）和建圖誤差都可以用來描述SLAM方法的定位精度和建圖精度［33］。定位誤差是指AUV及信標(biāo)的估計(jì)位置和真實(shí)位置的距離。AUV的絕對(duì)軌跡誤差eATE可以用估計(jì)軌跡X={X0，X1，…，XN}和實(shí)際軌跡計(jì)算得到

式中，N是AUV軌跡中位姿節(jié)點(diǎn)的數(shù)目。M是地圖中測(cè)點(diǎn)數(shù)目，估計(jì)測(cè)深為H={h1，h2，…，hM}，實(shí)際深度HR={hR1，hR2，…，hRM}，地形匹配誤差為

本部分使用了測(cè)深SLAM方法和RO-SLAM方法作為對(duì)比試驗(yàn)。其中，測(cè)深SLAM運(yùn)用了MCM方法［15］來增強(qiáng)魯棒性，RO-SLAM運(yùn)用了圖優(yōu)化算法［27］。

4.1 八信標(biāo)仿真試驗(yàn)

仿真試驗(yàn)比較了本文提出的融合測(cè)深/測(cè)距信息SLAM算法、RO-SLAM算法和測(cè)深SLAM算法。圖7中紫色的點(diǎn)代表信標(biāo)的真實(shí)位置，黑色虛線圓代表水下5 m處信標(biāo)的通信范圍（假定AUV的工作深度是水下5 m）。信標(biāo)深度和海床地形深度一致，AUV在某一固定深度面工作。

圖7 仿真試驗(yàn)中的海底地形圖及信標(biāo)位置Fig.7 Topography map and locations of beacons in simulation

本文提出融合測(cè)深/測(cè)距信息SLAM算法、RO-SLAM算法和測(cè)深SLAM算法分別得出的AUV實(shí)時(shí)定位結(jié)果如圖8所示，得出的信標(biāo)位置最終估計(jì)結(jié)果如圖9所示。

圖8 八信標(biāo)工況下三種SLAM算法實(shí)時(shí)定位表現(xiàn)Fig.8 Real-time performance of three SLAM methods with 8 beacons

圖9 僅測(cè)距SLAM和融合測(cè)深/測(cè)距SLAM對(duì)信標(biāo)的最終定位誤差Fig.9 The final positioning error of beacons using RO-SLAM and SLAM fusing bathymetric and range information

融合測(cè)深/測(cè)距信息SLAM算法ATE的平均值是9.55 m，最大值是16.22 m。RO-SLAM相應(yīng)的結(jié)果為11.58 m和18.82 m。誤匹配導(dǎo)致測(cè)深SLAM相應(yīng)的結(jié)果最大，為21.40 m和71.93 m。在第2800 s和第130 s的測(cè)深數(shù)據(jù)關(guān)聯(lián)被檢測(cè)出來之前，融合測(cè)深/測(cè)距信息SLAM算法ATE和ROSLAM算法ATE差不多，在此之后直到任務(wù)結(jié)束，本文提出的算法都保持著最小ATE值。由此可見，對(duì)于AUV位姿準(zhǔn)確的、實(shí)時(shí)的估計(jì)有助于剔除無效數(shù)據(jù)關(guān)聯(lián)。任務(wù)結(jié)束時(shí)，RO-SLAM算法、測(cè)深SLAM算法、本文提出的算法ATE值分別為18.27 m、19.39 m、7.79 m。由此可見，八信標(biāo)工況下，融合測(cè)深/測(cè)距信息SLAM算法的定位精度比另外兩種算法都要高。

本文提出算法和RO-SLAM算法對(duì)于信標(biāo)位置的最終定位誤差如圖9所示。本文提出方法對(duì)所有信標(biāo)的最終定位誤差都小于10 m，然而RO-SLAM算法對(duì)信標(biāo)的平均定位誤差接近18.3 m。由此可見，本文提出算法能夠給出更準(zhǔn)確的信標(biāo)位置估計(jì)。

融合測(cè)深/測(cè)距信息SLAM算法給出的最終AUV軌跡、建圖結(jié)果和建圖誤差如圖10所示。由圖10（a）可見，本文提出算法得出的AUV軌跡與真實(shí)運(yùn)動(dòng)軌跡幾乎重合。圖10（b）給出了本文提出算法的建圖結(jié)果。圖10（c）表明了本文提出算法的建圖誤差，平均建圖誤差為0.42 m，68.84%的測(cè)點(diǎn)的建圖誤差小于0.5 m。

圖10 八信標(biāo)融合測(cè)深/測(cè)距信息SLAM算法結(jié)果Fig.10 Results of SLAM fusing bathymetric and range information with 8 beacons

融合測(cè)深/測(cè)距信息SLAM算法精度受信標(biāo)有效測(cè)距范圍在AUV作業(yè)區(qū)域中的占比影響，更具體而言，受到信標(biāo)位置和數(shù)目影響，在實(shí)際工程運(yùn)用中，信標(biāo)通常采用無動(dòng)力下潛方式布放至海底，其在深水應(yīng)用時(shí)下沉至海底后準(zhǔn)確位置往往難以控制，因而增加信標(biāo)數(shù)量是實(shí)現(xiàn)融合測(cè)深/測(cè)距信息SLAM算法精確導(dǎo)航定位的有效途徑。但另一方面，當(dāng)信標(biāo)數(shù)目過多，通信范圍嚴(yán)重重疊時(shí)，不僅產(chǎn)生了高昂的導(dǎo)航成本，算法還會(huì)獲得大量測(cè)距關(guān)鍵幀，導(dǎo)致后端軌跡優(yōu)化占用大量運(yùn)算資源，RO-SLAM和本文提的算法都會(huì)失去導(dǎo)航實(shí)時(shí)性。本節(jié)中，仿真試驗(yàn)在0.64 km2作業(yè)區(qū)域內(nèi)隨機(jī)布置了八個(gè)信標(biāo)，八信標(biāo)理論最大覆蓋率為100%，實(shí)際覆蓋率為89.2%，試驗(yàn)結(jié)果證明，理論最大覆蓋率為100%時(shí)融合測(cè)深/測(cè)距信息SLAM算法可取得精確導(dǎo)航定位和建圖結(jié)果。

4.2 概率圖分割算法剔除測(cè)距異常值試驗(yàn)

本文提出了一種概率圖分割算法來剔除長(zhǎng)時(shí)間通信中斷后AUV恢復(fù)與信標(biāo)通信時(shí)的測(cè)距異常值，為了檢驗(yàn)概率圖分割算法的有效性，進(jìn)行概率圖分割算法剔除測(cè)距異常值試驗(yàn)。

本試驗(yàn)選取了AUV連續(xù)七個(gè)時(shí)刻與同一個(gè)信標(biāo)的測(cè)距數(shù)據(jù)，其中包括兩個(gè)異常值。更改異常值的個(gè)數(shù)，將數(shù)據(jù)代入概率圖分割算法中進(jìn)行判斷。試驗(yàn)結(jié)果如表2所示。

由表2可見，在未知信標(biāo)位置的情況下，本文提出的概率圖分割算法成功地識(shí)別出距離量測(cè)中的異常值。

4.3 位姿圖冗余節(jié)點(diǎn)剔除試驗(yàn)

本文提出了應(yīng)用信息論的節(jié)點(diǎn)約簡(jiǎn)算法來剔除位姿圖中對(duì)于信標(biāo)位置有相似估計(jì)的節(jié)點(diǎn)，從而達(dá)到減少軌跡優(yōu)化計(jì)算量、增強(qiáng)算法實(shí)時(shí)性的目的。為了檢驗(yàn)ITNR方法的有效性，進(jìn)行位姿圖冗余節(jié)點(diǎn)剔除試驗(yàn)。

本次試驗(yàn)對(duì)比了運(yùn)用ITNR進(jìn)行節(jié)點(diǎn)約簡(jiǎn)的融合測(cè)深/測(cè)距信息SLAM算法和不運(yùn)用ITNR的融合測(cè)深/測(cè)距信息SLAM算法，進(jìn)行了最終保留的關(guān)鍵幀數(shù)目和算法運(yùn)行時(shí)間對(duì)比。

本次試驗(yàn)電腦的基本配置為：Intel（R） Core（TM） i5-9300H CPU @ 2.40 GHz 2.40 GHz，內(nèi)存16 GB。在以上環(huán)境進(jìn)行位姿圖冗余節(jié)點(diǎn)剔除試驗(yàn)。試驗(yàn)結(jié)果如表3所示。

表3 兩種算法運(yùn)行結(jié)果比較Table 3 Comparison between two algorithms

由表3可見，在本次試驗(yàn)中，ITNR算法剔除了61個(gè)冗余節(jié)點(diǎn)，不使用ITNR融合SLAM算法用時(shí)大約是使用ITNR融合SLAM算法的2.5倍。由此可見ITNR算法能有效地約簡(jiǎn)位姿圖節(jié)點(diǎn)，提高運(yùn)算效率。

5 結(jié) 論

本文首先針對(duì)傳統(tǒng)SLAM方法在檢測(cè)到有效閉環(huán)前定位誤差快速發(fā)散的問題，提出了一種融合測(cè)深/測(cè)距信息的水下SLAM技術(shù)，該技術(shù)吸收了RO-SLAM與測(cè)深SLAM的優(yōu)點(diǎn)，能為AUV提供精確的實(shí)時(shí)定位與建圖結(jié)果。然后通過對(duì)八信標(biāo)工況下海上實(shí)測(cè)數(shù)據(jù)回放仿真試驗(yàn)，證實(shí)了本文提出算法的有效性，并得出了下面主要結(jié)論：

（1）本文提出的融合測(cè)深/測(cè)距信息的水下SLAM技術(shù)能夠解決傳統(tǒng)SLAM方法在檢測(cè)到有效閉環(huán)前定位誤差快速發(fā)散的問題。對(duì)比三種算法全過程定位ATE可知，本文提出算法比測(cè)深SLAM和RO-SLAM的定位效果好。

（2）概率圖分割方法能準(zhǔn)確地識(shí)別出測(cè)距異常值，提高了本文提出算法的魯棒性。

（3）常規(guī)圖優(yōu)化算法位姿圖中含有大量節(jié)點(diǎn)，運(yùn)算耗時(shí)長(zhǎng)，而應(yīng)用信息論節(jié)點(diǎn)約簡(jiǎn)算法可以剔除掉位姿圖中大量的多余節(jié)點(diǎn)，減少程序運(yùn)算時(shí)間。

本文中，AUV沿著預(yù)定軌跡進(jìn)行海底建圖任務(wù)，這導(dǎo)致AUV機(jī)動(dòng)性不足、抗干擾能力弱。根據(jù)AUV和信標(biāo)的位置估計(jì)，實(shí)時(shí)規(guī)劃AUV運(yùn)動(dòng)軌跡既有助于增強(qiáng)信標(biāo)的能觀性，也有助于構(gòu)建更加精確的測(cè)深圖。因此在未來的工作中，我們將應(yīng)用動(dòng)態(tài)SLAM方法來實(shí)時(shí)控制AUV運(yùn)動(dòng)，從而高效地獲得更加精確的海底地形圖。