An Improved Hierarchical Optimization Framework for Walking Control of Underactuated Humanoid Robots Using Model Predictive Control and Whole Body Planner and Controller

Abstract

This paper addresses the fundamental challenge of achieving stable and efficient walking in a lightweight, underactuated humanoid robot that lacks an ankle roll degree of freedom. To tackle this relevant critical problem, we present a hierarchical optimization framework that combines model predictive control (MPC) with a tailored whole body planner and controller (WBPC). At the high level, we employ a matrix exponential (ME)-based discretization of the MPC, ensuring numerical stability across a wide range of step sizes (5 to 100 ms), thereby reducing computational complexity without sacrificing control quality. At the low level, the WBPC is specifically designed to handle the unique kinematic constraints imposed by the missing ankle roll DOF, generating feasible joint trajectories for the swing foot phase. Meanwhile, a whole body control (WBC) strategy refines ground reaction forces and joint trajectories under full-body dynamics and contact wrench cone (CWC) constraints, guaranteeing physically realizable interactions with the environment. Finally, a position–velocity–torque (PVT) controller integrates feedforward torque commands with the desired trajectories for robust execution. Validated through walking experiments on the MuJoCo simulation platform using our custom-designed lightweight robot X02, this approach not only improves the numerical stability of MPC solutions, but also provides a scientifically sound and effective method for underactuated humanoid locomotion control.

1. Introduction

Humanoid robots, as highly articulated floating-base robotic systems, have become a focal point of whole body control (WBC) research over the past decade. Underactuated leg structures lacking ankle roll joints can simplify robot design and reduce costs, but also impose significant control challenges for WBC. Whole body plan (WBP) methods based on operational space (OS) control [1], and null space (NS) techniques [2] have been widely applied to biped humanoid robots for tasks such as dynamic walking [3], jumping [4], and manipulation [5]. These methods present two main advantages for humanoid robots: First, they enable the design of NS-based multi-priority tasks, which are crucial for scenarios like walking that involve stance leg contact, swing leg planning, and center of mass (CoM) maintenance [6], thus ensuring orderly trajectory planning. Second, the mapping operations between OS and joint space Jacobians can reduce the influence of a specific task space degree of freedom (DOF) on joint space, enhancing control reliability over underactuated leg structures [7,8]. Furthermore, walking task control needs to meet the reliability requirements of high-frequency control commands, actual physical constraints, and accurate ground reaction forces [9,10]. WBC based on contact force constraints and dynamic constraints has also been widely applied in humanoid robots. A WBC approach involving ground reaction forces and whole-body dynamic constraints was proposed to calculate desired joint torques [11]. A hybrid approach combining NS kinematic planning and friction cone-constrained whole-body dynamic optimization has been shown to effectively control humanoid gait [12].

Although WBC can generate high-frequency, real-time joint control commands to drive humanoid or quadruped robots, the solutions produced by WBC are typically local optima [13]. Model predictive control (MPC), as an optimization-based approach with a finite prediction horizon, can fully exploit the robot’s kinematic range and torque limits within this horizon [14,15]. Furthermore, MPC’s optimization result adheres to the Bellman optimality principle, ensuring that each update yields a globally optimal solution with continuity [16]. However, for high-dimensional humanoid or quadruped robots, the combination of a long prediction horizon and system complexity significantly increases the dimensionality of the optimization variables, which is especially burdensome for nonlinear MPC [17], leading to computational challenges that undermine real-time performance. Common solutions include simplifying the humanoid robot model to an inverted pendulum [18,19,20] or a single rigid-body dynamic (SRBD) model [21,22,23], using linearized convex MPC models [24,25], and employing discretization techniques such as forward Euler (FE) or matrix exponential (ME) methods [26,27,28,29]. However, simplifying the robot model or MPC model, or applying discretization inevitably reduces the accuracy of MPC solutions, making it infeasible to directly use the output for robot control in complex systems.

Despite these advancements, three key challenges remain: underactuated Robot Control, optimization numerical stability, and integrated optimally planning and control. This study aims to develop an improved hierarchical optimization control strategy for underactuated humanoid robots by integrating the global optimal control capabilities of MPC with the high-frequency, precise control capabilities of WBC. First, for the high-level optimization, a convex MPC based on the SRBD model (as in MIT Cheetah 3 [21]) is employed to generate optimal CoM trajectories and estimated ground reaction forces. To ensure numerical stability in high-stiffness systems and reduce computational load, the ME method is utilized instead of the FEr method, allowing for accurate predictions even with larger discretization steps. Second, for the low-level optimization, a multi-priority WBP is designed to generate desired trajectories from operational space to joint space for an underactuated humanoid robot without ankle roll joints. Since the simplified MPC model cannot directly provide accurate ground reaction forces for WBC, further optimization of these forces is necessary. As described in [30], the foot contact surface can be modeled as a polygon, and the resultant wrench from multiple contact points must satisfy the contact wrench cone (CWC) constraints to ensure feasible contact forces and a stable contact mode. Thus, a WBC strategy is constructed with constraints on whole-body dynamics, joint torques, and CWC. This ensures that the output joint torques comply with physical constraints while achieving precise trajectory tracking. The WBC’s optimization objective focuses on incremental ground reaction forces and joint accelerations. Using the upstream planner’s output as the initial solution to the QP problem enables warm-starting, which accelerates convergence, improves continuity, and enhances smoothness of the output.

To address the challenging problem of whole-body complex planning and control for underactuated humanoid robots, the proposed method has the following three innovations:

• The proposed convex MPC strategy employs ME discretization method, ensuring numerical stability even with large discretization steps. This approach enables globally optimal trajectory prediction while reducing computational costs.
• The proposed complementary hierarchical optimization framework integrates the high-level MPC planner and the low-level WBPC, effectively balancing the need for globally optimal trajectories and high-frequency control commands that comply with physical constraints. This enhances system real-time performance, stability and robustness.

The proposed framework is validated on X02: a lightweight underactuated humanoid robot lacking ankle roll DOF. Walking experiments verify the effectiveness of the hierarchical optimization strategy. The subsequent sections are organized as follows: Section 2 introduces the preliminaries, including MPC, SRBD, and WBP; Section 3 presents the proposed hierarchical optimization framework; Section 4 conducts walking simulation experiments on the X02 robot based on MuJoCo; Section 5 discusses the experimental results; and Section 6 concludes with a summary and future prospects.

2. Preliminaries

2.1. Single Rigid-Body Dynamics Model

For a single rigid robot, we can use its simplified approximate linear dynamics equations as equality constraints. The dynamics equation of a single rigid robot in a world coordinate frame is as follows:

$$\begin{array}{cc}\hfill \ddot{\mathit{p}}& ={\displaystyle \frac{{\sum }_{i=1}^n{\mathit{f}}_i}{m}}-\mathit{g}\hfill \\ \hfill {\displaystyle \frac{\mathrm{d}}{\mathrm{d}t}}\left(\mathit{I}\mathit{\omega }\right)& =\sum _{i=1}^n{\mathit{r}}_i\times {\mathit{f}}_i\hfill \\ \hfill \dot{\mathit{R}}& ={\left[\mathit{\omega }\right]}_{\times }\mathit{R}\hfill \end{array}$$

(1)

where ${\mathit{f}}_i\in {\mathbb{R}}^3$ is ground reaction force, and n represents the number of contact points. The distance from the CoM to each contact point is ${\mathit{r}}_i\in {\mathbb{R}}^3$. $\mathit{\omega }\in {\mathbb{R}}^3$ is the robot’s angular velocity. $\mathit{p}\in {\mathbb{R}}^3$ is the robot’s position, m is the mass, $\mathit{g}\in {\mathbb{R}}^3$ is the gravitational acceleration, $\mathit{I}\in {\mathbb{R}}^3$ is the robot’s inertia tensor, and $\mathit{R}\in {\mathbb{R}}^{3\times 3}$ is the orthogonal coordinate transformation matrix from the body coordinate system to the world coordinate system. ${\left[\mathit{x}\right]}_{\times }\in {\mathbb{R}}^{3\times 3}$ is the skew-symmetric matrix.

According to the linearization process in the literature [21], by neglecting the effects of roll and pitch in the Euler angles and only considering the case where the robot’s yaw angle changes slowly, the angular momentum equation in Equation (1) can be approximated as

$${\displaystyle \frac{d}{dt}}\left(\mathit{I}\mathit{\omega }\right)=\mathit{I}\dot{\mathit{\omega }}+\mathit{\omega }\times \left(\mathit{I}\mathit{\omega }\right)\approx \mathit{I}\dot{\mathit{\omega }}$$

where $\mathit{I}$ can be approximated by $\widehat{\mathit{I}}={\mathit{R}}_b\mathit{I}{\mathit{R}}^{\mathrm{T}}{, }_b\mathit{I}$ is the inertia tensor of the robot in the body coordinate system, which is a constant physical property.

Thus, the approximated simplified SRBD is

