基于磁信標(biāo)的水下SLAM方法

常 帥,付曉梅,張翠翠,趙玉新,杜 雪

(1.天津大學(xué) 海洋科學(xué)與技術(shù)學(xué)院,天津,300072;2.哈爾濱工程大學(xué) 自動(dòng)化學(xué)院,黑龍江 哈爾濱,150001)

0 引言

水下同步定位與構(gòu)圖(simultaneous localization and mapping,SLAM)是一種無(wú)人水下航行器(unmanned undersea vehicle,UUV)在未知環(huán)境中,利用傳感器探測(cè)的環(huán)境特征作為導(dǎo)航參照,并利用距離方位等觀測(cè)量實(shí)現(xiàn) UUV導(dǎo)航狀態(tài)的估計(jì)及特征地圖構(gòu)建的方法。SLAM導(dǎo)航系統(tǒng)相比單一慣性導(dǎo)航系統(tǒng)(inertial navigation system,INS)或航位推測(cè)(dead reckoning,DR)導(dǎo)航系統(tǒng),可在更長(zhǎng)的時(shí)間內(nèi)實(shí)現(xiàn)高精度導(dǎo)航估計(jì),已成為近年來(lái)UUV 自主導(dǎo)航的重要發(fā)展趨勢(shì)之一。

受傳感器精度的影響,INS或DR存在明顯的誤差積累效應(yīng)。為減少通過衛(wèi)星定位校準(zhǔn)而帶來(lái)的作業(yè)效率和能耗損失,在水下環(huán)境尋找校準(zhǔn)導(dǎo)航狀態(tài)估計(jì)誤差的觀測(cè)量是一種更加高效的解決方案。海底地形/地貌通常具有明顯的空間變化,可利用水聲探測(cè)捕獲海底典型地形特征,作為路標(biāo)信息來(lái)支撐 SLAM 的開展[1-6]。海底環(huán)境通常具有非結(jié)構(gòu)化特征,很多情況下難以獲取清晰、明確的特征物,在海底平原區(qū)域,這種方法甚至將失效。傳統(tǒng)的光學(xué)探測(cè)方式在水下環(huán)境受到極大制約,探測(cè)距離通常只有米級(jí),難以滿足絕大多數(shù)場(chǎng)景下的水下導(dǎo)航需求。水聲探測(cè)方式的作用距離雖然大大提升,但探測(cè)分辨率相比于光學(xué)探測(cè)方式有明顯劣勢(shì),同時(shí)海流等環(huán)境干擾也會(huì)帶來(lái)額外的噪聲。因此,在自然環(huán)境特征制約和現(xiàn)有探測(cè)技術(shù)條件下,急需其他的環(huán)境特征信息來(lái)提升水下SLAM導(dǎo)航技術(shù)的性能和適用范圍。

磁場(chǎng)分布受水環(huán)境、多徑效應(yīng)及信號(hào)時(shí)延等影響較小,其探測(cè)方式是被動(dòng)的,這使得磁場(chǎng)探測(cè)相比于水聲、光學(xué)等主動(dòng)探測(cè)方式更易實(shí)現(xiàn)。因此,若能在 UUV 導(dǎo)航區(qū)域,尤其是地形/地貌特征貧乏的區(qū)域布設(shè)一些磁信標(biāo),利用實(shí)時(shí)探測(cè)的磁場(chǎng)信息對(duì)磁信標(biāo)進(jìn)行準(zhǔn)確的相對(duì)位置反演,可為航行器SLAM系統(tǒng)的路標(biāo)提供觀測(cè)量,提升系統(tǒng)的導(dǎo)航性能。其中的關(guān)鍵因素是對(duì)磁信標(biāo)位置的位置反演。

