CN103150728A - 一种动态环境中的视觉定位方法 - Google Patents

Abstract

本发明公开了一种可以在动态环境中实现精确定位的立体视觉定位方法。通过双目摄像机实时采集环境图像。通过特征提取算法提取图像中的特征点。对双目摄像机前后两个时刻采集的四幅图像进行立体匹配。利用双视几何原理恢复特征点三维信息。以回环匹配的方式提高匹配准确度。使用场流法剔除位于动态物体上的特征点。考虑可能引起误差的因素并通过计算协方差矩阵改善场流法效果。使用高斯牛顿迭代的方法由特征点位置信息求得机器人运动参数。使用RANSIC算法进一步提高视觉定位准确度。整个过程不断迭代，实现对机器人姿态和位置的实时计算。

Description

一种动态环境中的视觉定位方法

技术领域：

本发明是涉及图像处理及机器视觉的移动机器人自定位技术，该技术可以在复杂的动态场景中以视觉信息作为输入实现移动机器人自定位。

背景技术：

在智能机器人领域，移动机器人定位与导航技术一直是研究的关键问题。定位的精度及速度直接影响机器人完成既定任务的效率和准确度。之前的定位方法，如：编码器或轮式里程计等无法克服由摩擦、滑动等非线性因素引起的定位误差，近几年兴起的视觉定位方法可以很好地解决非线性误差，但现有的视觉定位方法存在一定缺陷：

1:当环境中存在纹理相似区域，及大量相似的重复性结构时，容易引起特征点的误匹配，导致定位错误。

2:环境中存在大量独立运动物体时，位于动态物体上的特征点会干扰里程计结果，导致定位精度降低。

为了使视觉定位技术可以在复杂的动态场景中得以应用，需要采用一定的方法来剔除位于动态物体上的特征点，然而目前的算法，如离群度测量算法，图像理解算法等均不能同时满足定位的实时性及有效性要求。如何让视觉里程计在动态环境中实时有效地工作成为本专利申请人关注和研究的新课题。

发明内容：

本发明的目的在于解决现有技术中存在的问题，提供一种可以在复杂动态环境中应用的视觉定位算法，使得移动机器人可以依靠视觉信息作为输入，完成在复杂环境中的自定位任务。本发明的技术方案包括以下步骤：

1：用已标定好的双目相机，进行左右图像连续采集。

2：采集当前时刻左右两幅图像，使用sobel算子提取图像中的特征点，并利用SIFT描述符进行描述,通过计算左右图像各点间特征描述符的欧式距离进行匹配，使用三维重建算法将特征点二维图像坐标转换为三维空间坐标，初始化位姿变换矩阵为单位矩阵，并设当前位置为原点。如果图像是第一帧，从新进入步骤2；如果图像不是第一帧进入步骤3。

3：将前时刻获取的左右两帧图像中的特征点，与前一时刻的两帧图像一起进行特征点的立体匹配，并通过回环匹配增加匹配准确度，得到各帧图像之间特征点的对应关系，利用双视几何原理求解出特征点在两帧图像中相对于摄像机坐标系的坐标。

4：使用场流法对匹配后的特征点集进行计算，求出该点在t-1时刻的位置经过位姿变换矩阵RT投影以后的三维坐标与该点t时刻三维坐标的差值，差值大于阈值的点认为是位于动态物体上的特征点，将其从特征点集中删除。其详细步骤如下：

4-1．由于机器人的运动过程是连续的，所以认为机器人在t时刻的位姿变化与t-1时刻位姿变化近似相等，以t-1时刻的RT作为t时刻RT的估计值。

4-2.利用4-1的结果，考虑相机成像时的像素位置误差，计算估计RT的不准确度，求出场流法的误差模型。

4-3.计算每个点对应于该模型的马氏距离值，值越大说明其离群度较高，将大于一定阈值的点从点集中删除。

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

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

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

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

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

6：使用步骤5的结果对全局的RT进行更新，得到机器人相对于初始坐标系的位置和姿态。

本发明的优点在于：

1）本发明通过回环匹配来增加匹配稳定性，减小了因误匹配引入的误差。

2）本发明使用场流法对特征点进行处理，可以将处于运动物体上的特征点剔除出去，使视觉定位可以在复杂的动态场景中实时有效的工作。

3）本发明在使用场流法时，考虑了可能产生误差的因素，求解其误差模型，计算各点对应该模型的马氏距离，使得对局外点的判断可以通过统一的阈值来决定。

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

附图说明：

图1是本发明提出的动态环境中视觉定位方法总体流程图。

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

图3是本发明中使用的回环匹配算法示意图。

图4是本发明中场流法的计算流程图。

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

具体实施方式：

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

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

步骤2：开启双目相机，进行左右图像连续采集，利用步骤1获取的相机参数对图像进行校正。

步骤3：如果图像是第一帧,对采集的左右图像使用sobel算子提取特征，并利用SIFT描述符进行描述,在左右图像之间进行特正匹配，得到特征点对应关系。如果图像不是第一帧，则将前后两个时刻共四幅图像进行特征匹配，匹配通过回环检测的方式提高准确度，如说明书附图3：从当前时刻左图像开始，按照顺序进行立体匹配，如果最后通过匹配可以找到该特征点本身，则将特征点信息存储于点集中，否则予以舍弃。

步骤4：利用步骤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)---\left(1\right)$

步骤4-1：计算特征点定位的误差模型，假设最初摄像机成像时的像素误差符合高斯分布，忽略像素之间的协方差，按照步骤4中的公式可以得出空间点三维位置

