基于多波束聲吶的同時(shí)定位與地圖構(gòu)建

張飛虎，稱晨升，王 璨

（西北工業(yè)大學(xué) 航海學(xué)院，陜西 西安 710072）

0 引言

隨著科技的進(jìn)步，智能水下機(jī)器人（Autonomous Underwater Vehicle，AUV）已成為海洋調(diào)查、海洋資源開發(fā)、海洋考古救援以及海洋測(cè)繪的重要載體。AUV在水下環(huán)境中發(fā)揮重要的作用，主要在于其能在無(wú)人控制的情況下獨(dú)立完成復(fù)雜任務(wù)。由于AUV的自主能力主要受水下導(dǎo)航技術(shù)制約，因此，突破GPS受限場(chǎng)景下的高精度導(dǎo)航技術(shù)瓶頸，對(duì)實(shí)現(xiàn)AUV智能化具有重要的意義[1]。

通常，水下導(dǎo)航技術(shù)主要包括：1）使用慣性導(dǎo)航系統(tǒng)[2-4]。通過(guò)將陀螺系統(tǒng)與多普勒測(cè)速儀結(jié)合使用，實(shí)現(xiàn)小尺度定位與導(dǎo)航功能。然而，其面臨誤差累計(jì)的影響，需要經(jīng)過(guò)一定周期利用 GPS信號(hào)重新標(biāo)定。2）使用地磁導(dǎo)航系統(tǒng)。與慣性導(dǎo)航相反，地磁導(dǎo)航系統(tǒng)成本相對(duì)較低，且能提供全局航向基準(zhǔn)，但面臨地磁異常區(qū)域所導(dǎo)致的航向偏離以及無(wú)法準(zhǔn)確定位的問(wèn)題。綜上所述，無(wú)論是慣性導(dǎo)航還是地磁導(dǎo)航，在深遠(yuǎn)海領(lǐng)域中所面臨的迫切問(wèn)題可概括為2點(diǎn)︰1）對(duì)其自身位置難以進(jìn)行大尺度精確估計(jì)；2）對(duì)所處環(huán)境難以進(jìn)行高精度有效測(cè)繪。

令人欣慰的是，同時(shí)定位與地圖構(gòu)建（Simultaneous Localization and Mapping，SLAM）技術(shù)可以很好地解決上述難題[5]。當(dāng)前，視覺(jué) SLAM[6-9]以及激光SLAM[10-14]都已取得了很大進(jìn)展，并通過(guò)成果轉(zhuǎn)化實(shí)現(xiàn)商業(yè)部署，如無(wú)人駕駛/掃地機(jī)器人領(lǐng)域。相較于陸地，水下同時(shí)定位與地圖構(gòu)建主要面臨著“看不清”和“看不準(zhǔn)”等技術(shù)瓶頸：看不清是因?yàn)樵谏钏袥](méi)有光線，電磁信號(hào)在水下大幅衰減，很多在地面上可正常使用的傳感器（如GPS、激光雷達(dá)、視覺(jué)傳感器等）在水下無(wú)法使用[15-16]；看不準(zhǔn)是由于受地質(zhì)結(jié)構(gòu)制約，水下傳感器所獲的觀測(cè)數(shù)據(jù)面臨在不同區(qū)域難以有效區(qū)分，易導(dǎo)致將不同處相似場(chǎng)景識(shí)別為同一場(chǎng)景，進(jìn)而影響導(dǎo)航系統(tǒng)整體性能。因此，尋求一種具有容錯(cuò)特性的 SLAM方法，使其能夠針對(duì)水下復(fù)雜場(chǎng)景進(jìn)行全局優(yōu)化，很有必要。

本文深入研究水下相似場(chǎng)景的導(dǎo)航方法：首先，建立基于因子圖的全局優(yōu)化 SLAM 模型，將多波束聲吶與多普勒測(cè)速儀進(jìn)行耦合解算，構(gòu)建多約束方程確保導(dǎo)航邊界性條件。其次，圍繞導(dǎo)航過(guò)程中存在地形相似等場(chǎng)景，采用自適應(yīng)變結(jié)構(gòu)拓?fù)淠Ｐ?，通過(guò)優(yōu)化迭代保障SLAM的容錯(cuò)性。最后，通過(guò)搭建軟硬件平臺(tái)，對(duì)所提方案可靠性和魯棒性進(jìn)行了初步驗(yàn)證。

