A humanoid robot standing method with variable load based on model reference adaptive control

Abstract

The present invention discloses a method for humanoid robot standing with variable load based on model reference adaptive control, comprising the following steps: constructing a three-link model of the humanoid robot, deriving its dynamic equation, and obtaining the joint torque expression of the three-link model; using the calculated joint torque as the feedforward torque, and forming the joint torque control quantity together with the output of the PD controller, and inputting it into the control system of the humanoid robot; calculating the update rate of the dynamic parameter estimation value, updating the feedforward torque, until the control error of the joint angle and the joint angular velocity is eliminated, and completing the construction of the model reference adaptive control algorithm when the humanoid robot stands with both legs side by side; and debugging the humanoid robot standing with variable load. The present invention uses feedforward control, and calculates the joint torque required to support the gravity load through the dynamic equation of the three-link model when the legs stand side by side, and directly adds it to the output end of the PD controller, which not only shortens the control time, but also ensures the joint control accuracy.

Description

Technical Field

[0001] The invention relates to the field of humanoid robot motion control, and in particular to a humanoid robot variable load standing method based on model reference adaptive control. Background Art

[0002] Humanoid robots have flourished in recent decades, especially in the fields of gait generation and balance control, and have achieved good results. However, there is still a long way to go before they can be truly applied, and many key issues remain to be resolved. At present, research on humanoid robots often ignores research on their load capacity, and there is little research on load gravity compensation required for common tasks such as carrying and lifting heavy objects. The main issues that need to be considered in load gravity compensation are the ability to resist joint offset and variable load compensation. Among them, joint offset refers to the difference between the actual joint position and speed of the humanoid robot under the action of load gravity and the joint position and speed instructions, as well as the overall posture offset of the robot caused by this difference; variable load compensation capability refers to the ability to compensate for gravity loads and resist joint offsets in the face of unknown loads or changes in load size. Variable load compensation capability is an advanced form of robot load capacity and the basis for humanoid robots to complete common tasks such as carrying and lifting heavy objects, which is of great significance to the practical application of humanoid robots.

[0003] The load capacity of the above-mentioned robot depends on the joint drive mode and mechanical design structure. When the load gravity is large enough, the robot joint will be offset and cause posture control errors. In particular, for humanoid robots driven by motors, the above problems are particularly significant due to the limited joint drive capacity. In fact, by adopting appropriate control strategies and giving full play to the joint drive capacity, the load capacity can be effectively improved. The current gravity load compensation strategies include increasing the rigidity of the joints and adding integrators to the controller. Although increasing the rigidity of the joints only reduces the position offset during the lifting task, it sacrifices the compliance of the robot; although only using integrators can adapt to the changes in the robot's gravity load, it takes time for the integrator to take effect and cannot meet the requirements of some high-dynamic tasks. It can be seen that although these methods in the prior art can solve the problems of gravity load changes and model errors, they are designed for specific problems in specific situations, and are not systematic or general methods for gravity compensation problems in multi-link systems. Summary of the invention

[0004] In view of the shortcomings of the prior art, the present invention proposes a variable load standing control method for a humanoid robot when standing with two legs side by side. The present invention derives the dynamic equation based on a simplified three-link model of the humanoid robot when standing with two legs side by side; the design model refers to the adaptive control algorithm, and provides the joint torque required to support the load gravity by applying appropriate feedforward torque. Since the adaptive control method can adjust the dynamic parameters in real time according to the joint control error, the humanoid robot of the present invention can maintain a high joint control accuracy when supporting an unknown load. The present invention has a certain versatility on common bipedal robots or humanoid robot platforms, and has good applicability for different types of load tasks such as handling and lifting heavy objects.

[0005] The purpose of the present invention is achieved through the following technical solutions:

[0006] A method for standing with a humanoid robot under variable load based on model reference adaptive control comprises the following steps:

[0007] Step 1: Construct a simplified humanoid robot dynamics model and derive the dynamics equations, which can be achieved by:

