基于RGB-D相机数据的SLAM算法

洪 亮 ，冯 常

（1.中国科学院光电技术研究所四川省成都市610209；2.中国科学院大学北京100049）

同步定位与建图，又名Simultaneous Localiza⁃tion and Mapping（SLAM），是机器人导航、无人驾驶和AR等技术的基础。从1986年提出以来，已经发展了30余年，目前已经形成了基于滤波的方法和基于优化的方法两种主流思路。早期SLAM主要使用激光雷达作为传感器[1]，但价格昂贵，近几年，用视觉传感器实现的SLAM方案展现了很好的效果，2010年，微软推出一款廉价RGB-D相机Kinect，RGB-D相机通过同时获得彩色图和对应的深度图[2-3]，使得稠密点云地图的构建更加容易实现。

2012年，Henry等人[4]首先将Kinect用于室内稠密地图重建，2016年，Mut-Artal等人开源的ORB_SLAM2方案支持RGB-D相机，但是只能构建稀疏的地图。KinectFusion和ElasticFusion方案虽然能构建三维稠密的地图，但平台需要配置GPU来实现[5]，Endres的方案虽然不需要使用GPU设备，但重建效率不高[6]。基于以上的问题，本文提出一种成本低廉、基于图优化框架的SLAM方案，通过使用RGB-D相机数据，匹配得到较为精确的相机位姿估计结果，实现室内环境的三维稠密点云地图的快速构建，且对电脑的配置要求不高。

1 算法方案

早期SLAM主要使用基于滤波器的方法，近些年，基于优化的方法展现出优势[7]，本文采用基于图优化的视觉SLAM框架，系统可以分为3个部分：前端、后端和建图[8]。

在前端，机器人通过携带的RGB-D相机采集室内环境的数据作为输入，即连续的RGB图和对应的深度图序列。以相机采集第一幅图像时的位置为原点，建立世界坐标系。为了得到整个环境的三维图，需要将图像序列关联起来，即建立连续图像之间的位姿变换关系。在RGB图中提取均匀化的ORB特征，对连续的两幅图进行特征匹配，并用随机采样一致性（RANSAC）方法优化匹配结果，将优化后的匹配点对作为初值，用PnP求解两幅图的位姿变换矩阵，并用Bundle Adjustment优化两帧之间的位姿变换矩阵。

由于相机在采集数据时帧率大概在30Hz[9]，及两幅连续帧之间变化一般不大，所以并不是每一帧图像都会对三维地图的构建有作用，冗余帧会降低系统的效率，通过设置关键帧选取机制去掉冗余帧。得到所有连续关键帧之间的位姿变换矩阵后，送入后端进行优化。

在后端，采用位姿图优化的方式，以ORB特征为路标点，将机器人在世界坐标系中的位置作为优化变量，即图优化中的点，将帧与帧的匹配得出的两个位姿之间的位姿变换矩阵作为边的初始值，使用g2o工具对其进行优化，得到最终的机器人全局位姿估计。

在建图模块，将关键帧对应的深度图生成点云图，并通过优化后的位姿变换矩阵将当前帧的点云图变换到世界坐标系中，实现室内环境三维稠密点云地图的构建。

本文采用TUM的数据集[10]，最终快速构建了全局一致的三维稠密点云图，该地图很好地反应了室内环境的情况。

算法流程图如图1所示。

图1 算法流程图

2 相机跟踪

2.1 ORB特征提取

常用的特征提取和描述的方法有SIFT、SURF、Shi-Tomasi和ORB[11]等。SIFT和SURF算法的提取效率不高，Shi-Tomasi是Harris算法的改进，不具有尺度不变性。

文中采用的ORB（Oriented FAST and Rotated BRIEF）特征是Ethan Rublee在2011年提出的一种新的特征检测与描述算法[12]，并在OpenCV中实现，相对于SIFT和SURF，ORB具有更快的速度。ORB以改进的FAST角点为关键点，以BRIEF为描述子，该描述子特有的2进制串的表现形式不仅节约了存储空间，同时大大缩短了匹配的时间，这使得ORB的计算速度非常快。但原始的ORB特征算法有集中分布的特点，为了解决这一问题，提高匹配精度，本文采用ORB_SLAM2中均匀化的方法，将均匀化后的ORB作为特征。

本方案采用暴力匹配的方法来实现两幅图像间的特征匹配，基于BRIEF描述子的特点，使用汉明距离来度量两个特征点的相似程度。同时，采用随机采样一致性（RANSAC）算法去除误匹配，该算法对噪声大的数据具有良好的鲁棒性。

2.2 位姿估计

2.2.1 针孔相机模型

相机将环境中的三维点映射到二维图像平面通常采用针孔相机模型来表示。若RGB图像中某像素点坐标为p（u，v），深度值为d，该点在相机坐标系下的坐标为：

图2 针孔相机模型

其中，fx和fy指相机在x、y两个轴上的焦距，cx和cy指相机的光圈中心，s为深度缩放因子，这些构成了相机的内参，可以通过相机标定来获得。

P（X，Y，Z）点的世界坐标Pw需要根据相机当前位姿来求：

