CN116872219A - Robot control method, electronic device and storage medium based on U-K equation - Google Patents

Abstract

The application relates to a robot control method based on a U-K equation, electronic equipment and a storage medium, wherein the method comprises the following steps: carrying out forward kinematic modeling and dynamic modeling on the mechanical arm to obtain a dynamic equation; processing the dynamics equation to obtain the relation between the acceleration of the system and the external force of the system in an unconstrained state; adding system constraint to the relation between the acceleration of the system and the external force of the system in the constraint state; solving constraint force meeting system constraint through a U-K equation to obtain an accurate expression of the constraint force; and calculating an accurate motion equation of the system motion through an accurate expression of the constraint force, and realizing accurate control of a mechanical system. According to the application, the U-K controller based on the U-K equation is added in the system dynamics solving part, so that the control precision, effect and quick response of the system are improved.

Description

Robot control method, electronic device and storage medium based on U-K equation

Technical field

The present invention relates to the field of robot control technology, and in particular to a robot control method, electronic equipment and storage medium based on the U-K equation.

Background technique

At present, commonly used motor control methods in the industrial field include control based on PID regulators, improved PID control methods, robust control, adaptive control, synovial membrane control, etc. Among them, PID control is the most common control method with many applications. It usually consists of three parts: proportional link (P), integral link (I), and differential link (D). Adjusting the parameters of these three parts can make the motor The speed is stable near the set speed. In current research, the mechanical characteristics of complex multi-lift mechanisms/mechanical systems will continue to change. Furthermore, in the field of medical rehabilitation, rehabilitation machinery often comes into contact with the patient's limbs, which makes traditional control methods unable to adapt to multi-lift mechanisms. The working conditions change, and the high-speed and high-precision working requirements cannot be guaranteed.

At the same time, since traditional dynamic modeling of mechanical systems uses the Lagrangian dynamic modeling method based on D'Alembert's principle, using this method will simplify and idealize the relevant conditions and limitations of the system. , thus causing errors and affecting the accuracy of the model. In the fields of industrial production, robot control, medical rehabilitation and other fields, the actual complex and changeable environmental conditions and mechanical structures will also bring more uncertain factors to traditional trajectory tracking. When the system has non-ideal constraints, the traditional dynamic modeling method based on D'Alembert's principle will not be able to solve the problem.

Contents of the invention

In order to achieve the above objects and other advantages of the present invention, the first object of the present invention is to provide a robot control method based on the U-K equation, which includes the following steps:

Perform forward kinematics modeling and dynamics modeling on the robotic arm to obtain the dynamics equation;

Process the dynamic equations to obtain the relationship between the acceleration of the system and the external force of the system in the unconstrained state;

Add system constraints to the relationship between the acceleration of the system and the external force of the system in the constrained state;

Solve the binding force that satisfies the system constraints through the Udwadia-Kalaba equation and obtain the exact expression of the binding force;

The precise motion equation of the system motion is calculated through the precise expression of the binding force, thereby achieving precise control of the mechanical system.

Further, the forward kinematics modeling and dynamics modeling of the robotic arm includes the following steps:

Methods based on spinor algebra, Lie groups and Lie algebras are used to model the forward kinematics of the robotic arm;

The Lagrangian dynamics method is used for dynamic modeling and the dynamic equations are obtained.

Furthermore, the constraint equation of the system is:

;

Among them, matrix is the constraint matrix of the system,/> is the acceleration vector,/> is the constraint vector.

Further, solving the binding force that satisfies the system constraints through the Udwadia-Kalaba equation includes the following steps:

For a six-degree-of-freedom manipulator, the expression of its dynamic equation is:

;

in, is the inertia matrix of the system,/> is the Coriolis force,/> is gravity,/> Input for system control;

The constraints on the system are:

;

The controller and control torque given based on the Udwadia-Kalaba equation are:

;

in, Represents the Mooore-Penrose inverse of a matrix.

Further, the following steps are included:

Add uncertainty parameters in the controller given based on the Udwadia-Kalaba equation;

Decompose matrices, vectors and inverses containing uncertainty parameters;

Design a robust controller based on the Udwadia-Kalaba equation.

Further, by adding uncertainty parameters to the controller given based on the Udwadia-Kalaba equation, the obtained controller is:

;

in, is an unknown quantity.

Further, by decomposing the matrix, vector and inverse containing uncertainty parameters, we obtain:

;

in, , /> , /> is the nominal part, and/> ,/> ,/> ,/> It contains an uncertain part;

It also includes the following formula:

;

.

Further, the design of a robust controller based on the U-K equation includes:

;

in,

;

;

in, is a scalar, a parameter to be designed; select function/> ,make/> ;

Assuming three conditions, for any Full rank:

There is a function , making all/> satisfy ;

for a given and/> , make/> ;

There is a constant , make/> .

Furthermore, it also includes system stability analysis steps:

Use the selection requirements of Lyapunov asymptotic stability function to select a legal Lyapunov function;

Analysis based on Lyapunov's stability determination theorem proves that the robust controller satisfies consistent boundedness and consistent ultimate boundedness, and the control system is asymptotically stable.

A second object of the present invention is to provide an electronic device, including: a memory having program code stored thereon; a processor coupled to the memory, and when the program code is executed by the processor, the Robot control method based on U-K equation.

The third object of the present invention is to provide a computer-readable storage medium on which program instructions are stored. When the program instructions are executed, a robot control method based on the U-K equation is implemented.

Compared with the prior art, the beneficial effects of the present invention are:

The invention provides a robot control method, electronic equipment and storage medium based on the U-K equation. A U-K controller based on the U-K equation is added to the system dynamics solution part, thereby improving the control accuracy, effect and rapid response of the system. At the same time, a U-K robust controller is added that can converge to the inherent uncertain factors; the design of the robust controller not only improves the control effect of the system, but also maintains the control effect unchanged in the face of complex changes in the mechanical system, greatly reducing the It reduces the control cost of the motor when used by different patients, and there is no need to redesign parameters.

The above description is only an overview of the technical solutions of the present invention. In order to have a clearer understanding of the technical means of the present invention and implement them according to the contents of the description, the preferred embodiments of the present invention are described in detail below with reference to the accompanying drawings. Specific embodiments of the present invention are given in detail by the following examples and accompanying drawings.

Description of the drawings

The drawings described here are used to provide a further understanding of the present invention and constitute a part of this application. The illustrative embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention. In the attached picture:

Figure 1 is a flow chart of the robot control method based on the U-K equation in Embodiment 1;

Figure 2 is a schematic diagram of the electronic device of Embodiment 2;

Figure 3 is a schematic diagram of the storage medium in Embodiment 3.

Detailed ways

Below, the present invention will be further described with reference to the accompanying drawings and specific embodiments. It should be noted that, on the premise that there is no conflict, the various embodiments or technical features described below can be arbitrarily combined to form new embodiments. .

Example 1

The robot control method based on the U-K equation, as shown in Figure 1, includes the following steps:

S1. Perform forward kinematics modeling and dynamic modeling on the robotic arm to obtain the dynamic equation; specifically, the following steps are included:

Methods based on spinor algebra, Lie groups and Lie algebras are used to model the forward kinematics of the robotic arm, providing a mathematical basis for subsequent trajectory planning;

The Lagrangian dynamics method is used for dynamic modeling and the basic dynamic equations are obtained.

S2. Process the obtained dynamic equations to obtain the relationship between the acceleration of the system and the external force of the system in the unconstrained state;

S3. Add system constraints to the relationship between the acceleration of the system and the external force of the system in the constrained state; the constraints of the system generally come from the system's own mechanical structure and the trajectory constraints generated by the trajectory that satisfies the kinematic equations. Both the ideal constraints and non-ideal constraints of the system can be written in the form of the following constraint equations.

The constraint equation of the system is:

;

