A flexible joint robot trajectory tracking control algorithm

Abstract

The application discloses a flexible joint mechanical arm trajectory tracking control algorithm, which comprises the following steps: establishing a mathematical model of a flexible joint mechanical arm system; based on the mathematical model of the flexible joint mechanical arm, establishing a series integral type state space equation, designing an extended state observer, and estimating a state vector and a composite disturbance value; using the state vector and the composite disturbance value to design a sliding mode control law, realizing trajectory tracking control of the flexible joint mechanical arm, and proving the stability of the extended state observer and the sliding mode controller based on the extended state observer in the flexible joint mechanical arm system. The application avoids errors caused by multiple derivation or model conversion to obtain high-order derivative values of link angular displacement, improves the precision and stability of trajectory tracking control, and the robustness and stability of the flexible joint mechanical arm system, and proves that the extended state observer can observe each state variable, the observation value can finally approach the state variable, and the control system is stable.

Description

Technical Field

[0001] This invention relates to the field of automatic control technology, and in particular to a trajectory tracking control algorithm for a flexible joint robotic arm. Background Technology

[0002] Robotic arms have wide applications in automotive, medical, and aerospace fields, performing various tasks in different application scenarios according to requirements. Given the widespread use of flexible joint robotic arms, extensive research has been conducted on their modeling and control. In scenarios requiring close contact between humans and robotic arms, lightweight design often improves safety. However, with weight reduction, control problems become more complex, especially since the joint flexibility introduced by energy transfer components such as flexible gears, drive belts, and bearings significantly impacts control performance. Therefore, joint flexibility must be fully considered in controller design and cannot be ignored. The dynamic model of a flexible joint robotic arm is a typical nonlinear, strongly coupled fourth-order system. External disturbances in its working environment and parameter perturbations in its own model place higher demands on the design of control algorithms. Sliding mode control, due to its good robustness, is widely used in the position control of flexible joint robotic arms. Currently, the industry has proposed several sliding mode control algorithms for trajectory tracking control of flexible joint robotic arms. Theoretical calculations and computer simulations show that they can achieve good results and effectively overcome disturbances to achieve precise trajectory tracking of the robotic arm. However, these control algorithms are rarely applied in practice. This is because they all require measuring the higher-order derivatives of the link angular displacement. For flexible articulated robotic arms, unlike the state variables related to the motor side, the state variables related to the link side (including the higher-order derivatives of the link angular displacement) are difficult to measure accurately in practical applications. In particular, the higher-order derivatives of the link angular displacement, obtained by differentiating the angular displacement value, are easily affected by noise signals, resulting in significant deviations. Transforming them into the angular displacements and velocities of the motor and link through mathematical models is computationally cumbersome and easily affected by parameter perturbations. Therefore, when these algorithms are applied to the trajectory tracking control of physical objects, they will produce large errors. Summary of the Invention

[0003] To address the requirement for accurate measurement of higher-order derivatives of link angular displacement in flexible joint robotic arm control algorithms, this invention designs an extended state observer that treats the combined disturbances caused by external disturbances and parameter perturbations as extended state variables. This allows for the estimation of both complete and extended state variables. Combining this with a sliding mode controller provides a method with strong environmental adaptability and robustness. It achieves excellent performance theoretically and can be applied to the trajectory tracking control of practical flexible joint robotic arms, effectively improving the accuracy and stability of trajectory tracking control. The purpose of this invention is to provide a trajectory tracking control algorithm for flexible joint robotic arms that effectively estimates link angular displacement and its higher-order derivatives, avoiding errors caused by multiple differentiations or model transformations to obtain these values. Simultaneously, it provides a sliding mode control method to effectively overcome external disturbances and parameter perturbations, achieving precise trajectory tracking control.

[0004] To achieve the above objectives, the present invention adopts the following technical solution:

[0005] A trajectory tracking control algorithm for a flexible joint robotic arm includes the following steps:

[0006] Establish a mathematical model for a flexible joint robotic arm system;

[0007] Based on the mathematical model of the flexible joint manipulator, a series integral state-space equation is established, an extended state observer is designed, the state vector and the composite perturbation value are estimated, and the stability of the extended state observer is proved using Lyapunov's stability theorem.

[0008] Using the state vector and composite disturbance value, a sliding mode control law is designed. Based on the sliding mode control law, trajectory tracking control is achieved. Using Lyapunov's stability theorem, the stability of the sliding mode controller based on the extended state observer in the flexible joint manipulator system is proved.

