Trajectory tracking control method and device, electronic equipment and storage medium

Abstract

The invention provides a trajectory tracking control method and device, electronic equipment and a computer readable storage medium. The method comprises the steps that a control model of an operation arm and a control model of a workbench are established through the Lagrange method; establishing a sliding mode variable structure controller for improving the sliding mode reaching law; and the sliding mode variable structure controller with the improved sliding mode reaching law is used for carrying out trajectory tracking control on an operation arm and a workbench. The improved reaching law sliding mode variable structure control designed by the invention can greatly reduce the generation of high-frequency buffeting, avoid the adverse effect of the high-frequency buffeting on the system, improve the ability of the system to quickly respond to input, and improve the control precision and robustness, and the trajectory tracking control effect is more ideal compared with the trajectory tracking control effect obtained before improvement and PD control.

Description

Technical Field

[0001] This invention relates to the field of automation control technology, and in particular to a trajectory tracking control method, device, electronic device, and computer-readable storage medium. Background Technology

[0002] With the continuous improvement of industrial automation, robotic arms are widely used in industrial production. Robotic arms can replace human labor in complex and harsh environments to complete a series of repetitive and dangerous tasks, improving processing efficiency and playing an increasingly important role in human production and life. Trajectory planning refers to the process by which a robotic arm, based on actual task requirements and using a suitable trajectory interpolation algorithm, plans an ideal motion trajectory that passes through required path points and satisfies other constraints. Currently, scholars at home and abroad have conducted research on trajectory interpolation algorithms using various methods in Cartesian space and joint space to meet the operational needs of specific robotic arms. After obtaining the optimized trajectories of the woodworking manipulator and the worktable, how to further achieve high-precision trajectory tracking control of both has become an urgent problem to be solved. Summary of the Invention

[0003] The present invention aims to provide a method, apparatus, electronic device, and computer-readable storage medium that overcomes or at least partially solves the above-mentioned problems.

[0004] To achieve the above objectives, the technical solution of the present invention is specifically implemented as follows:

[0005] The first aspect of the present invention provides a trajectory tracking control method, comprising:

[0006] The control models of the manipulator and the worktable are established using the Lagrange method.

[0007] Establish an improved sliding mode convergence law sliding mode variable structure controller;

[0008] The improved sliding mode approach law is used to track and control the trajectory of the manipulator and the worktable.

[0009] Optionally, the establishment of the control model for the manipulator and the control model for the worktable using the Lagrange method includes:

[0010] Establish a Lagrangian control model for the manipulator:

[0011]

[0012] Where: m1, m2, m3, and m4 are the masses of joints 1, 2, 3, and 4 of the manipulator, respectively; d1, d2, and d4 are the joint variables of moving joints 1, 2, and 4 of the manipulator, respectively; g is the acceleration due to gravity; θ3 and θ4 are the joint variables of rotating joints 3 and 4 of the manipulator, respectively; and I3 is the moment of inertia of the manipulator's joint 3 link about the Y-axis. Let be the moment of inertia of the four-link joint of the manipulator about the Y-axis; Let x be the moment of inertia of the four joints of the manipulator about the X-axis.

[0013] Establish a Lagrange control model for the workbench:

[0014]

[0015] Where m1', m2', and m3' are the masses of joint 1, joint 2, and joint 3 of the worktable, respectively; d1', d2', and d3' are the joint variables of moving joint 1, moving joint 2, and moving joint 3 of the manipulator, respectively; θ3' is the joint variable of rotating joint 3 of the worktable; and I3' is the moment of inertia of the joint 3 of the worktable about the Y-axis.

[0016] Optionally, the improved sliding mode reaching law is:

[0017]

[0018] Where s is the hyperplane, σ is the adjustment factor, tanh() is the hyperbolic tangent function, ε is the rate of approaching the hyperplane s=0, -ks is the exponential approach term, and k is a constant.

[0019] Optionally, the sliding mode variable structure controller for establishing the improved sliding mode reaching law includes:

[0020] Establish control rate:

[0021]

[0022] Where: M(q) is the inertia matrix, c i e represents the constant coefficient to be designed. i Let q be the tracking error of the i-th joint variable. id Let G be the expected input for the i-th joint variable, and G be the gravity matrix. For the Coriolis force matrix, w c This is the default value.

[0023] Optionally, the method may also include: performing system stability analysis.

[0024] Optionally, the system stability analysis includes:

[0025] Create Lyapunov functions:

[0026] V = 0.5s 2

[0027]

[0028] in, This is the sum of external interference, modeling errors, and uncertainties.

[0029] Set the default value w c :

[0030]

[0031] Among them, w U and w L for The upper and lower bounds,

[0032] Determine whether the judgment condition is met; if it is, the system is stable.

[0033] Optionally, the judgment conditions include:

[0034]

[0035] A second aspect of the present invention provides a trajectory tracking control device, comprising:

[0036] The first module is used to establish the control model of the manipulator and the control model of the worktable using the Lagrange method.

[0037] The second module is used to establish an improved sliding mode convergence law sliding mode variable structure controller;

[0038] The control module is used to perform trajectory tracking control of the manipulator and the worktable using the improved sliding mode approach law sliding mode variable structure controller.

[0039] A third aspect of the present invention provides an electronic device, comprising: a processor and a memory;

[0040] The memory is used to store computer programs;

[0041] The processor is configured to execute the trajectory tracking control method described above by invoking the computer program.

[0042] A fourth aspect of the present invention provides a computer-readable storage medium having a computer program stored thereon that, when executed by a processor, implements the trajectory tracking control method as described above.

[0043] Therefore, it can be seen that the improved approach law sliding mode variable structure control, designed by the trajectory tracking control method, device, electronic device and computer-readable storage medium provided by the present invention, can greatly reduce the generation of high-frequency chattering, avoid its adverse effects on the system and improve the system's ability to respond quickly to inputs, improve the control accuracy and robustness, and achieve a more ideal trajectory tracking control effect compared with the original and PD control. Attached Figure Description

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

[0045] Figure 1 A flowchart of the trajectory tracking control method provided in an embodiment of the present invention;

[0046] Figure 2 This is a schematic diagram illustrating the variation of the tanh(σs) curve under different σ values ​​provided in this embodiment of the invention.

[0047] Figure 3 This is a tracking diagram of the trajectory positions of each joint of the operating arm provided in an embodiment of the present invention;

[0048] Figure 4 This is a schematic diagram of the output torque of the joint of the conventional exponential reaching law sliding mode variable structure control manipulator provided in an embodiment of the present invention;

[0049] Figure 5 A schematic diagram of the output torque of the manipulator joint in the improved approach law sliding mode variable structure control provided in an embodiment of the present invention;

[0050] Figure 6 This is a schematic diagram of the joint trajectory position tracking of the manipulator arm based on the improved reaching law sliding mode variable structure control provided in an embodiment of the present invention;

[0051] Figure 7 This is a schematic diagram of the joint trajectory position tracking of the worktable based on the improved reaching law sliding mode variable structure control provided in an embodiment of the present invention;

[0052] Figure 8 This is a schematic diagram of the output torque of the table joint based on the improved reaching law sliding mode variable structure control provided in an embodiment of the present invention;

[0053] Figure 9 This is a schematic diagram of the trajectory tracking control device provided in an embodiment of the present invention;

[0054] Figure 10 A schematic diagram of an electronic device provided in an embodiment of the present invention. Detailed Implementation

[0055] Exemplary embodiments of the present disclosure will now be described in more detail with reference to the accompanying drawings. While exemplary embodiments of the present disclosure are shown in the drawings, it should be understood that the present disclosure may be implemented in various forms and should not be limited to the embodiments set forth herein. Rather, these embodiments are provided so that this disclosure will be thorough and complete, and will fully convey the scope of the disclosure to those skilled in the art.

[0056] Figure 1 A flowchart of the trajectory tracking control method provided in an embodiment of the present invention is shown. See also: Figure 1 The trajectory tracking control method provided in this embodiment of the invention includes:

[0057] S1. Use the Lagrange method to establish the control model of the manipulator and the control model of the worktable;

[0058] S2, Establish an improved sliding mode convergence law sliding mode variable structure controller;

[0059] S3, the sliding mode variable structure controller with the improved sliding mode approach law is used to perform trajectory tracking control of the manipulator and the worktable.

[0060] Specifically, from the perspective of industrial robots, the manipulator arm can be considered as consisting of two linear motion joints and two rotary joints, while the worktable can be considered as consisting of two linear motion joints and one rotary joint. Joints one and two of the manipulator arm are linear motion joints, translating along the Z and X axes respectively, while joints three and four rotate around the B and A axes respectively, enabling the four high-speed electric spindles to move and rotate freely within the workspace. Joints one and two of the worktable are also linear motion joints, translating along the X and Y axes respectively, while joint three is a rotary joint that enables the end effector to rotate around the A' axis. The four high-speed electric spindles mounted on the woodworking manipulator arm can continuously switch tools to perform drilling, milling, and other machining functions on wooden components, ensuring the continuity of the machining process, effectively improving production efficiency and automation level, and reducing equipment investment. In addition to the X and Y axis feed motions in the worktable, the end clamping mechanism on the A' axis enables the rotation of wooden components in three-dimensional space. In this way, the woodworking manipulator arm and the worktable work together to meet the needs of machining complex curved surfaces and multiple surfaces of wooden structural components.

