CN103559711B - 基于三维视觉系统图像特征和三维信息的运动估计方法 - Google Patents

Abstract

本发明公开了一种基于三维视觉系统图像特征和三维信息的运动估计方法，该方法基于三维视觉系统实时获取场景二维图像信息和按二维图像像素对应的空间三维信息；采用相邻帧采样间隔自适应调整方法控制相邻帧采集间隔，根据t-1时刻和t时刻相邻帧之间有效匹配图像特征点对数目，自动调整t+1时刻相对t时刻的采样间隔，在满足三维估计准确性和可靠性的基础上有效减少了计算量；根据当前相邻帧二维图像特征匹配点对数目，综合场景二维图像特征和三维点云信息进行三维运动估计，获得高精度的三维运动估计，具有计算量小，应用场景限制性小等优点。

Description

基于三维视觉系统图像特征和三维信息的运动估计方法

技术领域

本发明涉及一种基于三维视觉系统图像特征和三维信息的运动估计方法。

背景技术

高精度高可靠的三维运动估计是机器人领域的研究难点和热点之一，其目标是实时计算出相邻两相邻时刻机器人三维空间位置变化量，是机器人自定位、地图创建、三维重建、运动跟踪等研究的核心内容。传统惯性导航系统原理简单，被广泛应用于运动估计，但是其存在漂移、误差累计等问题，其对应运动估计的精度低、可靠性比较差。相对于惯性导航系统，基于视觉的运动估计不存在物理漂移问题，具有较高的稳定性。目前基于单目视觉的运动估计无法获得三维相对运动的尺度信息；立体摄像机可获得三维运动估计，但是存在计算量大、可靠性受环境和噪声影响大、测量精度超过景深范围后急速下降等问题。随着三维激光测距仪、三维摄像机等三维传感器的引入，空间环境的三维信息实时获取成为可能。基于ICP的三维点云匹配算法开始被应用于相邻两帧的三维信息估计。但是由于噪声、误差和不匹配区域比例的增长，ICP迭代算法容易陷入局部优化解，从而该类算法仅在较小空间变换范围内或基于较好的初始估计情况下可获得准确的估计结果。为实现精确可靠的三维运动估计，通常采用高频率采样获取相邻帧三维信息，从而将相邻帧空间变换限制在较小范围；而高频率采样必然导致计算量巨大，难以实时应用。相对于三维激光测距仪，三维摄像机不仅可获得场景二维彩色图像信息，还可以获得空间场景对应的三维信息，如Kinect、SR4000等三维摄像机等。基于此人们提出基于相邻帧图像的不变性特征点构成三维点集求解三维运动的初始估计，以提高ICP算法的估计性能。但是该类方法的缺陷在于严重依赖于特征点选择，首先特征点集中的误匹配点集将对三维估计初始值造成严重影响；其次该算法仅对图像特征点较多的场景有效，若场景特征点集非常稀疏以致无法获取正确可靠的初始估计值时，将直接导致算法失败，从而无法获得三维相对估计。

因此，如何充分利用三维视觉系统的二维彩色图像信息和空间三维信息，实时获取精确的三维运动信息，是本领域急需解决的技术问题。

发明内容

本发明所要解决的技术问题是提供一种基于三维视觉系统图像特征和三维信息的运动估计方法，基于三维视觉系统获取的相邻帧的场景二维彩色图像信息和按二维彩色图像像素对应的空间三维信息，采用本发明的改进SIFT特征匹配算法、采样间隔自适应调整方法和三维运动优化估计算法实现相邻帧的快速高精度三维运动估计，具有精度高、计算量小、应用场景限制性小等突出优点。

一种基于三维视觉系统图像特征和三维信息的运动估计方法，包括以下步骤：

步骤1：基于三维视觉系统，动态确定相邻帧场景之间的采样间隔Δt；

步骤2：按照步骤1确定的采样间隔Δt，利用三维视觉系统分别在t时刻和t+1时刻获取相邻帧场景的二维彩色图像I^t和I^t+1、场景对应的三维点云D^t和D^t+1；

所述三维视觉系统可同时获取场景的二维彩色图像I与三维点云信息D，其中二维彩色图像I与三维点云D按二维彩色图像像素点一一对应，即二维彩色图像中第u行第v列的像素点I^t(u,v)与三维点云D_u,v(x,y,z)相对应；所述三维点云是指二维彩色图像所有像素点对应的三维空间点集合；

步骤3：利用SIFT算法对相邻帧场景的二维彩色图像I^t和I^t+1进行特征点提取与匹配，得到相邻帧场景的匹配图像特征点对集合M；

步骤4：剔除集合M中的误匹配图像特征点对，得到最终的匹配图像特征点对集合M₃；

1)基于双向一致性检查的误匹配剔除；

采用欧式距离作为特征点128维向量描述子的相似性度量，对t+1时刻的二维彩色图像I^t+1的任一图像特征点F_i ^t+1，依次从t时刻的二维彩色图像I^t的图像特征点中找出相匹配的特征点构成匹配特征点对集合M′，M和M′的交集构成新的匹配图像特征点对集合M₁，以剔除非双向一致的误匹配图像特征点对；

2）基于颜色一致性检查的误匹配剔除；

步骤a：分别提取t时刻和t+1时刻获取的二维彩色图像I^t和I^t+1的HSV空间颜色信息；

步骤b：依据1）中获得的匹配特征点对集合M₁，建立集合M₁中所有特征点的HSV空间分量的索引信息；

所述索引信息是指特征点在二维彩色图像坐标系中的坐标与该特征点的HSV空间颜色分量的对应关系；

步骤c：依据步骤b中建立的HSV空间分量索引信息和匹配特征点对集合M₁，分别获取匹配特征点对集合M₁的HSV信息[H^t,S^t,V^t]和[H^t+1,S^t+1,V^t+1]，其中，[H^t,S^t,V^t]为M₁中t时刻二维彩色图像匹配特征点对应的HSV信息，[H^t+1,S^t+1,V^t+1]为M₁中t+1时刻二维彩色图像匹配特征点对应的HSV信息；

