基于多传感器信息融合的未知环境下移动机器人的地图创建

李 艳, 唐达明, 戴庆瑜

(1.陕西科技大学 电气与控制工程学院， 陕西 西安 710021； 2.陕西农产品加工技术研究院, 陕西 西安 710021)

0 引言

未知环境中，机器人的地图创建与自主定位密切相关，SLAM(Simultaneous Localization and Mapping)将二者相结合，通过已经获得的环境地图信息完成自主定位，并根据定位结果完成对环境地图的更新[1].

SLAM问题是指机器人在未知的环境下，仅依靠自身传感器的观测数据zt和控制输入量ut的情况下确定机器人的大致位置，并且通过观测信息构建出大致的环境地图，同时用构建出来的地图继续对机器人的位置进行估计.如图1所示，在机器人的移动过程中，通过一系列的控制输入量u1∶t={u1,u2,…,ut}和观测数据z1∶t={z1,z2,…,zt}，对自身的运动轨迹x1∶t={x1,x2,…,xt}及环境地图m进行准确的估计.

图1 SLAM问题示意图

SLAM问题用数学语言描述为求解如下概率分布：

Bel(xt)=p(xt,m|u1∶t,z1∶t)

或Bel(x0∶t)=p(x0∶t,m|u1∶t,z1∶t)

(1)

当前，求解SLAM问题的方法可大致分为两大类：一类是基于滤波的方法，另一类则是基于图优化的方法.其中，基于滤波器的方法由于其算法简单而被大量应用于SLAM中[2-4]，但随着研究的不断深入，SLAM的应用范围也越来越大，当使用基于滤波的方法时，机器人每次移动只考虑当前状态，在建图时误差会不断累积，导致其最后构建出的地图与实际环境有差别，使其难以应用于较大环境下的地图创建.而基于图优化的方法在估计机器人运动轨迹时，会用到传感器的所有观测信息，在建图的过程中把对地图特征的描述转化为对运动轨迹的约束，提高了建图的效率与准确性，使其适用于较大环境下的地图创建.

目前，对基于图优化的建图算法研究越来越多，文献[5]使用激光雷达完成地图的构建，将基于图优化的方法应用到地图构建中提高了建图的效率；文献[6]使用双目相机完成地图的构建，将多层次松弛的方法应用在图优化的地图构建中，很好地解决了因算法形成闭环而无法更新地图的问题；文献[7]使用深度相机完成地图的构建，并在建图时应用梯度下降方法，提高了SLAM的鲁棒性.目前，对于SLAM的研究主要集中在对地图构建算法的研究[2-7]，而对于传感器数据处理方面的研究较少，并且以上方法都是基于单一的距离传感器或视觉传感器，都存在着很多缺陷，例如，二维激光雷达所获取的信息会因为安装位置不同而受到影响，视觉传感器获取的信息会因为环境的变化而受到影响，因此导致实验效果仍然不佳.

针对移动机器人在未知环境下利用单一传感器进行地图构建时所出现的问题，本文提出利用基于图优化的算法构建地图，并在建图时将移动机器人的激光与视觉信息进行融合，使得构建出的地图与实际环境更加接近.

1 传感器数据处理

1.1 激光雷达数据处理

激光雷达通过旋转的方式扫描外界物体获得信息[8]，并以极坐标的形式表示为Zl=[dl,θl]T，其中，θl为激光雷达扫描时，目标点和激光雷达扫描起始位置之间的夹角，并且0 °<θl<360 °，dl是目标到激光雷达的直线距离，将原始扫描数据转换至直角坐标，则表示形式为Zl=[x,y]T，如图2所示.

图2 激光雷达扫描测量示意图

利用式(2)，可将扫描得到的数据点转换至直角坐标系中.

(2)

处理激光雷达数据时，可以将周围环境的信息抽象的表示为点、线等特征[8].在扫描过的环境信息中可以得到线性与非线性的数据，若是线性的数据则直接化为直线进行处理；若是非线性的数据则需要将这些数据变换为折线特征(即直线加角点)，同时采用迭代的方法，对非线性区域中的角点进行寻找，直到扫描区域内不存在角点，由公式(3)可以计算出角点之间的距离.这个过程不断往复循环，直到最后可以形成2D环境地图.

db=|(xi-x1)sinδ+(yi-y1)cosδ|

(3)

