Collision Detection Method and System Based on Kinetic Model and Acceleration Adjustment Threshold

Abstract

The present invention relates to the technical field of collision detection, and specifically relates to a collision detection method and system based on a dynamic model and an acceleration adjustment threshold. The collision detection method includes: establishing a robot dynamic model with friction; setting a real-time collision threshold, calculating the absolute difference between the actual torque and the theoretical torque with friction, and comparing the absolute difference with the real-time collision threshold to determine whether a collision occurs. The present invention divides the running trajectory of the robot. During the process when the robot approaches the target object and is prone to collisions, by adjusting the initial collision threshold, reducing the set initial collision threshold, thereby increasing the sensitivity of collision detection; while in other path segments, the probability of a collision is small, increasing the set initial collision threshold, reducing the sensitivity of collision detection, so as to avoid downtime caused by too small a threshold setting.

Description

Technical Field

[0001] The present invention relates to the technical field of collision detection, and particularly to a collision detection method and system based on a dynamic model and an acceleration adjustment threshold. Background Art

[0002] A wafer transfer robot is a semiconductor robot specifically applied to the IC manufacturing industry, mainly used for the transfer and handling of wafers. Since wafers are fragile and extremely costly, and it is difficult to process the robot fingers, the occurrence of debris will pollute the entire vacuum environment and affect the efficiency of the entire production line. Therefore, the safety problem of the wafer transfer robot is particularly important, and collision detection is one of the cores to ensure safety.

[0003] Collision detection is essentially the detection of external interference forces. The methods can be mainly divided into two categories: collision detection methods relying on external sensors and collision detection methods without external sensors.

[0004] For this type of method based on external sensors, collision detection is performed by installing sensors (such as force / torque sensors, acceleration sensors, etc.) to sense external collision torques, and then making a judgment on external collisions. However, this technology increases the system cost and has a limited detection area.

[0005] For the collision detection method without external sensors, by establishing the dynamic model of the robot, the collected trajectory is substituted into the robot dynamics equation as the theoretical torque of the robot. At the same time, the current of the motors corresponding to each joint of the robot is collected, and after converting the current into torque, the absolute value of the difference from the theoretical torque is taken, and a fixed threshold or a dynamic threshold is set. If the set threshold is exceeded, it is determined that a collision has occurred. However, setting a fixed threshold faces two problems: First, if the threshold is too large, smaller collisions cannot be detected; Second, if the threshold is too small, the collision detection is too sensitive, and it may be misjudged as a collision due to the sudden change of torque caused by the acceleration and deceleration of the motor, resulting in downtime and affecting work efficiency. Summary of the Invention

[0006] The object of the present invention is to provide a collision detection method and system based on a dynamic model and an acceleration adjustment threshold to solve the problem that the setting of the collision threshold in the prior art is limited, resulting in collisions not being detected or being detected too sensitively.

[0007] The technical solution of the present invention is: A collision detection method based on a dynamic model and an acceleration adjustment threshold, comprising:

[0008] Establishing a robot dynamic model with friction;

[0009] Setting a real-time collision threshold, calculating the absolute difference between the actual torque and the theoretical torque with friction, comparing the absolute difference with the real-time collision threshold, and determining whether a collision has occurred;

[0010] Among them, the method for setting the real-time collision threshold is as follows:

[0011] a. Set the initial collision threshold and the adjustment threshold;

[0012] b. Determine the running trajectory of the robot, and perform real-time segmentation on the running trajectory based on the current running state of the robot, including the acceleration path, the constant-speed path, and the deceleration path;

[0013] c. Obtain the real-time trajectory distance of the robot from the nearest target position;

[0014] d. Based on the acceleration of the robot on different paths, set the threshold adjustment critical points corresponding to the running trajectory, and use the adjustment threshold to correct the initial collision threshold to obtain the real-time collision threshold.

[0015] Preferably, the two end points corresponding to the running trajectory of the robot are the first target position and the second target position respectively;

[0016] The current running state of the robot includes a first state and a second state with opposite running directions:

[0017] In the first state, the robot moves along the running trajectory from the first target position to the second target position, and divides the running trajectory from the first target position to the second target position into an acceleration path, a constant-speed path, and a deceleration path;

