Robot interference prevention control device

BACKGROUND OF THE INVENTION

1. Field of the Invention

0001The present invention relates to an interference prevention control device for preventing interference between robots when simultaneously operating a plurality of robots in a manner in which their operating regions partially overlap.

2. Description of the Related Art

0002When using a plurality of robots for joint work, if the operating regions of the robots overlap, it is necessary to prevent interference between the robots.

0003Document <patcit id="pcit0001" dnum="EP1366867A2"><text>EP 1 366 867 A2</text></patcit> discloses a method and a control device for avoiding a collision between a robot and obstacles, e.g. another robot. Said control device comprises means for calculating a stopping time and therewith the current position of moving robot parts in case of a stop command. Further, the control device comprises means for determining a collision between parts of the robot and other objects in a calculated trajectory as well as means for stopping the robot and/or the other objects, if a collision is predicted. The described control device is also adapted for interacting with other control devices for controlling a plurality of robots in the same working area.

0004As disclosed in <patcit id="pcit0002" dnum="JP60099591A"><text>JP-60-99591-A</text></patcit>, as a further method of checking for this interference between robots, a technique is known of expressing each link of a robot arm by a line segment, simulating robot operation, finding the distance between closest parts of two line segments, and judging that there is a possibility of corresponding robot arms interfering with each other when that distance becomes smaller than a predetermined distance D.

0005The interference check method described in <patcit id="pcit0003" dnum="JP60099591A"><text>JP-60-99591-A</text></patcit> simulates robot operation off line so as to check for interference and does not check for interference when actually driving robots. Along with the improvement in the capacity of current computers, performing similar calculations dynamically in real time has now become feasible. In this case, even if judging there is a possibility of interference immediately before that interference, it would be meaningless if the speed could not be reduced and interference ended up occurring anyway. Therefore, it is necessary to set a predetermined distance D of the above interference check to a value larger by the distance required for deceleration in advance and judge the possibility of interference earlier.

0006Further, if the robot arms would pass each other at a high speed, even if judging the smallest distance between two line segments by a distance D predetermined with the distance required for deceleration in mind, while in practice the arms would not interfere with each other, they would end up being liable to be judged as interfering.

SUMMARY OF THE INVENTION

0007Accordingly, an object of the present invention is to provide a robot interference prevention control device which checks for interference in the state of the robots being actually operated so as to prevent occurrence of interference and eliminates mistaken judgment of interference.

0008According to the present invention, there is provided a robot interference prevention control device which detects during operation the proximity among a plurality of robots placed in a state where operating regions thereof overlap and stops said robots before interference, which robot interference prevention control device includes a stop position calculating means for reading in advance a teaching program being executed by each robot covered and calculating a scheduled stop position in the case of executing a stop command for each robot after at least one predetermined interpolation period from the current time for each interpolation period; an interference judging means for judging the existence of the interference between robots at each calculated scheduled stop position; and a deceleration commanding means for commanding the robot to start deceleration when the interference judging means judges that there would be interference.

0009Preferably, the stop position calculating means reads in advance the teaching program being executed by each robot covered, expresses the position and orientation of each robot as a function of a time t, and uses the function to calculate a scheduled stop position in the case of executing a stop command for each robot after at least one predetermined interpolation period from the current time for each interpolation period.

0010In particular, preferably the stop position calculating means calculates a scheduled stop position from the value of the function at a time t+n*Δt+T_stop, wherein t means the time at the current point of time, t+n*Δt means the time after at least one interpolation period from said current time, and T_stop is the time required for stopping. For example, the position and orientation of the robot may be expressed by a linear sum of the function of the time t.

0011The robot interference prevention control device preferably further includes a means for outputting the results of judgment of the interference judging means to the outside as a signal.

0012Preferably, shape and position data of peripherals at which interference is liable to occur is set in advance, and the interference judging means further judges interference between the robots and the peripherals.

0013The robot interference prevention control device according to the present invention judges the existence of interference based on a scheduled position at which a robot would reach when outputting a stop command and, when judging that there would be interference, outputs a stop command at least one interpolation period before the position where the existence of interference is judged, so can reliably prevent interference and also will not mistakenly judge there would be interference in a case where interference would not occur. Even when robots pass each other at a high speed or when moving apart and where, in the prior art, interference would be judged, correct judgment becomes possible. Due to this, while guaranteeing that the robot would not interfere with each other, it becomes possible to use robots in a closer state than with the conventional interference prevention method.

BRIEF DESCRIPTION OF THE DRAWINGS