基于磁場(chǎng)梯度張量的直接反演方法最先得到研究。Wynn等[7]最早提出利用單測(cè)點(diǎn)的磁偶極子梯度張量和場(chǎng)分量,利用解析法計(jì)算場(chǎng)源三維位置,結(jié)合磁源位置的先驗(yàn)信息排除 3個(gè)干擾解得到確定位置。Brisan[8]針對(duì)動(dòng)態(tài)磁性目標(biāo)的跟蹤問題,提出一種磁源位置、磁矩和速度的迭代貝葉斯估計(jì)方法,利用連續(xù)觀測(cè)數(shù)據(jù)增強(qiáng)抗噪能力。基于梯度張量矩陣的特征分析法和解析信號(hào)方法也得到較多關(guān)注,特征分析法率先被 Beiki等[9]用于對(duì)重力梯度張量矩陣的分析,實(shí)現(xiàn)對(duì)目標(biāo)體的三維位置反演,而后被 Clark[10]推廣到磁場(chǎng)分析中,利用磁場(chǎng)梯度張量特征值計(jì)算歸一化磁源強(qiáng)度,結(jié)合梯度張量來(lái)確定磁源位置的唯一解。遲鋮等[11]基于磁梯度張量特征分析研究了多測(cè)點(diǎn)線性定位方法,但定位精度受測(cè)量系統(tǒng)基線大小和測(cè)量平臺(tái)位移誤差影響較大。萬(wàn)成彪等[12]以磁梯度張量作為磁測(cè)信息,先通過單點(diǎn)梯度張量解得位置方向矢量,再結(jié)合多點(diǎn)方向和輔助信息確定正解實(shí)現(xiàn)定位,取得與直接反演方法相當(dāng)?shù)姆囱荻ㄎ痪取５诙鄿y(cè)點(diǎn)觀測(cè)數(shù)據(jù)的定位方法對(duì)測(cè)點(diǎn)分布情況依賴性較強(qiáng),當(dāng)測(cè)點(diǎn)分布不理想時(shí),容易對(duì)最小二乘解準(zhǔn)確性產(chǎn)生不利影響。Zhang等[13]提出的歐拉反演法是目前采用較多的一種磁源反演定位方法,不需要磁源的磁矩信息,直接利用磁場(chǎng)矢量和梯度張量進(jìn)行定位求解。張朝陽(yáng)[14]分析了磁偶極子磁矩和磁梯度張量測(cè)量系統(tǒng)基線長(zhǎng)度對(duì)歐拉反演定位精度的影響。Teixeira等[15]在傳統(tǒng)歐拉反演方法的基礎(chǔ)上,利用磁梯度張量矩陣最小特征值對(duì)應(yīng)的特征向量與磁源和測(cè)點(diǎn)之間向徑的正交關(guān)系,提出一種張量歐拉反褶積(tensor Euler deconvolution,TED)和磁場(chǎng)梯度張量特征分析(eigenanalysis of the magnetic gradient tensor,EGT)相聯(lián)合的反演方法(TED+EGT),增強(qiáng)了反演結(jié)果的收斂性。Pei等[16]利用磁梯度場(chǎng)目標(biāo)模式匹配迭代搜索目標(biāo)磁源參數(shù),設(shè)計(jì)了基于UUV的磁梯度儀,并開展了物理驗(yàn)證[17]。

基于磁場(chǎng)的 SLAM 方法研究較少,多數(shù)研究利用室內(nèi)磁場(chǎng)豐富的磁異常空間特征變化及可預(yù)測(cè)性,建立了機(jī)器人 SLAM 模型[18-20]。基于文獻(xiàn)[21]研究的解析式磁梯度反演定位方法,Wu等[22]建立了無(wú)跡卡爾曼濾波(unscented Kalman filter,UKF)-SLAM模型,但該反演方法需要將磁源磁矩作為已知條件,因而在水下環(huán)境常常難以實(shí)現(xiàn)。

受定位精度或?qū)崟r(shí)性的影響,以上所述反演方法難以直接用于基于磁信標(biāo)的 SLAM 模型構(gòu)建。文中在文獻(xiàn)[15]TED+EGT方法的基礎(chǔ)上,提出連續(xù)反演定位結(jié)果的收斂性判斷準(zhǔn)則,準(zhǔn)確提取UUV與磁信標(biāo)的相對(duì)位置,進(jìn)而建立SLAM模型,試驗(yàn)對(duì)文中方法的有效性進(jìn)行了驗(yàn)證。

