Hierarchical disturbed recovery control method for humanoid robot based on motion divergence

Abstract

The invention discloses a humanoid robot hierarchical disturbance recovery control method based on motion divergence, and the method comprises the steps: detecting the mass center position and speed of a robot in real time through an IMU (Inertial Measurement Unit) and a state signal to estimate external force interference, and carrying out the dead-zone filtering and memory attenuation processing of the estimated external force; the method comprises the steps of calculating a current motion divergence # imgabs0 # based on a linear inverted pendulum model, correcting the actual offset direction of the motion divergence # imgabs1 # according to an estimated external force, calculating the offset of the motion divergence # imgabs1 # relative to a zero moment point # imgabs2 #, dividing the disturbance degree into a slight level, a medium level and a serious level by calculating the maximum safety threshold of the offset, and adopting corresponding recovery strategies for different levels. And finally, according to the corrected landing point, planning a real-time swing leg path for the robot to track and control, thereby realizing adaptive disturbance recovery of the robot.

Description

Technical Field

[0001] The present invention relates to the technical field of robot control, and particularly relates to a hierarchical disturbed recovery control method for a humanoid robot based on the amount of motion divergence. Background Art

[0002] When a biped robot walks in a complex environment, it faces various external disturbances, and it is difficult for traditional methods to quickly sense and effectively respond to these disturbances. In the prior art, methods based on the zero moment point or the center of mass momentum point have insufficiently direct response characteristics and physical meanings to disturbances and cannot effectively guide gait recovery. Summary of the Invention

[0003] Aiming at the problems existing in the prior art, the purpose of the present invention is to provide a hierarchical disturbed recovery control method for a humanoid robot based on the amount of motion divergence.

[0004] To solve the above problems, the present invention adopts the following technical solutions.

[0005] A hierarchical disturbed recovery control method for a humanoid robot based on the amount of motion divergence, the method comprising the following steps:

[0006] (1) Real-time detecting the current position and velocity of the robot's center of mass through an IMU and state signals to estimate the external force disturbance received;

[0007] (2) Performing dead zone filtering and memory attenuation processing on the estimated external force;

[0008] (3) Calculating the current corrected motion divergence amount based on a linear inverted pendulum model ;

[0009] (4) Calculating the offset between the current zero moment point and the corrected divergence amount ; ;

[0010] (5) Calculating the maximum safety threshold of the offset ;

[0011] (6) Classifying the degree of disturbance into three levels according to the magnitude of the offset : slight, medium, and severe;

[0012] (7) Adopting corresponding recovery strategies for different disturbance levels: no adjustment for slight disturbance, and different compensation amounts for medium and severe disturbances respectively;

[0013] (8) Planning the real-time swing leg path according to the corrected landing point for the robot to perform tracking control, and realizing the adaptive disturbed recovery of the robot through the above hierarchical control.

[0014] The external force disturbance estimation method in step (1): The external force estimation is limited to a two-dimensional vector in the horizontal direction, and its estimation method is as follows:

[0015] where represents the estimated external force, represents the time period of the external force estimation algorithm iteration, represent the attenuation and estimation gains respectively, represents the overall mass of the robot, represents the current center of mass position, is the natural frequency of the linear inverted pendulum model, which is calculated from the gravitational acceleration and the fixed center of mass height ; is the position of the current zero moment point.

[0016] The estimated force filtering method in step (2): Different settings are made according to the magnitude of the estimated external force to distinguish between large and small disturbances, and a dead zone threshold , is set, where is the friction coefficient of the ground,

[0017] When , it is regarded as a non-disturbed state, and is taken;

[0018] When , linear interpolation is used, and is taken;

[0019] When , .

[0020] The estimated force memory decay processing method in step (2): After the estimated force is filtered, detection of large impacts is performed. When it is detected that the current external force estimation value exceeds the threshold, the external force memory value is updated to the current external force; otherwise, the memory value decays exponentially:

[0021] ;

[0022] where represents the external force memory value, is the current external force estimation value, is the memory decay coefficient, satisfying 0 < < 1, is the impact detection threshold, and is taken.

[0023] In step (3), on the basis of the linear inverted pendulum model, an external force is added for correction, and the current motion divergence is calculated and denoted as:

[0024] ;

[0025] In step (4), the motion divergence The offset is: ,in is the position of the zero moment point;

[0026] Calculation of the safety threshold with the maximum offset in step (5): This threshold is calculated by combining the divergence characteristics of the linear inverted pendulum with the expected maximum footstep offset to obtain a two-dimensional threshold based on the robot base coordinate system:

[0027] ;

[0028] in is the maximum stride distance specified, is the currently planned landing location, The time for the current single step.

