Squat Motion of a Humanoid Robot Using Three-Particle Model Predictive Control and Whole-Body Control

Abstract

Squatting is a fundamental and crucial movement, often employed as a basic test during robot commissioning, and it plays a significant role in some service industries and in cases when robots perform high-dynamic movements like jumping. Therefore, achieving continuous and precise squatting actions is of great importance for the future development of humanoid robots. In this paper, we apply three-particle model predictive control (TP-MPC) combined with weight-based whole-body control (WBC) to a humanoid robot. In this approach, the arms, legs, and torso are simplified into three particles. TP-MPC is utilized to optimize the rough planning’s reference trajectory, while WBC is employed to follow the optimized trajectory. The algorithm is tested through simulations of a humanoid robot performing continuous squatting motions. It demonstrates the ability to achieve more accurate trajectory tracking compared to using WBC alone and also optimizes the issue of excessive knee torque spikes that occur with WBC alone during squatting. Moreover, the algorithm is less computationally intensive, and it is capable of operating at a frequency of 100 Hz.

1. Introduction

1.1. Background

In recent years, the development of humanoid robots has been accelerating rapidly, and their motion capabilities have improved remarkably. However, due to the complex dynamics and high degrees of freedom inherent in humanoid robots, coupled with external environmental interferences, achieving precise motion control remains a significant challenge. For service robots, like those utilized in warehousing and logistics, the ability to squat is essential and crucial. These robots have to bend down and squat to handle items, using their arms to perform tasks before standing up again. Traditionally, the control algorithms for bipedal robots have relied on optimization-based methods. Some researchers, such as Kajita et al. have employed the linear inverted pendulum (LIP) method to control bipedal robots on rugged terrains [1]. Xiaobin Xiong et al. have combined LIP with a spring-loaded inverted pendulum (SLIP) to achieve 3D walking in underdriven systems on a particular robot [2]. In 2018, Sangbae Kim et al. applied model predictive control algorithms to footed robot control for the first time [3], and they later proposed the classic MPC + WBC control framework [4], which has had a profound impact on the field.

1.2. Motivation

The execution of a squatting motion by a humanoid robot is a complex task that demands intricate whole-body coordination, involving not only the control of the torso’s position and orientation but also the precise positioning of the arms and feet. Moreover, it is crucial to account for the foot force magnitude and adhere to various dynamic constraints. This underscores the complexity of the whole-body control challenge at hand. To address this, we turn to optimization-based whole-body control algorithms.

There are two general approaches to implementing whole-body control algorithms. Hierarchical quadratic programming (HQP) offers a structured hierarchy among tasks, ensuring that tasks are executed without interference. However, this method’s requirement to solve the optimization problem iteratively demands significant computational resources. On the other hand, weighted quadratic programming (WQP) simplifies the process by requiring the optimization problem to be solved only once. Thus, this paper adopts a weight-based quadratic programming approach.

Furthermore, the MPC is often used for trajectory optimization in robot control problems. The study introduces a three-particle model predictive control [5] strategy to humanoid robotics, offering a novel simplification that enhances the accuracy of the control strategy. Conventional MPC approaches often simplify quadrupedal and bipedal robots into a single rigid body model, neglecting the presence of arms and legs, which can be an oversimplification. In contrast, the three-particle model acknowledges the presence of both arms and legs, providing a more nuanced representation. This paper leverages the squatting task as a testbed to explore the merits of the three-particle model, aiming to demonstrate its effectiveness in achieving complex motions with greater accuracy and efficiency. By integrating this approach with whole-body control, we aim to contribute to the advancement of humanoid robot control strategies that are both sophisticated and computationally feasible.

1.3. Related Works

There have already been some researchers conducting related studies in this direction. For instance, Francesco Nori and Yue Hu et al. achieved the squatting movement of a humanoid robot by employing a scaled-down version of the iCub bipedal robot along with multiple objective functions [6]. David B. Grimes et al. utilized probabilistic inference in a Bayesian network to explore the problem of a humanoid robot learning the whole-body movement of a human. Based on the HOAP-2 humanoid robot, they realized a squatting movement and a single-leg balancing movement [7]. Yuki Asano et al. designed a screw-home mechanism and applied it to the skeletonoid robot Kenshiro to accomplish continuous twisting squatting movements [8]. Sang-Ho Hyon et al. put forward an imitation learning framework and applied the proposed method to a four-link robot model, successfully realizing squatting actions [9]. David Galdeano et al. proposed a task-based zero-moment point (ZMP) planning, achieved whole-body planning on the humanoid robot HOAP-3, and carried out static balance experiments and squatting experiments under external thrust [10]. Sean Mason et al. proposed a linear quadratic regulator (LQR) for humanoid robots, making use of linearised whole-robot dynamic and contact constraints, and performed push recovery and squat experiments on the Sarcos humanoid robot [11]. Shuuji Kajita et al. implemented forward downward squatting and backward retracting squatting on the HRP-2Kai humanoid robot [12]. L. Penco et al. proposed a redirection method in the research of humanoid whole-body motion. It is a control framework based on inverse kinematics (IK) and a quadratic programming (QP) optimization solver combined with a remote-controlled iCub robot, and the squatting experiments were conducted on an iCub robot [13]. In 2011, Andrej Gams et al. modeled a real person, collected the joint trajectories of a human during squatting, and processed them for the implementation of a continuous squatting action on a bipedal robot. They used the ZMP method to ensure the stability of the robot, applying the idea of zero-space projection and making maintaining stability during the squatting process a top priority [14]. In 2022, J. Duran-Hernandez et al. used the second-order Super Twisting Sliding Mode Control (STSMC) method to achieve squatting on a low-cost bipedal robot and compared it with the proportional integral derivative (PID) method, proving that the method could achieve better trajectory tracking [15]. In 2009, Sang-Ho H et al. proposed a virtual implementation framework for a torque-controlled humanoid robot with a biological musculoskeletal system by integrating a task-space controller and a joint stiffness controller through a superposition method that tested the feasibility of the algorithm using squatting motion [16]. In 2017, Sang-Ho Hyon et al. designed a hydraulic humanoid robot TaeMu, which performed a squatting movement with a sinusoidal signal for the torso trajectory [17]. In 2016, Dingsheng Luo et al. proposed a solution to the problem of non-smooth overshooting when transforming the tasks performed by the robot, converting the problem to a machine-learning approach. The DARwIn-OP robot provided by webots was used in the simulation for the implementation of the training results, and finally, physical experiments were conducted on a PKU-HR5.1 humanoid robot to verify the feasibility of the method [18]. In 2013, Vincent Bonnet et al. proposed a method for estimating the state of a humanoid robot in the sagittal plane during squatting when the joints of the lower limbs as well as the torso are in motion, and they verified the accuracy of the estimation method using squatting motion on the HOAP-3 robot [19]. In the field of exoskeleton robotics, there is also relevant research on squatting. In 2021, Shuzhen L et al. achieved squatting on a lower-limb rehabilitation robot using a reinforcement learning method with strong robustness and anti-interference ability [20]. In 2018, A. N. Miyadaira et al. analyzed the motion during squat jumping for 3DOF articulated legs and proposed two methods to estimate the joint torque change during jumping to optimize the stability of the robot during landing [21].

