CN102519481B - 一种双目视觉里程计实现方法 - Google Patents

Abstract

本发明公开了一种移动机器人双目视觉里程计实现方法。该方法包括以下步骤：使用双目摄像机采集室内天花板图像对，对两幅图像分别提取连接点特征；采用点模式匹配方法建立图像对的特征匹配关系，进而根据立体视觉算法计算匹配点对对应的三维坐标；得到采集到的图像序列中的各相邻图像之间的匹配点对的三维对应关系；根据匹配点对的三维对应关系计算摄像机的相对运动参数，采用分段优化算法对其进行优化；根据摄像机与机器人的相对位置关系将摄像机的相对运动参数转化为机器人运动参数，并更新机器人运动的里程信息。本发明的双目视觉里程计不依赖于对轮轴旋转的计数，从而避免了车轮半径变化、空转、打滑等对里程计测量精度的影响。

Description

一种双目视觉里程计实现方法

技术领域

本发明涉及移动机器人自主导航技术领域，尤其是一种应用于室内环境中轮驱式移动机器人自主导航技术中的双目视觉里程计实现方法。

背景技术

移动机器人研究的目标是在没有人干预且无需对环境作任何规定和改变的情况下，有目的地移动和完成相应任务。在实际应用中，当机器人要自主的完成一项任务时，最重要的是进行准确的自定位，即机器人需要知道自己现在所处的位置和姿态。通常轮驱式移动机器人自定位采用航迹推算的方法：利用两个轮子加装的里程计测出的微小位移，计算出移动机器人的位置和姿态，通过累加实现移动机器人的自定位。这种自定位方法的最大问题是驱动轮空转或打滑情况下累积误差增大。此外，传感器精度降低、轮子半径的变化等情况下也会引起里程计误差增大。

为降低自定位误差并提高定位对环境的鲁棒性，计算机视觉被应用于机器人自定位中，构成视觉里程计。视觉里程计利用机器人摄像机采集的序列图像，通过特征跟踪和相对运动估计得出机器人自身的运动参数。与传统定位方式相比，视觉里程计采用被动视觉传感器，结构简单、信息量大，只依靠视觉输入信息即可完成定位，无需场景及运动的先验知识，且不存在传感器精度降低导致的误差，适用于各种环境。

发明内容

本发明的目的在于实现一种视觉里程计，该里程计可实现高精度、实时里程信息测量，并避免了机器人驱动轮半径变化及轮子空转、打滑等情况下里程信息累计误差的增大。

本发明所提出的一种双目视觉里程计实现方法，其特征在于，该方法包括以下步骤：

步骤1，使用双目摄像机采集室内天花板图像对，并对两幅图像分别提取连接点特征；

步骤2，采用点模式匹配方法建立双目摄像机采集到的图像对的特征匹配关系，进而根据立体视觉算法计算匹配点对对应的三维坐标；

步骤3，使用步骤2的方法得到摄像机采集到的图像序列中的各相邻图像之间的匹配点对的三维对应关系；

步骤4，根据步骤3得到的匹配点对的三维对应关系计算摄像机的相对运动参数，并采用分段优化算法对其进行优化；

步骤5，根据摄像机与机器人的相对位置关系将摄像机的相对运动参数转化为机器人运动参数，并更新机器人运动的里程信息。

本发明能产生如下有益效果：利用摄像头对构成立体视觉传感器，安装于移动机器人顶部，拍摄室内天花板场景，通过对相邻连续帧的处理，以分段优化算法估计摄像机运动参数，实现移动机器人里程信息的推算。视觉里程计不依赖于对轮轴旋转的计数，从而避免了车轮半径变化、空转、打滑等对里程计测量精度的影响，又通过对图像降采样提取连接点及采用点模式方法进行特征匹配跟踪，在提高测量精度的同时保证了视觉处理过程的实时性。另外，双目视觉里程计安装简单，维护方便。

附图说明

图1是根据本发明实施例的方法流程图。

图2是提取连接点特征实例示意图。

图3是点模式匹配方法流程图。

图4是图像序列分段优化示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明进一步详细说明。

本发明中，双目摄像机安装于移动机器人的顶部，随着机器人的运动，摄像机采集室内天花板图像得到序列图像，通过图像特征提取、特征匹配与跟踪和分段优化算法计算各帧图像对应的摄像机相对运动参数，进而根据摄像机与机器人的相对位置关系将摄像机运动参数转化为机器人的相对运动参数，这一相对运动参数的累计就构成了机器人运动的里程信息。

