A hierarchical disturbance recovery control method for humanoid robots based on motion divergence

Abstract

The present invention discloses a hierarchical disturbance recovery control method for a humanoid robot based on motion divergence. The method detects the robot's center of mass position and velocity in real time through an IMU and status signals to estimate external force interference. After performing dead-zone filtering and memory decay processing on the estimated external force, the current motion divergence #imgabs0# is calculated based on a linear inverted pendulum model. The actual offset direction of the motion divergence #imgabs1# is corrected according to the estimated external force, and its offset relative to the zero-torque point #imgabs2# is calculated. The maximum safety threshold of the offset is calculated to classify the disturbance degree into three levels: mild, moderate, and severe. Corresponding recovery strategies are adopted for different levels. Finally, a real-time swing leg path is planned according to the corrected landing point for robot tracking control, thereby realizing adaptive disturbance recovery of the robot.

Description

Technical Field

[0001] The present invention relates to the field of robot control technology, and in particular to a hierarchical disturbance recovery control method for a humanoid robot based on motion divergence. Background Art

[0002] Bipedal robots face various external disturbances when walking in complex environments. Traditional methods struggle to quickly perceive and effectively respond to these disturbances. Existing methods based on zero-moment points or center-of-mass momentum points lack direct response characteristics and physical meaning to these disturbances, making them ineffective in guiding gait recovery. Summary of the Invention

[0003] In view of the problems existing in the prior art, the object of the present invention is to provide a hierarchical disturbance recovery control method for a humanoid robot based on motion divergence.

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

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

[0006] (1) Real-time detection of the current robot center of mass position and velocity through IMU and status signals to estimate the external force interference;

[0007] (2) Perform dead-zone filtering and memory decay processing on the estimated external force;

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

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

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

[0011] (6) According to the offset The size of the disturbance is divided into three levels: slight, moderate and severe;

[0012] (7) Adopt appropriate restoration strategies for different levels of disturbance: no adjustment for slight disturbance, and different compensation amounts for moderate and severe disturbances;

[0013] (8) According to the corrected landing point, the real-time swing leg path is planned for the robot to track and control, and the robot's adaptive disturbance recovery is achieved through the above-mentioned hierarchical control.

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

[0015] in represents the estimated external force, represents the time period of the external force estimation algorithm iteration, denote the attenuation and estimated gain respectively, represents the overall mass of the robot, represents the current centroid position, is the natural frequency of the linear inverted pendulum model, which is determined by the gravitational acceleration and fixed centroid height Calculated, is the current zero moment point position.

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

[0017] when When the state is considered as undisturbed, take ;

[0018] when When linear interpolation is used, take ;

[0019] when hour, .

[0020] The estimated force memory decay processing method in step (2) is as follows: after the estimated force is filtered, a large impact is detected. When it is detected that the current external force estimate exceeds the threshold, the external force memory value is updated to the current external force; otherwise, the memory value decays exponentially:

[0021] ;

[0022] in, Indicates the external force memory value, is the estimated value of the current external force, is the memory decay coefficient, satisfying 0< <1, is the shock detection threshold, take .

[0023] In step (3), an external force is added to the linear inverted pendulum model to make corrections and calculate the current motion divergence , recorded 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] in is the calculated landing foot compensation, which is calculated according to the corresponding disturbance response level. are the gain coefficients of step position compensation, and 0< .

[0040] In the compensation method, in addition to increasing the compensation gain, the secondary response also requires increasing the stride height in the current stride cycle planning:

[0041] ;

[0042] in is the expected stride height after compensation, is the gain coefficient of step height compensation, The default step height in the current plan.

[0043] The specific compensation method for the step position is to superimpose the divergence compensation on the expected landing position of the swing leg, specifically:

[0044] ;

[0045] in The target swing leg position is is the expected input swing leg position, compensation Generated by the snapshot mechanism, the compensation value at the current moment is retained before the leg swings to calculate the current landing position.

[0046] Planning uses the path interpolation method to generate the trajectory. The starting point is the swing leg position before the leg is lifted, and the end point is the expected landing point. The specific implementation is:

[0047] ;

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

[0049] Beneficial effects 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 meaning and response characteristics than the traditional ZMP / CMP method.

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

[0053] 3. Based on The three-level grading strategy of the offset takes differentiated compensation measures for different disturbance degrees, achieving a balance between precise control of disturbance recovery and energy efficiency.

[0054] 4. This method is applicable to bipedal robot walking control in various complex environments, and exhibits excellent stability and rapid recovery capabilities under sudden disturbances.

[0055] 5. Through the coordinated control of dynamic compensation of landing point and step height adjustment, the robot's walking robustness and environmental adaptability after disturbance are significantly improved. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0057] Figure 2 Execute a flowchart for a hierarchical recovery strategy;

[0058] Figure 3 This figure shows the rapid recovery trajectory of the robot's center of mass and motion divergence during a perturbed impact experiment. In this experiment, a transient impact in the X direction was applied at the 5th second. After the perturbation recovery algorithm was adjusted, the robot regained stability after several walking cycles.