步骤d：对匹配特征点对集合M₁中的任一特征点对进行HSV空间颜色一致性检查处理，若同时满足如下约束，则认为是正确匹配点对，否则认为是误匹配点对，并从M₁中剔除，得到新的匹配特征点对集合M₂；

约束判定条件如下：

$\left\{\begin{array}{c}|H^t\left(F_i^t\right)-H^{t+1}\left(F_j^{t+1}\right)|\≤T_H\\ |S^t\left(F_i^t\right)-S^{t+1}\left(F_j^{t+1}\right)|\≤T_S\\ {|V}^t\left(F_i^t\right)-V^{t+1}\left(F_j^{t+1}\right)|\≤T_V\end{array}\right.$

其中，T_H为H分量阈值，T_S为S分量阈值，T_V为V分量阈值，取值范围分别为：T_H∈(0,π)，T_S∈(0,0.5)，T_V∈(0,0.5)；

3）采用RANSAC算法对匹配特征点对集合M₂中的误匹配进一步剔除，得到最终的匹配特征点对集合M₃；

步骤5：如果集合M₃中匹配特征点对数目大于阈值N₂，转入步骤6；否则转入步骤7；

步骤6：基于集合M₃中相邻场景所有匹配特征点对的对应三维点云信息，利用ICP(IterativeClosestPoint，迭代最近点)算法，求取t+1时刻相对于t时刻的三维旋转向量R和平移向量T，获取相邻帧的三维运动估计，并转入步骤9；

依据M₃中的匹配特征点对，从三维点云D^t和D^t+1中分别获取特征点对应的三维点集FD^t(X,Y,Z)和FD^t+1(X,Y,Z)；根据匹配三维点集FD^t(X,Y,Z)和FD^t+1(X,Y,Z)，利用ICP(IterativeClosestPoint，迭代最近点)算法，求取t+1时刻相对于t时刻的三维旋转向量R和平移向量T，即获得相邻两帧的三维运动估计；

步骤7：依据t时刻相对于t-1时刻的三维运动的估计值R^t和T^t，依据三维视觉系统呈匀速直线运动，线性估计t+1时刻相对于t时刻的三维运动信息初始估计值R₀和T₀；

$R_0=\mathrm{\Δ}{t}^{t+1}*\frac{R^t}{\mathrm{\Δ}{t}^t},T_0=\mathrm{\Δ}{t}^{t+1}*\frac{T^t}{\mathrm{\Δ}{t}^t}$

Δt^t为t时刻和t-1时刻相邻帧之间的采样间隔，Δt^t+1t+1时刻和t时刻相邻帧之间的采样间隔；

步骤8：基于相邻帧三维点云数据D^t和D^t+1、三维运动初始估计值R₀和T₀，采用ICP算法估计相邻帧的三维运动R和平移向量T；

根据三维运动估计初值（旋转矩阵R₀和平移向量T₀）、t时刻场景的三维点云D^t、t+1时刻场景的三维点云D^t+1，通过ICP迭代算法，计算t+1时刻相对于t时刻的三维旋转向量R和平移向量T，即获得相邻两帧的三维运动估计；

步骤9：若机器人系统运动结束，无需对三维运动进行估计，则退出三维运动估计；否则转至步骤1重复继续进行下一个采样时刻与相邻帧的三维运动估计。

所述步骤1中相邻帧场景之间的采样间隔Δt的确定依据如下：

所述三维视觉系统的采样频率范围为[f_min,f_max]，相邻帧场景的动态采样间隔Δt变化范围为[Δt_min,Δt_max]，其中Δt最小值Δt_min=1/f_max，最大值Δt_Max=1/f_min；

采样间隔Δt的初始值设定为Δt_min；在运动过程中，基于t-1时刻和t时刻相邻帧的匹配图像特征点对集合M₃中匹配图像特征点对数目Num(M₃)和当前采样间隔Δt^t，依据表1动态确定下一步采样间隔Δt^t+1：

表1相邻帧之间采样间隔ΔT动态计算

其中，Max()为取最大值函数，Min()为取最小值函数，Num(M₃)为M₃中的匹配特征点对数目；间隔调整最小步长其中L为采样间隔的等级数，取值范围为4-20；N₁和N₂分别为相邻帧匹配图像特征点对数目阈值，其中N₂>N₁，其取值范围是N₁≥10，N₂≥20。

所述步骤3利用SIFT算法对相邻帧场景的二维彩色图像I^t和I^t+1进行特征点提取与匹配，得到相邻帧场景的匹配特征点对集合M的具体步骤如下：

1）对t时刻和t+1时刻获取的相邻场景的二维彩色图像I^t和I^t+1，利用SIFT算法分别提取尺度旋转不变特征点集合F^t和F^t+1；

2）对F^t的任一特征点F_i ^t，采用欧式距离作为特征点128维向量描述子的相似性度量，然后从t+1时刻获取的二维彩色图像特征点F^t+1中，依次找出与F_i ^t匹配的特征点构成匹配特征点对

3）对F^t中所有特征点执行步骤2），得到匹配特征点对集合M。

所述步骤4误匹配点剔除中的3）采用RANSAC算法对匹配特征点对集合M₂中的误匹配进一步剔除，得到最终的匹配特征点对集合M₃的具体步骤如下：

①随机选择M₂中的s组匹配特征点对组成随机样本集M₃，并基于该样本计算匹配模型H；即使得匹配特征点对满足如下方程：

$H\×X_i^t=X_j^{t+1},$ 其中 $H=\left[\begin{array}{ccc}{h}_1& h_2& h_3\\ h_4& h_5& h_6\\ h_7& h_8& h_9\end{array}\right],X_i^t=\left[\begin{array}{c}{x}_i\\ y_i\\ 1\end{array}\right],X_j^t=\left[\begin{array}{c}{x}_j\\ y_j\\ 1\end{array}\right]$

②用①中计算的模型H，对M₂未选中的其它样本数据进行测试，若则该特征点对适用于估计的模型H，并称之为模型H的内点，放入M₃；否则认为它是外点；其中表示特征点相对于模型H的映射点和匹配对应点的欧式距离，T_d为距离阈值，×表示矩阵乘运算；

③重复K次采样，选择内点数目最多的集合M₃；

$K=\frac{\log (1-p)}{\log (1-{(1-\mathrm{\ϵ})}^s)}$

其中，ε为M₂中的误匹配点对占所有匹配点对的比例，p为设定的匹配成功概率期望值，s为采样数目，s≥4；

④利用M₃中的样本数据重新估计计算模型参数H；

⑤利用模型H重新测试样本集M₂，确定新的集合M₃；反复执行步骤④⑤，直至M₃中的匹配特征点对数目前后相差不超过5%-10%为止。

有益效果

本发明可实现高精度三维运动信息的快速获取，具有计算量小，应用场景限制性小等优点，可有效满足机器人自定位、地图创建、三维重建、三维运动跟踪等应用领域中对三维运动信息的迫切需求。

与现有三维运动估计方法相比，本发明的先进性表现在：1）发明采用三维视觉系统获取的三维信息用于三维运动估计，提高了三维信息获取和运动估计速度，并避免了传统立体视觉系统三维信息获取中的计算误差；2）本发明提出相邻帧采样间隔自适应调整方法，根据场景中二维彩色图像特征的稀疏性自动调整相邻帧采集间隔，对图像特征稀疏的场景通过提高采样频率，保证在相邻帧在较小的空间范围内变化；在图像特征丰富的场景，自动降低采样频率，保证本发明三维运动估计处理的准确性和鲁棒性的同时，减少了运算量；3）发明提出综合场景二维彩色图像特征和三维点云信息的三维运动优化估计算法。其中在二维彩色图像特征丰富的场景，基于改进SIFT特征匹配算法获取相邻帧的精确匹配特征点集及其对应的三维信息，获得精确三维运动估计，有效解决基于ICP算法的三维点云匹配中的局部收敛问题，并可通过降低采样频率减少运算量；在图像特征稀疏的场景，通过提高采样频率将相邻帧空间变化限制在较小的范围内，直接利用相邻帧的三维点云信息，采用ICP迭代算法求解三维运动估计，有效保证了算法的可靠性，避免了基于三维摄像机中图像特征匹配的三维运动估计方法对环境图像特征疏密性的高度依赖性；4）发明提出改进SIFT特征匹配算法，在传统SIFT特征匹配算法基础上加入了双向一致性约束、颜色一致性约束和Ransac约束有效提高相邻帧特征点匹配的正确性，有效降低了误匹配点对对运动估计的破坏性影响。

附图说明

图1为本发明整体流程图；

图2为相邻帧图像基于双向一致性检查的示意图，其中，图(a)为以t时刻二维彩色图像为基准的正向匹配示意图，图(b)为以t+1时刻二维彩色图像为基准的反向匹配示意图；

图3为相邻帧图像中后一帧图像相对前一帧图像平移50cm时，两帧图像进行图像特征匹配的实验过程和结果对比图，其中，图(a)为正向匹配特征点对情况图；图（b）为反向匹配特征点对情况图；图(c)为基于相互一致性检查约束剔除误匹配特征点对后的效果图；图(d)为基于颜色一致性检查约束剔除误匹配特征点对后的效果图；图(e)为基于Ransac算法剔除误匹配特征点对后的效果图；

图4为相邻帧图像中后一帧图像相对前一帧图像旋转30°时，两帧图像进行图像特征匹配的实验过程和结果对比图；其中，图(a)为正向匹配特征点对情况图；图（b）为反向匹配特征点对情况图；图(c)为基于相互一致性检查约束剔除误匹配特征点对后的效果图；图(d)为基于颜色一致性检查约束剔除误匹配特征点对后的效果图；图(e)为基于Ransac算法剔除误匹配特征点对后的效果图；

图5为相邻帧图像中后一帧图像相对前一帧图像同时平移80cm和旋转50°时，基于本发明计算的运动估计值进行相邻帧三维拼接效果示意图；

图6为固定采样间隔和本发明自适应采样间隔方法对比试验的部分场景图片显示；

图7为固定采样时间间隔和本发明自适应采样间隔方法对比试验的结果对比，图(a)为机器人实际运动轨迹，图(b)为分别采用固定采样间隔和自适应采样间隔方法所得轨迹对比。

具体实施方式

下面将结合附图和实施例对本发明做进一步的说明。

本发明所涉及的三维视觉系统可采用发明专利(201310220879.6)所涉及的二维/三维复合摄像机或微软Kinect三维摄像机等。所述三维视觉系统可同时获取场景二维彩色图像信息I和空间三维信息D，其中二维彩色图像信息I和空间三维信息D按二维彩色图像像素坐标逐一匹配对应，即二维彩色图像I中第u行第v列的像素点I(u,v)与三维点云D_u,v(x,y,z)相对应。作为应用实例，本发明给出了基于Kinect三维摄像机的应用效果。

如图1所示，为本发明的流程图，一种基于三维视觉系统图像特征和三维信息的运动估计方法，包括以下步骤：

步骤1：基于三维视觉系统，动态确定相邻帧场景之间的采样间隔Δt；

所述三维视觉系统的采样频率范围为[f_min,f_max]，相邻帧场景的动态采样间隔Δt变化范围为[Δt_min,Δt_max]，其中Δt最小值Δt_min=1/f_max，最大值Δt_Max=1/f_min；

采样间隔Δt的初始值设定为Δt_min；在运动过程中，基于t-1时刻和t时刻相邻帧的匹配图像特征点对集合M₃中匹配图像特征点对数目Num(M₃)和当前采样间隔Δt^t，依据表1动态确定下一步采样间隔Δt^t+1：

表1相邻帧之间采样间隔Δt动态计算

其中，Max()为取最大值函数，Min()为取最小值函数，Num(M₃)为M₃中的匹配特征点对数目；间隔调整最小步长其中L为采样间隔的等级数，取值范围为4-20；N₁和N₂分别为相邻帧匹配图像特征点对数目阈值，其中N₂>N₁，其取值范围是N₁≥10，N₂≥20。

步骤2：按照步骤1确定的采样间隔Δt，利用三维视觉系统分别在t时刻和t+1时刻获取相邻帧场景的二维彩色图像I^t和I^t+1、场景对应的三维点云D^t和D^t+1；

所述三维视觉系统可同时获取场景的二维彩色图像I与三维点云信息D，其中二维彩色图像I与三维点云D按二维彩色图像像素点一一对应，即二维彩色图像中第u行第v列的像素点I^t(u,v)与三维点云D_u,v(x,y,z)相对应；所述三维点云是指二维彩色图像所有像素点对应的三维空间点集合。

步骤3：利用SIFT算法对相邻帧场景的二维彩色图像I^t和I^t+1进行特征点提取与匹配，得到相邻帧场景的匹配图像特征点对集合M；

SIFT图像特征检测算子是一种图像局部特征描述算子，对平移、旋转、尺度缩放、亮度变化、遮挡和噪声等具有良好的不变性。因为特征检测算子的正确性和可靠性对运动估计有着重要影响，且运动过程中图像特征存在平移、旋转、尺度缩放、亮度变化、遮挡和噪声等问题，本发明选用SIFT算子提取作为运动估计的图像特征点。

SIFT图像特征检测与匹配处理主要包括四个步骤：尺度空间极值检测、确定特征点的方向参数、生成128维特征描述向量及特征匹配。

主要处理过程如下：

1）对t时刻和t+1时刻获取的相邻场景的二维彩色图像I^t和I^t+1，利用SIFT算法【文献1】分别提取二维彩色图像特征点集合F^t和F^t+1。

2）对F^t中的任一特征点F_i ^t，采用SIFT算法【文献1】，从t+1时刻获取的二维彩色图像特征点F^t+1中，依次找出与F_i ^t匹配的特征点构成匹配特征点对

3）对F^t中所有特征点执行步骤2），得到匹配特征点对集合M。

步骤4：剔除集合M中的误匹配特征点对，得到最终的匹配点对集合M₃；

利用SIFT特征提取与匹配处理，可以得到t和t+1时刻二维彩色图像I^t和I^t+1的匹配特征点对集合M。但由于噪声干扰和SIFT算法为增强鲁棒性而保留特征点辅方向等问题，仍不可避免的存在一定比例的错误匹配对。而误匹配点对的存在对运动估计的正确性、可靠性都会产生很大的破坏性影响。为避免上述问题，本文提出了如下改进方法剔除误匹配点对。

1)基于双向一致性检查的误匹配剔除；

