戴天， 繆玲娟， 邵?？?/p>

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

0 引言

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

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

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

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

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

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

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

1 通用圖優(yōu)化SLAM方法

圖優(yōu)化SLAM方法利用圖模型對SLAM問題建模，將SLAM問題劃分為前端和后端兩個部分，前端負(fù)責(zé)圖的構(gòu)建，后端負(fù)責(zé)圖的優(yōu)化。通用圖優(yōu)化SLAM方法如圖1所示[12]。

圖1 通用圖優(yōu)化SLAM方法Fig.1 General graph SLAM method

圖優(yōu)化SLAM方法在前端部分構(gòu)建的圖由節(jié)點和邊組成，節(jié)點表示載體離散的位置與姿態(tài)以及對應(yīng)傳感器的實測值，邊表示節(jié)點之間的約束，約束關(guān)系一般由兩個節(jié)點之間的位置與姿態(tài)變換關(guān)系構(gòu)成。如圖1中前端部分所示，圖優(yōu)化SLAM方法通常采用兩種方式構(gòu)建約束關(guān)系，一種是順序數(shù)據(jù)關(guān)聯(lián)，另一種是閉環(huán)檢測。

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

閉環(huán)檢測是檢測載體是否回到先前經(jīng)過的位置，該類約束是一種強(qiáng)約束關(guān)系，一旦檢測到閉環(huán)，將對整個位置與姿態(tài)圖進(jìn)行較大程度的修正。本文所提SLAM方法在每一個節(jié)點存儲該點對應(yīng)的子圖，通過設(shè)定S形航線，檢測不同節(jié)點所對應(yīng)的子圖是否包含重疊區(qū)域，進(jìn)而完成閉環(huán)檢測。

基于多波束測深儀的圖優(yōu)化SLAM方法構(gòu)建的圖模型如圖2所示，圖2中節(jié)點{x0，x1，…，xi+1}表示載體的位置與姿態(tài)以及子圖。

圖2 基于多波束測深儀的圖優(yōu)化SLAM方法中圖結(jié)構(gòu)Fig.2 Graph structure of multi-beam sonar-based graph SLAM method

2 基于多波束測深儀的圖優(yōu)化SLAM方法

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

1)水下長航導(dǎo)航任務(wù)中，難以通過順序數(shù)據(jù)關(guān)聯(lián)建立連續(xù)節(jié)點之間的約束關(guān)系；

2)閉環(huán)檢測對優(yōu)化結(jié)果影響很大，現(xiàn)有方法難以保證檢測精度。

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

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

2.1 閉環(huán)檢測

2.1.1 問題描述

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

(1)

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

(2)

(3)

Xp+lp-Xq-lq=0.

(4)

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

2.1.2 局部灰度值編碼算法

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

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

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

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

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

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

(5)

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

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

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

(6)

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

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

(7)

2.1.3 采用局部灰度值編碼算法執(zhí)行閉環(huán)檢測

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

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

2.2 誤匹配檢測

對全部采樣點執(zhí)行完閉環(huán)檢測后即可構(gòu)建約束條件，完成圖的構(gòu)建。但是錯誤的閉環(huán)檢測結(jié)果會對后續(xù)圖優(yōu)化造成干擾，因此本文提出基于多波束測深儀的圖優(yōu)化SLAM方法，剔除錯誤閉環(huán)。

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

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

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

基于不同點1，由于載體呈S形前進(jìn)，軌跡中會出現(xiàn)很多拐點，很難用某個單一模型擬合真實軌跡?；诓煌c2，由于本文所提圖優(yōu)化SLAM方法對誤匹配檢測的實時性要求不高，可在圖構(gòu)建完畢后一次性檢測所有的誤匹配點。

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

2.2.1 空間結(jié)構(gòu)檢測

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

空間結(jié)構(gòu)檢測的執(zhí)行步驟如下：

1)構(gòu)建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〉，其中η為相應(yīng)頂點集中所有頂點之間距離的中值。KNN圖構(gòu)建完畢后可以得到每一個圖的鄰接矩陣，記為Am，INS和Am，其中Am，INS可由(8)式計算得到，同理可得Am.

(8)

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

(9)

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

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

(10)

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

(11)

式中：μo為頂點權(quán)值的均值。

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

2.2.2 距離比檢測

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

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

2.3 后端圖優(yōu)化

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

(12)

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

后端優(yōu)化的目標(biāo)是找到最優(yōu)的節(jié)點配置XSLAM*，使得F(XSLAM)值最小。

(13)

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

3 仿真實驗結(jié)果與分析

3.1 仿真參數(shù)設(shè)置

本節(jié)規(guī)劃的真實軌跡起點為(東經(jīng)153°，北緯21°)，在此基礎(chǔ)上仿真產(chǎn)生一條71.7 min的INS指示軌跡，INS的仿真參數(shù)如表1所示。真實軌跡與INS指示軌跡如圖6所示。

表1 INS仿真參數(shù)

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

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

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

表2 地形相關(guān)參數(shù)

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

表3 SLAM方法相關(guān)參數(shù)

3.2 通用圖優(yōu)化SLAM方法仿真

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

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

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

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

3.3 基于多波束測深儀的圖優(yōu)化SLAM方法仿真

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

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

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

圖11 基于多波束測深儀的圖優(yōu)化SLAM方法生成的校正軌跡及INS指示軌跡的經(jīng)度和緯度誤差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指示軌跡上每一個采樣點的經(jīng)度和緯度誤差變化曲線。從圖11中可以看到，SLAM方法校正軌跡的位置誤差在全部航行時段均小于INS指示軌跡的誤差，基于多波束測深儀的圖優(yōu)化SLAM方法具有更高的定位精度。

4 結(jié)論

本文提出一種基于多波束測深儀的水下圖優(yōu)化SLAM方法，在無基準(zhǔn)圖的情形下校正INS的累積誤差。該方法在通用圖優(yōu)化SLAM方法基礎(chǔ)上增加了誤匹配檢測模塊，剔除了閉環(huán)檢測模塊輸出的錯誤閉環(huán)。此外，該方法針對多波束測深儀數(shù)據(jù)的特性，采用局部灰度值編碼算法進(jìn)行閉環(huán)檢測?；谡鎸嵑５椎匦螆D的仿真實驗表明，在無基準(zhǔn)圖情況下采用該方法對INS軌跡進(jìn)行校正可以取得更高的定位精度。

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