Delta robot multi-axis cooperative control system and method based on EtherCAT bus

Abstract

The invention provides a Delta robot multi-axis cooperative control system and method based on an EtherCAT bus. The Delta robot multi-axis cooperative control system comprises a controller, a motion controller and an EtherCAT interface. The controller is used for deploying a protocol stack so as to realize an EtherCAT master station function; the motion controller is used for receiving an instruction sent by the controller and controlling trajectory planning and speed control of the Delta robot; the EtherCAT interface is used for transmitting an instruction sent by the controller; according to the invention, the special motion controller is designed, and the EtherCAT bus protocol is combined, so that high-precision and high-real-time multi-axis cooperative control is realized; meanwhile, in combination with a distributed clock of EtherCAT and a CSP mode, an optimized S-shaped trajectory planning curve is adopted, and the reliability and stability of the system are enhanced.

Description

Technical Field

[0001] The present invention relates to the technical field of industrial automation control, and in particular to an EtherCAT bus-based Delta robot multi-axis collaborative control system and method. Background Art

[0002] With the development of industrial automation, robots are increasingly used in production and manufacturing, especially in the field of high-speed and high-precision operations. The traditional Delta robot control system uses a pulse signal drive method, such as Figure 1 , there are problems such as poor anti-interference and slow communication rate, which makes it difficult to meet the needs of modern manufacturing for high-performance control systems.

[0003] As a high-speed, real-time industrial Ethernet protocol, EtherCAT has been widely adopted in industrial automation for its superior performance and flexible topology. EtherCAT can process 1,000 distributed I / O signals in 30 μs and 100 servo axis data exchange in 100 μs, greatly satisfying the demands of high-speed control and offering exceptional performance. Furthermore, it supports a variety of topologies, including linear, tree, and star, adapting to diverse application scenarios.

[0004] However, existing research on EtherCAT-based Delta robot control systems mostly focuses on performance improvement, and lacks in-depth research on system reliability and stability.

[0005] In view of this, a high-performance Delta robot multi-axis collaborative control system based on EtherCAT bus is proposed, and an effective reliability enhancement method is proposed, which has important research significance and application value. Summary of the Invention

[0006] To address the shortcomings of the existing technology, the present invention provides a Delta robot multi-axis collaborative control system and method based on the EtherCAT bus. By designing a dedicated motion controller and combining it with the EtherCAT bus protocol, high-precision, high-real-time multi-axis collaborative control is achieved. At the same time, combined with EtherCAT's distributed clock and CSP mode, an optimized S-shaped trajectory planning curve is adopted to enhance the reliability and stability of the system, meeting the high requirements of modern industrial automation production lines for robot control systems.

[0007] The technical solution adopted in this disclosure is:

[0008] A Delta robot multi-axis collaborative control system based on EtherCAT bus, comprising:

[0009] Controller, used to deploy the protocol stack to implement EtherCAT master functionality;

[0010] A motion controller, which exchanges data with the controller and is used to receive instructions from the controller to control the trajectory planning and speed control of the Delta robot to achieve multi-axis collaborative control of the Delta robot;

[0011] The EtherCAT interface is provided between the controller and the motion controller and is used for transmitting instructions issued by the controller.

[0012] The deployment protocol stack to implement the EtherCAT master function includes:

[0013] Adopting the timer cascade architecture, the master and slave timers work together to optimize the configuration.

[0014] By optimizing the hardware abstraction layer OSHW, the SOEM abstraction layer is optimized to improve code conversion efficiency and access speed.

[0015] The timer cascade architecture is used to optimize the configuration by cooperating between the master and slave timers, including:

[0016] By cascading the TIM5 timer and the TIM4 timer, the interrupt trigger interval is extended at the same time;

[0017] Configuring the TIM5 timer as a microsecond timer includes:

[0018] Use the internal clock source CK_INT as input, and set the prescaler of the TIM5 timer to have an output frequency of 1 MHz, and automatically reload the register settings to generate an update event every 1 microsecond;

