JP2006068857A - ロボット間の干渉防止制御装置 - Google Patents

Abstract

【課題】 ロボット動作中において干渉チェックを行い干渉発生を未然に防止する。
【解決手段】 各ロボットの教示プログラムをそれぞれ先読みする（Ｓ５）。現在の補間周期よりｎ補間周期後において、停止指令を出したときの停止予定位置を各ロボット毎に求める（Ｓ１２）。各ロボットの停止予定位置において干渉が生じるかチェックする（Ｓ１５）。干渉すると判断されると、現補間周期から停止指令を出力する。これによって、干渉が発生するｎ補間周期前で停止指令が出力されることにより、干渉発生を防止することができる。
【選択図】 図７

Description

本発明は、複数のロボットを同時に動作させ、その動作領域が一部重なるような動作をさせるときのロボット間の干渉防止制御装置に関する。

複数のロボットで共同作業を行うとき、各ロボットの動作領域が重なり合うような場合、ロボット同士を干渉させないようにしなければならない。
このロボット間の干渉チェック方法として、ロボットの腕の各リンク部を線分で表し、ロボット動作シミュレーションを行い、２線分の最も近い部分間の距離を求め、該距離が所定の距離Dよりも小さくなったら、対応するロボットの腕が干渉する可能性ありと判定する手法が知られている（特許文献１参照）。

特開昭６０−９９５９１号公報

上述した特許文献１に記載された干渉チェック方法は、オフラインで、ロボットの動作シミュレーションを行うことによって、干渉チェックを行うものであり、実際にロボットを駆動しているときに干渉をチェックするものではない。現在の計算機能力の向上に伴い同様の計算をリアルタイムで動的に計算することが考えられる。その場合、干渉する直前に干渉する可能性ありと判定されても、減速しきれずに干渉してしまっては意味がないので、上述した干渉チェックの所定の距離Ｄを、予め減速に必要な距離だけ大きな値に設定して、早めに干渉可能性を判断する必要がある。

また、ロボットの腕同士が高速ですれ違うような場合、減速に必要な距離を見込んだ所定の距離Ｄで２線分間の最小距離を判定したとしても、実際には干渉しないにもかかわらず、干渉と判定されてしまう恐れがある。

そこで、本発明の目的は、ロボットを実際に動作させている状態において、干渉チェックを行い干渉発生を防止し、かつ干渉の誤判定をなくしたロボット制御間の干渉防止装置を提供することにある。

本願請求項１に係る発明は、夫々の動作領域が重なる状態で設置されたロボット同士の動作中の接近を検出し、干渉に至る前に停止させるロボット間の干渉防止制御装置において、対象となる各ロボットの実行中の教示プログラムを先読みして前記各ロボットに現在より少なくとも１補間時間以上後の所定の時刻に停止指令を行った場合の停止予定位置をそれぞれ補間周期毎求める計算手段と、求められた各停止予定位置において前記各ロボットの可動部間の干渉有無を判定する干渉判定手段と、前記干渉判定手段が干渉有りと判定したときロボットに減速開始を指令する手段とを備え、ロボット動作中に干渉チェックを行い干渉発生を未然に防止するようにしたものである。

又、請求項２に係る発明は、夫々の動作領域が重なる状態で設置されたロボット同士の動作中の接近を検出し、干渉に至る前に停止させるロボット間の干渉防止制御装置において、対象となる各ロボットの実行中の教示プログラムを先読みして前記各ロボットの位置および姿勢を時刻ｔの関数で表し、該関数を使って前記各ロボットに現在より少なくとも１補間時間以上後の所定の時刻に停止指令を行った場合の停止予定位置を補間周期毎それぞれ求める計算手段と、求められた各停止予定位置において前記各ロボットの可動部間の干渉有無を判定する干渉判定手段と、前記干渉判定手段が干渉有りと判定したときロボットに減速開始を指令する手段とを備え、ロボット動作中に干渉チェックを行い干渉発生を未然に防止するようにしたものである。

