RGB-D SLAM V2系统算法分析与性能评估*

李明灿，贾永兴，王 聪

（陆军工程大学，江苏 南京 210000）

0 引言

随着芯片技术和传感器技术的发展，移动机器人技术逐步进入生活的方方面面。无人机、扫地机器人等功能各异的机器人正深刻改变着人们的生产生活方式。想要完成复杂的任务，机器人需要能够自主规划自身的运动。在没有操作者的情况下，自主移动机器人失去了辅助判断状态的先决条件，所以必须具备感知外部信息的能力，凭借传感器数据估算自身的位置与周围环境的信息，从而依靠这些信息计算出最佳的运动路线和动作轨迹，而准确的机器人信息感知是困难的。机器人搭载的传感器只能测量到局部的环境数据，不能直观了解环境的整体情况，必须依靠位置状态进行环境信息的提取与合成。这种相互依存的关系导致移动机器人学中一个重大的开放性问题——要精确定位，因此机器人必须有精确的环境地图。为了构建精确的地图，必须精确知道机器人观测的位置。为了实现高精度的信息感知，研究者们进行了长期探索，经过建模分析与实际环境实验，提出了许多有意义的解决方案，其中，一个重要的领域是同时定位与地图构建（Simultaneous Localization and Mapping，SLAM）技术，目前已成为整个问题框架中的核心研究方向。

1 视觉SLAM技术介绍

SLAM技术旨在利用传感器数据，通过计算机估算出机器人平台的位置变化，并同时性地构建出周围环境的仿真地图。其中，利用视觉传感机器获取外部信息的SLAM技术称为视觉SLAM。视觉SLAM前端被叫做视觉里程计，目的是通过计算机视觉方法估算出机器人的位姿变化；视觉SLAM后端使用图优化[1]或束调整[2]等方式，优化机器人的运动轨迹。在视觉里程计研究领域，不少开发者推出许多优秀的算法，实现了使用不同类型的摄像头估算运动中位姿的变化。按是否需要提取图像特征[3]，它主要分为特征点法和直接法。

在当前视觉SLAM领域，最受关注的是利用深度相机完成感知任务。深度相机又称RGBD相机，可以用来观测视角范围的彩色图像和深度图像，输出一连串的三维图像。基于RGBD数据的特征点法利用彩色图像拾取特征点，而后利用对应的距离信息还原出其真实的空间分布，并根据3D-3D空间位姿变换的约束关系，依靠部分特征点使用纯几何手段估算关键帧之间的变换矩阵，进而分解出机器人运动的平移向量和旋转矩阵。在得到一段时期的位姿变换估计后，算法后端利用优化算法对输出结果进行高维度滤波，得到更符合实际变化特点的定位信息。系统的另一个任务是建图，即利用优化获得的定位信息，将三维特征点或者点云图变换参考系并进行耦合，得到稀疏或者稠密的仿真地图。

基础的视觉SLAM算法在结构上已趋于完善，但是在实际的应用实现中并不能完全稳定的工作，需要使用许多特别的技巧强化系统的健壮性。经过长期的探索，研究者推出了许多优秀的框架，在原有理论的基础上采用了新的理念，完成了准确的定位和建图。考虑到传感器适配、稠密建图以及准确性等方面，本文选择最新的RGB-D SLAM V2来进行典型算法的分析和评估。

2 算法结构

RGB-D SLAM V2由Endres提出，是一个基于深度相机的稠密视觉SLAM系统。RGB-D SLAM V2算法流程，如图1所示。它的代码直接兼容机器人操作系统（Robot Operating System，ROS），基本不用配置就可以非常方便地运行。对比基础的系统架构，它在位姿估计、重定位、回环检测和建图等方法上使用了创新算法，提高了系统的容错性，使系统对机器人的运动追踪更加快速和稳定，同时能够产生较为完整的环境地图，为后续的应用提供了更全面的信息。

图1 RGB-D SLAM V2流程

2.1 位姿估计

RGB-D SLAM V2使用的是标准的特征点法，支持SIFT、SUFT以及ORB[4]等特征选取方法，利用稀疏的3维特征点估计帧与帧之间的变换矩阵。如图2所示，比较3种特征点，有GPU时SIFT综合表现最好，而综合实时性、硬件成本和准确率来看，ORB具有均衡的优势。

图2 3种特征点效果比较

视觉SLAM基础的位姿估算方法利用的是连续两帧的空间变换关系。假设上一帧观测位置为p1，当前帧观测位置为p2，两帧之间的变换矩阵描述为T12，则观测位置之间的关系为：

RGB-D SLAM V2采用的估计算法不仅利用了一种策略，而且采用RANSAC[5]和ICP相结合的方式进行运动估计。初始阶段，前端使用3对特征点进行快速计算作为RANSAC的参考。在每一轮迭代中，利用最小二乘法估计其余点对相关程度，不断优化得到最佳的变换矩阵。

2.2 环境测量模型

Endres提出了一个专属的环境测量模型（Environmental Measurement Model，EMM），如图3所示，指出了观测图像的特征点服从高斯分布，开创性地利用高斯分布的吻合特性来考虑点位之间的一致性问题。模型把两帧之间的特征点进行空间初始估计后，将特征点的高斯分布空间按照设定的阈值划分边界。空间能够重叠的相同特征会被视作在可接受误差范围内的可靠点，可以被用来进行进一步的优化，而无法重叠的匹配点会被视作误匹配或者不合格的特征而被丢弃。另外，要考虑在当前帧中出现的新特征点，既没有旧特征点与之匹配，也不存在未匹配特征点与之高斯分布重合。