[0029] This threshold shows different margins in the front-to-back and side-to-side directions in the robot base coordinates. The maximum step distance needs to be set separately for the front-to-back and side-to-side dimensions:

[0030] ;

[0031] ;

[0032] in The direction is defined in the robot base coordinate system. Positive means the robot is facing the direction, The positive direction is the vertical direction of the robot's right hand side, and .

[0033] In step (6), the classification of disturbances is based on thresholds and large impact detection:

[0034] when When , it is in zero-order response;

[0035] when When the emergency occurs, it is in the first level response;

[0036] when or When the alarm is triggered, it is in the secondary response state.

[0037] In step (7), corresponding restoration strategies are adopted for different levels of disturbance: the implementation method is to perform graded compensation on the expected landing foot planning. The compensation method is divided into

[0038] ;

[0039] wherein is the calculated landing foot compensation amount, which is calculated according to the corresponding disturbance response levels respectively, are the gain coefficients for the step position compensation respectively, and 0 < .

[0040] In the compensation method, in addition to increasing the compensation gain, the second-level response needs to additionally increase the step height in the current step cycle plan:

[0041] ;

[0042] wherein is the expected step height after compensation, is the gain coefficient for the step height compensation, is the default step height in the current plan.

[0043] The specific compensation method for its step position is realized by superimposing the divergence amount compensation on the expected planned landing position of the swing leg, specifically:

[0044] ;

[0045] wherein is the position of the target swing leg, is the expected input position of the swing leg, and the compensation amount is generated by the snapshot mechanism, and the compensation value at the current moment is retained before the leg swings to calculate the current landing position.

[0046] The path interpolation method is used for trajectory generation in the plan, with the starting point being the position of the swing leg before lifting the leg and the ending point being the expected landing point position, and its specific implementation is:

[0047] ;

[0048] wherein represents the planned swing leg position at each moment, is the starting time of the swing, is the current moment, is the interpolation function, including linear interpolation, Bezier curve interpolation, is the starting position of the current swing leg, is the landing position after compensation, is the expected swing height, is the current swing time.

[0049] Advantages of the present invention

[0050] Compared with the prior art, the advantages of the present invention are:

[0051] The present invention estimates the external force in real time and The modified method can accurately sense external disturbances and quantify their impacts, and has more direct physical meanings and response characteristics compared with traditional ZMP / CMP methods.

[0052] 2. The innovative dead-zone filtering and memory decay processing mechanism can effectively distinguish between large and small disturbances and retain the impact memory, improving the system's adaptability to dynamic environments.

[0053] 3. Based on the three-level classification strategy of the offset, different differential compensation measures are taken for different disturbance degrees, achieving the balance between the precise control of disturbance recovery and energy efficiency.

[0054] 4. This method is applicable to the walking control of biped robots in various complex environments, and shows excellent stability and fast recovery ability in case of sudden disturbances.

[0055] 5. Through the collaborative control of dynamic compensation at the landing point and step height adjustment, the walking robustness and environmental adaptability of the robot after being disturbed are significantly improved. Description of the Drawings

[0057] Figure 1 is the overall control flow chart of the method of the present invention;

[0058] Figure 2 is the execution flow chart of the hierarchical recovery strategy;

[0059] Figure 3 is the schematic diagram of the rapid recovery trajectory of the center of mass and the motion divergence amount of the robot in the disturbed impact experiment. An instantaneous impact in the X direction is applied at the 5th second in this experiment. After being adjusted by the disturbed recovery algorithm, the robot recovers stability after several walking cycles;

[0060] Figure 4 is the schematic diagram of the recovery trajectory of the base attitude of the robot in the disturbed impact experiment. The robot is impacted from the rear, resulting in a large change in the pitch angle, and the attitude is quickly recovered through the disturbed recovery algorithm. Detailed Embodiments

[0062] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the drawings in the embodiments of the present invention; obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0063] Please refer to Figures 1 to 4, A hierarchical perturbed recovery control method for humanoid robots based on the amount of motion divergence, comprising the following steps:

[0064] 1. Initial state acquisition

[0065] The microprocessor reads model parameters such as the mass m, the height of the center of mass h, and the ground friction coefficient μ of the robot from the memory, as well as the attenuation gain , the estimation gain and other control parameters to complete the system initialization;

[0066] Through the IMU sensor and the robot model, the current position of the center of mass of the robot is calculated in real time in the processor (two-dimensional vector, unit: m) and speed (unit: m / s);

[0067] The current foot end position is obtained through the joint motor sensor, and the zero moment point (unit: m) is calculated as the balance state reference point.

[0068] 2. External force estimation and filtering processing

[0069] Calculate the natural frequency based on the linear inverted pendulum model ,

[0070] Use the recursive estimation algorithm to calculate the two-dimensional external force estimation value in the horizontal direction, calculated with a fixed iteration period,