[0018] In the second state, the robot moves along the running trajectory from the second target position to the first target position, and divides the running trajectory from the first target position to the second target position into a deceleration path, a constant-speed path, and an acceleration path.

[0019] Preferably, the robot has a first acceleration on the acceleration path and a second acceleration on the deceleration path;

[0020] The distance between the threshold adjustment critical point and the nearest target position is positively correlated with the absolute value of the acceleration.

[0021] Preferably, in each running state, there are two threshold adjustment critical points corresponding to the running trajectory. The two threshold adjustment critical points are respectively located on the acceleration path and the deceleration path, and are respectively defined as the acceleration threshold adjustment critical point and the deceleration threshold adjustment critical point.

[0022] Preferably, when the robot runs between the acceleration threshold adjustment critical point and the nearest target position, and between the deceleration threshold adjustment critical point and the nearest target position, the corresponding real-time collision threshold is the difference between the initial collision threshold and the adjustment threshold;

[0023] When the robot runs between the acceleration threshold adjustment critical point and the deceleration threshold adjustment critical point, the corresponding real-time collision threshold is the sum of the initial collision threshold and the adjustment threshold.

[0024] Preferably, a first intersection point is set between the acceleration path and the uniform speed path, and a second intersection point is set between the deceleration path and the uniform speed path;

[0025] When the robot runs between the acceleration threshold adjustment critical point and the nearest target position, and between the deceleration threshold adjustment critical point and the nearest target position, the corresponding real-time collision threshold is the difference between the initial collision threshold and the adjustment threshold;

[0026] When the robot runs between the acceleration threshold adjustment critical point and the first intersection point, and between the deceleration threshold adjustment critical point and the second intersection point, the corresponding real-time collision threshold is the initial collision threshold;

[0027] When the robot runs on the uniform speed path, the corresponding real-time collision threshold is the sum of the initial collision threshold and the adjustment threshold.

[0028] Preferably, the method for establishing a robot dynamics model with friction is as follows:

[0029] Establish a dynamics model of the robot;

[0030] Collect the actual torques of each joint of the robot, and calculate the frictionless theoretical torque according to the collected robot positions;

[0031] Determine the friction coefficients of each joint of the robot, and the identified friction coefficients minimize the sum of the squares of the errors between the theoretical torque and the actual torque, thereby establishing a robot dynamics model with friction.

[0032] Preferably, the method for obtaining the actual torque of each joint of the robot is as follows:

[0033] Collect the currents of the motors corresponding to each joint, and convert the currents into actual torques;

[0034] The conversion relationship is as follows:

[0035] ;

[0036] Wherein, is the actual torque, is the torque constant of the motor, is the collected current;

[0037] The method for obtaining the frictionless theoretical torque of each joint of the robot is as follows:

[0038] Collect the encoded data of the robot, convert the issued encoded value into the position, speed, and acceleration data of the robot through the central difference method, and substitute them into the robot dynamics model to calculate the frictionless theoretical torque of each joint of the robot.

[0039] Preferably, in the process of establishing the robot dynamics model with friction, the least squares method is used to identify the friction coefficients in the friction models of each joint of the robot. In the case where the robot has no collision, the identified friction coefficients should minimize the sum of the squares of the errors between the theoretical torque and the actual torque, and the friction identification is converted into an optimization problem:

[0040] ;

[0041] where n is the total number of samples, is the theoretical torque, is the actual torque;

[0042] represents the constraint condition of, that is, the sum of the squares of the errors between the theoretical torque and the actual torque;

[0043] The optimization problem is to find the static friction coefficient and dynamic friction coefficient that minimize ; that is, the friction coefficients of each joint of the robot are identified, and a robot dynamics model with friction is established.

[0044] This application also discloses a collision detection system based on a dynamics model and an acceleration adjustment threshold, which is used to execute the above-mentioned collision detection method based on a dynamics model and an acceleration adjustment threshold.

[0045] Compared with the prior art, the advantages of the present invention are:

[0046] (1) The present invention divides the running trajectory of the robot. During the process where the robot is close to the target and is prone to collision, by adjusting the initial collision threshold, reducing the set initial collision threshold, thereby increasing the collision detection sensitivity; while in other path segments, the probability of collision is small, avoiding the influence of some environmental interference and other factors, by increasing the set initial collision threshold, reducing the collision detection sensitivity to avoid downtime caused by too small threshold setting.

