基于RGB-D图像的视觉SLAM算法研究

孟特 李富才 刘邦彦 张凤生

摘要：针对主流的RGB-D SLAM系统精度较低并且仅生成稀疏点云地图的问题，提出一种改进的SLAM系统。前端采用改进的ORB特征提取算法，改进特征点簇集的问题;后端综合利用EPnP与ICP算法进行相机位姿优化，提高位姿估计精度;并增加稠密点云地图构建线程，得到场景的稠密点云地图，以用于机器人的导航与路径规划。在TUM数据集上，使用Kinect V2相机将改进的SLAM算法与ORB-SLAM2算法进行实验比对，验证了改进SLAM算法的综合性能优于ORB-SLAM2算法。

关键词：RGB-D SLAM;Kinect V2;EPnP;稠密点云地图

中图分类号：TP391        文献标志码：A

视觉SLAM可从环境中获取海量的、富于冗余的纹理信息，拥有超强的场景辨识能力。早期的视觉SLAM基于滤波理论，其非线性的误差模型和巨大的计算量限制了它的实用性[1-2]。近年来，随着具有稀疏性的非线性优化理论及深度相机技术、计算机性能与算法的进步，视觉SLAM得到了越来越广泛的研究及应用[3-7]。视觉SLAM根据相机不同可分为单目SLAM（Monocular SLAM）、双目SLAM（Binocular SLAM）和深度相机SLAM（RGB-D SLAM）三类，其中，RGB-D SLAM可直接获得深度信息，得到了越来越多的应用。Endres等[8]最早提出了一套完整的RGB-D SLAM系统，能够在较长轨迹和复杂环境中追踪机器人位姿，具有较好的鲁棒性和准确性。而ORB-SLAM系统只提供了单目接口[9]。ORB-SLAM2系统提供了单目、双目和深度相机接口，全程使用ORB（Oriented FAST and Rotated BRIEF）特征[10]，相比于尺度不变特性（Scale Invariant Feature Transform， SIFT）算法[11]和加速稳健性特性（Speed-up Robust Feature Transform， SURF）算法[12]，在保证具备旋转和尺度不变性的基础上又具有良好的实时性，成为目前视觉SLAM的主流方案[13-15]。但该系统使用EPnP（Effective Perspective-n-Point）算法进行相机位姿估计，存在较大误差;而且仅构建出了稀疏点云地图，无法直接用于导航与路径规划。本文的研究旨在解决上述问题。

1 视觉SLAM系统框架

ORB-SLAM2系统具有跟踪、局部建图和回环检测三个线程，在此基础上，本文的SLAM系统增加了稠密点云地图构建线程，系统框架如图1所示。跟踪线程采用ORB进行特征点提取，特征匹配时采用随机采样一致性（RANSAC）算法[16]，提高匹配的准确性，并利用PnP（Perspective-n-Point）算法对匹配成功的地图点和彩色图像进行位姿估计和优化[17-18]，计算出关键帧点云地图;在局部建图线程中通过对关键帧的处理生成稀疏点云地图。回环检测线程进行地图的检测，减少累积误差，优化地图;稠密地图构建线程完成稀疏点云地图到稠密点云地图的构建。

2 RGB-D SLAM算法的改进

针对目前的RGB-D SLAM系统精度较低，且只能生成稀疏点云地图的问题，本文利用RGB-D深度相机所获得的深度灰度图，基于四叉树（Q_tree）算法改进ORB特征提取，以减少冗余特征点;综合应用EPnP加ICP（Iterative Closest Point）的方法求解相机位姿，既保证求解的快速性，又因为引入3D-3D的ICP算法，提高求解的精度;此外，增加稠密点云地图的构建。

2.1 基于四叉树（Q_tree）算法改进ORB特征提取

ORB特征检测算法由FAST（Features from Accelerated Segment Test）特征点检测和BRIEF（Binary Robust Independent Elementary Feature）描述子构成。FAST是一种特征点检测算法，FAST角点定义为：若某像素与其周围邻域内足够多的像素点相差较大，则该像素可能是角点。以某一像素点P为圆心，以3个像素单位为半径的圆上16个像素点与像素点P的差来进行角点提取。若圆周上有连续N个像素点的亮度大于Ip+T，或者小于Ip-T，则将该圆心设为候选角点。其中，Ip为圆心P的亮度值，T为阈值，一般为Ip的20%，N一般取值为12。

由于不涉及尺度，梯度等复杂运算，FAST检测器速度非常快，缺点是不具有方向性和尺度不变性。为了克服这一缺点，ORB算法通过构建图像金字塔来实现尺度不变性，而旋转不变性通过灰度质心法[14]来实现。所谓质心是指以图像块的灰度值作为权重的中心，质心的计算步骤：

