Collision detection method and collision detection system

Abstract

Provided are a collision detection method and a collision detection system for being capable of substantially eliminating the effects of noise and sudden acceleration / deceleration of a robot without using an electronic filter or timer, and detecting an "accidental collision" earlier than conventional techniques. A calculated value P is compared with a predetermined threshold value Th0 within a predetermined period of time immediately after the occurrence of the accidental collision, the calculated value P being obtained by inputting a first parameter having a correlation with the torque of the motor and a second parameter having a correlation with the torque of the motor into a predetermined multiplication formula P = Q * X, the second parameter being different from the first parameter. When the calculated value exceeds a threshold value, it will be determined that an unexpected collision has occurred.

Description

Technical Field

[0001] The present invention relates to a collision detection method and a collision detection system for detecting an unexpected collision that occurs when a movable part in a mechanical device has accidentally collided with a collision object. The mechanical device includes a movable part supported by a supporting part, and the position of the supporting part is changed by a motor as a driving source. Background Art

[0002] If the arm of an industrial robot, etc., unintentionally (or accidentally) collides with an obstacle such as a peripheral device, the peripheral device or the robot itself may be damaged. Therefore, conventionally, a technology has been proposed that detects a collision between a robot and an obstacle at an early stage and automatically stops the robot when a collision occurs.

[0003] For example, Japanese Unexamined Patent Application Publication No. 1989-230107 (Patent Document 1) discloses an invention including the following steps: detecting a torque command value given to a servo motor at a predetermined cycle as a parameter representing the operating state of a robot; when the difference between the torque command value detected in the previous cycle and the torque command value detected in the current cycle is equal to or exceeds a predetermined value, determining that the robot has collided with an object; and stopping the rotation of the servo motor due to this determination. Summary of the invention

[0004] Technical issues

[0005] However, since electrical noise is generated in the sensor for detecting the parameters used to calculate the change in the robot's operating state and the servo amplifier of the robot, the operating state quantity changes significantly. In addition, the operating state quantity may change significantly due to mechanical characteristics (mechanical noise characteristics) caused by gear meshing, etc. or sudden acceleration / deceleration of the robot (initial jog during manual operation, etc.). Therefore, if the predetermined value (threshold) for detecting the change in the operating state quantity is set to a lower value to accelerate collision detection, the above-mentioned large change in the operating state quantity will be mistakenly detected as an "accidental collision", and then the robot will be emergency stopped. Depending on the type of robot, even a slight collision may cause performance degradation or an accident. Therefore, in the event of an emergency stop of the robot, measures such as inspections need to be taken every time, and production plans may be disrupted.

[0006] In order to avoid the above-mentioned problem, it is conceivable to set a higher predetermined value (threshold value) in consideration of noise. Therefore, the detection of an unexpected collision may be delayed.

[0007] In addition, it is conceivable to use an electronic filter or a timer to eliminate the influence of noise and sudden acceleration / deceleration of the motor, but the use of an electronic filter or a timer may cause a time delay, which may become a cause of delay in detection of an "unexpected collision".

[0008] An object of the present invention is to provide a collision detection method and a collision detection system for substantially eliminating the influence of noise and the influence of sudden acceleration / deceleration of a motor without using an electronic filter or a timer, and detecting an "unexpected collision" earlier than conventional techniques.

[0009] Solution to the problem

[0010] The present invention relates to a collision detection method for detecting an unexpected collision that occurs when a movable part has unexpectedly collided with a collision object in a mechanical device, the mechanical device including a movable part supported by a support part, the position of which is changed by a motor as a driving source. The collision detection method of the present invention comprises the following steps: comparing a calculated value with a predetermined threshold value within a predetermined time period just after the collision occurs, the calculated value being obtained by inputting a first parameter having a correlation with the torque of the motor and a second parameter having a correlation with the torque of the motor into a predetermined multiplication formula, the second parameter being different from the first parameter; and determining that an unexpected collision has occurred when the calculated value exceeds the threshold value.

[0011] The first parameter and the second parameter are parameters each having a correlation with the torque of the motor within a predetermined time period immediately after the collision occurs. Here, "the predetermined time period immediately after the collision occurs" refers to a time period within a time period in which each of the first parameter and the second parameter has a correlation with the torque of the motor. Some parameters lose their correlation with the torque after the predetermined time period immediately after the collision occurs, while other parameters continue to have a correlation with the torque.

