一種基于多波束測深儀的水下同步定位與地圖構建方法

戴天， 繆玲娟， 邵海俊

(北京理工大學 自動化學院, 北京 100081)

0 引言

自主水下潛航器(AUV)是探索海洋的重要工具之一，導航系統是AUV的重要組成部分，高精度、高可靠性的導航系統可以持續為AUV提供準確的位置與姿態信息，是AUV長時間水下作業的重要保障[1-2]。大多數AUV均配備慣性導航系統(INS)作為主導航系統[2]，但是INS的誤差隨時間累積而發散。提高加速度計和陀螺儀的精度可以延緩誤差累積的速度，但是無法從根本上消除累積誤差[3-4]。通常情況下，會使用其他導航系統作為輔助導航系統對INS累積誤差進行修正。水下環境中，使用重力場、地磁場和地形等地球物理信息作為信息源的數據庫輔助導航系統(DBRNS)，能夠很好地解決INS累積誤差的校正問題[1,5-7]。但是DBRNS的有效運行需要預先獲取相應信息的高分辨率數字基準圖，否則DBRNS無法作為輔助導航系統校正INS的累積誤差。

同步定位與地圖構建(SLAM)技術不依賴基準圖，可以在無基準圖時作為輔助導航手段來校正INS的累積誤差。SLAM最早于1986年由美國舊金山IEEE機器人與自動化會議提出[8]，SLAM方法通過實時感知周圍的環境特征定位自身的位置與姿態，再根據自身位置與姿態構建周圍環境的地圖，從而達到SLAM的目的。根據算法的不同，SLAM可以分為基于貝葉斯濾波的SLAM和圖優化SLAM[9-10].

基于貝葉斯濾波的SLAM多見于早期的研究，該類方法根據機器人運動模型和傳感器量測模型對問題進行建模，在隱形馬爾可夫假設下，實現系統的狀態更新和觀測更新[11]。基于貝葉斯濾波的SLAM中，基于擴展卡爾曼濾波器的SLAM(EKF-SLAM)算法和基于Rao-Blackwellized粒子濾波器的SLAM(RBPF-SLAM)算法是研究熱點。EKF-SLAM算法適用于弱非線性系統，但是該類算法在運行過程中需要不斷添加新的路標，在大規模環境下難以使用；RBPF-SLAM算法適用于強非線性及非高斯系統，但是該類算法計算量很大。基于貝葉斯濾波的SLAM假定下一時刻的狀態只與前一時刻有關，不考慮前一時刻之前的歷史記錄，長時間運行情況下由傳感器噪聲不確定性引起的誤差不斷累積，最終將導致地圖的不一致[12]。

圖優化SLAM采用全局優化方式解決SLAM問題，該類方法將所有狀態看成變量，將運動方程和觀測方程看成變量間的約束，然后構造誤差函數并最小化該誤差函數的二次型[13]。該技術被越來越多地應用于大規模、非結構化環境中，在此背景下基于貝葉斯濾波的SLAM逐漸被圖優化SLAM取代。

基于視覺傳感器的圖優化SLAM方法是目前最受關注的SLAM方法之一，如果將基于視覺傳感器的圖優化SLAM方法用于水下導航任務，則需要滿足3個條件才能保證導航精度：1)載體貼近海底航行；2)水域有足夠高的清晰度；3)海底地形為結構化環境[14]。但是這3個條件很難長時間同時保持，因此基于視覺傳感器的圖優化SLAM方法在水下導航任務中難以取得很好的定位精度。多波束測深儀探測距離遠，且對水域的清晰度與海底環境的結構性特征幾乎沒有要求，非常適用于水下導航任務。

本文采用多波束測深儀作為傳感器，提出一種基于多波束測深儀的圖優化SLAM方法。為了減少錯誤閉環檢測對優化結果所造成的不良影響，該方法在通用圖優化SLAM方法的前端部分增加了誤匹配檢測模塊。此外，為了提升閉環檢測的精度，該方法將局部灰度值編碼算法[15]用于閉環檢測模塊。

本文簡要介紹通用圖優化SLAM方法及其組成部分，指出通用SLAM方法在處理水下導航問題時的不足，并給出基于多波束測深儀的圖優化SLAM方法；對新方法中前端部分的閉環檢測模塊和誤匹配檢測模塊進行詳細闡述；簡要介紹了后端部分的優化原理；基于真實海底地形圖進行仿真實驗，展示兩種SLAM方法對INS指示軌跡的校正結果，并對結果進行分析。

1 通用圖優化SLAM方法

