基于ROS机器人的激光与视觉融合建图方法*

姬 鹏，高帅轩

(河北工程大学机械与装备工程学院，邯郸 056038)

0 引言

同时定位和地图构建(SLAM)技术是指移动平台在没有环境先验信息的情况下，在运动过程中建立环境的模型，同时估计自己的运动。SLAM技术是载有传感器的移动平台在未知环境中进行侦查、导航的前提。在实际应用中，单一传感器会存在易受环境干扰、环境信息缺失、精度低等问题。而传感器融合则可以综合各个传感器的优势，为机器人环境感知提供冗余信息，通过多传感器融合技术可以提升移动机器人在复杂环境中的定位和建图精度。

多传感器信息融合作为一种可有效增强系统数据准确性、提高数据利用率的信息处理技术，在机器人、医学、目标跟踪、导航、图像分析等方面获得广泛关注，因此国内外专家学者已提出许多传感器信息融合方法，MONTEMERLO、DOUCET等[1-2]使用基于粒子滤波器的SLAM方法，该算法近似得到机器人定位和建图的概率密度，但方法会造成粒子种类减少，造成计算资源浪费等问题，CASTELLANOS、庄严等[3-4]对激光雷达和单目相机融合SLAM进行了研究，将机器人所处的未知环境中一些相关特征关联作为地标，在一定程度上增加了数据的准确性，但计算量较大实时性较差。

本文针对单一传感器构建地图精度低、易受环境干扰等问题，提出将深度图像信息与单线激光雷达信息进行数据融合，并基于贝叶斯理论构建概率模型更新地图，保证环境地图信息的完整性和自主导航的实时性。

1 系统整体结构与传感器模型

ROS机器人[5]系统由两个相对独立部分构成，如图1所示，这两个部分采用串口通信的方式进行双向数据传输，STM32作为驱动部分与ROS主控进行信息交互完成对运动底盘目标速度的控制，上层ROS主控部分为英伟达JESTON NANO微型计算机，可以通过wifi模块接收本地计算机发出的控制命令，通过微型计算机Ubuntu系统下的ROS系统完成机器人运动、建图、导航等功能。

图1 系统结构图

奥比中光Astra pro RGB-D相机如图2所示，它的观测深度范围为0.6～8 m，水平探测范围为58°，垂直探测范围为45°，可获得物体颜色信息和深度信息。思岚单线激光雷达RPLIDAR A1如图3所示，扫描范围为360°、探测半径为12 m，扫描频率最高可达10 Hz。两种传感器的探测范围如图4所示，视觉传感器可采集到环境中三维信息，相比较单线激光雷达而言空间信息较为丰富，但视角有限且易受光照影响，单线激光雷达扫描范围较广，但只能扫描自身所在平面环境信息，本文的研究意义在于融合两种低成本传感器的优势，使机器人可提供更加完整、准确的环境信息。

图2 Astra pro RGB-D相机 图3 RPLIDAR A1激光雷达

图4 激光雷达和视觉传感器扫描范围

2 传感器数据融合方法

2.1 联合标定

在进行激光雷达和视觉传感器信息融合前，要对二者进行标定，图5中P点在激光雷达坐标系中坐标为PL，在相机坐标系中为Pk，本文通过文献[6]中所述标定方法求得不同坐标系间相互变换的旋转矩阵R和平移矩阵T，则图中P点在两个坐标系下的关系为：

Pk=RPL+T

(1)

图5 激光雷达和相机坐标系转换

2.2 深度图像转激光数据

RGB-D相机采集的深度图像中任一点都相对应一个像素值，图6中H表示物体A到视觉传感器所在平面的垂直距离，R表示障碍物A到视觉传感器的直线距离，α位障碍物与视觉传感器水平夹角，可得：

H=depth[x][y]

(2)

(3)

式中，depth[x][y]表示处于深度图像中第x行与第y列位置处像素点的信息值，为了融合二维激光雷达信息，需要将视觉传感器采集的数据转换为光线信息在二维平面投影使不同传感器信息处于同一维度，步骤如下：

步骤1：将深度图像中的p点映射到RGB-D相机坐标系下相对应的点P(x,y,z)。

步骤2：计算夹角OAB。

θ=arctan(X/Z)

(4)