Among them, matrix is the constraint matrix of the system,/> is the acceleration vector,/> is the constraint vector.

S4. Use the Udwadia-Kalaba equation to solve the binding force that satisfies the system constraints and obtain the precise expression of the binding force;

S5. Calculate the precise motion equation of the system motion through the precise expression of the binding force to achieve precise control of the mechanical system.

Among them, solving the binding force that satisfies the system constraints through the Udwadia-Kalaba equation includes the following steps:

The main content of the Udwadia-Kalaba equation is that at any time when the constraints are satisfied In the system,/> a particle/> The dimensional acceleration vector is given by the following equation:

.

For a six-degree-of-freedom manipulator, the expression of its dynamic equation is:

;

in, is the inertia matrix of the system,/> is the Coriolis force,/> is gravity,/> Input for system control;

The constraints on the system are:

;

The controller and control torque given based on the Udwadia-Kalaba equation are:

;

in, Represents the Mooore-Penrose inverse of a matrix, which is a generalized inverse matrix. For a reversible square matrix, it is an inverse matrix. For a /> The dimension rank is/> matrix/> , its/> dimensional matrix and satisfies:

;

For any matrix A, its MP inverse exists and is unique, and the MP inverse of a zero matrix is the zero matrix.

In order to solve the system uncertainties, a U-K robust controller is added. Specifically, it also includes the following steps:

S6. For the six-degree-of-freedom upper limb rehabilitation training robotic arm, various uncertain factors need to be considered when it is actually used on different patients, including changes in the mass and length of the robotic arm. It needs to be based on the Udwadia-Kalaba equation. Add uncertainty parameters to the controller, and the controller is:

;

;

in, is an unknown quantity.

S7. At this time, considering that the system must remain stable, decompose the matrix, vector and inverse containing the uncertainty parameters to obtain:

;

in, , /> , /> is the nominal part, and/> ,/> ,/> ,/> It contains an uncertain part;

In order to facilitate the design of the controller, the following formula is specified:

;

.

S8. Design a robust controller based on the Udwadia-Kalaba equation:

;

in,

;

in, is a scalar, a parameter to be designed; select function/> ,make/> ;

Assuming three conditions, for any Full rank:

There is a function , making all/> satisfy ;

for a given And/> , make/> ;

There is a constant , make/> .

This embodiment also includes system stability analysis steps:

S9. Use the selection requirements of Lyapunov asymptotic stability function to select a legal Lyapunov function;

S10. Combined with Lyapunov stability determination theorem analysis, it can be proved that the robust controller satisfies consistent boundedness and consistent final boundedness, and the control system is asymptotically stable.

The present invention adds a U-K controller based on the U-K equation in the system dynamics solution part, thereby improving the control accuracy, effect and rapid response of the system. At the same time, a U-K robust controller that can converge to intrinsic uncertainties is added. The design of the robust controller not only improves the control effect of the system, but also maintains the control effect unchanged in the face of complex changes in the mechanical system, greatly reducing the cost of controlling the motor when used by different patients without the need to redesign parameters.

Example 2

An electronic device 200, as shown in Figure 2, includes but is not limited to: a memory 201, on which program code is stored; a processor 202, which is connected to the memory, and when the program code is executed by the processor, the U-K equation is implemented robot control method. For a detailed description of the method, reference may be made to the corresponding description in the above method embodiment, which will not be described again here.

Example 3

A computer-readable storage medium, as shown in Figure 3, has program instructions stored thereon, and when the program instructions are executed, a robot control method based on the U-K equation is implemented. For a detailed description of the method, reference may be made to the corresponding description in the above method embodiment, which will not be described again here.

It should also be noted that the terms "comprises," "comprises," or any other variation thereof are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that includes a list of elements not only includes those elements, but also includes Other elements are not expressly listed or are inherent to the process, method, article or equipment. Without further limitation, an element qualified by the statement "comprises a..." does not exclude the presence of additional identical elements in the process, method, good, or device that includes the element.

