Humanoid robot whole-body coordination control method based on mass center dynamic model

Abstract

The invention discloses a humanoid robot whole-body coordination control method based on a mass center dynamical model. The method comprises the steps that system modeling is conducted on a humanoid robot according to the mass center dynamical model; establishing a relationship between arm momentum and centroid momentum based on whole-body kinematics and a momentum conservation principle; a whole nonlinear optimization control problem is constructed and solved in real time, and a controller predicts the motion trail of the robot in a future period of time; feedforward torque is obtained through inverse dynamics calculation, and joint torque is issued in combination with a joint PD controller. The invention designs a nonlinear model predictive control problem for whole-body coordination control of the humanoid robot, reasonably plans a system state, an arm tail end state, a foot end state and a reference trajectory of control input, and designs equality constraints and inequality constraints necessary for whole-body coordination movement of the robot; task trajectory constraint and momentum conservation constraint on the tail end of the arm are added, so that the ability of the arm to assist in motion balance is more prominent.

Description

Technical Field

[0001] The present invention relates to the field of whole-body coordinated control of a humanoid robot based on model control, and in particular to a whole-body coordinated control method of a humanoid robot based on a center of mass dynamics model. Background Art

[0002] In recent years, the rapid development of artificial intelligence, machine learning, sensor technology and material science has provided a good foundation for the research and development of humanoid robots. With the aging population and labor shortage, the demand for humanoid robots in the fields of nursing, education, housework, etc. is increasing. Colleges and universities and research institutions have continued to deepen their research on humanoid robot technology, which has promoted the development of the industry. As the walking ability of humanoid robots has gradually improved, they have shown a demand for more daily and diverse interactions.

[0003] Compared with quadruped robots, the operating ability of humanoid robots is mainly reflected in the arms, and the arm's movement ability should be released more; compared with industrial robotic arms with fixed bases, humanoid robots are mobile robotic arm platforms with a wide range of activities and complex scenarios; compared with wheel-robotic arm robots, humanoid robots have more complex degrees of freedom, corresponding to more flexible operating methods. In order to make the body more flexible and coordinated to perform various operations, a series of training and adjustment methods can be adopted. At the same time, it is also crucial to maintain body balance when performing these operations. As a result, how to make the body more flexible and coordinated to operate, and how to maintain body balance during operation have become increasingly important issues.

[0004] At present, most of the research on humanoid robots is still mainly based on leg movement, and the influence of arm movement on robot balance is often ignored. For example, patent application CN118682750A only uses a single rigid body model to optimize the robot's online trajectory when implementing the jumping control of a humanoid robot, while ignoring the influence or contribution of the movement of the robot's arms and joints on the center of mass. The operation capabilities of most humanoid robots only focus on the arms or dexterous hands, but not on the coordinated movement control of the whole body. For example, patent application CN118143954A only focuses on the planning of the upper limbs of humanoid robots. In essence, the arms are modeled and planned as mechanical arms with a fixed base, without considering the influence of the arms on the whole body. Some existing whole-body coordinated control methods are not sufficient to truly demonstrate the effects of coordinated movements of arms and the whole body, which is related to the design of their controllers. For example, although the model of patent application CN118528270A involves the end of the arm, it does not perform collaborative planning. For example, the limb coordinated motion controller designed in patent application CN117944055A only adopts a virtual model control strategy, and does not establish whole-body kinematics. Moreover, the control strategy does not have the ability of multi-constraint optimization control, and the whole-body movement of a humanoid robot should be a multi-constraint nonlinear optimization problem.

[0005] In summary, the current research on humanoid robot motion control methods has made good progress. However, these methods have problems such as neglecting the coordinated movement of the whole body, insufficient assessment of the importance of arm movement to overall balance, and lack of multi-constraint optimization capabilities in controller design, which in turn affects the motion stability and flexibility of humanoid robots in complex environments. This means that in practical applications, the robot cannot fully utilize its whole body's motion capabilities for effective dynamic adjustments, resulting in poor performance when performing complex tasks. Summary of the invention