[0071] The external force estimation is limited to a two-dimensional vector in the horizontal direction, and its estimation method is:

[0072] ;

[0073] Where represents the estimated external force, represents the time period of the external force estimation algorithm iteration, respectively represent the attenuation and estimation gains, represents the overall mass of the robot, represents the current position of the center of mass, is the natural frequency of the linear inverted pendulum model, calculated from the gravitational acceleration and the fixed center of mass height is calculated, is the current zero moment point position.

[0074] Filter the external force estimation value, set the dead zone threshold , , where is the ground friction coefficient,

[0075] When It is regarded as the non-disturbance state at this time, and take ;

[0076] When linear interpolation is adopted, and take ;

[0077] When at this time, .

[0078] 3. Estimation of force memory decay processing

[0079] Perform a large impact detection on the filtered external force, and set the impact threshold ;

[0080] When it is detected that ‖ ‖ > at this time, retain and update the external force memory value ;

[0081] Otherwise, decay in an exponential form: , where the decay coefficient γ is taken as 0.9 to retain the characteristics of recent disturbances.

[0082] Specifically expressed as:

[0083] ;

[0084] Among them, [[ID=ID=49]] represents the external force memory value, is the memory decay coefficient, satisfying 0 < < 1, is the impact detection threshold, and take .

[0085] 4. Correction of motion divergence amount and calculation of offset amount

[0086] Calculate the corrected motion divergence amount based on the linear inverted pendulum model :

[0087] ;

[0088] Calculate its offset amount (unit: m) relative to the ZMP, which reflects the deviation between the degree of motion divergence of the center of mass and the equilibrium state.

[0089] 5. Safety threshold calculation

[0090] According to the divergence characteristics of the linear inverted pendulum and the maximum step distance, calculate the two-dimensional safety threshold in the base coordinate system:

[0091] ;

[0092] ;

[0093] The maximum step distance in the front-back direction = 0.3 m, and the lateral direction , and the single-step time .

[0094] 6. Hierarchical disturbance discrimination

[0095] Perform three-level discrimination by combining the offset threshold and the large impact detection result:

[0096] Level 0 response (slightly disturbed): ;

[0097] Level 1 response (moderately disturbed): ;

[0098] Level 2 response (severely disturbed): or .

[0099] 7. Landing point compensation

[0100] Calculate the landing point compensation amount in segments according to the response level:

[0101] ;

[0102] Among them, the gain compensation landing point can take = 0.15, and take to increase the compensation gain.

[0103] Target landing point:

[0104] ;

[0105] Among them is the original planned position, Lock the current compensation value before swinging through the snapshot mechanism;

[0106] In the described compensation method, in addition to increasing the compensation gain for the level 2 response, it is necessary to additionally increase the step height in the current step cycle plan:

[0107] ;

[0108] Among them is the gain coefficient for step height compensation, is the default step height in the current plan, is the expected step height after compensation, take , , after compensation .

[0109] 8. Swing leg path planning

[0110] Generate the swing leg path using quintic Bezier curve interpolation:

[0111] ;

[0112] Taking the Bezier curve planning as an example, interpolate to generate the swing leg path

[0113] ;

[0114] In the formula is the starting position of the swing leg, and the interpolation curve smoothly transitions from within the time of to , ensuring that the trajectory is continuously trackable.

[0115] Among them is the Bezier curve basis function, take the control points ,

[0116] ;

[0117] After calculating the position coordinates of the path points through inverse kinematics, a series of joint sequences are obtained . Send this sequence to each joint motor and drive it to reach this position in the way of position control to achieve continuous trajectory tracking.

[0118] As described above, it is only the preferred specific implementation manner of the present invention; however, the protection scope of the present invention is not limited thereto. Any person skilled in the art within the technical scope disclosed by the present invention, according to the technical solution of the present invention and its improved concept, makes equivalent substitutions or changes, and should be covered by the protection scope of the present invention.

Claims

1. A hierarchical perturbed recovery control method for humanoid robots based on the amount of motion divergence, characterized in that: It includes the following steps: (1) Detect the current centroid position and velocity of the robot in real time through the IMU and status signals to estimate the external force disturbance received; (2) Perform dead zone filtering and memory attenuation processing on the estimated external force; (3) Calculate the current modified motion divergence based on the linear inverted pendulum model ; (4) Calculate the current zero moment point and the correction divergence between the offsets ; (5) Calculate the maximum safety threshold of the offset ; (6) Classify the degree of disturbance into three levels according to the magnitude of the offset : slight, medium, and severe; (7) Adopt corresponding recovery strategies for different levels of disturbance: no adjustment when slightly disturbed, and different compensation amounts are given when moderately disturbed and severely disturbed respectively; (8) Plan the real-time swing leg path according to the corrected landing point for the robot to perform tracking control, and realize the adaptive disturbance recovery of the robot through the above hierarchical control.

