A Robust Graph-Based Bathymetric Simultaneous Localization and Mapping Approach for AUVs

Due to the position drift of inertial navigation systems, it is still challenging to achieve long-term and accurate position estimates during underwater navigation. The seabed topography has been proven to be effective in aiding information for accurate positioning benefiting from its rich spatial variation. With the advantage of the multibeam echosounder (MBES) in efficient bathymetric survey, the simultaneous localization and mapping (SLAM) approach can be performed using bathymetric data in unknown environments for underwater vehicles to get good position estimates. The SLAM performance relies on the number and accuracy of loop closures heavily. Thereby, the capabilities of the data association method and solver in dealing with the uncertainties of vehicle pose estimates, bathymetric data, and topographic features affect the SLAM performance strongly. This work proposes a new graph-based bathymetric SLAM method to improve the robustness of the uncertainties in both factor-graph construction and optimization stages. In the front end, on the base of a matching suitability-based MBES submap construction method, a dual-stage bathymetric point cloud registration approach that is able to detect most false loop closures is proposed. In the back end, a robust optimizer based on Frechet distance is introduced to further identify and remove the false loop closures missed in front end. Experiments using field MBES bathymetric data sets are conducted to verify the effectiveness of the proposed approach.