1 因子圖SLAM概述

借鑒美國(guó)國(guó)防部高級(jí)研究計(jì)劃局DARPA舉辦多屆的無(wú)人駕駛挑戰(zhàn)賽經(jīng)驗(yàn)，由于所有參賽車輛都必須在雨霧天中無(wú) GPS信號(hào)的情況下行走，因此同時(shí)定位與地圖構(gòu)建方法當(dāng)之無(wú)愧的成為 GPS受限場(chǎng)景下導(dǎo)航的核心算法[17]。

SLAM最初被提出是用于研究陸地機(jī)器人的，剛開始都是基于卡爾曼濾波原理[18]來(lái)解決 SLAM問(wèn)題，之后在卡爾曼濾波的基礎(chǔ)上發(fā)展出了擴(kuò)展卡爾曼濾波（Extended Kalman Filter，EKF），其具有原理簡(jiǎn)單且易于操作的特點(diǎn)，逐漸成為解決SLAM問(wèn)題的經(jīng)典方法[19]。隨后，研究者們又陸續(xù)提出了粒子濾波器 PF（Particle Filter）方法[20-21]、EIF（Extended Information Filter）方法[22]、無(wú)損卡爾曼濾波的 UKF（Unscented Kalman Filter）方法[23]等。2009年，研究者們提出了圖優(yōu)化理論，比較有代表性的是 Lourakis等人提出的基于圖優(yōu)化的SLAM解決方案[24]。

1.1 因子圖SLAM建模拓?fù)涿枋?/h3>

因子圖 SLAM[25-26]是通過(guò)圖優(yōu)化模型來(lái)實(shí)現(xiàn)同時(shí)定位與地圖構(gòu)建算法技術(shù)方案。如圖1所示，因子圖由節(jié)點(diǎn)和邊沿2種元素構(gòu)造而成。其中，x節(jié)點(diǎn)表示機(jī)器人的自身狀態(tài)信息，在估計(jì)過(guò)程中通過(guò)相應(yīng)的運(yùn)動(dòng)模型來(lái)構(gòu)建條件概率密度從而進(jìn)行非線性優(yōu)化。黑色邊沿代表里程計(jì)約束，在這里指機(jī)器人自身的運(yùn)動(dòng)模型。

圖1 基于圖優(yōu)化模型的SLAM表示方法Fig. 1 SLAM representation method based on graph optimization model

因此，基于因子圖的 SLAM 包含關(guān)聯(lián)和估計(jì)這兩個(gè)既相互獨(dú)立、又交叉影響的過(guò)程：1）關(guān)聯(lián)，傳感器獲得的量測(cè)和機(jī)器人自身位置、地圖路標(biāo)進(jìn)行匹配；2）估計(jì)，針對(duì)匹配結(jié)果在貝葉斯框架下進(jìn)行狀態(tài)估計(jì)。

1.2 因子圖SLAM建模數(shù)學(xué)機(jī)理

因子圖核心思想是根據(jù)里程計(jì)（慣導(dǎo)）及不同點(diǎn)的量測(cè)信息對(duì)機(jī)器人及其周圍環(huán)境進(jìn)行最大后驗(yàn)概率估計(jì)，即

式中：U是機(jī)器人的輸入控制；Z為所探測(cè)到得測(cè)量信息；X為平臺(tái)的姿態(tài)；M路標(biāo)點(diǎn)位置。假設(shè)系統(tǒng)過(guò)程模型和觀測(cè)模型服從高斯噪聲分布，即

針對(duì)地形近似所導(dǎo)致的錯(cuò)誤回環(huán)問(wèn)題，需考慮在圖優(yōu)化過(guò)程中引入開關(guān)變量通過(guò)非線性迭代，實(shí)現(xiàn)優(yōu)化過(guò)程中自適應(yīng)關(guān)閉由相似場(chǎng)景所導(dǎo)致的錯(cuò)誤回環(huán)，提高并完善同時(shí)定位與地圖構(gòu)建算法的容錯(cuò)性。如圖2所示，其中s表示由相似地形所導(dǎo)致的錯(cuò)誤回環(huán)。

