一种多传感器更新的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算法轨迹和推位算法轨迹的效果。“×”轨迹线表示推位轨迹，“+”轨迹线由 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.