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

Abstract

The application discloses a kind of human-imitating robot step terrain walking methods based on whole body dynamics model, comprising: design nonlinear model predictive control problem for human-imitating robot walking, wherein the reference trajectory of system state, input and foot end is reasonably planned, the equality constraint and inequality constraint necessary for robot walking are designed, and the constraint that robot landing point needs to fall in the area where foot can fall is taken as one of inequality constraint, so that human-imitating robot has the ability to walk on step terrain;Finally, the nonlinear model predictive control problem is solved in real time using solver, and the joint torque of robot is issued according to the inverse dynamics equation of whole body dynamics model and the controller of PD combined feedforward, so that the control effect of robot is better, and walking is more stable.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of humanoid robot stable walking control method, and particularly relates to a step terrain walking method of a humanoid robot based on a whole-body dynamics model. BACKGROUND

[0002] In recent years, the control algorithm of humanoid robots has become more and more mature, so that it has entered the field of vision of people more and more. The advantage of humanoid robots over traditional wheeled mobile robots is that they can directly adapt to the production and living environment of human beings, and can directly use the tools used by human beings without changing the environment or redesigning the tools. From the application scene, since the wheeled robot is difficult to adapt to the terrain with large ups and downs, the flying robot cannot meet the demand of large load and complex operation, the humanoid robot can better perform specific tasks. Further, compared with the four-legged robot, the humanoid robot has smaller footprint and larger activity range when walking, and two mechanical 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 the wheeled, four-legged and other 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. Among these methods, the most representative ones are: Zero Moment Point (ZMP), Virtual Model Control (VMC), Hybrid Zero Dynamics (HZD), optimization control method, and reinforcement learning-based control method.

[0004] At present, the zero moment point method is widely used in the control of humanoid robots (such as CN112224300A and CN113830197A), which can realize 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, which is very suitable for controlling the robust walking of robots due to its ability to predict the state of the system for a period of time. CN115202378A uses a convex model predictive control method based on a single rigid body model, which enables the humanoid robot to walk stably on outdoor flat ground and has strong robustness. However, due to the limitation of the convex system dynamic equation and constraints of the convex model predictive controller, the system state dimension is low and the information is less, and the complete dynamic model cannot be considered, nor can the terrain-related foot-fallable area be designed into the state-related constraints. Therefore, when the robot faces some structured terrain with large fluctuations, such as high platforms, continuous steps, slopes, etc., the terrain factors cannot be modeled, and in most cases the terrain is regarded as a disturbance, resulting in falling or collision with the environment. In addition, although the single rigid body model, inverted pendulum model (such as CN112882467A and CN116300877A), and centroid dynamics model can effectively capture the system dynamics to some extent, reduce the problem size, and increase 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 the real-time nonlinear model predictive control based on the full-body dynamic model. SUMMARY

[0005] To overcome the limitations of the above prior art, the present application provides a humanoid robot step terrain walking method based on a full-body dynamic model to solve the above problems. The present application constructs a hybrid system for humanoid robots based on a full-body dynamic model, which has complete system modeling and high precision. At the same time, the present application designs a nonlinear model predictive control problem containing terrain information and solves it in real time, so that the humanoid robot considers terrain factors and reasonably plans its movement when walking. The present application realizes the humanoid robot to cross high platforms and continuous steps of different heights in a simulation environment, with strong walking stability and strong movement ability. The technical scheme of the present application includes the following contents:

[0006] A humanoid robot step terrain walking method based on a full-body dynamic model, comprising the following steps:

[0007] S1, draw a gray-scale map for representing the step terrain, process the gray-scale map to generate a grid map, and then generate a convex plane, a directed distance field, and a terrain in a simulation software from the grid map; wherein the convex plane generated by the grid map is used as a set of foot-fallable areas;

[0008] S2, design the robot system state, system input, system dynamic equation and jump function according to the whole body dynamics model;

[0009] S3, design the nonlinear model predictive control problem for the humanoid robot walking on the step terrain according to the footfall region information obtained in step S1 and the system dynamic equation and jump function obtained in step S2, and solve the problem to obtain the trajectory of the system desired state and input;

[0010] S4, based on the result calculated in step S3, calculate the robot joint torque according to the whole body dynamics model, and use the PD combined with the feedforward controller to issue the torque to the robot joint to achieve the control effect.

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

[0012] S11, use MATLAB software to draw a gray scale map representing the step terrain;

