A dynamic walking control method for humanoid robot

Abstract

The present invention discloses a method for dynamic walking control of a humanoid robot. In the modeling stage, the mass and inertia of the connecting rods of each part of the humanoid robot are converted to the center of mass, and the humanoid robot is simplified into an overall model with equal mass and equal inertia at the center of mass, which simplifies the model and retains the main information of the entire robot system. Since the present invention takes into account the effect of each connecting rod of the robot on the whole, the modeling accuracy is improved, and the robot state can be controlled more accurately. Secondly, when establishing the force model of the ground on the robot, the three-dimensional force and the three-dimensional torque are considered at the same time, so that the model can be universally used on humanoid robots with / without ankles. Finally, in the controller optimization problem solving stage, the entire problem is organized into a compact convex quadratic programming problem, which improves the calculation speed. By using the Riccati equation to determine the weight matrix of the terminal state, the stability of the proposed model prediction controller can be guaranteed.

Description

Technical Field

[0001] The invention belongs to the field of robot control, and in particular relates to a dynamic walking control method of a humanoid robot. Background Art

[0002] The movement of humanoid robots is characterized by discrete footholds, so they have strong ability to pass uneven and discontinuous terrain. At the same time, they also have a similar body shape and mobility to humans, have good adaptability to existing artificial tools, can be well integrated into the human living environment, and are expected to be applied in human life scenes.

[0003] The dynamic walking of humanoid robots has always been a hot topic in humanoid robot research. The dynamic walking of humanoid robots needs to consider the posture balance and center of mass speed of the robot. Among them, posture balance mainly refers to the internal force generated by the interaction between the supporting legs and the ground to maintain the posture angle and height stability of the body. Center of mass speed mainly refers to the control of traction and the foothold of the swing leg to make the center of mass of the robot follow the set speed in the horizontal direction. Compared with the center of mass speed control, the posture balance control of the robot is more challenging. How to obtain the torque of each joint of the supporting leg and realize the balance control of the posture has always been a research difficulty. In particular, uneven terrain will bring greater uncertainty to the robot. The posture balance controller must balance these disturbances and maintain the stability of the robot's posture.

[0004] In general, there are relatively few robots in the world that can achieve dynamic walking on uneven terrain. Most early humanoid robots achieved static or quasi-static walking based on the zero moment point (ZMP) criterion, that is, the ZMP was measured and calculated by the six-dimensional force / torque sensor on the sole of the foot, and the robot's stability was judged based on whether the ZMP was within the support domain of the sole. The ZMP solution generally requires robots to have large soles due to the need to install sole sensors and to increase the stable support domain of the sole. However, the robot's sole cannot fully contact the ground on uneven terrain, and the sole sensor cannot measure accurate data, which can easily lead to control failure. Therefore, humanoid robots with the ZMP solution usually have poor stability on rugged outdoor terrain.

[0005] The ATRIAS robot launched by Oregon State University has the ability to dynamically walk on uneven outdoor terrain. ATRIAS simplifies the robot into a spring inverted pendulum model (SLIP), which regards the robot's legs as springs that can store and release energy. Therefore, the legs of the ATRIAS robot are designed as a planar four-bar structure equipped with springs. ATRIAS can achieve stable walking on outdoor grass. However, the SLIP solution has high requirements for the mechanical structure design of the robot, and its controller has poor universality.

[0006] In recent years, the model predictive controller widely used in the chemical process industry has begun to be used for legged robot control. MIT's quadruped robot Cheetah 3 uses a model predictive controller based on a single rigid body model to achieve stable walking on stairs and grass, with a maximum movement speed of 3m / s. However, the bipedal structure of humanoid robots is quite different from that of quadrupeds, and the above-mentioned quadruped control scheme is difficult to be directly applied to humanoid robots. Up to now, there is little public information on the model predictive control scheme for dynamic walking of humanoid robots. The few patents, such as patent number CN111674486A "Bipedal Robot Stable Walking Control Method, Device, Equipment and Readable Medium", and the paper "The MIT Humanoid Robot: Design, Motion Planning, and Control For Acrobatic Behaviors", proposed a model predictive controller scheme for dynamic walking of humanoid robots, which achieved certain results in the simulation environment, but there are also the following problems:

