A control method and system for stable motion of a quadruped robot

Abstract

The present invention relates to the field of robot control technology, and in particular to a control method and system for the stable movement of a quadruped robot, the method comprising sampling the current state and ground reaction force of the quadruped robot according to a sampling time interval, and performing optimization processing; substituting the optimized current state and ground reaction force into the dynamic model of the quadruped robot to obtain the expected plantar reaction force of a series of sampling points in the prediction time domain; substituting the first expected plantar reaction force into the correction model of the quadruped robot to obtain the final expected plantar reaction force; feeding back the final expected plantar reaction force to the bottom controller of the quadruped robot, and the bottom controller is used to control the joint motor of the quadruped robot to make the quadruped robot move. The present invention can enable the quadruped robot to maintain its own stability and smooth movement while performing more diverse and freer movement performance and posture.

Description

Technical Field

[0001] The present invention relates to the field of robot control technology, and in particular to a control method and system for the stable movement of a quadruped robot. Background Art

[0002] As a multi-rotor drone among ground robots, quadruped robots have been favored by more and more research institutes, universities, enterprises and other institutions in recent years. They are used for secondary development of robots or for various applications such as industrial implementation, becoming one of the most popular robots.

[0003] Compared with wheeled robots and tracked robots, quadruped robots have stronger terrain adaptability. They can walk, climb stairs and avoid obstacles automatically, whether on soft ground or rugged terrain. However, the existing demand still puts higher requirements on quadruped robots: how to make quadruped robots maintain their stability and smooth movement while performing more diverse and freer movement performance and postures. Summary of the invention

[0004] The purpose of the present invention is to provide a control method and system for the stable movement of a quadruped robot, which can enable the quadruped robot to maintain its own stability and smooth movement while performing more diverse and freer movement performance and posture.

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

[0006] A control method for stable motion of a quadruped robot comprises the following steps:

[0007] S1. Sampling the current state and ground reaction force of the quadruped robot according to the sampling time interval, and optimizing the sampled current state and ground reaction force of the quadruped robot;

[0008] S2, substituting the optimized current state and ground reaction force into the dynamic model of the quadruped robot to obtain the expected values ​​of the plantar reaction force of a series of sampling points in the prediction time domain;

[0009] S3, substituting the first expected value of the plantar reaction force into the correction model of the quadruped robot to obtain the final expected value of the plantar reaction force;

[0010] S4. Feedback the final expected value of the plantar reaction force to the bottom-level controller of the quadruped robot, wherein the bottom-level controller is used to control the joint motors of the quadruped robot to enable the quadruped robot to move.

[0011] Preferably, in S1, the current state of the quadruped robot is expressed as:

[0012]

[0013] Among them, x i Represents the state vector of the system at the i-th sampling time point; and Respectively represent approximate values; It represents the Euler angle of the quadruped robot's body coordinate system in the world coordinate system; Represents the velocity of the quadruped robot's body coordinate system in the world coordinate system.

[0014] Preferably, in S1, the ground reaction force of the quadruped robot is expressed as:

[0015]

[0016] Where n is the number of robot legs; R z (ψ) represents the basic rotation matrix of the robot body around the z-axis, ψ represents the rotation angle; I represents the inertia tensor of the body in the world coordinate system; r i represents the vector from the contact point of the ith leg to the center of mass of the body; f i represents the ground reaction force on the i-th leg.

[0017] Preferably, in S1, the sampled current state of the quadruped robot and the ground reaction force are optimized by an optimization algorithm, and the optimization algorithm includes:

[0018]

[0019] subject to x i+1 =A i x i +B i u i , i=0...k-1

[0020]

[0021] D i u i =0, i=0...k-1

[0022] Among them, k represents the prediction time domain length; x i+1，ref represents the system state vector of the i+1th sampling point; Q i Represents the system state weight matrix; R i represents the system state weight matrix; u i represents the control input signal at sampling time i, i.e., the plantar reaction force f; c i Indicates the lower limit of plantar reaction force; Indicates the upper limit of plantar reaction force; C irepresents the input signal inequality constraint matrix; D i It means that the plantar reaction force of the foot that is not in contact with the ground at time i is set to 0.