[0061] During the processing of wooden structural components, the host computer sends commands, and the motion controller responds to the input by controlling the angles or displacements of each joint, thereby controlling the position and speed of the manipulator and the worktable. Therefore, the end of the manipulator's electric spindle and the end of the worktable's clamping mechanism can move smoothly and quickly along a predetermined trajectory, ultimately reaching the designated working position.

[0062] To obtain the kinematic model of the manipulator and the worktable, the relatively moving components in both are treated as rigid rods and represented by straight lines. The joints are represented as prismatic joints and revolute joints, simplifying the manipulator and worktable and resulting in a more concise and intuitive form. Furthermore, since the axes of motion of the manipulator and worktable are parallel or orthogonal, the worktable can be viewed as a Cartesian coordinate system manipulator with two prismatic joints and one revolute joint, i.e., a PPR-type manipulator; the manipulator can be viewed as a manipulator with two prismatic joints and two revolute joints, i.e., a PPRR-type manipulator.

[0063] As an optional embodiment of the present invention, the step of establishing the control model of the manipulator and the control model of the worktable using the Lagrange method includes:

[0064] Establish a Lagrangian control model for the manipulator:

[0065]

[0066] Where: m1, m2, m3, and m4 are the masses of joints 1, 2, 3, and 4 of the manipulator, respectively; d1, d2, and d4 are the joint variables of moving joints 1, 2, and 4 of the manipulator, respectively; g is the acceleration due to gravity; θ3 and θ4 are the joint variables of rotating joints 3 and 4 of the manipulator, respectively; and I3 is the moment of inertia of the manipulator's joint 3 link about the Y-axis. Let be the moment of inertia of the four-link joint of the manipulator about the Y-axis; Let x be the moment of inertia of the four joints of the manipulator about the X-axis.

[0067] Establish a Lagrange control model for the workbench:

[0068]

[0069] Where m1', m2', and m3' are the masses of joint 1, joint 2, and joint 3 of the worktable, respectively; d1', d2', and d3' are the joint variables of moving joint 1, moving joint 2, and moving joint 3 of the manipulator, respectively; θ3' is the joint variable of rotating joint 3 of the worktable; and I3' is the moment of inertia of the joint 3 of the worktable about the Y-axis.

[0070] Specifically, the Lagrange method does not rely on the establishment of a coordinate system or focus on the internal constraints of the system. Instead, it solves the dynamic equations from the perspective of work done and energy, and calculates the driving force or torque acting on the joints. The Lagrange equation is shown in equation (1) below:

[0071]

[0072] Where k is the total kinetic energy of all joint links; u is the total potential energy of all joint links; For joint variables, the joint variable of a rotational joint is the joint angle, and the joint variable of a translational joint is the joint displacement; L is the Lagrangian quantity or kinetic potential, which is equal to the difference between kinetic energy and potential energy. The driving force τ of each joint can be derived from equation (1). i The expressions for are shown in equations (2) and (3) below:

[0073]

[0074] From the above equation, it can be seen that when the kinetic and potential energies of each joint link are known, the driving force τ can be derived. i The magnitude of the force is determined by sequentially calculating the kinetic potential energy of each joint of the manipulator and the worktable, and further deriving the value of the joint driving force τ.

[0075] The manipulator has four joints. The first two joints are moving links, and the last two joints are rotating links. They must be distinguished when calculating the kinetic and potential energy of the links. For the moving links of joint one and joint two, there is only translational motion along the Z and X axes, and they only have translational kinetic energy. Therefore, the kinetic energy of the links of joint one and joint two is as shown in the following formula (4):

[0076]

[0077] Among them: Ek i Let d1 and d2 represent the kinetic energy of the i-th joint link; d1 and d2 are the joint variables of moving joint one and joint two, respectively; m1 and m2 are the masses of the joint one and joint two links, respectively.

[0078] Joint 1 undergoes a translational motion along the Z-axis, resulting in a change in potential energy. However, joint 2, connected to the base, does not move along the Z-axis and therefore does not experience a change in potential energy. Thus, the potential energy of the connecting rod between joints 1 and 2 is shown in equation (5):

[0079]

[0080] Among them: Ep i Let g be the potential energy of the i-th joint link, and g be the gravitational acceleration, taken as 9.8 m / s². 2 .

[0081] Joints three and four are rotary joints, possessing not only translational kinetic energy in the Z and X axes but also rotational kinetic energy from their own rotation. The sum of these two kinetic energies is shown in equation (6):

[0082]

[0083] Where: θ3 and θ4 are the joint variables of joint three and joint four, respectively, and I3 is the moment of inertia of the three-link joint about the Y-axis; Let be the moment of inertia of the four-bar linkage about the Y-axis; Let be the moment of inertia of joint four about the X-axis. In the simplified model, the change in the center of mass of joint three caused by the rotation of joint three is ignored, while the change in the center of mass of joint four can be calculated through geometric relationships. The potential energy of the joint three and joint four is finally obtained as shown in equation (7):

[0084]

