Collaborative robot trajectory tracking control method and system

Abstract

The present invention discloses a collaborative robot trajectory tracking control method and system, the system comprising: establishing a joint space dynamics model of an n-degree-of-freedom collaborative robot and establishing an expression of joint tracking error; establishing a sliding surface of a sliding mode controller; deriving a robust control rate of the collaborative robot based on sliding mode control; establishing an expression of the required updated parameter vector error; deriving an adaptive fuzzy control rate based on a sliding mode controller; establishing an expression of an external load observation error; and deriving a nonlinear state observer based on an external load observation error. The present invention combines a sliding mode controller and a nonlinear state observer to establish an adaptive fuzzy controller with an external disturbance observer, and improves the trajectory tracking control accuracy of the collaborative robot without the need to know the dynamic model parameters of the collaborative robot and the external load measurement is inaccurate. And because the method of the present invention is combined with a sliding mode controller, robustness to external disturbances can be achieved.

Description

Technical Field

[0001] The invention relates to collaborative robot trajectory tracking control. Background Art

[0002] Collaborative robots are often used in work scenarios that require interaction with people or the environment. In the process of interaction with people or the environment, external loads need to be estimated and accurately controlled. Collaborative robot systems have the characteristics of strong nonlinearity, strong coupling, and time-varying, and the cost of obtaining their accurate models is high. In actual engineering applications, external disturbances are inevitable, so it is also necessary to improve the external disturbance compensation of collaborative robots. Summary of the invention

[0003] The present invention provides a collaborative robot trajectory tracking control method and system, which utilize an adaptive fuzzy control method to improve the collaborative robot's trajectory tracking accuracy and anti-interference performance against external disturbances.

[0004] According to a first aspect of an embodiment of the present invention, a collaborative robot trajectory tracking control method is provided, comprising the following steps:

[0005] Establish the spatial dynamics model of the joints of the n-DOF collaborative robot and establish the expression of the joint tracking error;

[0006] According to the joint tracking error expression, the sliding surface of the sliding mode controller is established;

[0007] According to the sliding surface and the joint space dynamics model of the collaborative robot, the robust control rate of the collaborative robot based on sliding mode control is obtained.

[0008] Establishing the robust control rate requires updating the expression of the parameter vector error;

[0009] According to the expression of updating parameter vector error, the adaptive fuzzy control rate based on sliding mode controller is obtained;

[0010] Establish an expression for the external load observation error;

[0011] According to the external load observation error expression, a nonlinear state observer based on the external load observation error is obtained;

[0012] Combining the robust control rate, adaptive fuzzy control rate and nonlinear state observer, the adaptive fuzzy control rate with external disturbance observer of the collaborative robot is obtained.

[0013] The adaptive fuzzy control rate is used as input force to act on each joint of the robot to achieve trajectory tracking of the robot.

[0014] According to a second aspect of an embodiment of the present invention, a collaborative robot trajectory tracking control system is provided, comprising:

[0015] processor;

[0016] a memory for storing processor-executable instructions;

[0017] The processor is configured to execute all or part of the steps of the method.

[0018] According to a third aspect of an embodiment of the present invention, there is provided a non-transitory computer-readable storage medium having a computer program stored thereon, wherein the computer program implements all or part of the steps of the method when executed by a processor.

[0019] In general, compared with the prior art, the trajectory tracking control method based on adaptive fuzzy control for a collaborative robot provided by the present invention has the following beneficial effects:

[0020] 1) Use adaptive fuzzy control method to improve trajectory tracking accuracy and reduce input jitter;

[0021] 2) Combined with the nonlinear observer method, the external disturbance measurement error can be effectively compensated. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] In order to more clearly illustrate the technical solution of the embodiment of the present invention, the drawings of the embodiment are briefly introduced below.

[0023] Figure 1 A flow chart of a collaborative robot trajectory tracking control method based on adaptive fuzzy control provided by one embodiment of the present invention.

[0024] Figure 2 A schematic diagram of a 2-DOF robot provided in one embodiment of the present invention.

[0025] Figure 3 FIG. 4 is a schematic diagram of an ideal trajectory according to an embodiment of the present invention.

[0026] Figure 4 It is the second norm of the trajectory tracking error of the trajectory tracking control method (C3) based on adaptive fuzzy control of the present invention, the traditional sliding mode control (C1), and the traditional PD control (C2).

[0027] Figure 5 The invention provides a trajectory tracking control method (C3) based on adaptive fuzzy control and a traditional sliding mode control (C1) to suppress disturbances. DETAILED DESCRIPTION

[0028] Figure 1 A flow chart of a collaborative robot trajectory tracking control method based on adaptive fuzzy control is shown below. Figure 1 The method of the present invention is described in detail.

[0029] S1: Establish the joint space dynamics model of the n-DOF collaborative robot and establish the expression of joint tracking error;

