基于激光雷达与视觉信息融合的SLAM方法

2019-12-11王光庭

山东理工大学学报（自然科学版） 2019年1期

王光庭，曹凯，刘豪

(山东理工大学交通与车辆工程学院，山东淄博 255049)

同时定位和地图构建(SLAM)技术是机器人在未知的环境中完成跟踪、侦查、导航等任务的关键和基础[1]。在实际的应用中，机器人在不同的环境中执行不同的任务，对传感器的选择也是不尽相同。对于家庭服务型移动机器人选用单一的传感器会存在检测地图信息不全面、检测精度不够或是机器人定位误差太大等诸多问题[2-3]。如果采用多线激光雷达能够解决此问题，但会大幅度提高成本，市面上，4线或8线激光雷达的价格就将近10万元，而单线激光雷达只有几千元。为了降低成本，通过多传感器的融合技术能够使各廉价传感器测量的信息实现互补，能够极大地提高机器人SLAM系统的鲁棒性及自身定位与地图构建的精度。本文提出一种融合单线激光雷达信息和视觉信息的方法，实现两种冗余信息的互补，以提高地图的精度，保证地图信息的完整性和移动机器人导航的实时性。

1 传感器性能及功能

Kinect是微软推出的一款RGB-D视觉传感器，如图1所示。 Kinect有三个摄像头，中间的镜头是彩色摄像机，用来采集彩色的信息(RGB)，左右两边的摄像头分别是红外线发射器和红外线CMOS摄像机所构成的3D结构光深度感应器，用来采集深度信息(Depth)，也就是环境中物体到摄像头的距离信息[4]。Kinect的水平视角为57.5°，垂直视角为43.5°，能够感知的距离为0.8～3.5 m。RPLIDAR A2 是由SLAMTEC公司开发的新一代低成本二维激光雷达，如图2所示，它可以实现在二维平面的8 m半径范围内进行360°激光测距扫描，激光测距采样频率为每秒4 000次。两者参数对比见表1。

图1 Kinect视觉传感器Fig.1 Kinect vision sensor

图2 RPLIDAR A2激光雷达Fig.2 RPLIDAR A2 sensor

表1 传感器参数对比
Tab.1 The comparison of sensor parameters

参数水平视角/(°)垂直视角/(°)感知范围/mKinectRplidar57.5360 43.5 00.8～3.50.15～8

Kinect视觉传感器和RPLIDAR A2激光雷达测量的范围如图3所示，从图中可以看出，单线激光雷达测量的是二维平面信息，而Kinect视觉传感器测量的是三维的空间信息，很明显，视觉传感器所感知的空间信息要比单线激光雷达的丰富，而单线激光雷达所测量的范围比视觉传感器的大。本文的研究意义在于保证家庭服务型移动机器人低成本的基础上，结合视觉传感器Kinect和单线激光雷达RPLIDAR A2各自的优点，形成更宽阔的测量范围。

图3 传感器的感知范围Fig.3 Sensing range of the sensor

2 粒子滤波理论

粒子滤波[5-6](Particle Filter)理论思想是基于蒙特卡洛(Monte Carlo)方法，是利用粒子集来表示概率密度，通过从后验概率(观测模型)中抽取随机状态分布的粒子来表达分布，可以使用在任何形式的状态空间模型上。Rao-Blackwellized 粒子滤波理论是对传统粒子滤波的改进，它的核心思想是对机器人的路径姿态和地图进行分解。

Rao-Blackwellized粒子滤波算法实现的具体步骤如下：

1)状态初始化

(1)

2)初采样

根据粒子分布规则采集初始样本，从当前的粒子集中产生新的下一状态的粒子集。那么，此时机器人运动模型的概率密度函数可表示为

q=p(st|ut,st-1)

(2)

3)计算粒子的权值

粒子权值计算的递推公式如下

ωt-1p(zt/st,zt-1)

(3)

式中：zt-1为t-1时刻的观测值；zt为t时刻的观测值；st，st-1分别为t时刻和t-1时刻机器人的位姿。

4)重采样

在理论上，所有粒子都存在退化问题，为了避免这种由粒子退化问题导致的权值“两极分化”现象，需要进行重采样。设立一个衡量是否需要对粒子进行重采样的变量，并且设定一个阈值，当变量的值低于给定阈值时，说明粒子退化严重，需要进行重采样[7-8]。

5)状态参数估计

以激光雷达获取到的数据作为依据，用机器人当前的位姿估计对下一步的粒子滤波进行采样，高斯滤波的估计参数如下

(4)

(5)

归一化参数为

(6)

设机器人与路标的距离为r，机器人与路标的夹角为α，机器人自身的位姿为st，机器观测到的路标为θmt，则观测方程与其雅克比矩阵如下

(7)

(8)

式中的参数q如下

q=(θmt,y-st,y)2+(θmt,x-st,x)2

(9)

3 激光雷达和视觉信息融合

