JP2013169609A - Method for detecting collision of robot - Google Patents

Abstract

In order to prevent erroneous collision detection while maintaining high-precision collision detection, the collision detection threshold is accurately detected only when necessary, such as when the motor-generated torque is saturated due to factors other than collision with surrounding equipment. Need to be raised.
In a robot driven by a motor, a motor-generated torque feedback estimated value obtained by a calculation unit that calculates a motor-generated torque feedback estimated value from the torque output from the motor to the speed reducer by using the inverse dynamics calculation of the robot. Is detected by detecting the external force due to the collision, and if the detected value of the external force is greater than a predetermined threshold value, it is determined that the external force has been received, based on the position command, the speed command, and the acceleration command. If the predicted motor generation torque command value calculated by the calculation unit is larger than a predetermined value set in advance, the collision detection is performed by raising the threshold value for collision detection above the predetermined threshold value.
[Selection] Figure 1

Description

  The present invention relates to a collision detection method for an industrial robot.

  In recent years, in an industrial robot, there has been a demand for higher accuracy of a collision detection function in order to improve safety when the robot collides with surrounding facilities and to prevent loss due to mutual destruction. However, the use of a high-precision collision force sensor for this purpose increases costs, causes problems such as sensor robustness and the property of the sensor itself as a weight load, and contributes to limiting robot performance. End up. Therefore, a method that can detect the collision force with high accuracy and sensorlessness is the most demanding.

  In the sensorless condition, the collision force can be obtained with high accuracy by using the reverse speed of the robot from the reduction gear output torque obtained by subtracting the torque lost due to friction with the motor and reduction gear inertia from the torque generated by the motor drive current. There is a method of obtaining a collision force by subtracting the robot dynamic torque obtained from the dynamic calculation (hereinafter referred to as a dynamic calculation method) (see, for example, Patent Document 1).

  FIG. 4 is a control block diagram of the dynamic calculation method. In the position controller 20 shown in FIG. 4, the speed loop command ωcom is generated from the difference value between the position command θcom obtained by integrating the speed component dθcom of the position command and the motor position feedback θm obtained by integrating the motor speed feedback ωm.

  The speed controller 21 generates a motor current Im from the difference value between the speed loop command ωcom and the motor speed feedback ωm.

  Further, τm is a motor-generated torque, and assuming that the speed reducer is a rigid body, the motor-generated torque τm is expressed by (Equation 1-1) when viewed from the motor drive side in (Equation 1) shown below. From the load side, it is expressed by (Equation 1-2).

here,
Kt: Motor torque constant Im: Motor current αm: Motor acceleration feedback ωm: Motor speed feedback Jm: Motor inertia (rotor + reduction gear primary side)
D: viscous friction resistance τμ: dynamic friction torque τdyn: dynamic torque (sum of gravity torque, inertial force, centrifugal force, Coriolis force)
τdis: collision torque The dynamic friction torque τμ shown above can be calculated by the following (Equation 2).

here,
Kμ: Size of dynamic friction
1 (ωm> 0)
sgn = 0 (ωm = 0)
-1 (ωm <0)
Further, the collision torque τdis on the right side of (Equation 1-2) can be obtained by transforming (Equation 3) shown below from (Equation 1-1) and (Equation 1-2).

  In the above (Equation 3), Kt × Im−Jm × αm−D × ωm−Kμ × sgn is the torque output from the motor to the reduction gear, and τdyn is the dynamic torque.

  In the dynamic calculation module block 30 indicated by a broken line in FIG. 4, the estimated dynamic torque τdyno is reversed using the motor speed feedback ωm of all axes constituting the robot and the machine parameters of the robot in the reverse dynamic calculation block 27. It is obtained by executing dynamics calculation. The dynamic calculation module block 30 adds the dynamic torque estimated value τdyno, the dynamic friction torque estimated value τμo, and the inertia of the motor calculated in the inverse block 24 of the motor transfer function and the torque generated by the friction (hereinafter referred to as the motor torque calculation function block 30). , Motor estimated torque feedback estimated value). Then, a collision torque estimated value τdiso is obtained by taking the difference between the motor generated torque feedback estimated value and the motor generated torque τm, and this collision torque estimated value τdiso is output to the collision detection determination block 28.

  The collision detection determination block 28 detects a collision according to the following (Equation 4) using a predetermined collision detection threshold τth.

  While the above-described dynamic calculation method is highly accurate, it may be a habit. High accuracy means that collision detection is very sensitive. In other words, even when the difference between the motor-generated torque feedback estimated value obtained by the dynamics calculation method and the motor-generated torque is not so large (the robot has not collided anywhere), the collision may be detected. There is.

  Therefore, there is a great possibility that collision detection, that is, erroneous detection occurs at an operating point that is not intended by the operator of the robot. In order to prevent this, a method of increasing the collision detection threshold and reducing the sensitivity of collision detection can be considered. However, it can be said that this is a fall. Therefore, there is a need for a method for increasing the collision detection threshold only in an operation such as rapid acceleration, which tends to cause a deviation between the motor generated torque feedback estimated value and the motor generated torque.

  As a method of preventing such a collision erroneous detection, there is a method of increasing the collision detection threshold value under specific conditions. For example, Patent Document 1 describes a method of increasing a collision detection threshold based on an acceleration command. This method resets the collision detection threshold to a higher value than usual when the acceleration command becomes equal to or greater than a preset threshold.

  In addition, as a method for preventing erroneous collision detection when the frictional force of the robot drive shaft increases in a low-temperature environment, a method of increasing the collision detection threshold based on the temperature of the drive shaft is described (for example, Patent Documents). 2). In this method, the temperature of the drive shaft is detected by a temperature sensor or the like, and the collision detection threshold is reset to a higher value than usual when the temperature is equal to or lower than a preset threshold.