[0030] S2: Establishing the sliding surface of the sliding mode controller according to the joint tracking error expression established in step S1;

[0031] S3: According to the sliding mode surface in step S2, combined with the collaborative robot joint space dynamics model established in step S1, the robust control rate of the collaborative robot based on sliding mode control is obtained;

[0032] S4: Establishing the expression of the parameter vector error required to update the robust control rate in step S3;

[0033] S5: According to the expression of the parameter vector error updated in step S4, derive the adaptive fuzzy control rate based on the sliding mode controller;

[0034] S6: Establish an expression for external load observation error;

[0035] S107: according to the external load observation error expression in step S6, a nonlinear state observer based on the external load observation error is obtained;

[0036] S8: Combining the robust control rate in step S3, the adaptive fuzzy control rate in step S5 and the nonlinear state observer in step S7, the adaptive fuzzy control rate of the collaborative robot with an external disturbance observer is obtained.

[0037] S9: The adaptive fuzzy control rate is used as input force to act on each joint of the robot to achieve trajectory tracking of the robot.

[0038] As a highly robust control method, sliding mode control does not require an accurate dynamic model of the collaborative robot and can ensure the trajectory tracking control of the collaborative robot. Fuzzy control, as an adaptive intelligent control method, does not require manual parameter adjustment and can effectively reduce the high-frequency jitter caused by the sliding mode controller structure. Therefore, the use of adaptive fuzzy control methods by collaborative robots is an effective way to achieve good trajectory tracking control effects. Nonlinear observers are conducive to fast tracking at an exponential convergence rate when the external load measurement is inaccurate, and can converge errors at high speed to ensure its control accuracy.

[0039] Take a 2-DOF robot as an example. Figure 2 The masses of the rods are m1 and m2, the lengths of the rods are l1 and l2, and the distances from the center of mass to the end of the rods are lc1 and lc2. The specific values ​​of these parameters are as follows: Figure 2 As shown, the moment of inertia of each rod relative to the center of mass is I1 = I2 = 0.1 kg·m 2In this embodiment, position control is performed in the X direction and force control is performed in the Y direction.

[0040] The two-degree-of-freedom robot joint space dynamics model established in step S1 is as follows:

[0041]

[0042] Among them: q, are the joint angle, angular velocity and angular acceleration respectively; τ, τ v are joint torque and joint external disturbance torque respectively; D q , C q , G q They are the inertia matrix, centripetal Coriolis matrix and gravity matrix of the collaborative robot.

[0043] In formula (1):

[0044]

[0045] in:

[0046]

[0047] The joint tracking error expression is:

[0048]

[0049] Where: q d , are the expected angle and velocity of the joint respectively; e, They are joint tracking position error and velocity error respectively;

[0050] The sliding surface and its derivative of the sliding mode controller in step S2 are defined as:

[0051]

[0052] in: is the joint angular acceleration tracking error, and the coefficient c is a positive diagonal matrix. In this embodiment,

[0053]

[0054] The robust control rate based on sliding mode control in step S3 is:

[0055]

[0056] in: are the estimated values ​​of the inertia matrix, the centripetal Coriolis matrix and the gravity matrix of the collaborative robot; s is the sliding mode variable; are auxiliary variables and their derivatives respectively, among which, K s is the switching gain matrix of the sliding surface, K p is a positive diagonal matrix, and sgn(·) is the sign function.

[0057] In this embodiment,

[0058]

[0059] The expression of the parameter vector error required to update the robust control rate in step S4 is:

[0060]

[0061] in: To update the parameter vector, is θ i The estimated value of .

[0062] The adaptive fuzzy control rate based on the sliding mode controller in step S5 is:

[0063]

[0064] Among them, τ v is the actual external load force; λ is the basis function of the fuzzy system, and λ is the basis function of the fuzzy system for any centroid method defuzzifier y i The expression can be written as:

[0065]

[0066] The expression is:

[0067]

[0068] Where: m is the number of fuzzy rules; K d is a positive diagonal matrix; is the membership function that satisfies the jth rule in the fuzzy system.

[0069] In this embodiment,

[0070]

[0071] The expression of the external load observation error in step S6 is:

[0072]

[0073] in: is an estimate of the external load moment.

[0074] The nonlinear state observer based on the external responsible observation error in step S7 is:

[0075]

[0076] Z and are auxiliary variables and their derivatives respectively;

[0077] The auxiliary variable L(q) is:

[0078] L(q)=YD q (q) -1 (16)

[0079] The auxiliary variable P(q) is:

[0080]

[0081] Where: Y is a positive definite diagonal matrix. In this embodiment,

[0082]

[0083] The interference of the system's external load on each joint is set as:

[0084]

[0085] The adaptive fuzzy control rate with external disturbance observer in step S8 is:

[0086]

[0087] in: Represents the absolute value of the maximum error in external load estimation.

[0088] The trajectory tracking accuracy and the suppression effect on external disturbances obtained by the method (C3) provided by the present invention are compared with other algorithms, and compared with traditional sliding mode control (C1) and traditional PD control (C2).

[0089] The ideal trajectory used in the simulation is Figure 3 shown.

[0090] The trajectory tracking accuracy results are as follows Figure 4 As shown, from Figure 4 It can be seen that the second norm of the tracking error of C3 is the smallest after convergence compared with other methods, so it can achieve better joint trajectory tracking performance than C1 and C2.

[0091] The comparison results of the suppression effect on external disturbances are as follows: Figure 5 As shown, from Figure 5 It can be seen that the control input force curve of C3 is smoother than that of C1, which achieves effective suppression of external disturbances.

[0092] The beneficial effects of the present invention are as follows: the method improves the joint trajectory tracking accuracy of the robot and improves the ability of traditional sliding mode control to suppress external disturbances, which is conducive to its popularization and use in practical applications.

[0093] For robots with other degrees of freedom, the gain values ​​in formulas (6), (8), (13) and (18) need to be adaptively adjusted.

[0094] In an exemplary embodiment, a collaborative robot is also provided, comprising: a processor; and a memory for storing instructions executable by the processor; wherein the processor is configured to execute the instructions in the memory to complete all or part of the steps of the above-mentioned method.

[0095] In an exemplary embodiment, a non-transitory computer-readable storage medium including instructions is also provided, such as a memory including instructions, and the instructions can be executed by a processor to complete all or part of the steps of the above method. For example, the non-transitory computer-readable storage medium can be a ROM, RAM, CD-ROM, magnetic tape, floppy disk, optical data storage device, etc.

Claims

1. A collaborative robot trajectory tracking control method, characterized in that: The following steps are involved: Establish the spatial dynamics model of the joints of the n-DOF collaborative robot and establish the expression of the joint tracking error; According to the joint tracking error expression, the sliding surface of the sliding mode controller is established; According to the sliding surface and the joint space dynamics model of the collaborative robot, the robust control rate of the collaborative robot based on sliding mode control is obtained. Establishing the robust control rate requires updating the expression of the parameter vector error; According to the expression of updating parameter vector error, the adaptive fuzzy control rate based on sliding mode controller is obtained; Establish an expression for the external load observation error; According to the external load observation error expression, a nonlinear state observer based on the external load observation error is obtained; Combining the robust control rate, adaptive fuzzy control rate and nonlinear state observer, the adaptive fuzzy control rate with external disturbance observer of the collaborative robot is obtained. The adaptive fuzzy control rate is used as input force to act on each joint of the robot to achieve trajectory tracking of the robot; The joint space dynamics model of the n-DOF collaborative robot is: Among them: q, are the joint angle, angular velocity and angular acceleration respectively; τ, τ v are joint torque and joint external disturbance torque respectively; D q , C q , G q They are the inertia matrix, centripetal Coriolis matrix, and gravity matrix of the collaborative robot; The joint tracking error expression is: Where: q d , are the expected angle and velocity of the joint respectively; e, They are joint tracking position error and velocity error respectively; The sliding surface and its derivative of the sliding mode controller are defined as: Among them: coefficient c is a positive diagonal matrix, is the joint angular acceleration tracking error; Robust control rate based on sliding mode control: in: are the estimated values ​​of the inertia matrix, the centripetal Coriolis matrix and the gravity matrix of the collaborative robot; s is the sliding mode variable; are auxiliary variables and their derivatives respectively, among which, K s is the switching gain matrix of the sliding surface, K p is a positive diagonal matrix, sgn(·) is a sign function; The expression of the parameter vector error required to update the robust control rate is: in: To update the parameter vector, is θ i An estimated value of Adaptive fuzzy control rate based on sliding mode controller: Among them, τ v is the actual external load force; λ is the basis function of the fuzzy system, and λ is the basis function of the fuzzy system for any centroid method defuzzifier y i The expression can be written as: The expression is: Where: m is the number of fuzzy rules; K d is a positive diagonal matrix; is the membership function that satisfies the jth rule in the fuzzy system; The expression of external load observation error is: in: is the estimated value of the external load moment; The nonlinear state observer based on the external observation error is: Z and are auxiliary variables and their derivatives respectively; the auxiliary variable L(q) is: L(q)=YD q (q) -1 (11) The auxiliary variable P(q) is: Where: Y is a positive definite diagonal matrix; The adaptive fuzzy control rate with external disturbance observer is: in: Represents the absolute value of the maximum error in external load estimation.

2. A collaborative robot trajectory tracking control system, characterized in that: include: processor; a memory for storing processor-executable instructions; Wherein, the processor is configured to execute the steps of the method of claim 1.

Images