[0047] (2) Considering the possibility of the robot pausing due to collision during operation, and the situation where the uniform speed section may be very short or even non-existent, the present invention also combines the real-time trajectory distance between the robot and the target for threshold adjustment. This dual adjustment mechanism that comprehensively considers acceleration and trajectory distance enables the robot to accurately set an appropriate collision threshold under complex working conditions, further improving the adaptability and reliability of the robot. Description of the Drawings

[0048] The present invention will be further described below in conjunction with the accompanying drawings and embodiments:

[0049] Figure 1 It is a flowchart of a collision detection method based on a dynamic model and an acceleration adjustment threshold according to the present invention;

[0050] Figure 2 It is a speed broken line graph corresponding to the robot of the present invention in the first state and the second state;

[0051] Figure 3 It is a distribution diagram of real-time collision thresholds in a collision detection method based on a dynamic model and an acceleration adjustment threshold according to an embodiment of the present invention;

[0052] Figure 4 It is a distribution diagram of real-time collision thresholds in a collision detection method based on a dynamic model and an acceleration adjustment threshold according to another embodiment of the present invention. Specific embodiments

[0053] The following further elaborates on the content of the present invention in conjunction with specific embodiments:

[0054] As Figure 1 shown, a collision detection method based on a dynamic model and an acceleration adjustment threshold includes:

[0055] S1. Establish a dynamic model of the robot with friction.

[0056] Specifically:

[0057] S101. Establish a dynamic model of the robot using the Lagrangian method.

[0058] S102. Collect the actual torques of each joint of the robot, and calculate the theoretical torque without friction based on the collected robot positions;

[0059] Among them, the method for obtaining the actual torques of each joint of the robot is:

[0060] Collect the currents of the motors corresponding to each joint, and convert the currents into actual torques;

[0061] The conversion relationship is as follows:

[0062] ;

[0063] Among them, is the actual torque, is the torque constant of the motor, is the collected current;

[0064] The method for obtaining the frictionless theoretical torque of each joint of the robot is as follows:

[0065] Collect the encoded data of the robot, convert the issued encoded values into the position, velocity, and acceleration data of the robot through the central difference method, and substitute them into the robot dynamics model to calculate the frictionless theoretical torque of each joint of the robot.

[0066] The above uses the "issued encoding" instead of the "feedback encoding" mainly because the feedback encoded data may be affected by various noises during transmission and acquisition, resulting in data deviation or inaccuracy. The issued encoding is clear and stable original instruction information without these interference factors, thus avoiding the influence of noise on the dynamics calculation.

[0067] S103. Determine the friction coefficients of each joint of the robot. The identified friction coefficients minimize the sum of the squares of the errors between the theoretical torque and the actual torque, and then establish a robot dynamics model with friction.

[0068] During the establishment of the robot dynamics model with friction, the least squares method is used to identify the friction coefficients in the friction force model of each joint of the robot. The least squares method finds the best function match for the data by minimizing the sum of the squares of the errors. In the non-collision scenario of the robot, the identified friction coefficients should minimize the sum of the squares of the errors between the theoretical torque (the torque calculated according to the established dynamics model) and the actual torque (the torque measured based on current conversion during the actual operation of the robot). The friction identification is converted into an optimization problem:

[0069] ;

[0070] where n is the total number of samples, is the theoretical torque; is the actual torque;

[0071] denotes the constraint condition of, which is the sum of the squares of the errors between the theoretical torque and the actual torque, denotes "constrained to", that is, the abbreviation of subject to;

[0072] The optimization problem is to find the static friction coefficient and the dynamic friction coefficient that minimize , which converts the problem of identifying the friction coefficients into an optimization problem.

[0073] Based on the optimization problem, the friction coefficients of each joint of the robot are identified, and then the robot dynamics model with friction can be established using these friction coefficients.

[0074] S2. Set the real-time collision threshold, calculate the absolute difference between the actual torque and the theoretical torque with friction, compare the absolute difference with the real-time collision threshold, and determine whether a collision has occurred.

