Collision-Aware MPC: Safe Trajectory Optimization

• Collision-Aware MPC is an optimization-based control framework that explicitly incorporates collision constraints and risk-aware costs into trajectory planning.
• It leverages techniques like deep generative warm-starts, signed-distance computations, and probabilistic chance constraints to ensure real-time performance and robust collision avoidance.
• Empirical results across manipulation, autonomous vehicles, and aerial robotics demonstrate significant safety improvements and computational efficiency in complex scenarios.

A collision-aware @@@@1@@@@ (MPC) is an optimization-based control framework that computes input sequences for dynamical systems while explicitly constraining or penalizing trajectories to avoid collisions with environmental obstacles, other agents, or workspace boundaries. Modern collision-aware MPCs integrate advances in geometric signed distance computation, probabilistic risk assessment, deep learning-based perception, and real-time nonlinear optimization to generate safe, dynamically feasible, and smooth trajectories in cluttered or uncertain environments. The architecture spans exact geometric constraints, risk-aware costs, deep generative initialization, and robust, uncertainty-calibrated feedback, with applications across manipulation, autonomous vehicles, multi-robot systems, and aerial robotics.

1. Mathematical Structure of Collision-Aware MPC

Collision-aware MPC is cast as a finite-horizon optimal control problem with system-specific dynamics, performance objectives, and collision constraints embedded either as hard constraints or as penalties inside the running cost. For rigid manipulators, the archetypal formulation is:

$$\begin{aligned} &\min_{X,U}~ J(X,U) = \sum_{t=0}^{T-1} \ell_t(x_t,u_t) + \ell_T(x_T) \ &\text{s.t.}~~ x_{t+1} = f_t(x_t, u_t),~~ t=0…T-1 \ &\quad\;\;\;\;\;c_{t,ij}(x_t) = d(\mathcal{B}_i(x_t), \mathcal{B}_j(x_t)) - \epsilon_{safe} \ge 0 ~~\forall~(i,j) \end{aligned}$$

with $x_t$ the full system state (e.g., joint positions and velocities), $u_t$ the input (e.g., torques), $\mathcal{B}_i(x_t)$ the configuration of robot or obstacle body $i$ at time $t$, and $d(\cdot, \cdot)$ a signed distance function. The cost typically combines:

• $\ell_{ee}(x_t)$: end-effector goal error, e.g., $\|p_e(q_t) - p_d\|_{Q_{ee}}^2$,
• $\ell_x(x_t)$: state tracking, optionally w.r.t. a learned or heuristic prior trajectory,
• $\ell_u(u_t)$: control effort, e.g., $\|u_t - u_{grav}(q_t)\|_{Q_u}^2$.

Contemporary extensions generalize $d(\cdot, \cdot)$ to handle convex polygons, arbitrary 3D meshes, or use learned approximations from raw sensory input.

Chance constraints for stochastic systems or uncertain environments appear in the form:

$$\mathbb{P}\left[\min_{i,j} d(\mathcal{B}_i(x_t), \mathcal{B}_j(x_t)) - \epsilon_{safe} \ge 0\right] \ge 1 - \alpha$$

Probabilistic reformulation through linearization or scenario methods enables embedding in convex or nonlinear optimization.

2. Models and Algorithms for Collision Awareness

2.1 Geometric and Signed-Distance Constraints

Signed-distance constraints permit direct, exact enforcement of non-penetration between robot and obstacles, supporting generic convex models or point clouds [$d(\mathcal{B}_i(x_t),\mathcal{B}_j(x_t))$], and are prevalent in manipulation and surface vessel MPCs (Haffemayer et al., 6 Jan 2026, Helling et al., 2021). For vehicles and aerial robots, convex polygonal or ellipsoidal approximations are optimized for tractability (Kojima et al., 8 May 2025).

2.2 Risk-Aware and Probabilistic Methods

Risk-aware MPCs quantify collision risk via distributional models over predicted trajectories, using Gaussian processes, Maximum Mean Discrepancy surrogates, or scenario sampling. For instance, "MonoMPC" integrates a learned model mapping raw monocular perception and candidate controls to a probabilistic minimum clearance, then adds a penalty on estimated risk into the MPC cost (Sharma et al., 10 Aug 2025); scenario-based MPCs generate multiple disturbance realizations to enforce deterministic equivalents of chance constraints with distribution-free guarantees (Nascimento et al., 2024).

2.3 Robustness to Uncertainty

Robust convex tube-MPC methods embed feedback policies for disturbance rejection, propagating ellipsoidal over-approximations and Minkowski-tighened constraints, with collision avoidance imposed via polytopic corridors learned or generated from the workspace configuration (Wullt et al., 29 Aug 2025).

2.4 Learning-Integrated Warm Starting

State-of-the-art frameworks employ deep generative models for fast warm-start: "Warm-Starting Collision-Free Model Predictive Control With Object-Centric Diffusion" trains a conditional diffusion model to generate plausible joint-space trajectories conditioned on task, initial state, and a compact object-centric (Slot Attention) scene encoding; these are then used as initial guesses for the subsequent optimal control solver, dramatically accelerating convergence to collision-free solutions (Haffemayer et al., 6 Jan 2026).

3. Perception and Scene Representation

The collision model's efficacy depends on accurate world understanding:

• RGB or RGB-D images are processed through neural backbones and Slot Attention modules to produce permutation-invariant object-centric embeddings suitable for conditioning both diffusion trajectory generators and downstream control (Haffemayer et al., 6 Jan 2026).
• Monocular depth, processed either via foundation models or network-based predictors, delivers a point-cloud representation for collision risk assessment (Sharma et al., 10 Aug 2025).
• For real-time applications, deep CNNs are often symbolically embedded within the optimization graph, as in deep-NN-constrained N-MPC for depth-based aerial robot collision avoidance (Jacquet et al., 2024).

