Humanoid robot step terrain walking method based on whole-body dynamic model

Abstract

The invention discloses a humanoid robot step terrain walking method based on a whole-body kinetic model, and the method comprises the steps: designing a nonlinear model prediction control problem oriented to the walking of a humanoid robot, and reasonably planning a system state, input and a reference track of a foot end; equality constraints and inequality constraints which are necessary for walking of the robot are designed, and the constraint that a landing point of the robot needs to fall in a landing area is taken as one of the inequality constraints, so that the humanoid robot is capable of walking on a step terrain; and finally, solving a nonlinear model predictive control problem in real time by utilizing a solver, and issuing a robot joint torque according to an inverse kinetic equation of the whole-body kinetic model and PD in combination with a feedforward controller, so that the robot is better in control effect and more stable in walking.

Description

Technical Field

[0001] The invention relates to the field of stable walking control methods for humanoid robots, and in particular to a step terrain walking method for humanoid robots based on a whole-body dynamics model. Background Art

[0002] In recent years, the control algorithms of humanoid robots have become more and more mature, making them more and more visible to people. The advantage of humanoid robots over traditional wheeled mobile robots is that they can directly adapt to the production and living environment of humans and can directly use the tools used by humans without changing the environment or redesigning the tools. From the perspective of application scenarios, since wheeled robots are difficult to adapt to terrain with large undulations and flying robots cannot meet the requirements of large loads and complex operations, humanoid robots can perform specific tasks better. Furthermore, compared with quadruped robots, humanoid robots have a smaller footprint and a larger range of activities when walking, and the two robotic arms also provide more possibilities for complex operations. However, due to the more complex structure of the humanoid robot itself, the control difficulty of walking and operation is much greater than that of robots of other configurations such as wheeled and quadruped robots.

[0003] Humanoid robots have developed rapidly in the past decade and have achieved many results, such as Honda's ASIMO, Boston Dynamics' Atlas, Agility Robotics' Cassie and Digit, Tesla's Optimus, etc. After a long period of development, there are many methods for motion control of humanoid robots. The most representative methods are: Zero Moment Point (ZMP), Virtual Model Control (VMC), Hybrid Zero Dynamics (HZD), optimization-based control methods, and reinforcement learning-based control methods.

[0004] At present, the application of humanoid robot controlled by zero moment point method is relatively extensive (such as CN112224300A, CN113830197A), which can achieve stable walking, but has poor anti-disturbance ability and poor dynamic performance. Therefore, in recent years, the method based on optimization control has gradually replaced the zero moment point method. Among them, model predictive control is one of the methods based on optimization control. Because it has the ability to predict the system state for a period of time, it is very suitable for controlling the robust walking of the robot. CN115202378A uses a convex model predictive control method based on a single rigid body model to enable a humanoid robot to walk stably on a flat ground outdoors and has strong robustness. However, since the convex model predictive controller is limited to the convex system dynamic equations and constraints, its system state dimension is low and the information is small, and it is impossible to consider the complete dynamic model, and it is also impossible to design the landing area related to the terrain into the state-related constraints. Therefore, when the robot faces some structured terrains with large fluctuations, such as high platforms, continuous steps, slopes and other terrains, it is impossible to model terrain factors. In most cases, the terrain is regarded as a disturbance, causing it to fall or collide with the environment. In addition, although simplified models such as single rigid body model, inverted pendulum model (such as CN112882467A, CN116300877A), center of mass dynamics model, etc. effectively capture the system dynamics to a certain extent, while reducing the problem size and increasing the control frequency, they cannot accurately and completely represent the system dynamics, resulting in modeling errors and control errors. However, with the increase of existing computing power, some solvers have made it feasible to solve nonlinear model predictive control based on whole-body dynamics model in real time. Summary of the invention

[0005] In order to overcome the limitations of the above-mentioned prior art, the present invention provides a method for a humanoid robot to walk on stair terrain based on a whole-body dynamics model to solve the above-mentioned problems. The present invention constructs a hybrid system for a humanoid robot based on a whole-body dynamics model, and the system modeling is complete and accurate; at the same time, the present invention designs a nonlinear model predictive control problem containing terrain information and solves it in real time, so that the humanoid robot takes terrain factors into account when walking and plans its movement reasonably. The present invention enables the humanoid robot to cross high platforms and continuous steps of different heights in a simulated environment, while having strong walking stability and strong movement ability. The technical solution of the present invention includes the following contents:

[0006] A method for walking on step terrain of a humanoid robot based on a whole-body dynamics model comprises the following steps:

[0007] S1. Draw a grayscale image for representing the step terrain, process the grayscale image to generate a grid map, and then generate a convex plane, a signed distance field, and a terrain in the simulation software from the grid map; wherein the convex plane generated by the grid map is used as a set of landing areas;

[0008] S2. Design the robot system state, system input, system dynamic equations and jump function based on the whole body dynamics model;

[0009] S3, according to the landing area information obtained in step S1 and the system dynamic equation and jump function obtained in step S2, a nonlinear model predictive control problem for the humanoid robot walking on step terrain is designed and solved to obtain the system desired state and input trajectory;

[0010] S4. Based on the result calculated in step S3, the robot joint torque is calculated according to the whole body dynamics model, and the torque is sent to the robot joint by using the PD combined with the feedforward controller to achieve the control effect.

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

[0012] S11, using MATLAB software to draw a grayscale image representing the step terrain;

[0013] S12, converting the grayscale image obtained in step S11 into a grid map, using the grid map to generate a number of convex planes as a set of landing areas, and generating a signed distance field as an external collision detection criterion between the robot and the environment;

[0014] S13, converting the grid map generated in step S12 into terrain in the simulation software for use in simulation debugging.

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

[0016] S21. Design the state quantity in the nonlinear model predictive control based on the whole body dynamics model: Use the Euler-Lagrange method to express the dynamic equation of the humanoid robot, and then determine that the system state x is composed of six items: center of mass position, center of mass Euler angle, joint angle, center of mass linear velocity, center of mass Euler angle change rate, and joint velocity;

[0017] S22, deriving the system dynamic equation according to the system state quantity and the whole body dynamic model, dividing the whole body dynamic equation into two parts: under-actuated and fully-actuated, and then obtaining the body acceleration; selecting the system input u to consist of the joint acceleration and the end contact force-torque;

[0018] S23. Derive the jump function based on the system state quantity and the whole-body dynamics model: Combine the dynamic equation when the system state changes suddenly and assume that the position quantity does not change suddenly, and use the Schur complement method to solve the jump function of the velocity quantity.

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

[0020] S31. Design a nonlinear model predictive control problem for humanoid robot walking: Based on the general form of the control problem, design the reference trajectory, cost function, equality constraints and inequality constraints;

[0021] S32. Use the SQP method to solve the control problem in real time.

[0022] Furthermore, the step S31 includes the following sub-steps:

[0023] S311. Design the reference trajectory of system state x and system input u: the robot controller receives user instructions that only include the end point x, the center of mass position in the y direction and the yaw angle, generates a position reference trajectory of uniform acceleration-uniform speed-uniform deceleration through polynomial interpolation, and then obtains the reference trajectory of the speed through the difference method; the reference value in the z direction needs to take into account the terrain height, the reference values ​​of the two components of roll and pitch remain unchanged, the speed reference value is always zero, and its reference trajectory is a constant value trajectory; the reference landing point of the foot end is calculated by the Raibert heuristic landing point formula; the reference trajectory of the foot end position, velocity and acceleration is generated by interpolation of the swing leg foot end trajectory planner; the reference trajectory of the joint angle is calculated by the reference trajectory of the foot end posture through analytical inverse kinematics, and the reference trajectory of the joint angular velocity is calculated by the reference trajectory of the foot end velocity through the inverse calculation of the Jacobian matrix; the reference of the system input u is zero, and the reference trajectory is a constant value trajectory;

[0024] S312, designing a cost function, including the following two parts: state trajectory tracking cost and input cost;

[0025] S313, design equation constraints, including the following three parts: zero force-torque constraint at the end of the swing leg, double-leg trajectory tracking constraint, and support leg foot end velocity constraint;