[0008] A humanoid robot with two legs standing side by side is simplified to a three-link model; a dynamic equation of the three-link model is derived based on the Lagrange equation; the form of the dynamic equation is converted to obtain a joint torque expression of the three-link model related to the dynamic parameters;

[0009] Step 2: Design a model-referenced adaptive control algorithm for a humanoid robot standing with its legs side by side, which is implemented in the following ways:

[0010] Substituting the desired angle, desired angular velocity, and desired angular acceleration into the joint torque expression of the three-link model related to the dynamic parameters, the estimated value of the joint torque is calculated, and the estimated value is applied to the output end of the PD controller as a feedforward torque; and the reference joint angular velocity is calculated at the same time;

[0011] The joint torque of the three-link model is calculated according to the feedforward torque and the differential coefficient of the PD controller, and is input into the control system of the humanoid robot as a control input, and the actual angle and angular velocity of each joint of the humanoid robot are obtained as output;

[0012] According to the actual angular velocity of each joint of the humanoid robot and the reference joint angular velocity, a generalized error is calculated, and then the update rate of the dynamic parameter estimation value is calculated by the generalized error, the dynamic parameters are updated, and a more accurate feedforward torque is obtained until the control errors of the joint angle and angular velocity are eliminated.

[0013] Furthermore, the dynamic equation of the three-link model derived based on the Lagrange equation is specifically implemented in the following manner:

[0014] The Lagrange equation in a multi-link system is:

[0015]

[0016] Where θ is the joint angle; is the joint angular velocity; τ is the joint torque; is the kinetic energy of the system; V(θ,t) is the gravitational potential energy;

[0017] The three-link model includes from top to bottom: a trunk link, a hip joint, a thigh link, a knee joint, a calf link, an ankle joint, and a foot link; the hip joint is a structure connecting the trunk link and the thigh link, the knee joint is a structure connecting the thigh link and the calf link, and the ankle joint is a structure connecting the calf link and the foot link, and the hip joint and the ankle joint can rotate in multiple directions;

[0018] According to the physical parameters of each link of the humanoid robot, the kinetic energy and gravitational potential energy of each link in the three-link model are calculated and substituted into equation (1) to obtain:

[0019]

[0020] Where H(θ) is the real symmetric inertia matrix; is the joint angular acceleration; is the matrix related to the centripetal force and the Coriolis force; g(θ) is the matrix determined by gravity, H(θ), The specific expression of g(θ) is as follows:

[0021]

[0022]

[0023]

[0024] Wherein, θ1 is the angle between the shank link and the vertical direction, θ2 is the angle between the thigh link and the shank link, and θ3 is the angle between the trunk link and the thigh link; is the joint angular velocity of the ankle joint in the y direction, where the y direction is the direction pointing to the left side of the body when the humanoid robot stands with both legs side by side; is the joint angular velocity of the knee joint in the y direction, is the angular velocity of the hip joint in the y direction; a1, 2, 3, 4, 5, 6, 7, 8, 9 are all dynamic parameters, and their specific expressions are as follows:

[0025]

[0026] Wherein, m1 represents the mass of the shank link, m2 represents the mass of the thigh link, and m3 represents the mass of the trunk link; I1 represents the moment of inertia of the shank link around its center of mass, I2 represents the moment of inertia of the thigh link around its center of mass, and I3 represents the moment of inertia of the trunk link around its center of mass; l1 represents the length of the shank link, and l2 represents the length of the thigh link; d1 represents the distance between the center of mass of the shank link and the ankle joint, d2 represents the distance between the center of mass of the thigh link and the knee joint, and d3 represents the distance between the center of mass of the trunk link and the hip joint; g represents the acceleration due to gravity.

[0027] Furthermore, the conversion of the form of the kinetic equation is specifically achieved in the following manner:

[0028] Vector Decompose into a matrix and the joint angular velocity vector In product form, the matrix Satisfies the derivative with H(θ) The matrix formed For an antisymmetric matrix, the matrix and the joint angular velocity vector Substituting into formula (2) we get:

[0029]

[0030] In the formula, the matrix The specific expression is as follows:

[0031]

[0032] Using the linear properties of dynamic parameters, the joint torque τ is decomposed into:

