A robot trajectory tracking control method resistant to measurement noise interference

Abstract

The application discloses a kind of anti-measurement noise interference mechanical arm trajectory tracking control methods, wherein the derivative of the joint angle of mechanical arm with respect to time is obtained by integral method, effectively avoid the shortcomings that noise interference is greatly affected in derivation operation and noise interference is easily amplified, when the measurement value of mechanical arm joint angle contains noise interference with relatively large amplitude, the control algorithm of the application has strong resistance to noise signal interference.In addition, the sliding mode controller designed by the application has a faster convergence speed.Compared with existing sliding mode controller, the controller proposed by the application can effectively improve the performance of mechanical arm tracking expected trajectory under high measurement noise interference.

Description

Technical Field

[0001] This invention belongs to the field of robot system control technology, and in particular to a method for tracking and controlling the trajectory of a robotic arm under certain special circumstances. Background Technology

[0002] Robotic arms play a vital role in improving production automation and enhancing the level of national manufacturing. With industrial development, many complex tasks are performed by robotic arms with high-precision trajectory tracking capabilities, making the trajectory tracking and control of robotic arms an increasingly important issue. Trajectory tracking control of robotic arms involves controlling the trajectories of each joint to enable the robotic arm to track the desired trajectory with good dynamic performance. Since robotic arms may be subject to various disturbances during movement, many researchers have designed anti-interference control algorithms to achieve accurate trajectory tracking.

[0003] To address the anti-interference tracking control problem of robotic arm systems with external disturbances, Niu Chengbo et al. used the T-S fuzzy model to identify external disturbances, employed a fuzzy disturbance observer to observe and compensate for unknown disturbances, and designed a PID controller [Niu Chengbo, Yu Jianjiang, Yi Yang. Anti-interference tracking control of robotic arm systems based on TS fuzzy model [J]. Journal of Yangzhou University (Natural Science Edition), 2019, 22(04): 44-47. DOI: 10. 19411 / j. 1007-824x. 2019. 04. 011.]. To address the problems of modeling errors and external interference in trajectory tracking control of robotic arms, Li Hu et al. proposed an adaptive fuzzy sliding mode control algorithm for robotic arms based on an interference observer [Li Hu, Liu Hongbin. Adaptive fuzzy sliding mode control of robotic arms based on interference observer [J]. Manufacturing Technology & Machine Tool, 2023(03): 115-120. DOI: 10. 19287 / j. mtmt. 1005-2402. 2023. 03. 015.]. To address the issues of uncertain internal parameters and susceptibility to external environmental interference in multi-joint rotorcraft, as well as the problem of the manipulator's own motion interfering with the flight platform, Li Xi et al. designed an attitude control algorithm [Li Xi, Tan Jianhao. Application of Active Disturbance Rejection Control Based on Adaptive RBFNN Noise Estimation in Attitude Control [J]. Robot, 2019, 41(01): 9-18. DOI: 10. 13973 / j. cnki. robot. 180019.]. To enhance the joint space control performance of a robotic arm during water quality sampling at a sewage pipe outlet, Ding Li et al. proposed an anti-interference control strategy combining fast continuous non-singular terminal sliding mode control and a linear extended state observer [Ding Li, Yao Yong, Chao Yuan et al. Anti-interference control of a rope-driven aerial robotic arm for water quality sampling [J]. Transactions of the Chinese Society for Agricultural Machinery, 2022, 53(08): 452-458. DOI: 10. 6041 / j.issn. 1000-1298. 2022. 08.049]. To address the anti-interference challenge of flying robotic arms, Zhang Sen et al. used the Euler-Lagrange method to model the system as a whole and proposed a linear fuzzy active disturbance rejection control (ADRC) method that combines linear active disturbance rejection control and fuzzy control [Zhang Sen, Zhuang Zhiqiang, Song Shuzhong. An ADRC method for flying robotic arms based on linear fuzzy control [J]. Journal of Henan University of Science and Technology (Natural Science Edition), 2023, 44(05): 37-48+6-7. DOI: 10. 15926 / j.cnki.issn1672-6871.2023.05.006.].To address the problems of uncontrolled base, joint friction, and mismatched disturbances such as solar radiation experienced by the free-floating flexible space manipulator, Yao Wei et al. decoupled the flexible space manipulator system into rigid and flexible subsystems based on the singular perturbation method, transforming mismatched disturbances into matched disturbances, and designed a sliding mode controller with disturbance compensation [Yao Wei, Liu Liaoxue, Guo Yu et al. Disturbance resistance control of free-floating flexible space manipulator [J]. Journal of Nanjing University of Science and Technology, 2021, 45(01): 63-70. DOI: 10. 14177 / j. cnki. 32-1397n. 2021. 45. 01. 007.].

