JPS62162109A - Speed control method for multiple spindle robot - Google Patents

Abstract

PURPOSE:To drive a multiple spindle robot at high efficiency by obtaining maximum speed at which a multiple spindle robot can be operated automatically. CONSTITUTION:A valve W1 obtained by dividing a positional variation DELTAalpha1 by maximum value taumax of the quotient obtained by dividing positional variation DELTAalpha1 of the i-th spindle (i=1,2...) of a multiple spindle robot when it moves between two points by the maximum speed W1max allowed to the i-th spindle is made the maximum operatable speed of the i-th spindle. In the case where this is applied to between two teaching points, it is enough to adopt the minimum value (W1-W3) BIR, for instance, out of set of (W1-W3) between each interpolation point between two teaching points. Further, composed maximum operatable speed between teaching points can be obtained from these spindle speed.

Description

【発明の詳細な説明】 産業上の利用分野 本発明は、多軸ロボットの速度制御方法に関し、更に詳
しくは、安全性を確保しつつ高速で多軸ロボットを動作
させるための速度ｖＩ御方法に関する。DETAILED DESCRIPTION OF THE INVENTION Field of the Invention The present invention relates to a speed control method for a multi-axis robot, and more particularly to a speed vI control method for operating a multi-axis robot at high speed while ensuring safety. .

従来技術 多軸ロボットにおける手先部の移動速度は、各軸速度を
合成した速度である。The moving speed of the hand in a conventional multi-axis robot is a speed that is a combination of the speeds of each axis.

しかし、どのように合成が行われるかは、移動する空間
によって異なるため、移９Ｊノ速度が与えられζもこれ
から各軸速度を知ることは困難である。However, how the combination is performed differs depending on the space in which it moves, so it is difficult to know the velocity of each axis from the given velocity of 9J and ζ.

特に、多関節型ロボットにおいては、ｒｉ雑に合成され
るため、手先部の移動速度から各軸速度を知ることは大
変困趙である。In particular, in multi-jointed robots, it is very difficult to know the speed of each axis from the movement speed of the hand part because the RI is crudely synthesized.

ところで、各軸にはそれぞれ許容される最セＪ速度Ｗ−
，が定められＣおり、各軸速度がその！ｉｋ高速度Ｗｉ
を越えることはできない。By the way, each axis has a maximum permissible speed W-
, is determined C, and the speed of each axis is that! ik high speed Wi
cannot be exceeded.

もし一つの軸にその軸の許容最高速度Ｗｉつを越える軸
速度が与えられた場合には、目標値に追従不能の異常を
発生したり１、所定の作業線を逸脱して危険性を増ずな
どの問題を生しる。If one axis is given an axis speed that exceeds the maximum allowable speed for that axis, it may cause an abnormality that makes it impossible to follow the target value, or it may deviate from the specified work line, increasing the danger. This can lead to problems such as

しかし、前述のように、移動速度から各軸速度を得るこ
とは困難であるから、成る移動速度を与えたときに、各
軸速度が各々の最高速度ＷＬ、、、を越えないことを容
易にｇ！認することができず、実際に勤かしてみて異常
が発生するか否か試してみる外なかった。However, as mentioned above, it is difficult to obtain the speed of each axis from the movement speed, so when the movement speed is given, it is easy to ensure that the speed of each axis does not exceed its maximum speed WL, . g! The only option was to actually try it out and see if the problem occurred.

そこで、一般的には、安全を見込んで移動速度をかなり
低い速度に設定することが行われている。Therefore, generally, the moving speed is set to a considerably low speed in consideration of safety.

ｉ足来子支？、１にテのトｎ題点 安全を見込んで移動速度をかなり低めに設定するという
ことは、多軸ロボットの持つ能力を十分活用出来ないと
いうことであり、タクトタイムが増加するなどのロスを
生しさせることとなる。Is it a child? , Problem 1: Setting the movement speed to a fairly low level with safety in mind means that the capabilities of a multi-axis robot cannot be fully utilized, and losses such as increased takt time can be avoided. It will be brought to life.

発明の目的 本発明の目的とするところは、各軸速度が許容される最
高速度Ｗニーを越えない範囲で動作可能な最高速度を的
確にＩ）るごとか出来る多軸ロボットの速度制御方法を
提供することＧこある。OBJECT OF THE INVENTION The object of the present invention is to provide a speed control method for a multi-axis robot that can accurately control the maximum operating speed within a range where each axis speed does not exceed the maximum allowable speed W knee. There is so much to offer.