[0019] Configuring the TIM4 timer as a second-level timer includes:

[0020] The clock source of the TIM4 timer is set to the update event of the TIM5 timer, the update event of the TIM5 timer is configured as the trigger source of the TIM4 timer, and the TIM4 timer is set to slave mode to respond to an external trigger signal; and the TIM4 timer is enabled to start counting.

[0021] The optimization of the hardware abstraction layer OSHW is used to optimize the SOEM abstraction layer, including:

[0022] In the hardware abstraction layer OSHW, the byte order of 16-bit data is reversed in a single cycle through the REV16 instruction, which can be directly called through the compiler built-in function to achieve zero-overhead conversion;

[0023] Allocate the buffer of the hardware abstraction layer OSHW corresponding code to the controller DTCM memory area to improve access speed.

[0024] The data interaction with the controller is used to receive instructions from the controller and control the trajectory planning and speed control of the Delta robot, including:

[0025] Obtain the inverse kinematics equation of the parallel robot through the spatial position relationship and geometric relationship between any part of a single branch chain of the Delta robot and the coordinate origin of the static platform;

[0026] The control mode of the Delta robot is constructed by synchronizing the CSP mode of the EtherCAT bus with the distributed clock; and the motion trajectory of the Delta robot is planned by an S-shaped curve.

[0027] The inverse kinematics equation of the parallel robot is obtained by the spatial position relationship and geometric relationship between any part of a single branch of the Delta robot and the coordinate origin of the static platform, including:

[0028] The relationship between the height of any rail slider and the coordinates of the center point of the moving platform is:

[0029]

[0030] Among them, h A is the height of the linear rail slider; (x e ,y e , z e ) is the coordinate of the center point O2 of the moving platform; the angle between each tower column and the X axis in the static platform of the parallel robot is Φ i , i=A,B,C.

[0031] The control mode of the Delta robot is constructed by synchronizing the CSP mode of the EtherCAT bus with the distributed clock, including:

[0032] The master station periodically sends the target position and uses distributed clocks to trigger the local clock inside any slave station in the EtherCAT bus network, so that the internal clocks of all slave stations are synchronized to a unified time base.

[0033] Planning the motion trajectory of the Delta robot by using an S-shaped curve includes:

[0034] Assume that the three motors of the Delta robot need to move the displacements S1, S2, and S3 respectively, and each uses its own optimal S-curve planning, with the total movement time being T1, T2, and T3 respectively;

[0035] Get the shortest motion time of the axis corresponding to any motor under the default parameters

[0036] Select the time with the largest running time among the axes as the common time:

[0037]

[0038] For movement time less than T common axis, slowing down its trajectory by time stretching so that common Completed within

[0039] Obtain the relationship between the total displacement S and the total motion time T in the S-shaped curve trajectory, including:

[0040] s=f(T,J,a max ,v max );

[0041] Where, f(·) is the expression of the sum of displacements of each segment; V max is the peak velocity; a max is the acceleration.

[0042] The method of slowing down the trajectory by time stretching includes:

[0043] For the axis with shorter displacement, a time scaling factor λ is introduced, which is defined as:

[0044]

[0045] When planning the S-shaped trajectory, the time for the axis with the shorter displacement to move is extended to

[0046] A control method based on the EtherCAT bus-based Delta robot multi-axis collaborative control system, comprising:

[0047] According to the initial plan of any motor, the shortest completion time of each motor is obtained And, take the largest one as the common time T common ;

[0048] Use the time scaling factor to adjust the motion time of each motor i: Make the action of any motor occur at the common time T common Completed within

[0049] For any of the motors, when performing S-type planning, the time variable t is replaced by τ = λ i ·t, so that τ=T common When the required displacement S is reached i ;

