Mechanical arm trajectory tracking method based on improved super-spiral sliding mode control

Abstract

The invention relates to mechanical arm trajectory tracking control based on improved super-spiral sliding mode control, and belongs to the technical field of mechanical arm trajectory tracking. The method comprises the following steps: S1, constructing a mechanical arm dynamic model according to related parameters of a mechanical arm, a DH parameter method and a Lagrange method; s2, on the basis of the dynamic system model of the mechanical arm, constructing a super-spiral sliding mode surface; s3, designing an improved super-spiral sliding mode control reaching law; s4, performing mechanical arm trajectory planning by using a Cartesian interpolation method; s5, on the basis of the super-spiral sliding mode surface and the reaching law thereof, determining a trajectory tracking control law in combination with a mechanical arm dynamic model; s6, a control model combining improved super-spiral sliding mode control and a mechanical arm is built; and S7, selecting control parameters, and performing trajectory tracking control. The controller provided by the invention can effectively suppress buffeting on the premise of ensuring that a system can reach a stable state, has relatively high robustness to external disturbance, and ensures trajectory tracking of an end effector of the mechanical arm.

Description

Technical Field

[0001] The invention relates to the technical field of robot arm trajectory tracking, and in particular to a robot arm trajectory tracking method based on improved super-helical sliding mode control. Background Art

[0002] The main research content of robot trajectory control is to control the movement of the robot by designing a suitable control method so that it can complete the task according to the predetermined trajectory or path. The main goal of robot trajectory control is to make the robot reach the target position accurately from the starting position and perform movement along the predetermined trajectory to ensure the accuracy, smoothness and dynamic performance during the movement. In the early days of robotics, due to the simple tasks of the robot and the relatively single application scenarios, PID control was widely used due to its simple structure and easy implementation. PID control can meet the accuracy requirements to a certain extent, but as the complexity of the task increases, traditional PID control begins to face the problems of insufficient accuracy and slow response, so researchers began to explore more advanced control methods.

[0003] The sliding mode control method has become a widely-regarded control strategy in current engineering applications due to its strong anti-interference ability, good robustness, high reliability, and ability to solve nonlinear problems in the system. The first-order sliding mode controller has relatively few design parameters and a simple structure, so it is very convenient to adjust the parameters. There are mature controller design schemes and stability analysis methods, and it has been widely used in robotic arm control. However, the discontinuous switching characteristics of the sliding mode control may cause the system to produce high-frequency chattering. The reason is that when the state trajectory of the system approaches the sliding mode, it cannot accurately move to the equilibrium point along the sliding surface. Its actual movement process is to continuously cross both sides of the sliding surface and finally reach the equilibrium point, which causes the robotic arm system to vibrate. In order to alleviate the chattering phenomenon, a robotic arm trajectory tracking control method based on improved super-helical sliding mode control is proposed to solve the above problems. Summary of the invention

[0004] 1. Technical issues to be solved

[0005] In view of the deficiencies in the prior art, the present invention provides a robot arm trajectory tracking method based on improved super-helical sliding mode control, which solves the problems raised in the above-mentioned background technology.

[0006] (II) Technical solution

[0007] In order to achieve the above-mentioned purpose, the present invention specifically adopts the following technical solutions:

[0008] A robot arm trajectory tracking control method based on improved self-disturbance rejection includes the following steps:

[0009] S1. Construct the dynamic model of the robot arm according to the robot arm related parameters, DH parameter method and Lagrangian method;

[0010] S2. Based on the dynamic system model of the robot arm, a super-helical sliding surface is constructed;

[0011] S3, design and improve the reaching law of super-helical sliding mode control;

[0012] S4, using Cartesian interpolation method to plan the robot arm trajectory;

[0013] S5. Based on the super-helical sliding surface and its reaching law, the trajectory tracking control law is determined in combination with the robot dynamics model;

[0014] S6. Build a control model combining improved super-helical sliding mode control and robotic arm;

[0015] S7. Select control parameters and perform trajectory tracking control.

[0016] Furthermore, in S1, the following constraints are imposed on the multi-DOF manipulator: all mechanical structures of the manipulator are rigid; the friction generated in all mechanical structures is not taken into consideration; the configuration of the n-DOF robot is determined, a specific DH parameter table is obtained, and a dynamic model is established by the Lagrangian method;

[0017]

[0018] Where M(q), C(q,v), G(q) represent the positive defined moment of inertia, Coriolis centripetal force and gravitational force respectively. q, v and a represent the angular position, angular velocity and angular acceleration of the robot joint respectively. τ∈R 2 is the controlled input torque.

[0019] Furthermore, in S2, the superhelical sliding surface is constructed as follows:

[0020]

[0021] Among them, c is the sliding surface parameter, e is the error between the desired angle and the actual angle of the robot arm, is the error between the desired angular velocity of the robot and its actual angular velocity.

