CN1743148A - Interference prevention control device between robots - Google Patents

Abstract

A robot interference prevention control device reads in advance a teaching program of each robot, calculates a scheduled stop position for each robot when issuing a stop command after n number of interpolation periods from the current interpolation period, and checks whether or not interference would occur at the scheduled stop position of each robot. When the robot interference prevention control device judges that a robot will interfere with another robot, it outputs a stop command at the current interpolation period. Due to this, a stop command is output before n number of interpolation periods from the interpolation period where interference would occur and thereby the occurrence of interference can be prevented.

Description

Interference prevention control device between robots

technical field

The present invention relates to an interference prevention control device, which is used to make a plurality of robots work simultaneously, and to prevent interference between the robots when a part of the work area of the robots overlaps.

Background technique

When using multiple robots for joint work, it is necessary to prevent the robots from interfering with each other when the work areas of the robots overlap. As disclosed in Japanese Unexamined Patent Publication No. 60-99591, as a method for detecting the interference between robots, the following method is well known: using line segments to represent each connecting portion of the robot arm, performing robot operation simulation, and obtaining the relationship between the two line segments. If the distance between the closest parts becomes smaller than the predetermined distance D, it is determined that there is a possibility of interference with the corresponding robot arm.

The interference detection method described in Japanese Unexamined Patent Publication No. 66-99591 is a method of performing interference detection by performing an operation simulation of a robot in an off-line state, and does not actually detect interference when the robot is driven. Consider computing the same computation dynamically in real-time as current computer capabilities increase. At that time, even if it is judged that there is a possibility of interference before the interference is expected, the judgment is meaningless if the interference occurs without deceleration. Therefore, the predetermined distance D for the above-mentioned interference detection must be set in advance to be larger than the distance necessary for deceleration. The value of , to judge the possibility of interference in advance.

Also, when the robot arms intersect each other at high speed, even if the minimum distance between the two line segments is judged using the predetermined distance D in anticipation of the distance necessary for deceleration, there is a possibility that the interference may be judged to be interference even though there is no actual interference.

Contents of the invention

Therefore, an object of the present invention is to provide a control device for preventing interference between robots, which detects interference while actually operating the robots, prevents interference, and avoids interference misjudgment.

According to the present invention, there is provided a control device for preventing interference between robots, which detects the approach of a plurality of robots installed in a state where their work areas overlap each other during operation, stops the robots before reaching the interference, and It has: a stop position calculation unit, which first reads the teaching program being executed by each of the target robots, and calculates at least one interpolation cycle or one interpolation cycle preset from the current time for each interpolation cycle. The scheduled stop position when the stop command to the robots is executed at a time after the supplementary cycle or more; an interference determination unit that determines whether there is interference between the robots at each of the obtained scheduled stop positions; a deceleration command unit , which instructs the robot to start decelerating when the interference judging unit judges that there is interference.

Preferably, the stop position calculation unit first reads a teaching program being executed by each target robot, expresses the position and posture of each robot using a function at time t, and uses this function to calculate each The interpolation cycle is to obtain a planned stop position when a stop command to each of the robots is executed at a time after at least one interpolation cycle or more preset from the current time.

In particular, the stop position calculation unit desirably calculates the scheduled stop position based on the value of the function that has passed at least one interpolation period preset from the current time or The value of the function at a time after the time required for the stop after one interpolation cycle or more. For example, a linear sum of functions at time t may be used to represent the position and posture of the robot.

In addition, the control device for preventing interference between robots preferably includes means for outputting a determination result signal of the interference determination means to the outside.

Furthermore, it is desirable to preset shape position data of peripheral devices that may interfere, and the interference determining means also determines interference between the robot and the peripheral devices.

In the present invention, because the presence or absence of interference is determined based on the predetermined position that the robot will reach when the stop command is output, when it is determined that there is interference, an Stop command, so interference can be accurately prevented, and when there is no interference, it will not be judged as interference by mistake. When robots cross each other at high speed, or in scenes where they are far away from each other, it can be correctly judged even when it was judged to be interference in the past. As a result, it is possible to ensure that the robots do not interfere with each other, and at the same time, it is possible to use the robot in a close state compared with conventional interference prevention methods.

Description of drawings

