A stability margin awareness-based anti-interference full-body control method and system for a quadruped robot

Abstract

The application belongs to the field of motion control technology of legged robots, and particularly relates to a four-legged robot anti-interference whole-body control method based on stability margin perception and a system thereof. Desired joint positions, velocities and foot end contact forces are obtained through nonlinear model predictive control (NMPC); a whole-body controller (WBC) based on hierarchical quadratic programming calculates joint driving torques according to task priorities; foot end contact forces are estimated according to joint positions, velocities, torques fed back by the driver and a robot dynamics model; a resultant force leading to instability of the robot is calculated relative to each support polygon edge line of the robot; the resultant force is projected to calculate an included angle between the projected force and the vertical direction; a compensation acceleration is calculated through a PI controller; and the compensation acceleration is taken as a part of the body line acceleration task in the WBC to improve the anti-interference capability of the robot. The application is used to solve the problem that the prior art cannot effectively perceive the stable state of the four-legged robot and generate an anti-interference control strategy quickly accordingly.

Description

Technical Field

[0001] This invention belongs to the field of legged robot motion control technology, specifically relating to a method and system for anti-interference whole-body control of a quadruped robot based on stability margin perception. Background Technology

[0002] Quadruped robots, due to their structure resembling that of quadruped animals, possess excellent environmental adaptability and mobility, making them promising for applications in complex terrain exploration, disaster relief, and industrial inspection. However, in real-world environments, quadruped robots often face external disturbances such as sudden collisions, which can cause them to deviate from their intended trajectory or even lose balance and fall. Currently, model-based quadruped robot control methods perform well under normal operating conditions, but their anti-interference capabilities are limited when faced with unknown external disturbances due to a lack of disturbance perception and rapid response mechanisms.

[0003] Currently, control methods for quadruped robots mainly include model-based control methods and data-based control methods. Model-based control methods establish a dynamic model of the robot, plan its motion trajectory, and calculate joint torques. These methods perform well under normal operating conditions, but their anti-interference capability is limited when faced with unknown external disturbances due to the lack of disturbance perception and rapid response mechanisms. Data-based control methods, such as reinforcement learning and neural networks, learn control strategies through a large amount of training data. While they possess anti-interference capabilities to some extent, their generalization performance is limited, and it is difficult to guarantee real-time performance and safety.

[0004] In recent years, researchers have proposed several control methods based on stability metrics, such as Zero Moment Point (ZMP) and Supported Polygon (SP). These methods monitor the robot's stable state and adjust the control strategy to maintain balance. However, these methods are mostly used for bipedal robots or static stable states, and research on the stable control of quadruped robots under dynamic motion and complex disturbances is still insufficient. Currently, there is a lack of a method that can effectively sense the stable state of a quadruped robot and quickly generate an anti-disturbance control strategy based on it. In particular, there is a significant technological gap in combining advanced whole-body control frameworks to perceive and compensate for unknown external disturbances. Summary of the Invention

[0005] This invention provides a method and system for anti-interference whole-body control of quadruped robots based on stability margin perception, which solves the problem in the prior art that it is not possible to effectively perceive the stable state of quadruped robots and generate anti-interference control strategies accordingly.

[0006] This invention is achieved through the following technical solution: A method for interference-resistant whole-body control of a quadruped robot based on stability margin perception, the method comprising the following steps: Step 1: Use a nonlinear model to predict and control the NMPC to plan the whole-body motion trajectory of the quadruped robot, and obtain the desired joint positions, velocities and foot contact forces; Step 2: The whole-body controller WBC based on hierarchical quadratic programming calculates the joint driving torque according to the task priority; Step 3: Estimate the foot contact force based on the joint position, velocity, torque, and robot dynamics model fed back by the actuator; Step 4: Based on the foot position and the foot contact force in Step 3, calculate the resultant force relative to the edges of each supporting polygon of the robot that causes instability in the robot; Step 5: Project the resultant force from Step 4 and calculate the angle between the projected force and the vertical direction; Step 6: Based on the included angle from Step 5, calculate the compensation acceleration using a PI controller; Step 7: Based on the compensation acceleration in Step 6, use it as part of the bodyline acceleration task in WBC to improve the robot's anti-interference capability.