[0050] The master station discretizes the S-shaped curve into a time-position sequence according to the CSP control cycle and obtains the target position X(t) within the cycle T, where t = n·T, and n is the cycle count;

[0051] In any cycle, the target position X(t) is written into the object dictionary address of the slave through the PDO in EtherCAT, realizing the S-curve trajectory planning of the three motors of the Delta robot in the CSP mode of EtherCAT.

[0052] The beneficial effects of the present disclosure are:

[0053] The EtherCAT bus-based Delta robot multi-axis collaborative control system disclosed in the present disclosure introduces continuously changing jerk control through S-curve motion planning, eliminating the sudden change phenomenon of traditional pulse or trapezoidal speed curves in the acceleration and deceleration stages, significantly reducing mechanical shock and vibration; especially when the target displacement is small, the use of pure acceleration and deceleration symmetrical motion can achieve extremely high smoothness. At the same time, by determining the common motion time Tcommon and applying the time stretching factor λ, the problem of motion time differences caused by different displacements of each axis is solved, ensuring that all motors can start and stop at the same time, thereby making the motion trajectory of the robot dynamic platform smoother and more consistent, ensuring multi-axis motion synchronization, and improving the overall system performance and positioning accuracy; and using EtherCAT's distributed clock (DC) for precise synchronization, combined with the CSP mode, to achieve microsecond-level synchronous control, significantly improving system response speed and collaborative control efficiency, and enhancing system reliability and real-time performance. Moreover, during the control process, by optimizing timer cascades and buffer management, real-time performance is further guaranteed and context switching interference is reduced. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] Figure 1 Schematic diagram of the pulse drive method of the Delta robot in the prior art;

[0055] Figure 2 A block diagram of the collaborative control system described in the present disclosure;

[0056] Figure 3 This is a schematic diagram of the timer cascade described in the present disclosure;

[0057] Figure 4 Obtaining a flow chart for the robot inverse kinematics equations described in this disclosure;

[0058] Figure 5 Schematic diagram of the structure of the Delta robot disclosed in the present invention;

[0059] Figure 6 for Figure 5A simplified diagram of a single branched chain of a Delta robot;

[0060] Figure 7 A schematic diagram showing the angle between the static and dynamic platforms of a Delta robot.

[0061] Figure 8 This is a flow chart of the S-shaped trajectory planning curve program in the present disclosure;

[0062] Figure 9 for Figure 8 Schematic diagram of the S-shaped trajectory planning curve;

[0063] Figure 10 The following is a flowchart of the method described in the present invention. DETAILED DESCRIPTION

[0064] In order to more clearly understand the above-mentioned objectives, features and advantages of the present disclosure, the present disclosure is further described below in conjunction with the accompanying drawings and embodiments. It should be noted that the embodiments of the present application and the features therein can be combined with each other unless there is a conflict. The present disclosure is further described below in conjunction with the accompanying drawings. The following embodiments are only used to more clearly illustrate the technical solutions of the present disclosure and are not intended to limit the scope of protection of the present disclosure. Specific embodiment 1:

[0066] The present disclosure provides an embodiment:

[0067] like Figure 2 A Delta robot multi-axis collaborative control system based on EtherCAT bus includes: a controller, a motion controller and an EtherCAT interface; wherein the controller is used to deploy the protocol stack to realize the EtherCAT master station function; the motion controller exchanges data with the controller to receive instructions issued by the controller, control the trajectory planning and speed control of the Delta robot, and achieve multi-axis collaborative control of the Delta robot; the EtherCAT interface is set between the controller and the motion controller to transmit the instructions issued by the controller.

[0068] Preferably, the controller uses a high-performance embedded chip, such as STM32H743IIT6, as the core processing unit of the EtherCAT master station, which has high-speed data processing capabilities and rich I / O interfaces to meet complex control requirements; the EtherCAT interface uses a physical layer chip that complies with the EtherCAT standard, such as LAN8720A, to ensure high-speed and reliable data transmission; the motion controller supports precise trajectory planning and speed control, and communicates efficiently with the servo motor driver through EtherCAT.

