基于激光信息的移动机器人SLAM研究

柳俊城，李 迪，翁潇文

（华南理工大学 机械与汽车工程学院，广州 510640）

随着移动机器人的发展，传统的导航方式如电磁导航、磁带导航和惯性导航等，存在着灵活性差，易受干扰等不足。在移动机器人自主导航研究方面，实现机器人自主导航和灵活避障的关键是，解决移动机器人运动过程中的同时定位与地图构建SLAM（simultaneous localization and mapping）。 SLAM 问题可以描述为：机器人在未知环境中从一个未知位置开始运动，在运动过程中依据位置估量和传感器数据进行自身定位，同时构建增量式地图[1-2]。目前，针对SLAM的实现过程，研究学者们根据不同的传感信息提出了多种算法来实现，如基于里程计及二维传感信息实现的算法有卡尔曼滤波、扩展卡尔曼滤波、粒子滤波等[3-4]，基于视觉等三维传感信息实现的算法有RGBD SLAM等[5]。

激光导航，作为新型的移动机器人导航方式，具有高度灵活性、测量速度快等特点，且具有较强有抗干扰能力[6-7]。目前，应用在移动机器人导航且能获得激光信息的传感器有2类，一类是激光雷达，这类通过激光测距原理能直接取得周围环境的距离信息。另一类是三维传感器如Kinect，双目视觉传感器等，此类传感器能获得空间的任意位置的深度信息，通过信息转化可以转成相应激光信息。在此，首先介绍激光雷达和Kinect的激光信息获取方法，进而提出一种基于激光信息的SLAM实施方法。

1 传感器信息提取

1.1 Kinect激光信息提取

微软Kinect 1.0视觉传感器可以同时获得彩色图像和深度图像，其水平视角为57°，垂直视角为43°，检测范围为 0.8～4.0 m[8]。 Kinect获得的深度信息包含摄像机视野内任意点的空间信息，所以利用Kinect获取的深度信息进行转换时，可以将垂直平面上相应空间的信息整合而成二维激光信息，将Kinect所获取的三维信息有效地融合转换。

首先，Kinect在环境中测量获取到深度信息，而后根据式（1）和（2）计算X，Y坐标轴上对应的像素深度（即Z轴）为

式中：i和j分别为Z阵列中像素的行和列的数值；w和h分别为Z阵列的宽度和高度；M为Kinect NUI相机的骨架倍增系数。

而每个空间点在X轴上投影的点到相机光心O的距离r（如图1所示）为

为了将深度图像中的三维数据压缩成二维的激光数据，可以取Z阵列中每一列最小元素作为该列的标识数据，表示在前方纵向上最近障碍物的距离。

图1 空间点到相机光心距离Fig.1 Optical center distance from spatial point to camera

由式（4）可知，矩阵Z′包含了Z轴上每次水平扫描中最近障碍物的距离信息，因此根据能更为完整的构造出地图中障碍物的距离信息，成功避免发生碰撞。

1.2 激光雷达信息提取

激光雷达的测距原理有2类。一类是运用TOF（time of flight）原理，将激光束平行发射出去，测量发射到接收到反射回来的激光的时间差来计算目标距离，从而得到环境地图的平面图。另一类是基于激光三角测距法原理，被测物体表面被一束特定入射角度的激光照射，激光在物体表面发生反射和散射，利用透镜在另一角度对反射激光汇聚成像，光斑成像在CCD位置传感器上。当位置传感器上的光斑发生移动时，说明被测物体沿激光方向发生移动，而被测物体的移动距离对应了光斑的位移大小。据此可通过光斑位移的距离，结合入射光角度和反射光角度构成的三角形，利用几何三角定理[9]原理设计算法计算出被测物体与基线的距离值。

文中采用的激光雷达是一款基于三角测距原理的扫描式传感器，每次测距会发射已调制好的红外激光，激光信号在被照射物上产生反光被红外感受器所接收。经过雷达内置的处理器实时解算出与被照射物与激光雷达之间的距离及夹角信息[10]。在雷达内置的电机驱动下，测距模块能实现360°全方位环境扫描测距检测。该激光雷达主要技术参数见表1。

2 移动机器人控制平台

2.1 机器人操作系统

激光导航移动机器人的控制系统使用基于Linux操作系统开发的机器人操作系统ROS（robotoperating system）。开源性是ROS的一个重要特点，可以为操作者提供硬件抽象层，使其软件能够方便地在多种平台进行移植[11]；能够为机器人操作程序提供通讯功能。ROS还自带一些工具软件和库函数，能更方便地服务与编写高兼容性的程序。ROS框架中包含节点、数据包、堆栈以及监测工具，是一种分布处理框架，每个节点可以单独被设计，而后可以封装到数据包或者堆栈中，监测工具则用来识别主题发布的内容[12]。在这样的框架下，每个执行程序都能被各自独立地设计，使用时可以实时组合实现响应的功能。