I-Feng Lee et al. successfully implemented squatting movements on a single robotic leg, providing valuable insights for the study of humanoid robot dynamics [22]. Professor Zhang Liang’s team employed imitation learning algorithms to conduct a phase-time squatting test on the NAO robot [23]. Professor Chen Jianxin’s team achieved lower-body control on a humanoid robot based on gesture information, enabling the robot to perform tasks such as standing on one foot and squatting [24]. In 2022, Wenhan Cai integrated real-time kinematic programming (RKP) and WBC methods to simulate squatting motion on a bipedal robot [25]. In 2023, Pengcheng Lin proposed a method for joint trajectory planning using elastic primitives to enhance the energy storage effect during the landing phase of squatting. This involved conducting human motion capture experiments, data processing, and validating the method’s effectiveness through simulation experiments [26].

1.4. Contribution

In this paper, we mainly use the three-particle model predictive control algorithm and the whole-body control algorithm to implement the squatting action on a humanoid robot. The three-particle model predictive control algorithm is used for optimizing the rough trajectories, and the whole-body control algorithm achieves the accurate tracking of the trajectory optimized by the model predictive control and handles the corresponding constraints based on the design of the weights. Ultimately, we achieved more accurate trajectory tracking and optimized the phenomenon of large peak knee moments compared to the WBC-only approach. In addition, this model simplification allows the MPC problem to be solved within 5.5 ms per frame, in most cases, which greatly improves the computational efficiency.

The rest of the paper is organized as follows: the second part introduces the three-particle model predictive control and whole body control algorithm, the third part introduces the results and discussion, and the fourth section concludes the paper as well as provides some directions for the future.

2. Methods

2.1. Three-Particle Model Predictive Control

2.1.1. Modeling

As shown in Figure 1, the humanoid robot used in this paper has a mass of 20.5 kg and a total of 19 degrees of freedom. A single arm has 3 degrees of freedom, a single leg has 6 degrees of freedom, and the chest has 1 degree of freedom, all of which are rotary joints. Considering the characteristics of the squatting motion and the requirement for the real-time operation of the algorithm, we simplify the humanoid robot into three particles. Specifically, the two arms, two legs, and the torso are simplified into three particles in a three-dimensional space. The center of mass (CoM) of the torso is calculated from the central trunk position and the chest above; the CoM of the two legs and the two arms is calculated from the vector sum of the positions of the end joints and the hip (shoulder) joints to find the midpoints. The end position information is measured by the sensors in the simulation, and the hip (shoulder) joint position is obtained from the sensors at the torso by adding the respective biases. For example, the CoM of a single leg is given by

$${\mathit{p}}_{m,leg}={\displaystyle \frac{1}{2}}\left[\begin{array}{c}{\mathit{p}}_b+{\mathit{l}}_{hip}+{\mathit{p}}_{foot}\end{array}\right]$$

(1)

where ${\mathit{p}}_{m,leg}$ denotes the CoM position of a single leg, ${\mathit{p}}_b\in {\mathbb{R}}^{3\times 1}$ denotes the trunk position, ${\mathit{l}}_{hip}\in {\mathbb{R}}^{3\times 1}$ denotes hip offset relative to the trunk position, and ${\mathit{p}}_{foot}\in {\mathbb{R}}^{3\times 1}$ denotes the foot position. The CoM of the torso is obtained as follows:

$${\mathit{p}}_{m,torso}={\displaystyle \frac{m_{b1}{\mathit{p}}_{m,b1}+m_{b2}{\mathit{p}}_{m,b2}}{m_{b1}+m_{b2}}}$$

(2)

where $m_{b1}$ denotes the central trunk mass, $m_{b2}$ denotes the chest mass, ${\mathit{p}}_{m,b1}$ denotes the CoM of thr trunk, and ${\mathit{p}}_{m,b2}$ denotes the CoM of the chest.

The final overall simplified CoM is obtained as follows:

$$\begin{array}{c}{\mathit{p}}_{CoM}={\displaystyle \frac{{\displaystyle m_{leg}{\mathit{p}}_{m,leg}+m_{arm}{\mathit{p}}_{m,arm}+m_b{\mathit{p}}_{m,torso}}}{{\displaystyle m}}}\hfill \end{array}$$

