基于RGB-D的移动机器人地图构建方法研究

富 裕，刘振宇

（沈阳工业大学信息科学与工程学院, 沈阳110870）

1 引 言

近几年，随着廉价高可用性RGB-D 相机的流行，RGB-D SLAM 得到广泛应用。RGB-D SLAM 的实现与激光雷达方法相似，都是利用距离图像之间的密集三维点云来估计对应点云之间作为刚体变换的位姿间的运动。华盛顿大学（University of Washington）的Henry 等人于2010 年提出了一种室内三维视觉SLAM 方法[1]，该法通过对Kinect 采集到的RGB 图像提取SIFT（Scale-Invariant Feature Transform，尺度不变特征变换)特征点，结合深度数据与ICP（Iterative Closest Point，迭代最近点)算法[2]估计摄像机的位姿，最终建立起室内环境下的三维地图。其后，德国弗莱堡大学（University of Freiburg）计算机科学系的Nikolas Engelhard、Felix Endres 和Jurgen Hess 提出一套基于手持Kinect 相机的RGBD SLAM 方法[3]，在SLAM 算法的视觉里程计部分使用SURF（Speeded Up Robust Features，加速鲁棒性特征）[4]算法，然后根据特征点计算出三维空间坐标。与SIFT 相比，在保证具有尺度和旋转不变性的同时，其时间复杂度稍有降低，但目前为止计算机仍然无法实时地进行图像特征提取及匹配。另外，SLAM算法的误差主要来源于行进过程中机器人带来的累计误差。当移动机器人进行定位时，其可通过观测位置处已知的特征标志对前端的误差进行补偿，每一次观测使机器人的位置误差趋近特征标志的位置误差与观测误差之和[5]。然而在SLAM 中，由于移动机器人的位置和环境中特征标志的位置均未知，故观测信息不能有效纠正前端的误差，机器人的位置误差会随机器人运动距离的增大而增大。因此，机器人的位置误差与特征标志的位置误差紧密相关。它们之间的相互影响使机器人和特征标志的位置估计产生累计误差，难以保证地图的全局一致性。

针对上述研究现状，提出一种室内环境下的移动机器人RGB-D SLAM 方法。该方法利用Kinect获取图像彩色信息及深度信息，提取ORB（Oriented FAST and Rotated BRIEF）特征点进行特征匹配，在保证特征点匹配精度的同时，有效提高了算法实时性；引入回环检测方法，有效抑制累积误差造成的位姿漂移，最终完成室内环境下的三维地图重建。

2 视觉SLAM 框架

SLAM（Simultaneous Localization and Mapping，同时定位与地图构建）是指机器人在其自身位置不确定且无先验环境模型的条件下创建地图，同时基于该地图实现自定位[6]。在此提出一种基于深度图像的移动机器人RGB-D SLAM 框架，框架的结构与流程图如图1 所示，其主要包括前端（视觉里程计)、后端和回环检测三个部分。

图1 提出的RGB-D SLAM 框架

a.前端

VO（Visual Odometry，视觉里程计）用来估计两个时刻间机器人的相对运动。在视觉SLAM 中，将观测到的信息与全局地图进行匹配，采用RANSAC（Random Sample Consensus，随机抽样一致性）[7]算法去除误点得到位姿的初始估计；接着采用ICP 算法进行点云匹配改善位姿；最后得到深度相机的位姿。其中基于特征的方法是目前视觉里程计中流行的方式。若使用RGB-D 相机，利用已知的深度特征点，可直接估计移动机器人的运动。给定一组特征点及配准关系求解机器人的位姿，称为PnP（Perspective-n-Point）问题。用非线性优化求解PnP，得到帧间位置关系。最常用的是点特征，例如Harris 角点、SIFT、SURF 和ORB 等。

b.后端

在前端完成两个帧之间图像特征匹配后，理论上是可以得到移动机器人的轨迹的，但在前端会产生误差，且随着机器人的行进这些误差将会一直堆积，产生累计误差。例如，若在两个时刻间，估计的转角比真实转角少1 度，那么之后的轨迹将全部减少1 度，如此反复，重建的地图将会失真，估计的轨迹也会有严重的漂移。故在SLAM 中，还会把帧间相对运动放到后端的程序中进行优化。

c.回环检测

回环检测（Loop closure detection）是指机器人识别曾到达场景的能力，实质上是一种检测观测数据相似性的算法。回环检测主要用来解决移动机器人位置估计随时间漂移的问题，为了降低视觉里程计过程中的积累误差，利用各关键帧的相互关联性进行有效的闭合回环，在本质视图上优化位姿。这样就可以将累计的误差分散到位姿图中，并通过相似变换矫正尺度偏移。如果回环检测成功，则认为机器人曾到达过这个地方，把比对信息输送给后端的优化算法，后端再根据回环检测的信息调整移动机器人轨迹和地图。通过回环检测，可有效抑制累积误差造成的位姿漂移[8]。

