JP3357392B2 - Robot tool interference prevention device - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention prevents a tool equipped with a tool such as a torch from being damaged due to interference (contact) with a robot arm during teaching or automatic operation. To a device that

[0002]

2. Description of the Related Art Various types of tools are mounted on the distal end of a robot arm according to the type of work. In some robots, a joint pivotally attached to the tip of the arm is rotated up and down by a fifth axis, and a tool attached to the joint is rotated about a sixth axis perpendicular to the fifth axis. There is a vertical articulated robot with axes. Here, depending on the rotational positions of the fifth and sixth axes, the tool and the arm may interfere with each other and cause a serious accident such as breakage.

Therefore, as a device for preventing such accidents beforehand, the maximum to minimum ranges of the operating ranges of the fifth and sixth axes, which are assumed to cause no interference, are determined in advance. There is a type that controls each axis to be driven.

Further, the rotational positions of the respective rotating axes (first to sixth axes) are detected, and the three-dimensional position of the arm and the three-dimensional position of the tool, which are sequentially changed based on the detected rotational positions, are obtained by calculation. Some monitors interference in a three-dimensional space based on the results of these calculations.

[0005]

However, in the former technique, it is necessary to determine an operation range in advance for each of various tools mounted on the robot, which is troublesome and increases the cost required. There are inconveniences. In addition, the operating range (angle) of the sixth axis varies depending on the angle of the fifth axis, and the sixth range corresponds to the angle of the fifth axis.
It is necessary to create complicated formulas and tables that define the angular range of the axis. This makes it practically difficult to implement.

On the other hand, according to the latter technique, it is necessary to perform a complicated calculation for obtaining a three-dimensional position from the rotational positions of the first to sixth axes within a predetermined sampling time. For this reason, a large amount of calculation time is required, and it is virtually impossible to execute the calculation in real time.

[0007] The present invention has been made in view of such a situation, and increases the cost by performing a simpler operation than a conventional one to prevent a tool mounted on the end of a robot's arm from interfering with the arm. It is an object of the present invention to provide a tool interference prevention device for a robot that can monitor in real time without inducing.

[0008]

Accordingly, in the present invention,
In a robot tool interference preventing apparatus for preventing interference between the tool and the arm in a robot in which a tool pivotally attached to the arm tip is rotated by a tool rotation axis provided at the arm tip, The longitudinal axis of the tool and the tool rotation axis are coordinate axes, an interference area indicated by a coordinate position on the coordinate axis is set based on the dimensions of each part of the arm, and the current rotation position of the tool rotation axis is detected. Then, the current position of the predetermined part of the tool is obtained as a coordinate position on the coordinate axis based on the detected rotational position and the dimensions of each part of the tool, and the obtained current coordinate position of the tool is a coordinate position in the interference area. In the case of, it is determined that the tool and the arm interfere with each other, and a predetermined process is performed according to the determination result. That.

[0009]

According to this configuration, the axis in the longitudinal direction of the robot arm and the tool rotation axis are coordinate axes, and the interference area is set based on the dimensions of each part of the arm. Here, since the axis in the longitudinal direction of the arm is set as the coordinate axis, the coordinate position of each part of the arm does not change even if each axis of the robot is driven. Therefore, the coordinate position of each part of the interference area is also a position fixed with respect to the change of each axis of the robot. Therefore, the calculation for setting the interference area is simplified.

On the other hand, the current rotational position of the tool rotation axis is detected, and the current coordinate position of a predetermined portion of the tool is obtained based on the detected rotational position, the dimensions of each part of the tool, and the coordinate axes. Here, since the tool rotation axis is a coordinate axis, and the longitudinal direction of the arm is a coordinate axis, the coordinate position of each part of the tool is determined by the rotation of the tool rotation axis regardless of the rotation positions of the other robot axes. Determined by location only. Therefore, the calculation of the position of the predetermined part of the tool becomes simple. With such a simple calculation, the calculation is completed in a short time, and when the current coordinate position of the tool is a coordinate position in the interference area,
Monitoring to determine that the tool and the arm are interfering is performed in real time, and predetermined processing such as stopping the operation of the robot is reliably performed according to the determination result.