[0004] The aforementioned research findings have solved the control problems of robotic arms under certain mechanical structure and external environmental disturbances. However, the controller of the robotic arm needs to collect motion information of each joint of the robotic arm in real time and calculate the control force or torque required by the robotic arm based on the joint motion information. In some cases, the robotic arm needs to work in an environment with complex electromagnetic interference. Complex electromagnetic interference will cause the collected joint angle information of the robotic arm to contain large noise interference, affecting the trajectory tracking control effect of the robotic arm. Summary of the Invention

[0005] This invention provides a robotic arm trajectory tracking control method that resists measurement noise interference, with the aim of improving the control effect of the robotic arm under high measurement noise conditions and meeting the needs of practical engineering applications.

[0006] Technical Solution: To solve the above problems, this invention provides a robotic arm trajectory tracking control method resistant to measurement noise interference, used to control an n-degree-of-freedom serial robotic arm, where n is a positive integer, comprising the following steps:

[0007] (1) Establish the dynamic equations of the n-degree-of-freedom robotic arm:

[0008] ,

[0009] In the formula, The position vector of the robot joint. Let the first derivative of the robot joint position vector with respect to time be denoted as . Let be the second derivative of the robot joint position vector with respect to time. It is the inertia matrix. The centripetal force and Coriolis force are vectors. It is a gravity vector. It is the vector of Coulomb friction and viscous friction. The external disturbance force vector, For the generalized output vectors of the controllers at each joint of the robot;

[0010] (2) Solve for the robot joint position vector by integration. The first derivative with respect to time The calculation expression is as follows:

[0011] ,

[0012] in To represent a factorial, the integer in parentheses should be a non-negative integer. , , ,like If it is a non-negative integer, when hour The calculation method is as follows:

[0013] ;

[0014] (3) Determine the desired trajectory that the robotic arm needs to track. The desired trajectory is determined based on the robot arm's operational needs and should be a continuous function of time, obtained through numerical differentiation or derivation. The first derivative with respect to time and second derivative ; intercept We analyze the segment with the smallest radius of curvature; the time length of this segment is denoted as... ;

[0015] (4) Apply quadratic polynomial functions and cubic polynomial functions to the segments extracted in step (3) respectively. Fit the segment with the smallest radius of curvature; if the fitting effects of the quadratic and cubic functions are not significantly different, take 2 as the value in step (2). The value of ; if the fitting effects of the two functions differ significantly, then use cubic and quartic polynomial functions for fitting respectively. If the fitting effects of the cubic and quartic functions are not significantly different, take 3 as the value. The value;

[0016] (5) The dynamic equations of the robotic arm are rewritten in the following form:

[0017] ,

[0018] Among them As shown below:

[0019] ,

[0020] (6) Give the parameters of the robot arm dynamic equation in step (1). The estimated value ,Will , , Set to zero;

[0021] (7) The controller is designed using the sliding mode control method, and the sliding surface is defined as:

[0022] ,

[0023] In the formula For constant control parameters, To control error, for The derivative over time, where the control error and control error The derivative with respect to time Given below:

[0024] ,

[0025] ,

[0026] The terms on the right side of the equation are given by steps (2) and (3); The definition is as follows:

[0027] ,

[0028] (8) To enable the sliding mode controller to have a faster convergence speed, add to the reaching law For the relevant terms, the following convergence law is designed:

[0029] ,

[0030] In the formula These are constant control parameters;

[0031] (9) Differentiate the sliding surface in step (7), and the result is as follows:

[0032] ,

[0033] Given below:

[0034] ,

[0035] The right side of the equation As given in step (3), As given in subsequent steps (10);

[0036] (10) By combining the formulas in steps (8) and (9), we can derive the following:

[0037] ;

[0038] (11) Remove the derivation result from step (10) Items that cannot be known in step (5) are ignored. The complete expression for the controller is:

[0039] ,

[0040] These are constant control parameters.

[0041] Beneficial Effects: Compared with existing technologies, the significant advantages of this invention are: the control algorithm of this invention uses integration to calculate the time derivative of the robotic arm joint angles, effectively avoiding the drawbacks of large noise interference and easy amplification of noise interference in derivative calculations. When the measured values ​​of the robotic arm joint angles contain large-amplitude noise interference, the control algorithm of this invention has strong resistance to noise signal interference. Furthermore, the sliding mode controller designed in this invention has a faster convergence speed. Compared with existing sliding mode controllers, the controller proposed in this invention can effectively improve the performance of the robotic arm in tracking the desired trajectory under high measurement noise interference. Attached Figure Description

