A Robust INS/USBL/DVL Integrated Navigation Algorithm Using Graph Optimization

Abstract

The Autonomous Underwater Vehicle (AUV) is usually equipped with multiple sensors, such as an inertial navigation system (INS), ultra-short baseline system (USBL), and Doppler velocity log (DVL), to achieve autonomous navigation. Multi-source information fusion is the key to realizing high-precision underwater navigation and positioning. To solve the problem, a fusion scheme based on factor graph optimization (FGO) is proposed. Due to multiple iterations and joint optimization of historical data, FGO could usually show a better performance than the traditional Kalman filter. In addition, considering that USBL and DVL are usually heavily influenced by the environment, outliers are often present. A robust integrated navigation algorithm based on a maximum correntropy criterion and FGO scheme is proposed. The proposed algorithm solves the problem of multi-sensor fusion and non-Gaussian noise. Numerical simulations and field tests demonstrate that the proposed FGO scheme shows a better performance and robustness than the traditional Kalman filter. Compared with the traditional Kalman filtering, the positioning accuracy is improved by 5.3%, 9.1%, and 5.1% in the east, north, and height directions. It can realize a more accurate navigation and positioning of underwater multi-sensors.