[0007] Furthermore, in step 1, the NMPC planning includes establishing an optimal control problem with the center of mass momentum and generalized position as state variables and the foot contact force and joint velocity as input variables, and solving the problem to obtain the optimal state and input trajectory.

[0008] Furthermore, step 3 specifically involves calculating the foot contact force based on the lack of motion constraints at the foot end and the robot's dynamic equations. The specific calculation formula is as follows:

[0009] in, To access the Jacobian matrix, For the quality matrix, To select a matrix, For joint torque, It is a nonlinear term. For generalized speed.

[0010] Furthermore, step 4 specifically involves calculating each side of the robot-supported polygon to obtain the tipping axis. ; Calculate the perpendicular vector from the centroid to the overturning axis. ; Calculate the resultant force acting on the tipping axis that makes the robot unstable. and torque ; Convert torque into equivalent couple ; Calculating the net force that causes instability in the robot and its mean ; in, The unit flip axis vector, and These are the estimated resultant force and resultant torque of the foot contact force relative to the robot's center of gravity, respectively. It is a unit vertical vector. It is a cross product matrix.

[0011] Furthermore, step 5, which projects the resultant force from step 4, specifically involves... Projected onto the x and y directions respectively: and

[0012] Step 5, calculating the angle between the projected force and the vertical direction, specifically involves calculating the angle between the projected force and the vertical vector. The included angle is:

[0013] in, Indicates by The symbol is determined by the first element.

[0014] Furthermore, step 6 specifically involves using a PI controller to calculate the compensation acceleration based on the included angle:

[0015] in, and These are the proportional gain matrix and the integral gain matrix, respectively.

[0016] Furthermore, step 7 specifically involves the following formula for the base line acceleration task:

[0017] in, Optimize variables for WBC.

[0018] A whole-body anti-interference control system for a quadruped robot based on stability margin perception, the system employing the aforementioned whole-body anti-interference control method for a quadruped robot based on stability margin perception, the system comprising: The planning module is used to predict and control the NMPC to plan the whole-body motion trajectory of the quadruped robot using a nonlinear model; The control module is used to calculate joint driving torques for the whole-body controller (WBC) based on hierarchical quadratic programming. The estimation module is used to estimate the foot contact force based on the joint position, velocity, torque, and robot dynamics model fed back by the actuator; The compensation module projects the resultant force and calculates the angle with the vertical direction. Based on the angle, it calculates the compensation acceleration through a PI controller and incorporates the compensation acceleration as part of the body-line acceleration task in WBC, thereby improving the robot's anti-interference capability.

[0019] A computer device includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor, when executing the computer program, implements the method described above.

[0020] A computer-readable storage medium storing a computer program that, when executed by a processor, implements the method described above.

[0021] The beneficial effects of this invention are: This invention significantly improves the robot's ability to resist unknown external disturbances by calculating the stability margin, sensing the impact of external disturbances on the robot's stability, and generating corresponding compensation accelerations.

[0022] This invention is based on robot dynamics models and physical principles, requires no complex interference observers, has a low computational burden, and meets the requirements of real-time control.

[0023] This invention can effectively cope with various types of external interference, such as unidirectional linear interference, synthetic interference, and impulse interference, and has strong adaptability.

[0024] This invention is applicable not only to standing states but also to dynamic motion states such as trot gait. By innovatively utilizing virtual support points to form support polygons, it solves the problem of stability margin calculation in two-leg support states.

[0025] This invention can significantly improve the resistance of quadruped robots to unknown external disturbances, and is applicable to various types of disturbances and motion states, and has good application value.

[0026] This invention can sense the stable state of a quadruped robot in real time when it is subjected to unknown external disturbances, and generate compensating acceleration, thereby improving the robot's anti-interference ability and motion stability.

