Quadruped robot anti-interference whole-body control method and system based on stability margin perception

Abstract

Belongs to the technical field of motion control of foot-type robots, and particularly relates to an anti-interference whole-body control method and system for a quadruped robot based on stability margin perception. The expected joint position, speed and foot end contact force are obtained through nonlinear model predictive control NMPC; a whole body controller WBC based on hierarchical quadratic programming calculates a joint driving torque according to the task priority; according to the joint position, the speed and the torque fed back by the driver and a robot dynamic model, estimating the foot end contact force; calculating resultant force which causes instability of the robot relative to each support polygon side line of the robot; projecting the resultant force, and calculating an included angle between the projection force and the vertical direction; calculating a compensation acceleration through a PI controller; and the compensation acceleration is used as a part of a fuselage linear acceleration task in the WBC, so that the anti-interference capability of the robot is improved. The method is used for solving the problems that in the prior art, the stable state of the quadruped robot cannot be effectively sensed, and an anti-interference control strategy cannot be rapidly generated according to the stable state.

Description

Technical Field

[0001] The present invention belongs to the technical field of legged robot motion control, and in particular relates to a quadruped robot anti-interference whole-body control method and system based on stability margin perception. Background Art

[0002] Quadruped robots, with their structure similar to that of four-legged animals, possess excellent environmental adaptability and locomotion, and have broad application prospects in complex terrain exploration, disaster relief, industrial inspection, and other fields. However, in real-world applications, quadruped robots often face external disturbances such as sudden collisions, which can cause the robot to deviate from its intended trajectory or even lose balance and fall. Currently, model-based quadruped robot control methods perform well under normal operating conditions. However, when faced with unknown external disturbances, their ability to resist them is limited 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 reinforcement learning-based control methods. Model-based control methods establish a dynamic model of the robot to plan its motion trajectory and calculate joint torques. These methods perform well under normal operating conditions, but their ability to resist unknown external disturbances is limited due to a lack of disturbance perception and rapid response mechanisms. Data-based control methods, such as reinforcement learning and neural networks, learn control strategies using large amounts of training data. While these methods offer a certain degree of interference resistance, their generalization performance is limited, and they struggle to ensure real-time performance and safety.

[0004] In recent years, researchers have proposed a number of control methods based on stability metrics, such as the zero moment point (ZMP) and support 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 the research on the stable control of quadruped robots under dynamic motion and complex interference conditions is not in-depth enough. The existing technology lacks a method that can effectively perceive the stable state of a quadruped robot and quickly generate an anti-interference control strategy based on it. In particular, there is a clear technical gap in combining advanced whole-body control frameworks to achieve the perception and compensation of unknown external interference. Summary of the Invention

[0005] The present invention provides a quadruped robot anti-interference whole-body control method and system based on stability margin perception, which are used to solve the problem in the prior art that the stable state of the quadruped robot cannot be effectively perceived and the anti-interference control strategy cannot be quickly generated accordingly.

[0006] The present invention is achieved through the following technical solutions:

[0007] A method for anti-interference whole-body control of a quadruped robot based on stability margin perception, the method comprising the following steps:

[0008] Step 1: Use nonlinear model predictive control (NMPC) to plan the full-body motion trajectory of the quadruped robot and obtain the desired joint positions, velocities, and foot contact forces.

[0009] Step 2: The whole body controller (WBC) based on hierarchical quadratic programming calculates the joint driving torque according to the task priority;

[0010] Step 3: Estimate the foot contact force based on the joint position, velocity, torque and robot dynamics model fed back by the actuator;

[0011] Step 4: Based on the foot end position and the foot end contact force in step 3, calculate the resultant force that causes the robot to be unstable relative to the edges of each supporting polygon of the robot;

[0012] Step 5: Project the resultant force from step 4 and calculate the angle between the projected force and the vertical direction;

[0013] Step 6: Based on the angle in step 5, the compensation acceleration is calculated by the PI controller;

