A whole-body coordinated control method for a quadruped robot based on constraint and motion decoupling

Abstract

The present invention provides a whole-body coordinated control method of a quadruped robot based on constraint and motion decoupling, which relates to the field of robot control technology, and includes: step 1: establishing a dynamic model; step 2: obtaining a motion space equation by multiplying the dynamic model with the null space mapping matrix of the constraint matrix; step 3: obtaining a control rate that makes the system exhibit impedance characteristics; step 4: converting the control rate in the Cartesian space into a joint driving torque by Jacobi mapping, step 5: constructing a quadratic programming problem in the constraint space; step 6: obtaining the joint driving torque in the constraint space, and adding it to the joint torque obtained in the motion space as an instruction sent to each driving joint motor. The present invention realizes trajectory tracking control and compliant control, thereby maintaining accurate motion control during walking, and having a certain degree of compliance to adapt to uneven ground, realizing whole-body coordinated control, and making the robot walk smoothly without slipping.

Description

Technical Field

[0001] The present invention relates to the field of robot control technology, and in particular to a whole-body coordinated control method of a quadruped robot based on constraint and motion decoupling. Background Art

[0002] Legged robots have received extensive attention from academia and industry due to their strong ability to adapt to terrain. Quadruped robots are the most promising type of legged robots, because they are not only more flexible and faster than hexapod robots, but also more stable than biped robots. Quadruped robots have the potential to be widely used in industrial inspections, security inspections and other fields. Boston Dynamics in the United States has already applied quadruped robots to building surveying and security patrols.

[0003] Although the existing quadruped robot prototypes can walk or even run quickly, there is still no industry-recognized optimal control method. Quadruped robots are floating base systems that are in contact with the environment. To ensure that the robot's feet do not slip, the friction cone constraint must be satisfied at the contact point. The mathematical form of this constraint is an inequality constraint. Therefore, in order to obtain a suitable control rate, it is necessary to solve a constrained optimization problem that includes the friction cone inequality constraint. Since the system composed of the supporting legs, the ground, and the fuselage of the quadruped robot is a redundant drive system when standing, static walking (only one leg per step), and diagonal trotting, it is also necessary to construct a constrained optimization problem to calculate the optimal control rate. Therefore, optimal control has become the mainstream method for quadruped robot motion control.

[0004] However, the current optimal control methods for quadruped robots mostly use simplified dynamic models as equation constraints, resulting in low control accuracy. In addition, the speed of solving large-scale constrained optimization problems is slow, which requires the onboard control computer to have very high hardware configuration, thereby increasing the hardware cost of the robot. Summary of the invention

[0005] According to the above technical problems that the optimal control method of the existing quadruped robot has low control accuracy and slow solution speed, a whole-body coordinated control method of a quadruped robot based on constraint and motion decoupling is provided. The present invention mainly solves a small-scale constrained optimization problem, thereby achieving the technical effect of improving the solution speed and reducing the hardware requirements.

[0006] The technical means adopted by the present invention are as follows:

[0007] A whole-body coordinated control method for a quadruped robot based on constraint and motion decoupling comprises the following steps:

[0008] Step 1: Establish the dynamic model of the quadruped robot;

[0009] Step 2: The dynamic model of the quadruped robot is multiplied by the null space mapping matrix of the constraint matrix to obtain the motion space equation, and the difference between the original dynamic equation and the motion space equation is the constraint space equation;

[0010] Step 3: In the motion space, transform the dynamic equations of the joint space into the Cartesian space to obtain the control rate that makes the system exhibit impedance characteristics;

[0011] Step 4: Convert the control rate in Cartesian space into joint driving torque through Jacobi mapping;

[0012] Step 5: Solve the acceleration of the system and then transfer the driving torque obtained in the motion space to the constraint space, and then construct a quadratic programming problem in the constraint space;

[0013] Step 6: Solve the quadratic programming problem to obtain the joint driving torque in the constraint space, and add it to the joint torque obtained in the motion space in step 4 as the instruction sent to each driving joint motor.

[0014] Furthermore, in step 2, the null space mapping matrix of the constraint matrix is ​​used to decompose the system dynamic equation into a decoupled motion space part and a constraint space part.

[0015] Furthermore, the trajectory tracking and compliance control in the motion space in step 3 both use analytical motion control rates, which are expressed as follows:

[0016]

