本发明属于视觉导航技术领域，具体涉及一种基于多景深信息的视觉里程计测速方法。本发明的基于多景深信息的视觉里程计测速方法，包括以下步骤：1、根据任务特征选择合适的双目相机；2、标定MEMS惯导/视觉里程计安装误差；3、基于灰度方差信息提取图像特征点；4、使用双目相机实现特征点测距；5、对前后帧图像中特征点进行识别实现测速；6、运用小波分析方法消除测速噪声。本发明解决了现有的视觉里程计测速方法精度较低，难用于需要高精度长航时导航的智能化交通工具中的技术问题。本发明的方法在视觉里程计测速计算中加入MEMS惯导辅助速度信息可显著提高测速精度，降低了计算的复杂度，适用于长航时导航。
1.一种基于多景深信息的视觉里程计测速方法，其特征在于：包括以下步骤：步骤1、选择相机：根据任务特征选择合适的双目相机；
步骤2、误差标定：标定MEMS惯导系统/视觉里程计安装误差；
步骤3、特征点集提取：基于灰度方差信息提取图像特征点；
步骤4、特征点集测距：使用双目相机实现特征点测距；
步骤5、速度计算：对前后帧图像中特征点进行识别实现测速；
步骤1中所述根据任务特征选择合适的双目相机的具体方法为：根据运动速度、高度、测量精度、计算机运算速度以及其他条件选择双目相机的分辨率、视场角及其它相机参数；
步骤2中所述标定MEMS惯导系统/视觉里程计安装误差的具体操作为：利用卡尔曼滤波，根据不同传感器的输出特性的差异性，实现对MEMS惯导系统和视觉里程计之间安装误差角和安装杆臂的估计与补偿，实现传感器输出导航信息的统一化；
步骤3中所述基于灰度方差信息提取图像特征点的具体操作为：在t1时刻双目相机中A相机采集的图像中，将图像分为k个子区域，设每个子区域内有n个点集Q1，Q2，…Qn，任一点集的灰度方差为Gi ， i＝1,2,…,n，而点集Qi， i＝1,2,…,n中包含m个点qi， j＝1,2,…,m，对应的灰度值为gj ， j＝1,2,…,m，那么点集Qi的灰度方差Gi为其中， 为点集Qi中m个点的灰度均值；
选取子区域内灰度方差最大的点集中心点作为图像特征点，则在t1时刻采集图像中存在k个特征点集Pl， l＝1,2,…,k；
步骤4中所述使用双目相机实现特征点测距的具体方法为：在t1时刻双目相机中B相机采集的图像中识别A相机中的特征点集，基于空间上存在偏移的双目图像，完成特征点集所代表的实际物体到相机的距离测量；设Q是待测距的实际物体，其到相机的垂直距离为R，在A、B相机上形成的像点集分别是QA和QB，b是双目相机的基线长，f是焦距，x1-x2为视差，其中x1为QA中心点的水平像素值，x2为QB中心点的水平像素值；
利用相似三角形性质，可得Q到相机的垂直距离R，即QA的景深为：
由上式，能够计算出每个特征点集Pl的景深Rl ， l＝1,2,…,k；
步骤5中所述对前后帧图像中特征点集进行识别实现测速的具体方法为：在t2时刻双目相机中A相机采集的图像中，利用MEMS惯导系统提供的速度信息，设定t1时刻双目相机中A相机特征点集Pl在t2时刻的搜索空间，利用图像匹配算法，计算特征点集Pl在t1和t2时刻图像中的像素差，并结合特征点集景深Rl获得特征点集的运动距离，最后根据运动距离和时间差，计算k个不同景深特征点集的实时速度vl ， l＝1,2,…,k；
步骤6中所述运用小波分析方法消除测速噪声的具体方法为：
将vl设定为原始一维信号，将一维信号做二级小波变化，设定二级小波系数w2＝[wa2 wd2 wd1]式中，wa2为二级近似系数，wd2为二级细节系数，wd1为一级细节系数；对原始一维信号进行小波正变换，获得二级近似分解a2、二级细节分解d2和一级细节分解d1；
设定两个阈值λ1和λ2，认为一级细节分解d1中小于λ1的部分为一级噪声，而二级细节分解d2中小于λ2的部分为二级噪声，噪声去除之后获得一级小波细节d1`和二级小波细节d2`，使用下式对速度信号进行恢复：式中， 为去除噪声的速度信号，即视觉里程计的测速输出。
一种基于多景深信息的视觉里程计测速方法
技术领域
[0001] 本发明属于视觉导航技术领域，具体涉及一种基于多景深信息的视觉里程计测速方法。
背景技术
[0002] 目前，车辆、自主机器人等智能化地面交通工具对导航需求越来越高，而在城市、山谷等区域，卫星定位精度较差，甚至定位失效，因此在卫星定位不可用的环境下，自主导航系统精度是需要重点考虑的问题之一。视觉导航系统作为新兴的自主导航系统在近年来成为各国学者的研究热点。视觉里程计作为视觉导航中的重要组成部分，已应用于实际工程产品中。但是由于受图像匹配算法复杂度与特征点误匹配的存在，视觉里程计的精度仍然较低，难用于需要高精度长航时导航的智能化交通工具中。
[0003] 为解决视觉里程计面临的两大难题：图像匹配算法计算复杂度与特征点误匹配。可考虑从以下两方面解决途径：第一，目前常用的图像匹配方法包括SIFT与SURF等复杂匹配算法，虽然算法的匹配度较高，但是由于为了保障匹配算法的强鲁棒性，因此牺牲了算法的实时性，于是此类算法复杂度较高，往往只能应用于对实时性要求不高的使用环境。因此为了降低图像匹配复杂度，可通过引入外界参考信息，缩小图像匹配的搜索空间，以实现缩短图像匹配算法计算时间的目的。而MEMS惯导可提供速度与航姿信息，可作为外界辅助导航信息源；第二，对于二维平移运动，利用单一特征点匹配实现对速度的测量往往会由于误匹配导致速度计算误差较大，因此，考虑引入不同景深的特征点分别进行图像匹配与运动估计。通过综合分析各特征点的运动估计结果，利用小波分析方法，剔除运动估计野值特征点，降低测速噪声，实现对视觉里程计测速精度的提高。
发明内容
[0004] 本发明需要解决的技术问题为：现有的视觉里程计测速方法由于受到图像匹配算法复杂度与特征点误匹配的存在，因此精度较低，难用于需要高精度长航时导航的智能化交通工具中。
[0005] 本发明的基于多景深信息的视觉里程计测速方法，包括以下步骤：
[0006] 步骤1、选择相机：根据任务特征选择合适的双目相机；
[0007] 步骤2、误差标定：标定MEMS惯导系统/视觉里程计安装误差；
[0008] 步骤3、特征点集提取：基于灰度方差信息提取图像特征点；
[0009] 步骤4、特征点集测距：使用双目相机实现特征点测距；
[0010] 步骤5、速度计算：对前后帧图像中特征点进行识别实现测速；
[0011] 步骤6、噪声消除：运用小波分析方法消除测速噪声。
[0012] 优选的，步骤1中所述根据任务特征选择合适的双目相机的具体方法为：根据运动速度、高度、测量精度、计算机运算速度以及其他条件选择双目相机的分辨率、视场角及其它相机参数。
[0013] 优选的，步骤2中所述标定MEMS惯导系统/视觉里程计安装误差的具体操作为：利用卡尔曼滤波，根据不同传感器的输出特性的差异性，实现对MEMS惯导系统和视觉里程计之间安装误差角和安装杆臂的估计与补偿，实现传感器输出导航信息的统一化。
[0014] 优选的，步骤3中所述基于灰度方差信息提取图像特征点的具体操作为：在t1时刻双目相机中A相机采集的图像中，将图像分为k个子区域。设每个子区域内有n个点集Q1，Q2，…Qn，任一点集的灰度方差为Gi(i＝1,2,…,n)，而点集Qi(i＝1,2,…,n)中包含m个点qi(j＝1,2,…,m)，对应的灰度值为gj(j＝1,2,…,m)，那么点集Qi的灰度方差Gi为[0015]
[0016] 其中， 为点集Qi中m个点的灰度均值；
[0017] 选取子区域内灰度方差最大的点集中心点作为图像特征点，则在t1时刻采集图像中存在k个特征点集Pl(l＝1,2,…,k)。
[0018] 优选的，步骤4中所述使用双目相机实现特征点测距的具体方法为：在t1时刻双目相机中B相机采集的图像中识别A相机中的特征点集，基于空间上存在偏移的双目图像，完成特征点集所代表的实际物体到相机的距离测量；设Q是待测距的实际物体，其到相机的垂直距离为R，在A、B相机上形成的像点集分别是QA和QB，b是双目相机的基线长，f是焦距，(x1-x2)为视差，其中x1为QA中心点的水平像素值，x2为QB中心点的水平像素值；
[0019] 利用相似三角形性质，可得Q到相机的垂直距离R，即QA的景深为：
[0020]
[0021] 由上式，能够计算出每个特征点集Pl的景深Rl(l＝1,2,…,k)。
[0022] 优选的，步骤5中所述对前后帧图像中特征点集进行识别实现测速的具体方法为：在t2时刻双目相机中A相机采集的图像中，利用MEMS惯导系统提供的速度信息，设定t1时刻双目相机中A相机特征点集Pl在t2时刻的搜索空间，利用图像匹配算法，计算特征点集Pl在t1和t2时刻图像中的像素差，并结合特征点集景深Rl获得特征点集的运动距离，最后根据运动距离和时间差，计算k个不同景深特征点集的实时速度vl(l＝1,2,…,k)。
[0023] 优选的，步骤6中所述运用小波分析方法消除测速噪声的具体方法为：
[0024] 将vl设定为原始一维信号，将一维信号做二级小波变化，设定二级小波系数[0025] w2＝[wa2 wd2 wd1]
[0026] 式中，wa2为二级近似系数，wd2为二级细节系数，wd1为一级细节系数；对原始一维信号进行小波正变换，获得二级近似分解a2、二级细节分解d2和一级细节分解d1；
[0027] 设定两个阈值λ1和λ2，认为一级细节分解d1中小于λ1的部分为一级噪声，而二级细节分解d2中小于λ2的部分为二级噪声，噪声去除之后获得一级小波细节d1`和二级小波细节d2`，使用下式对速度信号进行恢复：
[0028]
[0029] 式中， 为去除噪声的速度信号，即视觉里程计的测速输出。
[0030] 本发明的有益效果为：
[0031] (1)本发明的方法在视觉里程计测速计算中加入MEMS惯导辅助速度信息可显著提高测速精度，降低了计算的复杂度，适用于长航时导航；
[0032] (2)采用多景深特征点来测量速度，并加入小波分析方法来剔除计算噪声，可增强本方法的鲁棒性，进一步提高导航测速精度。
附图说明
[0033] 图1为双目测距原理示意图；
[0034] 图2为视觉里程计自定位原理示意图；
[0035] 图3视觉里程计与GPS测速对比图。
具体实施方式
[0036] 本发明的方法，首先利用双目相机计算不同景深的特征点与相机的距离；然后，引入外界参考信息，即MEMS惯导的姿态与速度信息，缩小图像匹配的搜索空间，以实现缩短图像匹配算法计算时间的目的；之后，在完成前后帧图像中各特征点图像匹配后，分别估计各特征点的运动距离；最后，利用小波分析方法，剔除运动距离计算野值，减小运动估计噪声，实现对视觉里程计精度的提高。
[0037] 1、双目测量景深
[0038] 双目测量景深方法包括两部分内容：特征点提取与特征点测距。
[0039] 特征点的定义很多，包括SIFT特征点、Harris特征点等。本实施例从工程应用角度出发，利用灰度图像的特点，选取区域内灰度变化最大的像素点集的中心点作为特征点。具体算法如下：设区域内有n个点集Q1，Q2，…Qn，任一点集的灰度方差为Gi(i＝1,2,…,n)，而点集Qi(i＝1,2,…,n)中包含m个点qi(j＝1,2,…,m)，对应的灰度值为gj(j＝1,2,…,m)，那么点集Qi的灰度方差Gi为
[0040]
[0041] 特征点测距是基于空间上存在偏移的双目图像，完成其中匹配物体(左、右图像中均存在)的距离测量，测距原理见图1。
[0042] 图中，Q是待测距的特征点，其到相机的垂直距离为R，在左右相机上形成的像点分别是Q1和Q2。b是双目相机的基线长，f是焦距，(x1-x2)是Q点在左右两幅图像上像点的位置差，又称为视差。
[0043] 利用相似三角形性质，可得Q点到相机的垂直距离R，即Q点的景深为：
[0044]
[0045] 利用上述方法的测距精度可达1％。
[0046] 2、直线运动估计
[0047] 立体视觉里程计的基本原理：载体t1时刻位于点O1，t2时刻运动到O2。在全局世界坐标系中，载体位置和姿态的变化可以由六个参数(Δx,Δy,Δz,Δα,Δβ,Δγ)表示，前三个参数代表位置的变化，后三个参数代表姿态的变化。如果载体在移动过程中每一帧都能确定其与前一帧时的位置和姿态，那么就自然实现了载体的定位。对全局坐标系中一点A，载体在t1时刻观察到的A的坐标为(x1,y1,z1)T，t2时刻观察到的坐标为(x2,y2,z2)T，假如载体姿态发生了变化，也就是代表姿态的航向角、滚转角和俯仰角发生了变化。此三个角度的变化(Δα,Δβ,Δγ)T产生了3×3旋转矩阵R，位置变化(Δx,Δy,Δz)T产生了平移向量T，则有：
[0048]
[0049] 其中，
[0050] 如果能在相邻两帧中实现像素点的跟踪，即对前一帧图像中的像素点，在下一帧图像中能找到它的对应点，并利用立体视觉分别计算出此像素点在前后两帧中的三维坐标，则根据式(1)，一对对应的特征点就能产生三个方程。这样，只要存在4个对应的特征点，就能计算出旋转矩阵R和平移矩阵T。如果特征点的数目大于4个，则需要通过多个方程求得(R,T)的最优值。
[0051] 立体视觉里程计的计算复杂，尤其是求解旋转矩阵R的难度较大，另外运载交通工具的运动大部分为直线运动，且部分旋转运动由于旋转角度较小，可近似拟合为多时刻直线运动。因此本方法着重考虑的是提高直线运动的估计精度，将式(1)中的旋转矩阵可简化为单位阵，即
[0052]
[0053] 那么速度计算与特征点在图像中的初始位置无关，只与前后帧图像中特征点位置变化，如图2所示。当特征点为静止时，利用双目相机测得的景深信息与相对运动原理，可将特征点在图像中像素点的运动估计结果解算出视觉采集设备所在运动载体的运动速度。
[0054] 3、MEMS惯导辅助图像匹配
[0055] 图像匹配算法采用归一化积相关算法，其本质是一种两幅图像间的相似程度度量准则。匹配算法的一般过程是，模板图像在基准图像中从起始位置逐行扫过整个基准图；在每一个位置将模板图像中每个像素点与它所对应基准图区域像素点进行相关性运算，将结果记录在相应的位置处；选取相关性最高的匹配峰值点，作为匹配像素点。
[0056] 相关性运算的计算公式为
[0057]
[0058] 式中，(x,y)为模板图像中的坐标点；(u,v)为基准图像中的坐标点；T(x,y)为模板图像在点(x,y)处的灰度值； 为模板图像在基准图像(u,v)处的均值；I(x+u,y+v)为基准图像点(u,v)处与模板图像相对应位置处的灰度值； 为基准图像(u,v)处与模板图像相同区域的均值。
[0059] 由上式可见，图像匹配算法的计算复杂度是与搜索基准图像空间大小是相关的。为了降低算法的计算复杂度，引入MEMS惯导的姿态与速度信息，利用相邻图像采集周期内的移动距离参考信息，以及特征点的景深信息，可估计出匹配特征点在匹配图像中可能存在的区域。从而一方面降低了算法难度，另外也提高了匹配准确度。
[0060] 4、多景深误匹配点剔除技术
[0061] 利用k个不同景深的特征点计算出k个速度vl(l＝1,2,…,k)。但是由于匹配精度限制与图像匹配中误匹配的存在，部分速度vl计算并不准确，相当于速度计算噪声，因此需要剔除误匹配点计算速度的影响。本发明采用小波分析的方法来剔除速度噪声影响。将k个特征点对应计算速度vl(l＝1,2,…,k)设定为原始一维信号，将一维信号做二级小波变化，即设二级小波系数为
[0062] w2＝[wa2 wd2 wd1]
[0063] 式中，wa2为二级近似系数；wd2为二级细节系数；wd1为一级细节系数。利用原始一维信号可计算二级小波系数，并对一维信号分别做二级近似分解、二级细节分解和一级细节分解获得a2、d2和d1。
[0064] 去除误匹配点计算速度噪声相当于是去除一维信号中的高频信号的过程。对于二级小波而言，设定2个阈值λ1和λ2，认为一级小波细节分解d1中小于λ1的部分为一级噪声，而二级小波细节分解d2中小于λ2的部分为二级噪声。将噪声去除之后获得一级小波细节d1`和二级小波细节d2`，恢复信号可获得去除噪声的信号vl(l＝1,2,…,k)，即
[0065]
[0066] 图3为某条次利用本方法测量速度与GPS速度对比结果。