[0012] The first parameter and the second parameter in the predetermined time period just after the collision have similar trends of increasing / decreasing over time in terms of parameter values. On the other hand, the noise component included in the first parameter and the second parameter not only has no correlation with the torque of the motor before or after the collision, but also has no similarity in the trend of increasing or decreasing over time. Therefore, since both the first parameter and the second parameter have correlation with the torque of the motor just after the collision and have similar trends of increasing / decreasing over time in terms of parameter values, the calculated value obtained by inputting the first parameter and the second parameter in the time period just after the accidental collision (the predetermined time period just after the accidental collision) into the predetermined multiplication formula becomes a significantly increased value by multiplication. On the other hand, since the noise in these parameters has no correlation with the torque of the motor and no correlation with each other, and has no similarity in the trend of increasing or decreasing over time, even by multiplication, the calculated noise will not become a significantly increased value. Therefore, according to the present invention, compared with the conventional technology of comparing one parameter with a threshold, it is possible to prevent false detection due to the presence of noise. In addition, by comparing the larger calculated value obtained by inputting the first parameter and the second parameter into the multiplication formula with the predetermined threshold, it is possible to determine earlier that an unexpected collision has occurred by distinguishing collisions within the prediction range (collisions that occur naturally during operation).

[0013] At least one of the first parameter and the second parameter whose value is 1 (one) or less is converted into the reciprocal of the parameter, and the reciprocal of the parameter is input into the multiplication formula to obtain the calculated value. This is because when multiplied by 1 (one) or less, the calculated value does not increase, but decreases.

[0014] The first parameter is a parameter related to torque fed back to the drive unit of the motor, and the second parameter is a parameter related to the positioning deviation between the positioning command value given to the drive unit of the motor and the detected position obtained by detecting the position of the movable part. Since the change trends of the above two parameters within a predetermined time just after the collision are roughly similar, the calculated value is greater than the calculated value of one parameter of the conventional technology. It goes without saying that these parameters include the torque of the motor itself.

[0015] The method of the present invention can be used for various mechanical devices. The mechanical device may be a device for taking out a molded product, the movable part may be a take-out head of the device for taking out a molded product, and the supporting part may be a support arm that supports the take-out head. In the above case, the threshold value may be a value greater than a calculated value obtained by allowing a collision (a collision within a predicted range) in a teaching work for setting an operation step for taking out a molded product by the take-out head. In this way, an allowed collision (a collision within a predicted range) occurring during the teaching work for setting an operation process is not detected as an unexpected collision that may damage the take-out head, the molded product, etc.

[0016] For example, the multiplication formula may be P=Q×X, when the calculated value is P, the first parameter is Q, and the second parameter is X. This multiplication formula is the simplest formula and is easy to calculate.

[0017] The collision detection system implementing the method of the present invention is configured as follows: the collision detection system is used to detect an unexpected collision that occurs when a movable part in a mechanical device has accidentally collided with a collision object, the mechanical device includes a movable part supported by a support part that changes position of a motor as a driving source, and the collision detection system includes a first parameter acquisition part, a second parameter acquisition part, a calculation part and a determination part. The first parameter acquisition part acquires a first parameter that is correlated with the torque of the motor within a predetermined time period just after the collision. The second parameter acquisition part acquires a second parameter that is correlated with the torque of the motor and is different from the first parameter within a predetermined time period just after the collision. The calculation part obtains a calculated value by inputting both the first parameter and the second parameter into a predetermined multiplication formula. The determination part compares the calculated value with a predetermined threshold value, and when the calculated value exceeds the threshold value, it is determined that an unexpected collision has occurred.

[0018] The system of the present invention can be used for various mechanical devices. The mechanical device may be a device for taking out a molded product, the movable part may be a take-out head of the device for taking out a molded product, and the supporting part may be a support arm that supports the take-out head. In the above case, the threshold value may be a value greater than a calculated value obtained by allowing a collision (a collision within a predicted range) in a teaching work for setting an operation step for taking out a molded product by the take-out head. In this way, an allowed collision (a collision within a predicted range) occurring during the teaching work for setting an operation process is not detected as an unexpected collision that may damage the take-out head, the molded product, etc. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 is a block diagram showing a schematic configuration of an apparatus for taking out a molded product as a mechanical device, in which a collision detection system realizing the collision detection method of the present invention has been applied to detect a collision of a taking-out head.

[0020] Figure 2 is a diagram showing an example of a feedback loop within a feedback control portion of a motor driving a vertical frame.

[0021] Figure 3 is an example diagram showing changes over time in a state consisting of a torque value Q as a first parameter, a motor detection speed V, and a position deviation X as a second parameter when an unexpected collision has occurred after starting a teaching work in an apparatus for taking out a molded product.

