移动机器人RGB-D视觉SLAM算法

陈 劭 郭宇翔 高天啸 宫清源 张军国

(1.北京林业大学工学院， 北京 100083； 2.林业装备与自动化国家林业和草原局重点实验室， 北京 100083)

0 引言

移动机器人涉及控制理论、人工智能技术、多传感器数据融合技术、电子信息与计算机技术等多学科交叉，在很多领域得到了广泛应用。移动机器人通常在无全局定位信息的未知环境中作业，如林地勘测、灾害救援等。如何在未知环境中利用自身携带的传感器，通过感知自身状态和周围环境完成自主导航是其面临的主要挑战之一。同步定位与地图构建技术可以获取精确的位置信息与地图环境信息，成为解决上述难题的重要手段与当前机器人领域的研究热点。

在同步定位与地图构建技术中，根据采用的传感器类型，可以分为激光雷达[1-2]与视觉采集设备[3-4]两种。微软Kinect、英特尔RealSense、华硕Xtion Pro Live等一系列RGB-D相机，可同时获得RGB图像及与之匹配的深度图像，节省了大量的计算时间，较好地满足了移动机器人视觉同步定位与地图构建(Visual simultaneous localization and mapping, SLAM)过程中对实时性的要求，且设备价格低廉，因而得到广泛应用[5-9]。

2012年，基于Kinect相机，NEWCOMBE等[10]提出了Kinect Fusion算法，借助GPU工具使算法速度达到30 Hz，满足了实时性处理要求，但对硬件设备要求较高且算法中没有闭环检测环节，算法精确度较低、鲁棒性较差。随后，HENRY等[11]利用随机抽样一致性(Random sample consensus, RANSAC)的方法获得相邻两帧图像之间的6D变换矩阵，再结合深度数据与迭代最近点(Iterative closest point, ICP)算法优化变换矩阵，获得相机位姿，然后利用稀疏捆绑调整算法对得到的机器人位姿进行优化，得到全局一致的地图。2014年，ENDRES等[12]在上述基础上开发出一套较为完善的RGB-D SLAM系统，将整个SLAM过程分为前端处理与后端优化两部分，在HENRY等的研究基础上使用非线性误差函数的框架G2O优化方法进行全局位姿图优化，生成全局彩色稠密点云地图。但该方法的定位精度及算法实时性均有待改善。

因此，针对移动机器人视觉SLAM算法中存在的精确度较低、实时性较差等问题，本文提出一种用于移动机器人的RGB-D视觉SLAM算法，对传统的RANSAC算法进行改进，将改进后的RE-RANSAC算法与GICP算法相结合来解决上述问题，并利用FR1数据集对本文算法的性能进行测试。

1 算法框架

本文提出的基于RGB-D数据的视觉SLAM算法的总体框架如图1所示。首先提取出RGB图像中的特征点并完成匹配，结合深度图像数据将2D平面特征点转换为3D点云，根据三维数据估计优化相机位姿，然后对图像进行筛选得到关键帧序列并完成帧间配准，再通过闭环检测及图优化得到全局最优相机位姿，最终经过点云拼接构建三维点云地图。

图1 基于RGB-D数据的实时视觉SLAM算法总体框架Fig.1 Overall framework of real-time visual SLAM algorithm based on RGB-D data

2 特征点提取与匹配

每帧RGB-D数据约有30万个点，若直接用整帧RGB-D数据进行定位及地图构建，会因数据量过大导致算法执行速度过慢，实用性降低。因此在获取RGB-D数据后，需提取RGB图像中的特征点，以减少数据量，并获得二维匹配点对，再结合深度数据，进而得到特征点相对于相机坐标系的三维坐标及三维匹配点对。

2.1 特征点的提取

特征点的提取包括特征检测与描述符提取，目前常用的特征点提取算法有SIFT、SURF、ORB 3种。尺度不变特征变换(Scale invariant feature transform, SIFT)算法[13]与加速鲁棒特征(Speeded up robust features, SURF)算法[14]计算量大、耗时长，无法满足移动机器人在定位建图过程中对实时性的要求。因此，本文采用基于ORB的特征检测与描述符提取方法[15]对特征点进行提取，结果如图2所示。该方法利用oFAST特征检测与rBRIEF描述符提取，可保证特征点的旋转不变性，而且运算速度快。

图2 ORB特征点提取结果Fig.2 Key feature points extraction results based on ORB algorithm

