CN109813334B - 基于双目视觉的实时高精度车辆里程计算方法 - Google Patents

Abstract

本发明涉及基于双目视觉的实时高精度车辆里程计算方法，该方法包括以下步骤：1、采用视觉传感器获取图像序列；2、对获取的前后帧立体图像利用改进FAST角点检测算法提取特征点；3、利用SIFT算法计算特征点的主方向并生成特征描述子；4、根据生成的特征描述子进行特征匹配，并采用自适应RANSAC算法剔除误匹配点；5、根据匹配点集误差构造目标函数，求解目标函数得到运动估计信息；6、利用粒子滤波进行最优估计，减小误差。本发明采用改进FAST算法提取特征点，使得到的特征点更加稳定、可靠；使用自适应RANSAC算法剔除误匹配点，减少了算法的迭代次数，从而减少了计算量，降低了算法的运行时间；拥有很强的鲁棒性，而且提高了算法的运行效率和精确度。

Description

基于双目视觉的实时高精度车辆里程计算方法

技术领域

本发明属于智能车辆自主定位技术领域，尤其涉及一种基于双目视觉的实时高精度车辆里程计算方法。

背景技术

智能车作为智能交通系统中发展最活跃的一部分，受到了世界各研究机构和高校的重点关注。目前，许多国家都制定了智能车发展战略，并且随着科技的高速发展，智能车辆在大型工厂、公园内都已经得到了成功应用。里程计作为智能车系统的一部分，发挥着承上启下的重要作用，是智能车完成其它工作的基础和前提，同时也是智能车完成辅助驾驶和自主行为的重要保障。

基于视觉传感器的里程计技术在二十世纪六十年代被提出，到现在为止已经有五十多年的研究历史，自从被提出以来一直是计算机视觉和机器人领域研究的重点。视觉里程计是利用车载的视觉传感器(相机)采取的图像序列来估计车辆的六自由度运动信息，包括旋转和平移。视觉里程计的实现主要包括以下三个步骤：第一步是通过安装在车上的相机来采取图像序列；第二步从采取的图像中提取特征点并且与先前帧的图像特征点进行匹配；第三步是运动估计，利用第二步匹配的特征点来计算前后两个时刻之间车辆的相对运动T_x，然后通过将T_x与先前的位姿连接起来计算车辆当前的位姿信息，从而完成视觉定位。

视觉里程计主要依靠视觉传感器来得到车辆的具体位置信息，与其它传感器相比具有成本比较低、能耗小、安装方便、便携性好、抗电磁干扰能力强等优点。其次，从视觉传感器中采集到丰富的场景信息在用到车辆自主定位的同时还可以为更高级的功能如目标检测、目标识别、障碍物的避障以及三维地图构建等提供必不可少的信息。另外，与内置传感器定位的方法相比，视觉里程计技术不会受轮胎滑移的影响，也不会产生误差累积。但是视觉里程计系统中的图像处理计算成本高，实时性差，而且获取的外部场景信息复杂，具有尺度不确定性，容易引入误差，还会受到光照变化、阴影遮挡、视角变化等影响。所以说视觉里程计仍然面临许多挑战。

发明内容

本发明的目的是针对现有车辆视觉里程计技术的缺点与不足，提出了一种基于双目视觉的实时高精度车辆里程计算方法。

该方法包括以下步骤：

基于双目视觉的实时高精度车辆里程计算方法，该方法包括以下步骤：

步骤1、采用视觉传感器获取图像序列；

步骤2、对获取的前后帧立体图像利用改进FAST角点检测算法提取特征点；

步骤3、利用SIFT算法计算特征点的主方向并生成特征描述子；

步骤4、根据生成的特征描述子进行特征匹配，并采用自适应RANSAC算法剔除误匹配点；

步骤5、根据匹配点集误差构造目标函数，求解目标函数得到运动估计信息；

步骤6、利用粒子滤波进行最优估计，减小误差。

进一步的，步骤2中，使用改进的FAST角点检测算法提取特征点，具体步骤如下：

S1、获取一张图片；

S2、在图片中任意选择一个像素点p，假设它的亮度是I_p；

S3、设定一个阈值T；

S4、以像素点p为中心，分别选取半径为3和2的两个同心圆上的16和12个像素点；

S5、将选取的这些像素点的亮度分别于I_p+T或I_p-T相比较；