[0023] Preferably, in S2, the dynamic model is mathematically modeled on the quadruped robot by a model predictive control (MPC) algorithm to obtain the dynamic model;

[0024] The kinetic model includes the following:

[0025]

[0026]

[0027]

[0028] where p∈R 3 represents the position coordinates of the dynamic model in the world coordinate system; m∈R represents the mass of the dynamic model (robot mass); g∈R 3 represents the acceleration due to gravity; I∈R 3 Represents the inertia tensor of the dynamic model; ω∈R 3 Represents the angular velocity of the robot; R∈R 3 × 3 Represents the rotation transformation matrix from the body coordinate system to the world coordinate system; [ω] × ∈R 3×3 Represents the antisymmetric matrix corresponding to the robot's angular velocity.

[0029] Preferably, in S3, the correction model is obtained by mathematically modeling the quadruped robot using a model predictive control (WBC) algorithm;

[0030] The modified model includes:

[0031]

[0032]

[0033]

[0034]

[0035] Wfr≥0

[0036] Where Q1 and Q2 represent the ground reaction force calculated by the MPC controller; f r MPC represents the ground reaction force calculated by the MPC controller; S f represents the floating basis selection matrix; W represents the augmented constraint matrix; δ fThe relaxation variable representing the floating base acceleration; Represents the relaxation variable of the ground reaction force on the floating foundation.

[0037] A control system for stable motion of a quadruped robot, using a control method for stable motion of a quadruped robot as described above, comprising:

[0038] Sampling module: used to sample the current state of the quadruped robot and the ground reaction force according to the sampling time interval, and optimize the sampled current state of the quadruped robot and the ground reaction force;

[0039] Prediction module: used to substitute the optimized current state and ground reaction force into the dynamic model of the quadruped robot to obtain the expected values ​​of the plantar reaction force of a series of sampling points in the prediction time domain;

[0040] Correction module: used for substituting the first expected value of plantar reaction force into the correction model of the quadruped robot to obtain the final expected value of plantar reaction force;

[0041] Control module: used for feeding back the final expected value of the plantar reaction force to the bottom-layer controller of the quadruped robot, and the bottom-layer controller is used for controlling the joint motors of the quadruped robot to make the quadruped robot move.

[0042] One of the above technical solutions has the following beneficial effects: after each sampling time interval, usually every 2 ms, the MPC will correct the current state of the quadruped robot and perform the next solution again. This rapid update of the current state of the quadruped robot and the solution optimization greatly improve the stability of the quadruped robot's movement and can cope with situations such as stepping into the air and sharp turns. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] Figure 1 It is a flow chart of a control method for stable motion of a quadruped robot according to the present invention;

[0044] Figure 2 It is a schematic diagram of the structure of a control system for stable motion of a quadruped robot according to the present invention; DETAILED DESCRIPTION

[0045] The technical solution of the present invention is further described below with reference to the accompanying drawings and through specific implementation methods.

[0046] like Figure 1 As shown, a control method for stable motion of a quadruped robot comprises the following steps:

[0047] S1. Sampling the current state and ground reaction force of the quadruped robot according to the sampling time interval, and optimizing the sampled current state and ground reaction force of the quadruped robot;

[0048] S2, substituting the optimized current state and ground reaction force into the dynamic model of the quadruped robot to obtain the expected values ​​of the plantar reaction force of a series of sampling points in the prediction time domain;

[0049] S3, substituting the first expected value of the plantar reaction force into the correction model of the quadruped robot to obtain the final expected value of the plantar reaction force;

[0050] S4. Feedback the final expected value of the plantar reaction force to the bottom-level controller of the quadruped robot, wherein the bottom-level controller is used to control the joint motors of the quadruped robot to enable the quadruped robot to move.

[0051] The MPC controller samples the current state and ground reaction force of the quadruped robot, and obtains the expected plantar reaction force of a series of sampling points in the prediction time domain through the correction model of the quadruped robot according to the state space equation of the robot and the current state. After obtaining a series of plantar reaction force expectations at the sampling point, the first one is selected as the expected plantar reaction force value and transmitted to the WBC controller. The WBC controller receives the expected plantar reaction force from the MPC, projects it in the null space of the Jacobian matrix of the task of maintaining body balance, and obtains the input of the secondary motion task through the correction model of the quadruped robot, so that the expected plantar reaction force solved by the MPC can be corrected and transmitted to the bottom controller as the final expected value, and then the bottom controller controls the joint motor of the quadruped robot to make the quadruped robot move.

