基于改进ORB算法的视觉里程计定位方法

张天宇，吴怀宇，陈志环

(武汉科技大学 机器人与智能系统研究院，湖北 武汉 430081)

0 引 言

视觉同时定位与地图构建(vision simultaneous localization and mapping，V-SLAM)是一种通过视觉传感器来建立环境模型和估算自身位姿的系统。该系统中视觉里程计的实现方法主要有直接法[1]和特征点法[2-6]两种。由于特征点法具有较高的稳定性和较好的鲁棒性，所以特征点法一直都是当前SLAM系统的主流方案。ORB(oriented fast and rotated brief)算法是特征点法中表现较突出的一种方法。

因此很多学者为了提高视觉里程计的精度，围绕ORB算法进行研究和改进。Xin等[7]利用PROSAC算法剔除ORB特征匹配中的误匹配提高算法精度。姚海芳等[8]综合SURF算法的鲁棒性和ORB算法实时性，提出了一种S-ORB来改善算法的实时性和准确性。李艳山等[9]在提取特征ORB点时，采用二分区域法对图像进行预处理，以缩短特征点提取的时间，较好改善匹配速度和精度。

以上研究者所提出的方法虽能较好完成特征提取、特征匹配和误匹配剔除，提高视觉里程计的定位精度。但在上述方法大多采用固定阈值进行特征提取，并未考虑到环境亮度变化对特征提取的影响。例如在一些实际环境中，由于窗户和地面反光、阴雨天以及物体遮挡产生阴影等原因，会使得图像亮度发生变化，导致特征提取和匹配的结果并不准确，从而使视觉里程计的定位精度降低。本文针对灰度变化提出一种自适应阈值ORB算法，并引入渐进一致性算法剔除误匹配，旨在提高视觉里程计的定位精度。

1 本文工作

1.1 视觉里程计框架介绍

视觉里程计目标是根据相机拍摄的图像信息对相机的运动(包括相机平移T和旋转R)进行估算，获取实时的位姿信息。本文提出一种基于改进ORB算法的视觉里程计定位方法，具体的视觉里程计框架如图1所示，首先对Kinect相机采集的深度图和彩色图并进行配准，然后在特征匹配阶段根据图像的灰度变化设置ORB特征提取的阈值并使用渐进一致性算法(PROSSAC)进行误匹配剔除，提高特征提取和匹配的准确率，再结合Kinect相机的深度图和彩色图配准的结果采用ICP算法来求解相机位姿，最后用光束法平差(bundle adjustment，BA)对位姿图进行优化，获取Kinect相机的实时位姿信息以及轨迹。

图1 视觉里程计框架

1.2 三维点云生成

为了配合ICP求解位姿，需要将二维像素点转化为三维空间点，生成三维点云。首先采用张正友标定方法[10]对相机进行标定和配准，获取相机的内参矩阵K，如式(1)所示

(1)

其中，fx，fy分别是x，y轴上的焦距；cx，cy为像素坐标系和成像平面的平移量。

结合相机内参矩阵，利用针孔模型将二维像素点转化为对应的三维坐标点，设一个图像特征点的像素坐标 (u，v) 和相机的测量深度d，可以求得特征点在三维空间的位置坐标 (x，y，z)， 计算如式(2)～式(4)所示

x=(u-cx)·z/fx

(2)

y=(v-cx)·z/fy

(3)

z=d/s

(4)

其中，s为Kinect相机的缩放因子。

1.3 ORB特征提取和匹配

ORB算子是一种定向二进制描述符，由FAST关键子和BRIEF描述子组成，该算子与SURF算子、SIFT算子相比，具有更快的特征提取速度，因此采用速度更快的ORB算子更符合里程计定位实时性要求。

1.3.1 ORB特征提取及其改进

ORB算法的特征提取部分由FAST算法来完成，该方法主要检测局部像素灰度变化明显的位置，考虑中心像素点p，设中心像素点的灰度为Ip，如图2所示。

图2 FAST特征点

假设中心点附近的圆形区域上的16个像素，有12个连续点的灰度满足条件如式(5)，若满足，则为特征点，反正，不是特征点。其中T为阈值，Ix代表圆形区域上的16个点的像素

∑x∈circle(p)|Ix-Ip|>T

(5)

但是在特征提取时，传统ORB提取算法所使用的阈值T是人为设定的固定值，当环境亮度发生变化时，相机采集的图片亮度也会变化，特征提取的数量会剧烈减少或者增加，这会导致特征提取错误和特征匹配错误，故本文采用一种自适应阈值的方法，利用每个候选点周围矩形区域的亮度来求取阈值，使得算法能在环境亮度变化的情况能提取到稳定的特征点。

