CN103325108A - 一种融合光流与特征点匹配的单目视觉里程计的设计方法 - Google Patents

Abstract

本发明公开了一种融合光流与特征点匹配的单目视觉里程计的设计方法。准确的实时定位对于自主导航系统至关重要。基于SURF特征点匹配的定位具有对光照变化鲁棒、定位精度高的特点，但缺点是处理速度慢，无法做到实时定位。光流跟踪法具有较好实时性，缺点在于定位精度略差。本发明结合两者的优点，设计了一种融合光流与特征点匹配的单目视觉里程计。实验结果表明，融合后的算法能够提供准确的实时定位输出，并对光照变化和路面纹理较少的情况具有鲁棒性。

Description

一种融合光流与特征点匹配的单目视觉里程计的设计方法

技术领域

本发明涉及计算机视觉技术，特别是涉及一种融合光流与特征点匹配的单目视觉里程计的设计方法。

背景技术

精确定位对于自主导航系统是至关重要的。多种传感器被用于自主导航系统以提供准确的位置信息，比如GPS/差分GPS，惯性导航系统(IMUs)和轮式里程计等。但是，这些传感器各自都有应用场景的限制。近年来，基于视觉方法设计的里程计逐渐成为自主定位的重要选择。相比于传统的定位方法，视觉里程计在没有卫星的情况下也能使用，不会受到轮子侧滑的影响，在车体没有运动的情况下，不会出现输出结果上的漂移。

视觉里程计分为双目立体视觉里程计和单目视觉里程计。双目立体视觉里程计由于精度较高，被广泛应用于机器人自主定位中，Nister在2004年提出视觉里程计，采用Harris算子提取特征点，通过NCC算法匹配，进行了长距离实验，相对误差在1%～5%。Howard采用Harris和FAST特征点，能保证一定的实时性，但是由于特征点的性能较弱，存在一定几率的匹配失败。近些年来，多种立体视觉里程计是基于SIFT算子提取特征点的，精度很高，但是对于硬件要求高，处理速度较低，在要求实时性的系统中不适用。Bernd Kitt应用单目视觉里程计解决尺度问题，实现精确定位，但是处理时间长，没有经过大尺度实验进行验证。在城市化道路的环境中，大部分的路面满足平面假设的要求，所以在这种场景中，采用单目视觉里程计是合理且有效的，同时由于单目视觉里程计对于硬件要求低，处理速度快，能满足实时性的要求。经过将近30年的发展，光流法对基于图像的运动估计已经满足较低程度上精度和稳定性的要求，并且由于计算速度快，基于光流法的视觉里程计已经达到实际应用的速度要求。为解决其精度不高的问题，光流法需要与传统的特征点匹配方法进行融合，以提高精度，同时满足实时性的要求。

发明内容

本发明的目的在于提供一种融合光流与特征点匹配的单目视觉里程计的设计方法，将光流法和特征点匹配方法进行融合，该方法得到的结果相比于传统的单目视觉里程计系统硬件要求低，计算时间短，鲁棒性更好。

本发明采用的技术方案的步骤如下：

包括如下步骤：

1）对于系统输入的连续两帧图像I_k和I_k+1，首先通过采样构建图像金字塔：金字塔的底层图像为原始图像，对底层图像利用线性插值的方法进行降采样，得到长和宽减半的上一层图像，依次类推，向上采样，构建4层金字塔；

2）对于前一帧图像中的像素点进行金字塔跟踪：从最顶层开始跟踪像素点u，计算光流d＝[d_x,d_y]和变换矩阵A，使得通过d和A变换后的前一帧图像与后一帧在这层上的灰度差异最小；然后再把这一层的结果传递给下一层，重新计算光流d和变换矩阵A，再传递给下一层；不断循环，直到底层，也就是原始图像，计算光流d和变换矩阵A，前后两帧图像经过变换后的灰度差小于阈值δ₁或循环次数大于δ₂时，循环结束，输出结果；