2. A hierarchical disturbance recovery control method for a humanoid robot based on the amount of motion divergence according to claim 1, characterized in that: The external force disturbance estimation method in the step (1): The external force estimation is limited to a two-dimensional vector in the horizontal direction, and its estimation method is: ； wherein represents the estimated external force, represents the time period of the external force estimation algorithm iteration, represent attenuation and the estimated gain respectively, represents the overall mass of the robot, represents the current center of mass position, is the natural frequency of the linear inverted pendulum model, which is calculated from the gravitational acceleration and the fixed center of mass height and is obtained by calculation, is the current zero moment point position.

3. A hierarchical disturbance recovery control method for a humanoid robot based on the amount of motion divergence according to claim 2, characterized in that: The estimated force filtering method in step (2) is set differently according to the estimated magnitude of the external force to distinguish between large and small disturbances, and a dead zone threshold is set. , , where is the friction coefficient of the ground. When it is regarded as the non-disturbance state, take ; When linear interpolation is used, take ; When then .

4. A hierarchical disturbance recovery control method for a humanoid robot based on the amount of motion divergence according to claim 3, characterized in that: The estimation force memory attenuation processing method in the step (2): After the estimated force is filtered, detect large impacts. When it is detected that the current external force estimation value exceeds the threshold, the external force memory value is updated to the current external force; otherwise, the memory value decays exponentially: ； Among them, represents the external force memory value, is the current external force estimated value, is the memory decay coefficient, satisfying 0 < < 1, is the impact detection threshold, taking .

5. A hierarchical disturbance recovery control method for a humanoid robot based on the amount of motion divergence according to claim 4, characterized in that: In step (3), an external force is added to correct the linear inverted pendulum model, and the current motion divergence is calculated , denoted as: ； In the said step (4), the movement divergence amount has an offset of: , where is the position of the zero moment point.

6. A hierarchical disturbance recovery control method for a humanoid robot based on the amount of motion divergence according to claim 5, characterized in that: The calculation of the maximum safety threshold of the offset in the step (5): This threshold is calculated from the divergence characteristics of the linear inverted pendulum combined with the expected maximum footstep offset to obtain a two-dimensional threshold based on the robot's base coordinate system: ； wherein is the specified maximum step distance, is the position of the landing foot in the current plan, is the time for a single step in the current plan; This threshold shows different margins in the front-back direction and the lateral direction in the robot's base coordinate system, and the set maximum step distance needs to be set for the front-back and lateral sizes respectively: ； ； Among them The direction is defined in the robot base coordinate system, The positive direction represents the direction the robot is facing, The positive direction is the vertical direction on the right side of the robot, and .

7. A hierarchical disturbance recovery control method for a humanoid robot based on the amount of motion divergence according to claim 6, characterized in that: In the step (6), the classification of the disturbance is discriminated based on the threshold and large impact detection: When it is at level-zero response; When it is at the first-level response; When or it is in the second-level response.

8. A hierarchical disturbance recovery control method for a humanoid robot based on the amount of motion divergence according to claim 7, characterized in that: The corresponding recovery strategies are adopted for different levels of disturbance in the step (7): Its implementation method is to perform hierarchical compensation on the expected landing foot plan. This compensation method is divided into ； wherein is the calculated landing foot compensation amount, which is calculated respectively according to the corresponding disturbance response levels are respectively the gain coefficients for the stepping position compensation, and 0 < ; In the compensation method, in addition to increasing the compensation gain for the secondary response, it is necessary to additionally increase the step height in the current step cycle plan: ； wherein is the expected step height after compensation, is the gain coefficient for step height compensation, is the default step height in the current plan.

9. A hierarchical disturbance recovery control method for a humanoid robot based on the amount of motion divergence according to claim 8, characterized in that: The specific compensation method of its step position is realized by superimposing the divergence amount compensation on the expected planned landing position of the swing leg. Specifically: ； wherein is the position of the swinging leg as the target, is the position of the swinging leg of the expected input, and the compensation amount is generated by the snapshot mechanism, and the compensation value at the current moment is retained before the leg swings to calculate the current landing position.

10. A hierarchical disturbance recovery control method for a humanoid robot based on the amount of motion divergence according to claim 1, planning to use the path interpolation method to generate the trajectory, with the starting point being the position of the swing leg before lifting the leg and the ending point being the expected landing point position. Its specific implementation is: ； wherein represents the planned swing leg position at each moment, is the swing start time, is the current moment, is the interpolation function, including linear interpolation and Bezier curve interpolation, is the starting position of the current swing leg, is the landing position after compensation, is the expected swing height, is the current swing time.

Images