[0027] This invention does not require identifying external forces; it only needs to calculate the stable value through state variables and then map it to the desired acceleration compensation value of the fuselage to improve anti-interference capability. Attached Figure Description

[0028] Figure 1 This is a schematic diagram of the quadruped robot and its coordinate system according to the present invention.

[0029] Figure 2 This is a schematic diagram of the control architecture of the present invention. Detailed Implementation

[0030] In the following description, specific details such as particular system architectures and techniques are set forth for illustrative purposes and not for limitation, in order to provide a thorough understanding of the embodiments of this application. However, those skilled in the art will understand that this application may also be implemented in other embodiments without these specific details. In other instances, detailed descriptions of well-known systems, apparatuses, circuits, and methods are omitted so as not to obscure the description of this application with unnecessary detail.

[0031] It should be understood that, when used in this specification and the appended claims, the term "comprising" indicates the presence of the described features, integrals, steps, operations, elements and / or components, but does not exclude the presence or addition of one or more other features, integrals, steps, operations, elements, components and / or collections thereof.

[0032] It should also be understood that the terminology used in this application specification is for the purpose of describing particular embodiments only and is not intended to limit the application. As used in this application specification and the appended claims, the singular forms “a,” “an,” and “the” are intended to include the plural forms unless the context clearly indicates otherwise.

[0033] The following is in conjunction with the appendix to this application specification. Figure 1-2 The technical solutions in the embodiments of this application are clearly and completely described. Obviously, the described embodiments are only a part of the embodiments of this application, and not all of the embodiments. Based on the embodiments in this application, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of this application.

[0034] Many specific details are set forth in the following description in order to provide a full understanding of this application. However, this application may also be implemented in other ways different from those described herein. Those skilled in the art can make similar extensions without departing from the spirit of this application. Therefore, this application is not limited to the specific embodiments disclosed below.

[0035] Implementation Method 1 This embodiment provides a method for anti-interference whole-body control of a quadruped robot based on stability margin perception, the method comprising the following steps: Step 1: Use a nonlinear model to predict and control the NMPC to plan the whole-body motion trajectory of the quadruped robot, and obtain the desired joint positions, velocities and foot contact forces; Step 2: The whole-body controller WBC based on hierarchical quadratic programming calculates the joint driving torque according to the task priority; Step 3: Estimate the foot contact force based on the joint position, velocity, torque, and robot dynamics model fed back by the actuator; Step 4: Based on the foot position and the foot contact force in Step 3, calculate the resultant force relative to the edges of each supporting polygon of the robot that causes instability in the robot; Step 5: Project the resultant force from Step 4 and calculate the angle between the projected force and the vertical direction; Step 6: Based on the included angle from Step 5, calculate the compensation acceleration using a PI controller; Step 7: Based on the compensation acceleration in Step 6, use it as part of the bodyline acceleration task in WBC to improve the robot's anti-interference capability.

[0036] Furthermore, in step 1, the NMPC planning includes establishing an optimal control problem with the center of mass momentum and generalized position as state variables and the foot contact force and joint velocity as input variables, and solving the problem to obtain the optimal state and input trajectory.

[0037] Furthermore, step 3 specifically involves calculating the foot contact force based on the lack of motion constraints at the foot end and the robot's dynamic equations. The specific calculation formula is as follows:

[0038] in, To access the Jacobian matrix, For the quality matrix, To select a matrix, For joint torque, It is a nonlinear term. The calculated foot contact force is generalized velocity; a linear Kalman filter (LKF) is used to smooth the calculated force and reduce noise.

[0039] Furthermore, step 4 specifically involves calculating each side of the robot-supported polygon to obtain the tipping axis. ; Calculate the perpendicular vector from the centroid to the overturning axis. ; Calculate the resultant force acting on the tipping axis that causes instability in the robot. and torque ; Convert torque into equivalent couple ; Calculating the net force that causes instability in the robot and its mean ; in, The foot position, The location of the center of mass. It is a unit vector. The unit flip axis vector, and These are the estimated resultant force and resultant torque of the foot contact force relative to the robot's center of gravity, respectively. It is a unit perpendicular vector. It is a cross product matrix.

