Optimal Model-Based Trajectory Planning With Static Polygonal Constraints

The main contribution of this article is a novel method for planning globally optimal trajectories for dynamical systems subject to polygonal constraints. The proposed method is a hybrid trajectory planning approach, which combines graph search, i.e., a discrete roadmap method, with convex optimization, i.e., a complete path method. Contrary to past approaches, which have focused on using simple obstacle approximations, or suboptimal spatial discretizations, our approach is able to use the exact geometry of polygonal constraints in order to plan optimal trajectories. The performance and flexibility of the proposed method are evaluated via simulations by planning distance-optimal trajectories for a Dubins car model, as well as time-, distance-, and energy-optimal trajectories for a marine vehicle.