具体的自适应阈值计算方法如下：设图像上的p点(x0，y0)为候选特征点，取以(x0，y0)为中心且边长为L方形区域，如图2中矩形方框所示，定义特征阈值为T，计算公式如下

(6)

其中，Imax和Imin分别代表方形区域中最大的n个灰度值和最小的n个灰度值，Iarer为方形区域灰度的平均值，a为比例系数。文中a=3。

改进后的ORB特征提取流程如下：

(1)首先计算圆形区域中心点p的像素灰度Ip，再根据式(6)计算特征提取的阈值T。

(2)对p像素点附近的圆形区域上的16个像素进行采样，每间隔3个像素点采样一次，总共选取4个点，本文选取像素点分别为1，5，9，13(也可以取像素点2，6，10，14)，设这些点的像素灰度分别是I1、I5、I9、I13，判断这些点的灰度是否满足式(5)，若满足，则p点为候选特征点，反之舍去。

(3)计算圆形区域上剩下的12个像素，设它们的像素灰度为Ix，判断是否有9个点像素满足式(5)，若存在，则p点为特征点，否则不是特征点。

为了实现特征点的旋转不变性，通过灰度质心法来确定特征点的方向。在以p点为圆心，半径为3的圆形图像块内，定义(x，y)为图像块内的点相对于圆心的坐标，质心矩mab、质心C和特征方向θ如式(7)～式(9)所示

(7)

C=(m10/m00,m01/m00)

(8)

θ=arctan(m01/m10)

(9)

其中，m00为0阶质心矩；m10和m01分别代表x轴和y轴方向的1阶质心矩。

1.3.2 ORB特征匹配与误匹配剔除

特征匹配的作用主要是通过两张图片的空间位置关系求解，以获取相机的位姿。通常采用FLANN方法来进行粗匹配，然后继续使用RANSAC(random sample consensus)对错误匹配进行剔除，提高匹配的准确率。传统RANSAC算法虽然能剔除误匹配，但是该剔除误匹配的过程具有随机性，这会导致计算的迭代次数不稳定，因此本文采用PROSAC(progressive sampling consensus)算法对误匹配进行剔除，相比RANSAC方法均匀地从整个集合中采样，PROSAC则是先将数据排序，再按照质量和的高低来选取数据，所以这种方法具有更高的效率和准确性。该方法通常是将两幅图片的匹配结果作为一个采样集合un，再根据匹配对的质量方程进行降序排序[11]，最后从质量较高的匹配对中随机抽取一部分匹配对计算参数模型和内点数目，匹配质量的判断如式(10)

(10)

其中，βi和βj分别代表ui和uj样本点的最小欧式距离和次小欧氏距离的比值，γ为质量方程。

具体的PROSAC剔除误匹配的流程如下：

(1)首先设置初始迭代次数为0，同时根据需求设置最大迭代次数和内点数目阈值。

(2)将当前的迭代次数和最大迭代次数进行比较，若当前迭代次数大于最大迭代次数，找不到对应的模型参数，反之则继续执行步骤(3)。

(3)按照式(10)依次比较匹配对的质量大小并进行质量降序排序，同时选取质量最高的m个数据。

(4)从m个数据中随机选取K个采样点计算单应矩阵H和内点数目。同时判断内点数目是否大于设定的内点数目阈值，若大于，则返回单应矩阵以及内点数目，反之，重复步骤(2)、步骤(3)更新内点数目并且重新计算单应矩阵H。

1.4 ICP位姿求解

剔除误匹配之后得到较准确的匹配对，需要对相机的位姿进行求解。常用的位姿求解方法有PNP、ICP和对极几何方法，由于使用的深度相机采集图片信息深度是已知的，选择ICP方法进行位姿求解效率会更高。ICP方法是一种3D-3D的位姿估算方法，已知两帧图片分别为F1和F2，其中P={p1，…，p2}∈F1和Q={q1，…，q2}∈F2为一组匹配好的3D点，主要利用SVD[12]分解对相机位姿进行求解，即求取旋转矩阵R和平移矩阵T，使其满足式(11)

∀i,pi=Rqi+T

(11)

1.5 基于光速法平差的位姿图优化

