Collision detection method and collision detection system

Abstract

To provide a collision detection method and a system, capable of earlier detecting a "unexpected collision" as compared to a conventional one by actually removing an influence due to a noise and an acceleration without using an electric filter and a timer.SOLUTION: A first parameter Q having a correlation relation with a torque of a motor in a predetermined period just after the generation of a collision and a second parameter X having a correlation relation with the torque that is different from the first parameter are compared with a threshold value Th0 to which an arithmetic value P obtained by inputting a predetermined multiplication formula of P=Q*X is predetermined. When the arithmetic value P exceeds the threshold value Th0, it is determined that an unexpected collision has occurred.SELECTED DRAWING: Figure 5

Description

[Technical field]

[0001] The present invention relates to a collision detection method and system for detecting an unexpected collision of a moving part with a collision target in a mechanical device having a moving part supported on a support part whose position is changed using a motor as a drive source. [Background technology]

[0002] When the arm of an industrial robot unintentionally collides with an obstacle such as a nearby piece of equipment (accidental collision), it is possible that the surrounding equipment and the robot may be damaged. Therefore, technology has been proposed to detect collisions between a robot and an obstacle at an early stage and automatically stop the robot when a collision occurs.

[0003] For example, Japanese Patent Laid-Open Publication No. 1-230107 (Patent Document 1) discloses a technology in which a torque command value output to a servo motor is used as a parameter indicating the operating state of a robot, this torque command value is detected at a predetermined cycle, and when the difference between the torque command value detected in the previous cycle and the torque command value detected in the current cycle becomes equal to or greater than a predetermined value, it is determined that a collision has occurred with a driven object, and the rotation of the servo motor is stopped. [Prior art documents] [Patent documents]

[0004] [Patent Document 1] Japanese Patent Application Publication No. 1-230107 Summary of the Invention [Problem to be solved by the invention]

[0005] However, sensors and servo amplifiers that detect parameters for calculating changes in the robot's operating state generate electrical noise, causing large changes in the operating state quantity. In addition, the operating state quantity may also change large due to mechanical characteristics (mechanical noise characteristics) generated by gear engagement, or sudden acceleration / deceleration of the robot (initial inching during manual operation, etc.). Therefore, in conventional technology, if an attempt is made to speed up collision detection by setting a low predetermined value (threshold value) for the operating state quantity, the above-mentioned changes in the operating state are erroneously detected as "unexpected collisions," causing the robot to come to an emergency stop. Depending on the type of robot, even a minor collision may cause performance degradation or an accident, so that whenever the robot comes to an emergency stop, measures such as inspection must be taken, which may disrupt production plans.

[0006] In order to avoid this, it is conceivable to take noise into consideration and set the predetermined value (threshold value) high, but this would result in a delay in detecting an unexpected collision.

[0007] In addition, it is possible to use an electrical filter or timer to remove the effects of noise and sudden acceleration and deceleration, but using these would cause a time delay, which would also cause a delay in the detection of an unexpected collision.

[0008] An object of the present invention is to provide a collision detection method and system that can substantially eliminate the effects of noise and acceleration / deceleration without using an electric filter or timer, and that can detect an "unexpected collision" earlier than conventional methods. [Means for solving the problem]

[0009] The present invention is a collision detection method for detecting an unexpected collision of a movable part with a collision target in a mechanical device having a movable part supported on a support part that changes its position using a motor as a drive source. In the collision detection method of the present invention, within a predetermined period immediately after the occurrence of a collision, a first parameter having a correlation with the torque of a motor and a second parameter different from the first parameter but having a correlation with the torque are input into a predetermined multiplication formula, and the calculated value is compared with a predetermined threshold value, and if the calculated value exceeds the threshold value, it is determined that an unexpected collision has occurred.

[0010] The first parameter and the second parameter are parameters that have a correlation with torque within a predetermined period immediately after the occurrence of a collision. Here, the "predetermined period immediately after" refers to a period during which the first parameter and the second parameter have a correlation with torque. Some parameters lose their correlation with torque after the predetermined period immediately after the occurrence of a collision, while others continue to have a correlation with torque.

