Efficient Formulation of Collision Avoidance Constraints in Optimization Based Trajectory Planning and Control

Abstract

To be applicable to real world scenarios trajectory planning schemes for mobile autonomous systems must be able to efficiently deal with obstacles in the area of operation. In the context of optimization based trajectory planning and control a number of different approaches to formulate collision avoidance constraints can be found in the literature. Here the contribution of the present work is twofold. First, the most popular methods to represent obstacles are summarized, namely the simple ellipsoidal representation, the constructive solid geometry (CSG) method as well as a direct and an indirect implementation of a signed distance based approach. The formulations are characterized with respect to the impact on the complexity of the optimization problem, as well as the ability to meet different problem requirements. Second, this work presents a novel variant of the CSG method to describe collision avoidance constraints. It is highly efficient due to a very low number of nonlinear inequality constraints required for a given number of obstacles and sample points and in contrast to the original CSG formulation allows to consider the controlled system's shape. The good performance of the proposed formulation is demonstrated by a comparison to the previously mentioned alternatives. To this end optimal trajectory planning for marine surface vessels formulated as a nonlinear programming problem is used as a benchmark example where the scenario is designed based on the maritime test field in Kiel, Germany.