室内移动机器人主动SLAM技术研究

刘光伟，王 巍，祁贤雨，李明博

（北京航空航天大学机械工程及自动化学院，北京 100083）

1 引言

同时定位与建图（Simultaneous Localization and Mapping，SLAM）指的是移动机器人在运动过程中根据实时获取的传感器数据进行位姿估计，同时增量式的构建环境地图。作为移动机器人实现自主导航的关键技术，SLAM自1986年在ICRA首次提出以来，经过近几十年的发展，已经有了诸多适用于特定场景的解决方案；但这些SLAM算法都需人为控制机器人运动，为进一步提升移动机器人的智能水平和探索能力，即实现机器人在无人为干预的情况下自动构建环境地图，文献[1]提出了主动SLAM。

主动SLAM涉及到移动机器人定位、建图、路径规划等技术的结合，要求机器人在保证位姿估计精度的前提下，向能探测到尽可能多未知区域的方向运动。为解决该问题，文献[2]提出通过最优控制法离散控制输入，根据各输入对机器人位姿估计精度和新增探索面积的影响，选择出最优的控制输入进行执行，但实际使用中该方法可能导致机器人陷入局部最优；文献[3]使用线段特征表述环境，并结合线段特征特点制定目标函数以评价系统的不确定度，并通过沿墙走来避免机器人陷入局部最优；文献[4]根据部分可观测马尔可夫决策过程模型从当前地图中选择目标点，但计算复杂度随问题规模呈指数级上升，一般只适用于求解小规模问题。

对室内环境中的定位与建图问题进行了深入的研究、分析，并结合室内场景特点，提出采用基于概率的方法进行机器人的位姿估计和地图构建。首先通过机器人运动模型进行位姿预测，然后根据激光雷达数据进行位姿更新，从而实现高精度的定位和建图；同时通过边界探索不断确定出下一步目标点、规划全局路径，并通过动态窗口法实现了机器人运动过程中的实时避障。

2 同时定位与建图

移动机器人同时定位与建图指的是在已知机器人控制量（u1：t）和传感器观测量（z1：t）的情况下，估计环境地图（m）及机器人位姿（x1：t），表示成数学形式即：p（x1：t，m|z1：t+1，u1：t）。

目前SLAM算法可分为概率和图优化两种，但是由于室内场景通常面积较小，且室内移动机器人自身的处理能力有限，所以选择使用概率方法，并将SLAM问题分解为对路径的后验概率（位姿估计）和已知路径情况下地图的后验概率（地图更新）两部分[5]，如式（1）所示：

此外，由于栅格地图具有易于构建和保存、便于路径规划等优点，所以采用栅格地图来表示环境信息。

2.1 位姿估计

位姿估计是SLAM技术的核心，为提高机器人位姿估计精度，首先通过实时获取的里程计数据进行位姿预测，然后根据激光雷达数据进行扫描匹配以实现位姿更新。

2.1.1 位姿预测

室内环境中通常使用X=（x，y，θ）表示机器人的全局位姿，假设已知机器人t时刻位姿Xt、里程计观测量ut以及t+1时刻里程计观测量ut+1，则可根据里程计模型：Xt+1=Xt⊕（ut+1⊖ut）[6]对机器人的位姿Xt+1进行预测。

2.1.2 位姿更新

轮子打滑、地面坑洼等因素都会导致里程计估计的位姿不准确，长时间运行会产生较大的累计误差。而通过使用激光雷达数据进行扫描匹配，可以显著提高位姿估计精度、减小累计误差，所以通过扫描匹配进行机器人位姿更新。位姿更新主要包括采集粒子和扫描匹配两部分内容。

（1）采集粒子

在里程计估计的位姿Xt+1周围（（0.3×0.3）m的矩形区域）进行采样，为保证位姿估计的精度，在采集粒子时应确保同一位置相邻粒子间的角度差在一定范围内，这里的准则是最远处激光点dmax在相邻两粒子间的偏移量不超过栅格分辨率r，由几何关系可确定出相邻粒子间的偏角δθ为：

