基于ORB特征的改进RGB-D视觉里程计

刘慧杰，陈 强

（上海工程技术大学 电子电气工程学院，上海 201620）

0 引言

基于视觉的同时定位与建图（Visual Simultaneous Localization and Mapping，VSLAM），是一种采用视觉传感器实现自主移动机器人定位和构建地图的技术，其中定位的精确性和实时性至关重要。视觉里程计（Visual Odometry，VO）[1]作为VSLAM的重要组成部分，利用单个或多个相机采集的图像信息估计智能体的运动过程，已成功运用于自主移动机器人的导航定位中。

视觉里程计主要包括单目里程计、双目里程计和RGB-D里程计。自从微软发布第一款RGB-D相机Kinect以来，因其价格低廉、获取信息丰富、直接获取像素深度等优点，深受研究者的青睐。RGB-D里程计的实现方法有两种：一种是基于特征点的间接法，另一种是基于像素的直接法。基于特征点法的RGB-D里程计以其直观的框架、较低的计算成本和运行稳定的优点，广泛应用于机器人实时导航和三维重建中[2，3]。

学者们对基于特征点法的视觉里程计作了大量研究。Peter Henry[4]等使用SIFT方法提取特征点，使用随机采样一致性（Random Sample Consensus，RANSAC）方法对3D-3D匹配点对进行配准，通过ICP[5]方法求解相机的运动。不足之处是SIFT特征计算量较大，影响了系统的实时性。仇翔等人[6]提出PROSAC算法迭代估计机器人位姿，综合考虑深度信息和特征匹配质量。但是其提取的ORB特征点分布密集，导致图像局部特征信息丢失以及过多的误匹配，影响位姿估计的精度。张兆博等人[7]提出基于图优化的视觉里程计算法，结合关键帧初始位姿和位姿变换构建图模型，采用非线性最小二乘优化初始位姿。但是其估计模型单一致使位姿估计误差较大。

针对以上问题，本文提出一种基于ORB特征的改进RGB-D视觉里程计。由改进的四叉树均匀算法和RANSAC完成ORB特征的提取和匹配，然后对深度图进行处理，求解PnP完成3D-2D特征点的初始位姿估计，用三角测量更新深度缺失点的深度信息，对更新的3D-3D点构建最小二乘问题，采用光束平差法建立BA优化模型，使用g2o完成机器人的位姿优化。通过对比实验结果证明该方法在保证系统实时性的同时有效提高了视觉里程计的精确性。

1 RGB-D视觉里程计系统框架

本文RGB-D视觉里程计系统包含特征提取与匹配、位姿估计、BA优化三个部分，系统框图如图1所示。RGB-D视觉里程计实现步骤如下：

图1 RGB-D视觉里程计系统框图

1）由Kinect相机采集环境的彩色RGB图像和深度图，将其按照时间戳对齐。采用基于四叉树的ORB均匀算法和快速近似最近邻（Fast Library for Approximate Nearest Neighbors，FLANN）算法进行特征提取和匹配。

2）根据已知匹配的特征点，建立3D-2D和3D-3D运动估计模型，采用PnP和ICP的融合算法完成机器人位姿估计。

3）利用光束平差法建立BA问题，构建优化模型，使用g2o优化库求解得到位姿最优估计。

2 RGB-D视觉里程计算法

针对ORB图像特征，本文在得到由RANSAC剔除误匹配后的点对之后，提出基于3D-3D模型的ICP配准算法和结合三角测量的3D-2D模型PnP算法的融合优化算法。

整体算法流程图如图2所示。输入数据为Kinect深度相机采集的彩色RGB图像和深度图，利用时间戳将其对齐。由基于四叉树的ORB均匀算法提取特征，采用FLANN算法和RANSAC算法完成特征匹配和内点筛选。对相邻帧的深度图进行分析，特征点深度数据d=0时，利用直接线性变换求解3D-2D对应点的PNP问题，得到机器人的粗略位姿，然后利用位姿信息和三角测量更新3D-2D对应点的3D相机坐标；若深度信息无缺失，保留3D-3D匹配对。对混合3D-3D匹配点对进行ICP优化，利用光束平差法建立优化模型，使用g2o优化库求解得到特征点及位姿最优估计。

图2 融合算法流程图

2.1 ORB特征提取与匹配

计算机视觉领域的研究者设计了许多稳定的局部图像特征，先后提出了SIFT（Scale-Invariant Feature Transform）、SURF（Speed-up Robust Features）、ORB（Oriented FAST and Rotated BRIEF）[8]等特征点提取算法，ORB算法能够满足VO的精度和实时性要求。因此，本文选择ORB算法进行特征提取和匹配。