[0011]

BRIEF DESCRIPTION OF THE DRAWINGS FIG. 1 is a block diagram showing an embodiment of a robot tool interference preventing apparatus according to the present invention;

FIG. 1 shows the configuration of a six-axis vertical articulated robot RB applied to the embodiment.

As shown in FIG. 1, the first axis 1 of the robot RB
Is for horizontally rotating the revolving body 8 above the revolving base 7 arranged on the floor, and the axis angle thereof can be represented by θ1. The second shaft 2 rotates a first arm 9 pivotally mounted on the upper part of the revolving body 8 in the front-rear direction, and its axis angle is represented by θ2. The third shaft 3 pivots the joint 3a pivotally attached to the tip of the first arm 9 in the vertical direction, and its axis angle is represented by θ3. 4th axis 4 is joint 3a
Is rotated about the longitudinal center axis C of the second arm 10 as a rotation center, and the axis angle thereof is represented by θ4. The fifth shaft 5 is the second arm 10
The joint 6a pivotally attached to the tip of the shaft is rotated in the up and down directions A and B, and its axial angle is represented by θ5. 6th axis 6
Turns the tool TL mounted on the joint 6a around a sixth axis 6 perpendicular to the fifth axis 5, and its axis angle is represented by θ6.

The rotation angle θ1 of each axis 1 to 6 of the robot RB
To θ6 are detected by an encoder (not shown), and a detection signal is output to a controller (not shown). In this controller, the axes 1 to 6 are drive-controlled based on the detected angles θ1 to θ6. In addition, this controller uses the detection angle θ5,
The interference prevention processing shown in FIG. 5 is executed based on θ6. In such interference prevention processing, the second arm 10 of the robot RB is used.
The geometric relationship between the tool and the tool TL is set as shown in FIGS.

Setting of Coordinate System FIG. 3 is a perspective view showing the geometric relationship between the second arm 10 and the tool TL shown in FIG. 1, and firstly, of the orthogonal three-dimensional coordinate system XYZ. The Z axis is set to be coaxial with the longitudinal center axis C of the second arm 10, that is, the fourth axis 4. Then, the Y axis is set to be coaxial with the fifth axis 5. Further, the X axis is set as an axis perpendicular to these Y axis and Z axis. Setting of interference points and tool parameters on the tool Next, predetermined points (hereinafter, referred to as "interference points") P1 and P2 on the tool TL are selected. For example, it is assumed that the interference points P1 and P2 are selected as two vertices on the tool TL as shown in FIG. Next, these interference points P1, P2
And the distance between the fifth axis 5 and the fifth axis 5 are obtained as tool parameters. Now, as shown in FIG.
1, when the posture of P2 is such that the coordinate position on the Y axis becomes zero, the distance between the fifth axis 5 and the interference point P1 is Tz on the Z axis, and the fifth axis 5 and the interference point Since the distance from P2 is Tx on the X axis, these Tx and Tz are used as tool parameters. Incidentally, the tool parameter Ty on the Y axis is zero in the embodiment. In this embodiment, two interference points are set.
The number of points may be set, and three or more points may be set.

Setting of Interference Area Next, as shown in the perspective view of FIG. 4, the second arm 10 is modeled. That is, the second arm 10 is simplified if the thickness T and the width W of the cross section thereof are constant over the longitudinal direction. And the round part 10a at the tip of the second arm
Is assumed to have a constant value (r) from the fifth axis 5 (the radius portion 10a is semicircular in the XZ plane).

Next, the modeled second arm 1
Based on the data T, W, r, etc. regarding 0, the interference area V, which is considered to be likely to interfere if the interference points P1, P2 on the tool TL enter the interior, is larger than the second arm 10. The area is set as a range area (see a dashed line). This is because if the interference area V is in the same range as the second arm 10, the interference points P1 and P2 have already interfered when entering the area V. However, in the embodiment, for convenience of explanation, the description will be made on the assumption that the region V is the second arm 10. In this way, the longitudinal axis C and the fifth axis 5 of the second arm 10 of the robot RB are set as the coordinate axes Z and Y, and the interference area V is set based on the dimension data T of each part of the arm 10 and the like. Therefore, each robot axis θ1 to θ
Even if 4 is driven, the three-dimensional coordinate position (X, Y, Z) of each part of the arm 10 does not change, and the coordinate position (X, Y, Z) inside the interference area V also changes with each axis of the robot. The coordinate position is fixed. Therefore, the interference area V (X, Y, Z) is the rotational position θ1 to θ4 of each axis of the robot.
It can be seen that the calculation can be easily performed only by the dimension T of the arm 10 without depending on the above.