因为相机初始估算的位姿存在未知噪声和误差，会导致定位不准确。通常采用光速法平差将相机位姿最优化处理，将路标点和位姿作为代优化的变量，运动模型和观测模型作为约束条件。基于光速法平差的图优化模型可以表示为一个最小二乘问题，模型主要对位姿xk进行优化，那么目标函数F(X)可以表示为

F(X)=∑(i,j)∈ce(xi,xj,zij)TΩije(xi,xj,zij)

(12)

X*=argminF(X)

(13)

其中，F(X) 为目标函数，X是全部位姿变量的集合，xi，xj是i、j时刻估计出的机器人位姿，zij表示i、j时刻的观测约束条件，e为实际的观测值和计算出来的观测值的误差。Ωij代表约束信息矩阵，是对ei、ej误差项相关性的预估。

由于文中的观测方程是非线性函数，因此需要使用g2o[13]工具求解上述代价函数，该方法采用列文伯格-马夸尔特方法作为非线性优化的求解器，向求解器中添加当前位姿和图像特征点(路标点)，并将位姿和路标点作为g2o图的顶点，以位姿和路标点的连线作为g2o图的边，对g2o图的两个顶点和一条边进行优化，得到最优位姿的集合和路标点的空间位置集合。

2 实验结果与分析

为了验证本文算法的可行性和准确性，使用德国慕尼黑工业大学公开的TUM标准数据集的3个不同场景进行测试。笔记本电脑的配置：内存4 G、CPU为intel i5-4210U，2.5 GHZ，电脑操作系统为Ubuntu16.04。软件平台采用OpenCV3.2.0、PCL1.8.0和Eigen库。本文首先测试改进ORB特征提取和特征匹配算法的适应性和精度，最后使用改进后的ORB算法进行跟踪定位，并且与RGB-D SLAMv2算法和ORB-SLAM2算法的实验结果进行对比，最后使用JS-R移动机器人验证改进方法的可行性。

2.1 改进ORB算法实验验证

2.1.1 特征提取和匹配实验

为了验证改进ORB提取算法的对图像整体亮度变化的适应性，实验分为A、B、C(亮度50%、亮度100%和亮度150%)3组进行特征提取，并将传统ORB算法与改进的ORB算法进行对比，特征提取结果如图3所示。由于图像在亮度增强时提取到的特征点会变多而亮度减弱时提取到的特征点会变少，传统ORB算法在亮度变化时并不能提取稳定的特征点数量，为了验证改进的算法的特征提取效果，采用特征点数量作为判断亮度适应性好坏的标准。

图3 不同亮度下特征提取结果

从表1可以看出改进的ORB特征提取算法在光线变暗或者变亮的情况下，检测到的特征点数量变化较小。而传统的ORB特征提取算法在图像亮度增强时，特征点重叠较为严重；在亮度减弱时，特征点的数量会剧烈减少，亮度的变化对其影响较大。实验结果表明，改进ORB特征提取算法的在整体亮度的变化时能提取到更稳定的特征点，具有更好的适应性。

进一步测试改进的ORB算法在亮度变化时的匹配情况，并且与传统ORB特征匹配算法对比。在强光、弱光以及强弱光变化时，传统ORB特征匹配会产生较多的误匹配，因此实验主要在强光环境、弱光环境和强弱光变化3种环境中来验证改进ORB特征提取的效果。测试结果如图4所示。

表1 特征提取结果

图4 改进的ORB特征匹配结果对比

图4是改进的ORB特征匹配算法和传统ORB特征匹配算法的实验结果，从图4中可以看出传统ORB算法匹配时，会出现明显的交叉错误匹配对，特别的在强弱光变化的情况下交叉误匹配对更多，而改进后的ORB算法在3种光照环境中均未出现明显错误的匹配对，说明改进后ORB算法的特征匹配结果更为精确。

2.1.2 PROSAC误匹配剔除实验

为了验证PROSAC算法剔除误匹配的高效性，将ORB+PROSAC方法、传统ORB+RANSAC方法以及未进行误匹配剔除的ORB进行对比，测试结果如图5，图5中RANSAC算法和PROSAC算法都成功剔除了误差，没有出现图5(a)中明显的交叉匹配情况。为了更好评估PROSAC算法剔除误匹配的性能，进一步比较PROSAC算法和RANSAC算法的剔除情况，本文使用匹配总时间、剔除误匹配时间以及内点数作为评估指标，实验结果见表2。

图5 匹配效果对比

表2 RANSAC和PROSAC算法比较