[0009] As a preferred embodiment of a trajectory tracking control algorithm for a flexible joint robotic arm, the mathematical model of the flexible joint robotic arm system includes:

[0010]

[0011]

[0012] Where q = [q1 q2] T Let q1 be the angular displacement vector of the first link, q2 be the angular displacement vector of the second link, and T represent the transpose matrix operation. and Let θ be the angular velocity vector and θ be the angular acceleration vector of the connecting rod, respectively, and let θ = [θ1 θ2]. TLet θ1 be the output angular displacement vector of the motor rotor after passing through the reducer, and θ2 be the output angular displacement vector of the motor rotor through the first link after passing through the reducer. and These are the output angular velocity vector and the output angular acceleration vector, respectively. Here is the link inertia matrix. For the motor inertia matrix, It is a 2x2 matrix. Let the Coriolis force and centrifugal force vectors of the robotic arm be represented. It is a vector with 2 rows and 1 column. The gravity vector Let τ be the joint stiffness matrix, τ = [τ1 τ2] T Let τ1 be the controller output vector, τ2 be the controller output vector of the first link, and τ2 be the controller output vector of the second link.

[0013] As a preferred scheme for trajectory tracking control algorithm of a flexible joint robotic arm, the establishment of the cascade integral state-space equation includes:

[0014] definition To simplify the calculations, the symbols used are not meaningful. Based on the mathematical model of the flexible joint robotic arm system, the expressions for θ and its second derivative are constructed as follows:

[0015]

[0016]

[0017] according to Based on the mathematical model of the flexible joint robotic arm system, a dynamic model is constructed:

[0018]

[0019] in, g(q)=M -1 (q)KJ -1 q (3) and q (4) These represent the third and fourth derivatives of the link, respectively. Symbols are used to simplify calculations;

[0020] Let τ = g -1 (q)v, the dynamic model can be written as Since q = [q1q2] T , The dynamic model can be written as:

[0021]

[0022] Redefining the state variable z 11 =q1, and z 21 =q2, and Will and Defined as extended state variables z 15 and z 25 Assume that the extended state variables are differentiable and their derivatives are bounded, i.e. And satisfying |γ1|≤h1,|γ2|≤h2, where h1 and h2 are arbitrary positive real numbers, the dynamic model can be rewritten as a series integral state-space equation:

[0023]

[0024] Among them, z 11 =q1, These represent the 0th, 1st, and 2nd derivatives of the angular displacement of the first connecting rod, respectively. This represents the fourth derivative of the angular displacement of the first link. z represents the fourth derivative of the angular displacement of the second link. 15 Let v1 and v2 be the extended state variables, representing the sliding mode control laws of the first and second links, respectively.

[0025] As a preferred embodiment of a trajectory tracking control algorithm for a flexible joint robotic arm, the design of the extended state observer includes:

[0026] Based on the aforementioned cascade integral state-space equations, expandable state observers for the first and second links are designed respectively:

[0027]

[0028] in, It is all the relevant state variables z of the first link in the series integral state-space equation. 1i The estimated value of z for (i = 1, 2, 3, 4, 5) 15 For the extended state variable, This is the estimated value of the composite disturbance, which includes external disturbances and parameter perturbations. It is all the relevant state variables z of the second link in the series integral state-space equation. 2i The estimated value of α for (i = 1, 2, 3, 4, 5) 1i (i=1,2,3,4,5) and α 2i(i = 1, 2, 3, 4, 5) represent the gains of the two extended state observers to be designed. Choosing appropriate observer gains can improve the estimation of state variables. and Converging to the corresponding state variable z 1i and z 2i .

[0029] As a preferred embodiment of a trajectory tracking control algorithm for a flexible joint robotic arm, the estimated state vector includes:

[0030] Define the state vector z1 = [z 11 z 12 …z 15 ] T z2 = [z 21 z 22 …z 25 ] T Then the extended state observers of the first and second links can be written in matrix form respectively:

[0031]

[0032]

[0033] The coefficient matrices in the formula are as follows:

[0034]

[0035] The observation error of the complete state variables of the extended state observer is defined as follows: make That is, the observed value of the state vector z1 δ2=[δ 21 δ 22 …δ 25 ] T .

[0036] As a preferred embodiment of a trajectory tracking control algorithm for a flexible joint robotic arm, the design of a sliding mode control law using the state vector and composite disturbance values ​​includes:

[0037] Calculate the trajectory tracking error of the first link:

[0038] e1 = q 1d -q1=z 11d -z 11

[0039] Where, q 1d and z 11dAll of these are the angular displacements output from the reference trajectory of the first link. A sliding mode function is designed for the trajectory tracking control system of the first link in the dynamic model:

[0040]

[0041] Wherein, coefficient c 11 c 12 and c 13 The choice satisfies the Hurwitz condition. Let the third derivative of the trajectory tracking error of the first link be given, and calculate the first derivative of s1:

[0042]

[0043] To avoid measuring the higher-order derivatives of the first link's angular displacement, the observed values ​​of the state vector z1 are used. and estimates of composite disturbances Design the sliding mode control law v1 for the first link:

[0044]

[0045] in, Represents z 12 Observations The second derivative z with respect to the target tracking trajectory 12d deviation, Represents z 13 Observations The second derivative z with respect to the target tracking trajectory 13d deviation, Represents z 14 Observations The second derivative z with respect to the target tracking trajectory 14d deviation, The observed value representing the first derivative of s1, z 12d z 13d and z 14d These are the output angular velocity, angular acceleration, and angular jerk of the first link reference trajectory, respectively. Let η1 be the estimated value of the composite disturbance, and η1 be the coefficient of the sliding mode control law, which is a positive real number. Output the fourth derivative of the angular displacement for the reference trajectory of the first link;

[0046] Calculate the trajectory tracking error of the second link:

[0047] e2 = q 2d -q2=z 21d -z 21

[0048] Where, q 2d and z 21d All of these are the angular displacements output from the reference trajectory of the second link. A sliding mode function is designed for the trajectory tracking control system of the second link in the dynamic model:

[0049]

[0050] Wherein, coefficient c 21 c 22 and c 23 The choice of satisfies the Hurwitz condition. Calculate the first derivative of s²:

[0051]

[0052] To avoid measuring the higher-order derivatives of the second link's angular displacement, the observed values ​​of the state vector z2 are used. and estimates of composite disturbances Design the sliding mode control law v2 for the second link:

[0053]

[0054] in, Represents z 22 Observations The second derivative z with respect to the target tracking trajectory 22d deviation, Represents z 23 Observations The second derivative z with respect to the target tracking trajectory 23d deviation, Represents z 24 Observations The second derivative z with respect to the target tracking trajectory 24d deviation, The observed value representing the first derivative of s², z 22d z 23d and z 24d These are the output angular velocity, angular acceleration, and angular jerk of the second link reference trajectory, respectively. Let η2 be the estimated value of the composite disturbance, and η2 be the coefficient of the sliding mode control law, which is a positive real number. The fourth derivative of the angular displacement is output for the reference trajectory of the second link.

[0055] A third aspect of the present invention provides an electronic device including a memory and a processor, the memory storing a computer program that, when executed by the processor, causes the processor to perform the steps of the method as described in any of the preceding claims.

[0056] A fourth aspect of the present invention provides a computer-readable storage medium storing a computer program, which, when executed by a processor, causes the processor to perform the steps of the method as described in any one of claims 1-8.

[0057] Beneficial effects:

[0058] This invention uses an extended state observer to observe (estimate) the complete state variables of a flexible joint manipulator system, namely the link angular displacement and its higher-order derivatives. This avoids the errors caused by obtaining the higher-order derivatives of the link angular displacement through multiple differentiations or model transformations in traditional methods, thus improving the accuracy and stability of the trajectory tracking control of the flexible joint manipulator. The extended state observer designed in this invention also estimates external disturbances and parameter perturbations as extended state variables and compensates for them in real time, improving the robustness and stability of the flexible joint manipulator system. By designing a sliding mode control law, trajectory tracking control of the flexible joint manipulator is achieved, while also overcoming the chattering problem of traditional sliding mode controllers, exhibiting excellent dynamic performance.

[0059] By using Lyapunov's stability theorem to prove the stability of the extended state observer, it is shown that the designed extended state observer can observe each state variable and the observed value can eventually approximate the extended state variable. Using Lyapunov's stability theorem, the stability of the sliding mode controller based on the extended state observer in the flexible joint manipulator system is also proved, that is, the link angular displacement can track the reference trajectory and output the angular displacement. Attached Figure Description

[0060] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the description of the embodiments of the present invention will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the content of the embodiments of the present invention and these drawings without creative effort.

