A method for controlling a quadruped robot in combination with terrain constraints

Abstract

The application discloses a kind of terrain-restricted quadruped robot control method, comprising the following steps: establishing quadruped robot single rigid body model;The treadable area is simplified into several polygons by three-dimensional space line determination;Using nonlinear model prediction control method, and adding terrain constraint, obtain a time period within the position, velocity, acceleration of quadruped robot joint space The optimal control amount of expected value is the first layer control architecture;Establish the whole body control model of quadruped robot, introduce terrain constraint, obtain the position, velocity, acceleration of quadruped robot joint space The optimal control amount of expected value is the second layer control architecture;The first and second layer control architecture is combined, and the joint torque of quadruped robot is obtained, which meets the terrain constraint.The application adopts the form of nonlinear model prediction control, can add terrain constraint from planning level, so that the landing point of quadruped robot meets the dynamics constraint, and can adapt to the complex terrain with higher landing point position requirement.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of quadruped robot control method, and particularly relates to a quadruped robot control method combined with terrain constraints. BACKGROUND

[0002] The existing quadruped robot control method does not consider the terrain constraints well, and has poor terrain adaptability for terrains such as steps, plum-blossom piles and the like which have high requirements on the robot landing point position.

[0003] One advantage of the quadruped robot over the wheeled robot is its good terrain adaptability, and the quadruped robot has good passing ability on unstructured terrains such as muddy, gravel road, grassland, snowfield and the like.

[0004] However, for some more complex terrains such as steps, plum-blossom piles, gullies and the like which have high requirements on the robot landing point position, the existing quadruped robot control method does not consider the terrain or does not consider the terrain constraints well, which may cause the quadruped robot to fail on these terrains, thereby limiting the application scenarios of the quadruped robot. SUMMARY

[0005] In order to overcome the defects of the prior art, the application provides a quadruped robot control method combined with terrain constraints, which combines perception information to achieve better quadruped robot control.

[0006] The technical scheme adopted by the application to solve the technical problem is that the quadruped robot control method combined with terrain constraints comprises the following steps:

[0007] A single rigid body model of the quadruped robot is established;

[0008] The step of simplifying the step of simplifying the quadruped robot stepable area into a plurality of polygonal areas, the polygonal areas being determined by lines in a three-dimensional space;

[0009] A nonlinear model predictive control method is adopted, and terrain constraints are added therein to obtain optimal control amounts of expected values of positions, velocities and accelerations of the quadruped robot in a joint space within a time period, which is a first layer control architecture;

[0010] A whole body control model of the quadruped robot is established, and terrain constraints are introduced therein to obtain optimal control amounts of expected values of positions, velocities and accelerations of the quadruped robot in a joint space, which is a second layer control architecture;

[0011] The first layer control architecture and the second layer control architecture are combined to obtain joint torques of the quadruped robot meeting the terrain constraints.

[0012] Furthermore, in the step of establishing a single rigid body model of the quadruped robot, the mass of each leg of the quadruped robot is ignored, and only the mass and moment of inertia of the quadruped robot body are considered to establish the dynamic equation.

[0013] Furthermore, the quadruped robot single rigid body dynamics model is

[0014]

[0015]

[0016]

[0017] in is the acceleration of the robot's center of mass, f i is the foot end force of the i-th leg, g is the acceleration due to gravity, m is the mass of the robot, q is the Euler angle, R(q) is the rotation matrix, I and ω are the moment of inertia and angular velocity of the robot body coordinate system, respectively, r i is the position of the i-th leg in the world coordinate system, and T(q) is the transformation matrix that transforms the body angular velocity into the Euler angular velocity.

[0018] Furthermore, it also includes the step of changing the single rigid body dynamics model to obtain the discrete dynamics equality constraint.

[0019] First, write the single rigid body model into state space form. in is the state quantity, u(f,r) is the control quantity, where f,r are all foot end forces f i and landing point position r i Column vector combination of ;

[0020] Then, after selecting an appropriate time interval for discretization, we can obtain the equation constraint (4) for discrete dynamics.

[0021] x(k+1)=x(k)+f(x(k),u(k))dt(4).

[0022] Furthermore, in the step of simplifying the quadruped robot's treadable area into several polygonal areas, the polygonal areas can be written as A i r i +b i ≥0, i=0,1,2,3, can be used as the constraint of the control quantity, and the combined constraint form can be expressed as Ar+b≥0(5).

