JP2006116650A - Collision detecting method of robot - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To solve the problem of the conventional collision detecting method wherein it is difficult to reduce a collision torque detection error due to vibration of a speed reducer spring component, a collision detecting threshold has to be increased in order to prevent a mistake in collision detection when the operational frequency of a robot approaches the resonance frequency of the speed reducer spring component in torch lifting operation in starting arc for use in welding or weaving operation for vibrating the torch, and the sensitivity for detecting a collision is lowered in ordinary operation. <P>SOLUTION: When a command acceleration of robot operation is larger than a predetermined value, the collision detecting threshold is increased, and it is held for a predetermined time to thereby cope with a phase lag or duration of vibration of the speed reducer spring component so that a mistake in collision detection can be prevented. <P>COPYRIGHT: (C)2006,JPO&NCIPI

Description

  The present invention relates to a collision detection method for a robot driven by a motor via a speed reducer.

  In recent years, robots are required to have high accuracy in collision detection in order to improve safety at the time of collision and to prevent loss due to destruction. However, the use of a high-precision collision sensor increases the cost and swings the sensor as a heavy load, which is against the speeding up and energy saving of the robot. Therefore, there is a demand for higher accuracy in sensorless detection of collision force.

  As a method of obtaining the collision force without a sensor, the robot obtained from the reverse dynamics calculation of the robot from the reducer output torque obtained by subtracting the torque lost due to friction with the motor and reducer inertia from the torque generated by the motor drive current There are a method for obtaining a collision force by subtracting the dynamic torque (hereinafter referred to as a dynamics calculation method; see Non-Patent Document 1) and a method for obtaining a collision force using a disturbance estimation observer (hereinafter referred to as a disturbance estimation observer method). is there.

  FIG. 2 is a control block diagram of the dynamic calculation method.

  In FIG. 2, 6 is a position control block, and a position command θcom (3) obtained by integrating the velocity component dθcom (1) of the position command, and a motor position feedback θm (4) obtained by integrating the motor speed feedback ωm (2). The speed loop command ωcom (7) is generated from the difference value of.

  Reference numeral 10 in FIG. 2 denotes a speed control block, which generates a motor current command Im (11) from a difference value between the speed loop command ωcom (7) and the motor speed feedback ωm (2).

  2 in FIG. 2 is a block showing the motor and external force. If it is assumed that τm (13) is a motor-generated torque and the speed reducer is a rigid body, the motor-generated torque τm (13) can be obtained from (Equation 1) shown below (Equation 1-1). ), And when viewed from the load side, (Expression 1-2).

また、上記に示す動摩擦トルクτμ（１５）は、以下に示す（数２）で計算できる。

Further, the dynamic friction torque τμ (15) shown above can be calculated by the following (Equation 2).

また、（数１−２）の右辺にある衝突トルクτｄｉｓは、（数１−１）と（数１−２）より、以下に示す（数３）に変形して求めることが出来る。

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 FIG. 2, (30) represents (Equation 3) as a collision torque estimation block.

  In the collision torque estimation block (30), the estimated dynamic torque value τdyno (29) is obtained from the reverse power using the motor speed feedback of all axes constituting the robot and the machine parameters of the robot in the dynamic torque calculation block (26). It is required by performing academic operations. The collision torque estimation block (30) uses the dynamic torque estimation value τdyno (29) to determine the collision torque estimation value τdiso (28), and the collision torque estimation value τdiso (28) is sent to the collision determination block (31). Output.

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

以上説明した従来の動力学演算方式では、減速機が剛体であることが前提であった。

In the conventional dynamic calculation method described above, it was assumed that the speed reducer was a rigid body.

  However, an actual speed reducer has a spring component, and vibration may be generated by this spring component.

  FIG. 3 shows a model of a motor and a speed reducer in a robot. A motor (72), a speed reducer (73), and a bearing (74) are provided on an arm 1 (71) serving as a motor mounting base. The arm 2 (79), which is a load that is fixed and coupled to the rotating part of the speed reducer secondary side (77), is driven.

  The reduction gear primary side (76) is coupled to the rotor in the motor by the motor rotation shaft (80), and rotates at the motor rotation speed ωm (2). The speed reducer (73) reduces the motor rotation speed ωm (2) to the load rotation speed ωL (41) with a reduction ratio Rg.

  Here, the reduction ratio Rg of the reduction gear (73) is expressed by the following (Equation 5).

しかし、減速機（７３）は減速機１次側（７６）と減速機２次側（７７）の間にバネ成分が存在するので、（数５）が成立するのは、バネの伸びが一定となった定常状態の場合のみである。

