Robot end effector motion control system

Abstract

The invention relates to the technical field of robot motion control systems and man-machine interaction, and discloses a robot end effector motion control system which comprises a motion control module, a logical operation and simulation control module, an HMI control interface module, TwinCAT3 software, an EtherCAT bus and a 2R1T end effector. The motion control module, the logic operation and simulation control module and the HMI control interface module are all based on TwinCAT3 software to carry out motion program development, the 2R1T end effector is a three-degree-of-freedom (namely, two-rotation and one-translation) end effector, and an EtherCAT bus is adopted to carry out data transmission and motion control with the 2R1T end effector. A function library in the motion control module is called in the logic operation and simulation control module, and a motion control instruction of the 2R1T end effector is generated; the method comprises the following steps: firstly, using an EtherCAT bus to transmit a motion control instruction to a 2R1T end effector, then using the EtherCAT bus to transmit the motion control instruction to the 2R1T end effector, controlling the 2R1T end effector to execute a corresponding motion instruction, monitoring parameters such as position and speed of the 2R1T end effector in real time through an HMI control interface module, and carrying out safety protection operation on the 2R1T end effector in time according to a monitoring result and error reporting information. According to the invention, the safety efficiency of motion control of the end effector of the robot and the real-time performance of data acquisition are improved, and the secondary development capability of the system is also correspondingly improved.

Description

Technical Field

[0001] The present invention relates to the technical fields of robot motion control systems and human-machine interaction, and specifically to a motion control system for a robot end effector. Background Art

[0002] With the increasingly widespread application of robots in industrial manufacturing, medical rehabilitation, agricultural harvesting, and service fields, the motion control system of the end effector not only needs to meet the requirements of rigidity, high speed, and stability in traditional industrial scenarios, but also needs to demonstrate higher flexibility, adaptability, and intelligence levels in diverse scenarios. This improvement in performance requirements has driven the overall evolution of the end effector motion control system from single tasks to complex tasks and multi-dimensional capabilities.

[0003] Traditional robot end effector control methods mainly rely on rigid mechanisms and simple kinematic planning. Although they exhibit high stability in specific fixed scenarios, their limitations become increasingly prominent when faced with dynamically changing environments or unstructured scenarios. For example, in the operation of surgical robots, the actuator needs to achieve sub-millimeter-level high-precision control while also taking into account compliance to cope with the complexity of human tissues; in the field of agricultural harvesting robots, the actuator needs to handle fruits and vegetables with uncertain shapes and easily damaged characteristics. The requirements for the control system in the above application scenarios far exceed the capabilities of traditional rigid control frameworks.

[0004] In order to research a new control technology that can simultaneously integrate "precision" and "compliance" to solve the above problems, based on the structural characteristics of the 2R1T end effector, the present invention has developed a motion control system for a robot end effector in the Beckhoff control software to achieve precise and compliant control of the robot end effector and meet the motion control requirements in complex situations. Summary of the Invention

[0005] Aiming at the deficiencies of the prior art, the present invention provides a motion control system for a robot end effector to solve the problems described above.

[0006] The present invention provides the following technical solution: A motion control system for a robot end effector, including a motion control module, a logic operation and simulation control module, an HMI control interface module, TwinCAT3 software, an EtherCAT bus, and a 2R1T end effector. The motion control module, the logic operation and simulation control module, and the HMI control interface module are all developed for motion programs based on TwinCAT3 software, and perform data transmission and motion control with the 2R1T end effector through the EtherCAT bus. The 2R1T end effector is a three-degree-of-freedom (i.e., two rotations and one translation) end effector and is installed on a six-degree-of-freedom industrial robot through a flange.

[0007] The motion control module includes a forward kinematics algorithm function library, an inverse kinematics algorithm function library, a motion configuration function library, an interpolation calculation function library, and a velocity planning function library. The forward kinematics algorithm function library is responsible for calculating the end pose of the 2R1T end effector. The inverse kinematics algorithm function library is responsible for calculating the motion control instructions for the servo drivers of the driving elements of the 2R1T end effector. The inputs and outputs of the forward kinematics algorithm function library and the inverse kinematics algorithm function library are opposite. The motion configuration function library is responsible for configuring and setting the parameters of the basic motion function library corresponding to the voice coil motor of the 2R1T end effector's actuator. The interpolation calculation function library is responsible for planning the motion trajectory of the 2R1T end effector. The velocity planning function library sets and plans the motion speed of the voice coil motor of the 2R1T end effector's actuator. Data between the motion control module and the logic operation and simulation control module is transmitted and exchanged through variable linking.

[0008] The logic operation and simulation control module includes a logic instruction operation library and a motion simulation monitoring library. The logic instruction operation library is responsible for operating and transmitting the motion control instructions of various 2R1T end effectors in the motion control module. The motion simulation monitoring library is responsible for monitoring the relevant parameters operated by the logic instruction operation library. The relevant motion control instructions between the logic operation and simulation control module and the 2R1T end effector are transmitted and the motion is controlled through the EtherCAT bus.

[0009] The HMI control interface module includes a main control interface library, a single-axis control interface library, and a multi-axis control interface library. The main control interface library is responsible for the preparatory work before the motion control of the voice coil motor of the 2R1T end effector's actuator. The single-axis control interface library is responsible for the motion control of a certain motor in the voice coil motor of the 2R1T end effector's actuator. The multi-axis control interface library is responsible for the simultaneous motion control of the voice coil motors of the 2R1T end effector's actuator. The main control interface library, the single-axis control interface library, and the multi-axis control interface library can switch interfaces with each other. Data between the HMI control interface module and the logic operation and simulation control module is transmitted and exchanged through variable linking.

[0010] The 2R1T end effector is a three-degree-of-freedom (i.e., two rotations and one translation) end effector, including a driving element, an executing element, and a measuring element. The driving element is the servo driver of the 2R1T end effector, responsible for parsing and calculating the motion control instructions and transmitting them to the executing element. The executing element is the voice coil motor of the 2R1T end effector, responsible for receiving the motion instructions parsed and calculated by the driving element and performing corresponding motions. The measuring element is the incremental encoder of the 2R1T end effector, responsible for real-time feedback of relevant information such as the position and speed of the executing element to the HMI control interface module. The 2R1T end effector is installed on the six-degree-of-freedom industrial robot through a flange plate.

[0011] Preferably, the motion configuration function library includes an enabling function block, a jogging function block, a stop function block, a reset function block, and a homing function block. The enabling function block is responsible for powering on the executing element of the 2R1T end effector. The jogging function block is responsible for performing motion tests and motion controls on the executing element of the 2R1T end effector. The stop function block is responsible for stopping the motion of the executing element of the 2R1T end effector. The reset function block is responsible for resetting the motion of the executing element of the 2R1T end effector. The homing function block is responsible for controlling the executing element of the 2R1T end effector to return to the zero point.

[0012] Preferably, the interpolation calculation function library includes a linear interpolation function block and a circular interpolation function block. The linear interpolation function block is responsible for planning the linear motion trajectory of the 2R1T end effector. The circular interpolation function block is responsible for planning the curved motion trajectory of the 2R1T end effector.

[0013] The main control interface library includes a current position display block, a current speed display block, a control button function block, a status light display block, and a control interface conversion block. The current position display block is responsible for displaying the current position of the executing element of the 2R1T end effector. The current speed display block is responsible for displaying the current speed of the executing element of the 2R1T end effector. The control button function block is responsible for performing motion controls on the executing element of the 2R1T end effector. The status light display block is responsible for displaying various motion control states of the executing element of the 2R1T end effector. The control interface conversion block is responsible for implementing the control mode switching of the executing element of the 2R1T end effector.

[0014] Preferably, the single-axis control interface library includes a control interface conversion block, a select axis function block, a jog function block, a single-axis control button function block, and a single-axis status light display block. The control interface conversion block is responsible for implementing the control mode switching of the actuators of the 2R1T end effector. The select axis function block is responsible for selecting one of the axes of the actuators of the 2R1T end effector. The jog function block performs jog control on the selected axis. The control button function block is responsible for the motion control of the actuators of the 2R1T end effector. The single-axis status light display block is responsible for displaying various motion control states of the actuators of the 2R1T end effector.

[0015] Preferably, the multi-axis control interface library includes a current position display block, a current speed display block, a control button function block, a control interface conversion block, a multi-axis status light display block, a starting position display block, and an ending position display block. The current position display block is responsible for displaying the current position of the actuators of the 2R1T end effector. The current speed display block is responsible for displaying the current speed of the actuators of the 2R1T end effector. The control button function block is responsible for the motion control of the actuators of the 2R1T end effector. The control interface conversion block is responsible for implementing the control mode switching of the actuators of the 2R1T end effector. The multi-axis status light display block is responsible for displaying various motion control states of the actuators of the 2R1T end effector. The starting position display block is responsible for displaying the starting position of the actuators of the 2R1T end effector before executing the planned trajectory. The ending position display block is responsible for displaying the ending position of the actuators of the 2R1T end effector after executing the planned trajectory.

[0016] Compared with the prior art, the present invention has the following beneficial effects:

[0017] 1. For different industrial robot processing scenarios, the motion control system of the robot end effector can achieve compliant and precise control through the HMI control interface module, and can real-time feedback the end pose information, which is convenient for early safety detection and accident avoidance, improving the safety efficiency.

[0018] 2. Compared with the traditional motion control system of the robot end effector, the motion control system of the robot end effector can achieve module extensibility, and can be combined with the motion instruction modules developed on different software platforms, improving the secondary development ability of the system.

[0019] 3. The motion control system of the robot end effector uses the EtherCAT bus to achieve real-time communication and data transmission, and establishes a master-slave working mode and a data transmission mode. This working mode combines the offline control method and the online control method, and can complete the data interaction between the virtual prototype and the physical prototype, improving the real-time performance of data acquisition. Description of the Drawings

[0020] Figure 1 This is the framework diagram of the motion control system for the end effector of the robot of the present invention;

[0021] Figure 2 This is the framework diagram of the motion configuration function library of the present invention;

[0022] Figure 3 This is the framework diagram of the interpolation calculation function library of the present invention;

[0023] Figure 4 This is the flowchart of the linear interpolation of the present invention;

[0024] Figure 5 This is the flowchart of the velocity planning of the present invention;

[0025] Figure 6 This is the framework diagram of the main control interface library of the present invention;

[0026] Figure 7 This is the framework diagram of the single-axis control interface library of the present invention;

[0027] Figure 8 This is the framework diagram of the multi-axis control interface library of the present invention;

[0028] Figure 9 This is the main control interface diagram of the present invention;

[0029] Figure 10 This is the single-axis control interface diagram of the present invention;

[0030] Figure 11 This is the multi-axis control interface diagram of the present invention;

[0031] Figure 12 This is the framework diagram of the EtherCAT bus of the present invention;

[0032] Figure 13 This is the framework diagram of the industrial robot control platform of the present invention;

[0033] In the figure: 1. Motion control module; 2. Logic operation and simulation control module; 3. HMI control interface module; 4. TwinCAT3 software; 5. EtherCAT bus; 6. 2R1T end effector; 7. Six-degree-of-freedom industrial robot; 8. Forward kinematics algorithm function library; 9. Inverse kinematics algorithm function library; 10. Motion configuration function library; 11. Interpolation calculation function library; 12. Velocity planning function library; 13. Logic instruction operation library; 14. Motion simulation monitoring library; 15. Main control interface library; 16. Single-axis control interface library; 17. Multi-axis control interface library; 18. Driving element; 19. Executing element; 20. Measuring element; 21. Enable function block; 22. Jog function block; 23. Stop function block; 24. Reset function block; 25. Homing function block; 26. Linear interpolation function block; 27. Circular interpolation function block; 28. Current position display block; 29. Current speed display block; 30. Control button function block; 31. Status light display block; 32. Control interface conversion block; 33. Axis selection function block; 34. Jog function block; 35. Control button function block; 36. Status light display block; 37. Multi-axis status light display block; 38. Starting position display block; 39. Ending position display block. Detailed implementation manners

[0034] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. It should be noted that the embodiments described in the present invention are only partial embodiments, not all embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0035] Please refer to Figures 1-13, A motion control system for a robot end effector, comprising a motion control module (1), a logic operation and simulation control module (2), an HMI control interface module (3), TwinCAT3 software (4), an EtherCAT bus (5), and a 2R1T end effector (6). It is characterized in that the motion control module (1), the logic operation and simulation control module (2), and the HMI control interface module (3) all develop motion programs based on the TwinCAT3 software (4), and perform data transmission and motion control with the 2R1T end effector (6) through the EtherCAT bus (5). The 2R1T end effector (6) is a three-degree-of-freedom (i.e., two rotations and one translation) end effector, and is installed on a six-degree-of-freedom industrial robot (7) through a flange. The motion control module (1) includes a forward kinematic algorithm function library (8), an inverse kinematic algorithm function library (9), a motion configuration function library (10), an interpolation calculation function library (11), and a velocity planning function library (12). The forward kinematic algorithm function library (8) is responsible for calculating the end pose of the 2R1T end effector. The inverse kinematic algorithm function library (9) is responsible for calculating the motion control instructions for the servo drive (18) of the driving element of the 2R1T end effector. The inputs and outputs of the forward kinematic algorithm function library (8) and the inverse kinematic algorithm function library (9) are opposite. The motion configuration function library (10) is responsible for configuring and setting parameters for the basic motion function library corresponding to the voice coil motor (19) of the executing element of the 2R1T end effector. The interpolation calculation function library (11) is responsible for planning the motion trajectory of the 2R1T end effector (6). The velocity planning function library (12) sets and plans the motion speed of the voice coil motor (19) of the executing element of the 2R1T end effector. Data between the motion control module (1) and the logic operation and simulation control module (2) is transmitted and exchanged through variable linking.

[0036] In this embodiment, the forward kinematic algorithm function library (8) derives the end pose information of the end effector according to the forward kinematic formula of the 2R1T end effector (6), as shown in formula (1), and programs and calls it in the motion control module (1);

[0037] The forward kinematic derivation formula of the forward kinematic algorithm function library (8) is: Where: H 0 is the initial height (when the three active moving joints are fully retracted); q i (i = 1, 2, 3) are the displacement amounts of the three active moving joints of the 2R1T end effector (6); are the pose parameters of the end moving platform of the end effector;

[0038] In this embodiment, the inverse kinematics algorithm function library (9) derives the displacement of the three active moving joints of the end effector according to the inverse kinematics formula of the 2R1T end effector (6), as shown in formula (2), and programs and calls it in the motion control module (1);

[0039] The inverse kinematics derivation formula of the inverse kinematics algorithm function library (9) is: where q i (i = 1, 2, 3) is the displacement of the three active moving joints of the 2R1T end effector (6); is the pose parameter of the moving platform at the end of the end effector.

[0040] In this embodiment, the motion configuration function library (10) includes an enable function block (21), a jog function block (22), a stop function block (23), a reset function block (24), and a homing function block (25). The enable function block (21) is responsible for powering on the actuators (19) of the 2R1T end effector. The jog function block (22) is responsible for performing motion tests and motion control on the actuators (19) of the 2R1T end effector. The stop function block (23) is responsible for stopping the motion of the actuators (19) of the 2R1T end effector. The reset function block (24) is responsible for resetting the motion of the actuators (19) of the 2R1T end effector. The homing function block (25) is responsible for controlling the actuators (19) of the 2R1T end effector to return to the zero point.

[0041] The enable function block (21) mainly enables and prepares the three voice coil motors of the 2R1T end effector (6) through the following control codes;

[0042] The enable control instruction power1(Axis := axis.axis1, Enable := axis_enable, Enable_Positive := TRUE, EnableNegative := TRUE, Override :=, BufferMode :=, Options :=, Status =>, Busy =>, Active =>, Error =>, ErrorID =>) enables the power of the voice coil motor. In this control code, power1 is the name of the enable function for the corresponding axis being controlled, axis.axis1 is the name of the axis being controlled, axis_enable is the trigger block of this function block instruction, Enable_Positive supports the forward rotation of the voice coil motor, Enable_Negative supports the reverse rotation of the voice coil motor. The control instructions for the enable function blocks written for the three voice coil motor axes are basically the same, and only the name of the controlled axis needs to be changed;

[0043] The jog function block (22) is responsible for performing basic motion tests and motion control on the three voice coil motors of the 2R1T end effector;

[0044] The jog control instruction jog1 (Axis := axis.axisl, JogForward := axis_jogfor, JogBackwards :=, Mode :=, Position :=, Velocity :=, Acceleration :=, Deceleration :=, Jerk :=, Done =>, Busy =>, Active =>, CommandAborted =>, Error =>, ErrorID =>) performs basic motion tests and motion control on the voice coil motors. jog1 is the enabled function name for the corresponding axis to be controlled, axis.axis1 is the name of the axis to be controlled, and axis_jogfor is the trigger block for this function block instruction. The jog function block control instructions written for the three voice coil motor axes are basically the same, and only the name of the controlled axis needs to be changed;

[0045] The stop function block (23) is responsible for stopping the motion of the three voice coil motors of the 2R1T end effector;

[0046] The stop control instruction stop1 (Axis := axis.axis1, Execute := stop_do, Deceleration :=, Jerk :=, Options :=, Done =>, Busy =>, Active =>, CommandAborted =>, Error =>, ErrorID =>) controls the voice coil motors to stop. stop1 is the enabled function name for the corresponding axis to be controlled, axis.axisl is the name of the axis to be controlled, and stop_do is the trigger block for this function block instruction. The stop function block control instructions written for the three voice coil motor axes are basically the same, and only the name of the controlled axis needs to be changed;

[0047] The reset function block (24) is responsible for resetting the motion of the three voice coil motors of the 2R1T end effector;

[0048] The reset control instruction reset1 (Axis := axis.axis1, Execute := reset__do, Done =>, Busy =>, Error =>, Error ID =>) controls the voice coil motors to be reset. reset1 is the enabled function name for the corresponding axis to be controlled, axis.axis1 is the name of the axis to be controlled, and reset_do is the trigger block for this function block instruction. The reset function block control instructions written for the three voice coil motor axes are basically the same, and only the name of the controlled axis needs to be changed;

[0049] The homing function block (25) is responsible for controlling the three voice coil motors of the 2R1T end effector to return to the zero point;

[0050] The homing control instruction home1(Axis := axis.axis1, Execute := home_do, Position := 0, HomingMode :=, BufferMode :=, Options :=, bCalibrationCam := sensor, Done =>, Busy =>, Active =>, CommandAborted =>, Error =>, ErrorID =>) controls the voice coil motor to return to zero, so that the voice coil motor is at the zero point before reporting an error or moving. home1 is the name of the enabling function for the corresponding axis to be controlled, axis.axis1 is the name of the axis to be controlled, and home_do is the trigger block of the function block instruction. The homing function block control instructions written for the three voice coil motor axes are basically the same, and only the name of the controlled axis needs to be changed;

[0051] In this embodiment, the above-written function block control instructions are instantiated and called, and integrated into the motion configuration function library (21), and then called by the logic operation and simulation control module (2) together with other control instruction function libraries.

[0052] In this embodiment, the interpolation calculation function library (11) includes a linear interpolation function block (26) and a circular interpolation function block (27). The linear interpolation function block (26) is responsible for planning the linear motion trajectory of the 2R1T end effector (6), and the circular interpolation function block (27) is responsible for planning the curved motion trajectory of the 2R1T end effector (6);

[0053] The linear interpolation function block (26) is responsible for planning the linear motion trajectory of the 2R1T end effector (6). By setting the starting position, ending position, and speed of the end effector in the Cartesian space, then calculating the linear length L and the number of interpolation times N to obtain the displacement component dx of each axis, and according to whether the number of interpolation times meets the requirements, gradually update the coordinates of each interpolation point on the planned trajectory, and finally control the voice coil motor axis to move step by step according to the planned trajectory;

[0054] The circular interpolation function block (27) is responsible for planning the curved motion trajectory of the 2R1T end effector (6). The interpolation step size for each period can be calculated according to the velocity-planned curve, and using the interpolation step size and the interpolation point parameter u of the i-th period i , the interpolation point parameter u of the (i + 1)-th period can be obtained i+1 , and the position point coordinates of the (i + 1)-th period can be obtained according to the set planned trajectory expression;

[0055] The interpolation motion step size for the movement of each voice coil motor shaft is calculated as follows: L = v i × T s (3) where v i is the movement speed of the voice coil motor shaft for each interpolation period, and T s is the interpolation period of the control system;

[0056] The interpolation point parameter u of the next period can be obtained by setting the step size, i+1 that is, the calculation method of the interpolation point parameter of the discrete straight line segment is: u i+1 = u i + T c (4)

[0057] The recursive calculation of the interpolation point parameter of the space arc can be obtained through the above formula: u i+1 = u i + L / R (5)

[0058] Substitute the interpolation point parameter of the next period into the trajectory curve expression to calculate the interpolation point coordinates. Among them, the coordinates P i+1 at u i+1 of the straight line segment are calculated as: where P i+1 is the point on the curve when the node parameter is u i+1 , P s is the starting coordinate of the straight line segment, P e is the ending coordinate of the straight line segment, u s is the interpolation point parameter of the starting point, u e is the interpolation point parameter of the ending point (after the interpolation curve is given, u e , u e are known quantities, default: u s = 0, u e = 1);

[0059] The coordinate calculation of the interpolation point on the arc is: P i+1 = P c + R(u i+1 - u s , N)(P s - P c ) (7) where R(u i+1 - u s ,N) represents the rotation of the vector around N by u i+1 - u s angle, N is the normal vector of the space circle, Pc is the center coordinate, P s is the starting coordinate, u s is the node vector at the starting point;

[0060] Among them, after the interpolation calculation is completed, the command trajectory curve has been discretized into position coordinate values, and then through calculation and analysis, it is transmitted to each voice coil motor shaft to track the trajectory curve;

[0061] The interpolation calculation function library is characterized in that, first, each position coordinate command is transmitted to the main program of the PLC module, and the inverse kinematics solution function library is called to solve the position coordinate command. Second, the solved motion command is transmitted to the NC axis, and the NC axis transmits the control command to each driver after calculation and analysis. Finally, the driver receiving the control command drives each voice coil motor shaft to control the movement of the entire end effector to track the trajectory.

[0062] In this embodiment, the speed planning function library (12) sets and plans the motion speed of the voice coil motor, which is the actuator of the 2R1T end effector. The speed planning function library first obtains the initial end speed value of each planned trajectory according to the relevant parameters of the planned trajectory and the dynamic performance influence factor of the 2R1T end effector. Then, it judges whether the initial set speed and the initial end speed meet the conditions through the length of each planned trajectory. After the overall speed setting is completed, the motion control instruction code will be converted into an instruction queue and transmitted to the lower computer through ADS communication for interpolation calculation.

[0063] In this embodiment, the logic instruction operation library (13) is responsible for operating and transmitting various motion control instructions of the 2R1T end effector (6) in the motion control module (1). By setting the relevant parameters of the voice coil motor shaft in this module, and then calling the previously written and improved function library, the parameters in this module are connected and transmitted with the parameters in the motion control module (2) through variable linking.

[0064] In this embodiment, the motion simulation monitoring library (14) is responsible for monitoring the relevant parameters operated by the logic instruction operation library (13). By using the Scope view library to select the variables to be monitored, the real-time monitoring of the motion parameters of the voice coil motor can be realized in the Scope view library interface, which is convenient for ensuring the correctness of the voice coil motor control instruction in advance.

[0065] In this embodiment, the main control interface library (15) is responsible for the preparatory work before the movement control of the voice coil motor (19), which is the actuator of the 2R1T end effector. The main control interface library (15) includes a current position display block (28), a current speed display block (29), a control button function block (30), a status light display block (31), and a control interface conversion block (32). In this interface library, the user can achieve corresponding movement functions by pressing the corresponding control buttons, and can observe the current position and current speed of the voice coil motor shaft in real time. Then, the control interface can also be switched according to the movement control requirements;

[0066] The current position display block (28) is responsible for displaying the current position of the actuator (19) of the 2R1T end effector. By setting a control in the TwinCAT3 software HMI and linking the actual position variable MAIN.axis_axis1.NcToPlc.ActPos of the current control axis to this control, when running the movement control code, the user can observe the current position of the voice coil motor shaft through this display block. The control instructions written for the current position display blocks of the three voice coil motor shafts are basically the same, and only the control axis name needs to be changed;

[0067] The current speed display block (29) is responsible for displaying the current speed of the actuator (19) of the 2R1T end effector. By setting a control in the TwinCAT3 software HMI and linking the actual position variable MAIN.axis_axis1.NcToPlc.ActVel of the current control axis to this control, when running the movement control code, the user can observe the current position of the voice coil motor shaft through this display block. The control instructions written for the current speed display blocks of the three voice coil motor shafts are basically the same, and only the control axis name needs to be changed;

[0068] The control button function block (30) is responsible for the movement control of the actuator (19) of the 2R1T end effector. By setting multiple controls in the TwinCAT3 software HMI and linking the movement function instructions of the current control axis to these controls, such as axis_enable, stop_do, and reset_do, etc., the user can control the voice coil motor shaft through this control button function to achieve the corresponding preparatory work;

[0069] The status light display block (31) is responsible for displaying various movement control states of the actuator (19) of the 2R1T end effector. By setting a lamp icon in the TwinCAT3 software HMI and linking the movement function instructions of the current control axis to this control, such as power, stop, etc., when the user controls the voice coil motor shaft through this control button function to achieve the corresponding preparatory work, the corresponding lamp icon will light up, indicating that this movement control function block is ready;

[0070] The control interface conversion block (32) is responsible for implementing the control mode switching of the actuating element (19) of the 2R1T end effector. By setting multiple control interface libraries in the TwinCAT3 software HMI and linking the control interface name instruction required to complete the corresponding instruction to this interface library, the user can achieve multi-interface switching, and then can perform corresponding control function control setting in the switched interface.

[0071] In this embodiment, the single-axis control interface library (16) is responsible for the motion control of a certain motor in the voice coil motor (19) of the actuating element of the 2R1T end effector, including the control interface conversion block (32), the select axis function block (33), the jog function block (34), the single-axis control button function block (35) and the single-axis status light display block (36). In this interface library, the user can achieve the corresponding motion function by pressing the corresponding control button, and can observe the position and speed of the voice coil motor axis in real time, and then can also achieve control interface switching according to the motion control requirements.

[0072] The select axis function block (33) is responsible for selecting a certain axis of the actuating element (19) of the 2R1T end effector. By setting multiple controls in the TwinCAT3 software HMI and linking the motion function instruction of the current control axis to this control, such as axis_axis1, axis_axis2 and axis_axis3, after the user enables the corresponding axis in the main control interface library and switches to this interface library, the user can select the voice coil motor axis to be controlled through this control button function for motion control.

[0073] The jog function block (34) performs jog control on the selected voice coil motor axis. By setting a control in the TwinCAT3 software HMI and linking the motion function instruction of the current control axis to this control, after the user selects the voice coil motor axis to be controlled in this interface library, then controls this axis to perform forward and reverse motions according to the control requirements.

[0074] The single-axis control button function block (35) is responsible for the motion control of the actuating element (19) of the 2R1T end effector. By setting multiple controls in the TwinCAT3 software HMI and linking the motion function instruction of the current control axis to this control, such as axis_enable, stop_do and reset_do, etc., the user can control the voice coil motor axis through this control button function to achieve the corresponding preparation work.

[0075] The single-axis status light display block (36) is responsible for displaying various motion control states of the actuator (19) of the 2R1T end effector. By setting the lamp icon in the TwinCAT3 software HMI and linking the motion function commands of the current control axis to this control, such as power, stop, and jog, when the user controls the voice coil motor axis through the control button function to achieve the corresponding preparation work, the corresponding lamp icon will light up, indicating that the motion control function block is ready.

[0076] The multi-axis control interface library (17) is responsible for simultaneously controlling the motion of the voice coil motor (19), which is the actuator of the 2R1T end effector. It includes the current position display block (28), the current speed display block (29), the control button function block (30), the control interface conversion block (32), the multi-axis status light display block (37), the starting position display block (38), and the ending position display block (39). In this interface library, the user can achieve the corresponding motion function by pressing the corresponding control button, that is, control the voice coil motor to move along the planned trajectory, and can observe the position and speed of the voice coil motor axis in real time. Then, the control interface can also be switched according to the motion control requirements;

[0077] The multi-axis status light display block (37) is responsible for displaying various motion control states of the actuator (19) of the 2R1T end effector. By setting the lamp icon in the TwinCAT3 software HMI and linking the motion function commands of the current control axis to this control, such as power, stop, and reset, when the user controls the voice coil motor axis through the control button function to achieve the corresponding preparation work, the corresponding lamp icon will light up, indicating that the motion control function block is ready;

[0078] The starting position display block (38) is responsible for displaying the starting position of the actuator (19) of the 2R1T end effector before executing the planned trajectory. The user can input the trajectory starting position into this display block according to the control requirements. Then, after inputting the trajectory ending position, the planning function of the interpolation calculation function library is called in the logic operation and control simulation module, and the current position and current speed of the voice coil motor axis can be observed in the current position display block (28) and the current speed display block (29);

[0079] The ending position display block (39) is responsible for displaying the ending position of the actuator (19) of the 2R1T end effector after executing the planned trajectory. The user can input the trajectory starting position into this display block according to the control requirements. Then, after inputting the trajectory ending position, the planning function of the interpolation calculation function library is called in the logic operation and control simulation module, and the current position and current speed of the voice coil motor axis can be observed in the current position display block (28) and the current speed display block (29);

[0080] By integrating the above three control interface libraries into the HMI control interface module (3), users can flexibly achieve the motion control and real-time data monitoring of the end effector according to their own control requirements. The data between the HM control interface module (3) and the logic operation and simulation control module (2) is transmitted and exchanged through variable linking.

[0081] In this embodiment, the 2R1T end effector (6) is a three-degree-of-freedom (i.e., two rotations and one translation) end effector, including a driving element (18), an executing element (19), and a measuring element (20). This end effector is the main body of the motion control system of the end effector of the robot. By setting parameters and establishing a communication connection for this end effector, the corresponding motion control instructions can be completed.

[0082] The driving element (18) is the servo driver of the 2R1T end effector (6), which is responsible for parsing and calculating the motion control instructions and transmitting them to the executing element (19). By establishing a communication connection and a data transmission channel between this servo driver and the control system through the EtherCAT bus, the motion control instructions in the control system can be transmitted to the servo driver through this communication bus, and after being parsed and calculated by the driver, they are transmitted to the voice coil motor and drive the voice coil motor to move.

[0083] The executing element (19) is the voice coil motor of the 2R1T end effector (6), which is responsible for receiving the motion instructions parsed and calculated by the driving element (18) and performing corresponding motions. After receiving the corresponding motion control instructions, it can execute the motion actions.

[0084] The measuring element (20) is the incremental encoder of the 2R1T end effector (6), which is responsible for real-time feedback of relevant information such as the position and speed of the executing element (19) to the HMI control interface module (3). After establishing a data transmission channel and a communication connection relationship between the relevant parameters of the incremental encoder and the NC module in the control system and performing corresponding parameter settings, the corresponding position and speed of the voice coil motor can be read in real time.

[0085] In this embodiment, the 2R1T end effector (6) is installed on the six-degree-of-freedom industrial robot (7) through a flange, and a communication connection and a data transmission channel are established with the control system through the EtherCAT bus. The motion control instructions in the control system can be transmitted to the servo driver through this communication bus, and after being parsed and calculated by the driver, they are transmitted to the voice coil motor and drive the voice coil motor to move.

[0086] In this embodiment, a master-slave working mode is established through the EtherCAT bus. The key to this working mode is the construction of the EtherCAT master station and slave stations. The PC and the Beckhoff controller serve as the master station, and the slave station devices are three servo drivers of the 2R1T end effector. Data exchange and real-time processing are carried out through the EtherCAT protocol. This technology abandons the traditional mode of first receiving Ethernet data packets and then processing them. Instead, it directly decodes the data packets and efficiently copies the data to each slave station device. Subsequently, these slave station devices immediately receive and process the data. This innovative mechanism significantly reduces the time delay of system data transmission to only a few nanoseconds, greatly accelerating the communication process between the master station controller and other external slave station devices and significantly improving the communication efficiency.

[0087] In this embodiment, the control system is developed based on the TwinCAT3 software platform, and the communication connection and data transmission channel with the 2R1T end effector are realized through the EtherCAT bus. Specifically, its characteristics are as follows: the motion control steps of the end effector are as follows:

[0088] S1. Power on the industrial robot hardware platform through the industrial robot electrical control cabinet;

[0089] S2. The user opens the motion control system code of the end effector in the TwinCAT3 software, and realizes the communication connection and data transmission channel with the end effector through the network cable interface. Connect the physical axis of the voice coil motor to the virtual axis in the TwinCAT3 software, and then write the virtual axis into the control code to realize the data link between the physical axis and the parameter axis in the code;

[0090] S3. The user controls the enabling and homing of the voice coil motor in the control system HMI control interface module to make the voice coil motor in the zero position;

[0091] S4. The user executes the corresponding motion function button in the control interface according to their own control requirements in the HMI control interface module;

[0092] S5. After the user presses the motion function button, the background logic operation and simulation control module will execute the corresponding control code according to the motion control instruction, and at the same time will call the required motion function library to perform code operations;

[0093] S6. The motion control instruction generated after the above module calculates the corresponding code will reach the servo driver of the 2R1T end effector through the established data transmission channel via the virtual axis;

[0094] S7. After the servo driver receives the motion control instruction, it will perform parsing and calculation, and then transmit the parsed and calculated control instruction to the physical axis of the voice coil motor;

[0095] After the physical axis of the voice coil motor receives a control command, it performs corresponding control actions. At the same time, the incremental encoder feeds back the position and speed information of the physical axis of the voice coil motor to the HMI control interface module;

[0096] S9. The user can adjust the motion control command in a timely manner according to the feedback position and speed information and perform safety protection operations in advance.

[0097] In summary, in the specific operation steps, data transmission and the execution of motion control commands are carried out synchronously. Data acquisition and transmission are performed once every 2 ms, and the real-time collected data is monitored so that the user can adjust the motion control command in a timely manner.

[0098] In the present invention, a safety protection program and an external hardware protection device are also provided in the control system to avoid incorrect operations during the motion control of the end effector and play a safety protection role.

[0099] In the present invention, unless otherwise clearly defined and limited, terms such as "link", "connect", "install" should be understood in a broad sense. For example, it can be a fixed connection, a detachable connection, or integrated; it can be a mechanical connection, an electrical connection, or a communication; it can be the internal communication of two components or the interaction relationship between two components. For those of ordinary skill in the art, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0100] Although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: they can still modify the technical solutions described in the foregoing embodiments, or perform equivalent replacements for some of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A robot end effector motion control system, comprising a motion control module (1), a logic operation and simulation control module (2), an HMI control interface module (3), TwinCAT3 software (4), an EtherCAT bus (5) and a 2R1T end effector (6), characterized in that: The motion control module (1), the logic operation and simulation control module (2), and the HMI control interface module (3) are all based on TwinCAT3 software (4) for motion program development, and are used for data transmission and motion control with a 2R1T end effector (6) via an EtherCAT bus (5). The 2R1T end effector (6) is a three-degree-of-freedom (i.e., two rotations and one translation) end effector and is mounted on a six-degree-of-freedom industrial robot (7) via a flange. The motion control module (1) comprises a forward algorithm function library (8), an inverse algorithm function library (9), a motion configuration function library (10), an interpolation calculation function library (11) and a speed planning function library (12). The forward algorithm function library (8) is responsible for calculating the end posture of the 2R1T end effector, and the inverse algorithm function library (9) is responsible for calculating the motion control instructions of the servo driver (18) of the 2R1T end effector drive element. The input and output of the forward algorithm function library (8) and the inverse algorithm function library (9) are opposite. The motion configuration function library (10) is responsible for configuring and setting parameters of the motion basic function library corresponding to the voice coil motor (19) of the 2R1T end effector actuator, the interpolation calculation function library (11) is responsible for planning the motion trajectory of the 2R1T end effector (6), the speed planning function library (12) is responsible for setting and planning the motion speed of the voice coil motor (19) of the 2R1T end effector actuator, and the data between the motion control module (1) and the logic operation and simulation control module (2) are transmitted and exchanged through variable linking. The logic operation and simulation control module (2) comprises a logic instruction operation library (13) and a motion simulation monitoring library (14). The logic instruction operation library (13) is responsible for operating and transmitting motion control instructions of various 2R1T end effectors (6) in the motion control module (1). The motion simulation monitoring library (14) is responsible for monitoring relevant parameters of the operation of the logic instruction operation library (13). The relevant motion control instructions between the logic operation and simulation control module (2) and the 2R1T end effectors (6) are transmitted and controlled via an EtherCAT bus (5). The HMI control interface module (3) comprises a main control interface library (15), a single-axis control interface library (16) and a multi-axis control interface library (17). The main control interface library (15) is responsible for the preparation work before the motion control of the voice coil motor (19) of the 2R1T end effector actuator. The single-axis control interface library (16) is responsible for the motion control of a motor in the voice coil motor (19) of the 2R1T end effector actuator. The multi-axis control interface library (17) is responsible for the simultaneous motion control of the voice coil motors (19) of the 2R1T end effector actuator. The main control interface library (15), the single-axis control interface library (16) and the multi-axis control interface library (17) can switch interfaces with each other. Data between the HMI control interface module (3) and the logic operation and simulation control module (2) are transmitted and exchanged through variable linking. The 2R1T end effector (6) is a three-degree-of-freedom (i.e., two rotations and one translation) end effector, comprising a driving element (18), an actuator (19) and a measuring element (20). The driving element (18) is a servo driver of the 2R1T end effector (6), responsible for analyzing and calculating motion control instructions and transmitting them to the actuator (19). The actuator (19) is a voice coil motor of the 2R1T end effector (6), responsible for receiving the motion instructions analyzed and calculated by the driving element (18) and performing corresponding movements. The measuring element (20) is an incremental encoder of the 2R1T end effector (6), responsible for feeding back relevant information such as the position and speed of the actuator (19) to the HMI control interface module (3) in real time. The 2R1T end effector (6) is mounted on a six-degree-of-freedom industrial robot (7) via a flange.

2. A robot end effector motion control system according to claim 1, characterized in that: The motion configuration function library (10) comprises an enable function block (21), a jog function block (22), a stop function block (23), a reset function block (24) and a return to zero function block (25), wherein the enable function block (21) is responsible for powering on the actuator (19) of the 2R1T end effector, the jog function block (22) is responsible for motion testing and motion control of the actuator (19) of the 2R1T end effector, the stop function block (23) is responsible for stopping the motion of the actuator (19) of the 2R1T end effector, the reset function block (24) is responsible for resetting the motion of the actuator (19) of the 2R1T end effector, and the return to zero function block (25) is responsible for controlling the actuator (19) of the 2R1T end effector to return to zero.

3. A robot end effector motion control system according to claim 1, characterized in that: The interpolation calculation function library (11) comprises a linear interpolation function block (26) and a circular interpolation function block (27), wherein the linear interpolation function block (26) is responsible for planning a linear motion trajectory of a 2R1T end effector (6), and the circular interpolation function block (27) is responsible for planning a curved motion trajectory of the 2R1T end effector (6).

4. The robot end effector motion control system according to claim 1, characterized in that: The main control interface library (15) includes a current position display block (28), a current speed display block (29), a control button function block (30), a status light display block (31) and a control interface conversion block (32). The current position display block (28) is responsible for displaying the current position of the actuator (19) of the 2R1T end effector, the current speed display block (29) is responsible for displaying the current speed of the actuator (19) of the 2R1T end effector, the control button function block (30) is responsible for performing motion control of the actuator (19) of the 2R1T end effector, the status light display block (31) is responsible for displaying various motion control states of the actuator (19) of the 2R1T end effector, and the control interface conversion block (32) is responsible for implementing control mode switching of the actuator (19) of the 2R1T end effector.

5. The robot end effector motion control system according to claim 1, characterized in that: The single-axis control interface library (16) comprises a control interface conversion block (32), an axis selection function block (33), a jog function block (34), a single-axis control button function block (35) and a single-axis status light display block (36). The control interface conversion block (32) is responsible for realizing the control mode switching of the actuator (19) of the 2R1T end effector. The axis selection function block (33) is responsible for selecting a certain axis of the actuator (19) of the 2R1T end effector. The jog function block (34) performs jog control on the selected axis. The control button function block (35) is responsible for performing motion control of the actuator (19) of the 2R1T end effector. The single-axis status light display block (36) is responsible for displaying various motion control states of the actuator (19) of the 2R1T end effector.

6. A robot end effector motion control system according to claim 1, characterized in that: The multi-axis control interface library (17) comprises a current position display block (28), a current speed display block (29), a control button function block (30), a control interface conversion block (32), a multi-axis status light display block (37), a start position display block (38) and an end position display block (39), wherein the current position display block (28) is responsible for displaying the current position of the actuator (19) of the 2R1T end effector, the current speed display block (29) is responsible for displaying the current speed of the actuator (19) of the 2R1T end effector, and the control button function block (30) is responsible for performing the 2R1T end effector operation. The control interface conversion block (32) is responsible for realizing the control mode switching of the actuator (19) of the 2R1T end actuator, the multi-axis status light display block (37) is responsible for displaying various motion control states of the actuator (19) of the 2R1T end actuator, the starting position display block (38) is responsible for displaying the starting position of the actuator (19) of the 2R1T end actuator before executing the planned trajectory, and the ending position display block (39) is responsible for displaying the ending position of the actuator (19) of the 2R1T end actuator after executing the planned trajectory.

Images