A quadruped robot state correction method and system based on MPC

Abstract

The invention discloses a method and system for correcting a state of a quadruped robot based on MPC. The method comprises the following steps: the first step is to sort control tasks according to the redundant characteristics of multiple degrees of freedom of the quadruped robot; the second step is to set the control task of maintaining the body balance of the quadruped robot as the highest priority task; the third step is to obtain the expected value of the plantar reaction force of the quadruped robot, and project it in the null space of the Jacobian matrix of the control task of maintaining the body balance of the quadruped robot to obtain the input of the secondary priority task; the fourth step is to correct the expected value of the plantar reaction force of the quadruped robot according to the input of the secondary priority task to obtain the final expected value of the plantar reaction force to control the next movement of the quadruped robot; the invention solves the problem that the accuracy of the plantar reaction force obtained by using only a single rigid body dynamics model in MPC is not high, and the expected value of the plantar reaction force of the quadruped robot cannot be corrected, so it is difficult to ensure the stability of the movement of the quadruped robot.

Description

Technical Field

[0001] The invention relates to the technical field of quadruped robot state correction, and in particular to a quadruped robot state correction method and system based on MPC. Background Art

[0002] The quadruped robot belongs to a floating-base robot, that is, a robot whose base can move freely. The quadruped robot can be regarded as a single rigid body dynamics model in the world coordinate system. At present, in the process of controlling the movement of the quadruped robot, the controller is used to make the quadruped robot perform tasks such as stepping, turning, climbing and crossing obstacles. Specifically, the MPC controller is used to solve the expected value of the plantar reaction force of the quadruped robot to achieve the prediction and control of the next movement of the quadruped robot. However, the plantar reaction force obtained by simply using the single rigid body dynamics model in MPC is not accurate, and it is impossible to correct the expected value of the plantar reaction force of the quadruped robot, making it difficult to ensure the stability of the movement of the quadruped robot. Summary of the invention

[0003] In view of the above-mentioned defects, the present invention proposes a quadruped robot state correction method and system based on MPC, which aims to solve the problems that the plantar reaction force obtained by using only a single rigid body dynamics model in MPC is not accurate, and the expected value of the plantar reaction force of the quadruped robot cannot be corrected, making it difficult to ensure the stability of the quadruped robot's movement.

[0004] To achieve this object, the present invention adopts the following technical solutions:

[0005] A method for correcting a quadruped robot state based on MPC comprises the following steps:

[0006] Step S1: sorting the control tasks according to the redundant characteristics of the multi-degree-of-freedom of the quadruped robot;

[0007] Step S2: setting the control task of maintaining the body balance of the quadruped robot as the highest priority task;

[0008] Step S3: obtaining the expected value of the plantar reaction force of the quadruped robot, projecting it into the null space of the Jacobian matrix of the control task of maintaining the body balance of the quadruped robot, and obtaining the input of the secondary priority task;

[0009] Step S4: According to the input of the second priority task, the expected value of the plantar reaction force of the quadruped robot is corrected to obtain the final expected value of the plantar reaction force to control the next movement of the quadruped robot.

[0010] Preferably, in step S3, the following steps are specifically included:

[0011] Step S31: Calculate the Jacobian matrix null space projection operator N0 of the control task of maintaining the body balance of the quadruped robot;

[0012] Step S32: Calculate the Jacobian matrix null space projection operator N of other control tasks i|i-1 ;

[0013] Step S33: Obtain the modified acceleration according to the Jacobian matrix null space projection operator of the control task of maintaining the quadruped robot's body balance and the Jacobian matrix null space projection operator of the other control tasks. The modified acceleration is used as input for the lower priority task.

[0014] Preferably, in step S4, specifically according to the corrected acceleration Use quadratic programming to solve the final plantar reaction force f r , where the constraint formula for solving the quadratic programming is as follows:

[0015]

[0016] subject to

[0017]

[0018]

[0019]

[0020] W r ≥0 (4)

[0021] Wherein, equation (1) is the dynamic equation of the floating base system; equation (2) is the acceleration equation; equation (3) is the plantar reaction equation; equation (4) is the contact force constraint; Q1 and Q2 are the ground reaction forces calculated by the MPC controller; f r MPC is the ground reaction force calculated by the MPC controller; S f is the floating basis selection matrix; W is the augmented constraint matrix; δ f is the relaxation variable of the floating base acceleration; is the relaxation variable of the ground reaction force on the floating foundation; is the corrected acceleration; is the actual joint angular acceleration; A is the generalized mass matrix; b is the Coriolis force; g is the gravitational acceleration; J c is the (foot end) contact (ground) Jacobian matrix; is the zero vector.

