面向大尺度环境中的移动机器人同时定位与地图构建(Simultaneous localization and mapping,SLAM)问题,提出平方根容积Rao-Blackwillised粒子滤波SLAM算法.算法主要特点在于:1)采用容积律计算SLAM中的非线性函数高斯权重积分,达到减小SLAM非线性模型线性化误差、提高SLAM精度的目的;2)在SLAM中直接传播误差协方差矩阵的平方根因子,避免了耗费时间的协方差矩阵分解与重构过程,提高了SLAM计算效率.通过仿真、实验将提出的SLAM算法与FastSLAM2.0、UFastSLAM两种算法进行对比,结果表明本文算法在SLAM性能上优于另两者.
Simultaneous localization and mapping (SLAM) is a key technology for mobile robots operating under unknown environment. While FastSLAM algorithm is a popular solution to the SLAM problem, it suffers from two major drawbacks: one is particle set degeneracy due to lack of observation information in proposal distribution design of the particle filter; the other is errors accumulation caused by linearization of the nonlinear robot motion model and the nonlinear environment observation model. For the purpose of overcoming the above problems, a new iterated sigma point FastSLAM (ISP-FastSLAM) algorithm is proposed. The main contribution of the algorithm lies in the utilization of iterated sigma point Kalman filter (ISPKF), which minimizes statistical linearization error through Gaussian-Newton iteration, to design an optimal proposal distribution of the particle filter and to estimate the environment landmarks. On the basis of Rao-Blackwellized particle filter, the proposed ISP-FastSLAM algorithm is comprised by two main parts: in the first part, an iterated sigma point particle filter (ISPPF) to localize the robot is proposed, in which the proposal distribution is accurately estimated by the ISPKF; in the second part, a set of ISPKFs is used to estimate the environment landmarks. The simulation test of the proposed ISP-FastSLAM algorithm compared with FastSLAM2.0 algorithm and Unscented FastSLAM algorithm is carried out, and the performances of the three algorithms are compared. The simulation and comparing results show that the proposed ISP-FastSLAM outperforms other two algorithms both in accuracy and in robustness. The proposed algorithm provides reference for the optimization research of FastSLAM algorithm.