3 特征提取及匹配

在此采用ORB 算法进行特征提取和匹配。首先利用Oriented FAST（一种改进的FAST 角点）检测子在相邻图像上检测特征点，再通过Rotated BRIEF（Binary Robust Independent Elementary Features)描述符生产特征点的二进制描述向量，最后相邻图像的特征点的匹配采用汉明距离比值准则得到最终的特征点匹配对。此ORB 算法改进了FAST 检测子的方向性问题，并采用速度极快的二进制描述子BRIEF，大大提高整个图像特征提取实时性。

用此法提取ORB 特征主要包括以下两个步骤：

第一步，提取Oriented FAST 角点。首先，找出图像中的“角点”；然后，计算特征点的主方向，以便增加BRIEF 描述子的旋转不变性。

第二步，产生BRIEF 描述子。采用二进制，描述所提取特征点的周围图像区域。

3.1 Oriented FAST 关键点

Oriented FAST 关键点是一种角点，主要对局部像素灰度变化明显的地方进行快速检测。如果一个像素与其邻域的像素间差别较大（过亮或过暗）, 那它极有可能是角点。相比于其他角点检测算法，Oriented FAST 关键点只需比较像素灰度值的大小，使检测子具有尺度方向性的同时又提升了实时性。其检测过程如图2 所示，可归纳为：

1)在图像中任选像素p，它的亮度为Ip;

2)设定一个阈值T（示例中T=0.7 );

3)以像素p 为中心，选取半径为r 的圆上的n个像素点（示例中r=3, n=16）;

4)若该圆上有m 个连续的点亮度在Ip±0.7 范围内，则像素p 被认为是特征点（示例中m=9);

5)循环以上四步，对每个像素执行相同操作。

图2 Oriented FAST 特征点的检测

因为之前的FAST 角点不具有方向信息，取半径为3 的圆会存在尺度问题：从远处看是角点的地方，近距离看可能就不是角点。针对此问题，首先构建图像金字塔，检测每层金字塔的角点，然后采用灰度质心法（Intensity Centroid），具体步骤如下：

1.在一个小的图像块B 中，定义图像块的矩：

2.通过矩找到图像块的质心：

3.由图像块的几何中心O 指向质心C，得到方向向量OC，则所求特征点的方向如公式(3)所示：

综上，改进后的FAST 角点在具有了方向性的同时，也显著提升了在不同图像间描述的鲁棒性。

3.2 BRIEF 二进制描述子

在完成Oriented FAST 关键点提取后，得到特征点的主方向，就可以据此计算对每个点的描述子。本算法对BRIEF 特征描述做了改进。

此处，BRIEF 采用二进制，描述向量由许多个0和1 组成。这里的0 和1 表示的是两个附近关键点的像素（假如两个像素为x, y）的大小关系：若x 大于y，则BRIEF 描述子等于1，反之就等于0。如果取了n 对这样的x 和y，最后就得到n 维由0 和1 组成的向量。BRIEF 使用随机选点来进行比较，速度非常快；而由于采用了二进制来表示，储存方便所占空间小，适用于实时的图像匹配。原始的BRIEF 描述子不具有旋转不变性，因此在图像发生旋转时容易丢失。而改进后的Oriented FAST 在特征点提取阶段计算了关键点的方向，所以可以利用这一方向信息，计算旋转之后的“Steer BRIEF”特征，使ORB 的描述子具有较好的旋转不变性。

4 回环检测

回环检测用来判断当前位置是否曾是已访问过的环境区域。改进后方法的视觉回环检测是通过比较两幅图像数据的相似性来实现的。如果回环检测成功，则认为机器人曾经来过这个地方，将比对信息输送给后端算法进行优化。后端再根据回环检测的信息调整机器人轨迹和地图[9]。

在此采用贝叶斯滤波更新进行机器人位姿回环检测。随机变量St表示t 时刻的回环假设状态，即回环假设的可能分布，通过不断更新粒子的位置和权重来逼近系统的真实后验概率分布[10]。整个滤波更新过程分为预测/更新两步，递归执行，如下式：

预测：

更新：

在图像Ii与It结束匹配循环时，令St=i，则Ii与It表示相同的位置。当t 时刻没有发生闭环时，令St=-1，则It表示新图像。粒子滤波使用tN+2 个相关权重的样本集合计算相应时刻t 的后验概率密度,如下式：