[0007] First, when modeling the humanoid robot, the leg linkage is directly ignored, and the upper body is regarded as a whole. However, the legs of the humanoid robot are heavy, and the legs will generate a large inertia at the center of mass when moving. At the same time, the swing of the upper body arms will also generate a large inertia at the center of mass. To achieve precise control of the robot state, the effect of the legs and arms on the system should be considered when modeling. Secondly, when describing the effect of the ground on the robot, it only considers three-dimensional force, but most actual humanoid robots have ankles, and the effect of ground torque should be considered. Finally, when organizing the quadratic programming problem of the controller, the terminal state weight matrix problem is not considered, which may make the proposed model predictive controller unstable. Summary of the invention

[0008] The purpose of the present invention is to provide a method for controlling dynamic walking of a humanoid robot in view of the shortcomings of the prior art, such as poor robustness and weak universality of the controller when the robot dynamically walks on uneven terrain.

[0009] The object of the present invention is achieved through the following technical solutions: A method for controlling dynamic walking of a humanoid robot, comprising the following steps:

[0010] Step S1, constructing a humanoid robot mass center inertia model;

[0011] Step S2, writing the center of mass inertia model as a state space equation, and predicting the robot state through the recursive relationship of the linear time-invariant system;

[0012] Step S3, by minimizing the difference between the predicted state and the expected state, is designed as a compact convex quadratic programming problem;

[0013] Step S4, solving the convex quadratic programming problem and controlling the robot according to the obtained parameters.

[0014] Furthermore, step S1 includes: based on the mass, center of mass, and minimum inertia parameter set information of each link of the robot, calculating the inertia of the link relative to its joint coordinate system, and finally converting the inertia and mass at each joint coordinate system to the center of mass of the robot, the total inertia and total mass at the center of mass of the robot can be obtained, that is, the robot is regarded as a whole with equal mass and inertia at the center of mass.

[0015] Furthermore, step S2 includes: selecting important state quantities in humanoid robot control, writing the state space equation of the system based on the center of mass inertia model, and then writing the relationship between the future state and the initial state and the control output of each step according to the recursive relationship of the linear time-invariant system.

[0016] Furthermore, step S3 includes: setting the expected state trajectory in the finite time domain, minimizing the difference between the set state trajectory and the predicted trajectory, solving the infinite time domain discrete Riccati equation to determine the weight matrix of the terminal state, and then organizing the problem into a compact convex quadratic programming problem.

[0017] Furthermore, step S4 includes: using the Riccati equation to solve the terminal weights of the quadratic programming problem to ensure the stability of the controller, solving the compact quadratic programming problem, taking the ground force / torque of the first step as the control output, and using the supporting leg Jacobian matrix to convert it to the supporting leg joint torque to achieve the posture balance control of the robot; at the same time, let the swing leg follow the landing point calculated by the empirical formula to achieve dynamic walking of the robot.

[0018] Furthermore, step S1 includes the following sub-steps:

[0019] S1.1: Calculate the center of mass c of the humanoid robot using the following formula:

[0020]

[0021] Where K represents the total number of links of the robot, m k is the mass of the connecting rod k, c k is the center of mass position of the link k relative to the world coordinate system, and M is the overall mass of the robot;

[0022] S1.2: The inertia Ik of the link k relative to its joint coordinate system is:

[0023]

[0024] in,[] × Indicates the conversion of a vector into its corresponding cross product matrix, T represents the transpose of the matrix, 1 3×3 is a 3×3 unit matrix, l k is the center of mass position of link k relative to its own joint coordinate system, is the mass center inertia of the connecting rod k;