图1是根据本发明实施例的方法流程图。如图1所示，本发明提出的双目视觉里程计实现方法包括以下步骤：

步骤1，使用双目摄像机采集室内天花板图像对，并对两幅图像分别提取连接点特征。

本发明采用双目摄像机采集室内天花板图像对，其中，一台摄像机作为主摄像机，其采集的图像序列用于特征匹配；另一台摄像机作为辅助摄像机，与主摄像机采集同一帧图像，用于计算每一帧图像上的特征的三维坐标。

本实例中，移动机器人在室内运动，针对室内环境纹理简单且边缘直线较多这一特点，以图像边缘直线连接点作为实现视觉里程计的主要视觉特征。与其他常用的图像特征如角点和团块特征相比，连接点特征在环境光照条件变化情况下检出率稳定，且定位精度更高。

连接点特征提取的实例示意图如图2所示，其中，图2(a)为摄像机获取的原始图像：

首先，采用Canny算子提取图像边缘得到边缘图像，如图2(b)所示；

然后，利用Hough变换从边缘图像中提取直线，提取出的直线如图2(c)所示；由于图像模糊和边缘宽度的影响，Hough变换得到的是若干组相互平行的直线，通过最小二乘法拟合直线，将相互平行且距离较近的直线组合并为单一直线，通过最小二乘法拟合后得到的直线如图2(d)所示；

最后，计算多条直线的交点获得连接点特征，如图2(e)所示。

步骤2，采用点模式匹配方法建立双目摄像机采集到的图像对的特征匹配关系，进而根据立体视觉算法计算匹配点对对应的三维坐标。

采用双目摄像机采集同一场景的图像，如果两幅图像中对应同一空间点的两个像素点之间的对应关系已知，结合摄像机内部参数及相对位置关系，则可根据立体视觉算法由采集得到的图像恢复场景空间的三维信息。立体视觉算法的核心在于图像对之间像素点的匹配，一旦建立起精确的对应关系，就可由简单的三角计算得到三维坐标。

常用查找像素匹配点的算法为基于面积的匹配点搜寻方法，其基本思路是计算欲匹配点周围区域与另一幅图像上同样大小区域的灰度相似性。如果两幅图像局部结构相似性较高，基于面积的匹配方法易得出错误的匹配关系，并且匹配计算量较高。为提高系统的实时性，并考虑到同一场景对应的两幅图像及序列图像中相邻帧之间结构偏移量较小，本发明采用点模式匹配方法进行特征点的匹配。

所述点模式匹配方法，是将图像匹配问题转化为从两个满足一定映射关系的点集中找出最优匹配点对的问题。本发明中，摄像机采集天花板图像，当摄像机与天花板的距离远大于天花板上物体高度时，可将天花板视为平面。已知由同一空间平面映射而成的两幅图像上对应点之间满足单应映射关系：

其中，分别为两幅图像对应点的齐次坐标，H为3×3矩阵，称为单应映射矩阵，s_i为常系数，因此，本发明中可通过迭代求解特征点集之间的最优单应映射关系，同时获得匹配对应关系。

图3所示为点模式匹配方法的具体实现步骤：

步骤2.1，将图像对中分别所含的特征点组成特征点集P和Q；

步骤2.2，采用最近距离准则建立待匹配特征点集P与Q之间的一一对应关系；

考虑到P与Q为不完全匹配情况，且两者都存在出格点，于是当P中可匹配点在最近距离准则下都找到Q中相应的对应点时，P中出格点由于不存在可匹配点而会寻找到Q中出格点，其对应距离大于可匹配点的最近距离。因此，可设定距离阈值σ，当P中的某一点与Q中对应的最近点之间的距离大于σ时，则将P和Q中的对应点均作为出格点处理。

步骤2.3，估计单应映射矩阵H的初值；

将特征点集P和Q中最近点距离小于距离阈值σ的对应点作为初始匹配子集，对两幅图像中产生的两个初始匹配子集的坐标分别进行平移，使得特征点集P和Q的质心移至坐标原点，然后选择合适的缩放因子使特征点集P和Q中的各点到原点的平均距离为1，此时特征点集P和Q在坐标系的4个象限都有点分布，于是可以选择每个象限中特征点集P和Q的质心计算单应映射矩阵(维数为4*4)，作为初始匹配点单应映射矩阵H的估计初值。

步骤2.4，计算特征点集P与Q之间的映射距离ε，并根据计算出的映射距离ε判断H是否为最优映射关系。