根据栅格分辨率 r和窗口区域大小（Wx=Wy=0.3、Wθ=20°），可得采样范围为：

（2）扫描匹配

每个粒子都代表了机器人当前时刻的一个可能位姿，按粒子位姿对当前观测的激光点进行坐标变换，得到其在栅格地图坐标系下的坐标，然后根据已有地图得到各激光点为占有栅格的概率Pxy。

累加各激光点所对应的占有概率Pxy，可得当前位姿下所有激光点的概率和；但栅格地图的离散性会限制可以实现的精度，所以使用双线性插值来计算Pxy以提高精度。依次遍历所有粒子取得最大时的粒子即为机器人的后验位姿，也即机器人t+1时刻的最优估计位姿Xt+1。

2.2 地图更新

按照位姿Xt+1对当前观测到的激光点进行坐标转换，得到其在栅格地图坐标系下的坐标，从而实现地图更新。更新原则为：激光点对应的栅格为占有栅格，激光点与当前位姿Xt+1之间的栅格为空白栅格，若这些栅格在之前的地图中未探索过，则分别为其赋值0.6、0.4；若已经探索过，则按式（5）进行更新：

式中：P—该栅格前一时刻的概率；PN—更新后的概率；P0—占有栅格和空白栅格的初始概率，分别为0.6、0.4。

3 边界探索

通过边界提取不断从当前地图中提取边界[7]，并选择出最优边界的中点作为机器人下一步的目标点，同时规划出全局路径，从而不断驱动机器人向能探测到尽可能多未知区域的边界运动，具体实现分为以下几步：

（1）提取当前地图的边界，即当前栅格地图中与未知栅格相邻的空白栅格，两种栅格；为防止碰撞，需剔除长度小于机器人直径的边界，如图1所示。

图1 提取出的边界及各栅格的距离变换值Fig.1 Extracted Boundaries and Distance Transform

（2）提取出所有剩余边界的中点作为待定目标点，如图1中红色栅格。并以各目标点为中心，通过Flood-fill算法对已探索栅格进行填充，为各栅格计算距离变换值CostOT（i）。

（3）为避免发生碰撞，在计算距离变换值的同时，为各栅格计算障碍物变换值CostOT（i）。障碍物变换值按式（6）进行计算，各栅格CostOT（i），如图2所示。

式中：Ω（i）—空白栅格距离障碍物栅格的距离；X—常数，根据机器人半径确定。

（4）将各栅格距离变换值CostOT（i）和障碍物变换值CostOT（i）相加，可以得到各栅格的探索代价值，如图3所示。然后从机器人当前位置出发，沿探索代价值下降最快的方向进行搜索，最先到达的目标点即为下一步的目标点，该过程所经过的栅格即为机器人的全局路径。

图2 障碍物变换Fig.2 Obstacles Transform

图3 探索代价值及全局路径Fig.3 Exploration Transform and the Global Plan

（5）通过路径追踪算法驱动机器人沿规划出的全局路径运动，直至无法继续从已有地图中提取出有效边界。

4 实时避障

机器人的实际工作环境通常是动态的，在沿全局路径运动的过程中，为避免发生碰撞，需在全局路径规划的基础上引入局部路径规划，以实现实时避障。考虑到避障效果及实时性，选择动态窗口法（Dynamic Window Approach，DWA）[8]进行局部路径规划。动态窗口法主要包括以下几个步骤：

4.1 速度空间的确定

机器人在t+1的速度vt+1可以由机器人当前时刻的速度vt、角速度ωt确定，其范围为：

式中：v˙a、v˙b—机器人的最大减速度、最大加速度；ω˙a、ω˙b—机器人的最大角减速度和最大角加速度。

4.2 推测下一周期的轨迹

