CN103345751A - 一种基于鲁棒特征跟踪的视觉定位方法 - Google Patents

Abstract

本发明公开了一种基于图像处理及机器视觉的鲁棒特征跟踪与立体视觉定位技术。该技术可以在相机晃动以及室外光照条件下，融合惯性信息和视觉信息，实现可靠的立体视觉定位。通过双目摄像机实时采集图像，同时利用惯性测量单元采集相机旋转信息。利用特征提取算法提取图像中的特征点，并对左右图像中的特征点进行立体匹配。结合惯性信息，通过惯性融合KLT算法跟踪特征点，提高特征跟踪可靠性。利用双视几何原理恢复特征点的三维信息。使用高斯牛顿迭代的方法由特征点位置信息求得相机运动参数。使用RANSIC算法进一步提高视觉定位准确度。整个过程不断迭代，实现相机姿态和位置的实时计算。

Description

一种基于鲁棒特征跟踪的视觉定位方法

技术领域：

本发明是基于图像处理及机器视觉相关领域的鲁棒特征跟踪与立体视觉定位技术。该技术可以在相机晃动以及室外光照条件下，以惯性信息和视觉信息作为输入，实现可靠的立体视觉定位。

背景技术：

在计算机视觉领域，双目立体视觉模拟人类双眼处理景物的方式对空间点进行定位，可以很好地解决非线性误差，是其中一个重要研究方向并且具有广泛的适用性。在移动机器人自主执行任务时进行自定位以及盲人辅助出行定位系统中均可采用双目立体视觉进行定位。在这些系统中，定位的精度及速度直接影响着整个系统的性能和效率。但现有的视觉定位方法存在一定缺陷：

1）当相机在采集图像过程中发生剧烈晃动或旋转，无法在平稳状态下工作时，例如盲人辅助出行系统中相机搭载在使用者身上，随着使用者的行走会发生晃动；搭载有定位系统的自主机器人在崎岖环境中作业时，相机也会发生抖动甚至较大旋转。此时相机采集到的前后连续图像之间视差过大，导致特征点跟踪失败，特征点数量变化剧烈，运动估计误差过大，最终使得定位失败。

2）当环境中存在大量运动物体时，不加区分地将位于动态物体上的特征点带入计算会影响里程计的结果，导致定位精度降低。

3）当视觉定位系统工作环境中存在有大量纹理相似的区域或重复性结构时，左右图像在立体匹配过程中容易发生特征点的误匹配，从而导致定位失败。

4）由于工作环境中光照条件变化，导致特征点误匹配，特征跟踪失败，从而无法准确定位。

为了使视觉定位技术可以在不稳定平台中相机发生大幅晃动的情况下得以应用，需要采用一定的方法来补偿相机晃动对特征点跟踪造成的影响，然而目前的算法，如传统KLT算法，基于一定的假设条件，对图像灰度和图像间的视差变化都有严格的要求，不能同时满足定位的实时性及有效性要求。如何让视觉里程计在相机大幅晃动条件下实时有效地工作成为本专利申请人关注和研究的新课题。

发明内容：

本发明主要用于解决现有视觉定位技术在相机晃动以及室外光照环境中定位不可靠的问题，提出一种惯性信息与视觉信息相融合的视觉定位算法，通过加入惯性信息预测特征点在当前时刻图像中的迭代初始位置，并且结合仿射光度运动模型来实现鲁棒的特征跟踪，进而提高视觉定位的可靠性。本发明的技术方案包括以下步骤：

1：预先标定双目相机内参数，包括：焦距、中心点坐标、基线长度等。

2：将相机与惯性测量单元固连，用于同时采集图像信息和惯性信息（旋转信息）。标定相机坐标系与惯性测量单元坐标系之间的旋转变换矩阵，同步图像数据与惯性信息的采集。

3：用已标定好的双目相机，进行左右图像连续采集。同时使用惯性测量单元采集对应时刻相机的旋转信息。

4：读取当前时刻左右两幅图像和对应的相机旋转变化量，如果当前帧是第一帧，则进入步骤8，提取新特征点；如果当前图像不是第一帧，则根据相机旋转变化量计算出特征点在当前帧中的初始预测位置，使用惯性融合KLT算法对特征点进行跟踪，得出特征点在当前帧中的准确位置。

