Multi-sensor fusion is an effective way to enhance the positioning
performance of autonomous underwater vehicles (AUVs). However, underwater
multi-sensor fusion faces challenges such as heterogeneous frequency and
dynamic availability of sensors. Traditional filter-based algorithms suffer
from low accuracy and robustness when sensors become unavailable. The factor
graph optimization (FGO) can enable multi-sensor plug-and-play despite data
frequency. Therefore, we present an FGO-based strapdown inertial navigation
system (SINS) and long baseline location (LBL) system tightly coupled
navigation system (FGO-ILNS). Sensors such as Doppler velocity log (DVL),
magnetic compass pilot (MCP), pressure sensor (PS), and global navigation
satellite system (GNSS) can be tightly coupled with FGO-ILNS to satisfy
different navigation scenarios. In this system, we propose a floating LBL slant
range difference factor model tightly coupled with IMU preintegration factor to
achieve unification of global position above and below water. Furthermore, to
address the issue of sensor measurements not being synchronized with the LBL
during fusion, we employ forward-backward IMU preintegration to construct
sensor factors such as GNSS and DVL. Moreover, we utilize the marginalization
method to reduce the computational load of factor graph optimization.
Simulation and public KAIST dataset experiments have verified that, compared to
filter-based algorithms like the extended Kalman filter and federal Kalman
filter, as well as the state-of-the-art optimization-based algorithm ORB-SLAM3,
our proposed FGO-ILNS leads in accuracy and robustness