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

肖 晓， 孙新柱， 陈孟元

(安徽工程大学 安徽省电气传动与控制重点实验室， 安徽 芜湖 241000)

随着移动机器人在研究与应用过程中的不断发展，其所需要完成任务的形式、种类也变得多样化。移动机器人在完成自主导航的过程中，首先需要确定自身的位置，也就是实现机器人实时定位功能[1]。现代科学领域中比较常用的导航设备有激光测距仪(laser range finder)[21]、全球定位系统(Global Position System,GPS)[3]、红外(infrared)[4]和视觉里程计(visual odometry,VO)等[5]。激光雷达因能获取周围环境的点云数据，从而使定位精度高、实时性强，但实现成本较高。GPS定位装置可解出三维位置，但在室内及有遮挡的地方无法使用。红外定位实时性能强，但受环境影响较大，会产生很大的误差。视觉里程计主要是跟踪图像帧间的特征点，利用累计的帧间运动估计当前时刻位姿，并且在视觉里程计系统中形成三维点云[6]，以此作为路标存储在系统中。通过利用PnP(Perspectivenpoint)等方法进行定位。视觉里程计已经被广泛应用在无人机、无人驾驶等领域。

传统的相机因为视野受到限制，视觉里程计效果一般。RGB-D相机由于加入了与深度相关的数据，逐渐受到广大专家学者的喜爱。很多基于RGB-D传感器的SLAM算法也被提出[7-10]，取得了较好的效果。Henry等[11]提出的基于RGB-D相机的SLAM算法，主要是利用SIFT算法，对图像进行了特征检测和描述子计算，通过获取深度图像的数据，得到三维特征点对之间的信息。但是由于SIFT算法特征描述子维度过长，使特征匹配消耗大量时间，算法的实时性很难保证。仇翔等[12]提出结合特征匹配质量和深度信息，利用PROSAC算法对机器人帧间运行进行估计，并且运用g2o求解器进行局部优化，得到位姿的最优估计。但是这种方式并没有考虑ORB(Oriented FAST and Rotated BRIEF)算法在特征分布比较集中的情况下，提取特征比较密集，影响特征匹配精度。GAO X等[13]提出了一种在提取的平面上进行特征点匹配，虽然很大程度上减少了误匹配的发生，但此方法所应用的环境较为局限。Endres等[14]所使用的RGB-D SLAM方法，得到的相机变换矩阵仍然存在误差。所以RGB-D SALM前端仍有改进之处。

本文研究的重点是针对视觉里程计中误匹配问题作出优化，提出使用改进的ORB特征提取与匹配算法，使在特征比较密集的情况下，仍能准确匹配。为了减少系统计算量与丢帧现象，提出了一种关键帧提取与筛选策略，利用局部地图判别关键帧。

1 RGB-D视觉里程计算法

图1 视觉里程计框架

RGB-D视觉里程计算法主要是利用相机获取场景中的RGB图像和深度信息后，通过特征提取与匹配，确定当前路标与之前路标之间的对应关系。再利用位姿优化算法，提高位姿估计精度，获取实时的位姿信息。最后根据关键帧的筛选，构建出局部地图。本文运用利用改进的ORB特征提取与匹配算法，提高特征提取与匹配的准确率。使用ICP算法优化相机位姿估计，得出实时的位姿信息。并提出一种关键帧提取与筛选方法，构建出局部地图。其结构框架如图1所示。

2 算法介绍

2.1 特征点提取与匹配

特征点的判定和提取有很多算法,现在主流有SIFT、SURF、ORB等。ORB运行的速率快，其性能更加优越。本文选取ORB算法作为特征提取。在实验场景分布比较集中的情况下，ORB算法在提取特征点的过程中，存在特征点提取不均匀且误匹配较多的情况。针对这一问题提出一种利用四叉树的形式将特征信息进行划分，然后利用汉明距离对图像的特征信息进行匹配，为了提高匹配精度，通过粗筛选和RANSAC算法相结合从而实现特征的均匀分布减少误匹配[15]。

2.1.1 均匀化提取

ORB特征是由关键字和描述子组成，是目前较新的特征点检测与描述算法。对比于原版的FAST特征提取算法，ORB算法通过计算可以得出特征点的主方向，使它的描述子增加了旋转不变特性。