[0023] Furthermore, in the step of simplifying the quadruped robot's treadable area into several polygonal areas, friction constraints, motion range constraints, and foot contact constraints can also be considered. The constraints can all be expressed as bilateral inequality constraints, which can be recorded as

[0024] ub≥ g(x, u) ≥ lb (6),

[0025] where ub, lb are upper and lower bounds, g(x, u) is the mathematical model of the constraint.

[0026] Further, a value function step is given to obtain the desired trajectory and the prediction trajectory difference, the contact force, the foot position difference and the norm of

[0027]

[0028] where Q, R1, R2 are adjustable weight coefficients.

[0029] Further, in the step of establishing the whole-body control model of the quadruped robot, the WBC dynamics equation of the quadruped robot based on the floating base is constructed

[0030]

[0031] where A, b, g, τ, f r ,J c are the mass matrix, the Coriolis force, the gravity, the joint torque, and the contact Jacobian matrix, respectively.

[0032] Further, the attitude control, the center of mass control, and the swing leg control of the quadruped robot are divided into different tasks according to the priority from high to low, and the i-th task iteration rule is as follows

[0033] Δq i = Δq i-1 + J i|pre + (e i -J i Δq i-1 ) (9)

[0034]

[0035]

[0036] where

[0037] J c = J i|i-1 N i|i-1

[0038] N + = N0N i|i-1 ... N r (12)

[0039] N0= I - J NMPC f J c

[0040] N i|i-1 = I - J i|i-1 + J i|i-1 (13)

[0041] The acceleration expectation value of the i-th task is:

[0042]

[0043] There are two forms of pseudo-inverse, Dynamic consistent inverse matrix; the other is pseudo-inverse calculated by SVD.

[0044] Further, the four-legged robot whole body control model introduces a terrain constraint step, by merging the constraint form Ar+b≥0 (5) and the relationship between joint space and foot position By the idea of Lyapunov function to establish the high frequency constraint of joint angular velocity and angular acceleration, γ, ξ are adjustable coefficients

[0045]

[0046]

[0047] Set the quadratic programming problem with (15) (16) as constraints.

[0048]

[0049]

[0050] After multiple task space iterations, solve As the joint control instruction.

[0051] Further, according to the expected joint angular velocity and the expected joint angular acceleration, the expected joint angle calculation method is (19), where j is the joint sequence,

[0052]

[0053] Further, in the combination step of the first layer control architecture and the second layer control architecture, the ground reaction force obtained by using the nonlinear model predictive control method and the acceleration instruction are used to calculate the final reaction force by quadratic programming, and the mathematical form of quadratic programming is as follows,

[0054]

[0055] s.t.

[0056]

[0057] where f r NMPC S f is the solution of the nonlinear model predictive control and floating base selection matrix, J c W is the contact Jacobian matrix, δ f , is the body floating base acceleration and contact force relaxation variable;

[0058] (21) is the constraints of the quadratic programming, respectively, the dynamics constraint, the acceleration constraint, the ground reaction force constraint, and the contact force constraint,

[0059] Thus, the joint torque that meets the terrain constraints in the nonlinear model predictive control method and the whole body control model of the quadruped robot is obtained.

[0060] In order to realize better robot control combined with perception information, the robot motion control also takes the terrain information constraint as a constraint condition of motion control. Since the constraint needs to consider the swing leg landing point, a nonlinear constraint is introduced, so the nonlinear model predictive control (NMPC) method is adopted. First, the terrain information landing point stepable area is simplified as a polygon, a polygon mathematical model is established, and can be introduced as a plurality of inequality constraints.

[0061] Since the mass of each leg relative to the body is small, and the NMPC has a high demand for computing power, in order to simplify the calculation of NMPC, the mass of each leg is ignored, only the mass and moment of inertia of the body are considered, and the dynamics equation is established. Thus, the center of mass acceleration and each leg contact force and landing point position can be solved into the equation. Through the multi-joint floating base system, the dynamics equation containing the joint torque can be established, and thus the differential equation of the continuous system can be obtained. In order to construct the NMPC controller, the equality constraints and inequality constraints of the robot are given according to the actual situation. In addition to the above inequality constraints for the landing point, there are whether the leg touches the ground, the contact leg does not slip, the torque limit, the position limit, the friction force constraint and the like.