[0014] Step 7: The compensation acceleration based on step 6 is used as part of the fuselage linear acceleration task in WBC to improve the robot's anti-interference ability.

[0015] Furthermore, the NMPC planning in step 1 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.

[0016] Furthermore, the step 3 specifically includes calculating the foot end contact force based on the foot end having no motion constraint and the robot dynamics equation;

[0017] The specific calculation formula is:

[0018]

[0019] Among them, J c is the contact Jacobian matrix, M is the mass matrix, S is the selection matrix, τ is the joint torque, h is the nonlinear term, is the generalized speed.

[0020] Furthermore, the step 4 is specifically to calculate each edge line of the robot support polygon to obtain the tipping axis a i ;

[0021] Calculate the perpendicular vector I from the center of mass to the overturn axis i ;

[0022] Calculate the net force acting on the tipping axis that would destabilize the robot and torque

[0023]

[0024] Converting moments to equivalent couples

[0025] Calculate the net force f that causes the robot to become unstable ui =f ri +f ni and its mean

[0026] in, is the unit flip axis vector, f r and n r are the estimated resultant force and moment of the foot-ground contact force relative to the robot's center of gravity, is the unit vertical vector, is the cross product matrix.

[0027] Furthermore, the step 5 projects the resultant force of step 4 as follows: The projections to the x and y directions are:

[0028] and

[0029] The step 5 of calculating the angle between the projection force and the vertical direction is specifically to calculate the angle between the projection force and the vertical vector v=[0; 0; -1] as:

[0030]

[0031] in, Indicated by The sign is determined by the first element of .

[0032] Furthermore, the step 6 is specifically to use a PI controller to calculate the compensation acceleration according to the angle:

[0033]

[0034] Among them, K Pd and K Id are the proportional gain matrix and the integral gain matrix respectively.

[0035] Furthermore, the step 7 is specifically that the formula of the base linear acceleration task is:

[0036]

[0037] in, Optimize variables for WBC.

[0038] A quadruped robot anti-interference whole-body control system based on stability margin perception, the system using the above-mentioned quadruped robot anti-interference whole-body control method based on stability margin perception, the system comprising:

[0039] The planning module is used to plan the whole-body motion trajectory of the quadruped robot through nonlinear model predictive control (NMPC);

[0040] A control module is used to calculate the joint driving torque based on the whole body controller WBC of hierarchical quadratic programming;

[0041] 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 driver;

[0042] The compensation module projects the resultant force and calculates the angle with the vertical direction. Based on the angle, the compensation acceleration is calculated through the PI controller. The compensation acceleration is used as part of the fuselage linear acceleration task in the WBC to improve the robot's anti-interference ability.

[0043] A computer device includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the method described above is implemented.

[0044] A computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the method described above is implemented.

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

[0046] The present invention calculates the stability margin, senses the impact of external interference on the robot's stability, and generates corresponding compensation acceleration, thereby significantly improving the robot's ability to resist external unknown interference.

[0047] The present invention is based on the robot dynamics model and physical principles, does not require a complex interference observer, has a small calculation burden, and meets the requirements of real-time control.

[0048] The present invention can effectively cope with various types of external interferences such as unidirectional linear interference, synthetic interference and impact interference, and has strong adaptability.

[0049] The present invention is not only applicable to the standing state, but also to dynamic motion states such as trot gait. By innovatively using virtual support points to form support polygons, the problem of stability margin calculation in the two-leg support state is solved.

[0050] The present invention can significantly improve the quadruped robot's ability to resist unknown external interference, is applicable to various interference types and motion states, and has good application value.

[0051] The present invention can sense the stable state of the quadruped robot in real time when the robot is subject to unknown external interference and generate compensatory acceleration, thereby improving the robot's anti-interference ability and motion stability.

[0052] The present invention does not need to identify external forces, but only needs to calculate the stable value through the state quantity, and then map it into the expected acceleration compensation value of the fuselage, so as to improve the anti-interference ability. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] Figure 1 Schematic diagram of the quadruped robot and coordinate system of the present invention.