FAST(features from accelerated segment test)算法的原理主要是根据图像的灰度值判别。通过计算，检测出候选特征点N周围一圈的像素值，计算公式为

(1)

式中I(x)为圆周上任意一点的灰度，I(p)为圆心的灰度，εd为灰度值差的阈值。假如候选点周围领域内灰度值差别够大，就认为该候选点为一个特征点，并利用此方法来检测。

ORB算法增加了尺度和旋转的描述。通过构建图像金字塔从而实现尺度不变性，并用于金字塔的每一层角点的检测。而特征点旋转主要是由灰度质心法实现。利用矩来计算以r为半径范围内的特征点质心。特征点质心到坐标形成一个向量，作为该特征点的方向。矩定义如下：

mpq=∑xpyqI(x,y),

(2)

其中，p,q={0,1},I(x,y)为图像灰度表达式。而该矩的质心为

(3)

假如其角点的坐标为Q，则它的向量的角度为该特征点的方向，计算公式为

θ=arctan(m10/m01)。

(4)

因为ORB算法对图像所有区域的计算以及阈值的设定方式一致，因此在特征分布均匀的图像中提取的特征分布均匀，效果较好。但是在场景比较集中的情况下，提取特征分布就不均匀，所以本文提出采用改进四叉树的方式对特征进行划分，实现特征的均匀分布。首先需要构建图像金字塔，并计算出每一层提取的特征点数。计算公式为

(5)

式中n1为金字塔第一层所需分配的特征点数，α为尺度因子，n为所需要提取的ORB特征点总数，τ表示图像金字塔的层数。通过等比数列依次求出金字塔每层特征点数。当完成金字塔构建和实现特征点分配后，选定合适的格子对图像进行划分，同时在格子中利用FAST特征点检测，保证特征点分布均匀。根据四叉树的形式将图像划分。通过每一层分配的特征点数，确定四叉树的节点数。对节点进行判断，是否含有特征点，含有特征点则添加节点，当节点个数大于需分配的特征数或者所有的节点只有一个特征点的时候结束。最后对每个节点中的特征进行选取，选定最好的特征，从而实现均匀化处理。

2.1.2 特征点匹配

通过汉明距离对特征点进行粗匹配，匹配过程中可能存在伪匹配对造成的误差，这些点被称为外点。外点非常影响匹配的效果，所以需要对其筛选。假设有两幅待匹配的图片，设两幅待匹配图中的特征点分别为

Y={y1，y2，…,yk},

(6)

Z={z1,z2,…,zk},

(7)

图片中同一特征点构成的特征点子集对应表示为

M={m1,m2,…,mn},n≤k,

(8)

N={n1,n2,…,nn},n≤k,

(9)

特征点匹配对用(mi,ni)表示。将匹配点mi到特征点集Y中每个点的距离与匹配点ni到特征点集Z中每个点的距离依次进行比较，且满足如下公式：

|‖mi-yi‖2-‖ni-zi‖2|<δ,

(10)

式中δ为阈值。匹配对同时还要满足另一个条件：至少有两对匹配特征点，满足mi、mj与此两点在子集N中的匹配点ni、nj的距离相等。从而实现粗筛选，并且利用粗筛选算法与RANSAC结合，消除误匹配。最后产生一个相机位姿的变换矩阵。

2.2 位姿优化

将估算出的变换矩阵作为ICP算法的初始预估矩阵，结合使用经过预处理的点云数据，能够使变换矩阵更精确。ICP算法如下：

现假设经过预处理的目标点云数据为P={p1,p2,…,pM}，源点云为Q={p1,p2,…,pN}，其中点云P和Q经过预处理以后点云的数量为M、N。设在目标点云数据集P中选择初始的点集Pm1，利用k-D tree方法求出点集Pm1在数据点集Q中的最近点集Qm1，根据欧式距离，设置阈值剔除点云中的错误点对。并且通过应用方向向量的阈值确定出最终的对应点集Pm1、Qm1。最后使用非线性优化方法求得点集Pm1、Qm1之间的旋转矩阵R1和平移矢量T1，将计算出的R1和T1代入公式：

Pm2=R1Pm1+T1,

(11)

通过计算得出数据点集Pm2，假设第k次迭代得到的旋转矩阵为Rk，平移矢量为Tk，经循环计算得到

Pm(k+1)=RkPm1+Tk。

(12)

