An MPC-DCM Control Method for a Forward-Bending Biped Robot Based on Force and Moment Control

Abstract

For a forward-bending biped robot with 10 degrees of freedom on its legs, a new control framework of MPC-DCM based on force and moment is proposed in this paper. Specifically, the Diverging Component of Motion (DCM) is a stability criterion for biped robots based on linear inverted pendulum, and Model Predictive Control (MPC) is an optimization solution strategy using rolling optimization. In this paper, DCM theory is applied to the state transition matrix of the system, combined with simplified rigid body dynamics, the mathematical description of the biped robot system is established, the classical MPC method is used to optimize the control input, and DCM constraints are added to the constraints of MPC, making the real-time DCM approximate to a straight line in the walking single gait. At the same time, the linear angle and friction cone constraints are considered to enhance the stability of the robot during walking. In this paper, MATLAB/Simulink is used to simulate the robot. Under the control of this algorithm, the robot can reach a walking speed of 0.75 m/s and has a certain anti-disturbance ability and ground adaptability. In this paper, the Model-H16 robot is used to deploy the physical algorithm, and the linear walking and obstacle walking of the physical robot are realized.

1. Introduction

It is well known that the biped robot is a relatively complex model in the field of control. In the process of walking, it is necessary to maintain the static or dynamic balance of the system on the basis of considering the synchronous operation of multiple motors. Among them, static walking requires that the projection of the center of mass is always inside the support polygon to ensure balance at any time, but high balance also limits its walking speed [1]. The walking speed of Honda’s ASIMO robot [2] can only reach 0.44 m/s, representing a high level of static walking research. Dynamic walking relaxes the restriction of the projection position of the center of mass and uses the horizontal component of the ground reaction force to push the robot to move in the horizontal plane, which is similar to the running behavior of humans. The typical dynamic walking model is the Linear Inverted Pendulum Model (LIPM).

Due to the high-order, multi-variable, and strongly coupled characteristics of the biped robot model, it is extremely difficult to establish the model and the direct control is more complex. Dominguez [3] proposed the Linear Inverted Pendulum Model (LIPM) to establish the simplified dynamics of the biped robot. Many scholars have used LIPM in the field of biped robots for research. Caron [4] considered the change in the height of the centroid on the basis of the traditional LIPM, analyzed the feasible region of the centroid in the case of terrain changes, and carried out the planning of the centroid trajectory. Guan [5] proposed a virtual mass ellipsoid inverted pendulum model with varying angular momentum and height of the center of mass, and Li [6] applied LIPM to 3D complex environments to realize complex actions such as stair climbing.

DCM is also developed on the basis of LIPM. Pratt [7] first considered the problem of when and where the robot should land and proposed the concept of Capture Point (CP), which means that the robot can stop completely at the CP. Subsequent scholars have also made complementary developments to Pratt’s theory. Hof proposed the concept of extrapolated centroid similar to the CP point of Pratt’s theory and used the extrapolated centroid to plan the walking and stability control of biped robots. Kajita [8] proposed the concept of orbital energy in his research and pointed out that the point with zero orbital energy is the point where the linear inverted pendulum system can completely stop, and the projection of the point with zero orbital energy on the ground is the Capture Point (CP) proposed by Pratt [7]. Takenaka [9] and Englsberger [10] found in their study of the linear inverted pendulum that LIPM could be decoupled in the complex frequency domain to obtain the essentially divergent part—Motion Divergent Component (DCM). After calculation, DCM was consistent with Pratt’s capture point theory.

In the research direction of DCM, Englsberger [11] proposed two real-time anti-disturbance DCM strategies: CPT (Capture Point Tracking) and CPS (Capture Point End-of-step). Young-Dae Hong began to use a DCM walking planning strategy based on an optimization method and used a particle swarm optimization algorithm to optimize the selection of capture points and centroid trajectories [12]. Han [13] proposes a novel DCM model prediction method to predict multiple footstep positions during a fixed walking cycle, enabling fast and relevant footstep selection for biped robots and facilitating fast recovery of robot velocity. Morisawa [14] established a typical 12-DOF control scheme based on DCM theory and ZMP programming and realized the real-time tracking of the desired ZMP with limiting correction. Zhang Zhao [15] introduced the rolling optimization method of MPC to complete the real-time trajectory tracking of DCM, taking into account the influence of the angular momentum of the center of mass. Hopkins [16] and Griffin [17] used the time-varying DCM strategy to study the height variation of the center of mass during the walking process of a biped robot. However, they are all tested on 12 DOF fully controlled robots, and there is still a gap in the practical application of DCM theory for 10 DOF robots with more ground adaptability but less stability.

In this paper, the DCM of 10 DOF robots is studied. Due to the underactuated nature of the 10 DOF robots themselves, the force–torque control algorithm is adopted in this paper. Based on the linear inverted pendulum, the dynamic equilibrium equation is established by the reaction force of the foot end and the center of mass, and the mathematical description of the complete 10 DOF biped robot system is established by combining DCM theory. The rolling optimization method of MPC is used to track the target trajectory, and the optimal control input within the constraints is determined and applied to the robot entity.

In this paper, based on DCM theory and the MPC algorithm, we study and verify the control effect of the force–torque control algorithm on a 10-DOF biped robot. The main contributions of this paper are as follows:

• A new 10 DOF robot control framework is established by combining DCM theory and MPC algorithm.
• The constraint of DCM is added to the constraint conditions of MPC to enhance the stability of the system.
• A physical simulation model is established based on the Model-H16 robot.
• The feasibility and anti-interference ability of the algorithm are verified on the Model-H16 robot.