[0085] Based on the above calculation results, the total kinetic energy k and total potential energy u of each joint of the manipulator can be obtained, as shown in equations (8) and (9):

[0086]

[0087] Substituting equations (8) and (9) into Lagrange equation (1) yields the manipulator. The expression (10):

[0088]

[0089] By taking the partial derivatives of each joint variable and the partial derivatives of the first derivative of each joint variable with respect to L, we obtain equations (11) and (12).

[0090]

[0091] Differentiating equation (12) with respect to time t yields equation (13):

[0092]

[0093] Substituting equations (11), (12), and (13) into equation (2), we obtain the Lagrange dynamics equation of the manipulator as shown in equation (14):

[0094]

[0095] Similarly, the Lagrange dynamic equation of the worktable can be obtained as shown in equation (15), and the specific calculation will not be described in detail here.

[0096]

[0097] After deriving the dynamic equations of the manipulator and the worktable, the corresponding control model can be established. Furthermore, a suitable controller and control algorithm can be designed to achieve tracking control of the woodworking manipulator and the worktable trajectory.

[0098] As an optional embodiment of the present invention, the improved sliding mode reaching law is:

[0099]

[0100] Where s is the hyperplane, σ is the adjustment factor, tanh() is the hyperbolic tangent function, ε is the rate of approaching the hyperplane s=0, -ks is the exponential approach term, and k is a constant.

[0101] As an optional embodiment of the present invention, the sliding mode variable structure controller for establishing the improved sliding mode convergence law includes:

[0102] Establish control rate:

[0103]

[0104] Where: M(q) is the inertia matrix, c i e represents the constant coefficient to be designed. i Let q be the tracking error of the i-th joint variable. id Let G be the expected input for the i-th joint variable, and G be the gravity matrix. For the Coriolis force matrix, w c This is the default value.

[0105] Specifically, sliding mode variable structure control is widely used in nonlinear control systems such as robotic arms, offering advantages such as ease of design, robustness, and strong anti-interference capabilities. Furthermore, sliding mode variable structure control exhibits relatively low sensitivity to model uncertainties and parameter variations, maintaining good performance even under conditions of system parameter changes or uncertainties. It effectively addresses problems caused by inaccurate models in the control system of the manipulator and worktable, as well as external disturbances. Therefore, based on the above analysis, this invention, grounded in the control requirements of a woodworking manipulator and worktable control system, employs sliding mode variable structure control to achieve better control performance. Simultaneously, considering that the sliding mode variable structure control trajectory, after reaching the sliding surface, often fails to slide strictly along the sliding surface to the equilibrium point, instead traversing back and forth on both sides of the sliding surface, resulting in chattering, this invention improves upon the traditional exponential reaching law by designing a novel reaching law function. This improves traditional sliding mode variable structure control, reducing the impact of chattering, accelerating convergence speed, improving control quality, and enhancing control accuracy.

[0106] Traditional exponential reaching laws contain a sign function term sgn(s). When s < 0, sgn(s) = -1; when s > 0, sgn(s) = 1. When s = 0, the value of sgn(s) abruptly changes from -1 to 1, similar to a step signal. This can improve the convergence speed of the system when approaching the sliding surface, but it also causes high-frequency switching, leading to oscillations of the system state around the selected sliding surface, which is the cause of chattering. To improve the smoothness of the reaching law function when s approaches zero without affecting the system's convergence speed, the sign function term is replaced with the hyperbolic tangent function tanh. This reduces chattering by improving the smoothness of the reaching law function.

[0107] The improved convergence law obtained is shown in equation (16):

[0108]

[0109] Where σ is an adjustment factor that can effectively adjust the approach velocity of the moving point near the sliding surface. Figure 2 This reflects the changes in the tanh(σs) curve under different σ values.

[0110] Depend on Figure 2 It can be seen that when σ is larger, the curve is steeper when s approaches 0, the tanh(σs) curve is closer to the sign function sgn(s) curve, and the convergence speed is faster. However, the value of σ is not necessarily better the larger it is. Therefore, it is necessary to select an appropriate σ parameter value to reduce chattering while improving the convergence speed.

[0111] This invention utilizes the improved reaching law to design a controller, obtaining a sliding mode variable structure controller with an improved reaching law. First, the tracking error e of the joint variable and its derivative are defined. As shown in the following formula:

[0112] e = [q 1d -q1,q 2d -q2…q nd -q n ] T (17)

[0114]

[0115] Where q id q represents the expected input for the i-th joint variable; i The actual output value of the system for the i-th joint variable is represented by the sliding mode switching function designed as follows:

[0116]

[0117] in:

[0118] The dynamic equations (14) and (15) established by the Lagrange method can also be written in the general matrix form of the robotic arm model, which is equation (20).

[0119]

