搭载RGB-D相机轮式机器人误匹配特征点筛除方法

李少波，张 伟，孙采鹰，任 彦

(内蒙古科技大学 信息工程学院，内蒙古 包头 014010)

1 引 言

特征点的提取与匹配是同步定位与建图(Simultaneous Localization and Mapping)领域中一项重要的研究内容[1]，是实现SLAM关键步骤之一，特征点匹配结果直接影响到机器人运动轨迹的计算精度.因此提高匹配正确率的关键是筛选出正确的匹配点对[2].目前，主流的特征点提取方法有文献[3]提出的尺度不变特征转换(Scale Invariant Feature Transform，SIFT)算法[4]，该算法在保持尺度、旋转缩放不变性方面具有优势，但是算法复杂度较高，计算量较大.文献[5]提出的加速稳健性特征(Speed-up Robust Feature Transform，SURF)算法解决了SIFT存在的问题，但在光照变化的场景中鲁棒性较差.文献[6]中Ethan Rublee等人首次提出快速特征点提取和描述(Oriented FAST and Rotated BRIEF，ORB)算法，该算法由FAST[7]特征点和BRIEF[8]描述符组成，FAST特征点又称为“Oriented FAST”；ORB算法不仅具有良好的实时性而且能够保持特征点具有旋转，尺度不变的特点.特征匹配是SLAM中数据关联的关键部分，精确的特征匹配为SLAM的位姿估计和后端优化提供精确的初值.目前，最简单的匹配方法是暴力匹配，但是当特征点数量很大时，运算量会增加.文献[9]提出的快速最近邻(Fast Library For Approximate Nearest Neighbors，FLANN)算法更加适合匹配点数很多的情况[10]，但是不可避免的存在误匹配.文献[11]、文献[12]中Fischler等人提出了随机采样一致性(Random Sample Consensus，RANSAC)算法，该算法是目前主流的匹配点对优化方法.然而RANSAC算法的计算量较大，效率不高.Ondrej Chum和 Jiri Matas[13]提出的PROSAC方法，该方法首先要对样本点与模型的相关性排序，然后再从相关性大的样本点对中进行抽样.虽然减少了抽样次数，但容易造成局部抽样，导致算法的鲁棒性下降.张永祥[14]等人提出的改进RANSAC基础矩阵方法能够减少误匹配，但是算法的运行时间较长.

视觉里程计是SLAM算法的关键环节，主要用于估计帧间的相机运动.本文以RGB-D视觉SLAM为背景，提出一种轮式水平地面运动机器人误匹配特征点对筛除方法.该方法用于SLAM的视觉里程计部分，对粗匹配的特征点对进行了二次处理.首先，本文利用ORB法对机器人采集的图像进行了特征点的提取，并用暴力匹配法对帧间图像进行了特征点粗匹配.然后，本文利用机器人的运动约束条件建立了帧间图像特征匹配点对的约束条件，并用该条件滤除了暴力匹配法产生的误匹配特征点对.最后，本文将优选出来的特征匹配点对用于RANSAC算法，估计帧间相机的运动.实验证明，基于等高约束条件和直线约束条件的误匹配特征点对筛除算法可有效的筛除帧间误匹配特征点对，同时能够提高RANSAC算法的执行速度，并且能够提高RANSAC算法对机器人相机的运动估计精度.

2 匹配特征点对约束条件

2.1 相机运动约束条件

平面运动轮式机器人相机运动约束条件如下：

1)机器人相机只能绕与地面垂直的轴旋转;

2)机器人相机垂直方向无平移.

相机坐标系以图1方式建立，规定z轴为相机光轴方向.机器人在水平地面的运动时，其相机的运动可以分解为旋转部分和平移部分.相机旋转运动只能以y轴为轴，而相机在y轴方向的平移分量为零.因此机器人相机的运动是在有约束条件下进行的，这种约束关系最终会体现在所采集的RGB-D图像上.

2.2 等高约束条件下的匹配特征点对约束

根据机器人的运动约束条件，当相机随机器人从当前位置运动到下一个位置时，相机坐标系运动特点如图2所示.

规定图2中o1x1y1z1相机坐标系为参考坐标系，其位姿矩阵为4×4单位阵[15]，o2x2y2z2为相机运动后的坐标系，由相机运动约束条件可知，相机光心o1与o2等高.o2x2y2z2坐标系是由o1x1y1z1绕y1轴旋转并且沿着x1和z1方向平移得到，所以，o2x2y2z2坐标系到o1x1y1z1坐标系的传输矩阵为：

(1)

式(1)中，θ为绕y1轴的旋转角度，tx为x1轴方向的平移量，tz为z1轴方向的平移量，因为机器人相机高度不变，所以y轴方向的平移量为0.