本文的实验是在Turtlebot2机器人平台上完成的，机器人配置有微软XBox360 Kinect视觉传感器、RPLIDAR A2激光雷达以及处理这些设备之间的通信和数据的联想酷睿i7 2.4GHz处理器。PC端搭载的是在Linux(ubuntu14.04)系统下运行的ROS(Robot Operating System)Indigo操作系统[9]。搭建的融合单线激光雷达和视觉传感器Kinect平台如图4所示。

图4 移动机器人操作平台Fig.4 Mobile robot operating platform

3.1 基于单线激光雷达的地图构建

单线激光雷达是通过扫描二维平面得到的数据来进行地图构建的，当遇到图5所示的低于激光雷达扫描平面的障碍物，无法进行扫描，导致构建的地图信息不够全面。在实际的应用中，这些地图中缺失的环境信息，会对机器人的自身定位、导航以及路径规划等造成严重的后果，只用单线激光雷达构建的栅格地图如图6所示，该地图是用ROS系统中的Gmapping包来构建的，该包中的定位算法是由改进粒子滤波Rao-Blackwellized算法来实现的。从地图中可以看出，没有低于单线激光雷达扫描平面的障碍物2的信息。

图5 激光雷达扫描缺陷Fig.5 Defects of lidar scanning

图6 RPLIDARA2的地图构建Fig.6 Mapping of RPLIDAR A2

3.2 基于Kinect的点云提取

Kinect视觉传感器获取到的是三维的环境信息，然后提取三维点云，设置阈值，取一定高度范围内的点云，滤除全部外点，保证计算的简洁性。本文使用的是ROS-Indigo系统中的Pointcloud-to-laserscan包，需要将三维点云投影到二维的平面上，形成二维的离散障碍图。

Kinect获取的三维点云信息如图7所示，从图中可以看出，三维信息对机器人运动环境的描述比单线激光雷达的二维信息要丰富许多。

图7 Kinect获取的点云Fig.7 The point cloud obtained by Kinect

3.3 信息融合的过程

单线激光雷达获取的激光数据和Kinect获取的视觉信息数据的融合过程流程如图8所示。

在地图的更新阶段，采用Bayes估计方法。Bayes估计是基于Bayes定理的条件或后验概率的统计数据融合算法，它适用于具有可加高斯噪声的不确定性信息，可以通过已知的向量Z，估计出未知状态下n维向量X。假定完成既定任务所需的有关的环境信息向量为f，通过传感器获得的数据信息用向量d表示，则数据融合的任务就是要通过数据向量d推导来估计环境向量f，假设p(f,d)是随机向量f和d的联合概率分布密度函数，则有Bayes公式如下

p(f,d)=p(f|d)·P(d)=

p(f|d)·p(f)

(10)

式中：p(d)、p(f)分别表示d和f的边缘分布密度函数。

本实验传感器是从不同坐标系统获取数据，以间接的方式采用Bayes估计进行数据融合，为保证数据信息的一致性，在融合之前，需要对数据一致性进行检验[10]，检验公式如下

(11)

图8 信息融合流程图Fig.8 The flow chart of information fusion

4 实验

4.1 建图

本实验中单线激光雷达的离地高度是32 cm，而设置的障碍物的高度是26 cm，障碍物没有在单线激光雷达的扫描平面内，在使用单一的传感器RPLIDAR A2进行建图的过程中，根本无法识别这样的障碍物。具体的场景如图9所示。

图9 RPLIDAR A2与障碍物的位置Fig.9 Position of RPLIDAR A2 and obstacles

利用单一的激光雷达传感器RPLIDAR A2来做SLAM，建图的结果如图10所示，扫描建立的二维栅格地图信息不够全面，图9中障碍物信息没能在栅格地图中出现。而将Kinect获取到的视觉信息加入以后，会发现图9中障碍物信息扫描出现在建立的栅格地图中。

图10中，(a)是假设的场景图，(b)是使用单一的激光雷达传感器RPLIDAR A2扫描建立的二维栅格地图，(c)是融合Kinect视觉信息建立的二维栅格地图。从图中可以看出，扫描的范围基本没有变化，保持了单线激光雷达扫描范围大的特点，而且地图(b)中没有扫描到的障碍物信息在地图(c)中清晰地显示出来。

(a)场景图

(b)激光信息栅格地图 (c)融合视觉信息栅格地图图10 建图过程Fig.10 Mapping

4.2 导航

图11 障碍物场景Fig.11 The scene of obstacle

(a)初始规划路径 (b)重新规划路径

(c)避开障碍物 (d)导航完成图12 导航过程Fig.12 Navigation

在已有的地图中进行导航，使用图10中的栅格地图(b)做先验地图，此时，先验地图中无设定的障碍物信息。利用先验地图，实验场景如图11所示，进行导航的过程如图12所示。图12中，(a)中规划的路径是直接穿过设定的障碍物，那是因为Kinect的感知距离比较小，障碍物没在Kinect的感知范围之内；(b)中，随着移动平台Turtlebot2接近障碍物，障碍物进入Kinect的感知范围，检测到障碍物的信息以后，对路径进行了动态规划；(c)图中，移动平台Turtlebot2成功避开障碍物；(d)图中，障碍物的信息成功出现在代价地图里面。