基于单目视觉和里程计组合的移动定位方法和装置与流程

1.本发明涉及移动定位，尤其涉及机器人或者无人小车的移动定位。


背景技术：

2.移动定位技术是机器人或无人小车自主导航系统的核心。在室外，移动定位可以基于卫星导航系统。但在室内，卫星信号受建筑阻挡，卫星导航系统无法正常工作。故此，里程计被广泛应用于机器人或无人小车的移动定位。里程计通过对滚轮或者履带等移动装置的测量获得位姿变换数据，进而计算出当前所在位置。但由于滚轮或者履带存在打滑的问题，一旦滚轮或者履带存在打滑，通过里程计获得的位姿变换数据不准确，进而导致定位不准。
3.近年来随着计算机视觉技术的不断发展，视觉定位技术成为了目前移动机器人的研究热点。视觉定位技术的现有技术有很多种。视觉里程计是视觉定位技术的一个分支。现有技术下，视觉里程计通常由相机拍摄三维空间内的图像，然后通过构建三维光场、深度图，匹配三维空间内的特征点位置进行位姿变化的计算。但在室内情况下，可能存在无法构建三维深空场的问题。比如，相机朝向墙面时，相机所拍摄的图像就是墙面的一部分，此时，无法构建深度图，也缺少可匹配的特征点。


技术实现要素：

