Motivation

Articulated vehicles such as tractor-trailers must routinely reverse and maneuver in cluttered environments such as parking lots, loading docks, and narrow corridors. This demands a motion planning system that is simultaneously safe (collision-free with formal guarantees), kinodynamically feasible (respecting the vehicle's complex, nonholonomic dynamics), and real-time (fast enough for closed-loop control).

Classical planners can provide safety guarantees but are often computationally expensive, especially in non-convex environments with many obstacles. On the other hand, recent diffusion-based planners are powerful at generating high-quality trajectories but typically lack formal safety guarantees, and common workarounds like guidance or projection either break kinodynamic feasibility or are computationally intractable.

In this line of work, we propose a unified GPU-accelerated motion planning platform that addresses both challenges. The platform consists of two complementary modules:

• Safe Model Predictive Diffusion (Safe MPD): a high-level planner that generates globally optimal, provably safe reference paths in sub-seconds using diffusion-based sampling with an integrated safety shield.
• Barrier-Rate MPPI (BR-MPPI): a low-level controller that tracks the reference path in real-time (100+ Hz) while maintaining safety through Control Barrier Function constraints embedded directly in the MPPI importance-sampling update.

Both modules are fully parallelized on a single GPU. Safe MPD replaces classical global planners that would be computationally costly, producing high-quality reference trajectories in sub-seconds. The generated global path is then passed to BR-MPPI for real-time tracking and reactive control. Together, they form a complete planning-to-control pipeline validated on a 12m tractor-trailer in the high-fidelity CarMaker simulator.

---

High-Level Planner: Safe Model Predictive Diffusion

Safe MPD is a training-free diffusion planner that unifies a model-based diffusion framework with a safety shield, called Shielded Rollout, to generate trajectories that are both kinodynamically feasible and safe by construction. By enforcing feasibility and safety on all samples throughout the denoising process (not as a post-hoc correction) Safe MPD avoids the pitfalls of guidance (loss of feasibility) and projection (computational intractability in non-convex environments). The planner's design centers on three key ideas:

1. Training-Free Model-Based Score Estimation. Unlike standard diffusion models that learn a neural network from expert demonstrations, Safe MPD leverages the known dynamics model and cost function to compute the score function directly via Monte Carlo approximation. At each denoising step, K candidate trajectories are drawn around the current noisy estimate, scored using the true objective, and used to perform a principled weighted-average update. This training-free formulation eliminates the need for data collection and naturally supports test-time adaptation to new tasks and environments without retraining.

2. Shielded Rollout for Provable Safety. The core innovation is the Shielded Rollout mechanism, inspired by model predictive shielding. At each time step during forward simulation, before accepting a nominal control input from the diffusion process, the algorithm simulates a finite-horizon rollout under a backup policy to verify that the system can be brought back to a controlled-invariant safe set. If this validity check passes, meaning the trajectory stays within the safe set and reaches the invariant set within the backup horizon, the nominal input is accepted. Otherwise, the backup policy takes over for the remainder of the trajectory. This provides a formal guarantee that the system remains safe for all future time in each rollout.

3. In-Process Safety Enforcement. Rather than applying safety corrections only to the final output (as in filtering, guidance, or projection methods), Safe MPD integrates the Shielded Rollout at two stages: (i) within every denoising step on all K candidate samples, and (ii) on the final diffusion output. Because every shielded sample is guaranteed to be feasible and safe, the feasibility and safety probability terms become constant across all samples and can be dropped from the target distribution which then simplifies to depend solely on the cost objective. This dramatically improves sample efficiency, since no computational effort is wasted on samples that would otherwise receive zero weight from constraint violations. The iterative score ascent also helps the optimizer overcome local minima while shielding preserves safety at every step.

Through GPU parallelization of all K candidate rollouts and their shielded validity checks, Safe MPD achieves sub-second planning times even on challenging non-convex problems with dozens of obstacles that makes it a practical replacement for classical global planners.

---

Low-Level Controller: Barrier-Rate MPPI

BR-MPPI is a GPU-accelerated sampling-based controller that embeds Control Barrier Function (CBF) constraints directly into the Model Predictive Path Integral (MPPI) importance-sampling distribution. Rather than rejecting unsafe samples or penalizing collisions after the fact, BR-MPPI steers the sampling distribution itself toward collision-free, dynamically feasible trajectories. This fundamentally improves the exploration strength of MPPI and produces more robust control inputs. The controller's design centers on three key elements:

1. BR-MPPI Sampling with Projected Rollouts. Standard MPPI penalizes collisions through soft costs in the objective, which struggles in tight spaces because most random samples violate constraints. BR-MPPI instead converts each CBF inequality constraint into a parameterized equality constraint with a time-varying barrier rate α. During each rollout, the sampled control inputs are projected onto the safe manifold defined by these equality constraints via a weighted minimum-norm problem that admits a closed-form solution. Actuator bounds are incorporated as soft penalties within this projection, keeping the solution analytical and GPU-friendly. The result is that all rollouts per step are steered toward collision-free trajectories by construction, rather than relying on cost penalties to discourage unsafe ones.

2. Contouring Cost for Path Tracking. Instead of naively tracking a time-parameterized reference trajectory which can accumulate error and cause sudden corrections (especially in reverse), we adopt a contouring cost formulation. The cost jointly minimizes the contouring error (perpendicular distance to the reference path), the lag error (longitudinal distance behind the nearest path point), and heading deviations for both tractor and trailer. An additional progress-rate term encourages steady advancement along the path while allowing temporary slowdowns for safety. This formulation provides a tunable trade-off between tracking accuracy and forward progress, and is robust to the path parameterization issues that plague standard trajectory tracking in reverse maneuvers.

3. Hitch Angle Stabilizer. Reverse parking is inherently unstable for tractor-trailer systems: small steering deviations can cause large, uncontrolled articulations leading to jackknifing. To prevent this, we add a velocity-dependent hitch angle constraint (the maximum allowable hitch angle shrinks as speed increases). This constraint is enforced through a CBF-based Quadratic Program (CBF-QP) that filters BR-MPPI's control outputs at each time step, ensuring the hitch angle remains within a safe range.

On a single GPU, BR-MPPI computes control inputs at over 100 Hz (for scenarios with eight obstacles) using 5,000 parallel rollouts with a 4.8-second prediction horizon — enabling real-time closed-loop control for the full tractor-trailer system including reverse maneuvers.

---

Unified Platform

The key insight behind this platform is that both planning and control can be cast as GPU-parallelizable sampling problems. Safe MPD samples thousands of trajectory candidates during diffusion denoising, while BR-MPPI samples thousands of control rollouts during path-integral optimization. By keeping the entire pipeline on the GPU, we eliminate the latency of transferring data between heterogeneous compute units and achieve end-to-end real-time performance.

We validated the integrated system in CarMaker on a 12m tractor-trailer performing reverse and forward parking in a cluttered parking lot. Safe MPD generates the collision-free reference path in sub-seconds, and BR-MPPI tracks it in real time while maintaining formal safety via CBF constraints, all on a single GPU.