[0062] The value function can adopt the norm and form of the difference between the expected motion trajectory and the predicted motion trajectory, the minimum contact force, and the difference between the landing point position and the calculated landing point position of the capture point. A reasonable time interval is set to obtain the discrete dynamics equation, and the problem of the NMPC can be solved.

[0063] Due to the computing power limitation, the update frequency of NMPC is slow, about dozens of Hz, while the frequency of robot motion control is 1000 Hz, which makes the robot unable to make real-time adjustment to external disturbance and model error, thereby causing the control effect to be poor, so a high-frequency controller is needed to track the reference value generated by NMPC. The control frequency of the whole body control (WBC) model of the quadruped robot is 1000 Hz, which is consistent with the frequency of motion control, and priority control is adopted, and the terrain constraint is introduced through the control barrier function (CBF), so that the optimal solution for designing the control effect can be searched near the reference value. However, WBC has higher requirements for the dynamics model, because the high-priority task will directly affect the execution of the low-priority task, so the dynamics model of the WBC controller will be more accurate than that of NMPC, and the body, thigh, and shank will be subdivided. Because the low-priority task is the null space of the high-priority task, the low-priority task will not affect the high-priority task, and the CBF has a constraint effect on all tasks.

[0064] At this point, due to the combination of perception information, the landing point position is constrained, which can effectively avoid the positions that cannot be landed in the terrain, and improve the motion performance of the robot.

[0065] The beneficial effects of the present application are that, compared with linear model predictive control, the form of nonlinear model predictive control can add terrain constraints from the planning level, so that the landing point of the quadruped robot meets the dynamics constraints, and through the control barrier function, the terrain constraints can be added to the whole body control, which can adapt to complex terrains such as steps and plum-blossom stakes that have high requirements for landing point positions, and expand the use scenarios of the quadruped robot. Due to the large amount of calculation of the nonlinear model predictive control (NMPC) method, the mass of the legs which accounts for a small proportion of the total weight is ignored to establish the dynamics equation of the single rigid body model of the quadruped robot, so as to achieve the purpose of simplification as much as possible. The continuous dynamics model is discretized for easy computer solution. The optimization goal is set as minimizing the weighted sum of the square of the motion trajectory error, the total contact force, and the landing point error. The results of the final optimization are optimal in tracking the expected trajectory, contact force size, and landing point position. In order to compensate for the slow update frequency of the nonlinear model predictive control (NMPC) method, a high-frequency whole body control (WBC) model control is added based on NMPC. Since no prediction function is added, the amount of calculation is reduced, and a more accurate dynamics model of the quadruped robot based on the floating base can be used. BRIEF DESCRIPTION OF DRAWINGS

[0066] Figure 1 The control flowchart of the present application is shown.

[0067] Figure 2 The schematic diagram of the specific embodiment of the present application is shown. DETAILED DESCRIPTION

[0068] In order to better understand the technical scheme of the present application, the technical scheme in the embodiments of the present application will be described clearly and completely below in conjunction with the accompanying drawings of the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, rather than all the embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative effort should fall within the protection scope of the present application.

[0069] A four-legged robot control method combined with terrain constraints, comprising the following steps:

[0070] S1, a single rigid body model of the four-legged robot is established;

[0071] In the above S1 step, since the four limbs of the four-legged robot have small mass relative to the body, in order to simplify the model as much as possible, the mass of each leg of the four-legged robot is ignored, and only the mass and moment of inertia of the body of the four-legged robot are considered to establish the dynamics equation. Specifically, the single rigid body dynamics model of the four-legged robot is

[0072]

[0073]

[0074]

[0075] wherein is the robot centroid acceleration, f i is the foot force of the i-th leg, g is the gravity acceleration, m is the robot mass, q is the Euler angle, R(q) is the rotation matrix, I, ω are the moment of inertia and angular velocity in the body coordinate system of the robot, r i is the position of the i-th leg in the world coordinate system, and T(q) is the transformation matrix for transforming the body angular velocity to the Euler angular velocity.

[0076] The single rigid body dynamics model is changed to obtain discrete dynamics equation constraints,

[0077] First, the single rigid body model is written in the form of state space, wherein is the state quantity, and u(f, r) is the control quantity, wherein f, r are column vector combinations of all foot foot end forces f i and foot landing positions r i ;

