AUV同時定位與跟蹤算法研究

王 闖，盧 健，黃 杰

（西安工程大學 電子信息學院，陜西 西安710048）

0 引 言

與多合作自治水下航行器（簡稱AUV）間的協同導航［1－7］和高自定位精度AUV與非合作體間的目標跟蹤［8－13］都不同，低自定位精度AUV與非合作體間的同時定位與跟蹤（簡稱SLAT）要求AUV在運動過程中同時完成自身的準確定位和獲取較高質量的非合作運動目標體的航跡信息.相比于協同導航與目標跟蹤，SLAT的復雜之處在于SLAT的探測端位姿和被探測端位置都有很大的不確定性.攜帶低精度航位推算系統的AUV具有較高的推算誤差，并且誤差隨時間的累積會使推算航跡嚴重脫離實際航跡.當其跟蹤目標時，聲納系統探測到的與目標的相對距離、方位角也具有很大誤差，AUV的航位推算系統、聲納探測信息的不確定性導致AUV自身的狀態估計和被跟蹤目標的狀態估計同時具有不確定性.事實上，AUV的狀態估計和目標的狀態估計兩者間相互依賴且相互影響.AUV要得到好的自身定位，必須有好的目標狀態估計；要得到好的跟蹤目標航跡，必須有好的自身定位.也就是說自身定位和目標狀態估計要同時進行，而且估計結果都要保持一定的精度.

對于本文提出的SLAT問題，AUV與靜態周邊環境間的同時定位與制圖（簡稱SLAM）［14－19］提供了一種解決此類兩端狀態都不確定問題的思路.SLAM可表述為：AUV在未知環境中從一個未知位姿出發，在航行過程中根據傳感器觀測和航位推算估計進行自身的定位，同時創建環境地圖.SLAM方法中的基于EKF的隨機地圖創建法，是將AUV的位姿向量和環境特征向量組織在一個高維狀態向量中，并引入能綜合反應運動體與環境信息不確定性的系統協方差矩陣；而另外的一種在實際中廣為應用的精度更高的FastSLAM算法，是基于Rao-Blackwellised粒子濾波器，利用粒子濾波器估計移動機器人的路徑，并用每一個粒子所關聯的若干個EKF分別估計環境特征的位置，很好地解決了在高維狀態空間中進行采樣時，粒子濾波效率低的問題.

仿照SLAM方法，本文提出了兩種同時定位與跟蹤（SLAT）的方法.SLAT分別利用EKF-SLAM和FastSLAM結構框架，并將其廣義狀態向量結構中的靜態環境特征由運動目標的運動狀態替代，為配備低精度航位推算系統的AUV使用自身攜帶的聲納傳感器，結合濾波器穩定地跟蹤非合作目標，并同時利用量測和對目標狀態的估計值完成對自身航位推算誤差修正的準確自定位過程.

1 模型基礎

1.1 AUV的航位推算模型

為闡述方便，設只有1個AUV跟蹤1個非合作目標M.進行跟蹤的AUV在二維平面中的航位推算系統模型可表述為

其中 T（A），，為AUV的采樣周期以及k時刻的本體傳感器所得的運動速度和偏航角量測.其中：

1.2 目標運動的跟蹤模型

雖然目標幾乎從來不是一個真正的點，且其方向信息對于跟蹤也是有用的，但通常還是把目標看作沒有形狀的一個點.幾乎所有的機動目標跟蹤方法都是基于模型的.目標M在二維平面內k時刻的狀態向量用）＝［）］T來描述，其中，（，）是位移量，（，）是速度量，，）是加速度量.目標動態模型描述了目標狀態X（M）隨時間的演化過程，常用的離散狀態空間模型的線性化形式為

對于式（2），其具體運動模型有多種可能，從水下運動目標的運動特性上分析，最常見的運動模式為直線運動，模型分別為

Ⅰ 勻速模型（CV model）：

其中 T（M）為目標M的采樣周期為k時刻過程噪聲協方差矩陣的系數.

Ⅱ 勻加速模型（CA model）：

當目標發生大的機動，基本模型不能滿足跟蹤需要時，可以采用綜合基本模型的交互多模型［12］方法跟蹤復雜運動目標.

1.3 量測模型

當AUV的聲納系統探測目標M時，探測回波可提供徑向距離和方位角兩維量測信息，即

設νk＝ ［ΔrkΔθk］T服從高斯分布，則E［νk］＝0，E［νk（νl）T］＝Rkδkl.

量測模型可被簡單地描述為