0014Thee above and other objects, features and the advantages of the present invention will be described below in more detail based on the preferred embodiments of the present invention with reference to the accompanying drawings, wherein: <ul id="ul0001" list-style="none" compact="compact"><li><figref idref="f0001">FIG. 1</figref> is a view for explaining a robot trajectory;</li><li><figref idref="f0001">FIG. 2</figref> is a graph showing a relationship between a time t and a movement distance s;</li><li><figref idref="f0002">FIGS. 3A and 3B</figref> are views for explaining a B-spline curve in the case where N=6;</li><li><figref idref="f0003">FIGS. 4A to 4C</figref> are graphs for explaining a deceleration speed resulting from a stop command in one embodiment of the present invention;</li><li><figref idref="f0004">FIGS. 5A to 5C</figref> are views for explaining a method of obtaining a deceleration function;</li><li><figref idref="f0005">FIGS. 6A and 6B</figref> are views for explaining a method of checking for interference between robots;</li><li><figref idref="f0006">FIGS. 7A</figref> and <figref idref="f0007">7B</figref> are flow charts of an interference check and interference prevention control processing;</li><li><figref idref="f0008">FIG. 8</figref> is a flow chart of processing after the flow chart of <figref idref="f0007">FIG. 7B</figref>;</li><li><figref idref="f0009">FIG. 9</figref> is a flow chart of processing when outputting results of an interference check to an external device;</li><li><figref idref="f0009">FIG. 10</figref> is a view of the system configuration showing an embodiment of a system according to the present invention;</li><li><figref idref="f0010">FIG. 11</figref> is a view of the system configuration of another embodiment to which the present invention is applied;</li><li><figref idref="f0010">FIG. 12</figref> is a view of the system configuration of still another embodiment to which the present invention is applied; and</li><li><figref idref="f0011">FIG. 13</figref> is a block diagram of a robot interference prevention control device according to the present invention.</li></ul>

DETAILED DESCRIPTION

0015Several embodiments of the present invention will be described below with reference to the drawings.

0016First, the principle of operation of the present invention will be described. The present invention is intended for preventing interference among robots etc. when using a plurality of robots to perform joint work. A robot interference prevention control device according to the present invention, as shown in <figref idref="f0011">FIG. 13</figref>, is provided with a stop position calculating means 12, an interference judging means 14, a deceleration commanding means 16, and a signal outputting means 18 for outputting the results of judgment of the interference judging means 14 to the outside as a signal. The stop position calculating means 12 calculates a position where a robot would decelerate to and stop at when a stop command is output to each robot at a certain instant during operation of the robot. The interference judging means 14 judges that no interference will be caused if there would be no interference with other robots, peripherals, etc. at that stop position. This judgment of interference is performed for every interpolation processing. When it is judged that there would be interference, the deceleration commanding means 16 immediately commands the robot to stop to make it decelerate and stop. However, in practice, even if starting deceleration after it is judged that there would be interference at a stop position, there would be interference at the stop position, so the stop position calculating means 12 calculates the stop position in advance by at least one interpolation period. That is, the stop position calculating means 12 reads in advance the operating program of each robot when executing a certain interpolation processing and calculates in advance the position where the robot will stop when starting a stop operation from the time of interpolation processing a preset number of unit interpolation periods after the interpolation processing currently to be executed. The interference judging means 14 judges that no interference will be caused if there would be no interference with other robots, peripherals, etc. at that stop position and outputs an operation command moving to the current interpolated position. On the other hand, when judging that there would be interference, since it is found that interference would end up occurring even if starting the stop processing delayed by a predetermined number of unit interpolation periods from the interpolation processing of the current point, the deceleration commanding means 16 starts the stop processing from the time of the current interpolation processing. Due to this, the occurrence of interference can be prevented in advance.

0017For explanation, here, as shown in <figref idref="f0001">FIG. 1</figref>, assume that the trajectory of each robot is expressed as a function X(s). Further, assume that the vector value of the function X(s) (hereinafter referred to as the "value of the function X(s)") expresses a six-dimensional vector able to specify the position and orientation in a three-dimensional space. Further, assume the parameter s of the function X is the distance along the trajectory (amount of movement). For example, if considering the case of movement from the starting point to the end point of a curve with a total length of L in the required time T, the starting point can be expressed by the time t=0 and the position on the curve s=0, while the end point can be expressed by the time t=T and the position on the curve s=L.

0018<figref idref="f0001">FIG. 2</figref> shows a relationship between the time t and the position on the curve s (amount of movement). The function s(t) is a monotonously increasing function and expresses movement of a robot along a trajectory. The derivative s'(t) of s for t corresponds to the linear speed at the time t, while the second derivative s"(t) corresponds to the acceleration. Assuming that the movement of a robot is continuous in terms of both linear speed and acceleration and that the function s(t) is a function the second derivative of which is a continuous function. If combining the function X(s) and function s(t), the position and orientation of a robot at the time t can be expressed as X(s(t)). Further, if outputting a deceleration stop command at a certain time t0, designating the braking distance in that case as L_stop, and designating the braking time in that case as T_stop, the stop position becomes X (s(t0)+L_stop). Alternatively, the time (t0+T_stop) may be used to simply calculate the stop position X(s(t0+T_stop)) . In this way, the stop position for each robot is calculated and interference is checked for based on the calculated stop positions. When it is judged that interference would occur, a deceleration stop command is made to be output. Further, the teaching program is read in advance, the stop position X(s(t0)+L_stop) or X(s(t0+T_stop)) when assuming that a deceleration stop command is output at the time of interpolation processing executed after the elapse of a predetermined number of unit interpolation periods from the current interpolation processing is calculated, and, when judging that there would be interference, a deceleration stop command is output at the current time to thereby prevent the occurrence of interference.

0019In this way, according to the present invention, the teaching program is read in advance and a function X(s) expressing the trajectory of the robot is obtained in advance. In general, as methods for obtaining a smooth curve passing close to the given string of teaching points P₀, P₁, P₂,... P_N-1, Bezier curve interpolation, spline interpolation, nonuniform rational B spline (NURBS) interpolation, and other methods are known. In one embodiment of the present invention, a fourth order (third degree) B-Spline is used to obtain a function X(t) expressing the position and orientation at a time t from a string of teaching points P₀, P₁, P₂, . . . P_N-1 sequentially supplied by the operating program being executed.