[0120] Where M(q) is the inertia matrix; G is the gravity matrix; τ is the Coriolis force matrix; τ is the joint output torque matrix; Consider it as the sum of external interference, modeling errors, and uncertainties. From equation (20), we have:

[0121]

[0122] The improved convergence laws obtained are:

[0123]

[0124] Solving equations (21) and (22) together yields the control law τ:

[0125]

[0126] because The term is an unknown term, and an approximate value w is taken in practical applications. c To make an estimate, let's assume The upper and lower bounds are w respectively. U and w L Finally, the control law τ is obtained as shown in equation (24).

[0127]

[0128] As an optional implementation of the present invention, the trajectory tracking control method provided in the present invention further includes: performing system stability analysis.

[0129] As an optional implementation of this invention, the system stability analysis includes:

[0130] Create Lyapunov functions:

[0131] V = 0.5s 2

[0132]

[0133] in, This is the sum of external interference, modeling errors, and uncertainties.

[0134] Set the default value w c :

[0135]

[0136] Among them, w U and w L for The upper and lower bounds,

[0137] Determine whether the judgment condition is met; if it is, the system is stable.

[0138] As an optional implementation of this invention, the determination conditions include:

[0139]

[0140] Specifically, to ensure the stability of the control system, the present invention also needs to perform stability analysis.

[0141] In control systems, the Lyapunov method is often used to determine system stability. For sliding mode variable structure control, if there exists a continuous function V that satisfies:

[0142]

[0143] The system is stable at s=0, therefore:

[0144]

[0145] That is, the system can reach a stable point within a finite amount of time.

[0146] Design the Lyapunov function as: V = 0.5s 2 By combining equations (21) and (24), we can obtain for:

[0147]

[0148] Clearly, V satisfies equation (25), and it is also easy to see that when s≠0, -εstanh(σs)-ks 2 <0; M(q) is always greater than 0, therefore the approximate value w needs to be adjusted. c The design can be taken as follows:

[0149]

[0150] When s>0

[0151] When s < 0 If the term is always less than 0 in s≠0, then it satisfies equation (26), and the designed control law can maintain the stability of the control system.

[0152] Therefore, it can be seen that, compared with traditional sliding mode variable structure control, the trajectory tracking control method provided by the embodiments of the present invention significantly reduces the high-frequency oscillation of the output torque under the improved reaching law sliding mode variable structure control, significantly weakens the chattering phenomenon, and the output torque curve is also smoother, which has good robustness and is conducive to improving the control accuracy and operation stability of the system. The sliding mode variable structure control using the improved reaching law of hyperbolic tangent function can well balance the speed and accuracy of system control and has good anti-interference ability.

[0153] The following simulation experiment further illustrates the trajectory tracking control method provided in this embodiment of the invention:

[0154] The present invention takes the robotic arm and the workbench as the control objects to conduct a trajectory tracking control simulation experiment. According to the preset trajectories of the robotic arm and the workbench, the expressions for each segment with respect to time t are listed as follows in equations (30)-(35):

[0155] (1) Expressions for the trajectories of each segment of the robotic arm with respect to time t:

[0156] In the first stage from 0 to 2.3189 s, when 0 < t1 < 2.3189 s:

[0157]

[0158] In the second stage from 2.3189 s to 5.2729 s, when 0 < t2 < 2.9540 s:

[0159]

[0160] In the third stage from 5.2729 s to 7.8876 s, when 0 < t3 < 2.6147 s:

[0161]

[0162] (2) Expressions for the trajectories of each segment of the workbench with respect to time t:

[0163] In the first stage from 0 to 2.3189 s, when 0 < t1 < 2.3189 s:

[0164]

[0165] In the second stage from 2.3189 s to 5.2729 s, when 0 < t2 < 2.9540 s:

[0166]

[0167] In the third stage from 5.2729 s to 7.8876 s, when 0 < t3 < 2.6147 s:

[0168]

[0169] Taking the displacement function, velocity function, and acceleration function of the above trajectories as the desired trajectory inputs of the control system, a PD controller and a traditional sliding mode variable structure controller (based on the exponential reaching law) are used to conduct a trajectory tracking control experiment on the robotic arm, and the initial state of the robotic arm is set as shown in equation (36).

[0170]

[0171] The trajectory position tracking diagrams of each joint of the robotic arm can be obtained, as shown in Figure 3 shown.