Setting of arithmetic expression of interference point Now, the current rotational position θ5 of the fifth shaft 5 is detected and the sixth
Assuming that the rotational position θ6 of the shaft 6 has been detected, the coordinate positions P1 (P1) of the interference points P1 and P2 on the tool TL based on the detected rotational positions θ5 and θ6 and the tool parameters Tx, Ty and Tx determined above. X1, Y1, Z1), P2
(X2, Y2, Z2) is obtained by the following arithmetic expression (1).

[Xi] [cos θ50-sin θ5] [cos θ6-sin θ60] [Yi] = [0 10] [sin θ6 cos θ60] [Zi] [sinθ50 cosθ5] [0110] × [TX] × [ TY] (where i = 1, 2) (1) [TZ] As is apparent from the equation (1), the fifth axis 5 is the coordinate axis Y, and the longitudinal axis C of the arm 10 is the same as the coordinate axis Z. Therefore, the coordinate positions of the interference points P1 and P2 are determined by the rotational position .theta.5 of the fifth shaft 5 and the sixth position at the tip of the fifth shaft 5 irrespective of the rotational positions .theta.1 to .theta.4 of the other robot axes. It can be seen that it is determined only by the rotational position θ6 of the shaft 6.

Setting of Criteria for Interference Judgment Next, the conditional expressions when the interference points P1 and P2 enter the interference region V, that is, the conditional expressions for determining that there is a possibility of interference are set as follows. Is done.

1) When the interference points P1 and P2 exist at the tip of the arm (Zi> 0). In this case, when the following equations (2) to (4) are simultaneously satisfied, there is a possibility that "interference may occur". judge.

Zi> 0 (2) | Yi | ≤W / 2 (3) (Zi) 2+ (Xi) 2≤ (W / 2) 2 (4) 2) The interference points P1 and P2 are arms. In the case where it exists at the center (Zi ≦ 0) In this case, when the following expressions (5) to (7) are simultaneously satisfied, it is determined that “there is a possibility of interference”.

Zi ≦ 0 (5) | Yi | ≦ W / 2 (6) | Xi | ≦ T / 2 (7) If the above settings are made, the robot is set based on these settings. During the operation of the RB, the processing shown in FIG. 5 is executed by the controller. Note that this processing may be performed during the teaching of the robot RB, or may be performed during automatic driving.

First, among the interference points Pi (i = 1, 2),
First, the content of i is set to 1 to monitor the interference at the interference point P1 (step 101). Next, the coordinate position (X1, Y1, Z1) of the interference point P1 using the above equation (1).
Is calculated. Here, this calculation is based on the detection angles θ5, θ6
Since it is a simple calculation based only on the robot RB, it can be executed with a margin within the time of the interpolation cycle of the robot RB (step 102).

Next, the above formulas (3) and (6) are applied to the calculated coordinate position Y1 to determine whether or not there is a risk of interference. Here, since the expressions (3) and (6) are conditional expressions common to both the arm tip of 1) and the center of the arm of 2), if these expressions (3) and (6) are not satisfied, The determination in step 103 is NO), and the tool TL is the arm 1
It is determined that there is no risk of interference with 0, and the procedure proceeds to step 106. In step 106, it is determined whether or not the content of i has reached all the interference monitoring numbers n (= 2). If the interference monitoring number n has been reached (determination YES in step 106), the process is terminated. In this case, i is again set to 1
May be initialized and executed again from step 101.

If the interference monitoring number n has not been reached (NO in step 106), the content of i is incremented in the next step 107 to monitor the remaining interference point (P2), and the procedure is repeated in step 107. 102
Then, the operation of equation (1) is performed.