设空间一点P在o1x1y1z1坐标系中的齐次坐标为P1=[x1，y1，z1，1]T，在o2x2y2z2坐标系中的齐次坐标为P2=[x2，y2，z2，1]T，由式(1)可得：

(2)

由相机运动约束可得：

y1=y2

(3)

即，相机坐标系运动前后空间一点P的y轴坐标不变.式(3)为等高约束条件.

当相机坐标系为o1x1y1z1时，设P点的像素齐次坐标为A1=[u1，v1，1]T，当相机坐标系为o2x2y2z2，设P点像素齐次坐标为A2=[u2，v2，1]T.如图3所示.

图3 成像平面投影示意图Fig.3 Schematic diagram of imaging plane projection

相机内参矩阵为：

(4)

其逆矩阵为：

(5)

由相机成像模型可得：

A1=KP1

(6)

坐标A1与P1坐标的关系为：

P1=K-1A1

(7)

式(7)展开式为：

(8)

由式(8)可得：

(9)

同理可得：

(10)

式(9)与式(10)中的z1及z2可由深度图获得，v1及v2可由彩色图获得，相机内参可通过相机内参标定获得.由于相机径向畸变、切向畸变、深度值测量误差及机器人运动时相机轻微抖动等因素，式(9)式(10)计算出的y1和y2坐标往往不相等，同时，为了避免分母为零的情况出现，定义等高约束条件下的匹配特征点对约束为式(9)与式(10)的差值.为方便叙述，将等高约束条件下的匹配特征点对约束命名为等高约束.

(11)

式(11)中的Δy可以根据统计动态调节范围或简单的设定一个固定的值.

等高约束可筛除大部分误匹配点对，其中包括彩色图匹配正确但深度值误差较大的匹配点对，但无法筛除y轴高度相同的误匹配点对.

由于机器人运动速度较慢且图像采集频率较高，相邻两帧图像相机运动范围很小，因此像素坐标v1和v2非常接近.若v1和v2的值非常接近cy时，式(11)不能有效的筛除深度值误差较大的匹配点对，因此需要增加误匹配约束条件.

2.3 直线约束条件下的匹配特征点对约束

由式(2)可得：

x2=x1cosθ+z1sinθ+tx
z2=-x1sinθ+z1cosθ+tz

(12)

由于机器人运动速度较慢且图像采集频率较高，相邻两帧图像相机运动范围很小，所以θ值较小，式(12)可近似表示为：

(13)

整理式(13)得：

z1θ+tx=x2-x1

(14)

-x1θ+tz=z2-z1

(15)

对于RGB-D相机，当特征匹配点对的像素坐标已知时，式(14)、式(15)中的深度值z1、z2可直接由深度图得到，同时可由式(8)计算出x1、x2、y1、y2的空间坐标值.因此，式(14)、式(15)是直线方程，变量分别为平移量tx、tz及旋转量θ.

由于等高约束已经筛除了大部分的误匹配点，剩余少量的误匹配点对直线拟合效果影响很小，因此可使用所有经过等高约束过滤后的匹配特征点对，分别对式(14)、式(15)两条直线做拟合，并计算出直线方程中tx、tz及旋转量θ.距离式(14)、式(15)表示两条直线远的匹配点为误匹配点，该条件为直线约束条件下的匹配特征点对约束条件.为方便叙述，将直线约束条件下的匹配特征点对约束条件命名为直线约束.

等高约束式(11)与直线约束式(14) 、式(15)可通过较小的运算量有效的筛除误匹配的特征点对，其中包括像素匹配正确但深度值误差较大的匹配点对，为视觉里程计提供优质的特征匹配点对.

3 等高约束和直线约束与RANSAC算法的结合

随机采样一致性算法是目前主流的匹配点对优化方法，该算法的拟合模型参数由观测数据子集计算得到，从而找到足够多的有效样本点[16].

RANSAC算法原理包括3个阶段：抽样建模阶段、模型验证阶段、得出最优模型阶段.抽样建模阶段通过随机抽取样本生成模型；模型验证阶段将样本所有数据代入模型，根据内点误差阈值统计内点个数；得出最优模型阶段经过有限次迭代，选出内点数最多的模型.该模型为相机传输矩阵.RANSAC算法中的迭代次数由下式计算：

(16)

