基于RGB-D的移动机器人三维视觉SLAM

，，

(华南理工大学 聚合物新型成型装备国家工程研究中心 聚合物成型加工工程教育部重点实验室,广州 510640)

基于RGB-D的移动机器人三维视觉SLAM

文生平，舒凯翔，洪华锋

(华南理工大学聚合物新型成型装备国家工程研究中心聚合物成型加工工程教育部重点实验室,广州510640)

针对移动机器人三维视觉SLAM中存在的实时性不高，匹配误差较大的问题，提出了一种改进的特征匹配算法，提取ORB特征进行特征匹配，并采用基于最小距离与RANSAC结合的方法剔除误匹配;针对位姿估计不准确的问题，将传统的ICP算法与PNP算法结合提高配准成功率，并通过RANSAC与优化位姿估计;实验结果表明该方法能有效提高SLAM系统的实时性和位姿估计的准确性。

移动机器人；Kinect相机；同时定位与地图构建；特征匹配；点云配准

0 引言

智能移动机器人要求能够在环境中自我导航与定位,而同步定位和建图(SLAM)被认为是实现移动机器人完全自主移动的关键。随着传感器技术的发展，基于二维激光传感器的自定位技术已经逐渐成熟，然而高精度的激光传感器价格昂贵，性价比很低。相对于二维激光传感器，相机具有更高性价比的同时能够获得更多的信息。2010年6月微软发布的Kinect为三维视觉SLAM提供了一种很好的思路。Kinect价格低廉，结构紧凑，具有一个红外相机与一个普通相机，分别用于采集深度(Depth)信息与彩色(RGB)信息，因而十分适合运用于三维SLAM领域。

传统的SLAM通常是基于滤波器求解，使用卡尔曼滤波，扩展卡尔曼滤波和粒子滤波等作为数学模型。如果在视觉SLAM中采用基于滤波器的方法，随着时间推移地图扩大，内存消耗和计算量都很大，难以用于大规模建图。基于图优化的SLAM把优化问题表现成图的形式，相机的位姿节点和路标点表示图的顶点，相机的运动模型和观测模型表示为图的边，用图模型来表示一个非线性最小二乘问题。图优化能够利用更多的信息，相对于滤波器方法，图优化在精度和效率上都有更好的表现。2010年Henry等人提出了一种室内三维V-SLAM方法[1]，其方法通过从Kinect获取的RGB图像中提取SIFT特征点，结合深度数据与迭代最近点算法(ICP)[2]估计出手持摄像机的位姿，同时建立环境的三维模型。虽然SIFT特征具有尺度不变性，但是带来了巨大地计算量，目前为止计算机还无法实时地计算SIFT特征。针对上述研究现状，本文提出了一种室内环境下移动机器人三维视觉SLAM方法。该方法利用Kinect获取图像信息，提取ORB特征点进行特征匹配[3]；针对图像特征匹配精度不高影响位姿估计精度地问题，利用随机采样一致性(RANSAC)与最小距离法相结合消除误匹配特征点；在位姿估计阶段，采用ICP与PNP算法结合提高配准成功率，并采用RANSAC与优化当前位姿。实验验证本文方法可以有效提高系统的实时性和位姿估计的准确性。

1 系统总体结构

本文提出的基于RGB-D图像的移动机器人三维视觉SLAM总体框架如图1所示，主要包括特征提取与匹配，点云的生成与配准，位姿优化3部分。

图1 基于RGB-D图像的移动机器人三维视觉SLAM总体框架

2 特征提取与匹配

视觉SLAM按是否提取特征点，分为特征点法与直接法。直接法无需提取特征点，省去了计算特征点、描述子的时间，对于特征缺失的场景有很好的效果。但其完全依靠梯度搜索且目标函数非凸，使得算法容易陷入极小值，只有在运动很小时才能成功。本文针对于较为一般的场景建图，因此采用鲁棒性更好的基于特征点SLAM。特征匹配常用的特征有SIFT，SURF等，然而这些特征虽然稳定，但是较大的计算量难以满足实时SLAM的需求。ORB特征是2011年提出的一种特征匹配算法，它是对FAST特征点与BREIF特征描述子的一种结合与改进，他改进了FAST检测子不具有方向性的问题，并采用速度极快的二进制描述子BRIEF，使整个图像特征提取的环节大大加速，对于实时SLAM来说是一个很好的选择。

