Collision Detection Method, Device and Robot of Robot

Abstract

The present invention provides a collision detection method, device and robot for a robot. The method includes: real-time detecting the joint current and position deviation when the robot is working. When the change rate of the joint current is greater than a preset threshold and the position deviation is greater than a preset threshold, it is determined that the robot has a collision with a suddenly applied force; and real-time detecting the joint torque when the robot is working. When the joint torque exceeds the maximum limit torque threshold of the robot, it is determined that the robot has a collision with a slowly applied force; wherein the position deviation is the deviation between the position setpoint and the position feedback collected during the operation of the robot. Through the collision detection function composed of the change rate of the robot joint current, the corrected position difference, and the maximum rotation limit during operation, the present invention enables the robot to have a more accurate collision detection and judgment during operation, and greatly reduces the pressing force on the motor caused by the collision.

Description

Technical Field

[0001] The present invention relates to the field of robots, and more particularly to a method, device, and robot for collision detection of a robot. Background Art

[0002] In order to prevent a robot from causing harm to a person during a collision, or to protect the robot's own components from being damaged during a collision, the robot needs to have a collision detection function.

[0003] Currently, the mainstream solutions include:

[0004] (1) Install sensors outside the robot, such as wrapping a sensitive skin sensor on the outer surface of the robot, adding a vision sensor, installing a joint torque sensor, etc. Among them, installing a joint torque sensor is the most commonly used solution. However, there are two main deficiencies in installing sensors: on the one hand, the acquisition and processing of sensor data increase the complexity of the control system and are prone to problems of poor real-time performance; on the other hand, installing sensors will increase the complexity of the mechanical structure and the manufacturing cost.

[0005] (2) Implement collision detection through a control algorithm. By modeling the dynamic model of the robot, the magnitude of the joint torque is predicted, and then collision detection is judged through the difference in joint torque. For example, the Chinese patent with the publication number CN104985598A predicts the magnitude of the torque through dynamics, then corrects the predicted torque and the feedback torque through an algorithm, and finally obtains the torque difference as the condition for collision detection. Another example is the Chinese patent with the publication number CN1771114A, which predicts the joint torque of the robot through robot dynamics, then identifies the frictional torque, and finally establishes an external torque equation to calculate the magnitude of the torque during a collision. Such methods increase the computational load of the system, the establishment of dynamics is relatively complex, and the identification difficulty of friction is also relatively high, and the overall implementation difficulty is relatively large.

[0006] (3) Determine whether a collision occurs by directly detecting the change rate of the current of the robot joint. This solution has a low cost and a low implementation difficulty, but it is necessary to process the noise of the current to avoid misjudgment. For example, the Chinese patent with the publication number CN111906775A obtains the speed, acceleration, and current of the robot during operation. When the speed is less than the set threshold, it judges whether a collision occurs through the current, and when the speed is greater than the threshold, it judges whether a collision occurs through the second derivative of the current. This method is simple to implement, but there will be a sudden change in the current at the moment when the robot starts and stops or when it accelerates to a uniform speed, and it is easy to generate misjudgments at this time.

[0007] Therefore, the prior art requires a robot collision detection solution that can be non-dynamic.

[0008] The information disclosed in the background section above is only used to further understand the background of the present invention. Therefore, it may include information that is known to those of ordinary skill in the art and does not constitute prior art. Summary of the Invention

[0009] The present invention provides a method, device and robot for collision detection of a robot. The present invention realizes the function of robot collision detection under non-dynamic and sensorless conditions, and the implementation difficulty is not great and the feasibility is relatively high. It has better collision detection accuracy than general methods that only judge by current derivation or single speed and position difference, and can effectively improve the safety of the robot during operation.

[0010] The first aspect of the present invention provides a method for collision detection of a robot, characterized in that the method includes: detecting the joint current and position deviation of the robot during operation in real time, and when the change rate of the joint current is greater than a preset threshold and the position deviation is greater than a preset threshold, determining that the robot has a collision with a suddenly applied force; and detecting the joint torque of the robot during operation in real time, and when the joint torque exceeds the maximum limit torque threshold of the robot, determining that the robot has a collision with a slowly applied force; wherein the position deviation is the deviation between the position given and the position feedback collected during the operation of the robot.

