基于EKF的模板匹配VO与轮式里程计融合定位方法技术

本发明专利技术公开了基于EKF的模板匹配VO与轮式里程计融合定位方法，包括：创建地面图像数据集，为地面图像数据集添加地形复杂度作为标签；利用地面图像数据集训练卷积神经网络模型；采集地面图像输入训练好的卷积神经网络模型中，得到当前地面的地形复杂度；计算当前地面的图像熵；根据地形复杂度和图像熵，计算当前地面的模板选取策略，根据模板选取策略进行模板匹配VO估算，得到位姿结果；将轮式里程计估算出的位姿和经过惯性测量单元解算出的位姿通过扩展卡尔曼滤波方法进行融合，将融合后得到的位姿作为次级滤波器的预测值，结合模板匹配VO估算出的位姿结果，得到最终的机器人定位结果，提高定位结果的稳定性，实现更精准、更鲁棒的定位。鲁棒的定位。鲁棒的定位。

【技术实现步骤摘要】
基于EKF的模板匹配VO与轮式里程计融合定位方法


[0001]本专利技术涉及基于EKF的模板匹配VO与轮式里程计融合定位方法，属于机器人定位


技术介绍

[0002]近年来，轮式机器人在日常生活、工业生产等多个领域广泛应用，比如巡检、物流、施工作业等。但随着轮式机器人应用领域的不断发展以及其应用场景的逐渐复杂，不可避免地会对机器人的环境适应性提出更高的要求。因此轮式机器人如何在不同的场景中实现高效且准确的定位已成为研究的热点和难点。
[0003]SLAM(Simultaneous Localization And Mapping，即时定位与地图构建)主要用于解决移动机器人在运动中定位导航与地图构建的问题。Gmapping是一种基于2D激光雷达完成二维栅格地图构建的SLAM算法。该算法可以实时构建室内地图，在构建小场景地图所需的计算量较小且精度较高。
[0004]目前应用较广泛的自定位技术包括轮式里程计、惯性里程计、视觉里程计等，它们都有各自的优缺点，如轮式里程计直接通过轮式编码器的输出对机器人的位姿进行航迹推算，在地形颠簸或车轮打滑的情况下存在一定的误差；惯性里程计不受外界环境干扰，但其误差会随着时间累积；视觉里程计受环境光影响较大，主要分为基于特征和外观，基于特征的方法适用于纹理丰富的场景，例如具有大量特征点的城市环境。这种方法无法处理单一模式的无纹理或低纹理环境(例如沙土、沥青、混凝土等)，因为在这些环境中无法检测到足够的显著特征，相比之下，基于外观的模板匹配方法非常适合在低纹理场景中使用。
[0005]基于模板匹配的视觉里程计(Visual Odometry，VO)系统在车辆的定位系统中主要的优点是其仅需要通过相机来实现，且不需要场景、运动姿态等先验知识，且能够适应车辆的打滑、侧移以及非平地运动等容易导致轮式里程计产生误差的问题，且能够在低纹理的场景中依然具备出色的场景识别功能。但其局限主要体现在于模板匹配的过程中模板的选取严重的影响着算法的效果，如何最优的选取模板成为了决定了模板匹配VO算法优劣的关键。

技术实现思路

