Balanced Standing on One Foot of Biped Robot Based on Three-Particle Model Predictive Control

Abstract

Balancing is a fundamental task in the motion control of bipedal robots. Compared to two-foot balancing, one-foot balancing introduces new challenges, such as a smaller supporting polygon and control difficulty coming from the kinematic coupling between the center of mass (CoM) and the swinging leg. Although nonlinear model predictive control (NMPC) may solve this problem, it is not feasible to implement it on the actual robot because of its large amount of calculation. This paper proposes the three-particle model predictive control (TP-MPC) approach. It combines with the hierarchical whole-body control (WBC) to solve the one-leg balancing problem in real time. The bipedal robot’s torso and two legs are modeled as three separate particles without inertia. The TP-MPC generates feasible swing leg trajectories, followed by the WBC to adjust the robot’s center of mass. Since the three-particle model is linear, the TP-MPC requires less computational cost, which implies real-time execution on an actual robot. The proposed method is verified in simulation. Simulation results show that our method can resist much larger external disturbance than the WBC-only control scheme.

1. Introduction

1.1. Background

Motion control of a bipedal robot is a typical multi-task control problem. The robot has to keep its balance while moving or standing, interact with the environment, and complete tasks assigned by the user. Among the various tasks, balancing under external disturbance is a fundamental one for the robot to complete other tasks. Therefore, the balance control of a bipedal robot while walking or standing on both feet has been widely studied in recent years. However, the situation where robots stand on one leg and keep their balance has received little attention. Examples of such a situation include a humanoid trying to reach a distant object with its arms or a bipedal robot kicking a football. Compared with balancing on both feet, one-foot balancing presents new challenges, such as significantly reduced support polygons and reduced overall system stability. Furthermore, if the swing leg is heavy enough, its motion may significantly affect the motion of the entire robot [1]. In this sense, the problem of robot balance control with one foot is similar to that of robot balance control with two arms [2]. From a physical point of view, maintaining the balance of a biped robot with its arms reduces the difficulty of the control problem because the robot’s supporting polygon remains unchanged, while extra degrees of freedom can be used to accomplish the balance task [3]. Therefore, the single-leg balancing stance method can be applied to dual-arm robots and vice versa.

1.2. Motivation

A bipedal robot’s balancing on one foot is a typical multi-task control problem. The robot must keep the torso and the stance foot stable, move the swing foot properly, and conform to external and internal constraints. The whole-body control (WBC) method has been widely adopted in the balancing control of bipedal robots due to its capabilities to generate dynamically feasible control outputs and to handle various tasks and constraints [4]. However, the WBC only considers the current states of the robot and cannot handle strongly infeasible reference trajectories [5]. The model predictive control (MPC) is commonly used in conjunction with WBC to complement these insufficiencies [6]. In case of large external disturbance, the MPC foresees the robot’s future states according to a specific dynamical model and acts as an online re-planner for the WBC. However, the full dynamics of a one-foot standing robot are high degrees of freedom and strongly nonlinear. Therefore, an MPC employing full dynamics has to solve a complicated nonlinear optimization problem. Thus, the full-dynamics MPC is computationally expensive and cannot be executed in real time on an actual robot [7,8,9,10,11].

Current MPC algorithms for legged robots usually adopt simplified models [12], for example, the linear inverted pendulum (LIP) model [2,13,14], the single particle model [15] or the single rigid body (SRB) model [6], to balance between model precision and computation time. In the case of balancing on one foot, these models are unsuitable because they adopt the dynamics model of a particle or a rigid body. Thus, the motion of the swinging leg cannot be predicted in MPC. In this research, we propose to adopt a three-particle model in the model predictive control, which is called the three-particle model predictive control (TP-MPC) approach. It considers the motion of the swing leg and requires far less computation time than a full-dynamics MPC. As a result, the TP-MPC combined with the WBC will solve the problem of one-foot balance control for practical biped robots with real-time computing requirements.

1.3. Related Work

Simplified models have been widely used in the balance control of bipedal robots. For example, Vukobratovic et al. first proposed the zero moment point (ZMP) concept and applied it to balance a bipedal robot [16]. Next, Kajita et al. proposed the LIP model, which simplifies the biped robot into a single mass point moving horizontally and connected to the ground by a massless retractable stick [17]. Finally, Pratt et al. extended the LIP model to a Linear Flywheel Inverted Pendulum Model, which takes the robot’s angular momentum into account [18]. Their method can resist a large external disturbance by computing the robot’s Capture Point and Capture Region. Li et al. used the momentum to control the balance of a biped robot, and they found that linear momentum was more critical than angular momentum in the balance control task. When the two momentum tasks cannot be satisfied simultaneously due to a significant disturbance, the linear momentum will be given priority [19].

WBC has also been applied to the bipedal robot’s balancing. Xie et al. used the hierarchical WBC to maintain a balance of a standing biped robot [20]. Their algorithm can execute on an onboard computer in real time by optimizing the computing process. However, their control scheme needs a planner to generate feasible trajectories followed by the WBC to resist large external disturbances. Kim et al. proposed a new whole-body control approach called whole-body motion control [21]. It can realize dynamic walking on a bipedal robot with a passive ankle and maintain balance under external disturbance.

Model predictive control repeatedly solves a finite-horizon optimal control problem starting from the system’s current states [22]. Moreover, the MPC can prepare for future motions in advance. Therefore, it is very suitable for highly dynamic motion control of legged robots. Li et al. applied a single rigid body model predictive control to a biped robot with light legs [23]. The MPC generates optimal contact forces and torques at each foot, which are converted to joint torque commands through contact Jacobians. Their method enables the robot to walk at a speed of 1.6 m/s on complex terrains and achieve a wide range of dynamic motions. Luo et al. proposed a three-mass model predictive control method for gait control of biped robots [24]. Their planar three-mass model considers the robot’s ZMP and angular momentum. The swing leg trajectories are generated by heuristics rather than the MPC. Therefore, their method did not utilize the swing leg to balance the robot.

From a biomimicry point of view, swinging the legs is crucial for the balanced control of the torso [1]. Boston Dynamics released a video in 2013 of using swinging legs and arms to adjust posture but did not release relevant technical details [25]. To the best of our knowledge, other than the two works mentioned above, no literature has been published on the use of swinging legs to control the balance of biped robots.To the best of our knowledge, literature has yet to be published on the balance control of biped robots using the swinging leg.

1.4. Contribution

The main contributions of this paper are as follows:

(1)

The proposed TP-MPC method can generate feasible swing leg trajectories that balance the robot while standing on one foot. The WBC tracks the generated swing leg trajectories. As a result, the overall control scheme can resist large external disturbances.

(2)

The TP-MPC catches the main effects of the swing leg motion while being simple enough to operate at the same frequency as the WBC.

The overall structure of this paper is as follows: In Section 2, the three-particle model and the TP-MPC are derived. Section 3 introduces the hierarchical whole-body control approach and the overall control scheme. Section 4 discusses the simulation setups and results. Finally, Section 5 is for the conclusions and future work.

2. Three-Particle Model Predictive Control

In this section, we derive the three-particle model predictive control method. First, we introduce the simplified three-particle model for a bipedal robot standing on a single foot. Our MPC method requires to solve repeatedly a discrete, finite-horizon optimal control problem as follows:

$$\underset{\mathit{X},\mathit{U}}{min}=\sum _{k=0}^{n-1}{∥\begin{array}{c}{\mathit{x}}_{k+1}-{\mathit{x}}_{k+1,ref}\end{array}∥}_{{\mathit{Q}}_k}+{∥\begin{array}{c}{\mathit{u}}_k\end{array}∥}_{{\mathit{R}}_k}$$

(1)

$$\begin{array}{c}\hfill \mathrm{s}.\mathrm{t}.{\mathit{x}}_{k+1}={\mathit{A}}_k{\mathit{x}}_k+{\mathit{B}}_k{\mathit{u}}_k,k=0,1,\cdots ,n-1\end{array}$$

(2)

$$\begin{array}{c}\hfill {\mathit{lb}}_k\le {\mathit{C}}_k{\mathit{u}}_k\le {\mathit{ub}}_k,k=0,1,\cdots ,n-1\end{array}$$

(3)