从表2中可以看出来，PROSAC算法剔除误匹配与RANSAC算法剔除误匹配相比获得的内点数要更多，说明PROSAC算法获得正确模型的成功率也较高，因此匹配的正确率也会更高，另一方面该算法剔除误匹配时间和匹配总时间都较RANSAC算法要少，表明利用PROSAC算法剔除误匹配会更加高效和准确，进一步提高了相机定位的实时性和精度。

2.2 定位实验结果和误差分析

由于室内环境中容易出现阴影、桌子遮挡和地面反光等情况，导致相机采集的图片亮度发生变化，因此选择TUM标准数据集中室内场景fr1/desk、fr1/floor和fr1/room这3个序列来测试，并且与RGB-D SLAMv2算法和ORB_SLAM2算法进行对比，最后使用evo工具画出3种算法的轨迹图，并且对3种算法进行评估和分析。

为了对测试结果进行量化评估，通常会采用绝对轨迹误差(ATE)的均方根(root mean square error，RMSE)误差作为相机定位效果的量化指标。假设相机估算轨迹序列是X={X1，…，Xn}， 相机的实际运动轨迹序列Y={Y1，…，Yn}， 那么相机的绝对轨迹误差的均方根误差可表示为

(14)

图6是在3种算法在不同场景下相机运动轨迹和真实轨迹对比结果，从图6中可以看出改进后的算法轨迹图没有明显的偏离真实轨迹，与真实轨迹基本一致。通过绝对轨迹误差的均方根误差对实验结果进行量化评估，结果见表3，其中RMSE值越小，说明相机偏离真实轨迹的距离越大，相应相机的定位精度越低，反之说明相机偏离真实轨迹距离越小，相机的定位精度越高。

表3 不同场景下的RMSE对比

从表3中可以看出，在fr1/desk和fr1/floor场景下，改进算法的绝对轨迹均方根误差仅为0.0161 m和0.0154 m，且均低于ORB_SLAM2和RGB-D SLAMva。特别的在fr1/floor场景其绝对轨迹误差的均方根误差较其它两种算法分别降低了26.6%和15.2%。而在fr1/room场景下其绝对轨迹的均方根误差为0.0583 m，虽略大于ORB_SLAM2算法绝对轨迹均方根误差，但是误差小于RGB-D SLAMva算法的绝对轨迹均方根误差。实验结果表明，在大部分情况下本文提出的方法的绝对轨迹均方根误差都要小于另外两种方法，说明改进后的视觉里程计的定位精度更高。

2.3 实际场景测试

为验证改进后的算法在实际场景中的可行性，实验借助JS-R移动机器人来测试算法性能，JS-R移动机器人搭载Kinectv1相机，如图7(b)所示。图7(a)是实验室测试场地，控制机器人沿着标记线方向运动，用evo工具画出JS-R移动机器人的运动轨迹，测试的结果如图8所示，轨迹图的两端点分别是JS-R移动机器人的起点和终点，用START和END来表示。

图6 不同场景下各种算法轨迹

图7 实验场景和实验平台

图8 相机轨迹

由于实际groundtruth数据需要专业设备获取，本文无法获取移动机器人的真实轨迹，故选择图7(a)黑色线作为参考轨迹，图8的START和END分别对应图7(a)中的START和END，通过对比相机轨迹和红色参考轨迹发现相机运动较为稳定，其轨迹较为平滑并未出现异常的偏离现象，表明改进后的视觉里程计定位较为准确，满足实际工程中的需求。

3 结束语

本文提出一种基于改进ORB算法的视觉里程计定位方法，主要针对光照变化对特征提取和匹配的影响，提出一种基于自适应阈值的ORB算法，提高特征提取的稳定性和特征匹配的精度，同时为解决传统误匹配剔除方法(RANSAC)迭代次数不稳定的问题，使用PROSAC来剔除误匹配，实验结果表明，改进后的ORB算法具有较强的光照鲁棒性，并且在误匹配剔除阶段效率得到大幅提升。最后通过改进的ORB算法进行跟踪和定位，实验数据表明，在大部分场景本文提出的方法绝对轨迹误差的均方根误差(RMSE)比其它算法更小，最后通过JS-R移动机器人在实际场景中进行测试，实验结果表明，改进后的算法较为稳定满足实际需求。综上所述，本文所提出的方法能够较准确的进行定位，具有良好的实际应用价值。另外，本文只对定位结果进行测试和比较，并未测试基于改进ORB算法的地图构建效果，为了进一步测试改进的视觉里程计的性能，下一步主要工作将针对地图构建效果进行测试和分析。