[0025] S1.3: Inertia of connecting rod k converted to the center of mass o for:

[0026]

[0027]

[0028] in, k R o represents the 3×3 transformation matrix from the center of mass coordinate system to the link k joint coordinate system, is the vector from the center of mass o to the origin of the link k joint coordinate system, 0 3×3 is a 3×3 zero matrix, k X o is the 6-dimensional transformation matrix from the center of mass coordinate system to the link k-joint coordinate system;

[0029] S1.4: The total inertia at the center of mass is obtained by adding up the inertia of each connecting rod at the center of mass. com .

[0030] Furthermore, step S2 includes the following sub-steps:

[0031] S2.1: Take the attitude angle θ, center of mass position p, and center of mass angular momentum L of the robot's center of mass inertia model com , center of mass linear momentum P com is the state variable x, x=[θ,p,L com ,P com ] T , take the control variable u as the plantar force f and torque τ of the supporting leg, u=[f,τ] T , we get the following state space equation:

[0032]

[0033]

[0034] Among them, E is the transformation matrix of attitude angle change rate and angular velocity, r is the vector from the center of mass to the sole of the supporting leg;

[0035] S2.2: After the state space equation is discretized, we get x l+1 =A d x l +B d u l +C d According to the recursive formula of the linear time-invariant system, based on the state and the control output of each step, the future state is predicted. The prediction equation is as follows:

[0036]

[0037] Among them, x l represents the state of the next step l, x0 represents the initial state, A d j Indicates A d The jth power, A d l Indicates A d The lth power, u l-1-j Represents the control variable for the future l-1-j steps.

[0038] Furthermore, step S3 includes the following sub-steps:

[0039] S3.1: Let the expected trajectory of step l be x l,ref , by minimizing the difference between the predicted state and the expected state, we get the following optimal control problem:

[0040]

[0041] st.

[0042] x l+1 =A d x l +B d u l +C d ,l=0,...,N-1

[0043] x l ∈X stage ,u l ∈U stage ,l=0,...,N-1

[0044] x N ∈X f

[0045] x0=x(t)

[0046] P≥0,Q≥0,R>0

[0047] Where N is the prediction step length; x N is the terminal state, x N,ref represents the desired terminal state; x l is the process state in a prediction cycle, x l,ref is the corresponding expected state; u l is the process control variable in a prediction cycle; P is the terminal state weight matrix, Q is the process state weight matrix, and R is the control variable weight matrix; X stage represents the process state constraint, U stage represents the process control constraint, X f represents the terminal state constraint, x(t) represents the state at time t, and || || represents the 2-norm operation;

[0048] S3.2: The optimal control problem of S3.1 is transformed into a compact quadratic programming problem, taking X = [x0, x1, ..., x N ] T ,U=[u0,u1,...,u N-1 ] T , the prediction equation is transformed into the following matrix form:

[0049] X=A qp x0+B qp U+C qp

[0050]

[0051] Among them, 1 12×12 is a 12 by 12 identity matrix, setting the desired trajectory X ref =[x 0,ref ,x 1,ref ,...x N,ref ] T , the optimal control problem is transformed into the following problem:

[0052]

[0053] Among them, W Q is a diagonal matrix composed of the state weight matrices of each step, W Q =diag(Q,Q,...,Q,P);W R is a diagonal matrix composed of the weight matrices of each step control quantity, W R =diag(R,R,...,R);

[0054] S3.3: After expanding the matrix, it is transformed into a compact convex quadratic programming problem:

[0055]

[0056] H=2B qp T W Q B qp +W R

[0057] G=2B qp T W R (A qp x0+C qp -X ref ).

[0058] Further, step S4 includes the following sub-steps:

[0059] S4.1: The terminal weight matrix P is obtained by solving the infinite time domain discrete Riccati equation:

[0060] P=Q+A d T PA d -(A d T PB d )(R+B d T PB d ) -1 (B d T PA d )

[0061] S4.2: Solve the convex quadratic programming problem in S3 to obtain all optimal control outputs within the prediction period. Take the ground force / torque of the first step as the control quantity, and map it to the joint torque of the supporting leg through the joint space Jacobian matrix to achieve the posture balance control of the robot. The torque mapping formula is as follows:

[0062]

[0063] Among them, τ joint is the joint torque of the supporting leg, J is the Jacobian matrix of the supporting leg, and F ground ,τ ground is the first step ground force and moment;

[0064] Calculate the robot's landing point p:

[0065] p=p const +k p v curr +k d (v curr -vd )

[0066] Among them, p const represents the foothold constant bias, k p represents the landing point ratio parameter, k d represents the foothold differential parameter; v curr ,v d Indicates the current speed and expected speed of the robot.

[0067] The beneficial effects of the present invention are as follows: compared with the anthropomorphic model predictive control scheme in the prior art, the present invention not only improves the modeling accuracy but also ensures the stability of the controller, and the overall control method of the present invention has better universality and robustness. BRIEF DESCRIPTION OF THE DRAWINGS

[0068] The drawings described herein are used to provide further understanding of the present invention and constitute a part of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute improper limitations on the present invention.

[0069] Figure 1 Schematic diagram of the overall model of the humanoid robot in an embodiment of the present invention; wherein (a) is a side view and (b) is a front view.

[0070] Figure 2 The present invention is a schematic flow chart of a method for controlling dynamic walking of a humanoid robot according to an embodiment of the present invention.

[0071] Figure 3 Schematic diagram of the center of mass inertia model in an embodiment of the present invention.

[0072] In the figure, body 1, waist 2, left thigh 3, left calf 4, left foot 5, right thigh 6, right calf 7, right foot 8, left upper arm 9, left forearm 10, right upper arm 11, and right forearm 12. DETAILED DESCRIPTION

[0073] The present invention will be further described below in conjunction with the accompanying drawings.

[0074] like Figure 1 As shown, the humanoid robot of the present invention is composed of a body 1, a waist 2, arms and legs; wherein the arms include a left upper arm 9, a left lower arm 10, a right upper arm 11, and a right lower arm 12. The legs include a left thigh 3, a left calf 4, a left sole 5, a right thigh 6, a right calf 7, and a right sole 8. The hip joint of the leg includes three degrees of freedom: yaw, pitch, and roll; the knee joint includes one degree of freedom: pitch; and the ankle joint includes two degrees of freedom: pitch and roll; all degrees of freedom are driven by motors, and the six-degree-of-freedom legs make the position of the robot foot relative to the robot body completely controllable.

[0075] The present invention discloses a method for controlling dynamic walking of a humanoid robot. First, in the modeling stage, the mass and inertia of the connecting rods of each part of the humanoid robot are converted to the center of mass, and finally the complex multi-degree-of-freedom system of the humanoid robot is simplified into an overall model with equal mass and equal inertia at the center of mass, which simplifies the model and retains the main information of the entire robot system. Compared with the single rigid body model commonly used in the disclosed humanoid robot, which ignores the legs and regards the trunk as a whole, the present invention improves the modeling accuracy by considering the effect of each connecting rod of the robot on the whole, and can make more accurate control of the robot state. Secondly, when establishing the force model of the ground on the robot, the three-dimensional force and the three-dimensional torque are considered at the same time, so that the model can be universally used on humanoid robots with / without ankles. Finally, in the controller optimization problem solving stage, the whole problem is organized into a compact convex quadratic programming problem, which improves the calculation speed. By using the Riccati equation to determine the weight matrix of the terminal state, the stability of the proposed model prediction controller can be guaranteed.

[0076] like Figure 2 As shown, the specific steps include:

[0077] Step S1, considering the inertia of the humanoid robot's arms and legs and the torque effect of the ground, a more accurate center of mass inertia model of the humanoid robot is constructed.

[0078] Specifically, Figure 3 As shown in the figure, the inertia of all the links of the humanoid robot is concentrated at the center of mass and regarded as a Figure 3 For a whole with equal mass and inertia at the center of mass, the dynamic equation at the center of mass is as follows:

[0079] P com =Mv

[0080]

[0081]

[0082]

[0083]

[0084] Among them, P com is the linear momentum of the center of mass, including the three directions of x, y, and z, M is the overall mass of the robot, and v is the linear velocity of the center of mass; L com is the angular momentum of the center of mass, including the three directions of x, y, and z, J com is the mapping matrix from generalized velocity to center-of-mass angular momentum, is the generalized velocity of the robot; w is the angular velocity of the center of mass, I com is the total inertia at the center of mass; n is the number of supporting legs, f i,τ i is the ground force and torque at the foot of the supporting leg i, including the three directions of x, y, and z; r i is the vector from the center of mass to the foot of the supporting leg i, and g is the acceleration due to gravity.

[0085] In this embodiment, the humanoid robot has at most two supporting legs, so n is at most 2. Figure 3 In, f x1 ,f y1 ,f z1 ,τ x1 ,τ y1 ,τ z1 is the ground force and moment in the x, y, and z directions at the foot of the supporting leg 1 (left leg), r1 is the vector from the center of mass to the foot of the supporting leg 1, and f x2 ,f y2 ,f z2 ,τ x2 ,τ y2 ,τ z2 are the ground forces and moments in the x, y, and z directions at the foot of supporting leg 2 (right leg), and r2 is the vector from the center of mass to the foot of supporting leg 2.

[0086] Compared with the existing method, the inertia of each part of the arm and leg is converted into the total inertia I at the center of mass during the modeling stage of the present invention. com , which improves the modeling accuracy and enables more effective control of the robot. At the same time, the ground torque is taken into account, making the model universal on humanoid robots with or without ankles. Next, the main calculation steps of the equal mass and equal inertia model at the center of mass are explained.

[0087] First, calculate the center of mass c of the humanoid robot by the following formula:

[0088]

[0089] Where K represents the total number of links of the robot, m k is the mass of the connecting rod k, c k is the center of mass position of link k relative to the world coordinate system, and M is the overall mass of the robot. Then, the inertia of each link of the robot needs to be uniformly converted to the center of mass. According to the mechanical design drawing of the robot, the minimum inertia parameter set of each link can be obtained. Taking link k as an example, l k is the center of mass position of link k relative to its own joint coordinate system, m k is the mass of the connecting rod k, is the mass center inertia of connecting rod k.

[0090] The inertia of the link k relative to its joint coordinate system is k for:

[0091]

[0092] in,[] × Indicates the conversion of a vector into its corresponding cross product matrix, T represents the transpose of the matrix, 1 3×3 is a 3×3 identity matrix.

[0093] Take the 6-dimensional transformation matrix from the center of mass coordinate system (the origin is located at the center of mass, the origin is denoted by o, and the directions of each axis are the same as the world coordinate system) to the link k joint coordinate system k X o , then the inertia of the connecting rod k converted to the center of mass o is for:

[0094]

[0095]

[0096] in, k R o represents the 3×3 transformation matrix from the center of mass coordinate system to the link k joint coordinate system, is the vector from the center of mass o to the origin of the link k joint coordinate system, 0 3×3 is a 3×3 zero matrix.

[0097] The total inertia at the center of mass is obtained by adding up the inertia of each connecting rod at the center of mass. com .

[0098] Step S2, writing the center-of-mass inertia model as a state-space equation, and predicting the robot state through the recursive relationship of the linear time-invariant system.