Step 1 在一个小的图像块B中，定义图像的矩为：mpq=∑x，y∈BxpyqIx，y，p，q=0，1;

Step 2 通过矩找到B的质心C为：C=m10m00，m01m00;

Step 3 连接B的几何中心O和质心C，得方向向量OC，故特征点的方向定义为：θ=tan-1（m01/m10）。

图2为Q_tree区域分割和数据存储的结构图。Q_tree算法对提取到的FAST角點进行均匀的分割，方便后期的特征点匹配[15]，该算法改变了特征点的存储方式，在保留了ORB提取算法快速性的同时改善了特征点簇集和冗余的问题。算法步骤如下：

Step 1 初始化父节点O，将图像均匀分割为四个子区域（A，B，C，D），得到初始化的Q_tree结构;

Step 2 按照每个子区域的角点数量进行判断，若角点数量小于或等于1，则停止划分，否则继续均匀的四分该子区域;

Step 3 重复Step 2，直到递归出的子区域存在的角点数小于或等于1，或达到期望提取的角点数;对于角点簇集的情况，通过非极大值抑制条件，只保留Harris响应值最大的较优角点，去除其他冗余点。

2.2 相机位姿估计

RGB-D相机可直接获得环境的深度信息，可以根据特征匹配结果进行相机位姿估计。通常有两种情况，一种是3D-2D问题，即已知环境的3D点与其在相机中的2D点投影来求解位姿;另一种为3D-3D问题，即用已经匹配好的两组3D点进行位姿求解[19]。在SLAM问题中，常用EPnP算法来解决第一种问题，用ICP算法来解决第二种问题。但第一种情况没有有效利用更多的匹配点信息，而ICP算法是通过在待匹配的目标点云M和源点云Q中，按一定的约束条件找到最邻近点pi，qi，故该算法非常依赖待匹配目标点云初始的旋转向量R与平移向量t，否则容易陷入局部最优迭代。为此，本文采用EPnP算法与ICP算法相结合，利用EPnP算法获得目标点云较好的初值，由ICP算法有效利用匹配点信息。EPnP算法是利用四个非共面的控制点的加权和来组成新的坐标系，从而可用世界坐标系或相机坐标系下的四个控制点来表示世界坐标系或相机坐标系下的所有3D点。

假设匹配点在世界坐标系下的坐标为pwi，i=1，2，…，n，在相机坐标系下的坐标为pci，i=1，2，…，n;同时，每个控制点的坐标为ccj=xci，yci，zciT（j=1，2，3，4），则在世界坐标系下，匹配点与控制点的关系为

pwi=∑4j=1aijcwj，∑4j=1aij=1（1）

根据相机投影模型，有

siuivi1=Kpci=K∑4j=1aijccj（2）

其中，si为尺度因子，uiviT表示pci对应的2D投影坐标，K为相机内参矩阵，通过相机标定获得。

将式（1）带入式（2），矩阵形式为

Mx=0（3）

其中，待求量x是一个12维向量，代表控制点的坐标;M是2n×12的矩阵，其系数通过计算匹配点获取。

满足式（3）的x所有解的集合成为M的零空间，存在以下关系

x=∑Ni=1βivi（4）

其中，vi通过计算MTM的特征向量来求得，βi的求解与N的取值有关，N一般取1～4。

求解出βi后，根据式（4）可得到x，再将x带入式（1）就可计算出匹配点在相机坐标系下的3D点。

接下来就变成了3D-3D问题，利用ICP算法进行求解。

（1）分别计算匹配点在世界坐标系和相机坐标系下的质心

pwo=1n∑4i=1pwi， pco=1n∑4i=1pci

（2）计算pwi的去质心pwo后的矩阵A，计算pci的去质心pco后的矩阵B

A=pwTo-pwTopwTnpwTn B=pcTo-pcTopcTnpcTn

（3）计算本质矩阵H：H=BTA;

（4）计算H的SVD分解

H=U∑VT（5）

其中，U，V为正交阵;

（5）计算位姿的旋转R;

（6）计算位姿的平移t;

至此，可求得相机的估计位姿R，t。

2.3 地图构建

在ORB-SLAM2系统中，通过Q_tree算法提取图像均匀的特征点，采用快速近似最近邻算法（FLANN）进行特征点的匹配，并使用随机抽样一致算法（random sample consensus， RANSAC）剔除误匹配。通过局部BA优化选取关键帧构建出局部地图，并由PCL点云库构建出稀疏点云地图[20-23]，此法虽然可用于机器人的定位，但却很难实现机器人的导航和路径规划。为克服这一不足，本文基于RGB-D图像信息进行了稠密点云地图的构建，使之可以用于机器人的导航与路径规划[24]。