発明の構成 本発明の多軸ロポノＦ・の速度側１ｆｆ１１方法は、２
点間を移動するときの多軸ロボ−／　トの第１軸（１−
１，２，・・・）の位置変化量Δαｉを第１軸に許容さ
れた最高速度Ｗｉ−で除算した商τ□の最高値τ−で前
記位置変化量Δαｉを除算した値Ｗよを第１軸の動作可
能最高速度とすることを構成上の特徴とするものである
。Structure of the Invention The speed side 1ff11 method of the multi-axis Ropono F of the present invention is as follows:
The first axis (1-
1, 2, ...) divided by the maximum speed Wi- allowed for the first axis, the value W obtained by dividing the position change amount Δαi by the maximum value τ- of the quotient τ□. The structural feature is that it has the maximum operating speed of one axis.

なお、上記発明は、結果として上記Ｗ□を第１軸の動作
可能最高速度とすれば足り、そのＷ□の算出の過程を限
定するものではない。In addition, in the above invention, as a result, it is sufficient that the above W□ is the maximum movable speed of the first axis, and the process of calculating the W□ is not limited.

そこで、そのＷ□の算出の過程を限定した多軸ロボット
の速度制御方法として、教示された２点での位置姿勢と
移動速度とから１制御周期を毎に移動する補間点を得、
次いで各補間点間を移動するときの多軸ロボットの第１
軸（ｉ＝１．２．・・・）の速度Ｗ工を得、その速度Ｗ
Ｌがすべて第１軸に許容された最高速度ＷＬｍｘより小
ならその速度Ｗ工を多軸ロボットへの指示速度とし、一
方、速度Ｗ□の中にＷｉ−より大なる速度があれば、そ
の大なる速度のうち最も小なる速度を選び出し、それが
第ｍ軸の速度ＷｉならＷｉ、ＸＷｉ／Ｗ。Therefore, as a speed control method for a multi-axis robot that limits the process of calculating W
Then, the first of the multi-axis robot when moving between each interpolation point.
Obtain the speed W of the axis (i=1.2...), and obtain the speed W
If all of L are smaller than the maximum speed WLmx allowed for the first axis, that speed W will be used as the instruction speed to the multi-axis robot. On the other hand, if there is a speed greater than Wi- in the speeds W□, that speed Select the smallest speed among the speeds, and if it is the m-th axis speed Wi, then Wi, XWi/W.

＝ｗ１を求め、この値Ｗ工を第１軸の動作可能最高速度
とすることを特徴とする発明がさらに提供される。=w1 is determined, and this value W is set as the maximum movable speed of the first axis.

後者の発明は、結果として前者の発明と合致する。The latter invention is consistent with the former invention as a result.

作用 説明の都合上、３軸ロボツトをセ、定し、その第１軸、
第２軸、第３軸について許容される最高速度をそれぞれ
ｗ、、、ｗ、ｍ＋　Ｗ：１．ｘとする。For convenience of explaining the operation, a three-axis robot is set up, and its first axis,
The maximum speeds allowed for the second and third axes are respectively w, , w, m+ W:1. Let it be x.

また、２Ｊｉｌｉ間点間を３軸ロボツトの手先ｒａ＋が
移動するときの第１軸、第２軸、第３軸の位置変化量を
それぞれΔαｉ．Δαつ、Δα〕とする。Also, when the hand ra+ of the 3-axis robot moves between 2 points, the amount of position change of the first, second, and third axes is calculated as Δαi. Δα, Δα].

すると、τ・＝Δαｉ　／　ｗ　ｌ□８．τり＝Δα２
／Ｗ２　、、　　τ３−Δα３／Ｗ］−となる。Then, τ・=Δαi / w l□8. τ = Δα2
/W2,, τ3-Δα3/W]-.

ｒｌ、τ２．τ〕のうちの最大値をｒ、とすると、各軸
の動作可能最高速度は、ｗｌ　−Δαｉ／τｍ、Ｗ２　
−Δα２　／τ＋ａｘ、Ｗ３　　＝Δαｉ／τｉとなる
。rl, τ2. If the maximum value of [τ] is r, the maximum operable speed of each axis is wl −Δαi/τm, W2
-Δα2/τ+ax, W3 =Δαi/τi.

仮に、τｉ　〉τ２〉τ〕が成立したとするならば、τ
ｉが最大値で１となるから、Ｗｉ−Δαｉ／τｉ．Ｗ２
＝Δα２／τＩ　、　Ｗ３　＝Δα３／τｉとなる。If τi 〉τ2〉τ] holds, then τ
Since i is 1 at its maximum value, Wi-Δαi/τi. W2
=Δα2/τI, W3 =Δα3/τi.

ここで、 ■Ｗｉ＝＝Δαｉ／τオ＝Ｗｉ１であり、第１軸に許容
される最高速度である。Here, (1) Wi==Δαi/τo=Wi1, which is the maximum speed allowed for the first axis.

