Safety-Critical Centralized Nonlinear MPC for Cooperative Payload Transportation by Two Quadrupedal Robots
Authors: Ruturaj S. Sambhus, Yicheng Zeng, Kapi Ketan Mehta, Jeeseop Kim, Kaveh Akbari Hamed
Organizations: R.S. Sambhus, Y. Zeng, K. K. Mehta, and K. Akbari Hamed are with the Department of Mechanical Engineering, Virginia Tech, Blacksburg, VA 24061, USA · J. Kim is with The University of Texas at El Paso, El Paso, TX 79968, USA
This paper presents a safety-critical centralized nonlinear model predictive control (NMPC) framework for cooperative payload transportation by two quadrupedal robots. The interconnected robot-payload system is modeled as a discrete-time nonlinear differential-algebraic system, capturing the coupled dynamics through holonomic constraints and interaction wrenches. To ensure safety in complex environments, we develop a control barrier function (CBF)-based NMPC formulation that enforces collision avoidance constraints for both the robots and the payload. The proposed approach retains the interaction wrenches as decision variables, resulting in a structured DAE-constrained optimal control problem that enables efficient real-time implementation. The effectiveness of the algorithm is validated through extensive hardware experiments on two Unitree Go2 platforms performing cooperative payload transportation in cluttered environments under mass and inertia uncertainty and external push disturbances.
Figures & tables
Fig. 1: Snapshot of two Unitree Go2 quadrupedal robots cooperatively transporting a shared payload using the proposed CBF-based centralized NMPC framework in a cluttered environment with cylindrical obstacles. The framework autonomously adjusts the orientations of the robots and the payload to avoid collisions while maintaining safe and stable transportation.
Fig. 2: Overview of the proposed layered control framework. The high-level CBF-based NMPC computes optimal trajectories for the interconnected robot–payload SRB system under holonomic constraints, while low-level nonlinear whole-body controllers enforce full-order robot dynamics.
Fig. 3: Illustration of the interconnected SRB model of the robotic agents and the shared payload, showing the rigid holonomic coupling constraints and interaction wrenches.
Fig. 4: Illustration of the rigid mechanism between the robots and the payload, with additional masses placed in the basket. The connection enforces translational rigidity, restricts relative roll and pitch motions, and allows relative yaw motion between the robots and the payload.
Fig. 5: Snapshots of all experiments: (a) Experiment 1 with a nominal payload of 5 kg; (b) Experiment 2 with an unmodeled payload of 11.2 kg; (c) Experiment 3 with an unmodeled payload of 11.2 kg under external push disturbances; and (d)–(e) Experiment 4 with different obstacle layouts.
Experiment
Mean
Std. Dev.
Exp. 1 (Nominal Payload)
11.42
0.6
Exp. 2 (Robustness to Payload Uncertainty)
11.56
0.42
Exp. 3 (Robustness to Disturbances)
11.45
0.5
TABLE I: Statistics of NMPC solve time (in milliseconds) across three experiments
Fig. 6: Reference and actual CoM velocity trajectories along the xy -axes for agent 1 during nominal cooperative payload transportation with a 5 kg payload. The yaw plot corresponds to the payload orientation, which is autonomously adjusted by the proposed algorithm to safely navigate through obstacles. The figure also shows the optimal GRFs and interaction wrench profiles (forces and torques) computed by the high-level CBF-based NMPC. The second column illustrates the HOCBF functions hi,ℓ for each agent and the six obstacles. All HOCBF functions remain nonnegative throughout the experiment, certifying safety for both the robots and the payload.
Fig. 7: CoM trajectories of the robotic agents and the shared payload in the xy -plane. The rigid mechanism is illustrated by a black line connecting the three systems. The obstacles are represented by solid circles, and the dashed regions around them denote the corresponding safe sets. The plot presents results for Experiments 1, 3, and 4.
Fig. 8: Reference and actual CoM velocity trajectories along the xy -axes for agent 1 during cooperative payload transportation with an uncertain payload of 11.2 kg, subject to unmodeled external push disturbances. The yaw plot corresponds to the payload orientation, which is autonomously adjusted by the proposed algorithm to safely navigate through obstacles. The figure also shows the optimal GRFs and interaction wrench profiles (forces and torques) computed by the high-level CBF-based NMPC. The second column illustrates the HOCBF functions hi,ℓ for each agent and the six obstacles, with the time intervals of external pushes highlighted. The HOCBF functions remain nonnegative throughout the experiment, except for a minor and transient violation in h2,6 immediately following a push disturbance; the controller subsequently drives agent 2 back to the safe set.
Center for Artificial Intelligence and Robotics, Indian Institute of Technology Mandi, India · Department of Mechanical Engineering, Clemson University, Clemson, SC 29630, USA