[0172] As can be seen from the trajectory position tracking diagrams of joints one and two of the manipulator, the control accuracy of traditional sliding mode variable structure control is superior to that of PD control, with very small error. The actual output trajectory curve almost coincides with the desired trajectory. However, in the position tracking diagram of joint one, PD control exhibits significant overshoot around 5s, failing to achieve good trajectory position tracking control for joint one. In the trajectory tracking control experiment for rotating joints three and four, the initial state of the trajectory position was not consistent with the ideal trajectory to test the convergence speed and robustness of the algorithm. The trajectory position tracking diagrams of joints three and four show that traditional sliding mode variable structure control has a faster convergence speed and better robustness, both achieving trajectory position tracking of joints three and four within 0.3s, while PD control takes almost 1.5s to catch up with the desired trajectory. This indicates that sliding mode variable structure control exhibits good control performance for this type of nonlinear control system. In practical applications, the drive motor provides driving torque for joint movement. Good torque output is the foundation for ensuring stable motor operation; therefore, the output torque of the controller also needs to be considered. Figure 4 The output torque of each joint of the manipulator based on the traditional exponential approach law sliding mode variable structure control was demonstrated.

[0173] The output torque curves of the first, second, and fourth joints of the manipulator exhibited high-frequency chattering at the start, end, and segment connection points, affecting the stable operation of the system and reducing control accuracy. In practical applications, such high-frequency oscillations can even severely impact motor performance and lifespan. Figure 5 This is a torque output diagram of each joint of the manipulator based on the improved reaching-law sliding mode variable structure control. From the output torque diagrams of joints one and two, it is clear that compared to traditional sliding mode variable structure control, the high-frequency oscillation of the output torque under the improved reaching-law sliding mode variable structure control is significantly reduced, with only slight oscillations appearing at the connection points of each segment. Chattering is significantly weakened, and the output torque curve is smoother, which is beneficial for improving the control accuracy and operational stability of the system. From the output torque diagrams of joints three and four, it can be seen that in the initial state, because the initial positions of joints three and four are not set to match the ideal trajectory, there is a large deviation. Therefore, in order to achieve rapid tracking of the ideal trajectory, the controller outputs a large torque at the initial moment to force the system to respond quickly. However, within a very short time after the initial moment, the output torque quickly drops and tends to stabilize, indicating the good robustness of the proposed improved reaching-law sliding mode variable structure control.

[0174] Figure 6 , Figure 7 and Figure 8 These are, respectively, the tracking of the trajectory position of each joint of the manipulator, the tracking of the trajectory position of the worktable, and the output torque, all based on the improved approach law sliding mode variable structure control.

[0175] In the trajectory tracking diagrams of manipulator joints three and four based on the improved reaching law sliding mode variable structure control, the system can achieve tracking of the ideal trajectory within 0.2s, which is faster than the previous 0.3s. Furthermore, the tracking accuracy of the joint trajectory is also improved compared to traditional PD control and sliding mode variable structure control based on the traditional exponential reaching law. The above simulation results demonstrate that the sliding mode variable structure control using the improved reaching law of the hyperbolic tangent function can effectively balance the speed and accuracy of the system control, and also exhibits good anti-interference capabilities against external disturbances.

[0176] In the trajectory tracking diagram of the three joints of the worktable, the system achieved tracking of the ideal trajectory in only about 0.1 seconds. The trajectory curves of joint one and joint two also almost coincided with the ideal trajectory, indicating that the improved approaching law sliding mode variable structure control has a faster convergence speed and higher control accuracy. From the output torque diagrams of the moving joints one and two of the manipulator and the worktable, it can also be seen that the output torque under the improved approaching law sliding mode variable structure control does not have obvious high-frequency oscillations, and the chattering phenomenon is greatly reduced. In the output torque diagram of the rotating joint, the larger torque output by the controller at the initial moment is conducive to rapid trajectory tracking and does not cause unnecessary oscillations, thus achieving a good balance between tracking speed and operational stability.

[0177] In summary, the improved approach law sliding mode variable structure control of this invention can greatly reduce the generation of high-frequency chattering, avoid its adverse effects on the system, and improve the system's ability to respond quickly to inputs. The control accuracy and robustness are also improved, and the trajectory tracking control effect obtained is more ideal compared with the original control and PD control.

[0178] Therefore, this invention uses the simpler Lagrange method to establish a dynamic model of the woodworking manipulator and workbench. The traditional exponential reaching law is improved by replacing the original switching function sgn with the hyperbolic tangent function tanh, resulting in a new improved reaching law sliding mode variable structure control. Trajectory tracking simulation experiments of the manipulator and workbench, along with experimental results on position tracking and joint torque output, demonstrate that the improved reaching law sliding mode variable structure control, compared to the original control and PD control, more effectively eliminates output torque chattering, improves convergence speed and control accuracy, exhibits better robustness, and possesses superior control quality.

[0179] Figure 9 This diagram illustrates the structure of a trajectory tracking control device provided in an embodiment of the present invention. This trajectory tracking control device applies the aforementioned method. The following is only a brief description of the structure of the trajectory tracking control device; for other matters not covered herein, please refer to the relevant descriptions in the aforementioned trajectory tracking control method. Figure 9 The trajectory tracking control device provided in this embodiment of the invention includes:

[0180] The first module is used to establish the control model of the manipulator and the control model of the worktable using the Lagrange method.