计算映射距离时，除计算匹配子集之间的距离外，还需考虑出格点的数量对映射关系的影响。设点集P与Q的点数分别为n和m，n≤m，P与Q有k对匹配点，映射距离

当ε小于预设的最小映射距离ε_min时，表明已经得到最优的H估计，结束迭代过程跳转到步骤2.5；否则，将P经H映射为中间点集P′与Q组成待匹配点集，跳转到步骤2.2(即，将P′作为P)，继续迭代过程。

步骤2.5，累计相乘迭代过程中估计的各映射矩阵H构成原特征点集P与Q的最优映射关系矩阵H，得到特征点集P与Q之间的匹配关系。

在得到图像对特征点集之间的匹配关系后，可采用立体视觉算法计算各特征点对应的三维坐标。本发明中，摄像机对之间的相对位置固定，通过校准可使摄像机对的内部参数相同，且图像各点对应极线相互平行，又称为外极线配置。在外极线配置下，两幅图像的匹配点对(x，y)、(x′，y′)对应的三维点坐标为： $X=\frac{d(x-u_0)}{x-x^{\′}},$ $Y=\frac{d(0.5(y+y^{\′})-v_0)}{x-x^{\′}},$ $Z=\frac{\mathrm{df}}{x-x^{\′}},$ f为摄像机焦距，d为摄像机对光心之间的相对距离，(u₀，y₀)为主点坐标，即原特征点集P中光心的图像坐标。

步骤3，使用步骤2的方法得到摄像机采集到的图像序列中的各相邻图像之间的匹配点的三维对应关系。

机器人运动过程中，摄像机不断采集图像形成图像序列。根据步骤2描述的点模式匹配方法在图像序列中的各帧图像之间建立特征匹配关系，根据步骤2描述的立体视觉算法计算图像序列各个连接点特征对应的三维坐标，将得到的各连接点特征对应的三维坐标代入到相应的特征匹配关系中得到相邻图像之间的匹配点对的三维对应关系。

步骤4，根据步骤3得到的匹配点对的三维对应关系计算摄像机的相对运动参数，并采用分段优化算法对其进行优化。

根据三维对应关系可以方便的计算由机器人运动产生的摄像机相对运动参数旋转矩阵R和三维平移矢量t：假设三维对应关系确定的三维对应点为p_i、p′_i，而摄像机的相对运动使用3×3旋转矩阵R和三维平移矢量t描述，则有关系式p′_i＝Rp_i+t，当对应点对数目大于4时，可用最小二乘法由此关系式解出R和t。

上述采用最小二乘法求解摄像机的相对运动参数时假设数据不包含噪声，实际上由于图像点定位误差的存在，三维重建数据也存在一定的误差。在具有误差的情况下，一般以最小二乘法的结果为初值，采用一定的优化准则对结果进行迭代优化，提高其精确度。本发明采用图像点和三维点之间的重投影误差对摄像机的相对运动参数进行优化。

当运动过程中摄像机采样速度较快时，图像序列中相邻多帧由同一三维物体投影产生，形成图像重叠。按照图像重叠情况，将图像序列分为合理的多段，每段中第一帧图像提取的连接点三维坐标作为“路标”，后续各帧相对此“路标”求解相对运动参数，同时利用图像重叠性不断优化“路标”位置，以提高相对运动参数估计的精确性。这一利用图像序列重叠情况进行运动参数优化的方法称为分段优化算法。首先定义一个重投影误差：当运动参数R，t的估计存在误差时，三维点经投影矩阵计算得到的重投影点与图像点不能完全重合，因此定义重投影误差：

$d^2({\tilde{p}}_i,M{\tilde{q}}_i)={\left|\right|{\tilde{p}}_i-M{\tilde{q}}_i\left|\right|}^2,$

式中，

分别表示空间点q_i和图像点p_i对应的齐次坐标，M为投影矩阵，由摄像机内部参数矩阵K、摄像机的相对运动参数R，t构成：

$M=\left[\begin{array}{cc}K& 0\end{array}\right]\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right].$

图4为图像序列按照重叠情况分段计算相对运动参数的示意图，所述分段优化算法进一步包括以下步骤：

步骤4.1，计算第1帧图像各个特征点的三维坐标q_1i＝{x_1i，y_1i，z_1i}，其中，i＝1，2，...，n，n为第1帧图像特征点的个数；

步骤4.2，获取第2帧图像，根据点模式匹配方法将第2帧图像与第1帧图像进行匹配，得到与第1帧图像点匹配的第2帧图像中的特征点p_2i，其中，i＝1，2，...，m，m为第2帧图像特征点的个数；计算第2帧图像中各个特征点的三维坐标q_2i＝{x_2i，y_2i，z_2i}；计算摄像机的相对运动参数R₂₁，t₂₁；以重投影误差