where n is the MPC’s prediction length; ${\mathit{x}}_k$ and ${\mathit{x}}_{k,ref}$ are the actual state and reference state at step k, ${\mathit{u}}_k$ is the input of step k, and ${\mathit{Q}}_k$ and ${\mathit{R}}_k$ are the weight matrices, ${∥\begin{array}{c}\mathit{\lambda }\end{array}∥}_{\mathsf{\Gamma }}={\mathit{\lambda }}^{\top }\mathsf{\Gamma }\mathit{\lambda }$ means the weighted value of $\mathit{\lambda }$ of weight $\mathsf{\Gamma }$. Equations (2) and (3) are equality and inequality constraints, respectively.

2.1. Three-Particle Simplified Model

Figure 1 shows the three-particle simplified model. The stance leg of the biped robot is fixed, and the position of the robot’s center of mass can be adjusted by moving the torso and the swing leg to fall near the desired position. Here, we simplified the legs and the torso into three mass points without rotational inertia. In most cases, the torso’s center of mass is located directly above the midpoint of the two hip joints but has an offset in the sagittal direction.

As shown in Figure 1, ${\mathit{p}}_{st}={\left[\begin{array}{ccc}{p}_{st,x}& p_{st,y}& p_{st,z}\end{array}\right]}^{\top }$, ${\mathit{p}}_{sw}={\left[\begin{array}{ccc}{p}_{sw,x}& p_{sw,y}& p_{sw,z}\end{array}\right]}^{\top }$ are the positions of the end of the stance leg and the swing leg in the world coordinate system, ${\mathit{p}}_b={\left[\begin{array}{ccc}{p}_{b,x}& p_{b,y}& p_{b,z}\end{array}\right]}^{\top }$ is the position of the torso in the world coordinate system, ${\dot{\mathit{p}}}_{st}={\left[\begin{array}{ccc}{\dot{p}}_{st,x}& {\dot{p}}_{st,y}& {\dot{p}}_{st,z}\end{array}\right]}^{\top }$, ${\dot{\mathit{p}}}_{sw}={\left[\begin{array}{ccc}{\dot{p}}_{sw,x}& {\dot{p}}_{sw,y}& {\dot{p}}_{sw,z}\end{array}\right]}^{\top }$ are the velocities of the end of the stance leg and the swing leg in the world coordinate system, ${\dot{\mathit{p}}}_b={\left[\begin{array}{ccc}{\dot{p}}_{b,x}& {\dot{p}}_{b,y}& {\dot{p}}_{b,z}\end{array}\right]}^{\top }$ is the velocity of the torso in the world coordinate system. $m_{st}$, $m_{sw}$, and $m_b$ denote the mass of the stance leg, swing leg, and torso, respectively. In general, bipedal robots are symmetrical in the y direction, so the hip joint’s distance from the hip’s center in the y direction is $l_y$. Let the distance between the torso center of mass and the hip plane be $l_z$, and $l_x$ is the distance between the torso center of mass in the x direction and the hip motor of both legs.

Let ${\mathit{p}}_{m,st}$,${\mathit{p}}_{m,sw}$ be the center of mass position of the stance leg and the swing leg, respectively. To simplify the model, we assume that each leg’s center of mass lies on the midpoint of the hip and foot, which means

$${\mathit{p}}_{m,st}=\frac{1}{2}\left[\begin{array}{c}{p}_{b,x}+l_x+p_{st,x}\\ p_{b,y}+l_y+p_{st,y}\\ p_{b,z}-l_z+p_{st,z}\end{array}\right]$$

(4)

$${\mathit{p}}_{m,sw}=\frac{1}{2}\left[\begin{array}{c}{p}_{b,x}+l_x+p_{sw,x}\\ p_{b,y}-l_y+p_{sw,y}\\ p_{b,z}-l_z+p_{sw,z}\end{array}\right]$$

(5)

The robot system’s center of mass can be derived as follows:

$$m{\mathit{p}}_{CoM}=m_b{\mathit{p}}_b+m_{st}{\mathit{p}}_{m,st}+m_{sw}{\mathit{p}}_{m,sw}$$

(6)

where $m=m_b+m_{st}+m_{sw}$ represents the robot’s total mass. Therefore, the robot’s center of mass at time k is:

$${\mathit{p}}_{CoM,k}=\frac{m_b{\mathit{p}}_{b,k}+m_{st}{\mathit{p}}_{m,st,k}+m_{sw}{\mathit{p}}_{m,sw,k}}{m}$$

(7)

Let $\mathit{x}={\left[\begin{array}{cccc}{\mathit{p}}_b^{\top }& {\mathit{p}}_{sw}^{\top }& {\dot{\mathit{p}}}_b^{\top }& {\dot{\mathit{p}}}_{sw}^{\top }\end{array}\right]}^{\top }$ be the state variables of the system, $\mathit{u}={\left[\begin{array}{cc}{\ddot{\mathit{p}}}_b^{\top }& {\ddot{\mathit{p}}}_{sw}^{\top }\end{array}\right]}^{\top }$ be the control input. For the model predictive control design, using Taylor expansion, we obtain the state equation of the discrete-time linear system as:

$$\begin{array}{c}\hfill {\mathit{x}}_{k+1}=\underset{\mathit{A}}{\underbrace{\left[\begin{array}{cccc}{\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& \mathsf{\Delta }t{\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& \mathsf{\Delta }t{\mathit{I}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathit{I}}_{3\times 3}\end{array}\right]}}{\mathit{x}}_k+\underset{\mathit{B}}{\underbrace{\left[\begin{array}{c}\frac{1}{2}{\mathsf{\Delta }t}^2{\mathit{I}}_{6\times 6}\\ \mathsf{\Delta }t{\mathit{I}}_{6\times 6}\end{array}\right]\phantom{\left[\begin{array}{cccc}{\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& \mathsf{\Delta }t{\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& \mathsf{\Delta }t{\mathit{I}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathit{I}}_{3\times 3}\end{array}\right]}}}{\mathit{u}}_k\end{array}$$

(8)

Substituting Equations (4) and (5) into Equation (7) yields the location of the biped robot’s center of mass:

$$\begin{array}{c}\hfill {\mathit{p}}_{CoM,k}=\underset{\mathit{C}}{\underbrace{\frac{1}{m}\left[\begin{array}{ccc}{m}_1{\mathit{I}}_{3\times 3}& m_2{\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times 6}\end{array}\right]\phantom{\left[\begin{array}{c}{m}_{st}{x}_{st}+(m_{sw}+m_{st})l_x\\ m_{st}{y}_{st}+(m_{sw}-m_{st})l_y\\ m_{st}{z}_{st}-(m_{sw}+m_{st})l_z\end{array}\right]}}}{\mathit{x}}_k+\underset{\mathit{D}}{\underbrace{\frac{1}{2m}\left[\begin{array}{c}{m}_{st}{x}_{st}+(m_{sw}+m_{st})l_x\\ m_{st}{y}_{st}+(m_{sw}-m_{st})l_y\\ m_{st}{z}_{st}-(m_{sw}+m_{st})l_z\end{array}\right]}}\end{array}$$

(9)

where $m_1=\frac{1}{2}(m_{sw}+m_{st})+m_b$ and $m_2=\frac{1}{2}{m}_{sw}$.

2.2. Tasks

For the one-foot balance control of a biped robot, the primary task is to minimize the position and velocity tracking errors of the robot’s center of mass. The position tracking error of the swing leg should also be minimized. Moreover, the system input should be regularized to avoid excessive joint torques. Therefore, we formulate the cost function of the MPC as follows:

$$\begin{array}{cc}\hfill J=\underset{\mathit{U}}{min}\sum _{k=1}^n& {∥\begin{array}{c}{\mathit{p}}_{CoM,k}-{\mathit{p}}_{CoM,ref,k}\end{array}∥}_{\mathit{Q}}+{∥\begin{array}{c}{\dot{\mathit{p}}}_{CoM,k}\end{array}∥}_{\mathit{T}}+{∥\begin{array}{c}{\overline{\mathit{p}}}_{sw,k}-{\overline{\mathit{p}}}_{sw,ref,k}\end{array}∥}_{\mathit{S}}+\hfill \\ \hfill & {∥\begin{array}{c}{\overline{\mathit{p}}}_{b,k}-{\overline{\mathit{p}}}_{b,ref,k}\end{array}∥}_{\mathit{W}}+{∥\begin{array}{c}{\mathit{u}}_{k-1}\end{array}∥}_{\mathit{R}}\hfill \end{array}$$

(10)

where ${∥\begin{array}{c}{\mathit{p}}_{CoM,k}-{\mathit{p}}_{CoM,ref,k}\end{array}∥}_{\mathit{Q}}$ is the CoM position tracking task where ${\mathit{p}}_{ref,k}$ is the reference trajectory of the CoM; ${∥\begin{array}{c}{\dot{\mathit{p}}}_{CoM,k}\end{array}∥}_{\mathit{T}}$ is the CoM velocity tracking task, where the desired velocity is set to 0; ${∥\begin{array}{c}{\overline{\mathit{p}}}_{sw,k}-{\overline{\mathit{p}}}_{sw,ref,k}\end{array}∥}_{\mathit{S}}$ is the position and velocity tracking task of the swing leg, where ${\overline{\mathit{p}}}_{sw,k}$ and ${\overline{\mathit{p}}}_{sw,ref,k}$ are the actual and desired trajectory of the swing leg, respectively; ${∥\begin{array}{c}{\overline{\mathit{p}}}_{b,k}-{\overline{\mathit{p}}}_{b,ref,k}\end{array}∥}_{\mathit{W}}$ is the position and velocity tracking task of the torso; ${\overline{\mathit{p}}}_{b,k}$ and ${\overline{\mathit{p}}}_{b,ref,k}$ are the actual trajectory and the desired trajectory of the torso; ${∥\begin{array}{c}{\mathit{u}}_k\end{array}∥}_{\mathit{R}}$ is the input penalty term; $\mathit{Q}\in {\mathbb{R}}^{3\times 3}$, $\mathit{T}\in {\mathbb{R}}^{3\times 3}$, $\mathit{S}\in {\mathbb{R}}^{6\times 6}$, $\mathit{W}\in {\mathbb{R}}^{6\times 6}$, $\mathit{R}\in {\mathbb{R}}^{6\times 6}$ are the correspondent weight matrices.

Substituting Equation (8) into Equation (9), the position of the robot’s center of mass can be expressed as:

$${\mathit{p}}_{CoM,k}=\mathit{C}{\mathit{x}}_k+\mathit{D}=\mathit{C}\mathit{A}{\mathit{x}}_{k-1}+\mathit{C}\mathit{B}{\mathit{u}}_{k-1}+\mathit{D}$$

(11)

The velocity of the center of mass can be obtained by taking the derivative of Equation (6). Then, its discrete form can be obtained:

$${\dot{\mathit{p}}}_{CoM,k}=\mathit{F}{\mathit{x}}_k=\mathit{F}\mathit{A}{\mathit{x}}_{k-1}+\mathit{F}\mathit{B}{\mathit{u}}_{k-1}$$

(12)

where

$$\mathit{F}=\left[\begin{array}{cccc}{\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& \frac{m_1}{m}{\mathit{I}}_{3\times 3}& \frac{m_2}{m}{\mathit{I}}_{3\times 3}\end{array}\right]$$

(13)

The position and velocity of the swing leg can be derived by ${\overline{\mathit{p}}}_{sw,k}=\mathit{E}{\mathit{x}}_k$, where

$$\mathit{E}=\left[\begin{array}{cccc}{\mathbf{0}}_{3\times 3}& {\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathit{I}}_{3\times 3}\end{array}\right]$$

(14)

is the selection matrix. Similarly, for the torso position and velocity, ${\overline{\mathit{p}}}_{b,k}=\mathit{G}{\mathit{x}}_k$, where

$$\mathit{G}=\left[\begin{array}{cccc}{\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\end{array}\right]$$

(15)

2.3. Constraints

In order to ensure the real-time performance of the model predictive control algorithm, there should not be too many constraints in the MPC. Here, we only constrain the length of the swing leg to avoid the kinematic singularity. Since the Cartesian distance $L=\sqrt{{(x-x_1)}^2+{(y-y_1)}^2+{(z-z_1)}^2}$ is a nonlinear function of coordinates $x,y,z$, we use the Manhattan distance instead. The swing leg’s length constraint is written as

$$\left|\begin{array}{c}{x}_b-x_{sw}\end{array}\right|+\left|\begin{array}{c}{y}_b-y_{sw}\end{array}\right|+\left|\begin{array}{c}{z}_b-z_{sw}\end{array}\right|\le L_{leg}$$

(16)

Take the intermediate variable ${\mathit{s}}_{leg}={\left[\begin{array}{ccc}{s}_x& s_y& s_z\end{array}\right]}^{\top }$, which satisfies

$$\left\{\begin{array}{c}{\mathit{s}}_{leg}\ge {\mathit{p}}_b-{\mathit{p}}_{sw}\hfill \\ {\mathit{s}}_{leg}\ge {\mathit{p}}_{sw}-{\mathit{p}}_b\hfill \end{array}\right.$$

(17)

Then, $\left|\begin{array}{c}{\mathit{s}}_{leg}\end{array}\right|\le L_{leg}$ is equivalent to the constraints of Equation (16). The matrix form of the constraints can be derived as:

$$\left\{\begin{array}{c}{\mathit{s}}_{leg}\ge {\mathit{C}}_1{\mathit{x}}_k\hfill \\ {\mathit{s}}_{leg}\ge {\mathit{C}}_2{\mathit{x}}_k\hfill \\ {\mathit{1}}^{\top }{\mathit{s}}_{leg}\le L_{leg}\hfill \end{array}\right.$$

(18)

where

$${\mathit{C}}_1=\left[\begin{array}{cccc}{\mathit{I}}_{3\times 3}& -{\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\end{array}\right]$$

(19)

$${\mathit{C}}_2=\left[\begin{array}{cccc}-{\mathit{I}}_{3\times 3}& {\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\end{array}\right]$$

(20)

$$\mathbf{1}={\left[\begin{array}{ccc}1& 1& 1\end{array}\right]}^{\top }$$

(21)

2.4. MPC Optimization Problem

2.4.1. Tasks

In order to improve the calculation efficiency, we eliminate the intermediate variables by substituting system dynamics into the cost function. Equation (8) can be expanded to

$$\begin{array}{cc}\hfill {\mathit{x}}_1=& \mathit{A}{\mathit{x}}_0+\mathit{B}{\mathit{u}}_0\hfill \\ \hfill {\mathit{x}}_2=& \mathit{A}{\mathit{x}}_1+\mathit{B}{\mathit{u}}_1={\mathit{A}}^2{\mathit{x}}_0+\mathit{A}\mathit{B}{\mathit{u}}_0+\mathit{B}{\mathit{u}}_1\hfill \\ \hfill & ⋮\hfill \\ \hfill {\mathit{x}}_n=& \mathit{A}{\mathit{x}}_{n-1}+\mathit{B}{\mathit{u}}_{n-1}={\mathit{A}}^n{\mathit{x}}_0+{\mathit{A}}^{n-1}\mathit{B}{\mathit{u}}_0+\cdots +\mathit{A}\mathit{B}{\mathit{u}}_{n-2}+\mathit{B}{\mathit{u}}_{n-1}\hfill \end{array}$$

(22)

Rewrite Equation (22) into matrix form as

$$\underset{\mathit{X}}{\underbrace{\left[\begin{array}{c}{\mathit{x}}_1\\ {\mathit{x}}_2\\ ⋮\\ {\mathit{x}}_n\end{array}\right]}}=\underset{\tilde{\mathit{A}}}{\underbrace{\left[\begin{array}{c}\mathit{A}\\ {\mathit{A}}^2\\ ⋮\\ {\mathit{A}}^n\end{array}\right]}}{\mathit{x}}_0+\underset{\tilde{\mathit{B}}}{\underbrace{\left[\begin{array}{cccc}\mathit{B}& \mathbf{0}& \cdots & \mathbf{0}\\ \mathit{A}\mathit{B}& \mathit{B}& \cdots & \mathbf{0}\\ ⋮& ⋮& \ddots & ⋮\\ {\mathit{A}}^{n-1}\mathit{B}& {\mathit{A}}^{n-2}\mathit{B}& \cdots & \mathit{B}\end{array}\right]}}\underset{\mathit{U}}{\underbrace{\left[\begin{array}{c}{\mathit{u}}_0\\ {\mathit{u}}_1\\ ⋮\\ {\mathit{u}}_{n-1}\end{array}\right]}}$$

(23)

Then, by replacing the matrices with $\mathit{X},\tilde{\mathit{A}},\tilde{\mathit{B}}$ and $\mathit{U}$, Equation (23) can be rewritten as:

$$\mathit{X}=\tilde{\mathit{A}}{\mathit{x}}_0+\tilde{\mathit{B}}\mathit{U}$$

(24)

Similarly, for the robot’s center of mass position,

$${\mathit{P}}_{CoM}=\tilde{\mathit{C}}\mathit{X}+\tilde{\mathit{D}}$$

(25)

where $\tilde{\mathit{C}}=\mathrm{diag}(\mathit{C},\mathit{C},\cdots ,\mathit{C})\in {\mathbb{R}}^{3n\times 12n}$, $\tilde{\mathit{D}}={\left[\begin{array}{c}{\mathit{D}}^{\top },{\mathit{D}}^{\top },\cdots ,{\mathit{D}}^{\top }\end{array}\right]}^{\top }\in {\mathbb{R}}^{3n\times 1}$. The CoM velocity can be written as:

$${\dot{\mathit{P}}}_{CoM}=\tilde{\mathit{F}}\mathit{X}$$

(26)

where

$$\tilde{\mathit{F}}=\mathrm{diag}(\mathit{F},\mathit{F},\cdots ,\mathit{F})\in {\mathbb{R}}^{3n\times 12n}$$

(27)

The position and velocity equations of the swing leg can be written as:

$${\overline{\mathit{P}}}_{sw}=\tilde{\mathit{E}}\mathit{X}$$

(28)

where

$$\tilde{\mathit{E}}=\mathrm{diag}(\mathit{E},\mathit{E},\cdots ,\mathit{E})\in {\mathbb{R}}^{6n\times 12n}$$

(29)

Similarly, the position and velocity equations of the torso can be written as:

$${\overline{\mathit{P}}}_b=\tilde{\mathit{G}}\mathit{X}$$

(30)

where

$$\tilde{\mathit{G}}=\mathrm{diag}(\mathit{G},\mathit{G},\cdots ,\mathit{G})\in {\mathbb{R}}^{6n\times 12n}$$

(31)

The stacked acceleration vector is as follows:

$$\mathit{U}={\left[\begin{array}{c}{\mathit{u}}_0^{\top },{\mathit{u}}_1^{\top },\cdots ,{\mathit{u}}_{n-1}^{\top }\end{array}\right]}^{\top }\in {\mathbb{R}}^{6n\times 1}$$

(32)

Therefore, the cost function of Equation (10) can be rewritten as

$$\begin{array}{cc}\hfill \tilde{\mathit{J}}=\underset{\mathit{U}}{min}& {∥\begin{array}{c}{\mathit{P}}_{CoM}-{\mathit{P}}_{CoM,ref}\end{array}∥}_{\tilde{\mathit{Q}}}+{∥\begin{array}{c}{\dot{\mathit{P}}}_{CoM}\end{array}∥}_{\tilde{\mathit{T}}}+\hfill \\ \hfill & {∥\begin{array}{c}{\overline{\mathit{P}}}_{sw}-{\overline{\mathit{P}}}_{sw,ref}\end{array}∥}_{\tilde{\mathit{S}}}+{∥\begin{array}{c}{\overline{\mathit{P}}}_b-{\overline{\mathit{P}}}_{b,ref}\end{array}∥}_{\tilde{\mathit{W}}}+{∥\begin{array}{c}\mathit{U}\end{array}∥}_{\tilde{\mathit{R}}}\hfill \end{array}$$

(33)

where

$$\tilde{\mathit{Q}}=\mathrm{diag}(\mathit{Q},\mathit{Q}\cdots \mathit{Q})\in {\mathbb{R}}^{3n\times 3n}$$

(34)

$$\tilde{\mathit{T}}=\mathrm{diag}(\mathit{T},\mathit{T}\cdots \mathit{T})\in {\mathbb{R}}^{3n\times 3n}$$

(35)

$$\tilde{\mathit{S}}=\mathrm{diag}(\mathit{S},\mathit{S}\cdots \mathit{S})\in {\mathbb{R}}^{6n\times 6n}$$

(36)

$$\tilde{\mathit{W}}=\mathrm{diag}(\mathit{W},\mathit{W}\cdots \mathit{W})\in {\mathbb{R}}^{6n\times 6n}$$

(37)

$$\tilde{\mathit{R}}=\mathrm{diag}(\mathit{R},\mathit{R}\cdots \mathit{R})\in {\mathbb{R}}^{6n\times 6n}$$

(38)

By eliminating the intermediate variables, Equation (33) can be rewritten as

$$\begin{array}{cc}\hfill \tilde{\mathit{J}}=\underset{\mathit{U}}{min}& {∥\begin{array}{c}\tilde{\mathit{C}}\tilde{\mathit{A}}{\mathit{x}}_0+\tilde{\mathit{C}}\tilde{\mathit{B}}\mathit{U}+\tilde{\mathit{D}}-{\mathit{P}}_{CoM,ref}\end{array}∥}_{\tilde{\mathit{Q}}}+{∥\begin{array}{c}\tilde{\mathit{F}}\tilde{\mathit{A}}{\mathit{x}}_0+\tilde{\mathit{F}}\tilde{\mathit{B}}\mathit{U}\end{array}∥}_{\tilde{\mathit{T}}}+\hfill \\ \hfill & {∥\begin{array}{c}\tilde{\mathit{E}}\tilde{\mathit{A}}{\mathit{x}}_0+\tilde{\mathit{E}}\tilde{\mathit{B}}\mathit{U}-{\overline{\mathit{P}}}_{sw,ref}\end{array}∥}_{\tilde{\mathit{S}}}+{∥\begin{array}{c}\tilde{\mathit{G}}\tilde{\mathit{A}}{\mathit{x}}_0+\tilde{\mathit{G}}\tilde{\mathit{B}}\mathit{U}-{\overline{\mathit{P}}}_{b,ref}\end{array}∥}_{\tilde{\mathit{W}}}+{∥\begin{array}{c}\mathit{U}\end{array}∥}_{\tilde{\mathit{R}}}\hfill \end{array}$$

(39)

2.4.2. Constraints

According to Equation (3), the stacked form of the swing leg length constraint can be derived as

$$\left\{\begin{array}{c}{\tilde{\mathit{1}}}^{\top }{\tilde{\mathit{C}}}_1\tilde{\mathit{B}}\mathit{U}\le {\tilde{\mathit{L}}}_{leg}-{\tilde{\mathit{1}}}^{\top }{\tilde{\mathit{C}}}_1\tilde{\mathit{A}}{\mathit{x}}_0\hfill \\ {\tilde{\mathit{1}}}^{\top }{\tilde{\mathit{C}}}_2\tilde{\mathit{B}}\mathit{U}\le {\tilde{\mathit{L}}}_{leg}-{\tilde{\mathit{1}}}^{\top }{\tilde{\mathit{C}}}_2\tilde{\mathit{A}}{\mathit{x}}_0\hfill \end{array}\right.$$

(40)

where

$${\tilde{\mathit{C}}}_1=\mathrm{diag}({\mathit{C}}_1,{\mathit{C}}_1,\cdots ,{\mathit{C}}_1)\in {\mathbb{R}}^{3n\times 12n}$$

(41)

$${\tilde{\mathit{C}}}_2=\mathrm{diag}({\mathit{C}}_2,{\mathit{C}}_2,\cdots ,{\mathit{C}}_2)\in {\mathbb{R}}^{3n\times 12n}$$

(42)

$${\tilde{\mathit{L}}}_{leg}={\left[\begin{array}{c}{L}_{leg},L_{leg},\cdots ,L_{leg}\end{array}\right]}^{\top }\in {\mathbb{R}}^{n\times 1}$$

(43)

$$\tilde{\mathit{1}}=\mathrm{diag}(\mathit{1},\mathit{1},\cdots ,\mathit{1})\in {\mathbb{R}}^{3n\times n}$$

(44)

2.4.3. The Quadratic Programming Problem

The MPC problem can finally be transformed into the following quadratic optimization problem, which can be solved by off-the-shell QP solvers:

$$\begin{array}{cc}\hfill \underset{\mathit{U}}{min}& \frac{1}{2}{\mathit{U}}^{\top }{\mathit{H}}_{qp}\mathit{U}+{\mathit{g}}_{qp}^{\top }\mathit{U}\hfill \\ \hfill \mathrm{s}.\mathrm{t}.& {\mathit{lb}}_{qp}\le {\mathit{A}}_{qp}\mathit{U}\le {\mathit{ub}}_{qp}\hfill \end{array}$$

(45)

where

$${\mathit{H}}_{qp}={\mathit{H}}_1+{\mathit{H}}_2+{\mathit{H}}_3+{\mathit{H}}_4+{\mathit{H}}_5$$

(46)

$${\mathit{g}}_{qp}={\mathit{g}}_1+{\mathit{g}}_2+{\mathit{g}}_3+{\mathit{g}}_4+{\mathit{g}}_5$$

(47)

$${\mathit{A}}_{qp}=\left[\begin{array}{c}{\tilde{\mathit{1}}}^{\top }{\tilde{\mathit{C}}}_1\tilde{\mathit{B}}\mathit{U}\\ {\tilde{\mathit{1}}}^{\top }{\tilde{\mathit{C}}}_2\tilde{\mathit{B}}\mathit{U}\end{array}\right]$$

(48)

$${\mathit{ub}}_{qp}=\left[\begin{array}{c}{\tilde{\mathit{L}}}_{leg}-{\tilde{\mathit{1}}}^{\top }{\tilde{\mathit{C}}}_1\tilde{\mathit{A}}{\mathit{x}}_0\\ {\tilde{\mathit{L}}}_{leg}-{\tilde{\mathit{1}}}^{\top }{\tilde{\mathit{C}}}_2\tilde{\mathit{A}}{\mathit{x}}_0\end{array}\right]$$

(49)

$$\begin{array}{cc}\hfill {\mathit{H}}_1& =2{\tilde{\mathit{B}}}^{\top }{\tilde{\mathit{C}}}^{\top }\tilde{\mathit{Q}}\tilde{\mathit{C}}\tilde{\mathit{B}}\hfill \end{array}$$

(50)

$$\begin{array}{cc}\hfill {\mathit{H}}_2& =2{\tilde{\mathit{B}}}^{\top }{\tilde{\mathit{F}}}^{\top }\tilde{\mathit{T}}\tilde{\mathit{F}}\tilde{\mathit{B}}\hfill \end{array}$$

(51)

$$\begin{array}{cc}\hfill {\mathit{H}}_3& =2{\tilde{\mathit{B}}}^{\top }{\tilde{\mathit{E}}}^{\top }\tilde{\mathit{S}}\tilde{\mathit{E}}\tilde{\mathit{B}}\hfill \end{array}$$

(52)

$$\begin{array}{cc}\hfill {\mathit{H}}_4& =2{\tilde{\mathit{B}}}^{\top }{\tilde{\mathit{G}}}^{\top }\tilde{\mathit{W}}\tilde{\mathit{G}}\tilde{\mathit{B}}\hfill \end{array}$$

(53)

$$\begin{array}{cc}\hfill {\mathit{H}}_5& =2\tilde{\mathit{R}}\hfill \end{array}$$

(54)

$$\begin{array}{cc}\hfill {\mathit{g}}_1& =2{\tilde{\mathit{B}}}^{\top }{\tilde{\mathit{C}}}^{\top }\tilde{\mathit{Q}}\left(-{\mathit{P}}_{CoM,ref}+\tilde{\mathit{C}}\tilde{\mathit{A}}{\mathit{x}}_0+\tilde{\mathit{D}}\right)\hfill \end{array}$$

(55)

$$\begin{array}{cc}\hfill {\mathit{g}}_2& =2{\tilde{\mathit{B}}}^{\top }{\tilde{\mathit{F}}}^{\top }\tilde{\mathit{T}}\tilde{\mathit{F}}\tilde{\mathit{A}}{\mathit{x}}_0\hfill \end{array}$$

(56)

$$\begin{array}{cc}\hfill {\mathit{g}}_3& =2{\tilde{\mathit{B}}}^{\top }{\tilde{\mathit{E}}}^{\top }\tilde{\mathit{S}}\left(\tilde{\mathit{E}}\tilde{\mathit{A}}{\mathit{x}}_0-{\overline{\mathit{P}}}_{sw,ref}\right)\hfill \end{array}$$

(57)

$$\begin{array}{cc}\hfill {\mathit{g}}_4& =2{\tilde{\mathit{B}}}^{\top }{\tilde{\mathit{G}}}^{\top }\tilde{\mathit{W}}\left(\tilde{\mathit{G}}\tilde{\mathit{A}}{\mathit{x}}_0-{\overline{\mathit{P}}}_{b,ref}\right)\hfill \end{array}$$

(58)

$$\begin{array}{cc}\hfill {\mathit{g}}_5& =\mathbf{0}\hfill \end{array}$$

(59)

3. Hierarchical Whole-Body Control

3.1. Problem Formulation

The generalized coordinates of the floating base bipedal robot are defined as:

$$\mathit{q}=\left[\begin{array}{c}{\mathit{q}}_f\\ {\mathit{q}}_j\end{array}\right]\in {\mathbb{R}}^{n_q}$$

(60)

where ${\mathit{q}}_f\in {\mathbb{R}}^{n_f}$ represents the position and orientation of the robot’s floating base relative to the inertial coordinate system, and ${\mathit{q}}_j\in {\mathbb{R}}^{n_j}$ represents the angle of the actuated joints of the robot and $n_q=n_f+n_j$. The dynamic equation of the system is formulated as

$$\mathit{M}\left(\mathit{q}\right)\ddot{\mathit{q}}+\mathit{h}(\mathit{q},\dot{\mathit{q}})=\left[\begin{array}{c}{\mathbf{0}}_{n_f\times 1}\\ {\mathit{\tau }}_j\end{array}\right]+{\mathit{J}}_c^{\top }\left(\mathit{q}\right){\mathit{\omega }}_{\mathit{c}}$$

(61)

where $\dot{\mathit{q}}$ and $\ddot{\mathit{q}}$ are the first and second-order derivative of $\mathit{q}$, respectively, $\mathit{M}\left(\mathit{q}\right)\in {\mathbb{R}}^{n_q\times n_q}$ is the mass matrix of the robot, $\mathit{h}(\mathit{q},\dot{\mathit{q}})\in {\mathbb{R}}^{n_q}$ collects the Coriolis force, centrifugal force and gravity, and ${\mathit{\tau }}_j\in {\mathbb{R}}^{n_j}$ is the joint torque: ${\mathit{\omega }}_{c,h}={\left[\begin{array}{cc}{\mathit{f}}_{c,h}^{\top }& {\mathit{\tau }}_{c,h}^{\top }\end{array}\right]}^{\top }\in {\mathbb{R}}^{6\times 1}$ is the external wrench applied to one leg, including three-dimensional external contact force and three-dimensional contact torque. For the biped robot, ${\mathit{\omega }}_c={\left[\begin{array}{cc}{{\mathit{\omega }}^{\top }}_{c,0}& {{\mathit{\omega }}^{\top }}_{c,1}\end{array}\right]}^{\top }\in {\mathbb{R}}^{12\times 1}$. ${\mathit{J}}_c\left(\mathit{q}\right)\in {\mathbb{R}}^{12\times n_q}$ is the contact Jacobian matrix.

In order to enable the robot to complete tasks with different priorities, a quadratic programming method with priority hierarchy was proposed [26]. The idea is to put the solution space of the upper priority level as a constraint of the lower level in the optimization problem. The optimization problem of the ith level task is defined as:

$$\begin{array}{cc}\hfill \underset{{\mathit{\chi }}_i}{min}& {∥\begin{array}{c}{\overline{\mathit{A}}}_i{\mathit{\chi }}_i-{\overline{\mathit{b}}}_i\end{array}∥}_{{\mathit{Q}}_i}\hfill \\ \hfill \mathrm{s}.\mathrm{t}.& {\overline{\mathit{ld}}}_i⩽{\overline{\mathit{C}}}_i{\mathit{\chi }}_i⩽{\overline{\mathit{ud}}}_i\hfill \\ \hfill & {\overline{\mathit{A}}}_{i-1}^{aug}{\mathit{\chi }}_i={\overline{\mathit{A}}}_{i-1}^{aug}{\mathit{\chi }}_{i-1}^{*}\hfill \\ \hfill & {\overline{\mathit{ld}}}_{i-1}^{aug}⩽{\overline{\mathit{C}}}_{i-1}^{aug}{\mathit{\chi }}_i⩽{\overline{\mathit{ud}}}_{i-1}^{aug}\hfill \end{array}$$

(62)

where ${\overline{\mathit{A}}}_i$ and ${\overline{\mathit{b}}}_i$ represent the task matrix and target vector corresponding to the current priority. If multiple tasks have the same priority, they could be weighted and combined by a diagonal matrix ${\mathit{Q}}_i$. ${\overline{\mathit{C}}}_i$, ${\overline{\mathit{ld}}}_i$ and ${\overline{\mathit{ud}}}_i$ represent the constraint matrix, lower bound, and upper bound of all constraints of the current priority. ${\overline{\mathit{A}}}_{i-1}^{aug}={\left[\begin{array}{ccc}{\overline{\mathit{A}}}_1^{\top }& \cdots & {\overline{\mathit{A}}}_{i-1}^{\top }\end{array}\right]}^{\top }$ is the augmented task matrix corresponding to all tasks in the previous priority, and ${\mathit{\chi }}_{i-1}^{*}$ is the optimal solution of the previous priority. ${\overline{\mathit{C}}}_{i-1}^{aug}$, ${\overline{\mathit{ld}}}_{i-1}^{aug}$, ${\overline{\mathit{ud}}}_{i-1}^{aug}$, with a similar form of ${\overline{\mathit{A}}}_{i-1}^{aug}$, represent the augmented constraint matrix and bounds corresponding to all constraints in the previous priority. Here, the tasks in level i have higher priorities than tasks in level j when $i<j$. ${\mathit{\chi }}_i$ in level i represents the optimization variables to complete the tasks with priorities higher than $i+1$, which means that the final optimal solution of multi-tasks with priority level $n_l$ is ${\mathit{\chi }}_{n_l}^{*}$.

Here, we select the generalized joint acceleration and the contact wrench as the optimization variables in each level:

$$\mathbf{{\rm Y}}=\left[\begin{array}{c}\ddot{\mathit{q}}\\ {\mathit{\omega }}_{\mathit{c}}\end{array}\right]$$

(63)

3.2. Tasks

The definition of each level’s tasks and constraints is given in

Table 1. Tasks, Constraints, and Priority Setting of Whole-Body Control for Single Leg Balance Standing.

Priority	Tasks	Tasks Dimension	Constraints	Constraints Dimension
1	Floating Base Dynamics	6	Joint Torque	10
2	Linear Momentum	6	ZMP	9
	Torso Posture		Friction Cone
3	Foot Position & Posture	16
	Contact Wrench

. The floating base dynamics task is the basis of all motions of the mobile robot, and all the active forces come from joint motors, so the floating base dynamics task and joint moment constraints are set as the first priority. The linear momentum and trunk posture are critical factors for robot stability, while the ZMP constraint and the plantar friction cone constraint can ensure that the foot of the biped robot will not flip and slip, so these tasks and constraints are set as the second priority. The position of the robot’s feet, posture task, and plantar force screw task are set as the third priority. The detailed definition of each task and constraint is as follows.

3.2.1. Floating Base Dynamics Task

The first six rows of Equation (61) are part of the floating base dynamics, and it can be extracted by a selection matrix ${\mathit{S}}_f$:

$${\mathit{S}}_f\mathit{M}\left(\mathit{q}\right)\ddot{\mathit{q}}+{\mathit{S}}_f\mathit{h}(\mathit{q},\dot{\mathit{q}})={\mathit{S}}_f\left[\begin{array}{c}{\mathbf{0}}_{6\times 1}\\ {\mathit{\tau }}_j\end{array}\right]+{\mathit{S}}_f{\mathit{J}}_c^{\top }\left(\mathit{q}\right){\mathit{\omega }}_{\mathit{c}}$$

(64)

where ${\mathit{S}}_f=\left[\begin{array}{cc}{\mathit{I}}_{n_f\times n_f}& {\mathbf{0}}_{n_f\times n_j}\end{array}\right]$. Rearrange Equation (64) into the form of ${\overline{\mathit{A}}}_i{\mathit{\chi }}_i={\overline{\mathit{b}}}_i$:

$$\left[\begin{array}{cc}{\mathit{S}}_f\mathit{M}& -{\mathit{S}}_f{\mathit{J}}_c^{\top }\end{array}\right]\left[\begin{array}{c}\ddot{\mathit{q}}\\ {\mathit{\omega }}_{\mathit{c}}\end{array}\right]=-{\mathit{S}}_f\mathit{h}$$

(65)

Therefore, ${\overline{\mathit{A}}}_0=\left[\begin{array}{cc}{\mathit{S}}_f\mathit{M}& -{\mathit{S}}_f{\mathit{J}}^{\top }\end{array}\right]\in {\mathbb{R}}^{n_f\times (n_q+12)}$, ${\overline{\mathit{b}}}_0=-{\mathit{S}}_f\mathit{h}\in {\mathbb{R}}^{n_f}$.

3.2.2. Centroidal Dynamics Task

Set the desired external force of the centroid task as ${\mathit{f}}_{G,des}\in {\mathbb{R}}^3$, and design the following PD controller to track the centroid task:

$${\mathit{f}}_{G,des}=m[{\mathit{K}}_{p,0}({\mathit{p}}_{G,tar}-{\mathit{p}}_G)+{\mathit{K}}_{d,0}({\dot{\mathit{p}}}_{G,tar}-{\dot{\mathit{p}}}_G)+{\ddot{\mathit{p}}}_{G,tar}]$$

(66)

$${\overline{\mathit{A}}}_{1,CoM}=\left[\begin{array}{cc}{\mathit{S}}_G{\mathit{A}}_G& {\mathbf{0}}_{3\times 12}\end{array}\right]\in {\mathbb{R}}^{3\times (n_q+12)}$$

(67)

$${\overline{\mathit{b}}}_{1,CoM}={\mathit{f}}_{G,des}-{\mathit{S}}_G{\dot{\mathit{A}}}_G\dot{\mathit{q}}\in {\mathbb{R}}^3$$

(68)

where ${\mathit{p}}_{G,tar}\in {\mathbb{R}}^3$, ${\dot{\mathit{p}}}_{G,tar}\in {\mathbb{R}}^3$ and ${\ddot{\mathit{p}}}_{G,tar}\in {\mathbb{R}}^3$ represent the desired position, velocity, and acceleration, respectively; ${\mathit{p}}_G\in {\mathbb{R}}^3$, ${\dot{\mathit{p}}}_G\in {\mathbb{R}}^3$ and ${\ddot{\mathit{p}}}_G\in {\mathbb{R}}^3$ represent the actual position, velocity, and acceleration, respectively; ${\mathit{S}}_G=\left[\begin{array}{cc}{\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\end{array}\right]$ represents the selected matrix, ${\mathit{A}}_G\in {\mathbb{R}}^{6\times n_q}$ represents the centroidal momentum matrix [27]; ${\mathit{K}}_{p,0}\in {\mathbb{R}}^{3\times 3}$ and ${\mathit{K}}_{d,0}\in {\mathbb{R}}^{3\times 3}$ represent PD gain matrices.

3.2.3. Torso Orientation Task

Set the desired acceleration of the torso orientation task as ${\ddot{\mathit{\theta }}}_{t,des}\in {\mathbb{R}}^3$, and design the following PD controller to track the torso orientation:

$${\ddot{\mathit{\theta }}}_{t,des}={\mathit{K}}_{p1}({\mathit{\theta }}_{t,tar}-{\mathit{\theta }}_t)+{\mathit{K}}_{d1}({\dot{\mathit{\theta }}}_{t,tar}-{\dot{\mathit{\theta }}}_t)+{\ddot{\mathit{\theta }}}_{t,tar}$$

(69)

where ${\mathit{\theta }}_{t,tar}\in {\mathbb{R}}^3$, ${\dot{\mathit{\theta }}}_{t,tar}\in {\mathbb{R}}^3$, and ${\ddot{\mathit{\theta }}}_{t,tar}\in {\mathbb{R}}^3$ represent the desired torso orientation angle, angular velocity, and angular acceleration, respectively; ${\mathit{\theta }}_t\in {\mathbb{R}}^3$, ${\dot{\mathit{\theta }}}_t\in {\mathbb{R}}^3$ and ${\ddot{\mathit{\theta }}}_t\in {\mathbb{R}}^3$ represent the actual torso orientation angle, angular velocity, and angular acceleration, respectively; ${\mathit{K}}_{p,1}\in {\mathbb{R}}^{3\times 3}$ and ${\mathit{K}}_{d,1}\in {\mathbb{R}}^{3\times 3}$ represent PD parameter matrices.

The first six rows in the optimization variable $\mathbf{{\rm Y}}$ represent the position and orientation of the torso, and the fourth to sixth rows are the torso orientation, so, for the torso orientation task:

$${\overline{\mathit{A}}}_{1,torso}=\left[\begin{array}{ccc}{\mathbf{0}}_{3\times 3}& {\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times (n_j+12)}\end{array}\right]$$

(70)

$${\overline{\mathit{b}}}_{1,torso}={\ddot{\mathit{\theta }}}_{t,des}$$

(71)

3.2.4. Feet Position and Orientation Task

As above, let ${\ddot{\mathit{r}}}_{f,des}\in {\mathbb{R}}^{12}$ be the desired acceleration of the foot posture, and the PD controller is designed as follows:

$${\ddot{\mathit{r}}}_{f,des}={\mathit{K}}_{p2}({\mathit{r}}_{f,tar}-{\mathit{r}}_f)+{\mathit{K}}_{d2}({\dot{\mathit{r}}}_{f,tar}-{\dot{\mathit{r}}}_f)+{\ddot{\mathit{r}}}_{f,tar}$$

(72)

where ${\mathit{r}}_{f,tar}\in {\mathbb{R}}^{12}$, ${\dot{\mathit{r}}}_{f,tar}\in {\mathbb{R}}^{12}$ and ${\ddot{\mathit{r}}}_{f,tar}\in {\mathbb{R}}^{12}$ represent the desired foot posture, velocity, and acceleration, respectively; ${\mathit{r}}_f\in {\mathbb{R}}^{12}$, ${\dot{\mathit{r}}}_f\in {\mathbb{R}}^{12}$ and ${\ddot{\mathit{r}}}_f\in {\mathbb{R}}^{12}$ represent the actual foot posture, velocity and acceleration, respectively; ${\mathit{K}}_{p,2}\in {\mathbb{R}}^{12\times 12}$ and ${\mathit{K}}_{d,2}\in {\mathbb{R}}^{12\times 12}$ represent PD parameter matrices.

The acceleration of the foot of the end can be obtained according to the following equation:

$${\mathit{J}}_{\mathit{c}}\ddot{\mathit{q}}+{\dot{\mathit{J}}}_{\mathit{c}}\dot{\mathit{q}}={\ddot{\mathit{r}}}_{f,des}$$

(73)

Then,

$${\overline{\mathit{A}}}_{2,feet}=\left[\begin{array}{cc}{\mathit{J}}_{\mathit{c}}& {\mathbf{0}}_{12\times 12}\end{array}\right]\in {\mathbb{R}}^{12\times (n_q+12)}$$

(74)

$${\overline{\mathit{b}}}_{2,feet}={\ddot{\mathit{r}}}_{f,des}-{\dot{\mathit{J}}}_c\dot{\mathit{q}}\in {\mathbb{R}}^{12}$$

(75)

3.2.5. Contact Wrench Task

The contact wrench task tracks the desired contact wrench. Let ${\mathit{\omega }}_{c,des}$ be the desired contact wrench. The last 12 dimensions of the optimization variable $\mathbf{{\rm Y}}$ are the contact wrench, so for the contact wrench task,

$${\overline{\mathit{A}}}_{2,wrench}=\left[\begin{array}{cc}{\mathbf{0}}_{12\times n_q}& {\mathit{I}}_{12\times 12}\end{array}\right]$$

(76)

$${\overline{\mathit{b}}}_{2,wrench}={\mathit{\omega }}_{c,des}\in {\mathbb{R}}^{12}$$

(77)

3.3. Constraints

3.3.1. Joint Torque Constraint

The last ten rows of Equation (61) represent the actuated joints, which should be limited by torque:

$${\mathit{S}}_j\mathit{M}\left(\mathit{q}\right)\ddot{\mathit{q}}+{\mathit{S}}_j\mathit{h}(\mathit{q},\dot{\mathit{q}})={\mathit{S}}_j\left[\begin{array}{c}{\mathbf{0}}_{6\times 1}\\ {\mathit{\tau }}_j\end{array}\right]+{\mathit{S}}_j{\mathit{J}}_c^{\top }\left(\mathit{q}\right){\mathit{\omega }}_{\mathit{c}}$$

(78)

where ${\mathit{S}}_j=\left[\begin{array}{cc}{\mathbf{0}}_{n_j\times n_f}& {\mathit{I}}_{n_j\times n_j}\end{array}\right]$ represents the selection matrix. The joint torque can be derived as:

$${\mathit{\tau }}_j={\mathit{S}}_j\mathit{M}\ddot{\mathit{q}}+{\mathit{S}}_j\mathit{h}-{\mathit{S}}_j{\mathit{J}}_c^{\top }{\mathit{\omega }}_{\mathit{c}}$$

(79)

Therefore, the joint torque constraint is:

$$-{\mathit{\tau }}_{limit}\le {\mathit{\tau }}_j\le {\mathit{\tau }}_{limit}$$

(80)

where ${\mathit{\tau }}_{limit}\in {\mathbb{R}}^{n_j}$ is the torque limit of each actuated joint. Then, it can be obtained that:

$${\overline{\mathit{C}}}_0=\left[\begin{array}{cc}{\mathit{S}}_j\mathit{M}& -{\mathit{S}}_j{\mathit{J}}_c^{\top }\end{array}\right]\in {\mathbb{R}}^{n_j\times (n_q+12)}$$

(81)

$${\overline{\mathit{ld}}}_0=-{\mathit{\tau }}_{limit}-{\mathit{S}}_j\mathit{h}\in {\mathbb{R}}^{n_j}$$

(82)

$${\overline{\mathit{ud}}}_0={\mathit{\tau }}_{limit}-{\mathit{S}}_j\mathit{h}\in {\mathbb{R}}^{n_j}$$

(83)

3.3.2. ZMP Constraint

According to [28,29], the ZMP constraint on $\mathit{x}$ direction can be written as

$$l_x^{-}\le \frac{-{\tau }_{c,y}-f_{c,x}d}{f_{c,z}}\le l_x^{+}$$

(84)

where $l_x^{-}$ and $l_x^{+}$ are the minimum and maximum values of ZMP, d is the vertical distance between the foot joint and the ground. ${\tau }_{c,y}$ is the moment applied to the foot in the direction of y, $f_{c,x}$, and $f_{c,z}$ represent the force applied to the foot in the direction of x and z. For the $\mathit{y}$ direction, the ZMP constraint can be derived similarly. Rewrite the ZMP constraints in matrix form, where

$${\mathit{C}}_{zmp}=\left[\begin{array}{cccccc}-d& 0& -l_x^{-}& 0& -1& 0\\ d& 0& l_x^{+}& 0& 1& 0\\ 0& -d& -l_y^{-}& -1& 0& 0\\ 0& d& l_y^{+}& 1& 0& 0\end{array}\right]$$

(85)

$${\overline{\mathit{C}}}_{1,zmp}=\left[\begin{array}{ccc}{\mathbf{0}}_{4\times 16}& {\mathit{C}}_{zmp}& {\mathbf{0}}_{4\times 6}\\ {\mathbf{0}}_{4\times 16}& {\mathbf{0}}_{4\times 6}& {\mathit{C}}_{zmp}\end{array}\right]$$

(86)

$${\overline{\mathit{ld}}}_{1,zmp}={\mathbf{0}}_{8\times 1}$$

(87)

$${\overline{\mathit{ud}}}_{1,zmp}={\infty }_{8\times 1}$$

(88)

3.3.3. Foot Friction Cone Constraint

The stance leg’s ground reaction force should stay in the friction cone to avoid slipping. Here, we employ a pyramid approximation of the friction cone and formulate the foot friction cone constraint as

$$\left|\begin{array}{c}{f}_{x,y}\end{array}\right|\le \mu f_z$$

(89)

where $\mu$ is the coefficient of friction; $f_{x,y}$ is the resultant force in the direction of x and y, and $f_z$ is the force in the direction of z. Expand and arrange the aforementioned equation into the matrix form:

$${\mathit{C}}_f=\left[\begin{array}{ccc}1& 0& \mu \\ -1& 0& \mu \\ 0& 1& \mu \\ 0& -1& \mu \\ 0& 0& 1\end{array}\right]$$

(90)

$${\overline{\mathit{C}}}_{1,friction}=\left[\begin{array}{ccccc}{\mathbf{0}}_{5\times 16}& {\mathit{C}}_f& {\mathbf{0}}_{5\times 3}& {\mathbf{0}}_{5\times 3}& {\mathbf{0}}_{5\times 3}\\ {\mathbf{0}}_{5\times 16}& {\mathbf{0}}_{5\times 3}& {\mathbf{0}}_{5\times 3}& {\mathit{C}}_f& {\mathbf{0}}_{5\times 3}\end{array}\right]$$

(91)

$${\overline{\mathit{lb}}}_{1,friction}={\mathbf{0}}_{10\times 1}$$

(92)

$${\overline{\mathit{ub}}}_{1,friction}={\infty }_{10\times 1}$$

(93)

3.4. Control Framework

Figure 2 shows the overall control framework. Both TP-MPC and WBC run at 1 kHz. The input of TP-MPC is the desired position of CoM, swing leg, and torso in the future n steps. WBC then takes the target trajectory of the swing leg calculated by TP-MPC and the target trajectory of CoM and torso orientation given by the stand planner as input. Although TP-MPC can also obtain the trajectories of the torso and CoM, we do not use them in our controller.

4. Simulation Results and Discussion

4.1. Simulation Setup

The simulation platform is the open-source robot simulator Webots [30]. The control algorithms are implemented with C++. We adopt the rigid body dynamics library (RBDL) [31] to calculate robot kinematics and dynamics and the qpOASES [32] toolkit to solve quadratic programming problems. The simulation environment and the controller run on a computer with an i7-11800H 2.30 GHz processor, 32 GB memory, and RTX3070 8 GB RAM GPU.

The bipedal robot in simulation has ten actuated joints and a total weight of 45 kg. The weights of the torso and a single leg are 22 kg and 11.5 kg, respectively. A single leg accounts for around one-fourth of the robot’s total weight, which implies a significant effect of the swing leg’s motion on the entire robot states. The robot’s soles are 0.22 m-by-0.1 m rectangles.

As shown in Figure 3, the biped robot stands on a single leg, and a rigid hanging ball swings down to hit the robot. The suspension point of the ball is vertically above the robot. The ball is released from the same height and angle with zero initial speed. The robot’s torso is hit 0.8 m above the ground in the frontal or the side direction. The initial height of the robot’s CoM is 0.604 m. Therefore, the hit will exert a net torque concerning the robot’s CoM. By increasing the rigid ball’s mass, the impulse received by the robot at the instance of impact increases, meaning larger external disturbance. We compare our control scheme with the WBC-only control scheme in each experiment. For both control schemes, the WBC part is the same. For the WBC-only scheme, the desired torso and leg trajectories remain equal to their initial values.

4.2. Results

4.2.1. Frontal Impact

In the frontal impact experiment, the rigid ball swings along the x-axis and hits the center of the torso’s front surface. When the mass of the rigid ball is 9 kg or 11 kg, the robot’s CoM trajectories during the impact and recovery stages are shown in Figure 4 and Figure 5, respectively. In the 9 kg case, both control schemes can resist the external disturbance and bring the robot’s CoM back to its initial position, but our MPC-WBC scheme achieves smaller CoM motions and faster recovery than the WBC-only scheme. For both control schemes, Figure 6 and Figure 7 show the actual trajectories of the swing leg’s foot end and the correspondent reference trajectories given to WBC. For the WBC-only scheme, the WBC’s reference trajectories are constant, while the actual trajectories deviate significantly from the reference. This deviation results from the WBC sacrificing the foot-end tracking task for the high-priority CoM tracking task. For our MPC-WBC scheme, the reference foot end trajectories provided by the MPC and the actual trajectories almost coincide with each other. This suggests that the MPC generates swing foot trajectories that are coherent with the CoM tracking targets. It is also observed that the MPC-WBC scheme generates larger swing foot motions than the WBC-only scheme, which confirms the swing leg’s vital role in balancing the robot.

When the ball’s mass increases to 11 kg, the WBC-only scheme causes the divergence of the robot’s CoM trajectories and thus the failure of the entire robot. However, our MPC-WBC scheme can still withstand the impact. Screenshots in Figure 8 depict the robot’s motions during the impact and recovery process under our MPC-WBC scheme. Significant forward and upward motions of the swing leg can be observed after the impact, implying that our method successfully utilizes the swing leg motions to balance the robot.

Figure 9 and Figure 10 show the robot’s centroidal pitch angular momentum. Although the rotational dynamics are not included in our TP-MPC, the simulation results show that our method is sufficient to deal with a certain degree of torque perturbation relative to the CoM.

Figure 11 shows the computation time cost of the TP-MPC algorithm. It can be seen that most of the computations are within 500 $\mathsf{\mu }$s.

4.2.2. Side Impact

Compared with frontal impact, the robot is more vulnerable to side impact since the supporting polygon is narrower in the side direction. For side impact, when the ball’s mass is 2.7 kg or 3.2 kg, the robot’s CoM trajectories during the impact and recovery stages are shown in Figure 12 and Figure 13, respectively. Figure 14 and Figure 15 show the actual trajectories of the swing leg’s foot end and the correspondent reference trajectories given to WBC. Figure 16 and Figure 17 show the robot’s centroidal roll angular momentum. Similar to the situation of frontal impact in Section 4.2.1, both control schemes can balance the robot under the 2.7 kg ball’s impact, with our MPC-WBC scheme achieving better performance. Under the 3.2 kg ball’s impact, the WBC-only scheme fails, while our scheme still balances the robot. Screenshots in Figure 18 show that the robot can also side-swing its leg under a side impact. Figure 19 shows the computation time cost of the TP-MPC algorithm when the robot is subjected to a side impact.

4.3. Discussion

In the WBC, the centroidal dynamics task is considered the second-priority task. Due to its existence, the biped robot can mobilize all joints to maintain the position and velocity of the CoM. Therefore, the WBC-only controller has a certain degree of anti-interference ability. However, it may fail to deal with a considerable impact. The centroidal dynamics task is expressed on the acceleration level. When the acceleration required to achieve the second-priority tasks has been satisfied, the third-priority tasks will be executed as much as possible. That is to say, the WBC will execute the task of swing foot to some extent, even if the robot’s CoM has not returned to its initial state. In the WBC-only control scheme, the desired trajectory of the swing foot only keeps the initial state unchanged, which is almost unhelpful for maintaining the stability of the CoM. On the contrary, in our control scheme, the TP-MPC provides the swing foot with the desired motion trajectory that can balance the position of the CoM. The task of swing foot is almost consistent with the centroidal dynamics task in WBC. Therefore, the control scheme with TP-MPC shows a better ability to maintain the stability of the center of mass.

In the above experiments, the maximum computation time of our TP-MPC is 961 $\mathsf{\mu }$s, and the average value is about 200 $\mathsf{\mu }$s. Through proper optimization in coding, our algorithm can run at the frequency of 1 kHz on the real robot.

5. Conclusions

This paper proposes the three-particle model predictive control scheme to solve the balance control problem of a bipedal robot standing on one foot. We verified our control scheme in simulation. Compared with the WBC-only control scheme, our scheme can resist much larger disturbances and bring the robot back to its initial state much faster. Although the TP-MPC discards the complicated full dynamics of the robot, it can still catch the main effects of the swing leg motions on the robot’s CoM. Furthermore, since our MPC employs only linear dynamics and constraints, it requires little computational cost, implying real-time execution at the same frequencies as the WBC on a real robot.

Our controller also has some deficiencies. The three-particle model may be over-simplified in some cases, for example, where the robot’s legs or torso have large angular velocities. In the future, we plan to improve our simplified model, for example, including the rigid body’s rotational inertia or adding more particles to the model. We also plan to verify our method on an actual bipedal robot.