JPH0312323B2 - - Google Patents

Description

【発明の詳細な説明】

この発明は、テイーチング・プレイバツク方式
の産業用ロボツトにおいて、プログラムを再生実
行する間にスピードの変更を可能とした位置制御
装置に関するものである。 例えばα₆，α₇の２回転自由度を有するワークポ
ジシヨナー２を含むα₁〜α₅の５回転自由度の産業
用ロボツト１が、両テイーチング位置Pa（X_a，
Y_a，Z_a，Ψ_a，θ_a，α_6a，α_7a）とPb（X_b，Y_b，Z_b，
Ψ_b，θ_b，α_6b，α_7b）との間をプレイバツクする
際、予め設定した時間ｔごとにその両位置情報の
差を内分し、その内分点の位置を求め、この位置
を次に進むべき位置として逐次指令するべくなさ
れている。 すなわち初期設定スピードS₁から時間ｔにおけ
る移動量（ｔ・S₁）を求め、さらに両位置Pa，
Pb間の位置情報の差の最大値Ａを、 Ａ＝max(イ) √（−）²＋（−
）²＋（−）² (ロ) K₁√（_a−_b）²＋（_a−_b）² (ハ) K₂√（_6a−_6b）²＋（_7a−_7b）² なおK₁，K₂は、Ψ_a，Ψ_b，θ_a，θ_b，α_6a，α_6b，
α_7a，α_7bが、角度値であるため、距離としての
値に置換するためのそれぞれ予め設定した定数
である。 として求め、この差Ａに対する移動量（ｔ・S₁）
ごとのきざみ量Ｎを、 Ｎ＝Ａ／（ｔ・S₁）として求め、さらにはそのきざ みごとに次期指令位置Piを、 Pi＝（Ｎ−ｉ）Pa＋iPb／Ｎ （ただしｉは、Pa位置からのきざみ数） として求め、この位置情報に基づいた信号が各サ
ーボ系に出力される。 ところで今両位置Pa，Pb間のPio位置で前記
スピードS₁に対するスピード変更指示をしたい場
合、スピードS₁を変更後のスピードS₂に置換する
とともに、前記Pa位置をPio位置に置換して、前
述各演算を実行し直す必要がある。すると演算時
間が大となり、前記設定した時間ｔを大にしない
と、前記ロボツト１が前回指令した位置に到達し
ても、まだ次期指令位置に対する演算が終了しな
いという不具合が生じる。かといつて時間ｔを大
にすると、きざみ量Ｎが小となり、指令位置Piの
出力回数が減るため、精度上問題となる。 この発明は前述事情に鑑みなされたものであつ
て、時間ｔを大にすることなく、プログラム再生
実行中においてスピード変更可能とした位置制御
装置を提供せんとするものであり、以下実施例を
詳述する。 １は、α₁〜α₅の５回転自由度を有する多関節型
の産業用ロボツトであり、その先端出力部材に
は、溶接電源WSにコンジツトケーブルを介して
連結したMIG溶接用トーチ１ａが支持され、溶
接ロボツトとして構成されている。またロボツト
１の作業領域には、α₆，α₇の２回転自由度を有す
るワークポジシヨナー２が配置されている。 そしてトーチ１ａとポジシヨナー２のワーク取
付板２ａとを相互に位置制御してワークを自動溶
接するべく構成されている。 ３は、制御装置であり、中央処理装置CPUお
よびメモリMEMを含むコンピユータを主体とす
るものである。制御装置３には、各制御軸α₁〜α₇
のサーボ系Sα₁〜Sα₇や、遠隔操作盤REMや溶接
電源WSがバスラインＢを介して接続されてい
る。なおサーボ系Sα₁〜Sα₇には、それぞれ電動
機M₁〜M₇およびこれに接続されるエンコーダE₁
〜E₇が含まれている。 また制御装置３のメモリMEMには、プログラ
ムの再生実行中においてスピードの変更を可能と
した手段（F₁〜F₄）が含まれている。すなわち、 初期設定スピードS₁から予め設定した時間ｔに
おける移動量（ｔ・S₁）を求め、さらに両テイー
チング位置PaとPbとの位置情報の差Ａ（最大値）
に対する移動量（ｔ・S₁）ごとのきざみ量Ｎを、 Ｎ＝Ａ／（ｔ・S₁） として求める第１手段F₁と、 両位置Pa，Pb間における時間ｔごとの指令位
置Piを、 Pi＝（Ｎ−ｉ）Pa＋iPb／Ｎ （ただしｉは、Pa位置からのきざみ数） として求める第２手段F₂と、 手段F₁とF₂との間に介在され、スピードS₁に
対する変更指示の有無を判断する第３手段F₃、
およびこの手段F₃の結果、スピード変更指示有
の場合、変更後のスピードS₂から時間ｔにおける
移動量（ｔ・S₂）を求め、さらにはその変更のあ
つた指令位置Pioと位置Pbとの位置情報の差に対
する移動量（ｔ・S₂）ごとの変更後きざみ量
Nnewを、 Nnew＝（Ｎ−i₀）×（ｔ・S₁）／（ｔ・S₂） として求め、そして手段F₂におけるきざみ量Ｎ
をきざみ量Nnewに、また位置Paを位置Pi₀に、
それぞれ置換するとともに、手段F₂におけるき
ざみ数ｉを０にセツトする第４手段F₄と、 が制御装置３に記録されている。 さらにこの実施例の作用を述べる。 今例えば、第１図ロのような直角座標系（以下
直系）において、位置Pa（X_a，Y_a，Z_a，Ψ_a，θ_a，
α_6a，α_7a）からPb（X_b，Y_b，Z_b，Ψ_b，θ_b，α_6b，
α_7b）へ向かつて直線移動するというプログラム
を想定する。 まず両位置Pa，Pb間の直系における位置情報
の差の最大値Ａを、 Ａ＝max (イ) √（−）²＋（−）²＋（
−）² (ロ) K₁√（_a−_b）²＋（_a−_b）² (ハ) K₂√（_6a−_6b）²＋（_7a−_7b）²〕 として求める。（ステツプSP₁） なおΨ_a，Ψ_b，θ_a，θ_b，α_6a，α_6b，α_7a，α_7bは
角
度値であるため、前記(イ)，(ロ)式には、予め設定し
た定数K₁，K₂が乗じられており、Xa，Xb，
Ya，Yb，Z_a，Z_bと同じ距離としての値に置換さ
れている。 次に初期設定スピードS₁から予め設定した時間ｔ
（例えば0.1秒）における移動量（ｔ・S₁）を求
め、この移動量（ｔ・S₁）と両位置Pa，Pb間の
位置情報の差Ａとから、その差Ａに対する移動量
（ｔ・S₁）ごとのきざみ量Ｎを、 Ｎ＝Ａ／（ｔ・S₁） として求め、またｉ（位置Paからのきざみ数）を
０にセツトしておく。（ステツプSP₂） なお前記移動量（ｔ・S₁）は、前記位置情報の
差Ａが前記(イ)の場合は、トーチ１ａ先端溶接点の
位置の変位置となり、また前記差Ａが前記(ロ)の場
合は、トーチ１ａの姿勢変更のための回動量とな
り、さらに前記差Ａが前記(ハ)の場合は、ワーク取
付板２ａの回動量となる。 そして前記ステツプSP₁〜SP₂で第１手段F₁が
構成されている。 次にスピードS₁に対するスピード変更指示の有
無が判断されている。（ステツプSP₃） このステツプSP₃が第３手段F₃に相当する。 次にステツプSP₃の判断結果、スピード変更指
示有の場合、変更後のスピードS₂から時間ｔにお
ける移動量（ｔ・S₂）を求め、さらには変更のあ
つた指令位置Pi₀と位置Pbとの位置情報の差に対
する移動量（ｔ・S₂）ごとの変更後きざみ量
Nnewを、 Nnew＝（Ｎ−i₀）×（ｔ・S₁）／（ｔ・S₂） として求め、また後述するステツプSP₅における
位置Paからのきざみ数ｉを０にセツトし直して
おくとともに、位置Paを変更のあつた位置Pi₀に
置換しておく。（ステツプSP₄） このステツプSP₄が第４手段F₄に相当する。 次にステツプSP₃の判断結果、スピード変更指
示無の場合、あるいはステツプSP₄を経由した後
は、きざみ数ｉを（ｉ＋１）とし、時間ｔごとの
指令位置Piを、 Pi＝（Ｎ−ｉ）Pa＋iPb／Ｎ として求める。（ステツプSP₅） このステツプSP₅が第２手段F₂に相当する。 次に指令位置Piを直系から多関節座標系（以下
α系）へ座標変換する。（ステツプSP₆） 次に両指令位置Pi，Pi＋₁間のα系における位
置情報の差を各制御軸α₁〜α₇ごとに、Δα₁〜Δα₇
として求める。（ステツプSP₇） 次に各位置情報の差Δα₁〜Δα₇に対する各制御
軸α₁〜α₇の一定時間（例えば0.1t）における最高
速時での回動角ΔM₁〜ΔM₇ごとの各きざみ量Q_k
を、 Q_k＝Δα_k／ΔM_k（ただしｋ＝１〜７） としてそれぞれ７つの制御軸分を求める。（ステ
ツプSP₈） 次に７つのQ_k値の最大値Q_naxを求める。（ステ
ツプSP₉） 次に最大値Q_naxが予め設定した値Q₀（前記一定
時間0.1tに対応して10）より大か否かを判定す
る。（ステツプSP₁₀） そして最大量Q_nax＞Q₀ならば、Ｑ＝Q_naxとす
る。（ステツプSP₁₁） また最大値Q_nax≦Q₀ならば、Ｑ＝Q₀とする。
（ステツプSP₁₂） そして各制御軸α₁〜α₇ごとに（Δα_k／Ｑ）の各値 を（ΔD_k余りβ_k）として求め、ｊをｊ＝０にセツ
トする。（ステツプSP₁₃） 次にステツプSP₁₄において、各制御軸α₁〜α₇
ごとに、余りβ_kの振り分けを行なう。すなわち後
述するステツプSP₁₅におけるR_kjの値（ｋ軸のｊ
番目の値）を求める。ただしｋ＝１〜７、ｊ＝１
〜Ｑである。まず (1) β_k＝０の場合、 （１≦ｊ≦Ｑ）のすべてのｊの値に対して、
R_kj＝０とする。 (2) β_k＞Ｑ／２の場合、 (イ) （１≦ｊ≦Ｑ）の奇数のｊの値に対して、
R_kj＝１とする。 (ロ) ｛２≦ｊ≦２（β_k−Ｑ／２）｝の偶数のｊの
値 に対して、 R_kj＝１とする。 (ハ) 前記(イ)，(ロ)以外のｊの値に対して、R_kj＝
０とする。 (3) β_k≦Ｑ／２の場合、 （Ｑ／β_k）＝ΔQ_k余りγ_k）を求める。そして（ｊ＝ ａ・ΔQ_k＋１）のｊの値に対して、R_kj＝１と
する。ただしａは、 ｛０≦ａ≦（β_k−１）｝の整数 その他のｊの値に対して、 R_kj＝０とする。 次にｊ値を（ｊ＝ｊ＋１）とし、各制御軸α₁〜
α₇ごとに、α系での指令位置P_kj（ｋ軸でｊ番目の
位置）を、 R_kj＝P_k（ｊ−₁）＋ΔD_k＋R_kj （ただしｋ＝１〜７、ｊ＝１〜Ｑ） として求める。（ステツプSP₁₅） ここで前記ステツプSP₁₀〜SP₁₅の内容をより
理解し易くするため数値を用いて具体的にR_kjの
値を求めてみよう。なおここでは制御軸α₁につい
て説明する。 (a) 今Δα₁＝50、しかもQ_nax＜Q₀であつて、Ｑ＝
Q₀＝10、であつたとすると、ステツプSP₁₃に
おいて （Δα₁／Ｑ）＝50／10＝５ すなわちΔD₁＝５、Ｂ＝０、という値が得ら
れる。 するとステツプSP₁₄において、β₁＝０の場合
に該当するため、（１≦ｊ≦Ｑ）のすべてのｊ
の値に対してR_1j＝０とする。 よつてステツプSP₁₅において、P_1jは、 P_1j＝P_1(j-1)＋ΔD₁＋R_1j ＝P_1(j-1)＋５＋０ ＝P_1(j-1)＋５ ということになり、第３図ロのような等ピツチ
で指令位置P_1jが求められる。 (b) またΔα₁＝57、しかもQ_nax＜Q₀であつて、Ｑ
＝Q₀＝10、であつたとすると、ステツプSP₁₃
において （Δα₁／Ｑ）＝57／10＝５余り７ すなわちΔD₁＝５、β₁＝７という値が得られ
る。するとこのβ₁値はステツプSP₁₄において、
β₁＞Ｑ／２の場合に該当するため、 (イ) ｊが（１≦ｊ≦10）の奇数値に対しては、 R_1j＝１ すなわちR₁₁，R₁₃，R₁₅，R₁₇，R₁₉＝１と
し、 (ロ) ｊが｛２≦ｊ≦２（β₁−Ｑ／２）｝すなわち （２≦ｊ≦４）の偶数値に対しては、 R_1j＝１ すなわちR₁₂，R₁₄＝１とし、 (ハ) 前記(イ)，(ロ)以外のｊの値に対しては、 R_1j＝０ すなわちR₁₆，R₁₈，R₁₁₀＝０とする。 よつてステツプST₁₅において、P_1jは、下記第１
表のようになり、第３図ハのようなピツチで指令
位置P_1jが求められ、両指令位置P_1(j-1)とP_1jとの
間隔がほぼ均等となるように、余りβ₁が振り分け
られる。

