Four-legged robot planning and control method and system based on center of mass dynamics model

Abstract

The application discloses a kind of four-legged robot planning and control method and system based on centroid dynamics model, by centroid dynamics model, and join constraint condition, four-legged robot support leg and swing leg are converted into nonlinear model prediction problem, so that the motion of support leg and swing leg can consider the state of centroid, so as to combine support leg and swing leg, realize the planning and control of four-legged robot, ensure the stability of four-legged robot as a whole, and then realize stable walking in various complex terrains.

Description

Technical Field

[0001] This invention relates to the field of quadruped robot control technology, and more specifically, to a quadruped robot planning and control method and system based on a center-of-mass dynamics model. Background Technology

[0002] To achieve coordinated control between legs in a legged robot, a unified gait needs to be planned. Generally, the legs exhibit two states during movement: a support phase and a swinging phase. The leg in the support phase is called the support leg, and the leg in the swinging phase is called the swinging leg; they alternate cyclically. The support leg is in contact with the ground, responsible for supporting the ground and providing support force to the robot body, ensuring the center of mass follows the desired trajectory. The swinging leg is not in contact with the ground and is responsible for swinging the foot to a suitable support and force application point, so that when switching to the support leg, the torque vector of the support leg on the ground can adapt to the robot's state. Common support leg and swinging leg control uses decoupling control, meaning that the state of the swinging leg is not considered when controlling the support leg, resulting in the swinging leg failing to achieve predictive capabilities. Similarly, independently controlling the swinging leg also ignores the state of the support leg. This control method leads to mutual interference between the support leg and the swinging leg, affecting the robot's stability and causing unnatural swinging leg movements during motion.

[0003] Existing technology provides a quadruped robot control method based on posture information fitting. The method includes the following steps: establishing a quadruped robot dynamic model; fitting the relationship between the center-of-mass acceleration and the foot reaction force according to the dynamic model; establishing the relationship between the base angular acceleration and the foot reaction force according to the dynamic model; designing a control law to track posture information; integrating the relationships between the center-of-mass acceleration and the foot reaction force, as well as the relationships between the base angular acceleration and the foot reaction force, to obtain a matrix form of a quadratic programming problem; combining the matrix form of the quadratic programming problem, constraint terms, and confidence gain to obtain the standard form of the quadratic programming problem, and solving the quadratic programming problem; controlling the quadruped robot's motion based on the solution results of the quadratic programming problem. Summary of the Invention

[0004] To overcome the shortcomings of incoordination between the supporting legs and swinging legs and high computational complexity in the above-mentioned quadruped robots, this invention provides a quadruped robot planning and control method and system based on the center of mass dynamics model.

[0005] To solve the above-mentioned technical problems, the technical solution of the present invention is as follows:

[0006] This invention provides a planning and control method for a quadruped robot based on a center-of-mass dynamics model, the method comprising:

[0007] S1: Establish the initial center of mass dynamics model of the quadruped robot;

[0008] S2: Based on the initial centroid dynamic model, perform transformation derivation to obtain the state vector and input vector of the initial centroid dynamic model;

[0009] S3: Based on the state vector and input vector of the initial centroid dynamic model, construct the complete centroid dynamic model of the quadruped robot;

[0010] S4: Based on the complete centroid dynamics model, state vector, and input vector, set the cost function and constraints;

[0011] S5: Construct a nonlinear model predictive control problem with the objective of minimizing the cost function under the constraints.

[0012] S6: Transform and solve the nonlinear model predictive control problem to obtain the input vector corresponding to the minimum cost function, which is used as the optimal expected input to control the quadruped robot.

[0013] Preferably, in S1, the initial center-of-mass dynamic model of the quadruped robot is:

[0014]

[0015] The generalized joint coordinates of a quadruped robot consist of virtual joint coordinates and active joint coordinates of the legs; each leg of a quadruped robot includes a swing leg and a support leg;

[0016]

[0017] Where q is the generalized joint coordinate; M(q) is the mass matrix; For generalized joint velocity; This refers to generalized joint acceleration; denoted by , where is the Coriolis force and centrifugal force matrix; g(q) is the Lagrangian static force matrix; S is the selection matrix for the active joint; τ is the generalized joint torque; J(q) c For contact Jacobian matrix; f c For the contact force at the foot; θ b For Euler angles; p b For virtual joint coordinates; q j For the active joint coordinates of the leg; For active joint linear velocity; f is the active joint angular velocity; c1 ,f c2 ,f c3 ,f c4 These are the contact forces at the four feet.