[0022] Preferably, the Jacobian matrix null space projection operator N0 of the control task of maintaining the body balance of the quadruped robot and the Jacobian matrix null space projection operator N0 of the other control tasks are i|i-1and the corrected acceleration The calculation formula is as follows:

[0023]

[0024]

[0025]

[0026] in

[0027] J i|pre =J i N i-1

[0028] N i-1 =N0N 1|0 …N i-1|i-2

[0029]

[0030]

[0031] Since i ≥ 1,

[0032]

[0033]

[0034] in, is the generalized inverse matrix of the contact Jacobian matrix; e i is the position error, where is the expected position, x i is the actual position, e i Defined as:

[0035]

[0036] is the acceleration corresponding to the i-th task, where K p and K d are the position and velocity feedback gains, is the expected position acceleration, is the expected position velocity, is the actual position speed, Defined as:

[0037]

[0038] J i|pre is the projection of the Jacobian matrix of the i-th task in the null space of the Jacobian matrix of the previous task;

[0039] represents the generalized inverse matrix based on the following formula, where A′ is the weight matrix and J is the Jacobian matrix,

[0040]

[0041] Δq i is the joint error of the i-th task; Δq i-1 is the joint error of the i-1th task; e i for J i is the Jacobian matrix of the i-th task; is the expected joint velocity of the i-th task; is the expected joint velocity of the i-1th task; is the expected joint acceleration of the i-th task; is the expected joint acceleration of the i-1th task; is the expected position acceleration of the i-th task; is the time derivative of the Jacobian matrix of the i-th task; is the actual joint angular velocity; is the pseudo-inverse matrix of the projection matrix of the Jacobian matrix of the i-th task on the null space of the previous task; is the dynamic consistent pseudo-inverse matrix of the projection matrix of the Jacobian matrix of the i-th task on the null space of the previous task; is the dynamic consistent pseudo-inverse matrix of the projection matrix of the contact Jacobian matrix on the null space of the previous task; J c is the contact Jacobian matrix.

[0042] Preferably, the movement of the quadruped robot is controlled by joint torque, wherein the kinematic equation of the quadruped robot is as follows:

[0043]

[0044] Where A is the generalized mass matrix; b is the Coriolis force; g is the gravitational acceleration; τ is the joint torque, J c is the ground contact Jacobian matrix; f r ' is the ground reaction force; Represents the acceleration of the floating basis in the world coordinate system; Indicates the number of joints of the quadruped robot; represents the joint angle position of the quadruped robot; 06 represents the zero vector.

[0045] Another aspect of the present application provides a quadruped robot state correction system based on MPC, the system comprising

[0046] A sorting module is used to sort the control tasks according to the redundant characteristics of the multi-degree-of-freedom of the quadruped robot;

[0047] A setting module, used for setting the control task of maintaining the body balance of the quadruped robot as the highest priority task;

[0048] An acquisition module is used to obtain the expected value of the plantar reaction force of the quadruped robot;

[0049] A projection module is used to project in the null space of the Jacobian matrix of the control task of maintaining the body balance of the quadruped robot to obtain the input of the secondary priority task;

[0050] The correction module is used to correct the expected value of the plantar reaction force of the quadruped robot according to the input of the secondary priority task, and obtain the final expected value of the plantar reaction force to control the next movement of the quadruped robot.

[0051] Preferably, the projection module comprises

[0052] The first projection operator calculation submodule is used to calculate the Jacobian matrix null space projection operator N0 of the control task of maintaining the body balance of the quadruped robot;

[0053] The second projection operator calculation submodule is used to calculate the Jacobian matrix null space projection operator N of other control tasks i|i-1 ;

[0054] The modified acceleration calculation submodule is used to obtain the modified acceleration according to the Jacobian matrix null space projection operator of the control task of maintaining the body balance of the quadruped robot and the Jacobian matrix null space projection operator of the other control tasks.

[0055] Preferably, the correction module includes a plantar reaction force solving submodule, and the plantar reaction force solving submodule is used to calculate the plantar reaction force according to the corrected acceleration. Use quadratic programming to solve the final plantar reaction force f r .

