融合激光与视觉点云信息的定位与建图方法

张伟伟 陈 超 徐 军

(江苏科技大学机械工程学院 江苏 镇江 212003)

0 引 言

同步定位与建图(Simultaneous Localization And Mapping，SLAM)主要指机器人在未知环境中，通过携带的传感器获取外界信息以完成对自身位姿的估计并构建出环境地图[1]，是机器人自主化的重要技术。

激光雷达和视觉相机是目前SLAM常用的两种传感器。其中，以二维激光雷达为基础的SLAM[2]能快速构建出精确的地图，但其只能对某一平面进行扫描，会忽略一部分障碍物信息。基于三维视觉传感器的SLAM[3]能获取丰富的环境信息，但探测范围有限且系统实时性较差。为应对使用单一传感器进行SLAM所带来的局限性、提升机器人定位精度与地图质量，Chen等[4]通过在机器人上垂直安装二维激光雷达并融合深度相机采集到的信息实现了地图的精确构建，但定位精度较低。Ding等[5]提出了混合光束法平差框架，将激光与视觉建立的地图通过该框架进行融合对齐，有效提高地图构建精度，然而较大的计算量降低了运行效率。文献[6]利用视觉信息进行帧间匹配并以此约束激光点云之间的数据关联，通过迭代最近点法实现了对机器人运动轨迹的准确估计却无法建立精确的地图。

针对上述问题，本文在融合激光与视觉传感器点云数据[7]的基础上，创新性地提出一种分步式点云配准方法对点云进行匹配以实现机器人自定位和同步地图构建；结合改进的双重闭环检测方法可进一步提高定位与建图精度。搭建的移动机器人平台在实际环境下的实验结果验证了本文方法的有效性。

1 数据获取

1.1 激光雷达数据

(1)

式中：利用i与传感器的角度分辨率即可得到θi。本文所用激光雷达角度分辨率0.36°、探测距离0.06～5.60 m、扫描范围240°、测量精度±30 mm。

1.2 视觉传感器数据

Kinect相机水平探测范围为57°，垂直探测范围为43°，探测距离为0.8～3.5 m，可同时获取环境的颜色和深度信息[8]。采用文献[9]中的标定方法对Kinect进行标定，以此获得相机的内参数矩阵，完成校准后，图像中任意点的三维坐标可根据对应的深度信息得到，进而生成三维点云数据。对于深度图中的任意像素点(xd,yd)，其三维坐标计算如下：

(2)

式中：depth(xd,yd)代表该像素点所对应的深度值，(cxd,cyd)表示图像光心坐标，由标定可得相机焦距fxd与fyd。

2 数据融合

2.1 边界提取

考虑到机器人在导航时只需检测到环境中的障碍物，没必要对障碍物进行详细的描述，因此本文只对物体的几何轮廓进行检测即提取出边界点云信息。机器人搭载的Kinect相机可直接获取深度图像，通过遍历深度图像中的每个点，比较该点与其邻域点的距离变化信息，根据评分判断该点是否为边界点。步骤如下：

(1) 对于图像中每个点pi，构建以其为中心边长S=5的邻域，假设至少有Ν=((S+1)/2)2=9个点与pi共面。

(3) 计算点pi与其顶部、左侧、右侧以及底部邻域内点的欧式距离。以右侧为例，px,y是图像中坐标为(x,y)的一点，其右侧邻域内点的平均位置为：

(3)

(4)

(5)

式中：Sshadow表示潜在阴影边界的评分。

2.2 联合标定

在对两传感器进行数据融合之前，要先解决二者之间的标定问题，即建立二维激光数据点坐标与三维视觉数据点坐标之间的变换关系。本文采用文献[10]方法对两不同类型的传感器进行标定以此获得两不同坐标系间的旋转矩阵R和平移矩阵t。若一空间点在激光坐标系为PL、相机坐标系下为PC，则坐标间的变换关系为：

PC=RPL+t

(6)

2.3 点云融合

利用联合标定得到的两传感器间的旋转平移矩阵，可通过将两者获取的点云转换到同一坐标系下来进行数据的融合，具体步骤如下：

(1) 拟合平面方程：将激光坐标系下二维点云利用式(6)转换到相机坐标系并通过平面拟合构建包含相机坐标系原点的平面方程：ax+by+cz=d。

(2) 去除地面点云：相机采集到的点云数据中亦包含地面点云，地面点云不属于机器人导航时的障碍物信息，因此要将该部分点云去除。

(3) 三维点云投影：通过忽略相机获取的描述空间障碍物的三维点云的Z轴信息，只保留X轴和Y轴信息，可将三维点云投影至步骤(1)中的平面方程，以实现点云的融合。

3 点云配准

机器人利用配准相邻帧点云得到的相对变换关系估计自身位姿，从而实现在环境中的定位。为提高配准效率、降低误差，本文结合点对去除策略提出一种分步式配准方法,具体步骤如下：

(1) 点云初配准。对点云模型进行均匀网格化处理，计算格中点xk(n为点的个数)的均值向量与协方差矩阵，分别用q、C表示。

(7)

(8)

利用N(q,C)对点xk进行正态分布化处理，则其概率密度函数为：

(9)

用一组正态分布{N(μ,Σ)}表示点云模型并将待配准点云按转移矩阵T进行变换。利用Hessian矩阵对两点云模型对应的正态分布{N(μ,Σ)}的概率密度值进行优化即得相对变换矩阵，记为Tini。

(2) 点对去除。经初始配准后的两点云中存在误匹配点对，在进行ICP精配准之前要将该部分点对去除。对于源点云中的点xi，其在目标点云中的对应点为yi：

(10)

式中：超过距离阈值dk的点对(xi,yi)需被去除且距离阈值随迭代过程以步长δd逐渐下降：