[0061] Figure 1 This is a flowchart of the trajectory tracking control algorithm for a flexible joint robotic arm provided in an embodiment of the present invention;

[0062] Figure 2 This is a schematic diagram of the trajectory tracking control algorithm structure of the flexible joint robotic arm provided in an embodiment of the present invention. Detailed Implementation

[0063] The present invention will now be described in further detail with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and not intended to limit it. Furthermore, it should be noted that, for ease of description, the accompanying drawings show only the parts relevant to the present invention, and not all of the structures.

[0064] S1: Establish a mathematical model for the flexible joint robotic arm system;

[0065] See Figure 1 Based on the principle of flexible joint robotic arms, taking a double-link flexible joint robotic arm as an example, a mathematical model of the flexible joint robotic arm system is established. The specific process is as follows:

[0066] The mathematical model of the double-link flexible joint robotic arm is as follows:

[0067]

[0068]

[0069] Where q = [q1 q2] T Let q1 be the angular displacement vector of the first link, q2 be the angular displacement vector of the second link, and T represent the transpose matrix operation. and Let θ be the angular velocity vector and θ be the angular acceleration vector of the connecting rod, respectively, and let θ = [θ1θ2]. T Let θ1 be the output angular displacement vector of the motor rotor after passing through the reducer, and θ2 be the output angular displacement vector of the motor rotor through the first link after passing through the reducer. and These are the output angular velocity vector and the output angular acceleration vector, respectively. Here is the link inertia matrix. For the motor inertia matrix, It is a 2x2 matrix. Let the Coriolis force and centrifugal force vectors of the robotic arm be represented. It is a vector with 2 rows and 1 column. The gravity vector Let τ be the joint stiffness matrix, τ = [τ1τ2]. T τ1 is the controller output vector, which is the output torque of the joint motor. τ2 is the controller output vector of the first link and τ1 is the controller output vector of the second link.

[0070] S2: Based on the mathematical model of the flexible joint robotic arm, establish the cascade integral state space equation, design the extended state observer, estimate the state vector and the composite disturbance value, and use Lyapunov's stability theorem to prove the stability of the extended state observer.

[0071] Based on the mathematical model of a double-link flexible joint manipulator, a series integral state-space equation is established through state transformation, with the angular displacement of the link and its first to third derivative values ​​as state variables. An extended state observer is designed to observe composite disturbances (including external disturbances and parameter perturbations), and the stability of the extended state observer is verified using Lyapunov's stability theorem, as detailed below:

[0072] definition To simplify the symbols used in the calculations and avoid overly complex formulas, a substitution is made based on the mathematical model of the flexible joint robotic arm system. Only one θ is left on the left side of the equation, and all others are moved to the right side. This yields the expression for θ expressed in terms of the related quantities of the link angular displacement vector q:

[0073]

[0074] Construct the expression for the second derivative of θ:

[0075]

[0076]

[0077] according to Based on the mathematical model of the flexible joint robotic arm system, a dynamic model is constructed that is entirely represented by the link angular displacement vector q and its related quantities:

[0078]

[0079] in, g(q)=M -1 (q)KJ -1 q (3) and q (4) These represent the third and fourth derivatives of the link, respectively. Symbols are used to simplify calculations;

[0080] Before designing the extended state observer, first let τ = g -1 (q)v, the dynamic model can be rewritten as Since q = [q1 q2] T , The vectors are formed, and the dynamic model can be written as:

[0081]

[0082] Redefining the state variable z 11 =q1, and z 21 =q2, and Will and Defined as extended state variables z 15 and z 25 The logic behind this definition is to facilitate the design of the extended state observer later, and it doesn't mean that the derivatives must be fourth order here, following the previous rules of first, second, and third order derivatives. Because... Then there is Assume the extended state variables are differentiable and their derivatives are bounded, i.e. And satisfying |γ1|≤h1, |γ2|≤h2, where h1 and h2 are arbitrary positive real numbers, and |γ1| and |γ2| are bounded, the dynamic model can be rewritten as a series integral state-space equation:

[0083]

[0084] Among them, z 11 =q1, Let these represent the 0th, 1st, and 2nd derivatives of the angular displacement of the first link, respectively. The fourth derivative represents the angular displacement of the first link. The fourth derivative of the angular displacement of the second link, z 15 As extended state variables, v1 and v2 represent the sliding mode control laws of the first and second links, respectively.

