Three-Level Whole-Body DRC Framework

• The paper introduces a novel three-level control framework that leverages hierarchical planning, moving horizon disturbance observation, and optimization-based whole-body control to improve robust tracking and fault tolerance.
• It integrates full-state estimation with robust MPC to replan trajectories dynamically, ensuring effective handling of external disturbances, payloads, and sensor noise.
• Experimental and simulation results demonstrate superior performance over traditional two-level controllers in maintaining stability and adaptability under uncertain conditions.

A Three-Level Whole-Body Disturbance Rejection Control Framework (T-WB-DRC) is an advanced hierarchical control architecture that enhances the robustness and stability of legged robots in the presence of model uncertainties, external disturbances, and system faults. This framework integrates full-state estimation, moving horizon disturbance observation, and staged control optimization to simultaneously guarantee trajectory tracking, payload handling, and disturbance compensation across the entire system. It addresses the limitations of traditional two-level frameworks by introducing an explicit mid-level disturbance estimation and compensation layer, resulting in improved external perturbation rejection and fault tolerance during dynamic locomotion and complex interactions in both simulated and real-world scenarios (Li et al., 19 Aug 2025).

1. Hierarchical Structure of the T-WB-DRC Framework

The T-WB-DRC framework is explicitly organized into three distinct control levels, each serving a unique function in the overall disturbance rejection process:

1. High-Level Control (Task and Trajectory Planning):
   • Employs model predictive control (MPC) based on the nominal centroidal dynamics of the robot with no explicit consideration of uncertainties.
   • Generates reference trajectories for ground reaction forces (GRFs), joint positions/velocities, and centroidal momentum.
   • Governing dynamics:

   $$D(q)\ddot{q} + C(q,\dot{q})\dot{q} + G(q) = S\tau + J(q)F + d$$

   where $d$ denotes lumped uncertainties and disturbances relevant at the whole-body scale.

2. Mid-Level Control (Uncertainty Estimation and Compensation):

   • Integrates a Moving Horizon Extended State Observer (MH-ESO) to estimate lumped system uncertainties/disturbances and simultaneously mitigate sensor noise.
   • Deploys a robust MPC to replan commands and state trajectories with respect to the latest disturbance estimates.
   • The MH-ESO processes measurement data (positions, velocities), updating an augmented state that models additive disturbances as extra variables:

   $$\begin{aligned} \dot{x}_1 &= x_2 \ \dot{x}_2 &= f_e(x_1, x_2) + g_e(x_1)u + x_3 \ \dot{x}_3 &= h_x(t) \end{aligned}$$

3. Low-Level Control (Whole-Body Control and Optimization):

   • Utilizes a hierarchical optimization-based whole-body controller (WBC) that enforces both task constraints (e.g., trajectory tracking) and dynamic/physical constraints (e.g., torque, contact, friction limits).
   • The WBC takes re-planned mid-level references and compensates for residual uncertainties, ensuring robust tracking under perturbations.
   • Key variables are ground reaction forces $F$, actuator torques $\tau$, and resulting states $q, \dot{q}$, governed by the corrected full-body dynamics.

The division of control responsibilities—trajectory planning under nominal conditions, estimation and robust re-planning under uncertainty, and physically consistent low-level actuation—yields separation of concerns, modular upgrade potential, and improved stability compared to two-level structures (Li et al., 19 Aug 2025).

2. Moving Horizon Extended State Observer (MH-ESO)

The MH-ESO is a central element in the T-WB-DRC, designed to estimate dynamically evolving uncertainties/disturbances and reduce the sensitivity of the observer to measurement noise:

• Architecture and Dynamics:
  • Augments the system state with an extra disturbance variable, resulting in a system of the form:

  $$\dot{x}_1 = x_2, \;\; \dot{x}_2 = f_e(x_1, x_2) + g_e(x_1)u + x_3, \;\; \dot{x}_3 = h_x(t)$$ - The MH-ESO solves an optimal estimation problem over a receding (moving) time window, penalizing errors between predicted and measured states to filter noise.

• Mathematical Formulation:

  • The moving horizon estimation is posed as a constrained optimization:

  $$\min_{X,U}\;\; J_u(x_M) + \sum_{i=0}^{M-1} \varphi_{u}(x_k, u_k) + l_{i}(x_k,u_k)$$

  subject to

  $$\begin{aligned} x_{0,k} - x_{m,k} &= 0, \ x_{i+1,k} - x_{i,k} - f_c(x_{i,k},u_{i,k})\delta t_0 &= \hat{d}_c, \ g_i(x_{i,k},u_{i,k}) &= 0, \quad i = 0,\dots,M-1 \end{aligned}$$

  where $\hat{d}_c$ denotes the estimated centroidal uncertainty.

• Noise Mitigation:

  • Incorporates signal processing (saturators and filtering) and leverages sparse LU decomposition to solve the resulting equations efficiently.
  • Provides less noisy and more reliable estimates than classical high-gain ESOs under real-world sensor conditions.
• Integration Role:
  • Outputs are fed into the mid-level MPC, enabling informed re-planning and adaptation to rapidly changing disturbances or modeling errors.

3. Enhanced Payload, Fault, and Disturbance Robustness

The T-WB-DRC framework achieves robustness against a wide class of disturbances and faults:

• Simulation Demonstrations:
  • Using both humanoid and quadruped robot models in the Gazebo simulator, the T-WB-DRC demonstrated maintenance of desired base heights and postures, even under 10 kg payloads or when actuator torques were artificially degraded (e.g., knee joint torque reduced by 50%), conditions under which conventional two-level or baseline WBCs failed.
• Experimental Trials:
  • On a physical Unitree A1 quadruped, T-WB-DRC enabled stable locomotion and upright posture both during load-carrying (up to 10 kg) and in the face of actuator output faults, outperforming previous disturbance rejection frameworks in terms of trajectory tracking (base link height, orientation) and fault tolerance.
• Disturbance Coverage:
  • The staged use of disturbance estimation (MH-ESO), robust trajectory re-planning, and hierarchical physical constraint enforcement leads to resilience under external perturbations, environment unpredictability (e.g., rough terrain), and internal damage.
• Quantitative Indicators:
  • Metrics included reduced deviation from reference base positions/angles, preserved momentum trajectories, and performance continuity in the presence of both uncertainty and actuation faults (Li et al., 19 Aug 2025).

4. Mathematical and Optimization Formulations

Several key mathematical formulations underpin the T-WB-DRC:

• Full-Body Dynamics:

$$D(q)\ddot{q} + C(q,\dot{q})\dot{q} + G(q) = S\tau + J(q)F + d$$

where $q$ (configuration), $\dot{q}$ (velocity), $D$ (inertia), $C$ (Coriolis), $G$ (gravity), $S$ (selection), $\tau$ (actuator torques), $J$ (contact Jacobian), $F$ (contact force vector), $d$ (lumped uncertainty/disturbance).

• Centroidal Dynamics (Planning):

$$\begin{gathered} \dot{h} = \sum_{i=1}^{n_c} f_i - mg \ \dot{L} = \sum_{i=1}^{n_c} ((p_i - c) \times f_i) \end{gathered}$$

$h$ is centroidal linear momentum, $L$ is centroidal angular momentum, $p_i$ are contact points, $c$ is the centroid.

• Observer Update Laws:

$$\begin{aligned} \dot{\hat{x}}_1 &= \hat{x}_2 + 3\omega_0 (x_1 - \hat{x}_1), \ \dot{\hat{x}}_2 &= f_e(x_1,x_2) + g_e(x_1) \hat{u} + \hat{x}_3 + 3\omega_0^2 (x_1 - \hat{x}_1), \ \dot{\hat{x}}_3 &= \omega_0^3 (x_1 - \hat{x}_1) \end{aligned}$$

where $\omega_0$ tunes observer bandwidth.

• Robust MPC (Mid-Level):

$$\min_{X,U}\;\; J_u(x_M) + \sum_{i=0}^{M-1} \varphi_{u}(x_k, u_k) + l_{i}(x_k,u_k)$$

with constraints incorporating estimated disturbances.

5. Integration and Comparative Advantages over Prior Art

• Explicit Three-Stage Modularization:
  • Division into nominal reference planning, robust estimation/adaptation, and task/actuator-level enforcement—each optimized for its respective role.
• Decoupling of Estimation and Tracking:
  • MH-ESO-based estimation is shielded from direct actuation noise and operates on state trajectories, increasing reliability even in high-noise environments.
• Improved Performance under Heavy Loads, Faults, and Uncertainties:
  • The inclusion of a dedicated mid-level allows both high-level planning and low-level control to be dynamically recalibrated as new uncertainty information becomes available.
• Computational Feasibility:
  • The approach leverages real-time moving horizon estimation techniques with computationally efficient solvers (e.g., sparse LU), ensuring practical applicability on modern robot hardware.
• Comparison with Two-Level WB-DRC:
  • The additional intermediate stage in T-WB-DRC enables more granular and noise-robust adaptation, as evidenced by better experimental outcomes on physical quadrupeds and humanoids (Li et al., 19 Aug 2025).

6. Applications and Implications for Legged Robotic Systems

• Domains of Impact:
  • Search and rescue robotics in unstable or unpredictable terrains.
  • Industrial or service robots handling heavy payloads or subject to actuator/sensor faults.
  • Field robotics in planetary exploration, disaster response, or environments with severe external perturbations.
• Research and Extension Trajectories:
  • The modular T-WB-DRC structure allows for future integration with learning-based adaptive estimators, more expressive uncertainty models, or task-level planners with dynamic priority re-allocation.
  • The successful experimental validation with real robots (e.g., Unitree A1) and simulation in high-fidelity physics engines (Gazebo) supports extension to more complex morphologies and multi-modal robots.
• Potential Limitations:
  • Scalability is subject to the computational complexity of the MH-ESO and robust MPC layers, although current results indicate real-time feasibility for medium-scale legged robots.
  • Performance is contingent upon accurate modeling of centroidal dynamics and the appropriate parameterization of the moving horizon window and associated gains.

7. Summary Table: Core Structural Distinctions

Level	Main Function	Key Methods
High-Level	Trajectory & task planning (nominal)	MPC on centroidal dynamics
Mid-Level	Uncertainty/disturbance estimation & adaptation	MH-ESO, robust MPC re-planning
Low-Level	Whole-body control, constraint enforcement	Hierarchical optimization (WBC)

The T-WB-DRC framework thus delivers a comprehensive, experimentally validated approach to robust, noise-resilient, and highly adaptive disturbance rejection in dynamic legged robotic systems, significantly advancing payload handling, fault tolerance, and terrain adaptability over previous two-level frameworks (Li et al., 19 Aug 2025).