表1 激光雷达主要技术参数Tab.1 Main technical parameters of laser radar

ROS从图级分析可以分为4部分：节点、消息、主题、节点管理器。每一个节点都是一个数据处理点，可通过主题与其他节点通信；通信的内容即为消息，是一种自定义的ROS数据类型；节点管理器对每个节点进行查找及注册，方便开发人员查看并维护。

2.2 移动机器人平台控制

文中所使用的移动机器人为两轮差动驱动的机器人Powerbot，其能在最高达到100 kg的负载下能以正常速度1.6 m/s行驶，是一个理想的可用于研究运输、导航和操控等问题的实验平台。在机器人操作系统ROS中，节点rosaria为大多数移动机器人平台提供了一个ros接口，利用开源库Aria来控制电机、检测电池和获取编码器数据[13]。文中试验均在Powerbot机器人上完成，其节点控制如图2所示。

图2 ROS节点运行Fig.2 ROS node operation

3 SLAM问题的解决方案

3.1 二维SLAM模型

在运动过程中移动机器人位姿可以用一个三维向量（x，y，θ）表示。 假设，已经观测到的地图为mt，SLAM的观测模型如图3所示。图中，zt为t时刻传感器的观测量；ut为t时刻给机器人的控制信息。随着传感器的移动，附近的路标被感知到，依据输入信息从而估计出机器人运动路径x1∶t以及地图m。采用递归贝叶斯估计，可得

式中：η为归一化常量。由此可知t时刻后验证概率能够表示成t-1时刻的后验概率、感知模型和运动模型的迭代形式。

图3SLAM模型Fig.3 SLAM model

3.2 SLAM算法

根据式（5）给定的分布 p（zt∣xt，mt）和 p（xt，mt∣xt-1，mt-1，ut-1），可以得到 SLAM 系统中的 2 个模型，分别为机器人的感知模型和运动模型。前者表示在移动机器人地图mt与机器人位姿xt给定的条件下，传感器获取环境的不确定性；后者表示在上一时刻移动机器人轨迹xt-1和控制命令ut-1下，机器人新位姿xt的概率密度。使用Rao-Blackwellised粒子滤波器（RBPF），可以将贝叶斯滤波器状态空间分解为

通过RBPF方法将SLAM问题分解成2个独立的后验概率的乘积。可先通过对移动机器人移动的轨迹进行估计，然后再结合观测模型对地图进行更新。对轨迹的估计采用一个包含n个粒子的集合来表示系统状态的后验概率，每一个粒子的状态表示了机器人的一条可能运动轨迹。对每个粒子来说，它所表示的机器人位姿是完全确定的，因此能保证上式正确成立。

RBPF对使用里程计结合传感器观测信息构建增量式地图非常有效[14]，更新地图和机器人轨迹的采样有以下4步：

步骤1 采样。利用观测数据zt和里程计信息ut-1进行最优位姿估计，结果x1∶t-1对应粒子分布，根据建议分布函数q，获取机器人的位姿x1∶t组成下一时刻的粒子集。

步骤2 粒子权重。为弥补上一步采样分布与目标分布的误差，需要计算每一独立粒子的权重，用于判定粒子的有效性，由重要性采样公式得

步骤3 重采样。由于粒子是依照其权重比例来选取，并且使用了有限个粒子来近似连续分布。利用地图中随机分布多个粒子，每一个粒子都代表机器人的一种潜在的运动轨迹，机器人在运动过程中，粒子一直通过迭代收敛，保留权重高的粒子同时舍弃权重低的粒子，最终保留与机器人轨迹基本吻合的粒子集。重采样过程中，自适应阈值Neff为粒子群与机器人轨迹的吻合程度，其计算公式为

式中：ωi为粒子i的归一化权重。在筛选过程中为避免出现粒子枯竭的情况出现，当粒子个数＜N/2时，需要进行重采样步骤[15]。

步骤4 地图更新。对于每一个粒子来说，可以用其轨迹和观测信息来计算相应的位姿，从而对地图更新。

对于采样分布函数，结合观测数据和里程计数据，采样的结果要尽可能分布在观测数据的似然函数可行区域内，则对每个粒子建议分布为

而为了获得更高效的下一代采样粒子，可以将似然函数峰值附近的分布近似为高斯分布。这样，每个粒子的高斯分布参数由次采样和里程计数据可得

其中

为归一化因子。

3.3 SLAM算法流程

当每一个时刻有更新时，粒子的提议分布会重新计算并更新粒子。具体如下：①依据上一时刻的位姿xt-1和里程计信息ut-1对机器人当前初始位姿进行估计；②获得地图mt-1执行SLAM匹配算法；③根据式（12）求取采样建议分布④在建议分布函数中采样粒子；⑤计算并更新粒子权重；⑥重采样；⑦根据机器人位姿、观测信息及里程计信息计算新时刻地图，并更新。

