A Trajectory-Level Constraint Aware Framework For Safe Sampling
Diffusion-based trajectory planners generate high-quality robot trajectories, but they do not inherently guarantee safety in multi-obstacle environments. Prior safe diffusion methods often use QP-based waypoint-level corrections, which can be slow and difficult to scale in cluttered scenes. Practical robotic deployment requires trajectory generation that is both safety-aware and fast enough for high-frequency control.
We propose a deterministic safe sampling framework that integrates a closed-form solution of a single smooth trajectory-level CBF correction directly into the diffusion update and highlight the role of sampling geometry in reducing local traps. Across two 2D planning environments, it maintains high success rates with low generation time.
Maze2D — Narrow Passage (6 Obstacles)
Safe trajectory generation through a narrow corridor environment using our method.
PointMass2D Dense (30 Obstacles)
Safe trajectory generation in a densely cluttered environment with 30 mixed circle and square obstacles (represented as ellipses and superellipses). The trajectory (yellow) avoids all obstacles while reaching the goal.
Nested softmins aggregate obstacle-wise and waypoint-wise constraints into a single smooth trajectory-level barrier, enabling a single closed-form safety correction over the entire trajectory.
Waypoint-level CBF
\[ h(\tau_i^t) = d_{\min_j}(\tau_i^t) - k_1 \log \sum_{j=1}^{N_{obs}} e^{-(d_{ij}(\tau_i^t) - d_{\min_j}(\tau_i^t))/k_1} \]Trajectory-level softmin CBF
\[ h(\tau^t) = h_{\min_i}(\tau^t) - k_2 \log \sum_{i=1}^{T} e^{-(h(\tau_i^t) - h_{\min_i}(\tau^t))/k_2} \]The safe control $u^*$ is incorporated directly into the generative ODE as a non-linear correction term, applied only when the trajectory violates the CBF condition and leaving safe trajectories unperturbed.
Generative ODE with safe control
\[ \frac{d\tau}{dt} = f(\tau^t, t) - \frac{1}{2}g(t)^2 \nabla_\tau \log p(\tau^t) + I \cdot u \]Closed-form safe control
\[ u^*(\tau^t) = \begin{cases} -\dfrac{\omega(\tau^t)}{d(\tau^t)} \nabla_\tau h(\tau^t) & \text{if } \omega(\tau^t) < 0 \\[8pt] 0 & \text{if } \omega(\tau^t) \geq 0 \end{cases} \]
Trajectory generation with our method over 20 denoising steps in the Maze2D environment, as the diffusion timestep decreases from $t=1000$ to $t=0$. During the early steps (steps 0–12), the trajectory converges from pure noise toward a nominal path. CBF safety corrections are triggered only during the final one-third of the sampling process (steps 13–20), ensuring the trajectory has sufficiently converged before safety is enforced — a key design choice that drastically reduces local trap occurrance.
a) SD (cf, 256) | b) SD (cf, 20) | c) SafeFlowMatcher | d) OURS
Our method achieves 100% safe success rate (SR) with 0% trap rate (TR) in the Maze2D narrow passage environment, matching SafeFlowMatcher at the fastest inference time of 0.0701 s (20 denoising steps). Critically, SafeDiffuser at both 256 and 20 steps exhibits a 100% trap rate — almost every generated trajectory falls into a local trap. Our method achieves a positive Min Safe of 0.0174, confirming true constraint satisfaction. The trajectory score of 1.6277 is competitive with SafeFlowMatcher (1.6282).
OURS vs. DPM nominal — Case 1 and Case 2. Yellow represents collision-free trajectories, while black represents trajectories that collide with obstacles(with a clearance).
The PointMass2D Dense environment, with 30 randomly placed circular and box obstacles modeled as superellipses, is a significantly harder test where the distinction between methods becomes decisive. SafeDiffuser at 20 steps completely fails At 256 steps, SafeDiffuser recovers to a Safe Success Rate of 92% by allowing more refinement, but at a cost of 1.422 s per trajectory. Our method achieves a Safe Success Rate of 91% in only 20 steps at 0.0526 s — approximately a 27× speedup — by using a VE schedule that rapidly establishes global coherence and then activating a single closed-form safety correction(from trajectory-level CBF) only during the final one-third of sampling. SafeFlowMatcher peaks at a Safe Success Rate of 59% (at 256 correction steps). Our residual 9% failure rate stems from the same root cause: cases where the nominal trajectory intersects an obstacle by a large margin cause the trajectory-level gradient to split waypoints around both sides of the obstacle. The qualitative comparisons (right) show our method finding clean collision-free paths for two such test cases while the nominal DPM passes through obstacles.