$${\displaystyle \frac{d}{dt}}\left[\begin{array}{c}\widehat{\mathbf{\Theta }}\\ \widehat{\mathit{p}}\\ \widehat{\mathit{\omega }}\\ \widehat{\dot{\mathit{p}}}\end{array}\right]=\left[\begin{array}{cccc}{\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{R}}^{\top }\left(\psi \right)& {\mathbf{0}}_3\\ {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{1}}_3\\ {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3\\ {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3& {\mathbf{0}}_3\end{array}\right]\left[\begin{array}{c}\widehat{\mathbf{\Theta }}\\ \widehat{\mathit{p}}\\ \widehat{\mathit{\omega }}\\ \widehat{\dot{\mathit{p}}}\end{array}\right]+\left[\begin{array}{ccc}{\mathbf{0}}_3& \cdots & {\mathbf{0}}_3\\ {\mathbf{0}}_3& \cdots & {\mathbf{0}}_3\\ {\widehat{\mathbf{I}}}^{-1}{\left[{\mathbf{r}}_1\right]}_{\times }& \cdots & {\widehat{\mathbf{I}}}^{-1}{\left[{\mathbf{r}}_n\right]}_{\times }\\ {\displaystyle \frac{1_3}{m}}& \cdots & {\displaystyle \frac{1_3}{m}}\end{array}\right]\left[\begin{array}{c}{\mathbf{f}}_1\\ ⋮\\ {\mathbf{f}}_n\end{array}\right]+\left[\begin{array}{c}\mathbf{0}\\ \mathbf{0}\\ \mathbf{0}\\ \mathbf{g}\end{array}\right]$$

(2)

The compact form is

$$\dot{\mathit{x}}\left(t\right)={\mathbf{A}}_c\left(\psi \right)\mathit{x}\left(t\right)+{\mathbf{B}}_c({\mathbf{r}}_1,\cdots ,{\mathbf{r}}_n,\psi )\mathit{u}\left(t\right)$$

(3)

where $\Theta ={[\varphi ,\theta ,\psi ]}^{\mathrm{T}}$ represents the Euler angles in the Z-Y-X sequence, ${\mathbf{A}}_c\in {\mathbb{R}}^{13\times 13}$ and ${\mathbf{B}}_c\in {\mathbb{R}}^{13\times 3n}$ represent the state transition matrix and control matrix, respectively.

2.2. Convex MPC Problem Formulation

Convex MPC is a special case of the MPC method. This advanced control strategy is capable of solving for the control inputs over a finite time horizon based on the system’s predictive model, ensuring that the system follows the desired trajectory. The convex MPC problem for a SRBD [21], constrained by the aforementioned dynamics model, with the input being the desired trajectory of the CoM ${\mathit{x}}_{ref}$, and the output being the predicted trajectory $\mathit{x}\left(t\right),t\in [0,T]$ and the sequence of contact forces $\mathit{u}\left(t\right),t\in [0,T]$, can be formulated as

$$\begin{array}{cc}\hfill & \underset{\mathit{x}(·),\mathit{u}(·)}{min}{\int }_0^T\left|\right|\mathit{x}\left(t\right)-{\mathit{x}}_{\mathrm{ref}}{\left(t\right)\left|\right|}_Q+{\left|\right|\mathit{u}\left(t\right)\left|\right|}_{S}dt\hfill \\ \hfill \mathrm{s}.\mathrm{t}.& \dot{\mathit{x}}\left(t\right)={\mathbf{A}}_c\mathit{x}\left(t\right)+{\mathbf{B}}_c\mathit{u}\left(t\right)\hfill \\ \hfill & \mathit{C}\left(t\right)\mathit{u}\left(t\right)=0\hfill \\ \hfill & \underline{\mathit{d}}\left(t\right)\le \mathit{D}\left(t\right)\mathit{u}\left(t\right)\le \overline{\mathit{d}}\left(t\right)\hfill \end{array}$$

(4)

The cost function is a convex quadratic form, which aligns with the definition of convex MPC. The term $\left|\right|\mathit{a}\left|\right|s$ represents a weighted two-norm. The equality constraints consist of the linear dynamics model and the condition that the aerial (non-contact) foot does not experience any forces. The inequality constraints impose physical upper and lower bounds on the contact forces. $\mathit{C}\left(t\right)$ is a matrix which selects forces corresponding with feet not in contact with the ground.

2.3. Whole Body Planner

Based on NS and OS theory, this section introduce a multi-task priority WBP. The input for the planner includes the desired trajectory and real-time state of task i in the corresponding OS, along with the Jacobian matrix. By computing the NS projection matrix and the mapped Jacobian matrix, incremental kinematic control commands in the joint space are obtained based on the current joint configuration.

First, it is necessary to calculate the pose error, velocity error, and acceleration error between the desired and current values for each task in the OS.

$$\begin{array}{cc}\hfill e_i& =x_i^d-x_i-J_i{q}_{i-1}^d\hfill \\ \hfill {\dot{e}}_i& ={\dot{x}}_i^d-J_i{\dot{q}}_{i-1}^d\hfill \\ \hfill {\ddot{e}}_i& ={\ddot{x}}_i^d-({\dot{J}}_i\dot{q}+J_i{\ddot{q}}_{i-1}^d)\hfill \end{array}$$

(5)

where $x_i^d,{\dot{x}}_i^d,{\ddot{x}}_i^d$ and $J_i$ are the desired trajectory and Jacobian matrix of task i in OS, respectively. $q\in {\mathbb{R}}^{(n+6)\times (n+6)},\dot{q}\in {\mathbb{R}}^{(n+6)\times (n+6)},\ddot{q}\in {\mathbb{R}}^{(n+6)\times (n+6)}$ represents the position, velocity, and acceleration of the robot in the generalized coordinate system. $q_i^d,{\dot{q}}_i^d,{\ddot{q}}_i^d$ represent the joint space incremental trajectory output from WBP. $J_i{q}_{i-1}^d,J_i{\dot{q}}_{i-1}^d$ and ${\dot{J}}_i\dot{q}+J_i{\ddot{q}}_{i-1}^d$ are the influence of task $i-1$ with respect to task i.

Next, calculate the NS projection matrix and the Jacobian projected into the NS,

$$\begin{array}{cc}\hfill J_{i|pre}& =J_i{N}_{i-1}\hfill \\ \hfill N_{i-1}& =I-J_{i-1|pre}^{†}{J}_{i-1|pre}\hfill \end{array}$$

(6)

where $J_{i-1|pre}^{†}=W^{-1}{J}_{i-1|pre}^{\top }{\left(J_{i-1|pre}{W}^{-1}{J}_{i-1|pre}^{\top }+\lambda I\right)}^{†}$ is the weighted right pseudo-inverse of $J_{i-1|pre}$. $W\in {\mathbb{R}}^{n\times n}$ is a weighted diagonal matrix.

Finally, compute the joint control command increments from task space to joint space,

$$\begin{array}{cc}\hfill q_i^d& =q_{i-1}^d+J_{i|pre}^{†}{e}_i\hfill \\ \hfill {\dot{q}}_i^d& ={\dot{q}}_{i-1}^d+J_{i|pre}^{†}{\dot{e}}_i\hfill \\ \hfill {\ddot{q}}_i^d& ={\ddot{q}}_{i-1}^d+J_{i|pre}^{†}{\ddot{e}}_i\hfill \end{array}$$

(7)

The above calculation process provides the method for computing the joint space control commands for task i. In practice, it is necessary to iteratively calculate for tasks $i=1,2\cdots ,K$ to obtain the total joint control command increments.

Remark 1.

When designing multi-priority tasks, it is important to ensure that tasks do not conflict with each other. According to null space theory, higher-priority tasks will take precedence in utilizing the degrees of freedom. It is crucial to carefully allocate weights among different tasks. By adjusting the elements of the weight matrix $W\in {\mathbb{R}}^{n\times n}$, the influence of a particular task on the joint space degrees of freedom can be strengthened or weakened, thereby avoiding conflicts between tasks.

2.4. Joint Space PVT Controller

The joint space controller uses a hybrid position–velocity–torque (PVT) controller, taking the desired velocity ${\dot{q}}^d$, the desired position $q^d$, and the feedforward torque of the joint ${\tau }_{ff}$ as inputs. The specific form of the controller from reference [31] is

$$\tau ={\tau }_{\mathrm{ff}}+k_p\left(q^d-q\right)+k_d\left({\dot{q}}^d-\dot{q}\right)$$

(8)

3. Improved Hierarchical Optimization Framework

An improved hierarchical optimization framework is proposed for high-frequency real-time whole-body motion control of humanoid robots. The hierarchical optimization algorithm consists of two parts: (1) a convex MPC high-dimensional planner based on ME discretization, used for planning the centroid trajectory and desired ground contact forces within the optimization finite-time horizon; (2) a low-level WBC based on the output of MPC and WBP. The desired joint torques computed by WBC are combined with the desired joint trajectories obtained by WBP to feed to PVT controller that driving the motor moving.

3.1. High Level Planner Based on Convex MPC

This section introduces a convex MPC control strategy based on ME discretization. The method treats the entire robot as a SRBD subject to forces at the contact patches. For the high-level planning problem of multi-body robots, the influence of leg dynamics can be neglected while maintaining robust system performance. Taking the robot used in this study as an example, due to its excellent structural design, the mass of the arms and legs accounts for only 30% of the total mass. Through model simplification and ME discretization, the algorithm ensures real-time performance while maintaining the accuracy of the model approximation.

The ME method is employed for the discretization of the system, offering a robust approach to handle the inherent stiffness in dynamic models. This method ensures greater numerical stability by accurately capturing both the fast and slow dynamics of the system, even with larger time steps, compared to traditional explicit methods. In particular, humanoid robot dynamics, despite simplifications, exhibit stiffness due to rapid variations in ground reaction forces and frequent gait changes. Explicit discretization methods, such as FE, often require extremely small time steps to maintain stability in stiff systems, which can significantly reduce computational efficiency. By contrast, the ME method mitigates this challenge, enabling stable and efficient numerical solutions without the need for excessively small time steps.

The convex MPC (4) problem after discretization using the ME method is

$$\begin{array}{cc}\hfill & \underset{\{\mathit{x},\mathit{u}\}}{min}\sum _{k=0}^{N-1}\left(\left|\right|{\mathit{x}}_{k+1}-{\mathit{x}}_{\mathrm{ref},k+1}{\left|\right|}_Q+\left|\right|{\mathit{u}}_k{\left|\right|}_S\right)\hfill \\ \hfill \mathrm{s}.\mathrm{t}.& {\mathit{x}}_{k+1}=e^{{\mathbf{A}}_c\Delta t}{\mathit{x}}_k+{\int }_0^{\Delta t}{e}^{{\mathbf{A}}_c\tau }d\tau {\mathbf{B}}_c{\mathit{u}}_k,\hfill \\ \hfill & {\mathit{C}}_k{\mathit{u}}_k=0,\hfill \\ \hfill & {\underline{\mathit{d}}}_k\le {\mathit{D}}_k{\mathit{u}}_k\le {\overline{\mathit{d}}}_k,\hfill \end{array}$$

(9)

In a linear time-varying system, although the state matrix changes over time, it can still be regarded as locally linear at each discrete time step, and thus these matrices remain linear. The solution to the problem is repeated at every iteration, but only the control input for the first step is used for control. Therefore, the linear time-varying system can still be considered to meet the premise of convex MPC.

The above problem needs to be transformed into the standard QP form for solving. First, by concatenating the state equations at each time step, the system’s state can be expressed as a function of the initial state ${\mathit{x}}_0$ and the sequence of control inputs,

$${\mathit{x}}_k={\mathbf{\Phi }}^k{\mathit{x}}_0+\sum _{i=0}^{k-1}{\mathbf{\Phi }}^{k-i-1}\mathbf{\Gamma }{\mathit{u}}_i$$

where $\mathbf{\Phi }=e^{{\mathbf{A}}_c\Delta t}$ and $\mathbf{\Gamma }={\int }_0^{\Delta t}{e}^{{\mathbf{A}}_c\tau }d\tau {\mathbf{B}}_c$. By combining all of the states $\{{\mathit{x}}_1,{\mathit{x}}_2,\cdots ,{\mathit{x}}_N\}$, we obtain the following form:

$$\left[\begin{array}{c}{\mathit{x}}_1\\ {\mathit{x}}_2\\ ⋮\\ {\mathit{x}}_N\end{array}\right]=\underset{{\mathsf{\Phi }}_{qp}}{\underbrace{\left[\begin{array}{c}\mathbf{\Phi }\\ {\mathbf{\Phi }}^2\\ ⋮\\ {\mathbf{\Phi }}^N\end{array}\right]}}{\mathit{x}}_0+\underset{{\Gamma }_{qp}}{\underbrace{\left[\begin{array}{cccc}\mathbf{\Gamma }& 0& \cdots & 0\\ \mathbf{\Phi }\mathbf{\Gamma }& \mathbf{\Gamma }& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ {\mathbf{\Phi }}^{N-1}\mathbf{\Gamma }& {\mathbf{\Phi }}^{N-2}\mathbf{\Gamma }& \cdots & \mathbf{\Gamma }\end{array}\right]}}\left[\begin{array}{c}{\mathit{u}}_0\\ {\mathit{u}}_1\\ ⋮\\ {\mathit{u}}_{N-1}\end{array}\right]$$

(10)

where ${\Phi }_{qp}$ is the condensed form of the state transition matrix, and ${\Gamma }_{qp}$ is the condensed form of the control input matrix.

Express all the states and control inputs within the prediction horizon in vector form as follows:

$$J={\displaystyle \frac{1}{2}}{\left(\mathbf{X}-{\mathbf{X}}_{\mathrm{ref}}\right)}^{\top }Q\left(\mathbf{X}-{\mathbf{X}}_{\mathrm{ref}}\right)+{\displaystyle \frac{1}{2}}{\mathbf{U}}^{\top }S\mathbf{U}$$

(11)

where $\mathbf{X}={\left[{\mathit{x}}_1^{\top },{\mathit{x}}_2^{\top },\cdots ,{\mathit{x}}_N^{\top }\right]}^{\top }$, ${\mathbf{X}}_{\mathrm{ref}}={\left[{\mathit{x}}_{\mathrm{ref},1}^{\top },{\mathit{x}}_{\mathrm{ref},2}^{\top },\cdots ,{\mathit{x}}_{\mathrm{ref},N}^{\top }\right]}^{\top }$, and $\mathbf{U}={\left[{\mathit{u}}_0^{\top },{\mathit{u}}_1^{\top },\cdots ,{\mathit{u}}_{N-1}^{\top }\right]}^{\top }$.

According to the method for constructing QP outlined in the reference [21], the following standard QP form can be obtained:

$$\begin{array}{cc}\hfill & \underset{\mathbf{U}}{min}{\displaystyle \frac{1}{2}}{\mathbf{U}}^{\top }\mathbf{P}\mathbf{U}+{\mathbf{U}}^{\top }\mathbf{q}\hfill \\ \hfill \mathrm{s}.\mathrm{t}.& \underline{\mathit{d}}\le \mathit{D}\mathit{U}\le \overline{\mathit{d}}\hfill \end{array}$$

(12)

where $\mathbf{P}=2{\Gamma }_{qp}^{\top }Q{\Gamma }_{qp}+S$, and $\mathbf{q}=2{\Gamma }_{qp}^{\top }Q\mathbf{e}=2{\Gamma }_{qp}^{\top }Q\left({\Phi }_{qp}{\mathit{x}}_0-{\mathbf{X}}_{\mathrm{ref}}\right)$.

Remark 2.

Compared to the FE method for discretization, the ME method yields a more accurate numerical solution and effectively addresses numerical stiffness issues. To mitigate the increased computational complexity and the additional computational burden from the integral term in the ME method, the following improvements can be made in numerical calculations: By leveraging the sparse characteristics of the state transition matrix ${\mathbf{A}}_c$, a block matrix approach can be used to reduce the dimensionality of matrix computations and improve the efficiency of calculating the ME. Additionally, based on the form of the ME, the integral for Γ can be converted into a matrix calculation ${\int }_0^{\Delta t}{e}^{A_c\tau }d\tau =A_c^{-1}\left(e^{A_c\Delta t}-I\right)$. With these optimizations, the ME method can maintain excellent accuracy while reducing the computational load.

3.2. Low Level Controller Based on WBPC

Based on the WBP theory, reference trajectories ${\mathit{q}}_K^d,{\dot{\mathit{q}}}_K^d,{\ddot{\mathit{q}}}_K^d$ in joint space can be obtained by iteratively solving K OS tasks. The estimated contact forces ${\mathit{F}}_{r,mpc}$ from the MPC and the joint acceleration ${\ddot{\mathit{q}}}_K^d$ from the WBP are used as input commands for the WBC, where an incremental form of the WBC QP problem is constructed.

Using the whole-body dynamics model as equality constraints and CWC as inequality constraints, the joint control torques optimized by WBC satisfy the actual physical constraint. The QP based WBC problem is formulated as follows:

$$\begin{array}{cc}\hfill \underset{\mathbf{\Delta }{\mathit{F}}_{\mathit{r}}(·),\mathbf{\Delta }{\ddot{\mathit{q}}}_{\mathit{K}}}{min}& \left|\right|\mathbf{\Delta }{\mathit{F}}_r{\left(t\right)\left|\right|}_{W_r}+\left|\right|\mathbf{\Delta }{\ddot{\mathit{q}}}_K\left(t\right){\left|\right|}_{W_k}\hfill \end{array}$$

(13)

$$\begin{array}{cc}\hfill & {\mathit{S}}_{c1}\mathit{M}({\ddot{\mathit{q}}}_K\left(t\right)+\mathbf{\Delta }{\ddot{\mathit{q}}}_K\left(t\right))+{\mathit{S}}_{c1}\mathit{b}+{\mathit{S}}_{c1}\mathit{g}={\mathit{S}}_{c1}{\mathit{J}}_c^{\mathrm{T}}({\mathit{F}}_{r,mpc}+\mathbf{\Delta }{\mathit{F}}_{\mathit{r}})\hfill \end{array}$$

(14)

$$\begin{array}{cc}\hfill & {\mathit{S}}_{c2}\mathit{M}({\ddot{\mathit{q}}}_K\left(t\right)+\mathbf{\Delta }{\ddot{\mathit{q}}}_K\left(t\right))+{\mathit{S}}_{c2}\mathit{b}+{\mathit{S}}_{c2}\mathit{g}=\mathit{\tau }+{\mathit{S}}_{c2}{\mathit{J}}_c^{\mathrm{T}}({\mathit{F}}_{r,mpc}+\mathbf{\Delta }{\mathit{F}}_{\mathit{r}})\hfill \end{array}$$

(15)

$$\begin{array}{cc}\hfill & \mathrm{lb}\left(\mathit{\tau }\right)\le \mathit{\tau }\le \mathrm{ub}\left(\mathit{\tau }\right)\hfill \end{array}$$

(16)

$$\begin{array}{cc}\hfill & \mu F_{c,z}\ge \sqrt{F_{c,fx}^2+F_{c,fy}^2}\hfill \end{array}$$

(17)

$$\begin{array}{cc}\hfill & F_{c,z}\ge {\displaystyle \frac{\sqrt{F_{c,{\tau }_x}^2+F_{c,{\tau }_y}^2}}{X^2+Y^2}}\hfill \end{array}$$

(18)

$$\begin{array}{cc}\hfill & \mathrm{ub}\left(F_{c,{\tau }_z}\right)\le F_{c,{\tau }_z}\le \mathrm{ub}\left(F_{c,{\tau }_z}\right)\hfill \end{array}$$

(19)

where $\mathbf{\Delta }{\mathit{F}}_{\mathit{r}}(·),\mathbf{\Delta }{\ddot{\mathit{q}}}_{\mathit{K}}$ represent the optimized increments of contact force and generalized acceleration, respectively. $W_r$ and $W_k$ represent the weights of the two optimization variables, respectively. $\mathit{M},\mathit{b}$, and $\mathit{g}$ represent the generalized mass/inertia matrix, the sum of Coriolis/centrifugal forces, and gravity forces, respectively. Equation (14) indicates that the stability of the CoM can be maintained even after adjusting the CoM acceleration and contact forces. ${\mathit{S}}_{c1}\in {\mathbb{R}}^{6\times (n+6)}$ is the centroidal coordinate selection matrix. Equations (15) and (16) indicate that after fine-tuning the joint acceleration and contact forces; the resulting torques must satisfy the physical upper and lower limit constraints. ${\mathit{S}}_{c2}\in {\mathbb{R}}^{n\times (n+6)}$ is the joint space selection matrix. ${\mathit{J}}_c$ is the contact Jacobian matrix. Equations (17)–(19) jointly form the CWC. X and Y represent the half-length and half-width of the foot contact surface, respectively.

After obtaining $\mathbf{\Delta }{\mathit{F}}_{\mathit{r}}(·),\mathbf{\Delta }{\ddot{\mathit{q}}}_{\mathit{K}}$, the torque control command for the joint actuators, ${\tau }_{ff}\in {\mathbb{R}}^{n\times 1}$, can be solved using the dynamic model

$$\mathit{M}({\ddot{\mathit{q}}}_K\left(t\right)+\mathbf{\Delta }{\ddot{\mathit{q}}}_K\left(t\right))+\mathit{b}+\mathit{g}=\left({\displaystyle \genfrac{}{}{0pt}{}{0^{6\times 1}}{{\tau }_{\mathit{ff}}}}\right)+{\mathit{J}}_c^{\mathrm{T}}({\mathit{F}}_{r,mpc}+\mathbf{\Delta }{\mathit{F}}_{\mathit{r}})$$

(20)

The joint space controller uses the PVT controller, taking the desired velocity ${\dot{q}}_K^d$ and desired position $q_K^d$ from WBP, as well as the joint feedforward torque ${\tau }_{ff}$ from WBC as inputs. The specific form of the controller is

$$\mathit{\tau }={\mathit{\tau }}_{\mathrm{ff}}+k_p\left({\mathit{q}}_K^d-\mathit{q}\right)+k_d\left({\dot{\mathit{q}}}_K^d-\dot{\mathit{q}}\right)$$

(21)

The pseudocode for the low-level control algorithm based on WBPC and PVT is shown in Algorithm 1.

Algorithm 1 Low-Level Controller Based on WBPC and PVT

Require:  $x_i^d,{\dot{x}}_i^d,{\ddot{x}}_i^d,J_i,x_i,\dot{q},\ddot{q},{\ddot{x}}_c,F_{r,mpc},M,b,g,J_c$ ▹ The inputs required for the algorithm Ensure:  $\tau$                   ▹ The output of the algorithm    1: Initialization: $N_0=I$, $q_0^d=0$, ${\dot{q}}_0^d=0$, ${\ddot{q}}_0^d=0$    2: for  $i=1,2,\dots ,K$ do    3:     $(e_i,{\dot{e}}_i,{\ddot{e}}_i)\leftarrow \mathrm{computeErr}(x_i^d,{\dot{x}}_i^d,{\ddot{x}}_i^d,J_i,x_i,\dot{q},\ddot{q})$   ▹ Compute the error for task i    4:     Calculate the null space projection matrix $N_{i-1}$    5:     Compute the mapped Jacobian matrix $J_{i|pre}$    6:     $(q_i^d,{\dot{q}}_i^d,{\ddot{q}}_i^d)\leftarrow \mathrm{planJointTraj}(J_{i|pre},e_i,{\dot{e}}_i,{\ddot{e}}_i)$▹ Get the trajectory increments for task i    7: end for    8: Save the final trajectory: $q_K^d,{\dot{q}}_K^d,{\ddot{q}}_K^d$    9: Compute the dynamic equality constraints and CWC constraints  10: Formulate the QP problem for the objective function  11: Solve the QP problem to obtain increments $\Delta F_r,\Delta {\ddot{q}}_K$  12: Calculate the feedforward torque ${\tau }_{\mathrm{ff}}$ from the whole-body dynamic model  13: Calculate the PVT command: $\tau ={\tau }_{\mathrm{ff}}+k_p\left(q_K^d-q\right)+k_d\left({\dot{q}}_K^d-\dot{q}\right)$  14: Return  $\tau$

4. Simulation Experiment

In this section, we validate the effectiveness of the proposed hierarchical optimization control algorithm on the X02 platform, which lacks the ankle roll DOF. First, we design a WBP multi-priority task adapted to the absence of the ankle roll DOF. Next, we evaluate the numerical stability and prediction accuracy of the convex MPC based on the ME method under different step sizes. Finally, we validate the effectiveness of the incremental WBC based on CWC constraints and whole-body dynamics constraints, along with an interpretation of joint trajectory tracking.

4.1. Experimental Setup

The X02 is a unique, lightweight humanoid robot designed by us, characterized by low energy consumption and long endurance. The leg degrees of freedom and sequence of X02 are: {hip_yaw, hip_roll, hip_pitch, knee_pitch, ankle_pitch}. In this experiment, the X02 robot model, shown in Figure 1, is loaded onto the MuJoCo simulation platform. The robot model parameters and MuJoCo physics engine settings are shown in

Table 1. Parameters of simulation environment and robot.

Parameters of Robot	Values	Parameters of MuJoCo	Values
m	24.19 kg	damping	0.1
$g_z$	−9.8 m/s2	frictionloss	0.02
$\mu$	0.6	armature	0.01

. The control algorithm architecture is modified based on the OpenLoong [32] framework. To facilitate the deployment of the control algorithm on the physical robot, we have stripped the simulation portion from the original framework. The upper-level control and lower-level drives communicate using Google gRPC. The control program is deployed on a Lenovo Y9000R laptop with an Ubuntu 22.04 operating system, Intel i9-14900HX × 32 CPU, and 32 GB of memory. The upper-level optimization algorithm runs at 1k Hz, while the lower-level drive operates at 2k Hz.

4.2. Implementation Details

The multi-priority tasks based on WBP are prioritized as follows: maintaining_contact, holding_CoM_pose, tracking_swing_leg, in order from high to low priority. In the tracking_swing_leg task, due to the absence of the ankle roll DOF, we need to set the roll row in both the Jacobian ${\mathit{J}}_c$ and its derivative ${\mathit{dJ}}_c$ to 0, as well as set the roll component of the error vector $e_{roll}$ to 0. This eliminates the contribution of the roll direction to the control of each joint when using WBP for joint-space trajectory planning.

4.3. Results of Convex MPC

In this experiment, The whole parameters are designed as follows in

Table 2. Parameters of convex MPC.

Parameters of MPC	Values
N (Finite prediction horizon)	10
$dt$	{5, 10, 50, 100} ms
State weight	$\left[\begin{array}{ccc}1.0& 1.0& 1.0\\ 1.0& 1.0& {10}^{-9}\\ {10}^{-7}& {10}^{-7}& {10}^{-7}\\ 100.0& 10.0& 10.0\end{array}\right]$
Force weight	$\left[\begin{array}{ccc}10.0& 1.0& 1.0\\ 1.0& 1.0& 1.0\\ 10.0& 1.0& 1.0\\ 1.0& 1.0& 1.0\end{array}\right]$
$f_{z,max}$	500 N
$f_{z,min}$	10 N

.

In Figure 2, the first 5 s represent the simulation’s stable standing phase, and the MPC is introduced at the fifth second. The sampling time step increases sequentially from top to bottom in the figure. From the data, it can be observed that both methods exhibit large errors in the y-direction before 10 s. This is due to the first step of the robot’s gait planning based on a finite state machine, which drives the swing leg to move laterally. As the sampling time step increases, the FE method shows significant prediction errors caused by numerical instability, while the ME method does not. From the MSE metrics in the table and the trend in the figure, it can be seen that both methods achieve the smallest prediction error at $dt=10$ ms, rather than at $dt=5$ ms. This is because too small a time step increases the computational load and the setup of the QP solver.

Table 3. Performance of convex MPC using FE and ME discretization method.

Sampling Step	MSE		Average Solving Time (μs)
	FE	ME	FE	ME
5	0.134	0.045	688.5	844.8
10	0.115	0.029	716.8	982.1
50	0.128	0.037	767.8	1004.5
100	0.178	0.088	777.9	968.8

also shows that the average solution time for ME is longer than for FE. Overall, under all four sampling time steps, the ME discretization method, compared to the FE discretization method, achieves smaller prediction errors and greater numerical stability, albeit at the cost of longer solving times.

4.4. Results of WBPC

The relevant parameters of the WBC optimization problem are presented in

Table 4. Parameters of WBPC.

Parameters of WBC	Values
$f_{z,max}$	500 N
$f_{z,min}$	10 N
X	1.5 cm
Y	12.5 cm
${\tau }_{max}$	{50, 110, 72, 150, 45} Nm
$F_{z,{\tau }_z}$ = 80 Nm

. The joint torque limiting sequence is as follows: hip yaw, hip roll, hip pitch, knee pitch, and ankle pitch.

From Figure 3, it can be observed that the WBC further optimizes the contact forces predicted by the MPC based on the CWC constraints and dynamic constraints in joint space. The figure illustrates the optimized contact forces for several complete gait cycles of both the left and right feet. The foot–ground contact coordinate system aligns with the global coordinate system, following the right-hand convention. For the force in the y-direction, the output by the WBC for the left foot is negative, while it is positive for the right foot, aligning better with the physical scenario. For the z-direction force, the WBC outputs smoother results that satisfy the CWC constraints.

Figure 4 and Figure 5 show the joint torques performance and joint trajectories response, respectively. In the figure, the abbreviations in the legend are defined as follows: LHY represents “left hip yaw”, LHP represents “left hip pitch”, LHR represents “left hip roll”, LKP represents “left knee pitch”, and LAP represents “left ankle pitch”. Similarly, R indicates the right, and the other abbreviations follow the same naming convention. From Figure 4, it can be seen that the optimized torques output by the WBC satisfy the physical constraints of the joint torque limits. The hip yaw joint contributes minimally during forward walking, requiring only a small torque for stabilization. Notably, the hip yaw motors of X02 rotate in opposite directions; to maintain stability in the yaw direction, the torque control commands exhibit a synchronized trend. During the swing phase, the knee pitch experiences nearly zero torque, while during the support phase, it generates significant torque. The pitch joints of the hip, knee, and ankle exhibit periodic contributions during the transition between the stance and swing legs. From the right subplot of Figure 5, it can be observed that since the motor angles are zero when X02 is standing upright, the knee pitch angle during walking approximately equals the sum of the hip pitch and ankle pitch angles to maintain the stability of the center of mass.

5. Discussion

In the experimental section of this study, we compare the proposed ME-based MPC algorithm with the FE-based MPC algorithm used in previous research. The experimental results show that under large discrete step sizes, the ME approach can still ensure numerical stability. The MSE metric for ME is generally an order of magnitude lower than that of FE, reaching the level of ${10}^{-2}$. However, due to the integral nature of the ME approach, its computational load increases, leading to longer solution times compared to the FE approach. To reduce the computational cost of the ME method, we leverage the derivative property of the exponential function to convert the matrix exponential integration into ${\int }_0^{\Delta t}{e}^{A_c\tau }d\tau =A_c^{-1}\left(e^{A_c\Delta t}-I\right)$, thus shortening the solution time. From the average solution-time comparison data, the ME approach is only about 0.2 ms slower than the FE approach, which is fully acceptable for large discrete step sizes. Therefore, when solving MPC problems with large step sizes, the ME approach provides both numerical accuracy and computational efficiency.

In the WBC algorithm, ground reaction force is key to driving joint motion and maintaining the stability of the floating base. In the traditional WBC algorithm [9], the lack of prior estimation for foot contact forces means that the initial value is typically set to zero, often requiring multiple iterations to find the optimal solution—particularly costly when gait switches occur frequently during walking. In this study, we use the ground reaction forces computed by the MPC as the initial guess for the WBC optimization in an incremental optimization manner, allowing a rapid and accurate determination of the foot contact forces needed to drive joint motion. From the experimental data, although the ground reaction forces output by the MPC are not fully accurate or continuous, they converge quickly and become smooth and continuous after WBC optimization in each gait cycle for both feet.

6. Conclusions and Future Work

This study proposes an improved optimization control algorithm based on MPC and WBPC to address the numerical stability, computational efficiency, and real-time requirements of humanoid robot walking tasks. By comparing two discretization-based MPC algorithms under different step sizes with respect to convergence time and error accuracy, the results indicate that the ME method is more suitable for solving the MPC problem of a simplified humanoid robot model using large discrete step sizes, ensuring numerical stability and reducing computational demands. Moreover, to meet the high-frequency real-time requirements and physical constraints of humanoid walking, the ground reaction forces estimated by the MPC are utilized as the initial guess for the WBC optimization problem, combined with CWC constraints in an incremental optimization scheme for joint trajectories and feedforward torques. This reduces the number of optimization iterations while enhancing the accuracy and real-time performance of the numerical solutions. Simulation-based walking experiments verify the feasibility of the proposed algorithm. By integrating the global optimal solution from MPC with the high-frequency real-time characteristics of WBC, the proposed hierarchical optimization framework improves overall system robustness. Leveraging a discretization approach with stronger numerical stability and a hierarchical optimization strategy, this framework effectively balances computational load and solution accuracy, making it well-suited for deployment on edge computing platforms and providing theoretical support for physical implementation. In the future, we plan to further investigate gait planning algorithms with strong disturbance rejection to enhance the stability of this hierarchical optimization framework and achieve faster walking speeds. Additionally, capitalizing on the existing X02 communication framework allows rapid deployment of the algorithm from simulation to physical hardware, and we will validate its performance on a real robot in a forthcoming work.