[0054] Figure 2 Schematic diagram of the control architecture of the present invention. DETAILED DESCRIPTION

[0055] In the following description, specific details such as specific system structures and technologies are provided for illustration rather than limitation to facilitate a thorough understanding of the embodiments of the present application. However, it should be clear to those skilled in the art that the present application may be implemented in other embodiments without these specific details. In other cases, detailed descriptions of well-known systems, devices, circuits, and methods are omitted to avoid obstructing the description of the present application with unnecessary details.

[0056] It will be understood that when used in this specification and the appended claims, the term "comprising" indicates the presence of described features, integers, steps, operations, elements and / or components, but does not preclude the presence or addition of one or more other features, integers, steps, operations, elements, components and / or groups thereof.

[0057] It should also be understood that the terms used in this specification are only for the purpose of describing specific embodiments and are not intended to limit the present application. As used in this 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.

[0058] The following is attached to this application specification Figure 1-2 , clearly and completely describe the technical solutions in the embodiments of this application. Obviously, the described embodiments are only part of the embodiments of this application, not all of them. Based on the embodiments in this application, all other embodiments obtained by ordinary technicians in this field without making creative efforts shall fall within the scope of protection of this application.

[0059] In the following description, many specific details are set forth to facilitate a full understanding of the present application. However, the present application may also be implemented in other ways different from those described herein. Those skilled in the art may make similar generalizations without violating the connotation of the present application. Therefore, the present application is not limited to the specific embodiments disclosed below.

[0060] Implementation Method 1

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

[0062] Step 1: Use nonlinear model predictive control (NMPC) to plan the full-body motion trajectory of the quadruped robot and obtain the desired joint positions, velocities, and foot contact forces.

[0063] Step 2: The whole body controller (WBC) based on hierarchical quadratic programming calculates the joint driving torque according to the task priority;

[0064] Step 3: Estimate the foot contact force based on the joint position, velocity, torque and robot dynamics model fed back by the actuator;

[0065] Step 4: Based on the foot end position and the foot end contact force in step 3, calculate the resultant force that causes the robot to be unstable relative to the edges of each supporting polygon of the robot;

[0066] Step 5: Project the resultant force from step 4 and calculate the angle between the projected force and the vertical direction;

[0067] Step 6: Based on the angle in step 5, the compensation acceleration is calculated by the PI controller;

[0068] Step 7: The compensation acceleration based on step 6 is used as part of the fuselage linear acceleration task in WBC to improve the robot's anti-interference ability.

[0069] Furthermore, the NMPC planning in step 1 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.

[0070] Furthermore, the step 3 specifically includes calculating the foot end contact force based on the foot end having no motion constraint and the robot dynamics equation;

[0071] The specific calculation formula is:

[0072]

[0073] Among them, J c is the contact Jacobian matrix, M is the mass matrix, S is the selection matrix, τ is the joint torque, h is the nonlinear term, is the generalized velocity; the linear Kalman filter LKF is used to smooth the calculated foot contact force to reduce the influence of noise.

[0074] Furthermore, the step 4 is specifically to calculate each edge line of the robot support polygon to obtain the tipping axis a i =p i+1 -p i ; Calculate the perpendicular vector from the center of mass to the overturn axis Calculate the net force acting on the tipping axis that would destabilize the robot and torque

[0075] Converting moments to equivalent couples

[0076] Calculate the net force f that causes the robot to become unstable ui =f ri +f ni and its mean

[0077] Among them, p i is the foot end position, p G is the center of mass position, 1 is the unit vector, is the unit flip axis vector, f r and n r are the estimated resultant force and moment of the foot-ground contact force relative to the robot's center of gravity, is the unit vertical vector, is the cross product matrix.

[0078] Furthermore, the step 5 projects the resultant force of step 4 as follows: The projections to the x and y directions are:

[0079]

[0080] The step 5 of calculating the angle between the projection force and the vertical direction is specifically to calculate the angle between the projection force and the vertical vector v=[0; 0; -1] as:

[0081]

[0082] in, Indicated by The sign is determined by the first element of .

[0083] Furthermore, the step 6 is specifically to use a PI controller to calculate the compensation acceleration according to the angle:

[0084]

[0085] Among them, K Pd and K Id are the proportional gain matrix and the integral gain matrix respectively.

[0086] Furthermore, the step 7 is specifically that the formula of the base linear acceleration task is:

[0087]

[0088] in, Optimize variables for WBC.

[0089] like Figure 1 As shown in the figure, the quadruped robot involved in the present invention mainly includes a base (body) and four legs, each leg has three degrees of freedom, a total of 12 joint degrees of freedom. The figure shows the main coordinate system: inertial coordinate system Body coordinate system Barycentric coordinate system and foot-end coordinate system As well as the main forces and vectors used in the calculation of the force angle stability margin (FASM). The generalized position and generalized velocity of the robot are defined as:

[0090]

[0091] in, I r IB is the position of the fuselage in the inertial coordinate system, is the ZYX Euler angle representation of the fuselage posture, q j is the joint position, I v B and B ω IB are the linear velocity and angular velocity of the fuselage in the inertial coordinate system respectively. j is the number of joints. For the quadruped robot of the present invention, n j =12.

[0092] The rigid body dynamics equation of the robot is:

[0093]

[0094] Where M(q) is the generalized mass matrix, Contains Coriolis force, centrifugal force and gravity terms, S is the selection matrix, τ is the joint torque, J c is the stacked support Jacobian matrix, F c is the stacking constraint.

[0095] The dynamic equation is divided into the driven part and the underdriven part. The underdriven part can be transformed into the center-of-mass dynamics:

[0096]

[0097] The relationship between center-of-mass momentum and generalized velocity is:

[0098]

[0099] The fuselage speed can be calculated by the following formula:

[0100]

[0101] like Figure 2 As shown in FIG, the control architecture of the present 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.

[0102] 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:

[0103]

[0104] Among them, the state variable x(t)=[h com ;q], control input The constraints include system dynamics constraints, foot kinematic constraints, joint torque limits, and ground friction constraints. The cost function is:

[0105] WBC solves a hierarchical quadratic programming problem to calculate joint torques that meet multiple task priorities:

[0106]

[0107] in, is the optimization variable, and the tasks are sorted from high to low priority as follows: floating base dynamics equation; torque limitation, friction cone constraint and contactless motion constraint; base linear acceleration, base angular acceleration and swing leg motion tracking; contact force tracking.

[0108] The formula for the base linear acceleration task is:

[0109]

[0110] in, It is a compensatory acceleration based on stability margin, which improves the robot's anti-interference ability by sensing external interference and compensating for it.

[0111] There is no motion constraint at the foot of the supporting leg And the robot dynamics equation, calculate the foot end contact force:

[0112]

[0113] Since the joint torque estimated by the motor driver is noisy, a linear Kalman filter (LKF) is used to smooth the calculated foot contact force.

[0114] Based on the force angle stability margin theory, calculate the resultant force and torque acting on the center of mass that make the robot unstable:

[0115]

[0116] For each edge of the supporting polygon (overturning axis) a i and the corresponding perpendicular vector I from the center of mass to the overturning axis i , calculate the resultant force and its mean that make the robot unstable:

[0117]

[0118] The unit vector is defined as: Will Projected to the x and y directions respectively:

[0119]

[0120] Calculate the angle between the projected force and the vertical vector v = [0; 0; -1]

[0121]

[0122] in Defines the sign of the angle.

[0123] Use the PI controller to calculate the compensation acceleration based on the angle:

[0124]

[0125] Among them, K Pd and K Id They are the proportional gain matrix and the integral gain matrix respectively. By adjusting these parameters, the compensation strength and response speed can be controlled.