[0056] The technical solution provided by the embodiments of the present application may have the following beneficial effects:

[0057] This scheme first uses the MPC controller to solve the expected value of the plantar reaction force of the quadruped robot, and then uses the WBC controller to correct the expected value of the plantar reaction force of the quadruped robot while ensuring that the quadruped robot body always maintains a balanced state. The final predicted plantar reaction force will be more accurate, thereby improving the stability of the quadruped robot's movement. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] Figure 1 It is a step diagram of a state correction method of a quadruped robot based on MPC. DETAILED DESCRIPTION

[0059] The embodiments of the present invention are described in detail below, and examples of the embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals represent the same or similar elements or elements having the same or similar functions from beginning to end. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and cannot be understood as limiting the present invention.

[0060] A method for correcting a quadruped robot state based on MPC comprises the following steps:

[0061] Step S1: sorting the control tasks according to the redundant characteristics of the multi-degree-of-freedom of the quadruped robot;

[0062] Step S2: setting the control task of maintaining the body balance of the quadruped robot as the highest priority task;

[0063] Step S3: obtaining the expected value of the plantar reaction force of the quadruped robot, projecting it into the null space of the Jacobian matrix of the control task of maintaining the body balance of the quadruped robot, and obtaining the input of the secondary priority task;

[0064] Step S4: According to the input of the second priority task, the expected value of the plantar reaction force of the quadruped robot is corrected to obtain the final expected value of the plantar reaction force to control the next movement of the quadruped robot.

[0065] This scheme is a quadruped robot state correction method based on MPC, such as Figure 1 As shown in the figure, the MPC controller is used to take the current state of the system as output and the ground reaction force as input. According to the state space equation of the quadruped robot and the current state, the optimal input of a series of sampling points in the future (prediction time domain) is obtained. Because most optimization problems are difficult to solve, the penalty function in the optimization algorithm is written in quadratic form, and the MPC problem is converted into a quadratic programming problem for solution. After obtaining a series of optimal inputs at the sampling point, the first one is selected as the expected value of the plantar reaction force and passed to the WBC controller. After a sampling time interval (usually 2ms), the MPC controller will correct the current state of the system and re-solve the next time.

[0066] The WBC controller first sorts the control tasks according to the redundant characteristics of the quadruped robot's multiple degrees of freedom, and sets the control task of maintaining the quadruped robot's body balance as the highest priority task, so that the quadruped robot can always maintain its body balance while performing tasks such as stepping, turning, climbing and crossing obstacles. Then, the expected value of the plantar reaction force of the quadruped robot is obtained. Specifically, the expected value of the plantar reaction force of the quadruped robot comes from the MPC controller, and is projected in the zero space of the Jacobian matrix of the control task of maintaining the body balance of the quadruped robot to obtain the input of the secondary priority task. Specifically, the method of using the zero space projection can not only strictly define the priority of the control task, but also improve the computational efficiency of the priority solution. Finally, according to the input of the secondary priority task, the expected value of the plantar reaction force of the quadruped robot is corrected to obtain the final expected value of the plantar reaction force to control the next step of the quadruped robot's movement, so that the quadruped robot moves more smoothly.

[0067] This scheme first uses the MPC controller to solve the expected value of the plantar reaction force of the quadruped robot, and then uses the WBC controller to correct the expected value of the plantar reaction force of the quadruped robot while ensuring that the quadruped robot body always maintains a balanced state. The final predicted plantar reaction force will be more accurate, thereby improving the stability of the quadruped robot's movement.

[0068] Preferably, in step S3, the following steps are specifically included:

[0069] Step S31: Calculate the Jacobian matrix null space projection operator N0 of the control task of maintaining the body balance of the quadruped robot;

[0070] Step S32: Calculate the Jacobian matrix null space projection operator N of other control tasks i|i-1 ;

[0071] Step S33: Obtain the modified acceleration according to the Jacobian matrix null space projection operator of the control task of maintaining the quadruped robot's body balance and the Jacobian matrix null space projection operator of the other control tasks. The modified acceleration is used as input for the lower priority task.

[0072] In this embodiment, the Jacobian matrix zero space projection operator N0 of the control task of maintaining the body balance of the quadruped robot is first calculated to ensure that the WBC controller always runs the task of maintaining the body balance of the quadruped robot and sets it as the highest priority task; then the Jacobian matrix zero space projection operator N0 of other control tasks is calculated. i|i-1 , and finally obtain the corrected acceleration This includes acceleration considerations for the floating basis model.