2.2 特征点的匹配

提取到相邻两帧图像的特征点后，通常采用暴力匹配 (BruteForce)方法进行匹配[12]，但当处理特征点数量较多或需匹配一帧图像和整张地图时，该方法匹配时间长，且误匹配较多，如图3a所示。针对上述问题，本文利用基于FLANN算法的双向KNN特征匹配方法以减少误匹配点，并采用多重随机k-d树方法，提高快速最近邻搜索的速度，特征匹配效果如图3b所示。

图3 BruteForce与本文方法匹配对比 Fig.3 Matching effect comparison between BruteForce and proposed algorithm

为进一步提高特征点的匹配准确度，需剔除匹配中的误匹配点。本文在RANSAC算法[16]的基础上进行改进，利用改进后的RE-RANSAC算法剔除误匹配点，算法原理如图4所示。

图4 RE-RANSAC算法原理图Fig.4 Schematic of RE-RANSAC algorithm

本算法基于迭代思想，在每次迭代过程中随机从三维坐标匹配点对中少量采样，估计得出变换模型，然后使用整个三维坐标匹配点对集合对该模型进行评估，得到满足该模型的局内点集合，根据局内点集合中的匹配点对重新求得新的变换模型。与原算法不同，RE-RANSAC算法在重新求得新的变换模型后，再次利用整个三维坐标匹配点对集合对该模型进行评估，获得新的局内点集合与变换模型，再与当前最优模型进行比较筛选。当随机采样k个点时，有

1-p=(1-uk)N

(1)

式中p——迭代N次后得到的采样集合中不包含局外点的概率

u——单次采样得到局内点的概率

令v=1-u代表单次采样得到局外点的概率，则迭代次数N为

(2)

在迭代结束得到最优变换模型、局内点集合及误差后，若局内点数目大于阈值，误差小于阈值，则利用该局内点集合再次计算运动变换模型，并使用上述方法获得最终的局内点集合。

改进后的RE-RANSAC算法在每一次迭代过程中均对运动模型进行二次评估筛选，以提高算法精确度，并通过减少迭代次数提高运算速度。RE-RANSAC算法剔除误匹配的效果如图5所示。

图5 RE-RANSAC算法剔除误匹配后的效果Fig.5 Effect after removal of mis-match by using RE-RANSAC algorithm

3 位姿估计优化与地图构建

完成相邻帧的匹配后，需通过相邻帧间的运动变换矩阵求解相机位姿并对其进行优化，进而得到全局最优相机位姿和相机运动轨迹，并经点云拼接，构建出三维点云地图。

3.1 位姿估计与优化

相机位姿估计与优化的过程是对相邻两帧图像间运动变换矩阵求解的过程。该矩阵由一个旋转矩阵R和一个三维平移向量(tx,ty,tz)组成

(3)

若已知点P=(x,y,z,1)和对应的运动变换矩阵T，则点P在运动变换矩阵T的投影P′为

P′=T×P

(4)

基于ICP算法[17]求解相邻两帧间的运动变换矩阵，从而得到相机的估计位姿是一种较为经典的方法。但当初始变化选取不当或匹配点对数量较多时，该算法的误差较大。因此本文提出先使用2.2节提出的改进后的RE-RANSAC算法迭代筛选获得最优局内点集合估计的运动变换矩阵T，将其作为位姿优化过程中的初始条件。并在匹配点数量较多时采用精度更高的GICP算法对估计的运动变换矩阵进行优化。

利用GICP算法[18]优化运动变换矩阵T，实质是通过概率模型进行求解。在概率性模型中，假设存在2个集合，={i}和={i}，若、集合完全对应，则存在运动变换T*使得

i=T*i(i=1,2,…,N)

(5)

依据概率模型

ai～N(i,Ci,A)

(6)

bi～N(i,Ci,B)

(7)

式中Ci,A——集合A待观测点的协方差矩阵

Ci,B——集合B待观测点的协方差矩阵

产生实际的点云集合A={ai}和B={bi}。

(8)

结合极大似然估计(MLE)可求得最佳运动变换T为

(9)

式(9)可以简化为

(10)

由此可得到优化后的运动变换矩阵T。

3.2 随机闭环检测

由于仅考虑了相邻帧间的运动变换，导致上一帧所产生的误差会传递到下一帧。经过多帧图像累积，累积误差最终使得计算出的轨迹出现严重偏移。本节通过增加闭环检测环节来减少累计误差。