其中K为相机的内参矩阵，由式（1）中的内参组成，相机的位姿R和t即为相机的外参，外参表示了相机的轨迹和位置，通过外参可以得到两帧之间的变换矩阵T，精确的位姿求解算法是实现机器人定位的关键。

文中通过求出当前帧与上一帧之间的位姿变换矩阵，再与之前的变换矩阵作连乘，即可以得到机器人此刻在世界坐标系的位置和姿态。

2.2.2 PnP求解与优化

完成两幅图像的匹配后，接下来可以求解相机的位姿变换矩阵，由于本文采用RGB-D相机，每一幅RGB图都有一幅与之对应的深度图，所以2D-2D、3D-2D、3D-3D这3种方法均能求解。文献[13]指出，2D-2D的对极几何方法存在初始化和尺度的问题，而3D到2D运动估计比3D到3D的方法更精确，文中采用 PnP（Perspective-n-Point）算法来求解3D-2D问题，得到两帧的初始位姿估计。

为了得到更精确的位姿，将得到的初始位姿作为初值，用非线性优化的方法求取优化的位姿变换矩阵，这是一个两帧图像之间的Bundle Adjustment问题，即最小化重投影误差问题[8]，通过最小化公式（4）来求。

其中Pi为空间点的坐标，ui为该点对应的像素坐标，相机初始位姿的李代数为ξ，通过迭代优化来得到优化后的相机位姿。

2.3 关键帧选择机制

在求解位姿的同时，系统启动关键帧选取机制：

1）由于相机快速运动，部分图像存在模糊的问题，为了保证关键帧图像质量较高，需要在两两匹配时设置最小内点阈值；

2）由于相机采集数据的频率较高，相邻图像之间的位姿变换较小，设置每两个连续关键帧之间最小间隔5帧；

3）设置一个相机的运动范围，避免过大或过小。

通过关键帧选择机制，使计算效率大大提高，在保证了全局地图质量的情况下，同时减轻了后端优化的计算压力。

3 后端位姿图优化

相机跟踪部分又称为视觉里程计，由于两帧之间的位姿估计存在一定的误差，而且误差会累积，所以整个视觉里程计会存在不同程度的漂移。在一个局部范围内，视觉里程计能够满足定位和建图精度，但是如果要建立较大范围全局一致的三维环境地图模型，需要引入后端优化机制，来减小视觉里程计的累积误差。

早期优化采用滤波的方法，通过建立机器人的运动方程和观测方程，优化位姿和路标[8]。

uk为k时刻运动数据，xk为k时刻机器人位置，zk,j为观测数据，yj为路标点，wk和vk,j都为噪声数据。通过估计xk和yj来解决机器人定位和建图问题。

目前，基于优化的方法展现出一定的优势[14]。后端优化的本质是一个状态估计问题。本文的SLAM系统只有视觉传感器，没有IMU等惯性测量单元，所以只有观测方程，没有运动方程。

本方案在后端采用位姿图优化的方法，图中的路标点（特征点）为固定点，将相机位姿作为待优化的变量，即图优化中的节点，通过帧与帧的匹配得出两个位姿之间的相对运动估计，作为边的初始值，从而转化成图优化的问题。文中使用g2o优化工具箱，采用L-M方法进行迭代优化[15]。

图3 位姿图优化

4 地图构建

早期基于激光雷达的方案主要建立的是环境的二维地图模型，Velodyne公司推出的三维激光雷达虽然能建立环境的三维点云图，但是价格昂贵。基于视觉的方案不仅成本低廉，且能恢复环境的原貌[16]。

目前，视觉方案建立的三维地图模型可以分为3种，第一种为稀疏地图，如ORB_SLAM[17]，第二种为半稠密地图，如LSD_SLAM[18]，第三种为稠密地图，如RGBD SLAM[11]。

文中采用三维稠密点云地图来重建室内环境，该地图模型能很好的表示环境情况。

5 实验分析

5.1 特征匹配

图4为初始匹配结果，筛选前大概有500组匹配，但存在很多错误匹配的情况，图5为使用RANSAC去除了外点后的ORB特征匹配情况，筛选后约有50组匹配，同时匹配也比较准确。

5.2 方案效率

本文将从运算效率上与文献[6]提出的RGB-D SLAM进行对比，计算平均每帧的处理时间，本实验采用四核i5、内存为4G的电脑，RGB-D SLAM运行在一个四核8G内存的电脑上，对比结果如表1所示。

图4 初始匹配结果

图5 优化后匹配结果

表1 本文方案与RGBDSLAM方案对比

文中采用TUM的两个数据集进行测试，与文献[6]中的数据进行对比，从表1可以看出，文中方案处理速度约为RGB-D SLAM方案的2倍，能快速实现三维室内环境的重建。

5.3 建 图

建图实验采用TUM的fr1_xyz数据集，该数据通过手持深度相机在实验室桌面往复运动来采集RGB图和对应的深度图。该数据集约有780帧图片，本实验处理了前700帧。