圖2 自適應(yīng)變結(jié)構(gòu)因子圖Fig. 2 Factor diagram of adaptive variable structure

根據(jù)變結(jié)構(gòu)因子圖拓?fù)涿枋?，其最大后?yàn)概率估計(jì)可描述為

基于式（7），圖優(yōu)化 SLAM 算法可通過(guò)開關(guān)因子的區(qū)間估計(jì)來(lái)自適應(yīng)調(diào)整回環(huán)檢測(cè)約束的權(quán)值，從而具有在相似場(chǎng)景下錯(cuò)誤閉環(huán)的容錯(cuò)性能。

2 實(shí)驗(yàn)

基于上述的理論支撐，我們結(jié)合實(shí)驗(yàn)平臺(tái)BlueROV2的結(jié)構(gòu)和攜帶的傳感器特性，選擇了合適的SLAM算法框架，并完成完整SLAM算法的設(shè)計(jì)和測(cè)試。在實(shí)驗(yàn)中，我們使用聲吶作為感知單元，IMU和DVL作為定向和定位單元，把這些感知數(shù)據(jù)輸入到 SLAM 算法中，完成受限場(chǎng)景下的定位與建圖。

2.1 平臺(tái)搭建及預(yù)處理

本實(shí)驗(yàn)中使用的是水下機(jī)器人平臺(tái)BlueROV2（如圖3（a）所示）來(lái)完成水下航行器的SLAM實(shí)驗(yàn)。BlueROV2是一款小型的商用ROV，在原有設(shè)備基礎(chǔ)上，我們另外配備了 Oculus多波束圖像聲吶、PathfinderDVL多普勒測(cè)速儀等。當(dāng)前改裝完成的 BlueROV2（如圖 3（b）所示），能很好地滿足水下SLAM實(shí)驗(yàn)要求。

在真實(shí)環(huán)境中，多波束聲吶的數(shù)據(jù)會(huì)有很多雜波和野值，直接進(jìn)行建圖效果比較差，需要根據(jù)環(huán)境的不同進(jìn)行合適的濾波，以使數(shù)據(jù)能夠更好地反應(yīng)出環(huán)境特征，如圖4所示。在本實(shí)驗(yàn)中，對(duì)聲吶的原始數(shù)據(jù)進(jìn)行了處理，分別為閾值分割、數(shù)據(jù)轉(zhuǎn)換、基于距離約束的濾波。

圖4 聲吶數(shù)據(jù)處理Fig. 4 Sonar data processing

2.2 室內(nèi)水池實(shí)驗(yàn)

首先，在無(wú)人水下運(yùn)載技術(shù)重點(diǎn)實(shí)驗(yàn)室多功能水池進(jìn)行了測(cè)試。水池長(zhǎng)度為70 m，寬度為44 m，淺水區(qū)10 m，深水區(qū)15 m，足夠支撐我們完成算法的初步驗(yàn)證。

實(shí)驗(yàn)水池環(huán)境如圖5所示。在實(shí)驗(yàn)中，我們控制裝載多波束聲吶的BlueROV沿著水池的墻壁進(jìn)行掃描，繪制出水池的輪廓，并未在水池中間持續(xù)掃描，所以水池中部顯示未探測(cè)區(qū)域。

定位與建圖效果如圖 6所示。圖中，紅色部分代表航行器依靠慣導(dǎo)進(jìn)行航位推算的軌跡，白色部分代表探測(cè)的可行駛區(qū)域，黑色部分代表SLAM 所構(gòu)建的障礙物地圖，灰色部分代表未探測(cè)區(qū)域。

圖5 實(shí)驗(yàn)水池環(huán)境Fig. 5 Environment of experimental pool

圖6 實(shí)驗(yàn)水池建圖效果Fig. 6 Construction effect of experimental pool

