RBPF-MSIS: Towards Rao-Blackwellized Particle Filter SLAM for Autonomous Underwater Vehicle with Slow Mechanical Scanning Imaging Sonar

Simultaneous Localization and Mapping (SLAM)has the potential to play a fundamental and significant role in achieving autonomy for Autonomous Underwater Vehicles (AUV). This paper proposes a Rao-Blackwellized Particle Filter (RBPF) SLAM algorithm for an AUV equipped with a Mechanically Scanning Imaging Sonar (MSIS) that has a very slow scanning frequency. To tackle the issues of scan distortion and sonar data sparseness caused by the slow-sampling MSIS, the core of the algorithm is a carefully designed sliding window based scan forming module. Then the formed scans are fed into the modified RBPF to build a consistent grid-based map thus localizing the AUV accurately. Extensive simulation and experiments are carried out to verify the proposed algorithm. The results show that the proposed algorithm outperforms existing ones in terms of the level of map consistency with the environment as well as the accuracy of pose estimation.