AUV的圖優(yōu)化海底地形同步定位與建圖方法

馬 騰，李 曄，趙玉新，沈 鵬，凌 宇

(1.哈爾濱工程大學水下機器人技術重點實驗室，哈爾濱 150001；2.哈爾濱工程大學自動化學院，哈爾濱 150001；3.國家深?；毓芾碇行?，青島 266237)

0 引言

隨著人類對海洋探索和開發(fā)需求的日益旺盛和多波束測深聲納技術的不斷成熟，構建高精度的海底地形圖已經(jīng)成為了幫助人類了解海洋和開發(fā)海洋的重要手段。受多波束聲納等聲學測繪儀器的工作特性(1.聲波作用范圍有限；2.測繪精度與載體到海底的距離有關)限制，智能水下機器人(Autonomous Underwater Vehicle，AUV)作為一種能夠?qū)崿F(xiàn)大潛深自主航行和自主作業(yè)的水下航行器，已經(jīng)成為了海底地形測繪的重要載體。

AUV在進行海底地形測繪時，精確的實時導航信息尤為重要。AUV在水下作業(yè)時，無法同衛(wèi)星進行通信獲取準確的位置，推算導航系統(tǒng)則會存在累計誤差無法消除的問題。目前，常用的水聲定位系統(tǒng)如長基線存在基陣布設困難，校正基陣位置耗時耗力；而超短基線與母船通信延遲受距離影響，在大深度時延遲較大，定位的實時性也無法保證，且AUV作業(yè)范圍受限于母船活動范圍[1-3]。

同步定位與建圖(Simultaneous Localization and Mapping，SLAM)是機器人在陌生環(huán)境下依靠自身傳感器進行定位與同時地圖構建的過程，可以從本質(zhì)上提高無人系統(tǒng)的自主性。由于信息源廣泛且容易獲取，目前SLAM已經(jīng)在陸地和空中機器人中得到了較為深入的研究[4-5]。但受水下環(huán)境與感知設備條件的限制，水下SLAM的研究尚不成熟，依然存在很多問題。

目前，關于水下SLAM的研究主要以機械掃描聲納[6-7]或水下攝像機[8-9]為環(huán)境感知設備。但采用機械掃描聲納的SLAM以堤壩等人造環(huán)境為信息源，只適用于港口等結構化環(huán)境。而在海洋環(huán)境中，結構化環(huán)境極為罕見，這極大地限制了前視聲納SLAM的應用。同時，絕大部分的水下環(huán)境是光學傳感器的弱視區(qū)甚至是盲區(qū)，光學SLAM在深水區(qū)或水質(zhì)渾濁區(qū)域同樣很難開展。

在這種情況下，利用多波束聲納作為傳感器進行海底地形SLAM(Bathymetric SLAM，BSLAM),由于其應用范圍廣、可靠性高等特點，已經(jīng)成為了解決AUV水下精確定位與導航問題的可行手段。Roman[10]提出了使用將點云地圖劃分為假設導航誤差各不相關的一系列子地圖，各個子地圖間通過地形匹配的方法獲取數(shù)據(jù)關聯(lián)，通過延遲卡爾曼濾波進行求解。Palomer[11]在擴展卡爾曼濾波器SLAM方法的基礎上，考慮測深點和機器人位姿的不確定性，建立了迭代最近點法的概率學表達方式，并將其分為了點-點匹配和點-面匹配2個階段。Barkby[12-13]基于粒子濾波器提出了使用軌跡地圖的BPSLAM(Bathymetric Distributed Particle Filter SLAM)方法，BPSLAM通過建立高斯過程模型，將測線長度的不確定性變?yōu)闇y點處水深的不確定性，從而對測點處水深進行估計，并根據(jù)估計值與實際測深值的似然程度，計算粒子權重?？梢钥吹剑壳皩τ诤５椎匦蜸LAM算法框架的研究主要集中于濾波方法，濾波方法求解方法簡單有效且效率高，但需要對AUV的運動模型進行準確建模。而受高階水動力系數(shù)的影響，AUV運動模型的準確建模是極為困難的。

Lu與Milios[14]提出的圖優(yōu)化的方法，通過保留所有的觀測數(shù)據(jù)幀以及幀間的空間約束關系，并將約束看作是隨機觀測，然后采用最大似然方法估計機器人的位姿，這種思想可以通過圖的方式形象地表現(xiàn)出來。使用圖優(yōu)化的方法，通過使用AUV位姿數(shù)據(jù)和傳感器觀測數(shù)據(jù)構建圖，通過對圖進行優(yōu)化求解海底地形SLAM，在一定程度上擺脫了運動模型和觀測模型的限制，從而解決了AUV運動模型和傳感器觀測噪聲不準確的問題。受多波束聲納物理特性限制，相鄰時刻測深數(shù)據(jù)間無信息冗余，無法建立相鄰幀間數(shù)據(jù)關聯(lián)，導致傳統(tǒng)圖優(yōu)化算法中建圖一致性被破壞，需要對傳統(tǒng)圖優(yōu)化算法進行改進。

