基于限制增广的新息叠加同步定位建图方法技术

基于限制增广的新息叠加同步定位建图方法，解决了如何提高移动机器人定位建图过程中精度和速度的问题，属于移动机器人定位建图领域。本发明专利技术包括：移动机器人k时刻扫描环境及获取移动机器人当前位姿，确定观测值，根据观测值确定k时刻观测向量的维数n,并获取k

【技术实现步骤摘要】
基于限制增广的新息叠加同步定位建图方法


[0001]本专利技术涉及一种基于限制增广的新息叠加同步定位建图方法，属于移动机器人定位建图领域。

技术介绍

[0002]随着计算机技术的快速发展和开源软件的兴起，机器人技术得以长足发展，尤其是移动机器人已经在各个领域得到应用，如家政服务、室外巡检救援、交通物流、医学机械、教育等，都发挥着十分重要的作用。定位建图一直是移动机器人导航领域的研究热点，也是移动机器人真正意义上实现自主作业的一个重要条件。SLAM算法可以确定移动机器人自身在环境中的位置，同时可以确定其周围的环境情况。SLAM算法可以看成是对移动机器人自身位姿以及周围环境特征的估计算法。
[0003]扩展卡尔曼滤波(Extended Kalman Filter,EKF)是目前机器人导航领域中一种较为经典的SLAM方法，其保留了传统卡尔曼滤波(Kalman Filter,KF)可以融合观测数据和估计数据的特性，并且在实际应用中移动机器人运动模型和观测模型为非线性时，它仍可以对移动机器人位姿和周围环境进行估计。但标准扩展卡尔曼滤波存在一些局限性，标准扩展卡尔曼滤波是将观测噪声和过程噪声当作已知量进行计算；但在实际情况中，系统的观测噪声和过程噪声是无法得知的，随着滤波次数的增加，滤波器会出现一种发散现象，即估计值和实际值之间的误差会越来越大。自适应扩展卡尔曼滤波(Adaptive Extended Kalman Filter，AEKF)在标准扩展卡尔曼滤波的基础上引入渐消因子来实时修正观测噪声和过程噪声，保证算法收敛，但是在加入渐消因子后会导致算法的精度有所降低。新息叠加自适应扩展卡尔曼滤波(Innovation Superposed Adaptive Extended Kalman Filter，IS
‑
AEKF)算法通过将单个时刻新息扩展为多个时刻新息矩阵，有效利用相邻时刻信息，使得IS
‑
AEKF同步定位建图算法精度更高，即算法对移动机器人的状态估计以及对环境中路标点的特征估计更加准确。但移动机器人运动过程中，随着观测到路标点数量的增加，系统状态向量的维数不断增加，会导致算法运算速度降低。

技术实现思路

