A whole-body coordinated control method for humanoid robots based on center-of-mass dynamics model

Abstract

The present invention discloses a method for whole-body coordinated control of a humanoid robot based on a center of mass dynamics model, comprising: systematically modeling the humanoid robot according to the 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; constructing the entire nonlinear optimization control problem and solving it in real time, with the controller predicting the robot's motion trajectory for a period of time in the future; calculating the feedforward torque using inverse dynamics, and delivering the joint torque in conjunction with the joint PD controller. The present invention designs a nonlinear model predictive control problem for whole-body coordinated control of a humanoid robot, rationally plans the system state, arm end state, foot end state, and reference trajectory of the control input, and designs the equality and inequality constraints necessary for the robot's whole-body coordinated motion; adds task trajectory constraints and momentum conservation constraints to the arm end, making the arm's ability to assist in motion balance 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 materials science has provided a solid foundation for the research and development of humanoid robots. With an aging population and labor shortages, demand for humanoid robots in areas such as nursing, education, and housework continues to increase. Universities and research institutions are deepening their research in humanoid robot technology, driving the industry's development. As humanoid robots' walking abilities gradually improve, they are demonstrating a need for more everyday and diverse interactions.

[0003] Compared to quadruped robots, the operational capabilities of humanoid robots are primarily reflected in their arms, which should be further unleashed. Compared to fixed-base industrial robotic arms, humanoid robots are mobile robotic arm platforms with a wide range of motion and complex scenarios. Compared to wheeled robotic arms, humanoid robots have more complex degrees of freedom, corresponding to more flexible operational methods. To enable the body to perform various operations more flexibly and coordinately, a series of training and conditioning methods can be adopted. At the same time, maintaining body balance during these operations is also crucial. As a result, how to enable the body to operate more flexibly and coordinately, and how to maintain body balance during operation, are becoming increasingly important issues.

[0004] Currently, most humanoid robot research still focuses on leg movement, and the impact of arm movement on robot balance is often overlooked. For example, patent application CN118682750A only uses a single rigid body model to optimize the robot's online trajectory when implementing jumping control for a humanoid robot, ignoring the impact or contribution of arm and joint movement on the robot's center of mass. Most humanoid robots focus solely on the manipulation capabilities of their arms or dexterous hands, rather than on coordinated whole-body motion control. For example, patent application CN118143954A focuses solely on the planning of a humanoid robot's upper limbs, essentially treating the arms as fixed bases for modeling and planning, without considering their impact on the entire body. Some existing whole-body coordination control methods are not sufficient to truly demonstrate the effects of coordinated arm and whole-body motion. This 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 coordination motion controller designed in patent application CN117944055A only adopts a virtual model control strategy, and does not establish whole-body kinematics. In addition, the control strategy does not have the ability of multi-constraint optimization control, and the whole-body motion of a humanoid robot should be a multi-constraint nonlinear optimization problem.

[0005] In summary, research on humanoid robot motion control methods has made considerable progress. However, these methods often neglect whole-body coordinated motion, under-evaluate the importance of arm motion to overall balance, and lack multi-constraint optimization capabilities in controller design. This, in turn, impacts the stability and flexibility of humanoid robots in complex environments. This means that in practical applications, robots are unable to fully utilize their full-body 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 method for whole-body coordinated control of a humanoid robot based on a center-of-mass dynamics model. This includes systematically modeling the humanoid robot according to the 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 key to achieving arm coordinated balance; constructing the entire nonlinear optimization control problem and solving it in real time, enabling the controller to predict the robot's motion trajectory over time to better maintain stability; and calculating the feedforward torque using inverse dynamics, which is then combined with the joint PD controller to deliver the joint torque. This invention designs a controller that simultaneously achieves both the operational capabilities of the humanoid robot's arm and the coordinated whole-body motion and balance capabilities.