JP 2006-116650 A Japanese Patent Laid-Open No. 11-15511

  The method described in Patent Document 1 increases the collision detection threshold based on the acceleration command, and increases the collision detection threshold regardless of the value of the actually detected collision torque, which is not necessary. There is a case where the collision detection threshold value is raised even at the operating point. Therefore, as a result, the accuracy of collision detection is reduced by itself. Such cases often exist in actual robot operations.

  Here, erroneous collision detection is likely to occur when the motor current, that is, the motor-generated torque is near the limit value determined for each motor specification for safety. The outline is shown in FIG.

  FIG. 5 shows a motor generated torque 31 caused by the current actually flowing through the motor, a preset limit value 32 of the motor generated torque 31, and a motor generated torque feedback obtained by the dynamics calculation module block 30. An estimated value 33, a collision detection threshold value 34, and a collision torque 35 are shown.

  In FIG. 5, the motor generated torque 31 is limited by a preset limit value 32. However, the motor generated torque feedback estimated value 33 obtained by the dynamics calculation module block 30 has no limit value and is a value equal to or greater than the limit value 32. Therefore, a collision torque 35 exceeding the collision detection threshold 34 is generated, and erroneous collision detection occurs.

  Also, the motor generated torque may be saturated when it collides with surrounding equipment. Therefore, considering that case, it should be noted here that a limit value cannot be set for the motor-generated torque feedback estimated value.

  From these facts, it can be said that it is desirable that the erroneous collision detection prevention function can prevent the erroneous collision detection while maintaining high-precision collision detection by raising the collision detection threshold accurately only when necessary. .

  It is an object of the present invention to provide a collision detection method for a robot having a collision error detection function that satisfies the above-described conditions using a command value (position command, speed command, acceleration command) and a dynamics calculation module block.

  In order to solve the above-described problems, a robot collision detection method according to the present invention is a robot driven by a motor through a speed reducer, and uses the inverse dynamics calculation of the robot from the torque output from the motor to the speed reducer. The external force caused by the collision is detected by subtracting the estimated motor generated torque feedback value obtained by the calculation unit for calculating the estimated torque generated by the motor, and if the detected value of the external force is larger than a predetermined threshold value, the robot is configured. A collision detection method in which it is determined that the arm receiving the external force receives the motor generated torque command predicted value obtained by the calculation unit based on the position command, the speed command, and the acceleration command is greater than a predetermined value, The collision detection method is performed by raising a threshold in the collision detection above the predetermined threshold.

  In addition to the above, the robot collision detection method of the present invention maintains a state in which the threshold value used for collision detection is raised for a predetermined time, and returns it to the threshold value before being raised after the predetermined time.

  As described above, according to the present invention, by performing a collision error detection preventing function that changes a threshold value for collision detection using a calculation unit that performs a command value (position command, speed command, acceleration command) and dynamics calculation. Highly accurate collision detection can be realized.

The figure which shows the outline of the structure which implement | achieves the collision erroneous detection prevention function in Embodiment 1 of this invention. The figure explaining the operation | movement of the collision erroneous detection prevention function in Embodiment 1 of this invention Control block diagram of collision detection function (dynamic calculation method) in Embodiment 1 of the present invention Control block diagram of conventional collision detection function (dynamic calculation method) A diagram for explaining the occurrence of erroneous collision detection in the conventional collision detection function

