专利名称：一种双目视觉里程计实现方法
技术领域：
本发明涉及移动机器人自主导航技术领域，尤其是一种应用于室内环境中轮驱式移动机器人自主导航技术中的双目视觉里程计实现方法。
背景技术：
移动机器人研究的目标是在没有人干预且无需对环境作任何规定和改变的情况下，有目的地移动和完成相应任务。在实际应用中，当机器人要自主的完成一项任务时，最重要的是进行准确的自定位，即机器人需要知道自己现在所处的位置和姿态。通常轮驱式移动机器人自定位采用航迹推算的方法利用两个轮子加装的里程计测出的微小位移，计算出移动机器人的位置和姿态，通过累加实现移动机器人的自定位。这种自定位方法的最大问题是驱动轮空转或打滑情况下累积误差增大。此外，传感器精度降低、轮子半径的变化等情况下也会引起里程计误差增大。为降低自定位误差并提高定位对环境的鲁棒性，计算机视觉被应用于机器人自定位中，构成视觉里程计。视觉里程计利用机器人摄像机采集的序列图像，通过特征跟踪和相对运动估计得出机器人自身的运动参数。与传统定位方式相比，视觉里程计采用被动视觉传感器，结构简单、信息量大，只依靠视觉输入信息即可完成定位，无需场景及运动的先验知识，且不存在传感器精度降低导致的误差，适用于各种环境。

发明内容
本发明的目的在于实现一种视觉里程计，该里程计可实现高精度、实时里程信息测量，并避免了机器人驱动轮半径变化及轮子空转、打滑等情况下里程信息累计误差的增大。本发明所提出的一种双目视觉里程计实现方法，其特征在于，该方法包括以下步骤步骤1，使用双目摄像机采集室内天花板图像对，并对两幅图像分别提取连接点特征；步骤2，采用点模式匹配方法建立双目摄像机采集到的图像对的特征匹配关系，进而根据立体视觉算法计算匹配点对对应的三维坐标；步骤3，使用步骤2的方法得到摄像机采集到的图像序列中的各相邻图像之间的匹配点对的三维对应关系；步骤4，根据步骤3得到的匹配点对的三维对应关系计算摄像机的相对运动参数， 并采用分段优化算法对其进行优化；步骤5，根据摄像机与机器人的相对位置关系将摄像机的相对运动参数转化为机器人运动参数，并更新机器人运动的里程信息。本发明能产生如下有益效果利用摄像头对构成立体视觉传感器，安装于移动机器人顶部，拍摄室内天花板场景，通过对相邻连续帧的处理，以分段优化算法估计摄像机运动参数，实现移动机器人里程信息的推算。视觉里程计不依赖于对轮轴旋转的计数，从而避免了车轮半径变化、空转、打滑等对里程计测量精度的影响，又通过对图像降采样提取连接点及采用点模式方法进行特征匹配跟踪，在提高测量精度的同时保证了视觉处理过程的实时性。另外，双目视觉里程计安装简单，维护方便。


