張飛虎，稱晨升，王 璨

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

0 引言

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

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

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

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

1 因子圖SLAM概述

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

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

1.1 因子圖SLAM建模拓撲描述

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

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

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

1.2 因子圖SLAM建模數(shù)學機理

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

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

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

圖2 自適應變結構因子圖Fig. 2 Factor diagram of adaptive variable structure

根據(jù)變結構因子圖拓撲描述，其最大后驗概率估計可描述為

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

2 實驗

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

2.1 平臺搭建及預處理

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

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

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

2.2 室內(nèi)水池實驗

首先，在無人水下運載技術重點實驗室多功能水池進行了測試。水池長度為70 m，寬度為44 m，淺水區(qū)10 m，深水區(qū)15 m，足夠支撐我們完成算法的初步驗證。

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

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

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

圖6 實驗水池建圖效果Fig. 6 Construction effect of experimental pool

2.3 野外湖上實驗

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

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

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

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

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

針對湖上實驗過程中野外極端場景下，相似回環(huán)約束對算法的影響可通過圖10展現(xiàn)。將外場實驗中某段運行軌跡放大，可以看到紅色的軌跡是慣導的航位推算結果，相對比較平滑。受湖上相似場景導致的錯誤回環(huán)影響（暗礁以及樹枝在水下嚴重影響了ROV的探測和匹配性能），導致SLAM性能嚴重下降。其中，白色區(qū)域為所建立的地圖效果，可以看到，呈現(xiàn)出跳變及不收斂的現(xiàn)象。綜上所述，所提容錯性SLAM技術方案是可靠的。

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

為了定量分析 SLAM 的定位效果，即地圖的位置準確度，我們分別對真值坐標（GPS測量值）、慣導的推算坐標和 SLAM 算法的優(yōu)化值進行對比分析。

位置比較的結果如圖11-13所示，橫坐標代表時間軸，縱坐標代表x、y的值隨時間的變化，綠色線條表示慣導位置信息，藍色線條表示 SLAM算法優(yōu)化過的位置信息，紅色線條表示 GPS測量的真值。

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

圖11 場景1中3種位置坐標對比圖Fig. 11 Comparison of three position coordinates in scene 1

圖12 場景2中3種位置坐標對比圖Fig. 12 Comparison of three position coordinates in scene 2

圖13 場景3中3種位置坐標對比圖Fig. 13 Comparison of three position coordinates in scene 3

3 結論與先進性

將水池實驗以及湖上實驗反饋的結果進行總結分析，多波束聲吶SLAM實驗在建圖效果和定位精度方面都有較好的效果，證明了本實驗所用多波束聲吶 SLAM 在水下環(huán)境中的可行性和準確性?？梢钥隙ǎ诙嗖ㄊ晠鹊亩ㄎ痪群徒▓D效果突破了水下導航技術中的看不清和看不準2項瓶頸。

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

4 結束語

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

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

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