步骤3：将图中包含的所有深度信息关联到激光雷达数据中，激光雷达传感器探测范围为0～360°，将雷达光束分为M份，则激光雷达信息可表示为laser[M]，点P信息索引号为m，可得m的值：

m=θ/(360/M)=θM/360

(5)

laser[m]的值为点P投影到x轴的点B到A的距离d，即：

(6)

图6 深度图几何模型 图7 深度图转激光数据原理图

由于激光雷达信息类型是一个行向量，而视觉传感器通过上述步骤得到的数据类型为二维矩阵，因此考虑用深度图中每一列数据中的最小值替换此列较大的值，最小值即为障碍物到视觉传感器的最近距离，从而将深度数据转为激光数据。

2.3 贝叶斯法则

对激光雷达扫描到的场景信息和RGB-D摄像机三维转化为二维的场景信息，应用基于概率模型的贝叶斯推理方法进行融合，该方法可根据已有向量Z，预测n维向量X，假设观测Zk={zi,i=1,2,…,k}和此时概率xk，则先验分布如下：

(7)

式中，p(zk|xk)为基于给定传感器测量模型的似然函数；p(xk|Zk-1)为转换系统模型的先验分布；p(Zk|Zk-1)为归一化概率密度函数。

(8)

(9)

2.4 改进多传感器融合地图更新方法

(10)

图8 激光雷达与视觉传感器融合流程图

根据上述方法，利用简化之后的栅格单元占据概率取代原始栅格单元占据概率，对于两个传感器构建的栅格地图中同一处栅格点按表1融合规则进行融合。

(11)

3 实验论证

3.1 环境建图

为验证本文融合方法的有效性，将本文融合算法应用于ROS机器人中进行验证，机器人平台参数如表2所示，与机器人通讯的笔记本电脑在Ubuntu18.04下的ROS系统发布命令[7-10]，本文实验数据均为室内实验数据，该实验基于Gmapping建图算法利用单一传感器和多传感器融合分别建图，Gmapping算法在构建室内小场景地图时计算量相对较小且准确度较高、鲁棒性高，该算法充分的采集里程计信息，为自身提供了先验位姿。

表2 移动机器人硬件表

实验场景如图9所示，共包含5个形状各异的障碍物，其中障碍物D高度为6 cm，低于单线激光雷达检测平面，ROS机器人与障碍物D高度如图10所示，在栅格地图中黑色区域代表栅格存在障碍物，白色区域代表栅格不存在障碍物为可通行区域，图11为单线激光雷达建图结果，可以看出，激光雷达所扫描范围较宽阔，环境建图更为准确，但由于障碍物D高度值低于单线激光雷达扫描平面，因此并未被检测到。

图9 实验场景 图10 雷达与障碍物高度

图11 激光雷达环境建图

RGB-D相机检测环境三维建图如图12所示，三维点云信息较为稀疏[11]，视觉传感器易受光照影响且视野范围有限，但障碍物的重要特征信息并未丢失，这也是将视觉传感器与激光雷达融合的依据。

本文通过融合激光雷达和相机两个传感器的环境信息建图如图13所示，相比于单传感器所建地图获得更为完整的环境信息，更接近于真实环境。

图12 视觉传感器三维建图 图13 数据融合二维建图

3.2 自主导航

以激光雷达所建地图为先验地图进行导航，设置导航起点为图14中左下角1处，终点为右上角2处，图14a中由于障碍物D不在摄像头视角范围内，因此ROS机器人规划的路线经过障碍物D；图14b图中，随着机器人的行进，摄像机感知到障碍物D并对原始路径重新进行规划；图14c图中，机器人移动到障碍物附近并成功绕开；图14d图中，所有障碍物信息都被检测到，地图得到更新，机器人成功到达预设的终点。

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

(c) 绕开障碍物 (d) 到达终点

4 结束语

本文针对单一传感器在SLAM中存在建图精度低，易受干扰等问题，提出了一种多传感器数据融合方法，该方法将视觉传感器采集到的3D点云信息转化为2D激光雷达信息与激光雷达采集到的信息按一定规则融合，以弥补单线激光雷达丢失的较矮障碍物信息。实验结果表明，利用多传感器感知的冗余信息进行融合，构建了鲁棒性更高、精度更高的2D栅格地图，反映出了真实的环境地图，有效的提高了机器人导航的准确性，多传感器融合建图对机器人准确感知复杂环境、导航、避障具有重要意义。