3）通过相机标定，将I_n和I_n+1中的二维像素点反投影到三维空间内得到空间坐标：在地面上选取四个点，同时记录其在车体坐标系下的坐标和在图像坐标系下对应点的坐标，应用homography基本原理标定，将匹配的二维像素点对投影到空间中的三维点对，从而得到车体的旋转和平移；

4）对于图像重叠区域30%～90%的两帧图像I_k和I_k+n利用SURF特征点匹配算法得到两帧图像间的运动；

5）当车体运动，系统采集了多帧图像之后，首先针对连续图像进行光流计算，得到光流运动，同时对于这一段距离的头尾两帧图像进行SURF特征点匹配，将光流运动的累积结果和SURF特征点的匹配结果进行卡尔曼滤波，更新车体位置，输出结果。

所述步骤4)中，对于图像I_k和I_k+n利用SURF特征点匹配算法得到两帧图像间运动，包括如下步骤：

1）SURF算子选取图像在尺度空间上的极值点作为候选特征点，图像I(x,y)在尺度空间σ上的Hessian矩阵表示为：

$H=\left[\begin{array}{cc}{L}_{\mathrm{xx}}(x,\mathrm{\σ})& L_{\mathrm{xy}}(x,\mathrm{\σ})\\ L_{\mathrm{xy}}(x,\mathrm{\σ})& L_{\mathrm{yy}}(x,\mathrm{\σ})\end{array}\right]---\left(1\right)$

其中L_xx，L_xy，L_yy是对应方向高斯二阶偏导数和图像的卷积，高斯卷积函数为： $G\left(\mathrm{\σ}\right)=\frac{1}{{2\mathrm{\π\σ}}^2}{e}^{\frac{(x^2+y^2)}{{2\mathrm{\σ}}^2}}$

图像在不同尺度空间上与高斯函数进行卷积，从而构建图像金字塔，同时利用Hessian矩阵求出极值，在一个3*3*3的立体邻域内进行非极大值抑制，任何一个候选点都要与上一个尺度、当前尺度和下一个尺度的26个邻域值比较，只有比周围的都大或者都小，才能作为稳定的SURF特征点；

2）特征点匹配过程以特征向量的欧氏距离作为匹配基准：对于前一帧图像中的特征点p，在后一帧中计算距离特征点p最近点p₁和次近的两个特征点并且记录最近距离d₁和次近距离d₂，若d₁＜d₂*δ₃，δ₃∈(0.6,0.9)，认为p₁是p的匹配点。

所述步骤5)中，当车体运动，系统采集了多帧图像之后，首先针对连续图像进行光流计算，得到光流运动，同时对于这一段距离的头尾两帧图像进行SURF特征点匹配，将光流运动的累积结果和SURF特征点的匹配结果进行卡尔曼滤波，更新车体位置，输出结果，包括如下步骤：

1）两帧图像之间用光流法得到旋转R和位移T，并且不断累积，在n帧后，对于I_k帧图像和I_k+n帧图像用SURF特征点匹配得到旋转R和位移T，对于光流法的结果和特征点匹配得到结果进行卡尔曼滤波，得到更加准确的结果作为下一次计算的初始值，不断循环；

根据离散卡尔曼滤波器时间更新方程：

${\hat{x}}_{\stackrel{\‾}{k}}=A{\hat{x}}_{k-1}+{\mathrm{Bu}}_{k-1}---\left(2\right)$

$P_{\stackrel{\‾}{k}}={\mathrm{AP}}_{k-1}{A}^T+Q---\left(3\right)$

其中，

是光流法计算出来的前k-1帧累积的机器人位置坐标以及航向角，u_k-1是位置控制函数，这里设置为u_k-1=[Δx,Δy,Δθ]^T，B是控制增益，设为常数，A是一个常数，通过公式(2)，计算出当前状态

P_k-1是前一帧的协方差估计，Q是过程激励噪声协方差矩阵，这里设为常数，根据离散卡尔曼滤波器时间更新方程，计算当前协方差估计

观测方程为：

z_k=Hx_k+v_k    (4)

