A key motivation behind the development of legged robots is their ability to overcome complex terrain. Because legged locomotion only requires discrete footholds, obstacle such as steps, gaps, and stairs can be traversed, making legged robots a compelling alternative to wheeled systems. When a statically stable motion pattern is considered, several mature strategies for rough terrain locomotion have been proposed and successfully demonstrated on hardware for bipedal , quadrupedal [11, 21], and hexapedal  robots. However, inspired by the fast and dynamic motions seen in nature, the use of dynamic gaits—a gait where individual contact phases are statically unstable—is still an active area of research.
The challenge in dynamic locomotion lies in the fact that foothold locations are not only constrained by the terrain, but also affect the dynamic stability of the resulting contact configuration. Additionally, as the speed of the motions increases, the inertial and nonlinear effects described by the full rigid body dynamics of the system become more relevant. There is therefore a need for methods that can guarantee a safe foot placement while simultaneously considering the future impact on the dynamic stability of the system. A classical locomotion challenge that demands safe foot placement and dynamic stabilization is the “stepping-stones” scenario, see Figure 1, where viable foothold locations are discontinuous and sparsely available. We propose to combine the safety guarantees endowed by Control Barrier Functions (CBFs) with the longer horizon considered in Model Predictive Control (MPC) to guarantee safe foot placement while achieving dynamic locomotion and high tracking performance.
I-a Related work
Control Barrier Functions have recently become popular as a tool for synthesizing controllers that ensure safety of nonlinear systems [4, 2]. They have been used in the context of the stepping-stones problem via a quadratic programming (QP) based tracking controller [25, 24]. An offline optimized walking trajectory, or a library thereof , is tracked and locally modified to satisfy CBF safety constraints. While promising in simulation, we are not aware of the successful transfer of a CBF based stepping controller to hardware, despite extensions that add robustness , or a learning based model error correction . Indeed, in 
, the stepping-stones problem is demonstrated experimentally by increasing the look-ahead horizon of the gait library and through subsequent gait interpolation rather than a CBF based method. We hypothesize that it is exactly this reasoning over a longer horizon that is missing with the CBF-QP control formulation.
In contrast, Model Predictive Control has become a central method for the online synthesis and control of dynamic systems over a given time horizon . In the context of the stepping-stones problem, a distinction can be made between MPC based approaches where the footholds locations are determined separately from the torso motion optimization [19, 33, 20], and MPC based approaches where the foothold location and torso motions are jointly optimized. The benefit of jointly optimizing torso and leg motions has been demonstrated in the field of trajectory optimization [9, 34]. Following this idea, real-time capable methods have been proposed with the specification of leg motions made at the position , velocity , or acceleration level . One challenge of this approach is its computational costs, which can be resolved by coupling a low-frequency MPC controller with a high-frequency tracking controller .
In this work, we build upon a kino-dynamic MPC formulation  where joint velocities and contact forces are decision variables in a low frequency MPC controller. This removes the need for heuristic coordination of separate leg and torso controllers and allows direct integration of CBF safety constraints into the MPC formulation similar to . A higher rate tracking controller is implemented that fuses inverse dynamics with the CBF safety constraints to offer guarantees of safety with the whole-body dynamics in consideration . We note that the combination of discrete time CBFs with MPC has been considered in , but did not consider a multi-layered approach nor provided experimental results.
The main contributions of this work are two-fold. First, we propose a multi-layered control approach that combines CBFs with MPC (see Figure 2). This framework allows CBF safety constraints on the position coordinates of a robotic system to be incorporated in a low frequency MPC controller determining desired velocity coordinates as well as in a high frequency quadratic program based controller that incorporates the dynamics of the system. Compared to standard CBF approaches, this adds a horizon when determining safe control inputs. Compared to MPC approaches, the safety critical constraint is enforced at a higher rate than the MPC rate, and incorporates a higher fidelity whole-body dynamics model. The second contribution is, to the best of the author’s knowledge, the first successful experimental demonstration of CBFs, not only as an approach to the stepping-stones problem, but on a legged robot.
In Section II we provide background on CBFs and MPC. The proposed multi-layer combination for general robotic systems is introduced in Section III. The specific implementation for ANYmal, visualized in Figure 2, is detailed afterwards in Section IV. In Section V we provide simulation and experimental results.
This section provides a review of Control Barrier Functions (CBFs) and Nonlinear Model Predictive Control (NMPC). These tools will be combined in Section III to achieve desired control objectives while satisfying safety constraints.
Consider the nonlinear control affine system given by:
where , , and and are locally Lipschitz continuous on . Given a Lipschitz continuous state-feedback controller , the closed-loop system dynamics are:
The assumption on local Lipschitz continuity of and implies that is locally Lipschitz continuous. Thus for any initial condition there exists a maximum time interval such that is the unique solution to (2) on . In the case that is forward complete, .
Ii-a Control Barrier Functions
The notion of safety that we consider in this paper is formalized by specifying a (potentially time-varying) safe set in the state space that the system must remain in to be considered safe. In particular, consider a time-varying set defined as the 0-superlevel set of a continuously differentiable function , yielding:
We assume that is nonempty and has no isolated points, that is, . We refer to as the safe set. This construction motivates the following definitions of forward invariant and safety:
Definition 1 (Forward Invariant & Safety).
Before defining Control Barrier Functions, we note a continuous function , with , is said to belong to extended class () if and is strictly monotonically increasing. If , , and , then is said to belong to extended class ()
Certifying the safety of the closed-loop system (2) with respect to a set may be impossible if the controller was not chosen to enforce the safety of . Control Barrier Functions can serve as a synthesis tool for attaining the forward invariance, and thus the safety of a set:
Definition 2 (Control Barrier Function (CBF), ).
Let be the time-varying 0-superlevel set of a continuously differentiable function with a regular value. The function is a time-varying Control Barrier Function (CBF) for (1) on if there exists such that for all and :
Given a nominal (but not necessarily safe) locally Lipschitz continuous controller , a controller taking values in the set is the safety-critical CBF-QP:
Ii-B Nonlinear Model Predictive Control
We consider the following nonlinear optimal control problem with cost functional
where is the state and is the input at time . is a time-varying running cost, and is the cost at the terminal state . The goal is to find the continuous control signal that minimizes this cost subject to the following system dynamics, initial condition, and general equality and inequality constraints:
Various methods exist to solve this problem , and a detailed discussion is beyond the scope of this paper. For the illustrative example in Section III, we follow a direct multiple shooting approach where the continuous control signal is parameterized over subintervals of the prediction horizon to obtain a finite dimensional decision problem . For the implementation on ANYmal described in Section IV, we use the Sequential Linear Quadratic (SLQ) method, which is a Differential Dynamic Programming (DDP) based algorithm for continuous-time systems. In particular, the method in  is being used which extends the (SLQ) formulation of  for use with inequality constraints.
Iii Multi-Layered Control Formulation
In this section we present a multi-layered control formulation that unifies CBFs with MPC to achieve safety and longer horizon optimality for a general robotic system (see Figure 2).
Consider a robotic system with generalized coordinates and coordinate rates with dynamics given by:
with symmetric positive definite inertia matrix , centrifugal and Coriolis terms , gravitational forces , actuation matrix , and torques . Consider a continuously differentiable function that determines a time-varying safe set for the position coordinates of the robot. The time derivative of this function is given by:
The torques do not appear in this time derivative, making it impossible to choose inputs that ensure the barrier constraint:
is met for some . This challenge is often resolved through the notion of exponential CBFs , in which an auxiliary function is defined as:
with time derivative:
As the torques appear in affine relation to in (12), can serve as a CBF for the set by enforcing:
for some . Enforcing the forward invariance of this set implies the desired safety constraint (13) is met, implying the forward invariance of the set . Thus the constraint on the position coordinates of the robot are met.
Typical approaches using exponential CBFs only enforce the final constraint (15), often in a CBF-QP controller . In practice this can lead to aggressive behavior with the system moving along the boundary of the safe set . This behavior arises due to the fact that the desired controller is often synthesized ignoring the fact it will be filtered in the CBF-QP. As the goals of optimality and safety are often opposed, this leads to the system riding the boundary of the safe set.
The combination of multi-layered control with MPC enables the safety constraint (13) to be incorporated into the specification of . This is done via a low-frequency MPC controller with prediction horizon . When the MPC directly operates on the full state and input of (12), the safety constraint in (15) is readily posed as an inequality constraint along the horizon through (11), as was done in . In contrast, we consider a MPC controller that operates on a reduced order model in which case the barrier constraint in (13) is added to the MPC problem instead.
Although the following approach can be applied to the general robotic system (12), for simplicity of exposition we consider a double integrator (
) that we wish to drive to the origin. The MPC layer operates on a kinematic model of the system, while the tracking layer uses the full dynamics. We impose a safety constraint on the position of the system that keeps it above a sinusoidal signal. Given a current estimate of the stateat time , a kinematic MPC with temporal discretization solves the following optimization problem:
Low-Frequency Safe Kinematic MPC:
(Double integrator example)
where , is a sequence of generalized coordinates, and is a sequence of coordinate velocities, considered as inputs to the kinematic model. In this example, we constrain the first generalized velocity to be equal to the measured velocity to provide a continuous reference to the low level controller. Note that for the ANYmal implementation this condition is relaxed by using a cost functions that promotes smooth inputs .
For the low level controller is a state, and the true input to the system remains the torque . A finite difference of the first two elements of is therefore taken to yield a desired acceleration:
which is held constant between computations of the MPC solution. Drawing inspiration from the inverse dynamics approach in , the high-frequency controller is given by:
(Double integrator example)
This controller seeks to track the desired acceleration determined by the low-frequency MPC controller while ensuring that the full dynamics are incorporated into the determination of safe inputs.
We compare the described multi-layered controller with two variations: an MPC controller with state constraints and the high-frequency ID-CBF-QP, and an MPC controller with no state or barrier constraints with the high-frequency ID-CBF-QP. The results are seen in Figure 3. We see that adding information on the safety constraint to the MPC layer results in anticipation of the imposed barrier and therefore avoids sudden input peaks when the safety constraint becomes active.
Iv ANYmal Implementation
In this Section we provide an overview of how the multi-layer control formulation discussed in Section III is applied to the ANYmal quadrupedal robotic platform. An overview of the control structure is provided in Figure 2. In the following subsections we describe the MPC, the whole-body control, and finally, how the user commands and terrain selection provide the necessary inputs for both layers.
Iv-a MPC System Model
We apply our approach to the kino-dynamic model of a quadruped robot, which describes the dynamics of a single free-floating body along with the kinematics for each leg. The state and input are defined as:
where is the orientation of the base in Euler angles, is the position of the center of mass in the world frame , is the angular rate of the base, is the linear velocity of the center of mass in the body frame , and is the joint positions. The joint positions for leg are given by . The inputs of the model are end-effector contact forces in the body frame and desired joint velocities with equations of motion:
where is the rotation matrix of the body frame relative to the world frame and is the transformation matrix from angular velocities in the body frame to the Euler angles derivatives in the world frame. Model parameters include the gravitational acceleration in the body frame , the total mass
, and the moment of inertia about the center of mass. The moment of inertia is assumed constant and taken at the upright state of the robot. We denote as the position of foot relative to the center of the mass in the body frame.
Iv-B MPC Constraints
In this subsection we list the constraints that are included in the low-frequency kino-dynamic MPC controller.
Iv-B1 Mode Constraints
The mode constraints capture the different modes of each leg at any given point in time. We assume that the mode sequence is a predefined function of time. The resulting mode-dependent constraints are
where is the end-effector velocity in world frame. These constraints ensure that stance legs remain on the ground and a swing legs follow a predefined curve in the direction of the local surface normal to avoid foot scuffing. Furthermore, the mode constraints enforce zero contact force from swing legs.
Iv-B2 Friction Cone Constraints
The end-effector forces are constrained to lie in the friction cone:
defined by the surface normal and friction coefficient . After resolving the contact forces in the local frame of the surface, given by , a second-order cone constraint is specified as:
Iv-B3 State-Only Foot Placement Constraints
When foot-placement is formulated as a state-only constraint (rather than encoded in a CBF), it is specified as the following inequality constraint on stance feet:
where , , and is the position of foot in the world frame. The matrix and project the position of foot on to the target terrain and form a set of half-space constraints to ensure the foot lands within a desired target region. Instead of constraining the stance feet, a similar constraint can be placed on the swing feet with a constraint set that shrinks in time and converges to the desired foot placement region:
where converges to as the approaches the duration of the swing phase.
Iv-B4 Barrier Foot Placement Constraints
When posed as a CBF constraint as in the proposed low-frequency Safe Kinematic MPC controller, the foot placement constraints are specified with constant as:
Iv-C Whole-Body Tracking Control
The control signal determined by the low-frequency MPC layer consists of contact forces and desired joint velocities. A high-frequency hierarchical inverse dynamics controller is used to convert the optimized MPC trajectory into torque commands . This whole body control (WBC) approach considers the full nonlinear rigid body dynamics of the system. At each priority, a QP is solved in the null space of all higher priority tasks. Each task is a equality or inequality constraint that is affine in the generalized accelerations, torques, and contact forces. The CBF constraints, which are by design affine in the control torques, are therefore readily integrated into this framework. The full list of tasks is given in Table I.
As described in Section III, the following CBF constraint incorporating the dynamics can be included in the whole-body controller:
with . Finally, the torque derived from the whole body controller, , is computed. To compensate for model uncertainty for swing legs, the integral of joint acceleration error with gain is added to the torque applied to the system:
|0||Floating base equations of motion.|
|Friction cone constraint.|
|No motion at the contact points.|
|Control barrier constraints.|
|1||Torso linear and angular acceleration.|
|Swing leg motion tracking.|
|2||Contact force tracking.|
Iv-D User Commands & Terrain Selection
User commanded twists and a desired gait pattern are provided to the robot via joystick and extrapolated to a state reference signal . The reference input is constructed by equally distributing the weight over all contact feet. The MPC cost function is a frequency dependent quadratic cost around the reference trajectories to promote smooth optimal inputs .
We assume that a segmented terrain model with each segment described by a planar boundary and a surface normal is available. For each contact phase within the MPC horizon, the terrain segment is selected that is closest to the reference end-effector position determined by , evaluated at the middle of the stance phase. A convex polygon is fit to the selected terrain, starting from the reference end-effector position projected onto the segment boundary. This polygon, together with the surface normal, define the half spaces for the constraints in (23) and (24).
We evaluate the controller proposed in Section IV in simulation on a classical stepping-stones scenario as shown in Figure 4. The stones are configured with a pattern of width and longitudinal spacing, with random displacements of 10, 15, and , in longitudinal, lateral, and vertical direction respectively. The controller is commanded to perform a trotting gait with a forward velocity of , and commanded to stop on the final stone. We compare our proposed controller against three alternative formulations and report results in Table II.
|CBF-QP||CBF in MPC||MPC baseline||CBF in QP & MPC|
|avg. misstep [mm]||1.4||2.5||4.3||1.2|
|total swing time [s]||11.0||49.0||48.6||48.2|
As seen in the supplementary video , the controller that has no foot placement constraints in the MPC controller and a CBF constraint in the high-frequency controller (denoted CBF-QP, and the closest to the related work ) is able to enforce safety for a number of steps, but quickly destabilizes. We observe a similar effect as shown for the double integrator example in Section III. The absence of an information on the safety constraint in the MPC layer results in an abrupt and strong correction for safety by the high-frequency CBF. Only when the stepping-stones are placed close to the nominal gait of the robot the does this approach work well, but it fails in this more challenging scenario.
The second and third controllers include foot placement constraints in the MPC controller, but not in the high-frequency controller. In the second controller the constraints are implemented as CBFs (CBF in MPC) and in the third controller they are implemented as state constraints (MPC baseline). Both of these controllers are able to successfully traverse the length of the stepping-stones scenario. We see that the controllers exhibit similar numbers of missteps, but the CBF in MPC controller has smaller average misstep size.
The proposed controller (CBF in QP & MPC) successfully completes the scenario with the smallest number of missteps, smallest average misstep size, and least amount of time violating the barrier conditions. This indicates that including barrier constraints in the MPC controller is necessary to ensure the high-frequency controller does not destabilize the system, and that including barrier constraints in the high-frequency controller leads to safer behavior. The values of and for the CBF in MPC controller and the CBF in QP & MPC controller can be seen in Figures 5 and 6. The controller with CBFs at both levels has notably smaller violations of constraints (24) and (25).
We evaluate the efficacy of this method experimentally on the ANYmal robotic platform. All computation runs on a single onboard PC (Intel i7-8850H, , hexa-core 64-bit) with the MPC solver running asynchronously at and the whole-body QP tracking controller running at .
The robot is initialized on pre-mapped terrain and receives external base twist and gait commands. The size of the segmented regions are decreased by with respect to the real boundary to provide a margin for state estimation errors. In Figure 7 we see a snapshot of the experiment with the internal controller state visualized below. For legs that are in swing, a projection of the barrier constraint in (24) onto the terrain is plotted. In the supplementary video  we see how this barrier constraint shrinks over time and converges to the selected target foothold region at foot contact. Furthermore, it can be seen how the foothold target is large when stepping onto the wooden pallet. This shows that the proposed method can seamlessly transition between rough and flat terrain, restricting the motion only when necessary for safe foot placement. The values of and for several steps can be seen in Figure 8. Both constraints are rarely violated, which confirms that the safety constraints are successfully transferred to hardware.
We proposed a multi-layered control framework that combines CBFs with MPC. Simulation experiments show that enforcing CBF constraints on both the MPC and QP tracking layer outperforms variants where they are enforced at only one of the layers. Additionally, we validated the viability of the approach on hardware by demonstrating dynamic locomotion on stepping-stones with safety constraints. Future work includes developing a perception pipeline to automatically perform terrain-based segmentation from sensor data and studying the theoretical properties of the proposed controller.
-  Note: Supplementary video: https://youtu.be/TCDIirXfByE Cited by: Fig. 4, §V, §V, Multi-Layered Safety for Legged Robots via Control Barrier Functions and Model Predictive Control.
-  (2019) Control barrier functions: theory and applications. In European Control Conference (ECC), pp. 3420–3431. Cited by: §I-A.
-  (2017) Control barrier function based quadratic programs for safety critical systems. Transactions on Automatic Control 62 (8), pp. 3861–3876. Cited by: Definition 2.
-  (2014) Control barrier function based quadratic programs with application to adaptive cruise control. In Conference on Decision & Control (CDC), pp. 6271–6278. Cited by: §I-A, §II-A.
-  (2016) Adaptive motion planning for autonomous rough terrain traversal with a walking robot. Journal of Field Robotics 33 (3), pp. 337–370. External Links: Cited by: §I.
-  (2017-Sept) Policy-regularized model predictive control to stabilize diverse quadrupedal gaits for the mit cheetah. In International Conference on Intelligent Robots and Systems (IROS), Vol. , pp. 4102–4109. External Links: Cited by: §I-A.
-  (1984) A multiple shooting algorithm for direct solution of optimal control problems. IFAC Proceedings Volumes 17 (2), pp. 1603–1608. Cited by: §II-B.
-  (2020-07) Reinforcement Learning for Safety-Critical Control under Model Uncertainty, using Control Lyapunov Functions and Control Barrier Functions. In Robotics: Science and Systems (RSS), Corvalis, Oregon, USA. External Links: Cited by: §I-A.
-  (2014-11) Whole-body motion planning with centroidal dynamics and full kinematics. In International Conference on Humanoid Robots (Humanoids), Vol. , pp. 295–302. External Links: Cited by: §I-A.
-  (2016-11) Perception-less terrain adaptation through whole body control and hierarchical optimization. In International Conference on Humanoid Robots (Humanoids), Vol. , pp. 558–564. External Links: Cited by: §IV-C.
-  (2018) Robust rough-terrain locomotion with a quadrupedal robot. In International Conference on Robotics and Automation (ICRA), Cited by: §I.
-  (2017) Real-time motion planning of legged robots: a model predictive control approach. In International Conference on Humanoid Robotics (Humanoids), Vol. , pp. 577–584. Cited by: §I-A, §I-B.
-  (2017-05) An efficient optimal planning and control framework for quadrupedal locomotion. In 2017 IEEE International Conference on Robotics and Automation (ICRA), Vol. , pp. 93–100. External Links: Cited by: §II-B.
-  (2020) Nonlinear model predictive control of robotic systems with control lyapunov functions. In Robotics: Science and Systems (RSS), External Links: Cited by: §I-B.
-  (2018) Frequency-aware model predictive control. arXiv preprint arXiv:1809.04539. Cited by: §III, §IV-D.
-  (2019) Feedback mpc for torque-controlled legged robots. In International Conference on Intelligent Robots and Systems (IROS), pp. 4730–4737. Cited by: §I-A, §II-B.
-  (2019) Footstep planning for autonomous walking over rough terrain. In International Conference on Humanoid Robots (Humanoids), Vol. , pp. 9–16. Cited by: §I.
-  (2016) ANYmal - a highly mobile and dynamic quadrupedal robot. In 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Vol. , pp. 38–44. Cited by: Fig. 1.
-  (2020) Perceptive locomotion in rough terrain–online foothold optimization. IEEE Robotics and Automation Letters 5 (4), pp. 5370–5376. Cited by: §I-A.
-  (2020) Vision aided dynamic exploration of unstructured terrain with a small-scale quadruped robot. In International Conference on Robotics and Automation (ICRA), Vol. , pp. 2464–2470. Cited by: §I-A.
-  (2018) Motion planning for quadrupedal locomotion: coupled planning, terrain mapping and whole-body control. Prepint on HAL. Cited by: §I.
-  (2018-07) Whole-body nonlinear model predictive control through contacts for quadrupeds. IEEE Robotics and Automation Letters 3 (3), pp. 1458–1465. External Links: Cited by: §I-A.
-  (2020) Dynamic walking on stepping stones with gait library and control barrier functions. In Algorithmic Foundations of Robotics XII, pp. 384–399. Cited by: §I-A.
-  (2016) 3d dynamic walking on stepping stones with control barrier functions. In Conference on Decision and Control (CDC), pp. 827–834. Cited by: §I-A, §V.
-  (2015) Optimal robust control for bipedal robots through control lyapunov function based quadratic programs.. In Robotics: Science and Systems (RSS), Cited by: §I-A.
-  (2016) Exponential control barrier functions for enforcing high relative-degree safety-critical constraints. In American Control Conference (ACC), pp. 322–328. Cited by: §III.
-  (2016) Optimal robust time-varying safety-critical control with application to dynamic walking on moving stepping stones. In Dynamic Systems and Control Conference (DSCC), Vol. 50701, pp. V002T28A005. Cited by: §I-A.
-  (2018) Dynamic bipedal locomotion over stochastic discrete terrain. The International Journal of Robotics Research 37 (13-14), pp. 1537–1553. External Links: Cited by: §I-A.
-  (2013) Differential equations and dynamical systems. Vol. 7, Springer Science & Business Media. Cited by: §II.
-  (2017) Model predictive control: theory, computation, and design. Vol. 2, Nob Hill Publishing Madison, WI. Cited by: §I-A, §II-B.
-  (2020) An inverse dynamics approach to control lyapunov functions. In American Control Conference (ACC), pp. 2444–2451. Cited by: §III.
-  (2020) Multi-rate control design leveraging control barrier functions and model predictive control policies. arXiv preprint arXiv:2004.01761. Cited by: §I-B, §III.
-  (2020) MPC-based controller with terrain insight for dynamic legged locomotion. In International Conference on Robotics and Automation (ICRA), pp. 2436–2442. Cited by: §I-A.
-  (2018-07) Gait and trajectory optimization for legged systems through phase-based end-effector parameterization. IEEE Robotics and Automation Letters 3 (3), pp. 1560–1567. External Links: Cited by: §I-A.
-  (2020) Safety-critical model predictive control with discrete-time control barrier function. arXiv preprint arXiv:2007.11718. Cited by: §I-B, §III.