[0026] S314. Design inequality constraints and add them to the cost function in the form of penalties, including the following six parts: landing area constraints, self-collision constraints, external collision constraints, joint velocity constraints, joint torque constraints, and friction cone constraints.

[0027] Furthermore, the step S4 includes the following sub-steps:

[0028] S41, calculating the inverse dynamics joint torque feedforward using the inverse dynamics equation of the whole body dynamics model, the body acceleration in step S22, and the solution result of step S3;

[0029] S42, using PD combined with feedforward to calculate the final robot joint torque τ, and send it to the robot in real time.

[0030] The beneficial effects of the present invention are as follows: first, the present invention constructs a hybrid system based on a whole-body dynamics model for a humanoid robot. Compared with a simplified dynamics model of a robot, such as a single rigid body model or a center of mass dynamics model, the whole-body dynamics model has a richer state quantity. The introduction of a jump function can capture the sudden change of the state quantity of the robot when the foot of the humanoid robot lands to a great extent, making the change of the state quantity of the robot more accurate. Then, the present invention designs a nonlinear model predictive control problem for the walking of a humanoid robot, wherein the system state, input, and reference trajectory of the foot are reasonably planned, the equality constraints and inequality constraints necessary for the walking of the robot are reasonably designed, and the constraint that the foot of the robot needs to fall in the landing area is used as one of the inequality constraints, so that the humanoid robot is capable of walking on a stepped terrain. Finally, the present invention uses the solver OCS2Toolbox to solve the nonlinear model predictive control problem in real time, and sends the robot joint torque according to the inverse dynamics equation of the whole-body dynamics model and the PD combined with the feedforward controller, so that the robot has better control effect and more stable walking. BRIEF DESCRIPTION OF THE DRAWINGS

[0031] Figure 1 It is a system framework diagram of the present invention.

[0032] Figure 2 A grayscale image representing terrain generated by an embodiment of the present invention.

[0033] Figure 3 A grid map generated by an embodiment of the present invention.

[0034] Figure 4 A landing area map generated for an embodiment of the present invention.

[0035] Figure 5 A simulated terrain map generated by an embodiment of the present invention.

[0036] Figure 6 This is a schematic diagram of a humanoid robot going up a platform in an embodiment of the present invention.

[0037] Figure 7 Schematic diagram of a humanoid robot climbing continuous steps in an embodiment of the present invention. DETAILED DESCRIPTION

[0038] The present invention is described in detail below in conjunction with specific embodiments. The embodiments described below are exemplary and are only used to explain the present invention, and should not be construed as limiting the present invention. It should be noted that, for those of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present invention. These all belong to the protection scope of the present invention.

[0039] The present invention proposes a step terrain walking method for a humanoid robot based on a whole-body dynamics model. Figure 1 As shown, the specific steps include:

[0040] S1. Generate terrain information used by nonlinear model predictive controller and terrain in simulation software.

[0041] Specifically, MATLAB software is used to draw a grayscale image for characterizing the step terrain; the ElevationMapping Toolbox is used to process the grayscale image to generate a grid map, and then a number of convex planes are generated from the grid map as a set of landing areas, and a signed distance field is generated as a criterion for external collision detection between the robot and the environment; at the same time, the grid map is converted into the terrain in the simulation software Raisim for use in simulation debugging.

[0042] S11. In this embodiment, the method of using MATLAB software to draw a terrain grayscale map is as follows: a matrix is ​​used to represent the entire terrain map, and the number of rows and columns are the length and width of the terrain map respectively, and each element is an integer value of 0 to 255, representing the terrain grayscale. According to the terrain structure to be generated, such as a high platform, continuous stairs, etc., the value of each element of the matrix is ​​determined. Finally, the matrix is ​​converted into a grayscale image and saved, as shown in FIG. Figure 2 shown.

[0043] S12. In this embodiment, the grayscale image is processed to generate the landing area and the signed distance field by first converting the grayscale image into a raster map using the Elevation Mapping Toolbox, such as Figure 3 As shown in FIG. 1 , the convex planes are processed into several convex planes and signed distance fields. The generated convex planes will be used as a set of foot-falling areas, and will be used to generate foot-end reference trajectories in step S311, and to design foot-falling area constraints in step S314, as shown in FIG. Figure 4 The generated signed distance field will be used in step S314 to design external collision constraints.

