A Factor Graph Method for AUV Navigation in the Mobile Docking Progress

Navigation and localization in underwater environments for autonomous underwater vehicle (AUV) are particularly essential in the autonomous docking process. Because of unavailability of global positioning system (GPS) in the underwater, AUV need to estimate its attitude and position utilized proprioceptive sensors and external sensors on board. Past estimation algorithms are mostly based on filter approaches, such as Extended Kalman Filter (EKF), and few optimization-based methods. In this paper, we proposed a novel AUV navigation algorithm based on factor graph in the mobile docking process. We consider AUV kinetics model and measurements as factors adding to the factor graph, mobile object as a mobile landmark and add its motion model as a factor to the factor graph, then optimize the factor graph. Then we proposed a batch optimization approach which plays a trade between computing and accuracy. The simulation and pool experimental results both show the feasibility and accuracy of the algorithm.