(3)

where ${\mathit{p}}_{CoM}$ denotes the CoM position and $m=m_b+m_{leg}+m_{arm}$ is the overall mass. The positions and velocities of the arm ends, leg ends, and torso are selected as the system’s state variables, with their respective accelerations serving as control inputs to establish the discrete state-space equations as follows:

$${\mathit{x}}_{k+1}=\overline{\mathit{A}}{\mathit{x}}_k+\overline{\mathit{B}}{\mathit{u}}_k$$

(4)

where ${\mathit{x}}_k={\left[\begin{array}{cccccc}{\mathit{p}}_b^T& {\mathit{p}}_{lh}^T& {\mathit{p}}_{rh}^T& {\dot{\mathit{p}}}_b^T& {\dot{\mathit{p}}}_{lh}^T& {\dot{\mathit{p}}}_{rh}^T\end{array}\right]}^T$, and it includes the position and velocity of the torso, the end of the left hand, and the end of the right arm. ${\mathit{u}}_k={\left[\begin{array}{ccc}{\ddot{\mathit{p}}}_b^T& {\ddot{\mathit{p}}}_{lh}^T& {\ddot{\mathit{p}}}_{rh}^T\end{array}\right]}^T$, and it contains the acceleration of the torso, the end of the left hand, and the end of the right arm. $\overline{\mathit{A}}$ is the state matrix and $\overline{\mathit{B}}$ is the control matrix. In addition, the states of the simplified CoM can be deduced by combining Equations (1)–(3) as follows:

$${\mathit{p}}_{CoM,k}=\mathit{C}{\mathit{x}}_k+\mathit{D}$$

(5)

$${\dot{\mathit{p}}}_{CoM,k}=\mathit{E}{\mathit{x}}_k$$

(6)

after the N-step prediction, the compact form of the state equation is as follows:

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

(7)

among them,

$${\mathit{A}}_{qp}=\left[\begin{array}{c}\overline{\mathit{A}}\\ {\overline{\mathit{A}}}^2\\ ⋮\\ {\overline{\mathit{A}}}^N\end{array}\right]\in {\mathbb{R}}^{18N\times 18}$$

(8)

$${\mathit{B}}_{qp}=\left[\begin{array}{ccccc}\overline{\mathit{B}}& \mathbf{0}& \cdots & \mathbf{0}& \\ \overline{\mathit{A}}\overline{\mathit{B}}& \overline{\mathit{B}}& \cdots & \mathbf{0}\\ ⋮& ⋮& \ddots & ⋮& \\ {\overline{\mathit{A}}}^{N-1}\overline{\mathit{B}}& {\overline{\mathit{B}}}^{N-2}\overline{\mathit{B}}& \cdots & \overline{\mathit{B}}\end{array}\right]\in {\mathbb{R}}^{18N\times 9N}$$

(9)

${\mathit{x}}_0$ denotes the current state of the system that needs to be acquired each time the MPC problem is computed, and $\mathit{U}$ denotes the optimal control sequence that needs to be computed at the current torque. Correspondingly, the states of the simplified CoM are represented as follows:

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

(10)

$${\dot{\mathit{P}}}_{CoM}=\overline{\mathit{E}}\mathit{X}$$

(11)

Correspondingly,

$$\begin{array}{c}\overline{\mathit{C}}=\left[\begin{array}{ccc}\mathit{C}& & \\ & \ddots & \\ & & \mathit{C}\end{array}\right]\in {\mathbb{R}}^{3N\times 18N}\\ \overline{\mathit{D}}=\left[\begin{array}{c}\mathit{D}\\ ⋮\\ \mathit{D}\end{array}\right]\in {\mathbb{R}}^{3N\times 1}\\ \overline{\mathit{E}}=\left[\begin{array}{ccc}\mathit{E}& & \\ & \ddots & \\ & & \mathit{E}\end{array}\right]\in {\mathbb{R}}^{3N\times 18N}\end{array}$$

(12)

where the matrices $\mathit{C}$ and $\mathit{E}$ and the vector $\mathit{D}$ are used to calculate the state quantities of the simplified center of mass.

2.1.2. Cost Function

The significance of establishing the cost function is to ensure that the trajectory following error during squatting is as small as possible and the amount of control required is not too large, so the following cost function is established:

$$\begin{array}{c}J=\underset{\mathit{U}}{min}{∥{\mathit{P}}_{CoM}-{\mathit{P}}_{CoM}^{ref}∥}_{\overline{\mathit{Q}}}+{∥{\dot{\mathit{P}}}_{CoM}-{\dot{\mathit{P}}}_{CoM}^{ref}∥}_{\overline{\mathit{R}}}+{∥{\overline{\mathit{P}}}_{arm}-{\overline{\mathit{P}}}_{arm}^{ref}∥}_{\overline{\mathit{S}}}+{∥\mathit{U}∥}_{\overline{\mathit{T}}}\hfill \end{array}$$

(13)

The position and velocity tracking terms of the simplified CoM position are ${∥{\mathit{P}}_{CoM}-{\mathit{P}}_{CoM}^{ref}∥}_{\overline{\mathit{Q}}}$ and ${∥{\dot{\mathit{P}}}_{CoM}-{\dot{\mathit{P}}}_{CoM}^{ref}∥}_{\overline{\mathit{R}}}$, the position and velocity tracking term of the end of the arm is ${∥{\overline{\mathit{P}}}_{arm}-{\overline{\mathit{P}}}_{arm}^{ref}∥}_{\overline{\mathit{S}}}$, and the penalty term of the control input ${∥\mathit{U}∥}_{\overline{\mathit{T}}}$ is included in Equation (13). In Equation (13), the weight of the CoM position tracking term $\overline{\mathit{Q}}\in {\mathbb{R}}^{3N\times 3N}$, the weight of CoM velocity tracking term $\overline{\mathit{R}}\in {\mathbb{R}}^{3N\times 3N}$, the weight of end-of-arm position and velocity tracking term $\overline{\mathit{S}}\in {\mathbb{R}}^{12N\times 12N}$, and the weight of control penalty term $\overline{\mathit{T}}\in {\mathbb{R}}^{3N\times 3N}$, respectively.