[0073] Preferably, in step S4, specifically according to the corrected acceleration Use quadratic programming to solve the final plantar reaction force f r , where the constraint formula for solving the quadratic programming is as follows:

[0074]

[0075] subject to

[0076]

[0077]

[0078]

[0079] W r ≥0 (4)

[0080] Wherein, equation (1) is the dynamic equation of the floating base system; equation (2) is the acceleration equation; equation (3) is the plantar reaction equation; equation (4) is the contact force constraint; Q1 and Q2 are the ground reaction forces calculated by the MPC controller; f r MPC is the ground reaction force calculated by the MPC controller; S f is the floating basis selection matrix; W is the augmented constraint matrix; δ f is the relaxation variable of the floating base acceleration; is the relaxation variable of the ground reaction force on the floating foundation; is the corrected acceleration; is the actual joint angular acceleration; A is the generalized mass matrix; b is the Coriolis force; g is the gravitational acceleration; J c is the (foot end) contact (ground) Jacobian matrix; is the zero vector.

[0081] In this embodiment, the constraints solved by the quadratic programming are the floating base system dynamics equation, the acceleration equation, the plantar reaction equation and the contact force constraint. Among them, the acceleration equation also takes into account the acceleration of the floating base model, so that the quadruped robot can be more stable under high-speed motion. The plantar reaction equation also takes into account the reaction force received by the floating base model, so that the calculated plantar reaction force will be more accurate than the plantar reaction force obtained by simply using the single rigid body dynamics model in MPC.

[0082] Preferably, the Jacobian matrix null space projection operator N0 of the control task of maintaining the body balance of the quadruped robot and the Jacobian matrix null space projection operator N0 of the other control tasks are i|i-1 and the corrected acceleration The calculation formula is as follows:

[0083]

[0084]

[0085]

[0086] in

[0087] J i|pre =J i N i-1

[0088] N i-1 =N0N 1|0 …N i-1|i-2

[0089]

[0090]

[0091] Since i ≥ 1,

[0092]

[0093]

[0094] in, is the generalized inverse matrix of the contact Jacobian matrix; e i is the position error, where is the expected position, x i is the actual position, e i Defined as:

[0095]

[0096] is the acceleration corresponding to the i-th task, where K p and K d are the position and velocity feedback gains, is the expected position acceleration, is the expected position velocity, is the actual position speed, Defined as:

[0097]

[0098] J i|pre is the projection of the Jacobian matrix of the i-th task in the null space of the Jacobian matrix of the previous task;

[0099] represents the generalized inverse matrix based on the following formula, where A′ is the weight matrix and J is the Jacobian matrix,

[0100]

[0101] Δq i is the joint error of the i-th task; Δq i-1 is the joint error of the i-1th task; e i for J i is the Jacobian matrix of the i-th task; is the expected joint velocity of the i-th task; is the expected joint velocity of the i-1th task; is the expected joint acceleration of the i-th task; is the expected joint acceleration of the i-1th task; is the expected position acceleration of the i-th task; is the time derivative of the Jacobian matrix of the i-th task; is the actual joint angular velocity; is the pseudo-inverse matrix of the projection matrix of the Jacobian matrix of the i-th task on the null space of the previous task; is the dynamic consistent pseudo-inverse matrix of the projection matrix of the Jacobian matrix of the i-th task on the null space of the previous task; is the dynamic consistent pseudo-inverse matrix of the projection matrix of the contact Jacobian matrix on the null space of the previous task; J c is the contact Jacobian matrix.

[0102] Specifically, the Jacobian matrix null space projection operator N0 of the control task of maintaining the body balance of the quadruped robot and the Jacobian matrix null space projection operator N0 of the other control tasks are obtained by calculation. i|i-1 and the corrected acceleration This is conducive to the subsequent correction of the expected value of the plantar reaction force of the quadruped robot.

[0103] Preferably, the movement of the quadruped robot is controlled by joint torque, wherein the kinematic equation of the quadruped robot is as follows:

[0104]

[0105] Where A is the generalized mass matrix; b is the Coriolis force; g is the gravitational acceleration; τ is the joint torque, J c is the ground contact Jacobian matrix; f r ' is the ground reaction force; Represents the acceleration of the floating basis in the world coordinate system; Indicates the number of joints of the quadruped robot; represents the joint angle position of the quadruped robot; 06 represents the zero vector.