[0011] According to an embodiment of the present invention, during the process of determining that the robot has a collision with a suddenly applied force, S21: collect the joint currents of each axis of the robot during operation, and take the derivative and filter each joint current to obtain the change rate of the joint current; S22: when the position deviation is greater than a preset position deviation threshold, correct the position deviation to obtain a corrected position deviation threshold; S23: when the corrected position deviation threshold is greater than the preset position deviation threshold and the change rate of the joint current is greater than the preset threshold, determine that the robot has a collision with a suddenly applied force.

[0012] According to an embodiment of the present invention, after it is determined in S23 that the robot has a collision with a suddenly applied force, the robot enters a reflected torque response mode, wherein the reflected torque response mode makes the robot servo system immediately switch to the torque control mode after detecting the collision, and while balancing the self-gravity of the robot link, makes the motor torque over-react to the external collision force. When a collision is detected, the robot's robotic arm will move away from the collision position with a greater acceleration.

[0013] According to an embodiment of the present invention, after the S23, it further includes: S24: Record the magnitude and selected direction of the joint current when a collision with a suddenly applied force occurs, and determine whether it is a in-phase or anti-phase collision; S25: If it is a high-speed instantaneous anti-phase collision of the robot, the robot enters the current control mode. When the robot reaches a predetermined reverse speed, the robot stops and outputs an alarm signal; S26: If it is not a high-speed instantaneous anti-phase collision of the robot, the robot stops and outputs an alarm signal; wherein the current control mode is to control the current loop of the robot to control the output force of the motor.

[0014] According to an embodiment of the present invention, in the S22, the correction of the position deviation includes: measuring the follow-up delay of the position given and the position feedback during the acceleration and deceleration process of the robot, dividing the follow-up delay by the control execution cycle of the robot to obtain a compensation coefficient, and correcting the position deviation through the compensation coefficient.

[0015] According to an embodiment of the present invention, the threshold of the maximum torque limit is obtained by multiplying the average value of the maximum torque of the robot operation cycle by an amplification factor.

[0016] According to an embodiment of the present invention, the process of determining that the robot has a collision with a slowly applied force includes: S31: Set a counter, and the counter increments by 1 every time the robot completes one operation cycle; S32: Accumulate the maximum torque value during each specified action process of the robot to obtain the accumulated maximum torque value; S33: When the value of the counter reaches the preset number of operation cycles N; S34: Calculate the maximum average torque of the robot during N operation cycles, and the average maximum torque = accumulated maximum torque value / N; S35: Multiply the maximum average torque by an amplification factor to obtain the threshold of the maximum torque limit of the robot; S36: At the (N + 1)-th operation cycle, when the joint torque of the robot exceeds the maximum limit torque threshold of the robot, determine that the robot has a collision and alarm.

[0017] According to an embodiment of the present invention, when the robot is manually accelerated or decelerated, enabled up and down, or paused and stopped, the threshold of the maximum torque limit is recalculated.

[0018] The second aspect of the present invention provides a collision detection device for a robot, characterized in that the device includes: a collision detection device for suddenly applying a force, which real-time detects the joint current and position deviation during the operation of the robot, and determines that a collision with a suddenly applied force occurs when the change rate of the joint current is greater than a preset threshold and the position deviation is greater than a preset threshold; a collision detection device for slowly applying a force, which real-time detects the joint torque during the operation of the robot, and determines that a collision with a slowly applied force occurs when the joint torque exceeds the maximum limit torque threshold of the robot; wherein the position deviation is the deviation between the position given and the position feedback collected during the operation of the robot.

[0019] According to an embodiment of the present invention, the collision detection device for suddenly applying a force is used for: collecting the joint current of each axis during the operation of the robot, differentiating and filtering each joint current to obtain the change rate of the joint current; when the position deviation is greater than a preset position deviation threshold, correcting the position deviation to obtain a corrected position deviation threshold; and when the corrected position deviation threshold is greater than the preset position deviation threshold and the change rate of the joint current is greater than the preset threshold, determining that a collision with a suddenly applied force occurs. The collision detection device for suddenly applying a force is further used for: after determining that a collision with a suddenly applied force occurs, the robot enters a reflected torque response mode, wherein the reflected torque response mode makes the robot servo system immediately switch to the torque control mode after detecting the collision, and while balancing the self-gravity of the robot link, makes the motor torque overreact to the external collision force. When detecting a collision, the robotic arm of the robot will accelerate away from the collision position with a greater acceleration. The collision detection device for suddenly applying a force is further used for: recording the magnitude and selected direction of the joint current when a collision with a suddenly applied force occurs, and judging whether it is a in-phase or anti-phase collision; if it is a high-speed instantaneous anti-phase collision of the robot, the robot enters the current control mode, and when the robot reaches a predetermined reverse speed, the robot stops and outputs an alarm signal; if it is not a high-speed instantaneous anti-phase collision of the robot, the robot stops and outputs an alarm signal; wherein the current control mode is to control the current loop of the robot to control the output force of the motor.

