Two-Stage Optimized Trajectory Planning for ASVs Under Polygonal Obstacle Constraints: Theory and Experiments

We propose a method for energy-optimized trajectory planning for autonomous surface vehicles (ASVs), which can handle arbitrary polygonal maps as obstacle constraints. The method comprises two stages: The first is a hybrid A* search that finds a dynamically feasible trajectory in a polygonal map on a discretized configuration space using optimal motion primitives. The second stage uses the resulting hybrid A* trajectory as an initial guess to an optimal control problem (OCP) solver. In addition to providing the OCP with a warm start, we use the initial guess to create convex regions encoded as halfspace descriptions, which converts the inherent nonconvex obstacle constraints into a convex and smooth representation. The OCP uses this representation in order to optimize the initial guess within a collision-free corridor. The OCP solves the trajectory planning problem in continuous state space. Our approach solves two challenges related to optimization-based trajectory planning: The need for a dynamically feasible initial guess that can guide the solver away from undesirable local optima and the ability to represent arbitrary obstacle shapes as smooth constraints. The method can take into account external disturbances such as wind or ocean currents. We compare our method to two similar trajectory planning methods in simulation and have found significant computation time improvements. Additionally, we have validated the method in full-scale experiments in the Trondheim harbor area.