[0059] Figure 4 This is a schematic diagram of the recovery trajectory of the base posture in the robot's disturbed impact experiment. The robot was impacted from the rear, resulting in a large change in the pitch angle, and the disturbance recovery algorithm achieved rapid posture recovery. DETAILED DESCRIPTION

[0060] The technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention; it is obvious that the described embodiments are only 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 ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0061] See also Figures 1 to 4 A hierarchical disturbance recovery control method for a humanoid robot based on motion divergence comprises the following steps:

[0062] 1. Initial state acquisition

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

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

[0065] Get the current foot position through the joint motor sensor and calculate the zero torque point (Unit: m), as the reference point of equilibrium state.

[0066] 2. External force estimation and filtering

[0067] Calculation of natural frequency based on linear inverted pendulum model ,

[0068] The recursive estimation algorithm is used to calculate the estimated value of the horizontal two-dimensional external force with a fixed iteration cycle.

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

[0070] ;

[0071] in represents the estimated external force, represents the time period of the external force estimation algorithm iteration, denote the attenuation and estimated gain respectively, represents the overall mass of the robot, represents the current centroid position, is the natural frequency of the linear inverted pendulum model, which is determined by the gravitational acceleration and fixed centroid height Calculated, is the current zero moment point position.

[0072] Filter the external force estimate and set the dead zone threshold , ,in is the friction coefficient of the ground,

[0073] when When the state is considered as undisturbed, take ;

[0074] when When linear interpolation is used, take ;

[0075] when hour, .

[0076] 3. Estimated force memory decay processing

[0077] Perform large impact detection on the filtered external force and set the impact threshold ;

[0078] When detected ‖> When the external force memory value is retained and updated ;

[0079] Otherwise it decays exponentially: , where the attenuation coefficient γ is set to 0.9 to retain the recent disturbance characteristics.

[0080] The specific expression is:

[0081] ;

[0082] in, Indicates the external force memory value, is the memory decay coefficient, satisfying 0 < < 1, is the shock detection threshold, take .

[0083] 4. Motion divergence correction and offset calculation

[0084] Calculation of corrected motion divergence based on linear inverted pendulum model :

[0085] ;

[0086] Calculate its offset relative to ZMP (Unit: m), reflecting the deviation between the divergence of the center of mass motion and the equilibrium state.

[0087] 5. Safety threshold calculation

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

[0089] ;

[0090] ;

[0091] The maximum stride distance in the forward and backward directions =0.3m, lateral , single-step time .

[0092] 6. Graded Disturbance Identification

[0093] Combining the offset threshold and the large impact detection results, a three-level judgment is performed:

[0094] Zero-level response (slightly disturbed): ;

[0095] Level 1 Response (Moderate Disturbance): ;

[0096] Level 2 response (severe disturbance): or .

[0097] 7. Foothold compensation

[0098] Calculate the foot point compensation amount according to the response level:

[0099] ;

[0100] Among them, =0.15 gain compensation foothold, take The gain is increased to compensate.

[0101] Target landing point:

[0102] ;

[0103] in For the original planned location, The snapshot mechanism is used to lock the current compensation value before the swing;

[0104] In the compensation method, in addition to increasing the compensation gain, the secondary response also requires increasing the stride height in the current stride cycle planning:

[0105] ;

[0106] in is the gain coefficient of step height compensation, is the default step height in the current plan. is the expected stride height after compensation, take , , after compensation .

[0107] 8. Leg swing path planning

[0108] Use quintic Bezier curve interpolation to generate the swing leg path:

[0109] ;

[0110] Interpolation generation of swing leg path using Bezier curve planning as an example

[0111] ;

[0112] In the formula is the starting position of the swing leg, the interpolation curve is From the time Smooth transition to , ensuring that the trajectory is continuously trackable.

[0113] in is the Bezier curve basis function, take the control point ,

[0114] ;

[0115] The position coordinates of the path points are calculated through inverse kinematics to obtain a series of joint sequences , the sequence is sent to each joint motor and driven to reach the position in a position-controlled manner to achieve continuous trajectory tracking.

[0116] The above description is merely a preferred embodiment of the present invention; however, the scope of protection of the present invention is not limited thereto. Any person skilled in the art who, within the technical scope disclosed by the present invention, makes equivalent substitutions or modifications based on the technical solutions and improved concepts of the present invention shall be covered by the scope of protection of the present invention.

Claims