其中z_k是通过SURF特征点匹配法计算出来的机器人的位置，H是单位矩阵，v_k表示观测噪声，为正态分布白噪声：p(v)～N(0,R)其中，R是观测噪声协方差矩阵，这里假设是常数，计算卡尔曼增益K_k：

$K_k=P_{\stackrel{\‾}{k}}{H}^T{({\mathrm{HP}}_{\stackrel{\‾}{k}}{H}^T+R)}^{-1}---\left(5\right)$

根据离散卡尔曼滤波器状态更新方程，更新整个系统位置：

${\hat{x}}_k={\hat{x}}_{\stackrel{\‾}{k}}+K_k(z_k-H{\hat{x}}_{\stackrel{\‾}{k}})---\left(6\right)$

$P_k=\left({I-K}_{k}H\right)P_{\stackrel{\‾}{k}}---\left(7\right).$

与背景技术相比，本发明具有的有益效果是：

1）因为本发明使用了快速光流法，所以在处理时间上有优势，能实现实时定位。

2）因为本发明融合了特征点匹配法，所以在结果精度上比传统光流精确。

3）因为本发明使用的SURF特征点具有亮度不变性，所以在光照出现变化时能正常应用。

4）因为本发明使用了光流法，所以在地面纹理不清的情况下也能实现定位。

附图说明

图1是本发明的流程图。

图2是实验结果轨迹对比图。

图3是特征点匹配和光流两种方法处理时间对比图。

图4是特征点匹配法、光流法和两者融合之后的误差对比图。

具体实施方式

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

如图1所示，融合光流与特征点匹配的单目视觉里程计的设计方法包括如下步骤：

1）对于系统输入的连续两帧图像I_k和I_k+1，首先通过采样构建图像金字塔：金字塔的底层图像为原始图像，对底层图像利用线性插值的方法进行降采样，得到长和宽减半的上一层图像，依次类推，向上采样，构建4层金字塔；

2）对于前一帧图像中的像素点进行金字塔跟踪：从最顶层开始跟踪像素点u，计算光流d＝[d_x,d_y]和变换矩阵A，使得通过d和A变换后的前一帧图像与后一帧在这层上的灰度差异最小；然后再把这一层的结果传递给下一层，重新计算光流d和变换矩阵A，再传递给下一层；不断循环，直到底层，也就是原始图像，计算光流d和变换矩阵A，前后两帧图像经过变换后的灰度差小于阈值δ₁或循环次数大于δ₂时，循环结束，输出结果；

3）通过相机标定，将I_n和I_n+1中的二维像素点反投影到三维空间内得到空间坐标：在地面上选取四个点，同时记录其在车体坐标系下的坐标和在图像坐标系下对应点的坐标，应用homography基本原理标定，将匹配的二维像素点对投影到空间中的三维点对，从而得到车体的旋转和平移；

4）对于有一定间隔（图像重叠区域30%～90%）的两帧图像I_k和I_k+n利用SURF特征点匹配算法得到两帧图像间的运动；

5）当车体运动，系统采集了多帧图像之后，首先针对连续图像进行光流计算，得到光流运动，同时对于这一段距离的头尾两帧图像进行SURF特征点匹配，将光流运动的累积结果和SURF特征点的匹配结果进行卡尔曼滤波，更新车体位置，输出结果。

以上所述步骤4）中的对于图像I_k和I_k+n利用SURF特征点匹配算法得到两帧图像间运动，其中SURF算子选取图像在尺度空间上的极值点作为候选特征点，图像I(x,y)在尺度空间σ上的Hessian矩阵表示为：

$H=\left[\begin{array}{cc}{L}_{\mathrm{xx}}(x,\mathrm{\σ})& L_{\mathrm{xy}}(x,\mathrm{\σ})\\ L_{\mathrm{xy}}(x,\mathrm{\σ})& L_{\mathrm{yy}}(x,\mathrm{\σ})\end{array}\right]---\left(1\right)$

