基于RGB—D数据的SLAM算法

颜义鹏　许志强　翟漪璇　韩金鑫　成怡

【摘 要】本文提出了一种基于RGB-D数据的SLAM算法，通过对相机获取的图像进行ORB特征点的提取与匹配，估计相机运动关系，实现点云拼接，最后会得到全局一致的点云地图和轨迹。为了消除误差积累引起的干扰，引入通用图优化库g2o，得到光滑的优化轨迹；在后端回环检测的过程中，引入关键帧选取机制，提高点云地图的生成效率，减少其消耗的存储空间。实验结果表明，本文的研究方法在RGB-D SLAM 算法中具有可行性，并且能够满足实时性要求，具备较高的精度。

【关键词】RGB-D；ORB特征点提取；SLAM；图优化

中图分类号： TP391.41 文献标识码： A 文章编号： 2095-2457（2018）14-0152-002

DOI：10.19694/j.cnki.issn2095-2457.2018.14.069

SLAM algorithm research based on RGB-D data

YAN Yi-peng XU Zhi-qiang ZHAI Yi-xuan HAN Jin-xin CHENG Yi

（Tianjin polytechnic university，Tianjin 300387， China）

【Abstract】This paper proposes a SLAM algorithm based on RGB-D data. By extracting and matching the ORB keypoints of the images obtained by the camera， the camera motion relationship is estimated and the point clouds are stitched. Finally， a globally consistent point cloud map and trajectory are obtained. In the process of detection of the backend end loop closure， a key frame selection mechanism is introduced to improve the generation efficiency of the point cloud map to reduce the amount of storage space. The experimental results show that the research method of this paper is feasible in the RGB-D SLAM algorithm， and it can meet the real-time requirements with high accuracy.

【Key words】RGB-D； ORB keypoint extraction； SLAM； Graph optimization

0 引言

因激光雷达具有测距精度高的特点，所以早期的Slam多使用激光雷达作为传感器，但激光雷达存在价格昂贵、数据难以处理等固有缺陷。近年来随着Kinect等深度摄像机的发展，由深度摄像机采集而来的RGB-D图像越来越多的被用于SLAM领域。

通常情况下，RGB-D SLAM方法包括前端和后端两部分。前端的主要任务是对RGB-D相机获得的彩色图像和深度图像进行预处理，从而获得点云数据；首先根据RGB-D相机提供的彩色图像提取图像的特征点，进行特征匹配；然后根据特征匹配的结果以及RGB-D相机提供的深度图像估算两帧图像对应相机位姿的变换关系。后端的主要任务是通过图优化方法，减小轨迹估计过程中出现的非线性误差，从而获得更加精确的相机位姿。

1 特征点提取与匹配

常用的特征点提取算法包括SIFT、SURF、ORB等。考虑到RGB-D SLAM算法对实时性的要求较高，因此本文在图像特征点提取与匹配环节采用ORB方法。ORB特征提取方法由Rublee等于2011年提出，其全称是Oriented Brief，由于其计算速率更快、精度更高且具有更好的鲁棒性（尺度和旋转不变性），因此被广泛应用在图像处理研究领域中。

设F1和F2为相邻两帧，它们的两组一一对应的特征点为：

P={p1，p2，…，pn}∈F1（1）

Q={q1，q2，…，qn}∈F2（2）

求解旋轉矩阵r和位移矢量t，使得：

？坌i，pi=Rqi+t（3）

然而由于误差的存在，等号不可能成立，因此通过最小化一个误差来求解r，t：

此问题可以用经典的ICP算法求解，本文调用Opencv中的函数求解此问题。

2 点云拼接

点云的拼接，实质上是对点云做变换的过程。这个变换可用变换矩阵（transform matrix）来描述：

该矩阵的左上部分是一个3×3的旋转矩阵，它是一个正交阵。右上部分是3×1的位移矢量。左下是3×1的缩放矢量，在SLAM中通常取成0。右下角是个1.这样的一个阵可以对点或者其他东西进行齐次变换：

由于变换矩阵结合了旋转和缩放，是一种较为经济实用的表达方式。它在机器人和许多三维空间相关的科学中都有广泛的应用。

