An autonomous navigation algorithm for underwater vehicles based on inertial measurement units and sonar

The underwater vehicles technology has been developed rapidly in recent years. In the past years, the method of Simultaneous Localization and Mapping (SLAM) has prevailed and provided a feasible way of autonomous navigation. This paper introduces an algorithm based on Inertial Measurement Units (IMU) and sonar for autonomous navigation of underwater vehicles, which is called Inertial-SLAM algorithm. The algorithm uses a hybrid Rao-Blackwellised SLAM algorithm to estimate the velocity, position, attitude of the underwater vehicle, the bias errors of the Inertial Measurement Units as well as the features map around the underwater vehicle. Inertial-SLAM algorithm is a combination of Inertial Navigation System (INS) and particle filter-based SLAM algorithm. Both the time complexity and the estimation error of this algorithm are small. The simulation results of the Inertial-SLAM algorithm are given in the paper.