However, since the reduction gear (73) has a spring component between the reduction gear primary side (76) and the reduction gear secondary side (77), (Equation 5) is satisfied because the extension of the spring is constant. This is only in the steady state.

  FIG. 4 shows a block diagram of the model shown in FIG. 3 where KS is the spring constant of the spring component.

  In FIG. 4, Im (11) is a motor current command for driving the motor (72), Kt (12) is a torque constant of the motor (72), 1 / Rg (42, 43) is a reciprocal of the reduction ratio, and 44 is a motor. 45, a load transfer function, KS (46) is a spring constant of the speed reducer (73), and θs (47) is a twist generated between the speed reducer primary side (76) and the speed reducer secondary side (77). The angle 48 is an integral.

τdis (22) is a collision torque applied to the load (arm 2), τdyn ′ (49) is a dynamic torque excluding inertial force and gravity torque of its own axis, τG (50) is gravity torque, and τμ (15) is dynamic friction. Torque.

  In the motor transfer function (44), the motor inertia Jm is the moment of inertia around the rotating shaft (80) including the motor rotor (75) and the speed reducer primary side (76), and Dm is the motor viscous friction coefficient.

  Also in the load transfer function (45), the load inertia JL is the moment of inertia around the rotation axis (80) including the load (arm 2) (79) and the speed reducer secondary side (77), and DL is the load viscous friction coefficient. is there.

  Particularly in a large robot, the resonance frequency of the spring component of the speed reducer modeled in FIG. 4 becomes a low frequency of 10 Hz or less, and the probability of occurrence of vibration increases if the robot operating frequency is close to this.

  In normal use of the robot, the acceleration / deceleration is adjusted so that the operation frequency of the robot is lower than the resonance frequency of the spring component so that vibration does not occur, so that it does not become a big problem. However, in welding applications, the torch pulling operation at the arc start described in Patent Document 1 and the weaving operation to vibrate the torch require rapid response even if the vibration is somewhat degraded and the trajectory accuracy is somewhat degraded. There is a high possibility that the operating frequency of the robot approaches the resonance frequency of the spring component.

  In such a case, if the collision detection threshold is determined by ignoring the vibration due to the spring component of the speed reducer, there is a possibility that the collision is erroneously detected even though the collision is not occurring.

  FIG. 5 is a diagram showing an example of this erroneous detection. Normal operation is shown between times 0.1 and 0.5, and torch raising operation is shown between times 0.6 and 0.8. The time change of the velocity component dθcom (1) of the position command, the time change of the acceleration component αcom obtained by differentiating it, and the time change of the collision torque estimated value τdiso (28) are shown below.

  In normal operation, the absolute value of αcom is adjusted so as not to exceed the acceleration threshold value αth so as not to generate vibration due to the reduction gear spring component. The acceleration threshold value αth is obtained by actually performing a normal operation.

  However, in the torch pulling-up operation, even if some vibration is generated and the trajectory accuracy is somewhat degraded, quick response is required. Therefore, the acceleration αcom at this time may exceed the acceleration threshold αth.

  In the conventional dynamic calculation method shown in FIG. 2, since the spring component of the speed reducer is not modeled as shown in FIG. 4, the vibration appears as an error of the collision torque estimated value τdiso (28) as it is. This is shown between the times 0.6 to 0.8 in FIG. 5 and the absolute value of the collision torque estimated value τdiso (28) exceeds the collision detection threshold τth (39) twice.

  When such a phenomenon occurs, it is erroneously detected that a collision has occurred even though no collision has occurred. In order to avoid this erroneous detection, the collision detection threshold value τth (39) can only be increased. However, since the collision detection sensitivity is lowered, the detection when a collision actually occurs is delayed. When the collision detection is delayed, taking the impact mitigation means due to the collision is also delayed, and the arm, the speed reducer, and the work are damaged.

  On the other hand, in the disturbance estimation observer method, there is known a method for realizing collision detection for a model shown in FIGS. 3 and 4 in which a reduction gear has a spring (see, for example, Patent Document 2).

  FIG. 6 shows a block diagram for explaining the disturbance observer method. In the collision torque estimation block (69), the disturbance estimation observer (61) receives the motor current Im (11) and the motor rotational speed ωm (11) as inputs, the motor inertia Jm, the load inertia JL, and the motor viscous friction coefficient Dm as calculation parameters. , Load viscous friction coefficient DL, reducer spring constant KS (46) and reduction ratio Rg, load rotational speed ωL (41), torsion angle θs (47) and disturbance torque sum (τdis + τdyn ′ + τμ + τG) o (65 ).

  The gravitational torque calculation block (62) calculates a gravitational torque estimated value from position information (integrated speed (63)) of all axes constituting the robot, and outputs a gravitational torque estimated value τGo (67).