[0078] Then, after discretization at a proper time interval, the discrete dynamics equation constraints (4) are obtained, x(k+1) = x(k) + f(x(k), u(k))dt (4).

[0079] S2 simplifies the area that the quadruped robot can step on into several polygonal areas, which are determined by lines in three-dimensional space;

[0080] In the above step S2, the polygonal area can be written as A i r i +b i ≥0, i=0,1,2,3, can be used as the constraint of the control quantity, and the combined constraint form can be expressed as Ar+b≥0(5).

[0081] In the above step S2, other constraints can also be considered, including friction constraint, motion range constraint, and foot contact constraint. These constraints can be expressed as bilateral inequality constraints, denoted as ub≥g(x,u)≥lb(6), where ub and lb are upper and lower bounds, and g(x,u) is the mathematical model of the constraint.

[0082] In the above step S2, a value function step may also be included. The value function may be set to adopt the difference between the expected motion trajectory and the predicted motion trajectory, the minimum contact force, and the norm sum of the difference between the foothold position and the expected foothold position.

[0083] Among them, Q, R1, and R2 are adjustable weight coefficients.

[0084] S3 uses the nonlinear model predictive control (NMPC) method and adds terrain constraints to obtain the optimal control variables for the expected values ​​of the position, velocity, and acceleration of the quadruped robot's joint space within a time period. This is the first-level control architecture.

[0085] Combining the aforementioned value function and a series of constraints allows us to construct an optimization problem using a nonlinear model predictive control (NMPC) approach. The optimal control variable can be obtained using a nonlinear solver. Due to computing power limitations, NMPC has a slow update frequency of approximately tens of hertz, while the quadruped robot's motion control frequency is 1000 Hz. This makes it impossible for the quadruped robot to make real-time adjustments to external disturbances and model errors, resulting in poor control performance. Therefore, a high-frequency controller is required to track the reference value generated by the NMPC. Here, a quadruped robot whole-body control (WBC) model is employed.

[0086] S4 establishes a whole-body control (WBC) model for the quadruped robot and introduces terrain constraints into it to obtain the optimal control variables for the expected values ​​of the position, velocity, and acceleration of the quadruped robot's joint space. This is the second-level control architecture.

[0087] In the above step S4, since high-frequency control has higher requirements on the dynamic model, the WBC dynamic equation of the quadruped robot based on the floating basis is constructed

[0088]

[0089] where A, b, g, τ, f r ,J c are mass matrix, Coriolis force, gravity, joint torque, contact Jacobian matrix, respectively.

[0090] The posture control, center of mass control, swing leg control of quadruped robot are divided into different tasks from high to low priority, the whole body control (WBC) model of quadruped robot is the zero space of high priority task because of the low priority task, so the low priority task will not affect the high priority task, the i th task iteration rule is as follows

[0091] Δq i =Δq i-1 +J i|pre + (e i -J i Δq i-1 )(9)

[0092]

[0093]

[0094] where

[0095] J i|pre =J i N i-1

[0096] N i-1 =N0N 1|0 …N i-1|i-2 (12)

[0097] N0=I-J c + J c

[0098] N i|i-1 =I-J i|i-1 + J i|i-1 (13)

[0099] The acceleration expectation value of the i th task is:

[0100]

[0101] where there are two forms of pseudo-inverse, The dynamic consistent inverse matrix must be a mass-considered inverse matrix that minimizes the norm of the acceleration. Another option is the pseudo-inverse calculated using SVD. This allows setting task priorities, using null space projection, so that lower-priority tasks do not affect higher-priority tasks, and solving the expected value through iteration.

[0102] By combining the constraint form Ar+b≥0(5) and the relationship between the joint space and the foothold position The high-frequency constraints of joint angular velocity and angular acceleration are established through the Lyapunov function idea, and γ and ξ are adjustable coefficients.

[0103]

[0104]

[0105] Set up a quadratic programming problem with (15)(16) as constraints.

[0106]

[0107]

[0108] After multiple task space iterations, the desired joint angular velocity and desired joint angular acceleration are solved As joint control instructions, terrain constraints can be considered simultaneously in the high-frequency whole-body control (WBC) model of quadruped robots, improving the control effect.