圖優化SLAM方法利用圖模型對SLAM問題建模，將SLAM問題劃分為前端和后端兩個部分，前端負責圖的構建，后端負責圖的優化。通用圖優化SLAM方法如圖1所示[12]。

圖1 通用圖優化SLAM方法Fig.1 General graph SLAM method

圖優化SLAM方法在前端部分構建的圖由節點和邊組成，節點表示載體離散的位置與姿態以及對應傳感器的實測值，邊表示節點之間的約束，約束關系一般由兩個節點之間的位置與姿態變換關系構成。如圖1中前端部分所示，圖優化SLAM方法通常采用兩種方式構建約束關系，一種是順序數據關聯，另一種是閉環檢測。

順序數據關聯是建立連續節點之間的相對位置變換，該類約束關系的建立需要傳感器能夠不斷地重復觀測到地圖特征，大部分視覺SLAM方法均可以構建該類約束，而基于多波束測深儀的SLAM方法難以通過順序數據關聯構建約束關系。一般情況下，水下多波束測深儀可以獲得垂直于船體下方的條帶區域內多個測量點的地形數據，但是連續的觀測數據之間很少包含重復信息，因此通過順序數據關聯建立約束難以實現。

閉環檢測是檢測載體是否回到先前經過的位置，該類約束是一種強約束關系，一旦檢測到閉環，將對整個位置與姿態圖進行較大程度的修正。本文所提SLAM方法在每一個節點存儲該點對應的子圖，通過設定S形航線，檢測不同節點所對應的子圖是否包含重疊區域，進而完成閉環檢測。

基于多波束測深儀的圖優化SLAM方法構建的圖模型如圖2所示，圖2中節點{x0，x1，…，xi+1}表示載體的位置與姿態以及子圖。

圖2 基于多波束測深儀的圖優化SLAM方法中圖結構Fig.2 Graph structure of multi-beam sonar-based graph SLAM method

2 基于多波束測深儀的圖優化SLAM方法

采用通用圖優化SLAM方法處理基于多波束測深儀的水下SLAM問題，存在如下兩個缺陷：

1)水下長航導航任務中，難以通過順序數據關聯建立連續節點之間的約束關系；

2)閉環檢測對優化結果影響很大，現有方法難以保證檢測精度。

為了解決上述兩個問題，本文對通用圖優化SLAM方法進行改進，提出一種基于多波束測深儀的圖優化SLAM方法，如圖3所示。新方法在通用圖優化SLAM方法的基礎上，移除了順序數據關聯模塊，并引入誤匹配檢測模塊以剔除無效閉環。

圖3 基于多波束測深儀的圖優化SLAM方法Fig.3 Multi-beam sonar-based graph SLAM method

2.1 閉環檢測

2.1.1 問題描述

將航行過程中多波束測深儀傳回的測深數據轉換至大地坐標系可構建測深圖，以圖優化SLAM方法中節點所在位置為中心，按指定的長度和寬度將測深圖劃分為若干個子圖。在構造新子圖時，可以使用模板匹配方法檢測新子圖與歷史子圖之間是否包含閉環(重疊部分)。

(1)

圖4 子圖匹配過程示意圖Fig.4 Matching process of subgraph

(2)

(3)

Xp+lp-Xq-lq=0.

(4)

由于地形異常圖與灰度圖相似，本文借鑒灰度匹配算法中的局部灰度值編碼算法[15]，將該算法用于基于多波束測深儀圖優化SLAM方法中的閉環檢測模塊。相對于其他常用的灰度匹配算法，局部灰度值編碼算法時間復雜度更低，且對圖像尺寸不敏感，魯棒性更高。

2.1.2 局部灰度值編碼算法

局部灰度值編碼算法的匹配過程分為粗匹配和精匹配[15]。在粗匹配過程中，對模板和搜索圖根據灰度值進行編碼，然后將模板在搜索圖上按照固定步長遍歷，尋找搜索圖中與模板編碼最接近的圖塊，得到初始匹配參數。在精匹配過程中，根據初始匹配結果，在粗匹配位置裁剪出一個大小和模板一樣的子圖，然后使用相位相關法進行精確匹配。

智能倉儲機器人一旦出現問題，停止運作后，想去維修它就會變得非常困難，一方面是機器人本身就很重，一臺機器人就有上百斤重，所以很難去移動它。另一方面是機器人內部比較復雜，一般員工根本不懂，必須要研發的專業人員來維修，這樣就會對產品的售后造成很大困難。