在加入后端优化前，建立的室内环境地图如图6和图7所示。可以看到，优化前的地图中有很多杂乱的点云，桌面边缘轮廓线较模糊，墙边的凳子没有构建出来。

图6 优化前正面

加入后端优化机制后，散乱的点云收敛在一起，桌面边缘轮廓清晰，墙边的凳子构建较完善，从优化后的地图模型看出，形成了更加清晰一致的全局地图。

图7 优化前侧面

图8 优化后正面

图9 优化后侧面

从不同的角度去观察这个三维地图模型，文中算法构建出的三维稠密点云地图都能很好地反应室内环境的情况。

6 结束语

文中提出了一种基于RGB-D相机数据的快速低成本SLAM算法方案。本SLAM方案只需要一个搭载在机器人上的深度相机作为传感器[19-21]，如微软的Kinect相机。前端在RGB图提取中均匀化的ORB特征，通过跟踪相机位姿，得到位姿变换矩阵。挑选关键帧送入后端，用g2o工具箱进行全局的位姿优化，通过位姿变换矩阵将当前帧的点云图变换到世界坐标系中，实现三维稠密点云地图的构建。通过与RGB-D SLAM进行对比，本方案的速度是RGBD SLAM的2倍，能实现室内环境地图的快速构建，通过加入了后端优化机制，构建出全局一致的室内环境模型。

目前，本算法方案还不能达到完全实时的状态，且由于Kinect相机采集距离有限，对于大场景还不适用。因此下一步将通过优化算法方案来提高效率和鲁棒性。

参考文献：

[1]Khairuddin A R，Talib M S，Haron H.Review on simultaneous localization and mapping（SLAM）[C]//Control System，Computing and Engineering（ICCSCE），2015 IEEE International Conference on.IEEE，2015:85-90.

[2]辛菁，苟蛟龙，马晓敏，等.基于Kinect的移动机器人大视角3维 V-SLAM[J].机器人，2014，36（5）:560-568.

[3]李永锋，张国良，王蜂，等.基于快速视觉里程计和大回环局部优化模型的改进VSLAM算法[J].机器人，2015，37（5）:557-565.

[4]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 Jo-urnal of Robotics Research，2012，31（5）:647-663.

[5]Hsiao M，Westman E，Zhang G，et al.Keyframebased Dense Planar SLAM[C]//P-roc.International Conference on Robo-tics and Automation（ICRA），IEEE.2017:5110-5117.

[6]Endres F，Hess J，Engelhard N，et al.An evalua⁃tion of the RGB-D SLAM syst-em[C]//Robotics and Automation（ICRA），2012 IEEE Internation⁃al Conference on.IEEE，2012:1691-1696.

[7]Strasdat H，Montiel J M M，Davison A J.Visual SLAM:why filter[J].Image and Vision Computing，2012，30（2）:65-77.

[8]高翔，张涛.视觉SLAM十四讲[M].北京：电子工业出版社，2017.

[9]付梦印，吕宪伟，刘彤，等.基于RGB-D数据的实时 SLAM 算法[J].机器人，2015，37（6）:683-692.

[10]Sturm J，Engelhard N，Endres F，et al.A bench⁃mark for the evaluation of RGB-D SLAM systems[C]//Intelligent Robots and Systems（IROS），2012 IEEE/RSJ International Conference on.IEEE，2012:573-580.

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

[12]Rublee E，Rabaud V，Konolige K，et al.ORB:An efficient alternative to SIFT or SURF[C]//Computer Vision（ICCV），2011 IEEE international confer⁃ence on.IEEE，2011:2564-2571.

[13]Fraundorfer F，Scaramuzza D.Visual odometry:Part II:Matching，robustness，optimization，and applications[J].IEEE Robotics& Automation Magazine，2012，19（2）:78-90.

[14]梁明杰，闵华清，罗荣华.基于图优化的同时定位与地图创建综述[J].机器人，2013，35（4）:500-512.

[15]Kümmerle R，Grisetti G，Strasdat H，et al.g2o:A general framework for graph optimization[C]//Ro⁃botics and Automa-tion（ICRA），2011 IEEE In⁃ternational Conference on.IEEE，2011:3607-3613.

[16]刘浩敏，章国锋，鲍虎军.基于单目视觉的同时定位与地图构建方法综述[J].计算机辅助设计与图形学学报，2016，28（6）:855-868.

[17]Mur-Artal R，Montiel J M M，Tardos J D.ORBSLAM:a versatile and accurate monocular SLAM system[J].IEEE Transa-ctions on Robotics，2015，31（5）:1147-1163.

[18]Engel J，Schöps T，Cremers D.LSD-SLAM:Large-scale direct monocular SLAM[C]//European Conference on Compu-ter Vision.Springer，Cham，2014:834-849.

[19]徐震，邵波，王云鹏.无线传感器网络分布式数据采集功率控制研究[J].电力信息与通信技术，2017（2）:115-120.

[20]朱菊香.磁阻传感器在角位移测量中的非线性校正[J].自动化与仪器仪表，2015（12）:52-53.

[21]阳桂蓉.开环霍尔电流传感器磁芯设计[J].自动化与仪器仪表，2017（2）:74-76.