A modified FastSLAM with simple particle swarm optimization and consistent mapping for AUVs

Searching an accurate and effective simultaneous localization and mapping (SLAM) algorithm is fundamental to autonomous underwater vehicles (AUVs) to perform robust autonomous navigation. FastSLAM is one of the most popular SLAM algorithms. In spite of its wide applications, it still suffers from two major drawbacks, inconsistency and particle degeneration. In this paper, a modified FastSLAM algorithm is proposed using First-Estimates Jacobian (FEJ) to obtain a more consistent map and innovatively applying simple particle swarm optimization (sPSO) to address the particle degeneration problem. The modified FastSLAM algorithm effectively improves the accuracy of navigation and enhances the consistency of estimation. The paper also evaluates the proposed method with our own research platform, C-Ranger AUV, through sea trials in Tuandao Bay. The results of simulation and sea trial reveal that the modified algorithm has better performance in terms of the accuracy and consistency compared with standard FastSLAM.