0020If a string of N number of points P₀, P₁, P₂, ... P_N-1 and a knot vector T=[t₀,t₁,t₂, ... t_N+3] are given, the fourth order B-Spline curve is defined as shown in equation (1) by the linear sum of the function N_j,4(t) (j=0,1,... N-1) of the time t. <maths id="math0001" num="(1)"><math display="block"><mi mathvariant="normal">X</mi><mfenced><mi mathvariant="normal">t</mi></mfenced><mo>=</mo><mstyle displaystyle="true"><munderover><mo>∑</mo><mrow><mi mathvariant="normal">j</mi><mo>=</mo><mn>0</mn></mrow><mrow><mi mathvariant="normal">N</mi><mo>−</mo><mn>1</mn></mrow></munderover><msub><mi mathvariant="normal">P</mi><mi mathvariant="normal">j</mi></msub><mo>⁢</mo><msub><mi mathvariant="normal">N</mi><mrow><mi mathvariant="normal">j</mi><mo>,</mo><mn>4</mn></mrow></msub></mstyle><mfenced><mi mathvariant="normal">t</mi></mfenced><mspace width="2em" /><mfenced><msub><mi mathvariant="normal">t</mi><mn>3</mn></msub><mo>≤</mo><mi mathvariant="normal">t</mi><mo>&lt;</mo><msub><mi mathvariant="normal">t</mi><mi mathvariant="normal">N</mi></msub></mfenced></math><img file="EP1632318B1_D0001.tif" /></maths> where N_j,_M(t) is an Mth order B-Spline basis function which is generated from De Boor Cox recurrence formulas of equations (2-1) and (2-2). When M ≥ 2 <maths id="math0002" num="(2-1)"><math display="block"><msub><mi mathvariant="normal">N</mi><mrow><mi mathvariant="normal">j</mi><mo>,</mo><mi mathvariant="normal">M</mi></mrow></msub><mfenced><mi mathvariant="normal">t</mi></mfenced><mo>=</mo><mfrac><mrow><mi mathvariant="normal">t</mi><mo>−</mo><msub><mi mathvariant="normal">t</mi><mi mathvariant="normal">j</mi></msub></mrow><mrow><msub><mi mathvariant="normal">t</mi><mrow><mi mathvariant="normal">j</mi><mo>+</mo><mi mathvariant="normal">M</mi><mo>−</mo><mn>1</mn></mrow></msub><mo>−</mo><msub><mi mathvariant="normal">t</mi><mi mathvariant="normal">j</mi></msub></mrow></mfrac><mo>⁢</mo><msub><mi mathvariant="normal">N</mi><mrow><mi mathvariant="normal">j</mi><mo>,</mo><mi mathvariant="normal">M</mi><mo>−</mo><mn>1</mn></mrow></msub><mfenced><mi mathvariant="normal">t</mi></mfenced><mo>+</mo><mfrac><mrow><msub><mi mathvariant="normal">t</mi><mrow><mi mathvariant="normal">j</mi><mo>+</mo><mi mathvariant="normal">M</mi></mrow></msub><mo>−</mo><mi mathvariant="normal">t</mi></mrow><mrow><msub><mi mathvariant="normal">t</mi><mrow><mi mathvariant="normal">j</mi><mo>+</mo><mi mathvariant="normal">M</mi></mrow></msub><mo>−</mo><msub><mi mathvariant="normal">t</mi><mrow><mi mathvariant="normal">j</mi><mo>+</mo><mn>1</mn></mrow></msub></mrow></mfrac><mo>⁢</mo><msub><mi mathvariant="normal">N</mi><mrow><mi mathvariant="normal">j</mi><mo>+</mo><mn>1</mn><mo>,</mo><mi mathvariant="normal">M</mi><mo>−</mo><mn>1</mn></mrow></msub><mfenced><mi mathvariant="normal">t</mi></mfenced></math><img file="EP1632318B1_D0002.tif" /></maths> where, 0/0 = 0.

0021When M = 1

