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

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

(1.哈爾濱工程大學(xué)水下機(jī)器人技術(shù)重點(diǎn)實(shí)驗(yàn)室，哈爾濱 150001；2.哈爾濱工程大學(xué)自動(dòng)化學(xué)院，哈爾濱 150001；3.國(guó)家深?；毓芾碇行?，青島 266237)

0 引言

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

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

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

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

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

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

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

1 位姿圖構(gòu)建

1.1 子地圖建模

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

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

(1)

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

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

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

(2)

式中，n1、n2分別為支持半徑為r1、r2時(shí)地形剖面與圓的最外側(cè)交點(diǎn)連線的法線，且滿足r1

1.2 閉環(huán)檢測(cè)

(3)

(4)

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

Xi-Xj=lj-li+Δ-w

(5)

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

ΔXi-ΔXj=Δ-w

(6)

1.3 弱數(shù)據(jù)關(guān)聯(lián)構(gòu)建

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

(7)

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

(8)

2 位姿圖優(yōu)化

2.1 全局路徑修正

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

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

(9)

(10)

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

(11)

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

ΔXkey=HΔXkey

(12)

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

(13)

(14)

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

DS=HSΔXkey

(15)

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

2.2 局部路徑修正

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

(16)

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

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

(17)

參考彈簧系統(tǒng)，本節(jié)提出了一種誤差修正方法，同樣將AUV各時(shí)刻的狀態(tài)視為彈簧的端點(diǎn)。由于慣性導(dǎo)航系統(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)

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

3 回放式仿真試驗(yàn)

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

圖4 海上試驗(yàn)設(shè)備Fig.4 Devices in the sea trail experiment

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

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

圖5 子地圖劃分結(jié)果Fig.5 Submap division result

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

(21)

(a)慣性導(dǎo)航建圖結(jié)果

(b)慣性導(dǎo)航測(cè)點(diǎn)定位誤差直方圖

(c)非完全BSLAM建圖結(jié)果

(d)非完全BSLAM測(cè)點(diǎn)定位誤差直方圖

(e)BSLAM建圖結(jié)果

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

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

表1 測(cè)深點(diǎn)定位誤差Tab.1 Localization errors of measurement points

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

圖7 BSLAM實(shí)時(shí)修正效果Fig.7 BSLAM results in real time

4 結(jié)論

本文針對(duì)AUV長(zhǎng)時(shí)間遠(yuǎn)距離潛航中的精確導(dǎo)航問題，提出了圖優(yōu)化海底地形同步定位與建圖算法。算法分析與實(shí)驗(yàn)結(jié)果表明：

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

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

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