[0011] The first and second parameters in a predetermined period immediately after the occurrence of a collision have similar tendencies of increasing and decreasing the values ​​of the parameters over time. In contrast, the noise components contained in the first and second parameters not only have no correlation with the torque before or after the collision, but also have no similarity in the tendencies of increasing and decreasing the values ​​over time. Therefore, the calculated value obtained by inputting the first and second parameters in a period immediately after the occurrence of a collision (a predetermined period immediately after the collision), which have a correlation with the torque after the collision and similar tendencies of increasing and decreasing the changes, into a predetermined multiplication formula, is a value that is significantly increased by multiplication. In contrast, noise that has no correlation with the torque, no correlation with each other, and no similarity in the tendencies of increasing and decreasing the changes, does not significantly increase the calculated value even when multiplied. Therefore, according to the present invention, it is possible to prevent erroneous detection due to the presence of noise, compared to the conventional technology in which one parameter is compared with a threshold value. Furthermore, by comparing the large calculated value obtained by inputting the first and second parameters into a multiplication formula with a predetermined threshold value, it is possible to distinguish this from collisions within the predicted range (collisions that naturally occur during the operating process) and quickly determine that an unexpected collision has occurred.

[0012] For the first and second parameters whose values ​​are 1 or less, the calculated value is calculated by reciprocally inputting the parameter value into the multiplication formula. This is because multiplying a value less than 1 results in a smaller value, not a larger one.

[0013] The first parameter is a parameter related to the torque fed back to the motor drive unit, and the second parameter is the position deviation between the position command value given to the motor drive unit and the detected position obtained by detecting the position of the movable part. The tendency of change in these two parameters in a predetermined period immediately after the collision is roughly similar, so the calculated value is larger than that of a single parameter. It goes without saying that the parameters also include the torque itself.

[0014] The method of the present invention can be applied to various types of machinery, but the machinery may be a molded product removal machine, the movable part may be a removal head of the molded product removal machine, and the support part may be a support arm that supports the removal head. In this case, the threshold value may be set to a value greater than a calculated value obtained by a collision (a collision within a predicted range) that is allowed in a teaching operation that sets an operating process for removing a molded product by the removal head in the molded product removal machine. In this way, a necessary collision (a collision within a predicted range) that occurs in the teaching operation is not detected as an unexpected collision that may damage the removal head or mold.

[0015] When the calculation value is P, the first parameter is Q, and the second parameter is X, the multiplication formula can be P=Q×X. This multiplication formula is the simplest and easy to calculate.

[0016] A collision detection system for carrying out the method of the present invention includes a first parameter acquisition unit, a second parameter acquisition unit, a calculation unit, and a judgment unit for detecting a collision of a movable part with a collision target in a mechanical device having a movable part supported on a support part that changes its position using a motor as a drive source. The first parameter acquisition unit acquires a first parameter that is correlated with the torque of the motor within a predetermined period immediately after the occurrence of the collision. The second parameter acquisition unit acquires a second parameter that is different from the first parameter but correlated with the torque within a predetermined period immediately after the occurrence of the collision. The calculation unit calculates a calculated value obtained by inputting the first and second parameters into a predetermined multiplication formula. The judgment unit compares the calculated value with a predetermined threshold value, and judges that an unexpected collision has occurred when the calculated value exceeds the threshold value. [Brief description of the drawings]

[0017] [Figure 1] 1 is a block diagram showing a schematic configuration of a molded product removal machine as a mechanical device in which a collision detection system implementing a collision detection method of the present invention is applied to collision detection of a removal head. [Diagram 2]FIG. 13 is a diagram showing an example of a feedback loop in a feedback control unit for one motor that drives upper and lower frames. [Diagram 3] FIG. 13 is a diagram showing an example of the state of change over time of a torque command value (Q) as a first parameter, a motor detection speed V, and a position deviation (X) as a second parameter in a molded product removal machine when an unexpected collision occurs after starting a teaching operation. [Figure 4] FIG. 4 is a correlation diagram for observing the correlation in the change in the waveform of the torque command value and the position deviation X shown in FIG. 3. [Diagram 5] FIG. 1 is a diagram showing P=Q×X and the change in a first parameter Q over time. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0018] An embodiment of the present invention will now be described in detail with reference to the drawings.

[0019] [Collision detection system configuration] 1 is a block diagram showing a schematic configuration of a molded product removal machine 1 as a mechanical device in which a collision detection system implementing the collision detection method of the present invention is applied to collision detection of a removal head 3. In the molded product removal machine, an unpredicted "unexpected collision" often occurs during a teaching operation for setting an operation process for removing a molded product by the removal head 3 attached to the upper and lower frames 5. An "unexpected collision" refers to a collision that occurs when an erroneous operation during a teaching operation causes the removal head 3 to come into contact with a mold or an object around the mold, and does not include slight collisions or pressures that naturally occur during removal operation.

