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

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

0 引言

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

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

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

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

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

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

1 SLAM模型及工作過程

1.1 SLAM模型

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

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

系統(tǒng)量測模型

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

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

其中

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

1.2 SLAM工作過程

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

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

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

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

要將水下環(huán)境中的磁信標(biāo)作為SLAM的路標(biāo)特征,必須從環(huán)境磁場中準(zhǔn)確提取磁信標(biāo)磁場特征,并利用適當(dāng)?shù)姆囱莘椒ü烙嬈湎鄬ξ恢谩?/p>

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

條件2:m≥m0。

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

3 試驗分析

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

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

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

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

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

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

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

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

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

設(shè)定航行器航行速度為 2 m/s,最大轉(zhuǎn)向角為 30°,最大轉(zhuǎn)向速率為 20°/s,期望航跡為平行往復(fù)航線,磁場采樣頻率為5 Hz。設(shè)定3種磁信標(biāo)布局場景如下: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è)定航行器對磁信標(biāo)在x,y和z方向的相對位置反演結(jié)果均包含符合均值為0,均方差為1.5 m的高斯噪聲,磁信標(biāo)的磁矩參數(shù)設(shè)置同3.2節(jié)。

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

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

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

4 結(jié)束語

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