Path-Consistent Safety Filtering (PACS) Overview

• Path-Consistent Safety Filtering (PACS) is a framework that ensures real-time robotic safety by reducing speed on the intended path to prevent collisions.
• It employs set-based reachability analysis and formal induction guarantees to predict and avoid unsafe states without shifting the policy's training distribution.
• Empirical evaluations show PACS significantly improves task success and safety in dynamic human-robot interactions versus traditional reactive filters.

Path-Consistent Safety Filtering (PACS) is a control framework designed to provide formally guaranteed, real-time safety assurances for robots operating under diffusion policies (DPs). Unlike classical reactive safety mechanisms, PACS preserves consistency with the DP’s training distribution by enforcing safety exclusively through speed reduction along the originally intended geometric path. The methodology employs set-based reachability analysis, formal induction-based guarantees, and compositional integration with chunked trajectory outputs from deep learning policies, enabling robust performance in dynamic and human-interactive environments without introducing distribution shift or out-of-distribution states.

1. Formal Definition and Path-Consistent Braking

Given a robot with configuration (joint) state

$x(t) = \bigl(q(t), \dot{q}(t), \ddot{q}(t), \dddot{q}(t)\bigr) \in \mathbb{R}^{n \times 4},$

and control input

$u(t) \in \mathcal{U} \subset \mathbb{R}^n,$

a pretrained diffusion policy (DP) generates, at discrete high-level timepoints $t_k$, an action chunk

$$A_k = \bigl(\hat{a}_k, \hat{a}_{k+1}, \ldots, \hat{a}_{k+H-1}\bigr) \sim \pi(\cdot | o(t_k)),$$

where $\hat{a}_i \in \mathbb{R}^n$ and $H$ is the chunk length. For the first $h \leq H$ actions, waypoints are computed via forward integration: $$\begin{aligned} p &= (q_0, q_1, \ldots, q_h), \ q_0 &= q(t_k), \ q_{i+1} &= q_i + \Delta t \cdot \hat{a}_{k+i}, \quad i = 0, \ldots, h-1. \end{aligned}$$ This defines the desired joint configuration trajectory $q(t)$ over the planning horizon $[t_k, t_k + h \Delta t]$. In the absence of safety filtering, a lower-level controller would nominally track these waypoints or a corresponding spline: $$x_{\text{nom}}(t), \quad u_{\text{nom}}(t), \quad t \in [t_k, t_k + h \Delta t].$$

The safety constraint set $\mathcal{S} \subset X$ is defined according to task-specific predicates derived from international standards (ISO/TS 15066:2016):

• Speed and Separation Monitoring (SSM): forbids any collision,

$$c_{\text{safe,SSM}}(t) : \neg\,c_{\text{coll}}(t); \quad c_{\text{coll}}(t) : \mathcal{C}_{\text{robot}}(t) \cap \mathcal{C}_{\text{object}}(t) \neq \varnothing.$$

• Power and Force Limiting (PFL): allows contact if kinetic energy remains below a threshold, \begin{equation} c_{\text{safe,PFL}}(t):\,\neg c_{\text{coll}}(t) \;\lor\; T_{\text{robot}}(t) \leq T_{\text{safe}}(t). \end{equation}

Path-consistent braking is the central mechanism of PACS: if $u_I(\cdot)$ (the control input tracking the intended trajectory) is projected to violate safety in future steps, PACS computes a failsafe braking input $u_F(\cdot)$ that decelerates (and restricts jerk) along the same geometric path, bringing $\dot{q} \rightarrow 0$ as rapidly as dynamic limits allow—never steering away from the planned path. The executed input is

$$u_{\text{safe}}(t) = \begin{cases} u_I(t), & \text{if the monitored future is safe,} \ u_F(t), & \text{otherwise.} \end{cases}$$

This approach strictly reduces speed (and higher derivatives) but never projects off the geometric waypoints.

2. Set-Based Reachability Analysis

PACS determines whether continuing along $u_I(\cdot)$ stays within $\mathcal{S}$ via set-based reachability:

• Forward reachable set from initial state $x_0$ under all admissible $u(\cdot)$ is

$$\mathcal{R}(t; x_0) = \{\,\chi(t; x_0, u(\cdot))\,|\,u(\tau) \in \mathcal{U}\;\forall\,\tau \in [0, t]\,\}.$$

For an interval $[t_k, t]$,

$$\mathcal{R}([t_k, t]; x_0) = \bigcup_{\tau \in [t_k, t]} \mathcal{R}(\tau; x_0).$$

• In practice, $\mathcal{R}(t)$ is over-approximated via zonotopes or interval enclosures—one propagates

$Z_{k+1} = \mathcal{F}(Z_k \oplus U),$

using software such as CORA or the SaRA framework. This encodes one-step dynamics with $Z_k$ over-approximating reachable states at $t_k$ and $U$ the set of admissible control actions.

• To guarantee collision-free execution, PACS requires that

$$\mathcal{C}_{\text{robot}}(t) \cap \mathcal{C}_{\text{obj}}(t) = \varnothing \qquad \forall t \in [t_k, t_k+h\Delta t].$$

This suffices for $c_{\text{safe}}(t)$ to be maintained due to the soundness of the reachable set over-approximation.

3. Algorithmic Implementation and Real-Time Workflow

PACS operates two nested control loops:

• A high-level loop at DP’s chunk rate $1/(h\Delta t)$
• A low-level safety loop at $1/\alpha_s$

A streamlined version of the algorithm:

t = 0 while task not complete: o(t) ← get_observation(t) A = (Δq_0,…,Δq_{H-1}) ∼ π(·|o(t)) (q_0=q(t),…,q_h) ← integrate(A) χ_I ← plan_intended_trajectory(q_0…q_h) # time-optimal via Ruckig shield ← start_shield(χ_I) for k in 0 … (hΔt/α_s−1): m_o(t) ← measure_objects(t) u_safe(t+α_s), shield ← shield.step(t, m_o(t)) apply_input(u_safe(t+α_s)) t ← t+α_s

Within shield.step(…), PACS computes the monitored reachable set for the monitored trajectory: $$\chi_M(t) = \begin{cases} \chi_I(t), & t \in [t_k, t_k+T_I], \ \chi_F(t), & t \in [t_k+T_I, t_k+T_F], \end{cases}$$ and checks $$\mathcal{C}_{\text{robot}}(t) \cap \mathcal{C}_{\text{obj}}(t) = \varnothing$$ for $t \in [t_k, t_k+T_I]$. If future motion is safe, $u_I(t)$ is used; otherwise, PACS transitions to $u_F(t)$.

Notable engineering parameters:

• Safety frequency: $1/\alpha_s \approx 1\,\text{kHz}$; each step takes $\sim 0.2\,\text{ms}$.
• Intended-trajectory planning: via Ruckig (jerk limited), $\sim 5\,\text{ms}$ per chunk.
• Uncertainty handling: Object pose noise and model errors are addressed by over-approximating $\mathcal{C}_{\text{object}}(t)$ with a bounding ball.
• Dynamic obstacles: Re-measured at every safety step and checked against precomputed reachable sets.

4. Formal Safety Guarantees

PACS provides an inductive formal safety guarantee:

Proposition (Safety-by-Induction):

Suppose at $t=0$, $x(0) \in \mathcal{S}$. If the shield (low-level module) always ensures that the monitored trajectory $\chi_M$ satisfies $c_{\text{safe}}(t)$ on each safety subinterval, then the actual execution $x(t)$ remains in $\mathcal{S}$ for all $t \geq 0$.

The guarantee follows directly: Only inputs $u_{\text{safe}}(t)$ are ever commanded for which the forward reachable occupancy does not intersect the object occupancy or violate the kinetic energy threshold. The soundness of the set-based approximation suffices, and induction over safety steps establishes invariance in $\mathcal{S}$.

5. Distribution-Shift Mitigation and Policy Consistency

Reactive filters such as control barrier functions frequently cause distribution shift by rerouting the robot away from the DP’s intended path, which leads to states unseen during demonstration-based training. PACS mitigates this by:

• Only reducing the speed, acceleration, and jerk along the DP-planned geometric path
• Ensuring the policy’s observations (RGB images $I(t)$, joint angles $q(t)$) remain consistent with training; no explicit velocity state is observed, so slowing/pausing does not introduce out-of-distribution data

Empirical results indicate that this mitigation preserves task success rates even under extensive safety intervention.

6. Experimental Evaluation and Quantitative Results

PACS was evaluated on three real-world human-robot interaction tasks:

1. Sorting (Coexistence, SSM): Robot sorts red blocks while a human picks green blocks simultaneously; no contact is allowed.
2. Handover (Collaboration, PFL on Hand): Robot picks a block from a human’s hand; contact permitted up to $T_{\text{safe,hand}}$.
3. Feeding (Collaboration, PFL on Head): Robot brings a fork to the human mouth; contact up to $T_{\text{safe,head}} \ll T_{\text{safe,hand}}$.

Metrics used:

• Success: Percentage of completed tasks
• Safe Success: Tasks completed with no safety violation
• Safety Violations: Fraction of unsafe timesteps
• Duration: Execution time per task

Key quantitative findings:

• Simulation (Robomimic lifts):
  • Control barrier function (CBF): 4% success
  • Single-action SSM/PFL: 41–44%
  • PACS: 69% (SSM), 72% (PFL) — up to 68% absolute improvement over CBF
• Hardware (Sorting only):
  • CBF: 43% success
  • PACS: 80% — 37% improvement
• Across all real tasks, unshielded DP: 0% Safe Success (violations every trial); PACS: ~80% Safe Success
• Using chunked intended trajectories versus single-action shielding yielded +28% success (H3)
• Enforcing dynamic feasibility led to a 14% speedup in “no-human” Sorting

7. Reproducibility and System Hyperparameters

Parameter values confirmed in real-robot experiments:

• Action-chunk length: $H = 16$
• Execution steps per chunk: $h=6$
• Policy timestep: $\Delta t = 0.033\,\mathrm{s}$
• Safety step: $\alpha_s = 1\,\mathrm{ms}$
• Joint velocity limits: $1.25$–$2\,\mathrm{rad/s}$
• Acceleration limits: $10\,\mathrm{rad/s}^2$
• Jerk limits: $400\,\mathrm{rad/s}^3$
• Diffusion inference: 10 DDIM steps (hardware)
• Learning rate: $1 \times 10^{-4}$
• Batch size: 64

8. Context and Impact within Safety-Critical Robot Learning

PACS is the first formally guaranteed, real-time safety filter for DPs and other action-chunking policies in dynamic human-robot interaction. It preserves state-distribution consistency while providing closed-loop guarantees and is empirically shown to provide up to 68 percentage points increase in task success relative to reactive baselines. This suggests its adoption can substantially improve the practical deployability of demonstration-trained deep policies in safety-critical applications where both task completion and formal safety are required.