在SLAM过程中，由于图像帧数较多，因此首先需通过筛选得到关键帧以减少算法处理时间。具体方法如下：设关键帧序列为Pi(i=0,1，…，N),且第1帧图像P0为首帧关键帧，每采集到一帧新的图像需计算其与序列Pi中最后一帧的运动变换，得到运动变换矩阵Ti(i=0,1，…，N),若该运动变换量符合所设定的阈值要求，则判定该帧为关键帧，并将该帧与Pi中的最后m个关键帧进行匹配，最后将该帧与从Pi中随机取出的n个关键帧进行匹配。若匹配成功，则可由此形成图6所示的局部回环或全局闭环，快速有效地减小累积误差。

图6 局部回环Fig.6 Local closed-loop

3.3 G2O位姿图优化

在位姿图中增加闭环约束后，为得到全局最优运动变换约束和全局最优相机位姿，采用G2O优化方法对整个位姿图进行优化。G2O优化方法[19]可将位姿图优化的问题转换为非线性最小二乘法问题，在已知运动变换约束与闭环约束情况下，求解相机位姿最优配置x*。其中目标函数F(x)定义为

(11)

(12)

式中xi、xj——第i时刻和第j时刻相机的位姿，即G2O图中的节点

C——时间序列集合

zij——位姿xi和xj之间的实际约束关系

Ωij——位姿xi和xj之间的信息矩阵，即两节点间的不确定性

e(xi,xj,zij)——2个节点组成的边所产生的向量误差函数

向量误差函数可表示为

e(xi,xj,zij)=zij-ij(xi,xj)

(13)

该误差函数表示xi和xj满足约束条件zij的程度，当xi和xj完全满足约束条件时，其值为0。

3.4 地图构建

在得到优化后的相机位姿及相机运动轨迹后，根据不同相机位姿下的观察结果生成不同的点云，将这些点云全部变换到全局相机位姿下进行累加点云拼接，即可构建当前时刻的三维点云地图。

然而，由于每帧点云数据量巨大，一帧分辨率为640像素×480像素的图像，有30多万个点，导致地图过于庞大，占用大量存储空间。因此本文将稠密的三维点云地图保存为基于八叉树的OctoMap。OctoMap[20]本质上是一种三维栅格地图，其基本组成单元是体素，可以通过改变体素的大小来调整该地图的分辨率。该类型地图可用于后续移动机器人的路径规划及导航。

4 实验结果与分析

4.1 实验平台

为验证算法的有效性，并保证实验的一致对比性，本文实验采用TUM标准数据集[21]中基于Kinect视觉传感器采集的室内基准数据包。该基准数据包中包含Kinect产生的彩色图像和深度图像序列，以及相机的真实运动位姿。本文算法运行在配置四核2.5 GHz主频、i7处理器的PC机上，使用Ubuntu14.04操作系统。实验程序中的RGB数据和深度数据均设定以30 f/s的速度同时从数据包中读取。

4.2 数据集实验结果分析

4.2.1特征提取对比

本文分别使用SIFT、SURF、ORB 3种算法提取FR1/xyz数据包中全部798帧图像的特征点，并对3种算法的特征点提取数目以及提取时间进行对比，结果如图7所示。图7中μ表示平均值。

图7 SIFT、SURF、ORB算法对比Fig.7 Comparison between SIFT, SURF and ORB algorithms

从图7可知，SIFT算法、SURF算法、ORB算法平均每帧图像提取的特征点数目分别为1 328、1 641、500个，所用时间分别为118.3、131.8、6.2 ms。在满足特征点匹配数目要求的情况下，ORB算法的运行速度比SIFT算法和SURF算法快2个数量级，可满足视觉SLAM的实时性要求。

4.2.2RANSAC算法改进对比

为验证RE-RANSAC算法可提高局内点比例，使得算法精度提高、运行时间减少，本节根据FR1/xyz、FR1/desk、FR1/desk2和FR1/plant 4个数据包对RANSAC算法与RE-RANSAC算法进行对比分析，对比结果如表1所示。

表1 RANSAC算法与RE-RANSAC算法对比Tab.1 Comparison between RANSAC algorithm and RE-RANSAC algorithm