[0033]

[0034] Where a is a dynamic parameter, a1, a2, a3, a4, a5, a6, a7, a8, a9 are components of a, and a is related to the mechanical parameters of each link of the robot; Y is a regression matrix, which is only related to the motion state of the robot. The specific expression of Y is as follows:

[0035]

[0036]

[0037] In the formula, represents the angular acceleration of the ankle joint in the y direction, represents the angular acceleration of the knee joint in the y direction, represents the angular acceleration of the hip joint in the y direction.

[0038] Furthermore, the model reference adaptive control algorithm is specifically implemented in the following manner:

[0039] According to the joint torque expression of the three-link model related to the dynamic parameters, the estimated value of the joint torque is calculated and used as the feedforward torque. The expression of the feedforward torque is:

[0040]

[0041] In the formula, are the estimated values ​​of the kinetic parameters, is the estimated value of H(θ), represents the reference joint angular acceleration, for The estimated value of is the estimated value of g(θ);

[0042] The feedforward torque Applied to the output end of the PD controller, combined with the differential coefficient of the PD controller, the joint torque of the three-link model is calculated

[0043]

[0044] In the formula, d represents the differential coefficient of the PD controller, which is a positive diagonal matrix and needs to be adjusted manually; s represents the generalized error,

[0045] d s is the proportional-derivative term, which is equivalent to the PD controller;

[0046] Design parameter update law, update dynamic parameters according to generalized error s, and obtain more accurate feedforward torque The expression of the parameter update law is as follows:

[0047]

[0048]

[0049]

[0050]

[0051] In the formula, is the update rate of the estimated values ​​of the dynamic parameters; Γ represents the parameter update rate, which is a positive diagonal matrix and needs to be adjusted manually; Represents the joint angular velocity error; Λ represents the proportional coefficient of the PD controller, which is a positive diagonal matrix and needs to be adjusted manually; represents the joint angle error; Indicates the actual joint angular velocity; Represents the reference joint angular velocity, which is the reference output; represents the expected joint angular velocity; θ represents the actual joint angle, θ d represents the desired joint angle.

[0052] Furthermore, the physical parameters of each connecting rod of the humanoid robot include: mass, length, center of mass position, and inertia tensor of each connecting rod of the humanoid robot.

[0053] The beneficial effects of the present invention are:

[0054] (1) The existing humanoid robot gravity load compensation scheme, whether increasing the rigidity of the joint or adding an integrator in the controller, belongs to feedback control and cannot achieve high-precision joint control effect in a very short time. The present invention uses feedforward control to calculate the joint torque required to support the gravity load through the corresponding three-link model dynamic equation when the legs are standing side by side, and directly adds it to the output end of the PD controller, which not only shortens the control time but also ensures the joint control accuracy.

[0055] (2) In order to adapt to load changes, the present invention designs a model reference adaptive control algorithm, which adjusts the dynamic parameters and changes the joint torque calculated by the dynamic equation to ensure the accuracy of joint control.

[0056] (3) The present invention has certain versatility on common bipedal robots or humanoid robot platforms, and has good applicability for different types of load tasks such as carrying and lifting heavy objects. BRIEF DESCRIPTION OF THE DRAWINGS

[0057] Figure 1 This is a simplified equivalent three-link model diagram of a humanoid robot standing with its legs side by side.

[0058] Figure 2 It is a model reference adaptive control flow diagram of a humanoid robot.

[0059] Figure 3 This is a diagram showing the effect of controlling the center of mass height of a humanoid robot when a gravity load is suddenly added.

[0060] In the figure, the trunk link 1, the trunk link center of mass 2, the hip joint 3, the thigh link 4, the thigh link center of mass 5, the knee joint 6, the calf link 7, the calf link center of mass 8, and the ankle joint 9. DETAILED DESCRIPTION

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

[0062] The variable load standing method of a humanoid robot based on model reference adaptive control of the present invention is specifically implemented by the following steps:

[0063] Step 1: Construct a simplified humanoid robot dynamics model and derive the dynamics equations.