4-1.根据模板计算海森矩阵。

4-2.根据相机运动变换计算预测运动参数初值。

4-3.根据新输入图像，通过高斯牛顿迭代更新运动参数。

5：左右图像中立体匹配的一组特征点具有相同的ID序号，根据ID序号，对左、右图像中跟踪上的特征点进行筛选，去除其中丢失了匹配点的特征点（即孤立特征点，在另一幅图像的特征点集中找不到相同ID的匹配点），保留立体匹配的特征点，利用双视几何原理求解出特征点在两帧图像中相对于摄像机坐标系的坐标。

6：使用高斯-牛顿迭代算法，以当前时刻和前一时刻的匹配特征点集作为输入量，求解相机在当前时刻相对于前一时刻的位姿变化，同时使用RANSIC算法进一步提高定位准确度。

6-1.在特征点集中随机选择3对特征点，通过迭代算法，进行一次运动估计，得到一个[R T]。

6-2.利用当前的[R T]，对所有前一时刻特征点进行投影，对比投影结果与当前观测位置的差，将差值小于阈值的点记为局内点。

6-3.重复6-1、6-2，进行多次迭代，比较每次局内点数目，将局内点数目最大的一组局内点重新构成特征点集。

6-4.利用新的特征点集，使用高斯-牛顿迭代进行运动估计，得到更优的R和T。

7：使用步骤6的结果对全局的位姿变换矩阵[R T]进行更新，得到相机相对于初始坐标系的位置和姿态。

8：如果当前帧剩余的匹配特征点数目小于设定的阈值，使用sobel算子提取图像中的兴趣点，并利用SIFT描述符对这些点进行描述，经过立体匹配得到新的特征点，然后重复步骤4；否则直接重复步骤4。

本发明的优点在于：

1）本发明在利用KLT算法进行特征跟踪时融合了系统的惯性信息，对迭代计算的初始值进行了预测，大大增加了迭代收敛的概率，提升了在相机晃动情况下的特征跟踪性能。

2）本发明采用逆向KLT算法，具体运动模型使用仿射光度变换模型，加入了对光照和运动变换的补偿，同时减少模板更新次数和计算量，兼顾了跟踪性能和运行效率，缩短了程序运行时间，使视觉定位可以在室内以及室外的动态场景中实时有效的工作。

3）本发明在立体匹配过程中采用回环匹配来增加匹配稳定性，减小了因误匹配引入的误差。

4）在视觉定位过程中使用了RANSIC算法，该方法与KLT算法结合使用，提高了视觉定位方法的实时性与有效性。

附图说明：

图1是本发明提出的相机大幅晃动情况下视觉定位方法总体流程图。

图2是本发明中定义的各个坐标系。

图3是本发明中惯性融合KLT算法流程图。

图4是本发明中RANSIC算法流程图。

具体实施方式：

下面结合说明书附图对本发明方法做详细说明：

步骤1：对双目摄像机进行标定，获取相机内外参数，包括：焦距f，基线长度b，图像中心像素位置u₀，v₀，整幅图像的校正矩阵等。相机内参数矩阵为：

$K=\left[\begin{array}{ccc}{f}_x& 0& u_0\\ 0& f_y& v_0\\ 0& 0& 0\end{array}\right]$     （式-1）

步骤2：将相机与惯性测量单元固连，确保其不会发生相对运动。标定相机坐标系与惯性测量单元坐标系之间的旋转变换矩阵

同步图像数据与惯性信息的采集，在采集图像的同时获得此刻IMU测得的旋转信息。

步骤3：开启双目相机同时启动惯性测量单元，用已标定好的双目相机和惯性测量单元进行左右图像连续采集，利用步骤1获取的相机参数对图像进行校正，同时使用惯性测量单元采集对应时刻系统的旋转信息

步骤4：读取当前时刻左图像I1_t、右图像I2_t、对应的相机旋转信息如果当前输入图像是第一帧，则进入步骤8，提取新特征点；如果当前输入图像不是第一帧，则根据相机旋转变化量计算出特征点在当前帧中的初始预测位置，使用惯性融合KLT算法对特征点进行跟踪，如附图3所示流程，得出特征点在当前帧中的准确位置。

