人工路标辅助的室内移动机器人SLAM

冉 腾，袁 亮，吴金强，罗 高

（新疆大学机械工程学院，新疆 乌鲁木齐 830047）

1 引言

SLAM（simultaneous localization and mapping）全称同时定位与建图，这一概念于1986 年提出，各国学者一致将基于概率估计的定位与环境地图创建定为移动机器人的基本问题[1]。精准的定位与地图构建是移动机器人高效且安全作业的前提，因此使机器人对环境进行更准确的感知并且提高SLAM 精度是十分重要的。

起初基于Rao-Blackwellized 的粒子滤波算法被用于解决SLAM 问题[2-3]，但是构建精确地图需要大量的粒子，并且重采样过程极易发生粒子耗尽的问题。文献[4]提出改进的RBPF 方法将观测信息与里程计信息同时用于机器人轨迹预测，并减少重采样次数降低粒子耗尽的风险，算法在一定粒子数的前提下能得到较理想的SLAM 结果。里程计数据通常可通过编码器获得，而编码器自身精度受环境影响较大，并且随着机器人移动范围的扩大里程计数据变的不可靠，因此激光雷达等设备成为大规模环境下精确建图的主要传感器[5]，但由于其测距范围有限导致位置估计仍存在较大误差，因此在大规模的室内外场景中利用环境路标辅助定位的方法被广泛应用。文献[6]在路标组定位的基础上提出多元M 序列的人工路标设置方法，简化了路标种类优化出的方案表现出较好的性能。文献[7]提出一种几何路标关系法，将二维激光雷达扫描数据转化为位姿不变直方图，该方法在室内外场景中的环境识别与匹配均达到出较好的效果。文献[8]在室外大规模场景中，利用视觉和雷达传感器将检测到的物体作为自然路标并创建稀疏的路标地图用于无人车的定位与导航文献[9]利用激光雷达结合提出的改进相交圆的人工路标方法对托卡马克转运车进行定位，并验证了该定位方法具有较高的精度。文献[10]针对不携带编码器的大型移动平台，提出利用激光雷达提取环境路标外形特征，通过扩展卡尔曼滤波算法对移动平台进行运动估计和位姿校正，实验结果验证了定位系统的精度和可靠性。

大规模场景中的SLAM 依赖高精度大量程的激光雷达，当移动机器人配备低成本激光雷达时定位精度受到极大影响，因此使用环境路标辅助机器人进行实时定位能够较好估计机器人位姿提高SLAM 精度。传统的融合路标信息进行移动机器人定位仍然存在许多不足，针对路标设置过于复杂以及有关室内基于人工路标SLAM 的研究尚少，提出利用差速移动机器人在人工路标辅助下进行有效的室内大范围场景激光雷达SLAM。通过实验验证，提出的方法具有较好的效果和实用性，有效提高移动机器人SLAM 的精度。

2 差速底盘运动模型

图1 机器人运动模型示意图Fig.1 Schematic Diagram of Robot Motion Model

实验平台采用差速驱动底盘，机器人左右电机分别由两个驱动器控制，底盘运动模型，如图1 所示。机器人移动的过程中依靠左右两侧编码器进获取里程计数据，不考虑误差的情况下通过实时采集编码器数据并对其进行解算能够得到机器人每一时刻的速度和位姿，如图1 所示。L为底盘轴距，R为运动时转弯半径，假设机器人（t-1）时刻位姿为（Xt-1，Yt-1，θt-1），采样时间间隔为dt，采样时刻左轮速度为Vl，右轮速度为Vr，线速度、角速度分别为Vt和Wt，采样时间间隔内的坐标变换和偏航角分别为dx，dy，dθ，则有：

根据已知左右轮速度即可得到式（1）中机器人线速度、角速度以及转弯半径，同时依据采样时间可得式（2）中的航向角变化值和式（3）中的坐标位置，即确定机器人实时的估计位姿。

3 基于路标的激光雷达观测模型

二维激光雷达安装在机器人底盘正上方，根据三角测距原理可以得到障碍物相对于激光雷达的距离及角度。若以机器人初始位置为坐标系原点，以路标为固定参考物则机器人移动过程中相对于参考物的位置始终已知，如图2 所示。机器人0 时刻处于初始位置O1，下一时刻t处于位置O2，始终以右侧路标角点O为参考则可以得到机器人移动过程中相对初始位置的任意位置坐标，即用激光雷达进行机器人定位和轨迹的确定。

图2 基于路标的激光雷达观测模型Fig.2 Lidar Observation Model Based on Landmarks

4 基于Rao-Blackwellized 的粒子滤波SLAM算法原理

Rao-Blackwellized 思想是将同时定位与建图问题转化为已知位姿求解地图的形式，因此对联合后验概率函数因式分解。

式中：x—机器人轨迹；m—观测的地图；z—观测信息；u—里程计信息，地图的后验概率p（m|x1:t，z1:t）将按照已知机器人位姿建图进行处理[2]，机器人轨迹的后验概率p（x1:t，|z1:t，u1:t-1）可用粒子滤波方法计算，每个粒子都代表一条可能的机器人轨迹并关联一张地图，通过赋予粒子权重最终得到估计机器人运动轨迹。重要性重采样（sampling importance resampling，SIR）滤波是常用的一种粒子滤波方法，通常包括四个部分：