S6、如果在这两个同心圆上分别有M和N像素点的亮度大于I_p+T或小于I_p-T，则可以认为p是一个特征点；

S7、循环S2-S6，对每一个像素点进行上述操作；

S8、对选取的特征点进行非极大值抑制，并记录最终特征点的位置。

进一步的，步骤3中，根据得到的特征点的位置信息，利用SIFT算法计算特征点的主方向以及特征描述子，具体方法如下：

对于一个特征点，以它为中心，计算它邻域像素的梯度和方向，用直方图进行统计，得到特征点邻域像素的梯度分布，直方图中的峰值就是特征点的主方向；梯度和方向的计算公式如下：

θ(x,y)＝arctan((L(x,y+1)-L(x,y-1))/(L(x+1,y)-L(x-1,y)))  (2)

以关键点为中心选取16×16的邻域窗口，关键点所在的行和列不取，计算邻域窗口内所有像素点的梯度模值和梯度方向，然后将每个窗口分成4×4的子区域，总共16个子区域，每个子区域包含8个方向上的信息，一共有128个方向信息，生成128维的SIFT特征向量，即128维的特征描述子。

进一步的，步骤4中，根据生成的特征描述子进行特征匹配，具体方法如下：

对于t-1时刻这一帧图像上的特征点集{P}中的每一个特征点p，从t时刻这一帧图像上的特征点集{P′}中找到与特征点p的特征向量最近邻的特征点和次近邻的特征点，并记录它们之间的距离；

若最近邻特征点的距离与次近邻特征点的距离比小于等于设定的阈值T，则认为特征点p与最近邻的特征点p′是一对匹配点，记为(p,p′)；

若最近邻特征点的距离与次近邻特征点的距离比大于设定的阈值T，则认为特征点p在特征点集{P′}中没有相匹配的特征点；

由以上可知，对每一个特征点都进行上述操作，并记录匹配上的特征点对，最后可以得到一个匹配点对的集合{(P_n,P′_n)}；

自适应RANSAC算法剔除误匹配点，具体方法如下：

S1、对于得到的粗匹配点对集合{(P_n,P′_n)}，计算集合中每对匹配点对之间的欧氏距离，并按从小到大的顺序进行排列；

S2、取排在前20％的匹配点对组成新的样本点对集合U；

S3、从U中随机选取6个匹配点对，利用其中4个计算变换模型H_i；

S4、利用剩余的2个对H_i进行验证，如果通过验证，则进行下一步；否则，舍弃H_i，重新返回Step3；

S5、对匹配点对集合{(P_n,P′_n)}根据变化模型H_i计算对称变换误差d，若d小于设定的值t，则认为是一个内点，并统计内点的数量M；

S6、重复Step3-Step5三步N次，若M大于设定的阈值S，则利用M重新估算变换模型H，即为最优的变换模型。

进一步的，步骤5中，根据匹配点集的误差构造目标函数，求解目标函数得到运动估计信息，具体方法如下：

根据步骤(4)可以得出一组匹配好的3D点：

P_m＝{p₁,L,p_m}，P′_m＝{p′₁,L,p′_m}  (3)

到一个欧式变换R和t，使得

这个问题可以用迭代最近点(ICP)来求解，这里采用的方式SVD为代表的方法进行求解，先定义第i对点的误差项：

e_i＝p_i-(Rp′_i+t)  (5)

然后构建最小二乘函数，求使误差平方和达到极小的R,t:

最后使用SVD方法求解出两帧图像之间的运动估计信息(R,t)，即T_k。

进一步的，步骤6中，利用粒子滤波进行最优估计，减小误差，具体方法如下：

由步骤5得到的运动估计信息，通过计算C_k＝C_k-1T_k来连接转换，得到运动系统的状态方程和观测方程如式(7)，(8)所示：

x_k＝f(x_k-1,u_k,w_k)  (7)

z_k＝h(x_k,v_k)  (8)

其中，x_k为k时刻系统的状态变量，u_k为系统的控制变量，z_k为k时刻的观测变量，w_k和v_k分别为输入的白噪声和观测噪声。基于粒子滤波的视觉里程计算法是先利用k-1时刻的位置x_k-1和系统噪声v_k-1来预测k时刻的位置

然后在根据k时刻的观测值z_k来更新k时刻的位x_k置，就这样不停的循环迭代以得到车辆的最优位姿估计值。

本发明与现有的技术相比，具有以下优点：

