A fast shutdown method along a planned trajectory

Abstract

The application relates to the technical field of robot control, in particular to a quick stop method along a planned track, which comprises the following steps: when a robot starts, loading setting parameters required for calculation, setting a minimum stop time and a robot speed change rate feedback compensation amount; obtaining current robot state information and saving a current speed change rate in a speed change rate backup variable; adjusting the stop time of the robot according to real-time feedback positions of robot joint shafts; setting a speed change rate; setting a maximum deceleration of the robot joint shafts used in later calculation; updating the speed change rate of the robot; adjusting a current speed change rate value of the robot; and updating the speed change rate feedback compensation amount of the robot. The application can control the robot to execute a quick stop action with local maximum acceleration at different speeds, shorten the stop distance as much as possible within a fixed stop time, and reduce economic losses as much as possible in an emergency.

Description

Technical Field

[0001] This invention relates to the field of robot control technology, specifically a method for rapid stopping along a planned trajectory. Background Technology

[0002] To meet the demands of high-efficiency development in the digital age, robot control technology is iterating and upgrading at an astonishing pace. While demanding high-quality control from robots—such as speed, stability, and accuracy—robot safety remains the primary control indicator for evaluation. With the widespread application of robots in various fields, whether directly replacing human labor or in collaborative operations, ensuring the robot can stop smoothly with the shortest braking distance in emergency situations remains the primary goal for protecting life and property. Therefore, ensuring rapid robot cessation in emergency situations is of significant research value.

[0003] The Chinese invention patent document "A Method for Planning the Stopping Motion Trajectory of an Industrial Robot" (CN201911381022) addresses the drawback of interpolation information response delay between the robot trajectory interpolation layer and the control layer. It transfers the processing logic of the stop command to the control layer, directly performing secondary planning on the calculated joint positions, thus improving the response timeliness of rapid stopping. However, this implementation is sensitive to data noise when constructing a cubic spline model, lacks certain local control capabilities, and has high computational resource requirements and specific hardware configuration requirements. The "A Motion Control Emergency Stop Method" (CN202011098843) uses quartic spline curve planning to ensure continuous speed and acceleration during the emergency stop process, and that the stopping position will not exceed the target point. However, this method relies on the early determination of the stopping position. Inaccurate determination of the stopping position can lead to excessively long stopping times. Summary of the Invention

[0004] To address the aforementioned technical problems, this invention proposes a rapid shutdown method along a planned trajectory.

[0005] The technical problem to be solved by this invention is achieved by the following technical solution:

[0006] A rapid shutdown method along a planned trajectory includes the following steps:

[0007] Step 1: When the robot starts, load the settings required for calculation: maximum stoptimeupper (>0), maximum joint axis velocity (>0), maximum joint axis acceleration (>0), joint axis follow-up error threshold (>0), system control frequency (>0), and set the minimum stoptimelower and robot scaling speed feedback compensation amount extravel.

[0008] Step 2: After receiving the emergency stop signal, the system obtains the current robot status information: real-time feedback speed of joint axes (velfeedback), real-time feedback acceleration of joint axes (accfeedback), real-time feedback follow-up error of joint axes (fellowerr), and current robot magnification value (feedrade), and saves the current magnification to the magnification backup variable (feedratepre).

[0009] Step 3: Based on the real-time feedback position of the robot's joint axes, adjust the robot's stoptime to ensure that the stoptime is always within the range of [stoptimelower, stoptimeupper].

[0010] Step 4: Set the rate of change of the magnification (feedratevel);

[0011] Step 5: Set the maximum deceleration of the robot joint axis used in the later calculation, decupper, and compare the real-time feedback follow-up error of the joint axis, flywerr, with the joint axis follow-up error threshold, flywerrthrod, in Step 1. When flywerr > flywerrthrod, the joint axis deceleration scaling ratio, k, can be adjusted from the default value of 1.0 to 0.9.

[0012] Step 6: Update the robot's scaling factor change rate (feedratevel);

[0013] Step 7: Adjust the robot's current scaling factor (feedrade);

[0014] Step 8: Update the robot's scaling factor change speed feedback compensation amount extravel.

[0015] As a further improvement to the present invention, the expression for the minimum downtime stoptimelower in step 1 is as follows:

[0016]

[0017] The expression for the robot's magnification change speed feedback compensation amount (extravel) is as follows:

[0018] extravel = 0 (2). As a further improvement of the present invention, the expression for the rate backup variable feedratepre in step 2 is as follows:

[0019] feedratepre = feedrate (3).

[0020] As a further improvement to the present invention, the expression for the stoptime in step 3 is as follows:

[0021]

[0022] stoptime=min(stoptimeupper,max(stoptime,stoptimelower)) (5).

