Improving Localization Accuracy for an Underwater Robot With a Slow-Sampling Sonar Through Graph Optimization

This paper proposes a novel localization algorithm for an autonomous underwater vehicle equipped with a mechanical scanning sonar that has a slow frequency of data sampling. The proposed approach incrementally constructs a pose graph and conducts graph optimization to correct the robot poses. The construction of a pose graph has three stages: 1) scan generation which incorporates an extended Kalman filter-based dead reckoning algorithm that takes the robot motion into account while eliminating the sonar scan distortion caused by the motion; 2) data association which is based on Mahanalobis distance and shape matching for determining loop closures; and 3) scan matching which calculates constraints constructs pose graph. The constructed pose graph is then fed into a graph optimizer to find the optimal poses corresponding to each scan. A trajectory correction module uses these optimized poses to correct intermediate poses during the process of scan generation. Both simulation and practical experiments are conducted to verify the viability and accuracy of the proposed algorithm.