[0022] Figure 4is shown for observation Figure 3 A correlation diagram showing the correlation of changes in the waveform between the torque command value Q and the position deviation X is shown.

[0023] Figure 5 is a state diagram showing P=Q×X and the first parameter Q changing with time. DETAILED DESCRIPTION

[0024] Hereinafter, examples of embodiments of the present invention will be described in detail with reference to the accompanying drawings.

[0025] [Collision detection system configuration]

[0026] Figure 1 is a block diagram showing a schematic configuration of an apparatus for taking out a molded product 1 as a mechanical device, in which a collision detection system implementing the collision detection method of the present invention has been applied to detect a collision of a taking-out head 3. In the apparatus for taking out a molded product 1, an unpredicted "accidental collision" may often occur in the teaching work of the operation steps set for taking out a molded product by the taking-out head 3 mounted on a vertical frame 5. An "accidental collision" is a collision in which the taking-out head 3 comes into contact with a mold or other peripheral objects due to an incorrect operation, and does not include a slight collision or squeezing that occurs naturally during the taking-out operation.

[0027] In this embodiment, a plurality of servo motors 9 each having an encoder 10 are used as a driving source of the conveying mechanism 7. A control unit 11 that controls the plurality of servo motors 9 has a processor, and includes a feedback control section 13 implemented via the processor, the feedback control section 13 having a feedback loop section for the servo motors 9. Each feedback loop section obtains a deviation between the position information detected by the encoder 10 and a position command, and a deviation between the speed information detected by the encoder 10 and a speed command, and then performs feedback control on both the position and the speed of the drive shaft of the servo motor 9 based on these deviations.

[0028] The control unit 11 includes: a manual command generating part 14, which is implemented via a processor and is used to generate a manual command when manually controlling the feedback control part 13 in the servo system through the input part 24; a teaching data acquisition part 15, which is used to store teaching data obtained by the command output from the manual command generating part 14 in the teaching work; and a collision detection system 16. The device for removing the molded product of this embodiment provides an operation command to the servo motor 9 through the input part 24 to perform the teaching work, and records the teaching data in the teaching data acquisition part 15. Note that the input part 24 is used to generate an evacuation operation command when an evacuation operation is performed after a collision. In addition, this embodiment provides a signal generating device 25, which generates an alarm signal when the collision detection system 16 detects a collision.

[0029] The collision detection system 16 includes a first parameter acquisition part 17 implemented via a processor, a second parameter acquisition part 19 implemented via a processor, a calculation part 21 implemented via a processor, and a determination part 23 implemented via a processor, so as to detect that the removal head 3 (movable part) collides with a collision object (e.g., a mold, etc.). The collision detection system 16 corresponding to the servo motor 9 for driving the vertical frame 5 on which the removal head 3 is installed is provided. Note that the units constituting the collision detection system will be described later.

[0030] Figure 2 An example of a feedback loop within the feedback control section 13 of the motor driving the vertical frame 5 is shown. The feedback loop performs a dual-loop process of a position control feedback loop and a speed control feedback loop. In the dual-loop process, the deviation between the motor positioning command output from the encoder 10 and the detected position of the motor is determined as a position deviation, and the position control section 13A generates a speed command based on the position deviation. In addition, the deviation between the speed command output from the output speed detection section 13E and the detected speed of the motor is determined as a speed deviation. The speed control section 13B generates a motor torque command based on the speed deviation. Note that the detected speed of the motor can be calculated by performing a first-order time differentiation of the detected position of the motor using a differential operator in the output speed detection section 13E. Then, the PWM control section 13D drives the servo motor 9 by providing driving power based on the motor torque command through the current control section 13C.

[0031] The first parameter acquisition section 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 of time immediately after the collision occurs. Figure 2 The speed control unit 13B in the feedback loop shown in FIG. 1 outputs a torque command, which includes feedback information of the detected speed of the motor that has been fed back. Therefore, the first parameter acquisition part 17 acquires the torque command as the first parameter having a correlation with the torque of the motor within a predetermined period of time just after the collision occurs. Note that, it goes without saying that the first parameter acquisition part 17 can always obtain the torque command as the first parameter regardless of whether a correlation is generated.

[0032] The second parameter acquisition part 19 configured in the collision detection system 16 acquires a second parameter that is correlated with the torque of the motor and is different from the first parameter within a predetermined time period just after the collision occurs. The second parameter acquisition part 19 of the present embodiment acquires the deviation between the position command value provided to the position control part 13A as the driving part of the motor and the detected position of the motor that has been fed back (corresponding to the detected position of the removal head) as the second parameter. Note that the deviation is correlated with the torque within a predetermined time period just after the collision occurs, but is no longer correlated with the torque after the predetermined time period. Note that it goes without saying that the second parameter acquisition part 19 can always obtain the deviation as the second parameter regardless of whether a correlation is generated.