从表1中可以看出，针对所测试的4个FR1数据包，改进后的RE-RANSAC算法的平均每帧图像的局内点比例分别为41.7%、43.4%、42.9%、39.8%，平均每帧图像的迭代次数分别为243、231、257、389次，平均每帧图像的处理时间分别为0.005 3、0.005 2、0.005 4、0.007 8 s。经对比可知，改进后的RE-RANSAC算法较RANSAC算法提高了局内点比例，使得算法迭代次数减少，提升了算法的计算速度。

4.2.3相机运动轨迹估计

为使估计轨迹与真实轨迹的对比更加直观，实验给出了在本文算法下TUM数据集中FR1/desk、FR1/desk2、FR1/xyz、FR1/plant 4个数据包的相机真实轨迹及估计轨迹在XY平面上的投影，如图8所示。

图8 真实轨迹与估计轨迹比较Fig.8 Comparison between real trajectory and estimated trajectory

4.2.4定位准确性及实时性对比

为验证本文算法定位准确性及实时性，实验计算出真实位姿与利用本文方法得到的估计位姿之间的均方根误差(Root mean square error, RMSE)及平均每帧数据的处理时间，并将本文方法获得的RMSE值及算法处理时间与文献[12]中提出的RGB-D SLAM方法获得的数值进行对比，对比结果如表2所示。

从表2可以看出，针对所测试的4个FR1数据包，本文方法定位精度及处理速度均优于RGB-D SLAM。经计算，本文方法的平均RMSE约为0.024 5 m，平均每帧数据处理用时约为0.032 s，与RGB-D SLAM方法相比，分别降低了40%和90%。

表2 本文方法与RGB-D SLAM方法的性能对比Tab.2 Comparison between proposed algorithm and RGB-D SLAM algorithm

4.3 地图构建评估

以FR1/plant数据包和实验室楼道实际场景建图结果为例，说明本文算法构建地图的准确性。

4.3.1FR1/plant数据包建图

在FR1/plant数据包中，RGB-D相机围绕花盆旋转，从各个角度拍摄花盆，构建好稠密点云地图后将其保存为OctoMap形式，分辨率为2 cm，如图9所示。从三维地图的不同角度可发现花盆、桌子、黑板等物体均得到了较好的重建。

图9 FR1/plant 3D地图Fig.9 FR1/plant 3D maps

4.3.2实际场景建图

为验证本文算法在实际环境中效果，利用Kinect实时采集得到的实际场景与本文算法得到的3D地图进行对比，并对视觉传感器轨迹进行估计。

图10 实验室楼道及其3D地图Fig.10 Laboratory corridor and its 3D map

实验室楼道实际场景如图10a所示，共有两部分，左侧图像为纵向楼道，右侧图像为横向楼道。实验过程中，将Kinect传感器放置于移动机器人上，控制移动机器人从纵向楼道粉点处以直线运动至楼道岔口处，右转至横向楼道，转弯路径近似为半径0.6 m的圆弧曲线，随后行至横向楼道蓝点处并完成180°的转向，返回至楼道岔路口，最后再行至纵向楼道尽头。在此过程中平均移动速度为0.3 m/s。

根据本文算法所构建的实验室楼道3D地图如图10b所示，可以看出，3D地图中的地板、墙壁及直角拐弯处等均得到了较好地还原。移动机器人的位置轨迹如图11所示。实验过程中，移动机器人在楼道拐角处的转向角为90°，在纵向楼道和横向楼道中的实际直线位移距离分别为7.70、3.00 m。本文算法得到的转向角为92.2°，角度误差为2.4%，在纵向、横向定位结果中移动机器人的位移分别为7.84、3.05 m，纵向、横向位移定位误差分别为1.8%、1.7%。

图11 移动机器人轨迹Fig.11 Trajectory of mobile robot

5 结束语

提出了一种用于移动机器人的RGB-D视觉SLAM算法，该算法采用ORB算法提取特征点，随后提出一种改进的RE-RANSAC算法，通过与GICP算法的结合获得了最优相机位姿，最后利用随机闭环检测及G2O图优化方法减小了累积误差，并拼接构建了三维点云地图。通过实验对比SIFT、SURF、ORB 3种算法提取的特征点数目以及提取时间，说明本文采用的ORB算法在处理速度上优势明显。通过对FR1数据集的测试，对比了本文算法与RGB-D SLAM算法，验证了本文算法定位准确性及快速性，并以FR1/plant数据包和实验室楼道实际场景建图效果为例说明了算法建图的准确性。本文算法的最小定位误差为0.011 m，平均定位误差为0.024 5 m，平均每帧数据处理时间为0.032 s。