又、請求項３に係る発明は、請求項２に係る発明において、前記計算手段は、前記現在より少なくとも１補間時間以上後の所定の時刻から停止するのに必要な時間が経過した後の時刻での前記関数値により停止予定位置を計算するようにしたものである。
請求項４に係る発明は、請求項２，３に係る発明において、ロボットの位置および姿勢が時刻ｔの関数の線形和で表されるものとした。又、請求項５に係る発明、前記干渉判定手段からの判定結果を外部に信号出力する手段を備えるものとした。更に、請求項６に係る発明は、干渉が発生する恐れのある周辺機器の形状位置データを予め設定しておき、前記干渉判定手段でロボットと周辺機器の干渉も判定するようにした。

本発明は、停止指令を出力してロボットが到達する予定の位置に基づいて干渉の有無を判定し、干渉すると判定されたときには、停止指令を判定した位置より１以上の補間周期前に出力するので、干渉を確実に防止でき、干渉しないはずのケースで、誤って干渉すると判定されることがない。ロボット同士が高速ですれ違う場合や、互いに遠ざかるような場面で、従来であれば干渉すると判定されていたような場合でも正しく判定することが可能となる。これにより、ロボット同士が干渉しないことを保証しながら、従来の干渉防止方法よりも接近した状態でロボットを使用することが可能となる。

以下、本発明の一実施形態について図面と共に説明する。
まず、本実施形態の動作原理について説明する。本発明は、複数のロボットで、協同作業を行うときのロボット間等の干渉を防止するものであり、各ロボットが動作中、今この瞬間に停止指令を出力したとき、ロボットが減速して停止するであろう位置を計算により求め、その停止位置において他のロボット及び周辺機器等と干渉していなければ、干渉しないと判定するものである。この干渉判定計算を補間毎に行い、干渉すると判定された場合には、ただちにロボットに停止を指令して減速停止させるようにするものである。しかし、実際には、停止位置で干渉すると判定されてから、減速を開始しても停止位置で干渉してしまうことになるので、停止位置の計算は１補間以上先行して実行される。すなわち、各ロボットの動作プログラムを先読みし、現在実行している補間時よりも設定された補間周期数後の補間から停止を開始したときに停止するであろう位置を前もって計算により求め、その停止位置において他のロボットもしくは周辺機器等と干渉していなければ、干渉しないと判定し、今回の補間位置への動作指令を出力する。一方、干渉すると判断されたときには、現時点の補間から所定補間周期数遅れて停止処理を開始しても干渉してしまうことがわかっているので、今回の補間から停止処理を開始する。これによって、干渉発生を未然に防止するものである。

そのために、本実施形態では、図１に示すように、各ロボットの軌跡が関数Ｘ（ｓ）で表わされているとする。ここで、関数Ｘ(ｓ)の値は、三次元空間内で位置と姿勢を特定できる６次元のベクトルとする。また、関数Ｘのパラメータｓは、軌跡に沿っての距離（移動量）とする。例えば全長がＬの曲線を所要時間Ｔで始点から終点に向かう場合を考えると、始点では時刻ｔ＝０で曲線上の位置ｓ＝０、終点では時刻ｔ＝Ｔで曲線上の位置ｓ＝Ｌと表記できる。