4. Optimization and Real-Time Implementation

Real-time feasibility is achieved via warm-starting, decomposition, and specialized solvers:

• The collision-aware OCP can be solved by SQP, DDP, or stagewise SQP, with warm-starts from learned trajectories ensuring that even in dense obstacle scenarios convergence occurs within strict latency budgets (e.g., ≤72 ms for 100 Hz replanning (Haffemayer et al., 6 Jan 2026)).
• Two-stage sequential optimization, where a nominal MPC plan is refined by a predictive safety filter enforcing chance-constrained barrier conditions, improves feasibility especially in high-uncertainty or small feasible set regimes (Li et al., 2023).
• Dual formulations enable smooth, non-mixed-integer collision constraints for convex polygons and polytopes, reducing computational complexity versus disjunctive or integer approaches (Helling et al., 2021, Kojima et al., 8 May 2025).

5. Comparative Empirical Performance

Collision-aware MPC has been rigorously benchmarked:

Domain	Success Rate	Real-Time Latency	Key Metrics/Notes	Reference
Manipulation	82–83% (Table, Shelf)	≤72 ms/plan	Outperforms sampling, image-based, or config-space priors	(Haffemayer et al., 6 Jan 2026)
Mobile robot	6.6% collision (MonoMPC)	—	4×/7–9× fewer collisions than baseline/ROS/NoMaD	(Sharma et al., 10 Aug 2025)
Multi-robot	100% (scenario + RL horizon)	21.5 ms (14-robot task)	Reduces path cost/collision constraints by 7–30%+	(Gupta et al., 2023)
Surface vessels	<0.5 s/plan (polygons)	Subsecond (simulated)	Exact polygonal collision; modest speedup via norm relax	(Helling et al., 2021)
UAVs (deep NN)	0 collisions (real)	3–11 ms/plan	Symbolic CNN embed, 100 Hz, 95.4% accuracy on real data	(Jacquet et al., 2024)

Empirical results consistently demonstrate substantial improvements in safety, tractability, and reactivity over classical flexibility-limited MPC, sampling-based, or pure learning-based baselines.

6. Extensions: Risk, Cooperation, and Hierarchical Strategies

Collision-aware MPC is further extended in the following directions:

• Probabilistic and risk-based constraints: Chance-constrained MPC, scenario MPC, and risk metrics via MMD or analytic margin admit tuning of conservativeness given distributional knowledge or operational requirements (Mohamed et al., 15 Jan 2025, Brüdigam et al., 2021, Nascimento et al., 2024).
• Cooperative multi-agent collision avoidance: Decentralized MPC with soft penalties for deviation from others’ desired/planned trajectories yields emergent, robust cooperative behavior in connected autonomous vehicle groups (Wartnaby et al., 2019).
• Hierarchical control and safety filtering: MPC with high-level exponential and prescribed-time control barrier functions supplies safety overrides when nominal tracking would violate collision margin constraints, including under actuator saturation (Brüggemann et al., 2022).

7. Limitations and Future Directions

Despite progress, current collision-aware MPCs have limitations:

• Strong dependence on the fidelity of perceptual (sensor or neural) scene embeddings, especially in dynamic and unstructured environments (Sharma et al., 10 Aug 2025, Jacquet et al., 2024).
• Real-time tractability may be challenged by high-dimensional robots, complex non-convex scenes, or the need for large scenario/sample counts in risk-calibrated formulations (Mohamed et al., 15 Jan 2025, Nascimento et al., 2024).
• Analytical robustness guarantees typically rely on Gaussian uncertainty modeling or scenario-based sample-size bounds; extensions to heavy-tailed or adversarial uncertainties remain open.

Ongoing work seeks tighter real-time performance under non-Gaussian uncertainties, explicit multi-agent interaction, and learning-in-the-loop for both collision prediction and dynamic environment anticipation.

---

References:

• "Warm-Starting Collision-Free Model Predictive Control With Object-Centric Diffusion" (Haffemayer et al., 6 Jan 2026)
• "MonoMPC: Monocular Vision Based Navigation with Learned Collision Model and Risk-Aware Model Predictive Control" (Sharma et al., 10 Aug 2025)
• "RL-based Variable Horizon Model Predictive Control of Multi-Robot Systems using Versatile On-Demand Collision Avoidance" (Gupta et al., 2023)
• "Robust Convex Model Predictive Control with collision avoidance guarantees for robot manipulators" (Wullt et al., 29 Aug 2025)
• "Chance-Constrained Sampling-Based MPC for Collision Avoidance in Uncertain Dynamic Environments" (Mohamed et al., 15 Jan 2025)
• "Moving Obstacle Collision Avoidance via Chance-Constrained MPC with CBF" (Li et al., 2023)
• "Simultaneous Lane-Keeping and Obstacle Avoidance by Combining Model Predictive Control and Control Barrier Functions" (Brüggemann et al., 2022)
• "Real-Time Model Predictive Control of Vehicles with Convex-Polygon-Aware Collision Avoidance in Tight Spaces" (Kojima et al., 8 May 2025)
• "On the Dual Implementation of Collision-Avoidance Constraints in Path-Following MPC for Underactuated Surface Vessels" (Helling et al., 2021)
• "Decentralised Cooperative Collision Avoidance with Reference-Free Model Predictive Control and Desired Versus Planned Trajectories" (Wartnaby et al., 2019)