式(3)中：db为角点间的距离，(x1,y1)为机器人当前的位置坐标，(xi,yi)为扫描到的目标点的坐标，δ为角点之间形成的连线与X轴的夹角.

1.2 深度相机数据处理

深度相机的数据处理主要是将深度相机获取到的图像数据转换为模拟激光数据[9].深度相机能够通过相机模型将获取到的深度图像数据转换成为模拟的二维激光雷达数据，实现将三维数据压缩为二维数据的功能，从而完成传感器之间的数据融合.

深度图像数据转换模拟激光数据的步骤如下：

(1)计算图像坐标系下的点m(u,v)在世界坐标系下的位置坐标M(XW,YW,ZW) .深度数据转激光数据的原理如图3所示.

图3 深度转激光原理图

利用相机模型能够完成世界坐标系与图像坐标系之间的转换[10]，如式(4)所示.

(4)

式(4)中：(u,v)为图像坐标系中的某一坐标点，(u0,v0)为图像坐标系中心点，(xw,yw,zw)表示该点在世界坐标系下的坐标，其中zc表示目标到相机的距离.R、T为摄像机的外参数，用来表示旋转向量和平移向量；f为内参数，通过摄像机标定得到.

由于世界坐标系的原点与相机的原点重合，因此没有平移和旋转向量，所以外参数矩阵R、T可以设置为

(5)

由于在相机坐标系和世界坐标系下对于同一个目标的深度值相同，即ZC=ZW.因此公式可进一步简化为：

(6)

三维空间中任意一点的坐标，都能够从以上的变换矩阵公式推导得出，因此，图像点[u,v]T到世界坐标点[XW,YW,ZW]T的变换公式可以表示为：

(7)

(2)计算直线AO和DO的夹角AOD，计算公式如式(8)所示.

θ=arctan(x/z)

(8)

(3)将角AOD投影到对应的激光扫描范围内.已知激光雷达的扫描范围为[α,β]，将该范围内的光束分为N份，则可用数组laser[N]来表示[α,β]内的所有激光数据.因此点M投影到数组laser中的索引值n可用式(9)计算.

n=(θ-α)/((β-α)/N)=N(θ-α)/(β-α)

(9)

laser[n]的值为M在x轴上投影的点D到相机光心O的距离r，即

(10)

因此，以激光雷达数据格式为标准对深度数据进行转换时，通过将深度图像在水平范围内取整数角度上的最小深度值作为激光雷达距离数据，相当于取三维深度数据的水平投影，并加上对应角度值，这样就完成了深度数据转换为激光雷达数据的过程，再将数据保存到激光传感器获取的数组中，完成了深度相机关于前方障碍物距离信息的获取.Ranges[]数组体现了横向每个障碍点到相机的距离，完成数据获取后，将Ranges[]数组中关于同一角度的较大的数据都用小数据来替换，这样就完成了对于障碍物在高度上的压缩，形成了二维平面地图.

2 传感器数据融合

当单独使用激光雷达或深度相机进行地图构建时都会存在一定的误差，因此本文先采用扩展卡尔曼滤波将激光雷达的数据与深度相机的数据进行融合，再利用贝叶斯估计的方法将深度相机与激光雷达分别生成的地图进行二次融合，来提高建图的准确性.

2.1 基于扩展卡尔曼滤波的数据融合

在本文设计的方案中，利用扩展卡尔曼滤波器融合传感器的数据时，因为激光雷达的测量精度相较于深度相机更高，所以将激光雷达的测量值作为观测值，将深度相机的测量值作为预测值，这样能保证融合效果更好.令Xt=[xt,yt,θt]T作为系统的状态变量，扩展卡尔曼滤波数据融合的过程如下：

(1)预测阶段

(11)

(12)

(2)观测阶段

将激光雷达的测量值ZS作为系统测量值，

ZS=Ht+1Xt+1

(13)

式(13)中：Ht+1为激光雷达的观测矩阵.

(3)观测的预测阶段

将深度相机的测量值ZV作为预测的观测值，

(14)

式(14)中：Ht+1为深度相机观测矩阵，且与激光雷达的观测矩阵相同.

(4)数据关联阶段

在对激光雷达和深度相机进行数据融合之前，还需要解决两个传感器的数据关联问题，目的是辨别两个传感器获取的信息是不是源自于外界环境中的同一个物体[11].这是数据融合的前提，如果数据关联出错会导致对于环境目标的获取出现错误，最后无法构建出完整的地图.

