Warm-started optimized trajectory planning for ASVs

We consider warm-started optimized trajectory planning for autonomous surface vehicles (ASVs) by combining the advantages of two types of planners: an A* implementation that quickly finds the shortest piecewise linear path, and an optimal control-based trajectory planner. A nonlinear 3-degree-of-freedom underactuated model of an ASV is considered, along with an objective functional that promotes energy-efficient and readily observable maneuvers. The A* algorithm is guaranteed to find the shortest piecewise linear path to the goal position based on a uniformly decomposed map. Dynamic information is constructed and added to the A*-generated path, and provides an initial guess for warm starting the optimal control-based planner. The run time for the optimal control planner is greatly reduced by this initial guess and outputs a dynamically feasible and locally optimal trajectory.