改进ORB 特征的机器人RGB-D SLAM算法

伍锡如，黄榆媛，王耀南

1.桂林电子科技大学 电子工程与自动化学院，广西 桂林 541004 2

.湖南大学 电气与信息工程学院，长沙 410082

1 引言

随着鱼眼相机、深度相机和全景相机等先进设备的出现与计算机视觉技术的日益成熟，视觉同时定位与地图构建（Simultaneous Localization and Mapping，SLAM）成为国内外学者的重点研究领域。特别是在机器人、AR/VR、自动驾驶和无人机等应用中涌现出许多创新性成果[1-5]。SLAM技术是机器人实现自主移动的关键技术[6]，基于视觉的SLAM相比于激光SLAM具有功耗低、成本低、小型化等优点，理论和应用价值突出[7]。2010年微软公司推出的RGB-D深度相机Kinect，由于其低成本、轻量化、能采集丰富的图像信息等优势，广泛应用到视觉SLAM技术的研究和开发中[8-11]。文献[8]首次采用深度相机进行室内环境3D密集地图的构建。Endres F等人[10]设计了一个经典的RGB-D SLAM系统，该系统具有较好的鲁棒性和准确性，能在较长轨迹和有挑战性的场景中追踪机器人的运动。文献[11]中介绍了第一个基于单目、双目和RGB-D相机的完整SLAM系统，全程使用ORB[12]特征，兼具地图重用、闭环检测与重定位的功能，并且可实时在CPU上工作。

经过30多年的发展，机器人视觉SLAM的整体算法框架包括：前端视觉里程计、后端优化、回环检测和地图构建。从概念上来说，机器人视觉SLAM需要完成的工作是用视觉传感器采集到的图片数据，精确地估计出机器人的运动位姿，并同步构建周围场景的地图，以便于进行后续的路径规划或运动控制[13]。早期的视觉SLAM沿用机器人领域的滤波方法，如卡尔曼滤波[14]、扩展卡尔曼滤波[15]、粒子滤波[16]等，而现在应用计算机视觉领域的图优化方法成为主流[17-21]。文献[18]的视觉SLAM系统最早引入关键帧机制，通过串接关键帧而非采集到的全部图片完成轨迹和地图的优化。文献[19]总结和分析了基于关键帧的SLAM系统，并对SLAM研究中存在的外部干扰、地图维护、重定位等问题提供指导意见。KinectFusion[20]是Newcombe等人设计的首个用Kinect深度相机完成的三维重建系统，它从深度图中读取点云数据，根据迭代最近点（Iterative Closest Point，ICP）算法估计位姿来拼接点云，基于网格模型实时重建场景。Kintinuous[21]是KinectFusion的改进版本，增加回环检测与优化，融合直接法使得处理速度更快，适用于大场景重建。文献[10]的RGB-D SLAMv2是一个优秀且完整的SLAM系统，有前后端、闭环检测和地图构建，在使用SIFT特征时要用GPU加速。从这些优秀的文献工作来看，在静态不变、无人为干扰、光照不明显的应用环境中，视觉SLAM技术已经发展得较为成熟。但是现实的工作环境往往更加复杂多变，如何在已有的系统框架上通过改进算法提升视觉SLAM技术的实时性、鲁棒性和准确性，从而应用到实际场景中，成为该技术领域的一大挑战和难题。

本文提出一种新的RGB-D SLAM算法，旨在提高机器人视觉SLAM算法实时性、定位和建图精度。主要贡献在于：（1）基于四叉树（Quad-tree，Q-tree）算法改进ORB特征提取，减少冗余点，加速误匹配点的剔除；（2）应用EPnP和ICP组合算法求解得到较精准的机器人位姿；（3）基于关键帧和词袋（Bag of Words，BoW）模型构建快速回环检测，减小累积误差；（4）使用光速法平差（Bundle Adjustment，BA）优化位姿图得到最优轨迹，构建3D彩色点云地图，并以八叉树地图（Octomap）形式进行存储。通过公开数据集和实际移动机器人平台验证算法性能。

2 机器人RGB-D SLAM系统架构

图1 机器人RGB-D SLAM系统框图