粗匹配首先提取圖像的分塊編碼特征，如圖5(a)所示，將搜索圖像劃分為K×K個互不重疊的方塊，稱該方塊為R塊，K可根據問題任意選擇。分塊完畢后，將圖像中多余的行和列裁減掉。R塊與其周圍8個相鄰的R塊組成R塊的鄰域，將R塊的鄰域分成D1、D2、D3、D4共4個部分，稱為R塊的4個D鄰域。R塊R5的鄰域和4個D鄰域如圖5(b)所示。對每個D鄰域中4個R塊的灰度值進行排序(升序或降序)，將結果轉換成5位二進制碼，記作P(D)。如圖5所示，將R塊R5所在的4個D鄰域編碼拼接起來，得到F(R5)=P(D1)P(D2)·P(D3)P(D4)，F(R5)即為R5的編碼。因為圖像外圍的一圈R塊不存在8個鄰域，所以只有圖5中的陰影部分可以編碼。將圖像中所有R塊的編碼特征轉換成十進制數，將其按照行列順序組成1個一維向量，該向量稱作圖像的特征向量。將模板特征向量與子圖特征向量進行比較，相似度最高的子圖即為粗匹配結果。

圖5 圖像分塊及編碼Fig.5 Image segmentation and coding

粗匹配可以找到與模板相似度最高的子圖，但是該子圖與模板存在一定的偏差，并非完全重疊，精匹配使用相位相關法修正粗匹配結果的偏差。根據二維傅里葉變換的性質：空間域上的平移等價于頻域相位的平移。兩幅圖的平移矢量可以通過它們互功率譜的相位直接計算。假設圖像f1和f2之間的平移關系為

f1(x,y)=f2(x-x0,y-y0),

(5)

式中：(x0,y0)為空間(x,y)的1個點。

相應的傅里葉變換如(6)式所示：

F1(u,v)=F2(u,v)e-j2π(ux0+vy0)，

(6)

式中：(u,v)為(x,y)對應的頻域坐標；F1(u,v)為f1(x,y)的傅里葉變換結果；F2(u,v)為f2(x-x0,y-y0)的傅里葉變換結果。

定義圖像f1與f2之間的歸一化互功率譜為

(7)

2.1.3 采用局部灰度值編碼算法執行閉環檢測

灰度圖與測深圖結構類似，灰度圖存儲像素灰度，測深圖存儲地形深度，用于灰度圖匹配的算法也可用于測深圖匹配。但是，SLAM中的閉環檢測問題與圖像模板匹配問題并不完全相同。模板匹配是在一幅較大圖像上定位一幅給定的子圖像；閉環檢測是尋找與新構建子圖有重疊部分的已構建子圖。

在圖優化SLAM問題中，假設hn(n為當前采樣點數)為新構建的子圖，如果將hn與先前構建的n-1幅子圖分別當作模板和搜索圖的子塊，則局部灰度值編碼算法就可用于閉環檢測。

2.2 誤匹配檢測

對全部采樣點執行完閉環檢測后即可構建約束條件，完成圖的構建。但是錯誤的閉環檢測結果會對后續圖優化造成干擾，因此本文提出基于多波束測深儀的圖優化SLAM方法，剔除錯誤閉環。

三重約束條件下的誤匹配實時檢測算法[16]分為模型擬合檢測、空間結構檢測和距離比檢測3個檢測模塊，且3個檢測模塊均采用逐點實時檢測的方案。本文與文獻[16]處理的誤匹配檢測問題，有如下兩點不同：

1)文獻[16]中假設AUV的軌跡接近直線，但是在基于多波束測深儀的圖優化SLAM方法中，為了使航行過程中包含閉環，需要設定AUV的軌跡為S形軌跡。

2)圖優化SLAM方法是一種先建圖再優化的方法，該類方法對誤匹配檢測方法的實時性要求較低。

基于不同點1，由于載體呈S形前進，軌跡中會出現很多拐點，很難用某個單一模型擬合真實軌跡。基于不同點2，由于本文所提圖優化SLAM方法對誤匹配檢測的實時性要求不高，可在圖構建完畢后一次性檢測所有的誤匹配點。

基于上述原因，本文采用誤匹配檢測算法，在三重約束條件下的誤匹配實時檢測算法基礎上取消模型擬合檢測模塊，只包含空間結構檢測模塊和距離比檢測模塊，且每一個檢測模塊均采取批處理的方式。

2.2.1 空間結構檢測

設XINS={xINS1,xINS2,…,xINSN}為INS指示軌跡，如果XINS上的兩個位置點xINSi和xINSj之間被檢測到包含閉環，且沒有檢測到xINSi之前的點與xINSi之間包含閉環，則將xINSi與校正后的xINSj視為匹配點。如果xINSj之后檢測到新的位置點xINSk與xINSj之間包含閉環，則需對xINSj進行兩次校正，再將其添加進匹配軌跡。假設有N′個匹配點，通過上述方法可以構建匹配軌跡Xm={xm1,xm2,…,xmN′}，記Xm，INS為Xm對應的INS指示軌跡。

