基于隨機信標的水下SLAM導航方法

劉明雍，董婷婷，張立川

（西北工業(yè)大學航海學院，陜西西安710072）

基于隨機信標的水下SLAM導航方法

劉明雍，董婷婷，張立川

（西北工業(yè)大學航海學院，陜西西安710072）

研究了基于隨機信標的水下同時制圖定位（simultaneous localization and mapping，SLAM）導航定位方法。信標導航是目前導航領域的研究熱點，但往往需要提前對信標位置進行標定。對此文中提出一種無需位置標定的隨機信標導航方法，即在信標隨機散布的情況下，通過量測信標和航行器間的距離和方位，用SLAM方法對隨機信標位置進行估計，從而實現(xiàn)對航行器的導航，并在不同的信標密度和觀測誤差下分析了其導航精度。仿真結果表明，該導航方法具有良好的收斂性和定位精度。

自主水下航行器；隨機信標；導航；同時制圖定位

0 引 言

現(xiàn)階段，無需人類參與的自主水下航行器被廣泛應用于海洋資源的開發(fā)利用中，而航行器的自主導航能力是衡量其性能的重要指標之一。雖然導航方法種類繁多，但是由于水下環(huán)境的特殊限制，使得水下導航方法受到一定限制。目前，水下導航方法多為慣導和其他一些輔助導航設備的組合導航系統(tǒng)，常用的有慣導／長基線組合導航系統(tǒng)，長基線水聲定位系統(tǒng)由安裝在海底的應答器和被定位載體的問答器組成，需要提前對水下應答器進行位置校準，人力物力花費量大，且維護困難；慣性／全球定位系統(tǒng)的組合導航方法需要定期接收全球定位系統(tǒng)（global position system，GPS）信號，導致隱蔽性差，且不利于深水航行等［12］；慣導／多普勒組合導航方法沒有直接量測位置，對位置進行校正，因此誤差仍舊呈較明顯的發(fā)散趨勢，不利于長時間航行。

因此，信標導航方法由于不需要GPS信號，且直接對航行器位置進行校準等特點成為導航研究領域一種新的趨勢［3］，目前信標導航方法的研究多為基于固定信標的導航［4-5］，固定信標導航分為單信標導航和多信標導航。單信標導航方法簡單，容易實施，且具有很好收斂性［6］，但也具有一定的缺陷性。由文獻［7- 8］對單信標定位可觀測性分析可知，單信標導航在一定條件下可以滿足導航性能的要求，可是該方法是利用單個信標距離信息確定航行器的位置，使得求解不充分，所以必須在設計適當?shù)臋C動路徑的條件下利用該方法進行導航?？紤]一般自主水下航行器（autonomous umderwater vehicle，AUV）的實際航行路徑，在一些情況下，并不能滿足這個要求。文獻［9］提出的基于固定信標的導航方法需提前對信標位置進行標定，花費大量的人力物力，不經(jīng)濟也不能滿足導航及時性的要求。文獻［10- 11］提出了基于移動信標的導航方法，該方法對導航范圍沒有限制，且能滿足可觀測性的要求，但對信標自身的要求較高且需要提前對信標位置進行標定。

目前，國內外關于SLAM導航方法的研究很多［12］，尤其是在室內［13］、室外［14］、陸地機器人［1516］以及航空領域［17］，然而水下SLAM研究相對較少，主要是因為傳感器在水下應用時感知能力減弱，且水下環(huán)境中參考點較少，使得算法執(zhí)行過程中的特征點難以提取。

本文提出了一種基于隨機信標的水下SLAM導航方法，并重點分析了AUV區(qū)域導航中該方法的優(yōu)勢。該方法信標位置無需提前標定，且根據(jù)精度要求，信標數(shù)量可以人為控制，也能當作特征參考點，克服了水下SLAM參考點不足的缺陷。

1 隨機信標定位問題描述

隨機信標是指位置事先未知但具有通信功能的信標。在一些特殊的情況下，如AUV自身攜帶的導航設備發(fā)生故障而又對航行器導航精度要求較高等，需要臨時對AUV進行定位和導航，可以利用飛機向一定范圍海域散布信標，但信標位置未知，考慮利用同時制圖定位（simultaneous localization and mapping，SLAM）算法對航行器和信標進行定位，如圖1所示，通過測量航行器和信標之間的距離R1，R2，…，Ri（i＝1，2，…，N）和方位角對航行器不斷進行定位和SLAM地圖信標的不斷擴充。

圖1 隨機信標定位示意圖