判断基于二进制描述的特征点是否匹配，采用汉明距离来代替欧氏距离作为度量，两个特征点之间的汉明距离指的是两个二进制串对应位置不同字符的个数，计算过程简单，能有效缩短匹配所需的时间。

关于匹配算法的选择，由于视觉SLAM中特征点数量很大，为了有效提高匹配速度，采用快速近似最近邻(FLANN)作为匹配算法获得粗匹配[4]。但是因此将导致粗匹配中存在大量的误匹配，本文提出了一种基于最小匹配距离和RANSAC相结合的方法剔除误匹配。方法如下：首先根据场景特征点数量大小设置判断条件，当场景纹理丰富，特征点数量较多时，计算匹配点的最小匹配距离，根据工程经验选取汉明距离大于最小匹配距离两倍的匹配为误匹配，并通过RANSAC迭代获取内点，进一步去除误匹配；对于场景特征单调，容易引起歧义匹配的环境，为防止特征匹配过少引起匹配失败，只使用RANSAC去除误匹配。通过上述方法，能有效减少误匹配的数量。

3 点云生成与点云配准

3.1 Kinect相机标定

要获得三维点云的坐标需要知道相机的内参矩阵。本文采用了文献[5]的标定方法对Kinect的彩色相机与深度相机进行标定，在完成了Kinect的标定与校准之后，就可

以通过图中任意点的深度信息得到空间三维点云的坐标信息。

3.2 三维点云的生成

根据针孔相机的成像模型，给定一个图像点的像素坐标(u,v)和对应的深度值，对应的三维坐标可由如下计算公式得到：

(1)

(2)

(3)

式中，fx,fy分别为水平与垂直方向上的相机焦距；S为缩放系数；cx，cy为像素坐标系与成像平面的平移量。

以上参数皆可由相机标定得到。

3.3 点云配准

在点云配准阶段，对于RGBD-SLAM[6]来说通常采用ICP算法来估计位姿，但是考虑到Kinect相机精度限制导致特征点的深度可能测量不到，本文通过ICP与PNP结合的方式进行位姿估计。

3.3.1 ICP算法

通过特征提取与匹配环节，两帧图像间相同特征点的一一对应关系得以确立。在k时刻，当前相机的位姿可以表示为

(4)

Tk表示从k-1时刻到k时刻相机的位姿偏移量：

式中,Rcw为3x3旋转矩阵，tcw为3x1平移向量，cw表示从世界坐标系到相机坐标系。对于一组匹配好的3D点：

两帧图像间的欧式变换满足

(5)

要求出满足条件的R,t,通常采用ICP(Iterative Closest Point,迭代最近点)的方法来解决。

ICP求解通常用线性代数的方法，根据前面所述的ICP问题，定义第对i点的误差项：

(6)

然后构建最小二乘问题，求使误差平方和达到极小的R,t:

(7)

这个最小二乘问题可以通过SVD分解来求出。当特征匹配准确，深度信息准确的时候，根据参考文献[7-8]，可以通过SVD求出最优的R,t。

3.3.2 PNP算法

由于Kinect深度相机的测量范围限制，匹配特征点的深度信息有可能测量不到，这时可以通过3D-2D的投影关系来计算位姿。

P3P问题通过给定的3对3D-2D匹配点之间的几何关系计算出相机在世界坐标系中的位姿变化。方法原理如图2所示，已知A,B,C在世界坐标系下的坐标和对应点a,b,c在图像坐标系下的坐标，利用相似三角形原理和吴消元法，可以求得a,b,c三点在相机坐标系下的3D坐标，值得注意的是，这种方法会求出4个可能解，所以还需要一个验证点D来计算出最可能的解。通过4对3D-2D的匹配点对可以将问题转化为一个3D-3D的位姿估计问题，然后用ICP求解即可。

图2 P3P问题

因此，对于深度信息已知的特征点，通过ICP算法计算位姿；对于深度信息未知的特征点，通过P3P算法求出相机空间坐标，将3D-2D的问题转化为3D-3D的问题，然后再利用ICP求解。

3.4 位姿优化