[0109] The expected joint angular velocity and expected joint angular acceleration have been obtained above. The expected joint angle is calculated as (19), where j is the joint sequence. The angle and angular velocity can be used for PD control. The expected joint angular acceleration can be used to solve the optimized torque value in the subsequent quadratic programming.

[0110] The joint control position expectation value is solved above. The joint control position expectation value here and the joint control velocity expectation value calculated above are added to the joint control closed loop in the form of PD control. The joint closed loop here adds feedback of joint position and velocity to the control variable.

[0111] S5 combines the first-layer control architecture with the second-layer control architecture to obtain the joint torque of the quadruped robot that complies with terrain constraints.

[0112] In the above step S5, the ground reaction force and acceleration command obtained by the nonlinear model predictive control (NMPC) method are used to calculate the final reaction force through quadratic programming. The mathematical form of quadratic programming is as follows:

[0113]

[0114] s.t.

[0115]

[0116] where f r NMPC ,S f is the solution of NMPC and floating base selection matrix, J c ,W is the contact Jacobian matrix, δ f , is the body floating base acceleration and contact force slack variable;

[0117] (21) are the constraints of the quadratic programming, respectively, the dynamics constraint, the acceleration constraint, the ground reaction force constraint, and the contact force constraint, thus the joint torques that satisfy the terrain constraints are obtained in both the nonlinear model predictive control (NMPC) method and the whole body control (WBC) model of the quadruped robot.

[0118] Assume that the quadruped robot walks on a discontinuous platform with a trot gait, and the platform is a square (dark gray square) with a side length of 2 m, and a 30 cm wide ditch needs to be crossed in the middle. The quadruped robot (light gray rectangle) is currently located at the center of the square, and the current center of the quadruped robot is the origin of the world coordinate system, as shown in Figure 2 .

[0119] The entire calculation process is described with specific examples,

[0120] Let S = [s1, s2, s3, s4] be the contact vector of the robot, and the value is 0 for the swing state and 1 for the contact state. Assume that the robot walks with a trot gait, and in a gait cycle, the first and third legs of the robot are in contact with the ground (S = [1, 0, 1, 0]) for 50% of the time, and the second and fourth legs of the robot are in contact with the ground (S = [0, 1, 0, 1]) for the remaining 50% of the time. According to (1) (2) (3), we can get,

[0121]

[0122]

[0123]

[0124] The above formula can be written as

[0125]

[0126] ​The discretization time dt = 0.03 seconds and the prediction length is 10. (4) can be written as x(k+1) = x(k) + f(x(k), u(k)) × 0.03 where k = 0, 1, 2, ..., 9 and x(0) is the current state of the robot.

[0127] For the mathematical model of the steppable area polygon, take the first leg as the swing leg as an example, the steppable area at this time is Figure 2 So we can give the mathematical model of the first leg landing point in the two squares:

[0128]

[0129]

[0130] Switching between these two constraints based on the current position of the leg and combining it with the constraints of the other legs yields (5)

[0131] For other constraints, take the friction constraint when the second leg is supporting as an example. By setting the friction coefficient of the ground to 0.3, we can obtain the inequality constraint on the friction force, where f2(0), f2(1), and f2(2) are the x, y, and z components of the contact force of the second leg.

[0132] -f2(0)≤0.3f2(2)≤f2(0)

[0133] -f2(1)≤0.3f2(2)≤f2(1)

[0134] Similarly, combining other types of constraints, we can get (6)

[0135] The reference trajectory is generally calculated by the handle or navigation command. Here, the handle command is taken as an example. The handle gives the speed command ω ref , After k·dt time integration, we can get q ref ,p ref , so that the reference trajectory x at the kth moment can be obtained ref (k). For r ref (k), you can use the idea in Raibert's book "Legged Robot That Balance" and use k is an empirical parameter, which can be set to 0.05, and T is the single-leg support time. So far, we can get (7).

[0136] The floating-based dynamics model of the quadruped robot is established, and (8) is obtained. The whole-body control (WBC) part of the quadruped robot has three tasks, which are robot posture control, center of mass control, and swing leg control in descending order of priority. Taking robot posture control as an example, e0 is the posture angle error.

[0137] J 0|pre =J0N0

[0138]

[0139] Δq0=J 0|pre + e0

[0140]

[0141]

[0142]

[0143] Subsequent tasks can be calculated according to (9)-(13).