[0023] As a further improvement of the present invention, the expression for the rate of change of the magnification in step 4, feedratevel, is as follows:

[0024]

[0025] As a further improvement of the present invention, the expression for the maximum deceleration of the robot joint axis, decupper, in step 5 is as follows:

[0026] decupper=k*accupper (7).

[0027] As a further improvement of the present invention, the expression for the robot's rate of change feedratevel in step 6 is as follows:

[0028]

[0029] As a further improvement to the present invention, the expression for the robot's current magnification value feedrade in step 7 is as follows:

[0030]

[0031] As a further improvement of the present invention, the expression for the robot magnification change speed feedback compensation amount extravel in step 8 is as follows:

[0032] extravel=(feedratevelpre-feedratevel)*freq (10).

[0033] The beneficial effects of this invention are:

[0034] This invention provides a rapid shutdown method along a planned trajectory, which has the advantages of simple calculation logic, strong local control capability, dynamic adjustment of shutdown position, and no dependence on hardware configuration.

[0035] 1. Implemented in the real-time task of the robot control layer, the speed adjustment interface for rapid shutdown is directly called after receiving the stop signal. Rapid control is performed based on the real-time feedback data of the robot, eliminating the need to call the robot trajectory interpolation layer for secondary planning. This ensures the real-time performance of robot speed control and effectively avoids control lag caused by replanning.

[0036] 2. Relying on real-time speed feedback data from each joint, the downtime is dynamically adjusted to ensure that the robot can stop quickly with maximum acceleration within the corresponding downtime at different speeds.

[0037] 3. Relying on real-time follow-up error feedback data of each joint, the follow-up error is compared with the maximum follow-up error threshold. When the follow-up error exceeds the maximum threshold, the rate of change of the robot's speed ratio is dynamically reduced. This ensures that the follow-up error of each axis remains healthy during robot speed adjustment.

[0038] 4. This ensures that when an emergency stop signal is received, the robot's scaling ratio can be adjusted at the largest possible rate of change within a unit of time, keeping the robot's acceleration at its maximum value. This guarantees that the robot can complete the emergency stop task with the shortest braking distance within the same stop time, thereby minimizing economic losses in emergency situations.

[0039] 5. By implementing the solution in the real-time task of the robot control layer, the position is directly sampled on the original planned trajectory. There is no need to call the planning module for secondary planning, which saves some computing resources of the control system and effectively ensures that the movement position is always on the planned path during the shutdown process. Attached Figure Description

[0040] The present invention will be further described below with reference to the accompanying drawings and embodiments:

[0041] Figure 1 This is a flowchart of robot motion control.

[0042] Figure 2 This is a flowchart for the rapid shutdown control. Detailed Implementation

[0043] To make the technical means, creative features, objectives and effects of this invention easier to understand, the invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0044] Robot control process (e.g.) Figure 1(As shown) Typically, users connect to the robot control system via a robot teach pendant or remote connection and issue the required motion commands. Upon receiving the motion commands from the upper layer, the motion command parsing module first analyzes the robot's state to determine if it meets the execution conditions for the motion commands. Only when the execution conditions are met can the motion command be parsed and sent to the motion planner. If the current state does not meet the execution conditions, the motion command parser directly refuses to parse the command and provides feedback on relevant exceptions and log information. Upon receiving a new motion command, the motion planner first verifies the motion command parameters. If the parameter verification is abnormal, trajectory planning is rejected, and exception and log information feedback is provided. If the parameter verification passes, trajectory planning calculation is performed. If the planning is incorrect, exception and log information feedback must be provided promptly. After planning is completed, the system waits for the robot control module to hand over the planning results signal. Based on its own motion state, the robot control module, at an appropriate time, notifies the planning module to prepare to hand over the planning results via a handover signal, and completes the trajectory result handover with the planning module. Then, position sampling is performed according to the set interpolation cycle, and the position sampling results are sent to the servo management module to control the robot to move to the target position.

[0045] During robot control, if an abnormal alarm is encountered and the robot needs to be stopped quickly, then... Figure 2 As shown, this invention provides a method for rapid shutdown along a planned trajectory, with the following specific steps:

[0046] Step 1: When the robot starts, load the required settings parameters for calculation, including: maximum stoptimeupper (>0), maximum joint axis velocity (>0), maximum joint axis acceleration (>0), joint axis follow-up error threshold (>0), system control frequency (>0), and set the minimum stoptimelower and robot scaling speed feedback compensation amount extravel.

[0047] The expression for the minimum downtime stoptimelower is as follows:

[0048]

[0049] The expression for the robot's magnification change speed feedback compensation amount (extravel) is as follows:

[0050] extravel = 0 (2).

