自主水下航行器同時定位與制圖技術研究

潘 爽，施建禮，聶永芳，王希彬

(1.海軍潛艇學院 戰略導彈與水中兵器系，山東 青島 266041；2.海軍航空大學 航空基礎學院，山東 煙臺 264001)

0 引 言

自主水下航行器（Autonomous Underwater Vehicle，AUV）具有自主性高、機動性好、探測地域大、隱蔽性強等顯著優勢，在水下環境中得到廣泛應用，而要實現AUV的這些優勢，最關鍵的是要實現AUV的自主導航與探測。然而，精準的AUV自主導算法需要系統提供遠距離、長時間的精準速度、位置和姿態信息，而這一技術主要利用SLAM算法完成AUV的自主定位與制圖。該算法可以描述為[1]：AUV從初始的位置出發，在未知環境中運動，依靠自身所攜帶的傳感器對未知環境進行感知，處理計算機對傳感器數據進行處理并創建地圖，同時依據此地圖對AUV進行導航。

SLAM算法最先由國外學者提出[2]，并在國外發展迅速，由于在機器人的自主定位中廣泛應用，算法的種類也越來越多，技術較為成熟。國內SLAM算法研究起步較晚，目前已在地面機器人、空中無人機和水下航行器等領域展開應用。本文提出一種基于EKF的水下SLAM算法[3]，通過對特征進行2次探測，提高了AUV和特征的定位精度，以便更好實現AUV的自主精確導航與探測。

1 水下航行器模型構建

航行器的運動模型和環境探測模型是SLAM系統的關鍵部分，AUV在水下的運動狀態由航行器系統的運動模型體現，AUV裝載的聲吶傳感器由環境探測模型表示。模型的創建需要在一致的坐標系內，本文采用全局坐標系作為航行器系統模型的基礎坐標系。

1.1 AUV水下運動模型

AUV相對于全局坐標系的位置和姿態，均可以通過運動坐標系相對全局坐標系的坐標值和運動坐標系相對于全局坐標系的旋轉角確定。AUV在水下的運動模型為六自由度，則AUV的運動方程可表示為：

式中：為前進速度，m/s；為側向速度，m/s；為升降速度，m/s；為滾轉角速度，rad/s；為偏航角速度，rad/s；為俯仰角速度，rad/s。

1.2 水下觀測模型

聲吶的探測信息是AUV測量環境特征位置、方向的信息來源[4]，特征模型在全局坐標系[5]中的表[示，需要]對特征模型在傳感器坐標系中的位置和方向Z=ρ,ψs,θsT進行坐標變換。已知傳感器安裝位置到AUV重心的距離，即傳感器坐標原點到運動坐標系的坐標原點距離已知，假設兩坐標系坐標原點在同一水平面上且不考慮AUV滾轉運動，AUV的觀測模型如圖1所示。

則觀測方程可以表示為：

其中， (xs,ys,zs)為特征模型在觀測坐標系中的坐標表示，根據聲吶坐標系與全局坐標系和位置坐標系間的方位關系，可以得出觀測模型。

2 基于 EKF 的水下 SLAM 算法

在建立水下自主航行器（AUV）系統模型基礎上，提出采用基于捷聯慣導系統與聲吶傳感器的水下自主航行器同時定位與制圖算法EKF-SLAM算法。捷聯慣導的導航原理[6]是通過慣性器件測量水下自主航行器的角速度和3個敏感軸向的加速度，并通過四元數法解算出自主水下航行器的位姿和速度，但是計算所得到的是近似值，存在計算誤差，隨時間增長導航偏差會不斷增大。基于捷聯慣導系統與聲吶傳感器的水下自主航行器同時定位與制圖技術的EKF-SLAM算法，利用捷聯慣導的輸出參數與聲吶傳感器探測的環境特征估計定位，通過算法的遞推更新濾除誤差，提高導航定位精度[7]。

2.1 狀態方程

AUV的位置、姿態信息是通過對慣性導航器件的輸出進行積分運算得到的，慣導器件的輸出參數含有隨機誤差，將會影響到AUV的定位精度，且隨著時間的累積，誤差的影響也明顯，最終導致AUV迷失方向。本文解決這一問題的方法是通過在AUV的狀態向量中引入慣導器件的高斯誤差[8]，通過濾波器消除這一誤差。

水下自主航行器的狀態方程可以表示成：式中：為時刻的狀態向量；為狀態轉換矩陣；為時刻的控制向量；為高斯白噪聲向量，設其均值為零，協方差為，可以表示為

其中，為AUV在全局坐標系中的位置；為AUV各坐標線速度和轉向角速度；為AUV相對于前一時刻的坐標位置。

