基于自适应强跟踪的移动机器人同步定位与建图方法及系统技术方案

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

【技术实现步骤摘要】
基于自适应强跟踪的移动机器人同步定位与建图方法及系统


[0001]本专利技术属于移动机器人同步定位与建图及自主导航
，特别涉及一种基于自适应强跟踪的移动机器人同步定位与建图方法及系统。

技术介绍

[0002]近些年来，随着移动机器人领域相关技术的发展，人们越来越重视拥有自主导航能力并能自主完成设定任务的移动机器人的研发；具备自主导航能力的机器人越来越多的应用在森林火灾检测、快递物流搬运、餐饮店送餐等场景，而实现移动机器人路径规划和自主导航的基础是高精度的机器人位姿估计和精确的环境地图。
[0003]同步定位与地图构建(Simultaneous Localization and Mapping，SLAM)技术，是指移动机器人在未知环境中同时完成自身定位、路标估计以及构建环境地图的过程。根据后端优化策略的不同，SLAM可分为基于贝叶斯滤波和基于非线性优化两类框架；其中，相较于非线性优化算法，贝叶斯滤波算法简单易行，发展也相对成熟。
[0004]现有的扩展卡尔曼滤波(Extended Kalman Filter，EKF)算法通过对非线性系统进行泰勒展开并取一阶近似，实现较简单，已经广泛应用于非线性系统；然而EKF算法由于舍弃了系统的高阶信息，导致机器人位姿估计的精度较低，并且对于模型中不准确的参数没有自适应能力；在EKF基础上提出的强跟踪滤波(Strong Tracking Filter，STF)算法在一定程度上提高了机器人位姿估计精度，但是未能实现对噪声的自适应估计，对机器人运动状态的突变和参数的失配没有良好的自适应能力，并且在实际应用于SLAM问题时容易出现滤波发散现象。综上，亟需一种基于自适应强跟踪的移动机器人同步定位与建图方法。

技术实现思路

[0005]本专利技术的目的在于提供一种基于自适应强跟踪的移动机器人同步定位与建图方法及系统，以解决上述存在的技术问题。本专利技术提供的方法，能够在移动机器人发生运动状态突变或模型参数失配时，通过监测新息的变化对过程噪声进行自适应估计，使移动机器人保持较强的位姿跟踪能力和良好的路标估计精度，能提高移动机器人SLAM算法的鲁棒性。
[0006]为达到上述目的，本专利技术采用以下技术方案：
[0007]本专利技术提供的一种基于自适应强跟踪的移动机器人同步定位与建图方法，包括以下步骤：
[0008]假设同步定位与地图构建过程中的噪声均为加性高斯噪声的前提下，根据移动机器人的底盘样式和传感器的特性构建移动机器人的运动学模型和观测模型；对所述移动机器人的运动学模型和观测模型进行初始化，获取移动机器人在全局坐标系下的初始位姿和初始估计误差协方差；
[0009]基于自适应强跟踪扩展卡尔曼滤波方法对移动机器人的位姿进行迭代估计，实现移动机器人的实时位姿估计和地图构建；
[0010]其中，所述基于自适应强跟踪扩展卡尔曼滤波方法对移动机器人的位姿进行迭代估计过程中，k时刻迭代估计的步骤包括，
[0011]基于k
‑
1时刻移动机器人位姿和k
‑
1时刻估计误差协方差，利用扩展卡尔曼滤波方法和移动机器人的运动学模型，获得k时刻移动机器人位姿预测值；
[0012]获取k时刻传感器对环境路标的观测信息并进行路标数据关联，获得k时刻与k
‑
1时刻的路标关联结果；基于所述路标关联结果，获得关联后的k时刻传感器观测信息；
[0013]判断所述移动机器人同步定位与建图方法的迭代结果是否发散，获得判断结果；基于所述判断结果，利用k
‑
1时刻移动机器人位姿、k
‑
1时刻估计误差协方差、移动机器人的运动学模型和观测模型对k时刻估计误差协方差进行时间更新，获得k时刻估计误差协方差预测值；
[0014]基于所述关联后的k时刻传感器观测信息、所述k时刻移动机器人位姿预测值、所述k时刻估计误差协方差预测值，通过量测更新获得k时刻移动机器人的位姿和k时刻估计误差协方差。
[0015]本专利技术的进一步改进在于，构建获得的移动机器人的运动学模型和观测模型表示为，
[0016][0017]式中，z
k
为k时刻传感器的观测信息，u
k
为k时刻机器人的输入，f(
·
)为状态转移函数，h(
·
)为量测函数，w
k
‑1和v
k
为互不相关的过程噪声和量测噪声，均服从均值为0、协方差分别为R
k
和Q
k
高斯分布；为k时刻移动机器人的状态信息，为移动机器人在二维空间中的坐标位置，为移动机器人的朝向偏离全局坐标系的夹角。
[0018]本专利技术的进一步改进在于，所述判断所述移动机器人同步定位与建图方法的迭代结果是否发散，获得判断结果的过程中：
[0019]是否发散的判据表达式为，
[0020][0021]式中，为k时刻的新息序列，为移动机器人在k时刻量测函数在进行泰勒展开并取一阶近似的结果，且κ≥1为可调系数，tr[
·
]为矩阵求迹运算符，P
k/k
‑1为k时刻估计误差协方差的预测值；
[0022]满足所述判据表达式时，需要引入强跟踪算法抑制滤波发散。
[0023]本专利技术的进一步改进在于，所述基于所述判断结果，利用k
‑
1时刻移动机器人位姿、k
‑
1时刻估计误差协方差、移动机器人的运动学模型和观测模型对k时刻估计误差协方差进行时间更新，获得k时刻估计误差协方差预测值的过程中，
[0024]当滤波已经发散，引入自适应强跟踪算法，计算自适应强跟踪滤波器参数，并对估计误差协方差进行时间更新，包括：
[0025]计算新息序列协方差矩阵V
k
，表达式为，
[0026]式中，0＜ρ≤1为遗忘因子；
[0027]获取参数矩阵M
k
和N
k
，表达式为，
[0028][0029]式中，参数β≥1为弱化因子；为移动机器人在k
‑
1时刻状态转移函数在进行泰勒展开并取一阶近似的结果，为k
‑
1时刻移动机器人的位姿；
[0030]λ
k
的计算表达式为，
[0031]式中，
[0032]引入自适应强跟踪滤波算法后，估计误差协方差的时间更新公式为，
[0033][0034]式中，P
k
‑
1|k
‑1为k
‑
1时刻的估计误差协方差，λ
k
Q
k
‑1项可视为当噪声参数失配时对噪声的修正项。
[0035]本专利技术的进一步改进在于，所述基于所述关联后的k时刻传感器观测信息、所述k时刻移动机器人位姿预测值、所述k时刻估计误差协方差预测值，通过量测更新获得k时刻移动机器人的位姿和k时刻估计误差协方差的过程中，...