[0085] Referring to the form of the cascaded integral state-space equations, two linearly extended state observers are designed to estimate the complete state variables related to the angular displacements of the first and second links, respectively. The two linearly extended state observers are designed as follows:

[0086]

[0087] in, It is the state variable z of all relevant states of the first link in the series integral state-space equation. 1i The estimated values ​​of (i = 1, 2, 3, 4, 5), i.e., the estimated values ​​of the complete state variables, are obtained by designing an extended state observer to observe the complete state variables of the flexible joint manipulator system. The complete state variables are the link angular displacement and its higher-order derivatives, thus avoiding the errors caused by obtaining the higher-order derivatives of the link angular displacement through multiple differentiations or model transformations in traditional methods, and improving the accuracy and stability of the trajectory tracking control of the flexible joint manipulator. 11 =q1, Let z represent the 0th, 1st, 2nd, and 3rd order derivatives of the angular displacement of the first link, respectively. 15 To expand the state variables, The estimated value of the composite disturbance is the expanded state variable z, which consists of the external disturbance and the modeling error. 15 The extended state observer designed in this invention simultaneously estimates external disturbances and modeling errors as extended state variables, thereby improving the robustness and stability of the flexible joint robotic arm system. It is the set of all relevant state variables z of the second link in the series integral state-space equation. 2i The estimated value of α for (i = 1, 2, 3, 4, 5) 1i (i=

[0088] 1,2,3,4,5) and α 2i (i = 1, 2, 3, 4, 5) represent the gains of the two extended state observers to be designed. Choosing appropriate observer gains can improve the estimation of state variables. and Converging to the corresponding state variable z 1i and z 2i .

[0089] like Figure 2 As shown, the output of the flexible joint robotic arm system on the right side of the figure is the link angular displacement q =

[0090] [q1 q2] T That is, [z 11 z 12 ] T , and For the observations of z1, z2, z3, z4, and z5 by the extended state observer, these observations are compared with q. d , Applying this to the design of a sliding mode controller avoids the need to measure these variables using sensors, effectively preventing errors. Furthermore, by using the input τ and the output q of the flexible joint robotic arm system, an extended state observer can be designed to obtain the observed extended state variables. These observed values ​​of the extended state variables can then be applied to the design of the sliding mode controller. z1, z2, z3, z4, and z5 are the state vectors related to each link, and q... d , Output the derivatives of the angular displacement for the reference trajectory of the link.

[0091] Define the state vector z1 = [z 11 z 12 …z 15 ] T z2 = [z 21 z 22 …z 25] T Then the extended state observers of the first and second links can be written in matrix form respectively:

[0092]

[0093]

[0094] The coefficient matrices in the formula are as follows:

[0095]

[0096] Define the observation error of the complete state variables of the extended state observer as: make That is, the observed value of state vector z1 δ2=[δ 21 δ 22 …δ 25 ] T Then the state equations of the extended state observers for the first and second links can be written in matrix form as follows:

[0097]

[0098]

[0099] Where, α1=[α 11 α 12 … α 15 ] T and α2=[α 21 α 22 … α 25 ] T These are the gain vectors of the two extended state observers, T = [1 0 0 0 0].

[0100] make minus We can obtain:

[0101]

[0102] Wherein, matrix G is:

[0103]

[0104] The characteristic equation of matrix G, |λI-G|=0, is:

[0105] λ 5 +α 11 λ 4 +α 12 λ3 +α 13 λ 2 +α 14 λ+α 15 =0

[0106] Where I is the identity matrix with the same number of rows and columns as G, λ is the eigenvalue of matrix G, and a suitable gain α is chosen. 1i (i = 1, 2, 3, 4, 5) can make the roots of the characteristic equation of matrix G all negative real parts, thus making matrix G a Hurwitz matrix. Then, for any matrix Q = Q T If > 0, there exists a symmetric positive definite matrix P that satisfies:

[0107] G T P + PG = -Q

[0108] Define Lyapunov functions as Differentiating it, we get:

[0109]

[0110] Define λ min λ is the minimum value among all eigenvalues ​​of matrix Q. max Let p be the maximum value among all eigenvalues ​​of matrix p. Since ||D|| = 1 and the rate of change γ1 of the expansion state satisfies |γ1| ≤ h1, and D is the coefficient matrix, with ||D|| being its determinant, then we can obtain:

[0111]

[0112] when From time to time Therefore, the observation error of the first extended state observer will converge to the set. Within, δ1 includes the observation error of the extended state variables and the state vector. Its observation error is bounded, and choosing an appropriate gain can reduce the error to near the origin.