The corresponding weighting factors are shown in

Table 1. Weight matrix for each term in TP-MPC.

Weight Term	Weight Matrix
${\mathit{W}}_{CoMpos}$	diag($7.5\times {10}^2$, $1.5\times {10}^6$, $3.2\times {10}^2$)
${\mathit{W}}_{CoMvel}$	diag(7.0, $1.0\times {10}^1$, 2.5)
${\mathit{W}}_{leftarm}$	diag($1.0\times {10}^3$, $1.0\times {10}^1$, $1.0\times {10}^2$, $1.0\times {10}^3$, $1.0\times {10}^1$, $1.0\times {10}^2$)
${\mathit{W}}_{rightarm}$	diag(1.0, $1.0\times {10}^1$, 2.5, 1.0, $1.0\times {10}^1$, 2.5)
${\mathit{W}}_u$	diag($1.0\times {10}^{-3}$, 1.0, $1.0\times {10}^{-5}$, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0)

.

Due to the excessive length of the third term, we split it into separate representations for the left and right arms.

The cost function is further converted to a QP form by combining Equations (7) and (13) as follows:

$$\underset{\mathit{U}}{min}{\displaystyle \frac{1}{2}}{\mathit{U}}^{\mathrm{T}}\mathit{H}\mathit{U}+{\mathit{U}}^{\mathrm{T}}\mathit{g}$$

(14)

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

(15)

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

(16)

where $\mathit{H},\mathit{g}$ denotes the sum of the coefficients of the CoM position and velocity tracking, the arm end position and velocity tracking, and the input penalty term task. For example, ${\mathit{H}}_1$, ${\mathit{g}}_1$, ${\mathit{H}}_2$, ${\mathit{g}}_2$ are the task matrices and vectors obtained when deriving the CoM position and velocity following term into the standard QP form.

$${\mathit{H}}_1=2{\mathit{B}}_{qp}^T\overline{{\mathit{C}}^T}\overline{\mathit{Q}}\overline{\mathit{C}}{\mathit{B}}_{qp}$$

(17)

$${\mathit{g}}_1=2{\mathit{B}}_{qp}^T\overline{{\mathit{C}}^T}\overline{\mathit{Q}}(-{\mathit{p}}_{CoM,pos}^{ref}+\overline{\mathit{C}}{\mathit{A}}_{qp}{\mathit{x}}_0+\overline{\mathit{D}})$$

(18)

$${\mathit{H}}_2=2{\mathit{B}}_{qp}^T\overline{{\mathit{E}}^T}\overline{\mathit{R}}\overline{\mathit{E}}{\mathit{B}}_{qp}$$

(19)

$${\mathit{g}}_2=2{\mathit{B}}_{qp}^T\overline{{\mathit{E}}^T}\overline{\mathit{R}}(-{\mathit{p}}_{CoM,vel}^{ref}+\overline{\mathit{E}}{\mathit{A}}_{qp}{\mathit{x}}_0)$$

(20)

where ${\mathit{p}}_{CoM,pos}^{ref}$, ${\mathit{p}}_{CoM,vel}^{ref}$ are the reference trajectories of the simplified CoM obtained by rough planning.

This is finally solved using the qpOASES library, and the first frame of data obtained from the optimization solution is used as the reference input for the WBC, The overall control block diagram is shown in Figure 2.

2.2. Whole-Body Control

2.2.1. Modeling

The humanoid robot is generally modeled as a full-dynamics model containing a floating base when the whole-body control method is used, in which case, the generalized joints of the robot are expressed as follows:

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

(21)

where ${\mathit{q}}_f\in {\mathbb{R}}^6$ is used to describe the position and attitude of the robot torso and the attitude is generally expressed in Euler angles or quaternion; ${\mathit{q}}_j\in {\mathbb{R}}^{n_j}$ is used to describe the joint angles of all real joints, and

$$n_q=n_j+6$$

(22)

The full dynamic equation is expressed as follows:

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

(23)

where $\dot{\mathit{q}}$ is the generalized joint velocity; $\ddot{\mathit{q}}$ is the generalized joint acceleration; $\mathit{M}\left(\mathit{q}\right)\in {\mathbb{R}}^{n_j\times n_j}$ is the mass inertia matrix dependent on the $\mathit{q}$, $\mathit{h}(\mathit{q},\dot{\mathit{q}})$ and contains the Coriolis, centrifugal, and gravity terms; ${\tau }_{\mathit{j}}$ is the real joint driving torque; ${\mathit{J}}_c\left(\mathit{q}\right)\in {\mathbb{R}}^{12\times n_q}$ is the contact Jacobian matrix; and ${\mathit{\omega }}_c={\left[\begin{array}{cc}{\mathit{\omega }}_{c1}^T& {\mathit{\omega }}_{c2}^T\end{array}\right]}^T$ represents the contact wrenches, where ${\mathit{\omega }}_{c1}^T={\left[\begin{array}{cc}{\mathit{f}}_1^T& {\mathit{\tau }}_1^T\end{array}\right]}^T\in {\mathbb{R}}^{6\times 1}$ denotes the end-foot force and torque of the contact foot.

