基于多源信息的移动机器人建图方法研究

夏育泓，邓三鹏，张志军，祁宇明，吕松杰

（1.天津职业技术师范大学 机器人及智能装备研究院，天津 300222；2.天津市智能机器人技术及应用企业重点实验室，天津 300350；3.天津博诺智创机器人技术有限公司，天津 300350）

0 引言

同时定位与建图技术是当前移动机器人领域的研究重点。SLAM 技术是指利用移动机器人本身所带有的传感器来探测周围环境信号，进而形成当前区域的局部地图，然后再利用局部地图形成全局地图的技术。

目前，国内外研究人员已经开源了许多单一传感器的SLAM 方法，例如基于相机的视觉SLAM 算法ORB-SLAM[1]、RGBD-SLAM[2]等，基 于 激 光 雷 达 的Gmapping[3]、Cartographer[4]等。但是这些SLAM 算法使用单一传感器，所以会由于传感器本身的缺陷导致算法存在不足。GPS 可以在室外获得位置信息却无法获得室内的位置信息；二维激光雷达，虽然能获得较高的位置精度，但只能扫描雷达所在平面的二维信息；RGB-D 相机能获得外界环境的颜色和深度信息，但是精度较差且会受到光照的影响；编码器来估计机器人位姿但实际中会由于移动机器人打滑导致出现定位误差。IMU 利用加速度计和陀螺仪得到机器人的角加速度和加速度，从而计算出位移和角度信息，但是在运行的过程中随着温度的升高数据漂移会不断增加。因此研究人员根据各种传感器的优点进行信息的融合来提高SLAM 效果已经成为常态。王锦凯等[5]通过扩展卡尔曼滤波器的原理结合了激光雷达数据与RGB-D 相机数据，当相机与地图匹配错误时可以通过把激光雷达的数据与相机数据加以互补，而得到地图。李全峰等[6]将视觉里程计、激光里程计采用Kalman滤波算法进行融合，将融合后的数据与轮式里程计使用动态二次加权的方式融合，提高了里程计的精度。

为了创建的地图能显示更多的信息并提高移动机器人里程计的精度，提出将激光雷达数据和RGBD 相机数据融合提高对环境信息的感知，将里程计，IMU，激光雷达的信息进行融合提高里程计的精度，并结合Gmapping 建图算法，使得建图精度得到一定程度提升，最后通过仿真验证了方法的可行性。

1 基本原理

要进行数据融合，首先需要将数据转换到同一坐标系，再进行滤波处理并融合，最后将激光雷达和相机融合的数据转换成的激光雷达数据。

1.1 联合标定

数据融合需要使激光雷达数据和相机数据处在同一个坐标系下，采用文献[7]的方法对相机和激光雷达进行联合标定。由于相机和雷达的位置在移动机器人处相对位置一般都是固定的，直接使用ROS 中的static_transform_publisher 对雷达和相机进行静态坐标变换。

1.2 数据融合

采用点云融合的方式进行数据融合，经联合标定、坐标变换等处理使激光雷达数据和相机数据处于同一坐标系下，对相机数据进行滤波处理，最后将激光雷达数据和相机数据融合在一起，具体步骤如下：

（1）使用深度相机的原始点云会因为数据过于庞大导致处理数据的时间过长，从而使得极坐标的发布频率过低，最终出现误差较大的地图。所以通过体素滤波对深度相机点云进行降采样处理。

（2）去除地面和过高的点云：使用索引滤波器，过滤地面点云；使用直通滤波器，在Z 轴上设置高度阈值，将高于阈值的点云过滤。

（3）激光雷达数据截取：激光雷达扫描范围为360毅，本次选取小车前进方向为中轴线，左右截取90毅，所以当前扫描范围为前进方向正向的180毅。

（4）数据融合：通过PCL 中的指针变量将雷达和相机点云进行拼接。

（5）使用pointcloud2_to_laserscan 将融合后生成的点云转化为激光雷达数据。只有激光雷达的效果如图1 中（a）所示，融合后的效果如图1 中（b）所示。

图1 点云效果

2 多传感器里程计融合

对激光里程计数据、IMU 数据和轮式里程计数据进行融合。常见的激光里程计的基本原理是采用点-点匹配的ICP 及其变种算法，该算法采用扫描激光雷达上的相邻关键帧的方法实现定位，不过由于激光的高离散性，可能导致匹配失败。采用基于距离流的RF2O 来获取激光里程计信息，该方法相对于ICP 等点云匹配算法受到环境的影响较小，在平移信息的测量精度都高于ICP 及其变种算法。轮式里程计使用电机的光电编码器获得机器人的位移，同时结合左右轮的移动激励差来进一步计算位姿变化。IMU 的姿态结算常使用的是方向余弦算法。扩展卡尔曼滤波是卡尔曼滤波在非线性情况下的滤波方法，而DS 证据理论在对测量数据的信息处理和不确定的问题方面有良好的效果。所以采用扩展卡尔曼滤波结合DS 证据理论方式融合IMU，轮式里程计，激光雷达里程计数据。

2.1 扩展卡尔曼滤波系统模型及观测方程

移动机器人状态方程：

