Synergizing Decision Making and Trajectory Planning Using Two-Stage Optimization for Autonomous Vehicles

TL;DR

This paper presents a two-stage optimization approach that jointly addresses decision making and trajectory planning for autonomous vehicles, improving safety and efficiency through a decomposed nonlinear programming framework.

Contribution

The paper introduces a novel two-stage optimization method that effectively integrates decision making with trajectory planning in autonomous driving, handling mixed-integer nonlinear problems.

Findings

Significantly improves driving safety and efficiency.

Demonstrates high computational efficiency in simulations.

Adapts effectively to dynamic driving scenarios.

Abstract

This paper introduces a local planner that synergizes the decision making and trajectory planning modules towards autonomous driving. The decision making and trajectory planning tasks are jointly formulated as a nonlinear programming problem with an integrated objective function. However, integrating the discrete decision variables into the continuous trajectory optimization leads to a mixed-integer programming (MIP) problem with inherent nonlinearity and nonconvexity. To address the challenge in solving the problem, the original problem is decomposed into two sub-stages, and a two-stage optimization (TSO) based approach is presented to ensure the coherence in outcomes for the two stages. The optimization problem in the first stage determines the optimal decision sequence that acts as an informed initialization. With the outputs from the first stage, the second stage necessitates the use of a high-fidelity vehicle model and strict enforcement of the collision avoidance constraints as part of the trajectory planning problem. We evaluate the effectiveness of our proposed planner across diverse multi-lane scenarios. The results demonstrate that the proposed planner simultaneously generates a sequence of optimal decisions and the corresponding trajectory that significantly improves driving performance in terms of driving safety and traveling efficiency as compared to alternative methods. Additionally, we implement the closed-loop simulation in CARLA, and the results showcase the effectiveness of the proposed planner to adapt to changing driving situations with high computational efficiency.