CN114993298A - 基于ekf的模板匹配vo与轮式里程计融合定位方法 - Google Patents

基于ekf的模板匹配vo与轮式里程计融合定位方法 Download PDF

Info

Publication number
CN114993298A
CN114993298A CN202210472832.8A CN202210472832A CN114993298A CN 114993298 A CN114993298 A CN 114993298A CN 202210472832 A CN202210472832 A CN 202210472832A CN 114993298 A CN114993298 A CN 114993298A
Authority
CN
China
Prior art keywords
template
ground
image
matching
robot
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202210472832.8A
Other languages
English (en)
Inventor
曾庆喜
马鑫烨
陈斌华
常婷婷
欧邦俊
俞建浩
胡义轩
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Jiangsu King Lift Machinery Co ltd
Nanjing University of Aeronautics and Astronautics
Original Assignee
Jiangsu King Lift Machinery Co ltd
Nanjing University of Aeronautics and Astronautics
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Jiangsu King Lift Machinery Co ltd, Nanjing University of Aeronautics and Astronautics filed Critical Jiangsu King Lift Machinery Co ltd
Priority to CN202210472832.8A priority Critical patent/CN114993298A/zh
Publication of CN114993298A publication Critical patent/CN114993298A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G06T7/74—Determining position or orientation of objects or cameras using feature-based methods involving reference images or patches
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/74—Image or video pattern matching; Proximity measures in feature spaces
    • G06V10/761—Proximity, similarity or dissimilarity measures
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/77—Processing image or video features in feature spaces; using data integration or data reduction, e.g. principal component analysis [PCA] or independent component analysis [ICA] or self-organising maps [SOM]; Blind source separation
    • G06V10/7715—Feature extraction, e.g. by transforming the feature space, e.g. multi-dimensional scaling [MDS]; Mappings, e.g. subspace methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/82—Arrangements for image or video recognition or understanding using pattern recognition or machine learning using neural networks
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20081—Training; Learning
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20084—Artificial neural networks [ANN]
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30248—Vehicle exterior or interior
    • G06T2207/30252—Vehicle exterior; Vicinity of vehicle
    • G06T2207/30256—Lane; Road marking

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Computation (AREA)
  • Computing Systems (AREA)
  • Software Systems (AREA)
  • Medical Informatics (AREA)
  • General Health & Medical Sciences (AREA)
  • Databases & Information Systems (AREA)
  • Multimedia (AREA)
  • Artificial Intelligence (AREA)
  • Health & Medical Sciences (AREA)
  • Automation & Control Theory (AREA)
  • Image Analysis (AREA)

Abstract

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

Description

