Bathymetric Pose Graph Optimization with Regularized Submap Matching

The quality of sonar seabed mapping performed using autonomous underwater vehicles depends on the accuracy of the vehicle trajectory estimation. To reduce the accumulated pose estimation errors from dead reckoning, bathymetry observations from sonar sensors are often exploited within the framework of pose graph optimization, while the submaps of the seafloor are used to add loop-closure constraints to the pose graph by iterative closest point. However, matching with the submaps suffers from local minima because the seafloor is mostly flat and featureless. To resolve this issue, we regularized the sub-maps to enhance the spatial variations in the vertical direction; thus, we realized improved matching accuracy. Given these constraints, the pose graph can be optimized in real time and provide a corrected trajectory. The performance of the proposed method is validated through experiments using a surface vessel where the same navigation and sonar systems as used in the underwater vehicles are installed, in addition to a GPS receiver for ground truth acquisition.