[0022] Furthermore, in S3, according to the improved super-helical sliding mode control robot trajectory tracking control method of claim 1, it is characterized in that in order to reduce the chattering phenomenon in the system, a boundary layer is introduced and defined in the control law, and a sliding mode controller based on a saturation function sat is proposed. Design an improved super-helical sliding mode control reaching law:

[0023]

[0024] Where λ and α are the parameters to be designed, and δ is the boundary layer thickness of the saturation function, satisfying 0<δ<1.

[0025] Furthermore, in the Cartesian space trajectory planning of the robot in S4, the interpolation method is used for parameterized description according to the position and posture changes of the end effector of the robot in the process from the starting point to the end point in the three-dimensional space. Then, each trajectory point of the end effector in the Cartesian space is mapped to the joint space of the robot through the inverse kinematics algorithm, so that the end of the robot can move according to the pre-planned trajectory.

[0026] Furthermore, in S5, based on the super-helical sliding surface and its reaching law, the trajectory tracking control law is determined in combination with the robot arm dynamics model as follows:

[0027]

[0028] Where q is the actual angular velocity of the robot arm, is the actual angular acceleration of the robot arm, is the desired angular acceleration of the robot arm, is the actual angular acceleration of the robot arm, is the expected angular acceleration of the robot arm.

[0029] Furthermore, in S6, a superhelical sliding mode controller with improved control law is constructed to perform trajectory tracking control on the robot arm of the control object; wherein the desired trajectory position information is input, converted into a desired angle signal through the inverse solution of forward kinematics, and controlled by the improved superhelical sliding mode controller, so that the robot arm can better track the input signal and run the desired trajectory.

[0030] (III) Beneficial effects

[0031] Compared with the prior art, the present invention provides a robot arm trajectory tracking control method based on improved super-helical sliding mode control, which has the following beneficial effects:

[0032] The present invention adopts a new boundary layer super-helical sliding mode algorithm. The difference between the new boundary layer super-helical sliding mode algorithm and the traditional sliding mode algorithm is that the newly designed boundary layer super-helical sliding mode algorithm has the characteristics of a saturation function. The saturation function Sat(s) is used to replace the sign function Sign(s) in the traditional super-helical algorithm, which weakens the chattering problem existing in the traditional sliding mode control while achieving more accurate trajectory tracking of the robot arm. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] Figure 1 is a flow chart of a robot arm trajectory tracking method based on improved super-helical sliding mode control according to an embodiment of the present invention;

[0034] Figure 2The schematic diagram of the structure of the robot arm trajectory tracking method of the improved super-helical sliding mode control of the present invention;

[0035] Figure 3 A comparison diagram of the improved super-helical sliding mode control surface provided by the present invention and the ordinary sliding mode;

[0036] Figure 4 1 is a comparison curve diagram of the robot trajectory tracking when the improved super-helical sliding mode control provided by the present invention is compared with that when there is no control;

[0037] Figure 5 It is a curve diagram showing the effect of the improved super-helical sliding mode control provided by the present invention on the trajectory tracking of the robot arm;

[0038] Figure 6 It is a comparison curve diagram of the robot arm trajectory tracking error between the improved super-helical sliding mode control and the ordinary sliding mode control provided by the present invention; DETAILED DESCRIPTION

[0039] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0040] Example

[0041] An embodiment of the present invention provides a robot arm trajectory tracking method based on improved super-helical sliding mode control, comprising the following steps:

[0042] S1. Establish the dynamics and kinematics model of the robot arm;

[0043] The following constraints are imposed on the multi-DOF manipulator: all mechanical structures of the manipulator are rigid; the friction generated in all mechanical structures is not taken into account; the configuration of the n-DOF robot is determined, the specific DH parameter table is obtained, and the dynamic model is established through the Lagrangian method;

[0044]

[0045] Where M(q), C(q,v), G(q) represent the positive defined moment of inertia, Coriolis centripetal force and gravitational force respectively. q, v and a represent the angular position, angular velocity and angular acceleration of the robot joint respectively. τ∈R 2 is the controlled input torque.

[0046] S2. Based on the dynamic system model of the robot arm, a super-helical sliding surface is constructed;

[0047] Take the desired angle q of the robot armd The difference from the actual angle q is e = q d -q and its derivative are the desired angular velocity of the robot The actual angular velocity difference The superhelical sliding surface is constructed as follows:

[0048]

[0049] Among them, c is the sliding surface parameter, e is the error between the desired angle and the actual angle of the robot arm, is the error between the desired angular velocity of the robot and its actual angular velocity.

[0050] S3, design and improve the reaching law of super-helical sliding mode control;