图3 环境测量模型

2.3 重定位

定位技术的方法有很多，可以将其划分为绝对定位和相对定位。绝对定位是以固定的坐标为参考，计算每次定位的绝对位置。相对定位以初始位置为坐标原点，依次估计相邻两次观测点位间的相对运动，等效得到定位信息。但是，只是依靠连续帧简单估计变换矩阵时，误差累计会导致最终的定位发生严重偏移。重定位技术利用变换参考的方式很好地解决了这个问题，技术原理如图4所示。

重定位的具体方法是在每次位姿估计之前对当前帧的光照条件进行一次估算，将满足阈值的图像作为关键帧抽选出来，作为接下来三维帧位姿估计的参考。这种方式的实质是将连续的相对位姿估计转化成可变窗口的短时绝对定位，而参考的定位点是窗口的第一帧图像的位置。在后端的优化过程中，参考帧组成的稀疏图像集会被看作运动的节点，进一步优化估计的结果，从而实现对运动噪声的一种数字化滤波。由于关键帧的低数量级和可存储的特点，优化可以高效完成，不需要再在整段图像序列上进行操作，节约了后端处理的时间。

图4 重定位技术

2.4 图优化

位姿图优化是利用图论的算法优化位姿的手段。在RGB-D SLAM V2中，图优化只优化位姿图，并没有优化三维图像点。具体实现中，估算的位姿被画成了一个全向图，使用g2o图优化库来对被环境测量模型接受的运动估计结果进行噪声滤除，将两帧相机位姿作为优化顶点，将它们之间的运动估计结果作为优化边，利用两者的约束关系，在多次迭代后得到最佳的估计。值得注意的是，后端模块对检测到的回环也加入优化顶点和边，实际上对全局优化进行了一定的矫正。

优化边的误差函数：

式中，xi、xj为优化顶点，即位姿的估计值；zij是约束，也就是xi和xj之间的变换；e(xi,xj,zij)是满足xi到xj的误差程度；中间的Ωij是优化边的信息矩阵（协方差矩阵的逆），表示对边的精度的估计。

2.5 回环检测

回环检测是利用运动中相同路标的重复出现对轨迹进一步优化的方法，实质是一种采用反馈调节消除误差的策略。实现回环检测的关键是要依靠特征点包含的信息。健壮的图像特征点要在保证视角变换和旋转的情况下仍能被检测出来，并具有相同的描述信息，因此每个特征点要同时包含点位的坐标及其描述子，用来完成特征的准确匹配。回环检测就是利用特征点描述子的重复出现来判别机器人是否来到了自己走过的路线。

RGB-D SLAM V2系统的回环检测使用基于最小生成树的方法——一种随机森林的随机回环。算法使用描述子之间直接相减的差值，产生了一棵有限深度的最小生成树，去掉连续多帧内的最近点来避免重复比较，再随机从树上选k个偏向于更早时间的帧来判断闭环是否满足。

2.6 建 图

RGB-D SLAM V2系统适配点云地图和八叉树地图两种地图。点云是把二维像素三维化后得到的结果，使得所有像素点的空间位置被直白表示出来。在无失准的情况下，点云保留了观测的所有信息。经过空间变换拟合后的地图最大限度地还原了原始环境，但是这对运算、存储和显示资源的消耗都是巨大的。一个小场景的点云地图往往需要1×109bit的存储，显然后续应用的输入是难以适用的。此外，这种地图保留的信息未经进一步处理，后续信息的提取在轻量级平台上也难以达到实时性。

对比而言，八叉树地图具有更高的应用优势。如图5所示，八叉树地图使用滤波方式，将设定分辨率内的像素转换为单一的体素，利用算法中树的结构，逐级把相邻8个体素中颜色最多的体素作为上级分辨率节点形成一个八叉树，从而将整个地图更优化地存储，提高地图的可查找性和可利用性。

图5 八叉树地图原理

3 性能评估

实验阶段，利用标准的数据集衡量了系统定位的准确性，评估输出地图的视觉效果。

MIT推出了使用RGB-D相机测量的数据集，结合高精度的惯导设备重现运动轨迹，用以验证和评价SLAM算法的定位精度。由手持设备三维平行运动观测（图6、图7）、围绕目标运动观测（图8、图9）和360°运动观测（图10、图11）等场景测试结果可以看出，算法对光心移动的定位较为准确，基本可以还原真实的轨迹，对场景的构建较为真实，但是建图存在区域性的空洞现象。造成这个问题的原因在于算法使用了直接耦合的方式，对观测中的运动速度对数据的影响没有进行适配，导致速度快速变化时像素点的漂移较大，使得拟合后的地图具有空间点疏离的特点。

图6 三维平行运动地图构建效果

图7 三维平行运动定位估计效果

图9 围绕目标运动定位估计效果

图10 360°运动地图构建效果

图11 360°运动定位估计效果

4 结语

本文对RGB-D SLAM V2系统进行了详细讲解，并评估算法的数据集测试效果和真实场景的建图效果。系统在基础的视觉SLAM框架的基础上，利用一些创新的辅助技巧提高了系统的容错性，更健壮地完成了定位和建图的整个流程，在适配的RGBD相机上性能表现优秀。从实验结果可以看出，算法定位的精度基本拟合真实的运动，对环境的重现也可用于视觉观测，具有极好的可扩展性。下一步工作将对定位和建图的输出结果应用机器人导航进行更加细致的研究。