[0017] Where: i = b or s, b is the fuselage, s is the swing leg, D d,i is the desired motion damping, K d,i is the desired elastic modulus, e i is the position or attitude error in Cartesian space, Λ c,i is the constraint-consistent inertia matrix of the Cartesian space corresponding to the swinging leg or the fuselage, h c,i are the gravity term, Coriolis force and centrifugal force terms of the system in Cartesian space.

[0018] Furthermore, in step 5, the quadratic programming problem includes a friction cone, upper and lower limits of torque, and unilateral constraints, and the objective function of the quadratic programming problem is in the form of minimizing the sum of squares of driving torques in the constraint space to obtain the driving torque with the minimum energy consumption.

[0019] Furthermore, the kinetic model in step 1 is:

[0020]

[0021] in, is the robot's degree of freedom variable, where is the 6 degrees of freedom posture of the fuselage, are the angle variables of the 12 joints, is the inertia matrix of the entire robot, are gravity, Coriolis force and centrifugal force, is the joint torque, is the constraint Jacobian matrix.

[0022] Furthermore, in step 6, a quadratic programming solution algorithm such as an interior point method is used to solve the quadratic programming problem.

[0023] Compared with the prior art, the present invention has the following advantages:

[0024] The present invention maps the system dynamics equation to the constraint space and the motion space through null space mapping, thereby realizing the decoupling of constraint control and motion control. In the constraint space, a quadratic optimization problem is constructed to satisfy the inequality constraints such as the friction cone. Since only a small-scale constraint optimization problem needs to be solved, the solution speed can be improved and the hardware requirements can be reduced. At the same time, since there are no inequality constraints in the motion space, the motion control rate can be obtained by the dynamics equation to realize trajectory tracking control and compliance control, so that accurate motion control is maintained during walking, and a certain degree of compliance is provided to adapt to the uneven ground and realize the coordinated control of the whole body.

[0025] The present invention decomposes the problem into constraint space and motion space through null space mapping, so that the motion control rate in analytical form can be obtained in the motion space, and in the constraint space, only one quadratic programming problem needs to be solved to satisfy various physical inequality constraints, so that the robot can track the desired motion trajectory well while avoiding slipping and falling. The semi-analytical optimization feature of this method greatly reduces the hardware requirements for the controller, so it can be deployed on an Arm chip-level processor to run, and the control frequency can also reach 1000Hz, without having to run on an X86 architecture such as a computer equipped with an Intel processor. BRIEF DESCRIPTION OF THE DRAWINGS

[0026] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0027] Figure 1 It is a control block diagram of the present invention.

[0028] Figure 2 This is an illustration of an application example of the present invention. DETAILED DESCRIPTION

[0029] It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments can be combined with each other. The present invention will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0030] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. The following description of at least one exemplary embodiment is actually only illustrative and is by no means intended to limit the present invention and its application or use. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0031] It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit exemplary embodiments according to the present invention. As used herein, unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. In addition, it should be understood that when the terms "comprising" and / or "including" are used in this specification, it indicates the presence of features, steps, operations, devices, components and / or combinations thereof.

[0032] Unless otherwise specifically stated, the relative arrangement of the parts and steps described in these embodiments, the numerical expressions and numerical values ​​do not limit the scope of the present invention. At the same time, it should be clear that, for ease of description, the sizes of the various parts shown in the drawings are not drawn according to the actual proportional relationship. The technology, methods and equipment known to ordinary technicians in the relevant field may not be discussed in detail, but in appropriate cases, the technology, methods and equipment should be regarded as part of the authorization specification. In all examples shown and discussed here, any specific value should be interpreted as merely exemplary, rather than as a limitation. Therefore, other examples of exemplary embodiments may have different values. It should be noted that similar numbers and letters represent similar items in the following drawings, so once an item is defined in one drawing, it does not need to be further discussed in subsequent drawings.

[0033] like Figure 1 As shown, the present invention provides a whole-body coordinated control method of a quadruped robot based on constraint and motion decoupling, which specifically includes the following steps:

[0034] Step 1: Figure 2 The dynamic equation of the quadruped robot in can be obtained by the Newton-Euler modeling method, and the dynamic equation can be expressed as

[0035]