[0020] According to an embodiment of the present invention, the collision detection device for slowly applying force is configured to: set a counter, and the counter is incremented by 1 after each operation cycle of the robot; 32: accumulate the maximum torque values during each specified movement of the robot to obtain the accumulated maximum torque value; when the value of the counter reaches the preset number of operation cycles N; calculate the maximum average torque during the N operation cycles of the robot, where the average maximum torque = accumulated maximum torque value / N; multiply the maximum average torque by a magnification factor to obtain the maximum torque limit threshold of the robot; at the (N + 1)th operation cycle, when the joint torque of the robot exceeds the maximum limit torque threshold of the robot, it is determined that the robot has collided and an alarm is given.

[0021] The third aspect of the present invention provides a robot that uses the collision detection method of the above-mentioned robot or includes the collision detection device of the above-mentioned robot.

[0022] The solution of the present invention enables the robot to have a more accurate collision detection judgment during operation through the collision detection function composed of the joint current change rate, the corrected position difference, and the maximum rotation limit during operation of the robot, and performs different collision responses according to different collision forces and collision situations. When an instantaneous impact force collision occurs during high-speed operation, the robot will enter the current control mode, generate a reverse torque current, and then stop after reaching a certain speed. When the robot is running at a low speed or a slowly applied pressing force is detected, the robot will immediately stop and alarm, which will greatly reduce the pressing force on the motor caused by the collision. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] In order to more clearly illustrate the technical solutions of the present invention, the drawings required for the description of the embodiments will be briefly introduced below. Obviously, the drawings in the following description are only some embodiments of the present invention, and those of ordinary skill in the art can obtain other drawings based on these drawings without creative efforts.

[0024] Figure 1 FIG. shows schematic diagrams of two collision forces of a robot according to an exemplary embodiment of the present invention.

[0025] Figure 2 FIG. shows the overall flowchart of collision detection according to an exemplary embodiment of the present invention.

[0026] Figure 3 FIG. shows a schematic diagram of current detection collision according to an exemplary embodiment of the present invention.

[0027] Figure 4 FIG. shows a flowchart of position deviation compensation according to an exemplary embodiment of the present invention.

[0028] Figure 5 is the position command and position feedback waveform according to an exemplary embodiment of the present invention.

[0029] Figure 6 Shows the position command and position feedback waveform diagram collected during the operation of a robot according to an exemplary embodiment of the present invention.

[0030] Figure 7 Shows the maximum torque limit flow chart according to an exemplary embodiment of the present invention.

[0031] Figure 8 Shows the flow chart of the robot collision response process according to an exemplary embodiment of the present invention.

[0032] Figure 9 Shows the reflected torque response flow chart after collision according to an exemplary embodiment of the present invention.

[0033] Figure 10 Shows the block diagram of a robot collision detection device according to an exemplary embodiment of the present invention. Specific embodiments

[0034] As used herein, words such as "first", "second", etc. may be used to describe elements in the exemplary embodiments of the present invention. These words are only used to distinguish one element from another, and the inherent characteristics or order of the corresponding elements are not limited by these words. Unless otherwise defined, all terms (including technical or scientific terms) used herein have the same meaning as commonly understood by those of ordinary skill in the technical field to which the present invention belongs. Terms defined in a common dictionary are interpreted as having the same meaning as the context in the relevant technical field, and are not interpreted as having an ideal or overly formal meaning, unless explicitly defined as having such a meaning in the present invention.

[0035] Those skilled in the art will understand that the devices and methods of the present invention described herein and illustrated in the drawings are non-limiting exemplary embodiments, and the scope of the present invention is only defined by the claims. Features described or illustrated in connection with one exemplary embodiment may be combined with features of other embodiments. Such modifications and variations are included within the scope of the present invention.

[0036] Hereinafter, exemplary embodiments of the present invention will be described in detail with reference to the drawings. In the drawings, detailed descriptions of related known functions or configurations are omitted to avoid unnecessarily obscuring the technical points of the present invention. In addition, throughout the description, the same reference numerals always refer to the same circuit, module, or unit, and for the sake of brevity, repeated descriptions of the same circuit, module, or unit are omitted.