[0018] Preferably, in S2, the state vector and the input vector are:

[0019]

[0020] Where x is the state vector; u is the input vector; h com v is the center-of-mass momentum; j This represents the actual joint angular velocity.

[0021] Preferably, in S3, the complete center-of-mass dynamic model of the quadruped robot is:

[0022]

[0023] in, A complete center-of-mass dynamic model of a quadruped robot; A b Let A be the momentum matrix along the center of mass. j Here is the angular momentum matrix of the center of mass; For active joint linear velocity; For active joint angular velocity; The momentum of the center of mass line; r is the angular momentum of the center of mass. com,ci Let τ be the position of the i-th foot relative to the center of mass; ci Let be the torque of the i-th generalized joint.

[0024] Preferably, in S4, a cost function is set based on the complete centroid dynamics model, the state vector, and the input vector; the cost function is the second norm of the state error and the input error, specifically:

[0025]

[0026] Where, x ref The desired state; u ref The input is the desired input; the Q matrix is ​​the state weight matrix; the R matrix is ​​the input weight matrix.

[0027] Preferably, in S4, based on the complete centroid dynamic model, state vector, and input vector, constraints are set, including equality constraints and inequality constraints; the equality constraints include constraints for the supporting leg and the swing leg, and the equality constraints are as follows:

[0028]

[0029] Among them, v ci,ref Let C be the initial desired input trajectory for the swinging leg, and C be the set of supporting legs; v ci To support zero velocity at the foot and leg ends; f ci Let be the contact force at the i-th foot.

[0030] Preferably, in S4, the inequality constraints include active joint position constraints, active joint velocity constraints, and friction constraints between the foot end of the supporting leg and the ground, wherein the inequality constraints are:

[0031]

[0032] in, q j This is the position of minimum amplitude swing of the active joint; This is the position of maximum amplitude swing of the active joint; v j This represents the minimum speed of the active joint. f is the maximum velocity of the active joint; μ is the ground friction coefficient; f ci,z f is a scalar value in the z-direction; ci,x f is a scalar value in the x-direction; ci,y is a scalar value in the y-direction; ∈ is the friction cone smoothing coefficient.

[0033] Preferably, in S5, a nonlinear model predictive control problem is constructed with the objective of minimizing the cost function under the constraints. The nonlinear model predictive control problem is as follows:

[0034]

[0035] Where t0 is the initial time, t I Let x0 be the termination time and x0 be the initial state of the system. For system dynamics constraints, g(x(t),u(t),t)=0 is an equality constraint, and h(x(t),u(t),t)≥0 is an inequality constraint; From t0 to t I The minimum cost function in the shortest time.

[0036] Preferably, in S6, the nonlinear model predictive control problem is transformed into a nonlinear programming problem, and the nonlinear programming problem is solved using sequential quadratic programming. The quadratic programming subproblems in the sequential quadratic programming are solved using HPIPM. After solving, the input vector corresponding to the minimum cost function is obtained, which is used as the optimal expected input to control the quadruped robot.

[0037] This invention also provides a quadruped robot planning and control system based on a center-of-mass dynamics model to implement the above-mentioned method. The system includes:

[0038] The initial model building module establishes the initial center of mass dynamics model of the quadruped robot;

[0039] Vector acquisition module: Based on the initial centroid dynamic model, a transformation derivation is performed to obtain the state vector and input vector of the initial centroid dynamic model;

[0040] The complete model building module constructs a complete center-of-mass dynamic model of the quadruped robot based on the state vector and input vector of the initial center-of-mass dynamic model.

[0041] The constraint construction module sets the cost function and constraints based on the complete centroid dynamics model, state vector, and input vector.

[0042] The control problem construction module aims to minimize the cost function under constraints and constructs a nonlinear model to predict control problems.

[0043] The control module transforms and solves the nonlinear model predictive control problem to obtain the input vector corresponding to the minimum cost function, which is used as the optimal expected input to control the quadruped robot.

[0044] Compared with the prior art, the beneficial effects of the technical solution of the present invention are:

[0045] This invention uses a center-of-mass dynamics model and adds constraints to transform the support leg and swing leg of a quadruped robot into a nonlinear model prediction problem. This allows the motion of both the support leg and the swing leg to take into account the state of the center of mass, thereby combining the support leg and the swing leg to achieve planning and control of the quadruped robot, ensuring the overall stability of the quadruped robot, and thus enabling stable walking in various complex terrains. Attached Figure Description