【表】 (c) さらにΔα₁＝54、しかもQ_nax＜Q₀であつてＱ
＝Q₀＝10、であつたとすると、ステツプSP₁₃
において、 （Δα₁／Ｑ）＝54／10＝５余り４ すなわちΔD₁＝５、β₁＝４という値が得られ
る。するとステツプSP₁₄において、β₁≦Ｑ／２の 場合に該当する。 すると （Ｑ／β₁）＝10／４＝２余り２ となり、ΔQ₁＝２、γ₁＝２という値が得られ
る。よつてｊが（ｊ＝ａ・ΔQ₁＋１）の値に対
しては、｛ただしａは、０≦ａ≦（β₁−１）｝ R_1j＝０ すなわちR₁₁，R₁₃，R₁₅，R₁₇＝１とし、 その他のｊの値に対しては、 R_1j＝０ すなわちR₁₂，R₁₄，R₁₆，R₁₈，R₁₉，R₁₁₀＝０
とする。 よつてステツプST₁₅において、P_1jは下記第
２表のようになり、第３図ニのようなピツチで
指令位置P_1jが求められ、両指令位置P_1(j-1)と
P_1jとの間隔がほぼ均等となるように、余りβ₁
が振り分けられる。

【表】 (d) さらにはQ_nax＞Q₀の場合にあつては、Q₀よ
り大なるQ_kとなつた制御軸において、Q₀ごと
に指令位置P_kjを出力しても、そのサーボ系が
スピードにおいてついていけないため、Q_nax
ごとに指令位置P_kjを求めるべく考慮されてい
る。その具体的指令位置P_kjの求め方について
は、前記(a)〜(c)に準じて説明できるので、その
説明は省略する。 以上(a)〜(d)の説明により、前記ステツプSP₁₀
〜SP₁₅の内容は容易に理解できよう。このよう
にして制御軸α₁の他、α₂〜α₇についてもそれぞれ
指令位置P_kjが求められる。 そして前記ステツプSP₁₅で求めた各位置P_kjの
情報が各サーボ系Sα₁〜Sα₇に出力され、ロボツ
ト１およびポジシヨナー２は位置制御される。
（ステツプSP₁₆） 次に位置P_i+1（α系）に到達したかどうかを判
断する。（ステツプSP₁₇） そして位置P_i+1に到達していなければ、ステツ
プSP₁₅，SP₁₆を繰返す。 また位置P_i+1に到達すれば、今度は位置Pd（α
系）に到達したかどうかを判断する。（ステツプ
SP₁₈） そして位置Pbに到達するまで、ステツプSP₃〜
SP₁₈を繰返す。 以上のようにして位置PaからPbまでの間をロ
ボツト１およびポジシヨナー２は位置制御され、
しかも途中でスピードS₁に対してスピード変更指
示があつても、その変更指示のあつた位置P_i0か
ら変更後のスピードS₂で位置制御される。 前述説明は実施例であり、例えばポジシヨナー
２が廃止され、ロボツト１のみの場合にあつて
は、位置情報の差Ａは、 Ａ＝max (イ) √（−）²＋（−）²＋（
−）² (ロ) K₁√（_a−_b）²＋（_a−_b）²〕 として求めればよいし、またロボツト１がＸ，
Ｙ，Ｚ方向の３自由度しかもたない場合の位置情
報の差Ａは、 Ａ＝√（−）²＋（−）²＋（−
）² として求めればよい。さらにはステツプSP₁₄に
あつては、(1)のβ_k＝０の場合はそのままとし、
(2)，(3)の場合については、例えばβ_k≠０の場合と
し、ｊの値が１から順次R_kj＝１とし、しかもR_kj
＝１とするごとに余りβ_kを｛β_k＝（β_k−１）｝とし
て、β_kが０になれば以後のｊの値に対してはR_kj
＝０にするというパターン、すなわちｊの最初の
方でR_kj＝１、その他をR_kj＝０とするという具合
にしてもよいし、逆にｊの最後の方でR_kj＝１、
その他をR_kj＝０とするいう具合にしてもよい。
その他各構成の均等物との置換もこの発明の技術
範囲に含まれることはもちろんである。 この発明は前述したように、従来の第１手段
F₁と第２手段F₂との間、すなわちきざみ量Ｎを Ｎ＝Ａ／（ｔ・S₁） として求める手段F₁と、指令位置Piを Pi＝（Ｎ−ｉ）Pa＋iPb／Ｎ として求める手段F₂と、の間に、スピード変更
指示の有無を判断する第３手段F₃、およびスピ
ード変更指示有のときに変更後きざみ量Nnew
を、 Nnew＝（Ｎ−ｉ）×（ｔ・S₁）／（ｔ・S₂） として求め、このNnewを手段F₂のＮと置換する
とともに、位置Paをスピード変更指示のあつた
位置Ri₀と置換し、さらにはきざみ数ｉを０にセ
ツトするべくした第４手段F₄を介在させたので、
プログラム再生実行中のスピード変更が可能なこ
とはもちろん、スピード変更時に再び第１手段
F₁の複雑な演算（加減乗除の回数が多い）を行
なう必要がなく、演算時間が短縮される。よつて
次期指令位置Piを求めて、その情報をサーボ系
Sα₁〜Sα₇に出力するに要する時間が短かくて済
むから、ロボツト１が前回指令した位置に到達す
る前に次期指令位置Piを出力させるために設定す
る時間ｔは、スピード変更機能をもたない制御装
置の場合と同程度で済み、次期指令位置Piの出力
回数は前記スピード変更機能をもたない制御装置
の場合と変わらず、精度上の問題は全くない。