[0037] Reference to "embodiment" in this document means that the specific features, structures, or characteristics described in connection with the embodiment may be included in at least one embodiment of the present application. The phrase appearing at various positions in the specification does not necessarily refer to the same embodiment, nor is it an independent or alternative embodiment mutually exclusive with other embodiments. It is explicitly and implicitly understood by those skilled in the art that the embodiments described herein may be combined with other embodiments.

[0038] In the description of the embodiments of the present application, the term "and / or" is merely a description of the association relationship of associated objects, indicating that there can be three relationships. For example, A and / or B may represent: A exists alone, A and B exist simultaneously, and B exists alone. In addition, the character " / " in this document generally represents an "or" relationship between the associated objects before and after.

[0039] In the description of the embodiments of the present application, the term "plurality" refers to two or more (including two). Similarly, "multiple groups" refers to two or more groups (including two groups), and "multiple pieces" refers to two or more pieces (including two pieces).

[0040] In the description of the embodiments of the present application, the orientation or positional relationship indicated by technical terms such as "center", "longitudinal", "transverse", "length", "width", "thickness", "upper", "lower", "front", "rear", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer", "clockwise", "counterclockwise", "axial", "radial", "circumferential", etc. is based on the orientation or positional relationship shown in the drawings. It is only for the convenience of describing the embodiments of the present application and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore should not be construed as a limitation on the embodiments of the present application.

[0041] In the description of the embodiments of the present application, unless otherwise clearly specified and limited, technical terms such as "installation", "connection", "connection", "fixation", etc. should be understood in a broad sense. For example, it can be a fixed connection, a detachable connection, or integrated; it can also be a mechanical connection or an electrical connection; it can be directly connected or indirectly connected through an intermediate medium, and it can be the communication inside two elements or the interaction relationship between two elements. For those of ordinary skill in the art, the specific meanings of the above terms in the embodiments of the present application can be understood according to specific circumstances.

[0042] Hereinafter, embodiments of the technical solutions of the present application will be described in detail with reference to the drawings. The following embodiments are only used to illustrate the technical solutions of the present application more clearly, and therefore are only examples and should not be used to limit the protection scope of the present application.

[0043] The present invention proposes a non-dynamic collision detection method, which has a relatively low overall implementation difficulty. It performs collision detection based on the current change rate combined with torque limit and corrected position difference. After receiving the collision detection signal, the robot will respond to the collision. The response is divided into two modes: reflection torque response and immediate shutdown. The robot generates different response actions according to different situations. When an instantaneous collision impact force is detected, the robot will enter the current control mode and issue a shutdown alarm after reaching a certain speed. When a slowly applied clamping force is detected, it will immediately shut down as long as it exceeds the threshold set by the given maximum torque limit. This response scheme can reduce the clamping force on the motor, reducer, etc. of the robot when it encounters an instantaneous impact collision force, thereby avoiding damage to key components.

[0044] According to one or more embodiments of the present invention, when a robot collides, the current will suddenly change, the position setting and the position feedback will have a difference, and the torque of the robot will also increase. Therefore, this paper proposes to perform collision detection on the robot by taking the derivative of the joint current of the robot and combining the difference between the position setting and the feedback and the maximum limit of the torque during operation, and to perform periodic correction when the position feedback and the position setting are different, so as to reduce the misjudgment caused by the large difference caused by the position followability during acceleration and deceleration. Since the position deviation and the sudden change of the current are detected, the robot can detect the collision during zero position control and operation. The force slowly applied to the robot is detected by the maximum torque limit. When the slowly applied force exceeds the maximum torque during normal operation of the current working condition, the robot performs a shutdown alarm operation. And after the collision, the collision situation is judged, and then different response measures are proposed according to different collision situations. When an instantaneous impact collision occurs at high speed, the control will generate a reverse current to make the colliding joint produce a reverse or forward speed (depending on the same direction collision or opposite collision). When the speed controlled by the given current is reached, the shutdown alarm operation is performed. At low speed or when the slowly applied clamping force is detected, the robot directly performs a shutdown alarm operation.

[0045] Figure 1 A schematic diagram showing two collision forces of a robot according to an exemplary embodiment of the present invention is shown.

[0046] like Figure 1 As shown, the collision force on the robot motor can be roughly divided into two categories. One is the collision impact force applied suddenly. For this type of collision force, the current mutation and position deviation can be used to detect it. The other is the clamping force applied slowly to the motor. For this type of force, this article uses the maximum torque limit during operation for detection.

