An adaptive grouping sonar-inertial odometry for underwater navigation

Abstract

This paper presents a sonar-inertial navigation system with an adaptive grouping optimization framework, which tightly couples inertial measurements with acoustic data from forward-looking sonar (FLS). The system’s front-end uses the sonar-inertial constraint-based feature matching (SICFM) to increase the accuracy and robustness of data association. SICFM is designed by incorporating two novel measures. Firstly, it combines prior motion information to refine the feature search region. Then, a Gaussian-gamma mixture model is formulated to reduce the impact of speckle noise on the selection of matching points. In the back-end, an adaptive grouping factor graph optimization framework is established. A frame manager is designed using the solvability of motion estimation to dynamically group sonar frames, thereby ensuring stable feature tracking for factor graph optimization. Pre-integration-based inertial measurement factor and reprojection-based sonar measurement factor are constructed as constraints in the optimization, which reduces the dependence on prior knowledge of the scene. Furthermore, an elevation angle parameterization is introduced, aiming at the nonlinear projection of sonar features. The evaluation is presented in simulation and real-world experiments, which validate our proposed algorithm in terms of robust localization with high accuracy.