[0064] Based on the Lagrange equation, a humanoid robot standing with its legs side by side can be simplified to be equivalent to: Figure 1 The three-bar linkage model is shown and its dynamic equations are derived.

[0065] like Figure 1 As shown, it is a simplified equivalent three-link model diagram when the legs of the humanoid robot stand side by side. One leg of the humanoid robot includes from top to bottom: trunk link 1, hip joint 3, thigh link 4, knee joint 6, shank link 7, ankle joint 9, and foot link. Among them, the hip joint 3 is a structure connecting the trunk link 1 and the thigh link 4, which can rotate in multiple directions; the knee joint 6 is a structure connecting the thigh link 4 and the shank link 7; the ankle joint 9 is a structure connecting the shank link 7 and the foot link, which can rotate in multiple directions. In this embodiment, the joint angle, angular velocity, and angular acceleration of each joint specifically refer to the angle, angular velocity, and angular acceleration in the y direction, and the y direction is the direction pointing to the left side of the body when the legs of the humanoid robot stand side by side. The center of mass of each link is taken as the center of mass 2 of the trunk link, the center of mass 5 of the thigh link, and the center of mass 8 of the calf link; the distances from the center of mass of each link to each adjacent joint (downward) are d1, d2, and d3, the lengths of each link are l1, l2, and l3, and the joint angles are θ1, θ2, and θ3.

[0066] The Lagrange equation in a multi-link system is:

[0067]

[0068] Where θ is the joint angle; is the joint angular velocity; τ is the joint torque; is the kinetic energy of the system; V(θ,t) is the gravitational potential energy.

[0069] According to the mass, length, center of mass position, inertia tensor and other parameters of each link of the humanoid robot, the kinetic energy and gravitational potential energy of each link in the three-link model are calculated and substituted into equation (1) to obtain the dynamic equation of the three-link model:

[0070]

[0071] Where H(θ) is the real symmetric inertia matrix; is the joint angular acceleration; is the matrix related to the centripetal force and the Coriolis force; g(θ) is the matrix determined by gravity, H(θ), The specific expression of g(θ) is as follows:

[0072]

[0073]

[0074]

[0075] Wherein, θ1 is the angle between the shank link 7 and the vertical direction, θ2 is the angle between the thigh link 4 and the shank link 7, and θ3 is the angle between the trunk link 1 and the thigh link 4; is the joint angular velocity of ankle joint 9 in the y direction, is the joint angular velocity of knee joint 6 in the y direction, is the angular velocity of hip joint 3 in the y direction; a1, a2, a3, a4, a5, a6, a7, a8, a9 are all dynamic parameters, and their specific expressions are as follows:

[0076]

[0077] Wherein, m1 represents the mass of the shank link 7, m2 represents the mass of the thigh link 4, and m3 represents the mass of the trunk link 1; I1 represents the moment of inertia of the shank link 7 around its center of mass, I2 represents the moment of inertia of the thigh link 4 around its center of mass, and I3 represents the moment of inertia of the trunk link 1 around its center of mass; l1 represents the length of the shank link 7, and l2 represents the length of the thigh link 4; d1 represents the distance between the center of mass of the shank link 8 and the ankle joint 9, d2 represents the distance between the center of mass of the thigh link 5 and the knee joint 6, and d3 represents the distance between the center of mass of the trunk link 2 and the hip joint 3; g represents the acceleration due to gravity.

[0078] Step 2: Transform the form of the kinetic equation and define the kinetic parameters.

[0079] In order to facilitate the application of the model reference adaptive control method, it is necessary to transform the form of the dynamic equation. Decompose into a matrix and the joint angular velocity vector The product form of the matrix Satisfies the derivative with H(θ) The matrix formed For an antisymmetric matrix, the matrix and the joint angular velocity vector Substituting into formula (2) we get:

[0080]

[0081] In the formula, the matrix The specific expression is as follows:

[0082]

[0083] Using the linear properties of dynamic parameters, the joint torque τ is decomposed into:

[0084]