1 SLAM模型及工作過程

1.1 SLAM模型

用概率分布函數(shù)描述SLAM,即

式(1)表示以x0為初始位置,以為控制輸入向量,以為路標(biāo)觀測(cè)向量的系統(tǒng)狀態(tài)量概率分布情況,系統(tǒng)狀態(tài)量包括tk時(shí)刻的航行器位置xk和所觀測(cè)的路標(biāo)集合位置向量m。

系統(tǒng)量測(cè)模型

式中:h（*）為系統(tǒng)觀測(cè)量模型,即傳感器觀測(cè)數(shù)據(jù)表示環(huán)境特征的方法;vk為觀測(cè)噪聲。

以擴(kuò)展卡爾曼濾波(extended Kalman filter,EKF)為例進(jìn)行系統(tǒng)建模,主要包括運(yùn)動(dòng)更新(狀態(tài)預(yù)測(cè))和量測(cè)更新(狀態(tài)校準(zhǔn))2個(gè)環(huán)節(jié),并迭代進(jìn)行。運(yùn)動(dòng)更新

其中

式中:表示給定狀態(tài)更新、量測(cè)量和h（*）下的特征地圖;?h是h（*）的雅克比矩陣。

1.2 SLAM工作過程

SLAM工作過程如圖1所示。狀態(tài)向量xk表示航行器位置,在uk作用下從xk-1運(yùn)動(dòng)至xk。為路標(biāo)位置向量,對(duì)應(yīng)航行器的觀測(cè)量為。

圖1 水下同步定位與構(gòu)圖工作過程示意圖Fig.1 Working process of underwater simultaneous localization and mapping(SLAM)

此過程步驟如下:1)航行器首先初始化地圖z0來(lái)表示k時(shí)刻的觀測(cè)量(圖中為m1和mi分別為第1個(gè)和第i個(gè)路標(biāo)的位置向量),傳感器噪聲及特征提取過程使得此時(shí)估計(jì)的地圖不完全準(zhǔn)確;2)在u1控制下運(yùn)動(dòng)至′(根據(jù)航行器自身的運(yùn)動(dòng)模型和內(nèi)部傳感器估計(jì)求得);3)進(jìn)行下一次量測(cè)z1(z1在真實(shí)位置測(cè)得);4)航行器根據(jù)位置更新和觀測(cè)到的地圖特征對(duì)估計(jì)地圖進(jìn)行數(shù)據(jù)關(guān)聯(lián);5)更新自身位置和地圖,然后開啟下一次迭代。

2 磁信標(biāo)磁場(chǎng)模型及位置反演

要將水下環(huán)境中的磁信標(biāo)作為SLAM的路標(biāo)特征,必須從環(huán)境磁場(chǎng)中準(zhǔn)確提取磁信標(biāo)磁場(chǎng)特征,并利用適當(dāng)?shù)姆囱莘椒ü烙?jì)其相對(duì)位置。

2.1 磁信標(biāo)特征提取

1)磁信標(biāo)磁場(chǎng)模型

UUV到磁信標(biāo)的距離通常遠(yuǎn)大于磁信標(biāo)尺寸本身,因此可將磁信標(biāo)視作磁偶極子,其磁場(chǎng)符合磁偶極子模型。以磁信標(biāo)為原點(diǎn)建立直角坐標(biāo)系Oxyz,在遠(yuǎn)離原點(diǎn)的P（x,y,z）處磁場(chǎng)為

式中:Bx,By和Bz是信標(biāo)磁場(chǎng)磁感應(yīng)強(qiáng)度在x,y及z方向上的分量;μ為介質(zhì)磁導(dǎo)率;r為P點(diǎn)到磁信標(biāo)的距離;mx,my和mz分別為磁信標(biāo)磁矩在x、y及z方向上的分量。

磁梯度張量G描述了Bx,By和Bz分別沿x,y及z方向的空間變化率,即