2 基于EKF-SLAM框架的SLAT算法

2.1 狀態預測

在k＋1時刻系統的狀態協方差的預測為

2.2 量測更新

使用k＋1時刻傳感器量測更新系統狀態，設在k＋1時刻利用系統狀態對量測值的估計為

從而k＋1時刻的新息和新息協方差為

濾波器增益為

對廣義狀態向量及其協方差矩陣進行更新，有

其中

3 基于FastSLAM框架的SLAT算法

設定濾波器中有N個粒子，并設采樣時間T（A）＝T（M），粒子i的構成可由式（3）表示.

粒子i對AUV在k＋1時刻的狀態估計值可以表示

根據式（1），可得

根據式（2），可以推算得到目標M在k＋1時刻的狀態估計值為

在k＋1時刻目標M的狀態協方差的預測為

使用k＋1時刻傳感器量測更新系統狀態，設粒子i在k時刻狀態量對量測值的估計為

從而k＋1時刻的新息和新息協方差為

濾波器增益為

則對目標狀態向量及其協方差矩陣進行更新，有

其中，

權值可表示為AUV的運動分布函數與建議分布函數之比，即

則有

歸一化所有粒子的權值

則k＋1時刻AUV的位姿和目標狀態估計為

為了保證粒子的有效性，估計過程中經常需要進行重抽樣.判斷是否需進行重抽樣的指標為

Neff代表有效粒子數，當Neff小于設定最小粒子數時，需要進行重采樣.根據權值大小，復制權值大的粒子，并刪除權值小的粒子，最終重新抽取N個粒子，并設所有粒子權重為1／N.

4 仿真分析

為了標注簡單，將基于EKF-SLAM框架的SLAT算法和基于FastSLAM框架的SLAT算法簡化命名為SLATE和SLATF.設目標M和AUV沿著各自方向運動，并設定AUV可實時探測到目標M以獲取相應的距離和方位信息.為了驗證算法的有效性，本文仿真中將目標M的真實航跡，AUV的真實航跡和航位推算軌跡顯示以比較.進行100次Monte Carlo仿真，仿真結果如圖1～4所示.

圖3 目標位置估計軌跡比較

圖2 AUV位置估計軌跡比較

圖1 目標和AUV的真實軌跡

圖4 AUV位置估計的誤差比較

圖1 ～3為粒子N＝100時仿真結果.圖1顯示了目標和AUV真實的運動軌跡關系.在運動過程中，AUV與非合作目標體之間無通信聯系，通過AUV攜帶的聲納系統實時探測目標體得到目標的距離與方位信息.圖2顯示了AUV真實運動軌跡、航位推算結果、SLATE和SLATF的估計結果，SLATE的估計效果明顯優于航位推算，而SLATF與運動軌跡真值的復合度最高，優于SLATE.圖3顯示了目標M的真實軌跡，SLATE和SLATF的航跡估計結果.從估計值與真值的復合程度來看，SLATF對非合作目標體位置量有一個更好的估計，表明了算法的有效性.圖4～6對AUV的航位推算，SLATE和粒子數N取不同值時的SLATF估計誤差作了比較.隨著時間的推移，航位推算系統的累積誤差越來越大，而SLATE和SLATF能有效地抑制誤差在一個合理的水平內，證明估計航跡時本文提出的方法明顯優于航位推算，而SLATF優于SLATE.橫向比較圖4～6中SLATF的估計誤差曲線可以看出，隨著使用粒子的增多，估計誤差有變小的趨勢，說明足夠的粒子可以提升估計器的性能，但提升水平相對有限.

在理想情況下，衡量算法是否滿足一致性是將其估計值與理想濾波器中獲取的概率密度函數進行比較.在實際中理想概率密度函數是無法獲取的，但當被估計狀態真值已知時，可以使用標準化估計誤差方差（normalized estimation error squared，NEES）測試濾波一致性.圖5中兩種SLAT算法NEES的均值基本位于置信區間中，說明了估計過程具備一致性.

圖5 AUV位姿NEES均值

5 結束語

本文將AUV運動的推算模型、量測模型與目標運動的跟蹤模型融合于EKF-SLAM框架和FastSLAM框架內，提出了兩種AUV跟蹤目標同時自定位的方法.在基于EKF-SLAM框架的SLAT中，雖然目標和AUV的運動狀態會相互依賴和影響，但將二者統一在一個廣義狀態向量中，狀態估計誤差間相關性帶來的影響可以通過協方差矩陣在濾波器中消除.而對于植入粒子濾波器的基于FastSLAM框架的SLAT，具有更優的估計效果，更好地解決了邊跟蹤邊自定位問題.從仿真結果中可以看出，兩種SLAT方法不但AUV自定位的累積誤差相比于航位推算減小很多，而且同時也能對非合作目標的航跡有較準確的估計.