【技术保护点】

【技术特征摘要】
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所述的一种基于自适应强跟踪的移动机器人同步定位与建图方法，其特征在于，构建获得的移动机器人的运动学模型和观测模型表示为，式中，z
k
为k时刻传感器的观测信息，u
k
为k时刻机器人的输入，f(
·
)为状态转移函数，h(
·
)为量测函数，w
k
‑1和v
k
为互不相关的过程噪声和量测噪声，均服从均值为0、协方差分别为R
k
和Q
k
高斯分布；为k时刻移动机器人的状态信息，为移动机器人在二维空间中的坐标位置，为移动机器人的朝向偏离全局坐标系的夹角。3.根据权利要求2所述的一种基于自适应强跟踪的移动机器人同步定位与建图方法，其特征在于，所述判断所述移动机器人同步定位与建图方法的迭代结果是否发散，获得判断结果的过程中：是否发散的判据表达式为，式中，为k时刻的新息序列，为移动机器人在k时刻
量测函数在进行泰勒展开并取一阶近似的结果，且κ≥1为可调系数，tr[
·
]为矩阵求迹运算符，P
k/k
‑1为k时刻估计误差协方差的预测值；满足所述判据表达式时，需要引入强跟踪算法抑制滤波发散。4.根据权利要求3所述的一种基于自适应强跟踪的移动机器人同步定位与建图方法，其特征在于，所述基于所述判断结果，利用k
‑
1时刻移动机器人位姿、k
‑
1时刻估计误差协方差、移动机器人的运动学模型和观测模型对k时刻估计误差协方差进行时间更新，获得k时刻估计误差协方差预测值的过程中，当滤波已经发散，引入自适应强跟踪算法，计算自适应强跟踪滤波器参数，并对估计误差协方差进行时间更新，包括：计算新息序列协方差矩阵V
k
，表达式为，式中，0＜ρ≤1为遗忘因子；获取参数矩阵M
k
和N
k
，表达式为，式中，参数β≥1为弱化因子；为移动机器人在k
‑
1时刻状态转移函数在进行泰勒展开并取一阶近似的结果，为k
‑
1时刻移动机器人的位姿；λ
k
的计算表达式为，式中，引入自适应强跟踪滤波算法后，估计误差协方差的时间更新公式为，式中，P
k
‑
1|k
‑1为k
‑
1时刻的估计误差协方差，λ
k
Q
k
‑1项可视为当噪声参数失配时对噪声的修正项。5.根据权利要求4所述的一种基于自适应强跟踪的移动机器人同步定位与建图方法，其特征在于，所述基于所述关联后的k时刻传感器观测信息、所述k时刻移动机器人位姿预测值、所述k时刻估计误差协方差预测值，通过量测更新获得k时刻移动机器人的位姿和k时刻估计误差协方差的过程中，量测更新的步骤包括：计算k时刻的卡尔曼滤波增益K
k
，计算表达式为计算k时刻机器人的位姿计算表达式为更新估计误差协方差矩阵P
k|k
，更新表达式为P
k|k
＝[I
‑
K
k
H
k
)P
k|k
‑1；式中，I为单位矩阵。
6.一种基于自适应强跟踪的移动机器人同步定位与建图系统，其特征在于，...

【专利技术属性】
技术研发人员：蔡远利，王小彤，姜浩楠，
申请(专利权)人：西安交通大学，
类型：发明
国别省市：