[0046] Figure 1 This is a flowchart of a quadruped robot planning and control method based on a center-of-mass dynamics model as described in Example 1;

[0047] Figure 2 This is a schematic diagram of the planning and control system for a quadruped robot based on the center-of-mass dynamics model described in Example 3. Detailed Implementation

[0048] The accompanying drawings are for illustrative purposes only and should not be construed as limiting the scope of this patent.

[0049] To better illustrate this embodiment, some parts in the accompanying drawings may be omitted, enlarged, or reduced, and do not represent the actual product dimensions;

[0050] It will be understood by those skilled in the art that certain well-known structures and their descriptions may be omitted in the accompanying drawings.

[0051] The technical solution of the present invention will be further described below with reference to the accompanying drawings and embodiments.

[0052] Example 1

[0053] This embodiment provides a planning and control method for a quadruped robot based on a center-of-mass dynamics model, such as Figure 1 As shown, the method includes:

[0054] S1: Establish the initial center of mass dynamics model of the quadruped robot;

[0055] S2: Based on the initial centroid dynamic model, perform transformation derivation to obtain the state vector and input vector of the initial centroid dynamic model;

[0056] S3: Based on the state vector and input vector of the initial centroid dynamic model, construct the complete centroid dynamic model of the quadruped robot;

[0057] S4: Based on the complete centroid dynamics model, state vector, and input vector, set the cost function and constraints;

[0058] S5: Construct a nonlinear model predictive control problem with the objective of minimizing the cost function under the constraints.

[0059] S6: Transform and solve the nonlinear model predictive control problem to obtain the input vector corresponding to the minimum cost function, which is used as the optimal expected input to control the quadruped robot.

[0060] Example 2

[0061] This embodiment provides a planning and control method for a quadruped robot based on a center-of-mass dynamics model. The method includes:

[0062] S1: Establish the initial center of mass dynamics model of the quadruped robot;

[0063] The initial center-of-mass dynamic model of the quadruped robot is as follows:

[0064]

[0065] The generalized joint coordinate system of the quadruped robot consists of 6 virtual joint coordinates and 12 active joint coordinates of the legs; each leg of the quadruped robot includes a swing leg and a support leg;

[0066]

[0067] Where q is the generalized joint coordinate; M(q) is the mass matrix; For generalized joint velocity; This refers to generalized joint acceleration; denoted by , where is the Coriolis force and centrifugal force matrix; g(q) is the Lagrangian static force matrix; S is the selection matrix for the active joint; τ is the generalized joint torque; J(q) c For contact Jacobian matrix; f c For the contact force at the foot; θ b For Euler angles; p bFor virtual joint coordinates; q j For the active joint coordinates of the leg; For active joint linear velocity; f is the active joint angular velocity; c1 ,f c2 ,f c3 ,f c4 These are the contact forces at the four feet.

[0068] S2: Based on the initial centroid dynamic model, perform transformation derivation to obtain the state vector and input vector of the initial centroid dynamic model;

[0069] The state vector and input vector are:

[0070]

[0071] Where x is the state vector; u is the input vector; h com v is the center-of-mass momentum; j This represents the actual joint angular velocity.

[0072] S3: Based on the state vector and input vector of the initial centroid dynamic model, construct the complete centroid dynamic model of the quadruped robot;

[0073] The complete center-of-mass dynamic model of the quadruped robot is as follows:

[0074]

[0075] in, A complete center-of-mass dynamic model of a quadruped robot; A b Let A be the momentum matrix along the center of mass. j Here is the angular momentum matrix of the center of mass; For active joint linear velocity; For active joint angular velocity; The momentum of the center of mass line; r is the angular momentum of the center of mass. com,ci Let τ be the position of the i-th foot relative to the center of mass; ci Let be the torque of the i-th generalized joint.

[0076] To describe the effect of generalized joint velocity on the center of mass momentum, the center of mass momentum matrix A(q) is calculated using the dynamics library Pinocchio, resulting in:

[0077] A(q)=[A b (q)A j (q)]

[0078]

[0079] Substituting formulas (2) and (4) into (5) yields:

[0080]

[0081] The generalized joint velocity of an active joint is equivalent to the actual angular velocity v of the joint. j The angular velocity v of the joint j It can be read directly from the motor, therefore v j Given quantities, we have

[0082]

[0083] S4: Based on the complete centroid dynamics model, state vector, and input vector, set the cost function and constraints;

[0084] Based on the complete centroid dynamics model, state vector, and input vector, a cost function is set; the cost function is the second norm of the state error and the input error, specifically:

[0085]