[0085] Where a is a dynamic parameter, a1, a2, a3, a4, a5, a6, a7, a8, a9 are its components, a is related to the mechanical parameters of the robot such as the mass, length, and center of mass position of each link; Y is a regression matrix, which is only related to the robot's motion state (each joint angle, angular velocity, angular acceleration), and the specific expression of Y is as follows:

[0086]

[0087] In the formula, represents the angular acceleration of ankle joint 9 in the y direction, represents the angular acceleration of knee joint 6 in the y direction, represents the angular acceleration of hip joint 3 in the y direction; Y 14 , Y 24 , Y 15 , Y 25 , Y 35 , Y 16 , Y 26 , Y 36 The specific expression is as follows:

[0088]

[0089] Step 3: Design a model reference adaptive control algorithm to adjust the dynamic parameters to adapt to load changes.

[0090] like Figure 2 The figure shows the model reference adaptive control flow chart of the humanoid robot. When the robot carries or lifts heavy objects, the change of load can be regarded as the change of the robot's own dynamic parameters. The model reference adaptive algorithm adjusts the estimated value of the dynamic parameters according to the generalized error and changes the feedforward torque to ensure the joint control accuracy. The specific process is as follows:

[0091] (3.1) Model reference adaptive control algorithm with desired joint angle θ d , expected joint angular velocity Expected joint angular acceleration As input, the estimated value of the joint torque is calculated according to the joint torque expression of the three-link model related to the dynamic parameters, which is used as the feedforward torque And apply it to the output of the traditional PD controller.

[0092]

[0093] In the formula, are the estimated values ​​of the kinetic parameters, is the estimated value of H(θ), represents the reference joint angular acceleration, for An estimated value of represents the reference joint angular velocity, and its specific expression is shown in the following formula (16); is the estimated value of g(θ).

[0094] (3.2) Design the control law according to the feedforward torque and the differential coefficient K of the PD controller d Calculate the joint torque It is used as the control input and input into the humanoid robot control system, and the actual angle θ and angular velocity of each joint of the humanoid robot are output.

[0095]

[0096] In the formula, is the joint torque, which is the control quantity; K d represents the differential coefficient of the PD controller, which is a positive diagonal matrix and needs to be adjusted manually; s represents the generalized error, and its specific expression is shown in the following formula (15); -K d s is the proportional-derivative term, which is equivalent to the PD controller.

[0097] (3.3) Design parameter update law, based on actual joint angular velocity and the reference joint angular velocity The generalized error s is obtained, and then the update rate of the estimated value of the dynamic parameters is calculated by the generalized error s Update the dynamic parameters to obtain more accurate feedforward torque until the control errors of joint angle and angular velocity are eliminated.

[0098] The expression of the parameter update law is as follows:

[0099]

[0100]

[0101]

[0102]

[0103] In the formula, is the update rate of the estimated values ​​of the dynamic parameters; Γ represents the parameter update rate, which is a positive diagonal matrix and needs to be adjusted manually; s represents the generalized error; Represents the joint angular velocity error; Λ represents the proportional coefficient of the PD controller, which is a positive diagonal matrix and needs to be adjusted manually; represents the joint angle error; Indicates the actual joint angular velocity; Represents the reference joint angular velocity, which is the reference output; represents the expected joint angular velocity; θ represents the actual joint angle, θ d represents the desired joint angle.

[0104] The stability of this model reference adaptive control system is proved based on Lyapunov stability theory. The Lyapunov function is selected as:

[0105]

[0106] In the formula, is the error in the estimation of kinetic parameters.

[0107] Since H(θ) is the inertia matrix, the kinetic energy of the multi-link system is:

[0108]

[0109] The kinetic energy of a system is always greater than zero, so have Therefore, the inertia matrix H(θ) is positive definite; since Γ is a positive diagonal matrix given artificially, it is positive definite, so V(t) is positive definite, and its derivative is:

[0110]

[0111] In the formula, is the derivative of s, is the derivative of H, for The derivative of .

[0112] because is an antisymmetric matrix, so Transformation yields:

[0113]

[0114] Substituting formula (21) into formula (20), we get:

[0115]

