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

TL;DR

The paper tackles the challenge of jointly optimizing discrete driving decisions and continuous vehicle trajectories in autonomous driving by formulating it as a nonlinear program with mixed-integer variables. It introduces a two-stage optimization (TSO) framework: first, an MIP stage computes an informed sequence of lane decisions using a linear vehicle proxy, and second, a high-fidelity trajectory optimization stage generates feasible motions constrained by hard collision avoidance; the two stages share similar problem representations to ensure coherence. The approach is validated through diverse multi-lane simulations and closed-loop CARLA experiments, showing improved safety, efficiency, and comfort compared with baselines, while maintaining real-time performance. The results indicate the potential of a coherent local planner to enhance autonomous driving performance in dynamic traffic, with future work including prediction integration for surrounding vehicles.

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.

Figures (8)

• Figure 1: The ego vehicle (EV) in red is driving in a multi-lane scenario accompanied by various surrounding vehicles (SVs). The SVs are further labeled as the leading vehicles (LVs) in front of the EV and the neighbor vehicles (NVs) approaching from behind.
• Figure 2: Overview of the proposed planner for autonomous driving. We cast the decision making and trajectory planning modules into a nonlinear programming formulation. The two modules are jointly designed using an integrated objective function, and the TSO-based approach is proposed to solve the formulated problem in two stages. We first propose an MIP formulation to determine the optimal decision sequences, and then solve a well-defined trajectory optimization problem to generate motion plans for the EV.
• Figure 3: Trajectory and longitudinal velocity of the EV in Scenario 1. Due to the fast-traveling LV on the same lane (in brown), the EV (in red) decides on lane keeping and smoothly brings up its velocity.
• Figure 4: Trajectory and longitudinal velocity of the EV in Scenario 2. The EV (in red) decides to change lanes to the left and generates a trajectory that completes this maneuver, because of the slow LV on the same lane (in brown) and the right lane (in orange). The velocity initially decreases due to the slow LV on the same lane and then increases to around $9.5\,\text{m/s}$ as determined by the velocity of the LV on the left lane (in blue).
• Figure 5: Trajectory and longitudinal velocity of the EV in Scenario 3. The decision by the proposed planner is a right lane change, according to the slowly approaching NV (in grey) and the fast LV (in brown) on the right lane as compared to the traffic conditions on the left lane. The EV (in red) completes the right lane change maneuver with a smooth trajectory without collision and manages to increase its velocity to around $15\,\text{m/s}$ which is determined by the velocity of the LV on the right lane.

Theorems & Definitions (2)

• Remark 1
• Remark 2