0022If t_j ≤ t &lt; t_j+1, <maths id="math0003" num="(2-2)"><math display="block"><msub><mi mathvariant="normal">N</mi><mrow><mi mathvariant="normal">i</mi><mo>,</mo><mn>1</mn></mrow></msub><mfenced><mi mathvariant="normal">t</mi></mfenced><mo>=</mo><mrow><mo>{</mo><mtable columnalign="left"><mtr><mtd><mn>1</mn><mi> </mi><mfenced><mi mathvariant="normal">i</mi><mo>=</mo><mi mathvariant="normal">j</mi></mfenced></mtd></mtr><mtr><mtd><mn>0</mn><mi> </mi><mfenced><mi mathvariant="normal">i</mi><mo>≠</mo><mi mathvariant="normal">j</mi></mfenced></mtd></mtr></mtable></mrow></math><img file="EP1632318B1_D0003.tif" /></maths>

0023The knot vector T= [t₀, t₁, t₂, ... t_N+3] is any monotonously increasing numerical sequence comprised of N+M number of numbers. Even if the given string of teaching points P₀, P₁, P₂, ... P_N-1 is the same, it is known that the shape of the curve will differ depending on the definition of the knot vector. Here, however, for simplification, the elements t_i of the knot vector are defined as in equation (3). The curve X(t) of equation (1) is defined in t₃≤t&lt;t_N excluding the three knots from the starting point side and the end point side, that is, the range of 0≤t&lt;N-3. "t" is made an integer value at a knot, but is made a real value at other points. <maths id="math0004" num="(3)"><math display="block"><msub><mi mathvariant="normal">t</mi><mi mathvariant="normal">i</mi></msub><mo>=</mo><mi mathvariant="normal">i</mi><mo>-</mo><mn>3</mn><mspace width="1em" /><mfenced><mi mathvariant="normal">i</mi><mo>=</mo><mn>0</mn><mo>,</mo><mn>1</mn><mo>,</mo><mn>2</mn><mo>,</mo><mn>3</mn><mo>,</mo><mn>...</mn><mspace width="1em" /><mi mathvariant="normal">N</mi><mo>,</mo><mspace width="1em" /><mi mathvariant="normal">N</mi><mo>+</mo><mn>1</mn><mo>,</mo><mspace width="1em" /><mi mathvariant="normal">N</mi><mo>+</mo><mn>2</mn><mo>,</mo><mspace width="1em" /><mi mathvariant="normal">N</mi><mo>+</mo><mn>3</mn></mfenced></math><img file="EP1632318B1_D0004.tif" /></maths>

0024<figref idref="f0002">FIGS. 3A and 3B</figref> show an example of N=6. In this case, the B-Spline curve is defined by the following equation. <maths id="math0005"><math display="block"><mtable columnalign="left"><mtr><mtd><mi mathvariant="normal">X</mi><mfenced><mi mathvariant="normal">t</mi></mfenced></mtd><mtd><mo>=</mo><msub><mi mathvariant="normal">N</mi><mrow><mn>0</mn><mo>,</mo><mn>4</mn></mrow></msub><mfenced><mi mathvariant="normal">t</mi></mfenced><mo>⁢</mo><msub><mi mathvariant="normal">P</mi><mn>0</mn></msub><mo>+</mo><msub><mi mathvariant="normal">N</mi><mrow><mn>1</mn><mo>,</mo><mn>4</mn></mrow></msub><mfenced><mi mathvariant="normal">t</mi></mfenced><mo>⁢</mo><msub><mi mathvariant="normal">P</mi><mn>1</mn></msub><mo>+</mo><msub><mi mathvariant="normal">N</mi><mrow><mn>2</mn><mo>,</mo><mn>4</mn></mrow></msub><mfenced><mi mathvariant="normal">t</mi></mfenced><mo>⁢</mo><msub><mi mathvariant="normal">P</mi><mn>2</mn></msub><mo>+</mo><msub><mi mathvariant="normal">N</mi><mrow><mn>3</mn><mo>,</mo><mn>4</mn></mrow></msub><mfenced><mi mathvariant="normal">t</mi></mfenced><mo>⁢</mo><msub><mi mathvariant="normal">P</mi><mn>3</mn></msub></mtd></mtr><mtr><mtd><mspace width="1em" /></mtd><mtd><mo>+</mo><msub><mi mathvariant="normal">N</mi><mrow><mn>4</mn><mo>,</mo><mn>4</mn></mrow></msub><mfenced><mi mathvariant="normal">t</mi></mfenced><mo>⁢</mo><msub><mi mathvariant="normal">P</mi><mn>4</mn></msub><mo>+</mo><msub><mi mathvariant="normal">N</mi><mrow><mn>5</mn><mo>,</mo><mn>4</mn></mrow></msub><mfenced><mi mathvariant="normal">t</mi></mfenced><mo>⁢</mo><msub><mi mathvariant="normal">P</mi><mn>5</mn></msub></mtd></mtr></mtable></math><img file="EP1632318B1_D0005.tif" /></maths> where, <maths id="math0006"><math display="block"><mtable columnalign="left"><mtr><mtd><mi mathvariant="normal">T</mi></mtd><mtd><mo>=</mo><mfenced open="[" close="]"><msub><mi mathvariant="normal">t</mi><mn>0</mn></msub><mi> </mi><msub><mrow><mspace width="1em" /><mi mathvariant="normal">t</mi></mrow><mn>1</mn></msub><mspace width="1em" /><mi> </mi><msub><mi mathvariant="normal">t</mi><mn>2</mn></msub><mi> </mi><msub><mrow><mspace width="1em" /><mi mathvariant="normal">t</mi></mrow><mn>3</mn></msub><mi> </mi><msub><mrow><mspace width="1em" /><mi mathvariant="normal">t</mi></mrow><mn>4</mn></msub><mi> </mi><msub><mrow><mspace width="1em" /><mi mathvariant="normal">t</mi></mrow><mn>5</mn></msub><mspace width="1em" /><mi> </mi><msub><mi mathvariant="normal">t</mi><mn>6</mn></msub><mi> </mi><msub><mrow><mspace width="1em" /><mi mathvariant="normal">t</mi></mrow><mn>7</mn></msub><mi> </mi><mspace width="1em" /><msub><mi mathvariant="normal">t</mi><mn>8</mn></msub><mspace width="1em" /><mi> </mi><msub><mi mathvariant="normal">t</mi><mn>9</mn></msub><mi> </mi></mfenced></mtd></mtr><mtr><mtd><mspace width="1em" /></mtd><mtd><mo>=</mo><mfenced open="[" close="]"><mo>−</mo><mn>3</mn><mi> </mi><mo>−</mo><mn>2</mn><mi> </mi><mo>−</mo><mn>1</mn><mi> </mi><mspace width="1em" /><mn>0</mn><mi> </mi><mspace width="1em" /><mn>1</mn><mspace width="1em" /><mi> </mi><mn>2</mn><mi> </mi><mspace width="1em" /><mn>3</mn><mspace width="1em" /><mi> </mi><mn>4</mn><mi> </mi><mspace width="1em" /><mn>5</mn><mspace width="1em" /><mi> </mi><mn>6</mn></mfenced></mtd></mtr></mtable></math><img file="EP1632318B1_D0006.tif" /></maths>

0025As will be understood from equations (2-1) and (2-2) or <figref idref="f0002">FIG. 3B</figref>, the fourth order B-Spline basis function N_j,4(t) becomes zero except in the four-knot interval range. If utilizing this property, when calculating equation (1), there is no need to give in advance all of the teaching points P₀, P₁, P₂, ... P_N-1. From the initial four points, the following equation (4) effective in the initial curve interval t₃≤t&lt;t₄, that is, 0≤t&lt;1, is obtained. <maths id="math0007" num="(4)"><math display="block"><mi mathvariant="normal">X</mi><mfenced><mi mathvariant="normal">t</mi></mfenced><mo>=</mo><mstyle displaystyle="true"><munderover><mo>∑</mo><mrow><mi mathvariant="normal">j</mi><mo>=</mo><mn>0</mn></mrow><mn>3</mn></munderover><msub><mi mathvariant="normal">P</mi><mi mathvariant="normal">j</mi></msub><mo>⁢</mo><msub><mi mathvariant="normal">N</mi><mrow><mi mathvariant="normal">j</mi><mo>,</mo><mn>4</mn></mrow></msub><mfenced><mi mathvariant="normal">t</mi></mfenced></mstyle><mspace width="2em" /><mfenced><msub><mi mathvariant="normal">t</mi><mn>3</mn></msub><mo>≤</mo><mi mathvariant="normal">t</mi><mo>&lt;</mo><msub><mi mathvariant="normal">t</mi><mn>4</mn></msub></mfenced></math><img file="EP1632318B1_D0007.tif" /></maths>

0026In general, to obtain a function effective in a certain interval t_a≤t&lt;t_b, it is sufficient to find the smallest knot interval including this and obtain the sum for just the terms required for defining the curve of that range.

0027By reading in advance the teaching program, obtaining by equation (4) the function X(t) expressing a smooth curve passing close to the four teaching points based on the four teaching points, and performing curvilinear integration shown in the following equation (5) on the function X(t) along the curve, a function s(t) of the trajectory of the robot expressed as a function of the position s on the curve (amount of movement) is obtained <maths id="math0008" num="(5)"><math display="block"><mi mathvariant="normal">s</mi><mfenced><mi mathvariant="normal">t</mi></mfenced><mo>=</mo><mstyle displaystyle="true"><mo>∫</mo><msqrt><mfrac><mrow><mi mathvariant="normal">d</mi><mo>⁢</mo><mi mathvariant="normal">x</mi><mfenced><mi mathvariant="normal">t</mi></mfenced></mrow><mrow><mi mathvariant="normal">d</mi><mo>⁢</mo><mi mathvariant="normal">t</mi></mrow></mfrac><mo>•</mo><mfrac><mrow><mi mathvariant="normal">d</mi><mo>⁢</mo><mi mathvariant="normal">x</mi><mfenced><mi mathvariant="normal">t</mi></mfenced></mrow><mrow><mi mathvariant="normal">d</mi><mo>⁢</mo><mi mathvariant="normal">t</mi></mrow></mfrac></msqrt><mo>⁢</mo><mi mathvariant="normal">d</mi><mo>⁢</mo><mi mathvariant="normal">t</mi></mstyle></math><img file="EP1632318B1_D0008.tif" /></maths>

0028The physical meaning of the function s(t) is the movement distance along the curve until the time t, so conversely if the distance s is given, the corresponding time t can be uniquely found. If expressing the function of the time t at the distance s as t(s), the relationship between s and X as a combined function of the functions X(t) and t(s), that is, the function X(s), is obtained as the following equation (6). <maths id="math0009" num="(6)"><math display="block"><mi mathvariant="normal">X</mi><mfenced><mi mathvariant="normal">s</mi></mfenced><mo>=</mo><mi mathvariant="normal">X</mi><mfenced><mi mathvariant="normal">t</mi><mfenced><mi mathvariant="normal">s</mi></mfenced></mfenced></math><img file="EP1632318B1_D0009.tif" /></maths>

0029Further, since the position of the robot on the curve at the time t is expressed by s(t) and the position and orientation when the position on the curve is s is expressed by X(s), the position and orientation of the robot at the time t can be obtained by calculation as X(s(t)).

0030Next, the procedure for calculating the scheduled stop position in the case of decelerating and stopping a robot along a path from a certain time t0 will be described. Assume that the robot is operating at the time t0 at a position s(t0) and a linear speed s'(t0). A graph of the derived function s'(t) of s(t) at this time is shown in <figref idref="f0003">FIG. 4A</figref>. When not starting deceleration from the time t0, even after t0, the operation is planned to be continued in accordance with the linear speed such as shown in <figref idref="f0003">FIG. 4A</figref>. The speed pattern in the case of deceleration and stopping is obtained by multiplying the speed pattern of <figref idref="f0003">FIG. 4A</figref> with a function u(t-t0) smoothly monotonously decreasing from 1 to 0 as shown in <figref idref="f0003">FIG. 4B</figref>.

0031The result of multiplying the speed s'(t) with the deceleration function u(t) is shown in <figref idref="f0003">FIG. 4C</figref>. The braking time T_stop required for deceleration is the length of time of transition of the deceleration function u(t)(<figref idref="f0003">FIG. 4B</figref>) itself, the braking distance L_stop is the integrated value of the interval from the deceleration start time t0 to the stop time t0+T_stop of <figref idref="f0003">FIG. 4C</figref>, and the function s(t) is obtained by equation (5). Therefore, if u(t) is predetermined, the braking time T_stop and the braking distance L_stop can both be obtained by calculation.

0032In this way, it is possible to obtain by calculation the scheduled position at which a robot will stop when starting deceleration from the time t0 and its orientation X(s(t0)+L_stop) at that position. Here, the function u(t) of <figref idref="f0003">FIG. 4B</figref> may be any function so long as the graph of the result of multiplication shown in <figref idref="f0003">FIG. 4C</figref> is a continuous first order derivative (acceleration is a continuous function). As an example, by integrating a function w(t) inverting a triangular shape such as shown in <figref idref="f0004">FIG. 5A</figref> (isosceles triangle of width of T_stop and height of 2/T_stop) (function determined by T_stop), a deceleration function u(t) such as shown in <figref idref="f0004">FIG. 5B</figref> is obtained (where u(0)=1). Note that in this case, the deceleration function (t-t0) having a fall start time of u(t) of t0 becomes as shown in <figref idref="f0004">FIG. 5C</figref>. Further, the braking time length T_stop is made constant here for simplification, but more generally it can be made a suitable value determined in accordance with inertia of the motor at that time or the moment acting on the motor due to gravity so that the acceleration force acting on the motor at the time of deceleration becomes constant.

0033By this method, when the deceleration start time t0 is t+n·Δt (in this case, t means the time at the current point of time and t+n·Δt means the time after n number of interpolation periods from the current point of time), the position and orientation at the scheduled stop are obtained by calculation of X(s(t+n·Δt)+L_stop). Here, n is a constant of 1 or more. Further, Δt is a unit interpolation processing time (unit interpolation period).

0034Further, in the calculation of the stop position, it is also possible to deem as the stop position the position X(s(t+n·Δt+T_stop)) obtained as a value of the function at the time t+n·Δt+T_stop after the elapse of the time T_stop required for stopping from predetermined time t+n·Δt after at least one interpolation period.

0035In the above way, at the time t, the scheduled stop position when issuing a deceleration stop command at the time of interpolation processing (t+n·Δt) after n number of interpolation periods from this time t is determined at the point of time of the current time t for all robots covered by interference check. Further, it is judged whether or not interference would occur based on the scheduled stop positions of the robots.

0036For the interference check, the known interference check method described in <patcit id="pcit0004" dnum="JP60099591A"><text>JP-60-99591-A</text></patcit> is used. <figref idref="f0005">FIGS. 6A and 6B</figref> show the procedure for judgment of interface of a robot R1 and R2 comprised of a plurality of links in accordance with this interference check method. These robot links are formed into a model by line segments. The robot R1 is comprised of the line segments L11 and L12, while the robot R2 is comprised of the line segments L21 and L22. The distance d between the closest points of the line segments is determined for all pairs of the line segments forming the robot R1 and the line segments forming the robot R2. When the obtained distance d is smaller than a predetermined threshold value D, it is judged that there would be interference. Otherwise, it is judged that there would be no interference.

0037When it is judged that there would be no interference, an operation command to move each robot to the interpolated position at the current time t is output, while when it is judged that there would be interference, a deceleration stop command is output from the current interpolation period to stop the robots and prevent the occurrence of interference.

0038<figref idref="f0009 f0010">FIG. 10 to FIG. 12</figref> show embodiments of a system for control for prevention of interference between robots according to the present invention. In the embodiment shown in <figref idref="f0009">FIG. 10</figref>, a single robot control device 1 is used to control the drive operations of a plurality of robot mechanical parts R1 to Rn. The robot control device 1 is also provided with the functions of the robot interference control device according to the present invention.

0039In the embodiment shown in <figref idref="f0010">FIG. 11</figref>, the robots R1 to Rn are provided with corresponding robot control devices 1-1 to 1-n, respectively. The robot control devices 1-1 to 1-n are connected by an Ethernet® or other network and transmit their internal variables through the network so as to enable the calculations for determining the scheduled stop positions and the judgment of the existence of interference to be performed in the robot control devices. The calculations for determining the scheduled stop positions and the judgment of the existence of occurrence of interference may also be performed all together at a single control device.

0040<figref idref="f0010">FIG. 12</figref> shows an embodiment designed for use with a personal computer or other outside computer 2. In this embodiment, by connecting the computer 2 to a network of robot control devices 1-1 to 1-n and transmitting internal variables of the robot control devices 1-1 to 1-n to the computer 2, part or all of the calculations for determining the scheduled stop positions and the judgment of the existence of occurrence of interference can be performed at the computer 2.

0041<figref idref="f0006">FIGS. 7A</figref> and <figref idref="f0007">7B</figref> and <figref idref="f0008">FIG. 8</figref> are flow charts of the control processing for preventing interference between robots and show the processing at the embodiment shown in <figref idref="f0009">FIG. 10</figref>.

0042First, the robots (robot mechanical parts) for which an interference check is required is set in the robot control device 1, and data relating to the positions and shapes of the peripherals covered by the interference check is input and set in the robot control device 1. Further, the braking time T_stop is set in the robot control device 1. When starting the robot operation, the processor of the robot control device 1 executes the interference check and interference prevention control processing shown in <figref idref="f0006">FIGS. 7A</figref> and <figref idref="f0007">7B</figref> and <figref idref="f0008">FIG. 8</figref>.

0043The time t is set to "0" (step S1), the time t is set as the start time ta of the range for obtaining the B-spline curve from the string of teaching points, and "t+n·Δt+T_stop" is set as the end time tb of that range (step S2). Note that "n" is an integer number of 1 or more and designates the number of interpolation periods for checking whether or not interference would occur after n number of interpolation periods from the current point of time. It is determined in advance. The "n" may be set before the start of operation.

0044Next, the largest knot t_A at the B-spline below the starting point ta of the range and the smallest knot t_B above the end point tb of the range are determined. Note that when there is no knot below the starting point ta of the range, t_A=t₃. Further, when there is no knot above the end point tb of the range, the t_N of the biggest knot is made t_B (step S3).

0045The counter K for counting the robots (robot mechanical parts) set for coverage by the check is set to "1" (step S4), and the teaching data P_j between the knots t_A to t_B is read out from the programs being run by the robot indicated by the value of the counter K (step S5). Note that the knots t_A and t_B, as shown in equation (3), are integer values. The values correspond to the subscripts A and B of t_A and t_B, so the values of the knots are expressed by the subscripts A and B. Therefore, the teaching points P_j read out from the program become P_A-3,P_A-2,... P_B-1.

0046Next, from the following equation (7) corresponding to equation (4), the curve X(t) in the interval t_A≤t&lt;t_B, is obtained (step S6). <maths id="math0010" num="(7)"><math display="block"><mi mathvariant="normal">X</mi><mfenced><mi mathvariant="normal">t</mi></mfenced><mo>=</mo><mstyle displaystyle="true"><munderover><mo>∑</mo><mrow><mi mathvariant="normal">j</mi><mo>=</mo><mi mathvariant="normal">A</mi><mo>−</mo><mn>3</mn></mrow><mrow><mi mathvariant="normal">B</mi><mo>−</mo><mn>1</mn></mrow></munderover><msub><mi mathvariant="normal">P</mi><mi mathvariant="normal">j</mi></msub><mo>⁢</mo><msub><mi mathvariant="normal">N</mi><mrow><mi mathvariant="normal">j</mi><mo>,</mo><mn>4</mn></mrow></msub><mfenced><mi mathvariant="normal">t</mi></mfenced></mstyle><mspace width="2em" /><mfenced><msub><mi mathvariant="normal">t</mi><mi mathvariant="normal">A</mi></msub><mo>≤</mo><mi mathvariant="normal">t</mi><mo>&lt;</mo><msub><mi mathvariant="normal">t</mi><mi mathvariant="normal">B</mi></msub></mfenced></math><img file="EP1632318B1_D0010.tif" /></maths>

0047The obtained curve X(t) is linearly integrated by equation (5) to obtain the position s(t) on the curve. This position s(t) on the curve is the movement distance along the curve until 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).