1、本发明采用改进FAST算法提取特征点，使得到的特征点更加稳定、可靠；

2、本发明使用自适应RANSAC算法剔除误匹配点，减少了算法的迭代次数，从而减少了计算量，降低了算法的运行时间；

3、本发明拥有很强的鲁棒性，而且提高了算法的运行效率和精确度。

附图说明

图1为本发明实施例改进FAST算法检测过程示意图。

图2为本发明实施例改进FAST-SIFT融合算法框图。

图3为本发明实施例自适应RANSAC算法剔除误匹配点后的结果。

图4为本发明实施例基于双目视觉的实时高精度车辆里程计算法的方案流程图。

具体实施方式

下面结合附图和具体实施方式对本发明进行详细说明。

本发明的目的在于设计一种实时高精度的车辆视觉里程计算法，采用车载的双目相机完成智能车辆的自主定位和导航的需求。该算法利用改进FAST算法提取特征点，采用自适应RANSAC算法剔除误匹配点，解决了传统里程计算法抗干扰能力弱、耗费时间长、计算复杂度高等一系列问题，提高了算法的运行效率和鲁棒性。

算法的整体方案流程图如图4所示：

(1)将双目相机固定在智能车外围顶端，获取图像序列，通过USB传输到车载处理器上；

(2)使用改进FAST角点检测算法提取特征点；

对获取的第t帧图像采用改进FAST算法提取特征点，得到一个特征点集合P；

(3)利用SIFT算法计算特征描述符；

根据上一步得到的每一个特征点的位置信息，选取特征点的邻域像素，计算特征点的主方向和特征描述子；

(4)特征点匹配以及采用自适应RANSAC算法剔除误匹配点；

根据第t-1帧图像得到的特征点集合P′通过快速近似最近邻算法完成特征点粗匹配，并计算粗匹配点之间的欧氏距离，按从小到大的顺序排列；根据匹配对之间的欧式距离越短，说明它们之间的相似度越高的原理，选取排在前20％的匹配点对作为随机抽样的集合来求解变换模型，从而利用变换模型获得内点集合。

(5)求解两帧图像之间的运动信息；

根据第(4)步得到的匹配点对集合定义误差项，构建最小二乘函数，采用SVD方法求解出运动信息(R,t)；

(6)利用粒子滤波进行最优估计；

由第(5)步得到的运动估计信息，通过计算C_k＝C_k-1T_k来连接转换，得到运动系统的状态方程和观测方程，利用粒子滤波算法得到最优估计值。

参见图1，步骤2中，使用改进的FAST角点检测算法提取特征点，具体步骤如下：

S1、获取一张图片；

S2、在图片中任意选择一个像素点p，假设它的亮度是I_p；

S3、设定一个阈值T；

S4、以像素点p为中心，分别选取半径为3和2的两个同心圆上的16和12个像素点；

S5、将选取的这些像素点的亮度分别于I_p+T或I_p-T相比较；

S6、如果在这两个同心圆上分别有M和N像素点的亮度大于I_p+T或小于I_p-T，则可以认为p是一个特征点；

S7、循环S2-S6，对每一个像素点进行上述操作；

S8、对选取的特征点进行非极大值抑制，并记录最终特征点的位置。

参见图2，步骤3中，根据得到的特征点的位置信息，利用SIFT算法计算特征点的主方向以及特征描述子，具体方法如下：

对于一个特征点，以它为中心，计算它邻域像素的梯度和方向，用直方图进行统计，得到特征点邻域像素的梯度分布，直方图中的峰值就是特征点的主方向；梯度和方向的计算公式如下：

θ(x,y)＝arctan((L(x,y+1)-L(x,y-1))/(L(x+1,y)-L(x-1,y)))  (2)

以关键点为中心选取16×16的邻域窗口，关键点所在的行和列不取，计算邻域窗口内所有像素点的梯度模值和梯度方向，然后将每个窗口分成4×4的子区域，总共16个子区域，每个子区域包含8个方向上的信息，一共有128个方向信息，生成128维的SIFT特征向量，即128维的特征描述子。

步骤4中，根据生成的特征描述子进行特征匹配，具体方法如下：

对于t-1时刻这一帧图像上的特征点集{P}中的每一个特征点p，从t时刻这一帧图像上的特征点集{P′}中找到与特征点p的特征向量最近邻的特征点和次近邻的特征点，并记录它们之间的距离；