[0113] Since the mathematical models of the first link angular displacement q1 and the second link angular displacement q2, as well as the state equations of the two designed extended state observers, are consistent, differing only in the observer gain coefficient and the rate of change of the extended state, it can be similarly deduced that for the second extended state observer, choosing an appropriate gain α... 2i (i = 1, 2, 3, 4, 5) can also make the observation error δ2 bounded and reduced to near the origin. This invention demonstrates the stability of the extended state observer using Lyapunov's stability theorem, proving that the designed extended state observer can observe every state variable, meaning the observed values ​​can eventually approximate the extended state variables.

[0114] S3: Design a sliding mode control law using state vectors and composite disturbance values. Based on the sliding mode control law, realize trajectory tracking control. Using Lyapunov's stability theorem, prove the stability of the sliding mode controller based on the extended state observer in the flexible joint manipulator system.

[0115] Using the state vector estimated by the extended state observer in step S2 and the composite disturbance value, a sliding mode controller is designed to ensure that the flexible joint manipulator system always runs along the sliding surface and eventually converges to an equilibrium state, thereby improving the robustness of the flexible joint manipulator system and achieving trajectory tracking control, as detailed below:

[0116] When q d =[q 1d q 2d ] T When the output angular displacement vector is the reference trajectory of the link, the trajectory tracking control objective of the robotic arm is to make the trajectory tracking error e = q. d -q converges quickly to 0, q d Output angular displacement for reference trajectory.

[0117] Calculate the trajectory tracking error of the first link:

[0118] e1 = q 1d -q1=z 11d -z 11

[0119] Where, q 1d and z 11d All of these are the angular displacements output from the reference trajectory of the first link. A sliding mode function is designed for the trajectory tracking control system of the first link in the dynamic model:

[0120]

[0121] Wherein, coefficient c 11 c 12 and c 13 The choice satisfies the Hurwitz condition. Let the third derivative of the trajectory tracking error of the first link be given, and calculate the first derivative of s1:

[0122]

[0123] To avoid measuring the higher-order derivatives of the angular displacement of the first link, the observed values ​​of the state vector z1 are used. and estimates of composite disturbances Design the sliding mode control law v1 for the first link:

[0124]

[0125] in, Represents z 12 Observations The second derivative z with respect to the target tracking trajectory 12d deviation, Represents z 13 Observations The second derivative z with respect to the target tracking trajectory 13d deviation, Represents z 14 Observations The second derivative z with respect to the target tracking trajectory 14d deviation, The observed value representing the first derivative of s1, z 12d z 13d and z 14d These are the output angular velocity, angular acceleration, and angular jerk of the first link's reference trajectory, respectively. Let η1 be the estimated value of the composite disturbance, and η1 be the coefficient of the sliding mode control law, which is a positive real number. The larger the coefficient, the faster the convergence, but it should not be too large. Output the fourth derivative of the angular displacement for the reference trajectory of the first link;

[0126] Calculate the trajectory tracking error of the second link:

[0127] e2 = q 2d -q2=z 21d -z 21

[0128] Where, q 2d and z 21d The angular displacements are all output from the reference trajectory of the second link. A sliding mode function is designed for the trajectory tracking control system of the second link in the dynamic model:

[0129]

[0130] Wherein, coefficient c 21 c 22 and c 23 The choice of satisfies the Hurwitz condition. Calculate the first derivative of s²:

[0131]

[0132] To avoid measuring the higher-order derivatives of the angular displacement of the second link, the observed values ​​of the state vector z2 are used. and estimates of composite disturbances Design the sliding mode control law v2 for the second link:

[0133]

[0134] in, Represents z 22 Observations The second derivative z with respect to the target tracking trajectory 22d deviation, Represents z 23 Observations The second derivative z with respect to the target tracking trajectory 23d deviation, Represents z 24 Observations The second derivative z with respect to the target tracking trajectory 24d deviation, The observed value representing the first derivative of s², z 22d z 23d and z 24d These are the output angular velocity, angular acceleration, and angular jerk of the second link's reference trajectory, respectively. 25 Let η be the estimated value of the composite disturbance, and η2 be the coefficient of the sliding mode control law. The larger the coefficient, the faster the convergence, but it should not be too large. η2 is a positive real number. The fourth derivative of the angular displacement is output for the reference trajectory of the second link. This invention achieves trajectory tracking control of the flexible joint robotic arm by designing a sliding mode controller, and simultaneously... τ=g -1 (q)v is continuously changing and there are no sudden changes in the output value. In other words, there is no chattering phenomenon as in traditional sliding mode control. This overcomes the chattering problem of traditional sliding mode controllers and has good dynamic performance.