0048From the thus obtained function X(t) of the curve and function t(s), a function X(s)=X(t(s)) expressing the position and orientation of the robot having the position (movement distance) s as a variable is obtained (step S8). Further, the position of the robot on the curve at the time t is the s(t) obtained at step S7. When the position on the curve is s, the robot position and orientation are expressed by the function X(s), so the robot position and orientation at the time t are obtained as X(s(t)) (step S9)<i>.</i>

0049The derived function of the function s(t) obtained at step S7, that is, the linear speed s'(t), is obtained, the function u(t-t0) is obtained designating the time after the preset n number of interpolation periods from the current time t as t0 (t0 = current time t + n·Δt), and the derived function s'(t) is multiplied by the function u(t-t0) to obtain the linear speed v_t0'(t) expressing the speed after deceleration based on the following equation (8) (step S10). <maths id="math0011" num="(8)"><math display="block"><msub><mi mathvariant="normal">v</mi><mrow><mi mathvariant="normal">t</mi><mo>⁢</mo><msup><mn>0</mn><mo>′</mo></msup></mrow></msub><mfenced><mi mathvariant="normal">t</mi></mfenced><mo>=</mo><mi mathvariant="normal">s</mi><mfenced><mi mathvariant="normal">t</mi></mfenced><mo>×</mo><mi mathvariant="normal">u</mi><mo>⁢</mo><mfenced><mi mathvariant="normal">t</mi><mo>−</mo><mi mathvariant="normal">t</mi><mo>⁢</mo><mn>0</mn></mfenced></math><img file="EP1632318B1_D0011.tif" /></maths>

