(57)【要約】【課題】干渉チェックの見落としによる機器同士の干渉発生をより確実に防ぐことのできる干渉回避制御装置を得ること。【解決手段】干渉回避制御装置は、補間制御周期での停止予定位置に基づく干渉チェックに見落としが生じうるか否かを判定する第１判定手段１１と、干渉チェックに見落としが生じうると判定された見落とし区間において、１補間制御周期を整数で割った周期で停止指令を出した場合の停止予定位置を算出する第１計算手段１と、停止予定位置に基づいて動作機器と他の機器との機器間距離を算出する第２計算手段２と、３点以上の機器間距離に基づいて近似曲線を算出する第３計算手段３と、近似曲線に基づいて見落とし区間における機器間距離の最小値を算出する第１最小値算出手段４と、動作機器と他の機器とが近接した場合の近接距離と最小値とを比較して干渉の有無を判定する第２判定手段５と、を備える。【選択図】図１
2012216151.tif 000002
【特許請求の範囲】
【請求項１】
動作機器の動作範囲内での他の機器との干渉を、補間制御周期での前記動作機器の停止予定位置に基づいて検出し、前記動作機器を停止させる干渉回避制御装置であって、
前記動作機器を内包する幾何モデルおよび前記他の機器を内包する幾何モデルを第１のモデルとして設定する第１モデル設定手段と、
前記第１モデルより大きく設定されて前記第１モデルを内包する干渉判定用の第２モデルを設定する第２モデル設定手段と、
前記補間制御周期での停止予定位置に基づく干渉チェックに見落としが生じうるか否かを判定する第１判定手段と、
前記干渉チェックに見落としが生じうると判定された見落とし区間において、１補間制御周期を整数で割った周期で停止指令を出した場合の前記動作機器の停止予定位置を算出する第１計算手段と、
前記停止予定位置に基づいて前記動作機器と前記他の機器との機器間距離を算出する第２計算手段と、
３点以上の前記機器間距離に基づいて近似曲線を算出する第３計算手段と、
前記近似曲線に基づいて前記見落とし区間における前記機器間距離の最小値を算出する第１最小値算出手段と、
前記動作機器の第２モデルと前記他の機器の第２モデルとが近接した場合の近接距離と前記最小値とを比較して、前記見落とし区間での干渉の有無を判定する第２判定手段と、を備えることを特徴とする干渉回避制御装置。
【請求項２】
前記第２判定手段は、前記近接距離よりも前記最小値が小さい場合に、前記動作機器と前記他の機器が干渉すると判断し、前記動作機器および前記他の機器の少なくとも一方を停止させることを特徴とする請求項１に記載の干渉回避制御装置。
【請求項３】
前記動作機器と前記他の機器の駆動軸の最大速度出力から算出される前記動作機器と前記他の機器との相対最高接近速度をVijmaxとし、補間制御周期をtcycとし、前記動作機器と前記他の機器との停止予定位置での相対距離をLstpijとした場合に、前記第１判定手段は、以下の条件を満たす場合に前記干渉チェックに見落としが生じうるか否かを判定することを特徴とする請求項１または２に記載の干渉回避制御装置。
Lstpij＜Vijmax×tcyc
【請求項４】
前記第２モデル設定手段は、前記相対最高接近速度Vijmaxを用いて、前記動作機器の第２モデルと前記他の機器の第２モデルとのサイズの和を決定することを特徴とする請求項１に記載の干渉回避制御装置。
【請求項５】
ある時刻の補間周期の指令から補間制御周期よりも短い時間後に通過する前記動作機器の補間指令位置を少なくとも１つ以上算出する第４計算手段と、
前記動作機器と前記他の機器との間の機器間距離を、前記補間指令位置を用いて算出する第５計算手段と、
３点以上の前記機器間距離に基づいて近似曲線を算出する第６計算手段と、
前記近似曲線から前記機器間距離の最小値を算出する第２最小値算出手段と、をさらに備え、
前記第２判定手段は、前記第１最小値算出手段に算出された最小値と、前記第２最小値算出手段に算出された最小値を用いて、前記見落とし区間での干渉の有無を判定することを特徴とする請求項１に記載の干渉回避制御装置。
【請求項６】
前記動作機器に対して手動でティーチング作業を行う教示装置をさらに備え、
前記第１判定手段は、前記動作機器に対して前記教示装置からティーチング作業を行う際に、前記干渉チェックに見落としが生じうるか否かを判定することを特徴とする請求項１〜５のいずれか１つに記載の干渉回避制御装置。
【請求項７】
前記教示装置からティーチング作業を行った際に、前記第１判定手段によって、前記干渉チェックに見落としが生じうると判定され、前記第２判定手段によって干渉が有ると判定された場合に、前記第２判定手段は、前記動作機器の動作を実行させないことを特徴とする請求項６に記載の干渉回避制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、複数台のロボット同士が動作中に接近するのを検出し、衝突前に未然に停止させる干渉回避制御装置に関するものである。
【背景技術】
【０００２】
生産システム内に産業用ロボットを複数台据え付けて使用しそのロボット間の作業領域が重なるような場合には、ロボット同士の干渉を避ける必要があった。例えば、センサなどを用いて、２台以上のロボットの共通作業領域を１台のロボットが排他的に利用するインターロックを取る方式がある。インターロックを取る方式では、立ち上げ時などのユーザーによるインターロック設定ミスにより、干渉が生じてしまう場合がある。
【０００３】
また、ロボットの各要素を覆う近似立体で簡易なモデル化を行い、モデル同士の干渉を判定する方式がある。例えば、インターロックで生じていたような見落としが発生しないよう、球や円筒など簡単なモデルを用いてロボットをモデルで近似した近似立体を用いて干渉チェックする方式が特許文献１に開示されている。
【０００４】
また、ロボットのリンク部を線分としてモデル化し、線分同士の距離をオフラインでチェックする方法提案がなされている。オフラインとは違った動きが想定される場合には実時間の干渉チェックが必要となるが、ロボットの移動時の速度情報からロボットの停止予定位置を算出し停止予定位置にてロボット干渉が予測される場合には優先順位の低いロボットを停止させる技術が特許文献２に開示されている。また、同様にプログラムを先読みすることで得られるスプライン軌道から推定される停止予定位置を用いた判定手法が特許文献３に開示されている。
【０００５】
このような干渉チェックでは、補間周期ごとの停止予定位置で判定することで将来の衝突が予測できるが、ロボットの位置姿勢が急激に変化する場合には必ずしも停止予定位置だけの判定では検出が出来ない。これは判定に関して、速度が大きくなるとそれに応じて補間周期ごとにモデルの停止予定位置の移動量が大きくなるためである。モデルの移動量が、ある程度以上の大きさになると、ロボット同士がすれ違いざまにぶつかるような状況を検出できない場合がある。
【０００６】
ロボットが高速で接近する場合などの干渉チェックの見落としについて、オフラインでは移動開始時と移動終了時の物体間の最小距離が２つの物体のそれぞれのモデル評価点の最大移動長の和より大きい場合のみ干渉チェックの見落としが発生しないという条件で衝突可能性を予測する方法が特許文献４に開示されている。
【０００７】
また、実時間制御中でも評価できる方法としてモデルの移動速度や位置関係に応じてモデルサイズを拡張する方式が特許文献５に開示されている。この方式を併用することでロボット間の干渉を安全側で判定することが出来るので、干渉チェックの見落としが発生してしまうケースを低減することが出来る。
【先行技術文献】
【特許文献】
【０００８】
【特許文献１】特開昭６０−２１７４１０号公報
【特許文献２】特開平７−３１４３７８号公報
【特許文献３】特許第３９０７６４９号公報
【特許文献４】特公平７−１０７６４８号公報
【特許文献５】特許第３９７５３４１号公報
【発明の概要】
【発明が解決しようとする課題】
【０００９】
しかしながら、上記特許文献４に開示の技術によれば、ロボットの途中経路が既知で時刻ｔの関数で表現され、その関数が利用できる場合に限定されるという問題があった。また、上記特許文献５に開示の技術によれば、モデルサイズの拡張サイズは移動速度や位置から推定することになるため、設定方法への依存度が高く、ロボット同士が衝突しないにも関わらず停止指令を出してしまう場合があった。このようなモデルサイズの拡張サイズの決定方法においては、明白な基準を設定することが難しく、過不足の無い衝突判定を行うことが困難であった。
【００１０】
本発明は、上記に鑑みてなされたものであって、ロボット同士あるいは外部機器とロボットとの実時間の干渉チェックにおいて、干渉チェックの見落としによる干渉発生をより確実に防ぐことのできる干渉回避制御装置を得ることを目的とする。
【課題を解決するための手段】
【００１１】
上述した課題を解決し、目的を達成するために、本発明は、産業用ロボットを含む動作機器の動作範囲内での他の機器との干渉を、補間制御周期での動作機器の停止予定位置に基づいて検出し、動作機器を停止させる干渉回避制御装置であって、動作機器を内包する幾何モデルおよび他の機器を内包する幾何モデルを第１のモデルとして設定する第１モデル設定手段と、第１モデルより大きく設定されて第１モデルを内包する干渉判定用の第２モデルを設定する第２モデル設定手段と、補間制御周期での停止予定位置に基づく干渉チェックに見落としが生じうるか否かを判定する第１判定手段と、干渉チェックに見落としが生じうると判定された見落とし区間において、１補間制御周期を整数で割った周期で停止指令を出した場合の停止予定位置を算出する第１計算手段と、停止予定位置に基づいて動作機器と他の機器との機器間距離を算出する第２計算手段と、３点以上の機器間距離に基づいて近似曲線を算出する第３計算手段と、近似曲線に基づいて見落とし区間における機器間距離の最小値を算出する第１最小値算出手段と、動作機器の第２モデルと他の機器の第２モデルとが近接した場合の近接距離と最小値とを比較して、見落とし区間での干渉の有無を判定する第２判定手段と、を備えることを特徴とする。
【発明の効果】
【００１２】
本発明によれば、補間制御周期の停止予定位置を用いた干渉チェックだけでは見落としてしまうような機器同士の干渉を検出でき、機器同士の干渉を未然に防ぎやすくなる。また、干渉チェックの見落としが生じうるか否かを判別するので、計算負荷の増大を抑えることができる。
【図面の簡単な説明】
【００１３】
【図１】図１は、本発明の実施の形態１にかかる干渉回避制御装置の概略構成を示すブロック図である。
【図２−１】図２−１は、動作機器および他の機器としての産業用ロボットの構成例を示す図である。
【図２−２】図２−２は、産業用ロボットをロボット要素モデルとしてモデル化する例を示す図である。
【図３】図３は、干渉チェックの見落としが生じるケースの例を示す図である。
【図４】図４は、２つの球モデルが干渉する条件を説明するための図である。
【図５】図５は、第１モデルおよび第２モデルについて説明するための図である。
【図６】図６は、干渉チェックの見落としが生じうる条件について説明するための図である。
【図７】図７は、第３計算手段によって算出された近似曲線を示す図であって、干渉チェックの見落としを回避した例を示す図である。
【図８】図８は、本発明の実施の形態３にかかる干渉回避制御装置の概略構成を示すブロック図である。
【図９】図９は、本発明の実施の形態４にかかる産業用ロボットの概略構成を示す図である。
【図１０】図１０は、比較例として手先軌道をフィルタ処理により算出する手順を示す図である。
【図１１】図１１は、図１０に示す手順で算出された手先軌道を示す図である。
【発明を実施するための形態】
【００１４】
以下に、本発明の実施の形態にかかる干渉回避制御装置を図面に基づいて詳細に説明する。なお、この実施の形態によりこの発明が限定されるものではない。
【００１５】
実施の形態１．
図１は、本発明の実施の形態１にかかる干渉回避制御装置の概略構成を示すブロック図である。干渉回避制御装置は、動作機器としてのロボットと、他の機器としての他のロボットや周辺機器との干渉を推定し、停止制御などを行うことで未然に干渉（衝突）を防ぐための装置である。
【００１６】
干渉回避制御装置は、第１判定手段１１、第１計算手段１、第２計算手段２、第３計算手段３、第１最小値算出手段４、第２判定手段５、第１モデル設定手段１２、第２モデル設定手段１３、第１記憶手段６、第２記憶手段７、第３記憶手段８、第４記憶手段９、第５記憶手段１４を備えて構成される。干渉回避制御装置は、例えば図２−１に示すロボット制御装置５２に組み込まれる。
【００１７】
第１判定手段１１は、補間制御周期に基づく干渉チェックにおいて、補正制御周期の区間に干渉チェックの見落としが生じうるかを判定する。なお、以下の説明において、干渉チェックの見落としが生じうる区間を見落とし区間ともいう。
【００１８】
第１計算手段１は、現在時刻ｔ０に生成した指令値に対して補間制御周期Δｔよりも短い時間Δｔ×ｋｍ／ｎ後に停止開始した場合の補間位置Pstp(t0+Δt×km/n)を少なくとも１つ以上算出する。
【００１９】
第１記憶手段６は、算出された補間位置Pstpを停止予定位置情報１０２として受信して記憶する。第２計算手段２は、干渉する可能性があるロボット要素間のモデル間距離（機器間距離）Lstpijを補間位置Pstp(t0+Δt×km/n)から算出する。ここでi,jはそれぞれロボット要素を示している。
【００２０】
第３計算手段３は、モデル間距離Lstpijを３つ以上用いて近似曲線Lstpijdum（ｔ）を算出する。第１最小値算出手段４は、近似曲線Lstpijdum(t)から、見落とし区間におけるモデル間の最小値（最短距離）Lstpijdumminを算出する。第２判定手段５は、最小値Lstpijdumminを用いた干渉チェックを行い、必要に応じて停止指令を出す。なお、kmは添え字m=1,2,..,Nとする正数、ｎはΔｔをｎ分割する適当な正数とし、km/nは0≦km/n<1とする。
【００２１】
まず、本実施の形態１について、例えば図２−１に示すように、産業用ロボット４０を制御するロボット制御装置５２同士がＥｔｈｅｒｎｅｔ（登録商標）やＲＳ２３２Ｃなど工業用に用いられる一般的な通信手段１０によって接続された共通の作業領域を持つ２台のロボットの干渉チェックを行う場合を用いて説明する。なお、干渉回避制御装置は、例えば図２−１に示すロボット制御装置５２に組み込まれる。
【００２２】
ただし、２台のロボットの状態データ（現在位置や速度などを示すデータ）は必ずしもネットワーク上にある必要はなく、同ハードウェア（シェアメモリなど）によって共有されている構成でもよい。本実施の形態では、各ロボットがプログラムにしたがって自動運転されるときについて述べる。
【００２３】
まず、通常の実時間における（補間制御周期に基づく）干渉チェックについて説明する。そして、通常の実時間における干渉チェックでは、見落としが生じうるか否かの判定について、および、見落としが生じうると判定された場合のさらなる干渉チェックについては、後に詳説する。
【００２４】
一般にロボットは事前に教示あるいはプログラムされた目標指令値をロボット制御装置５２によって実行するために、その補間制御周期ごとに補間指令値を計算し、計算結果を駆動軸に出力する。そして、干渉チェックの方法として通常は、ロボットや周辺機器を包み込むようないくつかの幾何モデル（例えば球や円筒などの単純な幾何モデル）によってロボットや周辺機器をモデル化する。ロボットは、通常複数の球や円筒などのモデルでモデル化されるため、それぞれの幾何モデルをここでは要素モデルと呼ぶ。例えば、図２−２に示すように、産業用ロボット４０のアームやハンドなどが、ロボット要素モデル４１〜４５としてモデル化される。
【００２５】
距離判定として通常は、干渉が発生しうる全ての要素モデル同士について評価する。さらに、それぞれ出力軸の角度変化に応じて現在のモデル位置を更新し、そのモデル同士の幾何的な最近接距離を計算する。仮に、あるロボットの要素モデルＡと別のロボットの要素モデルＢについて判定する場合には、要素モデルＡのモデルサイズと要素モデルＢのモデルサイズの和Llimijが、要素モデルＡと要素モデルＢとの許容された距離である。要素モデルＡと要素モデルＢの最近接距離をLijと置くと、
Lij＜Llimij        （１）
となる場合に、要素モデルＡと要素モデルＢは干渉すると判断する。
【００２６】
一方、仮にある補間制御周期から停止指令を出した場合の停止予定位置でのモデル間距離Lstpijを計算すると、干渉が生じるかどうかを事前に判定することが出来る。この場合の判定条件は
Lstpij＜Llimij        （２）
であり、これを満たす場合には停止予定位置にて干渉を生じていることになる。
【００２７】
ただし、単純な停止予定位置の判定では、補間制御周期の刻みの大きさとモデルのサイズや形状によっては見落としを生じる可能性がある。例えば、図３に示す例では、要素モデルＡと要素モデルＢについて、簡単のため要素モデルＢのみが移動した場合に、干渉チェックの見落としが生じているケース（実際は衝突するのに衝突することが見落とされているケース）の例である。
【００２８】
すなわち、１補間制御周期あたりの停止予定位置の移動量が大きすぎるため、停止予定位置Ｐと停止予定位置Ｑでは干渉が生じていないものの、停止予定位置Ｐから停止予定位置Ｑに移動する間、例えば停止予定位置Ｒでは干渉が生じているのを見落としている。
【００２９】
本実施の形態では、このような干渉チェックの見落としが生じうるケースを、干渉チェックの見落としが発生する必要条件を用いて表現し、そのケースに該当するか否かを判定する。そして、見落としが生じうるケースでは、見落とし区間でのより詳細なモデルの動作を推定し、補間位置のモデル間距離Lstpijについて近似曲線を計算することで、停止予定位置の間で発生するモデル同士の干渉をより確実に検出する。
【００３０】
各モデル間の干渉チェックについて、速度変化が大きく干渉チェックの見落としが生じうるケースか否かを判定する第１判定手段１１について、簡単な２つの球モデルを用いて説明する。図４に示すように、２つの球モデルｉ４６と球モデルｊ４７の干渉は、２つのモデルで定義されている干渉と判定する距離LlimiとLlimjとの和Llimij（以下、干渉距離とも呼ぶ）で表現される。
【００３１】
ここで、干渉回避判定を行うモデル点について、図５に示すように、実際の物体を最小で包含する第１モデルのモデルサイズ（Ｒ１、Ｒ２）、Ｒ１、Ｒ２の周りをそれぞれ包含して干渉チェック判定に用いる第２モデルのモデルサイズ（Ａ１、Ａ２）、それぞれのモデルの移動速度（Ｖ１、Ｖ２）を定義する。この第１モデルは、第１モデル設定手段１２で設定されて、第５記憶手段１４に記憶される。また、第２モデルは、第２モデル設定手段１３で設定されて、第５記憶手段１４に記憶される。そして、干渉距離Llimijは、以下のように表現される。
Llimij＝Ａ１＋Ａ２        （３）
【００３２】
ここで、図６に示すように、要素モデルとして球モデル４６と球モデル４７を想定する。また、球モデル４６と球モデル４７とを、第１モデル同士が近接する距離、すなわちＲ１＋Ｒ２となるように配置した場合の球の中心と、球モデル４６と球モデル４７とを第２モデル同士が近接する距離、すなわちＡ１＋Ａ２となるように配置した場合の球の中心とが、一直線に並ぶような場合を想定する。そして、この場合の、Ａ１＋Ａ２となる距離に配置された２球間の中心間の距離をDlimと定義する。
【００３３】
距離Dlimとなるような球モデル４７の位置を停止予定位置として干渉チェックする状況は、実際のモデル領域(包含する最小のサイズ)と第２モデル領域を設定する場合に、最も検出しにくい干渉チェックの境界状態となる。すなわち、距離Dlimは、要素モデルの移動距離として、干渉チェックの見落としが生じうる最小値となる。この距離Dlimは、以下の式で算出される。
【００３４】
【数１】
2012216151.tif 000003
【００３５】
このとき、干渉チェックの見落としが生じうる場合というのは、２モデルの速度Ｖ１，Ｖ２の相対速度としてVijとするときに、次の条件式を満たすときである。ただし、補間制御周期をtcycと置く。
Vij×tcyc≧Dlim        （５）
【００３６】
式（５）に示す条件が、モデル同士が干渉チェックに見落としを生じさせる条件となる。つまり、この条件が干渉チェックに見落としを生じうる必要条件となる。各モデルについて、式（５）についてその境界条件となるVijの最小値Vijminを干渉チェックよりも前に求め、これ以上の速さで接近する場合があるかどうかをチェックし、その場合のみ干渉チェックをより細かく行うという処理を行えば良い。ここで、Vijminは次のように定義される。
Vijmin＝Dlim／tcyc        （６）
【００３７】
ここで、チェックするモデルの相対速度Vijは、各モデルの判定に用いるモデル位置の各周期ごとの差分によって求められる移動量を、補間制御周期で割った量ということになる。例えば、停止予定位置について必要なモデルを考えると、停止予定位置の場合のモデルの相対速度Vijは、モデルの停止予定位置の差分で以下のように求められる。ただし、ΔPi(t)，ΔPj(t)は相対移動を表すベクトルである。
ΔPi(t)＝（Pstpi(t)−Pstpi(t＋Δt)）        （７）
ΔPj(t)＝Pstpj(t)−Pstpj(t＋Δt)        （８）
ΔPij(t)＝｜｜ΔPi(t)−ΔPj(t)｜｜        （９）
Vij(t)＝ΔPij(t)／tcyc        （１０）
【００３８】
式（９）で表されるΔPij(t)を補間制御周期tcycで割った量をモデル相対移動速度とする。第１判定手段１１は、算出された停止予定位置の相対移動速度Vijがある量を超えると干渉チェックもれの可能性がある、と判定することになる。（ただし、上記速度の定義は表現し換えて、式（９）の距離表現で良い。その場合はDlimとΔPij(t)を比較することになる。）
【００３９】
本実施の形態で説明した方式はモデルの形状が長さＬで半径Ｒの円筒の場合でも、実物を最小包含している第１モデルと第１モデルの周りを包含する第２モデルが定義されていれば、半径Ｒの球と同じ方法で定義される。以下、判定に関するモデルは全て第２モデルで行うものとする。
【００４０】
計算コストを小さくする目的で、上記のような場合以外は出来るだけ計算を行わないようにするため、現在の２つのモデルが「接近中」であるかどうかを判別する方法を示す。具体的には、相互の停止予定位置から計算される相対距離Lstpijが次の相対最高接近速度Vijmaxに基づいて算出される接近状態距離La(＝Vijmax×tcyc)よりも接近している場合の干渉チェック判定については、第１判定手段１１が計算を行うものとする。
Lstpij(t)＜Vijmax×tcyc        （１１）
【００４１】
なお、式（１１）の右辺は、モデル生成時に既知であるので、動作前に計算可能な固定値となる。上記、干渉チェックの見落としが生じうる必要条件を満たすことが判明しているモデルが存在する場合に、その当該モデル間では、通常の干渉チェック処理ではなく、次のような処理を実施することで干渉チェックの見落としを防ぐ。干渉チェックの見落としを防ぐ処理について、図１、図２−１等を用いて以下に説明する。
【００４２】
ある停止予定位置Pstpが生成される時刻をt0とおくと、このとき同時に時刻t0から補間制御周期Δｔよりも短い時間Δｔ×km/n後のロボットの停止予定位置Pstp(t0＋Δｔ×km／n)をそれぞれのロボット制御装置５２内で計算する。
【００４３】
これは、ロボットの制御補間方法においてΔｔ毎に計算している補間指令値と同様の計算方法で、次補間周期ｔ＋Δｔの補間指令値を出力せずに停止を開始した場合に速度が0になり最終的に停止する停止予定位置についてΔtより細かい補間制御周期で制御する場合の停止予定位置を算出していることになる。このΔtより細かい停止予定補間位置について、t0時点で計算した停止予定位置を、ｔ0ステップ作成の停止予定補間位置と呼ぶ。
【００４４】
求められた停止予定位置および停止予定補間位置を停止予定位置情報１０２として第１記憶手段６に記憶しておき、外部通信時にはこの第１記憶手段６からデータを読み出して、外部のロボット制御装置５２にデータ送信する。また停止予定位置を停止予定位置情報１０２として、外部のロボット制御装置５２との通信手段によって取得した他のロボットのモデル位置をモデル位置情報１０３として第２計算手段２に入力して、時刻t0＋Δｔ×km／nにおけるモデル間距離Lijを計算する。モデル間距離Lijは、モデル間距離情報１０５として第３計算手段３に入力される。なお、本実施の形態では、他のロボットのモデル位置情報１０３は、第２記憶手段７に記憶されているものを読み出して用いているが、モデル間距離Lijを算出するたびに、通信手段を介して外部のロボット制御装置５２から取得してもよい。また、モデル間距離情報１０５は、第３記憶手段８に入力されて記憶される。
【００４５】
t0ステップ作成の補間指令値の生成時に同時に求められたモデル間距離Lij(t0＋Δｔ×km／n)、およびt0−Δtのステップにて求められたモデル間距離Lij(t0−Δｔ×km／n)のうち、３つ以上の点を用いて第３計算手段３にてLijの近似曲線Lstpijdumを計算する。
【００４６】
このとき、例えば近似曲線としてｎ次多項式を用いたとする場合に、一般に次の形で表される。
【数２】
2012216151.tif 000004
【００４７】
これについて、t＝t＋Δt×km／nのデータLij(t＋Δt×km／n)を用いて最小二乗法を用いてｎ次方程式を解くことができる。ｎ次は取得できる点データの数つまり計算負荷に応じて、小さい次数を選択することになるが２次以上であれば近似曲線による干渉チェック判定は可能である。ここでは簡単のため２次関数を用いて説明する。例えば、簡単のためn=2として算出する場合を説明する。まず、Pstp(t0)とPstp(t0+Δt)のデータの中間時刻で停止した場合、つまりk1=1としてPstp(t0+Δt×km/n)を算出し、Pstp(t0)、Pstp(t0+Δt×k1/2)、Pstp(t0+Δt)から2次関数を算出する。また、nを大きくし、kmのmの個数を増やし最小時情報を用いて解くこともできる。
【００４８】
このモデル間距離近似曲線Lstpijdum(t)について、第１最小値算出手段４で、Lstpijdum(t)の下に凸である極値あるいは定義域（t0≦t≦t0＋Δt，見落とし区間）の端で最も小さいLstpijdum(t)の値が最小値として算出され、最小値情報１０７として出力される。
【００４９】
ここでは、Lstpijdum(t)を２次関数として扱っているため、近似曲線Lstpijdum(t)の極値はたとえば次式（１４）の形に変形して求めることが出来る。式（１４）のtminは、Lstpijdum(t)を微分した式を0とする関係式からも求められる。ただし、Ａ、ＢはLstpijdum(t)を式変形した際に表れる定数で、tminは最小値をとるtを表す。特に、ＢはLstpijdum(t)の定義域における極値である。最小値は定義域内に極値が無い場合は左端か右端の値をとり、定義域内に極値がある場合は極値が最小値となる。
Lstpijdum(t)＝Ａ（t−tmin）２＋Ｂ        （１４）
【００５０】
t0からt0＋Δtにおけるモデル間距離の最小値を最小値情報１０７とし、モデル間距離制限値（干渉距離Llimij）をモデル間距離制限値情報１０１とする入力として第２判定手段５において干渉チェックを行い、モデル間距離制限値よりもモデル間距離の最小値が小さい場合には停止指令を出す。このとき、判定対象である外部の産業用ロボット４０に対してもロボット制御装置５２の通信手段を介して同時に停止指令が出力される。本実施の形態では、停止指令は第２判定手段５から出力される。すなわち、第２判定手段は停止制御手段としても機能する。
【００５１】
以上説明したように、例えば図７に示すように、２つのモデル間の距離Lijをグラフにした場合、通常の停止予定位置だけでは閾値を下回らないために実際には干渉が発生するにもかかわらず干渉チェックでは干渉無しと判定されるケースであっても、本実施の形態にかかる干渉回避制御装置によればその干渉を検出することができる。
【００５２】
また、干渉チェックの刻みを細かくする（近似曲線を生成する）のは、それが必要と判定された場合のみに行われるため計算負荷を必要最小限にとどめることが出来る。このような、実施の形態１を行うことで、ロボット干渉チェックにおいて見落としていた補間制御周期間の干渉現象が厳密に判定されるため、共通作業空間が大きくより干渉チェックによる判定が頻繁に行われる場合にも過剰な判定を避けることができ、かつお互いの動作が高速であってもより確実に干渉チェックで干渉を未然に検出し、産業用ロボット４０を停止させることができる。
【００５３】
実施の形態２．
本実施の形態は、実施の形態１で説明した式（４），（５）で表されるモデル同士が干渉チェックもれを生じさせる条件を用いて干渉チェックモデルサイズを決定しておき、それに基づいて干渉チェック判定を行うものである。なお、上記実施の形態と同様の構成については、同様の符号を付して詳細な説明を省略する。
【００５４】
本実施の形態では、Ａ１＋Ａ２を未知数とし、各モデル間でＲ１，Ｒ２と最大のモデル間の接近速度から２つの第２モデルサイズの和Ａ１＋Ａ２を算出し、第２モデルサイズに基づいて干渉チェックを実施するものである。このとき、モデル間の接近速度の最大値をVijmaxと置くと、Ａ１＋Ａ２は次のように計算できる。
【００５５】
【数３】
2012216151.tif 000005
【００５６】
このとき、式（４）’で計算された値を式（３）に代入して、式（２）の条件を満たす場合を干渉するものと判別することになる。駆動軸の最高速度を出力した場合の各モデルの最大移動速度から、予めモデル同士の最高接近速度を算出し、それに基づいてモデルの必要サイズを決定する方法を実施し、決定されたモデルサイズで干渉チェックを行い干渉するか否かの判定を行う。
【００５７】
この方法を実施すると、本方式で設定された干渉チェックモデルサイズを適用する場合に追加してモデルサイズを変更することなく、全ての場合において停止予定位置から干渉チェックを行うことができる。本実施の形態を実施することで、従来行われているモデル間の距離Lstpijに基づく判定では生じてしまう干渉チェックの見落としを抑えることができる。
【００５８】
また、予め各軸の駆動最大速度が分かっている場合は、その速度の値に基づいて、干渉が生じないサイズＡ１＋Ａ２を計算することで、駆動軸の最高速度で判別するよりも小さいモデルで第２モデルのサイズを決定することができる。なお、ここでは、第２モデルのサイズは、Ａ１、Ａ２が別々に算出されるのではなく、Ａ１＋Ａ２として算出されるので、Ａ１およびＡ２のサイズは相対的なものとして算出されることとなる。具体的には、駆動軸の最大速度から計算されるV’ijmaxを用いて、Ａ１＋Ａ２を算出する。
【数４】
2012216151.tif 000006
【００５９】
このとき、式（４）’’で計算された値を式（３）に代入して、式（２）の条件を満たす場合を干渉するものと判別することになる。本実施の形態では、判定自体は従来の干渉チェックと同様の計算コストで停止予定位置だけでは判定できなかった干渉チェックもれを加味するのに十分なモデルサイズを設定することが出来るため、従来と同様の計算コストで確実に干渉チェックを行うことが出来る。なお、第２モデル設定手段１３によって、相対第２モデルのサイズが設定される。ただし、サイズが常に大きい場合は判定が過剰になる場合もあるため、判定モデルサイズを大きくするのは実施の形態１の式（１１）と同様の基準を設け、式（１１）の条件が成り立つ場合のみモデルサイズを変更する。
【００６０】
実施の形態３．
図８は、本発明の実施の形態３にかかる干渉回避制御装置の概略構成を示すブロック図である。なお、上記実施の形態と同様の構成については、同様の符号を付して詳細な説明を省略する。
【００６１】
本実施の形態では、実施の形態１の処理に加えて、さらに時刻tにおける補間指令位置のモデル間距離Lij(t)に関して干渉チェックを行う処理を加える。補間指令位置とは、ある時刻t0に通過する補間軌道上の指令位置のことで、ある時刻t0の補間周期の指令から補間制御周期Δｔよりも短い時間Δｔ*km/n後に通過するロボットの補間指令位置Ppath（t0+Δｔ×km／n）を少なくとも１つ以上算出する第４計算手段３１を用いて補間指令位置を求める。
【００６２】
算出した補間指令位置については補間指令位置を記憶する第６記憶手段３５に記憶しておく。また、干渉する可能性があるロボット要素間のモデル間距離Lij(t0＋Δt×km／n)を、補間指令位置Ppath(t0＋Δｔ×km／n)を用いて第５計算手段３２によって算出する。第５計算手段３２は、モデル間距離を算出の際、第７記憶手段３６に記憶された他のロボットの補間指令位置情報などを読み出す。算出されたモデル間距離は第８記憶手段３７に記憶される。なお、t0＋Δｔ×km／nにおける補間指令位置はデカルト座標系上の位置ではなく関節角度指令値で表現されていても同様の処理が可能である。
【００６３】
本実施の形態３にかかる干渉回避制御装置は、３つ以上のモデル間距離Lij(t)の近似曲線Lijdum(t)を算出する第６計算手段３３と、近似曲線Lijdum(t)から最小値Lijdumminを算出する第２最小値算出手段３４と、を備える構成となっている。そして、第２判定手段５は、最小値情報１０７として取得した最小値（Lijdummin）と、最小値情報１１１として取得した最小値（Lijdummin）を用いて、干渉チェックを行う。
【００６４】
本実施の形態では、手先軌道上のt0+Δｔ×km／n後の補間指令位置の干渉チェックを加えることで、手先軌道上でしか干渉しないケースの干渉チェックも行える構成になっている。図１０は、比較例として手先軌道をフィルタ処理により算出する手順を示す図である。図１１は、図１０に示す手順で算出された手先軌道を示す図である。例えば、ロボット同士が僅かな距離を保ちながら曲線的な軌道ですれ違い動作をする接近動作があり、このとき実際の手先軌道は図１０に示すように算出したフィルタ後の軌道では、図１１に示すように内回りすることがあり、停止予定位置の判定だけではモデル間距離の推定が正確ではないため、動作中の干渉を見落としてしまうか不必要な干渉チェックが生じる場合がある。
【００６５】
一方、本実施の形態３においては、停止予定位置で干渉しても実際手先軌道上で干渉が生じない場合は動作を続けることで、その課題を解決できる。停止予定位置が実際の軌道上に無いような停止方法をする場合に、停止予定位置と現在補間位置および減速パターンを既知とするときに、途中通過位置を計算して判定すると、実際の停止軌道中に生じる干渉チェックを行うことができ、干渉チェックの見落としを低減する効果がある。
【００６６】
実施の形態４．
本実施の形態では、実施の形態１で干渉チェックを自動運転時に処理していたものをティーチング作業中も行われるように拡張したものである。ティーチング作業とはティーチングプレイバック方式で動作するロボットにおいて、マニュアルでティーチング用の教示装置を用いて、プログラム上のロボットの各動作用教示点の位置を教示して記録する作業のことである。例えば、図９に示すように、教示装置５３がロボット制御装置５２に接続される。
【００６７】
一般に２台のロボットを近接させて教示をしている場合にはロボット同士やロボットと周辺機器を衝突させてしまうことがある。本構成では、ロボット教示作業中に周辺機器や他のロボットに干渉させてしまう問題について、教示作業中にも干渉チェックを行い、教示装置によって出力される指令値を出した後での停止予定位置を、実施の形態１と同様に計算し、その場合に干渉が生じると判断される場合には、指令値を出さないという構成とする。
【００６８】
この構成を実施することで、教示装置による教示作業中での干渉する危険性がある方向への移動がなくなるため、より効率的に教示作業を進めることが出来る。干渉チェックを自動運転時に処理していたものを教示装置によってステップ送り動作時に実施するときも同様に判定処理に用いることも出来る（ロボットプログラミングに対するステップ送りとは、作成したロボットプログラムを１行ずつ実行する機能を指す）。
【産業上の利用可能性】
【００６９】
以上のように、本発明にかかる干渉回避制御装置は、複数台のロボットを動作させる環境に有用である。
【符号の説明】
【００７０】
１  第１計算手段
２  第２計算手段
３  第３計算手段
４  第１最小値算出手段
５  第２判定手段
６  第１記憶手段
７  第２記憶手段
８  第３記憶手段
９  第４記憶手段
１０  通信手段
１１  第１判定手段
１２  第１モデル設定手段
１３  第２モデル設定手段
１４  第５記憶手段
３１  第４計算手段
３２  第５計算手段
３３  第６計算手段
３４  第２最小値算出手段
３５  第６記憶手段
４０  産業用ロボット
４０  産業用ロボット
４１  ロボット要素モデル
４６  球モデル
４７  球モデル
５２  ロボット制御装置
５３  教示装置
１０１  モデル間距離制限値情報
１０２  停止予定位置情報
１０３  モデル位置情報
１０５  モデル間距離情報
１０７  最小値情報
１１１  最小値情報
【図１】
2012216151.tif 000007
【図２−１】
2012216151.tif 000008
【図２−２】
2012216151.tif 000009
【図３】
2012216151.tif 000010
【図４】
2012216151.tif 000011
【図５】
2012216151.tif 000012
【図６】
2012216151.tif 000013
【図７】
2012216151.tif 000014
【図８】
2012216151.tif 000015
【図９】
2012216151.tif 000016
【図１０】
2012216151.tif 000017
【図１１】
2012216151.tif 000018