[0075] Since a fixed collision threshold does not have environmental adaptability, on the one hand, when the collision threshold is set too large, the torque changes caused by some minor collisions may not exceed this threshold, resulting in the inability to detect collisions in a timely manner, so that the robot cannot respond in a timely manner when a collision occurs, which may cause equipment damage or other safety hazards. On the other hand, if the threshold is set too small, the collision detection system will become too sensitive. Because during the normal operation of the robot, the acceleration and deceleration operations of the motor will inevitably cause sudden changes in torque, and this normal torque fluctuation is very likely to exceed the smaller threshold and be misjudged as a collision, resulting in unnecessary shutdown of the robot and seriously affecting work efficiency. Therefore, based on the real-time operating state of the robot, this application sets a real-time collision threshold, and the setting method of the real-time collision threshold is as follows:

[0076] S201. Set the initial collision threshold Q1 and the adjustment threshold Q2.

[0077] Regarding the initial collision threshold Q1, this is a preset initial standard value for judging whether a collision has occurred. It is determined before starting the operation or performing collision detection, and is set based on a preliminary analysis and experience of various parameters (such as motor torque, movement speed, acceleration, etc.) during the normal operation of the robot. It provides a basic judgment starting point for collision detection, but since the preset initial collision threshold Q1 is a fixed value, it may not be able to fully adapt to the complex and changeable actual operating conditions, and there is a possibility of misjudgment or missed judgment. Given the limitations of the initial collision threshold Q1, it is necessary to adjust the threshold according to the actual operating conditions, so the adjustment threshold Q2 is introduced to correct the initial collision threshold Q1.

[0078] S202. As Figure 2 shown, determine the running track of the robot. The two end points corresponding to the running track of the robot are the first target position and the second target position respectively; under the actual operating conditions, the first target position can be defined as the workbench, and the second target position can be defined as the wafer cassette; the movement states of the robot along the running track include the first state and the second state with opposite running directions:

[0079] In the first state, the robot moves from the first target position to the second target position along the running trajectory, that is, it moves from the workbench to the wafer cassette. When the robot executes the material taking process step, the arm of the robot is in the empty material state in the first state, and the robot moves to wait for material taking inside the wafer cassette in the empty material state. When the robot executes the material receiving process step, the arm of the robot is in the full material state in the first state, and the robot is used to take out the wafers on the workbench and put them into the wafer cassette.

[0080] In the second state, the robot moves from the second target position to the first target position along the running trajectory, that is, it moves from the wafer cassette to the workbench. When the robot executes the material taking process step, the arm of the robot is in the full material state in the second state, and the robot is used to take out the wafers in the wafer cassette and put them on the workbench. When the robot executes the material receiving process step, the arm of the robot is in the empty material state in the second state, and the robot moves to wait for material taking at the workbench in the empty material state.

[0081] S203. Since the robot generally needs to go through the processes of acceleration, constant speed, and deceleration whether it moves from the first target position to the second target position or from the second target position to the first target position. Of course, in some scenarios, it may directly go through the processes of acceleration and deceleration without the process of constant speed. In this embodiment, the running trajectory is segmented in real time based on the current running state of the robot, including an acceleration path, a constant speed path, and a deceleration path.

[0082] In the ideal state, that is, in the case of collision-free stop, the robot will go through the processes of acceleration, constant speed, and deceleration. However, in the actual working condition, the robot will experience the situation of collision detection and stop. For example, in the acceleration stage, a collision stop occurs. After stopping, the robot will continue to experience acceleration, and during this process, there will be a speed interruption and a re-acceleration process. Still, the entire forward path is defined as the acceleration path. Another example is that a collision stop occurs in the deceleration stage. After stopping, the robot will still go through the process of acceleration and then deceleration. Even though this process includes an acceleration link, since its ultimate goal is to make the robot reach the target position and decelerate and stop, the entire forward path is still defined as the deceleration path at this time.

[0083] Specifically:

[0084] In the first state, since the robot moves from the first target position to the second target position, the running trajectory from the first target position to the second target position is segmented into an acceleration path, a constant speed path, and a deceleration path.

[0085] In the second state, since the robot moves from the second target position to the first target position, the running trajectory from the first target position to the second target position is segmented into a deceleration path, a constant speed path, and an acceleration path.