2.2.2. Whole-Body Control Formulation

There are various forms of implementing the full-body control algorithms, and building a quadratic programming problem containing weights is one of them. Its standard form is as follows:

$$\begin{array}{c}\underset{\mathit{\chi }}{argmin}{\displaystyle \sum _{i=1}^{n_{task}}}{∥{\mathit{W}}_i\left({\mathit{A}}_i\mathit{\chi }-{\mathit{b}}_i\right)∥}_2^2\hfill \\ s.t.\mathit{l}{\mathit{b}}_k\le {\mathit{C}}_k\mathit{\chi }\le \mathit{u}{\mathit{b}}_k(k=1,2,...k_{constraint})\hfill \end{array}$$

(24)

where ${\mathit{A}}_i{\mathit{B}}_i$ denotes the task matrix and task vector of the corresponding task; ${\mathit{W}}_i$ is the weight of the corresponding task; $\mathit{\chi }={\left[\begin{array}{cc}{\ddot{\mathit{q}}}^T& {\mathit{\omega }}^T\end{array}\right]}^T$ is the optimization variable, including the generalized joint acceleration and wrenches; ${\mathit{C}}_k$ is the constraint matrix of the corresponding constraint; and $\mathit{l}{\mathit{b}}_k\mathit{u}{\mathit{b}}_k$ is the lower limit and upper limit of the corresponding constraint. Finally, it is converted into an optimization problem to solve the optimization variables, and the joint torque ${\mathit{\tau }}_{\mathit{j}}$ can be obtained by combining it with the full dynamics Equation (17). The robot needs to coordinate the whole-body joint movement to achieve the squatting action, so the following subtasks and constraints are set as shown in the

Table 2. Tasks, Constraints for squat motion.

Tasks	Constraints
Torso trajectory tracking task	Floating-base dynamic
Arm trajectory tracking task	Friction cone
Feet trajectory tracking task	Joint torque
Wrench task	Joint velocity
Chest joint immobilization task	Joint power

.

The weighting factors for each task are shown in

Table 3. Weight matrix for each subtask in WBC.

Weight Term	Weight Matrix
${\mathit{W}}_{torso}$	diag(1750, 450, 300, 70, 150, 50)
${\mathit{W}}_{arm}$	diag( 300, 300, 300, 200, 200, 200)
${\mathit{W}}_{feet}$	diag(900, 900, 900, 190, 140, 150)
${\mathit{W}}_{wrench}$	diag(1, 1, 1, 2, 2, 2)
${\mathit{W}}_{chestjoint}$	diag(70)

.

2.2.3. Tasks

Torso Trajectory Tracking Task

It is important to achieve the accurate trajectory tracking of the torso during the squat, so the torso task was set up as follows:

$$\left[\begin{array}{cc}{\mathit{I}}_{6\times 6}& {\mathbf{0}}_{6\times (n_j+12)}\end{array}\right]\mathit{\chi }-\left[\begin{array}{c}{\ddot{\mathit{p}}}_{torso}^{des}\\ \ddot{{\mathbf{\Theta }}_{torso}^{des}}\end{array}\right]$$

(25)

where $\mathit{\chi }$ is the variable to be optimized; ${\mathit{I}}_{6\times 6}$ is a unit diagonal matrix used to select the optimization variables required for this task; and ${\ddot{\mathit{p}}}_{torso}^{des},{\ddot{\mathbf{\Theta }}}_{torso}^{des}$ are the desired torso linear acceleration and angular acceleration, obtained from the PD controller as follows:

$$\begin{array}{c}{\ddot{\mathit{x}}}_{torso}^{cmd}={\ddot{\mathit{x}}}_{torso}^{des}+k_p\left({\mathit{x}}_{torso}^{des}-{\mathit{x}}_{torso}^{fb}\right)+k_d\left({\dot{\mathit{x}}}_{torso}^{des}-{\dot{\mathit{x}}}_{torso}^{fb}\right)\hfill \end{array}$$

(26)

Here ${\mathit{x}}_{torso}^{des},{\dot{\mathit{x}}}_{torso}^{des},{\ddot{\mathit{x}}}_{torso}^{des}$ are obtained by TP-MPC.

Arm Trajectory Tracking Task

The arms need to follow the torso during the squat, so we set up the arm task as follows:

$$\left[\begin{array}{cc}{\mathit{J}}_{hand}\left({\mathit{q}}^{fb}\right)& {\mathbf{0}}_{6\times 12}\end{array}\right]\mathit{\chi }-\left[\begin{array}{cc}{\ddot{\mathit{x}}}_{hand}^{des}& -{\dot{\mathit{J}}}_{hand}\left({\mathit{q}}^{fb}\right){\dot{\mathit{q}}}^{fb}\end{array}\right]$$

(27)

where ${\ddot{x}}_{hand}$ contains $\begin{array}{cc}{\ddot{\mathit{p}}}_{hand}^{des}& {\ddot{\mathbf{\Theta }}}_{hand}^{des}\end{array}$, which represent the desired linear and angular accelerations of the arm; ${\mathit{q}}^{fb}$ and ${\dot{\mathit{q}}}^{fb}$ are the generalized joint position and velocity feedback, respectively; and ${\mathit{J}}_{hand}\in {\mathbb{R}}^{6\times n_q}$ is the arm Jacobian matrix.

Foot Trajectory Tracking Task

The feet need to remain stationary relative to the ground during the squat, so we set up the feet task as follows:

$$\left[\begin{array}{cc}{\mathit{J}}_{foot}\left(q^{fb}\right)& {\mathbf{0}}_{6\times 12}\end{array}\right]\mathit{\chi }-\left[\begin{array}{cc}{\ddot{\mathit{x}}}_{foot}^{des}& -{\dot{\mathit{J}}}_{foot}\left({\mathit{q}}^{fb}\right){\dot{\mathit{q}}}^{fb}\end{array}\right]$$

