A Tightly Coupled Inertial-Sonar Fusion for Localization of Underwater Robots

Abstract

This paper proposes a tightly coupled fusion method for inertial and forward-looking sonar (FLS) data, integrating underwater image observations from the FLS into the inertial odometry for underwater robot localization. Since the FLS images provide only horizontal plane information, this work focuses on the positioning of the underwater robot in a 2D plane. In underwater navigation systems, relying solely on inertial measurements often leads to error accumulation and suboptimal localization results, as demonstrated in previous studies. To address this issue, we integrate the FLS data into the inertial odometry. Specifically, we convert the sonar images into 2D underwater point clouds and use an Error State Kalman Filter (ESKF) to fuse Inertial Measurement Unit (IMU) data with the sonar point cloud data for joint estimation of the initial pose. Next, edge feature point clouds are extracted from the sonar images using a horizontal scanning method. Finally, by constructing edge feature error terms, we constrain the relative position changes between two adjacent sonar frames. Through experiments in an underwater simulation environment (Dave) and a real pool, the results show that the proposed fusion method can significantly improve the localization accuracy of underwater robots compared to using inertial odometers and sonar odometers alone.