二阶中心差分粒子滤波FastSLAM算法

摘要

为改善机器人同时定位与建图(simultaneous localization and mapping,SLAM)算法中非线性系统状态估计精度不高,计算繁杂的问题,本文创新性地提出了基于二阶中心差分滤波并融合最新观测数据来产生建议分布函数的新算法.新算法基于二阶sterling插值公式处理SLAM中的非线性系统问题,无须计算雅可比矩阵,容易实现.此外,该算法使用Cholesky分解技术,在SLAM概率估计中直接依据协方差平方根因子进行传播,保证协方差矩阵正定性的同时减小了局部线性化的截断误差.仿真试验表明,在粒子数相同的情况下,二阶中心差分FastSLAM(second-order FastSLAM,SOFastSLAM)在不同噪声条件下的估计精度均优于FastSLAM2.0和UFastSLAM算法,且用时最少,证实了SOFastSLAM算法的优越性.