[0033] In addition, the calculation part 21 obtains a calculated value by inputting both the first parameter and the second parameter into a predetermined multiplication formula. The predetermined multiplication formula is a formula included in an arithmetic formula having both the first parameter and the second parameter. The determination part 23 compares the calculated value P with a predetermined threshold value, and when the calculated value exceeds the threshold value, determines that an "unexpected collision" has occurred.

[0034] Here, as a typical example, when the calculated value is P, the first parameter is Q, and the second parameter is X, the calculation section 21 can obtain the calculated value by using P=Q×X as a multiplication formula. This multiplication formula is the simplest formula and is easy to calculate.

[0035] [Accidental collision detection method]

[0036] Figure 3 : is an example diagram showing changes over time in a state consisting of a torque command value Q as a first parameter, a detection speed V of a motor, and a position deviation X as a second parameter when a collision has occurred after a teaching operation has been started in an apparatus for taking out a molded product. The torque command value Q is Figure 2 The “torque command” within the feedback loop shown.

[0037] exist Figure 3 In the present invention, the term "stable state" refers to a state in which the removal head of the device for removing the molded product is moving without colliding with any object. In addition, the term "pressed state" refers to a state in which the removal head is squeezed after having just collided with a certain object and continues in this state. Figure 3 In the example, the time period indicated by tp refers to the time period during which the first parameter and the second parameter both related to the torque of the motor just after the collision can be obtained, that is, the above-mentioned "predetermined time period". Figure 3 , the position deviation X as the second parameter has no correlation with the torque after the time period tp has passed, but the reason for the above state is unclear.

[0038] Even in Figure 3 In the waveform in, noise n also appears in the torque command value Q. Since noise n occurs accidentally depending on the design of the equipment or the surrounding environment, it is difficult to predict noise n. Therefore, an attempt is made to adopt a method or system that detects collisions only by comparing torque and a threshold as in conventional technology, and the method or system sets the threshold for collision detection by acquiring the waveform of the torque under the design state. The threshold will mainly be set to several times (for example, twice) the peak value Tp of the torque command waveform before the removal head has been squeezed to avoid misdetecting noise n as a collision taking into account the accidental occurrence of noise n. However, the above setting will result in a decrease in the sensitivity of collision detection, so that the detection of accidental collisions must be delayed, including situations where the removal head is squeezed too much. Therefore, damage to the removal head of the device for removing the molded product may be increased.

[0039] Note that a method of detecting collision based on the position deviation X can also be considered. Figure 3 As shown, in the case where the position deviation is large at the beginning of the acceleration operation, the threshold should be set to several times the position deviation Dp at the beginning to avoid false detection. Therefore, there is a risk of delayed detection of unexpected collisions compared to detecting unexpected collisions based on the torque command value and the threshold.

[0040] Figure 4 The following figure shows the Figure 3 The correlation diagram of the correlation between the waveform change of the torque command value Q and the position deviation X is shown. Figure 4 As shown in the figure below, in the steady state, there is no correlation between the change in the torque command value Q and the change in the deviation X. However, after the removal head collision, in the state where the removal head is squeezed, there is obviously a positive correlation between the change in the position deviation X and the change in the torque command value Q.

[0041] The torque command value Q as the first parameter and the position deviation X as the second parameter both have a correlation with the motor torque in a predetermined time period just after the collision occurs, so that the increase / decrease trends of the changes in the values ​​of the first parameter and the second parameter over time are similar. In the predetermined time period from when the correlation occurs until just after the "squeeze state" occurs, the product P=Q×X of the first parameter Q and the second parameter X increases significantly, and the waveform of the product P after the collision has occurred changes rapidly in a short period of time.

[0042] Figure 5 The product P = Q x X and the parameter Q are shown as a function of time. Figure 5It can be understood that the noise n has no correlation with the torque of the motor, and the increase / decrease trends of the changes in these values ​​have no approximation, so that even if the first parameter Q and the second parameter X are multiplied, the noise value during calculation may not increase significantly. Therefore, compared with the conventional technology of comparing one parameter and a threshold value, according to the present embodiment, false detection due to the presence of noise can be prevented.