０ｗ２　＝Δα２／ｒ、＜Δα２　／　τ２　＝　ｗ２
１であり、第２軸に許容される最高速度を越えない。0w2 = Δα2/r, <Δα2 / τ2 = w2
1, and does not exceed the maximum speed allowed for the second axis.

■Ｗ〕−Δα３　／　ｒ　＋　＜Δα３　／　τ３　＝
　ｗ３＋＋ｗであり、第３軸に許容される最高速度を越
えない。■W] −Δα3 / r + <Δα3 / τ3 =
w3++w, and does not exceed the maximum speed allowed for the third axis.

■各軸は共にｒ、時間でそれぞれΔαｉ．Δαり１　Δ
α３だけ位置変化する。■Each axis is both r and time is Δαi. Δαri1 Δ
The position changes by α3.

したがって、Ｗｌ、Ｗ２．Ｗ３は、各軸について許容さ
れる最高速度ｗ、　、ｌ　ｗ、　ＩＩＩＸ＋　ｗ３１ａ
ｌ＋をどの軸についても越えないで且つ２点間を逸脱し
ないで移動することがごきる最高の動作可能速度である
。Therefore, Wl, W2. W3 is the maximum speed allowed for each axis w, , l w, IIIX+ w31a
This is the highest operating speed at which the robot can move without exceeding l+ on any axis and without deviating between two points.

以上は２７ｆｔ間点間についての説明であるが、これを
２教示点間に適用する場合は、例えば２＠示点間におけ
る各補間点間についての（ｗ、、ｗ２、Ｗｉ）の築合の
中で最小の値（ｗｌ、ｗり、Ｗｌ）１．Ｉを採用すれば
よい。また、これらの各軸速度から、合成した教示点間
動作可能最高速度が求められる。The above is an explanation between 27ft points, but when applying this between two teaching points, for example, the construction of (w,, w2, Wi) between each interpolation point between 2 @ teaching points. The minimum value among them (wl, wri, Wl)1. You should adopt I. Further, from these respective axis velocities, the combined maximum movable speed between teaching points is determined.

奥方Ｓ１イタリ 以下、図に示す実施例に基づいて本発明を更に詳しく説
明する。ここに第１図は本発明の速度制御方法を実施す
る多軸ロボット装置の構成外観図、第２図は第１図に示
す多軸ロボット装置による速度算出処理の要部フローチ
ャート、第３図は第１図に示す多軸ロボント装ヱによる
再補間処理の要部フローチャート、第４図は速度算出処
理の他の例の第２図相当図である。なお、以下の実施例
が本発明を限定するものではない。Madam S1 Itari Hereinafter, the present invention will be explained in more detail based on the embodiment shown in the drawings. Here, FIG. 1 is an external view of the configuration of a multi-axis robot device that implements the speed control method of the present invention, FIG. 2 is a flowchart of the main part of speed calculation processing by the multi-axis robot device shown in FIG. 1, and FIG. FIG. 1 is a flowchart of the main part of the re-interpolation process by the multi-axis robot equipment, and FIG. 4 is a diagram corresponding to FIG. 2 of another example of the speed calculation process. Note that the following examples do not limit the present invention.

第１図に示す多軸ロボット装置１０は、多関節型ロボッ
ト１を制御装置２により駆動するもので、制御の内容は
制御盤２３もしくはリモートコン上ローラ３からオペレ
ータが教示する。In the multi-axis robot device 10 shown in FIG. 1, an articulated robot 1 is driven by a control device 2, and the details of the control are taught by an operator from a control panel 23 or a remote control upper roller 3.

説明の都合上、多関節型ロボットｌは、α、〜α１の５
つの自由度を有し、制御周期を毎の定時間制御をなされ
るものとする。For convenience of explanation, the articulated robot l is α, ~ α1 5
It has two degrees of freedom and is controlled for a fixed period of time every control period.

始点Ａから終点Ｂまで一定速度■で多関節型ロボットｌ
を駆動したい場合は、オペレータは、始点Ａでの位置姿
勢（Ｘ、、Ｙ、、Ｚ、、　　θ８．φ。An articulated robot l moves at a constant speed from start point A to end point B.
When the operator wants to drive the position and orientation at the starting point A (X, , Y, , Z, θ8.φ.

）および終点Ｂでの位置姿勢（Ｘ、、ＹＩ、、ｚ、。) and the position/orientation at end point B (X,,YI,,z,.

、θｂ、φＩ、）および希望の移動速度■を入力する。, θb, φI, ) and the desired moving speed ■.

