SafeFlowMatcher: Safe Trajectory Planning

• SafeFlowMatcher is a trajectory planning framework that combines neural ODE-based flow matching with control barrier functions to enforce hard state constraints.
• It employs a two-phase prediction–correction integrator where an initial candidate path is rapidly generated and then refined using a CBF-based QP for safety enforcement.
• Empirical evaluations on robotics and locomotion benchmarks demonstrate its ability to outperform traditional FM planners by reducing constraint violations, local traps, and ensuring smoother trajectories.

SafeFlowMatcher is a trajectory planning framework that integrates flow-matching (FM) generative modeling with control barrier functions (CBFs) to achieve both formal safety guarantees and real-time efficiency. It is designed to address the limitations of pure FM planners, which, while capable of producing high-quality and efficient sample paths via neural ODE integration, lack guarantees with respect to hard state constraints and are susceptible to failures such as constraint violations and local trapping near constraint boundaries. SafeFlowMatcher employs a two-phase, prediction–correction integrator and proves safety properties via a barrier certificate, enabling robust execution across high-dimensional planning problems, including robotics locomotion and navigation benchmarks (Yang et al., 29 Sep 2025).

1. Underlying Principles and Problem Formulation

SafeFlowMatcher addresses planning over a finite horizon $H$, considering discrete time steps $t \in \{t_0, ..., t_N\}$ with state $x_k \in D \subset \mathbb{R}^d$. Hard state constraints are specified via a continuously differentiable barrier function $h: D \to \mathbb{R}$, defining the robust safe set

$C = \{x \in D \mid h(x) \geq 0 \}.$

The core objective is to generate a path $(x_0, ..., x_H)$ that (i) reaches a target distribution, (ii) remains in $C$ for all digitally executed waypoints, and (iii) is computed with minimal latency.

FM models learn a time-varying vector field $f(x,t)$, typically approximated by a neural network $v_\theta(x, t)$, allowing one to transport an initial distribution $x(0) \sim p_0$ to a target $t \in \{t_0, ..., t_N\}$0 in as few as one or a few ODE steps. However, FM does not encode safety, so naive integration can cause the system to violate constraints, fall into local traps, or output incomplete paths near boundaries.

2. Flow Matching Component

The FM component of SafeFlowMatcher is based on training $t \in \{t_0, ..., t_N\}$1 to match optimal conditional velocity fields according to a conditional flow matching loss:

$t \in \{t_0, ..., t_N\}$2

where $t \in \{t_0, ..., t_N\}$3 and $t \in \{t_0, ..., t_N\}$4. At inference, the field is integrated, usually via the explicit Euler method:

$t \in \{t_0, ..., t_N\}$5

This produces rapid, low-latency generation of candidate paths that, absent constraints, exhibit excellent quality with respect to the learned distribution (Yang et al., 29 Sep 2025).

3. Control Barrier Function (CBF) Correction

The core safety mechanism in SafeFlowMatcher is a CBF-based correction applied not during FM integration per se, but in a post-prediction refinement phase. The CBF safety set $t \in \{t_0, ..., t_N\}$6 is numerically tightened to $t \in \{t_0, ..., t_N\}$7 for robustness. The correction uses a vanishing time-scaling factor $t \in \{t_0, ..., t_N\}$8 applied to the FM vector field, yielding nominal dynamics $t \in \{t_0, ..., t_N\}$9 that progressively fade as $x_k \in D \subset \mathbb{R}^d$0.

At each step, safety is enforced by solving a quadratic program to find the minimal corrective velocity $x_k \in D \subset \mathbb{R}^d$1:

$x_k \in D \subset \mathbb{R}^d$2

with $x_k \in D \subset \mathbb{R}^d$3. The solution ensures finite-time CBF constraints are satisfied, certifying forward invariance and convergence to the safe set (Yang et al., 29 Sep 2025).

4. Two-Phase Prediction–Correction Integrator

Planning is executed via a two-phase strategy:

• Prediction phase: Integrate the FM field for $x_k \in D \subset \mathbb{R}^d$4 steps to generate an initial candidate path, without intervention.
• Correction phase: Starting from the last prediction-phase state, integrate the vanishingly scaled FM field plus CBF-QP correction over $x_k \in D \subset \mathbb{R}^d$5 steps. At each step, the QP ensures the barrier constraint holds, guaranteeing $x_k \in D \subset \mathbb{R}^d$6 at every executed waypoint.

The update for each correction step is

$x_k \in D \subset \mathbb{R}^d$7

