Global Trajectory Optimization for Autonomous Driving using Nonlinear Programming with Topology Classes*

Obstacle avoidance is an essential characteristic of autonomous driving cars. This paper presents a new method for efficiently determining possible routes that a vehicle can follow for overtaking obstacles and classifying them into different topology classes of paths. Impossible routes are eliminated based on the physical constraints of the vehicle. To evaluate each topology class and pick the one with the lowest cost as the globally optimal path, we run several topology-aided Nonlinear Programming (NLP) problems. For each topology-aided NLP problem to overtake the obstacles from the prespecified side determined by the topology class, corresponding auxiliary halfspace constraints are activated in the optimization problem. Through simulation results and comparisons with a standard NLP planner, we show that the overall computation time for solving several topology-aided NLP problems is faster than that of the standard NLP planner. Additionally, in complex driving scenarios, where the standard planner gets stuck in a sub-optimal solution due to internal faults or a local optimum, our proposed topology-aided planner can still find the globally optimal path.