Autonomous docking trajectory optimization for unmanned surface vehicle: A hierarchical method

Docking trajectory planning for unmanned surface vehicles is challenging due to complicated obstacle-avoidance in the harbor and highly nonlinear motion equations. In this paper, a two-stage (i.e., searching and optimization) hierarchical method is developed for a safe and efficient solution. In the searching stage, we establish a grid map for the harbor environment. The unmanned surface vehicle is represented by multiple featured discs and an artificially-guided Hybrid A* algorithm is implemented to search a coarse collision-free path. In the optimization stage, we first estimate the total sailing time under time-optimal maneuvering assumptions. Given the discretization scheme, we resample state and control variables along the coarse path by interpolation. Then, we construct a series of safe sailing corridors along the interpolated results. The within-corridor constraints are used to reformulate the optimal control problem, and the interpolated results are used for warm-start. The developed method is numerically validated by two docking scenarios, where high-quality trajectory as well as profiles of control inputs can be obtained with satisfactory computational efficiency. Meanwhile, it shows superiority in robustness over traditional planning techniques in the presence of narrow waterways and concave harbor shores. An animation of the simulation results is available at www.bilibili.com/video/BV14D4y1t7QL/.