[0052] Specifically, after each sampling time interval, usually every 2ms, the MPC will correct the current state of the system and perform the next solution again. This method of quickly updating the current state of the quadruped robot and solving the optimal input greatly improves the stability of the quadruped robot's movement and can cope with situations such as stepping into the air and making sharp turns.

[0053] To further illustrate, in S1, the current state of the quadruped robot is represented as:

[0054]

[0055] Among them, x i Represents the state vector of the system at the i-th sampling time point; and Respectively represent approximate values; It represents the Euler angle of the quadruped robot's body coordinate system in the world coordinate system; Represents the velocity of the quadruped robot's body coordinate system in the world coordinate system.

[0056] To further illustrate, in S1, the ground reaction force of the quadruped robot is expressed as:

[0057]

[0058] Where n is the number of robot legs; R z (ψ) represents the basic rotation matrix of the robot body around the z-axis, ψ represents the rotation angle; I represents the inertia tensor of the body in the world coordinate system; r i represents the vector from the contact point of the ith leg to the center of mass of the body; f i represents the ground reaction force on the i-th leg.

[0059] To further illustrate, in S1, the sampled current state of the quadruped robot and the ground reaction force are optimized by an optimization algorithm, and the optimization algorithm includes:

[0060]

[0061] subject to x i+1 =A i x i +B i u i , i=0...k-1

[0062]

[0063] D i u i =0, i=0...k-1

[0064] Among them, k represents the prediction time domain length; x i+1，ref represents the system state vector of the i+1th sampling point; Q i Represents the system state weight matrix; R i represents the system state weight matrix; u i represents the control input signal at sampling time i, i.e., the plantar reaction force f; c i Indicates the lower limit of plantar reaction force; Indicates the upper limit of plantar reaction force; C i represents the input signal inequality constraint matrix; D i It means that the plantar reaction force of the foot that is not in contact with the ground at time i is set to 0.

[0065] 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.

[0066] To further illustrate, in S2, the dynamic model is obtained by mathematically modeling the quadruped robot using a model predictive control MPC algorithm;

[0067] Because the mass of the legs accounts for less than 10% of the overall mass of the robot, the robot can be regarded as a single rigid body. The dynamic model in the world coordinate system includes the following:

[0068]

[0069]

[0070]

[0071] where p∈R 3 represents the position coordinates of the dynamic model in the world coordinate system; m∈R represents the mass of the dynamic model (robot mass); g∈R 3 represents the acceleration due to gravity; I∈R 3 Represents the inertia tensor of the dynamic model; ω∈R 3 Represents the angular velocity of the robot; R∈R 3 × 3 Represents the rotation transformation matrix from the body coordinate system to the world coordinate system; [ω] × ∈R 3×3 Represents the antisymmetric matrix corresponding to the robot's angular velocity.

[0072] Further explanation, in S3, the correction model is obtained by mathematically modeling the quadruped robot using a model predictive control (WBC) algorithm;

[0073] The modified model includes:

[0074]

[0075]

[0076]

[0077]

[0078] W r ≥0

[0079] Where Q1 and Q2 represent the ground reaction force calculated by the MPC controller; f r MPC represents the ground reaction force calculated by the MPC controller; S f represents the floating basis selection matrix; W represents the augmented constraint matrix; δ fThe relaxation variable representing the floating base acceleration; Represents the relaxation variable of the ground reaction force on the floating foundation.

[0080] The constraints solved by this modified model are the basic dynamic equations of the floating base system, the acceleration equation, the plantar reaction equation, and the contact force constraint. The acceleration equation takes the acceleration of the floating base model into consideration, making the robot more stable at high speeds. The plantar reaction equation also takes the reaction force on the floating base model into consideration, and 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.

[0081] like Figure 2 As shown, a control system for stable motion of a quadruped robot adopts a control method for stable motion of a quadruped robot as described above, comprising:

[0082] Sampling module: used to sample the current state of the quadruped robot and the ground reaction force according to the sampling time interval, and optimize the sampled current state of the quadruped robot and the ground reaction force;

