Effective cubature FastSLAM: SLAM with Rao-Blackwellized particle filter and cubature rule for Gaussian weighted integral

Abstract

AbstractSimultaneous localization and mapping (SLAM) is a key technology for mobile robot autonomous navigation in unknown environments. While FastSLAM algorithm is a popular solution to the large-scale SLAM problem, it suffers from two major drawbacks: one is particle set degeneracy due to lack of measurements in proposal distribution of particle filter; the other is errors accumulation caused by inaccurate linearization of the nonlinear robot motion model and the environment measurement model. To overcome the problems, a new Jacobian-free cubature FastSLAM (CFastSLAM) algorithm is proposed in this paper. The main contribution of the algorithm lies in the utilization of third-degree cubature rule, which calculates the nonlinear transition density of Gaussian prior more accurately, to design an optimal proposal distribution of the particle filter and to estimate the Gaussian densities of the feature landmarks. On the basis of Rao-Blackwellized particle filter, the proposed algorithm is comprised by two main parts: in the first part, a cubature particle filter (CPF) is derived to localize the robot; in the second part, a set of cubature Kalman filters is used to estimate environment landmarks. The performance of the proposed algorithm is investigated and compared with that of FastSLAM2.0 and UFastSLAM in simulations and experiments. Results verify that the CFastSLAM improves the SLAM performance.Keywords: mobile robotSLAMcubature ruleRao-Blackwellized particle filterKalman filter AcknowledgmentsThis work was jointly supported by State Key Laboratory of Robotics and System of HIT (SKLRS-2009-ZD-04), National Nature Science Foundation of China (60909055, 61005070), China Postdoctoral Science Foundation Special Funded Project (201003144, 201104160), and Fundamental Research Funds for the Central Universities of China (2012JBM017).