[0126] In situations like the trot gait, where only two legs support the stance, the traditional support polygon degenerates into a single line, making it insufficient for calculating the stability margin. To address this issue, the present invention innovatively mirrors the foot of the stance leg along the xz plane, using this as the virtual support point for the swing leg. This new support polygon is then used to calculate the stability margin.

[0127] The result calculated by equation (14) is integrated into equation (8), and the compensation acceleration is calculated according to the change of robot stability, thereby improving the robot's anti-interference ability.

[0128] The method for calculating stability of the present invention is: improving the anti-interference ability of the robot through the stability of the robot.

[0129] The present invention can also solve the problem of how to combine the currently used method for evaluating robot stability with the existing framework, how to use the obtained value describing the robot's stability as the robot's control quantity, and how to improve the robot's ability to resist external interference by improving the robot's stability.

[0130] Implementation Method 2

[0131] This embodiment provides a quadruped robot anti-interference whole-body control system based on stability margin perception. The system uses the quadruped robot anti-interference whole-body control method based on stability margin perception as described in Embodiment 1. The system includes:

[0132] The planning module is used to plan the whole-body motion trajectory of the quadruped robot through nonlinear model predictive control (NMPC);

[0133] A control module is used to calculate the joint driving torque based on the whole body controller WBC of hierarchical quadratic programming;

[0134] 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 driver;

[0135] The compensation module projects the resultant force and calculates the angle with the vertical direction. Based on the angle, the compensation acceleration is calculated through the PI controller. The compensation acceleration is used as part of the fuselage linear acceleration task in the WBC to improve the robot's anti-interference ability.

[0136] Implementation Method 3

[0137] An embodiment of the present invention provides an electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor. The memory is used to store software programs and modules, and the processor executes various functional applications and data processing by executing the software programs and modules stored in the memory. The memory and processor are connected via a bus. Specifically, the processor implements any step of the first embodiment described above by executing the computer program stored in the memory.

[0138] It should be understood that in the embodiments of the present invention, the processor referred to may be a central processing unit (CPU), and the processor may also be other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor may be a microprocessor or any conventional processor, etc.

[0139] The memory may include a read-only memory, a flash memory, and a random access memory, and provides instructions and data to the processor. A portion or all of the memory may also include a non-volatile random access memory.

[0140] It should be understood that if the above-mentioned integrated modules / units are implemented in the form of software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the present invention implements all or part of the processes in the above-mentioned embodiment method, and can also be completed by instructing the relevant hardware through a computer program. The above-mentioned computer program can be stored in a computer-readable storage medium. When the computer program is executed by the processor, it can implement the steps of the above-mentioned various method embodiments. Among them, the above-mentioned computer program includes computer program code, and the above-mentioned computer program code can be in source code form, object code form, executable file or some intermediate form. The above-mentioned computer-readable medium may include: any entity or device capable of carrying the above-mentioned computer program code, recording medium, USB flash drive, mobile hard disk, magnetic disk, optical disk, computer memory, read-only memory (ROM), random access memory (RAM), electric carrier signal, telecommunication signal and software distribution medium. It should be noted that the content contained in the above-mentioned computer-readable storage medium can be appropriately increased or decreased according to the requirements of legislation and patent practice in the jurisdiction.

[0141] The above description of the disclosed embodiments will enable one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be readily apparent to one 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 present invention. Therefore, the present invention is not limited to the embodiments shown herein, but is to be construed in the widest manner consistent with the principles and novel features disclosed herein.

[0142] Those skilled in the art can clearly understand that, for the convenience and brevity of description, only the division of the above-mentioned functional units and modules is used as an example for illustration. In actual applications, the above-mentioned functions can be distributed and completed by different functional units and modules as needed, that is, the internal structure of the above-mentioned 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 implementation method can be integrated into one processing unit, or each unit can exist physically alone, or two or more units can be integrated into one unit. The above-mentioned integrated unit can be implemented in the form of hardware or in the form of software functional units. In addition, the specific names of the functional units and modules are only for the convenience of distinguishing each other, and are not used to limit the scope of protection of the present invention. The specific working process of the units and modules in the above-mentioned system can refer to the corresponding process in the aforementioned method implementation method, and will not be repeated here.

