李 超，徐东勋，袁昌斌

（中国海洋大学 信息科学与工程学院,山东 青岛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所示。……