3 RGB-D SLAM实验与结果分析

为验证本文对RGB-D SLAM算法的改进效果，应用本文的算法在德国慕尼黑工业大学发布的用于RGB-D SLAM实验的开源TUM数据集进行处理，选用的3个数据集序列分别为：fr1_desk、fr1_desk2、fr2_360，并利用SLAM算法评测工具EVO从特征点检测、轨迹输出和系统精度3个方面评价算法的效果。

实验环境：操作系统为64位Ubuntu 18.04.5 LTS，CPU为Intel core i5-8250u CPU@1.60GHZ * 8，内存为8GB;视觉传感器为微软公司的Kinect V2相机。实验前，在ROS melodic环境下采用张正友标定法，使用9×11棋盘格（每格20 mm×20 mm），对相机进行标定，得到相机的内参矩阵K和畸变参数。

3.1 Q_tree特征点提取算法验证

为验证特征点提取的Q_tree算法，利用Kinect V2相机采集实验室真实环境的RGB图片，分别利用Q_tree算法和ORB算法对同一帧图片提取特征点，结果如图3所示。可见，ORB特征点有明显的扎堆、簇集现象，Q_tree算法实现的ORB关键点分布均匀，更利于之后的特征匹配。

图3 角点对比图

（a）Q_tree算法提取結果;（b） ORB算法提取结果

3.2 轨迹验证

选用TUM数据集中的fr1_desk、fr1_desk2进行相机真实运行轨迹与估计轨迹的验证，并用真实数据集进行轨迹的跟踪，实验结果如图4、图5所示，实线为相机的位姿估计轨迹，虚线为真实轨迹。可以看出在两个不同的数据集中估计轨迹与真实轨迹的偏移量都非常小，估计轨迹与真实轨迹整体方向是一致的。

在自建的真实环境数据集中的运行结果如图6所示。尽管由于缺少设备追踪真实的轨迹，无法比较轨迹的效果，但从图中可以看出，本文的算法在实际环境中可以正常运行并产生自身估计的位姿轨迹。

3.3 精度对比

绝对位姿误差（APE）描述的是系统每一帧相机的真实位姿和估计位姿的差值，可以直观的显示系统的精度[25]，利用均方根误差来衡量系统的精度。本文的SLAM系统与ORB-SLAM2系统的精度对比见表1。在fr1_desk、fr1_desk2和fr2_360三个序列上，本文系统的均方根误差均比ORB-SLAM2系统的小，说明本系统在不同环境中的稳定性要优于原ORB-SLAM2系统，精度更好。

3.4 稠密地图构建

本文算法的应用场景为室内的静态场景，所以稠密地图构建实验选用的数据集为fr1与fr2序列，并在真实环境中进行验证，实验结果如图7、图8所示。可以看出，本文算法可以在完整保留数据集信息的情况下构建出稠密点云地图。图9是在实际环境中初始化和建图的情况。实验过程中，算法运行顺畅、稳定，能够正确构建出实际环境的稠密点云地图，且能够保留完整的信息，用于机器人的路径规划。

4 结论

通过对RGB-D SLAM算法的改进与实验研究，可知，RGB-D图像特征提取采用Q_tree算法，与ORB算法相比，所提取的特征点更均匀，减少了特征点的冗余和簇集;将EPnP与ICP算法相结合进行相机位姿估计的优化，提高了位姿估计的准确性;利用PCL点云库进行稀疏点云的拼接，构建了能够用于机器人导航的稠密点云地图。进一步的研究是利用八叉樹结构，将稠密点云地图压缩成八叉树地图，剔除与导航无关的冗余特征，提高算法的实时性;并研究如何生成二维栅格地图，以用于AGV的路径规划。

参考文献

[1]DISSANYAKE G， WILLIAMS S B， DURRANT-WHYTE H， et al. Map management for efficient simultaneous localization and mapping （SLAM）[J]. Autonomous Robots， 2002， 12（3）：267-286.

[2]贾棋，高新凯，罗钟铉，等.基于几何关系约束的特征点匹配算法[J].计算机辅助设计与图形学学报，2015，27（8）：1388-1397.

[3]NAMITHA N， VAITHEESWARAN S M， JAYASREE V K， et al. Point cloud mapping measurements using kinect RGB-D sensor and kinect fusion for visual odometry[C]//12th International Cenference on Communication Networks（ICCN），Bangalore， 2016：209-212.

[4]YU， H Y， ZHU H J， HUANG F R. Visual simultaneous localization and mapping（SLAM） based on blurred image detection[J]. Journal of Intelligent & Robotic Systems，2021，103（1）：12.