[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 whole-body kinematics and the principle of momentum conservation, a model is established for two scenarios: manipulation and coordinated motion of the robot arm. This includes the relationship between arm momentum and center of mass momentum, and task trajectory tracking at the end of the arm. The 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 obtain the optimized system output trajectory;

[0011] S4. Establish the whole-body dynamic equation of the humanoid robot, obtain the robot's feedforward joint torque based on the inverse dynamic equation, and combine it with 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. Establish the complete floating basis dynamic equation of the humanoid robot according to the Newton-Euler-Lagrange method, simplify it to the Newton-Euler equation at the center of mass, and then obtain the center-of-mass dynamic equation of the humanoid robot;

[0014] S12. According to the center of mass dynamics model established in step S11, the state quantity and control quantity of the robot system are selected 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, 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, and obtain the task trajectory tracking constraint, so that the arm end can track the planned task trajectory and realize the task operation capability of the arm;

[0017] S22. Establish the conservation relationship between the arm momentum and the angular momentum of the center of mass, and obtain the angular momentum constraint of the arm end velocity, 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, 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. Use 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. Plan and interpolate a desired trajectory using a Bezier curve in conjunction with a cubic spline curve, wherein the desired trajectory includes a desired state trajectory and a desired control trajectory; the desired state trajectory includes the center of mass angle, position, angular velocity, linear velocity, joint position, arm task space, arm momentum, foot position, and foot velocity; the desired control trajectory includes the joint velocity, foot force, and torque; wherein the desired state trajectory is different during the swing phase and the stance phase, and the desired control trajectory is a constant;

[0023] S312: Based on the desired state quantity trajectory and the desired control quantity trajectory in step S311, a mean square error function is used to establish a state quantity tracking function and a control quantity tracking function to measure the gap between the current actual state and the desired state; and a cost function is obtained;

[0024] S313. Constraints include equality constraints and inequality constraints; among them, 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, step S4 includes the following sub-steps:

[0026] S41. Establish a full-body dynamics model of the humanoid robot. Given the current joint positions, joint velocities, joint accelerations, and external contact forces, obtain the joint feedforward torques through inverse dynamics equations.

[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 the single rigid body model or the 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 higher 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 end 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 based on 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 generated for an arm static task according to an embodiment of the present invention;

[0031] Figure 3 A whole-body coordinated motion graph generated for an arm dynamic task according to 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 5This is a motion diagram of an arm-assisted balance under interference generated by an embodiment of the present invention. DETAILED DESCRIPTION

[0034] The present invention will be described in detail below based on the accompanying drawings and preferred embodiments. The purpose and effects of the present invention will become more apparent. 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 for explaining 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 for a humanoid robot, we first need to define the state variables, input variables, and output variables of the robot system. Then, we can 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 whole-body control, that is, the input and output of the system include joint variables, and the entire system is nonlinear. Compared to the whole-body dynamics model that considers 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. First, the complete floating base dynamics model is 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 characteristics of the joint, represents the center of mass acceleration, Indicates the acceleration of the joint state, D b Denotes 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), on which the joint torque does not directly act, 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 equations to the Newton-Euler equations at the center of mass:

[0045]

[0046] Among them, 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 angles change, the CMM also changes, thus affecting the center of mass state. This equation is the center of mass dynamics equation for the humanoid robot.

[0047] S12. Determine the state variables and control variables of the robot system to derive the state equations of the nonlinear system. In the center of mass dynamics model established based on step S11, the state variables include the angle, position, angular velocity, linear velocity, and joint positions of the center of mass, while the control variables include the joint velocities, the contact forces at the two leg ends, and the applied torques.

[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 under the action of the system input and the derivative of the system state quantity.

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

[0052]

[0053] in, is the center of mass 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 of 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 net 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 center of mass inertia matrix, and the center of mass 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 center of mass itself and the influence of the joints on the center of mass, while ignoring the coupling between joints. This can greatly reduce the amount of calculation 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 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 entire 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, Represents the state description of the arm end trajectory in the robot coordinate system, represents the position of the arm end trajectory, Indicates 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 arm end trajectory 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 of the end 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, even when a humanoid robot lacks arms, the robot can achieve motion balance by planning the footholds during the swing phase. Therefore, the conservation of angular momentum here only considers the relationship between the arm momentum and the center of mass angular momentum.

[0076] Given the angular velocity of the center of mass, the position of the arm joint, 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 follows:

[0077]

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

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

[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] Angular momentum constraint will be 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 in different directions to the robot, its arm-assisted balance ability is shown as follows: Figure 5 shown.

[0084] With center-of-mass dynamics modeled, a humanoid robot can achieve basic stable motion. After completing the modeling of the robot's arm's manipulation and coordinated motion, the robot can utilize task trajectory constraints to achieve autonomous arm trajectory planning in task space and utilize angular momentum constraints to achieve dual-arm assisted balance. The specific solution method will be discussed below.

[0085] S3. Establish a nonlinear model predictive optimal control (NMPC) problem, which consists of three parts: cost function, system state equation, and constraints. 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 desired 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 discretized, and the discretization method usually adopts the Newton-Euler method or the 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; 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 samples 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 the 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 end position and linear velocity of the arm, 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 equality constraints are divided into the swing leg period and the stance leg period. The equality constraints during the swing leg period include the end zero force-torque constraint, the double leg trajectory tracking constraint, and the arm trajectory tracking constraint; the equality constraints during the stance 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 steps S22 and S311. The other equality constraints are as follows:

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

[0111]

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

[0113] Secondly, 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 of the ground, is the foot end acceleration, is the foot end velocity, 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). NMPC will perform regional linearization at the current sampling point each time it solves the equation.

[0118] Fourthly, given the starting point, highest point, and end point, the desired trajectory curve can be obtained through step S311. For the end of the arm, this 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 swing leg can reach, and the end point is the desired landing point. The desired landing point p is the desired landing point. desire 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] 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 position of the end of the arm, the position of the foot, the position of the knee and the position of the body; d threshold is the closest distance threshold.