[0044] S13. In this embodiment, in order to verify the walking performance of the robot in the simulation software, the present invention converts the grid map generated in step S12 into the terrain in the simulation software Raisim for use in simulation debugging. Figure 5 shown.

[0045] S2. Further, a hybrid system equation for a humanoid robot is constructed. Since the bipedal walking process consists of discrete phases of contact with the environment, it naturally needs to be modeled as a hybrid system, which includes a continuous phase represented by the system dynamic equation and a discrete state switching phase represented by the system jump function. Therefore, this step includes designing the state and input quantities in the nonlinear model predictive control, and deriving the system dynamic equation and jump function.

[0046] Specifically, the state quantities in the nonlinear model predictive control are designed based on the whole-body dynamics model to capture the whole-body system dynamics; the system input quantities are designed based on the determined system state quantities and the whole-body dynamics model, and the system dynamic equations are derived to satisfy the form of general dynamic equations; the jump function is derived based on the determined system state quantities and the whole-body dynamics model to capture system mutations.

[0047] S21. Specifically, in this embodiment, the method for designing the state quantity in the nonlinear model predictive control according to the whole body dynamics model is as follows. The configuration q of the floating base humanoid robot control system is composed of the floating base quantity q b and joint volume q j Composition, namely:

[0048]

[0049] where q b is a six-dimensional vector, including the three-dimensional center of mass position and the three-dimensional center of mass Euler angle (roll angle roll, pitch angle pitch, yaw angle yaw) attitude, q j is the joint angle, T is the transpose operation, and then we can get:

[0050]

[0051] in is a six-dimensional vector, including the three-dimensional center-of-mass linear velocity and the three-dimensional center-of-mass angle Euler angle change rate, is the joint velocity.

[0052] Using the Euler-Lagrange method, the system can be represented by the following dynamic equation:

[0053]

[0054] Where M(q) is the inertia matrix, is the Coriolis force and gravity term, B(q) is the joint torque mapping matrix, τ is the joint torque, J c (q) is the contact Jacobian matrix, and λ is the end contact force-torque vector. Assuming that the foot end and the ground remain stationary during contact, that is, the relative velocity is zero, we can obtain:

[0055]

[0056] in, The speed when the foot contacts the ground.

[0057] By taking the derivative of formula (4), we can obtain:

[0058]

[0059] Combined with the definition of the whole body dynamics model, this embodiment defines the system state as:

[0060]

[0061] In summary, the system state x is composed of six items: center of mass position, center of mass Euler angle, joint angle, center of mass linear velocity, center of mass Euler angle change rate, and joint velocity.

[0062] S22. The method for deriving the system dynamic equation based on the determined system state quantity and the whole body dynamics model is as follows. The general form of the system dynamic equation is:

[0063]

[0064] Where u is the system input, f(x) is the uncontrolled state transfer matrix, and g(x) is the system input matrix. Since the whole body dynamics equation (3) can be divided into two parts: the underactuated floating basis and the fully actuated joints, that is:

[0065]

[0066] The first six lines are the underactuated floating basis, and the rest are the fully actuated joints. Thus, the floating basis, i.e. the acceleration of the body, can be obtained:

[0067]

[0068] On this basis, the derivative of formula (6) can be obtained:

[0069]

[0070] Therefore, according to the concept of the whole-body dynamics model, the system input u is selected to consist of the joint acceleration and the end contact force-torque vector, that is:

[0071]

[0072] Then formula (10) can be transformed into:

[0073]

[0074] make This shows that equation (7) conforms to the general form of the system dynamic equation.

[0075] S23. The method for deriving a jump function based on the determined system state quantity and the whole-body dynamics model is as follows. In the process of walking of the humanoid floating-based robot, when the robot contacts the environment, especially when the foot contacts the ground, the state quantity of the system may mutate. Assuming that the contact between the robot and the ground is hard contact, the position and posture of its body and the joint angle will not mutate when in contact, but the six-dimensional velocity and joint velocity of the body will mutate due to this contact. Since the system state quantity selected in step S21 includes the six-dimensional velocity and joint velocity of the body, it is necessary to design a jump function to model this type of mutation.