[0047] Figure 2 FIG. 4 shows an overall flow chart of collision detection according to an exemplary embodiment of the present invention.

[0048] likeFigure 2 As shown, the alarm prerequisite for the left - hand process is that the position deviation reaches the threshold value and the current change rate reaches the threshold value; when judging the shutdown response, it will be judged whether it is a high - speed head - on impact. If so, it will enter the current control mode to make the robot reach the reverse speed threshold, and then the robot will stop and output an alarm number; if the alarm prerequisite has occurred and it is not a high - speed head - on impact, the robot will immediately stop and output an alarm number. The right - hand process is parallel. When the maximum limit torque threshold is exceeded, the robot will immediately stop and output an alarm number. The left - hand and right - hand processes run simultaneously. The left - hand path detects the instantaneous impact collision force, and the right - hand path detects the slowly applied pressing force.

[0049] As Figure 2 shown, during the robot collision detection process, the joint current and position deviation during the robot's operation are detected in real - time. When the change rate of the joint current is greater than the preset threshold value and the position deviation is greater than the preset threshold value, it is determined that the robot has a collision with a suddenly applied force; and the joint torque during the robot's operation is detected in real - time. When the joint torque exceeds the maximum limit torque threshold value of the robot, it is determined that the robot has a collision with a slowly applied force; where the position deviation is the deviation between the position given and the position feedback collected during the robot's operation.

[0050] As Figure 2 shown: During the process of determining that the robot has a collision with a suddenly applied force, S21: Collect the joint current of each axis during the robot's operation, and take the derivative and filter each joint current to obtain the change rate of the joint current;

[0051] S22: When the position deviation is greater than the preset position deviation threshold value, correct the position deviation to obtain the corrected position deviation threshold value;

[0052] S23: When the corrected position deviation threshold value is greater than the preset position deviation threshold value and the change rate of the joint current is greater than the preset threshold value, it is determined that the robot has a collision with a suddenly applied force;

[0053] S24: Record the magnitude and selected direction of the joint current when a collision with a suddenly applied force occurs, and judge whether it is a in - phase or head - on collision;

[0054] S25: If it is a high - speed instantaneous head - on collision of the robot, the robot enters the current control mode. When the robot reaches the predetermined reverse speed, the robot stops and outputs an alarm signal;

[0055] S26: If it is not a high - speed instantaneous head - on collision of the robot, the robot stops and outputs an alarm signal.

[0056] According to one or more embodiments of the present invention, the present invention is applied to the three-loop control of a motor. The three loops include a position loop, a speed loop, and a current loop. The position loop can control the robot to reach the target position, the speed loop can make the motor reach the specified target speed, and the current loop can control the motor output to the target torque. The current control mode is to control the current loop, input a certain current command to the motor, and control the output magnitude of the motor. Entering the current control mode is a method for the robot to respond to a collision after a collision. The purpose is to control the motor to generate a torque in the same direction as the direction of the collision force when the robot collides, so that the motor output generates compliance, thereby reducing the damage to the motor or the reducer during the collision. Wherein the current control mode is to control the current loop of the robot to control the output magnitude of the motor.

[0057] As Figure 2 shown, the process of determining that the robot has a collision with a slowly applied force on the right includes:

[0058] S31: Set a counter, and the counter is incremented by 1 after each operation cycle of the robot is completed;

[0059] S32: Accumulate the maximum torque values during each specified action process of the robot to obtain the accumulated maximum torque value;

[0060] S33: When the value of the counter reaches the preset number of operation cycles N;

[0061] S34: Calculate the maximum average torque of the robot during N operation cycles. The average maximum torque = accumulated maximum torque value / N;

[0062] S35: Multiply the maximum average torque by a magnification factor to obtain the maximum torque limit threshold of the robot;

[0063] S36: At the (N + 1)th operation cycle, when the joint torque of the robot exceeds the maximum limit torque threshold of the robot, it is determined that the robot has collided and an alarm is given.

[0064] Figure 3 Shows a schematic diagram of current detection collision according to an exemplary embodiment of the present invention.