(Embodiment 1)
The first embodiment will be described with reference to FIGS. FIG. 1 is a diagram showing a configuration for executing the function of preventing erroneous collision detection according to the first embodiment. FIG. 2 is a diagram for explaining an actual operation of the erroneous collision detection preventing function in the first embodiment. FIG. 3 is a control block diagram of the collision detection function (dynamic calculation method) in the first embodiment of the present invention.

  In the first embodiment, the same portions as those in FIG. 4 described in the background art section are denoted by the same reference numerals, and detailed description thereof is omitted.

  In FIG. 1, a position command setting unit 1, a speed command setting unit 2, an acceleration command setting unit 3, a calculation unit 4 that calculates a motor generated torque command predicted value by dynamic calculation, and a collision erroneous detection prevention function execution determination A threshold setting unit 5, a collision erroneous detection prevention function execution determination unit 6, a process switching switch 7, a normal collision detection threshold setting unit 8, a collision detection threshold increase unit 9, and an increase holding time setting unit 10; A collision detection threshold value setting unit 11 for preventing erroneous collision detection is shown. Note that these components are provided, for example, in the robot controller among the manipulators and robot controllers that constitute the articulated robot. The calculation unit 4 corresponds to the dynamics calculation module block 30 shown in FIG.

  The operation of FIG. 1 will be described. The position command setting unit 1 is a command in an operation program stored in the robot controller, and sets a position command based on an operation command including a distance and an operation speed given by teaching. The speed command setting unit 2 sets the speed command based on the operation command as in the case of the position command setting unit 1. The acceleration command setting unit 3 sets the acceleration command based on the operation command as in the case of the position command setting unit 1.

  The calculation unit 4 receives the set position command, speed command, acceleration command, and speed command for the other axis as input, and calculates a motor generated torque command predicted value based on these. In order to detect a collision, as described with reference to FIG. 4, when the motor generated torque feedback estimated value is calculated in FIG. 3, the calculation is performed using the motor speed feedback ωm of the own axis and the other axis. Do. However, when calculating the motor generated torque command predicted value in order to perform the erroneous collision detection preventing function in the first embodiment to be described later, the motor speed feedback ωm between the own axis and the other axis is not used, Calculation is performed using the speed command of the other axis. Further, the calculation unit 4 obtains a position command and an acceleration command from the speed command and uses them for calculation. That is, the position command and the acceleration command are set based on a speed command that is one of the operation commands.

  The collision error detection prevention function execution determination threshold setting unit 5 sets a threshold value for determining whether or not to execute the collision error detection prevention function stored in advance in the robot controller. The collision error detection prevention function execution determination unit 6 compares the motor generated torque command predicted value calculated by the calculation unit 4 with the threshold value set by the collision error detection prevention function execution determination threshold setting unit 5 to prevent collision error detection. Determine whether to execute the function. When the motor generated torque command predicted value ≧ the threshold value, it is determined that “a collision erroneous detection preventing function is executed”. Further, when the motor generation torque command predicted value <the threshold value, it is determined that “the collision erroneous detection preventing function is not executed”. It should be noted that the threshold for determining whether to perform the erroneous collision detection preventing function is set as a value lower than the limit value of the motor-generated torque based on experiments and empirical rules.

  When the collision error detection prevention function execution determination unit 6 determines that “the collision error detection prevention function is not executed”, the collision error detection prevention function execution determination unit 6 and the normal collision detection threshold setting unit 8 are connected. The switch 7 is activated. Then, in the normal collision detection threshold value setting unit 8, a collision detection threshold value stored in advance in the robot controller is set as the collision detection threshold value. That is, a collision detection threshold is set.

  On the other hand, when it is determined by the collision error detection prevention function execution determination unit 6 that “the collision error detection prevention function is executed”, the collision error detection prevention function execution determination unit 6 is connected to the collision detection threshold value increase unit 9. The switch 7 operates. In the collision detection threshold value increasing unit 9, the collision detection threshold value is higher than the threshold value set in the normal collision detection threshold value setting unit 8 stored in advance in the robot control device, It is corrected to the collision detection threshold for preventing erroneous collision detection stored in the apparatus. Further, in the ascending hold time setting unit 10, the time for raising the collision detection threshold is set based on the ascending hold time stored in advance in the robot controller. Then, based on the output of the collision detection threshold value increasing unit 9 and the output of the ascending hold time setting unit 10, the collision detection threshold value setting unit 11 for preventing collision error detection sets the collision detection threshold value for preventing collision error detection. In other words, a collision detection threshold for preventing erroneous collision detection and a rising holding time that is a time for maintaining the collision detection threshold for preventing erroneous collision detection are set.

  The collision detection threshold for preventing collision erroneous detection and the rising retention time of the collision detection threshold are determined, for example, experimentally and empirically.

  Next, the behavior at the time of execution of the collision error detection preventing function will be described with reference to FIG. In FIG. 2, the motor generation torque 12 resulting from the current actually flowing in the motor, the preset limit value 13 of the motor generation torque 12, and the motor generation torque command predicted value 14 obtained by the calculation unit 4. In addition, a threshold value 15 for determining whether or not to perform the collision error detection preventing function, a collision detection threshold value 16, a collision torque 17, and an increase holding time 18 of the collision detection threshold value are shown.

  When the motor generated torque command predicted value 14 exceeds the threshold value 15, the collision detection threshold value 16 is changed to a collision detection threshold value for preventing collision error detection that is higher than normal only during the rising holding time 18 by the collision error detection prevention function. And the collision detection threshold is increased.

  Then, in the state in which the collision detection threshold is raised, the collision is detected in FIG. 3 as in FIG. 4 described in the background art.

  In the robot controller, whether or not the erroneous collision detection prevention function can be executed at a predetermined timing and the corresponding collision detection threshold value are set, and collision detection is performed at a predetermined timing, which is repeated. In addition, after a collision detection threshold value is set in accordance with whether or not the erroneous collision detection prevention function is executed, collision detection is performed based on the set collision detection threshold value.

  Further, according to the collision detection method of the robot according to the first embodiment, as in Patent Document 1 described in the background art, the collision detection threshold is not changed based only on the command value, but the command value and the actual A motor generation torque command predicted value is calculated using the detected motor speed feedback ωm of the own axis and the other axis, and the collision detection threshold value is changed. Therefore, as in the method described in Patent Document 1, it is possible to prevent the collision detection threshold from being raised even at an unnecessary operating point, and to perform collision detection with higher accuracy than in the method of Patent Document 1. .

  Further, the calculation of the motor generated torque command predicted value used for determining whether or not to execute the collision error detection preventing function is performed by the calculation unit 4 that calculates the motor generated torque feedback estimated value used for collision detection. That is, it is performed by sharing the arithmetic unit 4. Therefore, it is not necessary to provide a new calculation unit for calculating the motor generation torque command predicted value.

  According to the robot collision detection method of the present invention, it is possible to improve the collision detection accuracy. Therefore, the method is industrially useful as a robot collision detection method used in, for example, industrial robots used in production.