[0135] After implementing trajectory tracking control for the flexible joint robotic arm system, the stability of the sliding mode controller based on the extended state observer in the trajectory tracking control system of the flexible joint robotic arm is proven by Lyapunov's stability theorem, as follows:

[0136] First, we analyze the stability of the trajectory tracking control system of the first link flexible joint robotic arm, and define the Lyapunov function as follows:

[0137]

[0138] The first derivative of V2 is:

[0139]

[0140] Substituting control law v1, we obtain:

[0141]

[0142] because The symbol is not intended to represent stability; it's simply a notation used to prove stability using Lyapunov's theorem. The reason it's written this way is precisely to use Lyapunov's theorem to prove stability. The value of is related to the observer's observation error for each state variable. The above formula also contains... The two terms cancel each other out, i.e., real-time compensation. From the analysis and proof in step 2, it can be seen that the observation error of the extended state observer is bounded, therefore, we can obtain... The value of m is bounded, and any positive constant m is taken that satisfies Simplifying the above equation yields:

[0143]

[0144] Lemma: For any V: [0,∞)∈R, the inequality equations The solution is:

[0145]

[0146] Proof: By the lemma, let a = 2η1 - 1, b = 0.5m 2 Then the equation The solution is:

[0147]

[0148] If we take η1 > 0.5, then we can obtain:

[0149]

[0150] Choosing an appropriate extended state observer gain and a sufficiently large η value can make... It can also guarantee The value of is small enough that the tracking error e1 of the first link converges to a very small interval near 0. When t→∞, e1→0. Similarly, the tracking error e2 of the second link can also be asymptotically converged, thus completing the proof. This embodiment of the invention uses Lyapunov's stability theorem to prove that the flexible joint robotic arm system is stable, that is, the angular displacement q of the link can track the reference trajectory and output the angular displacement q. d .

[0151] This application provides a computer-readable storage medium including program code, which, when run on a computer, causes the computer to execute the steps of the trajectory tracking control algorithm for the flexible joint robotic arm described above.

[0152] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product embodied on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0153] This application is described with reference to flowchart illustrations and block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It will be understood that each block and / or block in the flowchart illustrations and block diagrams, as well as combinations of blocks and processes in the flowchart illustrations and block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the process. Figure 1 One or more processes and boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0154] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and boxes Figure 1 The function specified in one or more boxes.

[0155] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and boxes Figure 1 The steps of the function specified in one or more boxes.

[0156] Note that the above description is merely a preferred embodiment of the present invention and the technical principles employed. Those skilled in the art will understand that the present invention is not limited to the specific embodiments described herein, and various obvious changes, readjustments, and substitutions can be made without departing from the scope of protection of the present invention. Therefore, although the present invention has been described in detail through the above embodiments, the present invention is not limited to the above embodiments, and may include many other equivalent embodiments without departing from the concept of the present invention, the scope of which is determined by the scope of the appended claims.

Claims