[0099] Specifically, take the attitude angle θ, center of mass position p, center of mass angular momentum L of the robot's center of mass inertia model com , center of mass linear momentum P com is the state variable x, that is, x=[θ,p,L com ,P com ] T , take the control variable u as the plantar force f and torque τ of the supporting leg, that is, u=[f,τ] T , we get the following state space equation:

[0100]

[0101]

[0102] Among them, E is the transformation matrix of attitude angle change rate and angular velocity, and r is the vector from the center of mass to the sole of the supporting leg.

[0103] Compared with the existing method of taking angular velocity as the state quantity, the present invention uses angular momentum to describe the speed of the robot's rotation, avoiding the existing method The whole model is more accurate.

[0104] After the state space equation of the present invention is discretized, x is obtained. l+1 =A d x l +B d u l +C d According to the recursive formula of the linear time-invariant system, the future state can be predicted based on the initial state and the control output of each step. The prediction equation is as follows:

[0105]

[0106] Among them, xl represents the state of the next l-th step, x0 represents the initial state, A d j Indicates A d The jth power, A d l Indicates A d The lth power, u l-1-j Represents the control variable for the future l-1-j steps.

[0107] Step S3, by minimizing the difference between the predicted state and the expected state, a compact convex quadratic programming problem is organized.

[0108] Let the expected trajectory of step l be x l,ref , by minimizing the difference between the predicted state and the expected state, the following optimal control problem can be obtained:

[0109]

[0110] st.

[0111] x l+1 =A d x l +B d u l +C d ,l=0,...,N-1

[0112] x l ∈X stage ,u l ∈U stage ,l=0,...,N-1

[0113] x N ∈X f

[0114] x0=x(t)

[0115] P≥0,Q≥0,R>0

[0116] Where N is the prediction step length; x N is the terminal state, x N,ref represents the desired terminal state; x l is the process state in a prediction cycle, x l,ref is the corresponding expected state; u l is the process control variable in a prediction cycle; P is the terminal state weight matrix, Q is the process state weight matrix, and R is the control variable weight matrix. The state variables and control variables need to satisfy their own constraints, such as ground contact friction constraints, joint torque constraints, etc.; X stage represents the process state constraint, U stage represents the process control constraint, X f represents the terminal state constraint, x(t) represents the state at time t, and || || represents the 2-norm operation.

[0117] In order to speed up the calculation time, the above optimal control problem is transformed into a compact quadratic programming problem, taking X = [x0, x1, ..., x N ] T ,U=[u0,u1,...,u N-1 ] T , the above prediction equation can be transformed into the following matrix form:

[0118] X=A qp x0+B qp U+C qp

[0119]

[0120] Among them, 1 12×12 is a 12 by 12 identity matrix, setting the desired trajectory X ref =[x 0,ref ,x 1,ref ,...x N,ref ] T , the above optimal control problem can be transformed into the following problem:

[0121]

[0122] Among them, W Q is a diagonal matrix composed of the state weight matrices of each step, that is, W Q =diag(Q,Q,...,Q,P);W R is a diagonal matrix composed of the weight matrices of each step control quantity, that is, W R =diag(R,R,...,R).

[0123] After expanding the matrix, it can be transformed into a compact convex quadratic programming problem:

[0124]

[0125] H=2B qp T W Q B qp +W R

[0126] G=2B qp T W R (A qp x0+C qp -X ref ).

[0127] Step S4, using the Riccati equation to solve the terminal weights of the quadratic programming problem, ensuring the stability of the controller, solving the convex quadratic programming problem, and controlling the robot according to the obtained parameters.

[0128] The terminal weight matrix P is obtained by solving the infinite time domain discrete Riccati equation, that is:

[0129] P=Q+A d T PA d -(A d T PB d )(R+B d T PB d )- 1 (B d T PA d )

[0130] Compared with the existing method of directly manually adjusting the terminal weights, the present invention uses the Riccati equation to solve the terminal weights to ensure the stability of the model predictive controller.

