Degeneracy-Aware Imaging Sonar Simultaneous Localization and Mapping

Abstract

High-frequency imaging sonar sensors have recently been applied to aid underwater vehicle localization, by providing frame-to-frame odometry measurements or loop closures over large timescales. Previous methods have often assumed a planar environment, thereby restricting the use of such algorithms mostly to seafloor mapping. We propose an algorithm to generate pose-to-pose constraints for pairs of sonar images, which may also be applied to larger sets of images, that makes no assumptions about the environmental geometry. The algorithm is sensitive to the inherent degeneracies of the imaging sonar sensor model, and may be tuned to tradeoff between providing more constraints on the sensor motion and not overfitting to noise in the measurements. For real-time localization, we fuse the resulting pair-wise sonar pose constraints with vehicle odometry in a pose graph optimization framework. We rigorously evaluate the proposed method and demonstrate improvement in accuracy over previously proposed formulations both in simulation and real-world experiments.