當(dāng)航行器在磁信標(biāo)磁場(chǎng)覆蓋范圍內(nèi)航行時(shí),所測(cè)量的磁場(chǎng)矢量和梯度張量必然是地磁場(chǎng)和磁信標(biāo)磁場(chǎng)共同作用的結(jié)果。從磁場(chǎng)矢量和梯度場(chǎng)的產(chǎn)生原理可知,測(cè)量結(jié)果為磁信標(biāo)磁場(chǎng)和地磁場(chǎng)特征的線性疊加,即

2)磁信標(biāo)磁場(chǎng)提取

航行器航行過程中,背景地磁場(chǎng)可通過模型計(jì)算、航空航海測(cè)量資料、遙感等多種方式獲得。根據(jù)導(dǎo)航系統(tǒng)輸出的位置信息從數(shù)據(jù)庫(kù)中提取Be和根據(jù)式(11)和式(12)可得Bb和Gb。設(shè)定閾值σ,若連續(xù)N個(gè)測(cè)量點(diǎn)位上均滿足則認(rèn)為當(dāng)前測(cè)量磁場(chǎng)信息中包含磁信標(biāo)磁場(chǎng)。由于環(huán)境噪聲和傳感器噪聲具有比較明顯的高頻特性,采用濾波方法對(duì)Bb和Gb進(jìn)一步進(jìn)行噪聲處理,保證位置反演的準(zhǔn)確性。

2.2 磁信標(biāo)位置反演

基于磁梯度 TED方法可以進(jìn)行磁信標(biāo)位置反演計(jì)算。在一般磁場(chǎng)環(huán)境中,磁異常滿足歐拉齊次方程,即

式中,n表示齊次度(結(jié)構(gòu)化指數(shù)),對(duì)于磁偶極子,n=3。利用梯度張量可得反演方程

式中,r為反演所得位置的位置矢量。文中方法能夠?qū)崿F(xiàn)單點(diǎn)反演,并且不需要將磁信標(biāo)的磁矩作為已知條件,符合水下應(yīng)用場(chǎng)景的需求。

但上述歐拉反演方法是一個(gè)明顯的不適定問題,并且對(duì)觀測(cè)噪聲非常敏感,文獻(xiàn)[15]尋求了進(jìn)一步的求解約束條件約束提高反演計(jì)算的準(zhǔn)確性,即 TED+EGT方法。由于磁場(chǎng)梯度張量矩陣是一個(gè)實(shí)對(duì)稱陣,可以得到張量矩陣G的正交特征向量集 ｛b1,b2,b3｝,其中b3對(duì)應(yīng)絕對(duì)值最小的特征值,并且與m-r平面相垂直,m為磁信標(biāo)磁矩向量。這意味著待求的磁信標(biāo)位置向量r與b3向量正交,且處于與m-b3平面相垂直的平面內(nèi)。令得

綜合式(14)和式(15),可得

相比于式(14),極大地縮小了位置矢量r的解空間,利用最小二乘即可進(jìn)行求解。

2.3 連續(xù)測(cè)量過程中的磁信標(biāo)位置確定

對(duì)于某一確定的磁信標(biāo),其磁場(chǎng)強(qiáng)度隨著距離的增加而迅速減弱,噪聲的干擾作用相應(yīng)較強(qiáng),利用TED+EGT方法所得反演結(jié)果仍具有明顯的不確定性,具體表現(xiàn)為利用相鄰觀測(cè)點(diǎn)上的觀測(cè)數(shù)據(jù)所得磁信標(biāo)位置會(huì)有明顯差異。而當(dāng)航行器處于磁信標(biāo)磁場(chǎng)分布較強(qiáng)的區(qū)域時(shí),噪聲的干擾作用相對(duì)較小,利用載體航跡上連續(xù)測(cè)量數(shù)據(jù)所得反演結(jié)果具有較強(qiáng)的收斂性,反演結(jié)果準(zhǔn)確。

基于此,在航行器連續(xù)航行的過程中,設(shè)計(jì)磁信標(biāo)位置反演結(jié)果提取方法如下:設(shè)航行器在連續(xù)時(shí)刻上利用 2.2節(jié)所述反演方法得到磁信標(biāo)與航行器之間的相對(duì)位置序列為轉(zhuǎn)化成空間幾何距離為當(dāng)此反演位置序列滿足以下2個(gè)收斂條件時(shí),認(rèn)為反演結(jié)果準(zhǔn)確。