[0086] The robot has a first acceleration on the acceleration path and a second acceleration on the deceleration path. During motion control, the setting of acceleration plays a crucial role in its efficient and stable operation. Generally speaking, from a theoretical perspective, the robot can have accelerations with equal absolute values on the acceleration path and the deceleration path, that is: the absolute value of the first acceleration is equal to the absolute value of the second acceleration. Such a setting can make the motion of the robot exhibit a certain symmetry in some simple motion scenarios, facilitating the design of motion control algorithms and the debugging of the system.

[0087] In actual application scenarios such as semiconductor manufacturing, taking wafer handling as an example, wafers are extremely precise and fragile semiconductor materials. A large number of wafers are placed in a wafer cassette. The end effector of the robot needs to accurately extend into the limited space of the wafer cassette. During this process, to ensure the safety of the wafers and avoid damage to the wafers caused by the impact of the robot's motion, the robot must decelerate slowly when approaching the wafer cassette. Because a smaller acceleration change can make the motion of the robot smoother and reduce the potential threat to the wafers.

[0088] In the peripheral area of the workbench, there is usually a relatively larger space range, without the strict restrictions on the robot's motion like inside the wafer cassette. In this area, to improve work efficiency, the robot can have a larger acceleration. A larger acceleration can enable the robot to reach the target speed faster, shorten the time from one working position to another, and thus improve the efficiency of the entire production process. Since during wafer processing, the robot's handling of wafers is just one process step in the entire equipment, this process step must ensure that it can cooperate with other process steps. Therefore, the control of the handling time (the operating cycle of the robot) is very important.

[0089] Of course, in more complex working scenarios, since the robot has both empty-load and full-load situations in the first state and the second state. In the full-load state, to ensure that the wafers do not shake, collide, etc. due to excessive acceleration during motion, the acceleration needs to be reduced; while in the empty-load state, the acceleration can be set relatively larger. Therefore, the robot can also have different accelerations in the first state and the second state.

[0090] S204. Obtain the real-time trajectory distance of the robot from the nearest target position.

[0091] The definition of the nearby target position is the distance of the robot from the starting end of the running trajectory during the acceleration path and the distance from the end of the running trajectory during the deceleration path. For example, in the first state, when the robot is on the acceleration path, the real-time trajectory distance from the nearby target position is the distance to the first target position; when the robot is on the deceleration path, the real-time trajectory distance from the nearby target position is the distance to the second target position. Of course, in the second state, when the robot is on the acceleration path, the real-time trajectory distance from the nearby target position is the distance to the second target position; when the robot is on the deceleration path, the real-time trajectory distance from the nearby target position is the distance to the first target position.

[0092] The definition of the real-time trajectory distance is the distance that the robot moves along the running trajectory to the target object, not the straight-line distance to the target object. That is to say, when the running trajectory is a curve, the real-time trajectory distance is the distance when moving along the curve.

[0093] S205. Set the threshold adjustment critical point corresponding to the running trajectory based on the acceleration of the robot in different paths, and use the adjusted threshold to correct the initial collision threshold to obtain the real-time collision threshold.

[0094] Specifically:

[0095] The distance between the threshold adjustment critical point and the nearby target position is positively correlated with the absolute value of the acceleration. That is to say, when the acceleration of the robot is greater, the threshold adjustment critical point should be farther from the nearby target position; when the acceleration of the robot is smaller, the threshold adjustment critical point should be closer to the nearby target position.

[0096] When the acceleration of the robot is large, its motion state changes more rapidly and the generated inertia is also greater. During the process of approaching the target position, due to the large acceleration, the speed of the robot increases or decreases rapidly. If the threshold adjustment critical point is close to the nearby target position, in the case of high-speed motion and large inertia, once the threshold adjustment critical point is reached for relevant threshold adjustments (such as the collision detection threshold, etc.), the robot may not be able to respond to the adjusted control strategy in time and effectively due to inertia, resulting in difficulty in accurately stopping at the target position or completing the corresponding actions, and even overshooting, collision and other situations may occur. For example, the robot moves rapidly towards the target position with a large acceleration and only performs threshold adjustment when it is close to the target position. Due to its large inertia, it is difficult to stop precisely according to the new control requirements immediately. Therefore, the threshold adjustment critical point needs to be set farther away from the nearby target position, and the threshold adjustment is carried out in advance to leave enough time for the robot to respond to the adjustment to adapt to the change of the motion state and ensure the accuracy and safety of the motion. When the acceleration of the robot is small, its motion state changes relatively slowly and the inertia is also small. During the process of approaching the target position, even if the threshold adjustment critical point is close to the nearby target position, the robot can relatively easily respond to the control strategy after the threshold adjustment and achieve smooth motion and accurate positioning.

