A vibration suppression control method for gantry robots

Abstract

This invention discloses a vibration suppression control method for a gantry robot, belonging to the field of robot system motion control technology. The steps include: establishing a mathematical model of a flexible gantry robot system based on Lagrange's dynamic equations and analyzing its characteristics; linearizing the mathematical model of the flexible gantry robot system and reconstructing the system state variables; designing a backstepping control algorithm based on the reconstructed system state variables, and improving the parameter explosion problem caused by continuous differentiation in the original backstepping control algorithm using the dynamic surface method; and canceling unmatched external disturbances in the gantry robot system through the output of a disturbance observer. This invention employs the above-mentioned vibration suppression control method for a gantry robot, solving the model coupling problem through the reconstruction of the gantry robot system state variables, improving the original backstepping control algorithm to solve the differential explosion problem, and adding a disturbance observer to observe disturbances and cancel unmatched disturbances, achieving efficient trajectory tracking, vibration suppression, and anti-interference control effects.

Description

Technical Field

[0001] This invention relates to the field of motion control technology for robot systems, and specifically to a vibration suppression control method for a gantry robot. Background Technology

[0002] A robotic system is an integrated whole comprised of a robot, the work object, and the environment. A gantry robot system, as a typical example, receives input from the rotational force of the robotic arm and the trolley's movement. It possesses automatically controllable, reprogrammable, multifunctional, and multi-degree-of-freedom capabilities, enabling it to transport objects and manipulate tools to complete various tasks. Gantry robots are used in industrial material handling and stacking. Their transport processes often involve the simultaneous translation of the trolley and the joint operation of the robotic arm. The high coupling between these two distinct motions increases the difficulty of dynamic modeling and controller design. Furthermore, considering insufficient rigidity of the robotic arm, vibration suppression at the end effector must be addressed. Therefore, traditional control methods are no longer sufficient for gantry robot control. Compared to traditional robot control, gantry robot systems require consideration of the gantry trolley's positioning, making the system's dynamic model more complex. Achieving positioning of the robotic arm and trolley, while simultaneously addressing vibration suppression at the end effector and the overall system's resistance to external interference, becomes a highly challenging problem. Summary of the Invention

[0003] The purpose of this invention is to provide a vibration suppression control method for gantry robots, which solves the technical problem that during the transportation process of gantry robots, the translation of the trolley and the joint operation of the robotic arm are often accompanied by the high coupling of two different types of motion, making the system dynamics model more complex. When the rigidity of the robotic arm is insufficient, it is impossible to quickly and effectively eliminate the vibration of the robotic arm end and the unknown external disturbances.

[0004] To achieve the above objectives, the present invention provides a vibration suppression control method for a gantry robot, comprising the following steps:

[0005] S1. Based on the operating parameters of the gantry robot system and combined with the Lagrange dynamics equation, establish a mathematical model of the flexible gantry robot system and perform characteristic analysis.

[0006] S2. Linearize the mathematical model of the flexible gantry robot system and reconstruct the original state variables of the gantry robot system to decouple the flexible model of the gantry robot.

[0007] S3. Design a backstepping control algorithm based on the reconstructed state variables of the gantry robot system, and improve the parameter explosion caused by continuous differentiation of the original backstepping control algorithm by using the dynamic surface method.

[0008] S4. According to the backstepping control algorithm, a disturbance observer is added to offset the non-matching external disturbances experienced by the gantry robot system through the output of the disturbance observer.

[0009] Preferably, the mathematical model expression of the flexible gantry robot system in step S1 is:

[0010]

[0011] G(q l )=[0K(q-θ)K(θ-q)+m2l2gsin q]

[0012] U = [Fτ0]

[0013] F S =[00d]

[0014] Where M(q) l Let be the inertia matrix of the gantry robot system. For the centripetal-Coriolis matrix, G(q) l ) is the gravity vector, U is the control input vector, and F is the control input vector. s For the external disturbances experienced by the gantry robot, q l For the state variables of the gantry robot system, The first derivative, Let θ be its second derivative, q be the output angle of the robot arm motor, q be the output angle of the robot arm joint, m2 be the mass of the robot arm, l2 be the length of the robot arm, g be the acceleration due to gravity, K be the spring constant, x be the translation distance of the trolley for describing the state variables of the gantry robot system, F,τ be the driving force of the trolley and the torque of the robot arm for the driving force / torque, and d be the unmatched disturbance experienced by the robot arm.

[0015] Preferably, step S2 specifically includes:

[0016] S21. Linearize the mathematical model of the flexible gantry robot system:

[0017] When the angle q output by the robotic arm joint is small, we can extend cos q≈1 and sin q≈q to:

[0018] in: Represents the derivatives of two different state variables;

[0019] The simplified mathematical model of the flexible gantry robot system is as follows:

[0020]

[0021]

[0022]

[0023] Where m1 is the mass of the cart, m2 is the mass of the robotic arm, l2 is the length of the robotic arm, g is the acceleration due to gravity, and I1 is the moment of inertia of the robotic arm. This represents the displacement acceleration of the car. This represents the angular acceleration output by the robotic arm joints. This represents the output angular acceleration of the robotic arm motor; J is the moment of inertia of the motor.

[0024] S22. Reconstruct the system state variables:

[0025]

[0026]

[0027] Where x1, x2, x3, x4, γ1, γ2 are the reconstructed state variables, and θ is the output angle of the robotic arm motor. Let ω be the angular velocity output by the robotic arm motor, and q be the angle output by the robotic arm joint. Let x be the angular velocity output by the robotic arm joint, and let x be the displacement distance of the cart. Let m be the displacement velocity of the trolley, m1 be the mass of the trolley, m2 be the mass of the robotic arm, l2 be the length of the robotic arm, g be the acceleration due to gravity, and I1 be the moment of inertia of the robotic arm.

[0028] Preferably, the improvement of the original backstepping control algorithm using the dynamic surface method in step S3 specifically includes:

[0029] S31, Order

[0030]

[0031]

[0032] S32. Convert the linearized mathematical model of the flexible gantry robot system into a cascaded form:

[0033]

[0034]

[0035] in: Let θ be the derivative of the reconstructed state variables, q be the output angle of the robotic arm motor, q be the output angle of the robotic arm joint, x be the displacement distance of the trolley, m2 be the mass of the robotic arm, l2 be the length of the robotic arm, g be the acceleration due to gravity, I1 be the moment of inertia of the robotic arm, K be the spring constant, and F,τ be the driving force of the trolley and the torque of the robotic arm, respectively.

[0036] Preferably, step S4, which uses the output of the disturbance observer to cancel the mismatched external disturbances experienced by the gantry robot system, includes:

[0037] Based on the mathematical model of the flexible gantry robot system established in step S21, a cascaded perturbation observer is designed:

[0038]

[0039]

[0040]

[0041]

[0042]

[0043] in, Let x1, x2, x3, and x4 be the derivatives of their estimated values, respectively. The derivatives of the estimates of γ1 and γ2 Let a be the difference between x2 and the estimated value of x2. 11 ,a 12 ,a 13 b1, b2 are the gain parameters of the observer, and ds is the integral sign.

[0044] Preferably, the method for offsetting the mismatched external disturbances experienced by the gantry robot system in step S4 further includes:

[0045] The tracking control of the gantry trolley and robotic arm uses the following reference trajectory:

[0046]

[0047] Wherein, the robotic arm drive angle θ is set to 0, x r t represents the translation distance of the trolley. f The travel time of the car is represented by x. d x represents the distance the car has traveled, and x0 represents the car's initial position.

[0048] Therefore, the present invention adopts the above-mentioned vibration suppression control method for gantry robots, which solves the model coupling problem by reconstructing the state variables of the gantry robot system and improves the original backstepping method to solve the differential explosion problem; at the same time, a disturbance observer is added to observe the disturbance to cancel the mismatched disturbance, which ultimately achieves efficient trajectory tracking, vibration suppression and anti-interference control effects, and improves control accuracy and robustness.

[0049] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. Attached Figure Description

[0050] Figure 1 This is a schematic diagram of the mathematical model structure of the gantry robot system in Embodiment 1 of the vibration suppression control method for a gantry robot according to the present invention;

[0051] Figure 2 This is a schematic diagram of a flexible joint model in Embodiment 1 of the vibration suppression control method for a gantry robot according to the present invention;

[0052] Figure 3 This is a comparison of the controller performance under non-matching disturbance conditions in Embodiment 2 of the vibration suppression control method for a gantry robot according to the present invention. Detailed Implementation

[0053] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are some embodiments of the present invention, but not all embodiments.

[0054] Example 1

[0055] Reference Figures 1-2 This invention provides a vibration suppression control method for a gantry robot, comprising the following steps:

[0056] S1. Based on the operating parameters of the gantry robot system and combined with the Lagrange dynamics equation, establish a mathematical model of the flexible gantry robot system and perform characteristic analysis.

[0057] The mathematical model expression for the flexible gantry robot system is:

[0058]

[0059] G(q l )=[0K(q-θ)K(θ-q)+m2l2gsin q]

[0060] U = [Fτ0]

[0061] F S =[00d]

[0062] Where M(q) l Let be the inertia matrix of the gantry robot system. For the centripetal-Coriolis matrix, G(q) l ) is the gravity vector, U is the control input vector, and F is the control input vector. s For the external disturbances experienced by the gantry robot, q l For the state variables of the gantry robot system, The first derivative, Let θ be its second derivative, q be the output angle of the robot arm motor, q be the output angle of the robot arm joint, m2 be the mass of the robot arm, l2 be the length of the robot arm, g be the acceleration due to gravity, K be the spring constant, x be the translation distance of the trolley for describing the state variables of the gantry robot system, F,τ be the driving force of the trolley and the torque of the robot arm for the driving force / torque, and d be the unmatched disturbance experienced by the robot arm.

[0063] The inertia matrix of the gantry robot system is:

[0064]

[0065] Where: m 11 =m1+m2,m 12 =0,m 13 =l2m2 cosq,m 21 =0,m 22 =J,m 23 =0,m 31 =l2m2 cosq,m 32 =0,m 33 =m2l2 2 -I1,

[0066] Where J is the moment of inertia of the motor.

[0067] Centripetal-Coriolis Matrix as follows:

[0068]

[0069] Where: c 11 =c 12 =0, c 21 =c 22 =c 23 =0.

[0070] S2. Linearize the mathematical model of the flexible gantry robot system and reconstruct the original state variables of the gantry robot system to decouple the flexible model of the gantry robot.

[0071] S21. Linearize the mathematical model of the flexible gantry robot system:

[0072] When the joint output angle q of the robotic arm is small, we can extend cos q≈1 and sin q≈q to:

[0073] The system model after linear simplification is as follows:

[0074]

[0075]

[0076]

[0077] Where: m1 is the mass of the cart, m2 is the mass of the robotic arm, l2 is the length of the robotic arm, g is the acceleration due to gravity, and I1 is the moment of inertia of the robotic arm. This represents the displacement acceleration of the car. This represents the angular acceleration output by the robotic arm joints. This indicates the angular acceleration output by the robotic arm motor;

[0078] S22. Reconstruct the state variables of the gantry robot system:

[0079]

[0080]

[0081] x3 = q

[0082]

[0083]

[0084] Where x1, x2, x3, x4, γ1, γ2 are the reconstructed state variables, and θ is the output angle of the robotic arm motor. Let ω be the angular velocity output by the robotic arm motor, and q be the angle output by the robotic arm joint. Let x be the angular velocity output by the robotic arm joint, and let x be the displacement distance of the cart. Let m be the displacement velocity of the trolley, m2 be the mass of the robotic arm, l2 be the length of the robotic arm, g be the acceleration due to gravity, and I1 be the moment of inertia of the robotic arm.

[0085] S3. Design a backstepping control algorithm based on the reconstructed state variables of the gantry robot system, and improve the parameter explosion caused by continuous differentiation of the original backstepping control algorithm by using the dynamic surface method.

[0086] S31, Order

[0087]

[0088]

[0089] S32. Convert the linearized mathematical model of the flexible gantry robot system into a cascaded form:

[0090]

[0091]

[0092] in: Let θ be the derivative of the reconstructed state variables, q be the output angle of the robotic arm motor, q be the output angle of the robotic arm joint, x be the displacement distance of the trolley, m2 be the mass of the robotic arm, l2 be the length of the robotic arm, g be the acceleration due to gravity, I1 be the moment of inertia of the robotic arm, K be the spring constant, and F,τ be the driving force of the trolley and the torque of the robotic arm, respectively.

[0093] The virtual control law is designed based on the cascade equations, and the disturbance observer takes the following form:

[0094]

[0095]

[0096]

[0097] To address the differential explosion problem caused by the traditional backstepping method, a first-order inertial element is added to the virtual control law:

[0098]

[0099]

[0100]

[0101] The control rate was:

[0102]

[0103] in Let θ be the derivative of the reconstructed state variable, and θ be the output angle of the robotic arm motor. Let ω be the angular velocity output by the robotic arm motor, and q be the angle output by the robotic arm joint. Let x be the angular velocity output by the robotic arm joint, and let x be the displacement distance of the cart. Let m be the displacement velocity of the trolley, m1 be the mass of the trolley, m2 be the mass of the robotic arm, l2 be the length of the robotic arm, g be the acceleration due to gravity, I1 be the moment of inertia of the robotic arm, K be the spring constant, and F,τ be the driving force of the trolley and the torque of the robotic arm, respectively. 1d Given a trajectory for the car, x 2d ,x 3d ,x 3d For virtual tracking targets x2, x3, x4, k1, k2, k3, k4 are the controller gains, τ1, τ2, τ3 are the time constants of the first-order inertial element, and x... 2c x 3c x 4c This is represented as the set virtual control rate. Let d be the observed value of the disturbance, e1 be the error value of x1-x1d, e2 be the error value of x2-x2d, e3 be the error value of x3-x3d, and e4 be the error value of x4-x4d.

[0104] S4. Based on the backstepping control algorithm, a disturbance observer is added to offset the non-matching external disturbances experienced by the gantry robot system through the output of the disturbance observer.

[0105] Based on the mathematical model of the flexible gantry robot system established in step S21, a disturbance observer is designed in a tandem form:

[0106]

[0107]

[0108]

[0109]

[0110]

[0111] in, Let x1, x2, x3, and x4 be the derivatives of their estimated values, respectively. The derivatives of the estimates of γ1 and γ2 Let a be the difference between x2 and the estimated value of x2. 11 ,a 12 ,a 13 b1, b2 are the gain parameters of the observer, and ds is the integral sign.

[0112] Specifically, the parameters selected in this invention are k1 = 5, k2 = 50, k3 = 10, k4 = 5, τ1 = 0.06, τ2 = 0.01, and τ3 = 1.

[0113] To mitigate mismatched external disturbances experienced by the gantry robot system, vibration suppression control of the gantry robot system is also achieved using reference trajectories for the robotic arm and the trolley. The reference trajectory is:

[0114]

[0115] Wherein, the robotic arm drive angle θ is set to 0; x r t represents the translation distance of the trolley. f The x represents the travel time of the trolley. d x0 represents the travel distance of the trolley, and x0 represents the initial position of the trolley.

[0116] In practical applications, robot systems are complex multi-input, multi-output systems, characterized by numerous uncertainties and insufficient rigidity of the robotic arm. These issues necessitate the addition of vibration suppression control to the existing trajectory tracking control, hindering the control of the gantry robot. Achieving high-precision vibration suppression control under these conditions is a pressing need in the robotics field. Therefore, this invention primarily addresses the vibration suppression and anti-interference issues of flexible gantry robots. First, a mathematical model of the flexible gantry robot based on Lagrange dynamics is established and its characteristics analyzed. The established flexible model is linearized and its state variables reconstructed to facilitate model decoupling and subsequent controller design. Then, considering the differential explosion problem caused by traditional backstepping control, an improved backstepping control algorithm is designed. Next, a disturbance observer is used to observe external uncertain disturbances and incorporate them into the controller to counteract them. Finally, the advantages of the proposed controller are verified by tracking a given trajectory that meets certain conditions. The main advantages are its ability to quickly and effectively achieve trajectory tracking, vibration suppression, and anti-interference functions for the gantry robot.

[0117] In another aspect of this embodiment, regarding parameter selection, the method is simple, the number of gains and parameters is small, and the response effect corresponding to each gain is clear. Therefore, in practical applications, the parameter adjustment process is not complicated, and the gain with a better response is easy to determine.

[0118] Example 2

[0119] This embodiment differs from the first embodiment in that, in order to better verify and illustrate the technical effects used in the method of the present invention, this embodiment selects a traditional controller (PD controller) and an unmodified backstepping control algorithm for comparative testing, and compares the test results using scientific demonstration methods to verify the real effect of the method.

[0120] Experiments were conducted using a PD controller and an unmodified backstepping controller, along with a controller employing this control method.

[0121] in:

[0122] The control formula for the controller PD is:

[0123]

[0124] For a PD controller, the state variable e is the deviation between the actual displacement and the given displacement. K represents the deviation between the actual velocity and the given velocity. p K is the proportional adjustment parameter. d These are integral adjustment parameters, ultimately resulting in the controller gain being K. p =diag(800, 800), K d=diag(100, 100).

[0125] The traditional backstepping control method formula is:

[0126]

[0127] For the backstepping controller, where

[0128] Where m1 is the mass of the cart, m2 is the mass of the robotic arm, l2 is the length of the robotic arm, I1 is the moment of inertia of the robotic arm, and x 1d Let k1 and k2 be the tracking error of the vehicle, k1 and k2 be the controller gain, and in this example, k1 = 20 and k2 = 100 are selected. c1, c2, c3, c4, c5, a are the equivalence symbols after the equal sign, and z1 and z2 represent the error.

[0129] Simultaneously refer to Figure 3 It can be seen that the proposed controller can completely track the target trajectory and achieve vibration suppression. However, after interference is introduced, the two methods lose their vibration suppression capabilities. But this method can still quickly and accurately locate and suppress the vibration of the robotic arm end in the presence of external disturbances. Therefore, this method has accurate positioning and better anti-interference effect.

[0130] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit them. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can still be made to the technical solutions of the present invention, and these modifications or equivalent substitutions cannot cause the modified technical solutions to deviate from the spirit and scope of the technical solutions of the present invention.

Claims

1. A flexible truss robot vibration suppression control method characterized by comprising: The method comprises the steps of: S1, based on the operating condition parameters of the flexible truss robot system, combining the Lagrange dynamics equation, establishing a mathematical model of the flexible truss robot system and performing characteristic analysis; S2, linearizing the mathematical model of the flexible truss robot system and reconstructing the original state quantity of the flexible truss robot system, decoupling the flexible model of the truss robot, comprising the steps of: S21, linearizing the mathematical model of the flexible truss robot system: The angle outputted by the joint of the robot arm When the time is small, , it is extended to ; wherein: denotes the derivative of two different state quantities; The linearized and simplified mathematical model of the flexible truss robot system is: ； ； ； wherein, is the mass of the gantry trolley, is the mass of the robot arm, is the length of the robot arm, is the acceleration of gravity, is the moment of inertia of the robot arm, denotes the acceleration of the displacement of the gantry trolley, denotes the angular acceleration of the joint output of the robot arm, denotes the angular acceleration of the motor output of the robot arm; J is the moment of inertia of the motor, is the spring constant of the spring, is the driving force of the gantry trolley, the torque of the robot arm; S22, reconstructing the state quantity of the flexible truss robot system: ； ； wherein, is the state quantity after reconstruction, is the mechanical arm motor output angle, is the mechanical arm motor output angular velocity, is the angle output by the mechanical arm joint, is the angular velocity output by the mechanical arm joint, is the displacement distance of the gantry trolley, is the displacement speed of the gantry trolley; S3, designing a backstepping control algorithm according to the reconstructed state quantity of the flexible truss robot system, and improving the parameter explosion caused by continuous derivation of the original backstepping control algorithm through the dynamic surface method; S4, according to the improved backstepping control algorithm, adding a disturbance observer, and offsetting the non-matching external disturbance suffered by the flexible truss robot system through the output of the disturbance observer.

2. The flexible truss robot vibration suppression control method according to claim 1, characterized by, The mathematical model expression of the flexible truss robot system in step S1 is: ； ； ； ； wherein, is the inertia matrix of the flexible truss robot system, is the centripetal-Coriolis matrix, is the gravity vector, is the control input vector, is the external disturbance to the flexible truss robot, is the state variable of the flexible truss robot system, is the first derivative, is the second derivative, is the motor output angle of the manipulator, is the joint output angle of the manipulator, is the mass of the manipulator, is the length of the manipulator, is the gravity acceleration, is the spring constant for describing the state variable of the flexible truss robot system, is the truss trolley displacement distance for the driving force / torque, is the truss trolley driving force, the torque of the manipulator, d is the non-matching disturbance to the manipulator. 3.The flexible truss robot vibration suppression control method of claim 1, wherein The dynamic surface method is used to improve the original backstepping control algorithm in step S3, which specifically comprises: S31, let ； ； S32, convert the linearized mathematical model of the flexible truss robot system into a cascade form: ； ； wherein: is a derivative of the state quantity after reconstruction, is a motor output angle of the robot arm, is an angle output by the robot arm joint, is a displacement distance of the gantry trolley, is a mass of the robot arm, is a length of the robot arm, is a gravitational acceleration, is a moment of inertia of the robot arm, is a spring coefficient of the spring, is a driving force of the gantry trolley and a torque of the robot arm.

4. The flexible truss robot vibration suppression control method according to claim 1, characterized by, The method for offsetting the non-matching external disturbance suffered by the flexible truss robot system through the output of the disturbance observer in step S4 comprises: According to the mathematical model of the flexible truss robot system established in step S21, a disturbance observer is designed in a cascade form, and the form of the disturbance observer is: ； ； ； ； wherein , , , are respectively derivative of the estimate, is derivative of the estimate, is difference between the estimate and estimate, is a gain parameter of the observer, is an integral sign.

5. The flexible truss robot vibration suppression control method according to claim 1, characterized by, The method for offsetting the non-matching external disturbance suffered by the flexible truss robot system in step S4 further comprises: The tracking control of the truss trolley and the mechanical arm, and the reference trajectory is: ； Wherein, the mechanical arm driving angle is set to 0, represents the truss trolley translation distance, represents the truss trolley running time, represents the truss trolley running distance, represents the truss trolley initial position.

Images