［1］ CURCIO J，LEONARD J，VAGANAY J，et al.Experiments in moving baseline navigation using autonomous surface craft［C］／／Proceedings of 2005MTS／IEEE International Conference on OCEANS，Washnigon DC，IEEE，2005：730-735.

［2］ GADRE A S，STILWELL D J.Toward underwater navigation based on range measurements from a single location［C］／／Proceedings of 2004IEEE International Conference on Robotics and Automation，New Orleans，IEEE，2004：4472-4477.

［3］ 許真珍，封錫盛.多 UUV協作系統的研究現狀與發展［J］.機器人，2007，29（2）：186-192.

［4］ MACZKA D K，GADRE A S，STILWELL D J.Implementation of a cooperative navigation algorithm on a platoon of autonomous underwater vehicles［C］／／Proceedings of 2007MTS／IEEE International Conference on OCEANS.Vancouver BC，Canada：IEEE，2007：1-6.

［5］ BAHR A，LEONARD J.Cooperative localization for autonomous underwater vehicles［J］.Autonomous Robots：Proceedings of Experimental Robotics，2006，39（5）：387-395.

［6］ VAGANAY J，LEONARD J J，CURCIO J A，et al.Experimental validation of the moving long base-line navigation concept［C］／／Proceedings of 2004IEEE／OES International Conference on Autonomous Underwater Vehicles Nagoa，Japan：IEEE，2004：59-65.

［7］ BAKER B N，ODELL D L，ANDERSON M J，et al.A new procedure for simultaneous navigation of multiple AUVs［C］／／Proceedings of 2005MTS／IEEE International Conference on OCEANS，Washnigon DC：IEEE，2005：1-4.

［8］ XU W M，LIU Y C，YIN X D.Method for underwater target tracking based on an interacting multiple model［J］.Geospatial Information Science，2008，11（3）：186-190.

［9］ El-Shafile A，OSMAN A，NOURELDIN A，et al.Performance enhancement of underwater target tracking by fusing data of array of global positioning system sonobuoys［J］.Journal of Computer Science，2009，5（3）：199-206.

［10］ RAO S K，MURTHY K S L，RAJESWARI K R.Data fusion for underwater target tracking［J］.IET Radar，Sonar and Navigation，2010，4（4）：576-585.

［11］ LI X R，JILKOV V P.Survey of maneuvering target tracking—partⅠ：Dynamic model［J］.IEEE Trans.on Aerospace and Electronic Systems，2003，39（4）：212-235.

［12］ LI X R，JILKOV V P.Survey of maneuvering target tracking—partⅤ：Multiple-model methods［J］.IEEE Trans on Aerospace and Electronic Systems，2005，41（4）：1255-1321.

［13］ BLACKMAN S S.Multiple hypothesis tracking for multiple target tracking［J］.IEEE Trans on Aerospace and Elec-tronic Systems Magazine，2004，19（1）：5-18.

［14］ THRUN S，BURGARD W，FOX D.Probabilistic Robotics［M］.Lodon：MIT Press，2005：52-60.

［15］ BAILEY T，NIETO J，GUIVANT J.Consistency of the EKF-SLAM algorithm［C］／／Proceedings of 2006IEEE／RSJ International Conference on Intelligent Robots and Systems.Beijing：IEEE，2006：3562-3568.

［16］ LAZARO M T，CASTELLANOS J A.Localization of probabilistic robot formations in SLAM［C］／／Proceedings of 2010IEEE International Conference on Robotics and Automation.Anchorage AK：IEEE，2010：3179-3184.

［17］ Durrant-Whyte H，BAILEY T.Simultaneous localization and mapping：partⅠ［J］.IEEE Robotics and Automation Magazine，2006，13（2）：99-110.

［18］ BAILEY T，DURRANT-WHYTE H.Simultaneous localization and mapping：partⅡ［J］.IEEE Robotics and Automation Magazine，2006，13（3）：108-117.

［19］ DISSANAYAKE M W M G，NEWMAN P，CLARK S，et al.A solution to the simultaneous localization and map building （SLAM）problem［J］.IEEE Transactions on Robotics and Automation，2001，17（3）：229-241.