2. Model of Biped Robot

This section will introduce the biped robot simulation model and physical model information used in this paper. The simulation model and the physical model use the product Model-H16 robot produced by Gaoqing Electromechanical. Figure 1a shows the complete structure of the robot, and Figure 1b shows the motor, rotation axis, link, and other information about the robot leg. A photograph of the physical robot is presented in Figure 2.

The robot used in this paper is a bipedal robot with 10 degrees of freedom, and there are mainly two kinds of motors used. One kind of small motor, model HTM4538, is used in the ankle joint alone, and HTM5046 motors are used in the other four motors of the legs. The simple parameter ranges of the motors are shown in

Table 1. Motor parameters of the Model-H16 robot.

Name of Motor	Maximum Moment of Force	Maximum Rotational Speed	Quantity
HTM5046	8 Nm	120 rpm	8
HTM4538	3 Nm	80 rpm	2

. Since this paper only considers the biped robot walking problem, the upper body structure of the Model-H16 robot is not involved.

The control structure of the system used in this paper is shown in Figure 3.

3. Dynamic Model Based on DCM

For a control system, in order to describe the system in a coded way and carry out corresponding control, it is necessary to establish a mathematical model of the system. Since the biped robot system is a complex system, considering the dynamics of all links makes the analysis extremely difficult. The common way is to simplify the complex biped robot. Since the mass of biped robot is mostly concentrated in the position of the center of mass battery, for the convenience of analysis, this paper simplifies it to LIPM without considering the mass of legs. At the same time, in order to improve the control effect of the system, DCM is introduced as the state variable, and the state transition matrix of the whole system is established.

3.1. Review of LIPM

The Linear Inverted Pendulum Model regards the biped robot as a simple structure with a center of mass and a lightweight link, ignores many variables such as leg configuration, leg mass, ankle joint torque, and simplifies the biped robot, a multi-variable, strong-coupling and high-order system, into the simplified model shown in Figure 4 (only the schematic diagram in the x-z two-dimensional coordinates is shown, and the same is true in the y-z direction). A force analysis is performed on the center of mass of the robot and Equation (1) is derived as follows:

$$\left\{\begin{array}{c}m\ddot{x}=Fsin\theta \\ m\ddot{z}=Fcos\theta -mg\end{array}\right.$$

(1)

According to the force equation of the simplified model (1), the dynamic equation of the LIPM is obtained as expressed in Equation (2):

$$\ddot{x}={\varsigma }^2\left(x-p\right)$$

(2)

where $\varsigma =\sqrt{{\displaystyle \frac{g}{z}}}$.

3.2. Review of DCM

Equation (1) shows the relationship of ZMP–COM [18]. Pratt puts forward the concept of complete static at the Capture Point, and in the control theory treatment of Equation (2), Takenaka maps the relationship of ZMP–COM to the complex frequency domain, obtaining the divergence component in its process, which is consistent with the Capture Point concept mentioned above. In terms of formulas, this is expressed as Equation (2), where ε is the motion diverging component (or Capture Point, which is referred to as the Motion Diverging Component in this paper).

$$\left\{\begin{array}{c}\dot{x}=-\varsigma \left(x-\epsilon \right)\\ \dot{\epsilon }=\varsigma \left(\epsilon -p\right)\end{array}\right.$$

(3)

The first row in Equation (3) describes the relationship between DCM and the centroid, and the second row describes the relationship between DCM and the foothold ZMP. By eliminating the ZMP variable in Equation (2), the relationship between ε and the centroid state is obtained as Equation (4).

$$\epsilon =x+{\displaystyle \frac{\dot{x}}{\varsigma }}$$

(4)

3.3. Dynamics

The traditional LIPM does not consider the influence of the foot end on the robot body, nor does it consider the torque of the foot end. In the actual robot system, because the foot end is rarely a point foot, there must be a certain foot moment. In order to achieve a better control effect and better ground adaptability, this paper will establish LIPM considering foot moment based on DCM. Inspired by the work of Li [19], the state variables of the system are selected as [centroid Euler angle, centroid position, centroid angular velocity, and motion divergence component].

The center of mass of the biped robot is basically kept upright during walking or other actions (other special cases are not considered), that is, roll and pitch are not considered in the Euler angle of the center of mass, only yaw angle is considered, and its angular velocity is obtained by the following Equations (5) and (6).

$$\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]\approx {\mathit{R}}_z\left(\psi \right)\mathit{\omega }$$

(5)

$$\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]\approx \left[\begin{array}{ccc}\cos \psi & \sin \psi & 0\\ -\sin \psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right]\mathit{\omega }$$

(6)

The linear velocity of the center of mass can be obtained according to the first row of Equation (2).

$$\dot{\mathit{x}}=-\varsigma \left(\mathit{x}-\mathit{\epsilon }\right)$$

(7)

For the calculation of angular acceleration of the center of mass, considering LIPM, a single rigid body model, the mass is only concentrated in the center of mass, so the overall inertia matrix is the center of mass inertia matrix ${\mathit{I}}_b$. Considering yaw offset of the center of mass as mentioned above, the new moment of inertia can be obtained through Equation (8).

$$\mathit{I}={\mathit{R}}_z\left(\psi \right){\mathit{I}}_b{{\mathit{R}}_z\left(\psi \right)}^T$$

(8)

Equation (9) for calculating angular acceleration can be obtained from Formula $\mathit{\tau }=\mathit{I}\mathit{\ast }\dot{\mathit{w}}$ for calculating torque of rigid body kinematics.