[0040] Furthermore, step 5, which projects the resultant force from step 4, specifically involves... Projected onto the x and y directions respectively: and

[0041] Step 5, calculating the angle between the projected force and the vertical direction, specifically involves calculating the angle between the projected force and the vertical vector. The included angle is:

[0042] in, Indicates by The symbol is determined by the first element.

[0043] Furthermore, step 6 specifically involves using a PI controller to calculate the compensation acceleration based on the included angle:

[0044] in, and These are the proportional gain matrix and the integral gain matrix, respectively.

[0045] Furthermore, step 7 specifically involves the following formula for the base line acceleration task:

[0046] in, Optimize variables for WBC.

[0047] like Figure 1 As shown, the quadruped robot of this invention mainly comprises a base (body) and four legs, each leg having three degrees of freedom, for a total of 12 joint degrees of freedom. The figure illustrates the main coordinate system: inertial coordinate system. fuselage coordinate system Centroid coordinate system and foot coordinate system The generalized position and generalized velocity of the robot are defined as follows: [The text then lists the main forces and vectors used in the calculation of the force angle stability margin (FASM).] (1) in, It refers to the position of the fuselage in the inertial coordinate system. It is the ZYX Euler angle representation of the fuselage attitude. It's the joint position. and These are the linear velocity and angular velocity of the fuselage in the inertial coordinate system, respectively. It refers to the number of joints. For the quadruped robot of this invention, .

[0048] The rigid body dynamics equations of the robot are: (2) in, It is a generalized mass matrix. Includes Coriolis force, centrifugal force, and gravity terms. It is a selection matrix. It is joint torque. It is a stacked supporting Jacobian matrix. It is the constraint force of stacking.

[0049] The dynamic equations are divided into a driven part and an underactuated part. The underactuated part can be converted into the center-of-mass dynamics: (3) The relationship between the center of mass momentum and the generalized velocity is: (4) Aircraft speed can be calculated using the following formula: (5) like Figure 2 As shown, the control architecture of this invention mainly includes four parts: a trajectory generator, a whole-body planner based on NMPC, a whole-body controller based on WBC, and a disturbance compensation module based on stability margin.

[0050] The NMPC planner plans the robot's full-body motion trajectory by solving the optimal control problem. The optimal control problem can be expressed as: (6) Among them, state variables Control input The constraints include system dynamics constraints, foot kinematics constraints, joint moment limits, and ground friction constraints. The cost function is: .

[0051] WBC calculates joint moments that satisfy multiple task priorities by solving a hierarchical quadratic programming problem: (7) in, The optimization variables are ordered by priority from highest to lowest as follows: floating base dynamic equations; torque constraints, friction cone constraints, and non-contact motion constraints; base linear acceleration, base angular acceleration, and swing leg motion tracking; and contact force tracking.

[0052] The formula for the base line acceleration task is: (8) in, It is a compensation acceleration based on stability margin, which improves the robot's anti-interference ability by sensing and compensating for external disturbances.

[0053] Based on the fact that there is no movement constraint at the foot of the supporting leg. Based on the robot's dynamic equations, calculate the contact force at the foot: (9) Because the joint torque estimated by the motor driver is noisy, a linear Kalman filter (LKF) is used to smooth the calculated foot contact force.

[0054] Based on the force angle stability margin theory, the resultant force and resultant torque acting on the center of mass that cause instability in the robot are calculated: (10) For each edge of the supporting polygon (flip axis) and the corresponding perpendicular vector from the centroid to the overturning axis Calculate the resultant force that makes the robot unstable and its mean: (11) The unit vector is defined as: , .

[0055] Will Projected onto the x and y directions respectively: (12) Calculate the projected force and vertical vector The included angle (13) in Define the symbol for the included angle.

[0056] Using a PI controller, calculate the compensation acceleration based on the included angle: (14) in, and These are the proportional gain matrix and the integral gain matrix, respectively. By adjusting these parameters, the intensity of the compensation and the response speed can be controlled.

