摘  要： 针对同步定位与地图构建SLAM(Simultaneous Localization and Mapping)算法中，单个传感器提供的信息受到传感器本身特性和周围环境制约的局限性，提出了在仅声纳更新的SLAM算法同时，引入航向和速度的多传感器更新，使得自主式水下机器人AUV（Autonomous Underwater Vehicle）的定位和构图更精确。通过对实验结果的分析验证了多传感器更新的SLAM算法的有效性。
关键词： 同步定位与地图构建；多传感器；自主式水下机器人

    同步定位与地图构建(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 伪代码描述
/* SLAM 算法 */
gloabl X Px;    //全局状态变量X，P
[X, Px]=initialize(0);    //初始化
WHILE sensors_data DO
    [X, Px]=predict (Head, Vx, Vy, Wn, dt);
    //预测阶段，Vx,Vy从DVL传感器获得, Wn从GYRO传感器中获得, head从AHRS传感器中获得,dt是预测间隔
    [X, Px]=update(Head, Vx,Vy);//更新阶段
    IF sonar_data THEN
    DO
    [Zf, Zn]=data_association(Z,GATE1,GATE2);
    //数据关联阶段，GATE1是进行数据关联的最大距离, GATE2 是建立新地图特征的最小距离
        IF ~empty(Zf) THEN
    DO [X, P]=update_sonar(Zf);
    //Zf是关联上的特征，即地图中已有特征，用于更新
        ELSE ~empty(Zn) THEN
        [X, Px]=augment_sonar(Zn);
    //Zn是未关联上的特征，即地图中没有的特征，用于扩充
        END IF
    END IF
END WHILE
1.3 航向速度传感器观测更新
    多传感器更新的SLAM算法中引入了速度航向的更新，即从传感器中获得观测值，结合EKF理论来更新航向和速度，得到的机器人位姿更为精确。航向和速度更新中，传感器的观测向量包含航向、横向速度和纵向速度三个分量。其中，传感器的观测向量和观测矩阵为：

轨迹线表示推位轨迹，“+”轨迹线由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.