(28)

where ${\ddot{\mathit{x}}}_{foot}$ contains $\begin{array}{cc}{\ddot{\mathit{p}}}_{foot}^{des}& {\ddot{\mathbf{\Theta }}}_{foot}^{des}\end{array}$, which are the desired linear and angular acceleration of the foot; ${\mathit{q}}^{fb}$ and ${\dot{\mathit{q}}}^{fb}$ are the generalized joint position and velocity feedback, respectively; and ${\mathit{J}}_{foot}\in {\mathbb{R}}^{6\times n_q}$ is the foot Jacobian matrix.

Chest Trajectory Tracking Task

The squatting process does not require the chest joint, so this joint is fixed to the zero position and the chest joint task is set as follows:

$$\left[\begin{array}{ccc}{\mathbf{0}}_{1\times 6}& \mathit{I}& {\mathbf{0}}_{1\times 30}\end{array}\right]\mathit{\chi }-\left[\begin{array}{c}\mathbf{0}\end{array}\right]$$

(29)

The chest joint expectation is set to zero in this task.

Wrench Task

To ensure that contact forces are not excessive during the squat, the task is formulated as follows:

$$\left[\begin{array}{cc}{\mathbf{0}}_{12\times (n_j+6)}& {\mathit{I}}_{12\times 12}\end{array}\right]\mathit{\chi }-\left[\begin{array}{c}{\mathbf{0}}_{12\times 1}\end{array}\right]$$

(30)

2.2.4. Constraints

In the process of solving the optimization problem, it is necessary to set the appropriate constraints; otherwise, the results will be solved, which cannot be achieved in reality, so the following constraints are set:

Floating Base Dynamics Constraint

In the full dynamics model, we often describe the 6-dimensional motion possessed by the floating base as the result of being driven by a virtual joint containing 6 DoF, so we need to ensure that the actual driving force of these 6 DoF is zero when solving the optimization, and we therefore set this constraint as follows:

$$\left[{\mathit{S}}_f\mathit{M}\left({\mathit{q}}^{fb}\right)-{\mathit{S}}_f{\mathit{J}}_{foot}^T\left({\mathit{q}}^{fb}\right)\right]\mathit{\chi }=-{\mathit{S}}_f\mathit{h}\left({\mathit{q}}^{fb},{\dot{\mathit{q}}}^{fb}\right)$$

(31)

where ${\mathit{S}}_f=\left[\begin{array}{c}{\mathit{I}}_{6\times 6}{\mathbf{0}}_{6\times n_j}\end{array}\right]$ is the floating base selection matrix used to select the virtual joints mentioned earlier.

Contact Force Constraint

In order to ensure that the robot foot end does not slip, it should be ensured that the foot end force is within the cone of friction, and also that the foot sole force in the vertical direction should not be too large, so the constraint is formulated as follows:

$$\begin{array}{c}\left|{\mathit{f}}_x\right|\le \mu {\mathit{f}}_z\hfill \\ \left|{\mathit{f}}_y\right|\le \mu {\mathit{f}}_z\hfill \\ 0\le {\mathit{f}}_z\le {\mathit{f}}_{zmax}\hfill \end{array}$$

(32)

where $\mu$ is the coefficient of friction and $f_z$ is the maximum contact force in the z direction. We describe Equation (32) as a form of Equation (24) as follows:

$${\mathit{lb}}_f\le {\mathit{C}}_f\mathit{\chi }\le {\mathit{ub}}_f$$

(33)

The corresponding constraint matrix with upper and lower bounds is denoted as follows:

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

(34)

$$\begin{array}{c}\mathit{l}{\mathit{b}}_f={\mathbf{0}}_{10\times 1}\hfill \\ \mathit{u}{\mathit{b}}_f={\infty }_{10\times 1}\hfill \end{array}$$

(35)

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

(36)

Joint Torque Constraint

In order to ensure the availability of the joint moments, it is necessary to limit the joint moments obtained from the optimization solution to a certain range. We morph Equation (23) into the form of Equation (24), from which we can obtain the correlation matrix for this constraint as follows:

$${\mathit{\tau }}_j-{\mathit{S}}_j\mathit{h}({\mathit{q}}^{fb},{\dot{\mathit{q}}}^{fb})={\mathit{S}}_j\left[\mathit{M}\left({\mathit{q}}^{fb}\right)-{{\mathit{J}}_{foot}}^T\left({\mathit{q}}^{fb}\right)\right]\mathit{\chi }$$

(37)

$${\mathit{lb}}_{\tau }\le {\mathit{C}}_{\tau }\le {\mathit{ub}}_{\tau }$$

(38)

$$C_{\tau }={\mathit{S}}_j\left[\mathit{M}\left({\mathit{q}}^{fb}\right)-{{\mathit{J}}_{foot}}^T\left({\mathit{q}}^{fb}\right)\right]$$

(39)

$$\mathit{l}{\mathit{b}}_{\tau }={\mathit{\tau }}_{min}-{\mathit{S}}_j\mathit{h}({\mathit{q}}^{fb},{\dot{\mathit{q}}}^{fb})$$

(40)

$$\mathit{u}{\mathit{b}}_{\tau }={\mathit{\tau }}_{max}-{\mathit{S}}_j\mathit{h}({\mathit{q}}^{fb},{\dot{\mathit{q}}}^{fb})$$

(41)