根据文献[10]的系统框架，本文的机器人RGB-D SLAM系统框图如图1所示。前端负责读取传感器数据和估计位姿。Kinect相机作为环境感知设备能同时读取RGB彩色图和深度图。本文选择特征法开展研究，因为它不易受动态障碍物、光线等的影响，可稳定运行在移动机器人平台。常用的图像特征有SIFT[22]、SURF[23]和ORB，其中的ORB算法因为较优的快速性和实时性得到广泛应用。为使提取的ORB特征点均匀分布，本文先引入Q-tree来管理FAST角点，然后采用快速最近似最近邻（Fast Library for Approximate Nearest Neighbors，FLANN）算法匹配特征，最后在随机采样一致性（Random Sample Consensus，RANSAC）算法的基础上，迭代使用EPNP和ICP方法求解不同坐标系下的几何关系，获得机器人位姿。由于相机获取的数据存在不可避免的外界干扰，位姿估计必然会出现累积漂移，所以需要回环测和后端优化来减小累积误差，进一步优化位姿。回环检测的目的是判别机器人是否重新回到之前已经走过的区域，检测到的有效回环信息能将具有累积误差的轨迹拉回正确的地方。实现回环检测的原理是对关键帧进行数据相似性的计算，具体的方法是使用BoW模型来创建字典，通过字典对关键帧之间的相似性做出量度，进而判断回环的存在。在后端中，需要用到优化理论构造图优化模型，这个模型使用估计出的位姿和回环检测数据构造位姿图，通过BA算法优化位姿图的结构，得到长时间内机器人的最优轨迹，从而构建全局一致的机器人运动轨迹和环境地图。这部分工作将基于通用图形优化器G2O进行求解。在已知机器人的最优轨迹后，可通过轨迹来重建环境地图。3D点云地图数据规模较大，为降低硬件内存要求，使用紧凑的Octomap进行数据存储。Octomap易于信息的压缩和更新，同时也可以根据应用需求调节分辨率得到不同精度的导航地图。

3 机器人RGB-D SLAM算法实现

本章介绍整个算法的实现流程。图2是整体算法流程图。该算法的输入数据为Kinect深度相机于同一时间戳采集到彩色RGB图像帧和深度图像帧，它们在时空上是对齐的。算法前端，对RGB图像帧使用基于四叉树的ORB特征算法来提取均匀的Fast关键点，获得这些关键点的BRIEF描述子；通过FLANN算法完成描述子的特征匹配，并根据筛选条件剔除错误匹配点对；按照RANSAC迭代方式，结合EPNP和ICP算法估计出机器人的位姿。同时，每一时间戳的图像帧经过关键帧判别后存储到关键帧数据集中，该集合经冗余处理后用于词袋模型的回环检测。在后端，前端计算的机器人位姿和回环检测数据被构造成位姿图，基于BA算法求解得到最优的机器人位姿轨迹。根据最优轨迹和对应的3D点云重建出运行环境的彩色点云地图，点云地图在转换后被表示成紧凑的Octomap地图。

图2 整体算法流程图

3.1 前端

3.1.1 基于Q-tree的ORB特征提取

相比于像素或直方图的图像内容描述方法，基于外观的图像底层特征保留了丰富的原始信息，能更好地区分和表达图像内容。根据硬件条件和实时性的需求，选择同时兼顾特征检测速度和稳定性能的ORB算法对场景内容进行描述。

（1）基于Q-tree的FAST关键点检测

对同一张图片提取不同的空间尺度信息，得到图像局部和全局尺度上的一系列多分辨率图像集合，构建高斯金字塔（见图3），再从每一层中进行FAST角点检测，解决FAST算法本身没有尺度不变特性的问题。

图3 多尺度高斯金字塔

采用灰度质心法计算FAST关键点的主方向，增加旋转描述。假设点O(x,y)是某一待检测的特征点，取包含点O的一个小图像块U计算(p+q)阶邻域矩：

式中，I(x,y)为点O的灰度值。

通过邻域矩来求图像块U的质心C：

连接O和C获得方向向量OC，则关键点O的主方向为：

均匀地对环境进行采样是进行良好特征匹配的前提，但在研究中发现检测出的FAST角点在一张图片中会堆积成点斑，不利于后期的特征匹配。本文结合Q-tree均匀算法[24]加以改进，减少冗余特征点的同时，能使提取的关键点更均匀地遍布在整个图像中。