[0097] In each operating state, there are two threshold adjustment critical points corresponding to the operating trajectory. The two threshold adjustment critical points are respectively located on the acceleration path and the deceleration path, and are respectively defined as the acceleration threshold adjustment critical point and the deceleration threshold adjustment critical point.

[0098] It should be noted that Figure 2 The speed line graph in is a graph showing the relationship between the real-time speed and time. The "acceleration threshold adjustment critical point" and "deceleration threshold adjustment critical point" indicated therein are the areas of the corresponding regions. For example, the area of the region between the acceleration threshold critical point, the first target position and the speed line graph is the trajectory distance between the acceleration threshold critical point and the first target position; the area of the region between the deceleration threshold critical point, the second target position and the speed line graph is the trajectory distance between the deceleration threshold critical point and the second target position.

[0099] As Figure 3 shown, in an implementation manner, when the robot runs between the acceleration threshold adjustment critical point and the nearby target position, and between the deceleration threshold adjustment critical point and the nearby target position, the corresponding real-time collision threshold is the difference between the initial collision threshold and the adjustment threshold, that is, Q1 - Q2; when the robot runs between the acceleration threshold adjustment critical point and the deceleration threshold adjustment critical point, the corresponding real-time collision threshold is the sum of the initial collision threshold and the adjustment threshold, that is, Q1 + Q2.

[0100] For example, when the robot is in the first state and moving from the first target position to the critical point for accelerating threshold adjustment, the real-time collision threshold is Q1 - Q2. The smaller threshold can improve the sensitivity of collision detection. When moving from the critical point for accelerating threshold adjustment to the critical point for decelerating threshold adjustment, the real-time collision threshold is Q1 + Q2. At this time, the robot has a certain distance from both ends of the target position. By increasing the collision threshold, the possibility of false judgment of the device can be prevented. When moving from the critical point for decelerating threshold adjustment to the second target position, the real-time collision threshold is Q1 - Q2, and the sensitivity is still improved by the adjusted smaller threshold.

[0101] It should be noted that, generally, acceleration is considered an important factor affecting the collision threshold. In some conventional operating conditions, adjusting the threshold based on acceleration can indeed adapt to the change of the robot's motion state to a certain extent. However, in actual complex operating conditions, there are many special situations. For example, in some specific operation tasks, the running trajectory of the robot may be restricted by various factors such as the working environment, the operation object, and the process requirements, resulting in no uniform speed path or a very short uniform speed path, and it is basically in a state of slow acceleration or slow deceleration during the whole running process. When the robot is always in a state of slow acceleration or slow deceleration, if only adjusting the collision detection threshold based on acceleration, obvious limitations will occur. Because there is always acceleration during the whole running process, if following the traditional acceleration-based adjustment method, the real-time collision threshold may remain constant throughout the running trajectory, making it difficult to make effective adaptive adjustments according to the change of the actual motion state of the robot.

[0102] Adjusting the real-time collision threshold in combination with the real-time trajectory distance can well make up for this defect. The real-time trajectory distance reflects the position of the robot during the running process and the relationship with the target position, and it can more comprehensively reflect the motion process of the robot. As the robot moves along the running trajectory, the real-time trajectory distance is constantly changing. Adjusting the threshold based on this dynamically changing parameter can make the real-time collision threshold more in line with the actual needs of the robot at different positions.

[0103] At the same time, adjusting the threshold in combination with the real-time trajectory distance can also show advantages when the robot stops due to interruption. After the collision interruption stops and the collision is released, the robot will experience an acceleration process again. At this time, by monitoring the real-time trajectory distance to the nearest target position, it can be judged whether to adjust the real-time collision threshold. If only using the acceleration-based adjustment rule, it will result in the real-time collision threshold of Q1 - Q2 when the robot is at a relatively long distance from the nearest target position.

[0104] Such as Figure 4As shown, in other embodiments, for a case where the path segment is relatively long, including a relatively long acceleration path, a constant-speed path, and a deceleration path, a first junction point is set between the acceleration path and the constant-speed path, and a second junction point is set between the deceleration path and the constant-speed path.

