CN115328168A - 基于自适应强跟踪的移动机器人同步定位与建图方法及*** - Google Patents

基于自适应强跟踪的移动机器人同步定位与建图方法及*** Download PDF

Info

Publication number
CN115328168A
CN115328168A CN202211137399.9A CN202211137399A CN115328168A CN 115328168 A CN115328168 A CN 115328168A CN 202211137399 A CN202211137399 A CN 202211137399A CN 115328168 A CN115328168 A CN 115328168A
Authority
CN
China
Prior art keywords
mobile robot
time
pose
estimation
covariance
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
CN202211137399.9A
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.)
Xian Jiaotong University
Original Assignee
Xian Jiaotong University
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 Xian Jiaotong University filed Critical Xian Jiaotong University
Priority to CN202211137399.9A priority Critical patent/CN115328168A/zh
Publication of CN115328168A publication Critical patent/CN115328168A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
    • G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Optics & Photonics (AREA)
  • Electromagnetism (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种基于自适应强跟踪的移动机器人同步定位与建图方法及***，所述方法包括以下步骤：假设同步定位与地图构建过程中的噪声均为加性高斯噪声的前提下，根据移动机器人的底盘样式和传感器的特性构建移动机器人的运动学模型和观测模型，初始化获取移动机器人在全局坐标系下的初始位姿和初始估计误差协方差；基于自适应强跟踪扩展卡尔曼滤波方法对移动机器人的位姿进行迭代估计，实现移动机器人的实时位姿估计和地图构建。本发明提供的方法，能够在移动机器人发生运动状态突变或模型参数失配时，通过监测新息的变化对过程噪声进行自适应估计，使移动机器人保持较强的位姿跟踪能力和良好的路标估计精度。

Description

基于自适应强跟踪的移动机器人同步定位与建图方法及***
技术领域
本发明属于移动机器人同步定位与建图及自主导航技术领域，特别涉及一种基于自适应强跟踪的移动机器人同步定位与建图方法及***。
背景技术
近些年来，随着移动机器人领域相关技术的发展，人们越来越重视拥有自主导航能力并能自主完成设定任务的移动机器人的研发；具备自主导航能力的机器人越来越多的应用在森林火灾检测、快递物流搬运、餐饮店送餐等场景，而实现移动机器人路径规划和自主导航的基础是高精度的机器人位姿估计和精确的环境地图。
同步定位与地图构建(Simultaneous Localization and Mapping，SLAM)技术，是指移动机器人在未知环境中同时完成自身定位、路标估计以及构建环境地图的过程。根据后端优化策略的不同，SLAM可分为基于贝叶斯滤波和基于非线性优化两类框架；其中，相较于非线性优化算法，贝叶斯滤波算法简单易行，发展也相对成熟。
现有的扩展卡尔曼滤波(Extended Kalman Filter，EKF)算法通过对非线性***进行泰勒展开并取一阶近似，实现较简单，已经广泛应用于非线性***；然而EKF算法由于舍弃了***的高阶信息，导致机器人位姿估计的精度较低，并且对于模型中不准确的参数没有自适应能力；在EKF基础上提出的强跟踪滤波(Strong Tracking Filter，STF)算法在一定程度上提高了机器人位姿估计精度，但是未能实现对噪声的自适应估计，对机器人运动状态的突变和参数的失配没有良好的自适应能力，并且在实际应用于SLAM问题时容易出现滤波发散现象。综上，亟需一种基于自适应强跟踪的移动机器人同步定位与建图方法。
发明内容
本发明的目的在于提供一种基于自适应强跟踪的移动机器人同步定位与建图方法及***，以解决上述存在的技术问题。本发明提供的方法，能够在移动机器人发生运动状态突变或模型参数失配时，通过监测新息的变化对过程噪声进行自适应估计，使移动机器人保持较强的位姿跟踪能力和良好的路标估计精度，能提高移动机器人SLAM算法的鲁棒性。
为达到上述目的，本发明采用以下技术方案：
本发明提供的一种基于自适应强跟踪的移动机器人同步定位与建图方法，包括以下步骤：
假设同步定位与地图构建过程中的噪声均为加性高斯噪声的前提下，根据移动机器人的底盘样式和传感器的特性构建移动机器人的运动学模型和观测模型；对所述移动机器人的运动学模型和观测模型进行初始化，获取移动机器人在全局坐标系下的初始位姿和初始估计误差协方差；
基于自适应强跟踪扩展卡尔曼滤波方法对移动机器人的位姿进行迭代估计，实现移动机器人的实时位姿估计和地图构建；
其中，所述基于自适应强跟踪扩展卡尔曼滤波方法对移动机器人的位姿进行迭代估计过程中，k时刻迭代估计的步骤包括，
基于k-1时刻移动机器人位姿和k-1时刻估计误差协方差，利用扩展卡尔曼滤波方法和移动机器人的运动学模型，获得k时刻移动机器人位姿预测值；
获取k时刻传感器对环境路标的观测信息并进行路标数据关联，获得k时刻与k-1时刻的路标关联结果；基于所述路标关联结果，获得关联后的k时刻传感器观测信息；
判断所述移动机器人同步定位与建图方法的迭代结果是否发散，获得判断结果；基于所述判断结果，利用k-1时刻移动机器人位姿、k-1时刻估计误差协方差、移动机器人的运动学模型和观测模型对k时刻估计误差协方差进行时间更新，获得k时刻估计误差协方差预测值；
基于所述关联后的k时刻传感器观测信息、所述k时刻移动机器人位姿预测值、所述k时刻估计误差协方差预测值，通过量测更新获得k时刻移动机器人的位姿和k时刻估计误差协方差。
本发明的进一步改进在于，构建获得的移动机器人的运动学模型和观测模型表示为，
Figure BDA0003852696870000031
式中，zk为k时刻传感器的观测信息，uk为k时刻机器人的输入，f(·)为状态转移函数，h(·)为量测函数，wk-1和vk为互不相关的过程噪声和量测噪声，均服从均值为0、协方差分别为Rk和Qk高斯分布；
Figure BDA0003852696870000032
为k时刻移动机器人的状态信息，
Figure BDA0003852696870000033
为移动机器人在二维空间中的坐标位置，
Figure BDA0003852696870000034
为移动机器人的朝向偏离全局坐标系的夹角。
本发明的进一步改进在于，所述判断所述移动机器人同步定位与建图方法的迭代结果是否发散，获得判断结果的过程中：
是否发散的判据表达式为，
Figure BDA0003852696870000035
式中，
Figure BDA0003852696870000036
为k时刻的新息序列，
Figure BDA0003852696870000037
为移动机器人在k时刻量测函数在
Figure BDA0003852696870000038
进行泰勒展开并取一阶近似的结果，且
Figure BDA0003852696870000039
κ≥1为可调系数，tr[·]为矩阵求迹运算符，Pk/k-1为k时刻估计误差协方差的预测值；
满足所述判据表达式时，需要引入强跟踪算法抑制滤波发散。
本发明的进一步改进在于，所述基于所述判断结果，利用k-1时刻移动机器人位姿、k-1时刻估计误差协方差、移动机器人的运动学模型和观测模型对k时刻估计误差协方差进行时间更新，获得k时刻估计误差协方差预测值的过程中，
当滤波已经发散，引入自适应强跟踪算法，计算自适应强跟踪滤波器参数，并对估计误差协方差进行时间更新，包括：
计算新息序列协方差矩阵Vk，表达式为，
Figure BDA0003852696870000041
式中，0＜ρ≤1为遗忘因子；
获取参数矩阵Mk和Nk，表达式为，
Figure BDA0003852696870000042
式中，参数β≥1为弱化因子；
Figure BDA0003852696870000043
为移动机器人在k-1时刻状态转移函数在
Figure BDA0003852696870000044
进行泰勒展开并取一阶近似的结果，
Figure BDA0003852696870000045
为k-1时刻移动机器人的位姿；
λk的计算表达式为，
Figure BDA0003852696870000046
式中，
Figure BDA0003852696870000047
引入自适应强跟踪滤波算法后，估计误差协方差的时间更新公式为，
Figure BDA0003852696870000048
式中，Pk-1|k-1为k-1时刻的估计误差协方差，λkQk-1项可视为当噪声参数失配时对噪声的修正项。
本发明的进一步改进在于，所述基于所述关联后的k时刻传感器观测信息、所述k时刻移动机器人位姿预测值、所述k时刻估计误差协方差预测值，通过量测更新获得k时刻移动机器人的位姿和k时刻估计误差协方差的过程中，
量测更新的步骤包括：
计算k时刻的卡尔曼滤波增益Kk，计算表达式为
Figure BDA0003852696870000049
计算k时刻机器人的位姿
Figure BDA00038526968700000410
计算表达式为
Figure BDA00038526968700000411
更新估计误差协方差矩阵Pk|k，更新表达式为Pk|k＝(I-KkHk)Pk|k-1；
式中，I为单位矩阵。
本发明提供的一种基于自适应强跟踪的移动机器人同步定位与建图***，包括：
模型获取及初始化模块，用于假设同步定位与地图构建过程中的噪声均为加性高斯噪声的前提下，根据移动机器人的底盘样式和传感器的特性构建移动机器人的运动学模型和观测模型；对所述移动机器人的运动学模型和观测模型进行初始化，获取移动机器人在全局坐标系下的初始位姿和初始估计误差协方差；
迭代估计模块，用于基于自适应强跟踪扩展卡尔曼滤波方法对移动机器人的位姿进行迭代估计，实现移动机器人的实时位姿估计和地图构建；
其中，所述基于自适应强跟踪扩展卡尔曼滤波方法对移动机器人的位姿进行迭代估计过程中，k时刻迭代估计的步骤包括，
基于k-1时刻移动机器人位姿和k-1时刻估计误差协方差，利用扩展卡尔曼滤波方法和移动机器人的运动学模型，获得k时刻移动机器人位姿预测值；
获取k时刻传感器对环境路标的观测信息并进行路标数据关联，获得k时刻与k-1时刻的路标关联结果；基于所述路标关联结果，获得关联后的k时刻传感器观测信息；
判断所述移动机器人同步定位与建图方法的迭代结果是否发散，获得判断结果；基于所述判断结果，利用k-1时刻移动机器人位姿、k-1时刻估计误差协方差、移动机器人的运动学模型和观测模型对k时刻估计误差协方差进行时间更新，获得k时刻估计误差协方差预测值；
基于所述关联后的k时刻传感器观测信息、所述k时刻移动机器人位姿预测值、所述k时刻估计误差协方差预测值，通过量测更新获得k时刻移动机器人的位姿和k时刻估计误差协方差。
本发明的进一步改进在于，所述模型获取及初始化模块中，构建获得的移动机器人的运动学模型和观测模型表示为，
Figure BDA0003852696870000051
式中，zk为k时刻传感器的观测信息，uk为k时刻机器人的输入，f(·)为状态转移函数，h(·)为量测函数，wk-1和vk为互不相关的过程噪声和量测噪声，均服从均值为0、协方差分别为Rk和Qk高斯分布；
Figure BDA0003852696870000061
为k时刻移动机器人的状态信息，
Figure BDA0003852696870000062
为移动机器人在二维空间中的坐标位置，
Figure BDA0003852696870000063
为移动机器人的朝向偏离全局坐标系的夹角。
本发明的进一步改进在于，所述迭代估计模块中，实现所述判断所述移动机器人同步定位与建图方法的迭代结果是否发散，获得判断结果的过程中：
是否发散的判据表达式为，
Figure BDA0003852696870000064
式中，
Figure BDA0003852696870000065
为k时刻的新息序列，
Figure BDA0003852696870000066
为移动机器人在k时刻量测函数在
Figure BDA0003852696870000067
进行泰勒展开并取一阶近似的结果，且
Figure BDA0003852696870000068
κ≥1为可调系数，tr[·]为矩阵求迹运算符，Pk/k-1为k时刻估计误差协方差的预测值；
满足所述判据表达式时，需要引入强跟踪算法抑制滤波发散。
本发明的进一步改进在于，所述迭代估计模块中，实现所述基于所述判断结果，利用k-1时刻移动机器人位姿、k-1时刻估计误差协方差、移动机器人的运动学模型和观测模型对k时刻估计误差协方差进行时间更新，获得k时刻估计误差协方差预测值的过程中，
当滤波已经发散，引入自适应强跟踪算法，计算自适应强跟踪滤波器参数，并对估计误差协方差进行时间更新，包括：
计算新息序列协方差矩阵Vk，表达式为，
Figure BDA0003852696870000069
式中，0＜ρ≤1为遗忘因子；
获取参数矩阵Mk和Nk，表达式为，
Figure BDA00038526968700000610
式中，参数β≥1为弱化因子；
Figure BDA0003852696870000071
为移动机器人在k-1时刻状态转移函数在
Figure BDA0003852696870000072
进行泰勒展开并取一阶近似的结果，
Figure BDA0003852696870000073
为k-1时刻移动机器人的位姿；
λk的计算表达式为，
Figure BDA0003852696870000074
式中，
Figure BDA0003852696870000075
引入自适应强跟踪滤波算法后，估计误差协方差的时间更新公式为，
Figure BDA0003852696870000076
式中，Pk-1|k-1为k-1时刻的估计误差协方差，λkQk-1项可视为当噪声参数失配时对噪声的修正项。
本发明的进一步改进在于，所述迭代估计模块中，实现所述基于所述关联后的k时刻传感器观测信息、所述k时刻移动机器人位姿预测值、所述k时刻估计误差协方差预测值，通过量测更新获得k时刻移动机器人的位姿和k时刻估计误差协方差的过程中，
量测更新的步骤包括：
计算k时刻的卡尔曼滤波增益Kk，计算表达式为
Figure BDA0003852696870000077
计算k时刻机器人的位姿
Figure BDA0003852696870000078
计算表达式为
Figure BDA0003852696870000079
更新估计误差协方差矩阵Pk|k，更新表达式为Pk|k＝(I-KkHk)Pk|k-1；
式中，I为单位矩阵。
与现有技术相比，本发明具有以下有益效果：
本发明提供的移动机器人同步定位与建图方法，基于自适应强跟踪算法并结合机器人***模型和激光雷达的量测数据，能够在移动机器人运动状态突变、***参数建模不准确及参数失配等情况下对机器人位姿和路标位置进行高精度估计，有助于提高移动机器人在未知环境中的定位和建图精度。相比于EKF、STF等传统SLAM方法，本发明提供的基于自适应强跟踪的移动机器人同步定位与建图方法可以获得准确的机器人位姿和路标估计结果，能够实现未知环境的在线识别，所构建的高精度全局环境地图可用于移动机器人的路径规划和自主导航。本发明在STF算法框架的基础上，通过在时间更新步直接将渐消因子作用于过程噪声协方差矩阵，实现了对***过程噪声的自适应估计和滤波增益的实时修正。本发明基于协方差匹配的思想建立了强跟踪滤波的引入条件，解决了STF算法中因渐消因子计算异常导致的滤波发散现象，提高了机器人在模型不确定和状态突变场景下的位置跟踪能力和路标估计精度。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面对实施例或现有技术描述中所需要使用的附图做简单的介绍；显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来说，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明实施例提供的一种基于自适应强跟踪的移动机器人同步定位与建图方法的流程示意图；
图2是本发明实施例中，以差分底盘移动机器人为例进行***建模的示意图；
图3是本发明实施例中，进行仿真实验的场景示意图；
图4是本发明实施例中，使用EKF算法进行机器人SLAM的实验结果；
图5是本发明实施例中，使用STF算法进行机器人SLAM的实验结果；
图6是本发明实施例中，使用自适应强跟踪滤波算法进行机器人SLAM的实验结果；
图7是本发明实施例中，机器人使用EKF和自适应强跟踪滤波两种算法所得到的机器人位姿估计结果的均方误差变化曲线示意图。
具体实施方式
为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。
需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、***、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
下面结合附图对本发明做进一步详细描述：
请参阅图1，本发明实施例提供的一种基于自适应强跟踪的移动机器人同步定位与建图方法，具体包括以下步骤：
步骤S1：对移动机器人进行建模。假设移动机器人SLAM过程中的噪声均为加性噪声，根据移动机器人的底盘样式和传感器的特性构建移动机器人的运动学模型和观测模型。
本发明实施例的步骤S1的具体建模过程如下：
针对某一移动机器人，选取移动机器人工作场景中的某一点作为坐标原点构建全局坐标系，同时选取移动机器人的几何中心作为坐标原点构建局部坐标系；
定义
Figure BDA0003852696870000091
为k时刻移动机器人的状态信息，
Figure BDA0003852696870000092
为移动机器人在二维空间中的坐标位置，
Figure BDA0003852696870000093
为移动机器人的朝向偏离全局坐标系的夹角；定义
Figure BDA0003852696870000094
为k时刻路标的特征信息；则SLAM问题的状态向量可描述为：
Figure BDA0003852696870000095
根据移动机器人不同底盘的运动方式和传感器的测量原理，可以构建抽象化的移动机器人运动学方程和观测方程，表示为：
Figure BDA0003852696870000101
式中，zk为k时刻的量测信息，uk为k时刻机器人的输入，f(·)为状态转移函数，h(·)为量测函数，wk-1和vk为互不相关的过程噪声和量测噪声，均服从零均值高斯分布，噪声协方差分别为Qk和Rk。
请参阅图2，本发明实施例中，以差分型底盘移动机器人为例对移动机器人进行建模；图2中，以点Og为全局坐标系原点，以移动机器人几何中心Or为局部坐标系原点，建立移动机器人的动力学方程和观测方程如下：
Figure BDA0003852696870000102
Figure BDA0003852696870000103
式中，(xr,k,yr,k,θk)为k时刻机器人在全局坐标系下的位姿，εk和ωk分别为机器人在k时刻的线速度和角速度，T为采样周期，wk-1为过程噪声。rk和
Figure BDA0003852696870000104
分别为k时刻第i个路标到机器人几何中心的距离和与机器人行驶方向的夹角，(xi,k,yi,k)为第i个路标在全局坐标系下的坐标，vk为传感器的量测噪声。
步骤S2：参数初始化。对传感器的最大量测距离、分辨率等信息进行初始化，并获取移动机器人在全局坐标系下的初始位姿
Figure BDA0003852696870000105
和初始估计误差协方差P0，具体计算方法为：
Figure BDA0003852696870000106
Figure BDA0003852696870000107
式中，
Figure BDA0003852696870000111
为初始时刻机器人真实位姿，
Figure BDA0003852696870000112
为数学期望运算符。
本发明实施例中，设定移动机器人的初始位姿(xr,0,yr,0,θ0)＝(0m,-3m,0rad)，初始估计误差误差协方差
Figure BDA0003852696870000113
请参阅图3，本实施例中，在MATLAB中搭建一个长宽均为20m的仿真场地，以场地中心为圆心，分别以3m、5m、8m为半径，间隔30°均匀设置三圈路标点，如图3中“*”所示。设定机器人所携带激光雷达的最大量测距离为5m。为了模拟机器人在运行过程中模型参数失配和状态突变现象，在仿真过程中均匀设置了三段不同的过程噪声Qk和线速度εk，详细的参数设定请参阅表1。
表1.过程噪声和线速度参数
Figure BDA0003852696870000114
此外，还需初始化路标状态向量
Figure BDA0003852696870000115
通常路标状态向量使用增量式管理，初始时刻无任何路标信息，故
Figure BDA0003852696870000116
为空向量。
步骤S3：移动机器人位姿预测。根据扩展卡尔曼滤波的思想，通过k-1时刻机器人位姿的后验估计
Figure BDA0003852696870000117
结合机器人的动力学模型，预测k时刻机器人的位姿
Figure BDA0003852696870000118
其计算公式为：
Figure BDA0003852696870000119
步骤S4：获取传感器的观测信息并进行路标数据关联。在将路标数据融入到全局地图中前，新观测到的路标与地图中已存在的路标必然存在数据关联。为了保证算法的收敛性，在每一时刻传感器完成对路标信息的采样后，均需要将k时刻的路标信息与k-1时刻的路标信息进行数据关联。对传感器观测到的路标的处理可以分为以下两类，包括：
(1)若该路标已经被观测过。只需要在后续量测更新中对该路标的状态进行更新，获取最新的路标状态。
(2)若该路标尚未被观测过。则需要根据传感器的观测模型，通过逆运算获取新观测路标的状态，并将其追加到路标状态向量
Figure BDA0003852696870000121
中，实现路标状态向量的增广；同时需要计算增广后的协方差矩阵。
步骤S5：判断滤波器是否发散。本实施例中，基于协方差匹配思想构建了误差发散判据，仅在***模型参数失配或状态突变引起滤波器发散时才引入强跟踪算法，详细的推导过程为：
当***状态未发生突变或***模型准确时，满足下式：
Figure BDA0003852696870000122
式中，γk为k时刻的新息序列，且
Figure BDA0003852696870000123
Figure BDA0003852696870000124
为移动机器人在k时刻量测函数在
Figure BDA0003852696870000125
进行泰勒展开并取一阶近似的结果。
***状态的突变或模型参数的失配将导致新息增加，不再满足正交性原理，根据新息序列的这一特性，可以构造滤波器发散的判据：
Figure BDA0003852696870000126
式中，κ≥1为可调系数，可根据需要选择，当κ＝1时为最严格的判断条件；tr[·]为矩阵求迹运算符；当滤波器满足上式时，证明预测模型可能出现参数失配，或量测信息受到了严重的干扰，此时需要引入强跟踪算法抑制滤波发散。
步骤S6：对估计误差协方差进行时间更新。根据步骤S5中滤波是否发散的判断结果，可以将误差协方差的更新分为如下两类情况：
(1)滤波尚未发散。此时不需要引入自适应强跟踪算法，仅需要按照传统EKF算法中估计误差协方差的计算公式进行时间更新即可：
Figure BDA0003852696870000131
式中，
Figure BDA0003852696870000132
为移动机器人k时刻状态转移函数在
Figure BDA0003852696870000133
处进行泰勒展开并取一阶近似的结果。
(2)滤波已经发散。此时需要引入自适应强跟踪算法，计算自适应强跟踪滤波器参数，并对估计误差协方差进行时间更新。自适应强跟踪算法的关键在于求解时变减消因子λk，通常寻取最优的λk需要复杂的求解过程，不适用于在线计算，故而工程中一般使用近似次优公式，其计算过程如下：
步骤S6.1：首先计算新息序列协方差矩阵Vk：
Figure BDA0003852696870000134
式(11)中，0＜ρ≤1为遗忘因子。
步骤S6.2：计算两个参数矩阵Mk和Nk：
Figure BDA0003852696870000135
Figure BDA0003852696870000136
式(13)中，参数β≥1为弱化因子，可增强状态估计的平滑性。
步骤S6.3：λk可通过下式得出：
Figure BDA0003852696870000137
式(14)中，
Figure BDA0003852696870000138
引入自适应强跟踪滤波算法后，估计误差协方差的时间更新公式变为：
Figure BDA0003852696870000139
式(15)中，λkQk-1项可视为当噪声参数失配时对噪声的修正项。
步骤S7：量测更新。量测更新这一步主要是根据步骤S4中传感器的量测信息对状态进行校正，获取状态的后验估计xk|k并更新估计误差协方差Pk|k。
本发明实施例中，量测更新环节的详细步骤如下：
步骤S7.1：计算卡尔曼滤波增益Kk：
Figure BDA0003852696870000141
步骤S7.2：计算状态的后验估计
Figure BDA0003852696870000142
Figure BDA0003852696870000143
步骤S7.3：更新估计误差协方差矩阵Pk|k：
Pk|k＝(I-KkHk)Pk|k-1 (18)
步骤S8：循环迭代步骤S1至步骤S7，从而实现移动机器人的位姿估计和地图构建过程。
请参阅图4至图7，为本实施例中使用扩展卡尔曼滤波、强跟踪滤波和自适应强跟踪滤波三种算法的模拟移动机器人同步定位与建图的实验结果，观察移动机器人在整个建图过程中估计轨迹与实际运动轨迹的偏离程度以及机器人位置估计均方误差的变化情况。
图4至图6分别为移动机器人使用扩展卡尔曼滤波、强跟踪滤波和自适应强跟踪滤波三种算法的同步定位与建图实验的结果。结合图4至图6这三张图片可以看出，当过程噪声较小且噪声先验与真实值相同时，三种算法的估计轨迹与真实轨迹基本一致；随着噪声的增加和机器人状态的突变，EKF算法在机器人位姿估计中的误差逐渐增大，STF算法的估计结果在经过一段时间的震荡后迅速发散，而自适应强跟踪滤波算法仍能实现较好的估计效果。整个仿真过程中，自适应强跟踪滤波算法的估计轨迹与真实轨迹基本重合。除此之外，对比实验结果还容易看出，自适应强跟踪滤波算法在路标位置的估计上也明显优于另外两种算法。
图7为EKF算法和自适应强跟踪滤波在整个SLAM过程中机器人位姿估计的均方误差变化曲线，可以看出，相比于EKF算法，自适应强跟踪滤波算法的均方误差曲线相对平缓，且均方误差绝对值明显小于EKF算法，这说明自适应强跟踪滤波算法在机器人SLAM的位姿估计中优于EKF算法。
下述为本发明的装置实施例，可以用于执行本发明方法实施例。对于装置实施例中未纰漏的细节，请参照本发明方法实施例。
本发明再一实施例中，提供的一种基于自适应强跟踪的移动机器人同步定位与建图***，包括：
模型获取及初始化模块，用于假设同步定位与地图构建过程中的噪声均为加性高斯噪声的前提下，根据移动机器人的底盘样式和传感器的特性构建移动机器人的运动学模型和观测模型；对所述移动机器人的运动学模型和观测模型进行初始化，获取移动机器人在全局坐标系下的初始位姿和初始估计误差协方差；
迭代估计模块，用于基于自适应强跟踪扩展卡尔曼滤波方法对移动机器人的位姿进行迭代估计，实现移动机器人的实时位姿估计和地图构建；
其中，所述基于自适应强跟踪扩展卡尔曼滤波方法对移动机器人的位姿进行迭代估计过程中，k时刻迭代估计的步骤包括，
基于k-1时刻移动机器人位姿和k-1时刻估计误差协方差，利用扩展卡尔曼滤波方法和移动机器人的运动学模型，获得k时刻移动机器人位姿预测值；
获取k时刻传感器对环境路标的观测信息并进行路标数据关联，获得k时刻与k-1时刻的路标关联结果；基于所述路标关联结果，获得关联后的k时刻传感器观测信息；
判断所述移动机器人同步定位与建图方法的迭代结果是否发散，获得判断结果；基于所述判断结果，利用k-1时刻移动机器人位姿、k-1时刻估计误差协方差、移动机器人的运动学模型和观测模型对k时刻估计误差协方差进行时间更新，获得k时刻估计误差协方差预测值；
基于所述关联后的k时刻传感器观测信息、所述k时刻移动机器人位姿预测值、所述k时刻估计误差协方差预测值，通过量测更新获得k时刻移动机器人的位姿和k时刻估计误差协方差。
本领域内的技术人员应明白，本申请的实施例可提供为方法、***、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(***)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
最后应当说明的是：以上实施例仅用以说明本发明的技术方案而非对其限制，尽管参照上述实施例对本发明进行了详细的说明，所属领域的普通技术人员应当理解：依然可以对本发明的具体实施方式进行修改或者等同替换，而未脱离本发明精神和范围的任何修改或者等同替换，其均应涵盖在本发明的权利要求保护范围之内。

Claims (10)

1.一种基于自适应强跟踪的移动机器人同步定位与建图方法，其特征在于，包括以下步骤：
假设同步定位与地图构建过程中的噪声均为加性高斯噪声的前提下，根据移动机器人的底盘样式和传感器的特性构建移动机器人的运动学模型和观测模型；对所述移动机器人的运动学模型和观测模型进行初始化，获取移动机器人在全局坐标系下的初始位姿和初始估计误差协方差；
基于自适应强跟踪扩展卡尔曼滤波方法对移动机器人的位姿进行迭代估计，实现移动机器人的实时位姿估计和地图构建；
其中，所述基于自适应强跟踪扩展卡尔曼滤波方法对移动机器人的位姿进行迭代估计过程中，k时刻迭代估计的步骤包括，
基于k-1时刻移动机器人位姿和k-1时刻估计误差协方差，利用扩展卡尔曼滤波方法和移动机器人的运动学模型，获得k时刻移动机器人位姿预测值；
获取k时刻传感器对环境路标的观测信息并进行路标数据关联，获得k时刻与k-1时刻的路标关联结果；基于所述路标关联结果，获得关联后的k时刻传感器观测信息；
判断所述移动机器人同步定位与建图方法的迭代结果是否发散，获得判断结果；基于所述判断结果，利用k-1时刻移动机器人位姿、k-1时刻估计误差协方差、移动机器人的运动学模型和观测模型对k时刻估计误差协方差进行时间更新，获得k时刻估计误差协方差预测值；
基于所述关联后的k时刻传感器观测信息、所述k时刻移动机器人位姿预测值、所述k时刻估计误差协方差预测值，通过量测更新获得k时刻移动机器人的位姿和k时刻估计误差协方差。
2.根据权利要求1所述的一种基于自适应强跟踪的移动机器人同步定位与建图方法，其特征在于，构建获得的移动机器人的运动学模型和观测模型表示为，
Figure FDA0003852696860000021
式中，zk为k时刻传感器的观测信息，uk为k时刻机器人的输入，f(·)为状态转移函数，h(·)为量测函数，wk-1和vk为互不相关的过程噪声和量测噪声，均服从均值为0、协方差分别为Rk和Qk高斯分布；
Figure FDA0003852696860000022
为k时刻移动机器人的状态信息，
Figure FDA0003852696860000023
为移动机器人在二维空间中的坐标位置，
Figure FDA0003852696860000024
为移动机器人的朝向偏离全局坐标系的夹角。
3.根据权利要求2所述的一种基于自适应强跟踪的移动机器人同步定位与建图方法，其特征在于，所述判断所述移动机器人同步定位与建图方法的迭代结果是否发散，获得判断结果的过程中：
是否发散的判据表达式为，
Figure FDA0003852696860000025
式中，
Figure FDA0003852696860000026
为k时刻的新息序列，
Figure FDA0003852696860000027
为移动机器人在k时刻量测函数在
Figure FDA0003852696860000028
进行泰勒展开并取一阶近似的结果，且
Figure FDA0003852696860000029
κ≥1为可调系数，tr[·]为矩阵求迹运算符，Pk/k-1为k时刻估计误差协方差的预测值；
满足所述判据表达式时，需要引入强跟踪算法抑制滤波发散。
4.根据权利要求3所述的一种基于自适应强跟踪的移动机器人同步定位与建图方法，其特征在于，所述基于所述判断结果，利用k-1时刻移动机器人位姿、k-1时刻估计误差协方差、移动机器人的运动学模型和观测模型对k时刻估计误差协方差进行时间更新，获得k时刻估计误差协方差预测值的过程中，
当滤波已经发散，引入自适应强跟踪算法，计算自适应强跟踪滤波器参数，并对估计误差协方差进行时间更新，包括：
计算新息序列协方差矩阵Vk，表达式为，
Figure FDA0003852696860000031
式中，0＜ρ≤1为遗忘因子；
获取参数矩阵Mk和Nk，表达式为，
Figure FDA0003852696860000032
式中，参数β≥1为弱化因子；
Figure FDA0003852696860000033
为移动机器人在k-1时刻状态转移函数在
Figure FDA0003852696860000034
进行泰勒展开并取一阶近似的结果，
Figure FDA0003852696860000035
为k-1时刻移动机器人的位姿；
λk的计算表达式为，
Figure FDA0003852696860000036
式中，
Figure FDA0003852696860000037
引入自适应强跟踪滤波算法后，估计误差协方差的时间更新公式为，
Figure FDA0003852696860000038
式中，Pk-1|k-1为k-1时刻的估计误差协方差，λkQk-1项可视为当噪声参数失配时对噪声的修正项。
5.根据权利要求4所述的一种基于自适应强跟踪的移动机器人同步定位与建图方法，其特征在于，所述基于所述关联后的k时刻传感器观测信息、所述k时刻移动机器人位姿预测值、所述k时刻估计误差协方差预测值，通过量测更新获得k时刻移动机器人的位姿和k时刻估计误差协方差的过程中，
量测更新的步骤包括：
计算k时刻的卡尔曼滤波增益Kk，计算表达式为
Figure FDA0003852696860000039
计算k时刻机器人的位姿
Figure FDA00038526968600000310
计算表达式为
Figure FDA00038526968600000311
更新估计误差协方差矩阵Pk|k，更新表达式为Pk|k＝[I-KkHk)Pk|k-1；
式中，I为单位矩阵。
6.一种基于自适应强跟踪的移动机器人同步定位与建图***，其特征在于，包括：
模型获取及初始化模块，用于假设同步定位与地图构建过程中的噪声均为加性高斯噪声的前提下，根据移动机器人的底盘样式和传感器的特性构建移动机器人的运动学模型和观测模型；对所述移动机器人的运动学模型和观测模型进行初始化，获取移动机器人在全局坐标系下的初始位姿和初始估计误差协方差；
迭代估计模块，用于基于自适应强跟踪扩展卡尔曼滤波方法对移动机器人的位姿进行迭代估计，实现移动机器人的实时位姿估计和地图构建；
其中，所述基于自适应强跟踪扩展卡尔曼滤波方法对移动机器人的位姿进行迭代估计过程中，k时刻迭代估计的步骤包括，
基于k-1时刻移动机器人位姿和k-1时刻估计误差协方差，利用扩展卡尔曼滤波方法和移动机器人的运动学模型，获得k时刻移动机器人位姿预测值；
获取k时刻传感器对环境路标的观测信息并进行路标数据关联，获得k时刻与k-1时刻的路标关联结果；基于所述路标关联结果，获得关联后的k时刻传感器观测信息；
判断所述移动机器人同步定位与建图方法的迭代结果是否发散，获得判断结果；基于所述判断结果，利用k-1时刻移动机器人位姿、k-1时刻估计误差协方差、移动机器人的运动学模型和观测模型对k时刻估计误差协方差进行时间更新，获得k时刻估计误差协方差预测值；
基于所述关联后的k时刻传感器观测信息、所述k时刻移动机器人位姿预测值、所述k时刻估计误差协方差预测值，通过量测更新获得k时刻移动机器人的位姿和k时刻估计误差协方差。
7.根据权利要求6所述的一种基于自适应强跟踪的移动机器人同步定位与建图***，其特征在于，所述模型获取及初始化模块中，构建获得的移动机器人的运动学模型和观测模型表示为，
Figure FDA0003852696860000051
式中，zk为k时刻传感器的观测信息，uk为k时刻机器人的输入，f(·)为状态转移函数，h(·)为量测函数，wk-1和vk为互不相关的过程噪声和量测噪声，均服从均值为0、协方差分别为Rk和Qk高斯分布；
Figure FDA0003852696860000052
为k时刻移动机器人的状态信息，
Figure FDA0003852696860000053
为移动机器人在二维空间中的坐标位置，
Figure FDA0003852696860000054
为移动机器人的朝向偏离全局坐标系的夹角。
8.根据权利要求7所述的一种基于自适应强跟踪的移动机器人同步定位与建图***，其特征在于，所述迭代估计模块中，实现所述判断所述移动机器人同步定位与建图方法的迭代结果是否发散，获得判断结果的过程中：
是否发散的判据表达式为，
Figure FDA0003852696860000055
式中，
Figure FDA0003852696860000056
为k时刻的新息序列，
Figure FDA0003852696860000057
为移动机器人在k时刻量测函数在
Figure FDA0003852696860000058
进行泰勒展开并取一阶近似的结果，且
Figure FDA0003852696860000059
κ≥1为可调系数，tr[·]为矩阵求迹运算符，Pk/k-1为k时刻估计误差协方差的预测值；
满足所述判据表达式时，需要引入强跟踪算法抑制滤波发散。
9.根据权利要求8所述的一种基于自适应强跟踪的移动机器人同步定位与建图***，其特征在于，所述迭代估计模块中，实现所述基于所述判断结果，利用k-1时刻移动机器人位姿、k-1时刻估计误差协方差、移动机器人的运动学模型和观测模型对k时刻估计误差协方差进行时间更新，获得k时刻估计误差协方差预测值的过程中，
当滤波已经发散，引入自适应强跟踪算法，计算自适应强跟踪滤波器参数，并对估计误差协方差进行时间更新，包括：
计算新息序列协方差矩阵Vk，表达式为，
Figure FDA0003852696860000061
式中，0＜ρ≤1为遗忘因子；
获取参数矩阵Mk和Nk，表达式为，
Figure FDA0003852696860000062
式中，参数β≥1为弱化因子；
Figure FDA0003852696860000063
为移动机器人在k-1时刻状态转移函数在
Figure FDA0003852696860000064
进行泰勒展开并取一阶近似的结果，
Figure FDA0003852696860000065
为k-1时刻移动机器人的位姿；
λk的计算表达式为，
Figure FDA0003852696860000066
式中，
Figure FDA0003852696860000067
引入自适应强跟踪滤波算法后，估计误差协方差的时间更新公式为，
Figure FDA0003852696860000068
式中，Pk-1|k-1为k-1时刻的估计误差协方差，λkQk-1项可视为当噪声参数失配时对噪声的修正项。
10.根据权利要求9所述的一种基于自适应强跟踪的移动机器人同步定位与建图***，其特征在于，所述迭代估计模块中，实现所述基于所述关联后的k时刻传感器观测信息、所述k时刻移动机器人位姿预测值、所述k时刻估计误差协方差预测值，通过量测更新获得k时刻移动机器人的位姿和k时刻估计误差协方差的过程中，
量测更新的步骤包括：
计算k时刻的卡尔曼滤波增益Kk，计算表达式为
Figure FDA0003852696860000069
计算k时刻机器人的位姿
Figure FDA00038526968600000610
计算表达式为
Figure FDA00038526968600000611
更新估计误差协方差矩阵Pk|k，更新表达式为Pk|k＝[I-KkHk)Pk|k-1；
式中，I为单位矩阵。
CN202211137399.9A 2022-09-19 2022-09-19 基于自适应强跟踪的移动机器人同步定位与建图方法及*** Pending CN115328168A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211137399.9A CN115328168A (zh) 2022-09-19 2022-09-19 基于自适应强跟踪的移动机器人同步定位与建图方法及***

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211137399.9A CN115328168A (zh) 2022-09-19 2022-09-19 基于自适应强跟踪的移动机器人同步定位与建图方法及***

Publications (1)

Publication Number Publication Date
CN115328168A true CN115328168A (zh) 2022-11-11

Family

ID=83913740

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211137399.9A Pending CN115328168A (zh) 2022-09-19 2022-09-19 基于自适应强跟踪的移动机器人同步定位与建图方法及***

Country Status (1)

Country Link
CN (1) CN115328168A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115979240A (zh) * 2022-12-05 2023-04-18 哈尔滨理工大学 基于限制增广的新息叠加同步定位建图方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115979240A (zh) * 2022-12-05 2023-04-18 哈尔滨理工大学 基于限制增广的新息叠加同步定位建图方法
CN115979240B (zh) * 2022-12-05 2023-09-29 哈尔滨理工大学 基于限制增广的新息叠加同步定位建图方法

Similar Documents

Publication Publication Date Title
CN109597864B (zh) 椭球边界卡尔曼滤波的即时定位与地图构建方法及***
Guivant et al. Simultaneous localization and map building using natural features and absolute information
CN111207774A (zh) 一种用于激光-imu外参标定的方法及***
CN106772524B (zh) 一种基于秩滤波的农业机器人组合导航信息融合方法
EP3159658A1 (en) Method and device for real-time target location and map creation
CN107084714A (zh) 一种基于RoboCup3D的多机器人协作目标定位方法
Demim et al. A new adaptive smooth variable structure filter SLAM algorithm for unmanned vehicle
Demim et al. Cooperative SLAM for multiple UGVs navigation using SVSF filter
CN112904358A (zh) 基于几何信息的激光定位方法
Tang et al. Robot tracking in SLAM with Masreliez-Martin unscented Kalman filter
CN108303095B (zh) 适用于非高斯***的鲁棒容积目标协同定位方法
CN111291471A (zh) 一种基于l1正则无迹变换的约束多模型滤波方法
Lin et al. Graph-based SLAM in indoor environment using corner feature from laser sensor
CN115328168A (zh) 基于自适应强跟踪的移动机器人同步定位与建图方法及***
CN113709662B (zh) 一种基于超宽带的自主式三维反演定位方法
Burguera et al. Probabilistic sonar scan matching for robust localization
Garcia et al. Portable multi-hypothesis Monte Carlo localization for mobile robots
Font et al. Mobile robot localization. Revisiting the triangulation methods
Tang et al. An improved H-infinity unscented FastSLAM with adaptive genetic resampling
CN115950414A (zh) 一种不同传感器数据的自适应多重融合slam方法
Fabresse et al. Active perception for 3D range-only simultaneous localization and mapping with UAVs
Xu et al. Estimating the odometry error of a mobile robot by neural networks
Goronzy et al. Weighted online calibration for odometry of mobile robots
Wang et al. Simultaneous localization and mapping embedded with particle filter algorithm
Benedettelli et al. Multi-robot SLAM using M-Space feature representation

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