Tight Collision Avoidance for Stochastic Optimal Control: with Applications in Learning-based, Interactive Motion Planning

TL;DR

This paper tackles risk-aware motion planning for autonomous vehicles in dense traffic by explicitly modeling human driver behavior as a Markov decision process and enforcing collision avoidance with non-convex vehicle geometries via distance constraints between compact sets. It develops a stochastic optimal control framework with three tight, differentiable outer-approximations of chance constraints and a scenario-tree approach to propagate the learned decision distribution over multiple possible futures. The key contributions include a scalable squared-distance formulation for non-convex shapes using unions of convex polytopes, a tight outer-approximation scheme for chance constraints, and empirical validation in two challenging scenarios—a road crossing and a highway lane-change—showing improved performance over robust or approximate methods while maintaining probabilistic safety bounds. The work provides a principled approach to integrate learning-based models of human behavior with tractable, probabilistically safe planning, though it acknowledges limitations in guaranteeing stochastic safety when machine learning components are involved and outlines paths for future enhancement with richer branching and higher-fidelity validation.

Abstract

Trajectory planning in dense, interactive traffic scenarios presents significant challenges for autonomous vehicles, primarily due to the uncertainty of human driver behavior and the non-convex nature of collision avoidance constraints. This paper introduces a stochastic optimal control framework to address these issues simultaneously, without excessively conservative approximations. We opt to model human driver decisions as a Markov Decision Process and propose a method for handling collision avoidance between non-convex vehicle shapes by imposing a positive distance constraint between compact sets. In this framework, we investigate three alternative chance constraint formulations. To ensure computational tractability, we introduce tight, continuously differentiable reformulations of both the non-convex distance constraints and the chance constraints. The efficacy of our approach is demonstrated through simulation studies of two challenging interactive scenarios: an unregulated intersection crossing and a highway lane change in dense traffic.

Figures (6)

• Figure 1: Forced lane-change scenario for an autonomous heavy-vehicle (blue) in dense traffic. To reach the upcoming exit ramp, the AV needs to interact with an adjacent heavy-vehicle (green).
• Figure 2: Kinematic bicycle model for a tractor-trailer HVC.
• Figure 3: An example of a scenario tree approximation over a horizon $N=6$ based on ${N_\xi = 2}$ decisions $\mathrm{d}$ of an uncontrollable vehicle. Branching and non-branching nodes are marked with red and blue, respectively. The different realizations of the random variable $\xi_\iota$ are similarly indicated in the corresponding color at each node.
• Figure 4: Conceptual visualization of the tight chance constraint approximation for one combination of $\epsilon_i$, $p_i$ and $\Bar{\mathbf{x}}_i$. The infeasible set is displayed in red and the constraint \ref{['eq:prop1_ccs']} is displayed in blue for some $\lambda_i$.
• Figure 5: Initial conditions of road crossing case (left) and an S-OCP solution with $N=5$, using the joint chance constraint formulation (right). Each color indicates a path through the scenario tree, from the root node to a leaf node.

Theorems & Definitions (6)

• Remark 1
• Proposition 1
• Remark 2
• Proposition 2
• Corollary 1
• Proposition 3