[0105] When the robot runs between the acceleration threshold adjustment critical point and the nearest target position, and between the deceleration threshold adjustment critical point and the nearest target position, the corresponding real-time collision threshold is the difference between the initial collision threshold and the adjustment threshold; when the robot runs between the acceleration threshold adjustment critical point and the first junction point, and between the deceleration threshold adjustment critical point and the second junction point, the corresponding real-time collision threshold is the initial collision threshold; when the robot runs on the constant-speed path, the corresponding real-time collision threshold is the sum of the initial collision threshold and the adjustment threshold.

[0106] For example: in the first state of the robot, during the process of moving from the first target position to the acceleration threshold adjustment critical point, the real-time collision threshold is Q1 - Q2. The smaller threshold can improve the sensitivity of collision detection. During the process of moving from the acceleration threshold adjustment critical point to the first junction point, the real-time collision threshold is Q1. At this time, the initial collision threshold Q1 is set as the real-time collision threshold, as a kind of transitional setting, to adapt to the robot's conversion from a scenario with a relatively high requirement for collision detection sensitivity to a relatively low one. During the process of moving from the first junction point to the second junction point, the real-time collision threshold is Q1 + Q2. At this time, the robot has a certain distance from the target positions at both ends. By increasing the collision threshold, the possibility of misjudgment of the device is prevented. During the process of moving from the second target position to the deceleration threshold adjustment critical point, the real-time collision threshold is Q1. During the process of moving from the deceleration threshold adjustment critical point to the second target position, the real-time collision threshold is Q1 - Q2. The sensitivity is still improved by the adjusted smaller threshold.

[0107] In summary, in this application, based on the acceleration and the real-time trajectory distance, the running trajectory of the robot is segmented. During the process of the robot approaching the target object, due to the presence of the target object and the change of the relative position, the possibility of collision significantly increases. At this time, the present invention makes a targeted adjustment to the initial collision threshold, reduces the set initial collision threshold, so that the sensitivity of the collision detection system is greatly improved, thereby effectively avoiding collision with the target object and ensuring the safe operation of the robot in the dangerous area. In other path segments of the robot's operation, the set initial collision threshold is selected to be increased to reduce the sensitivity of collision detection, avoiding the frequent shutdown of the robot caused by too small a threshold setting. The overly sensitive collision detection may be affected by some environmental interference factors, resulting in misjudgment, causing the robot to stop running unnecessarily and reducing the work efficiency. By reasonably increasing the collision threshold, the robot can run more smoothly in these path segments, maximizing the work efficiency on the premise of ensuring safety.

[0108] In an exemplary embodiment, a collision detection system based on a kinetic model and an acceleration adjustment threshold is further provided for implementing the above-mentioned collision detection method based on a kinetic model and an acceleration adjustment threshold.

[0109] The above embodiments are only for illustrating the technical concept and features of the present invention, and the purpose is to enable those skilled in the art to understand the content of the present invention and implement it accordingly. It is not intended to limit the protection scope of the present invention. For those skilled in the art, it is obvious that the present invention is not limited to the details of the above exemplary embodiments, and can be implemented in other specific forms without departing from the spirit or basic features of the present invention. Therefore, from any point of view, the embodiments should be regarded as exemplary and non-limiting. The scope of the present invention is defined by the appended claims rather than the above description. Therefore, it is intended to include all changes falling within the meaning and scope of the equivalent elements of the claims in the present invention.

Claims