对于场景特征分布均匀的图像中提取的特征点分布均匀，有利于匹配。但在实验中发现提取出的特征点呈现集群分布，不利于后续特征匹配。本文改进四叉树均匀算法，在算法[9]的基础上添加节点边缘检测，均匀提取特征点并减少特征点冗余。

初始化整个图像为父节点O，将RGB图像按面积四等分为四个区域，得到A，B，C，D子节点，构成四叉树初始结构。基于四叉树的ORB均匀算法步骤如下：

1）对RGB图像构建n层高斯图像金字塔，按照尺度因子设置每层提取的特征点数量。对每层图像提取的“Oriented FAST”关键点做四叉树划分。第α层应分配的特征点数量为：

式(1)中s为尺度因子（0

2）在4个子节点中进行ORB特征点提取，检测跨节点分布的冗余特征点，将其剔除。判断子节点中的特征点数量，若子节点所含特征点个数大于1，再次对各子节点四分割并提取特征点。反之，停止四叉树划分。

3）重复步骤2）。直到各子节点所含特征点个数小于等于1或者达到期望提取的ORB特征点数量为止。

4）对四叉树子节点中的提取的关键点计算Harris响应值，选择该子节点中响应值最大的关键点为最终提取结果。

提取了图像的ORB特征后，需要确定两帧图像的对应关系，实现特征点的匹配。随着特征点的数量的增加，常用的暴力匹配法的计算开销已经不能满足VO的实时性，本文采用快速近似最近邻FLANN算法[10]实现特征匹配。但是经过以上处理的匹配中存在大量的误匹配，错误的匹配输入会导致PnP和ICP算法优化迭代陷入局部最优或无法收敛，不能达到全局最优值。首先对匹配对采取预处理，选出描述子之间汉明距离小于最小距离两倍的匹配对，之后使用RANSAC算法剔除离群点，筛选出正确的匹配对。

2.2 运动估计与位姿优化

视觉里程计实现相邻两帧图像间的机器人位姿估计。一般有两种情形，一种是求解3D到2D匹配点对的PnP优化问题，另一种就是3D-3D匹配点对的ICP优化问题。一旦求得3D点的相机坐标，即可将3D-2D点转换为3D-3D对应点，采取ICP优化。根据3D-2D和3D-3D运动模型，设计基于PnP和ICP的融合算法。

1）3D-2D运动估计

对于经RANSAC筛选的匹配对，考虑N个深度信息缺失的3D-2D特征点对，此时PnP问题描述为已知N个3D空间点及其投影位置时求解相机的位姿。已知某个3D空间点的齐次坐标为P=(x，Y，Z，1)T，其对应的2D投影点在归一化平面的齐次坐标为x=[u，v，1]T。相机内参矩阵K为标定好的已知量。定义增广矩阵[Rlt]为3×4的矩阵，含有12个未知参数A=[a1...a12]T，表示旋转与平移信息。

那么3D点到2D点的变换关系为：

对于N个3D-2D特征点，可得到以下方程组：

A是待求变量，其中A1=[a1...a12]T，A2=[a1...a12]T，A3=[a1...a12]T。每对3D-2D特征点提供了两个关于A的线性约束，由于A有12维，因此至少通过6对匹配点才能实现矩阵A的线性求解。当匹配点对大于6时，对超定方程使用SVD方法求其最小二乘解，得到初始相机位姿R，t，李代数表达为ζ0。

通过直接线性变换求解PnP问题得到了初始相机位姿，利用三角测量计算特征点的深度，更新特征点的空间位置。考虑相邻两帧图像为I1和I2，以I1为参考，两帧图像间的相机运动的以R21，t21表示。

在I1的相机坐标系下，设P的空间位置为P=[x，Y，Z]T，取x1和x2为特征点的归一化相机坐标，根据相机投影模型，两个匹配的特征点的像素坐标为：

特征点在两帧的深度值分别为S1和S2，变换关系为：

式两边左乘x1的反对称矩阵x1^，有：

其中R21t21为PnP求解的已知量，解式(6)右侧方程式，直接得到s2的值，同时s1的值亦可求得，从而得到了两帧下的点的深度，确定了匹配点在相机坐标系下的坐标。

2）3D-3D运动估计

对深度信息缺失的3D-2D特征点对，在PnP求解的结果上进行三角测量，得到了两个匹配特征点的深度信息，更新了特征点的相机空间坐标。结合深度信息已知的3D-3D匹配点对，将问题构建为混合3D-3D点的相机位姿优化模型。对于已知匹配的3D点集和其中变换后的点为存在一个欧式变换R，t满足：