Here, assuming that the conditions of equations (3) and (6) are satisfied for the Y coordinate position Y2 of the interference point P2 (determination YES in step 103), then (2),
It is determined whether the equation (4) is satisfied at the same time (step 104). If it is determined that the expressions (2) and (4) are not satisfied at the same time (determination N in step 104)
O) Then, it is determined whether the expressions (5) and (7) are satisfied at the same time (step 105). Here (5),
If it is determined that Expression (7) is simultaneously satisfied (determination YES in step 105), the interference point P is determined as shown in FIG.
2 interferes at the center of the second arm 10, and if the robot RB is driven as it is, it is determined that it is dangerous, and a process of stopping each axis of the robot RB for safety is executed (step). 108). On the other hand, the content of the error "danger of interference" is displayed on a predetermined display to warn the operator (step 10).
9).

In step 104, (2)
When the equation (4) is satisfied at the same time (YES in step 104), there is a possibility that the interference point P2 may interfere at the tip of the second arm 10.
Steps 08 and 109 are executed to avoid danger.

The content of the process for avoiding danger (step 108) is optional, and it is possible to switch from automatic operation to manual operation.

In the embodiment, the robot RB in which the joint 6a at the tip of the second arm 10 rotates by the sixth axis 6 is assumed, but the tool TL having no sixth axis 6 is rotated only by the fifth axis 5. It is also applicable to robots that do. In this case, since the calculation in the above equation (1) does not need to consider the rotation angle θ6, the processing time is further reduced by further simplifying the calculation. Here, for a robot that does not rotate by the sixth axis 6, there is no interference due to this rotation, so it is only necessary to consider interference on the XZ plane.
Therefore, the interference point of the tool and the coordinate position of the interference area are also 2
Since it is sufficient to consider the dimension, the operation can be further simplified.

Further, the present invention can be applied to a robot further provided with a rotating shaft at the end of the sixth shaft 6.

Although the embodiment assumes a vertical articulated robot, the present invention is not limited to this and can be applied to a horizontal articulated robot.

[0033]

As described above, according to the present invention,
Whether or not the robot tool and the arm interfere with each other is determined by a simple calculation, so that the interference is monitored in real time without increasing the cost.

[Brief description of the drawings]

FIG. 1 is a diagram showing a configuration of a robot in an embodiment of a robot interference prevention device according to the present invention.

FIG. 2 is a view showing dimensions of each part of the tool shown in FIG. 1;

FIG. 3 is a perspective view showing a geometric relationship between an arm and a tool shown in FIG. 1;

FIG. 4 is a perspective view showing a shape of an arm shown in FIG. 1;

FIG. 5 is a flowchart illustrating an interference prevention process according to the embodiment;

[Explanation of symbols]

 5 5th axis 6 6th axis 10 2nd arm TL tool

Continued on the front page (58) Fields surveyed (Int. Cl. ⁷ , DB name) G05B 19/18-19/46 B25J 3/00-3/04 B25J 9/10-9/22 B25J 13/00-13 / 08 B25J 19/02-19/06

Claims (3)

(57) [Claims] 1. A tool interference prevention for a robot for preventing interference between the tool and the arm in a robot in which a tool pivotally attached to the arm tip is rotated by a tool rotation shaft provided at the tip of the arm. In the apparatus, an axis in a longitudinal direction of the arm and the tool rotation axis are coordinate axes, and an interference area indicated by a coordinate position on the coordinate axis is set based on dimensions of each part of the arm. The rotational position of the tool is detected, and the current position of the predetermined part of the tool is determined as the coordinate position on the coordinate axis based on the detected rotational position and the dimensions of each part of the tool. When the coordinate position is within the area, it is determined that the tool and the arm are interfering, and a predetermined process is performed according to the determination result. A tool interference prevention device for robots. 2. The robot tool interference prevention device according to claim 1, wherein the longitudinal axis of the arm and the tool rotation axis are two orthogonal axes, and the axis orthogonal to the two orthogonal axes is a coordinate axis. . 3. The tool has one or more rotation axes, and detects a current rotation position of the rotation axis,
2. The apparatus according to claim 1, wherein a current coordinate position of a predetermined portion of the tool is obtained based on the detected rotation position.