Hereinafter, the above-mentioned and other objects, features, and advantages of the present invention will be described in more detail based on preferred embodiments of the present invention with reference to the accompanying drawings. In the attached picture,

FIG. 1 is an explanatory diagram of a trajectory of a robot.

FIG. 2 is a diagram showing the relationship between time t and travel distance s.

3A and B are explanatory diagrams of the B-Spline curve when N=6.

4A to 4C are graphs illustrating deceleration speeds based on a stop command in one embodiment of the present invention.

5A to 5C are explanatory views of a method of obtaining a deceleration function.

6A and B are explanatory diagrams of a method of detecting interference between robots.

7A and B are flowcharts of interference detection and interference prevention control processing.

Fig. 8 is a flowchart showing processing continuing the flow shown in Fig. 7B.

FIG. 9 is a flowchart showing processing when outputting the result of interference detection to an external device.

Fig. 10 is a system configuration diagram showing an embodiment of the system according to the present invention.

FIG. 11 is a system configuration diagram of another embodiment to which the present invention is applied.

FIG. 12 is a system configuration diagram of another embodiment to which the present invention is applied.

Fig. 13 is a block diagram of an interference prevention control device between robots according to the present invention.

Detailed ways

Hereinafter, several embodiments of the present invention will be described with reference to the drawings.

First, the working principle of the present invention will be described. The present invention is an invention for preventing interference between robots, etc. when a plurality of robots are used for cooperative work. The interference prevention control device between robots based on the present invention, as shown in FIG. 13 , has a stop position calculation unit 12, an interference A determination unit 14 , a deceleration command unit 16 , and a signal output unit 18 that outputs a determination result signal of the interference determination unit 14 to the outside. The stop position calculation unit 12 calculates, for each robot, the position at which the robot decelerates and stops when a stop command is output at a certain moment during operation. The interference judging unit 14 judges that there is no interference if there is no interference with other robots, peripheral devices, etc. at the stop position. This interference judgment is performed for each interpolation process, and when it is judged to be interference, the deceleration instruction unit 16 immediately instructs the robot to stop, and decelerates it to a stop. However, in fact, after it is determined that interference will occur at the stop position, even if the deceleration is started, interference will occur at the stop position, so the stop position calculation unit 12 pre-executes the calculation of the stop position for one interpolation cycle or more. . That is, the stop position calculation unit 12, when executing a certain interpolation process, first reads the working program of each robot, and calculates the stop position in advance, which is preset from the current interpolation process to be executed. The position to stop when the interpolation processing after the interpolation period is started and the stop operation is started. The interference judging unit 14 judges that there is no interference if there is no interference with other robots, peripheral equipment, etc. at the stop position, and outputs an operation command to the current interpolation position. On the other hand, when it is judged that an interference will occur, since it is known that the interpolation process is delayed by a predetermined number of unit interpolation cycles from the current point in time and the stop process is started, the deceleration command unit 16 starts from the current interpolation process. When the processing starts to stop. As a result, interference is prevented from occurring in the first place.

For illustration, as shown in FIG. 1 , the function X(s) is used here to represent the trajectories of each robot. In addition, the vector value of the function X(s) (hereinafter referred to as the value of the function X(s)) is a value representing a 6-dimensional vector capable of specifying a position and an orientation in a three-dimensional space. Also, the parameter s of the function X is the distance (move amount) along the trajectory. For example, when considering a curve with a total length of L from the starting point to the end point at a specified time T, the starting point can be expressed as time t=0, the position s=0 on the curve, and the end point can be expressed as t=T, on the curve The position s = L.

FIG. 2 shows the relationship between the time t and the position (movement amount) s on the curve. The function s(t) is a monotonically increasing function representing the movement of the robot along the trajectory. The derivative function s′(t) of t with respect to s corresponds to the linear velocity at time t, and the second-order derivative function s″(t) corresponds to the acceleration. Assume that the working linear velocity and acceleration of the robot are continuous, and the function s(t) is a second-order differential continuous function. If the composite function X(s) and function s(t), the position and posture of the robot at time t are also expressed as X(s(t)). And , at time t0, output the deceleration stop command, if the braking distance at that time is L_stop, the braking time is T_stop, then the stop position is X(s(t0)+L_stop). Alternatively, the time (t0+T_stop) can be used The stop position X(s(t0+T_stop)) is easily obtained. In this way, the stop position is obtained for each robot, and interference detection is performed based on the obtained stop position. And, when it is judged that interference will occur, the output deceleration In addition, the teaching program is read in advance, and the stop position X ( s(t0)+L_stop) or X(s(t0+T_stop)), when it is judged that interference will occur, a deceleration and stop command is output at the current time point, thereby preventing interference from occurring.