图4是Q-tree区域分割及数据存储的示意图。对每张图像提取到的FAST角点进行Q-tree均匀处理，步骤如下（一个子区域对应树状结构中的一个子节点）：

步骤1初始化父节点O，将2D空间均匀分割成4个子区域(A,B,C,D)，得到初始的Q-tree结构。

图4 Q-tree数据结构示意图

步骤2将检测出的FAST角点映射到初始Q-tree中，根据角点位置把角点划分到不同的子区域。

步骤3按照子区域存在的角点数量判断是否进行再分割：若角点个数小于等于1，则不再分割；反之，则再次均匀四分割并划分角点。

步骤4重复步骤3，直至递归划分出的子区域中存在的角点个数小于等于1或达到期望提取的角点数量为止。

步骤4中停止分割的条件是每个子区域中的角点数都不大于1，这在实际中很难达到，因为检测出的角点如存在扎推现象，相邻的多个角点几乎是重叠的，所以当子区域的角点个数大于1时，需要通过非极大值抑制[23]条件，只保留Harris响应强度最大的较优角点，去除Harris响应强度小的冗余点。

（2）BRIEF描述子计算

BRIEF[25]用随机点对生成的描述子的抗噪性能不佳，处理方法是使用积分图做平滑处理，通过高斯滤波降噪。依据取法之间相关性最小的规则，在特征点的31×31像素邻域中选择5×5子窗口对，比较随机N对子窗口对的像素灰度值之和得到一个N维的二进制描述子，对于一个平滑后的图像邻域Δ：

其中，λ(Δ;g,h)表示二值化的结果，I(Δ,g)和 I(Δ,h)是邻域Δ内的两个对应子窗口g、h各自的灰度值之和。fN(Δ)是获得的二进制比特串，N=256。

依据FAST关键点的主方向对描述子进行旋转，克服BRIEF描述子不具备旋转不变性的缺点。这个方向信息用关键点31×31像素邻域内的N对(xj,yj)构成2×N的矩阵Τ：

由旋转矩阵R和式（3）中的主方向φ对Τ旋转，得到校正后的Τφ矩阵：带方向的描述子[12]可表示为：

在检测出特征点并计算对应描述子后，通过描述子计算图片间的汉明距离进行匹配。暴力匹配法对于特征点多的场景运算成本高，不利于机器人SLAM的实时运行，选用FLANN[26]算法完成特征匹配。同时根据工程经验，用汉明距离小于最小距离的2倍为筛选依据，去除错误的匹配点对。

3.1.2 机器人位姿估计

在使用深度相机获得深度图的情况下，可以得到特征点的距离信息，所以根据ORB特征匹配后的结果对机器人的位姿进行估计，通常可以考虑两种情形，一种是根据已知3D点与其在相机中的投影2D点求解PNP问题，另一种就是使用匹配好的两组3D点云求解ICP问题。在SLAM中，常见的一些做法是使用P3P或者非迭代的EPNP算法估计位姿再进行优化，这种做法没有有效利用更多的匹配点信息，而直接使用ICP算法求解则需要依赖较好的初始值，不然极易在迭代中陷入局部最优，且在深度相机不能获得距离信息后难以执行，可见这些方法在噪声干扰或误匹配时会导致算法失效。本研究在这些算法的基础上进行优化，使用RANSAC算法简单有效地过滤误匹配，通过迭代的方式提高匹配点对的信息利用率，把良好的匹配数据应用到EPNP[27]算法中改善稳健性，在有了较好的初始值情况下，进一步使用ICP[28]算法求解机器人位姿。

EPNP算法的思想是用4个非共面虚拟控制点的加权和表示世界坐标系下的3D点坐标。为保证算法的稳定性，需要根据世界坐标系下匹配点集的质心以及主成分分析法得到的三个主方向选择控制点。它每次随机地从所有匹配好的点对中选择四个点对进行计算，匹配点相对于世界坐标系的坐标为,i=1,2,…,n ，相对于相机坐标系的坐标为,i=1,2,…,n，同时，假设4个控制点的坐标表示那么在世界坐标系下，匹配点与4个虚拟控制点的线性关系为：

由于相机坐标系与世界坐标系是相似变换的关系，在相机坐标系下也存在：

根据相机的投影模型有：