[0051] The saturation function Sat(s) is used to replace the sign function Sign(s) in the traditional superhelical algorithm, and the reaching law of the improved superhelical sliding mode control is designed:

[0052]

[0053] By introducing and defining the boundary layer δ in the control law, the sign function Sign(s) is optimized with the saturation function Sat(s). The existence of the sliding surface will still cause the failure to switch in time, which further weakens the chattering problem and enables each joint to track the desired tracking trajectory quickly and with high precision.

[0054] S4, using Cartesian interpolation method to plan the robot arm trajectory;

[0055] In the Cartesian space trajectory planning of the robot arm, the coordinates of the middle point, i.e. the interpolation point, can be obtained through the interpolation algorithm. After obtaining the middle point, the robot arm end posture at the middle point is converted into the corresponding joint angle of each robot arm, and then the joint angle is controlled so that the end of the robot arm can move according to the pre-planned trajectory.

[0056] The interpolation methods include linear interpolation and circular interpolation. The Cartesian space linear interpolation method is to find the position and posture of each trajectory interpolation point when the positions and postures of the starting and ending points of the straight line are known.

[0057] Assume that a straight line has two points M and N at its start and end, and their coordinates relative to the base coordinate system. v is the required speed along the line, and t is the interpolation time interval. We can calculate the length of the line L, the distance d within the interval t, and the number of interpolations N as follows:

[0058]

[0059] d=v*t

[0060]

[0061] Then the increments of each axis of adjacent interpolation points and the coordinate values ​​of each axis interpolation point are as follows:

[0062] Increment of adjacent interpolation points:

[0063]

[0064] Where: i = 0, 1, 2…N

[0065] Coordinates of interpolation points:

[0066] x i+1 =x i +iΔx

[0067] y i+1 =y i +iΔy

[0068] z i+1 =z i +iΔz

[0069] In the trajectory planning of Cartesian space arcs, for the convenience of calculation, coordinate system transformation must be applied, that is, a new rectangular coordinate system must be established in the plane where the arc is located, and the values ​​of each interpolation point of the arc in the new coordinate system must be calculated in this new rectangular coordinate system. Finally, the intermediate interpolation points are transformed into Cartesian space through the transformation matrix.

[0070] Three points determine an arc. Assume that the robot end effector moves from the starting position P1 (x1, y1, z1) through the middle point P2 (x2, y2, z2) and finally reaches the end point P3 (x3, y3, z3). If these three points are not collinear, there must be an arc from the starting point P1 through the middle point P2 to the end point P3. The algorithm steps for arc trajectory planning are as follows:

[0071] (1) First find the center P0 (X0, Y0, Z0) and its radius:

[0072] (2) Establish a new spatial coordinate system based on the plane where the arc is located, and find the mapping relationship between the coordinate system and the base coordinate system

[0073] (3) Determine the arc trajectory and find the total angle of the arc

[0074] (4) Using the relationship between trigonometric functions, find the coordinates of each interpolation point and map it to the base coordinate system

[0075] S5. Based on the super-helical sliding surface and its reaching law, the trajectory tracking control law is determined in combination with the robot dynamics model;

[0076] It is known that the variables that control the motion trajectory of the robot arm are the angular velocity and angular acceleration of each joint. Combined with the robot arm dynamics model, the trajectory tracking control law is determined as follows:

[0077]

[0078] Where q is the actual angular velocity of the robot arm, is the actual angular acceleration of the robot arm, is the desired angular acceleration of the robot arm, is the actual angular acceleration of the robot arm, is the expected angular acceleration of the robot arm.

[0079] S6. Build a control model combining an improved super-helical sliding mode controller and a robotic arm to achieve trajectory tracking control;

[0080] A superhelical sliding mode controller with improved control law is built to perform trajectory tracking control on the robot arm, the object of control. The desired trajectory position information is input and converted into the desired angle signal through the inverse solution of forward kinematics. The improved superhelical sliding mode controller is used to control the robot arm so that it can better track the input signal and run the desired trajectory.

[0081] S7, select control parameters and perform trajectory tracking control;

[0082] Through engineering experiments, the estimated values ​​of key system parameters such as sliding surface parameter c, super-helical sliding mode control approach law parameters λ, α, and boundary layer thickness δ of the saturation function in step S2 and S3 are determined. c, λ, and α are all greater than zero. The selection principle is to ensure that the system state point has a faster approach speed when it is far away from the switching surface, avoiding too small approach speed and too slow approach speed, and too large causing severe chattering. Through comparative observation of experimental data, appropriate selection is made to make the system approach the switching surface at an appropriate speed.

[0083] δ satisfies 0<δ<1, and it can be equivalent to a slope of δ. The selection of δ will directly affect the jitter phenomenon of the system. When δ is small, the slope will be steeper, resulting in more obvious jitter of the system. On the contrary, when δ is large, the slope is flatter and the jitter phenomenon is weaker. In order to balance accuracy and jitter, it is necessary to select an appropriate δ value for adjustment based on experimental data.