[0065] As Figure 3 shown, in the above S21, the detection of the collision is obtained through the joint current (i.e., the quadrature axis current of the robot motor shaft). That is, in the control strategy of id = 0, the quadrature axis current and torque are proportional, so when the external torque changes suddenly, the quadrature axis current will also change suddenly. Therefore, the change rate of the quadrature axis current can be used as one of the means to judge the collision detection. Collect the quadrature axis currents of each axis, take the derivative, then perform filtering, and finally set a threshold. From Figure 3From the detection results, it can be seen that as long as a reasonable threshold is set for the change rate of the joint current after filtering, the moment of collision can be detected. The specific implementation process of joint current detection is as follows: (1) Collect the Iq feedback current; (2) Set the filter for filtering; (3) Differentiate the filtered Iq and then filter it; (4) Set the collision judgment threshold; (5) Judge the collision according to the current derivative.

[0066] Figure 4 Fig. shows a flowchart of position deviation compensation according to an exemplary embodiment of the present invention. Figure 5 is a waveform of position command and position feedback according to an exemplary embodiment of the present invention. Figure 6 Fig. shows a waveform diagram of position command and position feedback collected during the operation of the robot according to an exemplary embodiment of the present invention.

[0067] As Figure 4 、 Figure 5 and Figure 6 shown, there is an obvious following error during the acceleration and deceleration processes. The position feedback cannot keep up with the position command. If the difference between the position command and the feedback is calculated in real time, a large difference will be generated (such as the position deviation waveform in Figure 5 ). Therefore, in the above step S22, it is necessary to make certain corrections to the position command and the feedback. Measure the following delay time tdelay of the position command and the feedback during the acceleration and deceleration processes, and then divide tdelay by the control execution cycle Ts to obtain the compensation coefficient K. The specific compensation process is shown in the Figure 4 flowchart. The detection of position deviation can reduce the misjudgment caused by the sudden change of current when the robot starts and stops in the current derivative detection method.

[0068] According to one or more embodiments of the present invention, one of the prerequisites for collision detection in the present invention is position deviation. Then, there is a following delay between the position command and the position feedback during actual operation. Therefore, there is a certain position deviation when the robot does not collide. The purpose of this is to reduce the influence of the position deviation generated by the robot itself during collision detection on the setting of the position deviation threshold. Where K = tdelay / Ts; K is essentially the offset period between the position command and the position feedback. By calculating the offset period and compensating it into the algorithm to correct the position deviation, the ultimate ideal effect is that the waveforms of the position command and the corrected position feedback coincide during normal operation, so that the position deviation is near 0 during normal operation. The specific waveform diagram is as shown in Figure 6 .

[0069] Figure 7 Fig. shows a flowchart of maximum torque limit according to an exemplary embodiment of the present invention.

[0070] During Figure 7Among them, Run_num: the number of times the robot has completed the upper-level operation instructions; Tr_total: the sum of the maximum torques for each time during the robot's operation; N: the number of times to start calculating the maximum average torque; Tr_MAX: the maximum average torque value; Kf: the amplification factor of the maximum average torque value (greater than 1, generally not exceeding 1.5 during actual operation). During actual operation, the robot often executes a fixed set of actions. Therefore, when the robot is running, the magnitude of the joint torque of the robot can be collected in real time, the maximum torque value within each action cycle is recorded, the average value is obtained by recording N times, and then multiplied by the torque limit coefficient to obtain the torque limit threshold. This function becomes effective during the (N + 1)-th run. When the joint torque of the robot during operation exceeds the threshold, an alarm will be generated. When the robot manually accelerates and decelerates, enables up and down, pauses and stops, the threshold will be recalculated. Figure 7 The specific execution process is as shown in steps S31 - S36.

[0071] According to one or more embodiments of the present invention, the robot has two responses to collisions. One is the zero-torque response mode. In the zero-torque response mode, immediately after detecting a collision, the robot servo system switches to the torque control mode, and uses the motor torque to balance the self-gravity of the robot link to make the robot in a force balance state. Therefore, when a collision is detected, the robotic arm will move away from the collision position with an acceleration opposite to the external collision force, effectively ensuring safety during human-robot interaction. The other is the reflected torque response mode. In the reflected torque response mode, also immediately after detecting a collision, the robot servo system switches to the torque control mode, and while balancing the self-gravity of the robot link, makes the motor torque overreact to the external collision force. Therefore, when a collision is detected, the robotic arm will move away from the collision position with a greater acceleration. Different from the passive zero-torque response mode, the reflected torque response mode is an active control method, which can effectively ensure the safety of the SCARA robot for human-robot interaction during the insertion process.

[0072] Figure 8 The flowchart showing the robot collision response process according to an exemplary embodiment of the present invention is shown. Figure 9 The flowchart of the reflected torque response after collision according to an exemplary embodiment of the present invention is shown.