式(16)中，k表示迭代次数，z表示置信度，一般取z=0.99.n代表计算模型需要的数据点对数，取n=4，因为RANSAC使用P3P算法计算单应矩阵，P3P最少需要4对匹配点.ω表示内点在数据集中的比例.由于暴力匹配法产生的误匹配特征点对较多，导致ω较小，由式(16)可知，迭代次数k增大.即误匹配点对越多，迭代次数越多.本文采用等高约束条件和直线约束条件与RANSAC算法结合，在暴力匹配法产生的匹配点对的基础上进一步筛除误匹配，增加了正确匹配点对比例，因此内点的概率ω增大，迭代次数k随之减小，进而减少了算法的计算时间，提高了算法的效率.

因此，先使用本文提出的约束条件对误匹配特征点对进行筛除，再使用RANSAC算法计算相机运动.算法步骤如下：

Step1.获取第一组图中特征点的像素值及深度值.

Step2.计算该特征点对应的空间3D点在相机坐标系下的坐标.

Step3.获取第二组图中对应匹配特征点的像素值及深度值.

Step4.计算该匹配特征点的空间3D点在相机坐标系下的坐标.

Step5.计算式(11)中等高约束条件的阈值，筛除大于阈值的匹配点对.

Step6.使用step 5优选的匹配点对，根据式(14)、式(15)做两条直线拟合.

Step7.筛除到直线距离超过阈值的匹配点对.

Step8.保存经过两次筛选后的匹配点对.

Step9.将保留匹配点对作为RANSAC算法的输入计算相机运动.

4 实验结果和分析

4.1 基于MORO机器人算法验证

MORO是北京一维弦教育科技有限公司研发的一款轮式机器人，底盘配备有差动轮，并配置有RGB-D相机.如图4所示.

图4 MORO机器人Fig.4 MORO robot

本实验首先通过机器人采集实验室的相邻两帧图像，然后使用ORB算法进行了特征点提取，最后使用暴力匹配法对两帧图像的特征点进行了匹配.其中匹配结果如图5所示.图5(a)中存在大量误匹配结果，不能直接用于计算相机运动的计算，需要进一步筛选优化.参照式(11)与式(14)、(15)，在图5(a)匹配实验结果的基础上增加等高约束条件和直线约束条件.本实验等高条件阈值为20，直线约束条件阈值为0.15.

经等高约束条件和直线约束条件筛选后，图5(b)中保留的匹配点对全部正确.图5(c)显示结果为筛除的匹配点对，其中包括彩色图匹配错误点对和彩色图匹配正确但深度值误差较大的匹配点对.

图5 特征匹配点对筛选结果Fig.5 Feature matching point pair screening results

4.2 基于TUM数据集的算法验证

本文采用TUM数据集(rgbd_dataset_freiburg2_pioneer_slam3)对基于运动约束的匹配特征点对筛选方法进行了进一步的分析与验证.该数据集采用搭载RGB-D相机的轮式机器人采集图像，同时提供了采集每帧图像时相机的位姿信息，可以用于匹配点对误差分析.TUM数据集是机器人在水泥地面上采集的，且机器人运动过程中相机有轻微的抖动，因此本实验也可检验本文算法是否具有良好的适应性.

图6 特征匹配点对筛选结果Fig.6 Feature matching point pair screening results

本文首先采用RANSAC算法对所有暴力匹配特征点对进行运算，同时显示保留的匹配特征点对，实验结果如图6.图6(a)是两幅图像暴力匹配结果.图6(b)是RANSAC算法保留的匹配点对，可见匹配结果中仍有明显的误匹配.

图7 等高约束条件特征匹配点对筛选结果Fig.7 Contour constraint feature matching point pair filter results

图7(a)是经等高约束条件过滤后保留的匹配点对，可见等高约束条件可滤除图6(a)中大部分误匹配，但仍有少量的误匹配点对无法滤除.根据式(14)，对等高约束条件筛选后匹配点对进行直线拟合，其效果如图7(b)所示.可以根据式(15)做另一条直线的拟合，方法类似，不再累述.本实验等高约束条件阈值为30.

图8 直线约束结合等高约束特征匹配点对筛选结果Fig.8 Line constraint combined with contour Constraint feature matching point pair filter results

去掉离两条直线较远的点，其效果如图8(a)所示.可见保留的匹配点对全部正确.图8(b)是图7(a)去掉离线较远的点得到的.本实验等高约束条件阈值为30，两条直线约束条件阈值均为0.03.可以看出直线约束条件对匹配点对筛选效果进一步改善，滤除掉更多误匹配.

图9(a)是RANSAC算法筛除的误匹配点对，图中显示被筛选出来的误匹配较少.图9(b)是等高约束条件结合直线约束条件筛除的误匹配点对.可见加入等高约束条件和直线约束条件后对误匹配点对筛除效果更好.

4.3 匹配误差分析