SIFT算法在特征提取时，为了增强匹配鲁棒性，保留了峰值大于主方向峰值80％的方向作为该特征点的辅方向，同时，在特征匹配阶段是以t时刻图像为基准，通过一定的搜索策略遍历搜索t+1时刻图像，计算两时刻图像特征点描述子的欧式距离，若最近距离与次最近距离的比值小于某个阈值时，即被认为最近距离的为一对匹配点。因而，SIFT特征点可能有两个及以上的方向，并且产生“一对多”的匹配点对。这与相邻帧中图像特征的最优“一对一”精确匹配相矛盾，必然产生系统误差。

如图2所示，为相邻帧图像基于双向一致性检查的示意图；首先以t时刻二维彩色图像特征点为基准搜索匹配t+1时刻二维彩色图像特征点，t时刻二维彩色图像I^t中的特征点F_i ^t可能搜索匹配了t+1时刻二维彩色图像I^t+1中的特征点形成4对匹配成功点；然后以t+1时刻二维彩色图像特征点为基准搜索匹配t时刻二维彩色图像特征点，t+1时刻二维彩色图像中的特征点可能搜索匹配了t时刻二维彩色图像中的特征点F_i ^t、形成3对匹配成功点；而这7对匹配点对中，只有一对共同的匹配点，其中i、j、i1、i2、j1、j2、j3分别为相应特征点在其对应图像特征点集中的序号标识。