【図面の簡単な説明】

図はいずれもこの発明の一実施例を示し、第１
図イはポジシヨナーを含む産業用ロボツト（多関
節型）の概略説明図、第１図ロは、前記イの産業
用ロボツトの直角座標系における座標説明図、第
２図は制御装置のブロツク図、第３図イは両テイ
ーチング位置間におけるスピード変更指示無の場
合の直角座標系指令位置説明図、第３図ロ〜ニは
多関節座標系指令位置説明図、第３図ホは両テイ
ーチング位置間におけるスピード変更指示有の場
合の直角座標系指令位置説明図、第４図はフロー
チヤート、である。 図において、１……産業用ロボツト、α₁〜α₅…
…産業用ロボツト１の各制御軸、２……ワークポ
ジシヨナー、α₆，α₇……ワークポジシヨナー２の
各制御軸、３……制御装置、S₁……初期設定スピ
ード、S₂……変更後のスピード、ｔ……予め設定
した時間、Pa，Pb……それぞれテイーチング位
置、Ａ……両位置Pa，Pbの位置情報の差（最大
値）、Ｎ……位置情報の差Ａに対する時間ｔにお
ける移動量（ｔ・S₁）ごとのきざみ量、F₁……
第１手段、Pi……両位置Pa，Pb間における時間
ｔごとの直角座標系指令位置、F₂……第２手段、
F₃……第３手段、Pi₀……スピード変更指示のあ
つた直角座標系指令位置、Nnew……両位置Pi₀，
Pb間の位置情報の差に対する時間ｔにおける移
動量（ｔ・S₂）ごとの変更後きざみ量、ｉ……位
置Paからのきざみ数、F₄……第４手段、である。