本文提出了一種基于圖優(yōu)化的海底地形SLAM算法，該方法共包括位姿圖構建和優(yōu)化2個部分，位姿圖構建中利用水深預測手段和地形匹配方法進行幀間數(shù)據(jù)關聯(lián)構建和閉環(huán)檢測。在圖優(yōu)化過程中，構建全局路徑修正和局部路徑修正相結合的優(yōu)化算法框架，對海底地形同步定位與建圖問題進行求解。相較于傳統(tǒng)的濾波器海底地形SLAM算法，該算法無需對運動模型進行準確建模，計算精度較高，且通過構建位姿圖保留了全部歷史信息，能夠?qū)崿F(xiàn)對于歷史數(shù)據(jù)的更新、修改等操作。最終通過仿真試驗結果對該算法的有效性進行了證明。

1 位姿圖構建

1.1 子地圖建模

由于多波束聲納僅僅能夠獲得聲學測線的回波時間和回波角度數(shù)據(jù)，需要將其轉(zhuǎn)化為大地坐標系下的三維測深點信息。為簡化計算，通常不考慮AUV在垂直面的運動，即將三維的地圖轉(zhuǎn)化為每點代表不同高度值的2.5D點云模型。

如圖1所示，由船位推算導航和光纖羅經(jīng)可以獲得AUV位置(xV,yV,zV,θV),其中θV為AUV首向角，對于每一個測點測深側掃系統(tǒng)會返回聲速值，接收到波束的時間值和回波角度的正弦值。通過計算可以得到測線相對于AUV的位置為(r,β)，其中r為連接AUV載體坐標系圓心與測點的半徑長度，β為相對于zV的有向開角，將其轉(zhuǎn)化為大地坐標系下，公式為

(1)

圖1 多波束聲納地形測量示意圖Fig.1 The schematic diagram of terrain measurement with multi-beam sonar

從而將關聯(lián)在載體坐標系下的極坐標轉(zhuǎn)化為大地坐標系下帶有高度值的2.5D點陣。

考慮到地形匹配耗時的問題，將路徑劃分為諸多子地圖的形式[15]。子地圖根據(jù)2.5D地圖的信息量和路徑的長度創(chuàng)建，信息量根據(jù)法線間差計算[16]。法線間差利用不同支持半徑下的法線值的差值檢測有明顯變化區(qū)域的點云數(shù)據(jù)，本文中利用其檢測地圖中地形起伏劇烈區(qū)域，計算公式為

(2)

式中，n1、n2分別為支持半徑為r1、r2時地形剖面與圓的最外側交點連線的法線，且滿足r1

1.2 閉環(huán)檢測

(3)

(4)

圖2 閉環(huán)檢測過程的向量示意圖Fig.2 Vector diagram of loop closure detection

Xi-Xj=lj-li+Δ-w

(5)

若使用在Xi與Xj處的慣性導航偏移量ΔXi和ΔXj代替Xi和Xj，式(5)可以寫為

ΔXi-ΔXj=Δ-w

(6)

1.3 弱數(shù)據(jù)關聯(lián)構建

在使用參考文獻[18]提出的地形外推方法獲得i+1時刻地形外推結果后，可通過與i+1時刻多波束實測數(shù)據(jù)之間的相似程度對弱數(shù)據(jù)關聯(lián)進行求解。結合多波束測量信息，對i到i+1時刻建立狀態(tài)模型

(7)

因而，弱數(shù)據(jù)關聯(lián)p(Xi+1|Xi,ui)可以表示為

(8)

2 位姿圖優(yōu)化

2.1 全局路徑修正

全局路徑修正的前提是選取合適的狀態(tài)轉(zhuǎn)移方程和觀測方程。對于AUV而言，存在包括六自由度操縱性方程等多種狀態(tài)轉(zhuǎn)移方程。但由于一、二階水動力系數(shù)計算困難，SLAM中一般選擇簡化的運動模型，即

Xi=f(Xi-1,ui)+vi

(9)

(10)

關鍵幀i處的導航偏移量連續(xù)性方程可以表示為

(11)

因此，可以將導航偏移量連續(xù)性方程表示為矩陣的形式

ΔXkey=HΔXkey

(12)

另一方面，環(huán)形閉合的觀測方程也可以表示為

(13)

(14)

若將觀測方程表示為矩陣形式，可以得到

DS=HSΔXkey

(15)

其中，DS代表對所有環(huán)形閉合結果觀測的集合，ΔXkey為所有關鍵幀上的AUV導航偏移量，HS則是由1、-1和0組成的系數(shù)矩陣。

2.2 局部路徑修正

在得到AUV在關鍵幀的位置修正結果后，局部路徑修正解決的就是如何將通過全局路徑修正求得的AUV在關鍵幀處的導航偏差傳遞到整條路徑上。

(16)

圖3 局部路徑修正示意圖Fig.3 Schematic diagram of local trajectory optimization

需要注意的是，與傳統(tǒng)的運動模型不同，ui不再表示控制輸入，而是表示i時刻AUV真實的位移，而vi則代表了i時刻AUV慣性導航值相對于真實位移的偏移量。在n時刻AUV慣性導航的偏移量ε可以表示為

(17)