式中，si为尺度因子，[uivi]T表示 pci对应的2D投影坐标；K是相机的内参数矩阵，其中 fu,fv代表相机焦距，(uc,vc)为相机中心，该矩阵可以通过标定获得。

将公式（10）和（11）代入公式（12）中展开得：

把式（14）写成矩阵形式：

向量x位于M的右零空间[27]，存在下列关系：

其中，vi是矩阵M的零特征值相应的N特征向量，通过计算MTM的特征向量可以解出vi。βi的求解与N的取值有关，详细求解参照文献[27]。

求解出βi后，依据公式（16）就能得到x，即相机坐标系下4个虚拟控制点的坐标。再将x代入式（11）计算出匹配点在相机坐标系下的3D坐标。

在已知匹配点在世界坐标系下和相机坐标系下的两组3D点后，使用ICP[28]算法估计机器人的位姿，获得旋转矩阵R和平移向量t。算法的操作流程为：

（1）假设两组点分别为P和Q：

存在欧式变换R,t，满足∀i,pi=Rqi+t。

（2）第i对点的误差定义为：

构建最小二乘的目标函数F，求出一个R,t使误差平方和得到极小值：

（3）求解式（19），先计算两组点的质心坐标 ϕ,ϕ′，得到每个点的去质心坐标κi,κ′i：

（4）由优化的目标函数求旋转矩阵R：

（5）按照（4）中的 R，进一步求出t：

至此，就能求得机器人的估计位姿R,t。

用每次估计出的位姿计算反投影误差，确定内点的数量，然后通过内点数量判断出有效的位姿估计，最后选择具有最大内点数的位姿作为此次的最准确估计。在RANSAC算法框架中，以上过程不断迭代，获得当前帧对应的最优位姿估计。

3.2 基于关键帧的回环检测

根据降低控制问题规模和保证稀疏性的需要，采用关键帧技术进行有效采样能提高视觉SLAM系统的实时性和稳定性。而且在后端优化中使用成功的回环信息有利于构建全局一致的轨迹和地图。

3.2.1 关键帧选择

两张图片之间相似性的可靠测量是许多关键帧选择算法的基础。本文研究了基于特征匹配的相似性量度S[29]的方法。S与两张图片的距离函数Λ相关：

式中，Σt和Σi分别表示当前图像帧Ft及该帧对应的最近上一关键帧(KF)i中视觉特征的个数，M表示两个图像帧匹配的视觉特征个数。Λmax和Λmin是图像之间距离函数Λ期望取到或者能够取到的最大值和最小值，S 的取值范围是[0，1]。

将系统输入的第一帧作为初始关键帧，下一个关键帧的选择判据有两个：（1）距离上一个关键帧插入已经过去超过20帧；（2）检测到的内点数要大于设定的最小阈值，并且相似性量度S小于设定的最大阈值。满足（1）、（2）中一个条件即将当前帧设为关键帧。

3.2.2 BoW模型

BoW模型的建立是基于外观的，它应用于回环检测时可以脱离视觉SLAM系统前、后端估计的几何信息，比基于图像特征的回环检测方式更加快速和有效。在BoW中涉及单词和字典的概念，每个单词代表某一类特征的组合，多个单词组成字典。根据文献[30]，使用类似于层次聚类的k叉树来生成字典。图5是深度为l的k叉树字典数据结构图。从所有训练图片中提取ORB特征，在根节点用k-means++算法均匀地将所有特征点进行层次聚类。这里使用文献[11]中的ORB特征类型字典用于回环的检索。

图5 k叉树字典结构

回环检测前要删除关键帧集合中的冗余帧，删除的标准是如果有不少于3个的其他关键帧能够观测到某个关键帧90%以上的路标点，则把该关键帧视作冗余关键帧并剔除[11]。回环检测在去除冗余帧的关键帧集合中进行，通过比较不同关键帧间的相似性判断回环，将回环信息作为约束交给后端处理。

3.3 后端优化与建图

3.3.1 基于BA的图优化

视觉SLAM中的优化问题可以用一个由顶点和边组成的图表示，通过优化图的结构获得待优化变量的最优解。图6给出了简单的图优化模型。路标点和位姿作为顶点是待优化的变量，使用运动模型和观测模型得到顶点之间的约束边，可以是估计位姿约束或者回环约束。

图6 图优化模型