当迭代满足设置的终止条件时，则停止算法的计算。终止条件满足

(13)

其中ε为设置的欧式距离阈值，dk为第k次迭代后对应点的距离。

2.3 关键帧筛选

图2 局部地图判别关键帧

关键帧的选取在整个视觉里程计中起着重要作用。两个相邻的关键帧之间的距离太近，则说明提取关键帧过多，系统的计算资源浪费；相邻关键帧之间的距离过远，则会使特征点之间的匹配精度降低，甚至会出现关键帧之间特征点匹配失败，造成跟踪丢失。本文提出一种改进的关键帧筛选与剔除算法。首先通过计算可以得到初始的局部地图，并且设置第一帧为关键帧。为了给特征提取与匹配提供时间，设定关键帧选取的间隔为10帧。10帧以后再对下一个关键帧判断，这样使前端算法能够有足够的时间完成。然后通过距离判断机器人的运动量，设置阈值，判断运动量是否在阈值范围内。对关键帧进行初步的筛选。为了使机器人在快速运动中仍然能够提供较为准确的位姿估计，通过局部地图判别关键帧。局部地图是由每个关键帧的特征点构成，局部地图的每一个点都有观测记录。利用关键帧中的特征点匹配三维点云中对应的三维点，设置阈值θ，从而确定是否为关键帧。为了避免关键帧冗余，最后需要对关键帧进行二次判断，通过帧与帧之间的特征相似程度再次准确判断。如果相同特征之比超过阈值，则判断为冗余关键帧，剔除。同时利用筛选后的关键帧构建局部地图(如图2)。

3 实验与分析

实验场景选取9 m×6 m的室内环境。利用移动机器人的实验平台搭载kinect相机。完成文中所提方法进行实验与评估。图3为实验场景及实验平台。

图3 实验场景及平台

3.1 特征点均匀化与匹配实验

视觉里程计中，合理的特征点提取是其他步骤的前提，本文利用ORB特征提取优化前后对比实验如图4所示，可得出进行ORB特征提取均匀化处理前后的特征分布的明显差异。在均匀化之前，特征提取分布比较集中。而均匀化处理后，ORB特征分布较均匀。从而证明本文所提方法有效。当提取结束后为实现定位必须要对特征进行匹配，如图5所示。通过滤除误匹配和优化后，剔除了大量的ORB特征误匹配，减少特征匹配数量，不仅为运动估计节省的大量时间，也提高了最终的运动估计精度。表1为特征匹配实验后原始匹配对数数量和优化前后匹配对数数量。

(a) 均匀化前 (b) 均匀化后图4 特征点提取优化前后分布情况

表1 特征点匹配数

图5 特征点均匀化后优化匹配实验

3.2 整体算法评估

通过实验，在实际场景中构建局部地图。因为使用关键帧筛选方法，局部地图的规模很小，所占用的计算资源和内存也非常有限，从而加快了系统运行速度。为了体现算法优势，实验选取直线加速、快速转弯、原地运动3种不同的运动状态来证实实验结果，并且截取了实验过程中的局部地图。图6(a)为直线加速运动，通过改进的算法，减少了机器人因为速度过快而造成的匹配失败、跟踪丢失等问题。同时通过关键帧选取与筛选规则，加强了图像之间的关联约束。图6(b，c)为机器人快速转弯与原地运动的局部地图。如果使用编码器或者IMU，机器人在转弯和原地运动时会出现测量误差，定位失准。而关键帧的筛选和局部地图的建立可以保证在转弯过快旋转过程中仍然能准确定位。

(a) 直线加速 (b) 快速转弯 (c) 原地运动图6 视觉里程计局部地图

3.3 导航误差分析

移动机器人在实际运动中真实轨迹会与计算得出的轨迹产生误差。本文采用均方根误差(Root Mean Square Error,RMSE)方法作为评判算法精度的标准：

(14)

4 结束语

本文提出的视觉惯性里程计系统，一方面改进了ORB特征提取的不均匀问题，有效提高了特征点提取匹配的精度。同时采用关键帧选取策略，加快运行速度，减少了特征丢失。实验结果表明该方法能有效提高机器人定位的精度与鲁棒性，但是本文没有考虑光线变化、地形等因素，这些因素的障碍有可能也会导致定位失败，下一步将把这些因素考虑进去作为研究重点。

图7 实验误差对比