针对上述问题，本发明提出基于双向一致性检查的误匹配剔除方法，采用欧式距离作为特征点128维向量描述子的相似性度量，对t+1时刻的二维彩色图像I^t+1的任一图像特征点F_i ^t+1，依次从t时刻的二维彩色图像I^t的图像特征点中找出相匹配的特征点构成匹配特征点对集合M′，M和M′的交集构成新的匹配特征点对集合M₁，以剔除非双向一致的误匹配图像特征点对。

由于特征点数目一般比较少（几十到几百个），因而时间复杂度几乎没有增加，而实验结果可以剔除20%甚至50%以上的误匹配点对。

2）基于颜色一致性检查的误匹配剔除；

SIFT算法为追求最优鲁棒匹配效果，以保证对不同摄像机、不同时间、不同光照、不同角度、不同尺度下的图像特征都具有较好的一致性和稳定性，算法先将彩色图像转换为灰度图像，然后再进行特征提取和匹配运算，从而丢失了色彩信息。

在本发明所涉及的应用领域，需进行特征检测与匹配处理的图像对为同一视觉系统在相邻时刻相邻场景拍摄的两组图像，成像条件相对稳定，剧烈变化的环境因素较少。

基于上述分析，为尽可能利用已有图像信息提高匹配的鲁棒性，本发明提出了基于颜色信息约束的特征匹配处理。具体步骤如下：