[0106] Specifically, since the joint motors of the quadruped robot adopt torque control when it moves, when solving the joint torque corresponding to the expected value of the plantar reaction force calculated by the MPC controller, the quadratic programming solution is used again, which is beneficial to reduce the tracking error of the floating base acceleration and the plantar reaction force.

[0107] Another aspect of the present application provides a quadruped robot state correction system based on MPC, the system comprising

[0108] A sorting module is used to sort the control tasks according to the redundant characteristics of the multi-degree-of-freedom of the quadruped robot;

[0109] A setting module, used for setting the control task of maintaining the body balance of the quadruped robot as the highest priority task;

[0110] An acquisition module is used to obtain the expected value of the plantar reaction force of the quadruped robot;

[0111] A projection module is used to project in the null space of the Jacobian matrix of the control task of maintaining the body balance of the quadruped robot to obtain the input of the secondary priority task;

[0112] The correction module is used to correct the expected value of the plantar reaction force of the quadruped robot according to the input of the secondary priority task, and obtain the final expected value of the plantar reaction force to control the next movement of the quadruped robot.

[0113] The scheme is a quadruped robot state correction system based on MPC, including a sorting module, a setting module, an acquisition module, a projection module and a correction module. The modules cooperate with each other to correct the expected value of the plantar reaction force of the quadruped robot while ensuring that the body of the quadruped robot always maintains a balanced state. The final predicted plantar reaction force will be more accurate, thereby improving the stability of the quadruped robot's movement.

[0114] Preferably, the projection module comprises

[0115] The first projection operator calculation submodule is used to calculate the Jacobian matrix null space projection operator N0 of the control task of maintaining the body balance of the quadruped robot;

[0116] The second projection operator calculation submodule is used to calculate the Jacobian matrix null space projection operator N of other control tasks i|i-1 ;

[0117] The modified acceleration calculation submodule is used to obtain the modified acceleration according to the Jacobian matrix null space projection operator of the control task of maintaining the body balance of the quadruped robot and the Jacobian matrix null space projection operator of the other control tasks.

[0118] In this embodiment, the first projection operator calculation submodule, the second projection operator calculation submodule and the correction acceleration calculation submodule cooperate with each other to obtain the correction acceleration Corrected acceleration That is, the input of the priority task, which is conducive to the subsequent modification of the expected value of the plantar reaction force of the quadruped robot.

[0119] Further explanation: the Jacobian matrix null space projection operator N0 of the control task of maintaining the body balance of the quadruped robot and the Jacobian matrix null space projection operator N0 of the other control tasks i|i-1 and the corrected acceleration The calculation formula is as follows:

[0120]

[0121]

[0122]

[0123] in

[0124] J i|pre =J i N i-1

[0125] N i-1 =N0N 1|0 …N i-1|i-2

[0126]

[0127]

[0128] Since i ≥ 1,

[0129]

[0130]

[0131] in, is the generalized inverse matrix of the contact Jacobian matrix; e i is the position error, where is the expected position, x i is the actual position, e i Defined as:

[0132]

[0133] is the acceleration corresponding to the i-th task, where K p and K dare the position and velocity feedback gains, is the expected position acceleration, is the expected position velocity, is the actual position speed, Defined as:

[0134]

[0135] J i|pre is the projection of the Jacobian matrix of the i-th task in the null space of the Jacobian matrix of the previous task;

[0136] represents the generalized inverse matrix based on the following formula, where A′ is the weight matrix and J is the Jacobian matrix,

[0137]

[0138] Δq i is the joint error of the i-th task; Δq i-1 is the joint error of the i-1th task; e i for J i is the Jacobian matrix of the i-th task; is the expected joint velocity of the i-th task; is the expected joint velocity of the i-1th task; is the expected joint acceleration of the i-th task; is the expected joint acceleration of the i-1th task; is the expected position acceleration of the i-th task; is the time derivative of the Jacobian matrix of the i-th task; is the actual joint angular velocity; is the pseudo-inverse matrix of the projection matrix of the Jacobian matrix of the i-th task on the null space of the previous task; is the dynamic consistent pseudo-inverse matrix of the projection matrix of the Jacobian matrix of the i-th task on the null space of the previous task; is the dynamic consistent pseudo-inverse matrix of the projection matrix of the contact Jacobian matrix on the null space of the previous task; J c is the contact Jacobian matrix.

