Robust bathymetric SLAM algorithm considering invalid loop closures

Abstract Autonomous underwater vehicles (AUVs) are important tools for topography mapping and geological surveys, and bathymetric simultaneous localization and mapping (BSLAM) method holds high potential for accurate navigation in long endurance AUV missions. However, invalid loop closures caused by nearly flat seabed topography may cause BSLAM algorithm fail catastrophically. In this paper, a robust BSLAM algorithm is proposed, whose robustness is guaranteed by both a residual-based loop closure identification method and a robust filter algorithm. In residual-based loop closure identification, a Gini impurity based on probabilistic labels is proposed in the random forest (RF) method to classify invalid and valid loop closures; meanwhile, a posteriori state estimate function of the vehicle considering the stability of robust BSLAM algorithm is proposed to generate both stable and accurate navigational results in robust filter. The play-back experiments have shown that robust BSLAM algorithm can provide accurate and stable navigational results in real-time.