[0006] The present invention aims to provide a whole-body coordinated control method for a humanoid robot based on a center of mass dynamics model. It includes systematic modeling of a humanoid robot based on a center of mass dynamics model; establishing the relationship between arm momentum and center of mass momentum based on whole-body kinematics and the principle of momentum conservation, which is the key to reflecting the coordinated balance of the arm; constructing the entire nonlinear optimization control problem and solving it in real time, so that the controller can predict the robot's motion trajectory for a period of time in the future and better maintain stability; inverse dynamics calculation to obtain the feedforward torque, and combining the joint PD controller to issue the joint torque. The present invention designs a controller that simultaneously realizes the operation ability of the humanoid robot arm and the whole-body coordinated motion balance ability.

[0007] The present invention provides a method for whole-body coordinated control of a humanoid robot based on a center of mass dynamics model, comprising the following steps:

[0008] S1. Establish a center of mass dynamics model of the humanoid robot, select the state, system input and system output of the robot system, and obtain the nonlinear system state equation;

[0009] S2. Based on the whole body kinematics and the principle of momentum conservation, a model is established around the two scenarios of robot arm operation and coordinated motion; including the relationship between arm momentum and center of mass momentum, and task trajectory tracking at the end of the arm; task trajectory tracking constraints and angular momentum constraints of the end-of-arm velocity are obtained;

[0010] S3, construct a nonlinear model to predict the optimal control problem, which consists of three parts: cost function, system state equation and constraints; use the solver to solve and obtain the output trajectory of the optimized system;

[0011] S4. Establish the whole-body dynamics equation of the humanoid robot, obtain the robot's feedforward joint torque according to the inverse dynamics equation, and combine the joint PD control to obtain the final output torque sent to the robot.

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

[0013] S11. According to the Newton-Euler-Lagrange method, a complete floating basis dynamic equation of the humanoid robot is established, which is simplified to the Newton-Euler equation at the center of mass, and then the center of mass dynamic equation of the humanoid robot is obtained;

[0014] S12. According to the center of mass dynamics model established in step S11, select the state quantity and control quantity of the robot system to obtain the nonlinear system state equation; wherein the state quantity is composed of the center of mass angle, position, angular velocity, linear velocity, and joint position, and the control quantity is composed of the joint velocity, the contact force and torque at the ends of the two legs.

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

[0016] S21, establish a mapping relationship between the Cartesian spatial degrees of freedom of the arm end and the task trajectory, obtain the task trajectory tracking constraint, so that the arm end tracks the planned task trajectory and realizes the task operation capability of the arm;

[0017] S22. Establish the conservation relationship between the momentum of the arm and the angular momentum of the center of mass, and obtain the angular momentum constraint of the velocity of the end of the arm, so that the movement of the arm assists the movement balance of the robot and realizes the movement balance ability of the arm.

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

[0019] S31, establish a nonlinear model to predict the optimal control problem, which consists of three parts: cost function, system state equation and constraint conditions;

[0020] S32, using the SQP solver to solve the nonlinear model predictive optimal control problem in real time at a frequency of 100 Hz.

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

[0022] S311, using a Bezier curve in combination with a cubic spline curve to plan and interpolate the desired trajectory, the desired trajectory including an expected state quantity trajectory and an expected control quantity trajectory; the expected state quantity trajectory includes the mass center angle, position, angular velocity, linear velocity, joint position, arm task space, arm momentum, foot end position, and foot end velocity; the expected control quantity trajectory includes joint velocity, foot end force, and torque; wherein the expected state quantity trajectory is different during the swing phase and the support phase, and the expected control quantity trajectory is a constant;

[0023] S312, based on the expected state quantity trajectory and the expected control quantity trajectory of step S311, a state quantity tracking function and a control quantity tracking function are established using a mean square error function to measure the gap between the current actual state and the expected state; and a cost function is obtained;

[0024] S313. Constraints include equality constraints and inequality constraints; wherein equality constraints include system dynamic equation constraints, foot end curve tracking constraints and end zero force-torque constraints during the swing phase, and end zero velocity constraints during the support phase; inequality constraints include joint position constraints, joint velocity constraints, joint torque constraints, self-collision constraints, and friction cone constraints; inequality constraints are added to the cost function in the form of penalty terms.

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

[0026] S41, establish a full-body dynamics model of the humanoid robot, knowing the joint position, joint velocity, joint acceleration, and external contact force at the current moment, and obtain the joint feedforward torque through the inverse dynamics equation;

