一种基于SR-UKF的FastSLAM算法

摘要

标准FastSLAM算法存在着粒子集退化和线性化误差累积的缺陷.针对上述问题,提出了基于平方根无迹卡尔曼滤波(SR-UKF)的FastSLAM算法.SR-UKF选取一组能够代表状态向量统计特性的代表点带入非线性函数处理后重新构建出新的统计特性；使用SR-UFK取代EKF来估计每个粒子的后验位姿提议分布,可以提高粒子采样精度,减缓粒子集的退化；同时SR-UKF可以确保协方差矩阵的非负定,保证了SLAM算法的稳定性.仿真实验结果表明,基于SR-UKF的FastSLAM算法在估计精度和鲁棒性两方面均优于FastSLAM 2.0算法.%Standard FastSLAM algorithm suffers from particle set degeneracy and accumulation errors caused by linearization of the nonlinear model. To overcome the above problems, this paper proposed a novel FastSlam algorithm based on square root unscented Kalman filter(SR-UKF). SR-UKF selected a group of representative sigma points to approximate the covariance, these sigma points were propageted through the non-linearforce model to reconstruct the new statistical characteristics. Using SR-UKF to replace EKF for posteriori estimation of particles could reduce the linearization error and slow down particle set degeneracy. SR-UKF ensured the non-negative definite of covariance matrix to guarantee the stability of SLAM algorithm. The simulation experiments demonstrate that the proposed algorithm is better than FastSLAM 2. 0 both in accuracy and robustness.