1. A hierarchical disturbance recovery control method for a humanoid robot based on motion divergence, characterized by: The following steps are involved: (1) The current center of mass position and velocity of the robot are detected in real time through IMU and status signals to estimate the external force interference; (2) performing dead-zone filtering and memory decay processing on the estimated external force; (3) Calculate the current corrected motion divergence ξ based on the linear inverted pendulum model f ; (4) Calculate the current zero moment point p zmp and the corrected divergence ξ f The offset Δξ between f ; (5) Calculate the maximum safety threshold Δξ of the offset fmax ; (6) According to the offset Δξ f The size of the disturbance is divided into three levels: slight, moderate and severe; (7) Adopt appropriate restoration strategies for different levels of disturbance: no adjustment for slight disturbance, and different compensation amounts for moderate and severe disturbances; (8) According to the corrected landing point, the real-time swing leg path is planned for the robot to track and control, and the adaptive disturbance recovery of the robot is achieved through the above-mentioned hierarchical control; The estimated force filtering method in step (2) is as follows: different settings are made according to the estimated external force size to distinguish between large and small disturbances, and the dead zone threshold F is set. dead =0.5μmg, F min =1.0μmg, where μ is the friction coefficient of the ground, when When the state is considered as undisturbed, take when When linear interpolation is used, take when hour, The estimated force memory decay processing method in step (2) is as follows: after the estimated force is filtered, a large impact is detected. When it is detected that the current external force estimate exceeds a threshold, the external force memory value is updated to the current external force; otherwise, the memory value decays exponentially: in, Indicates the external force memory value, is the estimated value of the current external force, γ is the memory decay coefficient, satisfying 0<γ<1, F th is the shock detection threshold, take F th =2F min .

2. A hierarchical disturbance recovery control method for a humanoid robot based on motion divergence according to claim 1, characterized in that: In step (1), the external force interference estimation method is as follows: the external force estimation is limited to a two-dimensional vector in the horizontal direction, and the estimation method is: in represents the estimated external force, Δt represents the time period of the external force estimation algorithm iteration, K f ,K o denote the attenuation and estimated gain respectively, m denotes the overall mass of the robot, x c represents the current centroid position, is the natural frequency of the linear inverted pendulum model, which is calculated from the gravitational acceleration g and the fixed center of mass height h, p zmp is the current zero moment point position.

3. A hierarchical disturbance recovery control method for a humanoid robot based on motion divergence according to claim 1, characterized in that: In step (3), an external force is added to the linear inverted pendulum model for correction, and the current motion divergence ξ is calculated. f , recorded as: In the step (4), the motion divergence ξ f The offset is: Δξ f =ξ f -p zmp , where p zmp is the zero moment point position.

4. A hierarchical disturbance recovery control method for a humanoid robot based on motion divergence according to claim 3, characterized in that: 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: where Δp max is the maximum stride distance specified, p nominal is the currently planned landing position, T step The time of the current single step; 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: The x and y directions are defined in the robot base coordinate system. The positive x direction indicates the direction the robot is facing, the positive y direction is the vertical direction of the robot's right hand side, and Δp max,x >Δp max,y .

5. A hierarchical disturbance recovery control method for a humanoid robot based on motion divergence according to claim 4, characterized in that: In step (6), the classification of disturbances is based on thresholds and large impact detection: When Δξ f <0.15Δξ fmax When , it is in zero-order response; When 0.15Δξ fmax ≤Δξ f <0.3Δξ fmax When the emergency occurs, it is in the first level response; When Δξ f ≥0.3Δξ fmax or When the alarm is triggered, it is in the secondary response state.

6. A hierarchical disturbance recovery control method for a humanoid robot based on motion divergence according to claim 5, characterized in that: In step (7), corresponding restoration strategies are adopted for different disturbance levels: the implementation method is to perform graded compensation on the expected landing foot planning. The compensation method is divided into where Δp comp is the calculated landing foot compensation, which is calculated according to the corresponding disturbance response level. K1 and K2 are the gain coefficients of step position compensation, and 0 <K1<K2; In the compensation method, in addition to increasing the compensation gain, the secondary response also requires increasing the stride height in the current stride cycle planning: h des =K3h0; where h des is the expected stride height after compensation, K3 is the gain coefficient of stride height compensation, and h0 is the default stride height in the current plan.

7. A hierarchical disturbance recovery control method for a humanoid robot based on motion divergence according to claim 6, characterized in that: The specific compensation method for the step position is to superimpose the divergence compensation on the expected landing position of the swing leg, specifically: p des =p0+Δp comp ; where p des is the target swing leg position, p0 is the expected input swing leg position, and the compensation amount Δp comp Generated by the snapshot mechanism, the compensation value at the current moment is retained before the leg swings to calculate the current landing position.

8. A hierarchical disturbance recovery control method for a humanoid robot based on motion divergence according to claim 1, wherein the trajectory is generated using a path interpolation method, with the starting point being the swing leg position before the leg is lifted and the end point being the expected landing point. The specific implementation is as follows: p plan (T0+t)=f plan (p start ,p des ,h des ,T step ,t); where p plan represents the planned swing leg position at each moment, T0 is the swing start time, t is the current moment, and f plan is the interpolation function, including linear interpolation and Bezier curve interpolation, p start is the current starting position of the swing leg, p des is the landing position after compensation, h des is the expected swing height, T step is the current swing time.

Images