Each embodiment in this specification is described in a progressive manner. The same and similar parts between the various embodiments can be referred to each other. Each embodiment focuses on its differences from other embodiments.

The above are only embodiments of this specification and are not intended to limit one or more embodiments of this specification. For those skilled in the art, various modifications and transformations may be made to one or more embodiments of this specification. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of one or more embodiments of this specification shall be included in the scope of the claims of one or more embodiments of this specification.

Claims (11)

1. The robot control method based on the U-K equation is characterized by comprising the following steps of:

carrying out forward kinematic modeling and dynamic modeling on the mechanical arm to obtain a dynamic equation;

processing the dynamics equation to obtain the relation between the acceleration of the system and the external force of the system in an unconstrained state;

adding system constraint to the relation between the acceleration of the system and the external force of the system in the constraint state;

solving constraint force meeting system constraint through Udwadia-Kalaba equation to obtain an accurate expression of the constraint force;

and calculating an accurate motion equation of the system motion through an accurate expression of the constraint force, and realizing accurate control of a mechanical system.

2. The robot control method based on the U-K equation according to claim 1, wherein: the forward kinematics modeling and the dynamics modeling of the mechanical arm comprise the following steps:

forward kinematics modeling is carried out on the mechanical arm by adopting a method based on rotation algebra, liqun and Liqun;

and carrying out dynamics modeling by adopting a Lagrange dynamics method to obtain a dynamics equation.

3. The robot control method based on the U-K equation according to claim 1, wherein: the constraint equation of the system is:

；

wherein the matrixFor constraint matrix of system, < >>For acceleration vector +.>Is a constraint vector.

4. A method of controlling a robot based on the U-K equation according to claim 3, wherein: the solving of the constraint force meeting the system constraint through the Udwadia-Kalaba equation comprises the following steps:

for a six-degree-of-freedom mechanical arm, the expression of the kinetic equation is:

；

wherein ,for the inertia matrix of the system, < > for>For coriolis force, ->For gravity (I)>Is a system control input;

the system is constrained to:

；

the controller and control moment given based on the Udwadia-Kalaba equation are:

；

wherein ,representing the moore-Penrose inverse of the matrix.

5. The robot control method based on the U-K equation according to claim 4, wherein: the method also comprises the following steps:

adding uncertainty parameters in a controller based on a Udwadia-Kalaba equation;

decomposing the matrix, vector and inverse containing uncertainty parameters;

a robust controller based on the Udwadia-Kalaba equation was designed.

6. The robot control method based on the U-K equation according to claim 5, wherein: the uncertainty parameter is added into the controller based on the Udwadia-Kalaba equation, and the controller is obtained as follows:

；

wherein ,is an unknown quantity.

7. The robot control method based on the U-K equation according to claim 6, wherein: and decomposing the matrix, the vector and the inverse containing the uncertainty parameters to obtain:

；

wherein ,、 />、 />is a nominal part, and->，/>、/>、/>Is an uncertainty-containing part;

also included is defining the following formula:

；

。

8. the robot control method based on the U-K equation according to claim 7, wherein: the robust controller based on U-K equation comprises:

；

wherein ,

；

wherein ,is a scalar, is a parameter to be designed; select function->Make the following；

Assuming three conditions, for any oneFull rank:

there is a functionMake all->Satisfy the following requirements；

For a given setAnd->Make->；

There is a constantMake->。

9. The robot control method based on the U-K equation according to claim 8, wherein: the method also comprises the step of analyzing the system stability:

selecting a legal Lyapunov function by using the selecting requirement of the Lyapunov asymptotic stability function;

analysis is carried out by combining with Lyapunov stability judgment theorem, and the robustness controller is proved to meet consistent and final bounded property, so that the control system is asymptotically stable.

10. An electronic device, comprising: a memory having program code stored thereon; a processor connected to the memory and which, when executed by the processor, implements the method of any one of claims 1 to 9.

11. A computer readable storage medium, having stored thereon program instructions which, when executed, implement the method of any of claims 1-9.