干渉チェック制御装置および干渉チェック制御方法
【課題】複数のロボットの作業効率を改善する。
【解決手段】動作領域に重複部分を有するロボット同士の干渉を回避する干渉チェック制御装置であって、速度・位置記憶手段１１１と、速度・位置データに基づいてロボット構成部分同士の距離を計算するロボット間距離計算手段１１２と、速度・位置データに基づいてロボット構成部分同士の干渉判定領域を求める干渉判定用領域計算手段１１３と、干渉判定領域で規定される干渉判定距離とロボット構成部分同士の距離とを比較し干渉の有無を判定する干渉判定手段１１４とを備え、干渉判定用領域計算手段１１３は、干渉判定領域を速度データに基づいて移動速度と移動方向に応じて指向性を持って拡張し、拡張された干渉判定領域を決定し、干渉判定手段１１４は、拡張された干渉判定領域で規定される干渉判定距離とロボット構成部分同士の距離とを比較し干渉の有無を判定する。
【解決手段】動作領域に重複部分を有するロボット同士の干渉を回避する干渉チェック制御装置であって、速度・位置記憶手段１１１と、速度・位置データに基づいてロボット構成部分同士の距離を計算するロボット間距離計算手段１１２と、速度・位置データに基づいてロボット構成部分同士の干渉判定領域を求める干渉判定用領域計算手段１１３と、干渉判定領域で規定される干渉判定距離とロボット構成部分同士の距離とを比較し干渉の有無を判定する干渉判定手段１１４とを備え、干渉判定用領域計算手段１１３は、干渉判定領域を速度データに基づいて移動速度と移動方向に応じて指向性を持って拡張し、拡張された干渉判定領域を決定し、干渉判定手段１１４は、拡張された干渉判定領域で規定される干渉判定距離とロボット構成部分同士の距離とを比較し干渉の有無を判定する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、近接配置され、動作領域が重複する複数のロボット間の干渉チェックを行う干渉チェック制御装置および干渉チェック制御方法に関する。
【背景技術】
【０００２】
ロボット同士の干渉チェックを行うために、ロボットを、例えば、円柱といった簡易なモデル（近似立体）に近似し、近似したモデル同士の干渉状態をチェックする方法が、従来行われていた。
【０００３】
さらに、動作中などのオンラインで干渉チェックを行いながら停止指令を出す場合には、ロボットの動作速度および位置に応じてロボットモデルよりも大きい干渉判定領域を定義して、その領域同士が干渉すれば停止を開始する方法（例えば、特許文献１、２参照）が既知の技術として開示されている。
【０００４】
【特許文献１】特開昭６２−１５９２０３号公報
【特許文献２】特許３３５１３３０号公報
【発明の開示】
【発明が解決しようとする課題】
【０００５】
しかしながら、従来技術には、以下のような課題がある。
近似立体サイズを大きくした干渉チェックを行っている従来の方法は、動作速度に応じて近似立体サイズを大きくすることで、停止に必要とされる制動距離を見込んで判定ができる。このため、干渉する可能性を小さくすることができるが、その一方で、干渉することなく作業が継続できる場合にも、干渉チェックで干渉ありと判定し、頻繁に停止してしまい、作業時間が長くなってしまうといった問題があった。
【０００６】
本発明は、このような課題を解決するためになされたものであり、干渉することなく作業が継続できる場合に干渉ありと判定してしまうことを抑制し、複数のロボットの作業効率を改善することができる干渉チェック制御装置および干渉チェック制御方法を得ることを目的とする。
【課題を解決するための手段】
【０００７】
本発明に係る干渉チェック制御装置は、それぞれの動作領域に重複する部分を有するロボット同士の動作中の接近をチェックし、ロボット同士の干渉を回避する干渉チェック制御装置であって、前回ステップまでの計算周期で算出されたロボットの速度・位置データを記憶する速度・位置記憶手段と、速度・位置記憶手段に記憶された速度・位置データに基づいて、ロボット構成部分同士の距離を計算するロボット間距離計算手段と、速度・位置記憶手段に記憶された速度・位置データに基づいてロボット構成部分同士の干渉判定領域を求める干渉判定用領域計算手段と、干渉判定用領域計算手段で求めた干渉判定領域で規定される干渉判定距離と、ロボット間距離計算手段で計算されたロボット構成部分同士の距離とを比較し干渉の有無を判定する干渉判定手段と、干渉判定手段による干渉判定結果に応じて、計算周期ごとに各ロボットに対する移動指令値を作り出すとともに、現在の計算ステップにおけるロボットの速度・位置データを速度・位置記憶手段に記憶させる指令値生成手段とを備え、干渉判定用領域計算手段は、求めた干渉判定領域を速度データに基づいて移動速度と移動方向に応じて指向性を持って拡張し、拡張された干渉判定領域を決定し、干渉判定手段は、干渉判定用領域計算手段で決定された拡張された干渉判定領域で規定される干渉判定距離と、ロボット間距離計算手段で計算されたロボット構成部分同士の距離とを比較し干渉の有無を判定するものである。
【０００８】
また、本発明に係る干渉チェック制御方法は、それぞれの動作領域に重複する部分を有するロボット同士の動作中の接近をチェックし、ロボット同士の干渉を回避する干渉チェック制御方法であって、記憶部に記憶された前回ステップまでの計算周期で算出されたロボットの速度・位置データに基づいて、ロボット構成部分同士の距離を計算するロボット間距離計算ステップと、記憶部に記憶された前回ステップまでの計算周期で算出されたロボットの速度・位置データに基づいてロボット構成部分同士の干渉判定領域を求めるステップと、干渉判定用領域計算ステップで求めた干渉判定領域で規定される干渉判定距離と、ロボット間距離計算ステップで計算されたロボット構成部分同士の距離とを比較し干渉の有無を判定する干渉判定ステップと、干渉判定ステップによる干渉判定結果に応じて、計算周期ごとに各ロボットに対する移動指令値を作り出すとともに、現在の計算ステップにおけるロボットの速度・位置データを記憶部に記憶させる指令値生成ステップとを備え、干渉判定用領域計算ステップは、求めた干渉判定領域を速度データに基づいて移動速度と移動方向に応じて指向性を持って拡張し、拡張された干渉判定領域を決定し、干渉判定ステップは、干渉判定用領域計算ステップで決定された拡張された干渉判定領域で規定される干渉判定距離と、ロボット間距離計算ステップで計算されたロボット構成部分同士の距離とを比較し干渉の有無を判定するものである。
【発明の効果】
【０００９】
本発明に係る干渉チェック制御装置および干渉チェック制御方法によれば、干渉判定領域のサイズを、単純に近似立体の法線方向に拡張するのではなくて、移動速度と移動方向に応じた指向性を持たせて拡張することにより、干渉することなく作業が継続できる場合に干渉ありと判定してしまうことを抑制し、複数のロボットの作業効率を改善することができる干渉チェック制御装置および干渉チェック制御方法を得ることができる。
【発明を実施するための最良の形態】
【００１０】
以下、本発明の干渉チェック制御装置および干渉チェック制御方法の好適な実施の形態につき図面を用いて説明する。
本発明は、近似立体を拡張することによって不必要に停止が生じる場合について分析し、近似立体サイズを単純に大きくすることが原因で停止が頻繁に起こっていることを見出した。そこで、本発明は、近似立体の移動速度と移動方向に注目して、近似立体を効果的に変形させることで、不必要な停止が起こる方向には領域を拡張しない干渉判定領域を設定し、従来と比較して優れた干渉チェックを実現している。
【００１１】
実施の形態１．
【００１２】
まず始めに、ロボット同士の干渉判定について説明する。まず、ロボットの可動範囲によって形成される、ロボットが通過しうる空間を動作領域と表現する。２台以上のロボット同士で動作領域が交差する場合は、ロボット同士の衝突の危険性が出てくる。このようなロボット同士の衝突を表す表現として、ロボットの占めている空間同士が交差している状態に干渉という表現を用いる。そのほか、後述の干渉判定領域同士が交差する状態にも干渉という表現を用いる。また、このような干渉が生じるかどうかを確認することを干渉チェックと呼ぶ。干渉チェックは、ロボットを、例えば、簡易な幾何モデル（点、線分、球、円筒、多面体）に置き換えて、干渉をチェックしたいロボットモデル間の距離を計算する。そして、計算された距離が許容値以内か否かを判定することで、干渉チェックが行われるのが一般的である。干渉チェックは、目的によって、以下の２つの利用方法がある。
【００１３】
シミュレーション上のロボット同士において干渉するか否かを検出する場合は、干渉の検出が目的となる。このため、干渉チェックとしては、動作をシミュレーション上で再生し、ロボットモデル同士の干渉があるか否かを判定することとなり、指令値の変更は、ユーザにゆだねられる。
【００１４】
一方で、動作中のロボット同士あるいはシミュレーションにおいては、干渉の検出とともに、干渉を回避することが目的となる。この場合には、実際に干渉が起こるよりも十分に早く干渉チェックを行う必要がある。従って、通常、ロボットモデルよりも大きい制動距離を見込んだ干渉判定領域を定義して、干渉判定領域同士が交差した場合に、停止を開始して干渉を防止する。ここでは、線分の周りに一定距離上に境界をもつ空間領域をロボットモデルとして定義し、干渉判定領域は、ロボットモデルと相似形状をしたロボットモデルよりも大きい一定距離上に境界をもつ空間領域として定義している。
【００１５】
上述した干渉チェックに関する事項を踏まえ、次に、本実施の形態１における干渉チェック方法について、具体的に説明する。図１は、本発明の実施の形態１における干渉チェック制御装置の構成図である。この図１の干渉チェック制御装置は、ロボット速度・位置記憶手段１１１、ロボット間距離計算手段１１２、干渉判定用領域計算手段１１３、干渉判定手段１１４、および指令値生成手段１１５を備えている。
【００１６】
図示していないが、上位に位置するロボット制御装置は、一定の計算周期に従って計算ステップごとに、ロボットの指令値を作り出している。その際に、ロボットの目標手先位置姿勢や各関節の目標角度、目標角速度、目標角加速度を計算している。それらの情報を用いて、干渉チェック制御装置は、ロボットの現在の６自由度手先位置姿勢および現在速度と、ロボットを構成するロボット構成部分の端点位置と端点速度とを算出している。例えば、各関節角度データをもとに運動学を解いてロボットの姿勢を計算して、該当する端点位置を算出可能である。また、端点速度は、端点速度の差分あるいは各駆動関節の変化速度から運動学を用いて解くことができる。また、６自由度手先位置姿勢と端点位置および端点速度の計算は、ロボット制御装置内において得られた結果を用いて計算しているので、ロボット制御装置内でも算出することができる。算出されたこれらのデータを、以下の説明では、速度・位置データ１０１と称することとする。そして、ロボット制御装置は、算出した速度・位置データ１０１を、各計算周期ごとにロボット速度・位置記憶手段１１１に記憶させる。
【００１７】
なお、ここで、６自由度手先位置姿勢とは、ハンドなどエンドエフェクタのワールド座標系に対する位置姿勢を指す。ただし、スカラロボット（４自由度）など自由度が６に満たないロボットでは、自由度に応じて手先位置姿勢が求められる。また、ロボット構成部分とは、例えば、ロボットの代表的剛体要素（例えば、ベース部、ベース側アーム部、リスト側アーム部、リスト部、およびエンドエフェクタ部）に分けてそれぞれの部分同士で干渉を考えるための、ロボットの干渉チェックの時のチェックする単位要素である。また、あるロボット構成部分は、ある１つの可動部分の動作により、他のロボット構成部分との干渉が発生しうるが、可動部分ごとにすべてを構成部分にすると計算量が増加するため、この例では、干渉チェックの計算量を減らすためになるべく少ない構成部分に分割している。
【００１８】
また、ロボット構成部分の端点情報を記憶しておく理由は、本実施の形態１で後述するように、ロボットをモデル化し距離計算や干渉判定を行う際に、線分モデルを用いているためである。従って、このロボット構成部分の端点情報としては、各リンクのモデルを構成するのに必要な端点位置や端点の移動速度情報であればよい。例えば、ロボット構成部分のベース側アーム部についてみると、ある時刻ｔ０のベース側アーム部について線分化したモデルについて、ベース側アーム部の線分モデルの端点の位置座標とその座標点の移動速度を計算して記憶しておく。線分の端点の位置座標の計算方法は、ある時刻ｔ０の関節角度情報から運動学を用いて求め、線分の端点速度については、例えば、時刻ｔ０における関節角速度情報を情報から運動学を用いて求めることになる。
【００１９】
次に、ロボット間距離計算手段１１２は、ロボット構成部分同士の距離を計算する手段であり、ロボット速度・位置記憶手段１１１に記憶された前回ステップまでの計算周期で算出された速度・位置データ１０２と、速度・位置データ１０１と同等のデータとしてあるロボット制御装置以外の外部から得られる速度・位置データ情報１０７とを読み込む。ここで、１０１と同等のデータとは、外部のロボット制御装置で算出されている、ロボットの現在の６自由度手先位置姿勢および現在速度と、ロボットを構成するロボット構成部分の端点位置と端点速度のことである。また、あるロボット制御装置以外の外部とは、例えば、他のロボット制御装置であったり、可動する冶具の制御装置などあるロボットと干渉する危険性のある移動をする機械装置の制御装置が挙げられる。また、移動する機械装置に限らず、干渉する危険性がある静止している機械装置についても、その機械装置の位置情報を外部から提供する。
【００２０】
次に、ロボット間距離計算手段１１２は、読み込んだデータに基づいて、対象としているロボット構成部分同士（例えば、ここでは、ロボット構成部分をモデル化した簡易なロボットモデル同士）の距離計算を行い、ロボット構成部分間距離（例えば、ここでは、ロボットモデル同士のロボットモデル間距離）１０３を、Ｌｉｊ１０３として出力する。
【００２１】
図２は、本発明の実施の形態１におけるロボット構成部分をモデル化した簡易なロボットモデルと干渉判定領域の例示図である。図２（ａ）のような構成のロボットモデルは、図２（ｂ）に示すように、ロボット構成部分をモデル化した簡易なロボットモデルとして扱うことができる。
【００２２】
より具体的には、各構成部分は、点および線分でモデル化される。さらに、モデル化された構成部分が線分の場合には、干渉を判定するための領域は、その線分から等距離となるような円柱と、その両端を覆う半球とからなる立体モデルとしてモデル化される。また、モデル化された構成部分が点の場合には、干渉を判定するための領域は、その点から等距離となる球からなる立体モデルとしてモデル化される。このような立体モデルをロボットモデルと呼ぶ。
【００２３】
例えば、ロボット構成部分であるロボットアーム部１１ａ〜１４ａは、線分でモデル化されたロボットアーム部モデル１１ｂ〜１４ｂに置き換えられる。また、ロボット構成部分の１つであるハンド１０ａは、ハンドモデル１０ｂに置き換えられる。そして、ロボット間距離計算手段１１２は、ロボット同士の構成部分間の最も接近している部分の距離測定を、このようにしてモデル化された簡易なロボットモデル同士の構成部分間の最も接近している部分の距離測定により行う。
【００２４】
図３は、本発明の実施の形態１におけるモデル化された２線分間の最近接点を幾何的に表した図である。例えば、ロボットモデル間距離を計測するための２つの線分をモデルＬｉｎｋｉとＬｉｎｋｊとすると、ロボット間距離計算手段１１２は、この２線分間の最近接点３１、３２（４１、４２）からモデル間距離Ｌｉｊ３３（４３）を導くようにしている。
【００２５】
２線分間の最近接点およびモデル間距離Ｌｉｊ１０３の計算方法は、例えば、特開昭６０−２１７４１０号公報に開示されているような手法を用いることができる。ただし、ロボットモデル間の距離は、線分モデル同士に限らず、その他にも、例えば、球や円筒や多面体でモデル化したロボットモデルに対して、モデル同士の最近接距離を計算しても構わない。
【００２６】
次に、所定の干渉判定領域を、さらに移動速度と移動方向に応じて指向性を持って拡張させることで、「拡張された干渉判定領域」を求める干渉判定用領域計算手段１１３の機能について説明する。ここで、所定の干渉判定領域とは、予めロボットモデルと同じかロボットモデルよりも大きく設定され、ロボットモデルから距離ＬｉＬｉｍだけ離れた部分を境界とする領域で定義される。
【００２７】
先の図２（ｂ）を例に説明すると、ロボットを包む簡易のＬｉＬｉｍロボットモデルを表す領域は、１０ｃ〜１４ｃで表される。そして、干渉を判定するための所定の干渉判定領域は、１０ｄ〜１４ｄで表される。前述したように、干渉判定領域は、ロボットモデルよりも常に大きいものとする。そして、本実施の形態１における干渉チェックは、停止動作をするか否かを判定する干渉判定領域を用いて行うものを対象としている。
【００２８】
拡張された干渉判定領域を求めるために、干渉判定用領域計算手段１１３は、前回ステップまでの計算周期にて算出された速度・位置データ１０２（ロボットの現在の６自由度手先姿勢および現在速度と、ロボットを構成するロボット構成部分である各リンクの端点位置および各リンクの端点速度情報）を用いて、所定の干渉判定領域を、現在進行している方向に対して拡張することで、「拡張された干渉判定領域」を決定する。さらに、「拡張された干渉判定領域」を決定した後に、干渉判定用領域計算手段１１３は、判定用の閾値を決定する。
【００２９】
図４は、本発明の実施の形態１における干渉判定用領域計算手段１１３による拡張された干渉判定領域の決定方法の説明図である。この図４では、説明を簡単にするために、半径ｒの球で所定の干渉判定領域が定義されている際に、拡張された干渉判定領域ＬｉｊｅｘＬｉｍ８０４を決定するための方法について説明する。
【００３０】
図４における符号８００〜８０５は、次のものを表している。
８００：ロボットモデル速度
８０１：実際のロボットを球で覆うように近似したロボットモデル
８０２：球でモデル化を行った際の所定の干渉判定領域
８０３：従来方法による拡張された干渉判定領域
８０４、８０５：本発明による拡張された干渉判定領域
【００３１】
また、図４（ａ）は、所定の干渉判定領域８０２を示した図であり、図４（ｂ）は、従来方法による拡張された干渉判定領域８０３を示した図である。また、図４（ｃ）、図４（ｄ）は、本発明による２通りの拡張された干渉判定領域８０４、８０５を示した図である。
【００３２】
拡張された干渉判定領域８０４の領域拡張方法は、現在のロボットモデルの移動方向に向かって移動速度の大きさに比例して所定の干渉回避領域８０２を拡張するものである。現在のロボットモデルに対して、このロボットモデルの移動速度を３次元ベクトルＶｍｏｖとおく。この移動速度とは、例えば、干渉チェック制御装置内で計算された各ロボットモデルの端点速度という速度データ１０２から求められるロボットモデルの移動速度であり、ロボットモデルの並進速度成分を利用する。例えば、Ｖｍｏｖの大きさに比例した拡張比Ｋｅｘ（ただし、Ｋｅｘ＞１）分だけ干渉判定領域ｒを拡張すると、従来方法による拡張された干渉判定領域８０３は、ＬｉｊｅｘＬｉｍとして、下式（１）で定義される（図４（ｂ）参照）。
ＬｉｊｅｘＬｉｍ＝Ｋｅｘ・ｒ （１）
【００３３】
これに対して、本発明による拡張された干渉判定領域８０４は、モデル移動方向に関しては、半径ｒによる干渉判定領域がＫｅｘだけ拡張され、進行していない方向（モデル移動方向と１８０度異なる方向）に関しては、所定の干渉判定領域（すなわち、半径ｒによる干渉判定領域）からサイズ変更がないようにして決定される。
【００３４】
この結果、モデル移動方向に図４（ｃ）に示す８１４だけ中心をずらすことで、拡張された干渉判定領域８０４を決定している。結局、拡張された干渉判定領域８０４における拡張された半径ｒｅｘは、下式（２）というサイズになる。
【００３５】
【数１】
【００３６】
このとき、拡張された干渉判定領域８０４の中心点位置ベクトルＰｃｅｘは、ロボットモデル中心点位置ベクトルＰｃからずれて、下式（３）だけずらせばよいことになる。
【００３７】
【数２】
【００３８】
このように、拡張された干渉判定領域８０４を決定した場合に、干渉判定用領域計算手段１１３は、モデル位置修正情報１０４ａとして上式（２）、（３）で定義されたｒｅｘおよびＰｃｅｘを、ロボット構成部分同士の距離を計算するロボット間距離計算手段１１２に入力する（図１参照）。
【００３９】
本実施の形態１の構成では、ロボット構成部分のモデル同士の距離計算を行う際にも、ロボット構成部分のモデルの中心が移動したものとして扱う必要がある。このため、ロボット構成部分同士の距離を計算するロボット間距離計算手段１１２は、速度・位置記憶手段１１１に記憶された速度・位置データ１０２と、干渉判定用領域計算手段１１３で求められたモデル位置修正情報１０４ａとに基づいて、ロボット構成部分同士の距離を計算する。
【００４０】
干渉判定用領域計算手段１１３により拡張された干渉判定領域８０４のサイズは、従来方法による拡張された干渉判定領域８０３よりも小さくなる。しかしながら、本来干渉の可能性が高いロボットモデルの移動方向に対しては、同等の干渉判定領域を保ったままで、後方方向など不必要に拡張していた干渉判定領域をなくすか、あるいは小さくした状態で、干渉チェックを行うことができる。
【００４１】
また、図４（ｄ）に示すような、本発明による拡張された干渉判定領域８０５を決定することもできる。モデル移動方向に関しては、その移動分（図４（ｄ）における８１５に相当）だけ中心をずらした上で、所定の干渉判定領域（すなわち、半径ｒによる干渉判定領域）からサイズ変更がないようにして半球が決定される。
【００４２】
一方、進行していない方向（モデル移動方向と１８０度異なる方向）に関しても、所定の干渉判定領域（すなわち、半径ｒによる干渉判定領域）からサイズ変更がないようにして半球が決定される。そして、モデル移動方向に関する領域と進行していない方向に関する領域でそれぞれ規定された半球の間を、中心が移動した線分から等距離となるような円柱で結ぶことにより、拡張された干渉判定領域８０５が決定される。
【００４３】
また、図５は、本発明の実施の形態１における干渉判定領域の定義方法の説明図であり、線分から等距離となるような円柱と、その両端を覆う半球とからなる立体モデルとしてモデル化された構成部分における干渉判定領域を示している。図５（ａ）は、従来方法による干渉判定領域を示しており、図５（ｂ）は、本発明の干渉判定領域を示している。
【００４４】
図５における符号８１１〜８１４は、次のものを表している。
８１１：実際のロボットを線分から等距離となるような円柱と、その両端を覆う半球
とで近似したロボットモデル
８１２：８１１としてモデル化を行った際の所定の干渉判定領域
８１３：従来方法による拡張された干渉判定領域
８０４：本発明による拡張された干渉判定領域
【００４５】
このような図５に示すような形状の構成部分についても、先の図４（ｃ）、（ｄ）の説明と同様に、モデル移動方向に関する領域と進行していない方向に関する領域を考慮することで、適切な干渉判定領域を設定できる。
【００４６】
この図４（ｃ）、（ｄ）あるいは図５に示したように、拡張された干渉判定領域８０４、８０５、８１４を決定することによるメリットを、図６、図７を用いて具体的に説明する。図６は、本発明の実施の形態１における追う側と逃げる側のモデル間の干渉に関する説明図である。図６（ａ）は、従来方法による拡張された干渉判定領域８０３を用いた場合の追う側と逃げる側のモデル間の干渉を例示している。一方、図６（ｂ）は、本発明による拡張された干渉判定領域８０４を用いた場合の追う側と逃げる側のモデル間の干渉を例示している。
【００４７】
速度に応じて制動距離分だけ領域を拡張した場合に、図６（ａ）に示すように、従来手法の場合には、逃げる側の領域が過剰に延びており、交差領域８２０が発生し、追う側が過度に停止をすることになる。一方、図６（ｂ）に示すように、本発明では、モデル移動方向と１８０度異なる方向に関しては、所定の干渉判定領域からサイズ変更がないようにして決定されるため、交差領域が発生しない。この結果、本来拡張しなくてよい方向に緩衝領域を拡張しないことで、ロボットの停止状態を減らし、ロボットシステムの作業効率を上げる効果がある。
【００４８】
また、図７は、本発明の実施の形態１におけるハンドなどのエンドエフェクタモデルが近くをすれ違う場合の干渉に関する説明図である。従来手法であるモデル８０３では、誤検知が発生（すなわち、先の図６（ａ）と同様に交差領域が発生）していた。これに対して、本発明による拡張された干渉判定領域８０５では、交差領域が発生せず、干渉判定で停止なくすれ違うことができる。この結果、先の図６の場合と同様に、ロボットの停止状態を減らし、ロボットシステムの作業効率を上げる効果がある。
【００４９】
最後に、干渉判定手段１１４および指令値生成手段１１５における処理について説明する。干渉判定手段１１４は、干渉判定を行いたいロボット構成部分２つ（ＬｉとＬｊ）の全てのロボット構成部分の組に対して、ロボット構成部分のロボットモデル同士の最近接距離で定義されるロボット構成部分のモデル間距離Ｌｉｊ１０３と干渉判定領域（ＬｉＬｉｍとＬｊＬｉｍ）の和で定義される干渉判定用の閾値ＬｉｊＬｉｍ１０４ｂを利用して干渉判定を行う。
【００５０】
干渉判定手段１１４は、下式（４）が成り立つときに、干渉危険ありと判定して停止開始を行う指示１０５を指令値生成手段１１５に通知する。
Ｌｉｊ≦ＬｉｊＬｉｍ （４）
【００５１】
次に、指令値生成手段１１５は、干渉判定結果に応じて計算ステップごとに指令値を作り出す。より具体的には、指令値生成手段１１５は、干渉判定領域同士が交差している場合には、停止の為の減速を指令し、干渉判定領域が交差していない場合には、目標指令に従った指令値を計算する。さらに、指令値生成手段１１５は、現在のロボット姿勢と速度１０１をロボット速度・位置記憶手段１１１に記憶させる。
【００５２】
以上のように、実施の形態１によれば、以下の効果を得ることができる。速度に応じてあるいはロボット間距離に応じて拡張された干渉判定領域を定義する際に、従来技術においては、進行方向に対しては十分に判定領域を広げる効果があった。しかしながら、進行方向以外の方向に対しては、過度に拡張されるために、交差領域を誤検知する結果となっていた。この結果、高速で動作をさせると、不要な停止を引き起こすといった問題があった。
【００５３】
これに対して、本実施の形態１では、移動速度と移動方向に応じた指向性を考慮しており、中心が進行方向にずれて、元の領域を含むように拡張された干渉判定領域（図４（ｃ）の干渉判定領域８０４参照）、あるいは球の進行方向に円筒領域として定義した拡張された干渉判定領域（図４（ｄ）の干渉判定領域８０５参照）を利用している。この結果、複数台のロボットから構成されるようなロボットシステムにおいて、交差領域を誤検知する可能性を低減することができ、不要な停止指令を少なくして、システムの作業効率を上げる効果がある。
【００５４】
実施の形態２．
図８は、本発明の実施の形態２における干渉チェック制御装置の構成図である。先の実施の形態１における図１の構成と比較すると、この図８の構成は、基本的には、図１と同一であるが、モデル位置修正１０４ａが必要ない構成となっている点が異なっている。
【００５５】
より具体的には、干渉判定用領域計算手段１１３の機能が異なっている。本実施の形態２における干渉判定用領域計算手段１１３は、ロボットモデルの進行方向ベクトルと最短距離方向ベクトルのなす角に応じて干渉判定領域の拡張サイズを決定するように構成されている。
【００５６】
先の実施の形態１では、所定の干渉判定領域をさらに移動速度と移動方向に応じて指向性を持って拡張させる干渉判定用領域計算手段１１３と、ロボット構成部分のモデル同士の距離を計算するロボット間距離計算手段１１２との間には関連があった。すなわち、先の実施の形態１における干渉判定領域の拡張方法は、モデルが中心軸となる線分を並進移動した際に、干渉判定領域を元の判定領域を包む形で拡張する形態であった。
【００５７】
このような、先の実施の形態１の拡張方法では、ロボット構成部分のモデル同士の距離計算を行う際にも、ロボット構成部分のモデルの中心が移動したものとして扱う必要がある。このため、ロボット構成部分同士の距離を計算するロボット間距離計算手段１１２に、修正されたモデル位置修正情報１０４ａを入力することが必要であった。
【００５８】
これに対して、本実施の形態２における干渉判定領域の計算方法では、モデル中心軸となる線分からロボットモデルの進行方向ベクトルと最短距離方向ベクトルのなす角に応じて拡張サイズを決定する。例えば、簡単のために、球モデルにおいて、球モデルの干渉判定領域をＬｉＬｉｍ進行方向への拡張率Ｋｅｘ（≧１）、ロボットモデル（ここでは球モデル）の移動する方向で定義する進行方向ベクトルＶｄ、干渉チェックを行う２つのロボットモデルのそれぞれの最近接点を結んでできる方向ベクトルを判定する最近接点の方向ベクトルＮｃｌｓとする。このとき、判定方向への干渉判定領域の閾値ＬｉＬｉｍｄは、下式（５）、（６）というＬｉＬｉｍｄを定義して、判定する線分モデルＬｉｎｋｉとＬｉｎｋｊに対して、それぞれＬｉＬｉｍｄとＬｊＬｉｍｄを生成する。
【００５９】
【数３】
【００６０】
ここで、ＬｉｊＬｉｍｄは、
ＬｉｊＬｉｍｄ＝ＬｉＬｉｍｄ＋ＬｊＬｉｍｄ （７）
で定義される。干渉判定用領域計算手段１１３は、このようにして、判定方向への干渉判定用の閾値１０４を算出し、干渉判定手段１１４に対して出力する。このとき、ＬｉＬｉｍｄおよびＬｊＬｉｍｄは、例えば、上式（５）（６）のように定義されることで、本実施の形態１に示したような円柱や球のような単純な形状に限らない拡張した干渉判定領域を定義できる。
【００６１】
そして、干渉判定手段１１４は、ロボット構成部分間距離１０３に対して、このＬｉｊＬｉｍｄを用い、干渉判定領域の交差判定を、下式（８）に基づいて行う。
Ｌｉｊ≦ＬｉｊＬｉｍｄ （８）
この結果、先の実施の形態１の図１で示したモデル位置修正１０４ａが不要となる。
【００６２】
以上のように、実施の形態２によれば、以下の効果を得ることができる。本実施の形態２では、モデル中心軸となる線分からロボットモデルの進行方向ベクトルと最短距離方向ベクトルのなす角に応じて拡張サイズを決定している。これにより、先の実施の形態１と同様に、複数台のロボットから構成されるようなロボットシステムにおいて、不要な停止指令を少なくして、システムの作業効率を上げる効果がある。
【００６３】
さらに、本実施の形態２では、拡張される干渉判定領域を、変形する形状として幾何的に算出するのではなく、数値的に算出できるため、判定方法としては、より広く使うことができる。つまり、特に、上式（５）（６）に限定することはなく、すべての方向に対して所定の干渉判定領域ＬｉＬｉｍが本実施の形態２で定義される拡張された干渉判定領域ＬｉｊＬｉｍｄ以下の大きさになっている数式表現であればよいので、本実施の形態１における拡張した干渉判定領域の定義よりも多くの拡張した干渉判定領域を定義することができ、より交差領域を誤検知することの少ない干渉判定領域を選ぶことができる。
【００６４】
実施の形態３．
本実施の形態３では、所定の干渉判定領域をさらに移動速度と移動方向に応じて指向性を持って拡張させる干渉判定用領域計算手段１１３が、停止時などの特定の条件下において、ロボットモデルと干渉判定領域の初期の大きさを揃えるように変更する場合について説明する。構成は、先の実施の形態１における図１と同様である。
【００６５】
図９は、本発明の実施の形態３におけるロボットとロボットモデルと干渉回避領域の包含関係を表した説明図である。干渉判定領域を用いた干渉判定方法において、一般的には、ロボットモデルや干渉判定領域は、図９に示すように、ロボット５１の周りを十分に取り囲むロボットモデル５２のサイズに対して、マージンを持たせた干渉判定領域５３を利用している。
【００６６】
これは、各ロボット間の精度誤差やロボット高速動作時にも干渉することなく停止するため、余分に領域を広げていることによる。本発明における干渉判定領域の拡張方法においても、先の実施の形態１、２で説明したように、確実に干渉回避を行うために、干渉判定領域は、従来手法と同様に、やや大きめに設定されることになる。しかしながら、このような設定によって、完全には不必要な停止を無くせない場合が生じることとなる。
【００６７】
そこで、本実施の形態３では、移動速度に応じて、例えば、ロボットが停止時あるいは所定の速度以下の低速移動時には、ロボットモデルと干渉判定領域の初期の大きさを揃えることで、不必要な停止をより低減させることを行う。このように、拡張させる干渉判定領域を変更し、例えば、停止時にはロボットモデルと干渉判定領域の大きさを同じにすることで、他の移動中のロボットを停止させなくても作業続行可能である場合（例えば、低速で近接してすれ違い移動するような場合）には、作業が続けられるようになる。
【００６８】
この際の干渉判定領域の大きさは、速度に応じて、先の数式（１）で定義したもので定義し、ロボットモデルの領域の大きさをｒｍ（＜球モデルの干渉判定領域の半径ｒ）とおくときに、干渉判定領域の大きさＬｉｊｅｘＬｉｍは、
ＬｉｊｅｘＬｉｍ＝Ｋｅｘ・ｒｍ （９）
と定義される。すなわち、ｒではなくｒｍに対して、移動速度の大きさに比例した拡張比Ｋｅｘを掛け合わせることで干渉判定領域の大きさを求めている。
【００６９】
以上のように、実施の形態３によれば、先の実施の形態１よりも交差領域を誤検知する可能性を低減することができ、さらに作業停止時間が減り、作業効率が上がるという効果がある。
【００７０】
実施の形態４．
本実施の形態４では、干渉ありと判定された後の減速停止動作中に、その速度に応じて干渉判定を継続して行い、干渉の心配がないと判断された場合には、停止のための減速を中断し動作を再開する方法について説明する。
【００７１】
図１０は、本発明の実施の形態３において、ロボットモデル間距離Ｌｉｊが許容値ＬｉｊＬｉｍに達したために停止のための減速開始を行う状態を示した図である。また、図１１は、本発明の実施の形態３において、減速動作による干渉判定領域の縮小に伴って小さくなった許容値Ｌ'ｉｊＬｉｍとの比較で、許容値よりもロボットモデル間距離Ｌｉｊが大きくなり、停止動作が不要になったため、動作を再開した状態を示した図である。
【００７２】
図１０に示すように、時刻ｔｎｏｗにおいて、拡張された干渉判定領域に対して大きさＬｉｊＬｉｍ６０３を用いて干渉判定を行った際に、干渉ありと判定された場合には、停止のための減速動作を開始する。この場合、本実施の形態４における干渉チェック制御装置は、減速中にも干渉判定を継続して行う。
【００７３】
そして、図１１に示すように、時刻ｔ’ｎｏｗにおいて、減速に従い縮小が生じた干渉判定領域から算出される判定のための閾値Ｌ'ｉｊＬｉｍ６０６を用いて干渉判定を行った際に、Ｌ'ｉｊＬｉｍ≦ＬｉｊＬｉｍが成り立った場合には、停止のための減速を中断し、動作を再開する。
【００７４】
停止動作に入った後に速度が減速すると、先の実施の形態３で説明した方法を用いると、算出される拡張された干渉判定領域の大きさは、現状の速度に応じて変化する。この結果、干渉する危険があるという判定（図１０の時刻ｔｎｏｗに相当）から、干渉する危険が無いという判定（図１１の時刻ｔ’ｎｏｗに相当）に変わる場合がある。
【００７５】
従って、本実施の形態４のように、現状の速度に応じて拡張された干渉判定領域の大きさを求めることで、干渉回避動作を中断できるかどうかを判定し、干渉回避動作が中断されるときには引き続き動作を継続させることができる。これによって、停止するロボットが停止位置として、より目標位置に近いところで停止する、あるいは作業を継続することもできる。
【００７６】
以上のように、実施の形態４によれば、現状の速度に応じて拡張された干渉判定領域の大きさを求めることで、干渉回避動作を再開することが可能となり、先の実施の形態１よりも交差領域を誤検知する可能性を低減することができ、さらに作業停止時間が減り、作業効率が上がるという効果がある。
【００７７】
実施の形態５．
第５の実施の形態では、ロボット相互の軌道に応じて、干渉判定領域を切り替える場合について説明する。所定の干渉判定領域をさらに移動速度と移動方向に応じて指向性を持って拡張させる干渉判定用領域計算手段１１３は、例えば、２つのロボット構成部分が直線軌道で行き違う場合（すなわち、互いの直線軌道が平行である場合）は、先の図４（ｄ）で説明したような、拡張された干渉判定領域８０５を用いることが考えられる。
【００７８】
図１２は、本発明の実施の形態５における干渉判定の説明図である。ここでは、図１２に示すように、２つのロボット構成部分が直線軌道で行き違うという場合において、従来方法と本発明の手法の比較を説明する。
【００７９】
図１２では、ある時刻ｔｉからｔｉ+４までの離散的な時間におけるロボット構成部分のモデル間の距離計算から干渉チェックを行う場合を例示している。図１２において、７７ａは、実際に停止が必要な場合に対して従来の判定方法を用いた場合を示しており、７７ｂは、実際に停止が不要な場合に対して従来の判定方法を用いた場合を示している。これに対して、７８ａは、実際に停止が必要な場合に対して本実施の形態５の判定方法を用いた場合を示しており、７８ｂは、実際に停止が不要な場合に対して本実施の形態５の判定方法を用いた場合を示している。
【００８０】
従来の判定方法（７７ａ、７７ｂ）では、速度に応じて判定領域を大きくする手法を用いることで、ロボット構成部分のモデル同士のチェックで、見落としていた干渉を見落とさなくなっている。しかしながら、７７ｂでは、拡張された干渉判定領域が速度に応じて大きくなっているため、実際には干渉しないのだが、干渉すると判定されてしまっている。
【００８１】
一方、本実施の形態５の判定方法（７８ａ、７８ｂ）では、先の図４（ｄ）で説明したような、拡張された干渉判定領域８０５を用いている。この結果、実際に停止が必要な場合（７８ａ）には、干渉すると判定でき、実際に停止が必要な場合（７８ｂ）には、不必要な干渉判定をしないで済んでいることが分かる。
【００８２】
この例では、直線軌道に適した図４（ｄ）の拡張された干渉判定領域８０５を用いたが、軌道に応じて、図４（ｃ）の拡張された干渉判定領域８０４を用いてもよく、ロボット相互の軌道があらかじめ特定できる場合には、その軌道に応じて、拡張された干渉判定領域を適切なサイズに設定することで、不必要な干渉判断を防止することができる。
【００８３】
以上のように、実施の形態５によれば、ロボット相互の軌道があらかじめ特定できる場合には、軌道に応じて、拡張された干渉判定領域を適切なサイズに設定することができる。この結果、従来方法では、干渉が実際に起こらなくても干渉すると判定されていた場合に対して、ユーザがチェックと指令値の再編集をする必要があったが、そのような手間がなくなり、オフライン干渉チェックに関して作業効率が上がるという効果が得られる。
【図面の簡単な説明】
【００８４】
【図１】本発明の実施の形態１における干渉チェック制御装置の構成図である。
【図２】本発明の実施の形態１におけるロボット構成部分をモデル化した簡易なロボットモデルと干渉判定領域の例示図である。
【図３】本発明の実施の形態１におけるモデル化された２線分間の最近接点を幾何的に表した図である。
【図４】本発明の実施の形態１における干渉判定用領域計算手段による拡張された干渉判定領域の決定方法の説明図である。
【図５】本発明の実施の形態１における干渉判定領域の定義方法の説明図である。
【図６】本発明の実施の形態１における追う側と逃げる側のモデル間の干渉に関する説明図である。
【図７】本発明の実施の形態１におけるハンドなどのエンドエフェクタモデルが近くをすれ違う場合の干渉に関する説明図である。
【図８】本発明の実施の形態２における干渉チェック制御装置の構成図である。
【図９】本発明の実施の形態３におけるロボットとロボットモデルと干渉回避領域の包含関係を表した説明図である。
【図１０】本発明の実施の形態３において、ロボットモデル間距離Ｌｉｊが許容値ＬｉｊＬｉｍに達したために停止のための減速開始を行う状態を示した図である。
【図１１】本発明の実施の形態３において、減速動作による干渉判定領域の縮小に伴って小さくなった許容値Ｌ'ｉｊＬｉｍとの比較で、許容値よりもロボットモデル間距離Ｌｉｊが大きくなり、停止動作が不要になったため、動作を再開した状態を示した図である。
【図１２】本発明の実施の形態５における干渉判定の説明図である。
【符号の説明】
【００８５】
１１１ ロボット速度・位置記憶手段、１１２ ロボット間距離計算手段、１１３ 干渉判定用領域計算手段、１１４ 干渉判定手段、１１５ 指令値生成手段。
【技術分野】
【０００１】
本発明は、近接配置され、動作領域が重複する複数のロボット間の干渉チェックを行う干渉チェック制御装置および干渉チェック制御方法に関する。
【背景技術】
【０００２】
ロボット同士の干渉チェックを行うために、ロボットを、例えば、円柱といった簡易なモデル（近似立体）に近似し、近似したモデル同士の干渉状態をチェックする方法が、従来行われていた。
【０００３】
さらに、動作中などのオンラインで干渉チェックを行いながら停止指令を出す場合には、ロボットの動作速度および位置に応じてロボットモデルよりも大きい干渉判定領域を定義して、その領域同士が干渉すれば停止を開始する方法（例えば、特許文献１、２参照）が既知の技術として開示されている。
【０００４】
【特許文献１】特開昭６２−１５９２０３号公報
【特許文献２】特許３３５１３３０号公報
【発明の開示】
【発明が解決しようとする課題】
【０００５】
しかしながら、従来技術には、以下のような課題がある。
近似立体サイズを大きくした干渉チェックを行っている従来の方法は、動作速度に応じて近似立体サイズを大きくすることで、停止に必要とされる制動距離を見込んで判定ができる。このため、干渉する可能性を小さくすることができるが、その一方で、干渉することなく作業が継続できる場合にも、干渉チェックで干渉ありと判定し、頻繁に停止してしまい、作業時間が長くなってしまうといった問題があった。
【０００６】
本発明は、このような課題を解決するためになされたものであり、干渉することなく作業が継続できる場合に干渉ありと判定してしまうことを抑制し、複数のロボットの作業効率を改善することができる干渉チェック制御装置および干渉チェック制御方法を得ることを目的とする。
【課題を解決するための手段】
【０００７】
本発明に係る干渉チェック制御装置は、それぞれの動作領域に重複する部分を有するロボット同士の動作中の接近をチェックし、ロボット同士の干渉を回避する干渉チェック制御装置であって、前回ステップまでの計算周期で算出されたロボットの速度・位置データを記憶する速度・位置記憶手段と、速度・位置記憶手段に記憶された速度・位置データに基づいて、ロボット構成部分同士の距離を計算するロボット間距離計算手段と、速度・位置記憶手段に記憶された速度・位置データに基づいてロボット構成部分同士の干渉判定領域を求める干渉判定用領域計算手段と、干渉判定用領域計算手段で求めた干渉判定領域で規定される干渉判定距離と、ロボット間距離計算手段で計算されたロボット構成部分同士の距離とを比較し干渉の有無を判定する干渉判定手段と、干渉判定手段による干渉判定結果に応じて、計算周期ごとに各ロボットに対する移動指令値を作り出すとともに、現在の計算ステップにおけるロボットの速度・位置データを速度・位置記憶手段に記憶させる指令値生成手段とを備え、干渉判定用領域計算手段は、求めた干渉判定領域を速度データに基づいて移動速度と移動方向に応じて指向性を持って拡張し、拡張された干渉判定領域を決定し、干渉判定手段は、干渉判定用領域計算手段で決定された拡張された干渉判定領域で規定される干渉判定距離と、ロボット間距離計算手段で計算されたロボット構成部分同士の距離とを比較し干渉の有無を判定するものである。
【０００８】
また、本発明に係る干渉チェック制御方法は、それぞれの動作領域に重複する部分を有するロボット同士の動作中の接近をチェックし、ロボット同士の干渉を回避する干渉チェック制御方法であって、記憶部に記憶された前回ステップまでの計算周期で算出されたロボットの速度・位置データに基づいて、ロボット構成部分同士の距離を計算するロボット間距離計算ステップと、記憶部に記憶された前回ステップまでの計算周期で算出されたロボットの速度・位置データに基づいてロボット構成部分同士の干渉判定領域を求めるステップと、干渉判定用領域計算ステップで求めた干渉判定領域で規定される干渉判定距離と、ロボット間距離計算ステップで計算されたロボット構成部分同士の距離とを比較し干渉の有無を判定する干渉判定ステップと、干渉判定ステップによる干渉判定結果に応じて、計算周期ごとに各ロボットに対する移動指令値を作り出すとともに、現在の計算ステップにおけるロボットの速度・位置データを記憶部に記憶させる指令値生成ステップとを備え、干渉判定用領域計算ステップは、求めた干渉判定領域を速度データに基づいて移動速度と移動方向に応じて指向性を持って拡張し、拡張された干渉判定領域を決定し、干渉判定ステップは、干渉判定用領域計算ステップで決定された拡張された干渉判定領域で規定される干渉判定距離と、ロボット間距離計算ステップで計算されたロボット構成部分同士の距離とを比較し干渉の有無を判定するものである。
【発明の効果】
【０００９】
本発明に係る干渉チェック制御装置および干渉チェック制御方法によれば、干渉判定領域のサイズを、単純に近似立体の法線方向に拡張するのではなくて、移動速度と移動方向に応じた指向性を持たせて拡張することにより、干渉することなく作業が継続できる場合に干渉ありと判定してしまうことを抑制し、複数のロボットの作業効率を改善することができる干渉チェック制御装置および干渉チェック制御方法を得ることができる。
【発明を実施するための最良の形態】
【００１０】
以下、本発明の干渉チェック制御装置および干渉チェック制御方法の好適な実施の形態につき図面を用いて説明する。
本発明は、近似立体を拡張することによって不必要に停止が生じる場合について分析し、近似立体サイズを単純に大きくすることが原因で停止が頻繁に起こっていることを見出した。そこで、本発明は、近似立体の移動速度と移動方向に注目して、近似立体を効果的に変形させることで、不必要な停止が起こる方向には領域を拡張しない干渉判定領域を設定し、従来と比較して優れた干渉チェックを実現している。
【００１１】
実施の形態１．
【００１２】
まず始めに、ロボット同士の干渉判定について説明する。まず、ロボットの可動範囲によって形成される、ロボットが通過しうる空間を動作領域と表現する。２台以上のロボット同士で動作領域が交差する場合は、ロボット同士の衝突の危険性が出てくる。このようなロボット同士の衝突を表す表現として、ロボットの占めている空間同士が交差している状態に干渉という表現を用いる。そのほか、後述の干渉判定領域同士が交差する状態にも干渉という表現を用いる。また、このような干渉が生じるかどうかを確認することを干渉チェックと呼ぶ。干渉チェックは、ロボットを、例えば、簡易な幾何モデル（点、線分、球、円筒、多面体）に置き換えて、干渉をチェックしたいロボットモデル間の距離を計算する。そして、計算された距離が許容値以内か否かを判定することで、干渉チェックが行われるのが一般的である。干渉チェックは、目的によって、以下の２つの利用方法がある。
【００１３】
シミュレーション上のロボット同士において干渉するか否かを検出する場合は、干渉の検出が目的となる。このため、干渉チェックとしては、動作をシミュレーション上で再生し、ロボットモデル同士の干渉があるか否かを判定することとなり、指令値の変更は、ユーザにゆだねられる。
【００１４】
一方で、動作中のロボット同士あるいはシミュレーションにおいては、干渉の検出とともに、干渉を回避することが目的となる。この場合には、実際に干渉が起こるよりも十分に早く干渉チェックを行う必要がある。従って、通常、ロボットモデルよりも大きい制動距離を見込んだ干渉判定領域を定義して、干渉判定領域同士が交差した場合に、停止を開始して干渉を防止する。ここでは、線分の周りに一定距離上に境界をもつ空間領域をロボットモデルとして定義し、干渉判定領域は、ロボットモデルと相似形状をしたロボットモデルよりも大きい一定距離上に境界をもつ空間領域として定義している。
【００１５】
上述した干渉チェックに関する事項を踏まえ、次に、本実施の形態１における干渉チェック方法について、具体的に説明する。図１は、本発明の実施の形態１における干渉チェック制御装置の構成図である。この図１の干渉チェック制御装置は、ロボット速度・位置記憶手段１１１、ロボット間距離計算手段１１２、干渉判定用領域計算手段１１３、干渉判定手段１１４、および指令値生成手段１１５を備えている。
【００１６】
図示していないが、上位に位置するロボット制御装置は、一定の計算周期に従って計算ステップごとに、ロボットの指令値を作り出している。その際に、ロボットの目標手先位置姿勢や各関節の目標角度、目標角速度、目標角加速度を計算している。それらの情報を用いて、干渉チェック制御装置は、ロボットの現在の６自由度手先位置姿勢および現在速度と、ロボットを構成するロボット構成部分の端点位置と端点速度とを算出している。例えば、各関節角度データをもとに運動学を解いてロボットの姿勢を計算して、該当する端点位置を算出可能である。また、端点速度は、端点速度の差分あるいは各駆動関節の変化速度から運動学を用いて解くことができる。また、６自由度手先位置姿勢と端点位置および端点速度の計算は、ロボット制御装置内において得られた結果を用いて計算しているので、ロボット制御装置内でも算出することができる。算出されたこれらのデータを、以下の説明では、速度・位置データ１０１と称することとする。そして、ロボット制御装置は、算出した速度・位置データ１０１を、各計算周期ごとにロボット速度・位置記憶手段１１１に記憶させる。
【００１７】
なお、ここで、６自由度手先位置姿勢とは、ハンドなどエンドエフェクタのワールド座標系に対する位置姿勢を指す。ただし、スカラロボット（４自由度）など自由度が６に満たないロボットでは、自由度に応じて手先位置姿勢が求められる。また、ロボット構成部分とは、例えば、ロボットの代表的剛体要素（例えば、ベース部、ベース側アーム部、リスト側アーム部、リスト部、およびエンドエフェクタ部）に分けてそれぞれの部分同士で干渉を考えるための、ロボットの干渉チェックの時のチェックする単位要素である。また、あるロボット構成部分は、ある１つの可動部分の動作により、他のロボット構成部分との干渉が発生しうるが、可動部分ごとにすべてを構成部分にすると計算量が増加するため、この例では、干渉チェックの計算量を減らすためになるべく少ない構成部分に分割している。
【００１８】
また、ロボット構成部分の端点情報を記憶しておく理由は、本実施の形態１で後述するように、ロボットをモデル化し距離計算や干渉判定を行う際に、線分モデルを用いているためである。従って、このロボット構成部分の端点情報としては、各リンクのモデルを構成するのに必要な端点位置や端点の移動速度情報であればよい。例えば、ロボット構成部分のベース側アーム部についてみると、ある時刻ｔ０のベース側アーム部について線分化したモデルについて、ベース側アーム部の線分モデルの端点の位置座標とその座標点の移動速度を計算して記憶しておく。線分の端点の位置座標の計算方法は、ある時刻ｔ０の関節角度情報から運動学を用いて求め、線分の端点速度については、例えば、時刻ｔ０における関節角速度情報を情報から運動学を用いて求めることになる。
【００１９】
次に、ロボット間距離計算手段１１２は、ロボット構成部分同士の距離を計算する手段であり、ロボット速度・位置記憶手段１１１に記憶された前回ステップまでの計算周期で算出された速度・位置データ１０２と、速度・位置データ１０１と同等のデータとしてあるロボット制御装置以外の外部から得られる速度・位置データ情報１０７とを読み込む。ここで、１０１と同等のデータとは、外部のロボット制御装置で算出されている、ロボットの現在の６自由度手先位置姿勢および現在速度と、ロボットを構成するロボット構成部分の端点位置と端点速度のことである。また、あるロボット制御装置以外の外部とは、例えば、他のロボット制御装置であったり、可動する冶具の制御装置などあるロボットと干渉する危険性のある移動をする機械装置の制御装置が挙げられる。また、移動する機械装置に限らず、干渉する危険性がある静止している機械装置についても、その機械装置の位置情報を外部から提供する。
【００２０】
次に、ロボット間距離計算手段１１２は、読み込んだデータに基づいて、対象としているロボット構成部分同士（例えば、ここでは、ロボット構成部分をモデル化した簡易なロボットモデル同士）の距離計算を行い、ロボット構成部分間距離（例えば、ここでは、ロボットモデル同士のロボットモデル間距離）１０３を、Ｌｉｊ１０３として出力する。
【００２１】
図２は、本発明の実施の形態１におけるロボット構成部分をモデル化した簡易なロボットモデルと干渉判定領域の例示図である。図２（ａ）のような構成のロボットモデルは、図２（ｂ）に示すように、ロボット構成部分をモデル化した簡易なロボットモデルとして扱うことができる。
【００２２】
より具体的には、各構成部分は、点および線分でモデル化される。さらに、モデル化された構成部分が線分の場合には、干渉を判定するための領域は、その線分から等距離となるような円柱と、その両端を覆う半球とからなる立体モデルとしてモデル化される。また、モデル化された構成部分が点の場合には、干渉を判定するための領域は、その点から等距離となる球からなる立体モデルとしてモデル化される。このような立体モデルをロボットモデルと呼ぶ。
【００２３】
例えば、ロボット構成部分であるロボットアーム部１１ａ〜１４ａは、線分でモデル化されたロボットアーム部モデル１１ｂ〜１４ｂに置き換えられる。また、ロボット構成部分の１つであるハンド１０ａは、ハンドモデル１０ｂに置き換えられる。そして、ロボット間距離計算手段１１２は、ロボット同士の構成部分間の最も接近している部分の距離測定を、このようにしてモデル化された簡易なロボットモデル同士の構成部分間の最も接近している部分の距離測定により行う。
【００２４】
図３は、本発明の実施の形態１におけるモデル化された２線分間の最近接点を幾何的に表した図である。例えば、ロボットモデル間距離を計測するための２つの線分をモデルＬｉｎｋｉとＬｉｎｋｊとすると、ロボット間距離計算手段１１２は、この２線分間の最近接点３１、３２（４１、４２）からモデル間距離Ｌｉｊ３３（４３）を導くようにしている。
【００２５】
２線分間の最近接点およびモデル間距離Ｌｉｊ１０３の計算方法は、例えば、特開昭６０−２１７４１０号公報に開示されているような手法を用いることができる。ただし、ロボットモデル間の距離は、線分モデル同士に限らず、その他にも、例えば、球や円筒や多面体でモデル化したロボットモデルに対して、モデル同士の最近接距離を計算しても構わない。
【００２６】
次に、所定の干渉判定領域を、さらに移動速度と移動方向に応じて指向性を持って拡張させることで、「拡張された干渉判定領域」を求める干渉判定用領域計算手段１１３の機能について説明する。ここで、所定の干渉判定領域とは、予めロボットモデルと同じかロボットモデルよりも大きく設定され、ロボットモデルから距離ＬｉＬｉｍだけ離れた部分を境界とする領域で定義される。
【００２７】
先の図２（ｂ）を例に説明すると、ロボットを包む簡易のＬｉＬｉｍロボットモデルを表す領域は、１０ｃ〜１４ｃで表される。そして、干渉を判定するための所定の干渉判定領域は、１０ｄ〜１４ｄで表される。前述したように、干渉判定領域は、ロボットモデルよりも常に大きいものとする。そして、本実施の形態１における干渉チェックは、停止動作をするか否かを判定する干渉判定領域を用いて行うものを対象としている。
【００２８】
拡張された干渉判定領域を求めるために、干渉判定用領域計算手段１１３は、前回ステップまでの計算周期にて算出された速度・位置データ１０２（ロボットの現在の６自由度手先姿勢および現在速度と、ロボットを構成するロボット構成部分である各リンクの端点位置および各リンクの端点速度情報）を用いて、所定の干渉判定領域を、現在進行している方向に対して拡張することで、「拡張された干渉判定領域」を決定する。さらに、「拡張された干渉判定領域」を決定した後に、干渉判定用領域計算手段１１３は、判定用の閾値を決定する。
【００２９】
図４は、本発明の実施の形態１における干渉判定用領域計算手段１１３による拡張された干渉判定領域の決定方法の説明図である。この図４では、説明を簡単にするために、半径ｒの球で所定の干渉判定領域が定義されている際に、拡張された干渉判定領域ＬｉｊｅｘＬｉｍ８０４を決定するための方法について説明する。
【００３０】
図４における符号８００〜８０５は、次のものを表している。
８００：ロボットモデル速度
８０１：実際のロボットを球で覆うように近似したロボットモデル
８０２：球でモデル化を行った際の所定の干渉判定領域
８０３：従来方法による拡張された干渉判定領域
８０４、８０５：本発明による拡張された干渉判定領域
【００３１】
また、図４（ａ）は、所定の干渉判定領域８０２を示した図であり、図４（ｂ）は、従来方法による拡張された干渉判定領域８０３を示した図である。また、図４（ｃ）、図４（ｄ）は、本発明による２通りの拡張された干渉判定領域８０４、８０５を示した図である。
【００３２】
拡張された干渉判定領域８０４の領域拡張方法は、現在のロボットモデルの移動方向に向かって移動速度の大きさに比例して所定の干渉回避領域８０２を拡張するものである。現在のロボットモデルに対して、このロボットモデルの移動速度を３次元ベクトルＶｍｏｖとおく。この移動速度とは、例えば、干渉チェック制御装置内で計算された各ロボットモデルの端点速度という速度データ１０２から求められるロボットモデルの移動速度であり、ロボットモデルの並進速度成分を利用する。例えば、Ｖｍｏｖの大きさに比例した拡張比Ｋｅｘ（ただし、Ｋｅｘ＞１）分だけ干渉判定領域ｒを拡張すると、従来方法による拡張された干渉判定領域８０３は、ＬｉｊｅｘＬｉｍとして、下式（１）で定義される（図４（ｂ）参照）。
ＬｉｊｅｘＬｉｍ＝Ｋｅｘ・ｒ （１）
【００３３】
これに対して、本発明による拡張された干渉判定領域８０４は、モデル移動方向に関しては、半径ｒによる干渉判定領域がＫｅｘだけ拡張され、進行していない方向（モデル移動方向と１８０度異なる方向）に関しては、所定の干渉判定領域（すなわち、半径ｒによる干渉判定領域）からサイズ変更がないようにして決定される。
【００３４】
この結果、モデル移動方向に図４（ｃ）に示す８１４だけ中心をずらすことで、拡張された干渉判定領域８０４を決定している。結局、拡張された干渉判定領域８０４における拡張された半径ｒｅｘは、下式（２）というサイズになる。
【００３５】
【数１】
【００３６】
このとき、拡張された干渉判定領域８０４の中心点位置ベクトルＰｃｅｘは、ロボットモデル中心点位置ベクトルＰｃからずれて、下式（３）だけずらせばよいことになる。
【００３７】
【数２】
【００３８】
このように、拡張された干渉判定領域８０４を決定した場合に、干渉判定用領域計算手段１１３は、モデル位置修正情報１０４ａとして上式（２）、（３）で定義されたｒｅｘおよびＰｃｅｘを、ロボット構成部分同士の距離を計算するロボット間距離計算手段１１２に入力する（図１参照）。
【００３９】
本実施の形態１の構成では、ロボット構成部分のモデル同士の距離計算を行う際にも、ロボット構成部分のモデルの中心が移動したものとして扱う必要がある。このため、ロボット構成部分同士の距離を計算するロボット間距離計算手段１１２は、速度・位置記憶手段１１１に記憶された速度・位置データ１０２と、干渉判定用領域計算手段１１３で求められたモデル位置修正情報１０４ａとに基づいて、ロボット構成部分同士の距離を計算する。
【００４０】
干渉判定用領域計算手段１１３により拡張された干渉判定領域８０４のサイズは、従来方法による拡張された干渉判定領域８０３よりも小さくなる。しかしながら、本来干渉の可能性が高いロボットモデルの移動方向に対しては、同等の干渉判定領域を保ったままで、後方方向など不必要に拡張していた干渉判定領域をなくすか、あるいは小さくした状態で、干渉チェックを行うことができる。
【００４１】
また、図４（ｄ）に示すような、本発明による拡張された干渉判定領域８０５を決定することもできる。モデル移動方向に関しては、その移動分（図４（ｄ）における８１５に相当）だけ中心をずらした上で、所定の干渉判定領域（すなわち、半径ｒによる干渉判定領域）からサイズ変更がないようにして半球が決定される。
【００４２】
一方、進行していない方向（モデル移動方向と１８０度異なる方向）に関しても、所定の干渉判定領域（すなわち、半径ｒによる干渉判定領域）からサイズ変更がないようにして半球が決定される。そして、モデル移動方向に関する領域と進行していない方向に関する領域でそれぞれ規定された半球の間を、中心が移動した線分から等距離となるような円柱で結ぶことにより、拡張された干渉判定領域８０５が決定される。
【００４３】
また、図５は、本発明の実施の形態１における干渉判定領域の定義方法の説明図であり、線分から等距離となるような円柱と、その両端を覆う半球とからなる立体モデルとしてモデル化された構成部分における干渉判定領域を示している。図５（ａ）は、従来方法による干渉判定領域を示しており、図５（ｂ）は、本発明の干渉判定領域を示している。
【００４４】
図５における符号８１１〜８１４は、次のものを表している。
８１１：実際のロボットを線分から等距離となるような円柱と、その両端を覆う半球
とで近似したロボットモデル
８１２：８１１としてモデル化を行った際の所定の干渉判定領域
８１３：従来方法による拡張された干渉判定領域
８０４：本発明による拡張された干渉判定領域
【００４５】
このような図５に示すような形状の構成部分についても、先の図４（ｃ）、（ｄ）の説明と同様に、モデル移動方向に関する領域と進行していない方向に関する領域を考慮することで、適切な干渉判定領域を設定できる。
【００４６】
この図４（ｃ）、（ｄ）あるいは図５に示したように、拡張された干渉判定領域８０４、８０５、８１４を決定することによるメリットを、図６、図７を用いて具体的に説明する。図６は、本発明の実施の形態１における追う側と逃げる側のモデル間の干渉に関する説明図である。図６（ａ）は、従来方法による拡張された干渉判定領域８０３を用いた場合の追う側と逃げる側のモデル間の干渉を例示している。一方、図６（ｂ）は、本発明による拡張された干渉判定領域８０４を用いた場合の追う側と逃げる側のモデル間の干渉を例示している。
【００４７】
速度に応じて制動距離分だけ領域を拡張した場合に、図６（ａ）に示すように、従来手法の場合には、逃げる側の領域が過剰に延びており、交差領域８２０が発生し、追う側が過度に停止をすることになる。一方、図６（ｂ）に示すように、本発明では、モデル移動方向と１８０度異なる方向に関しては、所定の干渉判定領域からサイズ変更がないようにして決定されるため、交差領域が発生しない。この結果、本来拡張しなくてよい方向に緩衝領域を拡張しないことで、ロボットの停止状態を減らし、ロボットシステムの作業効率を上げる効果がある。
【００４８】
また、図７は、本発明の実施の形態１におけるハンドなどのエンドエフェクタモデルが近くをすれ違う場合の干渉に関する説明図である。従来手法であるモデル８０３では、誤検知が発生（すなわち、先の図６（ａ）と同様に交差領域が発生）していた。これに対して、本発明による拡張された干渉判定領域８０５では、交差領域が発生せず、干渉判定で停止なくすれ違うことができる。この結果、先の図６の場合と同様に、ロボットの停止状態を減らし、ロボットシステムの作業効率を上げる効果がある。
【００４９】
最後に、干渉判定手段１１４および指令値生成手段１１５における処理について説明する。干渉判定手段１１４は、干渉判定を行いたいロボット構成部分２つ（ＬｉとＬｊ）の全てのロボット構成部分の組に対して、ロボット構成部分のロボットモデル同士の最近接距離で定義されるロボット構成部分のモデル間距離Ｌｉｊ１０３と干渉判定領域（ＬｉＬｉｍとＬｊＬｉｍ）の和で定義される干渉判定用の閾値ＬｉｊＬｉｍ１０４ｂを利用して干渉判定を行う。
【００５０】
干渉判定手段１１４は、下式（４）が成り立つときに、干渉危険ありと判定して停止開始を行う指示１０５を指令値生成手段１１５に通知する。
Ｌｉｊ≦ＬｉｊＬｉｍ （４）
【００５１】
次に、指令値生成手段１１５は、干渉判定結果に応じて計算ステップごとに指令値を作り出す。より具体的には、指令値生成手段１１５は、干渉判定領域同士が交差している場合には、停止の為の減速を指令し、干渉判定領域が交差していない場合には、目標指令に従った指令値を計算する。さらに、指令値生成手段１１５は、現在のロボット姿勢と速度１０１をロボット速度・位置記憶手段１１１に記憶させる。
【００５２】
以上のように、実施の形態１によれば、以下の効果を得ることができる。速度に応じてあるいはロボット間距離に応じて拡張された干渉判定領域を定義する際に、従来技術においては、進行方向に対しては十分に判定領域を広げる効果があった。しかしながら、進行方向以外の方向に対しては、過度に拡張されるために、交差領域を誤検知する結果となっていた。この結果、高速で動作をさせると、不要な停止を引き起こすといった問題があった。
【００５３】
これに対して、本実施の形態１では、移動速度と移動方向に応じた指向性を考慮しており、中心が進行方向にずれて、元の領域を含むように拡張された干渉判定領域（図４（ｃ）の干渉判定領域８０４参照）、あるいは球の進行方向に円筒領域として定義した拡張された干渉判定領域（図４（ｄ）の干渉判定領域８０５参照）を利用している。この結果、複数台のロボットから構成されるようなロボットシステムにおいて、交差領域を誤検知する可能性を低減することができ、不要な停止指令を少なくして、システムの作業効率を上げる効果がある。
【００５４】
実施の形態２．
図８は、本発明の実施の形態２における干渉チェック制御装置の構成図である。先の実施の形態１における図１の構成と比較すると、この図８の構成は、基本的には、図１と同一であるが、モデル位置修正１０４ａが必要ない構成となっている点が異なっている。
【００５５】
より具体的には、干渉判定用領域計算手段１１３の機能が異なっている。本実施の形態２における干渉判定用領域計算手段１１３は、ロボットモデルの進行方向ベクトルと最短距離方向ベクトルのなす角に応じて干渉判定領域の拡張サイズを決定するように構成されている。
【００５６】
先の実施の形態１では、所定の干渉判定領域をさらに移動速度と移動方向に応じて指向性を持って拡張させる干渉判定用領域計算手段１１３と、ロボット構成部分のモデル同士の距離を計算するロボット間距離計算手段１１２との間には関連があった。すなわち、先の実施の形態１における干渉判定領域の拡張方法は、モデルが中心軸となる線分を並進移動した際に、干渉判定領域を元の判定領域を包む形で拡張する形態であった。
【００５７】
このような、先の実施の形態１の拡張方法では、ロボット構成部分のモデル同士の距離計算を行う際にも、ロボット構成部分のモデルの中心が移動したものとして扱う必要がある。このため、ロボット構成部分同士の距離を計算するロボット間距離計算手段１１２に、修正されたモデル位置修正情報１０４ａを入力することが必要であった。
【００５８】
これに対して、本実施の形態２における干渉判定領域の計算方法では、モデル中心軸となる線分からロボットモデルの進行方向ベクトルと最短距離方向ベクトルのなす角に応じて拡張サイズを決定する。例えば、簡単のために、球モデルにおいて、球モデルの干渉判定領域をＬｉＬｉｍ進行方向への拡張率Ｋｅｘ（≧１）、ロボットモデル（ここでは球モデル）の移動する方向で定義する進行方向ベクトルＶｄ、干渉チェックを行う２つのロボットモデルのそれぞれの最近接点を結んでできる方向ベクトルを判定する最近接点の方向ベクトルＮｃｌｓとする。このとき、判定方向への干渉判定領域の閾値ＬｉＬｉｍｄは、下式（５）、（６）というＬｉＬｉｍｄを定義して、判定する線分モデルＬｉｎｋｉとＬｉｎｋｊに対して、それぞれＬｉＬｉｍｄとＬｊＬｉｍｄを生成する。
【００５９】
【数３】
【００６０】
ここで、ＬｉｊＬｉｍｄは、
ＬｉｊＬｉｍｄ＝ＬｉＬｉｍｄ＋ＬｊＬｉｍｄ （７）
で定義される。干渉判定用領域計算手段１１３は、このようにして、判定方向への干渉判定用の閾値１０４を算出し、干渉判定手段１１４に対して出力する。このとき、ＬｉＬｉｍｄおよびＬｊＬｉｍｄは、例えば、上式（５）（６）のように定義されることで、本実施の形態１に示したような円柱や球のような単純な形状に限らない拡張した干渉判定領域を定義できる。
【００６１】
そして、干渉判定手段１１４は、ロボット構成部分間距離１０３に対して、このＬｉｊＬｉｍｄを用い、干渉判定領域の交差判定を、下式（８）に基づいて行う。
Ｌｉｊ≦ＬｉｊＬｉｍｄ （８）
この結果、先の実施の形態１の図１で示したモデル位置修正１０４ａが不要となる。
【００６２】
以上のように、実施の形態２によれば、以下の効果を得ることができる。本実施の形態２では、モデル中心軸となる線分からロボットモデルの進行方向ベクトルと最短距離方向ベクトルのなす角に応じて拡張サイズを決定している。これにより、先の実施の形態１と同様に、複数台のロボットから構成されるようなロボットシステムにおいて、不要な停止指令を少なくして、システムの作業効率を上げる効果がある。
【００６３】
さらに、本実施の形態２では、拡張される干渉判定領域を、変形する形状として幾何的に算出するのではなく、数値的に算出できるため、判定方法としては、より広く使うことができる。つまり、特に、上式（５）（６）に限定することはなく、すべての方向に対して所定の干渉判定領域ＬｉＬｉｍが本実施の形態２で定義される拡張された干渉判定領域ＬｉｊＬｉｍｄ以下の大きさになっている数式表現であればよいので、本実施の形態１における拡張した干渉判定領域の定義よりも多くの拡張した干渉判定領域を定義することができ、より交差領域を誤検知することの少ない干渉判定領域を選ぶことができる。
【００６４】
実施の形態３．
本実施の形態３では、所定の干渉判定領域をさらに移動速度と移動方向に応じて指向性を持って拡張させる干渉判定用領域計算手段１１３が、停止時などの特定の条件下において、ロボットモデルと干渉判定領域の初期の大きさを揃えるように変更する場合について説明する。構成は、先の実施の形態１における図１と同様である。
【００６５】
図９は、本発明の実施の形態３におけるロボットとロボットモデルと干渉回避領域の包含関係を表した説明図である。干渉判定領域を用いた干渉判定方法において、一般的には、ロボットモデルや干渉判定領域は、図９に示すように、ロボット５１の周りを十分に取り囲むロボットモデル５２のサイズに対して、マージンを持たせた干渉判定領域５３を利用している。
【００６６】
これは、各ロボット間の精度誤差やロボット高速動作時にも干渉することなく停止するため、余分に領域を広げていることによる。本発明における干渉判定領域の拡張方法においても、先の実施の形態１、２で説明したように、確実に干渉回避を行うために、干渉判定領域は、従来手法と同様に、やや大きめに設定されることになる。しかしながら、このような設定によって、完全には不必要な停止を無くせない場合が生じることとなる。
【００６７】
そこで、本実施の形態３では、移動速度に応じて、例えば、ロボットが停止時あるいは所定の速度以下の低速移動時には、ロボットモデルと干渉判定領域の初期の大きさを揃えることで、不必要な停止をより低減させることを行う。このように、拡張させる干渉判定領域を変更し、例えば、停止時にはロボットモデルと干渉判定領域の大きさを同じにすることで、他の移動中のロボットを停止させなくても作業続行可能である場合（例えば、低速で近接してすれ違い移動するような場合）には、作業が続けられるようになる。
【００６８】
この際の干渉判定領域の大きさは、速度に応じて、先の数式（１）で定義したもので定義し、ロボットモデルの領域の大きさをｒｍ（＜球モデルの干渉判定領域の半径ｒ）とおくときに、干渉判定領域の大きさＬｉｊｅｘＬｉｍは、
ＬｉｊｅｘＬｉｍ＝Ｋｅｘ・ｒｍ （９）
と定義される。すなわち、ｒではなくｒｍに対して、移動速度の大きさに比例した拡張比Ｋｅｘを掛け合わせることで干渉判定領域の大きさを求めている。
【００６９】
以上のように、実施の形態３によれば、先の実施の形態１よりも交差領域を誤検知する可能性を低減することができ、さらに作業停止時間が減り、作業効率が上がるという効果がある。
【００７０】
実施の形態４．
本実施の形態４では、干渉ありと判定された後の減速停止動作中に、その速度に応じて干渉判定を継続して行い、干渉の心配がないと判断された場合には、停止のための減速を中断し動作を再開する方法について説明する。
【００７１】
図１０は、本発明の実施の形態３において、ロボットモデル間距離Ｌｉｊが許容値ＬｉｊＬｉｍに達したために停止のための減速開始を行う状態を示した図である。また、図１１は、本発明の実施の形態３において、減速動作による干渉判定領域の縮小に伴って小さくなった許容値Ｌ'ｉｊＬｉｍとの比較で、許容値よりもロボットモデル間距離Ｌｉｊが大きくなり、停止動作が不要になったため、動作を再開した状態を示した図である。
【００７２】
図１０に示すように、時刻ｔｎｏｗにおいて、拡張された干渉判定領域に対して大きさＬｉｊＬｉｍ６０３を用いて干渉判定を行った際に、干渉ありと判定された場合には、停止のための減速動作を開始する。この場合、本実施の形態４における干渉チェック制御装置は、減速中にも干渉判定を継続して行う。
【００７３】
そして、図１１に示すように、時刻ｔ’ｎｏｗにおいて、減速に従い縮小が生じた干渉判定領域から算出される判定のための閾値Ｌ'ｉｊＬｉｍ６０６を用いて干渉判定を行った際に、Ｌ'ｉｊＬｉｍ≦ＬｉｊＬｉｍが成り立った場合には、停止のための減速を中断し、動作を再開する。
【００７４】
停止動作に入った後に速度が減速すると、先の実施の形態３で説明した方法を用いると、算出される拡張された干渉判定領域の大きさは、現状の速度に応じて変化する。この結果、干渉する危険があるという判定（図１０の時刻ｔｎｏｗに相当）から、干渉する危険が無いという判定（図１１の時刻ｔ’ｎｏｗに相当）に変わる場合がある。
【００７５】
従って、本実施の形態４のように、現状の速度に応じて拡張された干渉判定領域の大きさを求めることで、干渉回避動作を中断できるかどうかを判定し、干渉回避動作が中断されるときには引き続き動作を継続させることができる。これによって、停止するロボットが停止位置として、より目標位置に近いところで停止する、あるいは作業を継続することもできる。
【００７６】
以上のように、実施の形態４によれば、現状の速度に応じて拡張された干渉判定領域の大きさを求めることで、干渉回避動作を再開することが可能となり、先の実施の形態１よりも交差領域を誤検知する可能性を低減することができ、さらに作業停止時間が減り、作業効率が上がるという効果がある。
【００７７】
実施の形態５．
第５の実施の形態では、ロボット相互の軌道に応じて、干渉判定領域を切り替える場合について説明する。所定の干渉判定領域をさらに移動速度と移動方向に応じて指向性を持って拡張させる干渉判定用領域計算手段１１３は、例えば、２つのロボット構成部分が直線軌道で行き違う場合（すなわち、互いの直線軌道が平行である場合）は、先の図４（ｄ）で説明したような、拡張された干渉判定領域８０５を用いることが考えられる。
【００７８】
図１２は、本発明の実施の形態５における干渉判定の説明図である。ここでは、図１２に示すように、２つのロボット構成部分が直線軌道で行き違うという場合において、従来方法と本発明の手法の比較を説明する。
【００７９】
図１２では、ある時刻ｔｉからｔｉ+４までの離散的な時間におけるロボット構成部分のモデル間の距離計算から干渉チェックを行う場合を例示している。図１２において、７７ａは、実際に停止が必要な場合に対して従来の判定方法を用いた場合を示しており、７７ｂは、実際に停止が不要な場合に対して従来の判定方法を用いた場合を示している。これに対して、７８ａは、実際に停止が必要な場合に対して本実施の形態５の判定方法を用いた場合を示しており、７８ｂは、実際に停止が不要な場合に対して本実施の形態５の判定方法を用いた場合を示している。
【００８０】
従来の判定方法（７７ａ、７７ｂ）では、速度に応じて判定領域を大きくする手法を用いることで、ロボット構成部分のモデル同士のチェックで、見落としていた干渉を見落とさなくなっている。しかしながら、７７ｂでは、拡張された干渉判定領域が速度に応じて大きくなっているため、実際には干渉しないのだが、干渉すると判定されてしまっている。
【００８１】
一方、本実施の形態５の判定方法（７８ａ、７８ｂ）では、先の図４（ｄ）で説明したような、拡張された干渉判定領域８０５を用いている。この結果、実際に停止が必要な場合（７８ａ）には、干渉すると判定でき、実際に停止が必要な場合（７８ｂ）には、不必要な干渉判定をしないで済んでいることが分かる。
【００８２】
この例では、直線軌道に適した図４（ｄ）の拡張された干渉判定領域８０５を用いたが、軌道に応じて、図４（ｃ）の拡張された干渉判定領域８０４を用いてもよく、ロボット相互の軌道があらかじめ特定できる場合には、その軌道に応じて、拡張された干渉判定領域を適切なサイズに設定することで、不必要な干渉判断を防止することができる。
【００８３】
以上のように、実施の形態５によれば、ロボット相互の軌道があらかじめ特定できる場合には、軌道に応じて、拡張された干渉判定領域を適切なサイズに設定することができる。この結果、従来方法では、干渉が実際に起こらなくても干渉すると判定されていた場合に対して、ユーザがチェックと指令値の再編集をする必要があったが、そのような手間がなくなり、オフライン干渉チェックに関して作業効率が上がるという効果が得られる。
【図面の簡単な説明】
【００８４】
【図１】本発明の実施の形態１における干渉チェック制御装置の構成図である。
【図２】本発明の実施の形態１におけるロボット構成部分をモデル化した簡易なロボットモデルと干渉判定領域の例示図である。
【図３】本発明の実施の形態１におけるモデル化された２線分間の最近接点を幾何的に表した図である。
【図４】本発明の実施の形態１における干渉判定用領域計算手段による拡張された干渉判定領域の決定方法の説明図である。
【図５】本発明の実施の形態１における干渉判定領域の定義方法の説明図である。
【図６】本発明の実施の形態１における追う側と逃げる側のモデル間の干渉に関する説明図である。
【図７】本発明の実施の形態１におけるハンドなどのエンドエフェクタモデルが近くをすれ違う場合の干渉に関する説明図である。
【図８】本発明の実施の形態２における干渉チェック制御装置の構成図である。
【図９】本発明の実施の形態３におけるロボットとロボットモデルと干渉回避領域の包含関係を表した説明図である。
【図１０】本発明の実施の形態３において、ロボットモデル間距離Ｌｉｊが許容値ＬｉｊＬｉｍに達したために停止のための減速開始を行う状態を示した図である。
【図１１】本発明の実施の形態３において、減速動作による干渉判定領域の縮小に伴って小さくなった許容値Ｌ'ｉｊＬｉｍとの比較で、許容値よりもロボットモデル間距離Ｌｉｊが大きくなり、停止動作が不要になったため、動作を再開した状態を示した図である。
【図１２】本発明の実施の形態５における干渉判定の説明図である。
【符号の説明】
【００８５】
１１１ ロボット速度・位置記憶手段、１１２ ロボット間距離計算手段、１１３ 干渉判定用領域計算手段、１１４ 干渉判定手段、１１５ 指令値生成手段。
【特許請求の範囲】
【請求項１】
それぞれの動作領域に重複する部分を有するロボット同士の動作中の接近をチェックし、前記ロボット同士の干渉を回避する干渉チェック制御装置であって、
前回ステップまでの計算周期で算出されたロボットの速度・位置データを記憶する速度・位置記憶手段と、
前記速度・位置記憶手段に記憶された前記速度・位置データに基づいて、ロボット構成部分同士の距離を計算するロボット間距離計算手段と、
前記速度・位置記憶手段に記憶された速度・位置データに基づいてロボット構成部分同士の干渉判定領域を求める干渉判定用領域計算手段と、
前記干渉判定用領域計算手段で求めた前記干渉判定領域で規定される干渉判定距離と、前記ロボット間距離計算手段で計算されたロボット構成部分同士の距離とを比較し干渉の有無を判定する干渉判定手段と、
前記干渉判定手段による干渉判定結果に応じて、計算周期ごとに各ロボットに対する移動指令値を作り出すとともに、現在の計算ステップにおけるロボットの速度・位置データを前記速度・位置記憶手段に記憶させる指令値生成手段と
を備え、
前記干渉判定用領域計算手段は、求めた前記干渉判定領域を速度データに基づいて移動速度と移動方向に応じて指向性を持って拡張し、拡張された干渉判定領域を決定し、
前記干渉判定手段は、前記干渉判定用領域計算手段で決定された前記拡張された干渉判定領域で規定される干渉判定距離と、前記ロボット間距離計算手段で計算されたロボット構成部分同士の距離とを比較し干渉の有無を判定する
ことを特徴とする干渉チェック制御装置。
【請求項２】
請求項１に記載の干渉チェック制御装置において、
前記干渉判定用領域計算手段は、前記拡張された干渉判定領域を決定する際に、前記指向性を考慮したことに伴うロボット位置修正情報を生成し、
前記ロボット間距離計算手段は、前記速度・位置記憶手段に記憶された前記速度・位置データ、および前記干渉判定用領域計算手段により生成された前記ロボット位置修正情報に基づいて、ロボット構成部分同士の距離を計算する
ことを特徴とする干渉チェック制御装置。
【請求項３】
請求項１に記載の干渉チェック制御装置において、
前記干渉判定用領域計算手段は、前記干渉判定領域を前記指向性を持って拡張する際に、ロボットの進行方向ベクトルと最短距離方向ベクトルのなす角とに応じて拡張サイズを決定し、前記拡張された干渉判定領域を決定する
ことを特徴とする干渉チェック制御装置。
【請求項４】
それぞれの動作領域に重複する部分を有するロボット同士の動作中の接近をチェックし、前記ロボット同士の干渉を回避する干渉チェック制御方法であって、
記憶部に記憶された前回ステップまでの計算周期で算出されたロボットの速度・位置データに基づいて、ロボット構成部分同士の距離を計算するロボット間距離計算ステップと、
前記記憶部に記憶された前回ステップまでの計算周期で算出されたロボットの速度・位置データに基づいてロボット構成部分同士の干渉判定領域を求めるステップと、
前記干渉判定用領域計算ステップで求めた前記干渉判定領域で規定される干渉判定距離と、前記ロボット間距離計算ステップで計算されたロボット構成部分同士の距離とを比較し干渉の有無を判定する干渉判定ステップと、
前記干渉判定ステップによる干渉判定結果に応じて、計算周期ごとに各ロボットに対する移動指令値を作り出すとともに、現在の計算ステップにおけるロボットの速度・位置データを前記記憶部に記憶させる指令値生成ステップと
を備え、
前記干渉判定用領域計算ステップは、求めた前記干渉判定領域を、速度データに基づいて移動速度と移動方向に応じて指向性を持って拡張し、拡張された干渉判定領域を決定し、
前記干渉判定ステップは、前記干渉判定用領域計算ステップで決定された前記拡張された干渉判定領域で規定される干渉判定距離と、前記ロボット間距離計算ステップで計算されたロボット構成部分同士の距離とを比較し干渉の有無を判定する
ことを特徴とする干渉チェック制御方法。
【請求項１】
それぞれの動作領域に重複する部分を有するロボット同士の動作中の接近をチェックし、前記ロボット同士の干渉を回避する干渉チェック制御装置であって、
前回ステップまでの計算周期で算出されたロボットの速度・位置データを記憶する速度・位置記憶手段と、
前記速度・位置記憶手段に記憶された前記速度・位置データに基づいて、ロボット構成部分同士の距離を計算するロボット間距離計算手段と、
前記速度・位置記憶手段に記憶された速度・位置データに基づいてロボット構成部分同士の干渉判定領域を求める干渉判定用領域計算手段と、
前記干渉判定用領域計算手段で求めた前記干渉判定領域で規定される干渉判定距離と、前記ロボット間距離計算手段で計算されたロボット構成部分同士の距離とを比較し干渉の有無を判定する干渉判定手段と、
前記干渉判定手段による干渉判定結果に応じて、計算周期ごとに各ロボットに対する移動指令値を作り出すとともに、現在の計算ステップにおけるロボットの速度・位置データを前記速度・位置記憶手段に記憶させる指令値生成手段と
を備え、
前記干渉判定用領域計算手段は、求めた前記干渉判定領域を速度データに基づいて移動速度と移動方向に応じて指向性を持って拡張し、拡張された干渉判定領域を決定し、
前記干渉判定手段は、前記干渉判定用領域計算手段で決定された前記拡張された干渉判定領域で規定される干渉判定距離と、前記ロボット間距離計算手段で計算されたロボット構成部分同士の距離とを比較し干渉の有無を判定する
ことを特徴とする干渉チェック制御装置。
【請求項２】
請求項１に記載の干渉チェック制御装置において、
前記干渉判定用領域計算手段は、前記拡張された干渉判定領域を決定する際に、前記指向性を考慮したことに伴うロボット位置修正情報を生成し、
前記ロボット間距離計算手段は、前記速度・位置記憶手段に記憶された前記速度・位置データ、および前記干渉判定用領域計算手段により生成された前記ロボット位置修正情報に基づいて、ロボット構成部分同士の距離を計算する
ことを特徴とする干渉チェック制御装置。
【請求項３】
請求項１に記載の干渉チェック制御装置において、
前記干渉判定用領域計算手段は、前記干渉判定領域を前記指向性を持って拡張する際に、ロボットの進行方向ベクトルと最短距離方向ベクトルのなす角とに応じて拡張サイズを決定し、前記拡張された干渉判定領域を決定する
ことを特徴とする干渉チェック制御装置。
【請求項４】
それぞれの動作領域に重複する部分を有するロボット同士の動作中の接近をチェックし、前記ロボット同士の干渉を回避する干渉チェック制御方法であって、
記憶部に記憶された前回ステップまでの計算周期で算出されたロボットの速度・位置データに基づいて、ロボット構成部分同士の距離を計算するロボット間距離計算ステップと、
前記記憶部に記憶された前回ステップまでの計算周期で算出されたロボットの速度・位置データに基づいてロボット構成部分同士の干渉判定領域を求めるステップと、
前記干渉判定用領域計算ステップで求めた前記干渉判定領域で規定される干渉判定距離と、前記ロボット間距離計算ステップで計算されたロボット構成部分同士の距離とを比較し干渉の有無を判定する干渉判定ステップと、
前記干渉判定ステップによる干渉判定結果に応じて、計算周期ごとに各ロボットに対する移動指令値を作り出すとともに、現在の計算ステップにおけるロボットの速度・位置データを前記記憶部に記憶させる指令値生成ステップと
を備え、
前記干渉判定用領域計算ステップは、求めた前記干渉判定領域を、速度データに基づいて移動速度と移動方向に応じて指向性を持って拡張し、拡張された干渉判定領域を決定し、
前記干渉判定ステップは、前記干渉判定用領域計算ステップで決定された前記拡張された干渉判定領域で規定される干渉判定距離と、前記ロボット間距離計算ステップで計算されたロボット構成部分同士の距離とを比較し干渉の有無を判定する
ことを特徴とする干渉チェック制御方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【公開番号】特開２０１０−５２１１４（Ｐ２０１０−５２１１４Ａ）
【公開日】平成２２年３月１１日（２０１０．３．１１）
【国際特許分類】
【出願番号】特願２００８−２２２１０５（Ｐ２００８−２２２１０５）
【出願日】平成２０年８月２９日（２００８．８．２９）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
【公開日】平成２２年３月１１日（２０１０．３．１１）
【国際特許分類】
【出願日】平成２０年８月２９日（２００８．８．２９）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
[ Back to top ]