[0057] In situations like the trot gait where only two legs provide support, the traditional support polygon degenerates into a single line, insufficient for calculating stability margin. To address this issue, this invention innovatively proposes mirroring the foot position of the supporting leg along the xz plane, using it as a virtual support point for the swing leg, thus forming a new support polygon for calculating stability margin.

[0058] The result calculated by equation (14) is integrated into equation (8), and the compensation acceleration is calculated based on the change in robot stability, thereby improving the robot's anti-interference ability.

[0059] The method for calculating stability-related parameters in this invention is to improve the robot's anti-interference capability by improving the robot's stability.

[0060] This invention can also solve the problems of how to combine the currently used methods for evaluating robot stability with the existing framework, how to use the obtained values ​​describing robot stability as the control variables of the robot, and how to improve the robot's ability to resist external interference by improving the robot's stability.

[0061] Implementation Method 2 This embodiment provides a quadruped robot anti-interference whole-body control system based on stability margin perception. The system uses a quadruped robot anti-interference whole-body control method based on stability margin perception as described in Embodiment 1. The system includes: The planning module is used to predict and control the NMPC to plan the whole-body motion trajectory of the quadruped robot using a nonlinear model; The control module is used to calculate joint driving torques for the whole-body controller (WBC) based on hierarchical quadratic programming. The estimation module is used to estimate the foot contact force based on the joint position, velocity, torque, and robot dynamics model fed back by the actuator; The compensation module projects the resultant force and calculates the angle with the vertical direction. Based on the angle, it calculates the compensation acceleration through a PI controller and incorporates the compensation acceleration as part of the body-line acceleration task in WBC, thereby improving the robot's anti-interference capability.

[0062] Implementation Method 3 This invention provides an electronic device including a memory, a processor, and a computer program stored in the memory and executable on the processor. The memory stores software programs and modules, and the processor executes various functional applications and data processing by running the software programs and modules stored in the memory. The memory and processor are connected via a bus. Specifically, the processor implements any step in Embodiment 1 by running the computer program stored in the memory.

[0063] It should be understood that, in the embodiments of the present invention, the processor may be a Central Processing Unit (CPU), but it may also be other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or any conventional processor.

[0064] Memory may include read-only memory, flash memory, and random access memory, and provides instructions and data to the processor. Some or all of the memory may also include non-volatile random access memory.

[0065] It should be understood that if the integrated modules / units described above are implemented as software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, all or part of the processes in the methods described above can also be implemented by a computer program instructing related hardware. This computer program can be stored in a computer-readable storage medium, and when executed by a processor, it can implement the steps of the various method embodiments described above. The computer program includes computer program code, which can be in the form of source code, object code, executable files, or certain intermediate forms. The computer-readable medium can include: any entity or device capable of carrying the computer program code, recording media, USB flash drives, portable hard drives, magnetic disks, optical disks, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signals, telecommunication signals, and software distribution media, etc. It should be noted that the content contained in the computer-readable storage medium can be appropriately increased or decreased according to the requirements of legislation and patent practice in the jurisdiction.

[0066] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

[0067] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the above-described division of functional units and modules is merely an example. In practical applications, the above functions can be assigned to different functional units and modules as needed, that is, the internal structure of the above device can be divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the embodiments can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit. Furthermore, the specific names of the functional units and modules are only for easy differentiation and are not intended to limit the scope of protection of this invention. The specific working process of the units and modules in the above system can be referred to the corresponding process in the foregoing method embodiments, and will not be repeated here.

[0068] It should be noted that the methods and detailed examples provided in the above embodiments can be incorporated into the apparatus and devices provided in the embodiments for mutual reference, and will not be repeated here.

[0069] Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementations should not be considered beyond the scope of this invention.

[0070] In the embodiments provided by this invention, it should be understood that the disclosed apparatus / terminal devices and methods can be implemented in other ways. For example, the apparatus / device embodiments described above are merely illustrative. For instance, the division of modules or units described above is merely a logical functional division, and in actual implementation, it can be divided in other ways. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed.