步骤a：分别提取t时刻和t+1时刻获取的二维彩色图像I^t和I^t+1的HSV空间颜色信息；

步骤b：依据1）中获得的匹配特征点对集合M₁，建立集合M₁中所有特征点的HSV空间分量的索引信息；

所述索引信息是指特征点在二维彩色图像坐标系中的坐标与该特征点的HSV空间颜色分量的对应关系；

步骤c：依据步骤b中建立的HSV空间分量索引信息和匹配特征点对集合M₁，分别获取匹配特征点对集合M₁的HSV信息[H^T,S^T,V^T]和[H^T+1,S^T+1,V^T+1]，其中，[H^t,S^t,V^t]和[H^t+1,S^t+1,V^t+1]，其中，[H^t,S^t,V^t]为M₁中t时刻二维彩色图像匹配特征点对应的HSV信息，[H^t+1,S^t+1,V^t+1]为M₁中t+1时刻二维彩色图像匹配特征点对应的HSV信息；

步骤d：对匹配特征点对集合M₁中的任一特征点对进行HSV空间颜色一致性检查处理，若同时满足如下约束，则认为是正确匹配点对，否则认为是误匹配点对，并从M₁中剔除，得到新的匹配特征点对集合M₂；

约束判定条件如下：

$\left\{\begin{array}{c}|H^t\left(F_i^t\right)-H^{t+1}\left(F_j^{t+1}\right)|\≤T_H\\ |S^t\left(F_i^t\right)-S^{t+1}\left(F_j^{t+1}\right)|\≤T_S\\ {|V}^t\left(F_i^t\right)-V^{t+1}\left(F_j^{t+1}\right)|\≤T_V\end{array}\right.$

其中，T_H为H分量阈值，T_S为S分量阈值，T_V为V分量阈值，取值范围分别为：T_H∈(0,π)，T_S∈(0,0.5)，T_V∈(0,0.5)；

3）采用RANSAC算法对匹配特征点对集合M₂中的误匹配进一步剔除，得到最终的匹配特征点对集合M₃；

在基于双向一致性检查和基于颜色一致性检查剔除误匹配后，得到的匹配特征点对集合M₂，其中M₂中误匹配点对数目已经比较少，接近精确匹配点数据集。因此将对数据集M₂应用RANSAC算法，可通过计算最优匹配映射模型，进一步剔除尽可能的误匹配特征点对。经RANSAC优化剔除后的特征匹配点对集合设为M₃。

①随机选择M₂中的s组匹配特征点对组成随机样本集M₃，并基于该样本计算匹配模型H；即使得匹配特征点对满足如下方程：

$H\×X_i^t=X_j^{t+1},$ 其中 $H=\left[\begin{array}{ccc}{h}_1& h_2& h_3\\ h_4& h_5& h_6\\ h_7& h_8& h_9\end{array}\right],X_i^t=\left[\begin{array}{c}{x}_i\\ y_i\\ 1\end{array}\right],X_j^t=\left[\begin{array}{c}{x}_j\\ y_j\\ 1\end{array}\right]$

②用①中计算的模型H，对M₂未选中的其它样本数据进行测试，若则该特征点对适用于估计的模型H，并称之为模型H的内点，放入M₃；否则认为它是外点；其中表示特征点相对于模型H的映射点和匹配对应点的欧式距离，T_d为距离阈值，×表示矩阵乘运算；

③重复K次采样，选择内点数目最多的集合M₃；

$K=\frac{\log (1-p)}{\log (1-{(1-\mathrm{\ϵ})}^s)}$

其中，ε为M₂中的误匹配点对占所有匹配点对的比例，p为设定的匹配成功概率期望值，s为采样数目，s≥4；

④利用M₃中的样本数据重新估计计算模型参数H；

⑤利用模型H重新测试样本集M₂，确定新的集合M₃；反复执行步骤④⑤，直至M₃中的匹配点对数目前后相差不超过5%-10%为止。

步骤5：如果集合M₃中匹配特征点对数目大于阈值T2，转入步骤6；否则转入步骤7；

步骤6：基于集合M₃中相邻场景所有匹配特征点对的对应三维点云信息，利用ICP(IterativeClosestPoint，迭代最近点)算法【文献2】，求取t+1时刻相对于t时刻的三维旋转向量R和平移向量T，获取相邻帧的三维运动估计，并转入步骤9；

依据M₃中的匹配特征点对，从三维点云D^t和D^t+1中分别获取特征点对应的三维点集FD^t(X,Y,Z)和FD^t+1(X,Y,Z)；根据匹配三维点集FD^t(X,Y,Z)和FD^t+1(X,Y,Z)，利用ICP(IterativeClosestPoint，迭代最近点)算法，求取t+1时刻相对于t时刻的三维旋转向量R和平移向量T，即获得相邻两帧的三维运动估计；

步骤7：依据t时刻相对于t-1时刻的三维运动的估计值R^t和T^t，依据三维视觉系统呈匀速直线运动，线性估计t+1时刻相对于t时刻的三维运动信息初始估计值R₀和T₀；

$R_0=\mathrm{\Δ}{t}^{t+1}*\frac{R^t}{\mathrm{\Δ}{t}^t},T_0=\mathrm{\Δ}{t}^{t+1}*\frac{T^t}{\mathrm{\Δ}{t}^t}$

Δt^t为t时刻和t-1时刻相邻帧之间的采样间隔，Δt^t+1t+1时刻和t时刻相邻帧之间的采样间隔；

步骤8：基于相邻帧三维点云数据D^t和D^t+1、三维运动初始估计值R₀和T₀，采用ICP算法估计相邻帧的三维运动R和平移向量T；

根据三维运动估计初值（旋转矩阵R₀和平移向量T₀）、t时刻场景的三维点云D^t、t+1时刻场景的三维点云D^t+1，通过ICP迭代算法，计算t+1时刻相对于t时刻的三维旋转向量R和平移向量T，即获得相邻两帧的三维运动估计；

步骤9：若机器人系统运动结束，无需对三维运动进行估计，则退出三维运动估计；否则转至步骤1重复继续进行下一个采样时刻与相邻帧的三维运动估计。

本发明的试验结果与对比效果如图3-图5所示。

图3为相邻帧图像中后一帧图像相对前一帧图像平移50cm时，两帧图像进行图像SIFT特征匹配的试验过程和结果对比图，该相对运动对应的旋转矩阵实际值R为单位矩阵，平移向量实际值为T=[0.5,0,0]'。其中,图3(a)为正向匹配特征点对情况图；图3（b）为反向匹配特征点对情况图；图3(c)为基于相互一致性检查约束剔除误匹配特征点对后的效果图；图3(d)为基于颜色一致性检查约束剔除误匹配特征点对后的效果图（阈值分别取T_H=π/8，T_S=0.2，T_V=0.1）；图3(e)为基于Ransac算法剔除误匹配特征点对后的效果图；从试验结果可以看出，本发明提出的改进SIFT特征匹配方法有效的剔除了现有SIFT特征匹配方法中可能存在的误匹配，提高了特征匹配的准确度和可靠性。

基于图3中的最终匹配的特征点对集合M₃，应用本发明方法求取的两相邻帧的三维相对运动信息为实验图像的运动估计结果为 $R_1=\left[\begin{array}{ccc}0.9962& -0.0008& 0.0005\\ -0.0008& 0.9979& 0.0000\\ 0.0005& 0.0000& 0.9983\end{array}\right],T_1=\left[\begin{array}{c}0.5168\\ -0.0068\\ 0.0005\end{array}\right].$ 算法计算的沿x方向平移量计算值为0.5168m，平移误差率为3.36%，绕y方向欧拉角旋转计算值为0.0286°。直接基于相邻帧三维点云信息，采用ICP迭代算法处理结果为 $R_2=\left[\begin{array}{ccc}0.9630& 0.0321& -0.2677\\ -0.0485& 0.9973& -0.0549\\ 0.2652& 0.0658& 0.9619\end{array}\right],T_2=\left[\begin{array}{c}0.9088\\ 0.1835\\ 0.0811\end{array}\right];$ 采用传统SIFT特征检测算子检测图像特征，然后基于特征点对应的三维点云信息，采用ICP迭代算法处理结果为（以下简称SIFT+ICP算法） $R_3=\left[\begin{array}{ccc}0.9980& -0.1626& -0.1088\\ -0.1626& 0.9980& 0.0009\\ 0.1087& -0.0015& 0.9999\end{array}\right],T_3=\left[\begin{array}{c}0.6451\\ 0.1037\\ 0.0531\end{array}\right].$ 本文算法与其它现有算法详细对比情况如表2所示，从试验对比结果中，看出本发明在平移运动情况下的三维运动估计的精确度和稳定性方面均优于现有算法。

表2本发明效果与已有算法对比1

图4为相邻帧图像中后一帧图像相对前一帧图像旋转30°时，两帧图像进行图像SIFT特征匹配的试验过程和结果对比示意图，该相对运动对应的旋转矩阵实际值为 $R=\left[\begin{array}{ccc}\cos (\mathrm{\π}/6)& 0& \sin (\mathrm{\π}/6)\\ 0& 1& 0\\ -\sin (\mathrm{\π}/6)& 0& \cos (\mathrm{\π}/6)\end{array}\right]\≈\left[\begin{array}{ccc}0.8660& 0& 0.5000\\ 0& 1& 0\\ -0.5000& 0& 0.8660\end{array}\right],$ 平移向量实际值为T=[0,0,0]'，图4(c)为基于相互一致性检查约束剔除误匹配特征点对后的效果图；图4(d)为基于颜色一致性检查约束剔除误匹配特征点对后的效果图（阈值分别取T_H=π/8，T_S=0.2，T_V=0.1）；图4(e)为基于Ransac算法剔除误匹配特征点对后的效果图；从试验结果可以看出，本发明提出的改进SIFT特征匹配方法有效的剔除了现有SIFT特征匹配方法中可能存在的误匹配，提高了特征匹配的准确度和可靠性。

基于图4中的最终匹配的特征对集合M₃，应用本发明方法求取的两相邻帧的三维相对运动信息为 $R_1=\left[\begin{array}{ccc}0.8601& 0.0014& 0.5101\\ -0.0093& 0.9999& 0.0129\\ -0.5100& -0.0158& 0.8600\end{array}\right],T_1=\left[\begin{array}{c}0.0004\\ 0.0062\\ 0.0142\end{array}\right],$ 即绕y方向欧拉角旋转计算值为30.6705°，旋转误差率为2.23%。

采用ICP迭代算法处理结果为: $R_2=\left[\begin{array}{ccc}0.9643& 0.0367& 0.2624\\ -0.0490& 0.9980& 0.0406\\ -0.2604& -0.0520& 0.9641\end{array}\right],T_2=\left[\begin{array}{c}-0.6039\\ -0.1062\\ 0.4134\end{array}\right];$ 采用SIFT+ICP结果为： $R_3=\left[\begin{array}{ccc}0.9043& 0.0451& 0.4424\\ -0.0415& 0.9976& 0.0511\\ -0.4614& -0.0717& 0.8991\end{array}\right],T_3=\left[\begin{array}{c}-0.1127\\ -0.0843\\ 0.0716\end{array}\right].$

算法与其它现有算法对比情况如表3所示。试验对比结果，本发明在旋转运动情况下的三维运动估计的精确度和稳定性方面均优于现有算法。

表3本发明效果与已有算法对比2

图5为Kinect拍摄的相距80cm同时旋转50°（大范围的平移和大角度的旋转情况）的两帧图像应用PCL点云库的3D显示效果，该相对运动对应的旋转矩阵实际值为 $R=\left[\begin{array}{ccc}\cos (5\mathrm{\π}/18)& 0& \sin (5\mathrm{\π}/18)\\ 0& 1& 0\\ -\sin (5\mathrm{\π}/18)& 0& \cos (5\mathrm{\π}/18)\end{array}\right]\≈\left[\begin{array}{ccc}0.6428& 0& 0.7660\\ 0& 1& 0\\ -0.7660& 0& 0.6428\end{array}\right],$ 平移向量实际值为T=[0.8000,0,0]'，利用本发明求取的旋转平移运动估计为 $R_1=\left[\begin{array}{ccc}0.6393& -0.0068& 0.7605\\ -0.0133& 0.9998& 0.0158\\ -0.7605& -0.0195& 0.6391\end{array}\right],$ T₁=[0.7868,0.0036,-0.0177]'，绕y方向旋转欧拉角计算值为49.5083°，旋转误差率为0.98%，沿x方向平移量计算值为0.7868m，平移误差率为1.65%，图5中3D效果的完美显示，反向验证了本发明算法的有效性，即本发明能实现同时存在大幅度旋转和平移运动情况下的三维运动估计，算法具有较高的精确度和稳定性。

图6所示固定频率和自适应步长频率对比试验环境的部分场景图片显示，试验场景尺寸为16m*5m，其中设定相邻帧固定采样间隔为0.5秒，共拍摄231帧，自适应采样间隔范围设定为[0.5秒,3秒]，采样等级为6，T1阈值选择为10，T2阈值选择为20，图中显示的十二张图片分别按照拍摄时间先后从左至右、从上至下依次排列。图7(a)为拍摄帧的机器人参考轨迹，图7(b)中“*”线为基于本发明固定采样间隔获取的三维运动轨迹，固定采样间隔计算所有231帧数据；图7(b)中“×”线为基于本发明自适应采样间隔获取的三维运动轨迹，自适应采样间隔共计算其中77帧。自适应采样间隔的计算量不足固定采样间隔方法的1/3，但自适应采样间隔方法却具有更高的实验精度，其原因是自适应采样间隔有效降低了匹配次数，从而降低了累计误差。因此，自适应采样时间间隔策略的引入，在保证精度的前提下，可以大幅度减少计算量。

根据上述实验分析，本发明算法在三维估计的精确度和稳定性方面均优于现有算法。

文献[1]DavidG.Lowe,DistinctiveImageFeaturesfromScale-invariantKeypoints,InternationalJournalofComputerVision,60(2)：91～110,2004.

文献[2]PaulJ.Besl.NeilD.McKay.Amethodforregistrationof3-Dshapes[J].IEEETransactionsonPatternAnalysisandMachineIntelligence.1992.14(2):239-256.

Claims (4)

1.一种基于三维视觉系统图像特征和三维信息的运动估计方法，其特征在于，包括以下步骤：

步骤1：基于三维视觉系统，动态确定相邻帧场景之间的采样间隔Δt；

步骤2：按照步骤1确定的采样间隔Δt，利用三维视觉系统分别在t时刻和t+1时刻获取相邻帧场景的二维彩色图像I^t和I^t+1、场景对应的三维点云D^t和D^t+1；

步骤3：利用SIFT算法对相邻帧场景的二维彩色图像I^t和I^t+1进行特征点提取与匹配，得到相邻帧场景的匹配图像特征点对集合M；

步骤4：剔除集合M中的误匹配图像特征点对，得到最终的匹配图像特征点对集合M₃；

1)基于双向一致性检查的误匹配剔除；