2.3 野外湖上實(shí)驗(yàn)

在湖上實(shí)驗(yàn)過(guò)程中，乘坐小艇接近目標(biāo)環(huán)境，投放BlueROV2，控制航行器掃描目標(biāo)環(huán)境，進(jìn)行定位與建圖實(shí)驗(yàn)。在小艇上有 GPS定位天線，在BlueROV2掃描環(huán)境的過(guò)程中，小艇緊緊跟隨，以獲取BlueROV2附近的GPS坐標(biāo)，為定位與建圖實(shí)驗(yàn)的定量分析提供真實(shí)數(shù)據(jù)。圖7-9分別是3個(gè)實(shí)驗(yàn)場(chǎng)景的衛(wèi)星圖和實(shí)驗(yàn)效果。

由實(shí)驗(yàn)結(jié)果可以看出，建圖效果與衛(wèi)星圖大部分是一致的。對(duì)于衛(wèi)星圖與 SLAM 建立的地圖形狀存在偏差的地方，有以下3方面原因：1）由于水位的變化可能會(huì)導(dǎo)致衛(wèi)星圖與實(shí)際建立的地圖存在偏差；2）由于島嶼和灣的周圍分布有暗礁，使得聲吶掃描到暗礁，而不是我們從衛(wèi)星圖上看到的島嶼部分；3）岸邊一些凹進(jìn)去的地方里面有樹枝和暗礁，航行器不能靠的太近，導(dǎo)致建立的地圖有些斷開的部分。

圖7 場(chǎng)景1衛(wèi)星圖與SLAM建圖效果Fig. 7 Satellite image and SLAM construction effect in scene 1

圖8 場(chǎng)景2衛(wèi)星圖與SLAM建圖效果Fig. 8 Satellite image and SLAM construction effect in scene 2

圖9 場(chǎng)景3衛(wèi)星圖與SLAM建圖效果Fig. 9 Satellite image and SLAM construction effect in scene 3

針對(duì)湖上實(shí)驗(yàn)過(guò)程中野外極端場(chǎng)景下，相似回環(huán)約束對(duì)算法的影響可通過(guò)圖10展現(xiàn)。將外場(chǎng)實(shí)驗(yàn)中某段運(yùn)行軌跡放大，可以看到紅色的軌跡是慣導(dǎo)的航位推算結(jié)果，相對(duì)比較平滑。受湖上相似場(chǎng)景導(dǎo)致的錯(cuò)誤回環(huán)影響（暗礁以及樹枝在水下嚴(yán)重影響了ROV的探測(cè)和匹配性能），導(dǎo)致SLAM性能嚴(yán)重下降。其中，白色區(qū)域?yàn)樗⒌牡貓D效果，可以看到，呈現(xiàn)出跳變及不收斂的現(xiàn)象。綜上所述，所提容錯(cuò)性SLAM技術(shù)方案是可靠的。

圖10 相似場(chǎng)景所導(dǎo)致的錯(cuò)誤閉環(huán)效果Fig. 10 Error closed-loop effect caused by similar scenarios

為了定量分析 SLAM 的定位效果，即地圖的位置準(zhǔn)確度，我們分別對(duì)真值坐標(biāo)（GPS測(cè)量值）、慣導(dǎo)的推算坐標(biāo)和 SLAM 算法的優(yōu)化值進(jìn)行對(duì)比分析。

位置比較的結(jié)果如圖11-13所示，橫坐標(biāo)代表時(shí)間軸，縱坐標(biāo)代表x、y的值隨時(shí)間的變化，綠色線條表示慣導(dǎo)位置信息，藍(lán)色線條表示 SLAM算法優(yōu)化過(guò)的位置信息，紅色線條表示 GPS測(cè)量的真值。

由對(duì)比結(jié)果分析：1）慣導(dǎo)的位置，SLAM 優(yōu)化值和真值基本上保持一致，而隨著時(shí)間的推移，慣導(dǎo)的位置信息與真值的曲線變化規(guī)律還基本一致，但是它們之間的差值卻越來(lái)越大；2）SLAM優(yōu)化后的位置信息變化比較平穩(wěn)，數(shù)值上更加接近于真值，而在對(duì)環(huán)境掃描完成1個(gè)周期時(shí)，SLAM優(yōu)化的位置信息與真值非常接近。