其中L_xx，L_xy，L_yy是对应方向高斯二阶偏导数和图像的卷积，高斯卷积函数为： $G\left(\mathrm{\σ}\right)=\frac{1}{{2\mathrm{\π\σ}}^2}{e}^{\frac{(x^2+y^2)}{{2\mathrm{\σ}}^2}}$

图像在不同尺度空间上与高斯函数进行卷积，从而构建图像金字塔，同时利用Hessian矩阵求出极值，在一个3*3*3的立体邻域内进行非极大值抑制，任何一个候选点都要与上一个尺度、当前尺度和下一个尺度的26个邻域值比较，只有比周围的都大或者都小，才能作为稳定的SURF特征点；

特征点匹配过程以特征向量的欧氏距离作为匹配基准：对于前一帧图像中的特征点p，在后一帧中计算距离特征点p最近点p₁和次近的两个特征点并且记录最近距离d₁和次近距离d₂，若d₁＜d₂*δ₃，δ₃∈(0.6,0.9)，认为p₁是p的匹配点。

以上所述步骤5)中，当车体运动，系统采集了多帧图像之后，首先针对连续图像进行光流计算，得到光流运动，同时对于这一段距离的头尾两帧图像进行SURF特征点匹配，将光流运动的累积结果和SURF特征点的匹配结果进行卡尔曼滤波，更新车体位置，输出结果。

两帧图像之间用光流法得到旋转R和位移T，并且不断累积，在n帧后，对于I_k帧图像和I_k+n帧图像用SURF特征点匹配得到旋转R和位移T，对于光流法的结果和特征点匹配得到结果进行卡尔曼滤波，得到更加准确的结果作为下一次计算的初始值，不断循环。

根据离散卡尔曼滤波器时间更新方程：

${\hat{x}}_{\stackrel{\‾}{k}}=A{\hat{x}}_{k-1}+B{u}_{k-1}---\left(2\right)$

$P_{\stackrel{\‾}{k}}=A{P}_{k-1}{A}^T+Q---\left(3\right)$

其中，

是光流法计算出来的前k-1帧累积的机器人位置坐标以及航向角，u_k-1是位置控制函数，这里设置为u_k-1=[Δx,Δy,Δθ]^T，B是控制增益，设为常数，A是一个常数，通过公式(2)，计算出当前状态

P_k-1是前一帧的协方差估计，Q是过程激励噪声协方差矩阵，这里设为常数，根据离散卡尔曼滤波器时间更新方程，计算当前协方差估计

观测方程为：

z_k=Hx_k+v_k    (4)

其中z_k是通过SURF特征点匹配法计算出来的机器人的位置，H是单位矩阵，v_k表示观测噪声，为正态分布白噪声：p(v)～N(0,R)

其中，R是观测噪声协方差矩阵，这里假设是常数，计算卡尔曼增益K_k：

$K_k=P_{\stackrel{\‾}{k}}{H}^T{(H{P}_{\stackrel{\‾}{k}}{H}^T+R)}^{-1}---\left(5\right)$

根据离散卡尔曼滤波器状态更新方程，更新整个系统位置：

${\hat{x}}_k={\hat{x}}_{\stackrel{\‾}{k}}+K_k(z_k-H{\hat{x}}_{\stackrel{\‾}{k}})---\left(6\right)$

$P_k=\left({I-K}_{k}H\right)P_{\stackrel{\‾}{k}}---\left(7\right)$

实施例：

本发明是需要实时采集图像，实时计算定位的，所以要想发挥本发明运算速度快的优点，采集图像场景必须近似是一个平面。本发明采集图像是一个循环过程，对于连续两帧图像使用光流法估计出车体运动，每n帧图像应用SURF特征点匹配结果进行矫正。所以本实施例就以n＝20帧图像为例，来阐述本发明的整个流程。

步骤1）对于系统输入的连续两帧图像I_k和I_k+1，首先通过采样构建图像金字塔：金字塔的底层图像为原始图像，对底层图像利用线性插值的方法进行降采样，得到长和宽减半的上一层图像，依次类推，向上采样，构建4层金字塔；