[0020] In this embodiment, a plurality of servo motors 9 with encoders 10 are used as drive sources for the conveying mechanism 7. A control device 11 that controls the plurality of servo motors 9 includes a feedback control section 13 for each of the servo motors 9, which determines the deviation between the position information and speed information detected by the encoder 10 and a position command and a speed command, and performs feedback control of the position and speed of the drive shaft of the servo motor 9 as a drive section based on the deviation. The feedback control section 13 functions as a so-called servo amplifier.

[0021] The control device 11 includes a manual command generating unit 14 that generates a manual command when the feedback control unit 13 of the servo system is manually operated by an input unit 24, a teaching data acquiring unit 15 that stores teaching data obtained by a command output from the manual command generating unit 14 during a teaching operation, and a collision detection system 16. The molded product extractor of this embodiment performs a teaching operation by manually operating the servo motor 9 through the input unit 24 to give an operation command, and records the teaching data in the teaching data acquiring unit 15. The input unit 24 is also operated to generate an evacuation operation command when an evacuation operation is performed after a collision occurs. In this embodiment, the device includes a signal generating means 25 that generates an alarm signal when the collision detection system 16 detects a collision.

[0022] The collision detection system 16 includes a first parameter acquisition unit 17, a second parameter acquisition unit 19, a calculation unit 21, and a determination unit 23 in order to detect a collision between the take-out head 3 (movable part) and a collision target (e.g., a mold, etc.). The collision detection system 16 is provided in correspondence with a servo motor for driving the upper and lower frames 5 to which the take-out head 3 is attached. The components constituting the collision detection system 16 will be described later.

[0023] FIG. 2 shows an example of a feedback loop in the feedback control unit 13 for one motor that drives the upper and lower frames 5. This feedback loop executes a double-loop process of a position control feedback loop and a speed control feedback loop. In this double-loop process, the deviation between the motor position command and the motor detection position output from the encoder 10 detected by the encoder unit 14 is calculated as a position deviation, and the position control unit 13A generates a speed command based on this position deviation. Furthermore, the deviation between the motor detection speed output from the output speed detection unit 13E and the motor detection speed based on this speed command and the motor detection position from the encoder unit 10 is calculated as a speed deviation, and the speed control unit 13B generates a motor torque command based on this speed deviation. The motor detection speed may be calculated by first-order time differentiation of the motor detection position by a differential operator in the output speed detection unit 13E. Then, the PWM control unit 13D drives the motor 9 by supplying driving power based on the motor torque command via the current control unit 13C.

[0024] The first parameter acquisition unit 17 configured in the collision detection system 16 acquires a first parameter having a correlation with the torque of the motor within a predetermined period immediately after the occurrence of a collision. In this embodiment, the torque command output from the speed control unit 13B in the feedback loop in FIG. 2 is generated including feedback information of the fed-back motor detection speed. Therefore, the first parameter acquisition unit 17 acquires this torque command as the first parameter having a correlation with the torque of the motor within a predetermined period immediately after the occurrence of a collision. It is needless to say that the first parameter acquisition unit 17 may constantly acquire the torque command as the first parameter regardless of the occurrence of the correlation.

[0025] Also, the second parameter acquisition unit 19 configured in the collision detection system 16 acquires a second parameter that is different from the first parameter but has a correlation with torque within a predetermined period immediately after the occurrence of a collision. In the second parameter acquisition unit 19 of this embodiment, the deviation between the position command value given to the position control unit 13A, which is the driving unit of the motor, and the motor detection position (corresponding to the detection position of the take-out head 3) that is fed back is acquired as the second parameter. Note that this deviation has a correlation with torque within a predetermined period immediately after the occurrence of a collision, but after this period has elapsed, it no longer has a correlation with torque. Note that it is needless to say that the second parameter acquisition unit 19 may constantly acquire the deviation as the second parameter regardless of the occurrence of a correlation.

[0026] Furthermore, the calculation unit 21 inputs the first parameter and the second parameter into a predetermined multiplication formula to calculate a calculation value. The predetermined multiplication formula is an equation in which the first parameter and the second parameter are included. The determination unit 23 compares the calculation value P calculated by the calculation unit 21 with a predetermined threshold value, and determines that an "unexpected collision" has occurred if the calculation value exceeds the threshold value.