[0076] The general form of the jump function is x + =f(x - ), where Indicates the system state before the mutation, Indicates the state of the system after the mutation. impacr represents the impact force-torque vector at the time of mutation, then the dynamic equation at the time of mutation satisfies:

[0077]

[0078] Combining equations (13) and (14), we get:

[0079]

[0080] Using the Schur Complement method to solve equation (15) we get:

[0081]

[0082] The matrices M and J c are defined before the mutation occurs. According to the above assumptions, the position quantity q in the system state will not mutate, that is, q + =q - , then we can get the system jump function:

[0083]

[0084] In which, the matrix T is used instead express.

[0085] S3. Further, based on the landing area information obtained in step S1 and the system dynamic equation and jump function obtained in step S2, a nonlinear model predictive control problem for humanoid robot walking can be designed and solved.

[0086] Specifically, the general form of the control problem is:

[0087] min cost function: subject to: Dynamic equations:

[0088] Initial condition: x(0) = x0 (18b)

[0089] Equality constraint:h e (x(t),u(t))≥0 (18c)

[0090] Inequality constraints: h ie (x(t),u(t))≥0 (18d)

[0091] Among them, the time trajectory of the system state x and the system input u are the variables to be optimized, x Ref and u ref are the reference trajectories of x and u respectively. Formula (18) is the optimization evaluation index, i.e., the cost function, where the W matrix is ​​a semi-positive definite diagonal matrix, the element values ​​on the diagonal represent the weight of a certain component of the system state, and R is a positive definite diagonal matrix, the element values ​​on the diagonal represent the weight of a certain component of the system input. Formula (18a), (18b), (18c), and (18d) are the system dynamic equations, initial conditions, equality constraints, and inequality constraints, respectively, which are used to constrain the entire control process.

[0092] S31. In this embodiment, since the dynamic equations and jump functions of the system have been obtained in step S2, the specific method for designing the rest of the nonlinear model predictive control problem for humanoid robot walking is: respectively designing the reference trajectory, cost function, equality constraints and inequality constraints.

[0093] S311. Specifically, the method for designing a reference trajectory in this embodiment is as follows. Before designing the cost function in step S312, it is first necessary to design a reference trajectory of the system state x and the system input u. In this embodiment, the robot needs to minimize the gap between the actual trajectory and the reference trajectory, that is, the cost function, to achieve the purpose of tracking and control.

[0094] In this embodiment, the reference trajectory of the floating basis in the system state is generated as follows. In this embodiment, the robot controller receives a user instruction that only includes the center of mass position in the x and y directions of the movement end point and the yaw angle. In this embodiment, the movement of the robot is defined as a process of uniform acceleration-uniform speed-uniform deceleration. After specifying the maximum movement speed and acceleration value, the reference trajectory of the three components x, y, and yaw can be generated by interpolation, and then the reference trajectory of the three speeds can be obtained by difference. The reference value z(t) in the z direction needs to take into account the terrain height h terrain , we can set z(t)=z(0)+h terrain(t), and the speed reference trajectory is also obtained by difference. It is worth mentioning that the terrain height is obtained by the value of the grid map generated in step S1 at the (x(t), y(t)) coordinate. The reference values ​​of the roll and pitch components remain unchanged, the speed reference value is always zero, and their reference trajectories are all constant value trajectories.

[0095] In this embodiment, the reference trajectory of the joint quantity in the system state is generated as follows. In this embodiment, the reference trajectory q of the joint angle j,red is the reference trajectory p through the foot end posture doot,red The reference trajectory of the joint angular velocity is calculated by analytical inverse kinematics. is the reference trajectory of the foot end velocity Through the inverse of the Jacobian matrix J -1 Calculated, that is:

[0096] q j,ref =invK(p foot,ref )#(19)

[0097]