的误差模型。 ${\∑}_X=J\left(\mathrm{diag}({\mathrm{\σ}}_c^2,{\mathrm{\σ}}_r^2,{\mathrm{\σ}}_d^2)\right)J^T,$ 其中J为公式（1）的雅克比矩阵。

步骤4-2：利用步骤4-1的结果，可以求得机器人运动参数估计的不准确性：Σ_q＝(H^T×Σ_x×H)^-1。其中H为由三维点集推算机器人运动参数时的误差传递矩阵。

步骤4-3：计算场流的表达式为：

$M=[\left(\begin{array}{c}{X}_{t+1}\\ Y_{t+1}\\ Z_{t+1}\end{array}\right)-\left(\begin{array}{c}{X}_t\\ Y_t\\ Z_t\end{array}\right)]=[\frac{B}{u_r^{\′}-u_l^{\′}}\left(\begin{array}{c}{u}_l^{\′}-u_0\\ v^{\′}-v_0\\ f\end{array}\right)-\frac{B}{u_r-u_l}\·R\·\left(\begin{array}{c}{u}_l-u_0\\ v-v_0\\ f\end{array}\right)+T]---\left(2\right)$

场流法的误差模型表达式

式中S_SF为场流表达式中所有输入变量的协方差矩阵，可以通过对Z_sf＝(u_l',u_l',v',u_l,u_l,v,t_x,t_y,t_z,q_x,q_y,q_z)求偏导数获得，J_SF对应（2）式的雅克比矩阵。

步骤4-4：利用步骤4-3求得的Σ_SF可以求得取每一点对应场流的马氏距离

马氏距离反应的是符合一定概率分布总体中某一样本的离群度，值越大，对应的离群度越高，在场流法中越有可能是属于动态物体上的特征点。将大于一定阈值的特征点对从特征点集中删除从而达到排除动态物体对里程计影响的目的。

步骤5：对步骤4得到的三维特征点集结合RANSIC算法与牛顿高-斯迭代进行运动估计，得到R和T：

如图5，其中详细步骤如下：

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

5-2.利用求解出的R和T对点集

中的所有点进行投影变化

$\left(\begin{array}{c}u\\ v\\ 1\end{array}\right)=\left(\begin{array}{ccc}f& 0& c_u\\ 0& f& c_v\\ 0& 0& 1\end{array}\right)[\left(\begin{array}{cccc}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)]---\left(3\right)$

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

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

其中运动估计算法如下：

（1）将公式（3）展开并对待求未知量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₃     （4）

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₆

其中 $b_{\mathrm{ij}}=\underset{k=1}{\overset{n}{\mathrm{\Σ}}}\frac{\∂f_{k0}}{\∂a_i}\·\frac{\∂f_{k0}}{\∂a_j},$ $B_i=\underset{k=1}{\overset{n}{\mathrm{\Σ}}}\frac{\∂f_{k0}}{\∂a_i}(C_k-f_{k0}).$ 其中表示对（3）式的第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)对运动参数进行更新。

步骤6.将每一次步骤5求出的结果转化为4*4的位姿变换矩阵，从初始时刻开始进行累乘，最终得到的结果就是机器人目前相对于初始时刻的位姿状态。

Claims (4)

1.一种可以在复杂动态场景中应用的视觉定位方法，其特征在于，包括以下步骤： 

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

步骤二：开启相机，进行左右图像的连续采集。 

步骤三：如果图像是第一帧,sobel算子提取图像中的特征点位置，并利用SIFT描述符进行描述,在左右图像之间进行特正匹配，得到特征点对应关系。如果图像不是第一帧，则在前后两个时刻共四幅图像间进行特征匹配，匹配通过回环检测的方式提高准确度。 

步骤四：通过双视几何的原理计算特征像素点对应的三维空间位置，使用场流法对特征点进行筛选，考虑计算场流过程中可能引入的误差，通过计算每个点对应的马氏距离，将离群度大于一定阈值的点从特征点集中删除。 

步骤五：使用RANSIC算法进行迭代，每次迭代时利用求得的姿态R和位置T计算满足该变换的特征点数目，求取包含局内点最多一组结果，并利用该组中所有局内点求解更准确的R和T。 

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

2.权利要求1所述的一种动态场景中视觉定位方法，其特征在于，所述步骤三中采用回环匹配的方法减小误匹配概率。 

3.权利要求1所述的一种动态场景中视觉定位方法，其特征在于，所述步骤四中考虑了场流法的误差模型，通过每一组特征点对应的马氏距离，将离群度大于阈值的特征点对从点集中删除，达到排除位于动态物体上的特征点的效果。 

步骤4-1计算场流法误差模型，最初的像素点误差成高斯分布，计算定位空间点三维位置的误差。 

步骤4-2利用4-1求得的误差模型以及视觉定位模型的雅克比矩阵，求得估计出的RT的误差模型。 

步骤4-3利用4-2的结果以及和场流法对应的雅克比矩阵，求得场流法的误差模型。 

步骤4-4利用4-3求得的场流误差模型计算每一特征点对应的马氏距离。 

4.权利要求1所述的一种动态场景中视觉定位方法，其特征在于，使用RANSIC算法，每次选取三个特征点对计算RT，对所有特征点对按照计算出的RT进行投影，统计局内点数目，迭代N次后，以局内点数目最大的一组特征点对为输入，估计得到更精确的R和T。 

Images