上記入力が行われると、制御装置２は、第２図に示すよ
うに処理（速度算出Ｉ）を開始する。When the above input is performed, the control device 2 starts the process (speed calculation I) as shown in FIG.

まず、教示された移動速度■と制御周期ｔとから仮移動
量Δｌを求める。すなわち、速度Ｖで移動すると仮定し
たときの１制御周期当たりの移動量ΔｉをΔｇ−ｖｔに
より求める（Ｓｔ）。First, a provisional movement amount Δl is determined from the taught movement speed ■ and the control period t. That is, the amount of movement Δi per control period when it is assumed that the vehicle moves at a speed V is calculated from Δg−vt (St).

次に、現点Ｐ、ｌよりΔｌだけ進んだ仮次点ｃｉｒ１．
。Next, the tentative runner-up point cir1. which is advanced by Δl from the current point P,l.
.

の位置姿勢を求める（Ｓ２）。The position and orientation of is determined (S2).

最初の現点Ｐ、は始点Ａであり、仮次点Ｑ、の位置姿勢
は次のよ）になる。The first current point P is the starting point A, and the position and orientation of the tentative next point Q is as follows.

Ｘｑ　　”Ｘｉ　　”　　（Ｘｂ　　−Ｘ＠　）　　Δ
Ｉ／まただし、ｌは始点Ａから終点Ｂまでの長さである
。Xq “Xi” (Xb −X@) Δ
I/where l is the length from starting point A to ending point B.

他の位Ｅ座標ｙｑ、ｚｑおよび姿勢θ１．ｙ９について
も同様に求める。Other position E coordinates yq, zq and attitude θ1. Find y9 in the same way.

現点ＰＴｌおよび仮次点ＱＴｌヤ、の位置姿勢が得られ
たら、その位置姿勢を逆変換し、各点に対応する関節角
αＬｎおよびα□ｎ＋１を求める（３３）。Once the positions and orientations of the current point PTl and the tentative next point QTl are obtained, the positions and orientations are inversely transformed to find the joint angles αLn and α□n+1 corresponding to each point (33).

多関節型ロボット１が５つの自由度を有しているから、
　（αＩｎ＋　　α２ｎ、α３Ｔ１＋　　α４ｎ＋　　
αち、）および（ｙｌ　ｎｉｌ　＝　α２　＋’ｌ＋１
　、α、ｎや１．α、ｎ＋。Since the articulated robot 1 has five degrees of freedom,
(αIn+ α2n, α3T1+ α4n+
αchi, ) and (yl nil = α2 +'l+1
, α, n and 1. α, n+.

、α、ｎや、）を得ることになる。, α, n, etc.).

次に、各関節角の仮角変位量Δαｉ□を、Δαｉ。Next, the tentative angular displacement amount Δαi□ of each joint angle is Δαi.

−α、ｎ＋１−α、。により求める（Ｓ４）。−α, n+1−α,. (S4).

得られた仮角変位量Δαｉｎは、教示された移動速度Ｖ
で１制御周期ｔだけ移動した場合の各関節角の変位量で
ある。従って、仮関節速度ＷＬｒｌは、ＷＬｎ＝Δαｔ
　ｎ　／　Ｌにより求められる（Ｓ５）。The obtained tentative angular displacement amount Δαin is the taught moving speed V
This is the amount of displacement of each joint angle when the joint angle is moved by one control period t. Therefore, the pseudojoint velocity WLrl is WLn=Δαt
It is determined by n/L (S5).

次に、仮関節速度Ｗ□。が、各関節に許容された最高速
度Ｗｉ−より小さいか否かを比較する（３６）、すなわ
ち、Ｗｌ　ｎ　＜Ｗｌ　−＋　Ｗ２　ｎ　＜ｗ、　ＩＩ
ＩＸ＋　　Ｗｌ　ｙｌ　＜　Ｗｌ　１＋　Ｗｉ　ｙｌ　
＜　Ｗｉユ、Ｗｉ。＜Ｗｉ□のそれぞれを比較演算する
。Next, pseudojoint speed W□. is smaller than the maximum velocity Wi− allowed for each joint (36), i.e. Wl n < Wl −+ W2 n < w, II
IX+ Wyl yl < Wl 1+ Wi yl
< Wiyu, Wi. A comparison operation is performed on each of <Wi□.

全ての仮関節速度Ｗｉ。が対応する許容最高速度Ｗｉよ
り小さければ、教示された速度Ｖで多関節型ロボット１
を駆動しても何ら問題がない。All pseudo-joint velocities Wi. is smaller than the corresponding maximum allowable speed Wi, the articulated robot 1 moves at the taught speed V.
There is no problem with driving.