參考彈簧系統(tǒng)，本節(jié)提出了一種誤差修正方法，同樣將AUV各時刻的狀態(tài)視為彈簧的端點。由于慣性導航系統(tǒng)存在誤差，現(xiàn)階段系統(tǒng)是受力并且不穩(wěn)定的，其系統(tǒng)能量模型為

(18)

θi=-lnp(Xi|Xi-1)

(19)

將能量模型轉(zhuǎn)化為受力模型，根據(jù)串聯(lián)彈簧的受力形變公式

=θ1(v1-v0)2=…=θn(vn-vn-1)2

(20)

建立遞推模型，依次計算v1一直到vn。

3 回放式仿真試驗

使用中國膠州灣獲取的海試數(shù)據(jù)，試驗設備如圖4所示，試驗數(shù)據(jù)時長3613s，試驗船航速約4節(jié)，共行駛約8km。處理后的試驗數(shù)據(jù)以1s為一個數(shù)據(jù)更新節(jié)拍，同時更新AUV狀態(tài)信息(包括GPS數(shù)據(jù)、光纖羅經(jīng)數(shù)據(jù))和多波束數(shù)據(jù)。

圖4 海上試驗設備Fig.4 Devices in the sea trail experiment

使用船載的多波束聲納、光纖羅經(jīng)和GPS進行數(shù)據(jù)獲取，其中多波束聲納為GeoSwath Plus，測線寬度約為10倍水深，工作頻率為23Hz，斜距分辨率為1.5mm，采樣間距為12mm，系統(tǒng)主要包括主控艙和V型架，V型架上裝載2個聲學發(fā)射/接受陣和一個聲速計，發(fā)送多波束回波數(shù)據(jù)和聲速剖面信息給主控艙。光纖羅經(jīng)和GPS分別將姿態(tài)信息和導航信息發(fā)送給主控艙，其中光纖羅經(jīng)為哈爾濱工程大學研制，姿態(tài)測量的精度為0.05°，GPS為NovAtel 公司的ProPak-LB，圓概率定位誤差為0.8m。

圖5所示為子地圖劃分的結果，為驗證局部路徑修正的效果，在對位姿圖進行全局路徑修正的基礎上，分別使用均勻分配方法和提出的局部路徑修正算法將關鍵幀的慣性導航系統(tǒng)偏差分配到整條軌跡上，為便于表示，分別將使用了兩種方法的BSLAM算法稱為非完全BSLAM(均勻分配)和BSLAM(局部路徑修正方法)。為實現(xiàn)對無效閉環(huán)結果的剔除，應用了文獻[19]提出的投票算法。

圖5 子地圖劃分結果Fig.5 Submap division result

圖6(a)、(c)、(e)表示了分別使用慣性導航系統(tǒng)、非完全BSLAM和BSLAM輸出的軌跡和構建的海底地形圖，而圖6(b)、(d)、(f)則是對應的測深點定位誤差直方圖。誤差直方圖統(tǒng)計了任務結束后所有時刻測深點位置的誤差，以i時刻為例，該時刻所有測深點的誤差可以表示為

(21)

(a)慣性導航建圖結果

(b)慣性導航測點定位誤差直方圖

(c)非完全BSLAM建圖結果

(d)非完全BSLAM測點定位誤差直方圖

(e)BSLAM建圖結果

(f)BSLAM測點定位誤差直方圖圖6 BSLAM位姿圖優(yōu)化結果Fig.6 Graph optimization results in the BSLAM

如圖6和表1所示，試驗結果證明，全局路徑修正和局部路徑修正都在BSLAM位姿圖優(yōu)化中起到了重要的作用。相比于非完全BSLAM，BSLAM給出的結果其均值減小了10.82% ，中位數(shù)減小了8.87%；而與慣性導航結果相比BSLAM的效果更加明顯，BSLAM系統(tǒng)給出的定位誤差均值和中位數(shù)分別減小了83.13%和82.92%。

表1 測深點定位誤差Tab.1 Localization errors of measurement points

如圖7所示，AUV在A處第一次成功進行閉環(huán)檢測，在C處第一次于起始點附近成功進行閉環(huán)檢測后，一直均將導航誤差控制于10m左右。通過試驗結果可以證明，全局路徑修正和局部路徑修正在BSLAM系統(tǒng)中都發(fā)揮出了不可替代的作用。

圖7 BSLAM實時修正效果Fig.7 BSLAM results in real time

4 結論

本文針對AUV長時間遠距離潛航中的精確導航問題，提出了圖優(yōu)化海底地形同步定位與建圖算法。算法分析與實驗結果表明：

1)提出了一種基于圖優(yōu)化算法的BSLAM算法，該算法可以實現(xiàn)AUV多波束同步定位與構圖，能夠有效修正船位推算方法產(chǎn)生的累積誤差。

2)BSLAM算法的全局路徑修正算法對參考導航系統(tǒng)導航誤差的修正起到了最主要的作用。

3)BSLAM算法的局部路徑修正雖然只是在全局路徑修正結果的基礎上實現(xiàn)關鍵幀導航誤差在局部路徑上的分配，但仍然可以有效減小AUV定位與建圖誤差。