1. A trajectory tracking control algorithm for a flexible joint robotic arm, characterized in that, Includes the following steps: Establish a mathematical model for a flexible joint robotic arm system; Based on the mathematical model of the flexible joint manipulator, a series integral state-space equation is established, an extended state observer is designed, the state vector and the composite perturbation value are estimated, and the stability of the extended state observer is proved using Lyapunov's stability theorem. Using the state vector and composite disturbance value, a sliding mode control law is designed. Based on the sliding mode control law, trajectory tracking control is realized. Using Lyapunov's stability theorem, the stability of the sliding mode controller based on the extended state observer in the flexible joint manipulator system is proved. The mathematical model of the flexible joint robotic arm system includes: ， in, Let be the angular displacement vector of the link. Let be the angular displacement vector of the first link. Let T be the angular displacement vector of the second link, and let T represent the transpose matrix operation. and These are the angular velocity vector and angular acceleration vector of the connecting rod, respectively. This is the output angular displacement vector of the motor rotor after passing through the reducer. This is the output angular displacement vector of the motor rotor via the first connecting rod after passing through the reducer. Let be the output angular displacement vector of the motor rotor via the second connecting rod after passing through the reducer. and These are the output angular velocity vector and the output angular acceleration vector, respectively. Here is the link inertia matrix. For the motor inertia matrix, It is a 2x2 matrix. Let the Coriolis force and centrifugal force vectors of the robotic arm be represented. It is a vector with 2 rows and 1 column. The gravity vector Here is the joint stiffness matrix. For the controller output vector, The controller output vector for the first link. The controller output vector for the second link; The design of the sliding mode control law using the state vector and the composite disturbance value includes: Calculate the trajectory tracking error of the first link: ， in, and All of these are the angular displacements output from the reference trajectory of the first link. A sliding mode function is designed for the trajectory tracking control system of the first link in the dynamic model: ， Among them, coefficient , and The choice satisfies the Hurwitz condition. The third derivative of the trajectory tracking error of the first link is used to calculate... First derivative: ， To avoid measuring the higher-order derivative of the angular displacement of the first link, the state vector is used. Observations and estimates of composite disturbances Design the sliding mode control law for the first link. : ， in, , , , represent Observations deviation, , represent Observations The second derivative of the target tracking trajectory deviation, represent The observed value of the first derivative, , and These are the output angular velocity, angular acceleration, and angular jerk of the first link reference trajectory, respectively. This is an estimate of the composite disturbance. Let be the coefficient of the sliding mode control law, and be a positive real number. ; Calculate the trajectory tracking error of the second link: ， in, and All of these are the angular displacements output from the reference trajectory of the second link. A sliding mode function is designed for the trajectory tracking control system of the second link in the dynamic model: ， Among them, coefficient , and The choice satisfies the Hurwitz condition, and the calculation... First derivative: ， To avoid measuring the higher-order derivatives of the second link's angular displacement, the state vector is used. Observations and estimates of composite disturbances Design the sliding mode control law for the second link. : ， in, , , , represent Observations deviation, , represent Observations The second derivative of the target tracking trajectory deviation, represent The observed value of the first derivative, , and These are the output angular velocity, angular acceleration, and angular jerk of the second link reference trajectory, respectively. This is an estimate of the composite disturbance. Let be the coefficient of the sliding mode control law, and be a positive real number. .

2. The trajectory tracking control algorithm for the flexible joint robotic arm according to claim 1, characterized in that, The establishment of the cascaded integral state-space equations includes: definition , To simplify the calculations, the symbols used have no real meaning. Based on the mathematical model of the flexible joint robotic arm system, a system is constructed... and The expression for the second derivative: ， ， according to Based on the mathematical model of the flexible joint robotic arm system, a dynamic model is constructed: ， in, ; , and , Symbols are used to simplify calculations; make The dynamic model can be written as ,because The dynamic model can be written as: ， Redefining state variables and and ,Will and Defined as extended state variables respectively and Assume that the extended state variables are differentiable and their derivatives are bounded, i.e. And satisfy , and If is any positive real number, then the dynamic model can be rewritten as a series integral state-space equation: ， in, , representing the 0th, 1st, and 2nd derivatives of the angular displacement of the first link, respectively. derivative, derivative, For the extended state variable, These represent the sliding mode control laws for the first and second links, respectively.

3. The trajectory tracking control algorithm for a flexible joint robotic arm according to claim 1 or 2, characterized in that, The design of the extended state observer includes: Based on the aforementioned cascade integral state-space equations, expandable state observers for the first and second links are designed respectively: ， in, These are all the relevant state variables of the first link in the series integral state-space equation. The estimated value, For the extended state variable, This is the estimated value of the composite disturbance, which includes external disturbances and parameter perturbations. These are all the relevant state variables of the second link in the series integral state-space equation. The estimated value, and These are the gains of the two extended state observers to be designed. Choosing appropriate observer gains can improve the estimation of state variables. and Converging to the corresponding state variable and .

4. The trajectory tracking control algorithm for the flexible joint robotic arm according to claim 1, characterized in that, The estimated state vector includes: Define state vector Then the extended state observers of the first and second links can be written in matrix form respectively: ， ， The coefficient matrices in the formula are as follows: ， The observation error of the complete state variables of the extended state observer is defined as follows: , ,make That is, the state vector Observations , , .

5. An electronic device, characterized in that, include: Memory, used to store computer programs; A processor, when executing a computer program stored in the memory, implements the method steps of any one of claims 1-4.

6. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program, which, when executed by a processor, implements the method steps of any one of claims 1-4.

Images