在KLT算法中，采用仿设光度弯曲模型T(x;p)＝(α+1)T(Ax+b)+β作为仿设函数w(x;p)的具体形式，函数中一共包含8个参数p=(a₁,…，α₆，α，β)，其中：

$A=\left[\begin{array}{cc}1+a_1& a_2\\ a_3& 1+a_4\end{array}\right],$ $b=\left[\begin{array}{c}{a}_5\\ a_6\end{array}\right],$ （A，b）用于解决相机转动和移动产生的特殊形变，(α,β)用于补偿光照变化。

步骤4-1.判断当前读入图像是否是第一帧，若是则进入步骤8；若不是则判断特征点数量，如果特征点的数量小于设定的阈值，则进入步骤8选取新的特征点并更新模板T，否则保持模板不变。

步骤4-2.如果模板没有更新，直接进入步骤4-3；如果选取了新模板，则根据新模板重新计算海森矩阵：

计算雅克比矩阵

$J=\frac{\∂T}{\∂p}{|}_{p=0}=\left[\begin{array}{ccc}\frac{\∂T}{\∂a}& \frac{\∂T}{\∂\mathrm{\α}}& \frac{\∂T}{\∂\mathrm{\β}}\end{array}\right]=\left[\begin{array}{ccc}\frac{\∂T}{\∂a}& T& 1\end{array}\right]$     （式-2）

计算海森矩阵

$H=\underset{x}{\mathrm{\Σ}}{J}^{T}J$     （式-3）

4-3.计算相机旋转变化量

设t-1时刻和t时刻陀螺仪分别采集到的旋转矩阵为

则从t-1时刻到t时刻相机发生的旋转变化量为：

$\mathrm{\Δ}R_{t-1}^t{}_c=R_c^i\·R_t^{-1}{}_i{}^G\·R_{t-1}^i{}_G\·R_{-1}^c{}_i$     （式-4）

4-4.根据相机运动变换计算单应矩阵H。

采集到的图像中特征点的运动可以通过2维单应矩阵H来描述，H是（A，b）的高阶变形。可以从H中提取出运动模型的预测参数。

$H=K\·\mathrm{\Δ}R_{t-1}^t{}_c\·K^{-1}$     （式-5）

4-5.遍历全部特征点，利用KLT算法跟踪特征点，得到特征点在当前图像中的坐标位置。

（1）计算各个特征点的预测运动参数：