为进一步验证算法的性能，本文采用两幅图像匹配特征点对应世界坐标的误差来评价配准精度．本文对图6(a)、图7(a)、图8(a)中的实验结果进行误差分析.在o1x1y1z1参考坐标系下，通过像素坐标A1计算空间坐标P1;同理，在o2x2y2z2坐标系下，通过像素坐标A2计算得到空间坐标P2，然后通过传输矩阵T将P2变换到o1x1y1z1坐标系下.理论上这两个点应完全重合，但由于实际的参数误差及采样误差，两个点之间会存在一定的误差.误差计算公式如下：

e[i]=norm(z1[i]k-1A1[i]-z2[i]Tk-1A2[i])

(17)

式(17)中，e[i]为第i组匹配点误差的模，A1[i]为第一帧图像中第i组匹配点像素坐标，z1[i]是A1[i]对应深度值，A2[i]为第二帧图像中第i组匹配点像素坐标，z2[i]是A2[i]对应深度值，k为相机内参矩阵，T为o1x1y1z1坐标系到o2x2y2z2坐标系的传输矩阵，T可以通过数据集中提供的相机位姿计算.

图10(a)是图6(a)中两幅图像暴力匹配特征点对误差统计图，包含正确匹配误差和错误匹配误差.图10(b)是图7(a)中经等高约束条件过滤后保留的匹配点对误差统计图，可以看出等高约束能够有效的筛除大部分的误匹配，并提高了匹配精度.图10(c)是图8(b)中直线约束条件在等高约束条件优化的基础上保留的匹配点对误差统计图，加入直线约束条件后误匹配筛除能力进一步提升，保留特征点的匹配误差都下降到0.5以下.因此，本文提出的等高约束条件和直线约束条件能有效筛除误匹配，为RANSAC算法计算相机运动提供更加准确的匹配特征点对.

本文对传统RANSAC算法与结合了等高约束条件、直线约束条件的RANSAC算法性能进行了比较.首先进行了两种算法的误差比较，然后进行了运算时间比较.误差比较如图11所示.

图11(a)为平移误差，菱形点为RANSAC算法平移误差，五角形点是结合了等高约束条件的RANSAC算法平移误差，圆形点是结合了等高约束条件与直线约束条件的RANSAC算法平移误差.由图可知仅使用RANSAC算法计算的相机平移量误差相对较大，结合等高约束条件后，平移误差有明显的下降，但也存在少量匹配点误差较大的情况.结合了等高约束与直线约束条件后，RANSAC算法计算的平移误差进一步减小.

图10 特征点对应坐标误差分析结果Fig.10 Coordinate corresponding to characteristicpoints error analysis results

图11 平移误差旋转误差评估结果Fig.11 Translation error rotation errorevaluation results

图11(b)为相机旋转误差，评估量为垂直方向转角.菱形点为RANSAC算法旋转误差，五角形点是结合了等高约束条件的RANSAC算法旋转误差，圆形点是结合了等高约束条件与直线约束条件的RANSAC算法旋转误差.由图可知仅使用RANSAC算法时存在较大旋转误差，结合等高约束条件的RANSAC算法可有效降低旋转误差，但仍存在少量误差较大的点.在此基础上加入直线约束条件，旋转误差误差更小，达到进一步优化的效果.

表1 算法时间比较Table 1 Time comparison of algorithms

表1为3种算法运算时间统计.结合等高约束的RANSAC算法与传统RANSAC相比，计算时间减少了29.55%.结合等高约束与直线约束的RANSAC算法与传统RANSAC相比，计算时间减少了17.27%.

5 结束语

视觉里程计是视觉SLAM的重要环节，而正确的帧间特征点提取与匹配是视觉里程计的基础.本文根据装配RGB-D相机的平面运动轮式机器人的运动约束，提出了帧间特征点对匹配约束条件，分别为等高约束条件与直线约束条件.这两个约束条件可以有效的去除帧间误匹配特征点对，从而到了理想的特征点匹配点对.等高约束与直线约束对各种室内平面场景有很强的抗干扰能力，对非严格水平地面也有较好的适应性.同时，本文约束条件可与RANSAC算法相结合使用来估计帧间运动.实验结果表明，添加了本文约束条件的RANSAC算法估计相机帧间运动耗时更少，误差更小.

本文提出约束条件具有以下的优点：

1)对错误匹配点对的识别能力强;

2)可剔除匹配正确但深度值误差大的匹配点对;

3)用较小的计算量有效的排除误匹配点对;

4)可与视觉里程计常规算法结合，提高算法性能.

该方法也存在一些不足：

对于某些非严格满足约束条件的场景(如平整度不高的水泥地面，或摄像头固定水平度差等情况)会删除少量的正确匹配点，以牺牲少量正确匹配点的代价换取更快的计算速度和更高的计算精度.