若最近邻特征点的距离与次近邻特征点的距离比小于等于设定的阈值T，则认为特征点p与最近邻的特征点p′是一对匹配点，记为(p,p′)；

若最近邻特征点的距离与次近邻特征点的距离比大于设定的阈值T，则认为特征点p在特征点集{P′}中没有相匹配的特征点；

由以上可知，对每一个特征点都进行上述操作，并记录匹配上的特征点对，最后可以得到一个匹配点对的集合{(P_n,P′_n)}；

自适应RANSAC算法剔除误匹配点，具体方法如下：

S1、对于得到的粗匹配点对集合{(P_n,P′_n)}，计算集合中每对匹配点对之间的欧氏距离，并按从小到大的顺序进行排列；

S2、取排在前20％的匹配点对组成新的样本点对集合U；

S3、从U中随机选取6个匹配点对，利用其中4个计算变换模型H_i；

S4、利用剩余的2个对H_i进行验证，如果通过验证，则进行下一步；否则，舍弃H_i，重新返回Step3；

S5、对匹配点对集合{(P_n,P′_n)}根据变化模型H_i计算对称变换误差d，若d小于设定的值t，则认为是一个内点，并统计内点的数量M；

S6、重复Step3-Step5三步N次，若M大于设定的阈值S，则利用M重新估算变换模型H，即为最优的变换模型；

图3为自适应RANSAC算法剔除误匹配点后的结果。

步骤5中，根据匹配点集的误差构造目标函数，求解目标函数得到运动估计信息，具体方法如下：

根据步骤(4)可以得出一组匹配好的3D点：

P_m＝{p₁,L,p_m}，P′_m＝{p′₁,L,p′_m}  (3)

到一个欧式变换R和t，使得

这个问题可以用迭代最近点(ICP)来求解，这里采用的方式SVD为代表的方法进行求解，先定义第i对点的误差项：

e_i＝p_i-(Rp′_i+t)  (5)

然后构建最小二乘函数，求使误差平方和达到极小的R,t:

最后使用SVD方法求解出两帧图像之间的运动估计信息(R,t)，即T_k。

步骤6中，利用粒子滤波进行最优估计，减小误差，具体方法如下：

由步骤5得到的运动估计信息，通过计算C_k＝C_k-1T_k来连接转换，得到运动系统的状态方程和观测方程如式(7)，(8)所示：

x_k＝f(x_k-1,u_k,w_k)  (7)

z_k＝h(x_k,v_k)  (8)

其中，x_k为k时刻系统的状态变量，u_k为系统的控制变量，z_k为k时刻的观测变量，w_k和v_k分别为输入的白噪声和观测噪声。基于粒子滤波的视觉里程计算法是先利用k-1时刻的位置x_k-1和系统噪声v_k-1来预测k时刻的位置

然后在根据k时刻的观测值z_k来更新k时刻的位x_k置，就这样不停的循环迭代以得到车辆的最优位姿估计值。

以上应用了具体个例对本发明进行阐述，只是用于帮助理解本发明，并不用以限制本发明。对于本发明所属技术领域的技术人员，依据本发明的思想，还可以做出若干简单推演、变形或替换。

Claims (4)

1.基于双目视觉的实时高精度车辆里程计算方法，其特征在于，该方法包括以下步骤：

步骤1、采用视觉传感器获取图像序列；

步骤2、对获取的前后帧立体图像利用改进FAST角点检测算法提取特征点；

步骤3、利用SIFT算法计算特征点的主方向并生成特征描述子；

步骤4、根据生成的特征描述子进行特征匹配，并采用自适应RANSAC算法剔除误匹配点；

步骤5、根据匹配点集误差构造目标函数，求解目标函数得到运动估计信息；

步骤6、利用粒子滤波进行最优估计，减小误差；

步骤2中，使用改进的FAST角点检测算法提取特征点，具体步骤如下：

S1、获取一张图片；

S2、在图片中任意选择一个像素点p，假设它的亮度是I_p；

S3、设定一个阈值T；

S4、以像素点p为中心，分别选取半径为3和2的两个同心圆上的16和12个像素点；

S5、将选取的这些像素点的亮度分别与I_p+T或I_p-T相比较；

S6、如果在这两个同心圆上分别有M和N像素点的亮度大于I_p+T或小于I_p-T，则可以认为p是一个特征点；

S7、循环S2-S6，对每一个像素点进行上述操作；