其中：Xk为移动机器人位置，f为机器人的非线性系统模型，则f（Xk-1）为在k-1 时刻的状态值，uk-1表示k-1 时刻机器人控制状态。wk-1是动态噪声，为高斯白噪声。其协方差矩阵：

其中：Qk为过程噪声的协方差矩阵。

将状态矩阵进行离散化处理，并进行非线性分析，再以吟t对系统模型离散化，结果如下：

其中：xk为机器人x方向上k时刻的位置，yk为机器人y方向上k时刻的位置，渍k为机器人k时刻的偏航角，vk为机器人k时刻的线速度，wk为机器人k时刻的角速度。

由于是非线性系统，对函数f线性化处理，得到雅可比矩阵：

状态转移函数使用一阶泰勒展开，将得到的雅可比矩阵Fk带入系统方程，形成的系统状态方程：

根据卡尔曼滤波基本方程，滤波器的预测方程：

2.2 扩展卡尔曼滤波器更新过程

更新k时刻卡尔曼增益矩阵Kk：

更新K时刻的后验估计值：

更新k时刻后验估计协方差：

2.3 DS 证据理论在权值分配上的应用

DS 证据理论是一种不确定性推理方法，能够在没有先验概率的前提下，对不确定性建模。借鉴文献[8]的做法，利用DS 证据理论，在权值分配上对传感器的测量信息进行融合处理。

将多种传感器测量的信息Zi与经过滤波器的信息作为接收到的信息证据。

将证据按照DS 证据理论的计算方式加以处理，可以得到各个时刻传感器信息所占据的权重比例：

其中k表示时刻，i表示传感器的个数。

将信息进行数据融合得到了新的信息序列：

T表示转置。再对新的序列经过滤波处理，获得更为理想的滤波效果。

2.4 多传感器融合里程计流程

先利用轮式里程计的数据和IMU 姿态数据利用DS 证据理论在权值分配上的算法处理融合接收到的测量数据，改善移动机器人的转向误差，再利用融合后的里程计数据作为预测量，由激光里程计RF2O 得到的数据作为观测量，经过扩展卡尔曼滤波方式融合，改善移动机器人的位置精度。融合算法流程如图2 所示。

图2 融合算法流程

3 实验测试与分析

3.1 移动机器人仿真模型和仿真环境搭建

为了验证移动机器人SLAM 建图效果，采用ROS系统下的Gazebo 与Rviz 平台进行仿真实验。机器人操作系统（robot operating system，ROS）是免费且开放的框架，包含多种工具、库，并且支持C++Python 等语言，为机器人的开发人员提供了很多的便利。通过使用ROS 中的Gazebo（3D 动态模拟器），能够设置需要的模拟环境，以及搭建携带包括激光雷达、IMU、相机、里程计等多种传感器的机器人仿真模型。通过使用Rviz（三维可视化工具），可以实现对环境信息和传感器信息等外部信息用图形化的方式显示，并且能够发布信息，达到检测机器人数据以及控制机器人运动的效果。为验证算法在障碍物无法被激光雷达扫描时的效果，设置了两处低于扫描平面的障碍物。最终的仿真环境如图3 所示。

图3 仿真环境

3.2 实验结果与分析

在仿真环境中分别进行了多次激光雷达建图和激光雷达和相机融合建图，建图的效果如图4 所示。

图4 建图效果

为了检验多传感器融合里程计的效果，将融合里程计所获得的轨迹数据，原始里程计所获得的轨迹数据与真实轨迹数据进行比较，计算均方根误差并进行误差分析。其中一次实验的轮式里程计轨迹和实际轨迹路线图对比如图5 所示，融合里程计轨迹和实际轨迹路线图对比如图6 所示。一部分实验的融合里程计和轮式里程计与实际轨迹对比的均方根误差数据如表1 所示。

图5 轮式里程计轨迹和实际轨迹路线图对比

图6 融合里程计轨迹和实际轨迹路线对比

表1 融合里程计和轮式里程计与真实轨迹对比的均方根误差

从图5，图6 可以看出，融合激光里程计、轮式里程计、IMU 数据的里程计比只使用轮式里程计对移动机器人的位姿估计效果有提升，尤其是在移动机器人发生转向时，融合里程计的轨迹比轮式里程计轨迹更加接近真实轨迹。经过多次实验，融合里程计轨迹与真实轨迹对比，均方根误差为0.0875，而轮式里程计轨迹与真实轨迹对比，均方根误差为0.1791，融合后的里程计误差约为只使用轮式里程计的误差的48.86%，下降51.14%，效果明显。

4 结语

为了提升移动机器人建图效果，提出了一种激光雷达与相机融合，并结合多传感器融合里程计信息的建图方法。该方法融合了激光雷达数据精度和相机数据丰富的优势，相对单一使用激光雷达的建图方式，将激光雷达无法扫描到的物体加入到了地图中，有效增加了地图显示的内容。而且使用多传感器融合的里程计，也提升了建图中所需要的里程计信息的精确程度。实验结果表明，融合相机数据的建图方法可以得到激光雷达所无法扫描物体的信息，并且在地图中显示。使用多传感器融合里程计方式相比只使用轮式里程计的建图方式，降低了在移动机器人转向时造成的累计误差，提高了位置精度，融合里程计轨迹与轮式里程计轨迹对比，均方根误差下降了51.14%，效果明显。