[0143] It should be noted that the methods and detailed examples provided in the above embodiments can be combined with the devices and equipment provided in the embodiments, and references can be made to each other, and no further details will be given.

[0144] Those skilled in the art will appreciate that the units and algorithm steps of each example 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 performed in hardware or software depends on the specific application and design constraints of the technical solution. Professionals and technicians may use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of the present invention.

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

[0146] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit them. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the above embodiments, or make equivalent replacements for some of the technical features therein. These modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the various embodiments of the present invention, and should all be included in the scope of protection 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 comprises the following steps: Step 1: Use nonlinear model predictive control (NMPC) to plan the full-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 end position and the foot end contact force in step 3, calculate the resultant force that causes the robot to be unstable relative to the edges of each supporting polygon of 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 angle in step 5, the compensation acceleration is calculated by the PI controller; Step 7: The compensation acceleration based on step 6 is used as part of the fuselage linear acceleration task in WBC to improve the robot's anti-interference ability.

2. The anti-interference whole-body control method for a quadruped robot according to claim 1, characterized in that: The NMPC planning in step 1 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: Specifically, step 3 includes calculating the foot end contact force based on the support foot end having no motion constraints and the robot dynamics equation; The specific calculation formula is: Among them, J c is the contact Jacobian matrix, M is the mass matrix, S is the selection matrix, τ is the joint torque, h is the nonlinear term, is the generalized speed.

4. The anti-interference whole-body control method for a quadruped robot according to claim 1, characterized in that: The step 4 is specifically to calculate each edge of the robot support polygon to obtain the tipping axis a i ; Calculate the perpendicular vector I from the center of mass to the overturn axis i ; Calculate the net force acting on the tipping axis that would destabilize the robot sum moment Converting moments to equivalent couples Calculate the net force f that causes the robot to become unstable ui =f ri +f ni and its mean in, is the unit flip axis vector, f r and n r are the estimated resultant force and moment of the foot-ground contact force relative to the robot's center of gravity, is the unit vertical vector, is the cross product matrix.

5. The anti-interference whole-body control method for a quadruped robot according to claim 4, characterized in that: The step 5 projects the resultant force of step 4 as follows: The projections to the x and y directions are: and The step 5 of calculating the angle between the projection force and the vertical direction is specifically to calculate the angle between the projection force and the vertical vector v=[0; 0; -1] as: in, Indicated by The sign is determined by the first element of .

6. The anti-interference whole-body control method for a quadruped robot according to claim 5, characterized in that: Specifically, step 6 uses a PI controller to calculate the compensation acceleration according to the angle: Among them, K Pd and K Id are the proportional gain matrix and the integral gain matrix respectively.

7. The anti-interference whole-body control method for a quadruped robot according to claim 6, characterized in that: Specifically, the formula of the base linear acceleration task is: in, Optimize variables for WBC.

8. A quadruped robot anti-interference whole-body control system 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 according to any one of claims 1 to 7, and the system includes: The planning module is used to plan the whole-body motion trajectory of the quadruped robot through nonlinear model predictive control (NMPC); A control module is used to calculate the joint driving torque based on the whole body controller WBC of 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 driver; The compensation module projects the resultant force and calculates the angle with the vertical direction. Based on the angle, the compensation acceleration is calculated through the PI controller. The compensation acceleration is used as part of the fuselage linear acceleration task in the WBC to improve the robot's anti-interference ability.

9. A computer device, characterized in that: The method comprises 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, the method according to any one of claims 1 to 7 is implemented.

10. A computer-readable storage medium, characterized in that The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the method according to any one of claims 1 to 7 is implemented.

Images