The collision torque estimation block (69) subtracts the gravity torque estimated value τGo (67) and the dynamic friction torque estimated value τμo (24) from the sum of disturbance torques (τdis + τdyn ′ + τμ + τG) o (65), thereby calculating the collision torque estimation ( τdis + τdyn ′) o (66) is output to the collision determination block (30).
JP 2002-205169 A JP 2000-52286 A Kazuhiro Komine, 1 other, “Dynamic collision detection of manipulators”, Japan Society of Mechanical Engineers [No. 99-9] Robotics and Mechatronics Lecture '99 Proceedings 2A1-11-1030

  However, the conventional disturbance estimation observer method has the following problems.

  The first problem is that the collision torque estimation (τdis + τdyn ′) o (66) includes the dynamic torque τdyn ′ (49) excluding the inertial force and gravity torque of the own axis. τdyn ′ (49) is mainly composed of interference force (centrifugal force, Coriolis force) from another axis.

  Since this τdyn ′ (49) becomes an error component, it is conceivable that the collision detection threshold τth is increased (collision detection sensitivity is reduced) as compared with the dynamic calculation method. That is, even if the spring component of the speed reducer shown in FIGS. 3 and 4 is modeled, there is a possibility that the collision detection sensitivity may be lower than that of the dynamic calculation method.

  The second problem is that in the disturbance observer (61), accurate values of the load inertia JL and the spring constant KS (46) are required. If there is an error in these parameters, an error also occurs in the disturbance estimation. The meaning of modeling a spring is reduced.

  Since the load inertia JL varies depending on the posture of the robot and the load attached to the arm, the load inertia JL needs to be calculated in real time and can be calculated.

  However, there is a problem in setting the spring constant KS (19) to a fixed value. FIG. 7 is a diagram showing an example of a spring constant KS of a harmonic reducer that is a representative reducer used in a robot, and is described in a certain manufacturer catalog. In FIG. 7, since the spring constant also changes when the twist angle changes, it is approximated by three stages of straight lines, and the respective torque constants are K1, K2, and K3. For spring constants with a reduction ratio of 80 or more that are frequently used in robots, the average values of K1, K2, and K3 are obtained from the manufacturer catalog values, and the errors of K1, K2, and K3 are calculated based on the average values. The maximum is about 33%.

  Further, the torsion angle θs (47) and the load rotation speed ωL (41) are not directly measured, but are values estimated as one variable of the disturbance estimation observer. Therefore, when the spring constant KS (46) changes, the torsion angle The estimated value of θs (47) also changes. However, in reality, the spring constant KS (46) is a function of the torsion angle θs (47) and is dependent on each other, so that estimation is impossible.

  Therefore, the only way to establish the disturbance estimation observer is to regard the spring constant KS (46) as a fixed value, but the error is highly likely to deteriorate the accuracy of the disturbance estimation value.

  In other words, even if the reduction gear spring component is modeled, it is not always possible to sufficiently improve the detection accuracy of the collision force, so it is difficult to eliminate the possibility of erroneous detection as a collision even though there is no collision. It is.

  The present invention solves the above-described problem, and when the operating frequency approaches the resonance frequency of the reducer spring component without reducing the collision detection sensitivity when the operating frequency of the robot is low (for example, during normal operation). It is an object of the present invention to provide a robot collision detection method capable of preventing erroneous collision detection (for example, during a pulling up operation of a welding torch).

  In order to achieve the above object, a robot collision detection method according to the present invention is a robot driven by a motor via a speed reducer, and is obtained by a reverse dynamics calculation of the robot from a torque output from the motor to the speed reducer. It has a collision detection method that detects the external force due to the collision without sensor by subtracting the dynamic torque, and determines that the arm has received the external force if the detected value of the external force is greater than a preset threshold value. If the acceleration is larger than a predetermined value set in advance, the threshold for collision detection is raised to lower the collision detection sensitivity.

  In the robot collision detection method of the present invention, if the command acceleration of the robot operation is larger than a predetermined value set in advance, the threshold for collision detection is increased and held for a predetermined time.

  As described above, in the robot collision detection method of the present invention, the operating frequency is reduced to the resonance frequency of the reducer spring component without reducing the collision detection sensitivity when the robot operating frequency is low (for example, during normal operation). It is possible to prevent erroneous detection of collision when approaching (for example, when the welding torch is lifted).

  Also, if the command acceleration of the robot operation is larger than a predetermined value set in advance, the threshold value in the collision detection is raised and held for a predetermined time, so that the erroneous detection of the collision due to the phase delay of the reducer spring component vibration or the continued vibration Can be prevented.