[0013] S12, convert the gray scale map obtained in step S11 into a grid map, and use the grid map to generate a plurality of convex planes as a set of footfall regions, and generate a directed distance field as a criterion for external collision detection between the robot and the environment;

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

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

[0016] S21, according to the whole body dynamics model, design the state quantity in the nonlinear model predictive control: use the Euler-Lagrange method to represent the dynamics equation of the humanoid robot, and then determine that the system state x consists of six items, i.e. the center of mass position, the center of mass Euler angle, the joint angle, the center of mass linear velocity, the center of mass Euler angle change rate and the joint speed;

[0017] S22, derive the system dynamic equation according to the system state quantity and the whole body dynamics model, divide the whole body dynamics equation into two parts, i.e. underactuated and fully actuated, and then obtain the body acceleration; select the system input u consisting of joint acceleration and end contact force-torque;

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

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

[0020] S31, design a nonlinear model predictive control problem for biped robot walking: design the reference trajectory, cost function, equality constraints and inequality constraints four parts combined with the general form of the control problem;

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

[0022] Further, the step S31 comprises the following sub-steps:

[0023] S311, design the reference trajectory of system state x and system input u: the robot controller receives the user instruction containing only the motion end point x, y direction mass center position and yaw angle, generates the uniform acceleration-uniform speed-uniform deceleration position reference trajectory through polynomial interpolation, and then obtains the velocity reference trajectory through difference; The reference value of the z direction needs to consider the terrain height, the reference values of the roll and pitch components are unchanged, and the velocity reference value is always zero, and the reference trajectories of the reference values are constant value trajectories; The reference landing point of the foot end is calculated by the Raibert heuristic landing point formula; The reference trajectories of the foot end position, velocity and acceleration are generated by the swing leg foot end trajectory planner; The reference trajectory of the joint angle is calculated by the reference trajectory of the foot end pose 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 of the Jacobian matrix; The reference of the system input u is zero, and the reference trajectory is a constant value trajectory;

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

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

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

[0027] Further, the step S4 comprises the following sub-steps:

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

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

[0030] The beneficial effects of the present application are: firstly, the present application constructs a hybrid system based on a whole-body dynamics model for a humanoid robot, and the state quantity of the whole-body dynamics model is more abundant than that of a simplified dynamics model such as a single rigid body model or a center of mass dynamics model of the robot, and the introduction of a jump function can greatly capture the mutation influence of the foot landing of the humanoid robot on the state quantity of the robot, so that the change of the state quantity of the robot is more accurate. Then, the present application designs a nonlinear model predictive control problem for the walking of the humanoid robot, reasonably plans the reference trajectories of the system state, input and foot, reasonably designs the equality constraints and inequality constraints necessary for the walking of the robot, and takes the constraint that the robot landing point needs to fall in the landing area as one of the inequality constraints, so that the humanoid robot has the ability to walk on the step terrain. Finally, the present application uses the solver OCS2Toolbox to solve the nonlinear model predictive control problem in real time, and issues the joint torque of the robot according to the inverse dynamics equation of the whole-body dynamics model and the PD combined feedforward controller, so that the robot control effect is better and the walking is more stable. BRIEF DESCRIPTION OF DRAWINGS

[0031] Figure 1 The system framework diagram of the present application.

[0032] Figure 2 The gray scale map representing the terrain generated by the embodiment of the present application.

[0033] Figure 3 The grid map generated by the embodiment of the present application.

[0034] Figure 4 The landing area map generated by the embodiment of the present application.

[0035] Figure 5 The simulation terrain map generated by the embodiment of the present application.

[0036] Figure 6 The schematic diagram of the humanoid robot climbing high platform in the embodiment of the present application.

[0037] Figure 7 The schematic diagram of the humanoid robot climbing continuous steps in the embodiment of the present application. DETAILED DESCRIPTION

[0038] The present application will be described in detail below with specific embodiments. The embodiments described below are exemplary and are only used to explain the present application, and cannot be understood as a limitation of the present application. It should be noted that for those skilled in the art, without departing from the concept of the present application, a number of modifications and improvements can be made. These all belong to the protection scope of the present application.

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

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