[0086] Where, x ref The desired state; u ref The input is the desired input; the Q matrix is ​​the state weight matrix; the R matrix is ​​the input weight matrix.

[0087] Based on the complete centroid dynamics model, state vector, and input vector, constraints are set, including equality constraints and inequality constraints. The equality constraints include constraints for the supporting leg and the swing leg, and the equality constraints are as follows:

[0088]

[0089] Among them, v ci,ref Let C be the initial desired input trajectory for the swinging leg, and C be the set of supporting legs; v ci To support zero velocity at the foot and leg ends; f ci Let be the contact force at the i-th foot.

[0090] The inequality constraints include active joint position constraints, active joint velocity constraints, and friction constraints between the supporting leg foot and the ground. The inequality constraints are as follows:

[0091]

[0092] in, q j This is the position of minimum amplitude swing of the active joint; This is the position of maximum amplitude swing of the active joint; v j This represents the minimum speed of the active joint. f is the maximum velocity of the active joint; μ is the ground friction coefficient; f ci,z f is a scalar value in the z-direction; ci,x f is a scalar value in the x-direction; ci,y Let be a scalar value in the y-direction; ∈ is the friction cone smoothing coefficient, which requires ∈ > 0 to ensure that the friction cone constraint is within f. ci It provides continuity when the value is 0, while also offering a safety margin.

[0093] S5: Construct a nonlinear model predictive control problem with the objective of minimizing the cost function under the constraints.

[0094] With the objective of minimizing the cost function under satisfied constraints, a nonlinear model predictive control problem is constructed, which is as follows:

[0095]

[0096] Where t0 is the initial time, t I Let x0 be the termination time and x0 be the initial state of the system. For system dynamics constraints, g(x(t),u(t),t)=0 is an equality constraint, and h(x(t),u(t),t)≥0 is an inequality constraint; From t0 to t I The minimum cost function in the shortest time.

[0097] S6: Transform and solve the nonlinear model predictive control problem to obtain the input vector corresponding to the minimum cost function, which is used as the optimal expected input to control the quadruped robot.

[0098] The nonlinear model predictive control problem is transformed into a nonlinear programming problem. This nonlinear programming problem is solved using sequential quadratic programming. The quadratic programming subproblems in the sequential quadratic programming are solved using HPIPM. The input vector corresponding to the minimum cost function is obtained after solving the problem and is used as the optimal expected input to control the quadruped robot.

[0099] Example 3

[0100] This embodiment also provides a quadruped robot planning and control system based on a center-of-mass dynamics model, used to implement the method of Embodiment 1 or 2, such as... Figure 2 As shown, the system includes:

[0101] The initial model building module establishes the initial center of mass dynamics model of the quadruped robot;

[0102] Vector acquisition module: Based on the initial centroid dynamic model, a transformation derivation is performed to obtain the state vector and input vector of the initial centroid dynamic model;

[0103] The complete model building module constructs a complete center-of-mass dynamic model of the quadruped robot based on the state vector and input vector of the initial center-of-mass dynamic model.

[0104] The constraint construction module sets the cost function and constraints based on the complete centroid dynamics model, state vector, and input vector.

[0105] The control problem construction module aims to minimize the cost function under constraints and constructs a nonlinear model to predict control problems.

[0106] The control module transforms and solves the nonlinear model predictive control problem to obtain the input vector corresponding to the minimum cost function, which is used as the optimal expected input to control the quadruped robot.

[0107] The same or similar labels correspond to the same or similar parts;

[0108] The terms used to describe positional relationships in the accompanying drawings are for illustrative purposes only and should not be construed as limiting this patent.

[0109] Obviously, the above embodiments of the present invention are merely examples for clearly illustrating the present invention, and are not intended to limit the implementation of the present invention. Those skilled in the art will recognize that other variations or modifications can be made based on the above description. It is neither necessary nor possible to exhaustively describe all embodiments here. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the scope of protection of the claims of the present invention.

Claims

