基于图优化的移动机器人视觉SLAM

张毅，沙建松

同时定位与地图创建(SLAM)技术已经成为目前机器人研究领域的热点，随着SLAM问题研究的不断深入，对机器人位姿监测和环境地图创建的精确度、实时性也都有了更高的要求。因此，如何在精确度与实时性之间寻求一种平衡是目前众多SLAM算法中亟待解决的一个问题。而新型RGBD摄像机Kinect[1]，具有丰富3维空间信息与颜色纹理信息，为上述问题的解决提供了的可能。文献[2]提出的基于RGB-D相机的室内SLAM方法可实现对室内环境的稠密化建图，但论文中的SIFT特征提取算法较为费时，会占用较多的运算资源，因此其实时性较差。ORB-SLAM[3]采用了基于单目摄像机的SLAM方案，以高效的ORB特征贯穿于系统中的特征处理、匹配、回环检测、重定位等方面，更侧重于系统处理的实时性，但因为单目相机的限制，ORB-SLAM算法在稠密化建图方面还有所欠缺。针对目前移动机器人视觉SLAM研究中存在的实时性差、精确度不高、无法稠密化建图等问题，本文提出了一种基于Kinect深度摄像机的移动机器人三维视觉SLAM算法，以实现对环境进行实时的、精确的稠密化建图。在本算法前端处理中，通过分析多种特征点检测算法的优劣，采用了鲁棒性与实时性更好的ORB特征检测。利用 RANSAC算法[4]对可能存在的误匹配点进行剔除完成初始匹配，以PNP(perspective-N-point)[5]问题求解的方式实现了图像的快速配准。在后端优化中，通过对SLAM问题建模分析，遵循图优化思想，设计了一种非线性优化方法，实现对机器人位姿的优化。并与闭环检测[6]机制相结合，提出了一种基于局部闭环与全局闭环约束的点云优化算法，同时完成对位姿与稠密化点云的优化，可有效地抑制系统的累积误差，进一步提升整个系统的精确性。最后通过实验，验证了本文SLAM方法的可行性和有效性。

1 系统结构

本文提出的室内环境下移动机器人三维视觉SLAM算法的总体框架如图1所示，主要分为前端的关键帧筛选、帧间配准、闭环检测和后端的图优化、点云融合、点云优化几个部分。

图1 基于图优化的视觉SLAM系统框架Fig. 1 Visual-SLAM system framework based on graph optimization

2 特征点的检测与配准

2.1 特征点的检测

特征点的检测与匹配是视觉SLAM问题中“图像前端”的部分，也是视觉SLAM中占用计算资源最多，最影响系统实时性的部分。基于FAST特征检测改进的ORB特征[7]具有旋转不变和尺度不变特性的二进制特征，它是一种高效的具有良好针对视图不变的特征，比SIFT特征检测快两个数量级，同时特征点的质量较高、鲁棒性好。考虑到SLAM问题对鲁棒性、实时性的要求，本文采用了ORB特征提取算法。

2.2 特征点的配准

图像产生的本质是光线通过相机透镜投影到图像传感器上，这个过程是3-D场景到2-D平面的投影，因此场景与它的成像之间存在重要的联系，相同场景的不同成像之间也有联系。而通过这些联系，可以实现特征点的配准。

特征点配准算法可以分为以下5个步骤：

1) 提取特征点：Opencv提供了通用接口cv::FeatureDetector，这里将其设定为ORB特征。

2) 提取描述子：对于检测出的角点，用一些数学上的特征对其进行描述，这里通过cv::FeatureDetectorExtractor接口设置特征描述子提取器。

3) 匹配：通过各个角点的描述子来判断它们在两张图像中的对应关系。这里采用的方法为快速最近邻算法FLANN。

4) 去外点：利用RANSAC算法去除错误匹配的外点，保留正确的内点。

5) 位姿求解：调用Opencv的PnPsolver()函数，求解相机的位姿运动(R,t)。

3 位姿优化

针对传统的利用滤波方法估计位姿存在线性化误差、更新效率低下等问题，本文依据SLAM研究中的图优化[8]思想，设计了一种基于非线性优化的位姿优化方法，同时实现局部位姿优化和全局位姿优化。

3.1 模型的建立

SLAM的经典数学模型通常分为3部分：状态变量、运动方程、观测方程。

本文主要以Kinect作为主要的传感器输入，通过对视觉信号的处理，构建视觉里程计，用于位姿估计。所以这里只考虑状态变量和观测方程，构建出目标函数：

目标函数的物理意义：这里假设了误差函数e(·)符合高斯分布，则SLAM问题的贝叶斯概率描述为，求解适合的，使系统最有可能产生当前的观测数据Z，即最大似然(MLE)：

图2 优化求解流程Fig. 2 Process of optimizing solution