[0069] SOEM is an open-source EtherCAT master protocol stack used to implement EtherCAT master functionality. In this example, the SOEM (Simple Open EtherCAT Master) protocol stack is deployed on the STM32H743IIT6 microcontroller to implement EtherCAT master functionality. Porting SOEM to a microcontroller is a prior art approach. This example primarily optimizes the underlying configuration of the STM32H743 microcontroller and the SOEM abstraction layer for the hardware platform.

[0070] In traditional implementations, SOEM system clocks typically rely on the SysTick timing mechanism, which generates a time base by configuring the microcontroller's built-in timer. When a 16-bit timer is used and the clock source is set to 1MHz, the upper limit of its single count period is 65535 microseconds (approximately 65.535 milliseconds). If the auto-reload value is set to 60000 microseconds, the timer will trigger an interrupt service routine when the count overflows. At this time, the global time variable tick increments and is combined with the current count value TIM->CNT to form a complete timestamp (calculated as: system time = tick × 60000 + TIM->CNT).

[0071] This periodic interrupt mechanism has inherent defects: an interrupt event is generated every 60 milliseconds, frequently triggering context switching, resulting in fluctuations in real-time task scheduling delays. In order to reduce the impact of interrupt frequency on the system, this embodiment innovatively adopts a timer cascade architecture, such as Figure 3 By using the collaborative work of master and slave timers, the interrupt trigger interval is extended to nearly 17 times that of the original design, thereby significantly reducing the interference of the interrupt service program on the core control process.

[0072] First, configure TIM5 as a microsecond timer: use the internal clock source CK_INT (200MHz) and set the TIM5 prescaler so that its output frequency is 1MHz, that is, each counting cycle is 1 microsecond, as follows:

[0073] CK_INT / (prescaler value + 1) = 1MHz;

[0074] 200,000,000 / (prescaler value + 1) = 1,000,000;

[0075] Prescaler value = 199;

[0076] Auto-reload register (ARR) setting: Since an update event needs to be generated every 1 microsecond, the ARR value should be set to 0, that is, the counter starts incrementing from 0 to 1.

[0077] Then, configure TIM4 as a second-level timer: set TIM4's clock source to the TIM5 update event, configure the TIM5 update event as the trigger source for TIM4, then set TIM4 to slave mode to respond to external trigger signals. Finally, enable TIM4 timer to start counting.

[0078] TIM5's timing clock is 1MHz, with a count of 1 microsecond. Configuring TIM4 to output a 1Hz clock signal as its input effectively sets TIM4's timing clock to 1Hz. Thus, TIM5's count value is in microseconds, while TIM4's count value is in seconds.

[0079] sec=TIM4->CNT;

[0080] usec=TIM5->CNT;

[0081] This timer configuration ensures the accuracy of the system clock. To read the system clock value at any time, only the values of the two counters TIM4->CNT and TIM5->CNT need to be read, thus avoiding the interference of tick interrupts on the system.

[0082] The abstraction layer can be divided into two parts: the Operating System Abstraction Layer (OSAL) and the Hardware Abstraction Layer (OSHW). The OSAL module focuses on operating system abstraction and timing mechanisms and consists of osal.h and osal.c. The OSHW module provides network services for the upper layer and consists of four files: oshw.h / oshw.c and nicdrv.h / nicdrv.c. oshw.h / .c primarily implements network and local endian conversion. nicdrv.h / .c is the network driver, which primarily implements the sending and receiving of EtherCAT frames.

[0083] First, in EtherCAT communication, the conversion between host byte order and network byte order occurs frequently, especially in the processing of Ethernet frame headers. The original oshw.c code implements this by calling htons and ntohs from the standard library, but has the following problems:

[0084] Performance overhead: Standard library functions are not optimized for the STM32H7 hardware instructions, resulting in low conversion efficiency in software implementation.

[0085] Dependency issues: In embedded systems, reliance on standard libraries should be minimized to improve code controllability and portability.

[0086] The Cortex-M7 core supports the REV16 instruction, which reverses the endianness of 16-bit data in a single cycle. This instruction is directly called through a built-in function in the compiler, achieving zero-overhead conversion.

[0087] Specific implementation:

[0088] static inline uint16_t oshw_htons(uint16_t host){

[0089] return__builtin_bswap16(host); / / GCC / Clang built-in function, generates REV16 instruction

[0090] }

[0091] static inline uint16_t oshw_ntohs(uint16_t network){

[0092] return__builtin_bswap16(network); / / Same as above

[0093] }

[0094] Secondly, in the nicdrv.c source file, the code uses fixed-size buffers, such as EC_MAXBUF, but does not fully utilize the STM32H743's DTCM high-speed memory or cache mechanism, affecting real-time performance. Here, key buffers, such as txbuf and rxbuf, are allocated to the DTCM memory area to improve access speed:

[0095] __attribute__((section(“.dtcm”)))uint8_t txbuf[EC_MAXBUF][ETH_BUFFER_SIZE];

[0096] The multi-axis collaborative control process of the Delta robot involved in this embodiment includes:

[0097] Inverse kinematics analysis of Delta robot:

[0098] In this embodiment, Figure 4 The problem that the inverse kinematics of the Delta robot needs to solve is to find the positions of the three linear rail sliders when the spatial coordinates of the center of the parallel robot's moving platform are known. Since the structures of the three branch chains of the parallel robot are exactly the same, such as Figure 5 , so we can take one of the branches for analysis and get the following Figure 6 The schematic diagram of a single branch of a parallel robot is shown in FIG. By analyzing the spatial positional and geometrical relationships between each part of a single branch and the coordinate origin of the static platform, the inverse kinematics equation of the parallel robot is solved.

[0099] In this embodiment, the sizes of the static platform and the dynamic platform are measured by the circumscribed circles of the equilateral triangles, and their radii are set as R and r respectively. O2 is the center of the dynamic platform and the center of its circumscribed circle. Its spatial coordinates are set as (x e ,y e ,z e ), so the vector O1O2 is expressed as:

[0100]

[0101] K i is the coordinate position of the bottom of column i (i = A, B, C), O1K i The vector can be expressed as:

[0102]

[0103] N i is the edge point of the circumcircle of the moving platform, O2K i The vector can be expressed as:

[0104]

[0105] K i M i are the heights of the linear rail slider on the i-th column (i=A, B, C), and their vectors are expressed as:

[0106]

[0107] Combining equations (2-1) to (2-4) we can get the vector N i M i for:

[0108]

[0109] According to the mechanical structure of the parallel robot, the vector N i M i The module length is equal to the length l of the connecting rod, and the lengths of the three connecting rods are equal, so:

[0110] |N i M i |=l (2-6)

[0111] Therefore, the combination of equations (2-5) and (2-6) can be used to obtain the equation group about the height of the linear rail slider and the center coordinates of the moving platform:

[0112]

[0113] Solving the above equations and combining them with the mechanical structure of the robot, we can obtain the positional relationship between the height of each linear rail slider of the parallel robot and the coordinates of the center point of the moving platform:

[0114]

[0115] h A is the height of the linear rail slider, (x e ,y e , z e ) is the coordinate of the center point O2 of the moving platform, such as Figure 6 shown.

[0116] The static platform and dynamic platform of the parallel robot are both equilateral triangles. To facilitate position calibration, this project establishes separate Cartesian coordinate systems for the equilateral triangles of the static platform and dynamic platform, and sets the origin of the spatial rectangular coordinate system at the center of each equilateral triangle. Taking the static platform as an example, the z-coordinate component at the static platform is 0 and points upward. The definition of the XY axis components is as follows: Figure 7 As shown. The X-axis is parallel to AB and points to the direction of the B-pillar. The Y-axis coincides with OC and points to the direction of the C-pillar. The angle between each tower column and the X-axis is Φ i (i=A,B,C).

[0117] Through the structure of the linear Delta robot, it can be found that the changes in the heights of the three sliders hA, hB, and hC correspond to the displacements of the three motors.

[0118] Delta robot trajectory planning process:

[0119] As a high-speed, high-precision parallel robot, the Delta robot requires extremely low latency and strict synchronization for its control, so the control mode uses EtherCAT's CSP mode + distributed clock (DC) synchronization. CSP mode (Cyclic Synchronous Position): This is a position control mode in which the master station periodically sends the target position, and the slave station driver executes it on time and accurately; Distributed Clock DC: Each slave station in the EtherCAT network has its own local clock. The DC mechanism is a synchronization process triggered by the master station, which synchronizes the internal clocks of all slave stations to a unified time base; The advantages of DC + CSP: It ensures that all slave stations execute position commands at the same time point; It ensures that the motion behavior within the control cycle is predictable and highly synchronized;

[0120] In order to make the Delta robot move more smoothly, the trajectory planning method uses the S-curve for planning, such as Figure 8 .

[0121] First, a unified plan of the motion time is performed: assuming that the displacements that the three motors need to move are S1, S2, and S3, and their respective optimal S-curve plans are directly adopted, their total motion durations are (T1, T2, T3), which are often different. This will cause one motor of the Delta robot to rotate until the required displacement is completed, and the other motors may still be rotating. Obviously, this solution is not only inefficient, but also causes the motion path of the moving platform to be not smooth and coherent. In order to achieve the simultaneous start and stop of the three motors, the motion of each axis must be synchronized, that is, the motion time of all motors must be equal to a common time T common .

[0122] The method used is:

[0123] ①First obtain the shortest motion time of each axis under the default parameters

[0124] ②Select the largest time as the common time:

[0125]

[0126] ③For exercise time less than T common axis, slowing down its trajectory by time scaling so that common Completed within.

[0127] Then, the basic formula of the S-curve trajectory is described in sections; the S-curve is as follows Figure 9 Set the following motion parameters:

[0128] J: Maximum jerk.

[0129] a max : Maximum acceleration.

[0130] t: is the current time.

[0131] V1: represents the velocity at the end of the jerk phase and the beginning of the uniform acceleration phase.

[0132] V2: Indicates the velocity at the end of the uniform acceleration segment and the beginning of the deceleration segment (negative jerk).

[0133] X1: represents the displacement of the acceleration section

[0134] X2: represents the displacement of the sum of the acceleration section and the constant acceleration section

[0135] In an S-curve trajectory, motion is usually divided into acceleration, constant speed, and deceleration phases. Since the S-curve is symmetrical, only the first three phases are analyzed here, which can be described as follows:

[0136] Jerk phase (positive):

[0137] Acceleration: a(t) = J·t

[0138]

[0139] Constant acceleration section (acceleration reaches maximum value a max ):

[0140] Acceleration: a(t) = a max

[0141] Speed: v(t) = v1 + a max ·t

[0142]

[0143] Acceleration and deceleration section (Jerk negative):

[0144] Acceleration: a(t) = a max -J·t

[0145] speed:

[0146] Displacement:

[0147] In the entire trajectory, the relationship between the total displacement S and the total motion time T is composed of the integral sum of these stages. In an ideal symmetrical S-curve, a comprehensive relationship can be written:

[0148] s=f(T,J,a max ,v max );

[0149] Where f(·) is the expression of the sum of displacements of each segment. i If you want the exercise time to be T common , we need to inversely solve this relationship so that the trajectory parameters of each axis, such as the peak velocity V max or acceleration a max , after proper adjustment, the three motors are operated at the same T common Complete the movement inside.

[0150] Finally, the implementation steps of time stretching: For the axis with shorter displacement, its original plan may have a higher peak speed or a shorter acceleration time. In order to extend its motion time to T common A time scaling factor λ can be introduced, defined as:

[0151] When planning the trajectory, replace the time variable t with τ = λ i·t. The time variable in the original formula becomes τ, and the time of the entire movement is stretched to

[0152] This is equivalent to reducing the speed, acceleration and peak jerk, so that the axis with smaller displacement can also move at T common Complete the entire movement process within. Specific embodiment 2:

[0154] The present disclosure also provides an embodiment:

[0155] like Figure 10 , a specific implementation step of coordinating the motion of three motors through the system described in specific embodiment 1 includes:

[0156] S1: Determine public exercise time;

[0157] According to their respective initial plans, use the default parameters J, a max etc., to get the shortest completion time for each motor Take the largest one as T common .

[0158] For any motor i, the time scaling factor λ i :

[0159] If the displacement of a motor is large, its λ i May be close to 1; while the motor with smaller displacement has i Will be greater than 1.

[0160] S2: Replan the S curve;

[0161] For each motor, replace the time variable with τ = λ i t. The formulas for each stage are modified accordingly, for example:

[0162] Acceleration phase:

[0163]

[0164] The entire trajectory is calculated at τ = T common When the required displacement S is reached i .

[0165] S3: periodic synchronization control of the master station;

[0166] The master discretizes the continuous S-curve into a time-position sequence based on the 1ms CSP control cycle. The target position X(t) is calculated within a 1ms period, T, where t = n·T, where n is the cycle count. Each cycle, the target position X(t) is written to the slave's object dictionary address 0x607A (target position register) via the EtherCAT Process Data Object (PDO). This implements S-curve trajectory planning for the three Delta motors in EtherCAT's CSP mode.

[0167] Although the embodiments of the present disclosure have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and alterations may be made to the embodiments without departing from the principles and spirit of the present disclosure, and the scope of the present disclosure is defined by the appended claims and their equivalents.

Claims

1. A Delta robot multi-axis collaborative control system based on EtherCAT bus, characterized in that: include: Controller, used to deploy the protocol stack to implement EtherCAT master functionality; A motion controller, which exchanges data with the controller and is used to receive instructions from the controller to control the trajectory planning and speed control of the Delta robot to achieve multi-axis collaborative control of the Delta robot; The EtherCAT interface is provided between the controller and the motion controller and is used for transmitting instructions issued by the controller.

2. The EtherCAT bus-based Delta robot multi-axis collaborative control system according to claim 1, characterized in that: The deployment protocol stack to implement the EtherCAT master function includes: Adopting the timer cascade architecture, the master and slave timers work together to optimize the configuration. By optimizing the hardware abstraction layer OSHW, the SOEM abstraction layer is optimized to improve code conversion efficiency and access speed.

3. The EtherCAT bus-based Delta robot multi-axis collaborative control system according to claim 2, characterized in that: The timer cascade architecture is used to optimize the configuration by cooperating between the master and slave timers, including: By cascading the TIM5 timer and the TIM4 timer, the interrupt trigger interval is extended at the same time; Configuring the TIM5 timer as a microsecond timer includes: Use the internal clock source CK_INT as input, and set the prescaler of the TIM5 timer to have an output frequency of 1 MHz, and automatically reload the register settings to generate an update event every 1 microsecond; Configuring the TIM4 timer as a second-level timer includes: The clock source of the TIM4 timer is set to the update event of the TIM5 timer, the update event of the TIM5 timer is configured as the trigger source of the TIM4 timer, and the TIM4 timer is set to slave mode to respond to an external trigger signal; and the TIM4 timer is enabled to start counting.

4. The EtherCAT bus-based Delta robot multi-axis collaborative control system according to claim 2, characterized in that: The optimization of the hardware abstraction layer OSHW is used to optimize the SOEM abstraction layer, including: In the hardware abstraction layer OSHW, the byte order of 16-bit data is reversed in a single cycle through the REV16 instruction, which can be directly called through the compiler built-in function to achieve zero-overhead conversion; Allocate the buffer of the hardware abstraction layer OSHW corresponding code to the controller DTCM memory area to improve access speed.

5. The EtherCAT bus-based Delta robot multi-axis collaborative control system according to claim 1, characterized in that: The data interaction with the controller is used to receive instructions from the controller and control the trajectory planning and speed control of the Delta robot, including: Obtain the inverse kinematics equation of the parallel robot through the spatial position relationship and geometric relationship between any part of a single branch chain of the Delta robot and the coordinate origin of the static platform; The control mode of the Delta robot is constructed by synchronizing the CSP mode of the EtherCAT bus with the distributed clock; and the motion trajectory of the Delta robot is planned by an S-shaped curve.

6. The EtherCAT bus-based Delta robot multi-axis collaborative control system according to claim 1, characterized in that: The inverse kinematics equation of the parallel robot is obtained by the spatial position relationship and geometric relationship between any part of a single branch of the Delta robot and the coordinate origin of the static platform, including: The relationship between the height of any rail slider and the coordinates of the center point of the moving platform is: Among them, h A is the height of the linear rail slider; (x e ,y e , Ze ) is the coordinate of the center point O2 of the moving platform; the angle between each tower column and the X axis in the static platform of the parallel robot is Φ i , i=A,B,C.

7. The EtherCAT bus-based Delta robot multi-axis collaborative control system according to claim 5, characterized in that: The control mode of the Delta robot is constructed by synchronizing the CSP mode of the EtherCAT bus with the distributed clock, including: The master station periodically sends the target position and uses distributed clocks to trigger the local clock inside any slave station in the EtherCAT bus network, so that the internal clocks of all slave stations are synchronized to a unified time base.

8. The EtherCAT bus-based Delta robot multi-axis collaborative control system according to claim 5, characterized in that: Planning the motion trajectory of the Delta robot by using an S-shaped curve includes: Assume that the three motors of the Delta robot need to move the displacements S1, S2, and S3 respectively, and each uses its own optimal S-curve planning, with the total movement time being T1, T2, and T3 respectively; Get the shortest motion time of the axis corresponding to any motor under the default parameters Select the time with the largest running time among the axes as the common time: For movement time less than T common axis, slowing down its trajectory by time stretching so that common Completed within Obtain the relationship between the total displacement S and the total motion time T in the S-shaped curve trajectory, including: s=f(T,J,a max ,v max ); Where, f(·) is the expression of the sum of displacements of each segment; V max is the peak velocity; a max is the acceleration.

9. The EtherCAT bus-based Delta robot multi-axis collaborative control system according to claim 8, characterized in that: The method of slowing down the trajectory by time stretching includes: For the axis with shorter displacement, a time scaling factor λ is introduced, which is defined as: When planning the S-shaped trajectory, the time for the axis with the shorter displacement to move is extended to 10. A control method for a Delta robot multi-axis collaborative control system based on an EtherCAT bus according to any one of claims 1 to 9, comprising: According to the initial plan of any motor, the shortest completion time of each motor is obtained And, take the largest one as the common time T common ; Use the time scaling factor to adjust the motion time of each motor i: Make the action of any motor occur at the common time T common Completed within For any of the motors, when performing S-type planning, the time variable t is replaced by τ = λ i ·t, so that τ=T common When the required displacement S is reached i ; The master station discretizes the S-shaped curve into a time-position sequence according to the CSP control cycle and obtains the target position X(t) within the cycle T, where t = n·T, and n is the cycle count; In any cycle, the target position X(t) is written into the object dictionary address of the slave through the PDO in EtherCAT, realizing the S-curve trajectory planning of the three motors of the Delta robot in the CSP mode of EtherCAT.

Images