[0084] An embodiment of the present invention provides a robot arm trajectory tracking control method based on improved super-helical sliding mode control, including obtaining a desired trajectory input, and transitioning the input signal through a tracking differentiator, obtaining the angular velocity after the transition and comparing it with the actual angular velocity signal output by the robot arm to obtain an error signal, and performing trajectory tracking control on the robot arm through a super-helical sliding mode controller, wherein the trajectory planning uses a Cartesian interpolation method.

[0085] The present invention improves the super-helical sliding mode controller, and realizes the trajectory tracking control of the robot arm in the presence of modeling errors. Compared with the ordinary sliding mode control, the system chattering is reduced and the trajectory tracking accuracy is improved.

[0086] Finally, it should be noted that the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art can still modify the technical solutions described in the aforementioned embodiments or replace some of the technical features therein by equivalents. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A robot arm trajectory tracking method based on improved super-helical sliding mode control, characterized in that: The following steps are involved: S1. Construct the dynamic model of the robot arm according to the robot arm related parameters, DH parameter method and Lagrangian method; S2. Based on the dynamic system model of the robot arm, a super-helical sliding surface is constructed; S3, design and improve the reaching law of super-helical sliding mode control; S4, using Cartesian interpolation method to plan the robot arm trajectory; S5. Based on the super-helical sliding surface and its reaching law, the trajectory tracking control law is determined in combination with the robot dynamics model; S6. Build a control model combining improved super-helical sliding mode control and robotic arm; S7. Select control parameters and perform trajectory tracking control.

2. The method for tracking the trajectory of a manipulator based on improved super-helical sliding mode control according to claim 1, characterized in that: include: In S1, the following constraints are imposed on the multi-DOF manipulator: all mechanical structures of the manipulator are rigid; the friction generated in all mechanical structures is not taken into consideration; the model of the n-DOF robot is determined, a specific DH parameter table is obtained, and a dynamic model is established by the Lagrangian method; Where M(q), C(q,v), G(q) represent the positive defined moment of inertia, Coriolis centripetal force and gravitational force respectively. q, v and a represent the joint angular position, angular velocity and angular acceleration respectively. τ∈R 2 is the controlled input torque.

3. The robot arm trajectory tracking method according to claim 1, characterized in that: Based on the dynamic system model of the robot arm, a super-helical sliding surface is constructed, including: Determining the joint angle tracking error and the angular velocity tracking error in the tracking control process of the robotic arm based on the dynamic system model of the robotic arm to generate a joint angle tracking error set and an angular velocity tracking error set; The super-helical sliding surface is constructed based on the joint angle tracking error set and the angular velocity tracking error set. The sliding surface is as follows: Among them, c is the sliding surface parameter, e is the error between the desired angle and the actual angle of the robot arm, is the error between the desired angular velocity of the robot and its actual angular velocity.

4. The method for tracking the trajectory of a manipulator based on improved super-helical sliding mode control according to claim 1, characterized in that: In the Cartesian space trajectory planning of the robot arm, the interpolation method is used to parametrically describe the position and posture changes of the end effector of the robot arm from the starting point to the end point in the three-dimensional space. Then, each trajectory point of the end effector in the Cartesian space is mapped to the joint space of the robot arm through the inverse kinematics algorithm, so that the end of the robot arm can move according to the pre-planned trajectory.

5. The method for tracking the trajectory of a manipulator based on improved super-helical sliding mode control according to claim 1, characterized in that: In order to alleviate the chattering phenomenon in the system, the boundary layer is introduced and defined in the control law, and a sliding mode controller based on the saturation function sat is proposed. The design improves the super-helical sliding mode control reaching law: Where λ and α are the parameters to be designed, and δ is the boundary layer thickness of the saturation function, satisfying 0<δ<1.

6. The method for tracking the trajectory of a manipulator based on improved super-helical sliding mode control according to claim 1, characterized in that: Based on the super-helical sliding surface and its reaching law, the trajectory tracking control law is determined in combination with the robot dynamics model as follows: in, is the actual angular acceleration of the robot arm, is the desired angular acceleration of the robot arm, is the actual angular acceleration of the robot arm, is the expected angular acceleration of the robot arm.

7. The method for tracking the trajectory of a manipulator based on improved super-helical sliding mode control according to claim 1 is characterized by: A superhelical sliding mode controller with improved control law is built to perform trajectory tracking control on the robot arm, the object of control. The desired trajectory position information is input and converted into the desired angle signal through the inverse solution of forward kinematics. The improved superhelical sliding mode controller is used to control the robot arm so that it can better track the input signal and run the desired trajectory.

Images