Quadruped Locomotion: Convex Model Predictive Control and Whole-Body Impulse Control

Quadruped Locomotion: Convex Model Predictive Control and Whole-Body Impulse Control

Dynamic quadrupedal robots (such as MIT Cheetah, Boston Dynamics Spot, and Unitree B2) traverse irregular unstructured terrain with agile trot, bound, and gallop gaits. Achieving balance under external force perturbations requires a hierarchical control split: high-level Convex Model Predictive Control (Convex MPC) executing at $30\text{ Hz}$, combined with low-level Whole-Body Impulse Control (WBC) executing at $1\text{ kHz}$.

1. Single Rigid Body Model (SRBM) Dynamics

To maintain real-time solvability across rolling horizons, the high-level planner abstracts the multi-link quadruped into a single rigid body with mass $m$ and rotational inertia tensor $\mathbf{I} \in \mathbb{R}^{3 \times 3}$:

$$\ddot{\mathbf{p}} = \frac{1}{m} \sum_{i=1}^{4} \mathbf{f}_i - \mathbf{g}$$

$$\frac{d}{dt} (\mathbf{I} \boldsymbol{\omega}) = \sum_{i=1}^{4} (\mathbf{r}_i - \mathbf{p}) \times \mathbf{f}_i$$

Where:

• $\mathbf{p} \in \mathbb{R}^3$ is the center of mass (CoM) position.
• $\mathbf{r}_i \in \mathbb{R}^3$ is the contact footstep position of leg $i$.
• $\mathbf{f}_i \in \mathbb{R}^3$ is the Ground Reaction Force (GRF) exerted by the terrain onto foot $i$.

           Center of Mass (CoM) & Ground Reaction Forces
                    +--------------------+
                    | Quadruped Chassis  |  (m, I)
                    +--------------------+
                      /      |    |                          /       |    |                          /        |    |                      [Foot 1]   [Foot 2] [Foot 3] [Foot 4]
                 ^           ^        ^        ^
                 | f1        | f2     | f3     | f4
          ============================================== Terrain

2. Friction Pyramid Constraints & Quadratic Programming

Ground contact forces must strictly satisfy Coulomb friction cone boundaries to prevent slippage. The nonlinear cone $|f_x| \le \mu f_z$ and $|f_y| \le \mu f_z$ is approximated by a 4-sided friction pyramid:

$$-\mu f_{z,i} \le f_{x,i} \le \mu f_{z,i}, \quad -\mu f_{z,i} \le f_{y,i} \le \mu f_{z,i}, \quad f_{z,i} \ge 0$$

The MPC problem over horizon $K$ frames is cast as a constrained Quadratic Program (QP):

$$\min_{\mathbf{F}} \sum_{k=0}^{K-1} |\mathbf{x}{k+1} - \mathbf{x}{k+1}^{\text{ref}}|_{\mathbf{Q}} + |\mathbf{F}k|{\mathbf{R}}$$

Subject to linearized discrete state dynamics $\mathbf{x}_{k+1} = \mathbf{A}_k \mathbf{x}_k + \mathbf{B}_k \mathbf{F}_k$ and contact friction bounds.

3. High-Frequency Joint Torque Actuation (WBC)

Once optimal ground reaction forces $\mathbf{f}_i^*$ are resolved by the QP solver, the $1\text{ kHz}$ joint controller converts Cartesian forces into actuator motor currents via the leg kinematic Jacobian $\mathbf{J}_i(\mathbf{q})$:

$$\boldsymbol{\tau}_i = \mathbf{J}_i^T(\mathbf{q}) \mathbf{f}_i^* + \mathbf{K}_p (\mathbf{q}_i^{\text{des}} - \mathbf{q}_i) + \mathbf{K}_d (\dot{\mathbf{q}}_i^{\text{des}} - \dot{\mathbf{q}}_i)$$

// C++ Eigen Real-time Leg Torque Generator
#include <Eigen/Dense>

Eigen::Vector3d compute_leg_joint_torques(
    const Eigen::Matrix3d& jacobian,
    const Eigen::Vector3d& desired_grf,
    const Eigen::Vector3d& q_actual,
    const Eigen::Vector3d& q_desired,
    const Eigen::Vector3d& qd_actual,
    const Eigen::Vector3d& qd_desired,
    const Eigen::Vector3d& kp,
    const Eigen::Vector3d& kd) 
{
    // Transpose Jacobian mapping Cartesian forces to joint torques
    Eigen::Vector3d tau_feedforward = jacobian.transpose() * desired_grf;

    // Joint-space PD impedance stabilization
    Eigen::Vector3d tau_feedback = kp.cwiseProduct(q_desired - q_actual) +
                                   kd.cwiseProduct(qd_desired - qd_actual);

    return tau_feedforward + tau_feedback;
}

Through synchronized phase scheduling and high-bandwidth planetary gear motor controllers, quadruped platforms achieve robust recovery from unexpected kinetic impacts and variable soil compliance.