Adaptive SLAM Methodology Based on Simulated Annealing Particle Swarm Optimization for AUV Navigation

Abstract

Simultaneous localization and mapping (SLAM) is crucial and challenging for autonomous underwater vehicle (AUV) autonomous navigation in complex and uncertain ocean environments. However, inaccurate time-varying observation noise parameters may lead to filtering divergence and poor mapping accuracy. In addition, particles are easily trapped in local extrema during the resampling, which may lead to inaccurate state estimation. In this paper, we propose an innovative simulated annealing particle swarm optimization-adaptive unscented FastSLAM (SAPSO-AUFastSLAM) algorithm. To cope with the unknown observation noise, the maximum a posteriori probability estimation algorithm is introduced into SLAM to recursively correct the measurement noise. Firstly, the Sage–Husa (SH) based unscented particle filter (UPF) algorithm is proposed to estimate time-varying measurement noise adaptively in AUV path estimation for improving filtering accuracy. Secondly, the SH-based unscented Kalman filter (UKF) algorithm is proposed to enhance mapping accuracy in feature estimation. Thirdly, SAPSO-based resampling is proposed to optimize posterior particles. The random judgment mechanism is used to update feasible solutions iteratively, which makes particles disengage local extreme values and achieve optimal global effects. The effectiveness and accuracy of the proposed algorithm are evaluated through simulation and sea trial data. The average AUV navigation accuracy of the presented SAPSO-AUFastSLAM method is improved by 18.0% compared to FastSLAM, 6.5% compared to UFastSLAM, and 5.9% compared to PSO-UFastSLAM.