條件2:m≥m0。

其中,d0表示連續(xù)2次反演所得相對(duì)位置之間允許的最大變化,需要結(jié)合采樣時(shí)間間隔和航行器航行速度來(lái)確定。其物理意義在于當(dāng)反演結(jié)果能夠穩(wěn)定跟蹤某個(gè)磁信標(biāo)位置時(shí),反演所得磁信標(biāo)與航行器之間的距離(由三維相對(duì)位置得到)應(yīng)呈連續(xù)均勻變化,當(dāng)連續(xù) 2次反演結(jié)果的變化不符合航行器自身運(yùn)動(dòng)規(guī)律時(shí),則認(rèn)為反演結(jié)果不準(zhǔn)確;條件2中m0表示要求滿足條件1的連續(xù)反演結(jié)果的最小數(shù)量,顯然m0越大,對(duì)連續(xù)反演磁信標(biāo)位置的收斂性要求越高。

3 試驗(yàn)分析

3.1 磁信標(biāo)輔助的水下SLAM建模

定義系統(tǒng)狀態(tài)為

航行器在航行過程中,利用文中所述方法進(jìn)行磁信標(biāo)相對(duì)位置的反演,構(gòu)成對(duì)狀態(tài)量中航行器位置和信標(biāo)位置的線性觀測(cè),觀測(cè)方程

3.2 磁信標(biāo)反演定位試驗(yàn)分析

圖2 磁信標(biāo)反演定位結(jié)果Fig.2 Inverse positioning results of magnetic beacons

圖2(a)為利用 2.2節(jié)所述方法所得的磁信標(biāo)反演位置序列,圖 2(b)為利用 2.3節(jié)方法進(jìn)一步進(jìn)行準(zhǔn)確位置提取之后所得的反演位置結(jié)果,其中航跡上紅色位置點(diǎn)表示對(duì)應(yīng)提取反演結(jié)果的測(cè)點(diǎn)位置,針對(duì)磁信標(biāo)1,2和3,分別可提取5,7和12個(gè)連續(xù)反演位置。顯然按2.3節(jié)方法進(jìn)行位置序列提取后,所得結(jié)果均收斂在真實(shí)磁信標(biāo)位置附近。誤差統(tǒng)計(jì)如表1所示。

需要指出的是,不同磁信標(biāo)磁場(chǎng)的疊加是影響反演定位結(jié)果收斂性的主要因素之一,當(dāng)航行器與某一個(gè)磁信標(biāo)距離較近時(shí),其他磁信標(biāo)的磁場(chǎng)特征將表現(xiàn)為噪聲。當(dāng)磁信標(biāo)之間距離較遠(yuǎn)時(shí),磁場(chǎng)混疊效應(yīng)減弱,航行器對(duì)磁信標(biāo)的反演結(jié)果會(huì)有較好的收斂性。但過于稀疏的磁信標(biāo)又會(huì)影響SLAM系統(tǒng)觀測(cè)量的獲取和數(shù)據(jù)關(guān)聯(lián)效果,因此,磁信標(biāo)應(yīng)當(dāng)保持適中的空間分布已達(dá)到最佳輔助導(dǎo)航效果。

表1 磁信標(biāo)反演位置誤差統(tǒng)計(jì)Table 1 Inversion positions errors statistics of magnetic beacons

3.3 磁信標(biāo)輔助水下SLAM試驗(yàn)分析