计算得到机器人的速度空间后，可根据运动模型对下一周期的轨迹进行预测，机器人在速度［v，ω］的作用下，下一周期内运动轨迹为一段弧线，弧线方向与当前速度方向相切，弧线半径为v/ω，机器人在多组［v，ω］下得到的轨迹，如图4所示。

图4 动态窗口法采样得到的轨迹Fig.4 Trajectories Obtained by DWA

4.3 评价函数

根据第一节所述的地图构建方法，以机器人当前位置为中心，建立一幅实时更新的（3×3）m的局部地图，并从方位角（heading（v，ω））、距障碍物最短距离（dist（v，ω））、速度（velocity（v，ω））三方面对各条轨迹进行评价。

4.3.1 方位角

方位角指的是机器人到达下一周期模拟轨迹末端时的朝向与目标点之间的角度差，heading（v，ω）越小，代价值越大。

4.3.2 距障碍物最短距离

距障碍物最短距离指的是机器人在模拟轨迹上与局部地图中最近的障碍物之间的距离，若该轨迹上没有障碍物，则将其设定为常数；dist（v，ω）越大，代价值越大。

4.3.3 速度

速度指的是各模拟轨迹所对应的速度，velocity（v，ω）越大，代价值越大。为了简化计算，算法假设机器人在模拟轨迹内的速度大小不变。

4.3.4 归一化

为综合考虑方位角、距障碍物距离、速度大小对局部路径规划的影响，在计算出上述三项内容后，需进行归一化处理，得到归一化系数。归一化计算公式为：

式中：n—采样得到的总轨迹数；i—待评价的当前轨迹。

将上述各项的计算结果带入评价函数式（9），可得各轨迹的代价值；选择代价值取得最大值时的速度作为机器人下一周期的速度，从而使得机器人及时避开障碍，以较快的速度向目标点运动。

式中：α、β、γ—计算得到的归一化系数。

5 实验

为验证算法，使用装配有里程计和激光雷达的Kobuki移动机器人平台进行了相关实验，其中激光雷达型号为RPLIDAR A2、扫描范围360°、测量半径8m，实验场地为两室一厅结构的家庭环境（12×12）m，如图5所示。

图5 实验场地及器材Fig.5 The Experiment Site and Equipment

移动机器人上电后，在无人为干预情况下，使用所提算法自动构建的室内环境地图，该地图为栅格地图，栅格分辨率为5cm，如图6所示。图6中一条曲线为机器人自动规划的全局路径，曲线端点即从当前地图中提取出的下一步目标点，实验过程中机器人沿规划的全局路径运动。当机器人前进路径上突然出现障碍物时（图6中另一条线），机器人通过DWA算法规划出局部路径（图6中一条线），及时调整速度方向，灵活的避开了障碍物。

图6 主动SLAM算法构建的栅格地图Fig.6 Grid Map Build by Active SLAM

为验证所提算法的精度，在同一实验场景中使用目前精度最高的开源激光SLAM算法Cartographer[9]（精度约5cm）进行建图实验，建图方式为人为遥控，地图构建结果，如图7所示。

图7 Cartogerpher构建的栅格地图Fig.7 Grid Map Build by Cartogerpher

通过对比两种方法所构建的地图，可以发现在小尺度的室内家庭场景下（＜150m2），两种方法的建图精度非常接近。

为验证算法效率，在外界因素不变的情况下进行了多次重复实验，并对每次实验的全程用时进行统计。统计发现，人为遥控方式的平均用时约为4.3mins，所提主动SLAM算法的平均用时约为6.8mins。

6 结论

通过对室内动态环境中同时定位和建图问题的研究及相关实验分析，可以得出如下结论：

（1）利用激光雷达高精度特性进行扫描匹配，可以显著提高机器人定位精度，从而构建高精度的环境地图。

（2）通过将激光SLAM与边界探索相结合，可以在保证机器人位姿估计精度的前提下，实现机器人自动、高效构建全局地图。

（3）局部路径规划可以使机器人灵活躲避环境中的随机障碍，实现动态环境中的同时定位和建图。