where ${\mathit{S}}_j=\left[\begin{array}{c}{\mathbf{0}}_{n_j\times 6}{\mathit{I}}_{n_j\times n_j}\end{array}\right]$ is the joint torque selection matrix used to select the drive joints in the optimized variables. ${\mathit{\tau }}_{max}$ and ${\mathit{\tau }}_{min}$ are the actual maximum and minimum joint torque values.

Joint Velocity Constraint

In order to ensure the effectiveness of the joint rotational speed, it is necessary to limit the joint rotational speed to a certain range. We morph Equation (23) into the form of Equation (24), from which we can obtain the correlation matrix for this constraint as follows:

$$\dot{\mathit{q}}\left(t+\Delta t\right)=\dot{\mathit{q}}\left(t\right)+\Delta t\ddot{\mathit{q}}\left(t\right)$$

(42)

$${\mathit{lb}}_{{\dot{q}}_j}\le {\mathit{C}}_{{\dot{q}}_j}\le {\mathit{ub}}_{{\dot{q}}_j}$$

(43)

$${\mathit{C}}_{{\dot{\mathit{q}}}_j}=\left[{\mathbf{0}}_{n_j\times 6}\Delta t{\mathit{I}}_{n_j\times n_j}{\mathbf{0}}_{n_j\times 12}\right]$$

(44)

$$\mathit{l}{\mathit{b}}_{{\dot{q}}_j}={\dot{\mathit{q}}}_{j_{min}}-{\dot{\mathit{q}}}_j^{fb}$$

(45)

$$\mathit{u}{\mathit{b}}_{{\dot{\mathit{q}}}_j}={\dot{\mathit{q}}}_{j_{max}}-{\dot{\mathit{q}}}_j^{fb}$$

(46)

Joint Power Constraint

In order to ensure the long and stable operation of the joint, it is necessary to keep the operating power within the normal range. We morph Equation (23) into the form of Equation (24), from which we can obtain the correlation matrix for this constraint as follows:

$${\mathit{\tau }}_j-{\mathit{S}}_j\mathit{h}({\mathit{q}}^{fb},{\dot{\mathit{q}}}^{fb})={\mathit{S}}_j\left[\mathit{M}\left({\mathit{q}}^{fb}\right)-{{\mathit{J}}_{foot}}^T\left({\mathit{q}}^{fb}\right)\right]\mathit{\chi }$$

(47)

$${\mathit{lb}}_p\le {\mathit{C}}_p\le {\mathit{ub}}_p$$

(48)

$${\mathit{C}}_p=diag\left({\dot{\mathit{q}}}^{fb}\right){\mathit{S}}_j\left[\mathit{M}\left(q^{fb}\right)-{{\mathit{J}}_{foot}}^T\left({\mathit{q}}^{fb}\right)\right]\mathit{\chi }$$

(49)

$$\begin{array}{c}\mathit{l}{\mathit{b}}_P=-\infty \hfill \end{array}$$

(50)

$$\mathit{u}{\mathit{b}}_P={\mathit{P}}_{jmax}-diag\left({\dot{\mathit{q}}}^{fb}\right){\mathit{S}}_{j}h({\mathit{q}}^{fb},{\dot{\mathit{q}}}^{fb})$$

(51)

where $P_{jmax}$ is the joint maximum power.

2.3. Experimental Design for Simulation

The humanoid robot used is shown in Figure 3. The simulation experiments were conducted using the open source simulation software Webots, it is an open source and multi-platform desktop application for simulating robots. It provides a complete development environment to model, program, and simulate robots. The QP problem was solved using the qpOASES library [27], it is an open source C++ library specializing in solving small- and medium-sized convex quadratic programming problems. Favored for its efficient performance and guaranteed numerical stability, qpOASES is especially suited for areas such as robot motion planning, machine learning, and control. The dynamics and kinematics were resolved using the rigid body dynamics library (RBDL), which is a high-performance C++ library for rigid body dynamics modeling. It also includes some important rigid body dynamics algorithms, such as ABA, RNEA, CRBA, etc, which enable efficient operations on joint space inertia matrices [28].

For the determination of the trajectory of the humanoid robot when squatting, we conducted motion capture experiments. We used the Nokov optical 3D motion capture system, which can capture the reflective markers placed on the target to accurately capture the 3-dimensional spatial position. Through advanced algorithms used for processing and computing, the system can get the 3-dimensional spatial coordinates of the reflective markers in different time units of measurement (X, Y, Z); it can also be set up on the target object’s rigid body. The system can also be set up on the rigid body of the target object, and through the professional analysis software it can further process and calculate the data, obtaining the three-dimensional data of the target object such as the precise position and attitude. We obtained the trajectory of the X, Y, and Z directions of the real person when squatting through the motion capture equipment, as shown in Figure 4. Through the motion capture data, it can be observed that the torso not only reciprocates in the z-direction but also has a brief reciprocal displacement in the x-direction, and the trajectory of the torso is similar to the triangular wave in the process of squatting. Moreover, the trajectory is similar to the square wave if there is a short pause in the squatting of the two limiting positions, so we used the periodic triangular and square wave trajectories for the squatting test. In the simulation inspired by the squatting trajectories obtained from human motion capture, we first describe the trajectories in our experiments as square and triangular waves. Then, the trajectory is optimized using the three-particle model predictive control, and the optimized trajectory is given for the whole-body control to follow. In some related studies in the past, researchers primarily used the WBC method alone to realize some functions, such as push-recovery [29]. Therefore, we designed a comparative experiment to track the square and triangular wave trajectories using only the whole-body control method. The trajectory period in all experiments was set to 2 s and the squat height was 0.2 m.

3. Results and Discussion

3.1. Square Wave

Two sets of tests are set up in the simulation, and the reference trajectories are both square wave trajectories as well, as shown in Figure 5. Here, the blue dashed line is the reference trajectory, the red solid line is the trajectory of the squatting motion realized by the WBC-WQP control, and the yellow solid line is the trajectory curve obtained after adding the TP-MPC.