[0043] In the embodiment, the threshold value to be compared with the product P of the torque command value Q as the first parameter and the position deviation X as the second parameter is Th0. In the device for taking out the molded product, a value larger than the calculated value obtained by allowing the collision in the teaching work of setting the operation process for taking out the molded product by the taking-out head 3 is defined as the predetermined threshold value Th0. As described above, an "unexpected collision" is a collision in the case where the taking-out head has collided with the mold or an object surrounding the mold due to improper operation. Figure 5 In , since the product P has increased to exceed the threshold value Th0 at the time period (1), it is determined that an unexpected collision has occurred.

[0044] Note that Figure 5 As shown, in the case where the torque command value Q is compared with the threshold value Tp by the conventional method, the noise n will be erroneously detected, so that the conventional method has a larger threshold value. In short, in the case where the threshold value is doubled to 2Tp, the torque command value Q exceeds the threshold value at the time period (2). It will be understood that the detection time period of the unexpected collision is longer than the detection time period (1) in the present embodiment.

[0045] As described above, the present embodiment utilizes the fact that the product of the torque command value Q as the first parameter and the position deviation X as the second parameter adopts a fluctuation pattern different from the stable state and the normal squeeze state from the collision. The concept or idea of ​​the present invention is not only applicable to the product of the torque command value and the position deviation, but also to the case where two parameters both related to the motor torque after the collision are multiplied by a multiplication formula.

[0046] The method of the present invention can also be used for mechanical devices other than the device for taking out a molded product of the present embodiment described above. The "predetermined threshold value" used in the above case is a value exceeding a calculated value P obtained not during a collision but during noise generation or acceleration / deceleration of the mechanical device. Since the calculated value is different in each mechanical device, the "predetermined threshold value" can be defined in each mechanical device.

[0047] Although the preferred embodiments of the present invention have been described to some extent with reference to the accompanying drawings, modifications and variations are apparent in light of the above teachings. It should therefore be understood that within the scope of the appended claims, the present invention may be practiced in ways other than those specifically described.

[0048] Industrial Applicability

[0049] According to the present invention, by comparing a calculated value with a predetermined threshold within a predetermined time period just after a collision, it is possible to determine earlier that an unexpected collision has occurred, wherein the calculated value is obtained by inputting a first parameter and a second parameter both of which are correlated with the torque of the motor into a multiplication formula.

Claims

1. A collision detection method for detecting an unexpected collision in a mechanical device when a movable part has unexpectedly collided with a collision object, the mechanical device comprising a movable part supported by a support part, the position of the support part being changed by a motor as a driving source, the method comprising: a step of comparing a calculated value obtained by inputting a first parameter having correlation with the torque of the motor and a second parameter having correlation with the torque of the motor into a predetermined multiplication formula with a predetermined threshold value within a predetermined time period immediately after the collision occurs, the second parameter being different from the first parameter, and The step of determining that an unexpected collision has occurred when the calculated value exceeds the threshold value.

2. The collision detection method according to claim 1, wherein: At least one of the first parameter and the second parameter having a value of 1 (one) or less is converted into a reciprocal of the parameter, and the reciprocal of the parameter is input into the multiplication formula to obtain the calculated value.

3. The collision detection method according to claim 1, wherein: The first parameter is a parameter related to torque fed back to the drive unit of the motor, and the second parameter is a parameter related to a positioning deviation between a positioning 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 collision detection method for a movable part according to claim 1, wherein: The mechanical device is a device for taking out a molded product, the movable part is a taking-out head of the device for taking out a molded product, and the supporting part is a supporting arm supporting the taking-out head, and The threshold value is a value larger than a calculated value obtained by allowing collision in a teaching work setting an operation step for taking out a molded product by the taking-out head.

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

6. A collision detection system for detecting an unexpected collision in a mechanical device when a movable part has unexpectedly collided with a collision object, the mechanical device comprising a movable part supported by a support part, the position of the support part being changed by a motor as a driving source, the system comprising: a first parameter acquisition part, which acquires a first parameter having correlation with the torque of the motor within a predetermined time period just after the collision occurs, a second parameter acquisition section, which acquires a second parameter that is correlated with the torque of the motor and is different from the first parameter within a predetermined time period immediately after the collision occurs, a calculation section that obtains a calculated value by inputting both the first parameter and the second parameter into a predetermined multiplication formula, and The determination section 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. A collision detection system for a movable part according to claim 6, wherein: The mechanical device is a device for taking out a molded product, the movable part is a taking-out head of the device for taking out a molded product, and the supporting part is a supporting arm supporting the taking-out head, and The threshold value is a value larger than a calculated value obtained by allowing collision in a teaching work setting an operation step for taking out a molded product by the taking-out head.

Images