[0131] Specifically, by solving the above convex quadratic programming problem, we can obtain all the optimal control outputs within the prediction period. We take the ground force / torque of the first step as the control quantity and map it to the joint torque of the supporting leg through the joint space Jacobian matrix to achieve the posture balance control of the robot. The torque mapping formula is as follows:

[0132]

[0133] Among them, τ joint is the joint torque of the supporting leg, J is the Jacobian matrix of the supporting leg, and F ground ,τ ground These are the first step ground forces and moments obtained by MPC solution.

[0134] The current speed v of the robot is knowncurr and the expected speed v d , the robot landing point p can be calculated according to the following empirical formula:

[0135] p=p const +k p v curr +k d (v curr -v d )

[0136] Among them, p const represents the foothold constant bias, k p represents the landing point ratio parameter, k d Represents the foothold differential parameters; these three parameters need to be manually debugged in actual engineering.

[0137] After calculating the landing point according to the empirical formula, the expected position and speed of each joint of the swing leg are obtained through inverse kinematics. Following the landing point, the speed control of the robot is completed, and finally the dynamic walking of the humanoid robot is realized.

[0138] The present invention is not limited to the above-mentioned embodiments. All other embodiments obtained by ordinary technicians in the field without making creative work in the same or similar manner as the above-mentioned embodiments of the present invention are within the protection scope of the patent of this invention.

Claims