そこでステップＳ７からステップＳ８へ分岐し、仮次点
Ｑｎ＋１を次点Ｐｎ＋１　とする（Ｓ８）。また、教示
された移動速度Ｖを実際に多関節型ロボット１に指示す
る移動速度ｖｎとする（Ｓ９）。Therefore, the process branches from step S7 to step S8, and the provisional runner-up point Qn+1 is set as the runner-up point Pn+1 (S8). Further, the taught movement speed V is set as the movement speed vn actually instructed to the articulated robot 1 (S9).

一方、仮関節速度Ｗｉ、のいずれか１つでも対応する許
容最高速度Ｗｉ−を越えておれば、教示された移動速度
Ｖで多関節型ロボット１を駆動するのは好ましくないか
ら、実際に動作させ得る最高の速度を演算により算出し
なければならない。On the other hand, if any one of the temporary joint speeds Wi exceeds the corresponding maximum allowable speed Wi-, it is not desirable to drive the articulated robot 1 at the taught movement speed V, so the actual movement The maximum speed that can be achieved must be calculated by calculation.

そこで、ステップＳ７よりステップＳＩＯへ分岐する。Therefore, the process branches from step S7 to step SIO.

ステップＳＩＯでは、各関節をその関節に許容された最
高速度Ｗｉで仮角変位量Δαｉｏだけ駆動したときの所
要時間すなわち最短角変位時間τ、。を算出する。すな
わち、τ□、＝Δαｔｎ／Ｗｉ．を演算する。そして、
各関節について得られたτＩｎ・　τ２ｎ・　τ３ｎ・
　τ４ｎ・　τ５ｎのうち最大の値をτ−６とする（Ｓ
ＩＯ）。In step SIO, the time required when each joint is driven by a provisional angular displacement amount Δαio at the maximum speed Wi allowed for that joint, ie, the shortest angular displacement time τ. Calculate. That is, τ□,=Δαtn/Wi. Calculate. and,
τIn・τ2n・τ3n・ obtained for each joint
The maximum value of τ4n and τ5n is set as τ-6 (S
IO).

これは、現点Ｐｎから仮次点Ｑｎヤ、へ移動するのに、
τｓｕｎ以上の時間を与えなければ、いずれかの関節で
その許容最高速度Ｗｉ−を越えてしまうことを意味して
いる。This means that to move from the current point Pn to the tentative next point Qn,
This means that if a time longer than τsun is not given, the allowable maximum speed Wi- will be exceeded at any joint.

次に、動作可能最高角変位速度Ｗｉ．を、ＷＬｎ−Δα
□、ｌ／τ、、ｌにより算出する（Ｓｉｔ）。Next, the maximum operable angular displacement speed Wi. , WLn−Δα
Calculated by □, l/τ,, l (Sit).

これにより得られたＷｌｙ、、Ｗ２ｎ１ｗ３ｎ＋Ｗ◆ｎ
＋　Ｗｆｉｎのいずれかは、その軸の許容最高速度であ
り、その他は仮関節速度ＷＬｎの比率に応じて減速した
関節速度となる。Wly obtained by this,, W2n1w3n+W◆n
+Wfin is the maximum allowable speed of that axis, and the others are joint speeds that are decelerated according to the ratio of the temporary joint speed WLn.

次に、動作可能最高角変位速度ｗｉｎで１制御周期ｔの
間に変位する各変位量δαｌｎをδαＬｎ＝ｗ、ｎｔに
より算出する（Ｓ１２）。Next, each displacement amount δαln that is displaced during one control period t at the maximum operable angular displacement speed win is calculated by δαLn=w, nt (S12).

また、その各変位量δα、。たけ現関節角αＬｎより進
んだ次関節角αＬｎ＋＋をαＬＴｌ＋１＝α、。Also, each displacement amount δα,. The next joint angle αLn++ that is advanced from the current joint angle αLn is αLTl+1=α.

＋δαｔｎより算出する（Ｓ１３）。Calculated from +δαtn (S13).

この次関節角α、ｎや、をｊｉＪ］変換すれば動作可能
最高角変位速度Ｗｉ．ｌで各関節を駆動したときの制御
周Ｑｔ後の位置姿勢が分かる。すなわち次点Ｐｙ１＋１
の位置姿勢を算出できる（Ｓ１４）。By converting the next joint angles α, n, etc. to jiJ], the maximum possible angular displacement speed Wi. The position and orientation after the control cycle Qt when each joint is driven at l is known. That is, the runner-up Py1+1
The position and orientation of can be calculated (S14).