Thus, in the present invention, the teaching program is read in advance, and the function X(s) representing the trajectory of the robot is obtained in advance. Generally _, Bezier curve _{interpolation , spline} (Spline) curve interpolation, non-uniform rational B-spline (NURBS) curve interpolation and other methods. In one embodiment of the present invention, a 4th order (3 times) B-Spline is used, according to the sequence of teaching points P ₀ , P ₁ , P ₂ , ...P _{N- 1} , get the function X(t) representing the position and posture at time t.

If given N point columns P ₀ , P ₁ , P ₂ ,...P _N-1 and node vector T=[t ₀ , t ₁ , t ₂ ,...t _N+3 ], then the 4th-order B-Spline curve , according to the function N _j at time t, the linear sum of 4(t) (j=0, 1, . . . N-1) is defined as in equation (1).

$\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\underset{\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 0</span>}}{\overset{\mathrm{<span\; class="notranslate">\; N</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}{\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}{\mathrm{<span\; class="notranslate">\; N</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; 4</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; ,</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="3"\; label="t"\; filenames="A20051009824500191.png"\; state="\{\{state\}\}">t</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; \&le;</span>\mathrm{<span\; class="notranslate">\; t</span>}{<span\; class="notranslate">\; <</span>\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; N</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>$

Here, N _{j, M} (t) is an M-order B-Spline basis function, which is generated by the De Boor Cox recurrence formula of Equations (2-1) and (2-2).

When M≥2,

${\mathrm{<span\; class="notranslate">\; N</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}}{{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}}{\mathrm{<span\; class="notranslate">\; N</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\frac{{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; t</span>}}{{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; m</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}{\mathrm{<span\; class="notranslate">\; N</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&CenterDot;</span><span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>$

Among them, 0/0=0 is set.

When M=1

If t _j ≤t<t _j+1 , then

${\mathrm{<span\; class="notranslate">\; N</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\left\{\begin{array}{cc}\mathrm{<span\; class="notranslate">\; 1</span>}& <span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; )</span>\\ \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; \&NotEqual;</span>\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; )</span>\end{array}\right.<span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span>$

The node vector T=[t ₀ , t ₁ , t ₂ , ... t _N+3 ] is an arbitrary monotonically increasing sequence composed of N+M numbers. Even if the given teaching point series P ₀ , P ₁ , P ₂ , ... P _N-1 are the same, although it is known that the shape of the vector curve changes by defining the node, but here for simplicity, the node is defined as in formula (3) Element t ₁ of the vector. The curve X(t) of the formula (1) can be defined within the range of 0≤t<N-3 by dividing t ₃ ≤t<t _N by three nodes from the start point side and the end point side respectively. Although t takes an integer value at a node, it takes a real value at other points.

t ₁ =i-3 (i=0, 1, 2, 3, ... N, N+1, N+2, N+3) ... (3)

3A and B show an example where N=6.

At this time, the B-Spline curve is defined by the following equation.

X(t)=N _0,4 (t)P ₀ +N _1,4 (t)P ₁ +N _2,4 (t)P ₂ +N _3,4 (t)P ₃ +N _4,4 ( t)P ₄ +N _5,4 (t)P ₅

Where, T=[t ₀ t ₁ t ₂ t ₃ t ₄ t ₅ t ₆ t ₇ t ₈ t ₉ ]

=[-3-2-10123456]

As can be seen from equations (2-1), (2-2) and FIG. 3B , the fourth-order B-Spline basis function N _j,4 (t) is 0 except for the range of node 4. If this property is used, it is not necessary to give all the teaching points P ₀ , P ₁ , P ₂ ,...P _N-1 in advance when calculating formula (1). From the first 4 points, the initial curve interval t ₃ ≤ t<t ₄ , that is, the following formula (4) is valid when 0≤t<1.

$\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\underset{\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 0</span>}}{\overset{\mathrm{<span\; class="notranslate">\; 8</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}{\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}{\mathrm{<span\; class="notranslate">\; N</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; 4</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; ,</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="3"\; label="t"\; filenames="A20051009824500191.png"\; state="\{\{state\}\}">t</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; \&le;</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; <</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; 4</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 4</span>}<span\; class="notranslate">\; )</span>$

In general, in order to obtain an effective function in a certain interval t _a ≤ t<t _b , the minimum node interval containing it can be obtained, and the sum can be obtained by using only the necessary items of the curve defining its range.

By first reading the teaching program, through the formula (4), obtain the function X(t) representing the smooth curve passing through the vicinity of the 4 teaching points from the 4 teaching points, along the curve, the function X (t) Line integration represented by the following formula (5) is performed to obtain a function s(t) of the trajectory of the robot expressed as a function of the position (movement amount) s on the curve.

$\mathrm{<span\; class="notranslate">\; the\; s</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span><span\; class="notranslate">\; \&Integral;</span>\sqrt{\frac{\mathrm{<span\; class="notranslate">\; dx</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>}{\mathrm{<span\; class="notranslate">\; dt</span>}}<span\; class="notranslate">\; \&Center\; Dot;</span>\frac{\mathrm{<span\; class="notranslate">\; dx</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>}{\mathrm{<span\; class="notranslate">\; dt</span>}}}\mathrm{<span\; class="notranslate">\; dt</span>}<span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 5</span>}<span\; class="notranslate">\; )</span>$

Since the physical meaning of this function s(t) is the moving distance along the curve up to time t, conversely, given the distance s, the corresponding time t is uniquely determined. If the function of time t at a distance s is expressed as t(s), then as a composite function of functions X(t) and t(s), the relationship between s and X is obtained, that is, the function X(s) is as the following formula (6) Get it.

X(s)≡X(t(s)) (6)

In addition, since the position on the curve of the robot at time t is represented by s(t), and the position and posture when the position on the curve is s is represented by X(s), the position and posture of the robot at time t are represented by X (s(t)) can be obtained by calculation.

Next, the procedure for calculating the planned stop position when the robot is decelerated and stopped along the trajectory from a certain time t0 will be described. Assume that the robot is working at position s(t0) and linear velocity s'(t0) at time t0. The curve of the derivative function s'(t) of s(t) at this time is shown in FIG. 4A. When the deceleration has not started from the time t0, the operation is continued at the linear velocity shown in FIG. 4A after the predetermined time t0. The speed graph at the time of deceleration and stop is obtained by multiplying the speed graph in FIG. 4A by a function u(t-t0) that decreases smoothly and monotonically from 1 to 0 as shown in FIG. 4B.

Fig. 4C shows the result of multiplying the velocity s'(t) by the deceleration function u(t). The braking time T_stop necessary for deceleration is the width of the transition time of the deceleration function u(t) (Figure 4B), and the braking distance L_stop is the interval integral from the start deceleration time t0 to the stop time t0+T_stop in Figure 4C value, the function s(t) can be obtained by formula (5). Therefore, if u(t) is preset, the braking time T_stop and the braking distance L_stop can be calculated together.

In this way, when decelerating from time t0, the predetermined position where the robot is to stop and the posture X (s(t0)+L_stop) at the position can be obtained by calculation. Here, the function u(t) in FIG. 4B is arbitrary, and the graph in FIG. 4C as a multiplication result may be arbitrary as long as it is first-order differential continuous (acceleration continuous). As an example, by integrating the function w(t) (a function determined by T_stop) that inverts the triangle shown in Figure 5A (an isosceles triangle with a width of T_stop and a height of 2/T_stop), it is obtained as shown in Figure 5B The deceleration function u(t) of (wherein, u(0)=1). In addition, at this time, a deceleration function (t-t0) in which the falling start time of u(t) is set to t0 is shown in FIG. 5C. In addition, the braking time width T_stop is constant here for simplicity, but in more general cases, in order to make the acceleration of the motor constant during deceleration, it can also be based on the inertia or gravity of the motor at that time. Appropriate value obtained for the momentum on the motor.

Through the above method, when the start deceleration time t0 is t+n·Δt (at this time t means the moment of the current time point, t+n·Δt means the moment after the interpolation cycle from the current time point), the stop is scheduled The position and orientation of are obtained by calculating X(s(t+n·Δt)+L_stop). Here, n is a constant of 1 or more. In addition, Δt is a unit interpolation processing time (unit interpolation cycle).

In addition, in the calculation of the above-mentioned stop position, the time t+n. The position X (s(t+n·Δt+T_stop)) obtained from the value of the function of Δt+T_stop is regarded as the stop position.

As described above, for all the robots that are the subject of interference detection, at time t, at the time of interpolation processing (t+n·Δt) after n interpolation cycles from this time, it is judged that the deceleration is output at the current time t. The scheduled stop position at the time of the stop command. Then, it is judged whether or not interference occurs based on the planned stop positions of the respective robots.

As the interference detection, a well-known interference detection method described in JP-A-60-99591 was used. 6A and B show the procedure of judging the interference between the robots R1 and R2 constituted by a plurality of connections according to this interference detection method. Use line segments to model the connections of each robot. The robot R1 is composed of line segments L11 and L12, and the robot R2 is composed of line segments L21 and L22. For all the pairs of the line segments constituting the robot R1 and the line segments constituting the robot R2 , the distance d between the closest contact points between the line segments is obtained. When the obtained distance d is smaller than a preset threshold value, it is judged as interference. Other than that, it was judged as non-interference.

When it is judged that there is no interference, it outputs a work command to the interpolation position at the current time t. When it is judged that there is interference, it outputs a deceleration and stop command from the current interpolation cycle to stop the robot and prevent interference.

10 to 12 are diagrams showing an embodiment of a system for performing interference prevention control between robots according to the present invention. In the embodiment shown in FIG. 10 , a robot control device 1 is used to drive and control a plurality of robot mechanism parts R1 to Rn, and the robot control device 1 also has the function of the robot interference control device of the present invention.

In the embodiment shown in FIG. 11 , each robot R1 to Rn has robot control devices 1-1 to 1-n, respectively, and each robot control device 1-1 to 1-n is connected using a network such as Ethernet (registered trademark). By communicating mutual internal variables over the network, the calculation for obtaining the planned stop position and the interference determination for determining the presence or absence of interference can be performed in the respective robot controllers. Calculation for obtaining the planned stop position and determination of the presence or absence of interference can be collectively performed using one control device.

FIG. 12 is an embodiment in which an external computer 2 such as a general-purpose microcomputer can be used. In this embodiment, by connecting the computer 2 to the robot control devices 1-1 to 1-n and communicating with the computer 2 the internal variables of the respective robot control devices 1-1 to 1-n, the computer 2 can be used to obtain the above-mentioned stop. A part or all of the calculation of the predetermined position and the interference determination for determining the presence or absence of interference.

7A, 7B, and 8 are flowcharts of control processing for preventing interference between robots, and show processing in the embodiment shown in FIG. 10 .

First, a robot (robot mechanism unit) that requires interference detection is set in the robot controller 1 , and data on the position and shape of peripheral devices to be detected for interference is input and set. In addition, the control time T_stop is also set. Then, when the robot is started to operate, the processor of the robot control device 1 executes the interference detection and interference prevention control processes shown in FIGS. 7A , 7B, and 8 .

Set the time t to "0" (step S1), set the time t at the start time ta of the range of the B-spline curve obtained from the teaching point row, and set the time t at the end time tb of the range (t+ n·Δt+T_stop) (step S2). In addition, n is an integer of 1 or more, and is a predetermined number of interpolation cycles designated for detecting whether interference has occurred after n interpolation cycles from the current time point. Alternatively, n can be set before the operation starts.

Next, the node t _A that is at or below the range start time point ta and that is the largest on the B-spline curve is obtained, and the node t _B that is the smallest node at or above the range start time point tb or tb is obtained. Also, t _A =t ₃ when there is no node at or below the range start time point ta. Also, when there is no node at or beyond the range start time point tb, the maximum node t _N is set to t _B (step S3).

Set "1" in the counter K for counting the robot (robot mechanism part) set as the detection object (step S4), and the robot indicated by the value of the counter K reads the nodes t _{A to A} from the program being executed. The teaching data P _j between t _B (step S5). In addition, as shown in the formula (3), the nodes t _A and t _B are integer values, and since these values correspond to the subscripts A and B of t _A and t _B , the subscripts A and B are used to indicate the values of the nodes. Therefore, the teaching points P _j read out from the program are P _A-3 , P _{A-2 .} . . P _B-1 .

Then, the curve X(t) between the interval t _A ≤ t < t _B is obtained from the following formula (7) corresponding to the formula (4) (step S6 ).

$\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\underset{\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; A</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 3</span>}}{\overset{\mathrm{<span\; class="notranslate">\; B</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}{\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}{\mathrm{<span\; class="notranslate">\; N</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; 4</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; ,</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; A</span>}}<span\; class="notranslate">\; \&le;</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; <</span>{\mathrm{<span\; class="notranslate">\; t</span>}}_{\mathrm{<span\; class="notranslate">\; B</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 7</span>}<span\; class="notranslate">\; )</span>$

The position s(t) on the curve is obtained by line-integrating the curve X(t) obtained by the formula (5). The position s(t) on the curve is the moving distance along the curve up to the time t, so, conversely, if the distance s is given, the corresponding time t is uniquely determined. Therefore, the inverse function t(s) is obtained from the position function s(t) (step S7).

From the functions X(t) and t(s) of the curve obtained in this way, the function X(s)=X(t(s) representing the position and posture of the robot with the position (movement distance) s as a variable is obtained. ) (step S8). And, the position on the curve of the robot at time t is s(t) obtained in step S7, because when the position on the curve is s, the position and posture of the robot are represented by the function X(s), so in The position and posture of the robot at time t are obtained as X(s(t)) (step S9).

Find the linear velocity s'(t) which is the derivative function of the function s(t) obtained in step S7, and use the time after n interpolation cycles preset from the current time t as t0 (t0=the current time t+n·Δt), obtain the function u(t-t0), multiply the derivative function s'(t) by the function u(t-t0), and obtain the linear velocity representing the decelerated speed according to the following formula (8) v _t0 '(t) (step S10).

V _t0 '(t)=s(t)×u(t-t0) (8)

The linear velocity v _t0 '(t) is integrated from the deceleration start time t0 to the braking time T_stop to obtain the braking distance L_stop (step S11).

From the L_stop obtained in step S11, the s(t) obtained in step S7, and the function X(s) obtained in step S8, the interpolation cycle at (time t)n from the current interpolation process is obtained. The planned stop position X(s(t0)+L_stop)=X(s(t+n·Δt)+L_stop) at the time of deceleration and stop at a later time t0=t+n·Δt is stored (step S12).

Next, the value of the counter K is incremented by 1 (step S13), and it is judged whether the value of the counter K exceeds the number of robots set as detection objects (step S14). And, if not exceeding, then return to step S5, carry out the following processing of described step S5 to the next robot represented by the same number as the value of counter K, find out when from the current interpolation processing (time t) Scheduled stop position X (s(t+n·Δt)+L_stop) when deceleration stop is started at time t0=t+n·Δt after n interpolation cycles.

Next, when the scheduled stop position X(s(t+n·Δt)+L_stop) of the robot to be detected is obtained, interference detection between robots and interference detection with peripheral equipment and the like are performed (step S15 ). As mentioned above, this interference detection is based on a known method, and description thereof is omitted here.

When it is determined that there is no interference by the interference detection (step S16), an operation command is output to each robot to the interpolation position X(s(t)) at the time t obtained in step S9 by each robot (step S17) . And, it is judged whether the time t arrives at the last time _tN (step S18), if not, then at the time t, only the unit interpolation cycle Δt is added (step S19), returns to step S2, and carries out the steps below the above-mentioned step S2 deal with. Hereinafter, as long as the occurrence of interference is not predicted, the above-mentioned processing is repeatedly executed until the time t reaches the final time t _N , and the interference prevention process ends when the final time t _N is reached.

On the other hand, when it is judged that there is interference at step S16, the current time t is set as an initial value at tt representing the time of deceleration (step S20), and the speed at the time of deceleration operation obtained according to step S11 is The function V _t0 '(t) of , integrates the function V t '(tt) representing the speed obtained by taking the start of deceleration as t and the time variable as tt, and obtains the movement amount _{V t (} tt) (step S21) . Next, based on the function X(s) obtained in step S8, as s=V _t (tt), the position and posture X(V _t (tt)) of the robot are obtained, and an operation command is output to the robot so that the The obtained position and posture (step S22).

And, whether the value of judging time tt has reached the value (step S23) that t and braking time T_stop are added when starting to decelerate, if not, then time tt is added to calculation unit interpolation cycle Δt (step S24) . And, return to step S22, until the value of the time tt reaches the value obtained by adding the deceleration start time t to the braking time T_stop, the robot stops, and the above-mentioned processing is repeatedly executed.

As described above, when it is predicted that interference will occur only n interpolation periods predetermined from the current time, the occurrence of interference can be prevented by decelerating from the current time point.

In addition, in the above-mentioned embodiment, although the planned stop position is obtained in step S12 based on s and the position (movement amount) of the braking distance L_stop, the planned stop position can be easily obtained from time. That is, the scheduled stop position X(s(t0+T_stop))=X(s(t+n·Δt+T_stop)) can be obtained in step S12.

Furthermore, in addition to the above-mentioned interference prevention control processing, the signal output processing shown in FIG. 9 may be performed to output the determination result of interference detection to peripheral devices. For example, when it is determined in step S16 that there is interference, a signal of interference may be output to an external device, and the process may proceed to step S20.

In the embodiment described above, the system configuration shown in FIG. 10 is used to control a plurality of robots (robot mechanism units) R1 to Rn by one robot control device 1, and this robot control device 1 constitutes an interference prevention control device. On the other hand, when the system configuration in which each robot shown in FIG. 11 has its control devices 1-1 to 1-n respectively, although the processors of the robot control devices 1-1 to 1-n perform the above-mentioned processing in FIG. 7A , 7B, and the processing shown in FIG. 8, but wherein the processing of steps S4, S13, and S14 is not performed, and before step S15, each robot controller 1-1 to 1-n exchanges the obtained planned stop positions. X(s(t0)+L_stop)=X(s(t+n·Δt)+L_stop) or X(s(t0+T_stop))=X(s(t+n·Δt+T_stop)), carry out the steps S15 interference detection. Or, collect the planned stop position of the robot to be detected by the interference detection on any robot control device through the network, and perform interference detection through the robot control device. Work.

In addition, in the case of the system shown in FIG. 12, each robot controller 1-1 to 1-n communicates the internal variables of each robot controller 1-1 to 1-n through a network, thereby using The computer 2 of the computer performs the calculation of the planned stop position and the determination of whether there is interference. In addition, the processing of FIGS. 7A , 7B and 8 described above can all be performed by the computer 2 .

Claims (6)

1. A control device (10) for preventing interference between robots, which detects the proximity of a plurality of robots installed in a state where their work areas overlap each other during work, and stops the robots before reaching the interference, It is characterized in that it has: The stop position calculation unit (12) reads in advance the teaching program being executed by each robot to be the object, and calculates the position at least one interpolation cycle or one interpolation cycle preset from the current time for each interpolation cycle. The scheduled stop position when the stop command to each robot is executed at a time after the supplementary cycle or more; an interference judging unit (14), which judges whether there is interference between the robots at each of the obtained planned stop positions; A deceleration instruction unit (16) that instructs the robot to start deceleration when the interference determination unit determines that there is interference. 2. The interference prevention control device between robots according to claim 1, wherein: The stop position calculation unit (12) reads in advance the teaching program being executed by each target robot, expresses the position and posture of each robot using a function at time t, and uses this function to calculate each interpolation The interpolation period is to obtain a planned stop position when a stop command to each of the robots is executed at a time after at least one interpolation period or more preset from the current time. 3. The interference prevention control device between robots according to claim 2, wherein: The stop position calculation unit (12) calculates the scheduled stop position according to the value of the function, the value of the function is at least 1 interpolation cycle or more after the preset time from the current time The function value at the time after the time necessary to stop has elapsed. 4. The interference prevention control device between robots according to claim 2 or 3, wherein: The position and orientation of the robot is expressed using the linear sum of the functions at time t. 5. The interference prevention control device between robots according to claim 1 or 2, wherein: A unit (18) is provided for outputting a determination result signal of the interference determination unit (14) to the outside. 6. The interference prevention control device between robots according to claim 1 or 2, wherein: Shape position data of peripheral devices that may interfere may be preset, and the interference judging unit (14) also judges interference between the robot and the peripheral devices.

Images