图1是根据本发明实施例的方法流程图。图2是提取连接点特征实例示意图。图3是点模式匹配方法流程图。图4是图像序列分段优化示意图。
具体实施例方式为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明进一步详细说明。本发明中，双目摄像机安装于移动机器人的顶部，随着机器人的运动，摄像机采集室内天花板图像得到序列图像，通过图像特征提取、特征匹配与跟踪和分段优化算法计算各帧图像对应的摄像机相对运动参数，进而根据摄像机与机器人的相对位置关系将摄像机运动参数转化为机器人的相对运动参数，这一相对运动参数的累计就构成了机器人运动的里程信息。图1是根据本发明实施例的方法流程图。如图1所示，本发明提出的双目视觉里程计实现方法包括以下步骤步骤1，使用双目摄像机采集室内天花板图像对，并对两幅图像分别提取连接点特征。本发明采用双目摄像机采集室内天花板图像对，其中，一台摄像机作为主摄像机， 其采集的图像序列用于特征匹配；另一台摄像机作为辅助摄像机，与主摄像机采集同一帧图像，用于计算每一帧图像上的特征的三维坐标。本实例中，移动机器人在室内运动，针对室内环境纹理简单且边缘直线较多这一特点，以图像边缘直线连接点作为实现视觉里程计的主要视觉特征。与其他常用的图像特征如角点和团块特征相比，连接点特征在环境光照条件变化情况下检出率稳定，且定位精
度更高。连接点特征提取的实例示意图如图2所示，其中，图2(a)为摄像机获取的原始图像首先，采用Carmy算子提取图像边缘得到边缘图像，如图2 (b)所示；然后，利用Hough变换从边缘图像中提取直线，提取出的直线如图2(c)所示；由于图像模糊和边缘宽度的影响，Hough变换得到的是若干组相互平行的直线，通过最小二乘法拟合直线，将相互平行且距离较近的直线组合并为单一直线，通过最小二乘法拟合后得到的直线如图2(d)所示；最后，计算多条直线的交点获得连接点特征，如图2(e)所示。步骤2，采用点模式匹配方法建立双目摄像机采集到的图像对的特征匹配关系，进而根据立体视觉算法计算匹配点对对应的三维坐标。采用双目摄像机采集同一场景的图像，如果两幅图像中对应同一空间点的两个像素点之间的对应关系已知，结合摄像机内部参数及相对位置关系，则可根据立体视觉算法由采集得到的图像恢复场景空间的三维信息。立体视觉算法的核心在于图像对之间像素点的匹配，一旦建立起精确的对应关系，就可由简单的三角计算得到三维坐标。常用查找像素匹配点的算法为基于面积的匹配点搜寻方法，其基本思路是计算欲匹配点周围区域与另一幅图像上同样大小区域的灰度相似性。如果两幅图像局部结构相似性较高，基于面积的匹配方法易得出错误的匹配关系，并且匹配计算量较高。为提高系统的实时性，并考虑到同一场景对应的两幅图像及序列图像中相邻帧之间结构偏移量较小，本发明采用点模式匹配方法进行特征点的匹配。所述点模式匹配方法，是将图像匹配问题转化为从两个满足一定映射关系的点集中找出最优匹配点对的问题。本发明中，摄像机采集天花板图像，当摄像机与天花板的距离远大于天花板上物体高度时，可将天花板视为平面。已知由同一空间平面映射而成的两幅图像上对应点之间满足单应映射关系=H艮，其中，艮、分别为两幅图像对应点的齐次坐标，H为3 X 3矩阵，称为单应映射矩阵，Si为常系数，因此，本发明中可通过迭代求解特征点集之间的最优单应映射关系，同时获得匹配对应关系。图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的点数分别为η和m，η彡m，P与Q有k对匹配点，映射距离
化幻一。当ε小于预设的最小映射距离￡min时，表明已经得到最优的
i
H估计，结束迭代过程跳转到步骤2. 5 ；否则，将P经H映射为中间点集P'与Q组成待匹配点集，跳转到步骤2. 2 (即，将P'作为P)，继续迭代过程。步骤2. 5，累计相乘迭代过程中估计的各映射矩阵H构成原特征点集P与Q的最优
6映射关系矩阵H，得到特征点集P与Q之间的匹配关系。在得到图像对特征点集之间的匹配关系后，可采用立体视觉算法计算各特征点对应的三维坐标。本发明中，摄像机对之间的相对位置固定，通过校准可使摄像机对的内部参数相同，且图像各点对应极线相互平行，又称为外极线配置。在外极线配置下，两幅图像
的匹配点对(x，y)、(x'，y')对应的三维点坐标为义=^: ),Y = d(°-5(y+y')-vo)，
权利要求
1.一种双目视觉里程计实现方法，其特征在于，该方法包括以下步骤步骤1，使用双目摄像机采集室内天花板图像对，并对两幅图像分别提取连接点特征； 步骤2，采用点模式匹配方法建立双目摄像机采集到的图像对的特征匹配关系，进而根据立体视觉算法计算匹配点对对应的三维坐标；步骤3，使用步骤2的方法得到摄像机采集到的图像序列中的各相邻图像之间的匹配点对的三维对应关系；步骤4，根据步骤3得到的匹配点对的三维对应关系计算摄像机的相对运动参数，并采用分段优化算法对其进行优化；步骤5，根据摄像机与机器人的相对位置关系将摄像机的相对运动参数转化为机器人运动参数，并更新机器人运动的里程信息。
2.根据权利要求1所述的方法，其特征在于，连接点特征的提取进一步包括以下步骤 首先，采用Carmy算子提取图像边缘得到边缘图像；然后，利用Hough变换从边缘图像中提取直线； 最后，计算多条直线的交点获得连接点特征。
3.根据权利要求2所述的方法，其特征在于，利用Hough变换从边缘图像中提取直线时，由于图像模糊和边缘宽度的影响，Hough变换得到的是若干组相互平行的直线，通过最小二乘法拟合直线，将相互平行且距离较近的直线组合并为单一直线。
4.根据权利要求1所述的方法，其特征在于，采用点模式匹配方法建立双目摄像机采集到的图像对的特征匹配关系进一步包括以下步骤步骤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之间的匹配关系。
5.根据权利要求4所述的方法，其特征在于，所述步骤2.2中，当特征点集P中的某一点与特征点集Q中对应的最近点之间的距离大于ο时，则将特征点集P和Q中的对应点均作为出格点处理。
6.根据权利要求4所述的方法，其特征在于，所述步骤2.3中，将特征点集P和Q中最近点距离小于距离阈值σ的对应点作为初始匹配子集，对两幅图像中产生的两个初始匹配子集的坐标分别进行平移，使得特征点集P和Q的质心移至坐标原点，然后选择合适的缩放因子使特征点集P和Q中的各点到原点的平均距离为1，此时特征点集P和Q在坐标系的4个象限都有点分布，于是可以选择每个象限中特征点集P和Q的质心作为初始匹配点计算单应映射矩阵，作为单应映射矩阵H的估计初值。
7.根据权利要求4所述的方法，其特征在于，所述步骤2.4中，特征点集P与Q之间的映射距离s = Ι化-HpiI2，其中，k为特征点集P与Q的匹配点对数，η和m分别为特征点集P与Q的点数，η ( Π ,ρ,、化分别为两幅图像对应点的齐次坐标，O为预设的距离阈值。
8.根据权利要求1所述的方法，其特征在于，根据立体视觉算法计算匹配点对(X，y)、 (χ'，y')对应的三维点坐标为X = d(x-u0) γ = d(0.5(y +y')-V0) z= dfX X ，X X， X X，其中，f为摄像机焦距，d为摄像机对光心之间的相对距离，(Utl，Y0)为主点坐标，即特征点集P中光心的图像坐标。
9.根据权利要求1所述的方法，其特征在于，假设所述三维对应关系确定的三维对应点为Pi、P‘ i，那么摄像机的相对运动参数旋转矩阵R和三维平移矢量t根据其与三维对应点的关系式p' i = RPi+t来计算。
10.根据权利要求9所述的方法，其特征在于，所述分段优化算法进一步包括以下步骤步骤4. 1，计算第1帧图像各个特征点的三维坐标Qli = Ixli, ，ζπ}，其中，i = 1， 2，. . .，η，η为第1帧图像特征点的个数；步骤4. 2，获取第2帧图像，根据点模式匹配方法将第2帧图像与第1帧图像进行匹配， 得到与第1帧图像点匹配的第2帧图像中的特征点P2i，其中，i = 1，2，. . .，m，m为第2帧图像特征点的个数；计算第2帧图像中各个特征点的三维坐标q2i = {x2i, y2i，Z2J ；计算摄像机的相对运动参数1 21，t21 ；并以重投影误差最小min^>2 (A,M2A)为准则优化参数集 (R21, t21，Qli)，其中， 分别为图像点A和空间点知对应的齐次坐标，M为投影矩阵；步骤4.3，继续获取图像，得到第k帧图像，k>= 3，计算其与第1帧图像匹配的特征点Pki及其三维坐标qki = {xki，ykidki}，其中，i = 1，2，...，h，h为第k帧图像特征点的个数，如果第k帧图像与第1帧图像满足重叠条件其匹配对数目大于等于某一阈值，则计算摄像机的相对运动参数I kl，tkl，以重投影误差的和礼，MH礼)+…+ f (瓦，似21礼》 最小为准则优化参数集(Rkl，tkl，-,R21,t21,qii)；否则以第k-1帧为第1帧返回步骤4. 1， 划分新的图像段进行优化过程，直至最后一幅图像，结束。
11.根据权利要求1所述的方法，其特征在于，所述步骤5进一步包括以下步骤 根据摄像机相对运动参数R。。和t。。，计算对应的机器人运动参数Ree = RecRccRL，tee =Re。t。。-RM、，其中，旋转矩阵民。和平移矢量、为摄像机相对于机器人本体的位置关系的描述参数；更新运动后机器人的里程信息为Rtl,k = R0,^1Ree' t0,k = t^+R。^、，其中k为匹配点的个数，R0, k-!和t^i为运动前机器人相对起始点的里程信息。
全文摘要
本发明公开了一种移动机器人双目视觉里程计实现方法。该方法包括以下步骤使用双目摄像机采集室内天花板图像对，对两幅图像分别提取连接点特征；采用点模式匹配方法建立图像对的特征匹配关系，进而根据立体视觉算法计算匹配点对对应的三维坐标；得到采集到的图像序列中的各相邻图像之间的匹配点对的三维对应关系；根据匹配点对的三维对应关系计算摄像机的相对运动参数，采用分段优化算法对其进行优化；根据摄像机与机器人的相对位置关系将摄像机的相对运动参数转化为机器人运动参数，并更新机器人运动的里程信息。本发明的双目视觉里程计不依赖于对轮轴旋转的计数，从而避免了车轮半径变化、空转、打滑等对里程计测量精度的影响。
文档编号G01C22/00GK102519481SQ20111044992
公开日2012年6月27日 申请日期2011年12月29日 优先权日2011年12月29日
发明者刘宏林, 李成荣, 罗杨宇 申请人:中国科学院自动化研究所