[5]李亚宾.基于RGB-D数据的移动机器人室内定位与建图研究[D]. 哈尔滨：哈尔滨工程大学， 2016.

[6]朱东林，徐光辉，周捷，等. 视觉同时定位与建图方法综述[J].通信技术，2021，54（3）：523-533.

[7]ZHOU F， ZHANG L M， DENG C L， et al. Improved point-line feature based visual SLAM method for complex environments[J]. Sensors，2021，21（13）： 4604.

[8]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.

[9]MUR-ARTLA R， MONTIEL J M M， TARDOS J D. ORB-SLAM： A versatile and accurate monocular SLAM system[J]. IEEE Transactions on Robotics， 2015， 31（5）：1147-1163.

[10] CHEN S B， ZHOU B D， JIANG C H， et al. A LiDAR/visual SLAM backend with loop closure detection and graph optimization[J]. Remote Sensing，2021，13（14）： 2720.

[11] MUR-ARTAL R， TARDOS J D. ORB-SLAM2： An open-source SLAM system for monocular， stereo and RGB-D cameras[J]. IEEE Transactions on Robotics， 2017， 33（5）：1255-1262.

[12] HUANG H Y， YE H Y， SUN Y X， et al. Incorporating learnt local and global embeddings into monocular visual SLAM[J]. Autonomous Robots， 2021，45（6）： 789-803.

[13] RUBLEE E， RABAUD V， KONOLIGE K， et al. ORB： An efficient alternative to SIFT or SURF[C]//IEEE International Conference on Computer Vision， ICCV， Barcelona， 2011： 2564-2571.

[14] LOWE D G. Distinctive image feature from scale-invariant key points[J]. International Journal of Computer Vision， 2004， 60（2）：91-110.

[15] BAY H，TUYTELAARS T，VANGOOL L. SURF： Speeded-up robust features[C]// 9th European Conference on Computer Vision （ECCV 2006）， Graz， 2006：404-417.

[16] ZHONG Q B， FANG X Y. A bigbigan-based loop closure detection algorithm for indoor visual SLAM[J]. Journal of Electrical and Computer Engineering，2021，2021： 9978022.

[17] FISCHLER M A， BOLLES R C. Random sample consensus： A paradigm for model fitting with applications to image analysis and automated cartography[J]. Communications of the ACM， 1981， 24（6）：381-395.

[18] 董靜薇，蒋燚铭，韩知雨. RGB-D SLAM前端视觉里程计的优化算法[J]. 哈尔滨理工大学学报， 2020，25（6）：157-164.

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

[20] 伍锡如，黄榆媛，王耀南. 改进ORB特征的机器人RGB-D SLAM算法[J]. 计算机工程与应用， 2020（14）：35-44.

[21] 房德国，王伟，李自然，等.VIO-SLAM综述[J].电光与控制，2020，27（12）：58-62+100.

[22] 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.

[23] 赵宏，刘向东，杨永娟. 基于RGB-D图像的室内机器人同时定位与地图构建[J]. 计算机应用， 2020， 40（12）：3637-3643.

[24] 林辉灿，吕强，王国胜，等. 基于VSLAM的自主移动机器人三维同时定位与地图构建[J]. 计算机应用， 2017（10）：2884-2887+2894.

[25] 杨世强，白乐乐，赵成，等.一种改进的Qtree_ORB算法[J].计算机工程与应用，2021，57（20）：82-89.

Research on Visual SLAM Algorithm Based on RGB-D Image

MENG Te1， LI Fu-cai2，LIU Bang-yan1， ZHANG Feng-sheng1

（1. School of Mechanical and Electrical Engineering， Qingdao University， Qingdao 266071， China;

2.Qingdao Institute of Aeronautical Technology，Qingdao 266400， China）

Abstract：

Aiming at the problem that the mainstream RGB-D SLAM system has low accuracy and only generates sparse point cloud map， an improved slam system is proposed. The frontend uses improved ORB feature extraction algorithm to improve the problem of feature point extraction cluster. The back-end uses EPNP and ICP algorithm to optimize the camera pose and improve the pose estimation accuracy. And adding thread of constructing dense point cloud map to get dense point cloud map of scene for navigation and path planning of robot. On TUM data set， Kinect V2 camera is used to compare the improved SLAM algorithm with ORB-SLAM2 algorithm. The result shows that the comprehensive performance of SLAM algorithm is better than that of ORB-SLAM2 algorithm.

Keywords：

RGB-D SLAM; Kinect V2; EPnP; dense point cloud map

收稿日期：2021-09-06

通信作者：张凤生，博士，教授，主要研究方向为机电系统设计与控制、机器视觉技术应用。E-mail： fszhang1994@163.com

2519501186272