空間結構檢測的執行步驟如下：

1)構建Xm，INS和Xm的K最近鄰(KNN)圖Gm，INS={Vm，INS,Em，INS}和Gm={Vm,Em}，其中Vm，INS和Vm分別為Gm，INS和Gm中頂點的集合，Em，INS和Em分別為Gm，INS和Gm中邊的集合。如果xj是xi的KNN域中的點，且‖xi-xj‖≤η，則xj和xi之間存在一條有向邊〈i,j〉，其中η為相應頂點集中所有頂點之間距離的中值。KNN圖構建完畢后可以得到每一個圖的鄰接矩陣，記為Am，INS和Am，其中Am，INS可由(8)式計算得到，同理可得Am.

(8)

2)對于連接頂點vmi至vmm的邊，m≠i，使用(9)式計算權值W(i,m)：

(9)

式中：xm，INSm為Xm，INS上第m個位置點；xm，INSi為Xm，INS上第i個位置點；xmm為Xm上第m個位置點；xmi為Xm上第i個位置點；Rτ為旋轉矩陣。

4)使用(10)式計算Gm中每個頂點的權值w(i)：

(10)

5)使用(11)式計算μo，

(11)

式中：μo為頂點權值的均值。

然后刪除權值最大的點xmax，重新生成Xm，INS和Xm. 重復步驟1～步驟4，使用(11)式計算新生成頂點權值的均值μn. 如果|μn-μo|<ε(ε為需要手動設置的參數)且w(i)的最大值小于π，則檢測完畢，否則xmax是誤匹配點，算法進入下一次迭代。本文在仿真實驗部分設定ε值為0.02.

2.2.2 距離比檢測

假設dm，INSk-1,k和dmk-1,k分別表示INS指示軌跡和匹配軌跡上第k-1和第k個采樣點之間的距離。距離比檢測的執行步驟如下：

3)如果Smax和Smin均滿足由INS誤差傳播模型得到的范圍[17]則算法結束，否則存在誤匹配。假設Smax對應的點為xmkmax-1和xmkmax(kmax表示Smax對應的采樣點)，Smin對應的點為xmkmin-1和xmkmin(kmin表示Smin對應的采樣點)，如果上述4個點包含重復位置，則該重復點為誤匹配點；如果上述4個點互不相同，則xmkmax和xmkmin均為誤匹配點。刪除誤匹配點后開始下一輪迭代。

2.3 后端圖優化

通過前端完成圖的構建后，建立圖的節點和邊，在后端需要對該圖進行優化。設XSLAM={xSLAM1，xSLAM2，…，xSLAMN}是待優化的節點，定義zij為前端構建的第i個節點和第j個節點之間的位置與姿態變換關系，定義向量誤差函數e(xSLAMi,xSLAMj,zij)表示xSLAMi和xSLAMj之間的關系與zij的偏離程度。則求SLAM最優軌跡可以表示成求解節點位置，使得下述總體誤差函數最小[18]：

(12)

式中：Ωij為誤差的信息矩陣，表示誤差e(xSLAMi,xSLAMj,zij)所占的權重，本文設定Ωij為單位矩陣；Fij為[e(xSLAMi,xSLAMj,zij)]TΩije(xSLAMi,xSLAMj,zij)的簡化表達式。

后端優化的目標是找到最優的節點配置XSLAM*，使得F(XSLAM)值最小。

(13)

圖優化SLAM常采用高斯- 牛頓法或Levenberg- Marquardt法對(13)式進行求解，本文采用高斯- 牛頓迭代策略求解(13)式的數值解。該數值解用于校正INS的誤差，校正后的導航信息即為下一輪SLAM前端部分的輸入。

3 仿真實驗結果與分析

3.1 仿真參數設置

本節規劃的真實軌跡起點為(東經153°，北緯21°)，在此基礎上仿真產生一條71.7 min的INS指示軌跡，INS的仿真參數如表1所示。真實軌跡與INS指示軌跡如圖6所示。

表1 INS仿真參數

圖6 真實軌跡與INS指示軌跡Fig.6 Actual trajectory and INS indicated trajectory

圖7 三維地形圖Fig.7 3D terrain base map