4.本发明所要解决的问题：现有技术下，里程计和视觉里程计定位缺陷。
5.为解决上述问题，本发明采用的方案如下：根据本发明的基于单目视觉和里程计组合的移动定位方法，该方法包括如下步骤：步骤s1，用于：连续接收单目相机所拍摄的地面纹理图像和里程计所测的位姿变换数据，并获取上一时刻的位姿数据、上一时刻估计协方差；步骤s2，用于：通过对连续接收单目相机所拍摄的地面纹理图像进行模板匹配算法的匹配得到第一位姿变换数据，再结合上一时刻的位姿数据得到当前第一位姿评估数据，并计算模板图像熵值；通过对连续接收单目相机所拍摄的地面纹理图像进行surf特征点匹配算法进行匹配得到第二位姿变换数据，再结合上一时刻的位姿数据得到当前第二位姿评估数据；所述模板图像熵值采用如下公式计算：h＝－∑p(i)log(p(i))；其中，p(i)＝n(i)/nt；其中，n(i)表示模板图像中灰度值为i的像素点数，nt为模板图像的像素总数，h为模板图像熵值，log为对数函数；步骤s3，用于：将所述当前第一位姿评估数据和当前第二位姿评估数据进行融合得到当前视觉位姿评估数据，并计算当前时刻估计协方差；所述视觉位姿评估数据采用如下公式融合：xv(k)＝x1(k)＋k(k) (x2(k)－x1(k))；
当前时刻估计协方差采用如下公式计算：pp(k)＝(i－k(k))pr(k);其中，k(k)＝pr(k)/(pr(k)＋r)；pr(k)＝pp(k－1)＋q；其中，k表示当前时刻，k－1表示上一时刻；xv(k)为当前时刻的视觉位姿评估数据；x1(k)和x2(k)为当前时刻的第一位姿评估数据和第二位姿评估数据；pp(k－1)为上一时刻估计协方差；r和q为预先设定的矩阵；i为单位矩阵；步骤s4，用于：计算当前视觉位姿评估数据和里程计所测的位姿变换数据的差异量：d(k)＝ml(dxv(k)－dxm(k))－ml(dxv(k)＋dxm(k))*o_threshold；其中，dxv(k)为当前时刻的视觉位姿变换评估数据，dxv(k)＝xv(k)－x(k－1)；xv(k)为当前时刻的视觉位姿评估数据；x(k－1)为上一时刻的位姿数据；dxm(k)为当前时刻的里程计所测的位姿变换数据；o_threshold为预先设定的阈值；ml为向量取模运算；d(k)表示当前视觉位姿评估数据和里程计所测的位姿变换数据的差异量；步骤s5，用于：计算当前位姿数据：x(k)＝x(k－1)＋dx(k)；其中，dx(k)为当前位姿变换数据，dx(k)＝dxv(k)*a(d(k),h)＋dxm(k)*(1－a(d(k),h))；dxv(k)为当前时刻的视觉位姿变换评估数据，dxv(k)＝xv(k)－x(k－1)；xv(k)为当前时刻的视觉位姿评估数据；x(k－1)为上一时刻的位姿数据；x1(k)和x2(k)为当前时刻的第一位姿评估数据和第二位姿评估数据；dxm(k)为当前时刻的里程计所测的位姿变换数据；a(d(k),h)为根据当前视觉位姿评估数据和里程计所测的位姿变换数据的差异量d(k)和模板图像熵值h所确定的融合比例系数，a(d(k),h)∈[0,1]。
[0006]
进一步，根据本发明的基于单目视觉和里程计组合的移动定位方法，融合比例系数a(d(k),h)根据如下方式确定：若d(k)小于等于0，则a(d(k),h)取值0.5，否则a(d(k),h)为关于h的递增函数。
[0007]
根据本发明的基于单目视觉和里程计组合的移动定位装置，该装置包括如下模块：模块m1，用于：连续接收单目相机所拍摄的地面纹理图像和里程计所测的位姿变换数据，并获取上一时刻的位姿数据、上一时刻估计协方差；模块m2，用于：通过对连续接收单目相机所拍摄的地面纹理图像进行模板匹配算法的匹配得到第一位姿变换数据，再结合上一时刻的位姿数据得到当前第一位姿评估数
据，并计算模板图像熵值；通过对连续接收单目相机所拍摄的地面纹理图像进行surf特征点匹配算法进行匹配得到第二位姿变换数据，再结合上一时刻的位姿数据得到当前第二位姿评估数据；所述模板图像熵值采用如下公式计算：h＝－∑p(i)log(p(i))；其中，p(i)＝n(i)/nt；其中，n(i)表示模板图像中灰度值为i的像素点数，nt为模板图像的像素总数，h为模板图像熵值，log为对数函数；模块m3，用于：将所述当前第一位姿评估数据和当前第二位姿评估数据进行融合得到当前视觉位姿评估数据，并计算当前时刻估计协方差；所述视觉位姿评估数据采用如下公式融合：xv(k)＝x1(k)＋k(k) (x2(k)－x1(k))；当前时刻估计协方差采用如下公式计算：pp(k)＝(i－k(k))pr(k);其中，k(k)＝pr(k)/(pr(k)＋r)；pr(k)＝pp(k－1)＋q；其中，k表示当前时刻，k－1表示上一时刻；xv(k)为当前时刻的视觉位姿评估数据；x1(k)和x2(k)为当前时刻的第一位姿评估数据和第二位姿评估数据；pp(k－1)为上一时刻估计协方差；r和q为预先设定的矩阵；i为单位矩阵；模块m4，用于：计算当前视觉位姿评估数据和里程计所测的位姿变换数据的差异量：d(k)＝ml(dxv(k)－dxm(k))－ml(dxv(k)＋dxm(k))*o_threshold；其中，dxv(k)为当前时刻的视觉位姿变换评估数据，dxv(k)＝xv(k)－x(k－1)；xv(k)为当前时刻的视觉位姿评估数据；x(k－1)为上一时刻的位姿数据；dxm(k)为当前时刻的里程计所测的位姿变换数据；o_threshold为预先设定的阈值；ml为向量取模运算；d(k)表示当前视觉位姿评估数据和里程计所测的位姿变换数据的差异量；模块m5，用于：计算当前位姿数据：x(k)＝x(k－1)＋dx(k)；其中，dx(k)为当前位姿变换数据，dx(k)＝dxv(k)*a(d(k),h)＋dxm(k)*(1－a(d(k),h))；dxv(k)为当前时刻的视觉位姿变换评估数据，dxv(k)＝xv(k)－x(k－1)；xv(k)为当前时刻的视觉位姿评估数据；x(k－1)为上一时刻的位姿数据；x1(k)和x2(k)为当前时刻的第一位姿评估数据和第二位姿评估数据；dxm(k)为当前时刻的里程计所测的位姿变换数据；a(d(k),h)为根据当前视觉位姿评估数据和里程计所测的位姿变换数据的差异量
d(k)和模板图像熵值h所确定的融合比例系数，a(d(k),h)∈[0,1]。
[0008]
进一步，根据本发明的基于单目视觉和里程计组合的移动定位装置，融合比例系数a(d(k),h)根据如下方式确定：若d(k)小于等于0，则a(d(k),h)取值0.5，否则a(d(k),h)为关于h的递增函数。
[0009]
本发明的技术效果如下：1、本发明采用了两种视觉位姿的解算方法，通过卡尔曼滤波融合后形成视觉位姿数据，弥补单一视觉位姿解算的缺陷，从而减少视觉位姿解算的误差；2、视觉位姿解算和里程计通过加权系数进行融合，融合时判断哪一种位姿变换数据更优调整加权系数，从而减少整体位姿数据的误差。
附图说明
[0010]
图1是本发明实施例数据流图和流程图。
[0011]
图2是本发明实施例机器人小车的结构示意图。
具体实施方式
[0012]
下面结合附图对本发明做进一步详细说明。
[0013]
图2示例了一种机器人小车。该机器人小车内设置有电路板100，底部设置有滚轮200，前端设置有朝向地面的单目相机301。其中电路板100上设置有处理器101，滚轮200上连接有里程计201。里程计201和单目相机301连接处理器101。本实施例的机器人小车通过驱动滚轮200滚动实现在地面上的移动。与滚轮200对应的，里程计201为轮式里程计。本领域技术人员理解，若机器人小车采用其他移动装置，其对应的里程计201可能有所不同。本实施例的基于单目视觉和里程计组合的移动定位方法是通过处理器101执行存储于电路板100所设置的存储器上的计算机程序指令集所实现的方法。具体来说，处理器101连续获取单目相机301所拍摄的地面纹理图像和里程计201测量得到的位姿变换数据，然后经处理器101执行程序的分析，最终计算出当前机器人小车的位姿。当然该计算得到的位姿并非真实的位姿，而是估算的数据，存在误差。
[0014]
参照图1，处理器101通过执行程序实现移动定位的方法包括如下步骤：数据接收步骤、图像分析步骤、卡尔曼滤波步骤、差异量评估步骤以及视觉和里程计位姿融合步骤。
[0015]
数据接收步骤也就是前述步骤s1，连续接收单目相机所拍摄的地面纹理图像和里程计所测的位姿变换数据，并获取上一时刻的位姿数据、上一时刻估计协方差。其中，地面纹理图像通常是彩色的地面图像。具体到图1示例中，这里的单目相机即为单目相机301，里程计即为里程计201。这里的位姿变换数据是位姿数据两个时刻之间的变化量。具体来说，里程计所测的位姿变换数据是当前时刻和上一时刻之间的位姿数据的变化量。显而易见地，根据里程计所测的位姿变换数据和上一时刻之间的位姿数据，可以计算出里程计所测的位姿数据：xm(k)＝x(k－1)＋dxm(k)。其中，k表示当前时刻，k－1表示上一时刻，xm(k)和dxm(k)分别表示为里程计所测当前时刻的位姿数据和位姿变换数据，x(k－1)表示为上一时刻的位姿数据。这里的上一时刻的位姿数据和上一时刻估计协方差是存储于存储器的数据。本发明的移动定位方法是一个不断循环的步骤，方法中的数据接收步骤、图像分析步骤、卡尔曼滤波步骤、差异量评估步骤以及视觉和里程计位姿融合步骤组成了循环主体。循
环主体的每一轮执行，都会保存当前时刻的位姿数据和当前时刻估计协方差。循环主体的下一轮执行时，这所保存的当前时刻的位姿数据和当前时刻估计协方差就变成了上一时刻的位姿数据和上一时刻估计协方差。之所以需要保存位姿数据和估计协方差是因为这些数据会被循环主体的步骤所引用。
[0016]
此外，位姿数据是一个向量，可以表示为(px,py,pa)。其中，px,py,pa分别表示横纵坐标和朝向角。显而易见地，作为位姿数据变化量的位姿变换数据同样也是一个向量。
[0017]
图像分析步骤，也就是前述步骤s2，通过对连续接收单目相机所拍摄的地面纹理图像进行模板匹配算法的匹配得到第一位姿变换数据，再结合上一时刻的位姿数据得到当前第一位姿评估数据，并计算模板图像熵值；通过对连续接收单目相机所拍摄的地面纹理图像进行surf特征点匹配算法进行匹配得到第二位姿变换数据，再结合上一时刻的位姿数据得到当前第二位姿评估数据。具体来说，图像分析步骤可以分解为图1所示的三个步骤：模板匹配步骤、surf特征点匹配步骤以及模板图像熵计算步骤。模板匹配步骤，也就是，通过对连续接收单目相机所拍摄的地面纹理图像进行模板匹配算法的匹配得到第一位姿变换数据，再结合上一时刻的位姿数据得到当前第一位姿评估数据。surf特征点匹配步骤，也就是通过对连续接收单目相机所拍摄的地面纹理图像进行surf特征点匹配算法进行匹配得到第二位姿变换数据，再结合上一时刻的位姿数据得到当前第二位姿评估数据。图像模板匹配和surf特征点匹配为本领域技术人员所熟悉，本说明书无需赘述。模板图像熵计算步骤也即计算模板图像熵值。这里的熵值也就是本领域技术人员所熟悉的信息熵，计算公式为：h＝－∑p(i)log(p(i))。这里的p(i)为概率值，h为模板图像熵值，log为对数函数或对数运算。这里信息熵计算的对象为模板图像。模板图像来自于模板匹配步骤。模板匹配按照字面意思简单理解为在连续两个或多个地面纹理图像中找一个相同图像的区块。该相同图像的区块即为模板，或者模板图像，为模板图像熵计算的对象。由于地面纹理图像拍摄时刻不同，随着机器人小车的移动，该相同图像的区块在不同的地面纹理图像所在的位置和角度不相同，由此，解算出位置的变化量和角度的变化量。位置的变化量和角度的变化量的组合即为位姿变换数据。同理，surf特征点匹配首先得到的也是位姿变换数据。由此可以计算出第一位姿评估数据和第二位姿评估数据：x1(k)＝x(k－1)＋dx1(k)；x2(k)＝x(k－1)＋dx2(k)；其中，x1(k)和x2(k)分别表示为当前时刻的第一位姿评估数据和第二位姿评估数据，x(k－1)表示为上一时刻的位姿数据，dx1(k)和dx2(k)分别表示为当前时刻的第一位姿变换数据和第二位姿变换数据。
[0018]
此外，本实施例中，模板图像熵计算中的概率值p(i)为模板图像中灰度值为i的像素概率，可以表示为：p(i)＝n(i)/nt；其中，n(i)表示模板图像中灰度值为i的像素点数，nt为模板图像的像素总数，灰度值取值0~255，共计256个灰度值。显而易见地，若256个灰度值对应的概率值p(i)均取值1/256，则模板图像熵值h＝－∑p(i)log(p(i))＝－256*1/256* log(1/256)=log256，取值最大；若其中一个概率值p(i)取值为1，其他取值为0，则模板图像熵值h=0，取值最小。由此，模板图像熵值h的取值范围依赖于对数函数log的底。若对数函数log以2为底，则有log256=8，对应的模板图像熵值h取值范围为区间[0,8]；若对数函数log以256为底，则有log256=1，对应的模板图像熵值h取值范围为区间[0,1]。此外，模板图像熵
值h为0表示模板图像为单色图。
[0019]
卡尔曼滤波步骤，也就是前述步骤s3，将当前第一位姿评估数据和当前第二位姿评估数据进行融合得到当前视觉位姿评估数据，并计算当前时刻估计协方差。其中，视觉位姿评估数据采用如下公式融合：xv(k)＝x1(k)＋k(k) (x2(k)－x1(k))。当前时刻估计协方差采用如下计算公式计算：pp(k)＝(i－k(k))pr(k)。上述公式中，k(k)＝pr(k)/(pr(k)＋r)；pr(k)＝pp(k－1)＋q。上述四个公式分别由卡尔曼滤波器更新方程xv(k)＝x1(k)＋k(k)(x2(k)－h*x1(k))，pp(k)＝(i－k(k)*h)pr(k)，k(k)＝pr(k)*ht/(h*pr(k)*ht＋r)，以及pr(k)＝a*pp(k－1)*at＋q简化而来。其中，h为测量矩阵，ht为测量矩阵h的转置，a为状态转移矩阵，at为状态转移矩阵的转置。当上述卡尔曼滤波器更新方程中，测量矩阵h和状态转移矩阵a均取值为单位矩阵时，即可得到本发明步骤中的各计算公式。
[0020]
上述公式中，i为单位矩阵；xv(k)为当前时刻的视觉位姿评估数据；k(k)即为卡尔曼增益；pp(k－1)和pp(k)分别为上一时刻和当前时刻的估计协方差，对应卡尔曼滤波中的后验估计协方差；pr(k)对应卡尔曼滤波中的先验估计协方差；r和q为预先设定的矩阵，分别对应卡尔曼滤波中的测量噪声协方差和过程噪声协方差。
[0021]
本步骤输出的是当前视觉位姿评估数据xv(k)和当前时刻估计协方差pp(k)。当前时刻估计协方差pp(k)保存后即为下一循环步骤s1中的上一时刻的估计协方差pp(k－1)。先验估计协方差pr(k)和卡尔曼增益k(k)为中间数据。测量噪声协方差r和过程噪声协方差q根据事先实验数据预先设定。
[0022]
此外，上述视觉位姿评估数据的融合公式也可以表示为：xv(k)＝k(k)* x2(k)＋(i－k(k))x1(k)。若将其中的k(k)和i－k(k)分别看作是两个权重系数，则视觉位姿评估数据可以看作是第一位姿评估数据和第二位姿评估数据的加权平均。若k(k)为0，则xv(k)＝x1(k)；若k(k)为i，则xv(k)＝x2(k)。
[0023]
差异量评估步骤即为前述步骤s4，计算当前视觉位姿评估数据和里程计所测的位姿变换数据的差异量。差异量的计算采用如下计算公式：d(k)＝ml(dxv(k)－dxm(k))－ml(dxv(k)＋dxm(k))*o_threshold。
[0024]
其中，dxv(k)为当前时刻的视觉位姿变换数据，来自于当前视觉位姿评估数据xv(k)计算得到，具体为：dxv(k)＝xv(k)－x(k－1)；o_threshold为预先设定的阈值；ml为向量取模运算；d(k)表示当前视觉位姿评估数据和里程计所测的位姿变换数据的差异量。
[0025]
视觉和里程计位姿融合步骤即为前述步骤s5，计算当前位姿数据。当前位姿数据采用如下公式计算：x(k)＝x(k－1)＋dx(k)。其中，x(k)为当前时刻的位姿数据；dx(k)为当前位姿变换数据，由前述当前时刻的视觉位姿变换评估数据dxv(k)和当前时刻的里程计所测的位姿变换数据dxm(k)融合而来。当前位姿变换数据dx(k)采用如下公式融合计算得到：dx(k)＝dxv(k)*a(d(k),h)＋dxm(k)*(1－a(d(k),h))。其中，a(d(k),h)是融合比例系数，由前述当前视觉位姿评估数据和里程计所测的位姿变换数据的差异量d(k)和前述模板图像熵值h所确定取值范围为区间[0,1]。上述当前位姿变换数据dx(k)的融合计算公式中，若将a(d(k),h)和(1－a(d(k),h))分别看作两个加权系数，则上述当前位姿变换数据dx(k)的融合计算公式实质为当前时刻的视觉位姿变换评估数据dxv(k)和当前时刻的里程计所测的位姿变换数据dxm(k)的加权平均。若融合比例系数a(d(k),h)取值为0，则x(k)＝x(k－1)＋dxm(k)；若融合比例系数a(d(k),h)取值为1，则x(k)＝x(k－1)＋dxv(k)＝xv(k)；
本实施例中，融合比例系数a(d(k),h)根据如下方式确定：若d(k)小于等于0，则a(d(k),h)取值0.5；否则a(d(k),h)为关于h的递增函数。最为简单的情况下，若前述模板图像熵值h计算公式中对数函数log以256为底，模板图像熵值h取值0～1，此时，a(d(k),h)可以直接等于h。又或者，预先设定一个阈值hc，0《hc《log256；当模板图像熵值h为0~hc时，a(d(k),h)＝h*0.5/hc；当模板图像熵值h为hc~log256时，a(d(k),h)＝(0.5h＋0.5log256－hc)/(log256－hc)。这里的log256是模板图像熵值h的最大值。在上述公式下，若模板图像熵值h取值为0，此时模板图像为单色图案，也就是地面为纯粹的白色或者其他颜色，此种状态下，视觉里程计数据实际无效，也就是计算得到的视觉位姿变换评估数据dxv(k)是无效数据，此时，融合比例系数a(d(k),h)为0，最终得到的位姿数据为x(k)＝x(k－1)＋dxm(k)，也就是以里程计所测的位姿变换数据dxm(k)作为最终位姿数据。若模板图像熵值h取值最大值，表明计算得到的视觉位姿变换评估数据dxv(k)准确度比较高，里程计所测的位姿变换数据dxm(k)可能存在打滑的问题，此时，最终得到的位姿数据为x(k)＝x(k－1)＋dxv(k)＝xv(k)。
[0026]
此外，还需要指出的是，本发明前述装置与方法对应，装置中的模块与方法中的步骤相对应，不再赘述。