为准则优化参数集(R₂₁，t₂₁，q_1i)；

步骤4.3，继续获取图像，得到第k帧图像(k＞＝3)，计算其与第1帧图像匹配的特征点p_ki及其三维坐标q_ki＝{x_ki，y_ki，z_ki}，其中，i＝1，2，...，h，h为第k帧图像特征点的个数，如果第k帧图像与第1帧图像满足重叠条件：其匹配对数目大于等于某一阈值，则计算相对运动参数R_k1，t_k1，以重投影误差的和 $\mathrm{\Σ}(d^2({\tilde{p}}_{\mathrm{ki}},M_{k1}{\tilde{q}}_{\mathrm{ki}})+\·\·\·+d^2({\tilde{p}}_{2i},M_{21}{\tilde{q}}_{2i}))$ 最小为准则优化参数集(R_k1，t_k1，…，R₂₁，t₂₁，q_1i)；否则以第k-1帧为第1帧返回步骤(4.1)，划分新的图像段进行优化过程。直至最后一幅图像，结束。

在图4中，第4帧图像不满足上述重叠条件，从而以第3帧作为第1帧，划分新的图像段进行优化。

步骤5，根据摄像机与机器人的相对位置关系将摄像机的相对运动参数转化为机器人运动参数，并更新机器人运动的里程信息。

本发明中，摄像机安装于机器人顶部，与机器人本体的位置关系固定，通过手眼标定过程可得到摄像机与机器人的相对位置关系，则可将前述摄像机的相对运动参数转化为机器人本体的运动参数，从而实现机器人里程信息的推算。

假设摄像机相对于机器人本体的位置关系用旋转矩阵R_ec和平移矢量t_ec表示，当摄像机相对运动参数为R_cc和t_cc时，对应的机器人运动参数为t_ee＝R_ect_cc-R_eet_ec，此时假设运动前机器人相对起始点的里程信息为R_0，k-1和t_0，k-1，那么运动后机器人的里程信息可更新为R_0，k＝R_0，k-1R_ee，t_0，k＝t_0，k-1+R_0，k-1t_ee，其中k为匹配点的个数。

以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种双目视觉里程计实现方法，其特征在于，该方法包括以下步骤：

步骤1，使用双目摄像机采集室内天花板图像对，并对两幅图像分别提取连接点特征；

步骤2，采用点模式匹配方法建立双目摄像机采集到的图像对的特征匹配关系，进而根据立体视觉算法计算匹配点对对应的三维坐标；

步骤3，使用步骤2的方法得到摄像机采集到的图像序列中的各相邻图像之间的匹配点对的三维对应关系；

步骤4，根据步骤3得到的匹配点对的三维对应关系计算摄像机的相对运动参数，并采用分段优化算法对所述相对运动参数进行优化；

步骤5，根据摄像机与机器人的相对位置关系将摄像机的相对运动参数转化为机器人运动参数，并更新机器人运动的里程信息；

其中，采用点模式匹配方法建立双目摄像机采集到的图像对的特征匹配关系进一步包括以下步骤：

步骤2.1，将图像对中分别所含的特征点组成特征点集P和Q；

步骤2.2，采用最近距离准则建立待匹配特征点集P与Q之间的一一对应关系；

步骤2.3，估计单应映射矩阵H的初值；

步骤2.4，计算特征点集P与Q之间的映射距离ε，当ε小于预设的最小映射距离ε_min时，表明已经得到最优的H估计，结束迭代过程跳转到步骤2.5；否则，将特征点集P经单应映射矩阵H映射为中间点集P′与Q组成待匹配点集，跳转到步骤2.2，继续迭代过程；

步骤2.5，累计迭代过程中估计的各映射矩阵H构成原特征点集P与Q的最优映射关系矩阵H，得到特征点集P与Q之间的匹配关系。

2.根据权利要求1所述的方法，其特征在于，连接点特征的提取进一步包括以下步骤：

首先，采用Canny算子提取图像边缘得到边缘图像；

然后，利用Hough变换从边缘图像中提取直线；

最后，计算多条直线的交点获得连接点特征。

3.根据权利要求2所述的方法，其特征在于，利用Hough变换从边缘图像中提取直线时，由于图像模糊和边缘宽度的影响，Hough变换得到的是若干组相互平行的直线，通过最小二乘法拟合直线，将相互平行且距离较近的直线组合并为单一直线。