0050This linear speed 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).

0051The scheduled stop position X(s(t0)+L_stop)= X(s(t+n·Δt)+L_stop) when starting deceleration for stopping at the time t0=t+n·Δt after n number of interpolation periods from the current interpolation processing time (time t) is calculated and stored from the L_stop obtained at step S11, the s(t) obtained at step S7, and the function X(s) obtained at step S8 (step S12).

0052Next, the value of the counter K is incremented by exactly 1 (step S13), and it is judged whether or not the value of the counter K has exceeded the number of robots set for coverage under the check (step S14). If not exceeding it, the procedure returns to step S5, where the processing of the above step S5 on is executed for the next robot indicated by the same number as the value of the counter K, so that the scheduled stop position X(s(t+n·Δt)+L_stop) when starting deceleration for stopping at the time t0=t+n·Δt after n number of interpolation periods from the current interpolation processing time (time t) is then obtained.

0053Next, when the scheduled stop position X (s(t+n·Δt)+L_stop) of the robot under check is determined, the interference between robots is checked and the interference with peripherals etc. is checked (step S15). This interference check, as explained above, is performed by an already known method, so its explanation will be omitted.

0054When this interference check judges that there will be no interference (step S16), an operation command to move the robot to the interpolated position X(s(t)) at the time t obtained at step S9 for each robot is output to each robot (step S17). Further, it is judged whether or not the time t has reached the final time t_N (step S18). If it has not reached it, the time t is incremented by exactly the unit interpolation period Δt (step S19), then the procedure returns to step S2 where the processing of the above step S2 on is performed. After this, in so far as the occurrence of interference is not predicted, the above processing is repeated until the time t reaches the final time t_N. When it reaches the final time t_N, this interference prevention processing is ended.