図２に、時刻ｔと曲線上の位置（移動量）ｓの関係を示す。関数ｓ（ｔ）は単調増加な関数であり、軌跡に沿ってのロボットの動きを表している。ｓのｔに関する導関数s'（ｔ）は時刻ｔにおける線速度、また、２階導関数ｓ煤iｔ）は加速度にそれぞれ対応する。ロボットの動きは加速度まで連続であると仮定し、関数ｓ（ｔ）は２階微分連続な関数とする。関数Ｘ（ｓ）と、関数ｓ（ｔ）を合成すると、時刻ｔにおけるロボットの位置および姿勢は、Ｘ（ｓ(ｔ)）と表記できる。そして、ある時刻ｔ0において、減速停止指令を出力し、そのときの制動距離がＬ stopとし、制動時間がＴ stopとすると、停止位置は
Ｘ（ｓ(ｔ0)+Ｌ stop）となる。又は時間(ｔ0＋Ｔ stop）により簡易的に停止位置Ｘ（ｓ(ｔ0＋Ｔ stop）)として求めてもよい。各ロボット毎この位置を求め、求められた位置に基づいて、干渉チェックを行い、干渉が生じると判断されたときは、減速停止指令を出すようにする。しかも、教示プログラムを先読みしておき、現在の補間よりも設定数後の補間時で減速停止指令出したときの停止位置Ｘ（ｓ(ｔ0)+Ｌ stop）又はＸ（ｓ(ｔ0＋Ｔ stop）)を求めて、干渉すると判断されたときは現時点で減速停止指令を出すことによって、干渉発生を防止する。

そのために、本実施形態では、教示プログラムを先読みして、ロボットの軌跡の関数Ｘ（ｓ）を求める。一般に、与えられた教示点列Ｐ₀，Ｐ₁，Ｐ₂，…Ｐ_N-1の近くを通る滑らかな曲線を得る手法として、Bezier, Spline やNURBSなど様々な方法が知られている。本実施形態では、４階（３次）のB-Splineを使い、実行中の動作プログラムにより逐次的に供給される教示点列Ｐ₀，Ｐ₁，Ｐ₂，…Ｐ_N-1から、時刻ｔにおける位置姿勢の関数Ｘ（ｔ）を得る。

Ｎ個の点列Ｐ₀，Ｐ₁，Ｐ₂，…Ｐ_N-1が与えられると、式（１）に示すように、４階のB-Spline曲線は、時刻ｔの関数Ｎ_j,4（ｔ）（ｊ＝０，１，…Ｎ−１）の線形和により定義される。

但し、ノットベクトルＴ＝［ｔ₀，ｔ₁，ｔ₂，…ｔ_N+3］である。
ここで、Ｎ_j,M（ｔ）は、Ｍ階のB-Spline基底関数であり、式（２−１）、（２−２）の
De Boor Coxの漸化式から生成される。

ノットベクトルＴ＝［ｔ₀，ｔ₁，ｔ₂，…ｔ_N+3］は、個数Ｎ＋Ｍ個からなる任意の単調増加な数列である。与えられる教示点列Ｐ₀，Ｐ₁，Ｐ₂，…Ｐ_N-1が同じであっても、ノットベクトルの定義により曲線形状が変わることが知られているが、ここでは簡単の為以下、ノットベクトルの要素ｔ_iを式（３）のように定義する。式（１）の曲線Ｘ（ｔ）は、始点側と終点側からそれぞれ３つのノットを除くｔ₃≦ｔ＜ｔ_N、すなわち、０≦ｔ＜Ｎ−３の範囲で定義されていることになる。ｔは、ノット上で整数の値をとるが、それ以外での点では実数値をとる。
ｔ_i＝ｉ−３ （ｉ＝０,１,２,３,…Ｎ,Ｎ＋１,Ｎ＋２,Ｎ＋３） …（３）

図３にＮ＝６の例を示す。
式（２−１）、（２−２）もしくは、図３より、４階のB-Spline基底関数Ｎ_j,4（ｔ）は、ノット４区間分の範囲を除いて他は０となる。この性質を利用して式（１）を計算すると、予め全ての教示点Ｐ₀，Ｐ₁，Ｐ₂，…Ｐ_N-1を与える必要はなく、最初の４点から最初の曲線区間ｔ₃≦ｔ＜ｔ₄、すなわち、０≦ｔ＜１で有効な次の式（４）が得られる。

一般に、ある区間ｔ_a≦ｔ＜ｔ_bで有効な関数を得るには、それを含む最小のノット区間を求め、その範囲の曲線を定義するのに必要な項だけで和をとればよい。

教示プログラムを先読みして、４つの教示点より該４つの教示点の近くを通り滑らかな曲線の関数Ｘ（ｔ）を式（４）によって求め、該関数Ｘ（ｔ）を曲線に沿って、式（５）に示す線積分を行うことによって、曲線上の位置（移動量）ｓの関数として表されるロボットの軌跡の関数ｓ（ｔ）求める。

この関数ｓ（ｔ）の物理的意味は時刻ｔまでの曲線上の移動距離であるから、逆に距離ｓが与えれば対応する時刻ｔが一意に定まり求まる。距離ｓにおける時刻ｔの関数をｔ（ｓ）と表記すると、関数Ｘ（ｔ）とｔ（ｓ）の合成関数としてｓとＸの関係、すなわち関数Ｘ（ｓ）は次の式（６）として求まる。
Ｘ（ｓ）≡Ｘ（ｔ（ｓ）） （６）
又、時刻ｔにおけるロボットの曲線上の位置はｓ（ｔ）、曲線上の位置がｓのときの位置および姿勢はＸ（ｓ）であることから、時刻ｔにおけるロボットの位置および姿勢はＸ（ｓ（ｔ））として計算し求めることができる。

次に、ある時刻ｔ0からロボットを軌跡に沿って減速停止させた場合の停止予定位置を計算する手順を説明する。ロボットが時刻ｔ0に、パラメータ位置ｓ(ｔ0)、線速度ｓ'(t0)で動作しているとする。このときのs(t)の導関数s'(t)のグラフを図４(a)に示す。時刻ｔ0から減速を開始しない場合は、ｔ0以降も図４(a)で示されるような線速度に従って動作を継続する予定とする。減速停止する場合は、図４(a)の速度パターンに、図４(b)に示すような１から０に滑らかに単調減少するような関数ｕ(t‐t0)を掛け合わせることにより実現する。

掛け合わせた結果を図４(c)に示す。減速に必要な制動時間Ｔ_stopは、減速関数ｕ(t)（図４(b)）の遷移時間の幅そのものであり、制動距離Ｌ_stopは、図４(c)の減速開始時刻ｔ0から停止時刻ｔ0+Ｔ_stopまでの区間積分値であり、関数ｓ(t)は式（５）によって求まり、ｕ(t)を予め設定しておけば制動時間Ｔ_stop、制動距離Ｌ_stopはともに計算でき、求めることができる。

これより、時刻ｔ0から減速を開始したときにロボットが停止する予定の位置および姿勢Ｘ(ｓ(ｔ0)+Ｌ_stop)が計算できる。ここで図４(b)の関数ｕ(t)には任意性があり、掛け合わせた結果である図４(c)のグラフが１階微分連続（加速度が連続）であれば何でもよい。一例として、図５に示すような三角形を逆さにしたような関数ｗ(t)を積分することで得られる。また、制動時間幅Ｔ_stopは、ここでは簡単のため定数とするが、より一般的には減速時にモータに加わる加速度が一定となるように、そのときのモータのイナーシャや、重力によりモータにかかるモーメントに応じて適当な値を計算することもできる。
上記の方法により、減速開始時刻ｔ0がｔ＋n・Δt（この場合ｔは、現時点の時刻を意味し、ｔ＋n・Δtは、現時点よりｎ補間周期後の時刻を意味する）の場合は、停止予定の位置および姿勢は、Ｘ(ｓ(ｔ＋n・Δt)＋Ｌ_stop) と計算できる。ここで、ｎは１以上の定数とする。また、Δtは１補間時間とする。

また、前記の停止位置の計算において、1補間以上後の所定の時刻ｔ＋ｎ・Δtから停止するのに必要な時間Ｔ_stopが経過した後の時刻ｔ＋ｎ・Δt ＋Ｔ_stopでの前記関数値により計算される位置Ｘ(ｓ(ｔ＋ｎ・Δt ＋Ｔ_stop))を停止位置とみなしてもよい。

上述したようにして干渉チェック対象のすべてのロボットについて、時刻ｔにおいて、この時刻よりｎ補間周期後の補間時（ｔ＋ｎ・Δt）に減速停止を出したときの停止予定位置を現時刻ｔの時点で判断する。そして、この各ロボットの停止位置に基づいて干渉が発生するか判断する。

干渉チェックは、特許文献１に記載されたすでに公知の干渉チェック方法を用いる。図６に、この干渉チェック方法の複数のリンクからなるロボットＲ１とＲ２の干渉の判定手順を示す。それぞれのロボットのリンクを線分でモデル化する。ロボットＲ１は、線分Ｌ11，Ｌ12からなり、ロボットＲ２は線分Ｌ21, Ｌ22からなる。ロボットＲ１を構成する線分とロボットＲ２を構成する線分の全てのペアについて、線分間の最近接点間の距離ｄを求める。求めた距離ｄが予め定めた閾値Ｄよりも小さい場合、干渉していると判定する。それ以外は干渉なしと判定する。

干渉なしと判定された場合は、現在の時刻ｔの補間位置への動作指令を出力し、干渉ありと判定された場合は、現補間周期から減速停止指令を出力してロボットを停止させ、干渉発生を防止する。

図１０〜図１２は、本発明のロボット間の干渉防止制御を行うシステム形態を示すものであり、図１０に示す実施形態は、１つのロボット制御装置１で複数ロボット機構部Ｒ１〜Ｒｎを駆動制御するものであり、本発明のロボット干渉制御装置は、ロボット制御装置１がその機能を備え兼ねているものである。

図１１に示す実施形態は、各ロボットＲ１〜Ｒｎ毎にそれぞれロボット制御装置１-1〜１-nがを備えるものであり、各ロボット制御装置１-1〜１-nをEthernet（登録商標）などのネットワークで接続し、ネットワークを介して互いの内部変数を通信することにより、前記停止予定位置を求める計算手段と、干渉有無を判定する干渉判定手段を、それぞれの制御装置内で計算する。もしくは、１つの制御装置でまとめて計算するものとしている。

図１２は、パーソナルコンピューターなど、外部の計算機２を使った実施形態である。計算機を、ロボット制御装置１-1〜１-nのネットワークに接続し、各ロボット制御装置１-1〜１-nの内部変数を通信することにより、前記停止予定位置を求める計算と、干渉有無を判定する干渉判定の一部、もしくはすべてを計算機２で行うようにしたものである。

図７、図８は、ロボット間干渉防止制御装置として処理フローチャートであり、図１０に示す実施形態における処理を示している。

まず、ロボット制御装置１に干渉チェックを必要とするロボット（ロボット機構部）をセットすると共に、干渉チェックを行う周辺機器等の位置形状データを入力し設定しておく。又、制動時間Ｔ stopも設定しておく。そして、ロボット動作を開始させるとロボット制御装置１のプロセッサは図７、図８の干渉チェック、干渉防止制御処理を実行する。

時刻ｔを「０」にセットし（ステップＳ１）、教示点列よりB-Splineを求める範囲開始時点ｔａに時刻ｔをセットし、範囲の終了時点ｔｂに、「ｔ＋ｎ・Δｔ＋Ｔ stop」を格納する（ステップＳ２）。なお、ｎは１以上の整数で現時点よりｎ補間周期後で干渉が発生するか否かをチェックするための補間周期数を指定するもので、予め決められているものである。又は、動作開始前に設定するようにしておくものである。

次に、範囲の開始時点ｔａ以下で、B-Splineにおける最大のノットｔ_Aを求め、範囲の終了時点ｔｂ以上で最小のノットｔ_Bを求める。なお、範囲の開始時点ｔａ以下のノットとがない場合にはｔ_A＝ｔ₃とする。又範囲の終了時点ｔｂ以上においてノットがない場合には最大ノットのｔ_Nをｔ_Bとする（ステップＳ３）。

チェック対象として設定されているロボット（ロボット機構部）を計数するカウンタＫに「１」をセットし（ステップＳ４）、該カウンタＫの値で示されるロボットが実行中のプログラムよりノットｔ_A〜ｔ_B間の教示データＰ_jを読み出す（ステップＳ５）。なお、ノットｔ_A、ｔ_Bは、式（３）に示すように、整数の値であり、その値はｔ_A、ｔ_Bの添え字Ａ，Ｂに対応することから、ノットの値を添え字のＡ，Ｂで表す。よって、プログラムから読み出される教示点Ｐ_jは、Ｐ_A-3，Ｐ_A-2，…Ｐ_B-1となる。

そして、式（４）に対応する次の式（７）より、区間ｔ_A≦ｔ＜ｔ_B間の曲線Ｘ（ｔ）を求める（ステップＳ６）。

求められた曲線Ｘ（ｔ）を式（５）により線積分し曲線上の位置ｓ（ｔ）を求める。この曲線上の位置ｓ（ｔ）は、曲線上の時刻ｔまでの移動距離であるから、逆に距離ｓが与えれば対応する時刻ｔが一意に決まる。よって、位置の関数ｓ（ｔ）から逆関数ｔ（ｓ）を求める（ステップＳ７）。

こうして求められた曲線の関数Ｘ（ｔ）と関数ｔ（ｓ）より、位置（移動距離）ｓを変数とするロボットの位置姿勢を表す関数Ｘ（ｓ）＝Ｘ（ｔ（ｓ））を求める（ステップＳ８）。更に、時刻ｔにおけるロボットの曲線上の位置はステップＳ７で求めたｓ（ｔ）であり、曲線上の位置がｓのとき、ロボットの位置姿勢は関数Ｘ（ｓ）で表されることから、時刻ｔにおけるロボットの位置及び姿勢はＸ（ｓ（ｔ））として求められる（ステップＳ９）。

ステップＳ７で求めた関数ｓ（ｔ）の導関数の線速度ｓ'（ｔ）を求め、かつ現在時刻ｔの設定補間周期数ｎ後の時刻をｔ0とし（ｔ0＝現在時刻ｔ＋ｎ・Δｔ）、関数ｕ（ｔ−ｔ0）を求め、導関数ｓ'（ｔ）に関数ｕ（ｔ−ｔ0）を乗じて減速の速度を表す線速度
ｖ_t0'（ｔ）を求める（ステップＳ１０）。

ｖ_t0'（ｔ）＝ｓ（ｔ）×ｕ（ｔ−ｔ0） （８）
該線速度ｖ_t0'（ｔ）を減速開始ｔ0から制動時間Ｔ stopまで積分し、制動距離Ｌ stopを求める（ステップＳ１１）。

現在の補間時ｔよりｎ補間周期後の時刻ｔ0＝ｔ＋ｎ・Δt で減速開始して停止する予定位置を、ステップＳ１１で求めたＬ stopとステップＳ７で求めたｓ（ｔ）及びステップＳ８で求めた関数Ｘ（ｓ）より、停止予定位置Ｘ（ｓ（ｔ0）＋Ｌ stop）＝Ｘ（ｓ（ｔ＋ｎ・Δt）＋Ｌ stop）を求め、記憶する（ステップＳ１２）。

次にカウンタＫを１インクリメントし（ステップＳ１３）、カウンタＫの値がチェック対象として設定されているロボット数を超えたか判断し（ステップＳ１４）、超えてなれば、ステップＳ５に戻り、カウンタＫの値で示される次のロボットに対して前述したステップＳ５以下の処理を実行し、現在の補間時ｔよりｎ補間周期後の時刻ｔ0＝ｔ＋ｎ・Δt で減速開始して停止する予定停止位置Ｘ（ｓ（ｔ＋ｎ・Δt）＋Ｌ stop）を求める。

以下、チェック対象のロボットの予定停止位置Ｘ（ｓ（ｔ＋ｎ・Δt）＋Ｌ stop）が求められると、ロボット間の干渉チェック及び周辺機器等との干渉のチェックを行う（ステップＳ１５）。この干渉チェックは、前述したように、すでに公知の方法によるものであり、説明を省略する。

この干渉チェックにより干渉なしと判断されたときには（ステップＳ１６）、各ロボット毎にステップＳ９で求められた時刻ｔおける補間位置Ｘ（ｓ（ｔ））への動作指令を各ロボットに出力する（ステップＳ１７）。そして、時刻ｔが最後の時刻ｔ_Nに達したか判断し（ステップＳ１８）、達してなければ、時刻ｔに補間周期分Δｔだけ加算し（ステップＳ１９）、ステップＳ２に戻り前述したステップＳ２以下の処理を行う。以下、干渉の発生が予測されなければ、時刻ｔが最終時刻ｔ_Nに達するまで、上述した処理を繰り返し実行し、最終時刻ｔ_Nに達するとこの干渉防止処理を終了する。

一方、ステップＳ１６で干渉ありと判断されたときには、現在時刻ｔを減速時の時間を示すttに初期値として設定し（ステップＳ２０）、ステップＳ１１で求めた減速動作時の速度を表す関数ｖ_t0'（ｔ）より、減速開始時をｔとし、時間の変数をttとし、この速度を表す関数ｖ_t'（tt）を積分して移動量ｖ_t（tt）を求め（ステップＳ２１）、ステップＳ８で求めた関数Ｘ（ｓ）より、ｓ＝ｖ_t（tt）としてロボットの位置姿勢Ｘ（ｖ_t（tt））を求めこの位置姿勢への動作指令をロボットに出力する（ステップＳ２２）。

そして、時間ttの値が、減速開始時ｔに制動時間Ｔ stopを加算した値に達したか判断し（ステップＳ２３）、達してなければ、時間ttに補間周期Δｔを加算し（ステップＳ２４）、ステップＳ２２に戻り前述した処理を実行し、時間ttの値が、減速開始時ｔに制動時間Ｔ stopを加算した値に達しロボットが停止するまで行う。

以上のように、現在時刻より決められた補間周期数ｎだけ後で干渉が生じると予測されたときには、現時点から減速を開始することによって、干渉発生を防止することができるものである。

なお、上述した実施形態では、ステップＳ１２で停止予定位置をｓ及び制動距離Ｌ stopの位置（移動量）によって求めたが、時間によって簡易的に停止予定位置を求めてもよい。即ち、ステップＳ１２で停止予定位置Ｘ（ｓ（ｔ0＋Ｔ stop））＝Ｘ（ｓ（ｔ＋ｎ・Δｔ＋Ｔ stop））を求めてもよいものである。

又、上述した干渉防止制御処理に加えて、図９に示すような信号出力処理を付加し、干渉チェックの判定結果を周辺機器等に出力するようにしてもよいものである。例えば、ステップＳ１６で干渉ありと判断されたときには、外部装置へ干渉ありの信号を出力してステップＳ２０に移行するようにしてもよいものである。

上述した実施形態は、図１０に示すシステム構成で１台のロボット制御装置１によって複数のロボット（ロボット機構部）Ｒ１〜Ｒｎを制御し、このロボット制御装置１によって干渉防止制御装置を構成した。図１１に示す各ロボットがその制御装置１-1〜１-nをそれぞれ備えるシステム構成の場合には、各ロボット制御装置１-1〜１-nのプロセッサが上述した図７，図８に示す処理を行うが、このときステップＳ４，Ｓ１３，Ｓ１４の処理はなく、ステップＳ１５の前で、各ロボット制御装置１-1〜１-nで求めた停止予定位置Ｘ（ｓ（ｔ0）＋Ｌ stop）＝Ｘ（ｓ（ｔ＋ｎ・Δt）＋Ｌ stop）または、Ｘ（ｓ（ｔ0＋Ｔ stop））＝Ｘ（ｓ（ｔ＋ｎ・Δｔ＋Ｔ stop））をそれぞれ交換して、ステップＳ１５の干渉チェックを行うようにする。若しくは、いずれか１つのロボット制御装置に干渉チェック対象のロボットの停止予定位置を、ネットワークを介して収集し、このロボット制御装置で干渉チェックを行い、干渉する場合は干渉するロボットさらにはシステム全てのロボットの動作を停止するようにすればよい。

又、図１２に示すシステムの場合には、各ロボット制御装置１-1〜１-nからネットワークを介して各制御装置１-1〜１-nの内部変数を通信することにより、停止予定位置の計算や干渉有無の判定等の一部を、ネットワークを介して接続された計算機２で行うようにしてもよいものである。又、計算機２により前述した図７，図８の処理を全て行うようにしてもよいものである。

ロボット軌跡の説明図である。 時刻ｔと移動距離ｓの関係の説明図である。 Ｂ-Ｓpline曲線の定義（Ｎ＝６の場合）の説明図である。 本発明の一実施形態における停止指令による減速速度の説明図である。 同実施形態における減速関数を求める方法の説明図である。 同実施形態におけるロボット間の干渉チェックの説明図である。 同実施形態における干渉チェック、干渉防止制御処理のフローチャートである。 図７に示すフローチャートのつづきである。 同実施形態において干渉チェックの結果を外部装置に出力するときの処理フローチャートである。 同実施形態におけるシステム構成図である。 本発明を適用する別の態様のシステム構成図である。 本発明を適用する更に別の態様のシステム構成図である。

符号の説明

Ｒ１、Ｒ２、…Ｒｎ ロボット（ロボット機構部）
１、１-1、１-2、…１-N ロボット制御装置
２ 計算機

Claims (6)

1. 夫々の動作領域が重なる状態で設置されたロボット同士の動作中の接近を検出し、干渉に至る前に停止させるロボット間の干渉防止制御装置において、
   対象となる各ロボットの実行中の教示プログラムを先読みして前記各ロボットに現在より少なくとも１補間時間以上後の所定の時刻に停止指令を行った場合の停止予定位置をそれぞれ補間周期毎求める計算手段と、
   求められた各停止予定位置において前記各ロボットの可動部間の干渉有無を判定する干渉判定手段と、
   前記干渉判定手段が干渉有りと判定したときロボットに減速開始を指令する手段とを備えることを特徴とするロボット間の干渉防止制御装置。
2. 夫々の動作領域が重なる状態で設置されたロボット同士の動作中の接近を検出し、干渉に至る前に停止させるロボット間の干渉防止制御装置において、
   対象となる各ロボットの実行中の教示プログラムを先読みして前記各ロボットの位置および姿勢を時刻ｔの関数で表し、該関数を使って前記各ロボットに現在より少なくとも１補間時間以上後の所定の時刻に停止指令を行った場合の停止予定位置を補間周期毎それぞれ求める計算手段と、
   求められた各停止予定位置において前記各ロボットの可動部間の干渉有無を判定する干渉判定手段と、
   前記干渉判定手段が干渉有りと判定したときロボットに減速開始を指令する手段とを備えることを特徴とするロボット間の干渉防止制御装置。
3. 前記計算手段は、前記現在より少なくとも１補間時間以上後の所定の時刻から停止するのに必要な時間が経過した後の時刻での前記関数値により停止予定位置を計算することを特徴とする請求項２に記載のロボット間の干渉防止制御装置。
4. ロボットの位置および姿勢が時刻ｔの関数の線形和で表されることを特徴とする請求項２又は請求項３に記載のロボット制御間の干渉防止装置。
5. 前記干渉判定手段からの判定結果を外部に信号出力する手段を備える、請求項１乃至４の内いずれか１項に記載のロボット間の干渉防止制御装置。
6. 干渉が発生する恐れのある周辺機器の形状位置データを予め設定しておき、前記干渉判定手段はロボットと周辺機器の干渉も判定する請求項１乃至５の内いずれか１項に記載のロボット間の干渉防止制御装置。
   

Images