基于图优化模型，视觉SLAM的优化能直观地用数学语言表达成一个非线性最小二乘问题，通过BA[31]方法在G2O库中求最优解。视觉SLAM过程的基本数学表示包括运动方程xk和观测方程zk。

这里的uk是传感器的输入，wk为噪声。pj是在位置xk观测到的路标点，vk,j为观测中的噪声。

由公式（25）知道，运动和观测过程都存在噪声干扰，实际的观测值和用算法计算出来的观测值存在误差，图模型的边就代表了这个误差项e。本研究只对位姿xk做优化处理，优化的目标函数F(X)为：

其中，X是全部位姿变量xk的集合，D代表估计位姿的时间序列；xi,xj是i、j时刻估计出的机器人位姿，zij表示这两个顶点的实际约束，而z͂ij是通过计算得到的约束；Ωij代表约束的信息矩阵，即两顶点不确定性的度量，其中的每个Ωij作为ei、ej的系数，是对ei、ej误差项相关性的预估。此时，优化问题的求解变成寻找一个最优位姿序列X̂，使得目标函数F(X)取得最小值，即：

考虑优化算法的鲁棒性，对目标函数用鲁棒Huber核函数包装，避免优化过程专注于错误数据的优化，而忽略正确约束的影响。整体实现在G2O库中进行，使用列文伯格-马夸尔特下降策略和设置稀疏求解器完成。

3.3.2 地图构建

构建地图是对机器人运行环境进行建模的过程。把关键帧数据处理成点云，根据优化后的位姿使用点云库PCL拼接点云，得到3D彩色点云地图。目前由离散路标点构建的稀疏点云地图很难与导航、避障、路径规划等应用接轨，并且数据的存储对硬件内存要求高。研究中将点云地图转化成Octomap，八叉树节点能保留该点的占据情况，树状存储结构占据内存小，也使得数据的更新和维护灵活和快速。在Octomap库实现点云地图到Octomap的转换，其可视化界面有查看、缩放旋转、调节分辨率等操作。

4 实验与分析

为了验证本文算法的可行性和高效性，使用公开的RGB-D数据集进行验证，并在室内环境对多履带式全向移动机器人开展实验。通过对比实验分析设计算法的实时性、定位及建图效果。

4.1 实验准备

全部实验在Ubuntu16.04 LTS环境下完成，硬件平台是64 bit的ThinkPad-T460 PC，处理器为Intel Core i5-6200U@2.30 GHz×4。实验数据是德国慕尼黑工业大学的TUM数据集[32]，数据序列的详细信息见表1。

表1 实验数据的信息

实验前，在ROS Kinetic环境下对Kinect v1（见图7）进行标定，使用8×6×0.028的棋盘格，得到的结果如下。

相机内参数矩阵：

畸变参数：

D=[0.005 7-0.085 5 0.012 88-0.000] 3 0 （30）

实验中，主要参数配置有：相机帧率30 f/s，每张图片提取的ORB特征点数量为1 000，尺度金字塔层数设为8且层与层之间的尺度因子为1.2。

图7 Kinect v1与标定实验

4.2 改进算法的相关性能分析

4.2.1 基于Q-tree的ORB算法检测性能

图8为使用基于Q-tree均匀算法的FAST角点检测处理前后的角点分布情况对比。显然，与原始的ORB算法相比，经过改进算法处理后，可以在整张图像中提取出分布较均匀的角点。由式（23）、（24）可知，误匹配点对会严重影响图像相似度的计算结果，这里使用FLANN算法快速匹配特征并剔除误匹配对，获得良好的图像配准结果（见图9）。可见，改进处理后的ORB特征算法具有较稳定和优越的场景表达性能，适用于描述和匹配图像。

图8 角点检测对比

图9 改进算法的特征匹配对比图

4.2.2 实时性和定位误差对比

使用表1的数据，将RGB-D SALMv2[10]算法与改进算法的运行时间、均方根误差（Root Mean Square Error，RMSE）进行对比，统计数据于表2。

表2 RGB-D SLAMv2算法与改进算法的性能指标对比