基于EKF的模板匹配VO与轮式里程计融合定位方法
技术领域
本发明涉及基于EKF的模板匹配VO与轮式里程计融合定位方法，属于机器人定位技术领域。
背景技术
近年来，轮式机器人在日常生活、工业生产等多个领域广泛应用，比如巡检、物流、施工作业等。但随着轮式机器人应用领域的不断发展以及其应用场景的逐渐复杂，不可避免地会对机器人的环境适应性提出更高的要求。因此轮式机器人如何在不同的场景中实现高效且准确的定位已成为研究的热点和难点。
SLAM(Simultaneous Localization And Mapping，即时定位与地图构建)主要用于解决移动机器人在运动中定位导航与地图构建的问题。Gmapping是一种基于2D激光雷达完成二维栅格地图构建的SLAM算法。该算法可以实时构建室内地图，在构建小场景地图所需的计算量较小且精度较高。
目前应用较广泛的自定位技术包括轮式里程计、惯性里程计、视觉里程计等，它们都有各自的优缺点，如轮式里程计直接通过轮式编码器的输出对机器人的位姿进行航迹推算，在地形颠簸或车轮打滑的情况下存在一定的误差；惯性里程计不受外界环境干扰，但其误差会随着时间累积；视觉里程计受环境光影响较大，主要分为基于特征和外观，基于特征的方法适用于纹理丰富的场景，例如具有大量特征点的城市环境。这种方法无法处理单一模式的无纹理或低纹理环境(例如沙土、沥青、混凝土等)，因为在这些环境中无法检测到足够的显著特征，相比之下，基于外观的模板匹配方法非常适合在低纹理场景中使用。
基于模板匹配的视觉里程计(Visual Odometry，VO)系统在车辆的定位系统中主要的优点是其仅需要通过相机来实现，且不需要场景、运动姿态等先验知识，且能够适应车辆的打滑、侧移以及非平地运动等容易导致轮式里程计产生误差的问题，且能够在低纹理的场景中依然具备出色的场景识别功能。但其局限主要体现在于模板匹配的过程中模板的选取严重的影响着算法的效果，如何最优的选取模板成为了决定了模板匹配VO算法优劣的关键。
发明内容
本发明所要解决的技术问题是：提供基于EKF的模板匹配VO与轮式里程计融合定位方法，在里程计坐标系下将基于CNN和图像熵改进的模板匹配的视觉定位结果与轮式里程计进行融合，得到更精准、更鲁棒的定位结果。
本发明为解决上述技术问题采用以下技术方案：
基于EKF的模板匹配VO与轮式里程计融合定位方法，包括如下步骤：
步骤1，采集不同类型地面的图像数据，创建地面图像数据集，为地面图像数据集中的每幅图像添加地形复杂度作为标签；
步骤2，利用添加标签后的地面图像数据集训练卷积神经网络模型，得到训练好的卷积神经网络模型；
步骤3，利用机器人上安装的摄像头采集地面图像输入训练好的卷积神经网络模型中，得到当前地面的地形复杂度；
步骤4，计算当前地面的图像熵；
步骤5，根据地形复杂度和图像熵，实时计算当前地面的模板选取策略，根据模板选取策略进行模板匹配VO估算，得到位姿结果；
步骤6，将轮式里程计估算出的位姿和经过惯性测量单元解算出的位姿通过扩展卡尔曼滤波方法即初级滤波器进行融合，将融合后得到的位姿作为次级滤波器的预测值，结合模板匹配VO估算出的位姿结果作为次级滤波器的观测值，得到最终的机器人定位结果。
作为本发明的一种优选方案，步骤1所述地形复杂度的决定因素包括地面材质以及颠簸程度。
作为本发明的一种优选方案，步骤4所述图像熵的计算公式如下：
Figure BDA0003623646740000021
Figure BDA0003623646740000031
其中，S表示图像熵，pj表示灰度值为j的像素点在图像中出现的概率，nj表示灰度值为j的像素点的数量，Tw和Th分别为图像的宽和高。
作为本发明的一种优选方案，步骤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所述根据模板选取策略进行模板匹配VO估算，得到位姿结果，具体如下：
1)根据模板选取策略在第i帧地面图像中初始化模板a；
2)利用归一化互相关匹配方法在第i+1帧地面图像中从左到右、从上到下搜索与模板a相似度最大的匹配区域b，匹配区域b的大小与模板a大小相同；
3)根据匹配区域b与模板a的左上角像素位置计算像素位移增量Δu和Δv；
4)将像素位移增量Δu和Δv转换到相机在世界物理坐标系中的真实位移增量；
5)根据真实位移增量解算得到i+1时刻模板匹配VO估算的机器人位姿结果；
6)根据模板选取策略在第i+1帧地面图像中初始化模板并重复上述过程，得到i+2时刻模板匹配VO估算的机器人位姿结果，以此类推。
作为本发明的一种优选方案，所述惯性测量单元的观测模型如下：
惯性测量单元的姿态更新方程为：
Figure BDA0003623646740000032
其中，
Figure BDA0003623646740000033
和
Figure BDA0003623646740000034
分别表示当前t时刻和上一时刻机器人坐标系到导航坐标系的姿态变换矩阵；
Figure BDA0003623646740000035
表示当前时刻与上一时刻之间的相对旋转构成的反对称矩阵；
惯性测量单元在导航坐标系下的速度和位置分别为：
Figure BDA0003623646740000041
Figure BDA0003623646740000042
其中，vt表示机器人当前时刻速度；vt-1表示机器人上一时刻速度；at表示IMU测量到的当前时刻加速度；at-1表示IMU测量到的上一时刻加速度；Pt表示机器人当前时刻位置信息；Pt-1表示机器人上一时刻位置信息；Δt为当前时刻与上一时刻之间的时间差。
作为本发明的一种优选方案，所述扩展卡尔曼滤波方法根据系统的运动模型和观测模型实现预测和更新两个步骤，运动模型和观测模型分别为：
xt＝Gtxt-1+wt
zt＝Htxt+Vt
其中，xt为当前t时刻系统状态矩阵；xt-1为上一时刻系统状态矩阵；Gt为当前时刻系统状态传递矩阵；zt为当前时刻系统的观测值矩阵；Ht为系统当前时刻状态量与观测值之间的传递矩阵；wt和Vt分别为当前时刻系统的过程噪声矩阵和观测噪声矩阵；
预测过程：
Figure BDA0003623646740000043
Figure BDA0003623646740000044
其中，
Figure BDA0003623646740000045
为当前时刻系统状态矩阵估计值；
Figure BDA0003623646740000046
为当前时刻系统的状态估计协方差；Rt为当前时刻系统的过程噪声协方差；Σt-1为上一时刻系统状态协方差；
更新过程：
计算卡尔曼增益Kt：
Figure BDA0003623646740000047
其中，Qt为观测值误差协方差矩阵；
利用Kt和观测值更新移动机器人的状态矩阵xt和系统协方差矩阵：
Figure BDA0003623646740000048
Figure BDA0003623646740000049
其中，I为单位矩阵。
本发明采用以上技术方案与现有技术相比，具有以下技术效果：
1、本发明根据图像的信息熵作为主要的影响因子之一，结合卷积神经网络(Convolutional Neural Networks，CNN)识别出来的地形复杂度动态调整匹配过程中使用的模板尺寸，极大程度的削减了由于地面环境因素导致的算法误差，从而达到在保证计算效率的同时提高匹配准确率的目的。
2、针对单一传感器在复杂环境下不能准确估计机器人位姿的问题，本发明提出基于扩展卡尔曼滤波(Extended Kalman Filter，EKF)将改进的模板匹配VO和轮式里程计融合，从而提高定位结果的稳定性，实现更精准、更鲁棒的定位。
附图说明
图1是本发明基于EKF的模板匹配VO与轮式里程计融合定位方法流程图；
图2是卷积操作示意图；
图3是池化示意图；
图4是轮式里程计测速模型图；
图5是目标匹配示意图。
具体实施方式
下面详细描述本发明的实施方式，所述实施方式的示例在附图中示出。下面通过参考附图描述的实施方式是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。
针对单一传感器在复杂环境下不能准确估计机器人位姿的问题，本发明提出了基于改进模板匹配与轮式里程计EKF融合的定位方法。该方法是在里程计坐标系下将基于CNN和图像熵改进的模板匹配的视觉定位结果与轮式里程计进行融合，将融合后的定位信息输入gmapping算法中，从而实现更精准、更鲁棒的定位与建图。
首先，提出将图像熵与深度学习结合的模板匹配VO算法，充分的利用了朝下放置的摄像头的图像数据，根据图像的信息熵作为主要的影响因子之一，结合卷积神经网络(CNN)计算得到场景环境类型，计算得到当前图像的最优目标匹配策略，极大程度的削减了由于地面环境因素导致的算法误差，增强了模板匹配VO算法的鲁棒性和精度。然后，在得到改进后的模板匹配VO后，将机器人的轮式编码器估算出的位姿和经过惯性测量单元(Inertial Measurement Unit，IMU)姿态解算过后的位姿通过扩展卡尔曼滤波(EKF)方法进行融合，将融合过后得到的机器人位姿,作为初级滤波器的预测值，结合视觉里程计的观测值，得到最终姿态数据。如图1所示，为本发明基于EKF的模板匹配VO与轮式里程计融合定位方法流程图。具体步骤如下：
步骤1：采集不同类型地面的图像数据，创建地面图像数据集，为数据集添加表征地形复杂程度的变量(地形复杂度ρ)作为标签，地形复杂度由地面材质以及颠簸程度等因素决定，如草地、瓷砖地、水泥地等。
步骤2：使用该数据集训练卷积神经网络模型，训练完成后的预测模型可以根据移动机器人安装的朝下的摄像头采集的输入图像拟合不同类别的地面信息，并预测该地面图像的地形复杂度。
步骤3：将摄像头采集的图像输入训练好的卷积神经网络模型可以得到当前地面的地形复杂度ρ。该预测模型包含了输入层、卷积层、池化层、全连接层等主要神经网络层。其中，卷积层负责特征提取，由多个卷积核构成，能够根据目标函数，突破传统滤波器的限制，自动调整权重。卷积层参数量应按下式计算：
Weight＝C_h*C_w*in_channel*out_channel+out_channel
式中，Weight为卷积层参数量，C_h、C_w分别为卷积核高和宽，in_channel、out_channel分别为输入输出通道数，而输入输出尺寸关系如下式：
Figure BDA0003623646740000061
式中，oM是输出特征图尺寸，iM是输入特征图尺寸，C是卷积核尺寸，P是填充，S是步长。单一通道中，卷积实现过程与一般数字图像处理中所用卷积方法原理基本一致，即通过卷积核与图像局部的相乘相加，得到一个特征点，然后进行滑窗操作，对整张图像进行运算，得到输出特征图。图2显示使用了一个3*3卷积核，对9*9特征图进行卷积操作的过程。
池化层通常放置于卷积层之后，对特征图进行降采样，提炼特征图有效信息，减少网络计算量。常用的池化方法有最大池化(Maxpooling)、平均池化(Averagepooling)，最大池化通常用于卷积层之后的降采样操作，而平均池化通常用于替代全连接层，组成全卷积神经网络。池化层常用大小为2*2、步长为2的池化核进行池化操作，由于池化核并不含有需要学习的参数，可用一个池化核对所有输入通道的特征图进行处理，所以输出通道数等于输入通道数。为方便理解，图3展示了对4*4的特征图分别进行2*2最大池化和平均池化的过程。
根据经验对每一张图片标定复杂度后(打标签)，通过多层卷积层，池化层，全连接层合理的搭配，在模型训练完成后可以通过图像输入里的纹理、阴影、形状、分类，预测地形复杂度，得到地形复杂度ρ为预测结果。
步骤4：图像熵是一种特征的统计形式，是衡量图像中信息量丰富程度的重要指标。熵值越大意味着图像中含有的信息量越丰富，或是图像中信息分布越混乱。通过计算地面的图像熵S，可以克服神经网络模型中存在的误差，提高对地形描述的精确度。图像一维熵的计算公式为：
Figure BDA0003623646740000071
Figure BDA0003623646740000072
其中，pj表示灰度值为j的点在图像中出现的概率，nj表示灰度值为j的像素点的数量，Tw和Th分别为图像的宽和高。
步骤5：通过获取地形复杂度ρ和地面图像熵S，实时计算获得当前区域的模板选取策略，以达到高鲁棒性高精度的模板匹配VO算法。我们可以根据模板的地面复杂度和熵值大小进行模板尺寸的动态切换。为复杂度ρ和图像熵S分别设定一个阈值E和F，当地形复杂度ρ和图像熵S均小于其阈值时，说明地面特征点少，需要选择大模板进行匹配，即240×240像素尺寸的模板窗口。若地形复杂度ρ大于E而S小于F时，选择180×180像素尺寸的模板；当ρ小于E而S大于F时，这时的地面类型可能是两种地面的过渡类型，使用200×200像素尺寸的模板进行匹配，以保证准确度；当ρ和S均大于阈值时，使用160×160像素尺寸的模板进行匹配，以保证计算效率。阈值E大小的确定并没有具体公式，可通过对大量图像的统计分析及实验效果来确定。
步骤6：得到改进后的模板匹配VO后，将机器人的轮式编码器估算出的位姿和经过IMU姿态解算过后的位姿通过扩展卡尔曼滤波(EKF)方法进行融合，将融合过后得到的机器人位姿，作为初级滤波器的预测值，结合视觉里程计的观测值，得到最终姿态数据。
(1)编码器的测速模型
如图4所示，左右两个轮子上安装的编码器测得的脉冲数可通过下式转化为左右轮的线速度，其中n为每秒钟接收的脉冲数，r为车轮半径，f为编码器旋转一圈发出的脉冲数，即：
Figure BDA0003623646740000081
里程计速度模型：
Figure BDA0003623646740000082
Figure BDA0003623646740000083
其中v，w分别为机器人绕机器人中心的线速度与角速度，Vr、Vl为机器人左右两轮的速度，d为轮距。
理想情况下机器人运动模型如下式所示：
Figure BDA0003623646740000084
其中，(x′,y′,θ′)为在世界坐标系下的当前时刻位姿，(x,y,θ)为世界坐标系下的上一时刻位姿，(dx,dy,dθ)为机器人坐标系下的运动增量。
(2)IMU观测模型
IMU内部包括陀螺仪和加速度计，可依靠惯性导航解算得到机器人位姿。考虑二维环境中imu姿态更新方程为：
Figure BDA0003623646740000085
式中，
Figure BDA0003623646740000086
和
Figure BDA0003623646740000087
分别表示当前时刻和上一时刻机器人坐标系到导航坐标系的姿态变换矩阵；
Figure BDA0003623646740000088
表示当前时刻与上一时刻之间的相对旋转构成的反对称矩阵。
IMU在导航坐标系下的速度和位置分别为：
Figure BDA0003623646740000089
Figure BDA00036236467400000810
式中，vt表示机器人当前时刻速度；vt-1表示机器人上一时刻速度；at表示IMU测量到的当前时刻加速度；at-1表示IMU测量到的上一时刻加速度；Pt表示当前时刻机器人位置信息；Pt-1表示机器人上一时刻位置信息；Δt为当前时刻与上一时刻之间的时间差。
(3)如图5所示，通过单目摄像头模板匹配得到的视觉里程计步骤如下：
a、从相机中获取连续图像帧；
b、在第i图像帧中初始化模板，在第i+1图像帧中确定搜索区域；
c、利用归一化互相关匹配方法在第i+1图像帧中匹配第i图像帧中的模板，获取最大相似度的匹配结果；
d、根据匹配区域与原始模板的左上角像素位置计算出像素位移增量Δu和Δv；
e、将像素位移增量转换到相机在世界物理坐标系中的真实位移增量；
f、获取第i+1时刻IMU输出的偏航角；
g、利用位置递推公式，解算并输出第i+1时刻的机器人位置；
h、返回步骤1，重复上述步骤。
(4)扩展卡尔曼滤波融合过程
扩展卡尔曼滤波(EKF)主要思想是通过对非线性方程一阶泰勒展开为线性方程，然后再使用卡尔曼滤波算法对系统状态进行估计。EKF算法根据系统的运动模型和观测模型实现预测和更新两个步骤，运动模型和观测模型分别为：
xt＝Gtxt-1+wt
zt＝Htxt+Vt
式中，xt为当前t时刻系统状态矩阵；xt-1为上一时刻系统状态矩阵；Gt为当前时刻系统状态传递矩阵；zt为当前时刻系统的观测值矩阵；Ht为系统当前时刻状态量与观测值之间的传递矩阵；wt和Vt分别为当前时刻系统的过程噪声矩阵和观测噪声矩阵；
预测过程：
Figure BDA0003623646740000091
Figure BDA0003623646740000092
式中，
Figure BDA0003623646740000093
为当前时刻系统状态矩阵估计值；
Figure BDA0003623646740000094
为当前时刻系统的状态估计协方差；Rt为当前时刻系统的过程噪声协方差；Σt-1为上一时刻系统状态协方差；
更新过程：
首先，计算卡尔曼增益Kt：
Figure BDA0003623646740000101
其中，Qt为观测值误差协方差矩阵；
利用Kt和观测值更新移动机器人的状态矩阵xt和系统协方差矩阵：
Figure BDA0003623646740000102
Figure BDA0003623646740000103
其中，I为单位矩阵。
扩展卡尔曼滤波融合轮式里程计和IMU的过程为：通过编码器测速模型得到左右轮线速度，将其输入到运动模型中进行状态估计，对IMU预积分输入到观测方程进行状态更新。通过卡尔曼增益和公式的不断迭代，最终得到系统状态的最优估计。轮式里程计与IMU融合后的结果作为次级滤波器的预测值，模板匹配VO解算出的位姿结果作为次级滤波器的观测值，代入(4)中的预测过程和更新过程，利用卡尔曼增益和观测值更新移动机器人的状态矩阵和系统协方差矩阵，得到系统状态的最终估计值。
随着时间的变化，里程计累积的误差会越来越大，所以在滤波器中用模板匹配VO的观测来弥补里程计的累积误差，就可以大大减少算法收敛到局部最优的情况。
以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (7)