SLAM是指自主航行器在未知環(huán)境中行駛時，依賴傳感器所獲得的信息進行增量式環(huán)境建模，同時利用所創(chuàng)建的環(huán)境地圖估測其位姿［1820］。假設航行器初始位置已知，通過觀測信標，可對信標相對位置進行估計，重新觀測到以前觀測的信標可以對航行器的位置進行更新，從而對航行器進行定位。假設信標是具有零輸入擾動的靜態(tài)路標，則第i個信標的模型可以簡化為

當觀測到下一個新信標時，這個信標的位置信息就被擴充到地圖中，初始條件的不確定性和相互關系也相應地擴充到了組合SLAM濾波器中。

2 基于隨機信標的SLAM導航算法

基于隨機信標的SLAM導航算法分為3個主要步驟。

2.1 狀態(tài)方程

在航行器運動學模型已知的情況下，通過觀測得到相對航行器的信標位置，可以建立增廣的狀態(tài)估計數(shù)學模型，運用擴展卡爾曼濾波對狀態(tài)進行估計。定義系統(tǒng)的狀態(tài)量為

式中，xv（k）是航行器運動狀態(tài)，包括位置和姿態(tài)；xb（k）是信標地圖狀態(tài)向量，隨著觀測到的新的信標位置的加入，其維數(shù)不斷增加。

式中，p（k）＝［x（k） y（k）］，表示航行器的位置坐標；φ（k）表示航向；bi（k）（i＝1，2，…，n）表示信標在地圖中的位置。

若航行器狀態(tài)改變量用相鄰時刻的航向改變量以及在航向方向上和垂直航向方向上的距離變化量表示為ΔX＝［Δx Δy Δφ］T，則航行器位置可以描述為

根據(jù)上述狀態(tài)方程求雅克比矩陣

式中，I是隨信標位置求導，為單位矩陣。

2.2 觀測方程

對上述觀測方程求其雅克比矩陣：

所以，隨機信標對航行器的定位問題轉化為標準的非線性濾波問題，由于狀態(tài)方程和量測方程的雅克比矩陣均已求得，可直接對其進行擴展卡爾曼濾波。

其測量方差為R。測量新息v：

新息協(xié)方差矩陣S：

卡爾曼濾波增益W：

更新后的位置^X*和協(xié)方差P*：

通過式（9）～式（13），就可以利用測量完成對航行器狀態(tài)和特征狀態(tài)的同步更新過程。若沒有觀測到新的特征，則可以進入下一時刻狀態(tài)預測和更新的迭代遞推過程。否則，必須進行擴維，實現(xiàn)地圖的自動擴展。

2.3 觀測更新

航行器在運動過程中，不斷探索新的環(huán)境并發(fā)現(xiàn)新的信標，因此在完成對已有信標位置的更新后，需要對其狀態(tài)變量進行擴維處理，將新信標位置加入到狀態(tài)中去，建立航行器狀態(tài)及地圖已有特征與新特征間的關系，以構造精確、收斂的地圖。

設已確定量測z＝［r φ］T來源于一新特征，其中x和y表示航行器的位置坐標；r和φ分別表示新特征和航行器之間的相對距離和方位角，可將新特征在全局坐標系下的位置表示為

將新特征加入到系統(tǒng)特征向量中，擴充后的新系統(tǒng)狀態(tài)向量為

通過加入新的信標，算法實現(xiàn)了地圖的逐步自動擴展。

3 仿真與分析

在設定好初始條件下，使航行器分別實現(xiàn)圓圈形和蛇形區(qū)域搜索兩種軌跡，并將此方法與航位推算方法進行比較，航位推算與該方法量測誤差相同，仿真結果如圖2～圖5所示。

圖2 圓圈型區(qū)域搜索

圖3 圓圈型區(qū)域搜索導航誤差

圖4 蛇型區(qū)域搜索

圖5 蛇型區(qū)域搜索導航誤差

由圖2和圖3得出，基于隨機信標的導航方法精度遠遠高于航位推算，在仿真800多秒的時間里，隨機信標導航誤差在7 m之內，而航位推算導航誤差達到了100 m左右，基于隨機信標的SLAM導航方法具有更好的收斂性。

由圖4和圖5可以看出，在前700 s的仿真時間里，隨機信標定位誤差基本保持在6 m之內，而航位推算誤差達到了100多米，基于隨機信標的SLAM導航方法表現(xiàn)出了明顯的優(yōu)勢，相比航位推算具有更好的收斂性。由圖4也可以看出x軸上0～350 m內信標分布較密集，而基于隨機信標的導航精度基本控制在6 m之內，而350～450 m內信標分布稀疏，誤差突然增大到10 m左右，可以看出信標相對較密集的區(qū)域導航精度較高。

為了更進一步探究信標密度對導航精度的影響，在蛇形搜索方式下，將信標數(shù)量減少到8個，對該方法與航位推算方法進行了仿真，結果如圖6和圖7所示。