DESCRIPTION OF SYMBOLS 1 Position command setting part 2 Speed command setting part 3 Acceleration command setting part 4 Calculation part 5 Collision false detection prevention function execution judgment threshold setting part 6 Collision false detection prevention function execution judgment part 7 Switch 8 Normal collision detection threshold setting part 9 Collision Detection threshold rise unit 10 Rise holding time setting unit 11 Collision detection threshold setting unit for preventing erroneous collision detection 12 Motor generated torque 13 Limit value 14 Motor generated torque command predicted value 15 Threshold 16 Collision detection threshold 17 Collision torque 18 Increase holding time 19 Integration 20 Position controller 21 Speed controller 22 Motor torque constant 23 Motor transfer function block 24 Inverse block of motor transfer function 25 Motor rotation direction determination block 26 Dynamic friction coefficient 27 Inverse dynamics calculation block 28 Collision detection determination block 29 Motor and external force Block 30 showing dynamics calculation Ruburokku 31 motor torque 32 limit value 33 generated by the motor torque feedback estimate 34 collision detection threshold 35 collision torque

Claims (2)

In a robot driven by a motor via a speed reducer, motor generated torque feedback obtained by a calculation unit that calculates a motor generated torque feedback estimated value from the torque output from the motor to the speed reducer using the robot's inverse dynamics calculation A collision detection method for detecting an external force due to a collision by subtracting the estimated value, and determining that the arm constituting the robot has received an external force if the detected value of the external force is greater than a predetermined threshold value,
If the motor generated torque command predicted value obtained by the calculation unit based on the position command, the speed command, and the acceleration command is larger than a predetermined value set in advance, the threshold for collision detection is increased above the predetermined threshold, and the collision Robot collision detection method that performs the detection method. The robot collision detection method according to claim 1, wherein a state in which the threshold value used for collision detection is raised is maintained for a predetermined time, and is returned to the threshold value before being raised after the predetermined time.

Images