1.基于EKF的模板匹配VO与轮式里程计融合定位方法，其特征在于，包括如下步骤：
步骤1，采集不同类型地面的图像数据，创建地面图像数据集，为地面图像数据集中的每幅图像添加地形复杂度作为标签；
步骤2，利用添加标签后的地面图像数据集训练卷积神经网络模型，得到训练好的卷积神经网络模型；
步骤3，利用机器人上安装的摄像头采集地面图像输入训练好的卷积神经网络模型中，得到当前地面的地形复杂度；
步骤4，计算当前地面的图像熵；
步骤5，根据地形复杂度和图像熵，实时计算当前地面的模板选取策略，根据模板选取策略进行模板匹配VO估算，得到位姿结果；
步骤6，将轮式里程计估算出的位姿和经过惯性测量单元解算出的位姿通过扩展卡尔曼滤波方法即初级滤波器进行融合，将融合后得到的位姿作为次级滤波器的预测值，结合模板匹配VO估算出的位姿结果作为次级滤波器的观测值，得到最终的机器人定位结果。
2.根据权利要求1所述的基于EKF的模板匹配VO与轮式里程计融合定位方法，其特征在于，步骤1所述地形复杂度的决定因素包括地面材质以及颠簸程度。
3.根据权利要求1所述的基于EKF的模板匹配VO与轮式里程计融合定位方法，其特征在于，步骤4所述图像熵的计算公式如下：
Figure FDA0003623646730000011
Figure FDA0003623646730000012
其中，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转换到相机在世界物理坐标系中的真实位移增量；
5)根据真实位移增量解算得到i+1时刻模板匹配VO估算的机器人位姿结果；
6)根据模板选取策略在第i+1帧地面图像中初始化模板并重复上述过程，得到i+2时刻模板匹配VO估算的机器人位姿结果，以此类推。
6.根据权利要求1所述的基于EKF的模板匹配VO与轮式里程计融合定位方法，其特征在于，所述惯性测量单元的观测模型如下：
惯性测量单元的姿态更新方程为：
Figure FDA0003623646730000021
其中，
Figure FDA0003623646730000022
和
Figure FDA0003623646730000023
分别表示当前t时刻和上一时刻机器人坐标系到导航坐标系的姿态变换矩阵；
Figure FDA0003623646730000024
表示当前时刻与上一时刻之间的相对旋转构成的反对称矩阵；
惯性测量单元在导航坐标系下的速度和位置分别为：
Figure FDA0003623646730000031
Figure FDA0003623646730000032
其中，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分别为当前时刻系统的过程噪声矩阵和观测噪声矩阵；
预测过程：
Figure FDA0003623646730000033
Figure FDA0003623646730000034
其中，
Figure FDA0003623646730000035
为当前时刻系统状态矩阵估计值；
Figure FDA0003623646730000036
为当前时刻系统的状态估计协方差；Rt为当前时刻系统的过程噪声协方差；∑t-1为上一时刻系统状态协方差；
更新过程：
计算卡尔曼增益Kt：
Figure FDA0003623646730000037
其中，Qt为观测值误差协方差矩阵；
利用Kt和观测值更新移动机器人的状态矩阵xt和系统协方差矩阵：
Figure FDA0003623646730000038
Figure FDA0003623646730000039
其中，I为单位矩阵。
CN202210472832.8A 2022-04-29 2022-04-29 基于ekf的模板匹配vo与轮式里程计融合定位方法 Pending CN114993298A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210472832.8A CN114993298A (zh) 2022-04-29 2022-04-29 基于ekf的模板匹配vo与轮式里程计融合定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210472832.8A CN114993298A (zh) 2022-04-29 2022-04-29 基于ekf的模板匹配vo与轮式里程计融合定位方法

