Branch-Stochastic Model Predictive Control for Motion Planning under Multi-Modal Uncertainty with Scenario Clustering

Autonomous driving has witnessed rapid advancements, with motion planning being a critical component responsible for generating safe and efficient trajectories in dynamic environments. Surrounding vehicles exhibit multi-modal uncertainty, encompassing both intention-level ambiguity (e.g., lane change, acceleration, deceleration) and trajectory-level variability due to partially observable driving styles and dynamics. Traditional robust MPC methods guarantee safety by enforcing constraints over all uncertainty realizations but tend to be overly conservative, limiting vehicle performance. Stochastic MPC introduces chance constraints to allow controlled constraint violations, reducing conservatism at the trajectory level. However, it still treats intention uncertainty conservatively by enforcing constraints across all possible intentions simultaneously. Branch MPC addresses this by planning multiple contingency trajectories in parallel for different predicted intentions, postponing commitment to a specific trajectory until uncertainty reduces. Nevertheless, BMPC suffers from exponential growth in branches with increasing numbers of surrounding vehicles, leading to computational intractability. Recent advances in interaction-aware multi-modal trajectory prediction, such as IAIMM-KF, provide probabilistic multi-modal predictions that capture vehicle interactions, offering rich information for planning. Leveraging these predictions effectively for real-time, safe motion planning remains a significant challenge. This paper builds upon these foundations to propose a scalable, probabilistically safe motion planning framework under multi-modal uncertainty.

The core problem addressed is how to generate safe, efficient, and real-time feasible motion plans for autonomous vehicles in dynamic traffic environments characterized by multi-modal uncertainty in surrounding vehicles’ intentions and trajectories. Key bottlenecks include: (1) the combinatorial explosion of possible joint intention scenarios leading to exponential growth in BMPC branches, making optimization computationally prohibitive; (2) the conservatism of SMPC when enforcing chance constraints uniformly across all intentions, limiting flexibility; (3) the challenge of determining when to commit to a specific trajectory (branching time) to balance safety and adaptability; and (4) ensuring real-time computational performance to enable online replanning. Addressing these challenges is essential for deploying autonomous vehicles capable of proactive and safe navigation in complex interactive environments.

The paper’s core innovations are: (1) the Branch-Stochastic MPC framework that uniquely integrates SMPC’s probabilistic chance constraints with BMPC’s branching structure, enabling distinct trajectory planning per surrounding vehicle intention while explicitly handling trajectory uncertainty; (2) a scenario clustering algorithm based on high-level maneuver planning, which clusters prediction scenarios inducing similar autonomous vehicle maneuvers, thereby mitigating the exponential growth of branches and enhancing computational tractability; (3) an adaptive branching time computation leveraging Dynamic Time Warping (DTW) distance to quantify trajectory similarity and dynamically determine the optimal moment to commit to a specific branch, avoiding premature or delayed decisions. These innovations collectively overcome the conservatism and computational challenges of prior approaches, offering a scalable and flexible motion planning framework.

• �� Multi-modal Trajectory Prediction: Utilizes the IAIMM-KF model to generate probabilistic multi-modal trajectory predictions for surrounding vehicles, including mode probabilities and covariance sequences, capturing vehicle interactions.

• �� Branching Structure Construction: Builds a branching tree where each branch corresponds to a distinct joint realization of surrounding vehicle intentions, allowing the planner to generate separate trajectories per branch.

• �� Scenario Clustering: For each prediction scenario, a lightweight high-level maneuver planner based on a simplified vehicle model and LQR control computes the optimal maneuver (target speed and lateral position). Using the DBSCAN clustering algorithm, scenarios inducing similar maneuvers are grouped and merged into single branches, reducing the number of branches significantly.

• �� Chance Constraint Formulation: For critical surrounding vehicles, chance constraints are approximated via confidence ellipses derived from predicted state covariances, ensuring probabilistic collision avoidance. For non-critical vehicles, a computationally efficient rectangular over-approximation of occupancy regions is used, resulting in linear constraints.

• �� Adaptive Branching Time Computation: Employs DTW distance to measure similarity between predicted trajectories across branches, dynamically determining the branching time as the earliest time when trajectories diverge beyond a threshold, ensuring non-anticipatory control before branching.

• �� Optimization and Receding Horizon Control: Formulates a nonlinear optimization problem solved via CasADi and IPOPT, minimizing a weighted sum of branch costs subject to dynamics and chance constraints, applying only the first control input and replanning at each timestep.

Experiments are conducted on the CommonRoad benchmark suite, focusing on challenging highway scenarios with multiple interacting vehicles exhibiting multi-modal behaviors. Baselines include Nominal MPC (NMPC), which plans based on the most likely prediction, and a state-of-the-art SMPC method (Benciolini et al., 2023) considering multi-modal uncertainty with chance constraints. All planners share identical vehicle dynamics models, cost weights, and planning horizons (30 steps with 1-second timestep). Metrics evaluated include collision rate, trajectory conservatism (deviation from nominal paths), computation time, and real-time feasibility. Ablation studies assess the impact of scenario clustering and adaptive branching time. The computational platform uses an AMD Ryzen 7840HS CPU with 16GB RAM, employing CasADi and IPOPT solvers.

Results demonstrate that B-SMPC achieves a 30% reduction in collision rates compared to NMPC and traditional SMPC, indicating enhanced safety. Trajectory conservatism is reduced by 20%, reflecting more flexible and efficient paths. Scenario clustering reduces the number of branches substantially, resulting in a 40% decrease in average planning time, enabling real-time operation within the 1-second control interval. The adaptive branching time mechanism effectively delays commitment to specific branches until sufficient uncertainty reduction, improving robustness and reducing unnecessary conservatism. Ablation studies confirm that removing either scenario clustering or adaptive branching time degrades performance, validating the necessity of both components.

The proposed framework is directly applicable to autonomous driving in highway environments with dense traffic and multi-agent interactions, where multi-modal uncertainty is prevalent. Its real-time capability and safety guarantees make it suitable for integration into autonomous vehicle decision-making stacks. Beyond highways, the approach can be extended to urban driving scenarios, advanced driver assistance systems (ADAS), and autonomous taxi fleets, enhancing safety and operational efficiency in complex traffic conditions.

The framework’s performance depends on the accuracy of the underlying multi-modal trajectory predictor (IAIMM-KF); significant prediction errors may compromise safety and efficiency. Scenario clustering relies on high-level maneuver planning, which may misclassify scenarios in highly complex environments, potentially leading to suboptimal planning decisions. The current design supports only a single branching point, limiting adaptability in scenarios requiring multiple sequential decision branches. Computational demands, though reduced, remain significant, necessitating further optimization for deployment on resource-constrained platforms.