基于隨機(jī)信標(biāo)的水下SLAM導(dǎo)航方法

劉明雍，董婷婷，張立川

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

基于隨機(jī)信標(biāo)的水下SLAM導(dǎo)航方法

劉明雍，董婷婷，張立川

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

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

自主水下航行器；隨機(jī)信標(biāo)；導(dǎo)航；同時(shí)制圖定位

0 引 言

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

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

目前，國(guó)內(nèi)外關(guān)于SLAM導(dǎo)航方法的研究很多［12］，尤其是在室內(nèi)［13］、室外［14］、陸地機(jī)器人［1516］以及航空領(lǐng)域［17］，然而水下SLAM研究相對(duì)較少，主要是因?yàn)閭鞲衅髟谒聭?yīng)用時(shí)感知能力減弱，且水下環(huán)境中參考點(diǎn)較少，使得算法執(zhí)行過程中的特征點(diǎn)難以提取。

本文提出了一種基于隨機(jī)信標(biāo)的水下SLAM導(dǎo)航方法，并重點(diǎn)分析了AUV區(qū)域?qū)Ш街性摲椒ǖ膬?yōu)勢(shì)。該方法信標(biāo)位置無需提前標(biāo)定，且根據(jù)精度要求，信標(biāo)數(shù)量可以人為控制，也能當(dāng)作特征參考點(diǎn)，克服了水下SLAM參考點(diǎn)不足的缺陷。

1 隨機(jī)信標(biāo)定位問題描述

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

圖1 隨機(jī)信標(biāo)定位示意圖

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

當(dāng)觀測(cè)到下一個(gè)新信標(biāo)時(shí)，這個(gè)信標(biāo)的位置信息就被擴(kuò)充到地圖中，初始條件的不確定性和相互關(guān)系也相應(yīng)地?cái)U(kuò)充到了組合SLAM濾波器中。

2 基于隨機(jī)信標(biāo)的SLAM導(dǎo)航算法

基于隨機(jī)信標(biāo)的SLAM導(dǎo)航算法分為3個(gè)主要步驟。

2.1 狀態(tài)方程

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

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

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

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

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

式中，I是隨信標(biāo)位置求導(dǎo)，為單位矩陣。

2.2 觀測(cè)方程

對(duì)上述觀測(cè)方程求其雅克比矩陣：

所以，隨機(jī)信標(biāo)對(duì)航行器的定位問題轉(zhuǎn)化為標(biāo)準(zhǔn)的非線性濾波問題，由于狀態(tài)方程和量測(cè)方程的雅克比矩陣均已求得，可直接對(duì)其進(jìn)行擴(kuò)展卡爾曼濾波。

其測(cè)量方差為R。測(cè)量新息v：

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

卡爾曼濾波增益W：

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

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

2.3 觀測(cè)更新

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

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

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

通過加入新的信標(biāo)，算法實(shí)現(xiàn)了地圖的逐步自動(dòng)擴(kuò)展。

3 仿真與分析

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

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

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

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

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

由圖2和圖3得出，基于隨機(jī)信標(biāo)的導(dǎo)航方法精度遠(yuǎn)遠(yuǎn)高于航位推算，在仿真800多秒的時(shí)間里，隨機(jī)信標(biāo)導(dǎo)航誤差在7 m之內(nèi)，而航位推算導(dǎo)航誤差達(dá)到了100 m左右，基于隨機(jī)信標(biāo)的SLAM導(dǎo)航方法具有更好的收斂性。

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

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

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

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

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

為此，按式（16）選取絕對(duì)誤差平均值σ作為刻畫導(dǎo)航誤差的指標(biāo)，σ越大，表示誤差越大，精度越低。

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

圖8 信標(biāo)數(shù)量與導(dǎo)航誤差的關(guān)系圖

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

表1 不同觀測(cè)方差陣下的導(dǎo)航精度

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

4 結(jié)束語

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

［1］Wang W.Status and development trend of inertial technology［J］.Acta Automatica Sinica，2013，39（6）：723- 729.（王巍.慣性技術(shù)研究現(xiàn)狀及發(fā)展趨勢(shì)［J］.自動(dòng)化學(xué)報(bào)，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.（劉健，冀大雄.用固定單信標(biāo)修正水下機(jī)器人導(dǎo)航誤差［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.（劉明雍，李聞白，劉富檣，等.基于單信標(biāo)測(cè)距的水下導(dǎo)航系統(tǒng)可觀測(cè)性分析［J］.西北工業(yè)大學(xué)學(xué)報(bào)，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］.機(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)平.一種新的自主移動(dòng)機(jī)器人主動(dòng)式SLAM算法［J］.系統(tǒng)工程與電子技術(shù)，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- ），男，教授，博士，主要研究方向?yàn)樗聦?dǎo)航、慣性技術(shù)及非線性控制。

E-mail：liumingyong＠nwpu.edu.cn

董婷婷（1989- ），女，碩士研究生，主要研究方向?yàn)樗聦?dǎo)航。

E-mail：584845790＠qq.com

張立川（198-2- ），男，副教授，博士，主要研究方向?yàn)樗聦?dǎo)航與制導(dǎo)、慣性技術(shù)。

E-mail：zlc＠nwpu.edu.cn

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

2014- 11- 26；

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

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

國(guó)家自然科學(xué)基金（51109179，51179156，51379176）資助課題