[0181] The second module is used to establish an improved sliding mode convergence law sliding mode variable structure controller;

[0182] The control module is used to perform trajectory tracking control of the manipulator and the worktable using the improved sliding mode approach law sliding mode variable structure controller.

[0183] As an optional implementation of this invention, the first modeling module establishes the control model of the operating arm and the control model of the worktable using the Lagrange method in the following manner:

[0184] Establish a Lagrangian control model for the manipulator:

[0185]

[0186] Where: m1, m2, m3, and m4 are the masses of joints 1, 2, 3, and 4 of the manipulator, respectively; d1, d2, and d4 are the joint variables of moving joints 1, 2, and 4 of the manipulator, respectively; g is the acceleration due to gravity; θ3 and θ4 are the joint variables of rotating joints 3 and 4 of the manipulator, respectively; and I3 is the moment of inertia of the manipulator's joint 3 link about the Y-axis. Let be the moment of inertia of the four-link joint of the manipulator about the Y-axis; Let x be the moment of inertia of the four joints of the manipulator about the X-axis.

[0187] Establish a Lagrange control model for the workbench:

[0188]

[0189] Where m1', m2', and m3' are the masses of joint 1, joint 2, and joint 3 of the worktable, respectively; d1', d2', and d3' are the joint variables of moving joint 1, moving joint 2, and moving joint 3 of the manipulator, respectively; θ3' is the joint variable of rotating joint 3 of the worktable; and I3' is the moment of inertia of the joint 3 of the worktable about the Y-axis.

[0190] As an optional embodiment of the present invention, the improved sliding mode reaching law is:

[0191]

[0192] Where s is the hyperplane, σ is the adjustment factor, tanh() is the hyperbolic tangent function, ε is the rate of approaching the hyperplane s=0, -ks is the exponential approach term, and k is a constant.

[0193] As an optional embodiment of the present invention, the second establishing module establishes a sliding mode variable structure controller with an improved sliding mode convergence law in the following manner:

[0194] Establish control rate:

[0195]

[0196] Where: M(q) is the inertia matrix, c i e represents the constant coefficient to be designed. i Let q be the tracking error of the i-th joint variable. id Let G be the expected input for the i-th joint variable, and G be the gravity matrix. For the Coriolis force matrix, w c This is the default value.

[0197] As an optional embodiment of the present invention, the trajectory tracking control device provided in the present invention further includes: an analysis module for performing system stability analysis.

[0198] As an optional implementation of this invention, the analysis module performs system stability analysis in the following manner:

[0199] Create Lyapunov functions:

[0200] V = 0.5s 2

[0201]

[0202] in, This is the sum of external interference, modeling errors, and uncertainties.

[0203] Set the default value w c :

[0204]

[0205] Among them, w U and w L for The upper and lower bounds,

[0206] Determine whether the judgment condition is met; if it is, the system is stable.

[0207] As an optional implementation of this invention, the determination conditions include:

[0208]

[0209] Therefore, it can be seen that, compared with traditional sliding mode variable structure control, the trajectory tracking control device provided by the embodiments of the present invention significantly reduces the high-frequency oscillation of the output torque under the improved reaching law sliding mode variable structure control, significantly weakens the chattering phenomenon, and the output torque curve is also smoother, which has good robustness and is conducive to improving the control accuracy and operation stability of the system. The sliding mode variable structure control using the improved reaching law of hyperbolic tangent function can well balance the speed and accuracy of system control and has good anti-interference ability.

[0210] Figure 10 A schematic diagram of an electronic device provided in an embodiment of the present invention, see below. Figure 10 The electronic device provided in this embodiment of the invention includes: a processor and a memory;

[0211] The memory is used to store computer programs;

[0212] The processor is configured to execute the trajectory tracking control method described above by invoking the computer program.

[0213] Electronic device 10 may be a desktop computer, laptop, handheld computer, cloud server, or other electronic device. Electronic device 10 may include, but is not limited to, a processor 1001 and a memory 1002. Those skilled in the art will understand that... Figure 10 This is merely an example of electronic device 10 and does not constitute a limitation on electronic device 10. It may include more or fewer components than shown, or different components.

[0214] The processor 1001 can be a Central Processing Unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. In this embodiment of the invention, the processor can be the controller described in the above embodiments.

[0215] The memory 1002 can be an internal storage unit of the electronic device 10, such as a hard disk or RAM of the electronic device 10. The memory 1002 can also be an external storage device of the electronic device 10, such as a plug-in hard disk, SmartMedia Card (SMC), Secure Digital (SD) card, or Flash Card equipped on the electronic device 10. The memory 1002 can also include both internal and external storage units of the electronic device 10. The memory 1002 is used to store the computer program 1003 and other programs and data required by the electronic device 10.