enforcing $x_k \in D \subset \mathbb{R}^d$8 via projection if required for numerical robustness. Empirically, $x_k \in D \subset \mathbb{R}^d$9 is often sufficient for high performance, and $h: D \to \mathbb{R}$0 optimally trades off correction strength and goal reachability (Yang et al., 29 Sep 2025).

5. Theoretical Guarantees

SafeFlowMatcher establishes two central properties:

• Forward invariance: If $h: D \to \mathbb{R}$1 satisfies the QP at every step and $h: D \to \mathbb{R}$2, then $h: D \to \mathbb{R}$3 for all $h: D \to \mathbb{R}$4.
• Finite-time convergence: For $h: D \to \mathbb{R}$5, there exists $h: D \to \mathbb{R}$6 such that for all $h: D \to \mathbb{R}$7, $h: D \to \mathbb{R}$8, with explicit convergence rates derived via Lyapunov analysis:

$h: D \to \mathbb{R}$9

The construction leverages a Lyapunov candidate $C = \{x \in D \mid h(x) \geq 0 \}.$0. The CBF constraint ensures $C = \{x \in D \mid h(x) \geq 0 \}.$1 for some $C = \{x \in D \mid h(x) \geq 0 \}.$2, as per standard finite-time stability arguments (Yang et al., 29 Sep 2025).

6. Empirical Evaluation and Comparisons

SafeFlowMatcher has been validated on high-dimensional maze navigation (Maze2D with circular obstacles, $C = \{x \in D \mid h(x) \geq 0 \}.$3) and OpenAI Gym locomotion (Walker2D, Hopper). Key findings include:

• Maze2D:
  • Zero trap rate and $C = \{x \in D \mid h(x) \geq 0 \}.$4 (barrier safety), outperforming FM-based safety filters with up to 41% trap rates.
  • Score: $C = \{x \in D \mid h(x) \geq 0 \}.$51.63 (SafeFlowMatcher) vs. $C = \{x \in D \mid h(x) \geq 0 \}.$61.35 (SafeFM), $C = \{x \in D \mid h(x) \geq 0 \}.$71.44 (SafeDiffuser).
  • Computation per path $C = \{x \in D \mid h(x) \geq 0 \}.$84.7 ms, similar to unconstrained FM and 4x faster than diffusion planners.
  • Lower trajectory curvature and acceleration (smoother paths).
• Locomotion:
  • Achieves top normalized scores (Walker2D: 0.33 vs. 0.32 for baselines) while maintaining $C = \{x \in D \mid h(x) \geq 0 \}.$9.
  • Robust performance persists in high-dimensional state spaces.

Ablation studies emphasize:

• The necessity of both phases: prediction-only integration is unsafe, correction-only yields high trap rates and poor reachability.
• Minimal prediction steps ($(x_0, ..., x_H)$0) suffice; further increase yields only marginal gains at additional cost.
• Vanishing time-scaling is essential for balancing goal achievement and safety; overly strong correction impedes task performance as $(x_0, ..., x_H)$1 (Yang et al., 29 Sep 2025).

7. Relation to Prior and Alternative Approaches

SafeFlowMatcher is related to but distinct from fully projection-based safety (SafeFlow, SafeFM (Dai et al., 11 Apr 2025)) and test-time constraint-enforcing planners such as SAD-Flower (Huang et al., 7 Nov 2025) and HardFlow (Li et al., 11 Nov 2025):

Framework	Constraint Enforcement	ODE Steps	Safety Guarantee
SafeFlow/FM	CBF-projected velocity at all latent/integration steps	many	For all intermediate paths
SafeFlowMatcher	CBF-QP only on executed path (after FM prediction)	few	Only executed (visible) waypoints
SAD-Flower	QP for CBF/CLF on trajectory with virtual control	many	State, input, and consistency
HardFlow	MPC-based surrogate for endpoint constraint	many	Terminal constraint

By reducing constraint enforcement to only the executed trajectory, SafeFlowMatcher avoids the pitfall of “local trap” events (velocity fields becoming singular or pathological near constraint boundaries) that affect naive CBF correction applied everywhere. It thus matches the efficiency of unconstrained FM, while providing formal safety and convergence guarantees (Yang et al., 29 Sep 2025).

• "SafeFlowMatcher: Safe and Fast Planning using Flow Matching with Control Barrier Functions" (Yang et al., 29 Sep 2025)
• "Safe Flow Matching: Robot Motion Planning with Control Barrier Functions" (Dai et al., 11 Apr 2025)
• "SAD-Flower: Flow Matching for Safe, Admissible, and Dynamically Consistent Planning" (Huang et al., 7 Nov 2025)
• "HardFlow: Hard-Constrained Sampling for Flow-Matching Models via Trajectory Optimization" (Li et al., 11 Nov 2025)