[0073] As Figure 8 and Figure 9 shown, when the robot detects an instantaneous impact force during collision detection, it will enter the reflected torque response mode (i.e., Figure 1On the left side), the current magnitude of the current control mode of the robot after a collision is the magnitude of the joint torque at the time of the collision. The rotation direction of the robot motor is judged by the speed, and whether it is a head-on collision or a same-direction collision is judged by the position deviation and the torque. When a high-speed head-on collision is detected, the corresponding joint will reverse the torque to the torque-giving end of the torque control, and then when the motor rotates forward or backward to reach a certain speed value, the robot will lose enable and alarm. When a high-speed same-direction collision is detected, the corresponding joint will lose enable and alarm. In this way, the pressing force brought by the collision to the motor or the reducer is reduced. When a slowly applied collision force is detected through the torque limit function or a collision force is detected at a low speed, the robot will immediately stop and alarm. The threshold for judging high and low speeds can be given manually at the upper level. In addition, when a collision is detected, if the position given mutation is not less than the set threshold, then after a delay period, a re-collision detection is performed; when the position given mutation is less than the set threshold, information such as the current at the time of the collision, the collision direction speed, etc. is recorded, and it is judged whether it is a high-speed collision. If it is a high-speed collision and the rotation direction is determined, if the rotation direction is not a head-on collision, then an alarm and stop; if it is a head-on collision, then the collision current is reversed, and when the reverse speed is reached, an alarm and stop.

[0074] Figure 10 FIG. shows a block diagram of a collision detection device for a robot according to an exemplary embodiment of the present invention.

[0075] As Figure 10 shown, the robot collision detection device includes: a collision detection device for suddenly applying a force, which detects the joint current and position deviation in real time when the robot is working. When the change rate of the joint current is greater than a preset threshold and the position deviation is greater than a preset threshold, it is determined that the robot has a collision with a suddenly applied force; a collision detection device for slowly applying a force, which detects the joint torque in real time when the robot is working. When the joint torque exceeds the maximum limit torque threshold of the robot, it is determined that the robot has a collision with a slowly applied force; wherein the position deviation is the deviation between the position given and the position feedback collected during the operation of the robot.

[0076] The specific implementation functions of the collision detection device for suddenly applying a force and the collision detection device for slowly applying a force can refer to the description of the foregoing method flow, and will not be elaborated here one by one.

[0077] The present invention proposes a method for robot collision detection without a sensor and without a dynamic model. This method performs collision detection by detecting the change rate of the robot joint current, the corrected position deviation value, and the maximum torque during motor operation. And by classifying the collision force, the robot is controlled to perform different collision responses. When a high-speed and instantaneous impact collision force occurs, the robot first performs current control to generate a reverse current, and then stops after reaching a certain speed. When the speed is low or a slowly applied pressing force is detected, the robot directly performs a stop operation. This response mode will reduce the excessive pressing force caused by the collision force on the motor or the reducer. The collision detection and response scheme of the present invention is more reliable and accurate than the general single detection of the change rate of current or the deviation of speed and position, and is easier to implement than the method of detecting the external torque by a dynamic method, and is more cost-saving than the detection method using a sensor.

[0078] As for the accompanying drawings related to the above-mentioned example of the present invention and the detailed description of the present invention, they are used to explain the present invention, but do not limit the meaning or scope of the present invention described in the claims. Therefore, those skilled in the art can easily implement modifications from the above description. In addition, those skilled in the art can delete some of the components described herein without degrading the performance, or can add other components to improve the performance. In addition, those skilled in the art can change the order of the steps of the method described herein according to the environment of the process or equipment. Therefore, the scope of the present invention should not be determined by the embodiments described above, but by the claims and their equivalent forms.

[0079] Although the present invention has been described in connection with presently considered to be achievable embodiments, it should be understood that the present invention is not limited to the disclosed embodiments, but on the contrary, is intended to cover various modifications and equivalent configurations included within the spirit and scope of the appended claims.

Claims