圖6 蛇型區(qū)域搜索（信標數(shù)量為8）

圖7 蛇型區(qū)域搜索導航誤差（信標數(shù)量為8）

由圖6和圖7可以看出，信標數(shù)量由25個降低到8個時，基于隨機信標的導航在700 s誤差由6 m左右增加到15 m左右，雖然精度有所下降，但是降低了成本，并且和航位推算方法相比仍然表現(xiàn)出了明顯的優(yōu)勢。在不同情況下對導航精度要求的不同散布適當數(shù)量的信標，使得滿足不同情況下對導航精度的要求。

為此，按式（16）選取絕對誤差平均值σ作為刻畫導航誤差的指標，σ越大，表示誤差越大，精度越低。

在不同數(shù)量信標導航下，利用蒙特卡羅法進行大量仿真實驗，通過計算和分析仿真結果，分別得到不同數(shù)量信標下的導航誤差σ，繪制出誤差σ散點圖與誤差擬合曲線，如圖8所示。

圖8 信標數(shù)量與導航誤差的關系圖

由圖8可以看出，當信標數(shù)量在70個以內時，導航誤差σ隨著信標數(shù)量的增加而減小，而當信標數(shù)量達70個以上時，誤差σ基本保持在5.5 m附近，依然呈遞減趨勢。主要原因是當信標較稀疏時，導航誤差較大是由于信標數(shù)量不足引起的，隨著信標數(shù)量增多，加入到濾波器中的狀態(tài)量增多，對航行器的位置校正就越來越準確，而當信標數(shù)量達到一定程度時，由于受系統(tǒng)噪聲及傳感器的固有噪聲影響，不可能無限制的消除誤差，最終使得導航誤差保持在一定的范圍內。

表1 不同觀測方差陣下的導航精度

通過分析在不同的量測誤差下基于隨機信標導航方法的誤差，得出測量精度較高的情況下，該算法具有較高的精度。因此在不同的搜索目的下，可根據(jù)實際情況選擇精度不同的測量裝置，既能滿足導航精度的要求，還能降低導航成本。

4 結束語

本文對基于隨機信標的SLAM導航算法進行了研究，建立了系統(tǒng)狀態(tài)方程和量測方程，應用同時制圖定位方法，將隨機信標定位問題轉化為非線性濾波問題。通過建立增廣矩陣，利用擴展卡爾曼濾波同時對航行器與隨機信標的位置進行估計，并針對不同信標密度的情況，對該算法進行了MATLAB仿真。仿真結果證明了在同樣的導航測量誤差干擾下，基于隨機信標的水下SLAM導航算法獲得的定位精度都遠遠高于航位推算導航效果，且具有很好的收斂性，此算法既克服了目前水聲導航需提前布放基陣，且需對其位置進行校準的缺點，又解決了水下SLAM參考點難以捕捉的問題，所以在水下導航方面具有很大的應用潛力。盡管如此，信標散布后位置受洋流影響使得航行器導航會有所誤差，所以該算法仍需要改進以及系統(tǒng)地完善。

［1］Wang W.Status and development trend of inertial technology［J］.Acta Automatica Sinica，2013，39（6）：723- 729.（王巍.慣性技術研究現(xiàn)狀及發(fā)展趨勢［J］.自動化學報，2013，39（6）：723- 729.）

［2］Paull L，Saeedi S，Seto M，et al.AUV navigation and localization：a review［J］.IEEE Journal of Oceanic Engineering，2014，39（1）：131- 149.

［3］Ferreira B，Matos A，Cruz N.Single beacon navigation：localization and control of the MARES AUV［C］∥Proc.of the IEEE Oceans，2010：1- 9.

［4］Bahr A，Leonard J J，Martinoli A.Dynamic positioning of beacon vehicles for cooperative underwater navigation［C］∥Proc.of the IEEE／RSJ International Conference on Intelligent Robots and Systems，2012：3760- 3767.

［5］Liu J，Ji D X.Navigation error amendment for underwater robot using single beacon［J］.Control and Decision，2010，25（9）：1354-1358.（劉健，冀大雄.用固定單信標修正水下機器人導航誤差［J］.控制與決策，2010，25（9）：1354- 1358.）

［6］Webster S E，Eustice R M，Singh H，et al.Advances in singlebeacon one-way-travel-time acoustic navigation for underwater vehicles［J］.The International Journal of Robotics Research，2012，31：935- 950.

［7］Liu M Y，Li W B，Liu F Q，et al.Underwater navigation system observability analysis based on a single beacon ranging［J］.Journal of Northwestern Polytechnical University，2011，29（1）：87- 92.（劉明雍，李聞白，劉富檣，等.基于單信標測距的水下導航系統(tǒng)可觀測性分析［J］.西北工業(yè)大學學報，2011，29（1）：87- 92.）