点云配准环节系统存在的噪声会对位姿估计的结果产生很大的影响，这些噪声主要来自两方面：1.特征匹配环节仍然存在着误匹配；2.由于Kinect相机测量深度信息采用结构光原理，同时特征点往往处于物体边缘处，在这些位置上的深度信息通常误差比较大。这些噪声的存在使得ICP算法在迭代的过程中很容易陷入局部极值，对位姿估计的准确性带来很大的影响。本文在这里采用在剔除误匹配时采用的随机采样一致性方法来剔除噪声。

3.4.1 RANSAC优化位姿

通过RANSAC优化位姿的算法步骤如下：

1)首先从匹配点集中随机选取3个匹配对(满足ICP算法条件，当没有深度信息时改用P3P算法)，利用ICP算法估计变换矩阵T；

2)设置评价函数式(8)作为内点的筛选条件，为内点筛选阈值

(8)

3)当匹配点间距离小于则计算为一个内点，重复随机取点并统计内点的数量，迭代N次后，内点数量最多的T作为位姿估计。

3.4.2 图优化

对于空间点存在的测量误差，可以将空间点和位姿估计同时作为优化变量进一步优化。目标函数可以表示为：

(9)

这是一个非线性优化问题，可以采用g2o优化库来进行优化。将整个问题用图的形式来表示：

1)节点：所有特征点的空间位置p，相机的位姿节点ξ∈s;

2)边：每个空间点投影误差将点和边依次加入图中，使用RANSAC优化后得到的位姿估计作为初值，计算对应于当前估计值的雅可比矩阵和海塞矩阵，求解稀疏线性方程:

Hk△x=-bk

(5)

得到梯度方向,选用Levenberg-Marquardt不断迭代(LM算法)，最终整体误差稳定不变时算法收敛，即得到相机位姿的最优值。

4 实验结果及分析

4.1 实验平台介绍

本文实验采用的深度相机Kinect，彩色图像分辨率为640×480，深度图像分辨率为320×240，最高帧率为30帧每秒，水平方向视角。

本文实验采用的移动机器人平台为Turtlebot2，如图3所示，支持ROS操作系统，上面搭载一台笔记本电脑作为数据发送端，通过ROS系统采集、传输图像到远程数据处理端PC，处理端PC配备主频为2.5 GHZ的IntelCore i5处理器，运行Ubuntu 14.04操作系统，为了防止机器人移动太快导致特征点丢失，机器人以的线速度在室内环境移动建图。

图3实验装置

4.2 真实场景下的三维视觉SLAM

4.2.1 特征提取与特征匹配

图4对采集的图像分别采用ORB、SIFT和SURF提取特征，验证特征提取算法的效果。让机器人运行一段时间并用Kinect采集图像，按照时间顺序选取15张图像进行特征提取，计算平均特征点数目与平均耗时，结果如表1所示。从表中可知，ORB算法的运行速度远远优于SIFT算法与SURF算法，后续的特征匹配实验也可以证明，ORB算法在匹配准确率方面的表现也十分优异，所以对于实时性要求很高的三维SLAM来说是一个很好的选择。

图4特征提取算法效果示意图

表1 特征点检测结果对比

对相邻帧进行特征匹配，图5(a)是未经筛选的的匹配结果，图5(b)是经过最小匹配距离与RANSAC结合筛选后的匹配结果，由图可以看出该方法可以有效剔除误匹配，提高特征匹配的准确性。

图5 误匹配剔除效果示意图

4.2.2 三维地图的构建

机器人以0.2 m/s的速度在室内移动，位姿优化的最大迭代次数为50，建图流程如前所述：提取ORB特征，特征匹配与误匹配的剔除，位姿估计与位姿优化。实验中一般认为匹配的特征点对大于50为有效匹配，否则匹配点过少可能会引起位姿估计不准确。由于路面不平或机器人运动不平滑可能引起相机的抖动，使得前后两帧相差过大，我们认为这种情况为异常匹配，这时匹配点对较少，可以设置一个最小匹配阈值将其滤除，从而与下一帧继续匹配。另外，当目标帧与参考帧位移或转角大于一定阈值的时候将其设立为关键帧，这样可以避免将每一帧的数据都用来建图与更新数据所带来的巨大的计算量。