[0027] S42, feedforward torque combined with joint PD control controls the joint motor movement at a frequency of 1000 Hz.

[0028] The beneficial effects of the present invention are as follows: first, the present invention constructs a hybrid system based on the center of mass dynamics model for a humanoid robot. Compared with the simplified dynamics model of the robot, such as a single rigid body model or a whole-body dynamics model, the center of mass dynamics model not only takes into account the influence of the joint state, but also maintains a high solution frequency. Secondly, the present invention designs a nonlinear model predictive control problem for the whole-body coordinated control of a humanoid robot, wherein the system state, the state of the end of the arm, the state of the foot, and the reference trajectory of the control input are reasonably planned, and the equality constraints and inequality constraints necessary for the whole-body coordinated motion of the robot are reasonably designed. In particular, the task trajectory constraints and momentum conservation constraints on the end of the arm are added to make the arm's auxiliary motion balance ability more prominent. Finally, the present invention uses an SQP solver to solve the nonlinear model predictive control problem in real time, and issues the joint torque of the robot motion according to the inverse dynamics equation of the whole-body dynamics model and the PD combined with feedforward controller. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Figure 1 It is a system framework diagram of the present invention;

[0030] Figure 2 A whole body coordinated motion graph under a static arm task generated by an embodiment of the present invention;

[0031] Figure 3 A whole body coordinated motion graph under an arm dynamic task generated by an embodiment of the present invention;

[0032] Figure 4 A whole body coordinated motion graph during walking generated by an embodiment of the present invention;

[0033] Figure 5The arm-assisted balance motion diagram under interference generated by an embodiment of the present invention. DETAILED DESCRIPTION

[0034] The present invention will be described in detail below according to the accompanying drawings and preferred embodiments, and the purpose and effect of the present invention will become more clear. The present invention will be further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0035] The present invention proposes a method for controlling the whole body coordinated motion of a humanoid robot based on center of mass dynamics and momentum conservation. Figure 1 As shown, the specific steps include:

[0036] S1. To build a center of mass dynamics model of a humanoid robot, we first need to define the state variables, input variables, and output variables of the robot system, and then derive the state equation of the nonlinear system based on these variables. With the state equation of the system, we can further construct and solve the optimization problem. Specifically:

[0037] S11. Establish a center-of-mass dynamics model for a humanoid robot. The states in the floating-based dynamics model of a humanoid robot consist of joint states and center-of-mass states. The reason for the movement of the humanoid robot is the change in contact force. Compared to the single rigid body model that only considers external contact forces, the center-of-mass dynamics model takes into account the impact of each joint movement on the center of mass of the robot body. This is also the basis for achieving full-body control, that is, the input and output of the system include joint variables, and the entire system is nonlinear. Compared to the full-body dynamics model that considers the internal forces between joints, center-of-mass dynamics has better real-time performance in most current solvers, and the solution is faster and more efficient, and is sufficient to meet most basic motion requirements.

[0038] According to the Newton-Euler-Lagrange method, the complete floating basic dynamic equation of the humanoid robot can be expressed as:

[0039]

[0040] 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.

[0041] The center of mass dynamics model ignores the internal forces of the joints and only considers the effects of external forces and joint states on the center of mass. The complete floating base dynamics model is first divided into a 6+N-dimensional form:

[0042]

[0043] Among them, M bb represents the inertial characteristics of the robot's center of mass (main base), M bj represents the inertial coupling between the center of mass and the joint, M jj represents the inertial properties of the joint, represents the center of mass acceleration, represents the acceleration of the joint state, D b represents the damping (Coriolis force) and gravity characteristics of the center of mass, D j represents the damping (Coriolis force) and gravity characteristics of the joint, represents the Jacobian matrix of the contact point relative to the center of mass, Represents the Jacobian matrix of the contact point relative to the joint; the first six dimensions are the under-actuated floating basis degrees of freedom (including the position and angle of the center of mass), and the joint torque does not act directly on it, and the last N dimensions are the joint degrees of freedom.

[0044] The center-of-mass dynamics model only considers the mapping of all forces at the center of mass, thereby simplifying the complete dynamic equation to the Newton-Euler equation at the center of mass:

[0045]