[0027] Here, in the calculation unit 21, as a typical example, when the calculation value is P, the first parameter is Q, and the second parameter is X, the calculation value can be obtained using a multiplication formula P=Q×X. This multiplication formula is the simplest and easy to calculate.

[0028] [Method of detecting unexpected collisions] 3 is a diagram showing an example of the state of change over time of the torque command value (Q) as the first parameter, the motor detection speed V, and the position deviation (X) as the second parameter in the case where an unexpected collision occurs after the start of the teaching operation in the molded product removal machine. The torque command value (Q) is the "torque command" in the feedback loop shown in FIG.

[0029] In Fig. 3, the "steady state" is a state in which the take-out head of the molded product take-out machine moves without colliding with anything. The "pressed state" is a state in which the take-out head continues to be pressed against an object after colliding with it. In Fig. 3, the period indicated by tp is a period during which the first and second parameters that are correlated with the motor torque can be obtained immediately after the collision occurs, that is, the aforementioned "predetermined period immediately after the collision occurs." In Fig. 3, the position deviation (X), which is the second parameter, does not have a correlation with the torque after the period tp has elapsed, but the reason for this is unclear.

[0030] In the waveform of Fig. 3, noise n appears in the torque command value (Q). This noise n occurs accidentally depending on the design specifications of the equipment and the surrounding environment, so it is difficult to predict. Therefore, if a collision is detected only by comparing the torque with a threshold value, as in the conventional technology, the torque waveform is acquired at the design stage and the threshold value for collision detection is set. The threshold value is often set to several times (e.g., twice) the peak value Tp of the torque command waveform before the pressing occurs, taking into account the noise n that occurs accidentally, so as not to erroneously detect the noise n as a collision. However, if set in this way, the sensitivity of the collision detection is lowered, and the detection of unexpected collisions, including over-pressing, is delayed. As a result, there is a concern that damage to the arm of the molding remover may increase.

[0031] It is also possible to detect a collision using the position deviation (X). However, when the position deviation at the start of acceleration is large as shown in Fig. 3, the threshold value must be set to several times the initial position deviation Dp to avoid false detection, which may result in delayed detection of an unexpected collision compared to detection using the torque command value and threshold value.

[0032] The lower diagram in Fig. 4 is a correlation diagram for observing the correlation in the changes in the waveforms of the torque command value and the position deviation X shown in Fig. 3. As shown in the correlation diagram in the lower part of Fig. 4, in a steady state, there is no apparent correlation between the changes in the torque command value (Q) and the deviation (X). However, in the pressing state after the occurrence of a collision, it can be seen that there is a clear positive correlation between the changes in the position deviation X and the torque command value Q.

[0033] The torque command value Q, which is the first parameter, and the position deviation X, which is the second parameter, are both correlated with torque within a predetermined period immediately after the occurrence of a collision, and the increase and decrease trends of the change in the values ​​of these parameters over time are similar. Due to the occurrence of this correlation, within a predetermined period immediately after the occurrence of a push, the product P = Q x X of these first parameter Q and second parameter X increases significantly, and the waveform of the product P after the occurrence of a collision becomes a waveform that changes suddenly in a short time.

[0034] Fig. 5 shows the change over time of the product P = Q x X and the first parameter Q. As can be seen from Fig. 5, the noise n has no correlation with the torque, and there is no similarity in the increase and decrease trends of the changes, so even if the first parameter Q and the second parameter X are multiplied, the noise value during the calculation does not become significantly larger. Therefore, according to this embodiment, it is possible to prevent false detections due to the presence of noise, compared to the conventional technology that compares one parameter with a threshold value.

[0035] In this embodiment, the threshold value Th0 for comparing the product P of the torque command value Q, which is the first parameter, and the position deviation X, which is the second parameter, is set to a predetermined value greater than a calculated value obtained by a collision permitted in a teaching operation for setting an operation process for removing a molded product by the removal head 3 in the molded product removal machine. As described above, an "unexpected collision" refers to a collision that occurs when the removal head hits a mold or an object around the mold due to an erroneous operation during a teaching operation, etc. In FIG. 5, the product P has increased beyond the threshold value Th0 at the point (1), so it is determined that an unexpected collision has occurred.