[0051] Step 2: After receiving the emergency stop signal, the system obtains the current robot status information: real-time feedback speed of joint axes (velfeedback), real-time feedback acceleration of joint axes (accfeedback), real-time feedback follow-up error of joint axes (fellowerr), and current robot magnification value (feedrade), and saves the current magnification to the magnification backup variable (feedratepre).

[0052] The expression for the backup rate variable feedratepre is as follows:

[0053] feedratepre = feedrate (3).

[0054] Step 3: Based on the real-time feedback position of the robot's joint axes, adjust the robot's stoptime to ensure that the stoptime is always within the range of [stoptimelower, stoptimeupper].

[0055] The stoptime expression is as follows:

[0056]

[0057] stoptime=min(stoptimeupper,max(stoptime,stoptimelower)) (5).

[0058] Step 4: Set the rate of change (feedratevel).

[0059] The expression for the rate of change of the multiplier, feedratevel, is as follows:

[0060]

[0061] Step 5: Set the maximum deceleration of the robot joint axis used in the later calculation, decupper, and compare the real-time feedback follow-up error of the joint axis, flywerr, with the joint axis follow-up error threshold, flywerrthrod, in Step 1. When flywerr > flywerrthrod, the joint axis deceleration scaling ratio, k, can be adjusted from the default value of 1.0 to 0.9.

[0062] The expression for the maximum deceleration of the robot's joint axis, decupper, is as follows:

[0063] decupper=k*accupper (7).

[0064] Step 6: Update the robot multiplier change rate feedratevel.

[0065] The expression for the robot's rate of change, feedratevel, is as follows:

[0066]

[0067] Step 7: Adjust the robot's current scaling factor (feedrade).

[0068] The robot's current magnification value feedrade expression is as follows:

[0069]

[0070] Step 8: Update the robot's scaling factor change speed feedback compensation amount extravel.

[0071] The expression for the robot's magnification change speed feedback compensation amount (extravel) is as follows:

[0072] extravel=(feedratevelpre-feedratevel)*freq (10).

[0073] The foregoing has shown and described the basic principles, main features, and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The embodiments and descriptions in the specification are merely prisms of the invention. Various changes and modifications can be made to the invention without departing from its spirit and scope, and all such changes and modifications fall within the scope of the present invention as claimed. The scope of protection of this invention is defined by the appended claims and their equivalents.

Claims

1. A method for rapid shutdown along a planned trajectory, characterized in that: Includes the following steps: Step 1: When the robot starts, load and calculate the required settings parameters: maximum downtime. Maximum speed of joint axis Maximum acceleration of joint axis Joint axis follow-up error threshold System control frequency And set the minimum downtime. And robot magnification change speed feedback compensation amount ; Step 2: After receiving the emergency stop signal, the system obtains the current robot status information: real-time feedback speed of the joint axes. Real-time feedback acceleration of joint axis Real-time feedback of joint axis motion error Current robot magnification value And save the current multiplier to the multiplier backup variable. middle; Step 3: Adjust the robot's downtime based on the real-time speed feedback from the robot's joint axes. Ensure downtime Always in Within the range; Step 4: Set the rate of change of magnification ; Step 5: Set the maximum deceleration of the robot joint axes to be used in later calculations. And the joint axis feedback of the follow-up error in real time. Compared with the joint axis follow-up error threshold in step 1 When comparing, At that time, the joint axis deceleration is scaled up proportionally. Adjusted from the default value of 1.0 to 0.9; Step 6: Update the robot's scaling rate change speed ; Step 7: Adjust the robot's current magnification value ; Step 8: Update the robot's magnification change speed feedback compensation amount .

2. The rapid stopping method along a planned trajectory according to claim 1, characterized in that: Minimum downtime in step 1 The expression is as follows: 1）； Robot scaling rate change speed feedback compensation amount The expression is as follows: （2）。 3. The rapid stopping method along a planned trajectory according to claim 1, characterized in that: Step 2: Ratio Backup Variable The expression is as follows: （3）。 4. The rapid stopping method along a planned trajectory according to claim 1, characterized in that: Downtime in step 3 The expression is as follows: （4）； （5）。 5. The rapid stopping method along a planned trajectory according to claim 1, characterized in that: Rate of change of magnification in step 4 The expression is as follows: （6）。 6. The rapid stopping method along a planned trajectory according to claim 1, characterized in that: Maximum deceleration of robot joint axes in step 5 The expression is as follows: （7）。 7. The rapid stopping method along a planned trajectory according to claim 1, characterized in that: Step 6 Robot magnification change rate The expression is as follows: （8）。 8. The rapid stopping method along a planned trajectory according to claim 1, characterized in that: The current magnification value of the robot in step 7 The expression is as follows: （9）。 9. The rapid stopping method along a planned trajectory according to claim 1, characterized in that: Step 8: Robot scaling rate change speed feedback compensation amount The expression is as follows: （10）。

Images