由表2可以看出，本文算法的运行时间和定位误差比RGB-D SLAMv2算法显著减少，算法实时性和准确性得到极大提升。针对这12个数据序列，从数据处理时间来看，RGB-D SLAMv2算法平均每帧数据的处理时间约为0.153 s，本文算法的处理时间约为0.080 s，从两种算法估计出的轨迹与地面真实轨迹的RMSE来看，本文算法的平均RMSE低于RGB-D SLAMv2算法，其中RGB-D SLAMv2的平均RMSE约为0.258 m，本文算法的平均RMSE约为0.084 m。根据以上指标的统计结果，证明改进算法有更好的定位实时性和准确性。

为了更加直观地显示本文算法的定位准确性，采用文献[33]的定量评价方法对估计轨迹与真实地面轨迹进行误差量化分析。图10是估计轨迹与真实轨迹的对比图，其中（a）、（b）、（c）和（d）分别对应表1的fr1/xyz、fr1/floor、fr2/pioneer_360和fr2/pioneer_slam数据序列，灰色虚线reference表示真实轨迹，带颜色的实线表示算法的估计轨迹，图右侧从红到蓝的色带，显示误差的大小程度。图11为4组数据序列的估计轨迹与真实轨迹绝对位姿误差（Absolute Pose Error，APE）随时间的变化趋势曲线，表3为误差量化指标数据，包括最大值max、最小值min、均值mean、中位数median和标准差std。

表3 误差量化指标 m

图10 估计轨迹与真实轨迹对比图

图11 绝对位姿误差变化趋势图

从图10可以看出估计轨迹能准确贴合真实轨迹，全局一致性较好。分析图11的趋势曲线和表3的数据可知，算法在类似fr1代表的静态室内环境中误差值与变化波动都较小，而类似fr2代表的移动机器人上采集的数据序列，由于机器人的运动会有一些较大的误差波动，但经过本文算法处理后，误差会平缓减小，保证总体误差在较小的可接受范围。

4.3 建图效果评估

本节用公开的TUM数据集和实验室机器人平台自测数据进行实验，以便评估算法的建图效果。

4.3.1 TUM数据集建图

将本文算法与RGB-D SLAMv2算法构建的地图进行对比。图12（a）、（b）和（c）分别表示使用fr1/xyz、fr1/desk2和fr2/pioneer_360数据序列构建的三维地图，每组对比图的左边是用RGB-D SLAMv2算法构建的地图，右边是用本文算法构建的地图。可见RGB-D SLAMv2算法和本文算法都能有效地构建环境地图，有较好的三维重建效果。其中对于室内具有较多物件的丰富场景，两种算法构建的地图没有明显的优劣差距，但是对于大范围且较空旷的环境，改进算法的优势比较明显，构建的地图更清晰直观。

4.3.2 自测实验场景建图

图13 实验平台及环境

图14 自测实验场景下的建图对比

图13 给出了机器人平台和实验环境。图（a）为自主开发的多履带式全向移动机器人，图（b）是实验场景。图14（a）、（b）为两种算法构建出的环境地图对比，图（c）是由图（b）转换的Octomap。在实验过程中，发现机器人在跑RGB-D SLAMv2算法时会出现算法中断的现象，为保证实际建图，用手柄操作机器人在实验室中间空旷地面向前移动，均匀且缓慢运行。相比之下，本文算法运行较稳定，在一定程度上能减少大部分的光照干扰，使用关键帧的建图方式忽略了不必要的部分场景，地图更简洁直观。

5 结束语

本文提出了一种改进的机器人RGB-D SLAM算法，通过Q-tree均匀算法改进的ORB特征提取能有效剔除冗余特征点，使得保存下来的较优特征点均匀分布，从而促进良好的点对匹配，减少了错误匹配对机器人位姿估计带来的不利影响；在解算机器人位姿时，应用EPNP和ICP算法的组合优化模式，充分利用更多数据在迭代过程中获得最优的思想，提升机器人位姿估计的准确度；进一步地应用关键帧的有效回环和图优化技术减少累积误差，达到使优化后的运动轨迹和地图全局一致的目的。基于实验研究和对比，在TUM数据集和机器人平台的实验结果表明，算法的实时性及鲁棒性提高，估计的轨迹能较好地贴合数据集的真实轨迹，重建出清晰直观的场景地图。

进一步的研究会集中在联合激光雷达、IMU等较精确的设备优化位姿估计，引入深度学习方法赋予机器人语义识别能力并创建实时八叉树地图实现机器人的自主导航控制等。