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

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. This work presents a novel formulation building on constructive solid geometry (CSG) 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. To allow for a comparison, popular methods to represent obstacles from the literature are summarized and characterized, namely the simple ellipsoidal representation, the original CSG method as well as a direct and an indirect implementation of a signed distance based approach. A benchmark example shows the good performance of the proposed formulation. Here, optimal trajectory planning for marine surface vessels formulated as a nonlinear programming problem is used, where the scenario is designed based on the maritime test field in Kiel, Germany.