[0046] Where v represents the linear velocity of the center of mass, ω represents the angular velocity of the center of mass, and h G is the robot's center of mass momentum, including linear momentum and angular momentum. q is the joint angle, is the joint angular velocity, A G (q) is called the centroidal momentum matrix (CMM). Obviously, when the joint angle changes, the centroid momentum matrix will also change, thus affecting the center of mass state. This equation is the center of mass dynamics equation of the humanoid robot.

[0047] S12, determine the state variables and control variables of the robot system in order to derive the state equation of the nonlinear system. In the mass center dynamics model established based on step S11, the state variables include the angle, position, angular velocity, linear velocity and joint position of the mass center, while the control variables are composed of the joint velocity, the contact force at the ends of the two legs and the applied torque.

[0048] First, the general form of the system state equation is:

[0049]

[0050] in, It represents the derivative of the system state quantity, and f(x,u) represents the mapping relationship between the system state quantity and the derivative of the system state quantity under the action of the system input.

[0051] According to the center-of-mass dynamics equation (3), the system state quantity x is selected as:

[0052]

[0053] in, is the centroid position, is the centroid angle, is the linear velocity of the center of mass, is the angular velocity of the center of mass, is the joint angle. Derivative the center of mass dynamics equation (3) yields:

[0054]

[0055] Among them, f c represents the net external force at the center of mass, τ c is the total external moment acting on the center of mass.

[0056] This formula describes the relationship between the contact external force / external torque and the generalized coordinate acceleration, so the system input u is selected as:

[0057]

[0058] Among them, λ T represents the transposed end contact force-torque vector.

[0059] On this basis, we take the derivative of (6) and combine it with the dynamic equation (2) to obtain:

[0060]

[0061] in, represents the Jacobian matrix of each contact point relative to the center of mass, λ i Represents the contact force-torque vector at each contact point, R b (θ b ) is the rotation matrix from the world coordinate system to the robot coordinate system, T(θ b ) is the translation matrix from the center-of-mass world coordinate system to the robot coordinate system, M b is the mass center inertia matrix, which is obtained by the mass center inertia characteristic matrix M bb and the inertial coupling matrix M between the center of mass and the joint bj Composition, n b is the gravity compensation term, and thus the system state equation (8) is obtained. It can be seen that the system state equation is obtained by the established center of mass dynamics model, which only considers the influence of the center of mass itself and the joints on the center of mass, while ignoring the coupling between joints. Therefore, it can reduce the amount of calculation to a great extent while ensuring reasonable consideration of the movement of the limbs, and prepare for the real-time solution of the subsequent optimization problem.

[0062] S2. Based on the whole body kinematics and the principle of momentum conservation, relevant models are established for the two scenarios of robot arm operation and coordinated movement. Specifically:

[0063] S21. Task operation capability of the arm: Establish the mapping relationship between the Cartesian spatial degrees of freedom of the arm end and the task trajectory. The whole body coordinated motion trajectory achieved by NMPC is as follows: Figure 2 , Figure 3 As shown in the figure (static actions are only downward / upward / forward stretching instructions, and dynamic actions only plan arm movements, that is, no planning is done for joints other than the arms throughout the process), it can be seen that due to the advantages of the model, the whole body coordination ability is also possessed in the process of completing the task trajectory.

[0064] In the present invention, the spatial degrees of freedom of the end of the humanoid robot arm only include its position and velocity in the Cartesian coordinate system:

[0065]

[0066] in, It represents the state description of the trajectory of the end of the arm in the robot coordinate system. represents the position of the arm end trajectory, Represents the velocity of the arm end trajectory.

[0067] Assume that the planned task trajectory is discretized in the world coordinate system (absolute position) as follows:

[0068] r k = r(t k )#(10)

[0069] Among them, r k Represents the state description of the trajectory of the end of the arm in the world coordinate system, r(t k ) represents the discretization function of the trajectory of the end of the arm.

[0070] In the world coordinate system, the task trajectory tracking constraint can be expressed as:

[0071]