对于匹配已知的3D-3D混合运动估计进行ICP优化，通过代数方法SVD或者非线性优化求解混合运动估计问题。

3）位姿优化

对于混合3D-3D点的相机位姿优化模型，采用位姿的李代数表示方法来构建无约束的优化问题，式(7)的李代数位姿表达为：

引入最小化误差函数优化机器人位姿，对上述问题构建BA模型：

其中e为3维向量，采用高斯牛顿法求解。算法流程如下：

Step1：初始值给定为PnP结果ζ0。

Step2：对每次迭代，求出当前的雅可比矩阵J与误差e。

e为空间坐标的误差（3维），相机位姿为6维，则J为一个3×6的矩阵，位姿的李代数定义为平移在前，旋转在后。在求导上使用李代数的左乘扰动模型[11]，对ζ∧左乘扰动量δξ，单个误差项关于扰动量相机位姿的导数为：

由M个3D-3D匹配点对组成的误差函数e的雅可比矩阵为：

Step3：第k次迭代，依据下式计算增量 Δζk：

Step4：如果Δζk足够小，就停止迭代。否则，更新返回Step2。

3 实验与结果分析

3.1 实验环境

为了验证本文算法的有效性，使用公开的RGB-D数据集进行对比实验，全部实验在配备小型上网本的TurtleBot2移动机器人平台上完成，如图3所示。实验平台支持ROS操作系统，上网本的配置为：Intel Core i5-7260U@2.20GHz×4CPU，238GB硬盘，操作系统为64位的Ubuntu16.04LTS。

图3 移动平台

3.2 特征提取与匹配对比实验

采用RGB-D数据集fr1/xyz中的相邻两帧图像进行对比实验，特征点分布对比如图4所示。图4(a)为原始ORB算法提取的特征点分布情况，可见ORB特征点分布密集，冗余特征点较多。图4(b)为采用四叉树均匀算法提取结果，提取的ORB特征点在整张图中分布均匀，其描述子更具有代表性，提高了像素信息的利用率，有利于良好的点对匹配。

图4 特征点分布对比

融合算法求解依赖正确的特征点匹配，误匹配会严重影响机器人的位姿优化。特征匹配处理对比实验结果如图5所示。如图5(a)匹配特征点未经RANSAC筛选，可见其存在大量的误匹配，会导致迭代优化算法无法收敛。图5(b)为采用RANSAC算法处理特征匹配，基本消除了误匹配，点对匹配正确率得到提高，有利于提高里程计的精确性。

图5 特征匹配对比

3.3 实时性和定位误差对比

本文使用RGB-D数据集fr1/xyz、fr1/rpy、fr1/desk进行离线实验。为了验证实时性，以每帧运行时间为参考标准，包括特征提取、匹配时间和迭代优化时间，三种方法都采用本文四叉树均匀算法和FLANN算法进行ORB提取与匹配，只是位姿优化算法不同。定位误差采用均方根误差（Root Mean Square Error，RMSE）作为评价标准。用RMSE表示真实轨迹和估计轨迹之间的偏差，以此来判断算法的精度。将本文算法与原始ICP算法、RANSAC+ICP算法在不同数据集下的RMSE和耗时进行对比，结果如表1所示。

表1 三种方法性能对比

由表1直观地看出，本文算法在满足里程计实时性的同时，其定位误差优于传统ICP和RANSAC+ICP算法。实时性方面，三种方法的平均每帧处理时间均在30ms左右，里程计计算迅速。定位准确性方面，本文算法的平均RMSE为0.036m，相比于原始ICP的0.078m和RANSAC+ICP算法的0.057m有着更小的误差，降幅分别为56.4%和40.4%。本文算法均匀提取ORB特征点，剔除了较多的误匹配，同时将深度信息缺失的特征点加入到位姿估计中，提高了位姿估计的精度。

4 结语

本文提出一种融合PnP与ICP的RGB-D视觉里程计方法，有效针对了深度信息缺失的问题，通过改进四叉树均匀算法实现ORB特征均匀分布，有利于良好的图像配准，减小了误匹配引起的位姿估计误差。在求解机器人位姿时，将深度缺失的特征点加入到运动估计中，在基于3D-3D模型的ICP配准算法和结合三角测量的3D-2D模型PnP算法的融合优化基础上，采用光束平差法建立BA模型，利用g2o求解器优化机器人位姿。实验结果表明该方法在满足系统实时性要求的同时有效提高了视觉里程计的精确性。