采用欧式距离作为特征点128维向量描述子的相似性度量，对t+1时刻的二维彩色图像I^t+1的任一图像特征点F_i ^t+1，依次从t时刻的二维彩色图像I^t的图像特征点中找出相匹配的特征点构成匹配特征点对集合M′，M和M′的交集构成新的匹配图像特征点对集合M₁，以剔除非双向一致的误匹配图像特征点对；

2)基于颜色一致性检查的误匹配剔除；

步骤a：分别提取t时刻和t+1时刻获取的二维彩色图像I^t和I^t+1的HSV空间颜色信息；

步骤b：依据1)中获得的匹配特征点对集合M₁，建立集合M₁中所有特征点的HSV空间分量的索引信息；

所述索引信息是指特征点在二维彩色图像坐标系中的坐标与该特征点的HSV空间颜色分量的对应关系；

步骤c：依据步骤b中建立的HSV空间分量索引信息和匹配特征点对集合M₁，分别获取匹配特征点对集合M₁的HSV信息[H^t,S^t,V^t]和[H^t+1,S^t+1,V^t+1]，其中，[H^t,S^t,V^t]为M₁中t时刻二维彩色图像匹配特征点对应的HSV信息，[H^t+1,S^t+1,V^t+1]为M₁中t+1时刻二维彩色图像匹配特征点对应的HSV信息；