2.2 觀測方程

AUV利用裝載在機身上的主動聲吶，觀測環境信息從而獲得特征模型和探測聲吶之間的幾何位置關系，并以此為參考校正AUV的坐標。聲吶探測模型是通過構建聲吶傳感器同特征模型之間的距離和方向獲得的，則第i個特征的觀測方程為：

已知是特征模型的狀態向量，即，那么

即為表示在時刻的AUV狀態向量和特征模型狀態向量函數。設AUV在運動坐標系中的狀態向量為，則觀測方程可以表示為:

由于是非線性的，只有對觀測方程進行線性化，才能通過濾波器實現SLAM算法。利用對AUV狀態向量進行雅克比替代[9]，如下：

便可實現觀測方程的線性化，觀測方程可寫為：

3 仿真試驗與分析

AUV工作環境為100 m×100 m水下模擬環境，以特征點表示水下信標、海丘、海嶺等特征明顯的海底地貌，仿真圖用*表示特征點。AUV是以3±0.3 m/s的恒定航速做勻速運動，初始角為0°，最大轉彎角30±3°，最大轉彎角速度為20 rad/s。聲吶探測的最遠距離為30 m，測量誤差為0.1 m，則狀態方程的誤差協方差，觀測方程的誤差協方差。將初始化后的系統參數載入EKF-SLAM算法的工作環境，開始進行仿真。檢驗算法在環境特征點為50個時做圓周運動的定位與制圖精度，如圖2所示。

從圖3和圖4可以看出，在環境特征點為50個時，AUV的定位與制圖精度較高，且呈收斂趨勢，算法具有較好的穩定性。AUV在x軸向的定位誤差在沿x軸向運動時誤差較小，沿y軸向運動時誤差較大；在y軸向的定位誤差與x軸向的情況相反。

為檢驗算法的數據關聯作用，在環境特征點為50個時做2次圓周運動的定位與制圖精度，如圖5所示。

從圖6和圖7可以看出，AUV采用2次探測路徑運動時，定位誤差明顯降低，其主要原因是由于通過數據關聯對特征點的重復關聯匹配，使特征點位置更好地修正。在AUV的定位誤差較大時，對已探測過的區域進行重復探測，可以提高AUV的定位與制圖精度。

4 結 語

本文在建立AUV運動模型與聲吶模型的基礎上，研究了基于EKF的SLAM算法，并采用數據關聯方法優化了濾波算法，且提高算法定位精度。在環境特征點較多時，仿真驗證了算法在圓形路徑下的定位與制圖精度，結果表明算法的精度符合實際要求；對比相同環境特征密度下，采用2次探測路徑，結果表明該算法可有效提高AUV定位精度，從而驗證了該算法的有效性和可行性。實際應用中，外界的三維環境非常復雜，可能還包含許多動態目標，對此還需進一步深入研究。

[1]劉明雍, 趙濤, 周良榮.SLAM 算法在 AUV 中的應用進展[J].魚雷技術, 2010, 18(1): 41–48.

[2]張恒, 樊曉平.移動機器人同步定位與地圖構建過程中的軌跡規劃研究[J].機器人, 2006, 28(3): 285–290.

[3]FREDRIK O.Comparison of kalman filter estimation approaches for state space models with nonlinear measurements[C]//Proceedings of Scandinavian Conference on Simulation and Modeling, SIMS, 2005.

[4]曹軍, 曾碧, 何元烈.基于路標觀測的改進 EKF-SLAM 算法[J].自動化與信息工程, 2014, 35(1): 21–31.

[5]黃魯粵.基于慣性傳感器與聲納的水下機器人定位與地圖構建[D].青島: 中國海洋大學, 2010.

[6]秦永元, 張洪鉞, 汪叔華.卡爾曼濾波與組合導航原理[M].西安: 西北工業大學出版社, 2012.198–211.

[7]WEBB T, PRAZENICA R, KURDIAL A, et al.Vision based state estimation for autonomous micro air vehicles[J].Journal of Guidance, Control, and Dynamics, 2007, 30(3): 816–826.

[8]蔡自興, 賀漢根, 陳虹.未知環境中移動機器人導航控制理論與方法[M].北京: 科學出版社, 2009.

[9]王希彬.無人機視覺同時定位與地圖構建技術[D].煙臺: 海軍航空工程學院, 2012.

[10]王永清.同時定位與地圖創建中的數據關聯技術研究[D].青島: 中國海洋大學, 2007.

[11]張碧叢, 李琳.馬氏距離在移動機器人導航中的應用[J].機械設計與制造, 2012, 5(6): 59–63.