[0072] Among them, T hand (θ) is the homogeneous transformation matrix from the world coordinate system to the robot coordinate system at the end of the arm. In the robot coordinate system (relative to the robot's center of mass), the task trajectory tracking constraint can be expressed as:

[0073]

[0074] S22. Arm movement balance ability: Establish the conservation relationship between arm momentum and angular momentum of center of mass.

[0075] In the present invention, when the humanoid robot lacks arms, the robot can have the ability of motion balance by planning the landing point of the foot end swing phase. Therefore, the conservation of angular momentum here only considers the relationship between the momentum of the arm and the angular momentum of the center of mass.

[0076] Knowing the angular velocity of the center of mass, the position of the arm joints, the moment of inertia and mass of the body, and the distance vector from the center of mass to the end of the arm at a certain moment, the conservation relationship of angular momentum can be obtained as:

[0077]

[0078] Among them, I b (q) represents the mass center inertia matrix, r(q) is the position vector from the end of the arm to the mass center, M arm is the equivalent mass of the arm.

[0079] Specifically, the humanoid robot arm of the present invention has four joint degrees of freedom, three of which are shoulder joints and one is an elbow joint. The angular velocity of the center of mass can be obtained by an inertial measurement unit (IMU) sensor installed on the body; the arm joint position can be obtained by an encoder installed at the joint; the URDF model of the known robot can be used to read the distribution of the moment of inertia of the robot body relative to the center of mass at the current moment by using a dynamics library (such as Pinocchio); the position of the end of the arm relative to the center of mass can be obtained by the forward kinematics of the robot arm.

[0080] Then the angular momentum constraint of the arm end velocity can be obtained as:

[0081]

[0082] Among them, L hand represents the momentum of the arm relative to the center of mass, Represents the inverse of the cross product matrix corresponding to the position vector from the end of the arm to the center of mass.

[0083] The angular momentum constraint is added as a cost term to the NMPC problem. Figure 4 As shown in the figure, the coordinated movement effect of the whole body can be seen. Furthermore, by applying disturbances to the robot in different directions, its arm-assisted balance ability is shown as follows: Figure 5 shown.

[0084] After the center of mass dynamics is modeled, the humanoid robot can achieve basic stable motion. After completing the operation and coordinated motion modeling of the robot arm, the robot can use the task trajectory constraint to achieve autonomous arm task space trajectory planning motion, and can also use the angular momentum constraint to achieve dual arms to assist the body in balance. The specific solution method will be expanded below.

[0085] S3. Establish a nonlinear model predictive optimal control (NMPC) problem, which consists of three parts: cost function, system state equation and constraint conditions. By solving this problem, the optimized system output trajectory can be obtained.

[0086] S31, establish a nonlinear model predictive optimal control (NMPC) problem, which consists of three parts: cost function, system state equation and constraint conditions. The system state equation is the equation (8) in step S12.

[0087] The overall NMPC problem can be expressed as follows:

[0088] min cost function:

[0089] subject to: Equation of state:

[0090] Initial condition: x(0) = x0 (15b)

[0091] Equality constraint:h e (x,u)=0 (15c)

[0092] Inequality constraints: h ie (x,u)≥0 #(15d)

[0093] Among them, u ref represents the reference value of the system input variable, W represents the system state weight matrix, (xx ref ) represents the difference between the current state and the expected state of the system, R represents the system input weight matrix, (uu ref ) represents the difference between the current input and the reference value of the system, h e represents the equality constraint in the optimization problem, h ie represents the inequality constraint in the optimization problem, H is the prediction window of NMPC, and x ref is the expected trajectory value. In actual use, it is in discretized form, and the discretization method usually adopts Newton-Euler method or Runge-Kutta method (RK4).

[0094] S311. Plan and interpolate the expected trajectory using a Bezier curve combined with a cubic spline curve. The expected trajectory here includes the expected state quantity trajectory of the arm end and the state quantity trajectory of the swing phase of the foot end.

[0095] The expected trajectory includes the expected state quantity trajectory and the expected control quantity trajectory; the expected state quantity trajectory includes the center of mass angle, position, angular velocity, linear velocity, joint position, arm task space, arm momentum, foot end position, and foot end velocity; the expected control quantity trajectory includes joint velocity, foot end force and torque; wherein the expected state quantity trajectory is different during the swing phase and the support phase, and the expected control quantity trajectory is a constant.

[0096] Given the starting point P0, the highest point P1, and the end point P2 of a desired trajectory, a quadratic Bezier curve can be used to generate a smooth task trajectory between the three points:

[0097] B(t)=(1-t) 2 P0+2(1-t)tP1+t 2 P2,t∈[0,1]#(16)

[0098] Where B(t) represents the generated Bessel trajectory function, and t represents the current sampling time. During the operation, the NMPC stage will sample the smooth trajectory generated by (15). It is known that two adjacent discrete data points [x k ,x k+1 ], a cubic spline can be used to generate a smooth curve between two points:

[0099] S k (x) = a k +b k (xx k )+c k (xx k ) 2 +d k (xx k ) 3 #(17)

[0100] Among them, S k (x) represents the cubic spline trajectory function generated by discretization, a k represents the constant term, b k represents the first-order constant coefficient, c k represents the second-order constant coefficient, d k represents a third-order constant coefficient.

[0101] Thus, NMPC can input continuous smooth trajectories into the underlying inverse dynamics controller.

[0102] S312. Based on the expected state quantity trajectory and the expected control quantity trajectory of step S311, a state quantity tracking function and a control quantity tracking function are established, wherein the tracking function adopts a mean square error function (MSE); and the gap between the current actual state and the expected state is measured.

[0103] The mean square error function (MSE) of the variable x is defined as follows:

[0104] MSE(x)=0.5(x ref ) T W(xx ref )#(18)

[0105] where x ref is the expected value, x is the current value, and W is the weight matrix. Combined with step S311, the complete expression of the cost function is:

[0106]

[0107] where x com is the center of mass state, including the center of mass position, linear velocity, angle and angular velocity; q j is the joint angle; x hand is the arm end position and linear velocity, L hand is the momentum of the end of the arm; x foot is the foot end position and speed; is the joint velocity, and λ is the contact force / torque.

[0108] S313. Constraints include equality constraints and inequality constraints.

[0109] The equation constraints are divided into the swing leg period and the support leg period. The equation constraints of the swing leg period include the end zero force-torque constraint, the double leg trajectory tracking constraint and the arm trajectory tracking constraint; the equation constraints of the support leg period include the dynamic equation constraint, the foot end zero velocity constraint and the arm trajectory tracking constraint. Among them, the arm trajectory tracking constraint has been established in step S22 and step S311. The other equation constraints are as follows:

[0110] First, the terminal zero force-torque constraint means that during the swing leg movement, there is no output force / torque at the terminal:

[0111]

[0112] in, Indicates the current phase of the robot leg movement, and SwingPhase indicates the phase cycle of the swing leg.

[0113] Second, the foot-end zero velocity constraint means that the foot-end of the supporting leg has zero velocity relative to the ground in the tangential direction of the ground:

[0114]

[0115] The S 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 to the ground, is the foot end acceleration, is the foot end speed, a is a manually given hyperparameter used to adjust the constraint effect;

[0116] StancePhase represents the stance leg phase cycle.

[0117] Third, the dynamic equation constraint can be expressed as (8). Each time NMPC solves the equation, it will perform regional linearization based on the current sampling point.

[0118] Fourth, given the starting point, the highest point, and the end point, the expected trajectory curve can be obtained through step S311. For the end of the arm, it is the task trajectory curve. For the foot end, the starting point is the foot end position just before the swing phase, the highest point is the maximum height that the swinging leg can reach, and the end point is the expected foot landing point. The expected foot landing point p desire It is expressed as:

[0119]

[0120] where 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.

[0121] The inequality constraints include self-collision constraints, joint position constraints, friction cone constraints, joint velocity constraints, and joint torque constraints.

[0122] First, the purpose of the self-collision constraint is to prevent the left and right legs, left and right hands, and body from colliding with each other during walking, which can be expressed as:

[0123] ||p left -p right ||-d threshold ≥0#(23)

[0124] Among them, p left represents the convex position of the left side of the body, p right Indicates the convex position of the right side of the body; the convex position includes the arm end position, foot end position, knee position and body position; d threshold is the closest distance threshold.

[0125] Second, the joint position constraint uses the control barrier function (CBF). Joint motion has nonlinear characteristics. Compared with general linear constraints, the use of CBF constraints can better maintain the nonlinearity of the constraints, which can be expressed as:

[0126]

[0127] in, Represents the acceleration of the jth joint state; γ1, γ2 represent artificially debugged constants.

[0128] Third, the friction cone constraint is intended to ensure that the supporting leg is in contact with the ground but does not slip. The present invention uses a linear friction cone:

[0129] ‖F‖≤μN#(26)

[0130] Among them, F represents the tangential force of the sole of the foot in the support phase, N is the normal support force, and μ is the ground friction coefficient.

[0131] Fourth, the joint velocity constraints and joint torque constraints are similar to (22) and cannot exceed a certain threshold.

[0132] The inequality constraints are added to the cost function of the optimization problem by relaxation or penalty function. At this point, the NMPC problem is constructed as follows:

[0133]

[0134] Where c represents the cost function, L represents the constrained Lagrangian penalty function, g(x(t),u(t)) represents the equality constraint of the optimization problem, t0 represents the current sampling time when constructing the optimization problem, and t f Indicates the length of time for optimization.

[0135] S32. The NMPC problem is discretized through the multiple shooting method and transcribed into the standard form of nonlinear optimization problem (NLP), as shown below:

[0136]

[0137] Where f(x) represents the objective function of the nonlinear optimization problem, g i (x) represents the inequality constraint of the nonlinear optimization problem, h j (x) represents the equality constraint of the nonlinear optimization problem.

[0138] After constructing a complete NMPC problem, in order to speed up the solution of the subsequent solver, it is rewritten in the form of relaxation or penalty function, that is, the inequality constraints are removed. Then the NMPC is discretized and transcribed into the standard form of the NLP problem. Finally, the SQP solver is used to solve the NLP problem in real time at a frequency of 100Hz in the standard form. The results output by NMPC are the desired control trajectory U and the desired state trajectory (predicted trajectory) X with a time domain length of H.

[0139] S4. Finally, the whole body dynamics equation of the humanoid robot is constructed, and then the feedforward joint torque required by the robot is calculated through the inverse dynamics equation. Combined with the PD control strategy of the joint, the output torque sent to the robot is finally obtained.

[0140] S41, establish a whole-body dynamics model of the humanoid robot, and obtain the joint feedforward torque through the inverse dynamics equation. Specifically, the whole-body dynamics equation is expressed as in step S11. Given the current joint position, joint velocity, joint acceleration, and external contact force, the inverse dynamics equation can be obtained:

[0141]

[0142] Among them, D j represents the damping (Coriolis force) and gravity characteristics of the joint, represents the transpose of the contact Jacobian matrix, τ inv is the feedforward torque, λ NMPC is the first value of the optimal control trajectory U of NMPC, The desired acceleration is obtained by combining the output of NMPC with feedback linearization:

[0143]

[0144] in, represents the expected acceleration of the joint, K pinv represents the stiffness constant for feedback linearization, K dinv represents the damping constant for feedback linearization, is the joint acceleration at the current moment. d and are the desired joint velocities and positions, both calculated by NMPC.

[0145] S42, the feedforward torque is combined with the joint stiffness-damping (PD) control to control the joint motor movement at a frequency of 1000 Hz, and the output torque finally sent to the robot is obtained as shown below:

[0146]

[0147] Among them, K p represents the joint control stiffness constant, K d Represents the joint control damping constant.

[0148] Since the center-of-mass dynamics only considers the effect of external forces, after completing the solution of NMPC in step S3, only the expected external force λ after system optimization is obtained. NMPC and joint speed As output, the joint torque is needed to control the robot's motion. Therefore, the relationship between NMPC output and joint torque is established using the inverse dynamics equation to obtain the joint torque required to control the robot. Since the inverse dynamics equation is not a complex optimization problem, it can be sent at a higher frequency, which can improve the stability of the robot's underlying control.

[0149] The walking effect, stability, arm task trajectory planning ability, arm auxiliary balance ability and whole body coordination ability of the method of the present invention have been verified in the simulation software Raisim.

[0150] Those skilled in the art can understand that the above are only preferred examples of the invention and are not intended to limit the invention. Although the invention is described in detail with reference to the above examples, those skilled in the art can still modify the technical solutions recorded in the above examples or replace some of the technical features therein with equivalents. Any modification, equivalent replacement, etc. made within the spirit and principle of the invention shall be included in the protection scope of the invention.

Claims

1. A whole-body coordinated control method for a humanoid robot based on a center of mass dynamics model, characterized in that: The following steps are involved: S1. Establish a center of mass dynamics model of the humanoid robot, select the state, system input and system output of the robot system, and obtain the nonlinear system state equation; S2. Based on the whole body kinematics and the principle of momentum conservation, a model is established around two scenarios: the operation and coordinated movement of the robot arm; Including the relationship between arm momentum and center of mass momentum, and task trajectory tracking at the end of the arm; Obtain task trajectory tracking constraints and angular momentum constraints of the arm end velocity; S3, construct a nonlinear model to predict the optimal control problem, which consists of three parts: cost function, system state equation and constraints; use the solver to solve and obtain the output trajectory of the optimized system; S4. Establish the whole-body dynamics equation of the humanoid robot, obtain the robot's feedforward joint torque according to the inverse dynamics equation, and combine the joint PD control to obtain the final output torque sent to the robot.

2. The whole body coordinated control method of a humanoid robot based on a center of mass dynamics model according to claim 1, characterized in that: The step S1 comprises the following sub-steps: S11. According to the Newton-Euler-Lagrange method, a complete floating basis dynamic equation of the humanoid robot is established, which is simplified to the Newton-Euler equation at the center of mass, and then the center of mass dynamic equation of the humanoid robot is obtained; S12. According to the center of mass dynamics model established in step S11, select the state quantity and control quantity of the robot system to obtain the nonlinear system state equation; wherein the state quantity is composed of the center of mass angle, position, angular velocity, linear velocity, and joint position, and the control quantity is composed of the joint velocity, the contact force and torque at the ends of the two legs.

3. The whole body coordinated control method of a humanoid robot based on a center of mass dynamics model according to claim 1, characterized in that: The step S2 comprises the following sub-steps: S21, establish a mapping relationship between the Cartesian spatial degrees of freedom of the arm end and the task trajectory, obtain the task trajectory tracking constraint, so that the arm end tracks the planned task trajectory and realizes the task operation capability of the arm; S22. Establish the conservation relationship between the momentum of the arm and the angular momentum of the center of mass, and obtain the angular momentum constraint of the velocity of the end of the arm, so that the movement of the arm assists the movement balance of the robot and realizes the movement balance ability of the arm.

4. The whole body coordinated control method of a humanoid robot based on a center of mass dynamics model according to claim 1, characterized in that: The step S3 comprises the following sub-steps: S31, establish a nonlinear model to predict the optimal control problem, which consists of three parts: cost function, system state equation and constraint conditions; S32, using the SQP solver to solve the nonlinear model predictive optimal control problem in real time at a frequency of 100 Hz.

5. The whole body coordinated control method of a humanoid robot based on a center of mass dynamics model according to claim 4, characterized in that: The step S31 includes the following sub-steps: S311, using a Bezier curve in conjunction with a cubic spline curve to plan and interpolate the desired trajectory, the desired trajectory including an expected state quantity trajectory and an expected control quantity trajectory; the expected state quantity trajectory includes the mass center angle, position, angular velocity, linear velocity, joint position, arm task space, arm momentum, foot end position, and foot end velocity; the expected control quantity trajectory includes joint velocity, foot end force, and torque; wherein the expected state quantity trajectory is different during the swing phase and the support phase, and the expected control quantity trajectory is a constant; S312, based on the expected state quantity trajectory and the expected control quantity trajectory of step S311, a state quantity tracking function and a control quantity tracking function are established using a mean square error function to measure the gap between the current actual state and the expected state; and a cost function is obtained; S313. Constraints include equality constraints and inequality constraints; wherein equality constraints include system dynamic equation constraints, foot end curve tracking constraints and end zero force-torque constraints during the swing phase, and end zero velocity constraints during the support phase; inequality constraints include joint position constraints, joint velocity constraints, joint torque constraints, self-collision constraints, and friction cone constraints; inequality constraints are added to the cost function in the form of penalty terms.

6. The whole body coordinated control method of a humanoid robot based on a center of mass dynamics model according to claim 1, characterized in that: The step S4 comprises the following sub-steps: S41, establish a full-body dynamics model of the humanoid robot, knowing the joint position, joint velocity, joint acceleration, and external contact force at the current moment, and obtain the joint feedforward torque through the inverse dynamics equation; S42, feedforward torque combined with joint PD control controls the joint motor movement at a frequency of 1000 Hz.

Images