現点Ｐ。と次点Ｐｎ＋＋が分かれば、その間の移動時間
は１制御周期ｔだから、移動速度Ｖ。を算出することが
できる（Ｓ１５）。Current point P. If the runner-up Pn++ is known, the moving time between them is one control period t, so the moving speed is V. can be calculated (S15).

第１回目の上記ステップ３１〜５１５の処理により最初
の補間点Ｐ２および始点Ａからその補間点Ｐ２までの移
動速度■１が得られることになる。The first interpolation point P2 and the moving speed 1 from the starting point A to the interpolation point P2 are obtained by the first processing of steps 31 to 515.

Ｐ２が求まれば、これを現点として上記ステップ３１〜
Ｓ１５を行うことによりＰ〕およびＰ２からＰ３への移
動速度■２を求めることができる。Once P2 is determined, take this as the current point and proceed to steps 31 to 31 above.
By performing S15, it is possible to obtain P] and the moving speed ■2 from P2 to P3.

以下同様にして、補間点Ｐ、、Ｐ５．・・・および移動
速度ｖ〕２　ｖ＋、・・・が得られるのである。Similarly, interpolation points P, , P5 . . . . and the moving speed v]2 v+, . . . are obtained.

移動速度ｖＩ、ｖ２．・・・は、教示された移動速度■
が各関節の許容最高速度Ｗえ−に適合するものであれば
、その教示された速度■となり、教示された速度Ｖが適
合しなければ動作させ得る最高の速度まで減速された値
となる。Movement speed vI, v2. ... is the taught movement speed■
If the maximum permissible speed W of each joint is compatible with the maximum allowable speed W of each joint, the taught speed becomes the taught speed .

さて、上記により得られた補間点間の移動速度ｖ、、ｖ
２．・・・が一定値になるとは限らない、しかし、始点
Ａから終点Ｂまで一定の速度を保ちたいならば、第３図
に示す如く再補間処理を行って新補間点を算出すればよ
い。Now, the moving speed between the interpolation points obtained above is v,,v
2. ... is not necessarily a constant value.However, if you want to maintain a constant speed from the starting point A to the ending point B, you can calculate a new interpolation point by performing re-interpolation processing as shown in FIG.

すなわち、第３図に示すように、前記ステップ８１〜３
１５を繰り返すことによって得られた移動速度Ｖｌ、Ｖ
ｌ、・・・を相互に比較し、その最小の値をＶｌ、、と
する、（Ｓ２１）。That is, as shown in FIG.
Movement speed Vl, V obtained by repeating step 15
l, . . . are compared with each other, and the minimum value thereof is set as Vl, .

どの補間点間においても各関節の速度が許容最高速度Ｗ
□−を越えないようにするためにはこの最小の値Ｖ１１
．を採用すればよい。The speed of each joint between any interpolation points is the maximum allowable speed W
□In order not to exceed -, this minimum value V11
．． should be adopted.

そこで、この最小の値ｖ１１．を採用したときの１制御
周期を当たりの移動量δｅを、δ６＝ｖ、、、ｔにより
算出しく５２２）、その移動量δｌ毎の区間に始点Ａか
ら終点Ｂまでを分割し、新補間点を求めればよい（Ｓ２
３）、言うまでもな（、移動速度はｖ、１．一定である
。Therefore, this minimum value v11. Calculate the movement amount δe per one control cycle when adopting All you have to do is find (S2
3), Needless to say (, the moving speed is v, 1. constant).

移動速度Ｖの教示を行わずに、はじめから多関節型ロボ
ット１の動作可能な最高速度で駆動しようとする場合、
オペレータは始点Ａと終点Ｂでの位置姿勢のみを教示し
、制御装置２に第２図の破線の如き処理（速度算出■）
を行わせる。When attempting to drive the articulated robot 1 at the maximum movable speed from the beginning without teaching the movement speed V,
The operator teaches only the position and orientation at the starting point A and ending point B, and the control device 2 performs the processing as shown by the broken line in Figure 2 (speed calculation ■).
Have them do it.

ただし、Δｉとして、たとえば、Ｉｉ！線的に移動する
とみなされる最も長い２点間距離や１制御周期ｔで移動
しうると考えられる距離より少し大きめの距離を設定す
る。However, as Δi, for example, Ii! Set a distance slightly larger than the longest distance between two points that are considered to move linearly or the distance that is considered to be able to move in one control period t.

さて、以上の実施例は、始点ＡからΔβづつ進みながら
速度算出を行う処理であったが、第４図に示すものは、
予め始点Ａから終点Ｂまでを分割し、各々について移動
速度を算出する実施例である。Now, in the above embodiment, the speed is calculated while proceeding from the starting point A by Δβ, but the one shown in FIG.
This is an example in which the area from the starting point A to the ending point B is divided in advance and the moving speed is calculated for each.

