一種多傳感器更新的AUV-SLAM算法

李 超，徐東勛，袁昌斌

（中國海洋大學 信息科學與工程學院,山東 青島266100）

同步定位與地圖構建(SLAM)算法指自主式水下機器人[1-2](AUV)在未知水下環境中運動時，利用自身攜帶的各種傳感器識別周圍的環境特征，增量式地創建環境地圖，同時用該地圖實現自身位置的定位[3-4]。因此SLAM算法是水下機器人真正實現自主導航的關鍵。

水下機器人在實際的導航與定位中，只采用單個傳感器提供的信息容易受到本身特性及周圍環境的制約，使得部分測量信息不可靠，難以滿足水下機器人導航需要。針對單個傳感器感知周圍環境存在的這些缺陷[5]，采用多個傳感器可以獲得更可靠的量測，可以有效地補償單個傳感器帶來的誤差和不穩定性[6]。

通過AUV自身搭載的各種傳感器，可以獲得速度、加速度、角速度和角加速度。為了得到更優的估計，可以采用融合算法 (卡爾曼濾波)對不同傳感器提供的同一個量測進行融合。應用到SLAM算法中，即在未知的水下環境中，為了實現精確的機器人定位和環境地圖，以卡爾曼濾波理論和SLAM算法為核心，在僅聲納更新的SLAM算法的基礎上，提出航向和速度的更新，以此獲得更加精確的機器人位姿，并在此基礎上構建周圍環境地圖。

1 多傳感器更新的EKF-SLAM算法

1.1 算法流程圖

本文采用基于擴展卡爾曼濾波EKF(Extended Kalman Filter)的SLAM算法，其中機器人的狀態向量Xv=(xv，yv，φv，vx，vy)T[7]，其中前兩個分量為 x、y 方向的位置，φ為機器人的航向角，vx、vy分別為機器人 x、y方向的速度。多傳感器更新的SLAM算法的流程圖如圖1所示。

1.2 偽代碼描述

1.3 航向速度傳感器觀測更新

多傳感器更新的SLAM算法中引入了速度航向的更新，即從傳感器中獲得觀測值，結合EKF理論來更新航向和速度，得到的機器人位姿更為精確。航向和速度更新中，傳感器的觀測向量包含航向、橫向速度和縱向速度三個分量。其中，傳感器的觀測向量和觀測矩陣為：

其中，θ為傳感器羅盤提供的角度，vx、vy為傳感器 DVL采集的速度。

由 EKF公式，新息和新息協方差如式（3）和式（4）所示：

其中，Rd為高斯白噪聲，代表角速度和速度傳感器的觀測噪聲，具體形式如式（5）所示。

由EKF的更新方程可得卡爾曼增益、更新后狀態向量X和協方差矩陣P分別為：

2 湖試實驗和結果分析

本文試驗平臺是中國海洋大學UVL實驗室自主設計的“C-Ranger”號 AUV，如圖 2 所示。 “C-Ranger”采用開架式結構設計，由上下兩個電子艙和5個水下推進器組成，AUV 搭載有自主導航所需的 DVL、Gyro、HARS、GPS等多種傳感器。本次試驗是在中國海洋大學約60 m×35 m的湖中進行，為了使聲納能夠掃描到湖岸邊的環境特征，將聲納掃描距離設置為50 m，角度掃描范圍設置為 120°。

圖3和圖4分別為僅聲納更新的SLAM算法的軌跡和地圖以及多傳感器更新的SLAM算法的軌跡和地圖。其中，將由GPS數據得到的軌跡線作為一個基準，用于比較SLAM算法軌跡和推位算法軌跡的效果?！啊痢避壽E線表示推位軌跡，“+”軌跡線由 SLAM算法得到。圖中黑點是通過SLAM算法最終構建的全局環境地圖，圓圈為試驗中預設的10個特征點，從構建的全局環境地圖中可以明顯地看出湖岸的形狀，符合實際的試驗環境。

由圖3可以看出，隨著AUV的航行，推位軌跡出現了發散現象。僅聲納更新的SLAM算法的軌跡明顯改善了航位推算算法軌跡的發散現象，但在航程的最后階段SLAM算法的軌跡出現倒退，這是由于數據關聯時特征的錯誤關聯，使得機器人的軌跡偏離原先的方向。而由圖4可以看出，引入了多傳感器更新的SLAM算法的軌跡有效地改善了圖3中推位軌跡的發散現象，其軌跡最終也收斂于GPS軌跡。這是由于在聲納更新的同時引入了航向和速度更新，使得AUV的定位更精確，在此基礎上進行預測、觀測和更新，可以獲得最優的狀態估計，最終得到的SLAM軌跡更接近AUV的實際航行路徑。

由圖5和圖6可以明顯看出，多傳感器更新的SLAM算法的軌跡效果明顯優于僅聲納更新的SLAM算法軌跡，更接近AUV真實的航行路徑，并最終收斂于GPS軌跡。多傳感器更新的SLAM算法軌跡偏差也始終小于僅聲納更新的SLAM算法，且多傳感器更新的SLAM算法的機器人定位的最大偏差小于航程的1.3%，能夠滿足水下機器人導航的要求，構建的全局環境地圖與實際環境相符，驗證了多傳感器更新的SLAM算法的有效性。

本文主要在僅聲納更新的SLAM算法的基礎上，介紹了基于EKF的多傳感器更新的SLAM算法的流程圖和偽代碼。最后通過實驗結果從定位誤差和構圖精度上分析可以看到后者明顯優于前者，從而驗證了多傳感器更新的SLAM算法的有效性，可滿足水下機器人自主導航的要求。

[1]MILLER P A，FARRELL J A，ZHAO Y.Autonomous underwater vehicle navigation[J].IEEE Journal of Oceanic Engineering，2010，35(3)：663-678.

[2]BOVIO E，CECCHI D，BARALLI F.Autonomous underwater vehicles for scientific and naval operations[J].Annual Reviews in Control，2006(30)：117-130.

[3]LEONARD J，BENNETT A，SMITH C.Autonomous underwater vehicle navigation[C].MIT Marine Robotics Laboratory Technical Memorandum，1998：1-17.

[4]BAILEY T.Mobile robot localization and mapping in extensive outdoor environments[D].University of Sydney，Australian.Centre for Field Robotics，2002.

[5]DURRANT-WHYTE H.Uncertain geometry in robotics[J].IEEE Transactions on Robotics and Automation，1988(4)：23-31.

[6]譚民，王碩，曹志強.多機器人系統[M].北京：清華大學出版社，2005.

[7]周福娜，周梅，文成林.多傳感器數據融合效果分析[J].河南大學學報(自然科學版)，2003，33（2）：33-36.