［8］Batista P，Silvestre C，Oliveira P.Single beacon navigation：observability analysis and filter design［C］∥Proc.of the American Control Conference，2010：6191- 6196.

［9］Chitre M.Path planning for cooperative underwater range-only navigation using a single beacon［C］∥Proc.of the IEEE International Conference on Autonomous and Intelligent Systems，2010：1- 6.

［10］Scherbatyuk A P，Dubrovin F S.Some algorithms of AUV positioning based on one moving beacon［C］∥Proc.of the Navigation，Guidance and Control of Underwater Vehicles，2012，3（1）：1- 6.

［11］Liu J，Dong S F，Wen D，et al.Improved DV-Hop algorithm based on mobile beacon［J］.Transducer and Microsystem Technologies，2013：28- 32.

［12］Bailey T，Nieto J，Guivant J，et al.Consistency of the EKFSLAM algorithm［C］∥Proc.of the IEEE／RSJ International Conference on Intelligent Robot and Systems，2006：3562- 3568.

［13］Lee D.Experiments on localization of an AUV using graphbased SLAM［C］∥Proc.of the 10th IEEE International Conference on Ubiquitous Robots and Ambient Intelligence，2013：526- 527.

［14］Wang H J，Wang J，Bian X Q，et al.SLAM of AUV based on the combined EKF［J］.Robot，2012，34（1）：56- 64.（王宏健，王晶，邊信黔，等.基于組合EKF的自主水下航行器SLAM［J］.機器人，2012，34（1）：56- 64.）

［15］Zeng J，Guo X S，Zhang G L.Study on method of autonomous mobile robot integrated navigation based on SLAM［J］.Applied Mechanics and Materials，2013，263：1290- 1297.

［16］Wang X H，F(xiàn)u W P.New active SLAM algorithm for autonomous mobile robot［J］.Systems Engineering and Electronics，2012，34（11）：2334- 2338.（王曉華，傅衛(wèi)平.一種新的自主移動機器人主動式SLAM算法［J］.系統(tǒng)工程與電子技術，2012，34（11）：2334- 2338.）

［17］Achtelik M W，Lynen S，Weiss S，et al.Visual-inertial SLAM for a small helicopter in large outdoor environments［C］∥Proc. of the IEEE／RSJ International Conference on Intelligent Robots and Systems，2012：2651- 2652.

［18］Newman P，Leonard J.Pure range-only sub-sea SLAM［C］∥Proc.of the IEEE International Conference on Robotics and Automation，2003：1921- 1926.

［19］Durrant-Whyte H，Bailey T.Simultaneous localization and mapping：part I［J］.IEEE Robotics&Automation Magazine，2006，13（2）：99- 110.

［20］Johnson T，Seeling P.Mobile node localization using cooperation and static beacons［C］∥Proc.of the IEEE International Conference on Communications Workshops，2013：281- 285.

Underwater SLAM navigation algorithm based on random beacons

LIU Ming-yong，DONG Ting-ting，ZHANG Li-chuan
（School of Marine Science and Technology，Northwestern Polytechnical University，Xi’an 710072，China）

An underwater simultaneous localization and mapping（SLAM）navigation algorithm based on random beacons is studied.At present，beacon navigation is extensively studied in the field of navigation，but it often needs to locate the beacon’s positon in advance.In this paper，we propose a navigation method based on random beacons that does not need position calibration，that is，when beacons are randomly dispersed，we use the SLAM method to estimate the position of the random beacons，thus to navigate the autonomous underwater vehicle（AUV）by observing the distance and orientation between the beacons and the AUV.We also analyze the navigation accuracy under different beacons density and different observation errors.Simulation results show that the algorithm performs well in both convergence and precision.

autonomous underwater vehicle（AUV）；random beacons；navigation；simultaneous localization and mapping（SLAM）

TP 24

A

10.3969／j.issn.1001-506X.2015.12.25

劉明雍（1971- ），男，教授，博士，主要研究方向為水下導航、慣性技術及非線性控制。

E-mail：liumingyong＠nwpu.edu.cn

董婷婷（1989- ），女，碩士研究生，主要研究方向為水下導航。

E-mail：584845790＠qq.com

張立川（198-2- ），男，副教授，博士，主要研究方向為水下導航與制導、慣性技術。

E-mail：zlc＠nwpu.edu.cn

1001-506X（2015）12-2830-05

2014- 11- 26；

2015- 04- 10；網(wǎng)絡優(yōu)先出版日期：2015- 08- 31。

網(wǎng)絡優(yōu)先出版地址：http：／／www.cnki.net／kcms／detail／11.2422.TN.20150831.1933.016.html

國家自然科學基金（51109179，51179156，51379176）資助課題