[0116] therefore is negative definite. According to Lyapunov's stability theory, when t→∞, s→0, Considering the generalized error definition When s→0, This proves the global stability of the system, the convergence of the control errors of joint angles and joint angular velocities, and the estimation errors of dynamic parameters from a theoretical perspective.

[0117] The following is the variable load standing debugging of the humanoid robot.

[0118] (1) Obtain the mass, length, center of mass position, inertia tensor and other parameters of each link of the humanoid robot as the initial values ​​of the dynamic parameters; input the initial values ​​into the model reference adaptive control algorithm, adjust the parameter update rate Γ, differential coefficient K d , proportional coefficient Λ, so that when the load changes, the model reference adaptive control algorithm can adjust the dynamic parameters in time to eliminate the joint control error.

[0119] (2) Change the load of the humanoid robot to verify whether the variable load standing control of the humanoid robot achieves the expected effect. In order to verify the control effect of the model reference adaptive control algorithm, a humanoid robot that can be simplified into a three-link model is used for simulation verification on a dynamics simulation platform.

[0120] In this embodiment, the "Wukong-III" URDF model is used to perform simulation verification on the Raisim platform. The results are as follows: Figure 3 The humanoid robot "Wukong-III" has an upright height of 1.409m and weighs 55.0307kg. When a 300N vertical downward force is applied to its center of mass at t=4s, the height change of the center of mass is observed. It can be seen that after the sudden load, the center of mass height is restored within 0.5s, and the maximum offset is less than 0.012m, indicating that the model reference adaptive method can ensure a good posture maintenance effect under a large gravity load.

[0121] Based on the Lagrange equation, the present invention simplifies a humanoid robot with two legs standing side by side into a three-link model, and derives its dynamic equation. The joint angle and angular velocity are taken as the controlled objects, the joint torque is taken as the control input, and the dynamic parameters in the dynamic equation are taken as the adjustable parameters. A model reference adaptive control framework is constructed, and the traditional feedback control based on PD control and the feedforward control based on the dynamic equation are combined to obtain a high-precision joint control effect under unknown loads.