4 试验结果

试验在Powerbot机器人上进行，其上搭载了Kinect、激光雷达、内置编码器和与这些传感器通信的处理中心PC。平台结构如图4所示。PC系统为Ubuntu 14.04，搭载了 ROS Indigo 操作系统。

图4 移动机器人试验平台Fig.4 Mobile robot experimental platform

在试验中，移动机器人Powerbot的运行速度设定为 0.3 m/s，加速度为 0.1 m/s2，最大角速度为 1.5 rad/s。将移动机器人放置于20 m×10 m的室内环境中，并分别利用Kinect和激光雷达对室内环境进行扫描获取激光信息。Kinect和激光雷达都在距离地面75 cm的高度上进行试验。同时，通过PC向底盘控制系统发送控制指令，从而控制移动机器人的运动。在试验过程中，通过ROS中的RVIZ三维可视化工具，可以查看机器人当前的位置和在试验区域内的地图创建过程。试验的场景、相应的激光扫描地图如图5所示。图中，黑色边缘点表示障碍物，浅灰色区域表示可通行区域。

图5 试验场景以及相应的激光扫描地图Fig.5 Experimental scene and corresponding laser scanning map

试验结果表明，2种传感器在室内环境下能准确对机器人位姿定位和地图构建。但是在实验室同一个环境下，使用Kinect构建的地图可以表达出实验室中央的生产线位置信息，而激光雷达仅能表现出生产线中支撑脚的信息。但激光雷达单次扫描范围大，可以快速构建出环境地图。这说明，使用Kinect可以弥补激光雷达无法采集高度信息的缺陷，能较好地提高地图的可靠性，使得移动机器人导航过程中更具有安全性。

5 结语

文中提出一种基于激光信息的移动机器人同时定位与地图构建算法，在使用时可以选择直接输出激光数据的激光雷达，或者使用三维传感器转换成二维激光信息，这2种传感器在室内环境下能准确对机器人位姿定位和地图构建。使用Kinect可以弥补二维传感器无高度信息的不足，能较好地提高地图的可靠性，使得移动机器人导航过程中更具有安全性。而激光雷达的有效感知范围大，能快速构建环境地图。通过试验验证了所提算法的高效性和创建地图的准确性。

[1] 满增光．基于激光雷达的室内AGV地图创建与定位方法研究[D]．南京：南京航空航天大学，2014．

[2] Birk A，Pfingsthorn M．Simultaneous localization and mapping（SLAM）[C]//American Cancer Society，2016．

[3] Santos J M，Portugal D，Rui P R．An evaluation of 2D SLAM techniques available in robot operating system[C]//IEEE International Symposium on Safety，Security，and Rescue Robotics．IEEE，2014：1-6．

[4] 刘丹，段建民，于宏啸．基于自适应渐消EKF的FastSLAM算法[J]．系统工程与电子技术，2016，38（3）：644-651．

[5] Henry P，Krainin M，Herbst E，et al．RGB-D mapping：using depth cameras for dense 3-D modeling of indoor environments[M]//Khatib O，Kumar V，Pappas G J．Experimental Robotics．Berlin Heidelberg：Springer，2014：647-663．

[6] 肖鹏，栾贻青，郭锐，等．变电站智能巡检机器人激光导航系统研究[J]．自动化与仪表，2012，27（5）：5-9．

[7] 吴越，蓝伟，许大浦，等．一种基于单线激光雷达的无人车辆导航避障系统设计[J]．计算机测量与控制，2013，21（7）：1902-1904．

[8] Ghani M F A，Sahari K S M，Kiong L C．Improvement of the 2D SLAM system using Kinect sensor for indoor mapping[C]//International Symposium on Soft Computing and Intelligent Systems．IEEE，2015：776-781．

[9] 李昀泽．基于激光雷达的室内机器人SLAM研究[D]．广州：华南理工大学，2016．

[10] 章志诚，杜昌平．基于激光雷达的多旋翼飞行器实时避障系统[J]．计算机测量与控制，2016，24（9）：117-121．

[11] Quigley M，Conley K，Gerkey B P，et al．ROS：an open-source robot operating system[C]//ICRA Workshop on Open ource Software.2009．

[12] 王权，胡越黎，张贺．基于ROS与Kinect的移动机器人同时定位与地图构建[J]．计算机应用研究，2017，34（10）：3184-3187．

[13] Fathzadeh R，Mokhtari V，Abdollahi F，et al．Mobile robot in domestic environment[C]//International Conference on Information Science，Electronics and ElectricalEngineering．IEEE，2014：918-922．

[14] 卜燕．基于ROS的移动机器人地图创建方法与应用研究[D]．绵阳：西南科技大学，2016．

[15] 仇恒坦，平雪良，高文研，等．构建移动机器人地图的多传感信息融合算法研究[J]．机械制造，2017，55（8）：1-4．■