[0139] Preferably, the correction module includes a plantar reaction force solving submodule, and the plantar reaction force solving submodule is used to calculate the plantar reaction force according to the corrected acceleration. Use quadratic programming to solve the final plantar reaction force f r .

[0140] In this embodiment, the plantar reaction force f is calculated in the plantar reaction force solution submodule. r, which is more accurate than the plantar reaction force obtained by simply using the single rigid body dynamics model in MPC. To further illustrate, the constraint formula for the quadratic programming solution is as follows:

[0141]

[0142] subject to

[0143]

[0144]

[0145]

[0146] W r ≥0 (4)

[0147] Wherein, equation (1) is the dynamic equation of the floating base system; equation (2) is the acceleration equation; equation (3) is the plantar reaction equation; equation (4) is the contact force constraint; Q1 and Q2 are the ground reaction forces calculated by the MPC controller; f r MPC is the ground reaction force calculated by the MPC controller; S f is the floating basis selection matrix; W is the augmented constraint matrix; δ f is the relaxation variable of the floating base acceleration; is the relaxation variable of the ground reaction force on the floating foundation; is the corrected acceleration; is the actual joint angular acceleration; A is the generalized mass matrix; b is the Coriolis force; g is the gravitational acceleration; J c is the (foot end) contact (ground) Jacobian matrix; is the zero vector.

[0148] To further illustrate, the movement of the quadruped robot is controlled by joint torque, wherein the kinematic equation of the quadruped robot is as follows:

[0149]

[0150] Where A is the generalized mass matrix; b is the Coriolis force; g is the gravitational acceleration; τ is the joint torque, J c is the ground contact Jacobian matrix; f r ' is the ground reaction force; Represents the acceleration of the floating basis in the world coordinate system; Indicates the number of joints of the quadruped robot; represents the joint angle position of the quadruped robot; 06 represents the zero vector.

[0151] In addition, each functional unit in each embodiment of the present invention may be integrated into a processing module, or each unit may exist physically separately, or two or more units may be integrated into one module. The above-mentioned integrated module may be implemented in the form of hardware or in the form of a software functional module. If the integrated module is implemented in the form of a software functional module and sold or used as an independent product, it may also be stored in a computer-readable storage medium.

[0152] Although the embodiments of the present invention have been shown and described above, it is to be understood that the above embodiments are exemplary and are not to be construed as limitations of the present invention. A person skilled in the art may change, modify, replace and vary the above embodiments within the scope of the present invention.

Claims