Publications (1)

Publication Number Publication Date
CN114993298A true CN114993298A (zh) 2022-09-02

Family

ID=83024791

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210472832.8A Pending CN114993298A (zh) 2022-04-29 2022-04-29 基于ekf的模板匹配vo与轮式里程计融合定位方法

Country Status (1)

Country Link
CN (1) CN114993298A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115435775A (zh) * 2022-09-23 2022-12-06 福州大学 基于拓展卡尔曼滤波的多传感器融合slam方法
CN116524196A (zh) * 2023-06-21 2023-08-01 南方电网数字平台科技(广东)有限公司 一种基于图像识别技术的输电线智能检测系统

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115435775A (zh) * 2022-09-23 2022-12-06 福州大学 基于拓展卡尔曼滤波的多传感器融合slam方法
CN116524196A (zh) * 2023-06-21 2023-08-01 南方电网数字平台科技(广东)有限公司 一种基于图像识别技术的输电线智能检测系统
CN116524196B (zh) * 2023-06-21 2023-09-01 南方电网数字平台科技(广东)有限公司 一种基于图像识别技术的输电线智能检测系统

Similar Documents

Publication Publication Date Title
CN113781582B (zh) 基于激光雷达和惯导联合标定的同步定位与地图创建方法
CN110146099B (zh) 一种基于深度学习的同步定位与地图构建方法
CN103411609B (zh) 一种基于在线构图的飞行器返航路线规划方法
CN114993298A (zh) 基于ekf的模板匹配vo与轮式里程计融合定位方法
CN113903011B (zh) 一种适用于室内停车场的语义地图构建及定位方法
CN112734765B (zh) 基于实例分割与多传感器融合的移动机器人定位方法、系统及介质
CN112907678B (zh) 车载相机外参姿态动态估计方法、装置、计算机设备
CN115388902B (zh) 室内定位方法和系统、ar室内定位导航方法和系统
Qian et al. Robust visual-lidar simultaneous localization and mapping system for UAV
CN110992424B (zh) 基于双目视觉的定位方法和系统
CN113126115A (zh) 基于点云的语义slam方法、装置、电子设备和存储介质
CN115272596A (zh) 一种面向单调无纹理大场景的多传感器融合slam方法
CN113361499B (zh) 基于二维纹理和三维姿态融合的局部对象提取方法、装置
CN116222577B (zh) 闭环检测方法、训练方法、系统、电子设备及存储介质
CN111998862A (zh) 一种基于bnn的稠密双目slam方法
CN114777775A (zh) 一种多传感器融合的定位方法及系统
CN114708293A (zh) 基于深度学习点线特征和imu紧耦合的机器人运动估计方法
CN111123953A (zh) 人工智能大数据下粒子化移动机器人组及其控制方法
CN104751451A (zh) 基于无人机低空高分辨率影像的密集点云提取方法
CN110864670B (zh) 目标障碍物位置的获取方法和系统
CN107345814A (zh) 一种移动机器人视觉定位系统及定位方法
CN117029870A (zh) 一种基于路面点云的激光里程计
CN114868154A (zh) 包括多分辨率体素中的协方差的地图
Li et al. Improving Vehicle Localization with Lane Marking Detection Based on Visual Perception and Geographic Information
CN115060268A (zh) 一种机房融合定位方法、系统、设备及存储介质

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination