Robot smooth stop control method suitable for high-frequency communication period

Abstract

The invention discloses a robot smooth stop control method suitable for a high-frequency communication period, and belongs to the technical field of robot speed control. The method mainly comprises the steps of receiving a stop signal and preparing smooth stop; a smooth stop thread is started, preparation work before smooth stop trajectory planning is carried out, S-shaped smooth stop trajectory planning is carried out, the axis position, the speed, the acceleration and the torque value at each moment are calculated, and the data are linked to a data cache region. According to the method, the Ethercat communication cycle is smoothly stopped, and processing of operation cache data, smooth stopping trajectory planning, dynamic feed-forward operation, position loop operation and speed loop operation are covered. A multi-thread task architecture is constructed, smooth stop is stripped from a main thread, the time of Ethercat communication is not occupied any more, the problem of communication timeout caused by long operation time is solved, and meanwhile, the continuity, high precision and small mechanical impact of acceleration are ensured.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of robot speed control, in particular, especially relates to a robot smooth stop control method suitable for high-frequency communication cycle. BACKGROUND

[0002] In the process of manual debugging and automatic running of industrial robots, a safe and reliable controlled stop mechanism is a key technical link to achieve precise motion control. The termination of the conventional motion instruction belongs to pre-planned stop, and the deceleration parameters have been calculated in the trajectory planning stage. The controlled stop belongs to sudden stop control. When the operator triggers the stop signal, the control system needs to immediately start dynamic trajectory re-planning under real-time motion state, and ensure smooth transition of the servo system by quickly planning the interpolation trajectory point of the compound kinematics.

[0003] At present, the commonly used robot smooth stop control method mainly includes trapezoidal ramp curve stop and N-order polynomial interpolation stop as deceleration stage planning. However, the trapezoidal ramp curve stop as a simple stop algorithm has low calculation complexity, but the planning logic does not consider the current robot acceleration value, and directly uses the maximum deceleration value as the planning acceleration, which causes a step change in acceleration, and there is still residual acceleration when the speed is zero, which causes too much impact on the motor and the mechanical system, affecting the service life of the servo motor and the reliability of the mechanical transmission system. The N-order polynomial interpolation stop as the interpolation stop of the deceleration stage needs to set the stop position and stop time in advance. If the parameters are not set reasonably, it will have an unexpected impact on the motion chain and inertial load. Especially when a higher speed value is specified in a short time, this algorithm cannot constrain the acceleration peak value in the planning process, which may cause motor failure and safety risks. SUMMARY

[0004] The purpose of the present application is to provide a robot smooth stop control method suitable for high-frequency communication cycle, which realizes Ethercat communication cycle smooth stop, covers the processing of operation cache data, smooth stop trajectory planning, dynamics feedforward operation, position loop and speed loop operation. A multi-thread task architecture is constructed to separate the smooth stop from the main thread, so that it no longer occupies the Ethercat communication time, solves the problem of long operation time leading to communication timeout, and at the same time ensures the continuity of acceleration, high precision and small mechanical impact.

[0005] The present application is realized by the following technical solutions: A robot smooth stop control method suitable for high-frequency communication cycle, comprising the following steps: Step S1: receiving a stop signal and preparing for smooth stop; Step S2: Start the smooth stop thread to perform preparatory work before smooth stop trajectory planning; Step S3: S-shaped smooth stopping trajectory planning; Step S4: Calculate the axis position, velocity, acceleration and torque values ​​at each moment, and connect these data to the data buffer area.

[0006] Furthermore, when the stop signal is received, the robot switches its running state and enters a smooth stop preparation state, and the computation thread stops computation; the data buffer is processed, the smooth stop thread starts, and the acceleration continuous smooth stop trajectory planning is performed; after completion, the computation is returned to a separate computation thread, the Ethercat thread normally executes the position loop and velocity loop computations, and sends the torque target value, and this thread is not affected.

[0007] Furthermore, in step S1: the operator triggers a stop signal, the teach pendant sends a signal to the controller, and after receiving the message, the controller switches the robot to the smooth stop preparation state; if the robot is in a non-motion state or is executing a non-motion command, the robot state will be switched to the stop state; otherwise, the robot enters the smooth stop state; after switching to the smooth stop state, the calculation thread will be paused, and the previous trajectory will no longer be interpolated, and the calculation cache capacity will no longer increase.

[0008] Furthermore, in step S2, the cached data is first processed, retaining only the first N data points, which are the data to be sent to the servo system in the next N cycles. A spinlock is added when operating on the cached data. After processing, the last three data points are taken out and used as the initial data for the next trajectory planning. After the operation is completed, the spinlock is released so that the Ethercat thread can normally obtain the cached data. At this point, the preparation work is complete.

[0009] Furthermore, in step S3, a smooth stopping trajectory is planned for continuous acceleration. The entire deceleration phase can be divided into three stages: deceleration-acceleration phase, uniform deceleration phase, and deceleration-deceleration phase.

[0010] Furthermore, the smooth stopping trajectory planning includes the following steps: Step S31: First, assume that the trajectory can satisfy a three-segment curve, namely, the deceleration phase, the uniform deceleration phase, and the deceleration phase. Calculate the motion time of each segment, namely the deceleration time t1, the uniform deceleration time t2, and the deceleration time t3. Step S32: Determine if there is a deceleration / acceleration phase; Step S33: Determine if there is a uniform deceleration phase; Step S34: Determine if there is only a deceleration phase; Step S34: Calculate the position, velocity, acceleration, and jerk value at any given time.

[0011] Furthermore, the parameters required for the smooth stopping trajectory planning include the initial position S0, initial velocity v0, initial acceleration a0, and maximum acceleration A. max Maximum jerk j max At the end of the motion, the robot's speed and acceleration are both zero.

[0012] Furthermore, the calculation formulas for the acceleration / deceleration time t1, the uniform deceleration time t2, and the deceleration / deceleration time t3 are as follows:

[0013]

[0014] Furthermore, the initial acceleration a0 is equal to the maximum deceleration A. decmax When t1 is less than 0, there is no deceleration phase, only a uniform deceleration phase and a deceleration phase; when the uniform deceleration time t2 is less than 0, there is no uniform deceleration phase, only a deceleration phase and a deceleration phase; when t1 is less than 0, there is only a deceleration phase.

[0015] Furthermore, the data is connected to the data cache area. After the Ethercat thread obtains the data, it uses the speed value as the feedforward data for the position loop and the torque value as the feedforward data for the speed loop. After the dual-loop operation, the torque value is output and sent to the servo system to complete the entire control logic and realize the smooth stopping control of the robot in the high-frequency communication cycle.

[0016] Compared with the prior art, the beneficial effects of the present invention are: 1. This invention proposes a smooth stopping control method for robots with high-frequency Ethercat communication cycles. This method constructs a multi-threaded task scheduling architecture, separates the smooth stopping function from the Ethercat communication thread, and no longer occupies Ethercat communication time. By processing the data buffer, the Ethercat thread consumes data in the buffer while other threads perform stopping planning, and the two do not affect each other. This solves the problem of communication timeout caused by long computation time, and enables the robot to achieve smooth stopping normally under high-frequency Ethercat communication cycles.

[0017] 2. This invention proposes an S-shaped smooth stopping trajectory planning algorithm with continuous acceleration, suitable for smooth stopping. According to the actual situation, the number of acceleration change segments is adjusted to ensure the continuity of acceleration, high accuracy, and small mechanical impact on the robot. Attached Figure Description

[0018] Figure 1 This is a flowchart illustrating the specific process of handling stop messages in this invention; Figure 2 This is a flowchart illustrating the preparatory process before the smooth stop of the present invention; Figure 3 This is a three-segment acceleration variation curve diagram of the present invention; Figure 4 This is a graph showing the acceleration variation without deceleration in the present invention. Figure 5 This is a graph showing the acceleration variation without a uniform deceleration segment in this invention. Figure 6 This invention provides only the acceleration variation curve of the deceleration phase; Figure 7 This is a data diagram of the three-segment smooth stop motion of the present invention; Figure 8 This is a data diagram of the smooth stopping motion of the two-stage process without deceleration in this invention; Figure 9 This is a data diagram of the smooth stopping motion of the two-stage non-uniform deceleration stage of the present invention; Figure 10 This invention provides a smooth stopping motion data curve diagram with only one deceleration stage. Detailed Implementation

[0019] To enable those skilled in the art to better understand the technical solutions of the present invention, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings. Based on the embodiments in this application, other similar embodiments obtained by those skilled in the art without creative effort should all fall within the scope of protection of this application. Furthermore, directional terms mentioned in the following embodiments, such as "up," "down," "left," and "right," are only for reference to the directions in the accompanying drawings; therefore, the directional terms used are for illustrative purposes and not for limiting the invention.

[0020] The present invention will now be further described in conjunction with the accompanying drawings.

[0021] like Figures 1-7 As shown in Example 1, a smooth stopping control method for a robot suitable for high-frequency communication cycles includes the following steps: Step S1: Receive the stop signal and prepare for a smooth stop; Step S2: Start the smooth stop thread to perform preparatory work before smooth stop trajectory planning; Step S3: S-shaped smooth stopping trajectory planning; Step S4: Calculate the axis position, velocity, acceleration and torque values ​​at each moment, and connect these data to the data buffer area.

[0022] Example 2: A robot smooth stopping control method suitable for high-frequency communication cycles, such as... Figure 1 As shown, the operator triggers a stop signal, and the teach pendant sends a signal to the robot controller. Upon receiving the message, the controller switches the robot to the smooth stop preparation state RI_PREPARE_SMOOTH. If the robot is in a non-moving state or executing a non-moving command at this time, the robot state will be switched to the stop state R_Stop; otherwise, the robot enters the smooth stop state RI_SMOOTH. After switching to the smooth stop state, the computation thread will pause, no longer performing interpolation calculations on the previous trajectory, and the computation buffer capacity will no longer increase. After the above operations are completed, the smooth stop thread is triggered.

[0023] like Figure 2 As shown, after the smooth stop thread starts, it first processes the data in the buffer, retaining only the first N data items. These are the data that need to be sent to the servo system in the next N cycles. The value of N is an empirical value. If the value of N is too small, the trajectory planning and subsequent operations for smooth stop will not be completed within N cycles, resulting in data disconnection and servo system failure. If the value of N is too large, the stop will be delayed. In this system, the value of N is 8, and the smooth stop thread has 8 * 250us = 2ms to operate. When operating on the buffered data, because inter-thread communication is involved, a spinlock needs to be added to prevent the Ethercat thread from operating on the buffer while the smooth stop thread is operating on it, which would cause errors. It should be noted that the maximum capacity of the buffered data should not be too large. Otherwise, the operation of retaining the first N data items will take too long, causing the Ethercat thread to wait too long when retrieving the buffered data, resulting in communication timeouts. After processing, the last three data points are extracted and assigned to FirstPossAngle, SecondPossAngle, and thirdPossAngle respectively as initial data for subsequent trajectory planning. The current velocity is obtained by differencing these three data points, and the current acceleration is obtained by differencing the velocity. After the operation is complete, the spinlock is released, allowing the Ethercat thread to normally retrieve cached data. At this point, the preparation work is complete.

[0024] To plan a smooth stopping trajectory with continuous acceleration, the required parameters are: initial position S0, initial velocity v0, initial acceleration a0, and maximum acceleration A. max Maximum jerk j max At the end of the motion, both velocity and acceleration are zero.

[0025] Ideally, the entire deceleration phase can be divided into three stages: the acceleration phase, the uniform deceleration phase, and the deceleration phase, as follows: Figure 3 As shown.

[0026] The prerequisite for satisfying the three-stage equation is that the acceleration can be reduced to the maximum deceleration A. decmax And the absolute value of the initial acceleration is not greater than A. max First, the acceleration time t1 and deceleration time t3 can be easily obtained. The objective is to find the uniform deceleration time t2.

[0027] The formula for the acceleration value 'a' at each stage is: (1) The formula for calculating the deceleration / acceleration time t1 and the deceleration / deceleration time t3 is: (2) The change in speed during the deceleration phase is The velocity change during the uniform deceleration phase is The change in speed during the deceleration phase is... The sum of the velocity changes in each stage equals the initial velocity v0. The formula for calculating the velocity changes in each stage is: (3) Substituting equation (2) into equation (3) yields equation (4): (4) In summary, the solution is... The formula is: (5) When the initial acceleration equals the maximum deceleration A decmax At this time, it means that the trajectory has no deceleration phase, such as Figure 4 and Figure 8 As shown, there are only uniform deceleration and deceleration segments, and the motion time at this time is the uniform deceleration time t2 and the deceleration time t3.

[0028] Equation (5) yields the uniform deceleration time t2. However, according to the formula, the calculated uniform deceleration time t2 may be less than 0. In this case, it means that the trajectory does not have a uniform deceleration stage, but only a deceleration acceleration stage and a deceleration deceleration stage, such as... Figure 5 and Figure 9 As shown.

[0029] Assuming the acceleration drops to a1 during the deceleration phase and then rises to 0, we need to calculate the deceleration time t1 and the deceleration time t3 at this point. The value of the deceleration time t3 is shown in equation (6): (6) Since it is deceleration, a1 must be less than 0, therefore the deceleration time t3 is: (7) Integrate the accelerations in these two stages to find the change in velocity in each stage: (8) According to equation (8), we can obtain and : (9) According to equation (9), the deceleration / acceleration time t1 can be less than 0. In this case, the trajectory only has one deceleration / acceleration segment, such as... Figure 6 and Figure 10 As shown. Before determining the deceleration time t3, the maximum jerk j needs to be adjusted. max This ensures that when the trajectory acceleration reaches 0, the velocity also reaches 0. The adjusted values ​​of acceleration j and deceleration time t3 are shown in equation (10): (10) The acceleration and acceleration values ​​for each stage of the trajectory are calculated, as shown in equation (11): (11) Let the velocity at the end of the deceleration phase be v1 and the position be s1; let the velocity at the end of the uniform deceleration phase be v2 and the position be s2, as shown in equation (12): (12) Then calculate the velocity and position for each stage, as shown in equation (13): (13) At this point, the S-shaped smooth stopping trajectory planning is complete, and the trajectory parameters for each stage can be obtained according to equations (12) and (13).

[0030] After trajectory planning is completed, the computation thread is started to calculate the axis position, velocity, acceleration, and torque values ​​at each moment, and connect these data to the data buffer area. After the Ethercat thread obtains the data, it uses the velocity value as the feedforward data for the position loop and the torque value as the feedforward data for the velocity loop. After the dual-loop operation, the torque value is output and sent to the servo system to complete the entire control logic and realize the smooth stopping control of the robot in the high-frequency communication cycle.

[0031] Everything else is the same as in Example 1.

[0032] The present invention has been described in detail above. The above description is only a preferred embodiment of the present invention and should not be construed as limiting the scope of the present invention. All equivalent changes and modifications made within the scope of this application should still fall within the scope of the present invention.

Claims

1. A method for smooth stopping control of a robot suitable for high-frequency communication cycles, characterized in that: Includes the following steps: Step S1: Receive the stop signal and prepare for a smooth stop; Step S2: Start the smooth stop thread to perform preparatory work before smooth stop trajectory planning; Step S3: Smooth stopping trajectory planning; Step S4: Calculate the axis position, velocity, acceleration and torque values ​​at each moment, and connect these data to the data buffer area.

2. The robot smooth stopping control method applicable to high-frequency communication cycles according to claim 1, characterized in that: When the stop signal is received, the robot switches its running state and enters the smooth stop preparation state, and the calculation thread stops calculation; the data buffer is processed, the smooth stop thread starts, and the acceleration continuous smooth stop trajectory planning is performed; after completion, the calculation is returned to a separate calculation thread, the Ethercat thread performs the position loop and velocity loop calculations normally, and sends the torque target value.

3. The robot smooth stopping control method applicable to high-frequency communication cycles according to claim 1, characterized in that: In step S1: the operator triggers a stop signal, the teach pendant sends a signal to the controller, and after receiving the message, the controller switches the robot to the smooth stop preparation state; if the robot is in a non-moving state or is executing a non-moving command, the robot state will be switched to the stop state; otherwise, the robot enters the smooth stop state; after switching to the smooth stop state, the computing thread will be paused.

4. The robot smooth stopping control method applicable to high-frequency communication cycles according to claim 1, characterized in that: In step S2, the cached data is first processed, retaining only the first N data points, which are the data to be sent to the servo system in the next N cycles. A spinlock is added when operating on the cached data. After processing, the last three data points are taken out and used as the initial data for the next trajectory planning. After the operation is completed, the spinlock is released so that the Ethercat thread can obtain the cached data normally.

5. The robot smooth stopping control method applicable to high-frequency communication cycles according to claim 1, characterized in that: In step S3, a smooth stopping trajectory is planned for continuous acceleration. The entire deceleration phase can be divided into three stages: deceleration acceleration phase, uniform deceleration phase, and deceleration phase.

6. The robot smooth stopping control method applicable to high-frequency communication cycles according to claim 5, characterized in that: The smooth stopping trajectory planning includes the following steps: Step S31: First, assume that the trajectory can satisfy a three-segment curve, namely, the deceleration phase, the uniform deceleration phase, and the deceleration phase. Calculate the motion time of each segment, namely the deceleration time t1, the uniform deceleration time t2, and the deceleration time t3. Step S32: Determine if there is a deceleration / acceleration phase; Step S33: Determine if there is a uniform deceleration phase; Step S34: Determine if there is only a deceleration phase; Step S34: Calculate the position, velocity, acceleration, and jerk value at any given time.

7. The robot smooth stopping control method applicable to high-frequency communication cycles according to claim 6, characterized in that: The parameters required for the smooth stopping trajectory planning include initial position S0, initial velocity v0, initial acceleration a0, and maximum acceleration A. max Maximum jerk j max .

8. The robot smooth stopping control method applicable to high-frequency communication cycles according to claim 7, characterized in that: The calculation of the acceleration / deceleration time t1, uniform deceleration time t2, and deceleration time t3 is described above; the formulas for calculating the acceleration / deceleration time t1, uniform deceleration time t2, and deceleration time t3 are as follows: Where a0 is the initial acceleration, A max For maximum acceleration, j max Let j be the maximum jerk, and j be the jerk.

9. The robot smooth stopping control method applicable to high-frequency communication cycles according to claim 8, characterized in that: The initial acceleration a0 is equal to the maximum deceleration A. decmax When t1 is less than 0, there is no deceleration phase, only a uniform deceleration phase and a deceleration phase; when the uniform deceleration time t2 is less than 0, there is no uniform deceleration phase, only a deceleration phase and a deceleration phase; when t1 is less than 0, there is only a deceleration phase.

10. The robot smooth stopping control method applicable to high-frequency communication cycles according to claim 1, characterized in that: The data is connected to the data cache area. After the Ethercat thread obtains the data, it uses the speed value as the feedforward data for the position loop and the torque value as the feedforward data for the speed loop. After the dual-loop operation, the torque value is output and sent to the servo system.

Images