[0041] Specifically, MATLAB software was used to draw a grayscale image used to represent the stepped terrain. The grayscale image was processed using the ElevationMapping Toolbox to generate a raster map. The raster map was then used to generate several convex planes as a set of landing areas, and a signed distance field was generated as a criterion for external collision detection between the robot and the environment. At the same time, the raster map was converted into 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, where the number of rows and columns is the length and width of the terrain map, respectively, and each element is an integer value from 0 to 255, representing the terrain grayscale. The value of each element of the matrix is ​​determined according to the terrain structure to be generated, such as a platform, a continuous staircase, etc. Finally, the matrix is ​​converted into a grayscale image and saved, as shown in the following example. Figure 2 shown.

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

[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, such as Figure 5 shown.

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

[0046] Specifically, the state quantity in the nonlinear model predictive control is designed according to the whole-body dynamics model, so as to capture the whole-body system dynamics; the system input quantity is designed according to the determined system state quantity and the whole-body dynamics model, and the system dynamic equation is derived to meet the form of the general dynamic equation; and the jump function is derived according to the determined system state quantity and the whole-body dynamics model, so as to capture the system mutation.

[0047] S21, specifically, in the 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 the joint quantity q j , that is,

[0048]

[0049] wherein q b is a six-dimensional vector, including a three-dimensional mass center position and a three-dimensional mass center Euler angle (roll angle, pitch angle, yaw angle) attitude, q j is a joint angle, T is a transposition operation, and then the following can be obtained:

[0050]

[0051] wherein is a six-dimensional vector, including a three-dimensional mass center linear velocity and a three-dimensional mass center angular Euler angle rate, is a joint velocity.

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

[0053]

[0054] wherein M(q) is an inertia matrix, is a Coriolis force and gravity term, B(q) is a joint torque mapping matrix, τ is a joint torque, J c (q) is a contact Jacobian matrix, and λ is an end contact force-torque vector. Assuming that the foot plate and the ground are stationary when in contact, that is, the relative velocity is zero, the following can be obtained:

[0055]

[0056] wherein is the velocity when the foot end and the ground are in contact.

[0057] Derivation of equation (4) can obtain:

[0058]

[0059] In combination with the definition of the whole-body dynamics model, the embodiment defines the system state as:

[0060]

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

[0062] S22, the method for deriving the system dynamic equation according to 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 transition matrix, and g(x) is the system input matrix. Since the whole-body dynamics equation (3) can be divided into an underactuated floating base and a fully actuated joint, i.e.,

[0065]

[0066] Where the first six lines are the underactuated floating base part, and the remaining part is the fully actuated joint part. Thus, the acceleration of the floating base, i.e., the body, can be obtained as

[0067]

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

[0069]

[0070] Thus, according to the concept of the whole-body dynamics model, the system input u is selected to be composed of the joint acceleration and the end contact force-torque vector, i.e.,

[0071]

[0072] Then equation (10) can be transformed as

[0073]

[0074] Let That is, equation (7) conforms to the general form of the system dynamic equation.

[0075] S23, the method of deriving the jump function according to the determined system state quantity and the whole body dynamics model is as follows. For the process of walking of the humanoid floating base robot, when the robot is in contact with the environment, especially when the foot end is in contact with the ground, the state quantity of the system can be suddenly changed. It is assumed that the contact between the robot walking and the ground is a hard contact, and the position and posture of the body and the joint angle of the body do not suddenly change when the body is in contact, but the six-dimensional velocity of the body and the joint velocity will suddenly change due to the contact. Since the system state quantity selected in step S21 contains the six-dimensional velocity of the body and the joint velocity, it is necessary to design a jump function to model such sudden changes.

[0076] The general form of the jump function is x + = f(x - ), wherein x represents the system state before the mutation, and x represents the system state after the mutation. λ impacr represents the impact force-moment vector when the mutation occurs, and the dynamics equation when the mutation occurs satisfies:

[0077]

[0078] By combining equations (13) and (14), the following equation is obtained:

[0079]

[0080] By using the Schur complement method to solve equation (15), the following equation is obtained:

[0081]

[0082] wherein the matrices M and J c are defined before the mutation occurs. According to the above assumption, the q representing the position quantity in the system state will not be mutated, i.e. q + = q - , and then the system jump function can be obtained by summarizing the above:

[0083]

[0084] wherein the matrix T is used to replace .

[0085] S3, further, according to the foot-fallable region 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 walking of the humanoid robot 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. Equation (18) is the optimization evaluation metric, i.e., the cost function, where the W matrix is ​​a semi-positive diagonal matrix whose diagonal elements represent the weight of a component of the system state, and R is a positive diagonal matrix whose diagonal elements represent the weight of a component of the system input. Equations (18a), (18b), (18c), and (18d) are the system dynamic equations, initial conditions, equality constraints, and inequality constraints, respectively, 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 remaining parts 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, a reference trajectory of the system state x and system input u must first be designed. In this embodiment, the robot needs to minimize the difference between the actual trajectory and the reference trajectory, namely the cost function, to achieve tracking and control.

[0094] In this embodiment, the reference trajectory of the floating base quantity 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 the value of the acceleration, 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 differential means. 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 velocity reference trajectory is similarly obtained through differentiation. It is worth noting that the terrain height is obtained from the values ​​of the grid map generated in step S1 at the (x(t), y(t)) coordinates. The reference values ​​of the roll and pitch components remain unchanged, and the velocity reference value is always zero, resulting in a constant-value trajectory.

[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 of the joint angle q 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 By 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 pose 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. Next, based on the set of possible footholds obtained in step S1, a specific evaluation criteria is used to determine the next reference foothold point that best fits the current center of mass state. After obtaining the reference foothold point, a cubic polynomial program is used to generate a reference trajectory of foot-end position and velocity, encompassing six dimensions: x, y, z, roll, pitch, and yaw. The last three dimensions are related to the terrain posture of the reference foothold 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 of the embodiment is as follows. After obtaining the reference trajectory through step S311, the cost function of the 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 (x-x ref ) T W (x-x ref ) # (22)

[0104] c input = 0.5 (u-u ref ) T R (y-y ref ) # (23)

[0105] S313, further, the method for designing the equality constraint of the embodiment is as follows. The equality constraint of the embodiment is composed of the following three parts: swing leg end zero force-torque constraint, double leg trajectory tracking constraint, and support leg foot end speed constraint.

[0106] Firstly, the swing leg end zero force-torque constraint of the embodiment refers to that the swing leg does not output force and torque at the end during movement.

[0107] Secondly, the double leg trajectory tracking constraint of the embodiment refers to that the swing leg and the support leg track the foot end reference trajectory generated by step S311 in the normal direction of the ground. Since the foot end speed is determined only by the system state x and not by the system input u, in order to stabilize the solving process, the foot end acceleration is introduced into the constraint expression, which is determined by the system state x and the system input u. Then, the constraint can be expressed as:

[0108]

[0109] where n terrain is the normal direction of the ground, the foot end acceleration can be derived from the system state x and the system input u through kinematics, k d and k p are artificial given hyperparameters for adjusting the constraint effect.

[0110] Thirdly, the support leg foot end speed constraint of the embodiment refers to that the relative speed of the support leg foot end to the ground in the tangential direction of the ground is zero. Similarly, in order to stabilize the solving process, the foot end acceleration is introduced into the constraint expression. Therefore, the constraint can be expressed as:

[0111]

[0112] where S ` is the selection matrix that selects the left leg or the right leg as the support leg according to the current gait, t terrain is the tangent 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 are represented by h ie ≥ 0. The final cost function c final can be represented as:

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

[0115] where P(h ie,i ) is the penalty when each inequality constraint is violated.

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

[0117] First, the footfall region constraint in this embodiment refers to the actual footfall point falling within the footfall region obtained in step S1. When the footfall region 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] where A i and b i are the mathematical expressions of the boundary i.

[0120] Second, the self-collision constraint in this embodiment refers to the mutual non-collision of the left and right foot ends, which plays a key role in the stability of the walking process and can be represented as:

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

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

[0123] Third, the outer collision constraint of the embodiment refers to the collision between the robot and the terrain. The embodiment only considers the collision between the knee joint of the robot and the terrain, models the collision body of the knee joint as a sphere, calculates the distance between the center of the knee joint and the directed distance field obtained in step S1, and makes the distance greater than the radius r of the sphere.

[0124] Fourth, the joint speed constraint of the embodiment refers to the actual joint speed of the robot not exceeding the joint speed threshold.

[0125] Fifth, the joint torque constraint of the embodiment refers to the actual joint torque of the robot not exceeding the joint torque threshold.

[0126] Sixth, the friction cone constraint of the embodiment refers to the support leg not slipping when in contact with the ground. A linear friction cone constraint is used here to facilitate stable solving.

[0127] After obtaining the above six groups of inequality constraints, the respective penalty functions are added to the final cost function, and the processing of the inequality constraints can be completed.

[0128] In the embodiment, the method for solving the nonlinear model prediction problem is as follows. Since the system dynamic equation and the jump function are constructed in step S2, the reference trajectory, the cost function, the equality constraint and the inequality constraint are designed in step S31, the final nonlinear model prediction control problem can be represented as:

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

[0130] subject to: system dynamic equation:

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

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

[0133] Since the system dynamic equation and the jump function contain nonlinear terms, high-speed real-time solving needs to rely on a nonlinear solver. The embodiment uses the SQP method in the OCS2 Toolbox to solve the control problem in real time, and the frequency can reach 100 Hz. The solving results of the nonlinear model prediction control problem are represented by and

[0134] ​S4, further, according to the result calculated in step S3, the joint torque is calculated by the system inverse dynamics equation. Finally, the joint torque of the robot is issued according to the proportional differential (PD) combined with the feedforward mode.

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

[0136]

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

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

[0139]

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

[0141] The walking stability and the strong terrain crossing ability of the overall algorithm of the application are verified in the simulation software Raisim. In the Raisim simulation platform, the algorithm of the application can make the humanoid robot cross a high platform with a height of 30 cm, as shown in Figure 6 ; and can pass through a continuous step with a height of 15 cm, as shown in Figure 7 .

[0142] The above embodiments are only used to illustrate the technical solutions of the application, but not to limit it; although the application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that they can still modify the technical solutions recorded in the foregoing embodiments, or make equivalent replacement to part of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the application.​

Claims

1. A method for a humanoid robot to walk on stepped terrain based on a whole-body dynamics model, characterized in that: The following steps are involved: S1. Draw a grayscale image for representing the stepped terrain, process the grayscale image to generate a raster map, and then generate a convex plane, a signed distance field, and the terrain in the simulation software from the raster map; wherein the convex plane generated by the raster map serves 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; S21. Design state variables in nonlinear model predictive control based on the whole-body dynamics model: Use the Euler-Lagrange method to express the humanoid robot's dynamics equations, 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. Derive the system dynamic equations based on the system state variables and the whole-body dynamics model, divide the whole-body dynamics equations into underactuated and fully actuated parts, and then calculate the body acceleration; select the system input u to consist of the joint acceleration and the end contact force-torque; S23. Derivation of jump function based on system state and whole-body dynamics model: Combining the dynamics equations when the system state suddenly changes and assuming that the position does not suddenly change, the jump function of the velocity is solved using the Schur complement method; S3. Based on the landing area information obtained in step S1 and the system dynamic equations and jump functions obtained in step S2, a nonlinear model predictive control problem for the humanoid robot walking on stepped 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 using the PD combined with the feedforward controller to achieve the control effect.

2. The method for walking on stepped terrain by a humanoid robot based on a whole-body dynamics model according to claim 1, characterized in that: The step S1 includes the following sub-steps: S11. Use 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 simultaneously generating a signed distance field as a criterion for external collision detection 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 stepped terrain of a humanoid robot based on a whole-body dynamics model according to claim 1, characterized in that: The step S3 includes 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.

4. The method for walking on stepped terrain by a humanoid robot based on a whole-body dynamics model according to claim 3, characterized in that: The step S31 includes the following sub-steps: S311. Design the reference trajectory of the system state x and the system input u: The robot controller receives user instructions that only include the x-axis and y-axis center of mass positions of the motion endpoint and the yaw angle, and generates a position reference trajectory of uniform acceleration, uniform speed and uniform deceleration through polynomial interpolation, and then obtains the velocity reference trajectory through differential method; the reference value in the z direction needs to take into account the terrain height, the reference values ​​of the roll and pitch components remain unchanged, the velocity reference value is always zero, and its reference trajectories are all constant value trajectories; 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 all zero, and the reference trajectory is a constant value trajectory; S312. Design 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 foot-end velocity constraint of the supporting leg; S314. Design inequality constraints and add them to the cost function in the form of penalties, including the following six parts: landing area constraint, self-collision constraint, external collision constraint, joint velocity constraint, joint torque constraint, and friction cone constraint.

5. The method for walking on stepped terrain by a humanoid robot based on a whole-body dynamics model according to claim 1, characterized in that: The step S4 includes 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 of step S3; S42. Calculate the final robot joint torque τ using PD combined with feedforward, and send it to the robot in real time.

Images