Safe and Optimal Motion Planning for Autonomous Underwater Vehicles: A Robust Model Predictive Control Framework Integrating Fast Marching Time Objectives and Adaptive Control Barrier Functions

Autonomous Underwater Vehicles (AUVs) have shown significant promise across various underwater applications, yet face challenges in dynamic environments due to the limitations of traditional motion planning methods while Artificial Potential Field (APF)-based control barrier functions focus solely on obstacle proximity and distance-based methods oversimplify obstacle geometries, and both fail to ensure safety and satisfy turning radius constraints for under-actuated AUVs in intricate environments. This paper proposes a robust Model Predictive Control (MPC) framework integrating an enhanced fast marching control barrier function, specifically designed for AUVs equipped with fully directional sonar systems. The framework introduces a novel improvement for moving obstacles by extending the control barrier function field propagation along the obstacle’s movement direction. This enhancement generates precise motion plans that ensure safety, satisfy kinematic constraints, and effectively handle static and dynamic obstacles. Simulation results demonstrate superior obstacle avoidance and motion planning performance in complex scenarios, with key outcomes including a minimum safety margin of 1.86 m in cluttered environments (vs. 0 m for A* and FMM) and 1.76 m in dynamic obstacle scenarios (vs. 0.13 m for MPC-APFCBF), highlighting the framework’s ability to enhance navigation safety and efficiency for real-world AUV deployments in unpredictable marine environments.