Layered Safety: Enhancing Autonomous Collision Avoidance via Multistage CBF Safety Filters

1. Problem Statement

The deployment of legged robots (quadrupeds and humanoids) in unstructured, dynamic real-world environments faces a critical challenge: safe autonomous operation under sensor noise and partial observations.

• Limitations of Existing Methods:
  • Standard CBFs: Most Control Barrier Function (CBF) methods are myopic, relying on instantaneous Quadratic Programs (QP) without predictive planning. This leads to conservative behavior, non-persistent feasibility, or failure to meet long-term task objectives when immediate safe actions conflict with goals.
  • Model Predictive Control (MPC): While MPC offers predictive planning, it is often computationally prohibitive for high-dimensional full-order robot dynamics. Furthermore, existing safety-critical control often relies on pre-defined geometric constraints rather than real-time perception data.
  • Full-Order Complexity: Synthesizing safety constraints directly on full-order dynamics is computationally expensive, limiting reaction times to dynamic obstacles.
• The Gap: There is a need for a framework that combines predictive planning (for optimality) with real-time safety filtering (for robustness), utilizes raw perception data (point clouds) to generate safety constraints, and remains computationally tractable for high-dimensional systems.

2. Methodology

The authors propose a Multistage Safety Filter embedded in an end-to-end perception-to-control architecture. The framework operates on a Reduced-Order Model (ROM) (specifically a single-integrator model) while providing formal safety guarantees for the full-order system.

A. Perception-Based Safety Synthesis (Poisson Safety Functions)

Instead of using hand-designed geometric descriptions, the system generates safety constraints directly from local point cloud data:

1. Occupancy Mapping:
   • Raw point clouds are processed to remove ground planes and self-detections.
   • Points are projected onto a 2D grid ($M_0$).
   • Spatial Convolution: A kernel is applied to smooth gaps and noise, creating a spatially consistent map ($\bar{M}$).
   • Dynamic Confidence: A confidence map ($\Gamma$) is updated over time using a continuous-time law that increases confidence for clusters and decays for isolated detections.
   • Hysteresis Thresholding: A binary occupancy map ($\hat{M}$) is generated using upper and lower thresholds to prevent boundary flickering.
2. Poisson Safety Function (PSF):
   • The occupancy map is used to solve a Dirichlet problem for Poisson's equation.
   • The solution, $h_0$, is a smooth function where $h_0 \geq 0$ represents free space and $h_0 = 0$ represents obstacle boundaries.
   • This $h_0$ serves as the Control Barrier Function (CBF) for the system.

B. Multistage Control Architecture

The framework consists of two cascaded layers operating on a single-integrator ROM ($\dot{\chi} = \mu$):

1. Stage 1: Predictive Safety Layer (MPC)

   • Goal: Optimize long-horizon trajectories to minimize deviation from nominal commands while ensuring safety.
   • Mechanism: Solves a Finite-Time Optimal Control Problem (FTOCP) using Discrete-Time CBF (DCBF) constraints over a receding horizon.
   • Output: A planned safe velocity command ($\mu_p$) that balances task optimality and future safety.
   • Handling Dynamics: The environment is modeled as time-varying using a first-order extrapolation of the PSF.

2. Stage 2: Real-Time Safety Layer (CBF-QP)

   • Goal: Ensure instantaneous safety and robustness against tracking errors.
   • Mechanism: Takes the planned command $\mu_p$ as a nominal input and solves a CBF-QP with an Input-to-State Safe (ISSf) term.
   • Robustness: Includes a term ($\frac{1}{\epsilon}\|\nabla h\|^2$) to compensate for the tracking error between the ROM and the full-order robot dynamics.
   • Output: A filtered, safe velocity command ($\mu_s$) sent to the low-level controller.

C. Theoretical Guarantees

The paper proves Theorem 1 (Full-Order System Safety):

• If the ROM commands are tracked sufficiently well by the full-order system (via a Tracking Control Lyapunov Function), and the real-time layer enforces the ISSf condition, the full-order system is guaranteed to remain within a safe set.
• This allows the system to be model-free regarding the full-order dynamics; safety is guaranteed as long as the low-level controller tracks the ROM commands accurately.

3. Key Contributions

1. Perception-Based Safety Synthesis: A novel algorithm to generate smooth, geometrically parameterized CBFs directly from point clouds using Poisson Safety Functions, eliminating the need for manual geometric modeling.
2. Multistage Model-Free Architecture: A cascaded framework combining Predictive MPC (for optimality) and Real-Time CBF-QP (for robustness) on a reduced-order model, with formal guarantees extending to full-order dynamics.
3. Pareto Optimality Analysis: A characterization of the trade-off between task optimality (efficiency) and safety robustness (margin). The paper demonstrates that the multistage approach achieves a Pareto-optimal balance, outperforming single-stage filters that lean too heavily toward either extreme.
4. Hardware Validation: Extensive real-world experiments on both quadrupedal (Unitree Go2) and humanoid (Unitree G1) platforms in dynamic scenarios.

4. Results

The methodology was validated through Pareto analysis and hardware experiments:

• Pareto Analysis:

  • Predictive Layer (Single-Stage): High optimality but low robustness (prone to collisions in highly dynamic scenarios due to lack of immediate reaction).
  • Real-Time Layer (Single-Stage): High robustness but low optimality (conservative, inefficient movements like backing away).
  • Multistage Layer: Achieved an intermediate operating point, retaining the efficiency of the predictive layer while maintaining the safety margins of the real-time layer.

• Experimental Validation (Unitree Go2):

  • Scenario: Avoiding a rolling soccer ball at speeds of 1.0 m/s and 1.25 m/s.
  • Slow Speed (1.0 m/s): All architectures avoided collisions, but the multistage filter showed the best trade-off between efficiency and safety margin.
  • Fast Speed (1.25 m/s):
    • Predictive Filter: Failed (collisions occurred) due to limited control loop rates and inability to react to rapid changes.
    • Real-Time Filter: Failed due to myopic behavior and model mismatch (single integrator limits).
    • Multistage Filter: 100% success rate (10/10 trials), successfully navigating the dynamic obstacle.

• Generalization:

  • The system successfully avoided dynamic obstacles (rolling balls, approaching dogs, humans) on a quadruped.
  • The architecture was deployed on a humanoid robot using only onboard LiDAR, proving its generality across different morphologies without requiring full-order model re-tuning.

5. Significance

This paper addresses a fundamental bottleneck in autonomous robotics: the trade-off between planning optimality and real-time safety robustness.

• Scalability: By decoupling safety synthesis from full-order dynamics via ROMs, the method scales to complex legged robots where full-order MPC is intractable.
• Adaptability: The use of PSFs allows the robot to adapt to arbitrary, unstructured environments using only local perception, removing the need for pre-mapped environments or hand-crafted safety zones.
• Reliability: The theoretical proof of full-order safety ensures that the computational efficiency of the reduced-order model does not come at the cost of physical safety, making it suitable for deployment in human-robot interaction scenarios.

In summary, the proposed Layered Safety framework provides a robust, general, and theoretically grounded solution for dynamic collision avoidance, bridging the gap between high-level perception and low-level control.