3 后端优化

3.1 关键帧

在RGB-D SLAM算法中，若是把机器人采集到的所有图像均用来生成点云地图，这种做法有些欠缺。因此，在标准的RGB-DSLAM 算法中会有回环检测这一环节。由于用所有的图像进行拼接时会产生很大的累积误差，并且计算量较大，降低该算法的实时性。因此本文考虑在回环检测环节使用提取图像关键帧的方法进行点云地图的拼接，提高系统的准确性。同时，在回环检测的过程中，使用关键帧拼接点云还能显著地减少计算量和压缩存储点云数据所消耗的空间。

3.2 位姿图

完成图构建后，通过优化位姿图可以求解相机的最优位姿。常用的图优化方法包括非线性最小二乘法、基于松弛的优化方法、基于随机梯度下降法以及流形优化方法。非线性最小二乘法、基于松弛的优化方法和基于随机梯度下降法都是在欧氏空间中进行优化，但机器人的真实位姿变化却是在非欧氏空间中的。为了防止欧氏空间中奇异值的出现，本文使用四元數来表示机器人的位姿变化并在流形空间中进行优化。

4 实验与结果分析

本文实验操作环境基于ROS（Robot Operating System）开源机器人操作系统，算法代码为C++，均可通过gcc编译。实验运行的硬件设备平台是配置为4.00GB内存和1.70G Hz主频的32位Ubuntu 14.04 PC。实验使用的实验数据来自于标准开源测试数据集nyuv2。从中选取780帧彩色图像与780帧深度图像。彩色图像与深度图像均为640480的png格式。

本文首先分别选取较为相邻的彩色图像与深度图像各两帧，其中每一张彩色图形都有与其相对应的深度图像。如图1和图2所示。

使用ORB特征提取与匹配算法对图1和2的图像进行特征点的提取和匹配，匹配之后的结果如图3所示。通过前述的实验结果，可以计算出两张图像间的运动关系。将提取出的图像特征点的二维像素坐标转换为三维点云坐标，并采用相机内参参数，构建相机矩阵。调用Opencv中的SolvePnPRansac函数求解pnp，得到旋转向量rvec和平移向量tvec，然后再组装成变换矩阵，将一帧帧图像转换为点云，最后拼接起来就得到了点云图，如图4所示。本篇文章的实验为了能够保证局部运动的正确性，添加了回环检测环节，识别机器人曾经到过的地方，生成的点云地图如图5所示。对比可知，经回环检测，采取保留关键帧的点云地图的处理效果较好，采用g2o图优化工具求解的优化轨迹运动图，如图6所示。

5 总结

本文采用标准开源测试数据集nyuv2并对文中提出的一种基于RGB-D SLAM算法进行了一系列的实验验证：基于ORB算法的图像特征点提取与匹配、估计图像间的运动关系、生成点云地图与图优化轨迹。并在生成点云地图部分，将未引入回环检测环节与引入回环检测环节生成的点云地图进行了对比分析。结果表明，本文的研究方法具有可行性。

【参考文献】

[1]李海洋，李洪波，林颖，等.基于Kinect的SLAM方法[J]，中南大学学报：自然科学版，2013（S2）.

[2]贾松敏，王可，郭兵，等.基于RGB-D相机的移动机器人三维SLAM[J].华中科技大学学报：自然科学版，2014（I）：103-109.

[3]Gao X，Zhang T.Robust RGB-D simultaneous localization and mapping using planar point featuresfJl. Robotics&Autonomous; Systems， 2015， 72：1-14.

[4]Wang Y，Zhang Q，Zhou Y.Dense 3D mapping for indoor environment based on Kinect-style depth cameras[M].Robot Intelligence Technology and Applications 3.SDrinQer International Publishine. 2015：317-330.

[5]罗元，熊艳，张毅.基于改进迭代无迹粒子滤波的同时定位与地图构建[J].光学 精密工程（增），2015，23（10）：559-565.

[6]Kohlbrecher S，Stryk O V，Meyer J，et al.A flexible and scalable SLAM system with full 3D motion estimation[C].IEEE International Symposium on Safety， Security， and Rescue Robotics.IEEE， 2011：155-160.