(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210472832.8 (22)申请日 2022.04.29 (71)申请人 南京航空航天大 学 地址 210016 江苏省南京市秦淮区御道街 29号 申请人 江苏科力机械有限公司 (72)发明人 曾庆喜　马鑫烨　陈斌华　常婷婷　 欧邦俊　俞建浩　胡义轩　 (74)专利代理 机构 南京经纬专利商标代理有限 公司 32200 专利代理师 姜慧勤 (51)Int.Cl. G01C 21/16(2006.01) G01C 21/20(2006.01) G01C 22/00(2006.01)G06K 9/62(2022.01) G06T 7/73(2017.01) G06V 10/74(2022.01) G06V 10/77(2022.01) G06V 10/82(2022.01) (54)发明名称 基于EKF的模板匹配VO与轮式里程计融合定 位方法 (57)摘要 本发明公开了基于EKF的模板匹配VO与轮式 里程计融合定位方法， 包括： 创建地面图像数据 集， 为地面图像数据集添加地形复杂度作为标 签； 利用地面图像数据集训练卷积神经网络模 型； 采集地面图像输入训练好的卷积神经网络模 型中， 得到当前地面的地形复杂度； 计算当前地 面的图像熵； 根据地形复杂度和图像熵， 计算当 前地面的模板选取策略， 根据模板选取策略进行 模板匹配VO估算， 得到位姿结果； 将轮式里程计 估算出的位姿和经过惯性测量单元解算出的位 姿通过扩展卡尔曼滤波 方法进行融合， 将融合后 得到的位姿作为次级滤波器的预测值， 结合模板 匹配VO估算出的位姿结果， 得到最终的机器人定 位结果， 提高定位结果的稳定性， 实现更精 准、 更 鲁棒的定位。 权利要求书2页 说明书8页 附图2页 CN 114993298 A 2022.09.02 CN 114993298 A 1.基于EKF的模板匹配VO与轮式里程计融合定位方法， 其特 征在于， 包括如下步骤： 步骤1， 采集不同类型地面的图像数据， 创建地面图像数据集， 为地面图像数据集中的 每幅图像添加地形复杂度作为标签； 步骤2， 利用添加标签后的地面图像数据集训练卷积神经网络模型， 得到训练好的卷积 神经网络模型； 步骤3， 利用机器人上安装的摄像头采集地面图像输入训练好的卷积神经网络模型中， 得到当前地 面的地形复杂度； 步骤4， 计算当前地 面的图像熵； 步骤5， 根据地形复杂度和图像熵， 实时计算当前地面的模板选取策略， 根据模板选取 策略进行模板匹配VO估算， 得到位姿结果； 步骤6， 将轮式里程计估算出的位姿和经过惯性测量单元解算出的位姿通过扩展卡尔 曼滤波方法即初级滤波器进行融合， 将融合后得到的位姿作为次级滤波器的预测值， 结合 模板匹配VO估算出的位姿结果作为次级滤波器的观测值， 得到最终的机器人定位结果。 2.根据权利要求1所述的基于EKF的模板匹配VO与轮 式里程计 融合定位方法， 其特征在 于， 步骤1所述 地形复杂度的决定因素包括 地面材质以及颠簸 程度。 3.根据权利要求1所述的基于EKF的模板匹配VO与轮 式里程计 融合定位方法， 其特征在 于， 步骤4所述图像熵的计算公式如下： 其中， S表示图像熵， pj表示灰度值为j的像素点在图像中出现的概率， nj表示灰度值为j 的像素点的数量， Tw和Th分别为图像的宽和高。 4.根据权利要求1所述的基于EKF的模板匹配VO与轮 式里程计 融合定位方法， 其特征在 于， 步骤5所述 根据地形复杂度和图像熵， 实时计算当前地 面的模板 选取策略， 具体如下： 预先设置地形复杂度阈值E和图像熵阈值F， 对于大小为640*480像素尺寸的第i帧地面 图像， 根据步骤3和步骤4得到第i帧地面图像的地形复杂度ρi和图像熵Si， 若ρi小于E且Si小 于F， 则在地面图像中初始化 大小为240 ×240像素尺寸的模 板； 若ρi大于E且Si小于F， 则在地 面图像中初始化 大小为180 ×180像素尺寸的模 板； 若ρi小于E且Si大于F， 则在地面图像中初 始化大小为200 ×200像素尺寸的模 板； 若ρi大于E且Si大于F， 则在地面图像中初始化大小为 160×160像素尺寸的模板； 模板的中心与地 面图像的中心重合。 5.根据权利要求4所述的基于EKF的模板匹配VO与轮 式里程计 融合定位方法， 其特征在 于， 步骤5所述 根据模板 选取策略进行模板匹配VO估算， 得到位姿结果， 具体如下： 1)根据模板 选取策略在第i帧地 面图像中初始化模板a； 2)利用归一化互相关匹配方法在第i+1 帧地面图像中从左到右、 从上到下搜索与模板a 相似度最大的匹配区域b， 匹配区域b的大小与模板a大小相同； 3)根据匹配区域b与模板a的左上角像素位置计算像素位移增量Δu和Δv； 4)将像素位移增量Δu和Δv转换到相机在世界物理坐标系中的真实位移增量；权　利　要　求　书 1/2 页 2 CN 114993298 A 25)根据真实位移增量 解算得到i+1时刻模板匹配VO估算的机器人位姿结果； 6)根据模板选取策略在第i+1 帧地面图像 中初始化模板并重 复上述过程， 得到i+2时刻 模板匹配VO估算的机器人位姿结果， 以此类 推。 6.根据权利要求1所述的基于EKF的模板匹配VO与轮 式里程计 融合定位方法， 其特征在 于， 所述惯性测量单 元的观测模型如下： 惯性测量单 元的姿态更新方程 为： 其中， 和 分别表示当前t时刻和上一时刻机器人坐标系到导航坐标系的姿态变 换矩阵； 表示当前时刻与上一时刻之间的相对旋转构成的反对称矩阵； 惯性测量单 元在导航坐标系下的速度和位置分别为： 其中， vt表示机器人当前时刻速度； vt‑1表示机器人上一时刻速度； at表示IMU测量到的 当前时刻加速度； at‑1表示IMU测量到的上一时刻加速度； Pt表示机器人当前时刻位置信息； Pt‑1表示机器人 上一时刻位置信息； Δt为当前时刻与上一时刻之间的时间差 。 7.根据权利要求1所述的基于EKF的模板匹配VO与轮 式里程计 融合定位方法， 其特征在 于， 所述扩展卡尔曼滤波方法根据系统的运动模型和观测模型实现预测和更新两个步骤， 运动模型和观测模型分别为： xt＝Gtxt‑1+wt zt＝Htxt+Vt 其中， xt为当前t时刻系统状态矩阵； xt‑1为上一时刻系统状态矩阵； Gt为当前时刻系统 状态传递矩阵； zt为当前时刻系统的观测值矩阵； Ht为系统当前时刻状态量与观测值之间的 传递矩阵； wt和Vt分别为当前时刻系统的过程噪声矩阵和观测噪声矩阵； 预测过程： 其中， 为当前时刻系统状态矩阵估计值； 为当前时刻系统的状态估计协方差； Rt为 当前时刻系统的过程噪声协方差； ∑t‑1为上一时刻系统状态 协方差； 更新过程： 计算卡尔曼增益Kt： 其中， Qt为观测值 误差协方差矩阵； 利用Kt和观测值更新移动机器人的状态 矩阵xt和系统协方差矩阵： 其中， I为单位矩阵。权　利　要　求　书 2/2 页 3 CN 114993298 A 3