[0042] Figure 1 This is a schematic diagram of the two-degree-of-freedom robotic arm mechanism used in the simulation of this invention embodiment;

[0043] Figure 2 A schematic diagram of the desired trajectory that the robotic arm needs to track;

[0044] Figure 3 This is a schematic diagram of white noise interference in an embodiment of the present invention;

[0045] Figure 4 This is a schematic diagram of the trajectory tracking error of controller joint 1 in an embodiment of the present invention;

[0046] Figure 5 This is a schematic diagram of the trajectory tracking error of the controller joint 2 in an embodiment of the present invention;

[0047] Figure 6 This is a schematic diagram of the trajectory tracking error of joint 1 in a prior art controller for comparison.

[0048] Figure 7 This is a schematic diagram of the trajectory tracking error of joint 2 in a prior art controller, used for comparison. Detailed Implementation

[0049] The following example, using the controller design of a two-degree-of-freedom robotic arm, illustrates the specific implementation of this patent. The following embodiments are only used to describe the present invention and are not intended to limit the scope of application of the present invention. Engineers in various fields can use the method described in this invention to control multi-degree-of-freedom robotic arms or other objects.

[0050] A method for trajectory tracking control of a robotic arm that is resistant to measurement noise interference, used to control an n-degree-of-freedom serial robotic arm, where n is a positive integer, includes the following steps:

[0051] (1) Settings Establish a dynamic model for a 2-DOF robotic arm:

[0052]

[0053] In the formula, The position vector of the robot joint. Let the first derivative of the robot joint position vector with respect to time be denoted as . Let be the second derivative of the robot joint position vector with respect to time. It is the inertia matrix. The centripetal force and Coriolis force are vectors. It is a gravity vector. It is the vector of Coulomb friction and viscous friction. The external disturbance force vector, This represents the generalized output vectors of the robot's joint controllers. For greater brevity, the following abbreviations are used:

[0054] , ,

[0055] here , can be remembered

[0056]

[0057] Mode The specific expressions for each item are as follows:

[0058]

[0059]

[0060]

[0061]

[0062]

[0063]

[0064] For actual controlled objects such as robotic arms, their dynamic models cannot be known precisely. To illustrate the implementation steps of this patent, the values ​​of each parameter in the robotic arm dynamic model are set as shown in Table 1.

[0065] Table 1. Values ​​of parameters in the robotic arm dynamics model

[0066]

[0067] (2) Substitute the parameters and give the equation. , - The specific expressions for each item in the table are:

[0068]

[0069]

[0070]

[0071]

[0072] (3) Determine the desired trajectory that the robotic arm needs to track. ,like Figure 2 As shown:

[0073] (4) According to Shape, select ;set up .

[0074] (5) The robot joint position vector is approximately solved by integration. The first derivative with respect to time The calculation expression is as follows:

[0075]

[0076] Substitute the parameter values ​​determined in the previous steps into the input. The specific expression is:

[0077]

[0078] In practical engineering applications, if the robot's motion pattern is the same as in this embodiment, the above formula can be used directly for calculation. The robot's joint rotation angles are collected in real time by the robot's controller, and then calculated using numerical integration.

[0079] (6) In general engineering applications, gravity measurement is relatively easy and has high accuracy, so this setting... , , and The angle signal measurement will be affected by noise, so their estimated values ​​are set to zero. The specific values ​​are as follows:

[0080]

[0081]

[0082] (7) The dynamic equations of the robotic arm are rewritten in the following form:

[0083]

[0084] Among them As shown below:

[0085]

[0086] (8) Design the controller using the sliding mode control method, and define the sliding surface as:

[0087]

[0088] In the formula For constant control parameters, To control error, for The derivative over time, where the control error and control error The derivative with respect to time Given below:

[0089]

[0090]

[0091] The terms on the right side of the equation are given by steps (2) and (3). The definition is as follows:

[0092]

[0093] (9) To enable the sliding mode controller to have a faster convergence speed, add to the reaching law For the relevant terms, the following convergence law is designed:

[0094]

[0095] In the formula These are constant control parameters.

[0096] (10) Differentiate the sliding surface in step (6), and the result is as follows:

[0097]

[0098] Given below:

[0099]

[0100] The right side of the equation As given in step (3), As given in subsequent steps (10).

[0101] (11) By combining the formulas in steps (8) and (9), we can derive the following:

[0102]

[0103] (12) Remove the derivation result from step (10) Items that cannot be known in step (5) are ignored. This section describes the overall control scheme design for the controller. The complete expression for the controller is:

[0104]

[0105] In the formula This indicates the driving torque required by the robotic arm. These are constant control parameters, which need to be adjusted according to the actual situation. The specific values ​​of the control parameters used in this example are as follows:

[0106]

[0107] The controller designed in this patent is compared with a controller using the same sliding surface and reaching law. The controller used for comparison is similar to the one in the formula. The form is the same, but the formula is not used. , Instead of calculating the derivative in the traditional method, the derivative of the angle signal with respect to time is obtained using the conventional differential method. The same white noise interference is added to the angle measurement signals of both joints, such as... Figure 3 As shown,

[0108] Simulations were performed using MATLAB / Simulink software to record the tracking error of the mechanical gauge tracking the desired trajectory in real time when two different controllers were used. Figures 4 to 7 As shown.

[0109] Simulation results show that the controller designed in this patent has a smaller trajectory tracking error, indicating that the robotic arm can move along the desired trajectory. In contrast, the controller used for comparison has a large trajectory tracking error and is essentially useless. The simulation results demonstrate that the controller designed in this patent can meet the control requirements of the robotic arm under high noise interference, has a certain resistance to measurement noise, and can meet the control requirements of the robotic arm in environments with high measurement noise interference.

Claims

1. A method for trajectory tracking control of a robotic arm resistant to measurement noise interference, used to control an n-degree-of-freedom serial robotic arm, where n is a positive integer, characterized in that... Includes the following steps: (1) Establish the dynamic equations of the n-degree-of-freedom robotic arm: ， In the formula, The position vector of the robot joint. Let the first derivative of the robot joint position vector with respect to time be denoted as . Let be the second derivative of the robot joint position vector with respect to time. It is the inertia matrix. The centripetal force and Coriolis force are vectors. It is a gravity vector. It is the vector of Coulomb friction and viscous friction. The external disturbance force vector, For the generalized output vectors of the controllers at each joint of the robot; (2) Solve for the robot joint position vector by integration. The first derivative with respect to time The calculation expression is as follows: ， in To represent a factorial, the integer in parentheses should be a non-negative integer. , , ,like If it is a non-negative integer, when hour The calculation method is as follows: ； (3) Determine the desired trajectory that the robotic arm needs to track. The desired trajectory is determined based on the robot arm's operational needs and should be a continuous function of time, obtained through numerical differentiation or derivation. The first derivative with respect to time and second derivative ; intercept We analyze the segment with the smallest radius of curvature; the time length of this segment is denoted as... ; (4) Apply quadratic polynomial functions and cubic polynomial functions to the segments extracted in step (3) respectively. Fit the segment with the smallest radius of curvature; if the fitting effects of the quadratic and cubic functions are not significantly different, take 2 as the value in step (2). The value of ; if the fitting effects of the two functions differ significantly, then use cubic and quartic polynomial functions for fitting respectively. If the fitting effects of the cubic and quartic functions are not significantly different, take 3 as the value. The value; (5) The dynamic equations of the robotic arm are rewritten in the following form: ， Among them As shown below: ， (6) Give the parameters of the robot arm dynamic equation in step (1). The estimated value ,Will , , Set to zero; (7) The controller is designed using the sliding mode control method, and the sliding surface is defined as: ， In the formula For constant control parameters, To control error, for The derivative over time, where the control error and control error The derivative with respect to time Given below: ， ， The terms on the right side of the equation are given by steps (2) and (3); The definition is as follows: ， (8) To enable the sliding mode controller to have a faster convergence speed, add to the reaching law For the relevant terms, the following convergence law is designed: ， In the formula These are constant control parameters; (9) Differentiate the sliding surface in step (7), and the result is as follows: ， Given below: ， The right side of the equation As given in step (3), As given in subsequent steps (10); (10) By combining the formulas in steps (8) and (9), we can derive the following: ； (11) Remove the derivation result from step (10) Items that cannot be known in step (5) are ignored. The complete expression for the controller is: ， These are constant control parameters.

2. The robotic arm trajectory tracking control method against measurement noise interference according to claim 1, characterized in that, In step (3), No more than 1 / 6 of the robotic arm's round-trip cycle. The value should be at least 5 times the period of the interference signal.

3. The robotic arm trajectory tracking control method against measurement noise interference according to claim 1, characterized in that, In step (6), the parameters The estimated value is obtained based on the robot arm's design parameters or identified from its dynamic parameters, and will be used subsequently as... Design the controller.

4. An electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it implements the steps of the method according to any one of claims 1 to 3.

5. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the steps of the method according to any one of claims 1 to 3.

Images