本文使用美國地球物理中心發布的模型[19]計算從(東經152.4°，北緯20.5°)到(東經153.1°，北緯21.2°)一片矩形區域的地形深度數據，對該區域內的地形數據進行插值，得到仿真實驗用的真實地形圖，插值后網格分辨率為0.05′×0.05′，三維地形圖如圖7所示。地形圖的相關參數如表2所示。

表2 地形相關參數

仿真中所用的多波束測深儀固定于載體正下方，掃描長度為12個網格，在真實地形圖上重采樣并疊加一零均值高斯白噪聲，作為由多波束測深儀測量的地形實測值，噪聲方差為1 m. SLAM方法涉及的相關參數如表3所示。

表3 SLAM方法相關參數

3.2 通用圖優化SLAM方法仿真

第1組仿真實驗采用通用圖優化SLAM方法校正INS指示軌跡，由于順序關聯約束難以建立，故該方法的前端部分只包含閉環檢測模塊。本組仿真實驗采用局部灰度值編碼算法進行閉環檢測，采用高斯- 牛頓法進行后端圖優化，實驗結果如圖8所示。

圖8 通用圖優化SLAM方法生成的校正軌跡Fig.8 Corrected trajectory generated by general SLAM method

從圖8中可以直觀地看出，SLAM方法校正軌跡比INS指示軌跡更接近真實軌跡的形狀，但是在部分區域，尤其是軌跡的前半部分，SLAM方法校正軌跡的位置誤差反而比INS指示軌跡的位置誤差大。圖9給出了SLAM方法校正軌跡及INS指示軌跡上每一個采樣點的經度和緯度誤差變化曲線，從圖9中可以看出，在航行后半程SLAM方法校正軌跡的位置誤差要明顯小于INS指示軌跡，但是在航行的前40 min時間段，SLAM方法校正軌跡的緯度誤差在多個采樣點處要明顯大于INS指示軌跡。導致這一現象的原因是閉環檢測的結果包含了錯誤的閉環，錯誤的閉環檢測結果會影響到軌跡上的每一個采樣點。

圖9 通用圖優化SLAM方法生成的校正軌跡及INS指示軌跡的經度和緯度誤差Fig.9 Longitude and latitude errors of corrected trajectories generated by general SLAM method and INS indicated trajectory

3.3 基于多波束測深儀的圖優化SLAM方法仿真

第2組仿真實驗在第1組仿真基礎上引入了誤匹配檢測模塊，即采用本文所提基于多波束測深儀的圖優化SLAM方法校正INS指示軌跡，仿真結果如圖10所示。

圖10 基于多波束測深儀的圖優化SLAM方法生成的校正軌跡Fig.10 Corrected trajectory generated by multi-beam sonar-based graph SLAM method

對比圖8和圖10中的SLAM校正軌跡可以明顯看到，采用基于多波束測深儀圖優化SLAM方法生成的校正軌跡更接近真實軌跡。

圖11 基于多波束測深儀的圖優化SLAM方法生成的校正軌跡及INS指示軌跡的經度和緯度誤差Fig.11 Longitude and latitude errors of corrected trajectories generated by multi-beam sonar-based graph SLAM method and INS indicated trajectory

圖11給出了SLAM方法校正軌跡及INS指示軌跡上每一個采樣點的經度和緯度誤差變化曲線。從圖11中可以看到，SLAM方法校正軌跡的位置誤差在全部航行時段均小于INS指示軌跡的誤差，基于多波束測深儀的圖優化SLAM方法具有更高的定位精度。

4 結論

本文提出一種基于多波束測深儀的水下圖優化SLAM方法，在無基準圖的情形下校正INS的累積誤差。該方法在通用圖優化SLAM方法基礎上增加了誤匹配檢測模塊，剔除了閉環檢測模塊輸出的錯誤閉環。此外，該方法針對多波束測深儀數據的特性，采用局部灰度值編碼算法進行閉環檢測。基于真實海底地形圖的仿真實驗表明，在無基準圖情況下采用該方法對INS軌跡進行校正可以取得更高的定位精度。

需要指出的是，本文提出的圖優化SLAM方法對初始位置誤差敏感且需要設定S形軌跡，同時還需要地形深度有明顯的起伏。本文方法應與DBRNS相結合，共同在水下環境校正INS誤差。在可以獲得高分辨率基準圖的前提下，采用DBRNS的定位效果往往優于本文提出的方法，當單一信息源特征明顯時，應采用單信息源DBRNS進行水下自主導航；當單一信息源特征不明顯時，應增加傳感器種類，構建多信息源聯合DBRNS進行水下自主導航；如果無法獲得基準圖或基準圖分辨率低，則應采用本文提出的方法校正INS誤差。