Trot Gait Stability Control of Small Quadruped Robot Based on MPC and ZMP Methods

Abstract

The stability of a quadruped robot is mainly affected by the obstacles in the horizontal direction and the roughness in the vertical direction, which often leads to the robot unable to achieve the desired gait effect. In order to solve this problem, the Model Predictive Control (MPC) model and the Zero Moment Point (ZMP) method are combined, and applied to gait planning and the foot end landing control of a small quadruped robot. The tort gait of a small quadruped robot is the focus of research in this study, which simulated trajectory planning and gait stability. In addition, through comparative analysis with the corresponding experiments, the results show that the simulation results are similar to the experimental results, and the quadruped robot gait is stable. Meanwhile, it shows that the combination of the MPC model and ZMP method is feasible for gait stability control of a quadruped robot.

1. Introduction

Under the condition of the load bearing capacity, self weight, manufacturing cost, walking speed and other factors, a small quadruped robot is an outstanding and urgent problem in the design to carry out gait optimization research to ensure its walking stability [1,2,3]. In recent years, scholars have done a lot of research on quadruped robots, mainly focusing on robot structure design, gait control, rigid flexible coupling, etc. [4,5,6,7,8,9].

During the walking process of a quadruped robot, the leg-end fall point is greatly affected by the obstacles on the ground in the horizontal direction and the rugged road in the vertical direction, and the input and output control quantities and constraints are often nonlinear [10,11,12,13,14]. Many scholars use various methods to solve this nonlinear problem [15,16,17,18]. Sutyasadi, P. et al. introduces its trotting control algorithm, and the joint trajectory is controlled using hybrid Proportional Integral Derivative-Iterative Learning Control (PID-ILC) [19]. Kimura, H. proposed the necessary conditions for stable dynamic walking on irregular terrain in general, and the neural system is designed by comparing biological concepts with those necessary conditions described in physical terms [20]. The adaptive controller is able to track the resonant frequency of the robot, which is a function of different body parameters [21]. En, M. et al. present the designing and optimization of an effective hybrid control by combining LQR and PID controllers, and the tuning of a hybrid LQR-PID controller for foot trajectory control of a quadruped robot during step motion using the Grey Wolf Optimizer (GWO) algorithm, which is an alternative method that is comparatively investigated with two traditional benchmarking algorithms (PSO and GA) [22]. The application of the MPC strategy to the locomotion of a quadrupedal robot endowed with a Central Pattern Generator (CPG) neural locomotion architecture was also proposed [23]. The stability of the trot pattern and proposes trot pattern generation for a quadruped robot on the basis of ZMP stability margin was also proposed [24]. Among various advanced control methods, the traditional PID control method can not effectively deal with multiple nonlinear problems. The LQR (Linear Quadratic Regulator) control method requires high model accuracy and is not suitable for the stability control of small robots. The MPC has low requirements on model accuracy, and the system has good robustness and stability, which provides a good prospect for solving the balance problem of dynamic robots [25]. Meanwhile, the ZMP method can make quadruped robot obtain better gait dynamic balance. Therefore, the gait stability of the quadruped robot is studied by the method of combining MPC with ZMP in this study.

In this research, the foot fall point control based on the ZMP method in the X-Y plane, foot trajectory planning based on the MPC method in the X-Z plane, and inverse kinematics motor angle calculation is systematically integrated. Then, the trajectory planning and foot end fall control of the trot gait of the small robot are simulated and verified by experiments.

2. Theoretical Background

This research combines MPC [26], ZMP [27] and foot tip trajectory planning [28], and then provides a systematic method for the gait stability control of a small quadruped robot through forward kinematic and inverse kinematic. The gait stability control method is shown in Figure 1.

2.1. Trajectory Control of Quadruped Robot Based on MPC Method

Quadruped motion control belongs to the state space model [29]. The logic relationship between the input and output of MPC is shown in Figure 2.