1. A humanoid robot dynamic walking control method, characterized in that: The following steps are involved: Step S1, constructing a humanoid robot mass center inertia model; based on the mass, mass center, and minimum inertia parameter set information of each link of the robot, calculating the inertia of the link relative to its joint coordinate system, and finally converting the inertia and mass at each joint coordinate system to the robot mass center to obtain the total inertia and total mass at the robot mass center, and considering the robot as a whole with equal mass and equal inertia at the mass center; S1.1: Calculate the center of mass c of the humanoid robot using the following formula: Where K represents the total number of links of the robot, m k is the mass of the connecting rod k, c k is the center of mass position of the link k relative to the world coordinate system, and M is the overall mass of the robot; S1.2: Inertia I of link k relative to its joint coordinate system k for: in,[ ] × Indicates the conversion of a vector into its corresponding cross product matrix, Τ indicates the transpose of the matrix, 1 3×3 is a 3×3 unit matrix, l k is the center of mass position of link k relative to its own joint coordinate system, is the mass center inertia of the connecting rod k; S1.3: Inertia of connecting rod k converted to the center of mass o for: in, k R o represents the 3×3 transformation matrix from the center of mass coordinate system to the link k joint coordinate system, is the vector from the center of mass o to the origin of the link k joint coordinate system, 0 3×3 is a 3×3 zero matrix, k X o is the 6-dimensional transformation matrix from the center of mass coordinate system to the link k-joint coordinate system; S1.4: The total inertia at the center of mass is obtained by adding up the inertia of each connecting rod at the center of mass. com ; Step S2, writing the center-of-mass-inertia model as a state-space equation, and predicting the robot state through the recursive relationship of the linear time-invariant system; selecting important state quantities in the humanoid robot control, writing the state-space equation of the system based on the center-of-mass-inertia model, and then writing the relationship between the future state and the initial state and the control output of each step according to the recursive relationship of the linear time-invariant system; S2.1: Take the attitude angle θ, center of mass position p, and center of mass angular momentum L of the robot's center of mass inertia model com , center of mass linear momentum P com is the state variable x, x=[θ,p,L com ,P com ] Τ , take the control variable u as the plantar force f and torque τ of the supporting leg, u=[f,τ] T , we get the following state space equation: Among them, E is the transformation matrix of attitude angle change rate and angular velocity, r is the vector from the center of mass to the sole of the supporting leg; S2.2: After the state space equation is discretized, we get x l+1 =A d x l +B d u l +C d According to the recursive formula of the linear time-invariant system, based on the state and the control output of each step, the future state is predicted. The prediction equation is as follows: Among them, x l represents the state of the next step l, x0 represents the initial state, A d j Indicates A d The jth power, A d l Indicates A d The lth power, u l-1-j represents the control variable for the future l-1-j steps; Step S3, by minimizing the difference between the predicted state and the expected state, a compact convex quadratic programming problem is designed; the expected state trajectory of the finite time domain is set, the difference between the set state trajectory and the predicted trajectory is minimized, the weight matrix of the terminal state is determined by solving the infinite time domain discrete Riccati equation, and then the problem is organized into a compact convex quadratic programming problem; S3.1: Let the expected trajectory of step l be x l,ref , by minimizing the difference between the predicted state and the expected state, we get the following optimal control problem: st. x l+1 =A d x l +B d u l +C d ,l=0,...,N-1 x l ∈X stage ,u l ∈U stage ,l=0,...,N-1 x N ∈X f x0=x(t) P≥0,Q≥0,R>0 Where N is the prediction step length; x N is the terminal state, x N,ref represents the desired terminal state; x l is the process state in a prediction cycle, x l,ref is the corresponding expected state; u l is the process control variable in a prediction cycle; P is the terminal state weight matrix, Q is the process state weight matrix, and R is the control variable weight matrix; X stage represents the process state constraint, U stage represents the process control constraint, X f represents the terminal state constraint, x(t) represents the state at time t, and || || represents the 2-norm operation; S3.2: The optimal control problem of S3.1 is transformed into a compact quadratic programming problem, taking X = [x0, x1, ..., x N ] Τ ,U=[u0,u1,...,u N-1 ] T , the prediction equation is transformed into the following matrix form: X=A qp x0+B qp U+C qp Among them, 1 12×12 is a 12 by 12 identity matrix, setting the desired trajectory X ref =[x 0,ref ,x 1,ref ,...x N,ref ] Τ , the optimal control problem is transformed into the following problem: Among them, W Q is a diagonal matrix composed of the state weight matrices of each step, W Q =diag(Q,Q,...,Q,P);W R is a diagonal matrix composed of the weight matrices of each step control quantity, W R =diag(R,R,...,R); S3.3: After expanding the matrix, it is transformed into a compact convex quadratic programming problem: H=2B qp Τ W Q B qp +W R G=2B qp Τ W R (A qp x0+C qp -X ref ); Step S4, solving the convex quadratic programming problem, and controlling the robot according to the obtained parameters; using the Riccati equation to solve the terminal weight of the quadratic programming problem, ensuring the stability of the controller, solving the compact quadratic programming problem, taking the ground force / torque of the first step as the control output, using the supporting leg Jacobian matrix to convert it to the supporting leg joint torque, and realizing the posture balance control of the robot; at the same time, let the swing leg follow the landing point calculated by the empirical formula to realize the dynamic walking of the robot; S4.1: The terminal weight matrix P is obtained by solving the infinite time domain discrete Riccati equation: P=Q+A d Τ PA d -(A d Τ PB d )(R+B d Τ PB d ) -1 (B d Τ PA d ) S4.2: Solve the convex quadratic programming problem in S3 to obtain all optimal control outputs within the prediction period. Take the ground force / torque of the first step as the control quantity, and map it to the joint torque of the supporting leg through the joint space Jacobian matrix to achieve the posture balance control of the robot. The torque mapping formula is as follows: Among them, τ joint is the joint torque of the supporting leg, J is the Jacobian matrix of the supporting leg, and F ground ,τ ground is the first step ground force and moment; Calculate the robot's landing point p: p=p const +k p v curr +k d (v curr -v d ) Among them, p const represents the foothold constant bias, k p represents the landing point ratio parameter, k d represents the foothold differential parameter; v curr, v d Indicates the robot's current speed and expected speed.

Images