Source-linked AI summary
Multi-Layered Safety for Legged Robots via Control Barrier Functions and Model Predictive Control
Ruben Grandia, Andrew J. Taylor, Aaron D. Ames, Marco Hutter
TL;DR
Dynamic locomotion over rough terrain requires safe foot placement together with dynamic stability, but existing approaches do not jointly address both over a longer horizon. The paper combines CBF constraints with MPC in low- and high-frequency control layers. Simulation and hardware experiments on ANYmal validate the approach in a stepping-stone scenario.
Problem
Dynamic locomotion requires safe foot placement while accounting for its future effect on dynamic stability.
Method
The framework incorporates CBF safety constraints into low-frequency kino-dynamic MPC and high-frequency inverse-dynamics tracking control.
Results
Enforcing CBF constraints in both MPC and QP tracking layers outperforms variants enforcing them at only one layer and was demonstrated on hardware.
Takeaways & Limitations
Coordinating safety constraints across planning and tracking supports dynamic stepping over rough terrain while avoiding inconsistent layer behavior.
Abstract
from arXiv · showhide
The problem of dynamic locomotion over rough terrain requires both accurate foot placement together with an emphasis on dynamic stability. Existing approaches to this problem prioritize immediate safe foot placement over longer term dynamic stability considerations, or relegate the coordination of foot placement and dynamic stability to heuristic methods. We propose a multi-layered locomotion framework that unifies Control Barrier Functions (CBFs) with Model Predictive Control (MPC) to simultaneously achieve safe foot placement and dynamic stability. Our approach incorporates CBF based safety constraints both in a low frequency kino-dynamic MPC formulation and a high frequency inverse dynamics tracking controller. This ensures that safety-critical execution is considered when optimizing locomotion over a longer horizon. We validate the proposed method in a 3D stepping-stone scenario in simulation and experimentally on the ANYmal quadruped platform.
I. INTRODUCTION
Dynamic locomotion over rough terrain requires foot placement that is safe while preserving future dynamic stability. The proposed framework combines CBF safety constraints with MPC to coordinate both concerns across multiple control layers.
- Motivation: Dynamic footholds are constrained by terrain and influence the stability of the resulting contact configuration.Higher speeds further increase the relevance of full rigid-body inertial and nonlinear effects.
- Motivation: The stepping-stones scenario captures this challenge because viable footholds are discontinuous and sparsely available.The paper uses this scenario to study safe foot placement together with dynamic stabilization.
- Proposed approach: The method combines CBF safety guarantees with MPC’s longer horizon to achieve safe foot placement during dynamic locomotion.The framework addresses coordination that would otherwise be delegated to heuristic methods or separate controllers.
- Proposed approach: CBF constraints are incorporated in both a low-frequency MPC controller and a high-frequency whole-body tracking controller.The high-frequency layer incorporates full-body dynamics, while the MPC layer determines desired velocities and contact-related motion.
- Contribution: The paper reports the first successful experimental demonstration of CBFs on a legged robot, to the authors’ knowledge.The stated contribution concerns transfer of CBF-based stepping control to hardware.
II. BACKGROUND
The background defines safety through forward invariance of a time-varying safe set and introduces CBFs as a synthesis tool for enforcing that safety. A safety-critical CBF-QP modifies a nominal controller while satisfying the barrier condition.
- Control Barrier Functions: The paper models the system as a nonlinear control-affine dynamical system with state x, input u, and locally Lipschitz dynamics.A Lipschitz state-feedback controller yields locally Lipschitz closed-loop dynamics and a unique solution over a maximum interval.
- Control Barrier Functions: Safety is defined using a time-varying set C_t represented as the 0-superlevel set of a continuously differentiable function h.The set is the region the system must remain in.
- Control Barrier Functions: A set is forward invariant when every trajectory initialized inside it remains inside it throughout its solution interval.The system is safe on C_t when C_t is forward invariant.
- Control Barrier Functions: A Control Barrier Function provides a condition whose satisfaction by the controller ensures forward invariance and therefore safety.The condition uses an extended class K∞ function α.
- Control Barrier Functions: The safety-critical CBF-QP selects inputs satisfying the barrier condition while modifying a nominal controller that need not be safe.This provides a controller-synthesis mechanism for enforcing safety constraints.
B. Nonlinear Model Predictive Control
Nonlinear MPC formulates control as a finite-horizon optimal-control problem that minimizes running and terminal costs subject to dynamics and constraints. The paper solves this problem using a constrained Sequential Linear Quadratic method.
- Nonlinear Model Predictive Control: The nonlinear optimal-control problem minimizes a cost containing intermediate and terminal terms over a continuous control signal.The state, input, dynamics, initial condition, and general constraints define the optimization problem.
- Nonlinear Model Predictive Control: The optimization is subject to system dynamics, the initial condition, equality constraints, and inequality constraints.These constraints are expressed alongside the state trajectory and control input.
- Nonlinear Model Predictive Control: The paper uses Sequential Linear Quadratic, an inequality-constrained Differential Dynamic Programming method for continuous-time systems.The implementation follows a method that extends SLQ to handle inequality constraints.
III. MULTI-LAYERED CONTROL FORMULATION
The multi-layered formulation combines a low-frequency safe kinematic MPC with a high-frequency inverse-dynamics CBF controller. Exponential CBFs convert position safety requirements into torque-enforceable constraints while the tracking layer follows the MPC trajectory.
- Multi-Layered Control Formulation: The formulation uses generalized coordinates and rates to optimize a safe desired trajectory for a robotic system.The trajectory is passed to a tracking controller as the desired position and velocity evolution.
- Multi-Layered Control Formulation: Position-coordinate safety constraints cannot directly be enforced through torque because torques do not appear in the position constraint’s first derivative.This motivates introducing an exponential CBF with an auxiliary function involving higher-order dynamics.
- Multi-Layered Control Formulation: The exponential CBF makes the acceleration, which is affine in torque, usable for enforcing the desired position safety constraint.Enforcing forward invariance of the auxiliary set implies forward invariance of the position-safe set.
- Multi-Layered Control Formulation: The low-frequency safe kinematic MPC incorporates the safety constraint into the desired controller instead of leaving safety enforcement only to the tracking layer.This reduces aggressive behavior that can arise when the desired controller is synthesized without safety awareness.
- Multi-Layered Control Formulation: The high-frequency inverse-dynamics CBF-QP tracks the MPC acceleration while incorporating full system dynamics into safe-input determination.The controller combines inverse dynamics with CBF constraints to produce torque commands.
IV. ANYMAL IMPLEMENTATION
The multi-layer control formulation is applied to the ANYmal quadrupedal platform, with its control structure summarized in Figure 2.
- IV. ANYMAL IMPLEMENTATION: The approach is applied to the ANYmal quadrupedal robotic platform.The section introduces the platform-specific application of the multi-layer control formulation.
A. MPC System Model
The MPC system uses a kino-dynamic quadruped model combining free-floating-body dynamics with leg kinematics, with states and inputs describing body motion, joints, contact forces, and desired joint velocities.
- A. MPC System Model: The model describes a single free-floating body together with the kinematics of each leg.Its state has dimension 24 and its input has dimension 24.
- A. MPC System Model: The state comprises base orientation, center-of-mass position, angular rate, linear velocity, and twelve joint positions.Orientation and position are expressed in the stated world/body-frame conventions.
- A. MPC System Model: The inputs are twelve body-frame end-effector contact forces and twelve desired joint velocities.Joint positions for each leg contain three coordinates.
- A. MPC System Model: The model uses rotation and angular-velocity transformation maps, gravity, total mass, moment of inertia, and body-frame foot positions.The moment of inertia is assumed constant and evaluated at the robot’s upright state.
B. MPC Constraints
The low-frequency kino-dynamic MPC includes mode-dependent constraints that keep stance feet on the ground and guide swing feet along prescribed curves to avoid scuffing.
- B. MPC Constraints: The low-frequency MPC controller includes constraints listed for the kino-dynamic formulation.These constraints govern the admissible locomotion modes and foot behavior.
- 1) Mode Constraints: Mode constraints represent each leg’s mode at every time, with the mode sequence assumed predefined.The resulting constraints depend on the currently assigned leg mode.
- 1) Mode Constraints: Stance-leg constraints set end-effector velocity to zero, while swing-leg constraints impose a normal-direction velocity and zero contact force.These conditions distinguish stationary support from swing motion.
- 1) Mode Constraints: The constraints keep stance legs on the ground and make swing legs follow a prescribed curve along the local surface normal to avoid foot scuffing.The curve is denoted c and is defined over nonnegative time.
2) Friction Cone Constraints:
The MPC constrains contact forces within a friction cone and represents foot placement through terrain-region half-spaces, including a time-shrinking swing-foot constraint.
- 2) Friction Cone Constraints: End-effector contact forces are constrained to lie within a friction cone defined by the surface normal and friction coefficient µc = 0.7.After transforming forces into the local surface frame, the constraint is expressed as a second-order cone.
- 3) State-Only Foot Placement Constraints: State-only foot placement constrains stance-foot positions through an inequality of the form ht_i(x) = Ai · pWi(x) + bi ≥ 0.This formulation applies the constraint directly to the foot-position state.
- 3) State-Only Foot Placement Constraints: Ai and bi define half-spaces that project each world-frame foot position onto the target terrain and keep the foot within a desired region.The foot position pWi maps the state to the world-frame position of foot i.
- 3) State-Only Foot Placement Constraints: For swing feet, the constraint set shrinks over time and converges to the desired placement region as the swing phase ends.The slack term s(t) converges to zero near the swing duration.
4) Barrier Foot Placement Constraints:
The framework imposes foot-placement safety through CBF constraints in both the low-frequency MPC layer and the high-frequency whole-body controller. The whole-body controller converts optimized trajectories into torque commands while accounting for nonlinear dynamics, but model uncertainty can prevent exact CBF satisfaction.
- The low-frequency Safe Kinematic MPC controller specifies foot-placement constraints as CBF constraints with constant γ ∈ R++.
- The MPC layer outputs contact forces and desired joint velocities, which a high-frequency hierarchical inverse dynamics controller converts into torque commands.The whole-body controller considers the system's full nonlinear rigid-body dynamics.
- The whole-body controller incorporates a dynamics-based CBF constraint into its prioritized quadratic-program hierarchy.Each task is an affine equality or inequality constraint in generalized accelerations, torques, and contact forces.
- The controller computes whole-body torque τ_WBC and adds an integral joint-acceleration-error term with gain K to compensate for swing-leg model uncertainty on hardware.This compensation is not used in simulation.
- Model uncertainty means the applied torque, and potentially τ_WBC itself, may not satisfy the CBF condition, requiring a balance between safe inputs and uncertainty.
D. User Commands & Terrain Selection
The controller selects terrain-aware foothold regions from reference trajectories and evaluates multi-layer safety across challenging stepping-stone configurations. Simulation and hardware evidence indicates that combining CBF constraints in MPC and high-frequency tracking improves safety and avoids destabilizing plan-execution mismatches.
- D. User Commands & Terrain Selection: The MPC constructs reference trajectories from commanded twists and gait patterns, while terrain segments are selected near reference end-effector positions for each contact phase.A convex polygon is fit to the selected terrain segment for foothold constraints.
- D. User Commands & Terrain Selection: The stepping-stones simulation uses 0.5 m stones with 0.35 m longitudinal spacing and random displacements up to 10, 15, and 5 cm in longitudinal, lateral, and vertical directions.The robot trots at 0.25 m/s and is commanded to stop on the final stone.
- V. Results: A high-frequency CBF controller without MPC foot-placement constraints enforces safety briefly but quickly destabilizes because safety corrections abruptly depart from the planned motion.This formulation works only when stepping-stones are close to the robot’s nominal gait.
- V. Results: MPC controllers with foot-placement constraints traverse the scenario, while the CBF-based MPC variant achieves similar misstep counts with smaller average misstep size.These controllers omit CBF constraints from the high-frequency controller.
- V. Results: Both-layer CBF enforcement completes the scenario without missteps and yields the least time violating barrier conditions by preventing mismatch between MPC plans and high-frequency tracking.The CBFs in both layers also produce smaller constraint violations than using CBFs only in MPC.
- V. Results: On hardware, the MPC runs at 30 Hz and the whole-body QP tracking controller at 400 Hz; barrier constraints are rarely violated across several steps.The foothold barrier contracts toward the selected target and expands when terrain permits, supporting transitions between rough and flat terrain.
VI. CONCLUSIONS
The proposed multi-layered framework combines CBFs with MPC, with experiments showing benefits when safety constraints are enforced at both control layers and validation on hardware stepping-stones.
- Enforcing CBF constraints in both the MPC and QP tracking layers outperforms variants enforcing them at only one layer.
- The simulation and hardware evaluations use stepping-stones scenarios to assess safety-constrained dynamic locomotion.
- Figures 5 and 7 report the minimum h_w^i and h_w^{e,i} values for each leg in simulation and hardware experiments.
- Figure 6 compares planned MPC trajectories using touchdown-location state constraints against trajectories using CBF constraints.
- Future work includes terrain segmentation from sensor data and analysis of the proposed controller's theoretical properties.