圖11 場(chǎng)景1中3種位置坐標(biāo)對(duì)比圖Fig. 11 Comparison of three position coordinates in scene 1

圖12 場(chǎng)景2中3種位置坐標(biāo)對(duì)比圖Fig. 12 Comparison of three position coordinates in scene 2

圖13 場(chǎng)景3中3種位置坐標(biāo)對(duì)比圖Fig. 13 Comparison of three position coordinates in scene 3

3 結(jié)論與先進(jìn)性

將水池實(shí)驗(yàn)以及湖上實(shí)驗(yàn)反饋的結(jié)果進(jìn)行總結(jié)分析，多波束聲吶SLAM實(shí)驗(yàn)在建圖效果和定位精度方面都有較好的效果，證明了本實(shí)驗(yàn)所用多波束聲吶 SLAM 在水下環(huán)境中的可行性和準(zhǔn)確性?？梢钥隙?，基于多波束聲吶的定位精度和建圖效果突破了水下導(dǎo)航技術(shù)中的看不清和看不準(zhǔn)2項(xiàng)瓶頸。

在此基礎(chǔ)上，其先進(jìn)性還包括3點(diǎn)：1）如果沒(méi)有回環(huán)場(chǎng)景，SLAM誤差增長(zhǎng)速率小于慣導(dǎo)誤差增長(zhǎng)速率；2）如果存在回環(huán)場(chǎng)景，SLAM 誤差收斂在穩(wěn)定范圍；3）如果存在由相似場(chǎng)景所導(dǎo)致的錯(cuò)誤回環(huán)，SLAM能具有一定容錯(cuò)性，使得誤差繼續(xù)收斂在穩(wěn)定范圍。

4 結(jié)束語(yǔ)

本文主要研究了基于聲吶的自主水下航行器的定位與建圖，且自主水下航行器的定位與建圖實(shí)驗(yàn)在實(shí)驗(yàn)水池和野外湖泊都取得了不錯(cuò)的效果。這其中也存在一些不足之處，在未來(lái)的工作中需要繼續(xù)努力完成。

1）本文中主要使用聲吶圖像數(shù)據(jù)轉(zhuǎn)換為激光雷達(dá)數(shù)據(jù)格式來(lái)進(jìn)行 SLAM 試驗(yàn)，這其中涉及到聲吶的濾波和轉(zhuǎn)換過(guò)程的準(zhǔn)確匹配度。當(dāng)前還是存在一些問(wèn)題，并不能很好地把想要的環(huán)境特征完全提取出來(lái)，且轉(zhuǎn)換后的激光雷達(dá)數(shù)據(jù)與真正的激光雷達(dá)數(shù)據(jù)在描述環(huán)境特征方面存在一些差異，導(dǎo)致基于點(diǎn)云匹配的算法不能很好地使用，容易出現(xiàn)誤匹配。對(duì)于這個(gè)問(wèn)題，未來(lái)的工作中會(huì)繼續(xù)試驗(yàn)測(cè)試，研究更好的濾波方式，從而提高數(shù)據(jù)轉(zhuǎn)換準(zhǔn)確率。

2）本論文中對(duì)自主水下航行器的定位與建圖的實(shí)驗(yàn)驗(yàn)證平臺(tái)為BlueROV2，因?yàn)楸疚牟豢紤]控制，所以用其來(lái)驗(yàn)證 SLAM 算法的可行性和效果與使用AUV實(shí)質(zhì)上并無(wú)太大差別。但BlueROV2質(zhì)量輕且?guī)Ю|，在野外湖泊中進(jìn)行試驗(yàn)很容易受到風(fēng)浪的影響，導(dǎo)致對(duì)其位姿的估計(jì)出現(xiàn)較大偏差，建圖效果就會(huì)比較差。在未來(lái)的工作中，我們將使用AUV再次對(duì)本文的算法進(jìn)行驗(yàn)證，以便獲得更好的定位與建圖效果。