[0098] Where invK() represents the robot's analytical inverse kinematics equation. In this embodiment, the reference posture of the foot end is related to the selection of the reference foothold. The method for selecting the reference foothold is as follows: First, the nominal foothold p on the xy plane is calculated based on the center of mass state and the Raibert heuristic foothold formula. nom :

[0099]

[0100] Among them, p foot is the current foot position, h IP is the length of the virtual inverted pendulum, g is the acceleration due to gravity, v b is the current center of mass linear velocity, v b,ref is the reference center of mass linear velocity. Secondly, according to the set of landing areas obtained in step S1, the next reference landing point that is most suitable for the current center of mass state is obtained according to certain judgment criteria. After obtaining the reference landing point, the reference trajectory of the foot end posture and velocity is further generated through cubic polynomial planning, including six dimensions of x, y, z, roll, pitch, and yaw. Among them, the last three dimensions are related to the terrain posture of the reference landing point.

[0101] In this embodiment, the reference of the system input u is zero, and the reference trajectory is a constant value trajectory, which is used to reduce the cost of the system input u in step S312 and suppress excessive input.

[0102] S312: Further, the method for designing the cost function in this embodiment is as follows. After obtaining the reference trajectory in step S311, the cost function in this embodiment is composed of the following two parts: system state tracking cost c state , system input cost c input , which can be expressed as:

[0103] c state =0.5(xx ref ) T W(xx ref )#(twenty two)

[0104] c input =0.5(uu ref ) T R(yy ref )#(twenty three)

[0105] S313: Further, the method for designing the equality constraint in this embodiment is as follows: The equality constraint in this embodiment consists of the following three parts: zero force-torque constraint at the end of the swing leg, double leg trajectory tracking constraint, and speed constraint at the foot end of the supporting leg.

[0106] Firstly, the zero force-torque constraint at the end of the swing leg in this embodiment means that during the movement of the swing leg, there is no output force and torque at the end.

[0107] Second, the double-leg trajectory tracking constraint in this embodiment refers to the swing leg and the supporting leg tracking the foot-end reference trajectory generated by step S311 in the normal direction of the ground. Since the foot-end velocity is only determined by the system state x, not by the system input u, in order to make the solution process stable, it is necessary to introduce the foot-end acceleration determined by the system state x and the system input u. to the constraint expression. Then the constraint can be expressed as:

[0108]

[0109] Among them, n terrain is the normal to the ground, the foot end acceleration It can be derived from the system state x and the system input u through kinematics, k d , k p It is a manually given hyperparameter used to adjust the constraint effect.

[0110] Third, the foot-end velocity constraint of the supporting leg in this embodiment means that the foot-end velocity of the supporting leg in the tangential direction relative to the ground is zero. Similarly, in order to stabilize the solution process, this embodiment introduces the foot-end acceleration to the constraint expression. Therefore, the constraint can be expressed as:

[0111]