1. A collision detection method based on a kinetic model and an acceleration adjustment threshold, characterized in that Including: Establish a robot dynamics model with friction; Set a real-time collision threshold, calculate the absolute difference between the actual torque and the theoretical torque with friction, and compare the absolute difference with the real-time collision threshold to determine whether a collision occurs; Among them, the method for setting the real-time collision threshold is as follows: a. Set an initial collision threshold and an adjustment threshold; b. Determine the running trajectory of the robot. The two end points corresponding to the running trajectory of the robot are the first target position and the second target position respectively; Based on the current running state of the robot, the running trajectory is segmented in real time, including an acceleration path, a constant-speed path and a deceleration path; c. Obtain the real-time trajectory distance of the robot from the nearest target position; d. Based on the acceleration of the robot on different paths, set the threshold adjustment critical points corresponding to the running trajectory, and use the adjustment threshold to correct the initial collision threshold to obtain the real-time collision threshold; Among them, the current running state of the robot includes a first state and a second state with opposite running directions: In the first state, the robot moves along the running trajectory from the first target position to the second target position, and the running trajectory from the first target position to the second target position is segmented into an acceleration path, a constant-speed path and a deceleration path; In the second state, the robot moves along the running trajectory from the second target position to the first target position, and the running trajectory from the first target position to the second target position is segmented into a deceleration path, a constant-speed path and an acceleration path; In each running state, there are two threshold adjustment critical points corresponding to the running trajectory. The two threshold adjustment critical points are respectively located on the acceleration path and the deceleration path, and are respectively defined as the acceleration threshold adjustment critical point and the deceleration threshold adjustment critical point; When the robot runs between the acceleration threshold adjustment critical point and the nearest target position, and between the deceleration threshold adjustment critical point and the nearest target position, the corresponding real-time collision threshold is the difference between the initial collision threshold and the adjustment threshold; When the robot runs between the acceleration threshold adjustment critical point and the deceleration threshold adjustment critical point, the corresponding real-time collision threshold is the sum of the initial collision threshold and the adjustment threshold; Alternatively, set the first intersection point between the acceleration path and the constant-speed path, and the second intersection point between the deceleration path and the constant-speed path; When the robot runs between the acceleration threshold adjustment critical point and the nearest target position, and between the deceleration threshold adjustment critical point and the nearest target position, the corresponding real-time collision threshold is the difference between the initial collision threshold and the adjustment threshold; When the robot runs between the acceleration threshold adjustment critical point and the first intersection point, and between the deceleration threshold adjustment critical point and the second intersection point, the corresponding real-time collision threshold is the initial collision threshold; When the robot runs on the constant-speed path, the corresponding real-time collision threshold is the sum of the initial collision threshold and the adjustment threshold.

2. The collision detection method based on a kinetic model and an acceleration adjustment threshold according to claim 1, wherein: The robot has a first acceleration on the acceleration path and a second acceleration on the deceleration path; The distance between the threshold adjustment critical point and the nearest target position is positively correlated with the absolute value of the acceleration.

3. The collision detection method based on the kinetic model and the acceleration adjustment threshold according to claim 1, wherein The method for establishing a robot dynamics model with friction is as follows: Establish a dynamics model of the robot; Collect the actual torques of each joint of the robot, and calculate the theoretical torque without friction according to the collected position of the robot; Determine the friction coefficients of each joint of the robot. The identified friction coefficients minimize the sum of the squares of the errors between the theoretical torque and the actual torque, and then establish a robot dynamics model with friction.

4. The collision detection method based on the kinetic model and the acceleration adjustment threshold according to claim 3, characterized in that The method for obtaining the actual torque of each joint of the robot is as follows: Collect the current of the motor corresponding to each joint and convert the current into the actual torque; The conversion relationship is as follows: ； Among them, is the actual torque, is the torque constant of the motor, is the measured current; The method for obtaining the frictionless theoretical torque of each joint of the robot is as follows: Collect the encoded data of the robot, convert the issued encoded values into the position, velocity, and acceleration data of the robot through the central difference method, and substitute them into the robot dynamics model to calculate the frictionless theoretical torque of each joint of the robot.

5. The collision detection method based on the kinetic model and the acceleration adjustment threshold according to claim 3, wherein During the establishment of the robot dynamics model with friction, the least squares method is used to identify the friction coefficients in the friction force model of each joint of the robot. In the collision-free scenario of the robot, the identified friction coefficients should minimize the sum of the squares of the errors between the theoretical torque and the actual torque, and convert the friction identification into an optimization problem: ； where n is the total number of samples, is the theoretical torque, is the actual torque; representation constraint condition, which is the sum of squares of the errors between the theoretical torque and the actual torque; The optimization problem is to minimize the static friction coefficient and the dynamic friction coefficient, that is, the friction coefficients of each joint of the robot are identified, and a robot dynamics model with friction is established.

6. A collision detection system based on a kinetic model and an acceleration adjustment threshold, characterized in that: For implementing the collision detection method based on the dynamics model and the acceleration adjustment threshold according to any one of claims 1-5.

Images