3.2 图的建立与优化

上述SLAM问题全部抽象成节点与边，节点代表了优化变量，包括pose节点和point节点。边代表了误差项约束条件，包括pose-point约束和posepose约束。pose-point约束依赖于相机观测产生的约束，如图3(a)所示。pose-pose约束则依赖于系统中闭环检测产生的约束，如图3(b)、(c)所示，可有效地抑制系统的累积误差，从而得到信息一致的地图。

图3 图的建立与优化Fig. 3 Establishment and optimization of graph

4 闭环检测下的点云优化

在基于位姿增量估计的SLAM建图中，会产生误差的累积，则在长时间的大范围SLAM建图中，就需要闭环检测对位姿和生成的点云进行优化。RGBD-SLAM算法是在局部回环检测后，只使用局部回环优化后的位姿作为三维地图点云的重建。ORB-SLAM算法则是在运动跟踪过程中，时刻检测并维护关键帧之间的点云共视关系，同时实现对点云优化。但这种方法只适用于规模有限的稀疏点云，并不适用于稠密化点云的优化。基于此，提出了一种基于局部闭环与全局闭环约束的点云优化算法，同时完成对位姿与稠密化点云的优化。算法具体步骤如下：

1) 封装构造数据帧Frame，根据帧间的关联程度，筛选出关键帧序列。

2) 执行帧间配准。完成相机位姿初始估计。

3) 在相机观测约束下，直接应用式(6)，构建如图3(a)所示的图优化。求得优化后的相机的位姿变化，将原始点云进行坐标系变换，实现点云的拼接融合。

4) 执行闭环检测。通过动态计算闭环帧筛选阈值，得到匹配质量较高的闭环帧候选序列。

5) 闭环检测结束后，遍历闭环帧候选序列，在当前帧与闭环候选帧之间，进行Sim3(相似变换矩阵)求解与优化。当某一候选帧通过了Sim3求解与优化，则认为闭环检测成功，跳出对闭环帧候选序列的遍历。求得对应相似变换矩阵为

6) 位姿优化的传播。当前帧坐标系与世界坐标系之间的变换在5)中已经确定并优化。通过一级相连关系的传播，可以确定这些相连的关键帧坐标系与世界坐标系之间的相似变换矩阵。

5 实验结果及分析

5.1 实验平台

为了验证算法的有效性，并保证本文实验的一致可对比性，故采用了知名的TUM标准数据集[12]，并选用了基于Kinect视觉传感器采集的室内基准数据包。该基准数据包中包含Kinect 产生的彩色图像和深度图像序列，以及机器人的真实运动位姿。本文实验数据处理平台为一台Intel双核2.4 GHz主频的PC机，运行Ubuntu 14.04操作系统。程序均设定以30帧/秒的速度同时读取数据包中的RGB数据和深度数据。

5.2 室内环境下的三维视觉SLAM

5.2.1 特征提取与特征点配准

对两帧图像进行ORB特征提取后，在利用特征匹配算法(FLANN)获得相邻帧间对应关系的基础上，利用 RANSAC 算法对点云进行匹配，匹配结果如图4(c)所示。在图4(a)中，通过特征匹配算法，一共产生了929个匹配特征点，但误匹配点过多。因此，通过人工制定规则的筛选，去除了部分误匹配点，此时的匹配个数为250。为进一步提升匹配精读，通过RANSAC算法对图4(b)进行进一步的匹配筛选。最终，产生了97个经筛选后的匹配点，实验中整个匹配所耗时间为 0.003 5 s。

图4 特征提取与配准实验结果Fig. 4 The experimental results of feature extraction and matching

5.2.2 回环检测约束

通过回环检测，可以增加帧间的约束，进一步提升系统的鲁棒性，进而提升整个SLAM的轨迹与地图的质量。在本文算法中，通过筛选关键帧之后进行回环检测，并以PNP求解的结果作为对边的估计，增加到图优化中，如图5所示。

图5 回环检测产生的约束Fig. 5 Loop closure of constraint

5.2.3 三维环境重构效果对比

图6(a)为采用了ORB-SLAM方法得到的三维环境重构结果，所构建的三维地图为稀疏点云。图6(b)为采用RGBD-SLAM方法得到的三维环境重构结果，物体轮廓可以辨别，但冗余点相对较多。图6(c)为采用本文的SLAM方法得到的三维环境重构结果，物体轮廓清晰可辨，冗余点明显减少。本算法实时性也远优于 RGBD-SLAM，与ORBSLAM的实时性相当。RGBD-SLAM 处理一帧数据平均用时约为350 ms，每秒钟只能处理 3 帧数据，因此不适于实时 SLAM。而本算法前端处理一帧数据平均约为31 ms，后端经过位姿优化与点云优化，以约5帧/秒的频率更新点云地图。可以满足SLAM系统对实时性要求。