$\left\{\begin{array}{c}{A}_{\mathrm{pred}}=H_{2\×2}\\ b_{\mathrm{pred}}=x_H-x,x_H=H{\left[\begin{array}{cc}x& 1\end{array}\right]}^T\\ {\mathrm{\α}}_{\mathrm{pred}}={\mathrm{\β}}_{\mathrm{pred}}=0\end{array}\right.$     （式-6）

（2）计算运动参数的迭代初始值

    （式-7）

（3）利用高斯-牛顿迭代法计算运动参数变化量δp。

（4）更新运动参数p_t=（（A，b）_t α_t β_t）：

$\left\{\begin{array}{c}{(A,b)}_t\←(A_{t-1}\mathrm{\δ}{A}^{-1},b_{t-1}-A_{t-1}\mathrm{\δb})\\ {\mathrm{\α}}_t\←({\mathrm{\α}}_{t-1}+1)/(\mathrm{\δ\α}+1)\\ {\mathrm{\β}}_t\←{\mathrm{\β}}_{t-1}-({\mathrm{\α}}_{t-1}+1)\mathrm{\δ\β}\end{array}\right.$     （式-8）

（5）计算各个特征点的跟踪质量参数：误差平方和、归一化互相关、剪切度。

（6）判断各个特征点的跟踪质量参数是否低于设定的阈值，如果某个特征点的质量参数小于阈值，则剔除该点，否则记录特征点的当前位置。

（7）统计跟踪成功的特征点个数。

步骤5：左右图像中立体匹配的一组特征点具有相同的ID序号，根据ID序号，对左、右图像中跟踪上的特征点进行筛选，去除其中丢失了匹配点的特征点（即孤立特征点，在另一幅图像的特征点集中找不到相同ID的匹配点），保留立体匹配的特征点，利用双视几何原理求解出特征点在两帧图像中相对于摄像机坐标系的坐标。

5-1.遍历左图像中的特征点，查找在右图中是否有相同ID的特征点，若找到ID相同的特征点，则这组特征点是立体匹配对，可用于计算位姿变换；若某个特征点在右图中没有相同ID的特征点，则此点为孤立点，将其剔除。

5-2.仿照步骤5-1遍历右图像中的特征点，得到立体匹配对，并剔除孤立点。

5-3.根据立体匹配对计算像素点的三维坐标：

$\left(\begin{array}{c}X\\ Y\\ Z\end{array}\right)=\left(\begin{array}{c}(u-u_0)b/d\\ (v-v_0)b/d\\ f\×b/d\end{array}\right)$     （式-9）

步骤6：对步骤5得到的三维特征点集结合RANSIC算法与牛顿高-斯迭代进行运动估计，得到旋转和平移矩阵[R T]：

6-1.步骤5已经获得的三维点集包括同一个点在当前时刻和前一时刻在相机坐标系中的坐标，随机选择其中的三个特征点，进行一次运动估计，求解出一个[R T]。

6-2.利用求解出的[R T]对点集

中的所有点进行投影变化：

$\left(\begin{array}{c}u\\ v\\ 1\end{array}\right)=\left(\begin{array}{ccc}{f}_x& 0& u_0\\ 0& f_y& v_0\\ 0& 0& 1\end{array}\right)[\left(\begin{array}{cc}R\left(r\right)& t\end{array}\right)\left(\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right)-\left(\begin{array}{c}s\\ 0\\ 0\end{array}\right)]$     （式-10）

其中{x、y、z}为点集中一个点在t-1时刻的三维坐标，点对应的像素位置u、v在实际中均取整数，计算由（式-10）式得到的u、v值与该点在t时刻观测到的u、v值之差。差值小于阈值的点纪录为局内点。遍历当前时刻所有点，得到局内特征点集

记录局内特征点总数。

6-3.重复步骤6-1、6-2，进行多次迭代，记录每一次迭代中得到的局内特征点数，找出局内特征点数最多的一次，利用所有局内特征点作为输入，进行运动估计，得到更优的[R T]。

其中运动估计算法如下：

（1）将（式-10）展开并对待求未知量t_x,t_y,t_z与r_x,r_y,r_z求偏导数。

（2）利用（1）的结果，若为第一次计算，则首先将(t_x,t_y,t_z,r_x,r_y,r_z)的初值全部设为0，否则以上一次运算的结果作为初值。以Δ表示待求变量估计值与真实值之间的差值，构建标准方程组

b₁₁Δ_tx+b₁₂Δ_ty+b₁₃Δ_tz+b₁₄Δ_rx+b₁₅Δ_ry+b₁₆Δ_rz＝B₁

b₂₁Δ_tx+b₂₂Δ_ty+b₂₃Δ_tz+b₂₄Δ_rx+b₂₅Δ_ry+b₂₆Δ_rz＝B₂

b₃₁Δ_tx+b₃₂Δ_ty+b₃₃Δ_tz+b₃₄Δ_rx+b₃₅Δ_ry+b₃₆Δ_rz＝B₃    （式-11）

b₄₁Δ_tx+b₄₂Δ_ty+b₄₃Δ_tz+b₄₄Δ_rx+b₄₅Δ_ry+b₄₆Δ_rz＝B₄

b₅₁Δ_tx+b₅₂Δ_ty+b₅₃Δ_tz+b₅₄Δ_rx+b₅₅Δ_ry+b₅₆Δ_rz＝B₅

b₆₁Δ_tx+b₆₂Δ_ty+b₆₃Δ_tz+b₆₄Δ_rx+b₆₅Δ_ry+b₆₆Δ_rz＝B₆

其中

其中

表示对（式-10）的第k行对第i个变量求偏导数，其中a_i∈(t_x,t_y,t_z,r_x,r_y,r_z)。C_k为特征点在t时刻对应的像素坐标位置。

（3）使用消元法求解（2）中的方程组，可以解出Δ_i的值。将Δ_i与初值相加可以求得各个变量的当前估计值。

（4）使用（3）得出的运动参数(t_x,t_y,t_z,r_x,r_y,r_z)将前一时刻空间点三维位置，投影到当前时刻的图像平面，当满足精度要求或达到最大迭代次数时，使用当前的(t_x,t_y,t_z,r_x,r_y,r_z)对运动参数进行更新。

步骤7：使用步骤6的结果转化为4*4的位姿变换矩阵，从初始时刻开始进行累乘，最终得到的结果就是相机当前相对于初始时刻的位姿状态，对全局的[R T]进行更新。

步骤8：如果当前帧剩余的匹配特征点数目小于设定的阈值，提取新的特征点，然后重复步骤4；否则直接重复步骤4。

8-1.使用sobel算子提取图像中的兴趣点，并利用SIFT描述符对这些点进行描述。

8-2.以跟踪上的特征点作为中心在其周围邻域内设置掩膜，去掉距离这些特征点较近的兴趣点。

8-3.然后计算左图像中各个兴趣点与右图像中兴趣点的特征描述符的欧式距离，距离最小的一对兴趣点即为空间中某一点分别在左、右图像中的投影，作为一组立体匹配的新特征点。

8-4.将图像等分为几个大小相同的区块，若某些区块中特征点数量过多，明显多于其他区块，则去掉这些区块中的一些多余特征点，保证每个区块中特征点数量比较平均。

8-5.给左右图像中的同一组新特征点赋予相同的ID序号。每一对特征点的ID序号在整个程序运行过程中是唯一的。

8-6.若当前帧是第一帧，初始化位姿变换矩阵为单位矩阵，并设当前位置为原点。重复步骤4。

Claims (4)

1.一种基于鲁棒特征跟踪的视觉定位方法，其特征在于，包括以下步骤：

步骤一：对双目摄像机进行标定。

步骤二：将相机与惯性测量单元固连，标定相机坐标系与惯性测量单元坐标系之间的旋转变换矩阵。

步骤三：开启相机，进行左右图像与相机旋转信息的同步采集。

步骤四：如果当前图像是第一帧或当前特征点总数小于设定阈值,用sobel算子提取图像中的兴趣点，并用SIFT描述符对兴趣点进行描述,在左右图像之间进行立体匹配，得到特征点匹配对。如果读入的图像不是第一帧，则根据相机旋转变化量计算出特征点在当前帧中的初始预测位置，使用惯性融合KLT算法对特征点进行跟踪，得出特征点在当前帧中的准确位置。

步骤五：通过双目立体视觉原理计算特征点对应的三维坐标位置。

步骤六：使用RANSIC算法进行迭代计算，每次迭代时利用求得的旋转矩阵R和平移矩阵T计算满足该变换的特征点数目，求取包含局内点最多的一组结果，并利用该组中所有局内点求解出精确的R和T。

步骤七：将步骤六求得的结果进行累乘，得到相机在当前时刻相对于初始时刻的位姿变换矩阵。

2.权利要求1所述的一种基于鲁棒特征跟踪的视觉定位方法，其特征在于，所述步骤二中将相机与惯性测量单元固连，利用惯性测量单元采集相机的旋转信息，用于辅助视觉定位。

3.权利要求1所述的一种基于鲁棒特征跟踪的视觉定位方法，其特征在于，所述步骤四中采用惯性融合KTL算法对特征点进行跟踪，结合系统的惯性信息，对运动参数迭代计算的初始值进行了预测，大大增加了迭代收敛的概率，提升了在相机晃动情况下的特征跟踪性能。此外，使用仿射光度变换模型作为运动模型，加入了对光照和运动变换的补偿，减少模板更新次数和计算量，兼顾了跟踪性能和运行效率。

步骤4-1选取新的特征点并更新模板T。

步骤4-2根据步骤4-1中求得的新模板计算海森矩阵。

步骤4-3计算相机旋转变化量。

步骤4-4利用4-3求得的相机旋转变化量计算单应矩阵H，计算运动参数的迭代初始值。

步骤4-5利用高斯-牛顿迭代法计算运动参数变化量，最终更新运动参数。

4.权利要求1所述的一种基于鲁棒特征跟踪的视觉定位方法，其特征在于，使用RANSIC算法，每次先选取三个特征点匹配对计算位姿矩阵[R T]，按照计算出的[R T]对所有特征点进行投影，统计局内点数目，经过多次迭代，以局内点数目最多的一组特征点匹配对作为输入，计算得到精确的R和T。

Images