(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210984973.8 (22)申请日 2022.08.17 (66)本国优先权数据 202210660452.7 202 2.06.13 CN (71)申请人 南京航空航天大 学 地址 210007 江苏省南京市秦淮区御道街 29号 申请人 江苏科力机械有限公司 (72)发明人 曾庆喜　欧邦俊　陈斌华　郑宇宏　 于浩楠　赵昊　刘雁云　 (74)专利代理 机构 江苏银创律师事务所 32 242 专利代理师 孙计良 (51)Int.Cl. G06T 7/77(2017.01) G06T 7/73(2017.01)G06V 10/75(2022.01) G06F 17/16(2006.01) G06F 17/18(2006.01) (54)发明名称 基于单目视觉和里程计组合的移动定位方 法和装置 (57)摘要 本发明公开了基于单目视觉和里程计组合 的移动定位方法和装置。 该方法通过对连续的地 面纹理图像的模板匹配和SURF特征点匹配分别 计算出两个位姿评估数据， 然后对两个位姿评估 数据进行卡尔曼滤波后得到视觉位姿评估数据， 最后将视觉位姿评估数据和里程计测得的位姿 数据进行加权融合得到估算的位姿数据。 和里程 计测得的位姿数据融合时， 通过对 数据误差评估 动态调节加权系数。 本发明减 轻了单一视觉位姿 解算的缺陷， 从而减少了视觉位姿解算的误差， 融合里程计时， 动态调整加权系数， 从而减少了 纯粹采用视觉位姿估算或者纯粹采用里程计位 姿测量数据的缺陷误差， 提高了整体定位的准确 度。 权利要求书3页 说明书7页 附图1页 CN 115330879 A 2022.11.11 CN 115330879 A 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(dx v(k)－dxm(k) )－ml(dx v(k)＋dxm(k) )*o_thresho ld； 其中， dxv(k)为当前时刻的视 觉位姿变换评估数据， dx v(k)＝xv(k)－x(k －1)； xv(k)为当前时刻的视 觉位姿评估数据； x(k －1)为上一时刻的位姿数据； dxm(k)为当前时刻的里程计所测的位姿变换 数据； o_thresho ld为预先设定的阈值； 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)为当前时刻的视 觉位姿变换评估数据， dx v(k)＝xv(k)－x(k －1)； xv(k)为当前时刻的视 觉位姿评估数据；权　利　要　求　书 1/3 页 2 CN 115330879 A 2x(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.基于单目视 觉和里程计组合的移动定位装置， 其特 征在于， 该装置包括如下模块： 模块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(dx v(k)－dxm(k) )－ml(dx v(k)＋dxm(k) )*o_thresho ld； 其中， dxv(k)为当前时刻的视 觉位姿变换评估数据， dx v(k)＝xv(k)－x(k －1)； xv(k)为当前时刻的视 觉位姿评估数据； x(k －1)为上一时刻的位姿数据； dxm(k)为当前时刻的里程计所测的位姿变换 数据；权　利　要　求　书 2/3 页 3 CN 115330879 A 3