まず、教示された速度■と制御周期ｔとから仮移動量Δ
ｌを求める（Ｓ３１）。First, from the taught speed ■ and the control period t, the temporary movement amount Δ
1 is determined (S31).

次に始点へから終点ＢまでのΔβ毎の区間に分割して補
間点Ｐｎを求める（Ｓ３２）。Next, the interpolation point Pn is determined by dividing into intervals of Δβ from the start point to the end point B (S32).

次に、点Ｐｎと点Ｐ　ｎ＋１での位置姿勢を逆変換して
それぞれの関節角αＬｒｌ及びαｌｎｉを求める（Ｓ３
３）。Next, the positions and orientations at points Pn and Pn+1 are inversely transformed to obtain the respective joint angles αLrl and αlni (S3
3).

次に、仮角変位量Δαｔｎを求め（Ｓ　３４）、仮関節
速度ＷＬｎを求め（Ｓ３５）、仮関節速度Ｗ□。が各関
節に許容された最高速度Ｗｉより小さいか否か比較する
（Ｓ３６）。これらのステップＳ３４．３５．３６は、
前記ステップＳ４゜５．６と同じである。Next, the tentative angular displacement amount Δαtn is determined (S34), the tentative joint velocity WLn is determined (S35), and the tentative joint velocity W□. is smaller than the maximum speed Wi allowed for each joint (S36). These steps S34.35.36 are:
This is the same as step S4°5.6.

全ての仮関節速度Ｗｉｒｌが対応する許容速度ＷＬｗｍ
より小さければ、教示速度Ｖを移動速度ｖｎとする（Ｓ
３９）。Allowable speed WLwm to which all pseudo-joint speeds Wirl correspond
If it is smaller, the teaching speed V is set as the moving speed vn (S
39).

いずれかの仮関節速度Ｗｌｎが対応する許容最高速度Ｗ
ｉ−より大きければ、最短角変位時間τＬｎを求め、各
関節についてのτ、。のうち最大の値をτ−１とじ（５
４０）、そのτ−１で仮角変位量Δαｔｎを除算して動
作可能最高角変位速度ＷＬ１を算出する（Ｓ４１）、こ
れらのステップ８４０．４１は、前記ステップＳＩ０，
１１と同じである。Maximum allowable speed W corresponding to any pseudo-joint speed Wln
If it is larger than i-, find the shortest angular displacement time τLn and τ for each joint. The maximum value among them is bound by τ-1 (5
40), calculate the maximum operable angular displacement speed WL1 by dividing the tentative angular displacement amount Δαtn by that τ-1 (S41), these steps 840.41 are the steps SI0,
It is the same as 11.

求められた動作可能最高角変位速度ＷＬ□に制御時間ｔ
を乗じて角変位量δαｔｎを求め（Ｓ４２）、現関節角
αＬｎにその角変位量δα、ｎを加えて次関節角αＬｎ
＋１を求める（Ｓ４３）。これらのステップＳ４２゜４
３は前記ステップＳ１２、Ｓ１３と同じである。The control time t is applied to the determined maximum operable angular displacement speed WL□.
is multiplied to obtain the angular displacement amount δαtn (S42), and the angular displacement amount δα,n is added to the current joint angle αLn to obtain the next joint angle αLn.
+1 is calculated (S43). These steps S42゜4
3 is the same as steps S12 and S13.

次閏節角αｔｎや１を順変換すれば、ｌ制御周期ｔで進
め得る点Ｅ、ｌの座標が求められる（Ｓ４４）。By converting the next leap angle αtn and 1, the coordinates of point E and l that can be advanced in l control period t are obtained (S44).

次に点Ｐｎと点Ｅ、ｌの座標から移動速度■。を算出す
る（Ｓ４５）。Next, calculate the moving speed ■ from the coordinates of points Pn, E, and l. is calculated (S45).

上記ステップ５３１−３４５を各補間点間について繰り
返せば、各補間点間の移動速度Ｖ。が求められる。そこ
で、第３図に示す再補間処理を行えば、始点Ａから終点
Ｂまでほぼ一定の高速度で多関節型ロボット１を駆動す
るためのデータが得られる。If the above steps 531-345 are repeated between each interpolation point, the moving speed V between each interpolation point is obtained. is required. Therefore, by performing the re-interpolation process shown in FIG. 3, data for driving the articulated robot 1 from the starting point A to the ending point B at a substantially constant high speed can be obtained.