（1）采样粒子：根据提议分布进行粒子采样，新一代的粒子集得到，并且通常将里程计运动模型作为提议分布。（2）重要性权值：根据观测值计算每个粒子的重要性权值，使得提议分布尽可能与真实目标分布一致。（3）重采样：根据权重结果重新分布粒子。（4）地图估计：对于每个粒子，根据其采样轨迹和观测数据进行地图估计。里程计运动模型作为重要性概率密度函数计算简单，但是由于其本身误差将使估计分布偏离真实目标分布，因此文献[4]将里程计信息与观测信息融合并入重要性概率密度函数，使粒子分布估计更接近真实分布。基于文献[4]研究成果，移动机器人在配备低成本雷达进行大规模环境下同时定位与建图时，融合人工路标即可创建更精确的地图。

5 融合人工路标信息的移动机器人SLAM

大规模环境下进行地图构建时里程计信息变的不可靠，因此机器人需要依赖高精度大范围传感器，而配备低成本雷达时，机器人可以利用环境中人工路标辅助定位并修正里程计信息。由激光雷达观测模型可以知道，机器人通过激光雷达确定某一路标的相对位置，然后以当前路标为参考物进行定位，如图3 所示。机器人在初始位置O1确定1#路标的外侧边AB，通过A和B的坐标得到该边中点坐标，以该坐标为参考进行同时定位与建图，移动到O2点时同时观测到1#路标和2#路标，在确定AB中点的基础上可以确定CD中点坐标，进而在移动过程中不断获取新的路标进行定位修正里程计信息，构建与真实环境更接近的地图。

图3 融合路标信息的辅助定位Fig.3 Auxiliary Positioning of Integrated Landmark Information

实验室部分走廊真实场景，如图4（a）所示。该段走廊平面图，如图4（b）所示。移动机器人配备低成本雷达利用RBPF-SLAM 算法构建的二维栅格地图，如图4（c）所示。在走廊中设置人工路标后移动机器人构建的栅格地图，如图4（d）所示。可以发现未设置人工路标时，栅格地图的纵向长度明显小于实际尺寸，而在人工路标辅助移动机器人定位的情况下，创建的地图尺寸基本接近实际尺寸。

图4 设置人工路标前后SLAM 效果对比Fig.4 Comparison of SLAM Performance before and after Setting up Artificial Landmarks

6 实验结果与分析

6.1 实验平台

图5 硬件平台Fig.5 Hardware Platform

实验平台硬件，如图5 所示。小车为两轮差速驱动。实验采用的激光雷达为RPlidar A2，接收驱动指令后可以对环境轮廓进行360°扫描并返回检测到的路标数据，最大测量范围为（6～8）m，频率10Hz。根据激光雷达的工作特性，实验中选用的人工路标为（300*200*500）mm 长方体纸箱，无需考虑颜色等特性。箱体的平面及其不透明材质便于2D 激光雷达快速检测到直线特征，路标按照一定间距在走廊一侧进行布置。

软件部分基于ROS 平台进行开发，结构如图6 所示。移动机器人底盘控制指令通过ROS 平台发布，控制模块经串口接收指令进行PID 计算并驱动电机工作，同时接收里程计数据进行解算发送至上位机。ROS 中融合里程计数据与激光雷达扫描数据进行SLAM，同时发布路标估计位置与机器人运动轨迹。

图6 软件结构Fig.6 Software Structure

6.2 实验结果

移动机器人在图4（a）所示的走廊中行走一圈（176m）进行SLAM 实验，记录移动机器人进行同时定位与建图时的估计运动轨迹，并最终生成完整室内走廊地图，在有无路标的实验中各选取一幅地图，无路标参考的环境地图，如图7 所示。设置路标创建的地图，如图8 所示。可以看出图7 下半部分有倾斜趋势，右侧闭环部分出现明显偏移，图8 走廊整体垂直度较好，并且右侧闭环效果理想。依上述实验过程按设置人工路标和无人工路标两种情况在路标间隔分别为5m、7m 和10m 进行SLAM 实验，共得到六组轨迹长度，分别与实际走廊长度176m 进行比较，轨迹估计精度提高6%左右，结果统计，如表1 所示。运动轨迹，如图9 所示。在ROS 中获取机器人发布的轨迹信息并将相关数据导入Matlab，将实验结果进行均值化处理，得到设置路标前后的平均轨迹并进行显示。可以看出基于人工路标参考的移动机器人运动轨迹估计能较好的贴合真实走廊路径，无路标的轨迹则偏离较大，甚至无法实现闭环。

图7 设置路标前整体走廊地图Fig.7 The Overall Corridor Map Before Setting the Landmarks

图8 设置路标后整体走廊地图Fig.8 The Overall Corridor Map After Setting the Landmarks

表1 设置路标前后及路标间距改变时的SLAM 轨迹长度比较（单位：m）Tab.1 Comparison of the SLAM Track Length Before and after the Setting of Landmakes and When the Distance Have Changed Between Landmarks

图9 移动机器人估计运动轨迹Fig.9 Mobile Robot Estimating Motion Track

7 结论

实验证明，基于人工路标的室内移动机器人SLAM 方法能够有效提高室内地图创建的精度，移动机器人在移动过程中根据路标进行准确的位姿估计使得估计运动轨迹更接近真实运动轨迹，其中人工路标间隔在（5～10）m 范围时，移动机器人在室内走廊行进轨迹的精度平均提高6%左右，解决了室内移动机器人配备低成本雷达时由于测距有限带来的位姿估计偏差过大的问题。未来将在SLAM 的基础上，研究基于已知地图的移动机器人非结构环境下自主导航与实时避障。