根据激光雷达与深度相机获取的观测信息ZS，ZV之间的Mahalanobis距离[12]，来判断两个传感器获取到的数据的关联性，当距离小于设定阈值χ2时，即则说明关联性较高.

(15)

τt=ZS-ZV

(16)

式(15)、(16)中：d=rank(τij)，α为可信度，τt为测量的位置误差.当ZS，ZV中有多个特征相匹配时，则选取距离最近的两个特征作为最佳匹配，当完成了数据关联后就能够完成状态的更新.

(5)更新阶段

首先计算卡尔曼增益Kg，利用多传感器的测量值修正之前的系统预测值，完成激光雷达采集的数据和深度相机采集的数据之间的融合，

(17)

再利用卡尔曼增益计算传感器数据融合的最优估计值，

(18)

最后利用卡尔曼增益完成对协方差矩阵的更新，

(19)

2.2 基于贝叶斯估计的地图融合

在通过扩展卡尔曼滤波完成对于传感器的数据融合后，如果激光雷达与深度相机观测到的信息没有完成数据关联，那么系统状态不会完成更新，之后两个传感器的数据也不会进行融合，环境中的某些障碍物就不会被检测出来，进而导致地图构建出现误差.因此在对两个传感器获取的数据完成一次融合后，还应该在分别生成局部地图之后，将两个传感器分别生成的局部地图进行二次融合，以保证构建的地图的准确性.

本文利用贝叶斯估计的方法将激光雷达和深度相机获得的局部地图完成二次融合.贝叶斯估计通过将传感器采集的环境信息转换成后验概率分布函数[13]，并通过贝叶斯条件概率公式更新后验概率分布值，从而实现对多传感器的信息融合[14].

贝叶斯估计可以描述为：通过已知的向量Z,估计未知的m维状态向量X并完成对于后验概率的计算，已知t时刻的概率为p=(xt)，且k组观测数据为Zk=(Z1,…,Zk)，先验分布公式如下：

(20)

式(20)中：p(zt|xt)是基于传感器观测模型的似然函数；p(xt|zt-1)是基于系统模型得到的前一时刻的先验分布；p(zt|zt-1)表示当前时刻与上一时刻之间的状态变化.

在建图的初始阶段，由于环境信息未知，因此地图中存在障碍物和无障碍物的先验概率初始化均为0.5.随着建图的进行，利用传感器不断扫描周围环境，通过多传感器获得的环境信息并利用贝叶斯估计得到传感器信息融合的公式如下所示：

(21)

具体融合规则如表1所示，栅格地图属性分为三类：占据(O)、未占据(E)以及未知(U).

表1 激光和视觉传感器融合规则

通过本文提出的方法得到的传感器数据融合构建地图的流程如图4所示.

图4 数据融合构建地图流程

3 移动机器人的地图构建

完成对传感器数据处理之后，可以利用融合后的传感器数据，采用基于图优化的算法完成地图的构建.基于图优化的地图构建算法由图构建和图优化构成，如图5所示.其中图构建包括相邻两帧传感器数据之间的匹配与回环检测，图优化包括对于运动姿态的优化[15].

图5 基于图优化的SLAM算法框架

如图6所示，基于图优化的方法通过对机器人的位姿和地图节点之间的最小均方误差的求解来完成对SLAM问题的求解：将各个时刻的位姿xt及地图m中的路标点(P1,P2,…,Pn) 看成一个个待优化的节点，将相邻位姿节点间的边作为一个约束条件，将位姿节点和路标节点间的边作为另一个约束条件[16]，因此，建图的过程主要是通过传感器对环境的扫描匹配来建立位姿节点和路标节点之间的约束关系，当满足这个约束关系时，找到最佳的位姿节点xt和地图m值，同时让总体的误差平方和最小，以此来完成地图的构建[17].

图6 基于图优化模型的SLAM

在机器人仿真软件 中搭建环境模型，并分别对基于滤波和基于图优化的地图构建算法进行仿真.其中，如图7(a)所示为仿真环境图，图7(b)为滤波算法创建的环境地图，图7(c)为图优化算法创建的环境地图.通过对比图7(b)和图7(c)可以发现，基于滤波的建图算法在构建地图的过程中，由于数据不能及时更新，导致误差不断被累积[18]，因此构建出的地图与实际地图相差较大.而基于图优化算法构建的地图，通过对传感器的连续帧数据的提取，完成对位姿轨迹的确定[19]，并构建出位姿图，同时将得到的位姿图用回环检测进行校正，最后通过位姿优化消除建图中产生的误差，得到与仿真环境基本一致的地图.