発明の効果 本発明によれば、２点間を移Ωノするときの多軸ロボッ
トの第１軸（＋＝１．２．・・・）の位置変化量Δαｉ
を第１軸に認められた最高速度Ｗｉ−で除算した商τ、
の最高値τ−で前記位置変化量Δαえを除算した値〜１
．を第１軸の動作可能最高速度とすることを特徴とする
多軸ロボットの速度制御方法が提供され、これにより多
軸ロボットを動作させ得る最高速度が自動的に得られる
ので、多軸ロボットを高効率で駆動することができる。Effects of the Invention According to the present invention, the amount of position change Δαi of the first axis (+=1.2...) of a multi-axis robot when moving between two points is
τ divided by the maximum speed Wi− allowed for the first axis,
The value obtained by dividing the amount of positional change Δα by the maximum value τ− of ~1
．． A speed control method for a multi-axis robot is provided, characterized in that the maximum speed at which the first axis can operate is set as the maximum speed at which the multi-axis robot can operate. It can be driven with high efficiency.

【図面の簡単な説明】[Brief explanation of drawings]

第１図は本発明の速度制御方法を実施する多軸ロボット
装置の構成外観図、第２図は第１図に示す多軸ロボット
装置による速度算出処理の要部フローチャート、第３図
は第１図に示す多軸ロボット装置による再補間処理の要
部フローチャート、第４図は速度算出処理の他の例の第
２図相当図である。 （符号の説明） ■・・・多関節型ロボット　　　２・・・制御装置２８
・・・制御盤 ３・・・リモートコントローラ １０・・多軸ロボット装置。FIG. 1 is an external view of the configuration of a multi-axis robot device that implements the speed control method of the present invention, FIG. 2 is a flowchart of main parts of speed calculation processing by the multi-axis robot device shown in FIG. 1, and FIG. The main part flowchart of the re-interpolation process by the multi-axis robot device shown in the figure, and FIG. 4 is a diagram corresponding to FIG. 2 of another example of the speed calculation process. (Explanation of symbols) ■...Articulated robot 2...Control device 28
...Control panel 3...Remote controller 10...Multi-axis robot device.

Claims (1)

【特許請求の範囲】 １、２点間を移動するときの多軸ロボットの第ｉ軸（ｉ
＝１、２、・・・）の位置変化量Δα＿ｉを第ｉ軸に許
容された最高速度Ｗ＿ｉ＿ｍ＿ａ＿ｘで除算した商τ＿
ｉの最高値τ＿ｍ＿ａ＿ｘで前記位置変化量Δα＿ｉを
除算した値ｗ＿ｉを第ｉ軸の動作可能最高速度とするこ
とを特徴とする多軸ロボットの速度制御方法。 ２、教示された２点での位置姿勢と移動速度とからＩ制
御周期ｔ毎に移動する補間点を得、次いで各補間点間を
移動するときの多軸ロボットの第ｉ軸（ｉ＝１、２、・
・・）の速度Ｗ＿ｉを得、その速度Ｗ＿ｉがすべて第ｉ
軸に許容された最高速度Ｗ＿ｉより小ならその速度Ｗ＿
ｉを多軸ロボットへの指示速度とし、一方、速度Ｗ＿ｉ
の中にＷ＿ｉ＿ｍ＿ａ＿ｘより大なる速度があれば、そ
の大なる速度のうち最も小なる速度を選び出し、それが
第ｍ軸の速度Ｗ＿ｍならＷ＿ｍ＿ｍ＿ａ＿ｘ×Ｗ＿ｉ／
Ｗ＿ｍ＝ｗ＿ｉを求め、この値ｗ＿ｉを第ｉ軸の動作可
能最高速度とすることを特徴とする多軸ロボットの速度
制御方法。[Claims] The i-th axis (i
= 1, 2, ...) position change amount Δα_i divided by the maximum speed W_i_m_a_x allowed for the i-th axis τ_
A speed control method for a multi-axis robot, characterized in that a value w_i obtained by dividing the amount of positional change Δα_i by a maximum value τ_m_a_x of i is set as the maximum movable speed of the i-th axis. 2. Obtain an interpolation point that moves every I control period t from the position/posture and movement speed at the two taught points, and then calculate the i-th axis (i=1) of the multi-axis robot when moving between each interpolation point. ,2,・
...), and all the speeds W_i are the i-th
If it is smaller than the maximum speed W_i allowed for the axis, that speed W_
Let i be the commanded speed to the multi-axis robot, and on the other hand, the speed W_i
If there is a speed greater than W_i_m_a_x, select the smallest speed among those large speeds, and if it is the speed W_m of the m-th axis, then W_m_m_a_x×W_i/
A speed control method for a multi-axis robot, characterized in that W_m=w_i is determined and this value w_i is set as the maximum movable speed of the i-th axis.