[0125] Secondly, the joint position constraint uses a 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 constraint, which can be expressed as:

[0126]

[0127] in, Represents the acceleration of the j-th joint state; γ1 and γ2 represent constants given by artificial debugging.

[0128] Third, the friction cone constraint is intended to ensure that the support 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 constraint and joint torque constraint are similar to (22) and cannot exceed a certain threshold.

[0132] The inequality constraints are added to the cost function of the optimization problem through relaxation or penalty functions. 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, to speed up subsequent solver execution, it is rewritten in a relaxed or penalty form, removing inequality constraints. The NMPC is then 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 100 Hz in standard form. The NMPC outputs are the desired control trajectory U and the desired state trajectory (predicted trajectory) X, both with a time domain length of H.

[0139] S4. Finally, the full-body dynamics equations of the humanoid robot are constructed. The feedforward joint torques required by the robot are then calculated using the inverse dynamics equations. Combined with the PD control strategy of the joints, the output torques sent to the robot are ultimately obtained.

[0140] S41. Establish a full-body dynamics model for the humanoid robot and obtain the joint feedforward torque using the inverse dynamics equation. Specifically, the full-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 NMPC optimized control trajectory U, 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, feedforward torque combined with joint stiffness-damping (PD) control controls the joint motor motion at a frequency of 1000 Hz to obtain the final output torque sent to the robot. It is 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 NMPC solution in step S3, only the expected external force λ after system optimization is obtained. NMPC and joint velocity As output, the robot's motion is ultimately controlled through joint torques. Therefore, the relationship between NMPC output and joint torques is established using inverse dynamics equations, resulting in the joint torques required to control the robot. Because inverse dynamics equations are not complex optimization problems, they can be transmitted at a high frequency, improving 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 will understand that the foregoing descriptions are merely preferred embodiments of the invention and are not intended to limit the invention. Although the invention has been described in detail with reference to the foregoing examples, those skilled in the art will still be able to modify the technical solutions described in the foregoing examples or substitute equivalents for some of the technical features therein. Any modifications, equivalent substitutions, etc. made within the spirit and principles of the invention shall be included within the scope of protection of the invention.

Claims

1. A method for whole-body coordinated control of 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; This includes the relationship between arm momentum and center of mass momentum, and task trajectory tracking at the end of the arm; Obtain the task trajectory tracking constraints and the 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 obtain the optimized system output trajectory; S4. Establish the whole-body dynamic equation of the humanoid robot, obtain the robot's feedforward joint torque based on the inverse dynamic equation, and combine it with 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 includes the following sub-steps: S11. Establish the complete floating basis dynamic equation of the humanoid robot according to the Newton-Euler-Lagrange method, simplify it to the Newton-Euler equation at the center of mass, and then obtain the center-of-mass dynamic equation of the humanoid robot; S12. According to the center of mass dynamics model established in step S11, the state quantity and control quantity of the robot system are selected 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 includes the following sub-steps: S21. Establish a mapping relationship between the Cartesian spatial degrees of freedom of the arm end and the task trajectory, and obtain the task trajectory tracking constraint, so that the arm end can track the planned task trajectory and realize the task operation capability of the arm; S22. Establish the conservation relationship between the arm momentum and the angular momentum of the center of mass, and obtain the angular momentum constraint of the arm end velocity, 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 includes 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. Use 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. Plan and interpolate a desired trajectory using a Bezier curve in conjunction with a cubic spline curve, wherein the desired trajectory includes a desired state trajectory and a desired control trajectory; the desired state trajectory includes the center of mass angle, position, angular velocity, linear velocity, joint position, arm task space, arm momentum, foot position, and foot velocity; the desired control trajectory includes the joint velocity, foot force, and torque; wherein the desired state trajectory is different during the swing phase and the stance phase, and the desired control trajectory is a constant; S312: Based on the desired state quantity trajectory and the desired control quantity trajectory in step S311, a mean square error function is used to establish a state quantity tracking function and a control quantity tracking function to measure the gap between the current actual state and the desired state; and a cost function is obtained; S313. Constraints include equality constraints and inequality constraints; among them, 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 includes the following sub-steps: S41. Establish a full-body dynamics model of the humanoid robot. Given the current joint positions, joint velocities, joint accelerations, and external contact forces, obtain the joint feedforward torques through inverse dynamics equations. S42. Feedforward torque combined with joint PD control controls the joint motor movement at a frequency of 1000 Hz.

Images