步骤d：对匹配特征点对集合M₁中的任一特征点对进行HSV空间颜色一致性检查处理，若同时满足如下约束，则认为是正确匹配点对，否则认为是误匹配点对，并从M₁中剔除，得到新的匹配特征点对集合M₂；

约束判定条件如下：

$\left\{\begin{array}{c}\left|H^t\right({F_i}^t)-H^{t+1}\left(F_j^{t+1}\right)|\≤T_H\\ \left|S^t\right({F_i}^t)-S^{t+1}\left(F_j^{t+1}\right)|\≤T_S\\ \left|V^t\right({F_i}^t)-V^{t+1}\left(F_j^{t+1}\right)|\≤T_V\end{array}\right.$

其中，T_H为H分量阈值，T_S为S分量阈值，T_V为V分量阈值，取值范围分别为：T_H∈(0,π)，T_S∈(0,0.5)，T_V∈(0,0.5)；

3)采用RANSAC算法对匹配特征点对集合M₂中的误匹配进一步剔除，得到最终的匹配特征点对集合M₃；

步骤5：如果集合M₃中匹配特征点对数目大于设定的阈值N₂，转入步骤6；否则转入步骤7；

步骤6：基于集合M₃中相邻场景所有匹配特征点对的对应三维点云信息，利用迭代最近点ICP算法，求取t+1时刻相对于t时刻的三维旋转向量R和平移向量T，获取相邻帧的三维运动估计，并转入步骤9；