0055On the other hand, when it is judged at step S16 that there would be interference, the current time t is set as the initial value for the tt showing the time at the time of deceleration (step S20), and the function v_t' (tt) expressing the speed obtained designating the deceleration start time as t and the time variable as tt from the function v_t0' (t) expressing the speed at the time of the deceleration operation obtained at step S11 is integrated so as to obtain the amount of movement v_t(tt) (step S21). Next, based on the function X(s) obtained at step S8, the robot position and orientation X(v_t(tt)) are obtained assuming s=v_t(tt) and an operation command is output to the robot so as to bring the robot into the obtained position and orientation (step S22).

0056Further, it is judged whether or not the value of the time tt has reached the value of the deceleration start time t plus the braking time T_stop (step S23). If not reaching it, the time tt is incremented by the unit interpolation period Δt (step S24). Then, the procedure returns to step S22, where the above processing is repeated until the value of the time tt reaches the value of the deceleration start time t plus the braking time T_stop and the robot stops.

0057In this way, when it is predicted that interference will occur after a predetermined n number of interpolation periods from the current time, it is possible to prevent the occurrence of interference by starting deceleration from the current point of time.

0058Note that in the above embodiment, at step S12, the scheduled stop position was determined from s and the position of the braking distance L_stop (amount of movement), but it is also possible to determine the scheduled stop position simply from the time. That is, at step S12, it is also possible to calculate the scheduled stop position X(s(t0+T_stop)) = X(s(t+n·Δt+T_stop)).

