一种基于不变拓展卡尔曼滤波的移动机器人SLAM导航方法技术

本发明专利技术公开了一种基于不变拓展卡尔曼滤波的移动机器人SLAM导航方法，包括建立移动机器人的运动方程和观测方程、对移动机器人状态进行初始化、对移动机器人的状态进行预测、获得预测观测值、获得更新过程，利用预测值和观测值来更新系统状态向量及其协方差矩阵，并循环上述步骤获得移动机器人新时刻的状态估计值，实现系统矩阵与状态估计值的独立，扩展了保证状态和协方差收敛的状态空间，同时改善了滤波器发散和不一致性的问题，使得移动机器人SLAM导航有更好的收敛性和一致性，定位和建图的精度得到提高。的精度得到提高。的精度得到提高。

【技术实现步骤摘要】
一种基于不变拓展卡尔曼滤波的移动机器人SLAM导航方法


[0001]本专利技术涉及机器人自主导航
，具体为一种基于不变拓展卡尔曼滤波的移动机器人SLAM导航方法。

技术介绍

[0002]SLAM可以描述为移动机器人在一个完全未知的环境中，从未知位置开始递增式的移动，在移动过程中机器人通过自身所携带的传感器获取的环境观测值来构建环境地图，同时利用构建的环境地图更新自身在环境。在众多SLAM的算法中基于扩展卡尔曼滤波的SLAM算法使用最为普遍。传统EKF的系统矩阵依赖当前状态估计值，在有噪声引入时，系统对状态估计值的预测是不准确的，这就导致很难对系统方程做收敛性分析。当状态估计值与真值差距较大时，直接导致也有较大偏差，若使用这样的继续计算，将进一步放大误差，使系统形成正反馈，最终导致滤波器发散。此外，EKF还存在不一致性问题。即EKF的更新步骤是通过旧状态的线性化来计算当前状态的协方差矩阵，但是其与当前状态协方差矩阵的实际值并不一致，所以急需一种基于不变拓展卡尔曼滤波的移动机器人SLAM导航方法来解决上述问题。
专利
技术实现思路

本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于不变拓展卡尔曼滤波的移动机器人SLAM导航方法，其特征在于，包括如下步骤：S1、建立移动机器人的运动方程和观测方程；S2、对移动机器人状态进行初始化；S3、根据移动机器人的状态转移矩阵，通过移动机器人前一时刻的状态量预测下一时刻的状态量；S4、利用传感器获得实际观测值，利用观测方程获得预测观测值；S5、通过预测值和观测值来更新移动机器人状态向量及其协方差矩阵；S6、循环迭代步骤S3至步骤S5，获得移动机器人新时刻的状态估计值。2.根据权利要求1所述的一种基于不变拓展卡尔曼滤波的移动机器人SLAM导航方法，其特征在于：在步骤S1中，移动机器人的运动方程X
v,k
和观测方程z
k
为：为：为：其中，X
v,k
为k时刻移动机器人的位姿信息；u
k
是控制向量；z
k
是观测向量；f(
·
)代表机器人运动系统的状态传递函数；h(
·
)代表非线性测量函数；ε
k
和ε
j
表示移动机器人的控制噪声，该控制噪声是均值为零，协方差矩阵为Q
k
，是ε
j
的转置；η
k
表示激光雷达的观测噪声，该噪声是均值为零，协方差矩阵为R
k
，控制噪声和观测噪声相互独立，δ
kj
为单位脉冲信号。3.根据权利要求2所述的一种基于不变拓展卡尔曼滤波的移动机器人SLAM导航方法，其特征在于：移动机器人观测测路标的位置向量为：其中，N为环境中路标总数。4.根据权利要求3所述的一种基于不变拓展卡尔曼滤波的移动机器人SLAM导航方法，其特征在于：在步骤S2中，移动机器人状态初始化：征在于：在步骤S2中，移动机器人状态初始化：其中，为移动机器人的状态向量，P0为状态向量协方差矩阵。5.根据权利要求4所述的一种基于不变拓展卡尔曼滤波的移动机器人SLAM导航方法，其特征在于：在步骤S3中，预测具体为：
其中，为移动机器人k
‑
1...

【专利技术属性】
技术研发人员：佟钢，
申请(专利权)人：北京开心编程科技有限公司，
类型：发明
国别省市：