[0071] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit them. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention, and should all be included within the protection scope of the present invention.

Claims

1. A method for anti-interference whole-body control of a quadruped robot based on stability margin perception, characterized in that, The method includes the following steps: Step 1: Use a nonlinear model to predict and control the NMPC to plan the whole-body motion trajectory of the quadruped robot, and obtain the desired joint positions, velocities and foot contact forces; Step 2: The whole-body controller WBC based on hierarchical quadratic programming calculates the joint driving torque according to the task priority; Step 3: Estimate the foot contact force based on the joint position, velocity, torque, and robot dynamics model fed back by the actuator; Step 4: Based on the foot position and the foot contact force in Step 3, calculate the resultant force relative to the edges of each supporting polygon of the robot that causes instability in the robot; Step 5: Project the resultant force from Step 4 and calculate the angle between the projected force and the vertical direction; Step 6: Based on the included angle from Step 5, calculate the compensation acceleration using a PI controller; Step 7: Based on the compensation acceleration in Step 6, use it as part of the bodyline acceleration task in WBC to improve the robot's anti-interference capability; Step 3 specifically involves calculating the foot contact force based on the lack of motion constraints at the supporting foot end and the robot's dynamic equations. The specific calculation formula is as follows: in, To access the Jacobian matrix, For the quality matrix, To select a matrix, For joint torque, It is a nonlinear term. For generalized speed; Step 6 specifically involves using a PI controller to calculate the compensation acceleration based on the included angle: in, and These are the proportional gain matrix and the integral gain matrix, respectively. Projected force and vertical vector The included angle; Specifically, step 7 involves the following formula for the base line acceleration task: in, Optimize variables for WBC.

2. The anti-interference whole-body control method for a quadruped robot according to claim 1, characterized in that, In step 1, NMPC planning includes establishing an optimal control problem with center of mass momentum and generalized position as state variables and foot contact force and joint velocity as input variables, and solving the problem to obtain the optimal state and input trajectory.

3. The anti-interference whole-body control method for a quadruped robot according to claim 1, characterized in that, Step 4 specifically involves calculating each side of the polygon supported by the robot to obtain the tipping axis. ; Calculate the perpendicular vector from the centroid to the overturning axis. ; Calculate the resultant force acting on the tipping axis that makes the robot unstable. sum moment ; Convert torque into equivalent couple ; Calculating the net force that causes instability in the robot and its mean ; in, The unit flip axis vector, and These are the estimated resultant force and resultant torque of the foot contact force relative to the robot's center of gravity, respectively. It is a unit vertical vector. It is a cross product matrix.

4. The anti-interference whole-body control method for a quadruped robot according to claim 3, characterized in that, Step 5, which projects the resultant force from step 4, specifically involves... Projected onto the x and y directions respectively: and Step 5, calculating the angle between the projected force and the vertical direction, specifically involves calculating the angle between the projected force and the vertical vector. The included angle is: in, Indicates by The symbol is determined by the first element.

5. A whole-body anti-interference control system for a quadruped robot based on stability margin perception, characterized in that, The system uses a quadruped robot anti-interference whole-body control method based on stability margin perception as described in any one of claims 1-4, and the system includes: The planning module is used to predict and control the NMPC to plan the whole-body motion trajectory of the quadruped robot using a nonlinear model; The control module is used to calculate joint driving torques for the whole-body controller (WBC) based on hierarchical quadratic programming. The estimation module is used to estimate the foot contact force based on the joint position, velocity, torque, and robot dynamics model fed back by the actuator; The compensation module projects the resultant force and calculates the angle with the vertical direction. Based on the angle, it calculates the compensation acceleration through a PI controller and incorporates the compensation acceleration as part of the body-line acceleration task in WBC, thereby improving the robot's anti-interference capability.

6. A computer device, characterized in that, It includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, it implements the method as described in any one of claims 1-4.

7. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program that, when executed by a processor, implements the method as described in any one of claims 1-4.

Images