(Embodiment)
Hereinafter, an embodiment of the present invention will be described.

  FIG. 1 is a block diagram showing a collision detection method according to the present embodiment, which is configured by adding a collision detection threshold setting block (34) based on the dynamics calculation method shown in FIG. In FIG. 1, the same parts as those in FIG. 2 are denoted by the same reference numerals, and detailed description thereof is omitted.

  In FIG. 1, in the collision detection threshold value setting block (34), the position command acceleration component αcom (33) obtained by differentiating the position command speed component dθcom (1) is input and compared with a predetermined acceleration threshold value αth. The collision detection threshold τvth (35) is output to the collision determination block (31) according to (Equation 6) shown in FIG.

なお、上記τｔｈは通常動作を実際に行ってそれに基づいて予め求められたものであり、ｄτｔｈは、通常動作ではない動作を実際に行ってそれに基づいて予め求めたれたものである。

Note that τth is obtained in advance based on the normal operation actually performed, and dτth is obtained in advance based on the actual operation that is not the normal operation.

FIG. 8 shows a waveform when the collision is determined by the collision determination block (31) using the collision detection threshold τvth (35). 8 shows an example in which the robot performs the welding torch lifting operation as an example of an operation that is not a normal operation. As shown in FIG. 8, the time at which the torch lifting operation is performed is 0.6 to 0.00. Between 8, the absolute value of the acceleration component αcom (33) of the position command exceeds the predetermined acceleration threshold value αth (38), and during that period, as shown in (Equation 6), the collision detection threshold value τvth (35) is Then, it becomes larger by dτth (36) than the collision detection threshold value τth adjusted in the normal operation. As a result, even if a vibration error of the reduction gear spring is added to the collision torque estimated value τdiso (28) between time 0.6 and 0.8, the absolute value does not exceed the collision detection threshold τvth (35). No false detection of collision occurs.

    Further, after time 0.8, the absolute value of the acceleration component αcom (33) of the position command becomes equal to or less than the predetermined acceleration threshold value αth (38), so the collision detection threshold value τvth (35) is the collision detection adjusted by the normal operation. Returning to the threshold value τth, this does not decrease the collision detection sensitivity during normal operation.

  The above-described threshold determination and threshold change are performed by a program stored in a CPU (Central Processing Unit) provided in the robot system, for example.

  In the above, the collision detection threshold value τth obtained by adjusting the collision detection threshold value τvth (35) by the normal operation immediately when the absolute value of the acceleration component αcom (33) of the position command becomes equal to or less than the predetermined acceleration threshold value αth (38). However, the collision detection threshold τvth (35) is adjusted by the normal operation immediately after the absolute value of the acceleration component αcom (33) of the position command becomes equal to or less than the predetermined acceleration threshold αth (38). Instead of returning to the detection threshold τth, the value of the collision detection threshold τvth (35) may be held at τth + dτth for a predetermined time Td (37) as shown in FIG.

  This example will be described with reference to FIG. In FIG. 9, the absolute value of the acceleration component αcom (33) of the position command exceeds the predetermined acceleration threshold value αth (38) during the time 0.6 to 0.8 when the torch pulling operation is performed. As shown in (Equation 6), the collision detection threshold value τvth (35) is larger than the normal case by dτth (36), but once the absolute value of the acceleration component αcom (33) of the position command is a predetermined acceleration threshold value. When αth (38) is exceeded and next falls (time 0.8 in the figure), the value of the collision detection threshold τvth (35) is held at τth + dτth for a predetermined time Td (37).

  Thus, even when the absolute value of the acceleration component αcom (33) of the position command exceeds the predetermined acceleration threshold αth (38) and then falls below the acceleration threshold αth (38), the collision detection threshold τvth (35) By maintaining the value of the collision detection threshold τvth (35) at τth + dτth for a predetermined time Td (37) without immediately returning the value to τth, vibration phase delay or vibration persistence due to the reducer spring component occurs. Even in this case, erroneous detection of collision can be prevented.

  The process of holding the collision detection threshold value τvth (35) at τth + dτth for the predetermined time Td (37) described above is stored in, for example, a CPU (Central Processing Unit) provided in the robot system. Is performed by the program.

  In the present embodiment, the description has been made based on the dynamics calculation method, but it goes without saying that the same method can be used for the disturbance estimation observer method shown in FIG.

  As described above, when performing a pulling up operation of a welding torch in which the acceleration component increases with respect to the normal operation, it is possible to prevent erroneous collision detection by making the threshold value larger than the collision detection threshold value adjusted in the normal operation. When the acceleration component returns to the normal operation state, the collision detection threshold can be returned to the collision detection threshold adjusted by the normal operation, and the collision detection can be performed without reducing the collision detection sensitivity during the normal operation. .

  The robot collision detection method of the present invention can prevent erroneous detection of collision when the operating frequency approaches the resonance frequency of the reducer spring component without reducing the collision detection sensitivity when the operating frequency of the robot is low. It is possible to prevent false detection of collision even under conditions where quick response is required even if there is some vibration such as torch pulling action at arc start in welding applications and weaving action to vibrate the torch. Yes, it is industrially useful.