(11)

dk=dk-1-δd

(12)

式(11)中：Tk为第k次迭代所得的变换矩阵。

(13)

(14)

(3) 点云精确配准。为避免ICP算法陷入局部极值，以初始匹配得到的变换矩阵作为ICP精确配准的初值。目标误差函数为：

(15)

式中：(xi,yi)表示满足条件的关联点对；n为点对个数；Tk是第k迭代次时的变换矩阵。令T0=Tini并通过四元数法计算使目标函数E最小的变换参数T。

(4) 将T作用于两待配准点云，求取转换误差并判断是否小于误差阈值，若小于则终止迭代；否则返回步骤(2)继续迭代。

4 闭环与优化

4.1 闭环检测

机器人不断运动会使其自身位姿的累计误差不断增大，因此需进行闭环检测以判断机器人是否处于之前到过的位置，为全局位姿优化提供约束。本文结合关键帧机制提出一种双重闭环检测方法，具体步骤如下：

(1) 将传感器采集的第一帧作为关键帧，设置一定的时间间隔。若当前帧获取时间为tc，前一关键帧的获取时间为tp，则要满足tc-tp≤εt，令εt=0.5 s。

(2) 通过正态分布点云匹配算法可得每一关键帧的位姿。计算当前帧位姿与历史帧位姿的距离，将距离小于d0的帧作为候选帧。

(16)

(3) 将当前帧与在候选帧中随机挑选的4～6帧进行ICP匹配。

(17)

4.2 图优化

以每一关键帧位姿为节点、帧间变换为边构建位姿图，通过SLAM++优化库[11]进行优化可得全局一致的机器人运动轨迹；将每一帧点云进行拼接并映射到2D栅格可构建二维环境地图。

5 实 验

如图1所示，实验以TurtleBot2移动机器人为平台，其上搭载激光雷达和Kinect传感器。使用笔记本电脑进行控制和数据处理，算法运行在基于Ubuntu14.04的ROS[12]操作系统下并通过RVIZ显示建图与导航过程。实验场地长5.1 m，宽2.4 m，其中设置有大小不一、形状各异的四个物体(已编号)，用以模拟复杂环境。

图1 实验平台及环境

5.1 环境建图

对于外形较为复杂的椅子D，只需提取出其边界点云信息即可，如图2所示，利用深度点云图得到的椅子轮廓更为简化。

图2 边界提取

图3为局部场景的点云数据融合结果，该场景中仅有椅子D一个物体。Kinect采集到的数据通过边界点云提取与点云投影得到较为准确的椅子边缘轮廓并构建出场景的二维地图；激光雷达虽能构建更加精确的环境地图，但无法对椅子D进行准确描述。将两者数据进行融合，在得到精确地图的同时,也能准确探测出场景中的复杂物体。

图3 点云数据融合

图4是在整个实验场地中分别使用单一传感器方式、本文融合方式和文献[6]方式建立的二维地图。由于扫描高度的限制，激光雷达无法探测到箱子C且只能得到椅子的腿部信息(如图5所示)；而Kinect则可以检测到环境中的所有物体，但构建的地图精度较低。本文通过融合两传感器，可对环境中物体进行完整全面的探测并且构建的地图相较文献[6]更为精确。

图4 四种方式地图构建

图5 激光雷达与物体位置

5.2 定位分析

本文利用点云配准法来实现机器人位姿估计，而文献[5]采用光束平差法来估计位姿。为验证本文方法的有效性，分别在机器人移动2、4和6 m时将两种方法进行比较并得到如表1所示数据。

表1 位姿估计方法比较

相较于文献[5]方法，本文方法在处理速度上提高约18%且位姿估计误差降低约24%。点对去除策略降低了后续匹配点对数量，使得效率更高，同时由于初始配准为后续精确匹配提供了迭代初值，误差也大幅下降。

图6为三种不同方式的定位误差曲线。仅使用激光雷达时，由于其只能获取特定平面的点云，使得用于配准的点云量较少而导致位姿估计出现较大偏差。Kinect能获取丰富的点云信息，但冗余量大且探测精度较差，降低了位姿估计的效率。本文将传感器融合后，在降低数据冗余的同时提高了点云配准效率，再结合改进的闭环检测与优化方法，使得定位精度较激光雷达方式提高约85%、比Kinect方式提高约64%。

图6 定位误差曲线

图7为使用文献[4]方法和本文方法时机器人的运动轨迹。在进行长时间运动时，本文方法依然能保持较高的定位精度，相较于文献[4]方法优势明显。

(a)

(b)图7 两种方法的运动轨迹

5.3 自主导航

利用已构建的二维地图进行导航实验，1为起始点、2为目标点，结果如图8所示，黑线表示机器人自主规划的路径。

图8 不同地图下的导航

在激光雷达构图下的导航，由于无法探测到箱子C并准确识别椅子D,机器人在移动时会与它们发生碰撞。而使用Kinect地图进行导航时，机器人能有效避开场景中所有物体，但由于地图精度较低使得到达目标点的路径并非最优。本文融合方法构图下的导航，机器人不仅能准确规避障碍物，同时路径规划也更高效。

6 结 语

针对单一传感器在机器人SLAM中存在的不足，本文通过将两种不同传感器进行融合并结合提出的分步式点云配准策略与双重闭环检测方法，以实现机器人对自身的定位，同时构建出二维环境地图。实验结果表明，本文方法能有效提高定位精度且构建的地图更精确、完整，提高了自主导航的可靠性与效率。今后将开展在室外大场景下的SLAM研究,具体改进如下：(1) 引入动态物体识别技术以探测移动物体；(2) 利用多线程技术提高系统的处理速度；(3) 结合惯性传感器进一步提高定位精度。