4.根据权利要求1所述的方法，其特征在于，所述步骤2.2中，当特征点集P中的某一点与特征点集Q中对应的最近点之间的距离大于σ时，则将特征点集P和Q中的对应点均作为出格点处理。

5.根据权利要求1所述的方法，其特征在于，所述步骤2.3中，将特征点集P和Q中最近点距离小于距离阈值σ的对应点作为初始匹配子集，对两幅图像中产生的两个初始匹配子集的坐标分别进行平移，使得特征点集P和Q的质心移至坐标原点，然后选择合适的缩放因子使特征点集P和Q中的各点到原点的平均距离为1，此时特征点集P和Q在坐标系的4个象限都有点分布，于是可以选择每个象限中特征点集P和Q的质心作为初始匹配点计算单应映射矩阵，作为单应映射矩阵H的估计初值。

6.根据权利要求1所述的方法，其特征在于，所述步骤2.4中，特征点集P与Q之间的映射距离

其中，k为特征点集P与Q的匹配点对数，n为特征点集P的点数，

分别为两幅图像对应点的齐次坐标，σ为预设的距离阈值。

7.根据权利要求1所述的方法，其特征在于，根据立体视觉算法计算匹配点对(x，y)、(x′，y′)对应的三维点坐标为：

$X=\frac{d(x-u_0)}{x-x^{\′}},$ $Y=\frac{d(0.5(y+y^{\′})-v_0)}{x-x^{\′}},$ $Z=\frac{\mathrm{df}}{x-x^{\′}},$

其中，f为摄像机焦距，d为摄像机对光心之间的相对距离，(u₀，v₀)为主点坐标，即特征点集P中光心的图像坐标。

8.根据权利要求1所述的方法，其特征在于，假设所述三维对应关系确定的三维对应点为p_i、p′_i，那么摄像机的相对运动参数旋转矩阵R和三维平移矢量t根据其与三维对应点的关系式：p′_i＝Rp_i+t来计算。

9.根据权利要求8所述的方法，其特征在于，所述分段优化算法进一步包括以下步骤：

步骤4.1，计算第1帧图像各个特征点的三维坐标q_1i＝{x_1i，y_1i，z_1i}，其中，i＝1，2，...，n，n为第1帧图像特征点的个数；

步骤4.2，获取第2帧图像，根据点模式匹配方法将第2帧图像与第1帧图像进行匹配，得到与第1帧图像点匹配的第2帧图像中的特征点p_2i，其中，i＝1，2，...，m，m为第2帧图像特征点的个数；计算第2帧图像中各个特征点的三维坐标q_2i＝{x_2i，y_2i，z_2i}；计算摄像机的相对运动参数R₂₁，t₂₁；并以重投影误差最小

为准则优化参数集(R₂₁，t₂₁，q_1i)，其中，

分别为图像点和空间点

对应的齐次坐标，M为投影矩阵；

步骤4.3，继续获取图像，得到第k帧图像，k＞＝3，计算其与第1帧图像匹配的特征点p_ki及其三维坐标q_ki＝{x_ki，y_ki，z_ki}，其中，i＝1，2，...，h，h为第k帧图像特征点的个数，如果第k帧图像与第1帧图像满足重叠条件：其匹配对数目大于等于某一阈值，则计算摄像机的相对运动参数R_k1，t_k1，以重投影误差的和 $\mathrm{\Σ}(d^2({\tilde{p}}_{\mathrm{ki}},M_{k1}{\tilde{q}}_{\mathrm{ki}})+\·\·\·+d^2({\tilde{p}}_{2i},M_{21}{\tilde{q}}_{2i}))$ 最小为准则优化参数集(R_k1，t_k1，…，R₂₁，t₂₁，q_1i)；否则以第k-1帧为第1帧返回步骤4.1，划分新的图像段进行优化过程，直至最后一幅图像，结束。

10.根据权利要求1所述的方法，其特征在于，所述步骤5进一步包括以下步骤：

根据摄像机相对运动参数R_cc和t_cc，计算对应的机器人运动参数t_ee＝R_ect_cc-R_eet_ec，其中，旋转矩阵R_ec和平移矢量t_ec为摄像机相对于机器人本体的位置关系的描述参数；

更新运动后机器人的里程信息为R_0，k＝R_0，k-1R_ee，t_0，k＝t_0，k-1+R_0，k-1t_ee，其中k为匹配点的个数，R_0，k-1和t_0，k-1为运动前机器人相对起始点的里程信息。

Images