步骤7：依据t时刻相对于t-1时刻的三维运动的估计值R^t和T^t，依据三维视觉系统呈匀速直线运动，线性估计t+1时刻相对于t时刻的三维运动信息初始估计值R₀和T₀；

$R_0={\mathrm{\Δt}}^{t+1}*\frac{R^t}{{\mathrm{\Δt}}^t},T_0={\mathrm{\Δt}}^{t+1}*\frac{T^t}{{\mathrm{\Δt}}^t}$

Δt^t为t时刻和t-1时刻相邻帧之间的采样间隔，Δt^t+1t+1时刻和t时刻相邻帧之间的采样间隔；

步骤8：基于相邻帧三维点云数据D^t和D^t+1、三维运动初始估计值R₀和T₀，采用ICP算法估计相邻帧的三维运动R和平移向量T；

根据三维运动估计初值旋转矩阵R₀和平移向量T₀、t时刻场景的三维点云D^t、t+1时刻场景的三维点云D^t，通过ICP迭代算法，计算t+1时刻相对于t时刻的三维旋转向量R和平移向量T，即获得相邻两帧的三维运动估计；

步骤9：若机器人系统运动结束，无需对三维运动进行估计，则退出三维运动估计；否则转至步骤1重复继续进行下一个采样时刻与相邻帧的三维运动估计。

2.根据权利要求1所述的基于三维视觉系统图像特征和三维信息的运动估计方法，其特征在于，所述步骤1中相邻帧场景之间的采样间隔Δt的确定依据如下：

所述三维视觉系统的采样频率范围为[f_min,f_max]，相邻帧场景的动态采样间隔Δt变化范围为[Δt_min,Δt_max]，其中Δt最小值Δt_min＝1/f_max，最大值Δt_Max＝1/f_min；

采样间隔Δt的初始值设定为Δt_min；在运动过程中，基于t-1时刻和t时刻相邻帧的匹配图像特征点对集合M₃中匹配图像特征点对数目Num(M₃)和当前采样间隔Δt^t，依据表1动态确定下一步采样间隔Δt^t+1：

表1相邻帧之间采样间隔Δt动态计算

其中，Max()为取最大值函数，Min()为取最小值函数，Num(M₃)为M₃中的匹配特征点对数目；间隔调整最小步长其中L为采样间隔的等级数，取值范围为4-20；N₁和N₂分别为相邻帧匹配图像特征点对数目阈值，其中N₂>N₁，其取值范围是N₁≥10，N₂≥20。

3.根据权利要求2所述的基于三维视觉系统图像特征和三维信息的运动估计方法，其特征在于，所述步骤3利用SIFT算法对相邻帧场景的二维彩色图像I^t和I^t+1进行特征点提取与匹配，得到相邻帧场景的匹配特征点对集合M的具体步骤如下：

3.1)对t时刻和t+1时刻获取的相邻场景的二维彩色图像I^t和I^t+1，利用SIFT算法分别提取尺度旋转不变特征点集合F^t和F^t+1；

3.2)对F^t的任一特征点F_i ^t，采用欧式距离作为特征点128维向量描述子的相似性度量，然后从t+1时刻获取的二维彩色图像特征点F^t+1中，依次找出与F_i ^t匹配的特征点构成匹配特征点对

3.3)对F^t中所有特征点执行步骤3.2)，得到匹配特征点对集合M。

4.根据权利要求1-3任一项所述的基于三维视觉系统图像特征和三维信息的运动估计方法，其特征在于，所述步骤4误匹配点剔除中的3)采用RANSAC算法对匹配特征点对集合M₂中的误匹配进一步剔除，得到最终的匹配特征点对集合M₃的具体步骤如下：

①随机选择M₂中的s组匹配特征点对组成随机样本集M₃，并基于该样本计算匹配模型H；即使得匹配特征点对满足如下方程：

$H\×X_i^t=X_j^{t+1},$ 其中 $H=\left[\begin{array}{ccc}{h}_1& h_2& h_3\\ h_4& h_5& h_6\\ h_7& h_8& h_9\end{array}\right],X_i^t=\left[\begin{array}{c}{x}_i\\ y_i\\ 1\end{array}\right],X_j^t=\left[\begin{array}{c}{x}_j\\ y_j\\ 1\end{array}\right]$

②用①中计算的模型H，对M₂未选中的其它样本数据进行测试，若则该特征点对适用于估计的模型H，并称之为模型H的内点，放入M₃；否则认为它是外点；其中表示特征点相对于模型H的映射点和匹配对应点的欧式距离，T_d为距离阈值，×表示矩阵乘运算；

③重复K次采样，选择内点数目最多的集合M₃；

$K=\frac{log(1-p)}{log(1-{\left(1-\mathrm{\ϵ}\right)}^s)}$

其中，ε为M₂中的误匹配点对占所有匹配点对的比例，p为设定的匹配成功概率期望值，s为采样数目，s≥4；

④利用M₃中的样本数据重新估计计算模型参数H；

⑤利用模型H重新测试样本集M₂，确定新的集合M₃；反复执行步骤④⑤，直至前一次迭代与后一次迭代的M₃中的匹配特征点对数目差值不超过前一次迭代M₃中匹配特征点对数目的5％-10％为止。