1. A collision detection method for a robot, characterized in that, The method includes: Real-time detecting the joint current and position deviation during the operation of the robot. When the change rate of the joint current is greater than a preset threshold and the position deviation is greater than a preset threshold, it is determined that the robot has collided with a suddenly applied force; and Real-time detecting the joint torque during the operation of the robot. When the joint torque exceeds the maximum limit torque threshold of the robot, it is determined that the robot has collided with a slowly applied force; Wherein the position deviation is the deviation between the position given and the position feedback collected during the operation of the robot; During the process of determining that the robot has collided with a suddenly applied force, S21: Collect the joint current of each axis during the operation of the robot, and take the derivative and filter each joint current to obtain the change rate of the joint current; S22: When the position deviation is greater than a preset position deviation threshold, correct the position deviation to obtain a corrected position deviation threshold; S23: When the corrected position deviation threshold is greater than the preset position deviation threshold and the change rate of the joint current is greater than the preset threshold, it is determined that the robot has collided with a suddenly applied force; After the S23, it further includes: S24: Record the magnitude and selected direction of the joint current when a collision with a suddenly applied force occurs, and determine whether it is a head-on or in-phase collision; S25: If it is a high-speed instantaneous head-on collision of the robot, the robot enters the current control mode. When the robot reaches a predetermined reverse speed, the robot stops and outputs an alarm signal; S26: If it is not a high-speed instantaneous head-on collision of the robot, the robot stops and outputs an alarm signal; Wherein the current control mode is to control the current loop of the robot to control the output force of the motor.

2. The method according to claim 1, characterized in that, Wherein, After the S23 determines that the robot has collided with a suddenly applied force, the robot enters the reflected torque response mode. In the reflected torque response mode, after detecting the collision, the robot servo system immediately switches to the torque control mode, and while balancing the self-gravity of the robot link, the motor torque overreacts to the external collision force. When a collision is detected, the robot's robotic arm will move away from the collision position with a greater acceleration.

3. The method according to claim 1, characterized in that, In the S22, the correction of the position deviation includes: Measuring the following delay of the position given and the position feedback during the acceleration and deceleration process of the robot, dividing the following delay by the control execution cycle of the robot to obtain a compensation coefficient, and correcting the position deviation through this compensation coefficient.

4. The method according to claim 1, characterized in that, Wherein, The threshold of the maximum limit torque is obtained by multiplying the average value of the maximum torque of the robot operation cycle by an amplification factor.

5. The method according to claim 1, wherein, The process of determining that the robot has collided with a slowly applied force includes: S31: Set a counter, and the counter is incremented by 1 every time the robot completes one operation cycle; S32: Accumulate the maximum torque value during each specified action process of the robot to obtain an accumulated maximum torque value; S33: When the value of the counter reaches a preset number of operation cycles N; S34: Calculate the maximum average torque of the robot during N operation cycles, and the maximum average torque = accumulated maximum torque value / N; S35: Multiply the maximum average torque by the amplification factor to obtain the torque maximum limit threshold of the robot; S36: At the (N + 1)-th operation cycle, when the joint torque of the robot exceeds the maximum limit torque threshold of the robot, determine that the robot has collided and issue an alarm.

6. The method according to claim 1, characterized in that, When the robot is manually accelerated or decelerated, enabled up and down, or paused and stopped, recalculate the threshold of the maximum limit torque.

7. A collision detection device for a robot, characterized in that, The device includes: A collision detection device for sudden force application, which real-time detects the joint current and position deviation during the operation of the robot. When the change rate of the joint current is greater than the preset threshold and the position deviation is greater than the preset threshold, determine that the robot has collided with a suddenly applied force; A collision detection device for slow force application, which real-time detects the joint torque during the operation of the robot. When the joint torque exceeds the maximum limit torque threshold of the robot, determine that the robot has collided with a slowly applied force; Wherein the position deviation is the deviation between the position given and the position feedback collected during the operation of the robot; Wherein the collision detection device for sudden force application is used for: collecting the joint current of each axis during the operation of the robot, differentiating and filtering each joint current to obtain the change rate of the joint current; when the position deviation is greater than the preset position deviation threshold, correcting the position deviation to obtain a corrected position deviation threshold; and when the corrected position deviation threshold is greater than the preset position deviation threshold and the change rate of the joint current is greater than the preset threshold, determine that the robot has collided with a suddenly applied force; The collision detection device for sudden force application is further used for: recording the magnitude and selected direction of the joint current when a collision with a suddenly applied force occurs, and determining whether it is a head-on or in-line collision; if it is a high-speed instantaneous head-on collision of the robot, the robot enters the current control mode. When the robot reaches the predetermined reverse speed, the robot stops and outputs an alarm signal; if it is not a high-speed instantaneous head-on collision of the robot, the robot stops and outputs an alarm signal; wherein the current control mode is to control the current loop of the robot to control the output force of the motor.

8. A robot, characterized in that,It uses the robot collision detection method according to any one of claims 1-6, or includes the robot collision detection device according to claim 7.

Images