对于一幅大小为n_x×n_y的图像，金字塔的第0层就是图像本身，即I⁰＝I。图像的大小保持原有的分辨率。金字塔的构建是一个重复的过程：从I⁰中计算I¹，从I¹中计算I²，以此类推。定义L＝1,2,...是金字塔的层数，I^L-1代表第L-1层的图像，并且定义

和

是第L-1层图像的宽和高。这样第L层图像由第L-1层图像中该点周围领域像素差值产生：

$I^L(x,y)=$

$\frac{1}{4}{I}^{L-1}(2x,2y)+$

$\frac{1}{8}(I^{L-1}(2x-\mathrm{1,2}y)+I^{L-1}(2x+\mathrm{1,2}y)+I^{L-1}(2x,2y+1))+$

$\frac{1}{16}(I^{L-1}(2x-\mathrm{1,2}y-1)+I^{L-1}(2x+\mathrm{1,2}y+1)+I^{L-1}(2x,\mathrm{1,2}y+1)+I^{L-1}(2x+\mathrm{1,2}y+1))$

对于第L层的宽

和高

定义为满足如下条件的最大整数：

$n_x^L\≤\frac{n_x^{L-1}+1}{2}$

$n_y^L\≤\frac{n_y^{L-1}+1}{2}$

在本实施例中L取3。

步骤2）对于前一帧图像中的像素点进行金字塔跟踪：从最顶层开始跟踪像素点u，计算光流d＝[d_x,d_y]和变换矩阵A，使得通过d和A变换后的前一帧图像与后一帧在这层上的灰度差异最小；然后再把这一层的结果传递给下一层，重新计算光流d和变换矩阵A，再传递给下一层；不断循环，直到底层，也就是原始图像，计算光流d和变换矩阵A，前后两帧图像经过变换后的灰度差小于阈值δ₁或循环次数大于δ₂时，循环结束，输出结果；

在金字塔的每一层，目标是计算出光流d^L和仿射变换矩阵A^L从而使误差ε^L最小。每一层的迭代过程是相同的：首先将上一层的光流u和A传给这一层，计算这一帧图像中像素点的光照I(x)←I^L(x+u/2^L)，同时计算出图像在该点x方向和y方向上的偏导

$I_x\left(x\right)\←\frac{I(x+1,y)-I(x-1,y)}{2}$ $I_y\left(x\right)\←\frac{I(x,y+1)-I(x,y-1)}{2}$

在此基础上，计算出空间梯度矩阵：

更新光流v←2v

迭代过程：计算后一帧图像中对应像素点的灰度J(x)←J^L(Ax+v)，计算两帧图像间相同位置点的灰度值之差，δI(x)←I(x)-J(x)；在计算图像之间的误差向量，

$\stackrel{\‾}{b}\←\underset{x=-w_x}{\overset{w_x}{\mathrm{\Σ}}}\underset{y=-w_y}{\overset{w_y}{\mathrm{\Σ}}}\left[\begin{array}{c}{I}_x\mathrm{\δI}\\ .\\ .\\ .\\ {\mathrm{yI}}_y\mathrm{\δI}\end{array}\right]$

最后计算针对仿射Lucas-Kanade光流 ${\left[\begin{array}{cccccc}{\mathrm{\η}}_x& {\mathrm{\η}}_y& {\mathrm{\η}}_{\mathrm{xx}}& {\mathrm{\η}}_{\mathrm{xy}}& {\mathrm{\η}}_{\mathrm{yx}}& {\mathrm{\η}}_{\mathrm{yy}}\end{array}\right]}^T\←G^{-1}\stackrel{\‾}{b},$ 更新跟踪结果 $\left[\begin{array}{cc}A& v\\ 0& 1\end{array}\right]\←\left[\begin{array}{cc}A& v\\ 0& 1\end{array}\right]\left[\begin{array}{ccc}1+{\mathrm{\η}}_{\mathrm{xx}}& {\mathrm{\η}}_{\mathrm{xy}}& {\mathrm{\η}}_x\\ {\mathrm{\η}}_{\mathrm{yx}}& 1+{\mathrm{\η}}_{\mathrm{yy}}& {\mathrm{\η}}_y\\ 0& 0& 1\end{array}\right]$