1. A method for planning and control of a quadruped robot based on a center of mass dynamics model, characterized in that, The method comprises: S1: establishing an initial center-of-mass dynamics model of the quadruped robot; S2: transforming and deriving according to the initial center-of-mass dynamics model to obtain a state vector and an input vector of the initial center-of-mass dynamics model; S3: constructing a complete center-of-mass dynamics model of the quadruped robot according to the state vector and the input vector of the initial center-of-mass dynamics model; S4: setting a cost function and a constraint condition based on the complete center-of-mass dynamics model, the state vector and the input vector; S5: constructing a nonlinear model predictive control problem with minimization of the cost function as the target under the constraint condition; S6: solving the nonlinear model predictive control problem to obtain an input vector corresponding to the minimum cost function as an optimal expected input to control the quadruped robot; In S3, the complete center-of-mass dynamics model of the quadruped robot is: wherein, is the state vector, is the input vector, is the complete center of mass dynamics model of the quadruped robot; is the center of mass linear momentum matrix; is the center of mass angular momentum matrix; is the active joint linear velocity; is the active joint angular velocity; is the center of mass linear momentum; is the center of mass angular momentum; is the position of the foot end relative to the center of mass; is the i-th generalized joint torque, is the foot end contact force, , is the center of mass momentum, is the actual joint angular velocity; In S4, the cost function is set based on the complete center-of-mass dynamics model, the state vector and the input vector; the cost function is a quadratic norm of state error and input error, specifically: wherein, is a desired state; is a desired input; is a state weight matrix; is an input weight matrix.

2. The method of claim 1, wherein, In S1, the initial center-of-mass dynamics model of the quadruped robot is: The generalized joint coordinates of the quadruped robot are composed of virtual joint coordinates and leg active joint coordinates; each leg of the quadruped robot includes a swing leg and a support leg; where, is the generalized joint coordinate; is the mass matrix; is the generalized joint velocity; is the generalized joint acceleration; is the Coriolis and centrifugal force matrix; is the Lagrangian static force matrix; is the selection matrix of active joints; is the generalized joint torque; is the contact Jacobian matrix; is the foot end contact force; is the Euler angle; is the virtual joint coordinate; is the leg active joint coordinate; is the active joint linear velocity; is the active joint angular velocity; is the contact force of the four foot ends, respectively.

3. The method of claim 2, wherein, In S2, the state vector and the input vector are: wherein, is the state vector; is the input vector; is the center of mass momentum; is the actual joint angular velocity.

4. The method of claim 1, wherein, In S4, the constraint condition is set based on the complete center-of-mass dynamics model, the state vector and the input vector; the constraint condition includes an equality constraint and an inequality constraint; the equality constraint includes a support leg and a swing leg constraint, and the equality constraint is: wherein, is an initial desired input trajectory for the swing leg, is a set of support legs; is a foot end zero velocity for the support leg; is a contact force for the i-th foot end.

5. The method of claim 4, wherein, In S4, the inequality constraint includes an active joint position constraint, an active joint velocity constraint and a friction constraint between the support leg foot end and the ground, and the inequality constraint is: wherein, is the minimum swing position of the active joint; is the maximum swing position of the active joint; is the minimum velocity of the active joint; is the maximum velocity of the active joint; is the ground friction coefficient, is the scalar value in the z direction; is the scalar value in the x direction; is the scalar value in the y direction; is the friction cone smoothing coefficient.

6. The method of planning and control for a quadruped robot based on a center of mass dynamics model of claim 5, wherein, In S5, the nonlinear model predictive control problem is constructed with minimization of the cost function as the target under the constraint condition; the nonlinear model predictive control problem is: where, is the initial time, is the final time, is the initial state of the system, is the system dynamics constraint, is the equality constraint, is the inequality constraint; is to minimum cost function in the shortest time.

7. The method of claim 1, wherein, In S6, the nonlinear model predictive control problem is transformed into a nonlinear programming problem, the nonlinear programming problem is solved by using a sequential quadratic programming, a quadratic programming sub-problem in the sequential quadratic programming is solved by using HPIPM, and an input vector corresponding to the minimum cost function is obtained after the solving as an optimal expected input to control the quadruped robot.

8. A centroidal dynamics model based quadruped robot planning and control system for implementing the method of any one of claims 1-7, characterized by, The system comprises: An initial model construction module that establishes an initial center-of-mass dynamics model of the quadruped robot; A vector acquisition module that transforms and derives according to the initial center-of-mass dynamics model to obtain a state vector and an input vector of the initial center-of-mass dynamics model; A complete model construction module that constructs a complete center-of-mass dynamics model of the quadruped robot according to the state vector and the input vector of the initial center-of-mass dynamics model; A constraint condition construction module that sets a cost function and a constraint condition based on the complete center-of-mass dynamics model, the state vector and the input vector; A control problem construction module that constructs a nonlinear model predictive control problem with minimization of the cost function as the target under the constraint condition; The control module transforms and solves the nonlinear model predictive control problem, obtains the input vector corresponding to the minimum cost function, and controls the quadruped robot as the optimal expected input.

Images