[0036] in represents the robot's degree of freedom variables, where Represents the six degrees of freedom of the fuselage position, Represents the angle variables of 12 joints, Represents the inertia matrix of the entire robot, hereinafter abbreviated as M, represents gravity, Coriolis force and centrifugal force, hereinafter abbreviated as h, Represents the joint torque. It is worth noting that the first six elements of τ are always 0, indicating that the 6 degrees of freedom of the fuselage are not driven. Represents the constrained Jacobian matrix, hereinafter abbreviated as J c , where 3k means there are k feet in contact with the ground, and each contact point provides 3 force constraints, so the contact force vector at last

[0037]

[0038] Where I represents the identity matrix. Other forms of quadruped robots can also use the system's dynamic equations. If the number of driven joints is not 12, just change q j The dimension of .

[0039] Step 2: Through the constraint matrix J c The null space mapping matrix The dynamic equation of the quadruped robot can be decomposed into two parts. The first part is the dynamic equation of the unconstrained space, which can also be called the motion space. Its expression is multiplied by P, which is

[0040]

[0041] where τ m This is the torque required for motion control, such as Figure 1 As shown. By subtracting, we can get the dynamic equation of the constraint space

[0042]

[0043] where τ c That is, the driving torque required for the constraint space to satisfy the constraint conditions, such as Figure 1 shown.

[0044] Step 3: By transforming to Cartesian space, the dynamic equations in Cartesian space can be obtained, so that the fuselage and the swing foot can be impedance controlled respectively, and the constraint inertia matrix M can be defined c =PM+IP, note M c It is reversible, multiply both sides by Where i = b or s, b represents the fuselage, s represents the swinging leg, then we can get

[0045]

[0046] From the kinematic relationship, we know that the end acceleration exists between the joint velocity and acceleration , so we can use In the substitution And multiply both sides Available

[0047]

[0048] where Λ c,i is the constraint-consistent inertia matrix of the Cartesian space corresponding to the swinging leg or fuselage, and is the driving force of Cartesian space, let It can be simplified to

[0049]

[0050] The above equation is the dynamic equation of the swinging leg or the body in the Cartesian space of the motion space. Therefore, the impedance control rate can be constructed so that the system receives the external contact force or torque F x,i When the spring damping characteristic

[0051]

[0052] where e i is the position or attitude error in Cartesian space, D d,i and K d,i are the desired damping and elastic coefficients, respectively, which can be set by the programmer according to actual needs. Figure 2 The example in also expresses the effect of the spring damping impedance controller. By the can, the control rate of the impedance characteristic that can be achieved is

[0053]

[0054] The method can not only track the terminal trajectory (fuselage and swing foot), but also realize the impedance compliance characteristics after being subjected to external forces. The degree of compliance is determined by D d,i and K d,i Decide.

[0055] Step 4: This step requires converting the control rate in Cartesian space into joint torque and merging the torque of each task. For a quadruped robot, the motion control tasks in the motion space only include the control of the body and the motion control of the swinging legs. There are two options for merging them. One is to give them the same priority, and the other is to prioritize the control of the body and the swinging legs so that high-priority tasks are not affected by low-priority tasks, reflecting the coordinated control characteristics. If you expect the swinging legs and the body to have the same priority, use the following equation to get the joint control torque command in the motion space.

[0056]

[0057] where τ m That is Figure 1 The τ shown in m , which is determined by the control amount τ of the swing leg s and the control amount τ of the fuselage b If you want the swing legs and the fuselage to have different priorities, such as making the priority of the swing legs higher than that of the fuselage, you can add the null space mapping matrix to achieve this.

[0058]

[0059] in is the null space mapping matrix of the swing leg.

[0060] Step 5: This step aims to construct a quadratic programming problem to obtain the optimal control torque τ in the constraint space that satisfies the friction cone and other inequality constraints. c .because M c =PM+IP is brought into the dynamic equation of motion space to obtain

[0061]

[0062] That is, the robot's motion is completely determined by the torque command τ in the motion space m Substituting the formula into the dynamic equation of the constraint space, we can get

[0063]

[0064] where λ c =[λ x,1 λ y,1 λ z,1 …λ x,k λ y,k λ z,k ] T , including the force at each supporting foot and ground contact point [λ x λ y λz ]. By abbreviation we can get

[0065] λ c =η-(J c ) + τ c (1.14)

[0066] in

[0067] The contact foot of the quadruped robot must satisfy the friction cone constraint

[0068]

[0069] The unilateral constraint condition must also be met, that is, the robot's foot can only exert force on the ground, and there is no way to generate upward force, that is,

[0070] λ z ≥0(1.16)