$$\dot{\mathit{\omega }}={\mathit{I}}^{-}\left(\left({\mathit{P}}_1-\mathit{x}\right)\times {\mathit{F}}_1+\left({\mathit{P}}_2-\mathit{x}\right)\times {\mathit{F}}_2+{\mathit{M}}_1+{\mathit{M}}_2\right)$$

(9)

In Equation (9), ${\mathit{F}}_1$ and ${\mathit{F}}_2$ represent the foot end forces of the left and right legs, respectively, and ${\mathit{M}}_1$ and ${\mathit{M}}_2$ represent the moments of the left and right legs, the derivative of the motion diverging component can be obtained by taking the derivative of Equation (3), as shown in Equation (10).

$$\dot{\mathit{\epsilon }}=\dot{\mathit{x}}+{\displaystyle \frac{\ddot{\mathit{x}}}{\varsigma }}$$

(10)

Combined with the force analysis of the center of mass, $m\left(\mathit{g}+\ddot{\mathit{x}}\right)={\mathit{F}}_1+{\mathit{F}}_2$, where $\mathit{g}$ is the gravitational acceleration, and ${\mathit{F}}_1$ and ${\mathit{F}}_2$ are the ground reaction force of the right foot and the left foot, respectively. Finally, the derivative of DCM is obtained through Equation (11).

$$\dot{\mathit{\epsilon }}=\dot{\mathit{x}}+{\displaystyle \frac{{\mathit{F}}_1+{\mathit{F}}_2-m\mathit{g}}{\varsigma m}}$$

(11)

The dynamic model of the whole system obtained from all the above analysis is shown in Equation (12).

$$\dot{\mathit{X}}=\mathit{A}\mathit{\chi }+\mathit{B}\mathit{u}+{\left[0 0 0 {\displaystyle \frac{\mathit{g}}{m}}\right]}^T$$

(12)

where $\mathit{\chi }={\left[\mathit{\theta },\mathit{x},\mathit{\omega },\mathit{\epsilon }\right]}^T$, $\mathit{u}={\left[{\mathit{F}}_1,{\mathit{F}}_2,{\mathit{M}}_{\mathbf{1}},{\mathit{M}}_{\mathbf{2}}\right]}^T$