設(shè)定航行器航行速度為 2 m/s,最大轉(zhuǎn)向角為 30°,最大轉(zhuǎn)向速率為 20°/s,期望航跡為平行往復(fù)航線,磁場(chǎng)采樣頻率為5 Hz。設(shè)定3種磁信標(biāo)布局場(chǎng)景如下:1)磁信標(biāo)分布比較稀疏,磁信標(biāo)之間平均距離約為150 m;2)磁信標(biāo)分布比較適中,磁信標(biāo)之間平均距離約為100 m;3)磁信標(biāo)分布比較密集,磁信標(biāo)之間平均距離約為 50 m。磁信標(biāo)反演收斂性條件為d=2 m,m0=5。設(shè)定航行器對(duì)磁信標(biāo)在x,y和z方向的相對(duì)位置反演結(jié)果均包含符合均值為0,均方差為1.5 m的高斯噪聲,磁信標(biāo)的磁矩參數(shù)設(shè)置同3.2節(jié)。

SLAM系統(tǒng)仿真過程中觀測(cè)量的獲取方式如下:當(dāng)連續(xù)反演定位結(jié)果滿足收斂性條件 1和條件 2的最低要求時(shí),將其中最后一次反演所得磁信標(biāo)與航行器相對(duì)位置作為觀測(cè)量對(duì)SLAM狀態(tài)進(jìn)行更新。在此基礎(chǔ)上,對(duì)之后收斂性條件的連續(xù)反演所得相對(duì)位置都作為有效觀測(cè)量用于SLAM 系統(tǒng)估計(jì)的迭代更新,直至某一個(gè)反演結(jié)果不滿足收斂條件。而后重新上述過程。3種場(chǎng)景下系統(tǒng)仿真試驗(yàn)結(jié)果如圖 3所示。圖中,綠色軌跡線為期望航線,黑色軌跡為SLAM輸出航線,紅色橢圓為航行器SLAM過程中對(duì)每個(gè)磁信標(biāo)位置估計(jì)的誤差橢圓。

圖3 3種場(chǎng)景下磁信標(biāo)輔助水下SLAM仿真試驗(yàn)結(jié)果Fig.3 Simulation results of magnetic beacon-based underwater SLAM in three different scenes

根據(jù)試驗(yàn)結(jié)果可知,雖然磁信標(biāo)之間距離較大可增強(qiáng)連續(xù)反演結(jié)果的收斂性,但路標(biāo)過于稀疏會(huì)大大減少可以獲取的觀測(cè)量信息,降低對(duì)系統(tǒng)誤差的校正效果。而磁信標(biāo)分布過于密集也不能提升系統(tǒng)狀態(tài)估計(jì)的準(zhǔn)確性,甚至使其降低,這是因?yàn)楦鞔判艠?biāo)磁場(chǎng)的混疊效應(yīng)嚴(yán)重降低了反演結(jié)果的收斂性,有效觀測(cè)量并不會(huì)隨著磁信標(biāo)數(shù)量的增加而增加。因此,只有在磁信標(biāo)分布較適中時(shí),才開獲得更高的導(dǎo)航狀態(tài)估計(jì)精度。

4 結(jié)束語(yǔ)

文中研究了基于磁信標(biāo)反演定位的水下SLAM 方法,在歐拉反演法的基礎(chǔ)上提出連續(xù)反演結(jié)果收斂性條件,以從連續(xù)反演數(shù)據(jù)中提取準(zhǔn)確的磁信標(biāo)相對(duì)位置信息。仿真試驗(yàn)驗(yàn)證了研究方法的有效性。所提出的基于水下磁信標(biāo)的SLAM 方法不要求水下磁信標(biāo)位置準(zhǔn)確已知,對(duì)于水下航行器在未知環(huán)境中開展導(dǎo)航工作具有較好的參考價(jià)值。但由于磁信標(biāo)作用距離的限制,并且需要人工布設(shè),因此文中方法主要適用于區(qū)域?qū)Ш綀?chǎng)景,相比于水聲定位系統(tǒng),具有成本低、布設(shè)方便等優(yōu)勢(shì)。而且磁場(chǎng)的空間分布特點(diǎn)使得它在淺水、多障礙等水聲信號(hào)傳播受明顯限制的環(huán)境中可以表現(xiàn)出明顯優(yōu)勢(shì)。下一步工作將研究磁場(chǎng)混疊情形下的多磁信標(biāo)同步反演定位方法,以提升基于磁信標(biāo)反演定位的SLAM系統(tǒng)性能。