(a) GAZEBO仿真环境图

(b) 滤波算法创建的地图

(c) 图优化算法创建的地图图7 实际地图及算法仿真对比图

4 实验结果及分析

本文利用一台履带式的移动机器人进行实验，如图8所示.系统硬件框图如图9所示，系统由上位机和下位机两部分构成，其中下位机系统由STM32、电机驱动电路、惯性测量单元组成，上位机系统由树莓派、深度相机和激光雷达组成，通过树莓派的WIFI模块可以远程连接PC端，并且通过PC端完成对机器人的远程控制，最后，环境地图的构建结果将在PC端的Rviz中显示[20].

实验环境如图10(a)和图11(a)所示，分别是大小为6.0 mm×0.8 m的简单走廊环境和大小为6.0 m×5.0 m的较复杂室内环境，其中室内环境包含桌椅、箱子等障碍物.在实际应用时，由于二维激光雷达获取的信息会受到安装位置的影响，深度相机获取的信息会受到环境光线的影响，因此都会出现因为漏检障碍物而导致构建的环境地图精度不高的情况，所以通过设计这两种环境进行地图构建实验，将两个传感器获取的信息相融合，来验证本文提出的方法的可靠性.

图8 移动机器人模型

图9 系统硬件框图

图10(b)和图11(b)为仅使用激光雷达对环境进行建图.可以看出，当只使用激光雷达进行建图时，随着时间的推移，建图误差越来越大，构建出的地图与实际环境不一致，并且由于受激光雷达安装位置的影响，有些障碍物如椅子(B、C、D、E)的某些部分没有在扫描平面内，因此导致扫描获得的地图信息不够全面，造成建图精度低、噪点多的问题.

图10(c)和图11(c)为仅使用深度相机对环境进行建图.当只有深度相机完成地图构建时，虽然深度相机相对于激光传感器能够获取到环境的三维信息，对于障碍物的检测更加准确，但是深度相机的拍摄精度容易受到环境光线的影响，因此在建图的准确性上较差.

将深度相机与激光雷达的数据融合以后，构建的地图如图10(d)和图11(d)所示.通过对比可以发现，将两个传感器获取的信息融合之后，有效地弥补了单一传感器扫描环境的局限性，克服了复杂环境的干扰，使构建的地图更加完整，构建的地图与真实环境更接近，同时地图上的噪点也更少.

(a)走廊环境

(b)激光雷达构建地图

(c)深度相机构建地图

(d)数据融合后构建地图图10 走廊环境下构建的地图

(c)深度相机构建地图

(d)数据融合后构建地图图11 室内环境下构建的地图

移动机器人对于环境地图的构建精度测试可以转换为机器人在环境中移动时对于运动轨迹的精度测试[21].机器人在不同环境下的运动轨迹的对比图，如图12(a)和图13(a)所示.图12(b)和图13(b)所示为数据融合前后机器人运动轨迹的均方根误差对比，可以看出，数据融合后的机器人运动轨迹明显优于融合之前，并且走廊环境下的均方根误差在3 cm左右，室内环境下的均方根误差在8 cm左右，均小于数据融合之前的误差，同时曲线波动幅度更小且更稳定，表明了数据融合之后构建的地图更精确，更符合真实环境，从而验证了本文算法的准确性.

(a)走廊环境下不同轨迹对比

(b)数据融合前后的均方根误差图12 走廊环境下运动轨迹和误差对比

(a) 室内环境下不同轨迹对比

(b) 数据融合前后的均方根误差图13 室内环境下运动轨迹和误差对比

5 结论

本文提出了一种将激光雷达和深度相机信息融合的地图构建方法，先利用扩展卡尔曼滤波器将移动机器人获取的深度视觉数据与激光雷达的数据进行融合，再利用贝叶斯估计的方法将激光雷达与深度相机分别建立的局部地图进行二次融合，并在构建地图时采用基于图优化的地图构建算法，在完全未知的环境中，完成对环境地图的准确构建.实验结果表明，利用本文提出的多传感器信息融合的方法，获得的环境信息更加全面，提高了地图构建的可靠性和准确性，并且有效提高了移动机器人在SLAM时的鲁棒性.