其中tn是机器人的工作记忆（WM）中与新场景图像相关的时间指数；It表示t 时刻获取的图像序列，仅包含WM 中的图像，It=I-1,...It。用似然函数L(St/It)评估观测模型p(It/St)，比较当前图像It和每个闭环状态中St=j（j=-1,...,tn）对应图像的相似度，得出一个分数Sj=R(zt,zj)。对每个得分进行标准差σ 和均值μ 的归一化处理，计算t 时刻的似然值，即：

对每个新图像的状态St=-1 作似然估计如下：

若似然函数L(St=-1/It)的值很高，则说明It与WM 中的一个图像不相似，且当σ＜μ 时，则It极可能是个新场景图像。预测St的分布用时间演变模型p(St/St-1=i)来表示，其中St-1的状态均为已知，与机器人在t 时刻和t-1 时刻的状态相关。

此处的时间演变模型有以下四种取值情况：

此概率函数为一个以j 为中心的离散高斯曲线，邻域正好有八个值是非空的，且高斯系数之和是0.9。

通过比较图像特征的匹配率来衡量图像间的相似度。定义匹配率ROM（Ratio of Match）如下：

其误差函数为：

其中，T 表示深度相机位姿的描述集合；Tij表示不同时刻相机的相对位姿；e(Tij,Tij,Tij)表示不同时刻位姿观测值与预测值之间的误差，即闭环误差；ψij表示不同时刻相机位姿间观测相对性矩阵。当e(Tij,Tij,Tij)=0 时，表示从i 到j 时刻相机位姿估计满足闭环调节，该时刻相机的位姿漂移为零。

5 实验结果及分析

5.1 硬件平台搭建

实验采用的移动机器人平台为全向轮移动机器人，如图3 所示。机器人顶端采用深度相机Kinect，其彩色图像分辨率为640×480，深度图像分辨率为320×240，深度感应有效距离为1.2～3.5m，最高帧率为30 帧每秒。全向轮小车上搭载一台笔记本电脑，它配备主频为2.5GHz 的Intel Core i5 处理器，配置Ubuntu 16.04 LTS 操作系统，运行ROS 操作系统，作为数据发送端，通过ROS 系统采集、传输图像到远程数据处理端PC。

图3 移动机器人实验装置

5.2 真实室内场景下三维点云地图构建

5.2.1 实验场景选取

实验选取6m×6m 的办公室封闭场景，如图4 所示。场景中物品较多，环境较复杂，以此可检验复杂场景下特征点提取的可信度，进而验证本算法的可靠性。

图4 实验场景

5.2.2 特征提取

对Kinect 相机采集的图像进行ORB 特征提取，验证算法的效果。提取效果如图5，可见，算法准确提取了那些高信任度的角点，且运用灰度质心法估计了每个角点的方向。用BRIEF 描述子进行简单的二进制测试，产生一个位串来作为特征描述，从而有效提高了RGB-D SLAM 的实时性。

图5 ORB 特征提取

5.2.3 三维点云地图构建

为验证所采用的回环检测算法的有效性，要利用离线数据。选用的离线数据为Kinect 相机采集到的室内非结构化环境信息。以此来获取该实验环境下的全局三维点云地图，并对圆环检测算法添加前后的效果进行对比。

未经圆环检测的三维点云地图构建如图6 所示。图6(a)为未经回环检测的轨迹，其中以稀疏点云来显示路径，以稠密点云来构建地图，图中轨迹较为狭小，地图重建不完善；图6(b)为未经回环检测的三维地图构建效果，可以看到图中产生严重的位姿漂移，导致三维地图无法闭合创建。

图7 则为引入回环检测算法的三维点云地图构建。图7(a)为引入回环检测算法的轨迹，图中轨迹形成回路，地图重建完善；图7(b)为引入回环检测算法的三维地图构建效果，可以看到重建后的三维地图轮廓清晰、完整、光滑，与真实的实验环境基本一致，三维重建效果良好。

图6 未经圆环检测的三维点云地图构建

图7 引入回环检测算法的三维点云地图构建

6 结束语

针对室内环境提出了一种改进的移动机器人RGB-D SLAM 方法。该方法提取ORB 特征进行特征匹配，引入回环检测算法，显著减小了累积误差造成的位姿漂移，最终在实验场景中得到了全局一致的轨迹及三维地图。实验结果表明，运用该方法能够较好地抑制配准过程中的位姿漂移，创建出更为精确的室内环境三维地图。然而由于Kinect 相机拍摄范围有限，也可能会导致提取特征点不足从而影响位姿的估计，且本方法采用的全向轮小车的轮隙较大，在上端的深度相机会产生颠簸，影响画面感，导致特征点匹配不准，影响了位姿的估计，此类问题还需在后续的研究中通过进一步改进方法和设备来加以解决。