分别采用两种位姿估计方法对比建图：图6(a)为采用传统ICP算法估计位姿得到的三维点云地图，可以看出由于位姿估计不准确，图中存在较多冗余点，部分点云分布重叠，物体轮廓模糊。其位姿估计如图7(a)所示，X、Y、Z为空间三维坐标系，图中位姿曲线较不平稳，存在跳变，说明位姿估计的准确性不高。图6(b)为采用ICP与PNP结合的方式进行位姿估计，并通过两步位姿优化得到的三维点云地图，地图中冗余点大大减少，物体轮廓清晰。其位姿估计如图7(b)所示，此方法位姿曲线较为平稳，位姿估计更稳定。说明本文采用的位姿估计与优化方法能较好提高位姿估计的稳定性与准确性。

图6 三维建图结果

图7 位姿估计结果

5 结论

针对室内环境，提出了一种移动机器人三维视觉SLAM方法。该方法提取ORB特征进行特征匹配，提出了基于最小距离与RANSAC结合的方法剔除误匹配，提高了SLAM的实时性和特征匹配的准确性；在点云配准方面，将传统的ICP算法与PNP算法结合提高配准成功率，并通过RANSAC与迭代优化位姿估计。实验结果验证了方法的可行性与有效性，未来将以进一步提高位姿估计的精度与地图的全局一致性为主要的研究方向。

[1]Henry P, Krainin M, Herbst E, et al. RGB-D mapping: Using Kinect-style depth cameras for dense 3D modeling of indoor environments[J]. The International Journal of Robotics Research, 2012, 31(5): 647-663.

[2]Besl P J, Mckay N D. A Method for Registration of 3-D Shapes[A].Robotics - DL tentative[C]. International Society for Optics and Photonics, 1992:239-256.

[3]Rublee E, Rabaud V, Konolige K, et al. ORB: An efficient alternative to SIFT or SURF[A].International Conference on Computer Vision[C]. IEEE Computer Society, 2011:2564-2571.

[4]Muja M, Lowe D G. Fast approximate nearest neighbors with automatic algorithm configuration[J]. VISAPP (1), 2009, 2(331-340): 2.

[5]Smisek J, Jancosek M, Pajdla T. 3D with Kinect[A]. IEEE International Conference on Computer Vision Workshops[C]. IEEE, 2011:1154-1160.

[6]Endres F, Hess J, Sturm J, et al. 3-D mapping with an RGB-D camera[J]. IEEE Transactions on Robotics, 2014, 30(1): 177-187.

[7]Arun K S, Huang T S, Blostein S D. Least-squares fitting of two 3-D point sets[J]. IEEE Transactions on pattern analysis and machine intelligence, 1987 (5): 698-700.

[8]Pomerleau F, Colas F, Siegwart R. A review of point cloud registration algorithms for mobile robotics[J]. Foundations and Trends® in Robotics, 2015, 4(1): 1-104.

[9]高 翔，张涛等.视觉SLAM十四讲：从理论到实践[M].北京：电子工业出版社，2017.

ThreeDimensionalVisualSLAMforMobileRobotsBasedonRGB-D

Wen Shengping，Shu Kaixiang，Hong Huafeng

(National Engineering Research Center of Novel Equipment for Polymer Processing, Ministerial Key Laboratory of Polymer Processing Engineering, South China University of Technology, Guangzhou 510640, China)

To solve the problem of low real-time and large matching error in mobile robots 3D visual SLAM,an improved feature matching algorithm is proposed, which is based on the combination of the minimum distance and the RANSAC method to eliminate the false matching of the ORB feature.To solve the problem of inaccurate pose estimation, the ICP algorithm combined with PNP algorithmis is proposed to replace the traditional ICP algorithm, and the pose estimation is optimized by RANSAC.The experimental results show that this method can effectively improve the real-time and the accuracy of pose estimation in SLAM system

mobile robot; Kinect camera; SLAM; feature matching; point cloud registration

2017-05-05；

2017-06-07。

广州市科技计划项目(201508010058； 201604010079)。

文生平(1966-)，男，湖北荆州人，教授，硕士研究生导师，主要从事工业装备的智能控制与机器视觉方向的研究。

舒凯翔(1993-)，男，江西宜春人，硕士研究生，主要从事工业装备的智能控制与计算机视觉方向的研究。

1671-4598(2017)09-0246-04

10.16526/j.cnki.11-4762/tp.2017.09.063

TP24

A