Safety-Critical and Distributed Nonlinear Predictive Controllers for Teams of Quadrupedal Robots

TL;DR

This work addresses safe, cooperative locomotion for teams of quadrupedal robots under disturbances and partial obstacle information by developing a three-layer hierarchical control framework that couples distributed nonlinear model predictive control (DNMPC) with discrete-time high-order control barrier functions (HOCBFs). The middle layer executes real-time DNMPC at $N=10$ using SRB dynamics, with a one-step delay communication protocol and a Lennard-Jones-based consensus term to enforce a soft flocking-like sticking distance, while a high-level offline planner provides reference trajectories and a low-level 1 kHz whole-body controller enforces full-order dynamics. The approach unifies CBF safety within the NMPC optimization, yielding substantial gains in collision avoidance and planning horizon length, with hardware experiments (two A1s) and simulations (four A1s) showing improved collision-free success rates (up to $94\%$) and faster computation (NMPC loop at $100$ Hz, WBC at $1$ kHz) compared to prior NMPC/CBF setups. Overall, the framework enables safe, scalable, and robust cooperative locomotion for quadrupedal robot teams in challenging terrains and uncertain environments, with practical implications for rescue, exploration, and disaster-response missions.

Abstract

This paper presents a novel hierarchical, safety-critical control framework that integrates distributed nonlinear model predictive controllers (DNMPCs) with control barrier functions (CBFs) to enable cooperative locomotion of multi-agent quadrupedal robots in complex environments. While NMPC-based methods are widely adopted for enforcing safety constraints and navigating multi-robot systems (MRSs) through intricate environments, ensuring the safety of MRSs requires a formal definition grounded in the concept of invariant sets. CBFs, typically implemented via quadratic programs (QPs) at the planning layer, provide formal safety guarantees. However, their zero-control horizon limits their effectiveness for extended trajectory planning in inherently unstable, underactuated, and nonlinear legged robot models. Furthermore, the integration of CBFs into real-time NMPC for sophisticated MRSs, such as quadrupedal robot teams, remains underexplored. This paper develops computationally efficient, distributed NMPC algorithms that incorporate CBF-based collision safety guarantees within a consensus protocol, enabling longer planning horizons for safe cooperative locomotion under disturbances and rough terrain conditions. The optimal trajectories generated by the DNMPCs are tracked using full-order, nonlinear whole-body controllers at the low level. The proposed approach is validated through extensive numerical simulations with up to four Unitree A1 robots and hardware experiments involving two A1 robots subjected to external pushes, rough terrain, and uncertain obstacle information. Comparative analysis demonstrates that the proposed CBF-based DNMPCs achieve a 27.89% higher success rate than conventional NMPCs without CBF constraints.

Figures (6)

• Figure 1: Top-view snapshot of an experiment demonstrating CBF-based DNMPC algorithms, where two Unitree A1 robots navigate a challenging environment with uncertainties in obstacle positions and rough terrain.
• Figure 2: Overview of the proposed hierarchical control based on CBF-based distributed NMPCs for safe locomotion of multi-agent quadrupedal robots.
• Figure 3: Snapshots of experiments demonstrating the deployment of CBF-based DNMPCs in a multi-agent A1 setup under various conditions: (a) uncertain obstacles, (b) intentional intersecting reference trajectories on rough terrain, (c) straight-line reference trajectories with one agent physically pushed onto another, and (d) cooperative locomotion on rough terrain with uncertain obstacles. (e) Snapshot of simulations with four agents navigating rough terrain.
• Figure 4: Plot of the reference COM velocity trajectories ($\dot{x}^\textrm{ref}, \dot{y}^\textrm{ref}$) generated by the high-level planner along with the optimal velocity trajectories ($\dot{x}^{\star}, \dot{y}^{\star}$) adjusted by the middle-level DNMPCs for agents 1 and 2 in (a) and (b) during experiments of Fig. \ref{['Fig_Snaphosts']}(d). (c) Plot of the vertical component of the GRF profile prescribed by the DNMPC for agent 1.
• Figure 5: Snapshots of experiments (a) with consensus protocol and (b) without consensus protocol. (c) Plots for the consensus cost $w U^i$ for $w = 0$ (no consensus) and $w=10^9$ (with consensus). (d) Distance between the two A1 agents for $w = 0$ and $w=10^9$, where it can be seen that after the consensus has been achieved, agents maintain a sticking distance when obstacle avoidance is not an active constraint in the DNMPC.

Theorems & Definitions (1)

• theorem 1