直到

结束在这一层的迭代过程。

步骤3）通过相机标定，将I_n和I_n+1中的二维像素点反投影到三维空间内得到空间坐标：在地面上选取四个点，同时记录其在车体坐标系下的坐标和在图像坐标系下对应点的坐标，应用homography基本原理标定，将匹配的二维像素点对投影到空间中的三维点对，从而得到车体的旋转和平移；

步骤4）对于有一定间隔（图像重叠区域30%～90%）的两帧图像I_k和I_k+n利用SURF特征点匹配算法得到两帧图像间的运动。SURF算子选取图像在尺度空间上的极值点作为候选特征点。图像I(x,y)在尺度空间σ上的Hessian矩阵表示为：

$H=\left[\begin{array}{cc}{L}_{\mathrm{xx}}(x,\mathrm{\σ})& L_{\mathrm{xy}}(x,\mathrm{\σ})\\ L_{\mathrm{xy}}(x,\mathrm{\σ})& L_{\mathrm{yy}}(x,\mathrm{\σ})\end{array}\right]$

其中L_xx，L_xy，L_yy是对应方向高斯二阶偏导数和图像的卷积，高斯卷积函数为： $G\left(\mathrm{\σ}\right)=\frac{1}{{2\mathrm{\π\σ}}^2}{e}^{\frac{(x^2+y^2)}{{2\mathrm{\σ}}^2}}$

图像在不同尺度空间上与高斯函数进行卷积，从而构建图像金字塔，同时利用Hessian矩阵求出极值，在一个3*3*3的立体邻域内进行非极大值抑制，任何一个候选点都要与上一个尺度、当前尺度和下一个尺度的26个邻域值比较，只有比周围的都大或者都小，才能作为稳定的SURF特征点；

特征点匹配过程以特征向量的欧氏距离作为匹配基准：对于前一帧图像中的特征点p，在后一帧中计算距离特征点p最近点p₁和次近的两个特征点并且记录最近距离d₁和次近距离d₂，若d₁＜d₂*δ₃，δ₃∈(0.6,0.9)，认为p₁是p的匹配点。

步骤5）当车体运动，系统采集了多帧图像之后，首先针对连续图像进行光流计算，得到光流运动，同时对于这一段距离的头尾两帧图像进行SURF特征点匹配，将光流运动的累积结果和SURF特征点的匹配结果进行卡尔曼滤波，更新车体位置，输出结果。

两帧图像之间用光流法得到旋转R和位移T，并且不断累积，在n＝20帧后，对于I_k帧图像和I_k+n帧图像用SURF特征点匹配得到旋转R和位移T，对于光流法的结果和特征点匹配得到结果进行卡尔曼滤波，得到更加准确的结果作为下一次计算的初始值，不断循环。

根据离散卡尔曼滤波器时间更新方程：

${\hat{x}}_{\stackrel{\‾}{k}}=A{\hat{x}}_{k-1}+B{u}_{k-1}$

$P_{\stackrel{\‾}{k}}=A{P}_{k-1}{A}^T+Q$

其中，

是光流法计算出来的前k-1帧累积的机器人位置坐标以及航向角，u_k-1是位置控制函数，这里设置为u_k-1=[Δx,Δy,Δθ]^T，B是控制增益，设为常数，A是一个常数，通过公式，计算出当前状态P_k-1是前一帧的协方差估计，Q是过程激励噪声协方差矩阵，这里设为常数，根据离散卡尔曼滤波器时间更新方程，计算当前协方差估计

观测方程为：

z_k=Hx_k+v_k

其中z_k是通过SURF特征点匹配法计算出来的机器人的位置，H是单位矩阵，v_k表示观测噪声，为正态分布白噪声：p(v)～N(0,R)