S8、对选取的特征点进行非极大值抑制，并记录最终特征点的位置；

步骤4中，根据生成的特征描述子进行特征匹配，具体方法如下：

对于t-1时刻这一帧图像上的特征点集{P}中的每一个特征点p，从t时刻这一帧图像上的特征点集{P′}中找到与特征点p的特征向量最近邻的特征点和次近邻的特征点，并记录它们之间的距离；

若最近邻特征点的距离与次近邻特征点的距离比小于等于设定的阈值T，则认为特征点p与最近邻的特征点p′是一对匹配点，记为(p,p′)；

若最近邻特征点的距离与次近邻特征点的距离比大于设定的阈值T，则认为特征点p在特征点集{P′}中没有相匹配的特征点；

由以上可知，对每一个特征点都进行上述操作，并记录匹配上的特征点对，最后可以得到一个匹配点对的集合{(P_n,P′_n)}；

自适应RANSAC算法剔除误匹配点，具体方法如下：

S1、对于得到的粗匹配点对集合{(P_n,P′_n)}，计算集合中每对匹配点对之间的欧氏距离，并按从小到大的顺序进行排列；

S2、取排在前20％的匹配点对组成新的样本点对集合U；

S3、从U中随机选取6个匹配点对，利用其中4个计算变换模型H_i；

S4、利用剩余的2个对H_i进行验证，如果通过验证，则进行下一步；否则，舍弃H_i，重新返回S3；

S5、对匹配点对集合{(P_n,P′_n)}根据变换模型H_i计算对称变换误差d，若d小于设定的值t，则认为是一个内点，并统计内点的数量M；

S6、重复S3-S5三步N次，若M大于设定的阈值S，则利用M重新估算变换模型H，即为最优的变换模型。

2.根据权利要求1所述基于双目视觉的实时高精度车辆里程计算方法，其特征在于，步骤3中，根据得到的特征点的位置信息，利用SIFT算法计算特征点的主方向以及特征描述子，具体方法如下：

对于一个特征点，以它为中心，计算它邻域像素的梯度和方向，用直方图进行统计，得到特征点邻域像素的梯度分布，直方图中的峰值就是特征点的主方向；梯度和方向的计算公式如下：

θ(x,y)＝arctan((L(x,y+1)-L(x,y-1))/(L(x+1,y)-L(x-1,y)))  (2)

以关键点为中心选取16×16的邻域窗口，关键点所在的行和列不取，计算邻域窗口内所有像素点的梯度模值和梯度方向，然后将每个窗口分成4×4的子区域，总共16个子区域，每个子区域包含8个方向上的信息，一共有128个方向信息，生成128维的SIFT特征向量，即128维的特征描述子。

3.根据权利要求2所述基于双目视觉的实时高精度车辆里程计算方法，其特征在于，步骤5中，根据匹配点集的误差构造目标函数，求解目标函数得到运动估计信息，具体方法如下：

根据步骤4可以得出一组匹配好的3D点：

P_m＝{p₁,L,p_m}，P′_m＝{p′₁,L,p′_m}  (3)

得到一个欧式变换R和t，使得

这个问题可以用迭代最近点(ICP)来求解，这里采用的方式SVD为代表的方法进行求解，先定义第i对点的误差项：

e_i＝p_i-(Rp′_i+t)  (5)

然后构建最小二乘函数，求使误差平方和达到极小的R,t:

最后使用SVD方法求解出两帧图像之间的运动估计信息(R,t)，即T_k。

4.根据权利要求3所述基于双目视觉的实时高精度车辆里程计算方法，其特征在于，步骤6中，利用粒子滤波进行最优估计，减小误差，具体方法如下：

由步骤5得到的运动估计信息，通过计算C_k＝C_k-1T_k来连接转换，得到运动系统的状态方程和观测方程如式(7)，(8)所示：

x_k＝f(x_k-1,u_k,w_k)  (7)

z_k＝h(x_k,v_k)  (8)

其中，x_k为k时刻系统的状态变量，u_k为系统的控制变量，z_k为k时刻的观测变量，w_k和v_k分别为输入的白噪声和观测噪声；基于粒子滤波的视觉里程计算法是先利用k-1时刻的位置x_k-1和系统噪声v_k-1来预测k时刻的位置然后在根据k时刻的观测值z_k来更新k时刻的位置x_k，就这样不停的循环迭代以得到车辆的最优位姿估计值。

Images