[0216] Therefore, it can be seen that, compared with traditional sliding mode variable structure control, the electronic device provided by the embodiments of the present invention significantly reduces the high-frequency oscillation of the output torque under the improved reaching law sliding mode variable structure control, significantly weakens the chattering phenomenon, and the output torque curve is also smoother, which has good robustness and is conducive to improving the control accuracy and operation stability of the system. The sliding mode variable structure control using the improved reaching law of hyperbolic tangent function can well balance the speed and accuracy of system control and has good anti-interference ability.

[0217] The present invention also provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the trajectory tracking control method as described above.

[0218] Therefore, it can be seen that, compared with traditional sliding mode variable structure control, the computer-readable storage medium provided by the embodiments of the present invention significantly reduces the high-frequency oscillation of the output torque under the improved reaching law sliding mode variable structure control, significantly weakens the chattering phenomenon, and the output torque curve is also smoother, which has good robustness and is conducive to improving the control accuracy and operation stability of the system. The sliding mode variable structure control using the improved reaching law of hyperbolic tangent function can well balance the speed and accuracy of system control and has good anti-interference ability.

[0219] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the above-described division of functional units and modules is merely an example. In practical applications, the above functions can be assigned to different functional units and modules as needed, that is, the internal structure of the device can be divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the embodiments can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit.

[0220] The above are merely embodiments of this application and are not intended to limit the scope of this application. Various modifications and variations can be made to this application by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the scope of the claims of this application.

Claims

1. A trajectory tracking control method characterized by, The method comprises the following steps: a control model of the operating arm and a control model of the worktable are established by using a Lagrange method; a sliding mode variable structure controller with improved sliding mode reaching law is established; the operating arm and the worktable are controlled to track a trajectory by using the sliding mode variable structure controller with improved sliding mode reaching law.

2. The method of claim 1, wherein, The step of establishing the control model of the operating arm and the control model of the worktable by using the Lagrange method comprises the following steps: a Lagrange control model of the operating arm is established: wherein: m1, m2, m3, m4 are the masses of the joint one, joint two, joint three and joint four links of the operating arm respectively, d1, d2, d4 are the joint variables of the moving joint one, moving joint two and moving joint four of the operating arm respectively, g is the acceleration of gravity, θ3 and θ4 are the joint variables of the rotating joint three and rotating joint four of the operating arm respectively, I3 is the moment of inertia of the joint three link of the operating arm about the Y axis, I4 is the moment of inertia of the joint four link of the operating arm about the Y axis; I4x is the moment of inertia of the joint four of the operating arm about the X axis; a Lagrange control model of the worktable is established: wherein m1', m2' and m3' are the masses of the first joint, the second joint and the third joint of the worktable respectively, d1', d2' and d3' are the joint variables of the first moving joint, the second moving joint and the third moving joint of the operating arm respectively, θ3' is the joint variable of the third rotating joint of the worktable, and I3' is the moment of inertia of the third joint link of the worktable around the Y axis.

3. The method of claim 2, wherein, The improved sliding mode reaching law is: wherein s is a hyperplane, σ is an adjustment factor, tanh() is a hyperbolic tangent function, ε is the rate of approaching the hyperplane s=0, -ks is an exponential approaching term, and k is a constant.

4. The method of claim 3, wherein, The step of establishing the sliding mode variable structure controller with improved sliding mode reaching law comprises the following steps: a control rate is established: wherein: M(q) is an inertia matrix, c i is a constant coefficient to be designed, e i is a tracking error of the i-th joint variable, q id is a desired input of the i-th joint variable, G is a gravity matrix, is a Coriolis force matrix, w c is a preset value.

5. The method of claim 4, wherein, The method further comprises the following steps: system stability is analyzed.

6. The method of claim 5, wherein, The step of analyzing the system stability comprises the following steps: a Lyapunov function is established: V = 0.5s 2 wherein is the sum of external disturbances, modeling errors and uncertainty factors. Setting a preset value w c : where w U and w L are the upper and lower bounds of , respectively. it is determined whether a judgment condition is met, and if the judgment condition is met, the system is stable.

7. The method of claim 6, wherein, The judgment condition comprises the following steps:

8. A trajectory tracking control device characterized by comprising: The method comprises the following steps: a first establishing module is configured to establish a control model of the operating arm and a control model of the worktable by using a Lagrange method; a second establishing module is configured to establish a sliding mode variable structure controller with improved sliding mode reaching law; a control module is configured to control the operating arm and the worktable to track a trajectory by using the sliding mode variable structure controller with improved sliding mode reaching law.

9. An electronic device, comprising: The method comprises the following steps: a processor and a memory; the memory is configured to store a computer program; the processor is configured to execute the trajectory tracking control method according to any one of claims 1 to 7 by calling the computer program.

10. A computer-readable storage medium having stored thereon a computer program, characterized in that, The program is executed by the processor to implement the trajectory tracking control method according to any one of claims 1 to 7.

Images