Hierarchical Dynamic Trajectory Planning for Autonomous Underwater Vehicles: Algorithms and Experiments

Since Autonomous Underwater Vehicles (AUVs) generally have the limited sensing range and endurance, dynamic real-time trajectory planning is essential in the complex marine environment. In this paper, we propose a dynamic trajectory planning method for AUVs in a hierarchical and receding horizon way. Hierarchically, trajectory optimizations are conducted sequentially to maintain the high computational efficiency. Within each planning horizon, a safe global path is first searched for and the path is then optimized over a short horizon within the limited sensing range. Particularly, for the global path search, the hybrid A* algorithm is employed to find a collision-free path while considering the kinematic constraints of the AUV. To enhance the smoothness and safety of the path, a nonlinear optimization problem is further solved to refine the path over the short horizon. To further guarantee the feasibility of the planned path for the AUV, the inverse dynamics in the virtual domain (IDVD) method is proposed to perform a joint spatio-temporal optimization to obtain the feasible AUV velocity and acceleration trajectories. To validate the proposed trajectory planning method, both simulations and experiments with a newly developed flat disk-shaped AUV, Stingray II, are conducted. It is demonstrated that the proposed method is capable of efficient online trajectory planning in unknown and complex environments for AUVs.