[0006]本专利技术所要解决的技术问题是：提供基于EKF的模板匹配VO与轮式里程计融合定位方法，在里程计坐标系下将基于CNN和图像熵改进的模板匹配的视觉定位结果与轮式里程计进行融合，得到更精准、更鲁棒的定位结果。
[0007]本专利技术为解决上述技术问题采用以下技术方案：
[0008]基于EKF的模板匹配VO与轮式里程计融合定位方法，包括如下步骤：
[0009]步骤1，采集不同类型地面的图像数据，创建地面图像数据集，为地面图像数据集中的每幅图像添加地形复杂度作为标签；
[0010]步骤2，利用添加标签后的地面图像数据集训练卷积神经网络模型，得到训练好的
卷积神经网络模型；
[0011]步骤3，利用机器人上安装的摄像头采集地面图像输入训练好的卷积神经网络模型中，得到当前地面的地形复杂度；
[0012]步骤4，计算当前地面的图像熵；
[0013]步骤5，根据地形复杂度和图像熵，实时计算当前地面的模板选取策略，根据模板选取策略进行模板匹配VO估算，得到位姿结果；
[0014]步骤6，将轮式里程计估算出的位姿和经过惯性测量单元解算出的位姿通过扩展卡尔曼滤波方法即初级滤波器进行融合，将融合后得到的位姿作为次级滤波器的预测值，结合模板匹配VO估算出的位姿结果作为次级滤波器的观测值，得到最终的机器人定位结果。
[0015]作为本专利技术的一种优选方案，步骤1所述地形复杂度的决定因素包括地面材质以及颠簸程度。
[0016]作为本专利技术的一种优选方案，步骤4所述图像熵的计算公式如下：
[0017][0018][0019]其中，S表示图像熵，p
j
表示灰度值为j的像素点在图像中出现的概率，n
j
表示灰度值为j的像素点的数量，T
w
和T
h
分别为图像的宽和高。
[0020]作为本专利技术的一种优选方案，步骤5所述根据地形复杂度和图像熵，实时计算当前地面的模板选取策略，具体如下：
[0021]预先设置地形复杂度阈值E和图像熵阈值F，对于大小为640*480像素尺寸的第i帧地面图像，根据步骤3和步骤4得到第i帧地面图像的地形复杂度ρ
i
和图像熵S
i
，若ρ
i
小于E且S
i
小于F，则在地面图像中初始化大小为240
×
240像素尺寸的模板；若ρ
i
大于E且S
i
小于F，则在地面图像中初始化大小为180
×
180像素尺寸的模板；若ρ
i
小于E且S
i
大于F，则在地面图像中初始化大小为200
×
200像素尺寸的模板；若ρ
i
大于E且S
i
大于F，则在地面图像中初始化大小为160
×
160像素尺寸的模板；模板的中心与地面图像的中心重合。
[0022]作为本专利技术的一种优选方案，步骤5所述根据模板选取策略进行模板匹配VO估算，得到位姿结果，具体如下：
[0023]1)根据模板选取策略在第i帧地面图像中初始化模板a；
[0024]2)利用归一化互相关匹配方法在第i+1帧地面图像中从左到右、从上到下搜索与模板a相似度最大的匹配区域b，匹配区域b的大小与模板a大小相同；
[0025]3)根据匹配区域b与模板a的左上角像素位置计算像素位移增量Δu和Δv；
[0026]4)将像素位移增量Δu和Δv转换到相机在世界物理坐标系中的真实位移增量；
[0027]5)根据真实位移增量解算得到i+1时刻模板匹配VO估算的机器人位姿结果；
[0028]6)根据模板选取策略在第i+1帧地面图像中初始化模板并重复上述过程，得到i+2时刻模板匹配VO估算的机器人位姿结果，以此类推。
[0029]作为本专利技术的一种优选方案，所述惯性测量单元的观测模型如下：
[0030]惯性测量单元的姿态更新方程为：
[0031][0032]其中，和分别表示当前t时刻和上一时刻机器人坐标系到导航坐标系的姿态变换矩阵；表示当前时刻与上一时刻之间的相对旋转构成的反对称矩阵；
[0033]惯性测量单元在导航坐标系下的速度和位置分别为：
[0034][0035][0036]其中，v
t
表示机器人当前时刻速度；v
t
‑1表示机器人上一时刻速度；a
t
表示IMU测量到的当前时刻加速度；a
t
‑1表示IMU测量到的上一时刻加速度；P
t
表示机器人当前时刻位置信息；P
t
‑1表示机器人上一时刻位置信息；Δt为当前时刻与上一时刻之间的时间差。
[0037]作为本专利技术的一本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.基于EKF的模板匹配VO与轮式里程计融合定位方法，其特征在于，包括如下步骤：步骤1，采集不同类型地面的图像数据，创建地面图像数据集，为地面图像数据集中的每幅图像添加地形复杂度作为标签；步骤2，利用添加标签后的地面图像数据集训练卷积神经网络模型，得到训练好的卷积神经网络模型；步骤3，利用机器人上安装的摄像头采集地面图像输入训练好的卷积神经网络模型中，得到当前地面的地形复杂度；步骤4，计算当前地面的图像熵；步骤5，根据地形复杂度和图像熵，实时计算当前地面的模板选取策略，根据模板选取策略进行模板匹配VO估算，得到位姿结果；步骤6，将轮式里程计估算出的位姿和经过惯性测量单元解算出的位姿通过扩展卡尔曼滤波方法即初级滤波器进行融合，将融合后得到的位姿作为次级滤波器的预测值，结合模板匹配VO估算出的位姿结果作为次级滤波器的观测值，得到最终的机器人定位结果。2.根据权利要求1所述的基于EKF的模板匹配VO与轮式里程计融合定位方法，其特征在于，步骤1所述地形复杂度的决定因素包括地面材质以及颠簸程度。3.根据权利要求1所述的基于EKF的模板匹配VO与轮式里程计融合定位方法，其特征在于，步骤4所述图像熵的计算公式如下：于，步骤4所述图像熵的计算公式如下：其中，S表示图像熵，p
j
表示灰度值为j的像素点在图像中出现的概率，n
j
表示灰度值为j的像素点的数量，T
w
和T
h
分别为图像的宽和高。4.根据权利要求1所述的基于EKF的模板匹配VO与轮式里程计融合定位方法，其特征在于，步骤5所述根据地形复杂度和图像熵，实时计算当前地面的模板选取策略，具体如下：预先设置地形复杂度阈值E和图像熵阈值F，对于大小为640*480像素尺寸的第i帧地面图像，根据步骤3和步骤4得到第i帧地面图像的地形复杂度ρ
i
和图像熵S
i
，若ρ
i
小于E且S
i
小于F，则在地面图像中初始化大小为240
×
240像素尺寸的模板；若ρ
i
大于E且S
i
小于F，则在地面图像中初始化大小为180
×
180像素尺寸的模板；若ρ
i
小于E且S
i
大于F，则在地面图像中初始化大小为200
×
200像素尺寸的模板；若ρ
i
大于E且S
i
大于F，则在地面图像中初始化大小为160
×
160像素尺寸的模板；模板的中心与地面图像的中心重合。5.根据权利要求4所述的基于EKF的模板匹配VO与轮式里程计融合定位方法，其特征在于，步骤5所述根据模板选取策略进行模板匹配VO估算，得到位姿结果，具体如下：1)根据模板选取策略在第i帧地面图像中初始化模板a；2)利用归一化互相关匹配方法在第i+1帧地面图像中...

【专利技术属性】
技术研发人员：曾庆喜，马鑫烨，陈斌华，常婷婷，欧邦俊，俞建浩，胡义轩，
申请(专利权)人：江苏科力机械有限公司，
类型：发明
国别省市：