其中，R是观测噪声协方差矩阵，这里假设是常数，计算卡尔曼增益K_k：

$K_k=P_{\stackrel{\‾}{k}}{H}^T{({\mathrm{HP}}_{\stackrel{\‾}{k}}{H}^T+R)}^{-1}$

根据离散卡尔曼滤波器状态更新方程，更新整个系统位置：

${\hat{x}}_k={\hat{x}}_{\stackrel{\‾}{k}}+K_k(z_k-H{\hat{x}}_{\stackrel{\‾}{k}})---\left(6\right)$

$P_k=(I-K_{k}H)P_{\stackrel{\‾}{k}}---\left(7\right)$

本实验是在校园环境中道路上进行的，应用bumblebee2相机采集图像，图像分辨率是640*480，采集频率是20赫兹，车上搭载差分GPS，以10赫兹采集GPS点作为真值，误差是0.01米。车辆平均行驶速度是7.56km/h，行驶里程是302m。计算和测量的轨迹曲线如图2所示。

光流法在处理时间上相比较于传统的特征点匹配算法有优势，见图3。光流法处理时间平均是60.69ms；SURF特征点匹配算法的处理时间平均是304.96ms。

在车辆行驶速度较快的情况下，容易出现两帧之间变化较大，超出匹配范围的情况，特征点匹配精度不高。这种情况下，光流法可以提供一种快速定位的方法，用以弥补特征点定位信息的缺失。在运动速度加快时，光流法可以增加金字塔的层数来实现跟踪，提供一种稳定的光流，用于估计机器人的运动。同时由于光流法的精度不高，需要特征点匹配算法进行融合，减小误差，矫正位置和姿态。误差曲线是计算融合后的点和GPS点(真值)之间的欧式距离，如图4。可以看出卡尔曼融合后曲线的误差相比较于光流计算结果和单纯的特征点匹配算法误差要小。

Claims (3)

1.一种融合光流与特征点匹配的单目视觉里程计的设计方法，其特征在于，包括如下步骤：

1）对于系统输入的连续两帧图像I_k和I_k+1，首先通过采样构建图像金字塔：金字塔的底层图像为原始图像，对底层图像利用线性插值的方法进行降采样，得到长和宽减半的上一层图像，依次类推，向上采样，构建4层金字塔；

2）对于前一帧图像中的像素点进行金字塔跟踪：从最顶层开始跟踪像素点u，计算光流d＝[d_x,d_y]和变换矩阵A，使得通过d和A变换后的前一帧图像与后一帧在这层上的灰度差异最小；然后再把这一层的结果传递给下一层，重新计算光流d和变换矩阵A，再传递给下一层；不断循环，直到底层，也就是原始图像，计算光流d和变换矩阵A，前后两帧图像经过变换后的灰度差小于阈值δ₁或循环次数大于δ₂时，循环结束，输出结果；

3）通过相机标定，将I_n和I_n+1中的二维像素点反投影到三维空间内得到空间坐标：在地面上选取四个点，同时记录其在车体坐标系下的坐标和在图像坐标系下对应点的坐标，应用homography基本原理标定，将匹配的二维像素点对投影到空间中的三维点对，从而得到车体的旋转和平移；

4）对于图像重叠区域30%～90%的两帧图像I_k和I_k+n利用SURF特征点匹配算法得到两帧图像间的运动；

5）当车体运动，系统采集了多帧图像之后，首先针对连续图像进行光流计算，得到光流运动，同时对于这一段距离的头尾两帧图像进行SURF特征点匹配，将光流运动的累积结果和SURF特征点的匹配结果进行卡尔曼滤波，更新车体位置，输出结果。

2.根据权利要求1所述的一种融合光流与特征点匹配的单目视觉里程计的设计方法，其特征在于：所述步骤4)中，对于图像I_k和I_k+n利用SURF特征点匹配算法得到两帧图像间运动，包括如下步骤：

1）SURF算子选取图像在尺度空间上的极值点作为候选特征点，图像I(x,y)在尺度空间σ上的Hessian矩阵表示为：