Claims (1)

1. 【特許請求の範囲】 １ 初期設定スピードS₁から予め設定した時間ｔ
   における移動量（ｔ・S₁）を求め、さらに両テイ
   ーチング位置PaとPbとの位置情報の差Ａに対す
   る前記移動量（ｔ・S₁）ごとのきざみ量Ｎを、 Ｎ＝Ａ／（ｔ・S₁） として求める第１手段と、 前記両位置Pa，Pb間における前記時間ｔごと
   の指令位置Piを Pi＝（Ｎ−ｉ）Pa＋iPb／Ｎ （ただしｉはPa位置からのきざみ数）として
   求める第２手段と、 を備えた産業用ロボツトの位置制御装置であつ
   て、前記スピードＳ１に対するスピード変更指示
   の有無を判断する第３手段と、 この第３手段の結果、前記スピード変更指示有
   の場合、変更後のスピードS₂から前記時間ｔにお
   ける移動量（ｔ・S₂）を求め、さらにはその変更
   のあつた指令位置Pi₀と前記位置Pbとの位置情報
   の差に対する前記移動量（ｔ・S₂）ごとの変更後
   きざみ量Nnewを、 Nnew＝（Ｎ−i0）×（ｔ・S₁）／（ｔ・S₂） として求め、前記第２手段におけるきざみ量Ｎを
   前記きざみ量Nnewに、また前記位置Paを前記位
   置Pi₀に、それぞれ置換するとともに、前記第２
   手段における前記きざみ数ｉを０にセツトする第
   ４手段と、を前記第１手段と第２手段との間に挿
   入した、前記産業用ロボツトの位置制御装置。