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

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

0 引言

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

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

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

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

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

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

1 SLAM模型及工作過程

1.1 SLAM模型

用概率分布函數描述SLAM,即

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

系統(tǒng)量測模型

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

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

其中

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

1.2 SLAM工作過程

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

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

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

2 磁信標磁場模型及位置反演

要將水下環(huán)境中的磁信標作為SLAM的路標特征,必須從環(huán)境磁場中準確提取磁信標磁場特征,并利用適當的反演方法估計其相對位置。

2.1 磁信標特征提取

1)磁信標磁場模型

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

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

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

當航行器在磁信標磁場覆蓋范圍內航行時,所測量的磁場矢量和梯度張量必然是地磁場和磁信標磁場共同作用的結果。從磁場矢量和梯度場的產生原理可知,測量結果為磁信標磁場和地磁場特征的線性疊加,即

2)磁信標磁場提取

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

2.2 磁信標位置反演

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

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

式中,r為反演所得位置的位置矢量。文中方法能夠實現單點反演,并且不需要將磁信標的磁矩作為已知條件,符合水下應用場景的需求。

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

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

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

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

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

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

條件2:m≥m0。

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

3 試驗分析

3.1 磁信標輔助的水下SLAM建模

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

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

3.2 磁信標反演定位試驗分析

圖2 磁信標反演定位結果Fig.2 Inverse positioning results of magnetic beacons

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

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

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

3.3 磁信標輔助水下SLAM試驗分析

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

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

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

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

4 結束語

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