[0122] 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 method for humanoid robot standing with variable load based on model reference adaptive control, characterized in that: The following steps are involved: Step 1: Construct a simplified humanoid robot dynamics model and derive the dynamics equations, which can be achieved by: A humanoid robot with two legs standing side by side is simplified to a three-link model; a dynamic equation of the three-link model is derived based on the Lagrange equation; the form of the dynamic equation is converted to obtain a joint torque expression of the three-link model related to the dynamic parameters; Step 2: Design a model-referenced adaptive control algorithm for a humanoid robot standing with its legs side by side, which is implemented in the following ways: Substituting the desired angle, desired angular velocity, and desired angular acceleration into the joint torque expression of the three-link model related to the dynamic parameters, the estimated value of the joint torque is calculated, and the estimated value is applied to the output end of the PD controller as a feedforward torque; and the reference joint angular velocity is calculated at the same time; The joint torque of the three-link model is calculated according to the feedforward torque and the differential coefficient of the PD controller, and is input into the control system of the humanoid robot as a control input, and the actual angle and angular velocity of each joint of the humanoid robot are obtained as output; According to the actual angular velocity of each joint of the humanoid robot and the reference joint angular velocity, a generalized error is calculated, and then an update rate of the estimated value of the dynamic parameter is calculated by the generalized error, and the dynamic parameter is updated to obtain a more accurate feedforward torque until the control error of the joint angle and angular velocity is eliminated; The dynamic equation of the three-link model derived based on the Lagrange equation is specifically implemented in the following way: The Lagrange equation in a multi-link system is: Where θ is the joint angle; is the joint angular velocity; τ is the joint torque; is the system kinetic energy; V(θ,t) is the gravitational potential energy; The three-link model includes from top to bottom: a trunk link, a hip joint, a thigh link, a knee joint, a calf link, an ankle joint, and a foot link; the hip joint is a structure connecting the trunk link and the thigh link, the knee joint is a structure connecting the thigh link and the calf link, and the ankle joint is a structure connecting the calf link and the foot link, and the hip joint and the ankle joint can rotate in multiple directions; According to the physical parameters of each link of the humanoid robot, the kinetic energy and gravitational potential energy of each link in the three-link model are calculated and substituted into equation (1) to obtain: Where H(θ) is the real symmetric inertia matrix; is the joint angular acceleration; is the matrix related to the centripetal force and the Coriolis force; g(θ) is the matrix determined by gravity, H(θ), The specific expression of g(θ) is as follows: Wherein, θ1 is the angle between the shank link and the vertical direction, θ2 is the angle between the thigh link and the shank link, and θ3 is the angle between the trunk link and the thigh link; is the joint angular velocity of the ankle joint in the y direction, where the y direction is the direction pointing to the left side of the body when the humanoid robot stands with both legs side by side; is the joint angular velocity of the knee joint in the y direction, is the angular velocity of the hip joint in the y direction; a1, a2, a3, a4, a5, a6, a7, a8, a9 are all dynamic parameters, and their specific expressions are as follows: Wherein, m1 represents the mass of the shank link, m2 represents the mass of the thigh link, and m3 represents the mass of the trunk link; I1 represents the moment of inertia of the shank link around its center of mass, I2 represents the moment of inertia of the thigh link around its center of mass, and I3 represents the moment of inertia of the trunk link around its center of mass; l1 represents the length of the shank link, and l2 represents the length of the thigh link; d1 represents the distance between the center of mass of the shank link and the ankle joint, d2 represents the distance between the center of mass of the thigh link and the knee joint, and d3 represents the distance between the center of mass of the trunk link and the hip joint; g represents the acceleration due to gravity; The conversion of the form of the kinetic equation is specifically achieved in the following manner: Vector Decompose into a matrix and the joint angular velocity vector In product form, the matrix Satisfies the derivative with H(θ) The matrix formed For an antisymmetric matrix, the matrix and the joint angular velocity vector Substituting into formula (2) we get: In the formula, the matrix The specific expression is as follows: Using the linear properties of dynamic parameters, the joint torque τ is decomposed into: Where a is a dynamic parameter, a1, a2, a3, a4, a5, a6, a7, a8, a9 are components of a, and a is related to the mechanical parameters of each link of the robot; Y is a regression matrix, which is only related to the motion state of the robot. The specific expression of Y is as follows: In the formula, represents the angular acceleration of the ankle joint in the y direction, represents the angular acceleration of the knee joint in the y direction, represents the angular acceleration of the hip joint in the y direction; The model reference adaptive control algorithm is specifically implemented in the following way: According to the joint torque expression of the three-link model related to the dynamic parameters, the estimated value of the joint torque is calculated and used as the feedforward torque. The expression of the feedforward torque is: In the formula, are the estimated values ​​of the kinetic parameters, is the estimated value of H(θ), represents the reference joint angular acceleration, for The estimated value of is the estimated value of g(θ); The feedforward torque Applied to the output end of the PD controller, combined with the differential coefficient of the PD controller, the joint torque of the three-link model is calculated In the formula, K d Represents the differential coefficient of the PD controller, which is a positive diagonal matrix and needs to be adjusted manually; s represents the generalized error, -K d s is the proportional-derivative term, which is equivalent to the PD controller; Design parameter update law, update dynamic parameters according to generalized error s, and obtain more accurate feedforward torque The expression of the parameter update law is as follows: In the formula, is the update rate of the estimated values ​​of the dynamic parameters; Γ represents the parameter update rate, which is a positive diagonal matrix and needs to be adjusted manually; Represents the joint angular velocity error; Λ represents the proportional coefficient of the PD controller, which is a positive diagonal matrix and needs to be adjusted manually; represents the joint angle error; Indicates the actual joint angular velocity; Represents the reference joint angular velocity, which is the reference output; represents the expected joint angular velocity; θ represents the actual joint angle, θ d represents the desired joint angle.

2. The method for standing with variable loads of a humanoid robot based on model reference adaptive control according to claim 1, characterized in that: The physical parameters of each connecting rod of the humanoid robot include: mass, length, center of mass position, and inertia tensor of each connecting rod of the humanoid robot.

Images