[0071] In addition, the joint torque of the robot is not infinite and must satisfy the maximum torque constraint, that is,

[0072] τ min ≤τ≤τ max (1.17)

[0073] Therefore, the following quadratic programming problem can be constructed to obtain the constraint space moment command τ that satisfies the above inequality constraints: c

[0074]

[0075] The objective function is τ c The sum of squares is the smallest, which can be understood as minimizing the consumption capacity and achieving the optimal energy effect.

[0076] Step 6: Use the quadratic programming solution algorithm such as the interior point method to solve the equation and obtain τ c The optimal solution satisfies the physical inequality constraints, and, so that the robot does not slip and can walk normally. At the same time, because it is in the constraint space, and the constraint space and motion control are decoupled through the zero space mapping matrix, τ c Nor will it interfere with τ m The movement generated by the Figure 1 As shown in , the torque command sent to each joint motor is calculated as follows

[0077] τ=τ m +τ c (1.19)

[0078] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A whole-body coordinated control method for a quadruped robot based on constraint and motion decoupling, characterized in that: The steps include: Step 1: Establish the dynamic model of the quadruped robot; The kinetic model is: in, is the robot's degree of freedom variable, where is the 6 degrees of freedom posture of the fuselage, are the angle variables of the 12 joints, is the inertia matrix of the entire robot, are gravity, Coriolis force and centrifugal force, is the joint torque, is the constraint Jacobian matrix, is the contact force vector, and the expression of B is: ； Where I represents the identity matrix; Step 2: The dynamic model of the quadruped robot is multiplied by the null space mapping matrix of the constraint matrix to obtain the motion space equation, and the difference between the original dynamic equation and the motion space equation is the constraint space equation; The null space mapping matrix is: The motion space equation is: in, That is the torque required for motion control; The constraint space equation is: in, That is, the driving torque required for the constraint space to satisfy the constraint conditions; Step 3: In the motion space, transform the dynamic equations of the joint space into the Cartesian space to obtain the control rate that makes the system exhibit impedance characteristics; Step 4: Convert the control rate in Cartesian space into joint driving torque through Jacobi mapping; When you want to differentiate the priorities between the swinging legs and the fuselage, and make the priority of the swinging legs higher than that of the fuselage, then add the null space mapping matrix: in, , is the null space mapping matrix of the swing leg; is the joint torque of the swinging leg, is the joint moment of the fuselage; Step 5: Solve the acceleration of the system and then transfer the driving torque obtained in the motion space to the constraint space, and then construct a quadratic programming problem in the constraint space; In order to obtain the optimal control torque in the constraint space that satisfies the friction cone inequality constraint ;because , the constraint inertia matrix Substituting the dynamic equation into the motion space, we can get: That is, the robot's motion is completely determined by the torque command in the motion space. Decide; Step 6: Solve the quadratic programming problem to obtain the joint driving torque in the constraint space, and add it to the joint torque obtained in the motion space in step 4 as the instruction sent to each driving joint motor.

2. The whole-body coordinated control method of a quadruped robot based on constraint and motion decoupling according to claim 1 is characterized in that: In step 2, the null space mapping matrix of the constraint matrix is ​​used to decompose the system dynamic equation into a decoupled motion space part and a constraint space part.

3. The whole-body coordinated control method of a quadruped robot based on constraint and motion decoupling according to claim 1 is characterized in that: In step 3, both trajectory tracking and compliance control in the motion space use analytical motion control rates, which are expressed as follows: Among them: F i is the dynamic equation in Cartesian space, or , b is the fuselage, s is the swing leg, is the desired motion damping, is the desired elastic modulus, is the position or attitude error in Cartesian space, is the constrained consistent inertia matrix of the Cartesian space corresponding to the swing leg or the fuselage, are the gravity term, Coriolis force and centrifugal force terms of the system in Cartesian space.

4. The whole-body coordinated control method of a quadruped robot based on constraint and motion decoupling according to claim 1 is characterized in that: In step 5, the quadratic programming problem includes a friction cone, upper and lower limits of torque, and unilateral constraints. The objective function of the quadratic programming problem is to minimize the sum of squares of the driving torques in the constraint space to obtain the driving torque with the minimum energy consumption.

5. The whole-body coordinated control method of a quadruped robot based on constraint and motion decoupling according to claim 1, characterized in that: In step 6, the quadratic programming problem is solved by using an interior point method quadratic programming solution algorithm.

Images