[0083] Prediction module: used to substitute the optimized current state and ground reaction force into the dynamic model of the quadruped robot to obtain the expected values ​​of the plantar reaction force of a series of sampling points in the prediction time domain;

[0084] Correction module: used for substituting the first expected value of plantar reaction force into the correction model of the quadruped robot to obtain the final expected value of plantar reaction force;

[0085] Control module: used for feeding back the final expected value of the plantar reaction force to the bottom-layer controller of the quadruped robot, and the bottom-layer controller is used for controlling the joint motors of the quadruped robot to make the quadruped robot move.

[0086] Specifically, the sampling module is also used to execute the current state of the quadruped robot as:

[0087]

[0088] Among them, x i Represents the state vector of the system at the i-th sampling time point; and Respectively represent approximate values; It represents the Euler angle of the quadruped robot's body coordinate system in the world coordinate system; Represents the velocity of the quadruped robot's body coordinate system in the world coordinate system.

[0089] And the ground reaction force of the quadruped robot is expressed as:

[0090]

[0091] Where n is the number of robot legs; R z (ψ) represents the basic rotation matrix of the robot body around the z-axis, ψ represents the rotation angle; I represents the inertia tensor of the body in the world coordinate system; r i represents the vector from the contact point of the ith leg to the center of mass of the body; f i represents the ground reaction force on the i-th leg.

[0092] And the current state of the sampled quadruped robot and the ground reaction force are optimized by an optimization algorithm, wherein the optimization algorithm includes:

[0093]

[0094] subject to x i+1 =A i x i +B i u i , i=0...k-1

[0095]

[0096] D i u i =0, i=0...k-1

[0097] Among them, k represents the prediction time domain length; x i+1，ref represents the system state vector of the i+1th sampling point; Q i Represents the system state weight matrix; R i represents the system state weight matrix; u i represents the control input signal at sampling time i, i.e., the plantar reaction force f; c i Indicates the lower limit of plantar reaction force; Indicates the upper limit of plantar reaction force; C i represents the input signal inequality constraint matrix; D i It means that the plantar reaction force of the foot that is not in contact with the ground at time i is set to 0.

[0098] The prediction module is also used for the dynamic model to perform mathematical modeling on the quadruped robot by a model predictive control MPC algorithm to obtain the dynamic model;

[0099] The kinetic model includes the following:

[0100]

[0101]

[0102]

[0103] where p∈R 3 represents the position coordinates of the dynamic model in the world coordinate system; m∈R represents the mass of the dynamic model (robot mass); g∈R 3 represents the acceleration due to gravity; I∈R 3 Represents the inertia tensor of the dynamic model; ω∈R 3 Represents the angular velocity of the robot; R∈R 3×3 Represents the rotation transformation matrix from the body coordinate system to the world coordinate system; [ω] × ∈R 3×3 Represents the antisymmetric matrix corresponding to the robot's angular velocity.

[0104] The correction module is also used for the correction model to perform mathematical modeling on the quadruped robot by the model predictive control WBC algorithm to obtain the correction model;

[0105] The modified model includes:

[0106]

[0107]

[0108]

[0109]

[0110] W r ≥0

[0111] Where Q1 and Q2 represent the ground reaction force calculated by the MPC controller; f r MPC represents the ground reaction force calculated by the MPC controller; S f represents the floating basis selection matrix; W represents the augmented constraint matrix; δ f The relaxation variable representing the floating base acceleration; Represents the relaxation variable of the ground reaction force on the floating foundation.

[0112] The system of the present invention adopts the above-mentioned control method for stable movement of a quadruped robot, which can be actually applied to the quadruped robot, can improve the stability of the movement of the quadruped robot, and can cope with situations such as stepping into empty space and sharp turns.

[0113] The technical principle of the present invention is described above in conjunction with specific embodiments. These descriptions are only for explaining the principle of the present invention and cannot be interpreted as limiting the scope of protection of the present invention in any way. Based on the explanations herein, those skilled in the art can associate other specific embodiments of the present invention without creative work, and these equivalent variations or substitutions are all included in the scope defined by the claims of this application.

Claims