图6 室内环境三维重构结果Fig. 6 3D reconstruction result of the indoor environment

5.2.4 机器人运动轨迹跟踪

为了获得机器人位姿的变化，本文以3D-2D的特征点约束，采用PNP求解的方法对机器人相邻位姿进行估计，并借助图优化方法对估计位姿进行优化。为了使运动轨迹更为直观，实验给出了本文算法在不同数据集下，机器人运动轨迹在X/Y平面上的投影，如图7所示。表1则记录了不同算法在不同数据集下，由计算所得的机器人在三维空间中的运动轨迹的真实位姿之间的均方根误差(RMSE)。通过均方根误差的对比，可有效地评估算法对机器人位姿跟踪的误差大小。可以看出本文方法能较好地实现机器人的运动轨迹跟踪。

图7 真实位姿与估计位姿对比图Fig. 7 Comparison between ground truth and estimated trajectories

表1 不同算法位姿估计的均方根误差对比Table 1 Pose estimation comparison of different algorithms about RMSE /m

6 结束语

在本文基于图优化的视觉SLAM算法前端框架中，采用的ORB特征检测、RANSAC算法等方法，较好地解决了帧间配准过程中匹配误差较大、效率低下的问题。在三维地图生成部分，以图优化后的位姿为基础，进行点云构建，并引入闭环检测的机制，实现了对位姿与稠密化点云的双重优化。实验验证了本文所提方法能够准确、迅速地重构出稠密化的三维环境模型，并能精确估计出移动机器人的运动轨迹。下一步计划对系统进行两方面的改进: 1)改进前端的帧间配准方法，考虑采用直接法进行位姿跟踪，完成位姿估计；2)研究环境三维地图与语义信息的结合，实现更好的人机交互与导航。

[1]ZHANG Zhengyou. Microsoft kinect sensor and its effect[J]. IEEE multimedia, 2012, 19(2): 4–10.

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

[3]MUR-ARTAL R, MONTIEL J M M, TARDÓS J D. ORBSLAM: a versatile and accurate monocular slam system[J].IEEE transactions on robotics, 2015, 31(5): 1147–1163.

[4]RUSU R B, COUSINS S. 3D is here: point cloud library(PCL)[C]//Proceedings of 2011 IEEE International Conference on Robotics and Automation. Shanghai, China: IEEE,2011: 1–4.

[5]HARTLEY R, ZISSERMAN A. 计算机视觉中的多视图几何[M]. 韦穗, 杨尚骏, 章权兵, 等, 译. 合肥: 安徽大学出版社, 2002: 158–256.HARTLEY R, ZISSERMAN A. Multiple view geometry in computer vision[M]. WEI Sui, YANG Shangjun, ZHANG Quanbing, et al, Trans. Hefei: Anhui University Press, 2002:158–256.

[6]LABBÉ M, MICHAUD F. Online global loop closure detection for large-scale multi-session graph-based slam[C]//Proceedings of 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems. Chicago, IL, USA: IEEE,2014: 2661–2666.

[7]RUBLEE E, RABAUD V, KONOLIGE K, et al. ORB: an efficient alternative to SIFT or SURF[C]//Proceedings of 2011 International conference on computer vision. Barcelona, Spain: IEEE, 2011: 2564–2571.

[8]梁明杰, 闵华清, 罗荣华. 基于图优化的同时定位与地图创建综述[J]. 机器人, 2013, 35(4): 500–512.LIANG Mingjie, MIN Huaqing, LUO Ronghua. Graphbased SLAM: a survey[J]. Robot, 2013, 35(4): 500–512.

[9]FADDEEV L D, RESHETIKHIN N Y, TAKHTAJAN L A.Quantization of lie groups and lie algebras[M]//KASHIWARA M, KAWAI T. Algebraic analysis: papers dedicated to professor mikio Sato on the occasion of his sixtieth birthday. Boston, MA: Academic Press, 2014: 129–139.

[10]SHAWASH J, SELVIAH D R. Real-time nonlinear parameter estimation using the Levenberg–Marquardt algorithm on field programmable gate arrays[J]. IEEE transactions on industrial electronics, 2013, 60(1): 170–176.

[11]KÜMMERLE R, GRISETTI G, STRASDAT H, et al. G2o:a general framework for graph optimization[C]//Proceedings of 2011 IEEE International Conference on Robotics and Automation. Shanghai, China: IEEE, 2011: 3607–3613.

[12]STURM J, MAGNENAT S, ENGELHARD N, et al. Towards a benchmark for RGB-D SLAM evaluation[C]//RGB-D Workshop on Advanced Reasoning with Depth Cameras at Robotics: Science and Systems Conference. Los Angeles,United States: RSS, 2011: 1–2.