As can be observed in Figure 5, the tracking effect of the WBC-only model has an obvious lag effect and a large tracking error, which makes the tracking effect poor overall. However, the tracking effect finally achieved by adding the TP-MPC is better than that of the WBC-only algorithm for the WBC algorithm, which only considers the current moment. Alternatively, the MPC observes the reference state in the future segment of the time domain and thus optimizes the coarsely planned trajectory into a feasible trajectory that allows WBC to follow the trajectory completely.

Furthermore, during the squatting process, the knee joint plays a crucial role; as shown in Figure 6, it can be observed that a large torque spike occurs when only the WBC algorithm is used, which is extremely bad for joint motors. However, the spikes in the knee joint moments are obviously significantly weakened when the TP-MPC is added. The reason may be that when the reference trajectory is a square wave, there is a step between the position and the velocity, which leads to the existence of obvious spikes in the calculated moments, but when TP-MPC is added, the rough trajectory is optimised to a smoother trajectory, and the final optimised knee moments for tracking the smoother trajectory no longer have large spikes.

The real-time nature of the algorithms also needs to be guaranteed. As shown in Figure 7, the algorithm is able to compute within 5.5 ms in most cases and within 10 ms overall, which has a high computational efficiency. This high computational efficiency may be due to the simplification of the three-particle model used, as mentioned before.

Finally, we calculated the trajectory tracking error in each direction with Equation (52); we calculated the mean square error throughout the simulation. The trajectory tracking errors of the two algorithms when the reference trajectory is a square wave are shown in

Table 4. Tracking error at square wave.

Item	TP-MPC + WBC	WBC
Torso X Pos (${\mathrm{m}}^2$ · ${10}^{-5}$)	0.98	4.6
Torso Y Pos (${\mathrm{m}}^2$ · ${10}^{-7}$)	0.46	4.5
Torso Z Pos (${\mathrm{m}}^2$ · ${10}^{-3}$)	1.1	5.8

.

$$\mathrm{MSE}={\displaystyle \frac{1}{\mathit{N}}}\sum _{i=1}^{\mathit{N}}{\left({\mathit{y}}_i-{\stackrel{⌢}{\mathit{y}}}_i\right)}^2$$

(52)

where $\mathit{N}$ is the total amount of data, ${\mathit{y}}_i$ is the reference value, and ${\stackrel{⌢}{\mathit{y}}}_i$ is the feedback value. From Table 4, it can be observed that the overall tracking error is small, compared to the control framework with TP-MPC + WBC, which is more capable of achieving accurate trajectory tracking.

3.2. Triangle Wave

When the reference trajectory is a triangle wave, the tracking effect is as shown in Figure 8; the period of the reference trajectory is 2 s and the squatting height is 0.2 m, which is consistent with the square wave, and the same phenomenon can be observed from the local zoomed-in figure. Only the WBC realizes the squatting with an obvious lag phenomenon, while being far away from the reference trajectory, and the obvious tracking error contrast can be observed, especially in the y-direction.

As shown in Figure 8, when TP-MPC is added, the trajectory tracking accuracies in the x, y, and z directions are all improved due to the fact that TP-MPC is able to take into account the reference trajectories in the future time domain in advance.

The knee torque when the reference trajectory is a triangular wave is shown in Figure 9, and it can be observed that the phenomenon of large torque peaks has been somewhat reduced, which is also a result of the triangular wave being optimized to be relatively smooth.

When the reference trajectory is a triangular wave, the computational time of the MPC also mostly stays within 5.5 ms. It also shows high computational efficiency, as shown in Figure 10.

Finally, the trajectory tracking errors of the two algorithms when the reference trajectory is a triangle wave are shown in

Table 5. Tracking error with a triangle wave trajectory.

Item	TP-MPC + WBC	WBC
Torso X Pos (${\mathrm{m}}^2$ · ${10}^{-6}$)	1.1	3.7
Torso Y Pos (${\mathrm{m}}^2$ · ${10}^{-7}$)	0.1	6.2
Torso Z Pos (${\mathrm{m}}^2$ · ${10}^{-4}$)	1.2	3.2

. From the data, it can be seen that the squatting motion implemented by the TP-MPC + WBC framework has a smaller tracking error compared to the WBC alone.

4. Conclusions

In this paper, the TP-MPC is applied to a humanoid robot and combined with the WBC algorithm to achieve continuous squatting movements. TP-MPC optimizes the rough planning trajectory into a smoother reference path, while WBC strictly follows the optimized reference trajectory. Additionally, comparison experiments were conducted between the combination of TP-MPC and WBC and WBC alone to achieve continuous squatting, using square wave and triangle wave reference trajectories. Both sets of experiments confirmed that the trajectory tracking effect achieved by combining this method (TP-MPC + WBC) was superior to that of WBC alone. The phenomenon of larger spikes in knee torque during squatting, which occurred when only using WBC, was also optimized when the TP-MPC algorithm was included. Furthermore, the algorithm is computationally efficient, with the overall calculation completed within 10 ms. However, TP-MPC has some limitations. The algorithm simplifies the humanoid robot into a particle model and only considers kinematics, which means it cannot handle the robot’s pose information and has poor resistance to external dynamic disturbances. In the future, it may be necessary to continue improving the algorithm and attempt to incorporate dynamics to realize a more optimal control strategy.

Furthermore, reinforcement learning (RL) is gaining increasing popularity in the field of robot control. Consequently, employing RL methods could be considered for achieving squatting movements in humanoid robots and to compare the experimental outcomes with those of traditional control methods, followed by an analysis.