[0112] Where S ` The matrix is ​​the selection matrix for selecting the left leg or the right leg as the supporting leg according to the current gait, t terrain is the tangent direction of the ground, and a is an artificially given hyperparameter used to adjust the constraint effect.

[0113] S314. Further, the method for designing inequality constraints in this embodiment is as follows. The inequality constraints in this embodiment are added to the cost function in the form of soft constraints and penalties, and h ie ≥0 to indicate the constraint. Then the final cost function c final It can be expressed as:

[0114] c final =c state +c input +∑P(h ie,i )#(26)

[0115] Where P(h ie,i ) is the penalty for each inequality constraint being violated.

[0116] The inequality constraints of this embodiment are composed of the following six parts: foothold area constraint, self-collision constraint, external collision constraint, joint velocity constraint, joint torque constraint, and friction cone constraint.

[0117] First, the landing area constraint in this embodiment means that the actual landing point falls within the landing area obtained in step S1. When the landing area boundary is described by a straight line, the constraint can be written as:

[0118] A i p foot +b i ≤0 i=1,2,3,…#(27)

[0119] Among them, A i 、b i is the mathematical expression of boundary i.

[0120] Second, the self-collision constraint of this embodiment refers to the left and right foot ends not colliding with each other, which plays a key role in the stability of the walking process and can be expressed as:

[0121] ||p foot,left -p foot,right ||-d threshold ≥0#(28)

[0122] where d threshold is the closest distance threshold.

[0123] Third, the external collision constraint of this embodiment means that the robot and the terrain do not collide with each other. This embodiment only considers the collision between the robot's knee joint and the terrain, models the collision body of the knee joint as a sphere, and calculates the distance between the center of the knee joint and the signed distance field obtained in step S1, so that the distance is greater than the radius r of the sphere.

[0124] Fourthly, the joint speed constraint of this embodiment means that the actual joint speed of the robot does not exceed the joint speed threshold.

[0125] Fifth, the joint torque constraint in this embodiment means that the actual joint torque of the robot does not exceed the joint torque threshold.

[0126] Sixth, the friction cone constraint of this embodiment means that the support leg does not slip when in contact with the ground. A linear friction cone constraint is used here to facilitate stable solution.

[0127] After obtaining the above six sets of inequality constraints, the respective penalty functions are respectively included in the final cost function to complete the processing of the inequality constraints.

[0128] S32. In this embodiment, the method for solving the nonlinear model prediction problem is as follows. Since step S2 constructs the dynamic equation and jump function of the system, and step S31 designs the reference trajectory, cost function, equality constraints and inequality constraints, the final nonlinear model predictive control problem can be expressed as:

[0129] min cost function: c final =c state +c input +∑P(h ie,i )#(29)

[0130] subject to: System dynamic equations:

[0131] Initial condition: x(0) = x0 (29b)

[0132] Equality constraint:h e (x(t),u(t))≥0 (29c)

[0133] Since the system dynamic equations and jump functions contain nonlinear terms, high-speed real-time solutions need to rely on nonlinear solvers. This embodiment uses the SQP method in OCS2 Toolbox to solve the control problem in real time, with a frequency of up to 100Hz. as well as Represents the solution results of the nonlinear model predictive control problem.

[0134] S4. Further, according to the result calculated in step S3, the joint torque is calculated by the system inverse dynamics equation. Finally, it is sent to the robot joint in a proportional differential (PD) combined with a feedforward method.

[0135] S41. In this embodiment, the method for calculating the inverse dynamics joint torque from the system inverse dynamics equation of the whole body dynamics model is as follows. The inverse dynamics joint torque τ is solved by equation (8): inv for:

[0136]

[0137] The acceleration of the body That is, the system input u calculated by equation (9) and step S3 is nmpc Substituting into the above formula (38), the inverse dynamics joint torque can be calculated.

[0138] S42: In this embodiment, the method for sending the robot joint torque is as follows: The inverse dynamics joint torque calculated in step S41 can be used as the feedforward torque τ of the final torque ff The final torque τ of this embodiment is calculated by PD combined with feedforward, that is:

[0139]

[0140] The two control parameters p and d are given manually, q j , are the actual joint angles and joint velocities of the current robot, q j,nmpc , are the joint angle and joint angular velocity solved in step S32 respectively. The final torque τ is sent to the robot joint at a frequency of 1000 Hz.

[0141] The walking stability and strong ability to cross terrain of the overall algorithm of the present invention have been verified in the simulation software Raisim. In the Raisim simulation platform, the algorithm of the present invention can enable the humanoid robot to cross a platform with a height of 30 cm. Figure 6 As shown; can be through 15cm high continuous steps, such as Figure 7 shown.

[0142] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features may be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for walking on step terrain of a humanoid robot based on a whole-body dynamics model, characterized in that: The following steps are involved: S1. Draw a grayscale image for representing the step terrain, process the grayscale image to generate a grid map, and then generate a convex plane, a signed distance field, and a terrain in the simulation software from the grid map; wherein the convex plane generated by the grid map is used as a set of landing areas; S2. Design the robot system state, system input, system dynamic equations and jump function based on the whole body dynamics model; S3, according to the landing area information obtained in step S1 and the system dynamic equation and jump function obtained in step S2, a nonlinear model predictive control problem for the humanoid robot walking on step terrain is designed and solved to obtain the system desired state and input trajectory; S4. Based on the result calculated in step S3, the robot joint torque is calculated according to the whole body dynamics model, and the torque is sent to the robot joint by using the PD combined with the feedforward controller to achieve the control effect.

2. The method for walking on step terrain of a humanoid robot based on a whole-body dynamics model according to claim 1, characterized in that: The step S1 comprises the following sub-steps: S11, using MATLAB software to draw a grayscale image representing the step terrain; S12, converting the grayscale image obtained in step S11 into a grid map, using the grid map to generate a number of convex planes as a set of landing areas, and generating a signed distance field as an external collision detection criterion between the robot and the environment; S13, converting the grid map generated in step S12 into terrain in the simulation software for use in simulation debugging.

3. The method for walking on step terrain of a humanoid robot based on a whole-body dynamics model according to claim 1, characterized in that: The step S2 comprises the following sub-steps: S21. Design the state quantity in the nonlinear model predictive control based on the whole body dynamics model: Use the Euler-Lagrange method to express the dynamic equation of the humanoid robot, and then determine that the system state x is composed of six items: center of mass position, center of mass Euler angle, joint angle, center of mass linear velocity, center of mass Euler angle change rate, and joint velocity; S22, deriving the system dynamic equation based on the system state quantity and the whole body dynamic model, dividing the whole body dynamic equation into two parts: under-actuated and fully-actuated, and then obtaining the body acceleration; The system input u is selected to consist of joint acceleration and end contact force-torque; S23. Derive the jump function based on the system state quantity and the whole-body dynamics model: Combine the dynamic equation when the system state changes suddenly and assume that the position quantity does not change suddenly, and use the Schur complement method to solve the jump function of the velocity quantity.

4. The method for walking on step terrain of a humanoid robot based on a whole-body dynamics model according to claim 1, characterized in that: The step S3 comprises the following sub-steps: S31. Design a nonlinear model predictive control problem for humanoid robot walking: Based on the general form of the control problem, design the reference trajectory, cost function, equality constraints and inequality constraints; S32. Use the SQP method to solve the control problem in real time.

5. The method for walking on step terrain of a humanoid robot based on a whole-body dynamics model according to claim 4, characterized in that: The step S31 includes the following sub-steps: S311. Design the reference trajectory of system state x and system input u: the robot controller receives user instructions that only include the end point x, the center of mass position in the y direction and the yaw angle, generates a position reference trajectory of uniform acceleration-uniform speed-uniform deceleration through polynomial interpolation, and then obtains the reference trajectory of the speed through the difference method; the reference value in the z direction needs to take into account the terrain height, the reference values ​​of the two components of roll and pitch remain unchanged, the speed reference value is always zero, and its reference trajectory is a constant value trajectory; the reference landing point of the foot end is calculated by the Raibert heuristic landing point formula; the reference trajectory of the foot end position, velocity and acceleration is generated by interpolation of the swing leg foot end trajectory planner; the reference trajectory of the joint angle is calculated by the reference trajectory of the foot end posture through analytical inverse kinematics, and the reference trajectory of the joint angular velocity is calculated by the reference trajectory of the foot end velocity through the inverse calculation of the Jacobian matrix; the reference of the system input u is zero, and the reference trajectory is a constant value trajectory; S312, designing a cost function, including the following two parts: state trajectory tracking cost and input cost; S313, design equation constraints, including the following three parts: zero force-torque constraint at the end of the swing leg, double-leg trajectory tracking constraint, and support leg foot end velocity constraint; S314. Design inequality constraints and add them to the cost function in the form of penalties, including the following six parts: landing area constraints, self-collision constraints, external collision constraints, joint velocity constraints, joint torque constraints, and friction cone constraints.

6. The method for walking on step terrain of a humanoid robot based on a whole-body dynamics model according to claim 3, characterized in that: The step S4 comprises the following sub-steps: S41, calculating the inverse dynamics joint torque feedforward using the inverse dynamics equation of the whole body dynamics model, the body acceleration in step S22, and the solution result of step S3; S42, using PD combined with feedforward to calculate the final robot joint torque τ, and send it to the robot in real time.

Images