基于单目视觉和里程计组合的移动定位方法和装置制造方法及图纸

本发明专利技术公开了基于单目视觉和里程计组合的移动定位方法和装置。该方法通过对连续的地面纹理图像的模板匹配和SURF特征点匹配分别计算出两个位姿评估数据，然后对两个位姿评估数据进行卡尔曼滤波后得到视觉位姿评估数据，最后将视觉位姿评估数据和里程计测得的位姿数据进行加权融合得到估算的位姿数据。和里程计测得的位姿数据融合时，通过对数据误差评估动态调节加权系数。本发明专利技术减轻了单一视觉位姿解算的缺陷，从而减少了视觉位姿解算的误差，融合里程计时，动态调整加权系数，从而减少了纯粹采用视觉位姿估算或者纯粹采用里程计位姿测量数据的缺陷误差，提高了整体定位的准确度。度。度。

【技术实现步骤摘要】
基于单目视觉和里程计组合的移动定位方法和装置


[0001]本专利技术涉及移动定位，尤其涉及机器人或者无人小车的移动定位。

技术介绍

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

技术实现思路

[0004]本专利技术所要解决的问题：现有技术下，里程计和视觉里程计定位缺陷。
[0005]为解决上述问题，本专利技术采用的方案如下：根据本专利技术的基于单目视觉和里程计组合的移动定位方法，该方法包括如下步骤：步骤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),本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.基于单目视觉和里程计组合的移动定位方法，其特征在于，该方法包括如下步骤：步骤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]。2.根据权利要求1所述的基于单目视觉和里程计组合的移动定位方法，其特征在于，融合比例系数a(D(k),h)根据如下方式确定：若D(k)小于等于0，则a(D(k),h)取值0.5，否则a(D(k),h)为关于h的递增函数。3.基于...

【专利技术属性】
技术研发人员：曾庆喜，欧邦俊，陈斌华，郑宇宏，于浩楠，赵昊，刘雁云，
申请(专利权)人：江苏科力机械有限公司，
类型：发明
国别省市：