A = $\left[\begin{array}{cccc}{\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& R_z\left(\psi \right)& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& -\varsigma {\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& \varsigma {\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}& -\varsigma {\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& \varsigma {\mathit{I}}_{3\times 3}\end{array}\right]$

B = $\left[\begin{array}{cccc}{\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}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathit{I}}^{-1}\left({\mathit{P}}_1-\mathit{x}\right)\times & {\mathit{I}}^{-1}\left({\mathit{P}}_2-\mathit{x}\right)\times & {\mathit{I}}^{-1}\mathit{L}& {\mathit{I}}^{-1}\mathit{L}\\ {\displaystyle \frac{{\mathit{I}}_{3\times 3}}{\varsigma m}}& {\displaystyle \frac{{\mathit{I}}_{3\times 3}}{\varsigma m}}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\end{array}\right]$

L = $\left[\begin{array}{cc}0& 0\\ 1& 0\\ 0& 1\end{array}\right]$

$\mathit{I}$ = ${\mathit{R}}_z\left(\psi \right){\mathit{I}}_b{\mathit{R}}_z\left(\psi \right)$

3.4. Extended State

To unify the variables and facilitate subsequent processing, we add the gravity acceleration metric $\mathit{g}$ to the state quantity, and the state quantity $\mathit{\chi }$ is expanded to ${\mathit{\chi }=[\mathit{\theta },\mathit{x},\mathit{\omega },\mathit{\epsilon },\mathit{g}]}^T$. The system matrix $\tilde{\mathit{A}}$ and input matrix $\tilde{\mathit{B}}$ of the extended state transition matrix are Equations (13) and (14).

$$\tilde{\mathit{A}}=\left[\begin{array}{ccccc}{\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& R_z\left(\psi \right)& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 1}\\ {\mathbf{0}}_{3\times 3}& -\varsigma {\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& \varsigma {\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times 1}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 1}\\ {\mathbf{0}}_{3\times 3}& -\varsigma {\mathit{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& \varsigma {\mathit{I}}_{3\times 3}& {\displaystyle \frac{{\mathit{I}}_{3\times 1}}{m}}\\ {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 3}\end{array}\right]$$

(13)

$$\tilde{\mathit{B}}=\left[\begin{array}{cccc}{\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}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathit{I}}^{-1}\left({\mathit{P}}_1-\mathit{x}\right)\times & {\mathit{I}}^{-1}\left({\mathit{P}}_2-\mathit{x}\right)\times & {\mathit{I}}^{-1}\mathit{L}& {\mathit{I}}^{-1}\mathit{L}\\ {\displaystyle \frac{{\mathit{I}}_{3\times 3}}{\varsigma m}}& {\displaystyle \frac{{\mathit{I}}_{3\times 3}}{\varsigma m}}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 3}\end{array}\right]$$

(14)

The state transition matrix at this time is shown in Equation (15), Equation (15) is discretized to obtain Equation (16).

$$\dot{\mathit{\chi }}=\tilde{\mathit{A}}\mathit{\chi }+\tilde{\mathit{B}}\mathit{u}$$

(15)

$$\mathit{\chi }\left[i+1\right]=\tilde{\mathit{A}}\mathit{\chi }\left[i\right]+\tilde{\mathit{B}}\mathit{u}\left[i\right]$$

(16)

4. Controller of Support Leg

The dynamic model based on DCM is established in Section 3, and the control of the support leg will be considered in this part. Figure 3 shows the overall control block diagram of the system, which will be analyzed in detail by modules in this part.

4.1. MPC Controller

MPC (Model Predictive Control) is an optimization calculation method commonly used in the control field [20,21]. MPC is used in multiple-input multiple-output (MIMO) systems to calculate the control parameters when the loss function is lowest according to the system model, target state. and control input, which are particularly important in the foot robot. In this part, the typical MPC method is used to calculate the optimal foot state with the system model established in Section 3 and the target state entered by the user. The structure diagram of the controller is shown in Figure 5, where the inputs are the desired state and the actual state, and the output is the control input u of the state transition matrix.

The cost function and constraints in Figure 5 are given by Equations (18)–(21):

$$J=\underset{\mathit{\chi },\mathit{u}}{\min }\sum _{i=0}^{k-1}{\left({\mathit{\chi }}_{i+1}-{\mathit{\chi }}_{{i+1}_{ref}}\right)}^T{\mathit{Q}}_i\left({\mathit{\chi }}_{i+1}-{\mathit{\chi }}_{{i+1}_{ref}}\right)+‖{\mathit{u}}_{\mathit{i}}‖{\mathit{R}}_i$$

(17)

$$\mathit{s}.\mathit{t}.\mathit{} \mathit{\chi }\left[i+1\right]=\tilde{\mathit{A}}\mathit{\chi }\left[i\right]+\tilde{\mathit{B}}\mathit{u}\left[i\right], i=0\dots k-1$$

(18)

$${\mathit{c}}_i^{-}\le {\mathit{C}}_i{[{\mathit{\chi }}_i,\mathit{u}}_i]\le {\mathit{c}}_i^{+}, i=0\dots k-1$$

(19)

$${\mathit{D}}_i{[{\mathit{\chi }}_i,\mathit{u}}_i]=0, i=0\dots k-1$$

(20)

Equation (17) is the loss function of the system. In this paper, the deviation of the state variable ${\mathit{\chi }}_i$ is selected as the process loss, the input ${\mathit{u}}_i$ is taken as the input loss, and ${\mathit{Q}}_i$,${\mathit{R}}_i$ are the weight matrices of these two quantities, respectively. Equation (18) is the state transition matrix of the system, which is obtained in Equation (12). Equations (19) and (20) are the inequality and equality constraints of MPC. In this paper, both the state quantity ${\mathit{\chi }}_i$ and the input ${\mathit{u}}_i$ are constrained, ${\mathit{C}}_i$ and ${\mathit{D}}_i$ are the constraint aggregation matrices of the inequality and equality constraints, respectively.

4.2. Constraint Condition

4.2.1. Inequality Constraints

Friction Cone Constraint: In the stable walking process of the biped robot, in order to ensure the robustness of the robot’s walking, the position of the foot end should not change; that is, the plantar should not slip due to the action of the torque. This requirement requires that the component force of the ground reaction force in the x-y plane satisfies the friction cone constraint [22]. Equations (21) and (22) show the friction cone constraint in the x and y directions. Where $\mu$ is the static friction coefficient, $i=1, 2$, representing the left and right legs.

$$f_{ix}\le \mu f_{iz}$$

(21)

$$f_{iy}\le \mu f_{iz}$$

(22)

Motor Torque Constraints: All actions of the biped robot are affected by the torque of the left and right 10 motors, but the maximum torque of the motors is limited by the hardware of the motors. The robot adopted in this paper uses two types of motors (whose parameters are mentioned in Table 1, and the control torque imposed by the two types of motors should also be limited. According to the size, the biped robot is divided into two types: Equations (23) and (24).

$${\mathit{M}}_1\le {\mathit{M}}_{max}$$

(23)

$${\mathit{M}}_2\le {\mathit{M}}_{max}$$

(24)

Normal Force Constraint: Since the control target of this paper is the foot force and moment, the ground reaction force is particularly important. Besides the friction cone constraint, the ground reaction force also needs to control the normal force to ensure that the normal force is within a limited range and constantly upward. The formula can be summarized as Equation (25).

$$0\le f_{iz}\le f_{max}$$

(25)

Line-Foot Contact: In order to consider the Line-Foot Contact constraint proposed by Caron [23], it is also necessary to constrain the contact lines of both feet. Due to the existence of the ankle pitch angle of the robot, it may make the feet point to the ground or the heel point to the ground. Therefore, based on Caron’s research, this algorithm constrains the ankle pitch angle, and the constraint method is as follows.

The plantar reaction force F is divided into yellow and blue parts in Figure 6, which act on the heel and the tip of the foot, respectively (since there is only pitch, the component force in the y direction is not considered). These two forces are ${\mathit{F}}_l$ and ${\mathit{F}}_h$, and ${\mathit{F}}_{\mathit{l}}$ and ${\mathit{F}}_h$, respectively; they are separated in the x and y directions, respectively.

$$F_y=F_{l,y}+F_{h,y}$$

(26)

$$F_z=F_{l,z}+F_{h,z}$$

(27)

Equations (26) and (27) describe the splitting process in detail. The force at the heel and the tip of the foot is divided into two forces in the y direction and the z direction (the x direction does not exist), and then the relationship between the torque at the single point of action and the torque at the two positions during the ankle joint action is further considered. The torque splitting results are given by Equations (28) and (29).

$${\tau }_y={-\tau }_{l,y}+{\tau }_{h,y}={-F}_{l,y}*l_l+F_{h,y}*l_h$$

(28)

$${\tau }_z={\tau }_{l,z}-{\tau }_{h,z}=F_{l,z}*l_l-F_{h,z}*l_h$$

(29)

Here, $l_l$ is the distance from the ankle to the heel, $l_h$ is the distance from the ankle to the tip of the foot, ${\tau }_y$ and ${\tau }_z$ are the torques at the action points of the ground reaction force, and $F_l$ and $F_h$ are the ground reaction forces at the action points of heel and toe.

Equations (26)–(29) are solved simultaneously to obtain the force in y and z directions at the heel and tip of the foot, as shown in Equation (30).

$$\left\{\begin{array}{c}{F}_{l,y}={\displaystyle \frac{{\tau }_z+l_h*F_y}{l_l+l_h}}\\ F_{l,z}={\displaystyle \frac{-{\tau }_y+l_h*F_z}{l_l+l_h}}\\ F_{h,y}={\displaystyle \frac{-{\tau }_z-l_l*F_y}{l_l+l_h}}\\ F_{h,z}={\displaystyle \frac{{\tau }_y-l_l*F_z}{l_l+l_h}}\end{array}\right.$$

(30)

Equation (30) shows the y-direction component force of the heel of the tip of the foot, and these two components need to comply with the friction cone constraint, that is, $f_y\le \mu f_z$. Combined with the above equation, the constraint conditions of the feet are completed.

DCM Line Constraint: In the MPC control framework proposed by Li [19], the DCM trajectory still has a curved part, which causes the corresponding centroid trajectory to exhibit a certain degree of abrupt change [9,24], thereby, the stability is still challenged to some extent. In this paper, the DCM is constrained in each single gait, so that the real-time DCM in the running process is straight to ensure the stability of the centroid trajectory in the x-y plane. The calculation constraint formula is given in Equation (31).

$$\mathit{\epsilon }\left[k+1\right]={\displaystyle \frac{1}{T}}\left(\mathit{\epsilon }\left[k\right]-\mathit{\epsilon }\left[k-1\right]\right)\mathit{\epsilon }\left[k\right]$$

(31)

4.2.2. Equality Constraint

Since the biped robot model with 10 degrees of freedom is adopted in this paper, there is no ankle joint roll degree of freedom, and the ankle joint roll torque is not considered in the control objective, so the roll torque should always be 0 in the equality constraint, that is, Equation (32). The goal of this is to prevent the existence of roll torque, which makes the foot entity rotate. In single leg support, the support polygon is a straight line, which increases the risk of system instability.

$${\tau }_x=0$$

(32)

4.3. QP Problem Solution

According to Equation (18), the state of the next sampling time can be calculated by using the state transition matrix of the system and the control input, and so on, the state of the K control input at the N sampling time can be calculated according to the time series. In this paper, K = 10 is selected as the control period of MPC, and all the prediction results within N = 10 periods are calculated. The state set is expressed by Equation (33).

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

(33)

where $\mathit{X}$ is the system state including the next 10 cycles, ${\mathit{x}}_0$ is the initial state, and $\mathit{U}$ is the optimal control input for a total of 10 cycles including the initial state. The objective function used in the MPC design is written in the following form:

$$\underset{\mathit{U}}{\min }{\displaystyle \frac{1}{2}}{\mathit{U}}^T\mathit{h}\mathit{U}+{\mathit{U}}^T\mathit{f}$$

(34)

$$\mathit{s}.\mathit{t}. \mathit{C}\mathit{U}\le \mathit{d}$$

(35)

$${\mathit{A}}_{eq}\mathit{U}={\mathit{b}}_{eq}$$

(36)

where

$$\mathit{h}=2\left({{\mathit{B}}_{qp}}^T\mathit{M}{\mathit{B}}_{qp}+\mathit{K}\right)$$

(37)

$$\mathit{f}=2{{\mathit{B}}_{qp}}^T\mathit{M}{(\mathit{A}}_{qp}{\mathit{x}}_0-\mathit{y})$$

(38)

Equation (34) is the simplified loss function, which is the QP quadratic standard form. Equations (35) and (36) are the aggregation of all inequality constraint constraints and equality constraints of the control cycle, respectively. In Equations (37) and (38), M represents the weight of state quantity change in the prediction period, and K is the change rate of MPC control input in the prediction period. Since the QP problem processes all state quantities and control inputs in the prediction period, M and K are the aggregation of state weight Q and control input weight R in Equation (17). Equations (39) and (40) represent this process.

$$\mathit{M}={\left[\begin{array}{ccccc}\mathit{Q}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathit{Q}& \mathbf{0}& \cdots & \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathit{Q}& \cdots & \mathbf{0}\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& \cdots & \mathit{Q}\end{array}\right]}_{N\times N}$$

(39)

$$\mathit{K}={\left[\begin{array}{ccccc}\mathit{R}& \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathit{R}& \mathbf{0}& \cdots & \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathit{R}& \cdots & \mathbf{0}\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& \cdots & \mathit{R}\end{array}\right]}_{N\times N}$$

(40)

where N is the number of prediction periods.

In this paper, the QP problem in standard form is solved by qpOASES [25] open source C++ library, and the extreme value of convex MPC [26] loss function is calculated considering its constraints. The process is shown in Figure 7, where the pink and blue parts represent the constraints, and an optimal set of control input $\mathit{U}$ is obtained within the constraints.

4.4. Mapping of Joint Torque

In the multi-link model, the forward and inverse kinematics [27] is an extremely important link. The forward kinematics can carry out forward operations from the joint angle to the end position, and the inverse kinematics constructs the mapping from the joint angle to the end position and establishes the forward and inverse kinematics model of the biped robot system completely. The coordinate transformation of the Model-H16 robot used in this paper in Figure 8 obtained from the model measurement is shown in

Table 2. Motor coordinate transformation table.

Body Frame	q1L	q2L	q3L	q4L	q5L
0 0 0	(−0.038, 0.068, 0)	(0.038, 0, 0.049)	(0, −0.022, −0.049)	(0, 0, −0.015)	(0, 0, −0.015)
Body frame	q1R	q2R	q3R	q4R	q5R
0 0 0	(−0.038, −0.068, 0)	(0.038, 0, 0.049)	(0, 0.022, −0.049)	(0, 0, −0.015)	(0, 0, −0.015)

.

According to the structure shown in Figure 8, this paper uses the improved D-H method to establish the D-H table of the system as shown in

Table 3. Table of D-H parameters.

Number of Connecting Rod	${\mathit{a}}_{\left(\mathit{i}-1\right)}/\left(\mathit{m}\right)$	${\mathit{\alpha }}_{\left(\mathit{i}-1\right)}/(°)$	${\mathit{d}}_{\mathit{i}}/\left(\mathit{m}\right)$	${\mathit{\theta }}_{\mathit{i}}/(°)$
1	0	0	L1	${\mathit{\theta }}_{\mathbf{1}}$
2	0	90	−L2	${\mathit{\theta }}_{\mathbf{2}}$
3	L3	−90	0	${\mathit{\theta }}_{\mathbf{3}}$
4	L4	0	0	${\mathit{\theta }}_{\mathbf{4}}$
5	L5	0	0	${\mathit{\theta }}_{\mathbf{5}}$

.

The L₁–L₅ distribution in Table 3 represents the coordinate migration in 0, and ${\mathit{\theta }}_1$–${\mathit{\theta }}_5$ represents the joint rotation angle. The forward kinematic expression of the system can be easily calculated by the D-H parameters.

$${\mathit{p}}_{foot}={\mathit{T}}_0^5\mathit{x}$$

(41)

where, ${\mathit{T}}_0^5$ is the homogeneous transformation matrix from the center of mass to the foot end.

Through the position of the end in Equation (41), the linear velocity Jacobian matrix ${\mathit{J}}_v$ from the center of mass to the foot end can be calculated as Equation (42). Combined with the coordinate transformation of Equation (43), the ${\mathit{J}}_w$ obtained together constitute the velocity Jacobian of the system, and the velocity Jacobian matrix $\mathit{J}$ is shown in Equation (44).

$${\mathit{J}}_v=\left[\begin{array}{ccccc}{\displaystyle \frac{\partial {\mathit{p}}_{foot}}{\partial q_1}}& {\displaystyle \frac{\partial {\mathit{p}}_{foot}}{\partial q_2}}& {\displaystyle \frac{\partial {\mathit{p}}_{foot}}{\partial q_3}}& {\displaystyle \frac{\partial {\mathit{p}}_{foot}}{\partial q_4}}& {\displaystyle \frac{\partial {\mathit{p}}_{foot}}{\partial q_5}}\end{array}\right]$$

(42)

$${\mathit{J}}_w=\left[\begin{array}{ccccc}{\displaystyle \frac{\partial \mathit{R}}{\partial q_1}}& {\displaystyle \frac{\partial \mathit{R}}{\partial q_2}}& {\displaystyle \frac{\partial \mathit{R}}{\partial q_3}}& {\displaystyle \frac{\partial \mathit{R}}{\partial q_4}}& {\displaystyle \frac{\partial \mathit{R}}{\partial q_5}}\end{array}\right]$$

(43)

$$\mathit{J}=\left[\begin{array}{c}{\mathit{J}}_v\\ {{\mathit{L}}^T\mathit{J}}_w\end{array}\right]$$

(44)

where R is the rotation matrix from the center of mass to the end, $\mathbf{L}=\left[\begin{array}{cc}0& 0\\ 1& 0\\ 0& 1\end{array}\right]$.

The Jacobian matrix J I of the biped robot is obtained through Equation (44), which also means that we can directly calculate a set of joint torque through $\mathit{J}$, such that the end position satisfies the MPC output in Equation (34). The joint torques of the supporting legs are calculated as shown in Equation (45).

$$\mathit{\tau }={\mathit{J}}^T\mathit{u}$$

(45)

5. Swing Leg Planning

In the leg planning of biped robot, in addition to the support leg for maintaining balanced standing, another important part is the swing leg. Its role is to swing the swing leg at the end of the support cycle to the next position, and at the end of the half walking cycle (that is, half gait cycle), it falls to the next support position and is used as a new support leg. There are various gait planning methods for supporting legs, among which polynomial interpolation method [28] and PD control method [29] are typical. In this paper, the PD control method is used to control the swing leg, Figure 9 illustrates the structural diagram of the PD controller, the schematic diagram of the planned control result is shown in Figure 10a, the red curve in Figure 10a is the trajectory of the swinging leg, and the simulation swing process is shown in Figure 10b.

Because according to the PD control law, this paper focuses on the end force ${\mathbf{F}}_{\mathrm{s}\mathrm{w}\mathrm{i}\mathrm{n}\mathrm{g}}$ of the swing leg, and calculates ${\mathbf{F}}_{\mathrm{s}\mathrm{w}\mathrm{i}\mathrm{n}\mathrm{g}}$ according to the deviation of the speed of the swing leg end and the deviation of the position. Equation (46) calculates the expectation of the end position using the method from Carlo [30].

$${\mathit{p}}_{foot,d}={\mathit{p}}_{hip}-\varsigma \left(\mathit{x}-\mathit{\epsilon }\right){\displaystyle \frac{∆t}{2}}$$

(46)

According to the Jacobian matrix in Equation (45), Equation (47) can calculate the velocity of the end through the joint angular velocity.

$${\dot{\mathit{p}}}_{foot}={\mathit{J}}^T\dot{\mathit{q}}$$

(47)

The actual foot position ${\mathit{p}}_{foot}$ can be calculated from the actual position of the center of mass by forward kinematics, and ${\dot{\mathit{p}}}_{foot,d}$ is always kept as 0 in this paper to ensure continuity during movement. From the above four quantities, Equation (48) calculates the target force input ${\mathbf{F}}_{\mathrm{s}\mathrm{w}\mathrm{i}\mathrm{n}\mathrm{g}}$ at the foot end of the swing leg by means of PD.

$${\mathbf{F}}_{\mathrm{s}\mathrm{w}\mathrm{i}\mathrm{n}\mathrm{g}}=K_P\left({\mathit{p}}_{foot}-{\mathit{p}}_{foot,d}\right)+K_D\left({\dot{\mathit{p}}}_{foot}-{\dot{\mathit{p}}}_{foot,d}\right)$$

(48)

6. Experimental Section

6.1. Simulation Experiment

In order to demonstrate the theoretical effect of the framework and algorithm, a series of experimental simulations were carried out on the Model-H16 robot based on MATLAB/Simulink (2023b) physical simulation environment, including uniform walking test, accelerated walking test, anti-interference ability test, and so on. At the same time, to facilitate the readers to better understand this part of the experiment, it is suggested to refer to the accompanying video: https://youtu.be/zabeuKK1_9U (accessed on 15 August 2024).

6.1.1. Constant-Speed Walking Test

In this part, the simulation test of constant-speed walking of the biped robot was designed, and the goal was to test the low-speed walking effect of the robot and the real-time trajectory of DCM. In this experiment, walking was carried out at a speed of 0.25 m/s, and its centroid and DCM real-time trajectory were shown in Figure 11. It can be seen that in the case of normal walking at low speed, the real-time DCM of the robot was basically approximately fitted to a straight line between the switching points, which represented that the linear constraint effect of DCM played a good role in MPC.

In order to show the effect of MPC and PD controller, Figure 12a shows the 3D force and 2D torque demand output by MPC during uniform walking. The figure shows that each time the left leg lands, large force and torque demand is generated to meet the needs of supporting the leg, where Fx and Fy fluctuate within 100 N, and Fz is under the demand of the normal force of the foot. The fluctuation range is within 400 N, which meets the requirement of 500 N constraint on the ground reaction force in this paper, and Fx, Fy, and Fz all show periodic laws during stable walking. The fluctuation range of torque is within 10 N and shows a stable and controlled trend.

For the output effect of the PD controller, the output of the PD controller directly corresponded to the joint torques, and in this paper, the control torques of hip pitch, knee pitch, and ankle pitch are shown in Figure 12b. In each swing cycle, the torque at the moment of leg lift mutated to adapt to the position of the center of mass. During smooth walking, the output of the PD controller showed periodic changes, and the variation of the torque was within ±1.5 Nm.

6.1.2. Accelerated Walking Test

In order to verify the state effect of the algorithm under the maximum walking speed and acceleration of the Model-H16 robot, the acceleration walking test of the robot was carried out under the condition of flat terrain without interference. The test results of acceleration walking are shown in Figure 13 and Figure 14. Figure 13 shows the centroid tracking effect of the robot, and the robot shows good centroid tracking ability. In the figure, the maximum deviation between the actual centroid position and the target centroid position was 0.096 m/s, and the position repair can be completed after a certain time, and the robot can complete a good centroid following effect in normal walking. In this experiment, the robot reached a walking speed of 0.75 m/s. Considering that its leg length is only 0.3 m, the ratio of maximum speed to leg length was 2.5. Compared with Hector’s leg length of 0.44 m, the maximum speed of 1.1 m/s, and the ratio of maximum speed to leg length of 2.75, the robot can meet the needs of high-speed walking.

At the same time, the position of the ends of both feet was also detected in the process of accelerated walking. Due to the symmetric nature of the left and right legs, only the right leg effect is shown in this part. Figure 14 shows the x, y, and z trajectories of the right foot. Figure 14a shows the target position and the actual position in the x direction. The error adjustment time was only 0.05 s, and the error of the landing position was approximately 0, which has a good tracking effect. Figure 14b shows the actual position in the y direction, which is basically around the standing position of −0.1 m during walking, and the deviation is within 0.03 m, which can meet the requirements of high-speed walking of the robot. Figure 14c shows the real-time walking position on the z-axis, whose lifted leg height approximately fluctuates at the set point of 0.3 m, and the half-step period is 0.2 s; that is, the left and right legs are switched every 0.2 s. In Figure 14c, the position of the foot end of the right leg is altered between 1–2 s due to accelerated walking. However, it was quickly repaired under the action of the controller, allowing it to return to the normal target tracking effect.

6.1.3. Anti-Jamming Capability Test

In order to verify the robustness and stability of the algorithm, this part applied an approximate impulse thrust with a duration of 0.2 s from the side and front of the robot at T = 1.0 s under the condition of the robot walking in place, imitated the external artificial thrust, broke the equilibrium state of the robot walking in place, and tested its maximum thrust recovery value. The test results are shown in Figure 15.

In Figure 15a, the change in the radial direction (x-direction) of the centroid position of the robot is shown after a thrust of the same duration and different magnitude is applied to the lateral direction of the robot. In the experiment, the thrust from 20 N to 60 N was selected. The results show that 55 N thrust is the limit value of thrust recovery, and within the limit value, the offset in the radial direction of the robot is always less than 0.05 m. When the thrust increases to 60 N, the robot undergoes a large deviation (greater than 0.1 m) in the radial direction after 0.5 s of the thrust, resulting in the robot body flying and being unable to recover.

Figure 15b shows the lateral thrust effect, which is tested under the thrust of 10 N to 45 N, respectively. The test results show that when the thrust is small (10–20 N), the gait of the robot is almost not affected by the thrust, and the deviation of the center of mass in the y direction is less than 0.05 m. When the thrust increases from 20 N to 45 N, the robot exhibits a clear offset in the y direction, which is less than 0.1 m, and is recovered in the subsequent 2–5 s gait. When the thrust exceeds the limit value of 45 N, a large deviation (greater than 0.15 m) occurs in the y direction at 0.45 s after the thrust action. At this time, the lateral thrust makes the robot unable to recover the equilibrium state.

6.2. Physical Verification

In this paper, the algorithm was deployed on the Model-H16 robot to complete the basic walking function and obstacle crossing.

In the straight-line walking of the object, the right leg performed the action first and was used as the swing leg. After half a gait cycle (0.2 s), the swing leg and the support leg were exchanged, and the left leg was used as the swing leg. The continuous photo and centroid trajectory of the object were shown in Figure 16. In this paper, the Z-direction trajectory changes of the center of mass and the end of the right leg were collected in the process of uniform walking. Figure 17a shows the change of the height of the center of mass in the process of uniform walking. In the figure, the maximum deviation of the height of the center of mass in the 0–2 s gait is ±0.012 m and gradually tends to be stable, and the final position of the center of mass is constant at 4.15 ± 0.003 m. In the trajectory of the foot height, because the initial gait affected its balance, the gait was adjusted adaptively in the second gait cycle, and the foot height fluctuated in a small amplitude with high frequency. After one cycle of adjustment, normal walking was resumed, and the height and position of the center of mass were not affected. During normal walking, the foot height ranged from 0 to 0.03 m.

In the process of crossing the obstacle, the biped robot was able to successfully cross two levels of 10 mm wood boards and maintain balance during the crossing. Its physical photos are shown in Figure 18.

In the obstacle crossing test, the starting state was normal walking. When the gait was stable, the obstacle was encountered. The first stage of the board was crossed between 2–2.4 s, the second stage of the board was crossed between 2.4–2.8 s, and the landing buffer gait occurred between 2.8–3 s. As shown in Figure 19, the height of the center of mass fluctuates from 0.41 m to 0.42 m, then to 0.43 m, and finally changes to 0.41 m upon landing. The position of the foot end of the right foot changed from 0 to 0.02 m, and finally the normal walking state was restored by a half gait of 0.2 s.

7. Discussion and Conclusions

Through a series of experiments in Section 6, this paper shows the control effect of the proposed algorithm for the general forward flexion type 10-degree-of-freedom biped robot and also proves the excellent performance of the DCM-MPC control method in velocity following and disturbance resistance. This will extend the application of DCM in the field of biped robots, incorporating not only Englsberger’s [10] work and its related derivative theory but also considering the concept of multi-model fusion as proposed by Zhang Zhao [15]. Compared with the research of Englsberger [11], this paper simplifies the process of dynamic modeling, combines LIPM with simplified rigid body dynamics for modeling, and introduces the control method of force and torque, which is more universal for 10 DOF robots. Moreover, the control target becomes 10 DOF underactuated robots, so the research is more challenging. Compared with the research results of Li [19], this paper does not directly use the complete state of the center of mass for consideration but introduces the variable DCM. This approach not only considers the state of the center of mass but also makes it more convenient for MPC to constrain and optimize the trajectory of DCM, providing a more intuitive to analyze the stability and optimize the trajectory of the robot. When MPC constraints are considered, the linear foot constraints are consistent with Caron’s [23] algorithm and the friction cone is expanded in the two-dimensional plane x-z, and the toe and heel are constrained, respectively. Another feature of constraint in this paper is that DCM is transformed from control target to control constraint. Compared with Hopkins [16] and Griffin [17], perfect leg force, torque, friction cone, and DCM constraints are established in this paper. Although the speed of the algorithm is close to that of the Hector robot, and it has good anti-disturbance ability in radial and lateral directions, the effect of the concave and convex terrain of the ground still needs to be improved.

In summary, this paper proposes a new control framework based on the force and torque control framework, combined with DCM theory to establish the control model. Each constraint adjustment, such as line angle, friction cone, force and torque, and new DCM constraint, is introduced to establish the MPC control strategy. The program library qpOASES is used to solve the quadratic programming problem in MPC, and the PD control method is used to complete the control of the swing leg. Finally, the robot control system is completely realized by mapping the torque Jacobian to the joint motor. In the simulation experiments, this paper demonstrates that the DCM trajectory maintains a straight line in the gait, the control algorithm achieves a fast walking of 0.75 m/s on the Model-H16 robot, and the velocity deviation remains below 0.096 m/s in the velocity tracking process. Additionally, it shows the robot’s anti-disturbance ability in both the radial and lateral directions. In the physical experiment, the ability of the robot to walk in place and pass through the obstacle at low speed is demonstrated, which also verifies the effectiveness of the proposed algorithm in the physical deployment and migration. The follow-up work will focus on the realization of high-speed walking of the physical robot and achieve a breakthrough in the algorithm framework.