[0004]针对如何提高移动机器人定位建图过程中精度和速度的问题，本专利技术提供一种基于限制增广的新息叠加同步定位建图方法。
[0005]本专利技术提供一种基于限制增广的新息叠加同步定位建图方法，包括：
[0006]S1、移动机器人k时刻扫描环境及获取移动机器人当前位姿，确定观测值，观测值包括k时刻移动机器人的位姿和扫描到的路标点根据观测值确定k时刻观测向量的维数n,并获取k
‑
1时刻的状态向量的维数
[0007]S2、当且观测值中的路标点与移动机器人之间距离β＞β
max
时，删除该路标点信息，β
max
为传感器最大观测范围，转入S3；当且β≤β
max
或时，转入S3；
[0008]S3、判断观测值中路标点是否在k
‑
1时刻的状态向量中，若是，则利用观测值及k
‑
1时刻的状态向量采用新息叠加自适应扩展卡尔曼滤波同步定位建图方法更新k时刻移动机器人的状态向量，若否，将路标点加入地图，同时加入至k
‑
1时刻的状态向量中，利用观测值Z
k
及k
‑
1时刻的状态向量采用新息叠加自适应扩展卡尔曼滤波同步定位建图方法更新k时刻移动机器人的状态向量；
[0009]S3中，利用观测值Z
k
及k
‑
1时刻的状态向量采用新息叠加自适应扩展卡尔曼滤波同步定位建图方法更新k时刻移动机器人的状态向量，包括：
[0010]S31、获取移动机器人在SLAM系统中的运动方程；
[0011]S32、根据移动机器人在SLAM系统中的运动方程，建立移动机器人SLAM观测方程；
[0012]S33、预测和
[0013]表示状态向量预测值，表示状态向量预测值的协方差矩阵；
[0014]S34、根据观测值Z
k
获得新息将多个时刻的新息叠加，得到观测向量
[0015][0016][0017]H
k
表示h(
·
)对移动机器人状态求偏导得到的雅克比矩阵，表示k
‑
1时刻状态向量的预测值，表示移动机器人观测噪声估计值的均值；Z
k
表示观测值；
[0018]S35、根据和更新移动机器人的状态向量
[0019][0020]表示k时刻状态向量的预测值，是根据k
‑
1时刻的状态向量获得的；
[0021]卡尔曼增益矩阵
[0022]表示状态向量预测值协方差矩阵；Z
k
＝h(X
k
)+v
k
，h(
·
)表示非线性机器人系统的测量函数，v
k
表示非线性机器人系统的观测噪声，X
k
表示状态实际值；
[0023]k时刻观测噪声估计值的协方差矩阵
[0024][0025]d
k
表示加权系数，P
k
表示状态向量后验估计的协方差；
[0026][0027]作为优选，S34中，
[0028][0029][0030]W表示f(
·
)对X
k
求偏导得到的雅各比矩阵；
[0031]f(
·
)表示非线性机器人系统状态转移函数；
[0032]X
k
＝f(X
k
‑1,u
k
)+w
k
，u
k
表示非线性机器人系统的输入控制量，w
k
表示非线性机器人系统的过程噪声；
[0033]过程噪声估计值的协方差矩阵过程噪声估计值的协方差矩阵
[0034]θ
k
‑1分别表示k
‑
1时刻机器人运动方向与x轴夹角，v
k
表示k时刻机器人线速度，ω
k
表示k时刻机器人角速度，Δt表示k
‑
1时刻到k时刻所经过的时间；
[0035][0036]x
k
、y
k
分别表示k时刻移动机器人x轴坐标和y轴坐标，分别表示k时刻传感器观测到第i个路标点的x轴坐标和y轴坐标，器观测到第i个路标点的x轴坐标和y轴坐标，
[0037]作为优选，加权系数d
k
为：
[0038][0039]b表示渐消因子，0＜b＜1，b
k+1
表示b的k+1次幂。
[0040]作为优选，移动机器人在SLAM系统中的运动方程为：
[0041][0042]x
k
、本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.基于限制增广的新息叠加同步定位建图方法，其特征在于，所述方法包括：S1、移动机器人k时刻扫描环境及获取移动机器人当前位姿，确定观测值，观测值包括k时刻移动机器人的位姿和扫描到的路标点根据观测值确定k时刻观测向量的维数n,并获取k
‑
1时刻的状态向量的维数S2、当且观测值中的路标点与移动机器人之间距离β＞β
max
时，删除该路标点信息，β
max
为传感器最大观测范围，转入S3；当且β≤β
max
或时，转入S3；S3、判断观测值中路标点是否在k
‑
1时刻的状态向量中，若是，则利用观测值及k
‑
1时刻的状态向量采用新息叠加自适应扩展卡尔曼滤波同步定位建图方法更新k时刻移动机器人的状态向量，若否，将路标点加入地图，同时加入至k
‑
1时刻的状态向量中，利用观测值Z
k
及k
‑
1时刻的状态向量采用新息叠加自适应扩展卡尔曼滤波同步定位建图方法更新k时刻移动机器人的状态向量；S3中，利用观测值Z
k
及k
‑
1时刻的状态向量采用新息叠加自适应扩展卡尔曼滤波同步定位建图方法更新k时刻移动机器人的状态向量，包括：S31、获取移动机器人在SLAM系统中的运动方程；S32、根据移动机器人在SLAM系统中的运动方程，建立移动机器人SLAM观测方程；S33、预测和和表示状态向量预测值，表示状态向量预测值的协方差矩阵；S34、根据观测值Z
k
获得新息将多个时刻的新息叠加，得到观测向量将多个时刻的新息叠加，得到观测向量将多个时刻的新息叠加，得到观测向量H
k
表示h(
·
)对移动机器人状态求偏导得到的雅克比矩阵，表示k
‑
1时刻状态向量的预测值，表示移动机器人观测噪声估计值的均值；Z
k
表示观测值；S35、根据和更新移动机器人的状态向量更新移动机器人的状态向量更新移动机器人的状态向量表示k时刻状态向量的预测值，是根据k
‑
1时刻的状态向量获得的；卡尔曼增益矩阵卡尔曼增益矩阵表示状态向量预测值协方差矩阵；Z
k
＝h(X
k
)+v
...

【专利技术属性】
技术研发人员：杨柳，李春晖，徐东昊，李东洁，
申请(专利权)人：哈尔滨理工大学，
类型：发明
国别省市：