1. A quadruped robot state correction method based on MPC, characterized in that: The following steps are involved: Step S1: sorting the control tasks according to the redundant characteristics of the multi-degree-of-freedom of the quadruped robot; Step S2: setting the control task of maintaining the body balance of the quadruped robot as the highest priority task; Step S3: obtaining the expected value of the plantar reaction force of the quadruped robot, projecting it into the null space of the Jacobian matrix of the control task of maintaining the body balance of the quadruped robot, and obtaining the input of the secondary priority task; Step S4: According to the input of the second priority task, the expected value of the plantar reaction force of the quadruped robot is corrected to obtain the final expected value of the plantar reaction force to control the next movement of the quadruped robot; In step S3, the following steps are specifically included: Step S31: Calculate the Jacobian matrix null space projection operator N0 of the control task of maintaining the body balance of the quadruped robot; Step S32: Calculate the Jacobian matrix null space projection operator N of other control tasks i|i-1 ; Step S33: Obtain the modified acceleration according to the Jacobian matrix null space projection operator of the control task of maintaining the quadruped robot's body balance and the Jacobian matrix null space projection operator of the other control tasks. The modified acceleration is used as an input of a lower priority task; In step S4, specifically, the corrected acceleration Use quadratic programming to solve the final plantar reaction force f r , where the constraint formula for solving the quadratic programming is as follows: Wf r ≥0 (4) Wherein, equation (1) is the dynamic equation of the floating base system; equation (2) is the acceleration equation; equation (3) is the plantar reaction equation; equation (4) is the contact force constraint; Q1 and Q2 are the ground reaction forces calculated by the MPC controller; f r MPC is the ground reaction force calculated by the MPC controller; S r is the floating basis selection matrix; W is the augmented constraint matrix; δ f is the relaxation variable of the floating base acceleration; is the relaxation variable of the ground reaction force on the floating foundation; is the corrected acceleration; is the actual joint angular acceleration; A is the generalized mass matrix; b is the Coriolis force; g is the gravitational acceleration; J c is the Jacobian matrix of the foot contacting the ground; is the zero vector; The Jacobian matrix null space projection operator N0 of the control task of maintaining the body balance of the quadruped robot, the Jacobian matrix null space projection operator N i|i-1 and the corrected acceleration The calculation formula is as follows: in I i|pre =J i N i-1 N i-1 =N0N 1|0 …N i-1|i-2 Since i ≥ 1, in, is the generalized inverse matrix of the contact Jacobian matrix; e i is the position error, where is the expected position, x i is the actual position, e i Defined as: is the acceleration corresponding to the i-th task, where K p and K d are the position and velocity feedback gains, is the expected position acceleration, is the expected position velocity, is the actual position speed, Defined as: J i|pre is the projection of the Jacobian matrix of the i-th task in the null space of the Jacobian matrix of the previous task; represents the generalized inverse matrix based on the following formula, where A' is the weight matrix and J is the Jacobian matrix, Δq i is the joint error of the i-th task; Δq i-1 is the joint error of the i-1th task; e i for J i is the Jacobian matrix of the i-th task; is the expected joint velocity of the i-th task; is the expected joint velocity of the i-1th task; is the expected joint acceleration of the i-th task; is the expected joint acceleration of the i-1th task; is the expected position acceleration of the i-th task; is the time derivative of the Jacobian matrix of the i-th task; is the actual joint angular velocity; is the pseudo-inverse matrix of the projection matrix of the Jacobian matrix of the i-th task on the null space of the previous task; is the dynamic consistent pseudo-inverse matrix of the projection matrix of the Jacobian matrix of the i-th task on the null space of the previous task; is the dynamic consistent pseudo-inverse matrix of the projection matrix of the contact Jacobian matrix on the null space of the previous task; J c is the Jacobian matrix of the foot contacting the ground.

2. The method for correcting the state of a quadruped robot based on MPC according to claim 1, characterized in that: The motion of the quadruped robot is controlled by joint torque, wherein the kinematic equation of the quadruped robot is as follows: Where A is the generalized mass matrix; b is the Coriolis force; g is the gravitational acceleration; τ is the joint torque, J c is the Jacobian matrix of the foot contacting the ground; f t ’ is the ground reaction force; Represents the acceleration of the floating basis in the world coordinate system; Indicates the number of joints of the quadruped robot; represents the joint angle position of the quadruped robot; 06 represents the zero vector.

3. A quadruped robot state correction system based on MPC, characterized in that: Using the MPC-based quadruped robot state correction method according to any one of claims 1 to 2, the system comprises A sorting module is used to sort the control tasks according to the redundant characteristics of the multi-degree-of-freedom of the quadruped robot; A setting module, used for setting the control task of maintaining the body balance of the quadruped robot as the highest priority task; An acquisition module is used to obtain the expected value of the plantar reaction force of the quadruped robot; A projection module is used to project in the null space of the Jacobian matrix of the control task of maintaining the body balance of the quadruped robot to obtain the input of the secondary priority task; The correction module is used to correct the expected value of the plantar reaction force of the quadruped robot according to the input of the secondary priority task, and obtain the final expected value of the plantar reaction force to control the next movement of the quadruped robot.

4. The MPC-based quadruped robot state correction system according to claim 3, characterized in that: The projection module includes The first projection operator calculation submodule is used to calculate the Jacobian matrix null space projection operator N0 of the control task of maintaining the body balance of the quadruped robot; The second projection operator calculation submodule is used to calculate the Jacobian matrix null space projection operator N of other control tasks i|i-1 ; The modified acceleration calculation submodule is used to obtain the modified acceleration according to the Jacobian matrix null space projection operator of the control task of maintaining the body balance of the quadruped robot and the Jacobian matrix null space projection operator of the other control tasks.

5. The MPC-based quadruped robot state correction system according to claim 4, characterized in that: The correction module includes a plantar reaction force solving submodule, and the plantar reaction force solving submodule is used to calculate the plantar reaction force according to the corrected acceleration. Use quadratic programming to solve the final plantar reaction force f r .

Images