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

Abstract

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.

An improved real-time collision-avoidance algorithm based on Hybrid A* in a multi-object-encountering scenario for autonomous surface vessels

“…The variant, Hybrid A* (HA*), unlike conventional variants that only allow visiting centers, corners, or edges of grid cells, associates with each cell a continuous state of vehicles (Dolgov et al, 2008). Bitar et al (2020) detailed a two-stage trajectory planner, where the initial step uses a discrete polygonal representation of the configuration space, such that a Hybrid A* algorithm can compute an initial dynamically feasible trajectory. Ship dynamics can be considered during the graph search.…”

Grounding-aware RRT* for Path Planning and Safe Navigation of Marine Crafts in Confined Waters

“…Chen et al (2020) used a binary occupancy grid along with the Fast Marching Method, and Singh et al (2018) similarly with the A* algorithm. Bitar et al (2020) detailed a two-stage trajectory planner, where the initial step uses a discretized polygonal representation of the configuration space, such that a hybrid A* algorithm can compute an initial dynamically feasible trajectory. Xue et al (2011) generated potential functions from arbitrary polygons, as well as satellite images, for use with an Artificial Potential Fields algorithm.…”

Ship trajectory planning method for reproducing human operation at ports

“…The first candidate trajectory was made, subsequently, it was improved based on optimality. Furthermore, Bitar et al (2020) considered maneuver energy and performed trajectory planning in ports with external disturbances.…”