1. A control method for stable motion of a quadruped robot, characterized in that: The following steps are involved: S1. Sampling the current state and ground reaction force of the quadruped robot according to the sampling time interval, and optimizing the sampled current state and ground reaction force of the quadruped robot; S2, substituting the optimized current state and ground reaction force into the dynamic model of the quadruped robot to obtain the expected values ​​of the plantar reaction force of a series of sampling points in the prediction time domain; S3, substituting the first expected value of the plantar reaction force into the correction model of the quadruped robot to obtain the final expected value of the plantar reaction force; S4, feeding back the final expected value of the plantar reaction force to the bottom-layer controller of the quadruped robot, wherein the bottom-layer controller is used to control the joint motors of the quadruped robot to enable the quadruped robot to move; In S1, the current state of the sampled quadruped robot and the ground reaction force are optimized by an optimization algorithm, and the optimization algorithm includes: subject to x i+1 =A i x i +B i u i ,i=0…k-1 D i u i =0,i=0…k-1 Where k represents the prediction time domain length; x i+1,ref represents the system state vector of the i+1th sampling point; Q i Represents the system state weight matrix; R i represents the system state weight matrix; u i represents the control input signal at sampling time i, i.e., the plantar reaction force f; c i Indicates the lower limit of plantar reaction force; Indicates the upper limit of plantar reaction force; C i represents the input signal inequality constraint matrix; D i It means that the plantar reaction force of the foot that is not in contact with the ground at time i is set to 0; In S2, the dynamic model is mathematically modeled on the quadruped robot by a model predictive control (MPC) algorithm to obtain the dynamic model; The kinetic model includes the following: where p∈R 3 represents the position coordinates of the dynamic model in the world coordinate system; m∈R represents the mass of the dynamic model (robot mass); g∈R 3 represents the acceleration due to gravity; I∈R 3 Represents the inertia tensor of the dynamic model; ω∈R 3 Represents the angular velocity of the robot; R∈R 3×3 Represents the rotation transformation matrix from the body coordinate system to the world coordinate system; [ω] × ∈R 3×3 Represents the antisymmetric matrix corresponding to the robot's angular velocity; In S3, the correction model is mathematically modeled on the quadruped robot by a model predictive control (WBC) algorithm to obtain the correction model; The modified model includes: Wf r ≥0 Where Q1 and Q2 represent the ground reaction force calculated by the MPC controller; f r MPC represents the ground reaction force calculated by the MPC controller; S f represents the floating basis selection matrix; W represents the augmented constraint matrix; δ f The relaxation variable representing the floating base acceleration; Represents the relaxation variable of the ground reaction force on the floating foundation.

2. A method for controlling stable motion of a quadruped robot according to claim 1, characterized in that: In S1, the quadruped robot is: Among them, x i Represents the state vector of the system at the i-th sampling time point; and Respectively represent approximate values; It represents the Euler angle of the quadruped robot's body coordinate system in the world coordinate system; Represents the velocity of the quadruped robot's body coordinate system in the world coordinate system.

3. A method for controlling stable motion of a quadruped robot according to claim 2, characterized in that: In S1, the ground reaction force of the quadruped robot is expressed as: Where n is the number of robot legs; R z (ψ) represents the basic rotation matrix of the robot body around the z-axis, ψ represents the rotation angle; I represents the inertia tensor of the body in the world coordinate system; r i represents the vector from the contact point of the ith leg to the center of mass of the body; f i represents the ground reaction force on the i-th leg.

4. A control system for stable motion of a quadruped robot, characterized in that: A control method for stable motion of a quadruped robot as claimed in any one of claims 1 to 3, comprising: Sampling module: used to sample the current state of the quadruped robot and the ground reaction force according to the sampling time interval, and optimize the sampled current state of the quadruped robot and the ground reaction force; Prediction module: used to substitute the optimized current state and ground reaction force into the dynamic model of the quadruped robot to obtain the expected values ​​of the plantar reaction force of a series of sampling points in the prediction time domain; Correction module: used for substituting the first expected value of plantar reaction force into the correction model of the quadruped robot to obtain the final expected value of plantar reaction force; Control module: used for feeding back the final expected value of the plantar reaction force to the bottom-layer controller of the quadruped robot, and the bottom-layer controller is used for controlling the joint motors of the quadruped robot to make the quadruped robot move.

Images