$H=\left[\begin{array}{cc}{L}_{\mathrm{xx}}(x,\mathrm{\σ})& L_{\mathrm{xy}}(x,\mathrm{\σ})\\ L_{\mathrm{xy}}(x,\mathrm{\σ})& L_{\mathrm{yy}}(x,\mathrm{\σ})\end{array}\right]---\left(1\right)$

其中L_xx，L_xy，L_yy是对应方向高斯二阶偏导数和图像的卷积，高斯卷积函数为： $G\left(\mathrm{\σ}\right)=\frac{1}{{2\mathrm{\π\σ}}^2}{e}^{\frac{(x^2+y^2)}{{2\mathrm{\σ}}^2}}$

图像在不同尺度空间上与高斯函数进行卷积，从而构建图像金字塔，同时利用Hessian矩阵求出极值，在一个3*3*3的立体邻域内进行非极大值抑制，任何一个候选点都要与上一个尺度、当前尺度和下一个尺度的26个邻域值比较，只有比周围的都大或者都小，才能作为稳定的SURF特征点；

2）特征点匹配过程以特征向量的欧氏距离作为匹配基准：对于前一帧图像中的特征点p，在后一帧中计算距离特征点p最近点p₁和次近的两个特征点并且记录最近距离d₁和次近距离d₂，若d₁＜d₂*δ₃，δ₃∈(0.6,0.9)，认为p₁是p的匹配点。

3.根据权利要求1所述的一种融合光流与特征点匹配的单目视觉里程计的设计方法，其特征在于：所述步骤5)中，当车体运动，系统采集了多帧图像之后，首先针对连续图像进行光流计算，得到光流运动，同时对于这一段距离的头尾两帧图像进行SURF特征点匹配，将光流运动的累积结果和SURF特征点的匹配结果进行卡尔曼滤波，更新车体位置，输出结果，包括如下步骤：

1）两帧图像之间用光流法得到旋转R和位移T，并且不断累积，在n帧后，对于I_k帧图像和I_k+n帧图像用SURF特征点匹配得到旋转R和位移T，对于光流法的结果和特征点匹配得到结果进行卡尔曼滤波，得到更加准确的结果作为下一次计算的初始值，不断循环；

根据离散卡尔曼滤波器时间更新方程：

${\hat{x}}_{\stackrel{\‾}{k}}=A{\hat{x}}_{k-1}+B{u}_{k-1}---\left(2\right)$

$P_{\stackrel{\‾}{k}}=A{P}_{k-1}{A}^T+Q---\left(3\right)$

其中，

是光流法计算出来的前k-1帧累积的机器人位置坐标以及航向角，u_k-1是位置控制函数，这里设置为u_k-1=[Δx,Δy,Δθ]^T，B是控制增益，设为常数，A是一个常数，通过公式(2)，计算出当前状态

P_k-1是前一帧的协方差估计，Q是过程激励噪声协方差矩阵，这里设为常数，根据离散卡尔曼滤波器时间更新方程，计算当前协方差估计

观测方程为：

z_k=Hx_k+v_k    (4)

其中z_k是通过SURF特征点匹配法计算出来的机器人的位置，H是单位矩阵，v_k表示观测噪声，为正态分布白噪声：p(v)～N(0,R)

其中，R是观测噪声协方差矩阵，这里假设是常数，计算卡尔曼增益K_k：

$K_k=P_{\stackrel{\‾}{k}}{H}^T{({\mathrm{HP}}_{\stackrel{\‾}{k}}{H}^T+R)}^{-1}---\left(5\right)$

根据离散卡尔曼滤波器状态更新方程，更新整个系统位置：

${\hat{x}}_k={\hat{x}}_{\stackrel{\‾}{k}}+K_k(z_k-H{\hat{x}}_{\stackrel{\‾}{k}})---\left(6\right)$

$P_k=(I-K_{k}H)P_{\stackrel{\‾}{k}}---\left(7\right).$

Images