0059Further, in addition to the above interference prevention control processing, it is also possible to perform the signal output processing such as shown in <figref idref="f0009">FIG. 9</figref> and output the results of judgment of the interference check to the peripherals etc. For example, when it is judged at step S16 that there would be interference, it is also possible to output a signal indicating that there would be interference to an outside device and jump to step S20 in the procedure.

0060In the above embodiment, a single robot control device 1 of the system having the configuration shown in <figref idref="f0009">FIG. 10</figref> is used to control a plurality of robots (robot mechanical parts) R1 to Rn and the interference prevention control device is formed by the robot control device 1. On the other hand, in the case of the system configuration wherein the robots shown in <figref idref="f0010">FIG. 11</figref> are provided with their corresponding control devices 1-1 to 1-n, respectively, the processors of the robot control devices 11-1 to 1-n perform the processing shown in the above <figref idref="f0006">FIGS. 7A</figref> and <figref idref="f0007">7B</figref> and <figref idref="f0008">FIG. 8</figref>. Among them, the processing of step S4, S13, and S14 is not performed. Before step S15, the scheduled stop position 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)) calculated at each of the robot control devices 1-1 to 1-n is exchanged and the interference check of step S15 is performed. Alternatively, it is possible to have any one of the robot control devices collect the scheduled stop positions of the robots checked for interference through the network and use this robot control device for the interference check. When there would be interference, it is sufficient to stop the operation of the interfering robot or all of the robots in the system.

0061Further, in the case of the system shown in <figref idref="f0010">FIG. 12</figref>, by transmitting the internal variables of the robot control devices 1-1 to 1-n from the robot control devices 1-1 to 1-n through the network, it is possible to have part of the calculations of the scheduled stop positions or judgment of the existence of an occurrence of interference be performed at a computer 2 connected thereto through the network. Further, it is also possible to use the computer 2 to perform all of the processing of the above <figref idref="f0006">FIGS. 7A</figref> and <figref idref="f0007">7B</figref> and <figref idref="f0008">FIG. 8</figref>.