The specific calculation process of quadruped robot motion trajectory control is as follows:

(1)

The coordinate position of the quadruped robot is established in the geodetic coordinate system, as shown in Figure 3.

(2)

The nonlinear control of the quadruped robot can be seen as a front and rear dual drive, controllable suspension trolley moving on non-plane road, as shown in Figure 4.

When the quadruped robot is simplified to the trolley model, the Ackerman steering control algorithm [30] is introduced, and then the single leg speed formula is

$$\{\begin{array}{c}{V}_r=V_{f}cosa\\ V_{yw}=V_{f}sina\\ V_{yw}=V_{r}tana \end{array}$$

(1)

The overall speed state of the robot is:

$$\{\begin{array}{c}{f}_1=\dot{X}=V_x=V_{r}cos\phi \\ f_2=\dot{Y}=V_y=V_{r}sin\phi \\ f_3=\dot{\phi }=\frac{V_{yw}}{Len}=\frac{V_{r}tana}{Len} \end{array}$$

(2)

where $f_1,f_2,f_3$ are the derivatives of the robot states $\dot{X}, \dot{Y},\dot{\phi }$, respectively.

The state vector $\tau$ (which includes three components ${\tau }_1, {\tau }_2, {\tau }_3$) and control vector $u$ (which includes two components $u_1, u_2$) are defined as:

$$\tau =\left[\begin{array}{c}{\tau }_1\\ {\tau }_2\\ {\tau }_3\end{array}\right]=\left[\begin{array}{c}X\\ Y\\ \phi \end{array}\right]$$

(3)

$$u=\left[\begin{array}{c}{u}_1{}_{ }\\ u_2\end{array}\right]=\left[\begin{array}{c}{V}_r\\ a\end{array}\right]$$

(4)

The reference track ${\dot{\tau }}_{ref}$ and the reference control quantity $u_{ref}$ are introduced to establish a nonlinear relationship with the current state ${\tau }_{ }$ of the quadruped robot, and the following formula can be obtained by Taylor’s first-order expansion:

$$\dot{\Gamma }=\dot{\tau }-{\dot{\tau }}_{ref}=\frac{\partial f\left(\tau ,u\right)}{\partial \tau }\left(\tau -{\tau }_{ref}\right)+\frac{\partial f\left(\tau ,u\right)}{\partial u}\left(u-u_{ref}\right)$$

(5)

where $\Gamma$ is the trajectory error vector.

The trajectory error is linearized

$$\dot{\Gamma }=A\Gamma +BU$$

(6)

where $U$ is the control error vector, and A and B are the coefficients to be determined.

$$\Gamma =\left[\begin{array}{c}X-X_r\\ Y-Y_r\\ \phi -{\phi }_r\end{array}\right]$$

(7)

$$U=\left[\begin{array}{c}{u}_1{}_{ }\\ u_2\end{array}\right]=\left[\begin{array}{c}{V}_r\\ a\end{array}\right]$$

(8)

A and B are solved by partial derivative as follows:

$$\{\begin{array}{c}A=\frac{\partial f}{\partial \tau }=\left[\begin{array}{ccc}\frac{\partial f_1}{\partial {\tau }_1}& \frac{\partial f_1}{\partial {\tau }_2}& \frac{\partial f_1}{\partial {\tau }_3}\\ \frac{\partial f_2}{\partial {\tau }_1}& \frac{\partial f_2}{\partial {\tau }_2}& \frac{\partial f_2}{\partial {\tau }_3}\\ \frac{\partial f_3}{\partial {\tau }_1}& \frac{\partial f_3}{\partial {\tau }_2}& \frac{\partial f_3}{\partial {\tau }_3}\end{array}\right]=\left[\begin{array}{ccc}0& 0& -V_{r}sin\phi \\ 0& 0& V_{r}cos\phi \\ 0& 0& 0\end{array}\right]\\ B=\frac{\partial f}{\partial u}=\left[\begin{array}{cc}\frac{\partial f_1}{\partial u_1}& \frac{\partial f_1}{\partial u_2}\\ \frac{\partial f_2}{\partial u_1}& \frac{\partial f_2}{\partial u_2}\\ \frac{\partial f_3}{\partial u_1}& \frac{\partial f_3}{\partial u_2}\end{array}\right]=\left[\begin{array}{cc}cos\phi & 0\\ sin\phi & 0\\ \frac{tana}{Len}& \frac{V_r}{Len co{s}^{2}a}\end{array}\right]\end{array}$$

(9)

The forward Euler difference quotient is used to replace the differential method to discretize the trajectory error:

$$\dot{\Gamma }\left(k\right)=\frac{\Gamma \left(\mathrm{k}+1\right)-\Gamma \left(k\right)}{T}=A\Gamma \left(k\right)+BU\left(k\right)$$

(10)

where k is the current time, and T is the sampling period.

The next cycle can be predicted as:

$$\{\begin{array}{c}\Gamma \left(\mathrm{k}+1\right)=T\left(1+A\right)\Gamma \left(\mathrm{k}\right)+BTU\left(k\right)=C\Gamma \left(\mathrm{k}\right)+DU\left(k\right)\\ C=\left[\begin{array}{ccc}1& 0& -T{V}_{r}cos\phi \\ 0& 1& T{V}_{r}cos\phi \\ 0& 0& 1\end{array}\right] \\ D=\left[\begin{array}{cc}Tcos\phi & 0\\ Tsin\phi & 0\\ \frac{Ttana}{Len}& \frac{T{V}_r}{Len co{s}^{2}a}\end{array}\right] \end{array}$$

(11)

where C and D are the coefficients to be determined.

The standard form of secondary planning is constructed as follows:

$$\min \left(\frac{1}{2}{U}^{T}HU+f^{T}U\right) loa{d}_b\le U\le u{p}_b$$

(12)

where $loa{d}_b, u{p}_b$ are the upper and lower limits set by the quadratic planning; H is the Hessian matrix formed by the second derivative, and $f^T$ is Jacobi matrix formed by gradient.

The optimal solution can be obtained from Formula (12) as follows:

$$U\left(k\right)=\left[\begin{array}{c}u\left(k\right)\\ u\left(k+1\right)\\ \begin{array}{c}\cdots \\ u\left(k+m\right)\end{array}\end{array}\right]=\left[\begin{array}{c}\begin{array}{cc}Vr\left(k\right)& a\left(k\right)\end{array}\\ \begin{array}{cc}Vr\left(k+1\right)& a\left(k+1\right)\end{array}\\ \begin{array}{c}\cdots \\ \begin{array}{cc}Vr\left(k+m\right)& a\left(k+m\right)\end{array}\end{array}\end{array}\right]$$

(13)

$u\left(k\right), u\left(k+1\right)\dots u\left(k+m\right)$ are brought into the control system; the returned parameters $\dot{x}, \dot{y}, \dot{\phi }$ by the sensor are brought back into the prediction function for rolling optimization to calculate the optimal control at the next time.

2.2. Foot Drop Point Control Based on ZMP Theory in X-Y Plane

According to the MPC control method, the speed $Vr\left(k\right)$ and yaw angle $a\left(k\right)$ of the robot step at any time can be calculated. The change of the quadruped robot’s position is caused by the change of foot end position. According to the initial input of MPC reference trajectory and robot model, the reference position of the robot foot end can be obtained. However, the stability of the robot when it moves to this position still needs to be further verified. This study uses ZMP theory to verify.

ZMP theory assumes that there is a point in the supporting polygon of the robot, so that the ground reaction force received by the robot is balanced with gravity and inertial force [31]. For a small quadruped robot, the stability margin can be defined as the shortest distance between ZMP and the boundary of the supporting triangle. The ZMP input and output of the robot are shown in Figure 5.

The stability margin can judge the stability of the robot in the static state, but the robot needs to consider the changes of inertia force and foot end torque in the movement process, and it is necessary to control the projection point of the center of gravity on the horizontal ground to fall on the connection line of the motion support point, so as to ensure that the robot maintains balance in the movement process. After the first half cycle of quadruped robot operation, all foot ends contact the ground when the support phase and swing phase change, as shown in Figure 6.

Based on the geodetic coordinate system, ignoring the z-axis height, the foot end coordinates is:

$$Foo{t}_{\mathrm{i}}=\left[\begin{array}{c}{x}_{\mathrm{i}}\\ y_{\mathrm{i}}\end{array}\right]=\left[\begin{array}{c}\pm \frac{1}{2}Len\pm pac{e}_1\cos \left(angle\right)\\ \pm \frac{1}{2}Wid\pm pac{e}_1\sin \left(angle\right)\end{array}\right]-30°\le angle\le 30°$$

(14)

The stability margin of the robot can be solved by calculating $\min dth$.

$$\underset{s1>s2}{\min }dth=\frac{\sqrt[2]{{\left(x_{s1}{y}_{s2}-x_{s2}{y}_{s1}\right)}^2}}{\sqrt{{\left(y_{s1}-y_{s2}\right)}^2+{\left(x_{s2}-x_{s1}\right)}^2}}s1-s2=1$$

(15)

From this Formula (15), the minimum stability margin of different leg internal offset angles can be calculated. When the robot is running, because of the uneven ground or the need to change the direction of advance, its stability margin will change. The change amount can be calculated by solving the current foot position through forward kinematics of the motor rotation angle. The stability margin calculated by comparing the motor angle with the stability margin calculated by MPC control quantity can also be obtained. At the same time, the relationship between the current step length, offset angle, and the stability margin can be obtained.

2.3. Foot Trajectory Planning in X-Z Plan

In the X-Z plane, in the X axis direction, the swing phase is composed of multiple curves:

$$\{\begin{array}{c}Span=V{r}_{zmp}tcosa\\ x=-t t\in \left[0,r\right]\\ x=t+2r t\in \left[r,Span-4+r\right]\\ x=-t+2Span-8 t\in \left[Span-4+r,Span-4+2r\right]\end{array}$$

(16)

where $Span$ is the foot end span parameter; $V{r}_{zmp}$ is the foot tip speed of MPC after ZMP adjustment; t is the current time; and $r$ is the arc track radius of foot tip buffering with the ground when the phase changes.

In the Z-axis direction, the swing phase consists of three sections:

$$\{\begin{array}{c} \mathrm{Sec}\mathrm{tion}1: z=r-\sqrt{r^2-{\left(t+2\right)}^2} t\in \left[0,r\right] \hfill \\ \mathrm{Sec}\mathrm{tion}2:z1=H_{leg}sin\left(\frac{pi}{\mathrm{Span}\cos \left(a_{zmp}\right)}\left(t+2r\right)+l{f}_0\right) t\in \left[r,\mathrm{Span}-4+r\right] \hfill \\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}z2=H_{leg}\sin \left(\frac{pi}{\mathrm{Span}\sin \left(a_{zmp}\right)}\left(t+2r\right)\right) \hfill \\ z=z1+z2 \\ l{f}_0=\frac{pi}{\mathrm{Span} \cos \left(a_{zmp}\right)}\left(r+2\right)\\ \mathrm{Sec}\mathrm{tion}3:z=r-\sqrt{r^2-{\left(-t+\mathrm{Span}-4+2r\right)}^2} t\in \left[\mathrm{Span}-4+r,\mathrm{Span}-4+2r\right]\end{array}$$

(17)

where $H_{leg}$ is the leg lifting height parameter, which can realize the maximum amplitude change of foot tip track in the X-Z plane, and $a_{zmp}$ is the toe offset angle of MPC after ZMP adjustment.

The support phase is composed of a section of a non-standard sine curve. For the X axis direction and Z axis rotation direction:

$$\{\begin{array}{c}x=-t+2Span-8 t\in \left[Span-4+2r,2Span-8\right] \\ z=H_{leg}sin\left(\frac{pi}{\mathrm{Span}-2r-4}\left(-t+2\mathrm{Span}-8\right)\right) t\in \left[Span-4+2r,2Span-8\right] \end{array}$$

(18)

2.4. Calculation Method of Motor Rotation Angle in Inverse Kinematics

The leg structure of the small quadruped robot studied in this paper is the same, and only one leg kinematics equation can be derived [32]. Taking the right front leg as an example, the position of the right front leg in the space coordinate system is established, as shown in Figure 7.

In order to establish the inverse kinematics equation of the robot in the coordinate system, it is necessary to determine the position of the robot leg in the Y-Z and X-Z planes, as shown in Figure 8.

According to Figure 8, the rotation angle can be obtained:

$$\{\begin{array}{c}l=lc2-lc1=-arctan\frac{y}{z}+arctan\frac{L3}{\sqrt{y^2+z^2+L{3}^2}}\\ m=-arccos\frac{D5}{L2}=-arccos\frac{D{1}^2+x^2-L{2}^2-L{1}^2}{2L1 L2}\\ n=nc1-nc2=arccos\frac{L1+D5}{D3}-arctan\frac{x}{D1}=arccos\frac{D{1}^2+x^2-L{2}^2+L{1}^2}{2L1 \sqrt{D{1}^2+x^2}}-arctan\frac{x}{D1}\end{array}$$

(19)

3. Simulation and Experimental Verification of Quadruped Robot Trot Gait

3.1. Quadruped Robot Model

The quadruped robot model in this study is shown in Figure 9. The robot leg has 3 degrees of freedom, which is located in the hip joint, leg, and knee joint, respectively. The contact between the robot foot and ground is point contact. Since the leg mass of the robot is very small, its center of gravity is located at the geometric center of the fuselage.

3.2. Parameter Definition and Assignment

For the convenience of analysis, the definitions and assignments of the relevant parameters for the robot are given in

Table 1. The parameter table of the quadruped robot.

Order Number	Parameter	Value	Explain
1	Len	150 mm	Body length
2	Wid	80 mm	Body Width
3	L1	90 mm	Thigh Length
4	L2	85 mm	lower leg
5	L3	15 mm	Hip Joint Length
6	Pace1	80 mm	The distance from which the oscillating phase moves forward
7	Pace2	40 mm	Distance to move back the supporting phase
8	angle	−30° < angle < 30°	Drive Leg Offset Angle
9	m	2.03 kg	The dead-weight
10	T	0.1 s	The sampling period

.

3.3. Supporting Phase and Swinging Phase of Quadruped Robot

Steady motion is the most basic and core problem in the movement control of quadruped robots, from which all kinds of gait are derived, such as diagonal gait, translational gait, trotting gait, jumping gait, etc. This paper focuses on the stability of the quadruped robot with trot gait. The duty cycle of support phase and swing phase in a whole cycle is often used to describe gait under different attitudes, and the stance phase and swinging phase of trot gait are shown in Figure 10.

3.4. Simulation and Verification of Foot Tip Trajectory in X-Z Plane

According to the ground with different flatness, parameters A, B, and T are modified to control the step height, step length, and step cycle of the quadruped robot. After the unit track changes, the single leg foot end track in the XZ-plane is simulated by matlab and experimental verification, and the results are shown in Figure 11. For ease of description, through matlab simulation, the displacement of the foot tip track on the X-axis and Z-axis can be abbreviated as X-axis simulation and Z-axis simulation, respectively; Through the experiment, the displacement of the foot tip track on the X-axis and Z-axis is abbreviated as the X-axis experiment and Z-axis experiment, respectively.

According to the Figure 11, the simulation of the foot trajectory on the x-axis is basically consistent with the actual situation, which shows that the small quadruped robot runs stably when moving straight ahead.

When the foot end track is in the ascending phase of the Z-axis, the experimental value is slightly smaller than the simulation value at the beginning because of the delay of the motor driving the leg to move, and at the later stage, because of the inertia of the motor driving the calf, the experimental value is slightly larger than the simulation value. In the leg descending phase, because the experimental maximum value is greater than the simulation maximum value, when the leg motor speed is consistent, the experimental value is slightly more delayed than the simulation value.

3.5. Foot Drop Control Based on ZMP in X-Y Plane

The stability margin $\underset{n>m}{\min }dth$ is simulated as the angle and time changes, and the results are shown in Figure 12. The stability margin is 30.5° when the angle = 0°, which is shown as the red line; the stability margin is the black line when the angle ≠ 0°.

According to Figure 12, when the stability margin of angle ≠ 0° is greater than 30.5°, and the angle range is 0°~27°, it can be considered as foot tip control stability. When the angle is 9.6°, the stability margin reaches a maximum of 38°.

3.6. Motion Simulation Based on the MPC Model and ZMP Method

According to the MPC method, the trajectory and yaw angle of the robot are calculated, respectively. For comparative analysis, the reference trajectory and reference yaw angle are introduced, and the stability margin after MPC and ZMP optimization is also introduced, as shown in Figure 13. The curves calculated by the ZMP method are all combined with the MPC method in Figure 13. For the convenience of expression, the ZMP method and MPC method are abbreviated as the ZMP method in the following description.

According to Figure 13, the following conclusions can be drawn:

(1)

According to Figure 13, the displacement and yaw angle of the small quadruped robot obtained by the MPC method are basically consistent with the reference curve, only slightly delayed in time. It shows that the MPC method is basically stable for trot gait control of the quadruped robot.

(2)

According to Figure 13a, the walking track of the robot optimized by the MPC model and ZMP method is delayed relative to the reference track. As the ZMP method mainly changes the foot end fall point, it has little impact on the robot’s walking track, so the MPC track is similar to the ZMP track.

(3)

According to Figure 13b, the yaw angle of the robot optimized by the MPC model and ZMP method is delayed relative to the reference yaw angle. The yaw angle curve optimized by the ZMP method is smoother than that optimized by MPC, which shows that the robot optimized by the ZMP method is more stable.

(4)

According to Figure 13c, after adjustment by ZMP, when the robot turns, the stability margin changes more smoothly, ensuring that each foot end can obtain a more stable stability margin with a smaller amount of change when the stability margin suddenly changes.

4. Conclusions

(1)

In this study, MPC, ZMP, and foot end trajectory planning are combined to control the trot gait stability of a small quadruped robot. In this study, a small quadruped robot model and material object are established. Through the relevant simulation and experimental verification, the fusion method can achieve the trot gait control stability of the small quadruped robot, and the method is feasible and effective.

(2)

The foot end trajectory of a single leg of a quadruped robot is planned on the X-Z plane, and the results show that the experimental value is slightly smaller than the simulation value at the beginning because of the delay of the motor driving the leg to move, and the later stage because of the inertia of the motor driving the calf; therefore, the experimental value is slightly larger than the simulation value.

(3)

The MPC method can realize the displacement and yaw angle of the quadruped robot with better randomness compared with the reference curve, and the ZMP method can make the trajectory and yaw angle obtained by MPC method smoother and reduce fluctuation. The research results of the foot drop control based on ZMP in the XY-plane show that the stability range and stability margin are high.

(4)

The method presented in this paper is of great significance to improve the gait stability of a small quadruped robot, and it provides a reference for the walking stability of the quadruped robot.