The block diagram which shows the collision detection method in embodiment of this invention Block diagram showing collision detection method in the prior art (dynamic calculation method) Schematic configuration diagram showing the spring component of the robot reducer A block diagram modeling the spring component of a robot reducer Waveform diagram showing collision determination in the prior art form Block diagram showing the collision detection method in the prior art (disturbance observer method) Graph showing the spring constant of the robot reducer (harmonic reducer) The wave form diagram which shows the collision determination in embodiment of this invention The wave form diagram which shows the collision determination in embodiment of this invention

Explanation of symbols

1 Speed component of position command dθcom
2 Motor speed feedback ωm
3 Position command θcom
4 Position feedback θm
5 Position proportional gain KPP
6 Position control block 7 Speed loop command ωcom
8 Speed proportional gain KP
9 Speed integral gain KI / s
10 Speed control block 11 Motor current Im
12 Motor torque constant Kt
13 Motor generated torque τm
14 Dynamic torque (sum of gravity torque, inertial force, centrifugal force, Coriolis force) τdyn
15 Dynamic friction torque τμ
16 Impact torque τdis
17 Motor transfer function block 18 Block indicating motor and external force 19 Motor torque constant Kt
20 Inverse of motor transfer function 21 Motor rotation direction determination block 22 Motor direction signal sgn
23 Dynamic friction coefficient Kμ
24 Estimated value of dynamic friction torque τμo
25 Other axis motor speed 26 Dynamic torque calculation block 27 Other axis dynamic torque estimated value 28 Collision torque estimated value τdiso
29 Estimated value of dynamic torque τdyno
30 Collision torque estimation block 31 Collision determination block 32 Differential element 33 Acceleration component αcom of position command
34 Collision detection threshold setting block 35 Collision detection threshold τvth
36 Collision detection threshold increment dτth
37 Collision detection threshold retention time Td
38 Acceleration threshold αth
39 Collision detection threshold (fixed value) τth
40 Integral element 41 Load rotation speed ωL
42 Reciprocal of reduction ratio 1 / Rg
43 Inverse reduction ratio 1 / Rg
44 Motor transfer function 45 Load transfer function 46 Spring constant KS
47 Reducer twist angle θs
48 Integral element 49 Dynamic torque excluding inertial force and gravity torque of own axis τdyn '
50 Gravity torque τG
61 Disturbance observer block 62 Gravity calculation block 63 Integration element 65 Sum of disturbance torque (τdis + τdyn '+ τμ + τG) o
66 Collision torque estimate (τdis + τdyn ') o
67 Estimated value of gravity torque τGo
68 Other axis gravity torque estimated value 69 Collision torque estimation block (disturbance observer)
71 Motor mounting base (arm 1)
72 Motor 73 Reducer 74 Bearing 75 Motor rotor 76 Reducer primary side 77 Reducer secondary side 78 Reducer spring 79 Load (arm 2)
80 Motor rotating shaft

Claims (2)

In a robot driven by a motor via a reducer, the external force due to the collision is detected without a sensor by subtracting the dynamic torque obtained by the reverse dynamics calculation of the robot from the torque output from the motor to the reducer. A collision detection method for determining that the arm has received an external force if the detected value is greater than a predetermined threshold value. If the command acceleration of the robot operation is greater than a predetermined value, the threshold for collision detection is increased. A collision detection method for an articulated robot characterized by lowering collision detection sensitivity. 2. The collision detection of an articulated robot according to claim 1, wherein if the commanded acceleration of the robot operation is larger than a predetermined value set in advance, the threshold value in the collision detection is increased and the increased state is maintained for a predetermined time. Method.

Images