[0144] The previously calculated Substituting (15)-(18) and Δq into (19), we can obtain and q cmd .in and q cmd The position of the swinging leg can be directly controlled via PD control. The joint torque of the final supporting leg can be solved by substituting (20)-(21).

[0145] The above specific embodiments are used to illustrate the present invention rather than to limit the present invention. Any modifications and changes made to the present invention within the spirit of the present invention and the protection scope of the claims shall fall within the protection scope of the present invention.

Claims

1. A quadruped robot control method combined with terrain constraints, characterized in that: The following steps are involved: Establish a single rigid body model of a quadruped robot; The area that the quadruped robot can step on is simplified into several polygonal areas, which are determined by lines in three-dimensional space; the polygonal areas are written as , as the constraint of the control quantity, the combined constraint form is expressed as (5); A nonlinear model predictive control method is used, and terrain constraints are added to it to obtain the optimal control variables for the expected values ​​of the position, velocity, and acceleration of the quadruped robot's joint space within a time period. This is the first-level control architecture. A full-body control model of a quadruped robot is established, and terrain constraints are introduced into it to obtain the optimal control quantity of the expected value of the position, velocity, and acceleration of the quadruped robot's joint space, which is the second-level control architecture; by merging the constraints (5) And the relationship between joint space and foothold position , establish high-frequency constraints of joint angular velocity and angular acceleration through the Lyapunov function idea, Adjustable coefficient （15） （16） Set up a quadratic programming problem with (15) (16) as constraints; （17） （18） After multiple task space iterations, the solution is as joint control instructions; The first-layer control architecture is combined with the second-layer control architecture to obtain the joint torque of the quadruped robot that complies with terrain constraints.

2. The quadruped robot control method incorporating terrain constraints according to claim 1, characterized in that: In the step of establishing a single rigid body model of the quadruped robot, the mass of each leg of the quadruped robot is ignored, and only the mass and moment of inertia of the quadruped robot body are considered to establish the dynamic equation.

3. The method for controlling a quadruped robot in combination with terrain constraints according to claim 2, characterized in that: The single rigid body dynamics model of the quadruped robot is: （1） （2） （3） in is the acceleration of the robot's center of mass, is the foot end force of the i-th leg, is the acceleration due to gravity, is the robot quality, is the Euler angle, is the rotation matrix, are the moment of inertia and angular velocity of the robot body coordinate system, is the position of the i-th leg in the world coordinate system, is the transformation matrix that transforms the body angular velocity into Euler angular velocity.

4. The method for controlling a quadruped robot in combination with terrain constraints according to claim 1, wherein: In the step of simplifying the quadruped robot's treadable area into several polygonal areas, friction constraints, motion range constraints, and foot contact constraints are also considered. The constraints are all expressed as bilateral inequality constraints, denoted as （6）， in For the upper and lower bounds, A mathematical model for constraints.

5. The quadruped robot control method incorporating terrain constraints according to claim 1, characterized in that: In the step of establishing the whole-body control model of the quadruped robot, the WBC dynamic equation of the quadruped robot based on the floating basis is constructed. （8） in They are mass matrix, Coriolis force, gravity, joint torque, all foot end forces, and contact Jacobian matrix.

6. The method for controlling a quadruped robot in combination with terrain constraints according to claim 5, characterized in that: The quadruped robot posture control, center of mass control, and leg swing control are divided into different tasks according to their priority from high to low. The iteration rule of the i-th task is as follows （9） （10） （11） in （12） （13） The expected acceleration of the i-th task is: （14） There are two forms of pseudo-inverse, Dynamic consistent inverse matrix; the other is the pseudo-inverse calculated by SVD.

7. The quadruped robot control method incorporating terrain constraints according to claim 1, characterized in that: In the step of combining the first-layer control architecture and the second-layer control architecture, the ground reaction force and acceleration command obtained by the nonlinear model predictive control method are used to calculate the final reaction force through quadratic programming. The mathematical form of quadratic programming is as follows: （20） （21） in Select matrices for the solution and floating basis of nonlinear model predictive control, is the contact Jacobian matrix, are the relaxation variables of the body floating base acceleration and contact force; (21) is the constraints of the quadratic programming, which are dynamic constraints, acceleration constraints, ground reaction constraints, and contact force constraints. At this point, the joint torques that meet the terrain constraints in both the nonlinear model predictive control method and the quadruped robot full-body control model are obtained.

Images