[0036] As shown in Fig. 5, when the torque command value Q is compared with the threshold value (Tp) in the conventional method, noise n is erroneously detected, so the conventional method makes the threshold value larger. In other words, if the threshold value is doubled to 2Tp, the torque command value Q exceeds the threshold value at time (2), and the timing of detection of an unexpected collision is delayed compared to the detection time (1) in this embodiment. This shows that, according to this embodiment, it is possible to increase the detection sensitivity and also to widen the setting range of the threshold value.

[0037] As described above, this embodiment utilizes the fact that the product of the torque command value Q and the position deviation X, which is the first parameter, varies in a manner different from that in a steady state and in a normal pressing state starting from a collision. This concept can be applied not only to the torque command value and the position deviation, but also to the case where two parameters having a correlation with the torque after the occurrence of a collision are multiplied by a multiplication formula.

[0038] The method of the present invention can be applied to machinery other than the molded product remover of the above embodiment. The "predetermined threshold" used in this case is not a collision, but a value that exceeds the calculated value P obtained due to noise or acceleration / deceleration of the machinery. This differs for each machinery, so the "predetermined threshold" can be determined for each machinery. [Industrial Applicability]

[0039] According to the present invention, it is possible to determine early on that an unexpected collision has occurred by comparing a calculated value obtained by inputting first and second parameters correlated with the motor torque into a multiplication formula within a predetermined period immediately after the occurrence of a collision with a predetermined threshold value. [Explanation of symbols]

[0040] 1 Molded product removal machine 3 Extraction head 5 Upper and lower frames 7. Transport mechanism 9. Servo motor 10 Encoders 11 Control device 13 Feedback control section 14 Manual command generator 15 Teaching data acquisition section 16 Collision Detection System 17 First parameter acquisition section 19 Second parameter acquisition section 21 Arithmetic section 23 Judgment section

Claims

1. 1. A collision detection method for detecting an unexpected collision of a mechanical device having a movable part supported on a support part that changes its position using a motor as a drive source, the method comprising: a first parameter having a correlation with the torque of the motor and a second parameter different from the first parameter but having a correlation with the torque, inputting the calculated value into a predetermined multiplication formula within a predetermined period immediately after the occurrence of the collision, and comparing the calculated value with a predetermined threshold value; When the calculated value exceeds the threshold value, it is determined that the unexpected collision has occurred.

2. 2. A collision detection method according to claim 1, wherein the calculated value is determined by inputting the reciprocal of the first parameter and the second parameter, which have a value of 1 or less, into the multiplication formula.

3. 2. The collision detection method according to claim 1, wherein the first parameter is a parameter related to a torque fed back to a drive unit of the motor, and the second parameter is a position deviation between a position command value given to the drive unit of the motor and a detected position obtained by detecting the position of the movable part.

4. the mechanical device is a molded product remover, the movable part is a remover head of the molded product remover, and the support part is a support arm that supports the remover head; The method for detecting a collision of a movable part according to claim 1, wherein the threshold value is a value greater than the calculated value obtained by a collision that is allowed in a teaching operation that sets an operating process for removing a molded product by the removal head in the molded product removal machine.

5. 5. The collision detection method according to claim 4, wherein the multiplication formula is P=Q.times.X, where P is the calculated value, Q is the first parameter, and X is the second parameter.

6. 1. A collision detection system for detecting a collision between a movable part and a collision target in a mechanical device having a movable part supported on a support part that changes its position using a motor as a drive source, the system comprising: a first parameter acquisition unit that acquires a first parameter having a correlation with a torque of the motor within a predetermined period immediately after the occurrence of the collision; a second parameter acquiring unit that acquires a second parameter that is correlated with the torque and different from the first parameter within a predetermined period immediately after the occurrence of the collision; a calculation unit that inputs the first parameter and the second parameter into a predetermined multiplication formula to obtain a calculation value; a determination unit that compares the calculated value with a predetermined threshold value and determines that an unexpected collision has occurred when the calculated value exceeds the threshold value.

7. the mechanical device is a molded product remover, the movable part is a remover head of the molded product remover, and the support part is a support arm that supports the remover head; 7. The collision detection system for a movable part according to claim 6, wherein the threshold value is a value greater than the calculated value obtained by a collision allowed in a teaching operation that sets an operating process for removing the molded product by the removal head.

Images