Geometry-Aware Predictive Safety Filters on Humanoids: From Poisson Safety Functions to CBF Constrained MPC

Authors: Ryan M. Bena, Gilbert Bahati, Blake Werner, Ryan K. Cosner, Lizhi Yang, Aaron D. Ames · Humanoids 2025

Licences

Open Access
Paper: CC BY 4.0

Everything below draws on the full paper under its CC BY 4.0 licence; its figure is credited to the authors.

TL;DR

Poisson safety functions computed from perception and extended over time and robot heading feed a CBF-constrained MPC that steers G1 and Go2 robots around moving and static obstacles.

Why it matters

Geometry awareness lets a robot rotate rather than stop: a G1 carrying an oblong payload turned to pass through a corridor that would otherwise cause a navigation deadlock. The layered design keeps an existing learned walking policy and adds a safety layer above it that works directly from segmented camera images.

Problem

Safety filters based on control barrier functions (CBFs) need a safety function describing free space, usually hand-written geometric shapes or signed distance functions, and they often treat the robot as a point or a disc. Legged robots have asymmetric shapes, so a single robot radius is conservative, and in dynamic scenes the safe set itself moves over time.

Proposed solution

The method solves a Dirichlet problem for Poisson's equation over the free space seen by a camera to obtain a smooth safety function. It adds time by predicting how obstacle boundaries move over the horizon from optical-flow velocities (a moving-boundary problem), and adds orientation by shrinking the safe set with a Minkowski difference of the robot footprint at each heading. The resulting function is a discrete-time CBF constraint inside a nonlinear MPC on a 3-DOF single-integrator model (x, y, heading), solved by SQP at 100 Hz, which sends velocity commands to an RL locomotion controller.

Key insights

Solving Poisson's equation turns the boundary of an occupancy map into a smooth safety function that is a valid CBF for single-integrator dynamics.
Treating time as a parameter of the domain lets the safety function anticipate where moving obstacle boundaries will be along the MPC horizon.
Lifting the domain over orientation with Minkowski differences lets the safety filter use rotation, not only translation, to stay safe.
Placing the CBF constraint along an MPC horizon avoids the undesirable stable equilibria that pointwise CBF filters can create when goal and safety conflict.
Safety at the reduced-order level carries over to the full robot if the low-level controller tracks the commanded velocities sufficiently well.

Main results

Dynamic avoidance: a Unitree Go2 and a Unitree G1 holding a goal pose both avoided an incoming dodgeball by translating and rotating together, and the safety function stayed positive throughout each manoeuvre.
The quadruped aligned its longitudinal axis, and the humanoid its lateral axis, with the predicted obstacle path, reducing the translation needed.
Navigation: the G1, carrying an oblong payload and tele-operated for 100 s through a static cluttered space, reoriented to pass through a narrow corridor; the safety function stayed positive for most of the run.
Poisson PDE solves took 20–100 ms with successive over-relaxation; the MPC ran at 100 Hz with OSQP on an offboard PC (AMD Ryzen 9 9950X CPU, GeForce RTX 4070 GPU).

Limitations

Safety cannot be formally guaranteed in general time-varying environments, because the future behaviour of the safe set is unknown.
Obstacle motion is predicted with a constant-velocity optical-flow model; the authors note that more complex prediction models could improve performance.
The SQP solution of the nonconvex MPC is not necessarily optimal, and safety of the full robot depends on the low-level RL controller tracking commands sufficiently well.
The experiments rely on external infrastructure (OptiTrack motion capture for pose, an overhead ZED 2i camera and an offboard PC), use a 2D occupancy map and model the robot as a rigid body.
In the 100-second navigation run the safety function is reported positive for 'the preponderance' of the experiment, not all of it, and results are shown as representative trials without success-rate statistics.

Field impact

Finalist for the Humanoids 2025 Best Oral Paper Award, with 18 citations. The paper links experiment footage on YouTube; it does not state a code release.

Abstract

Autonomous navigation through unstructured and dynamically-changing environments is a complex task that continues to present many challenges for modern roboticists. In particular, legged robots typically possess manipulable asymmetric geometries which must be considered during safety-critical trajectory planning. This work proposes a predictive safety filter: a nonlinear model predictive control (MPC) algorithm for online trajectory generation with geometry-aware safety constraints based on control barrier functions (CBFs). Critically, our method leverages Poisson safety functions to numerically synthesize CBF constraints directly from perception data. We extend the theoretical framework for Poisson safety functions to incorporate temporal changes in the domain by reformulating the static Dirichlet problem for Poisson's equation as a parameterized moving boundary value problem. Furthermore, we employ Minkowski set operations to lift the domain into a configuration space that accounts for robot geometry. Finally, we implement our real-time predictive safety filter on humanoid and quadruped robots in various safety-critical scenarios. The results highlight the versatility of Poisson safety functions, as well as the benefit of CBF constrained model predictive safety-critical controllers.

Open Source Robotics