干渉チェック制御装置および干渉チェック制御方法
【課題】制動動作中におけるロボットの構成部分同士の干渉の有無を高精度に判定する。
【解決手段】動作領域に重複部分を有するロボット同士の干渉チェック制御装置であって、速度・位置記憶手段９１０と、干渉判定領域の閾値をあらかじめ記憶する干渉判定用閾値記憶手段９１８と、速度・位置データに基づいて制動開始から停止までに必要な時間を計算する制動時間計算手段９１１と、制動開始から制動完了までの各構成部分間の最近接距離の履歴を、接近する各構成部分の速度に基づいて推定するロボット間距離推定手段９１６と、各構成部分間の最近接距離の履歴と干渉判定領域の閾値とを比較し、制動期間中の干渉の有無を判定する干渉判定手段９１７と、干渉判定結果に応じて各ロボットに対する移動指令値を作り出すとともに、ロボットの速度・位置データを速度・位置記憶手段に記憶させる指令値生成手段９１９とを備える。
【解決手段】動作領域に重複部分を有するロボット同士の干渉チェック制御装置であって、速度・位置記憶手段９１０と、干渉判定領域の閾値をあらかじめ記憶する干渉判定用閾値記憶手段９１８と、速度・位置データに基づいて制動開始から停止までに必要な時間を計算する制動時間計算手段９１１と、制動開始から制動完了までの各構成部分間の最近接距離の履歴を、接近する各構成部分の速度に基づいて推定するロボット間距離推定手段９１６と、各構成部分間の最近接距離の履歴と干渉判定領域の閾値とを比較し、制動期間中の干渉の有無を判定する干渉判定手段９１７と、干渉判定結果に応じて各ロボットに対する移動指令値を作り出すとともに、ロボットの速度・位置データを速度・位置記憶手段に記憶させる指令値生成手段９１９とを備える。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、近接配置され動作領域が重複している複数のロボットの干渉チェック制御装置および干渉チェック制御方法に関する。
【背景技術】
【０００２】
少なくとも２台以上のロボットの動作領域が重なる状態で同時に作業するロボットシステムにおいて、ロボット同士の干渉チェックを行なう技術が開示されている。一般に、ロボットをいくつかの構成部分に分け、ロボット構成部分を覆うように例えば円筒と半球を用いた近似立体でロボット構成部分をモデル化して、これら近似立体間の距離を計算することから衝突を検知する方法（特許文献１参照）が行われており、例えばロボットを単純な線分でモデル化してその線分間の最近接距離を計算し干渉状態をチェックする方法が行われている。
【０００３】
前記のような干渉チェック方法を用いて各ロボット同士が干渉しないよう停止する技術として、例えば、エンドエフェクタなどの取り付けてある手先部分であるロボットのツールの周りに該ロボットのツールの大きさよりも大きい２重の干渉判定領域を設けてその干渉状態と各ロボット同士の相対的な移動方向の関係にあわせて、片方を停止させるか両方を停止させるか決定する方法（特許文献２参照）がある。また、干渉回避のために停止動作を開始した際の停止完了時の姿勢を計算し、その姿勢に対して各ロボット同士の干渉の有無を判定して、停止完了時の姿勢に対して各ロボット同士の干渉があれば停止必要と判断し停止を開始する方法（特許文献３参照）という先行技術がある。
【０００４】
【特許文献１】特開６０-２１７４１０号公報
【特許文献２】特許第３３５１３３０号公報
【特許文献３】特許第３９０７６４９号公報
【発明の開示】
【発明が解決しようとする課題】
【０００５】
しかしながら、従来技術には、以下のような課題がある。
ロボットの手先軌道が教示軌道に沿って、例えば直線や円弧を描く際には、各ロボット構成部分は、単調動作ではなく複雑に動く場合がある。あるロボット構成部分と他ロボット構成部分が相対的に近づいたり離れたりする動きに該当する。ロボット同士が近接配置され動作領域が重複する際には、該ロボット構成部分の複雑な動作によって各ロボット構成部分同士が干渉する危険性があるというのは大きな問題であり、従来方法ではそのようなロボット構成部分間の干渉チェックができていなかった。
【０００６】
従来方法は、例えば、特許文献２ではロボットのツールに限定されており構成部分同士の距離判定を行なっていないためツール部分同士は干渉していなくてもその停止動作中に別の構成部分同士が干渉する可能性があった。特許文献３では、ロボット制御指令を作る制御計算周期ごとに干渉チェックをする時に、ある時刻で計算されたロボット停止位置と、１制御周期分だけ次の時刻に計算された停止位置の停止位置間隔が、特に高速動作時に大きくなってしまうため、干渉チェックを行なう隣り合った停止位置間においてはロボット構成部分同士が干渉している可能性を判定できない。
【０００７】
本発明は、前記のような課題を解決するためになされたものであり、制動動作中におけるロボットの構成部分同士の干渉の有無を高精度に判定することのできる干渉チェック制御装置および干渉チェック制御方法を得ることを目的とする。
【課題を解決するための手段】
【０００８】
本発明に係る干渉チェック制御装置は、それぞれの動作領域に重複する部分を有するロボット同士の干渉チェック制御装置であって、計算周期ごとに算出されたロボットの速度・位置データを記憶する速度・位置記憶手段と、各ロボット間の干渉判定を行なうための干渉判定領域の閾値をあらかじめ記憶する干渉判定用閾値記憶手段と、速度・位置記憶手段に記憶された速度・位置データに基づいて、ロボットが現時点から制動を開始した際に完全に停止するまでに必要な時間を計算する制動時間計算手段と、制動開始から制動完了までの各ロボット同士の各構成部分間の最近接距離の履歴を、接近する各構成部分の速度に基づいて推定するロボット間距離推定手段と、推定された各構成部分間の最近接距離の履歴と、干渉判定用閾値記憶手段にあらかじめ記憶された干渉判定領域の閾値とを比較し、制動開始から制動完了までの間の干渉の有無を判定する干渉判定手段と、干渉判定手段による干渉判定結果に応じて、計算周期ごとに各ロボットに対する移動指令値を作り出すとともに、現在の計算周期におけるロボットの速度・位置データを速度・位置記憶手段に記憶させる指令値生成手段とを備えるものである。
【０００９】
本発明に係る干渉チェック制御方法は、それぞれの動作領域に重複する部分を有するロボット同士の干渉チェック制御方法であって、第１の記憶部に記憶された速度・位置データに基づいて、ロボットが現時点から制動を開始した際に完全に停止するまでに必要な時間を計算する制動時間計算ステップと、制動開始から制動完了までの各ロボット同士の各構成部分間の最近接距離の履歴を、接近する各構成部分の速度に基づいて推定するロボット間距離推定ステップと、推定された各構成部分間の最近接距離の履歴と、第２の記憶部にあらかじめ記憶された干渉判定領域の閾値とを比較し、制動開始から制動完了までの間の干渉の有無を判定する干渉判定ステップと、干渉判定ステップによる干渉判定結果に応じて、計算周期ごとに各ロボットに対する移動指令値を作り出すとともに、現在の計算周期におけるロボットの速度・位置データを第１の記憶部に記憶させる指令値生成ステップとを備えるものである。
【発明の効果】
【００１０】
本発明に係る干渉チェック制御装置および干渉チェック制御方法によれば、制動開始から制動完了までの間の構成部分同士の最近接距離の変化を、接近する各構成部分の速度に基づいて推定することにより、制動動作中におけるロボットの構成部分同士の干渉の有無を高精度に判定することのできる干渉チェック制御装置および干渉チェック制御方法を得ることができる。
【発明を実施するための最良の形態】
【００１１】
以下、本発明の干渉チェック制御装置および干渉チェック制御方法の好適な実施の形態につき図面を用いて説明する。
本発明は、制動開始から制動完了までにロボット構成部分同士の最近接距離の履歴を、接近する各構成部分の速度に基づいて推定し、制動開始から制動完了までにロボット同士の干渉があるか否かを、ロボット構成部分ごとの最近接距離の変化の推定結果に基づいて判定している。この結果、制動動作中に構成部分同士が干渉するケースを見落とすことがなくなり、近接配置され動作領域が重複するロボット間の干渉問題において、従来方法よりももれなく干渉状態を発見し、ロボットの破損などを防ぐ効果がある。
【００１２】
また、以下の説明中での制動とは、停止をするために各可動部分の駆動源に減速指令をかけて、現在速度と出力可能な減速のための加速度によって規定される時間後に停止する動作のことである。
【００１３】
実施の形態１．
図１は、本発明の実施の形態１における干渉チェック制御装置の構成図である。この図１の干渉チェック制御装置は、ロボット速度・位置記憶手段９１０、制動時間計算手段９１１、ロボット間距離推定手段９１６、干渉判定手段９１７、干渉判定用閾値記憶手段９１８、および指令値生成手段９１９を備えている。
【００１４】
各構成要件を説明する前に、本実施の形態１において行われる干渉チェックについて説明する。まず、ロボットの可動範囲によって形成される、ロボットが通過しうる空間を動作領域と表現する。２台以上のロボット同士で動作領域が交差する場合は、ロボット同士の衝突の危険性が出てくる。このようなロボット同士の衝突を表す表現として、ロボットの占めている空間同士が交差している状態に干渉という表現を用いる。そのほか、後述の干渉判定領域同士が交差する状態にも干渉という表現を用いる。また、このような干渉が生じるかどうかを確認することを干渉チェックと呼ぶ。図２は、本発明の実施の形態１において干渉チェックを行なうロボットシステムの構成例を表す図である。この図２で表されるようなロボットシステムにおいて、各ロボットのロボット制御装置のそれぞれに含まれている干渉チェック制御装置３Ａ、３Ｂは、計算周期毎にロボット同士の干渉チェックを行なう。
【００１５】
また、干渉チェック制御装置３Ａ、３Ｂによって、２台それぞれのロボットの位置と速度情報が通信されることで、情報が共有化されている。この情報は、それぞれの干渉チェック制御装置３Ａ、３Ｂによって計算され、例えば、Ｅｔｈｅｒｎｅｔ（登録商標）やＲＳ２３２Ｃによって通信がおこなわれ、共有化される。
【００１６】
ロボット間の干渉チェックを行なう際には、接近状態を表す指標として、ロボット間の距離情報を計算する必要がある。そこで、本実施の形態１では、ロボット構成部分ごとに線分でモデル化して、線分同士の距離計算を行なうことで、ロボット間距離を求めている。
【００１７】
図３は、本発明の実施の形態１におけるロボットの線分モデルの例示図である。それぞれの構成部分が、線分２０の集合として表される。また、その際に、ロボット構成部分モデルに対して付加されている添字ｉとｊは、ロボット構成部分を根元から１、２、・・・と数えたときの通し番号とする。
【００１８】
本実施の形態１において、ロボット動作を制御するロボット制御装置は、通常、計算周期毎に指令値生成手段９１９により生成される指令に基づいて、ロボットを制御する。ここで、ロボット動作を制御するために生成されるロボットの指令値とは、例えば、ある開始点と目標点を与えたときに、ロボットのエンドエフェクタが直線的な軌道を描いて開始点から目標点に対して移動するようにロボットに備えられたモータなど各駆動源の、現在時刻における目標値を決定することを指す。
【００１９】
本発明では、その計算周期で行なわれる指令値計算をするたびに干渉判定を行なう。そのために、図１に示すように、制動時間計算手段９１１は、現在のロボットの位置と姿勢を含む位置情報９０１、および現在のロボットの動作速度を含む速度情報９０２を、ロボット速度・位置記憶手段９１０から読み込み、現時点から制動を開始したときのロボットの制動時間ｔｓｔｐを計算する。
【００２０】
次に、ロボット間距離推定手段９１６は、制動時間ｔｓｔｐ９０３を制動時間計算手段９１１から読み込み、制動開始から制動完了までのロボット同士の各構成部分間の距離を推定計算する。より具体的には、ロボット間距離推定手段９１６は、制動開始から制動完了までに渡り、２台のロボットについて１つずつ選んだロボット構成部分ＬｉｎｋｉとＬｉｎｋｊのロボットモデル間の距離Ｌｉｊを、全ての組合せについて推定計算する。
【００２１】
次に、干渉判定手段９１７は、ロボット間距離推定手段９１６により推定計算されたロボットモデル間距離Ｌｉｊが、制動開始から制動完了までの間、干渉をしないための許容値に収まるか否かを比較する。そして、指令値生成手段９１９は、干渉判定手段９１７による比較判定結果を反映した指令値を生成し、ロボット間の干渉を防止する。
【００２２】
なお、ここで、ロボット構成部分とは、ロボットが複数の可動部からなっている場合における、干渉判定を行なうために定義したロボットを構成する一部分のことである。例えば、ロボットごとに見ると、リスト部、アーム部、ベース部といったものが、このロボット構成部分に相当する。
【００２３】
次に、先の図１に示した本実施の形態１における各構成要素について、順に、詳細に説明する。
ロボット速度・位置記憶手段９１０は、少なくとも１ステップ前の計算ステップまでに、指令値生成手段で生成された、ロボットの位置や速度に関する情報９００を記憶しておく記憶手段である。
【００２４】
制動時間計算手段９１１は、ロボット速度・位置記憶手段９１０から位置情報９０１、速度情報９０２を取得し、ロボットが制動開始した際に完全に停止するまでに必要な時間を計算する。
【００２５】
ここで、位置情報９０１は、ロボットの位置に関する情報であり、例えば、関節角度、エンドエフェクタの位置姿勢およびロボットを構成するロボット構成部分モデル間の距離計算をするのに必要なロボット構成部分の端点位置情報に相当する。また、速度情報９０２は、それぞれロボットの位置に関する情報の前回計算ステップと今回計算ステップとの差分によって求められる速度に関する情報に相当する。また、ロボットの関節角度情報からエンドエフェクタの位置姿勢および定義されたロボット構成部分の端点位置情報は、運動学計算によって計算できるので、単に位置情報９０１を関節角度情報とすることもできる。また、ロボットの関節角速度情報を用いて運動学を計算することでも、ロボットの速度に関する情報を計算することができる。
【００２６】
なお、ここでの計算ステップとは、所定の制御周期ごとに行なわれ、ロボットを動作させるための１制御周期分の指令計算を行なうための一連の計算過程の集合を意味する。これは、例えば、図１に示すような一連の計算群を１回分処理する過程に相当する。
【００２７】
制動時間計算手段９１１は、例えば、ロボットの指令生成の方法として、各関節角度目標値に向かって追従していく関節補間指令中であったときには、減速停止するための各ロボット構成部分の可動部の各加速度に応じて制動時間ｔｓｔｐを決定する。例えば、添字ｍを関節番号（ｍ＝１、２、・・・）とするとき、現在の関節角速度ｖｎｏｗｍで停止のための各関節によって大きさの異なる加速度ａｓｔｐｍが関節毎では一定値である場合には、各関節の制動時間ｔｓｔｐｍは、下式（１）となる。
ｔｓｔｐｍ＝|ｖｎｏｗｍ／ａｓｔｐｍ| （１）
【００２８】
制動時間計算手段９１１は、それぞれの関節ｍについて制動開始から制動完了までに必要な時間ｔｓｔｐｍを計算でき、さらに、そのｔｓｔｐｍの中から最も大きなｔｓｔｐｍを求め、これをｔｓｔｐとおくことができる。この際、制動時間計算手段９１１は、ｔｓｔｐに応じたそれぞれの関節の新たな加速度ａｓｔｐｍを、制動時間ｔｓｔｐが経過したと同時に停止をするように、ｔｓｔｐｍ＝ｔｓｔｐとして、上式（１）を用いて、再計算する。また、複数のロボットが存在する場合には、それぞれのｔｓｔｐで最も大きなｔｓｔｐを全体のｔｓｔｐとして選び、各ロボットの制動時間をｔｓｔｐｍ＝ｔｓｔｐとして、上式（１）を用いて、再計算する。
【００２９】
次に、現在時刻ｔｎｏｗから制動を開始した場合に、制動時間ｔｓｔｐの間にロボット同士が干渉しないことを保証する必要がある。そこで、次に、ロボット同士が制動時間ｔｓｔｐの間に干渉がないことを保証するための干渉チェック方法について述べる。
【００３０】
その準備として、まず、ロボットのモデル化について説明する。
ロボットは、例えば、先の図３に示したように、線分２０でモデル化され、ロボット同士の干渉チェックを行なうために、ロボットを表す線分同士の距離計算が行なわれる。
【００３１】
図４は、本発明の実施の形態１におけるモデル化された２線分間の最近接点を幾何的に表した図である。例えば、ロボットモデル間距離を計測するための２つの線分をモデルＬｉｎｋｉとＬｉｎｋｊとすると、この２線分間の最近接点３１、３２（４１、４２）からモデル間距離Ｌｉｊ３３（４３）が導かれる。最近接点の計算方法は、例えば、特許文献１に開示されているような手法を用いることができる。
【００３２】
そして、ロボットが干渉するか否かの判定は、ロボットを十分に包む円筒と半球とからなるロボット構成部分間の干渉判定領域を使って行われる。図５は、本発明の実施の形態１における干渉判定領域の説明図である。図５に示すように、線分モデルから距離ｒという大きさでロボットを円筒および半球で包み、これをロボット干渉判定領域と定義する。そして、干渉判定領域同士が干渉する場合に、ロボット同士が干渉すると判定する。
【００３３】
例えば、干渉判定領域の大きさが、全てｒであるとき（すなわち、図５において、線分２１からの距離がｒであるとして規定される干渉判定領域の場合）、現在時刻ｔｎｏｗから制動時間ｔｓｔｐが経過するまでの間、モデル間距離Ｌｉｊに対して下式（２）が成り立っている場合には、干渉がないものとする。
２ｒ≦Ｌｉｊ（ｔ） （ｔｎｏｗ≦ｔ≦ｔｎｏｗ＋ｔｓｔｐ） （２）
【００３４】
なお、距離を計算するのに用いるモデルは、線分だけでなく、ポリゴンモデルや詳細なＣＡＤモデルを利用することもできるが、ここでは、簡単のため、線分を取り扱っている。このようなモデル化されたロボット同士が干渉しないように制御を行なえば、実際のロボットも干渉しないことになる。
【００３５】
上述したような、モデル化されたロボット構成部分間の干渉チェック方法を踏まえ、ロボットが現在時刻ｔｎｏｗから制動完了時刻ｔｎｏｗ＋ｔｓｔｐまでに干渉を起こすか否かを確認する必要がある。従来技術では、解決すべき課題で述べたように、ロボットが動作中に制動をかけた場合に、全ロボット構成部分が制動完了するまでの間に干渉するか否かをチェックできていない場合があった。
【００３６】
そこで、本発明では、ロボット間の干渉を見るときに、それぞれのロボットのＬｉｎｋのすべての組み合わせについて、干渉チェックを行っている。図６は、本発明の実施の形態１における２つのロボット間の干渉チェック方法の説明図である。この図６の例では、第１のロボットであるロボットＡと第２のロボットであるロボットＢは、それぞれ４つのＬｉｎｋで構成されている。
【００３７】
そして、ロボットＡとロボットＢとの干渉を見る際には、ロボットＡのＬｉｎｋ１Ａ〜Ｌｉｎｋ４Ａと、ロボットＢのＬｉｎｋ１Ｂ〜Ｌｉｎｋ４Ｂとのすべての組み合わせについて、干渉チェックを行なう。ただし、衝突危険性のないＬｉｎｋ間（例えば、図６におけるＬｉｎｋ１ＡとＬｉｎｋ１Ｂ）は、干渉チェックを省略することができる。
【００３８】
さらに、図６では、干渉判定領域サイズがＬ３ＡＬｉｍの大きさを持つＬｉｎｋ３Ａと、干渉判定領域サイズがＬ３ＢＬｉｍの大きさをもつＬｉｎｋ３Ｂとの最近接距離について比較を行っている例を示している。干渉危険性のあるロボット構成部分同士の距離Ｌｉｊ（先の図４における３３に相当）の変化に注目して、これが干渉しないかどうかを次のように判定する。
【００３９】
各符号を次のように定義する。
Ｌｉｎｋｉ ：ロボットＡのロボット構成部分モデル（ｉ＝１，２、３、４）
Ｌｉｎｋｊ ：ロボットＢのロボット構成部分モデル（ｊ＝１，２、３、４）
Ｌｉｊ ：ＬｉｎｋｉとＬｉｎｋｊの距離
ＬｉＬｉｍ、ＬｊＬｉｍ ：干渉判定領域となるロボット構成部分モデルごとの線分モデ
ルからの距離
この場合、ロボットＡのＬｉｎｋｉと、ロボットＢのＬｉｎｋｊとの間の干渉判定領域の大きさＬｉｊＬｉｍは、下式（３）となる。
ＬｉｊＬｉｍ＝ＬｉＬｉｍ＋ＬｊＬｉｍ （３）
【００４０】
そこで、上式で規定されるＬｉｊＬｉｍを干渉判定用の閾値として用いることで、下式（４）の不等式を満たさない場合に、干渉が起こると判断できる。
ＬｉｊＬｉｍ≦Ｌｉｊ （４）
【００４１】
現在時刻から制動を開始した際に、停止までのロボット構成部分モデル間距離の変化曲線Ｌｉｊ（ｔ）が、ＬｉｊＬｉｍに１度でも達するか否かを推定し、曲線Ｌｉｊ（ｔ）がＬｉｊＬｉｍに達することがなければ、干渉がないと判定可能である。そこで、次に、ロボット構成部分モデル間距離Ｌｉｊの推定方法について説明する。
【００４２】
まず、２つのロボット構成部分モデル間の接近について考える。モデル間距離は、上述したように、ロボット構成部分モデル間の最近接距離Ｌｉｊで定義される。従って、ロボット構成部分モデル間の最近接距離Ｌｉｊの履歴Ｌｉｊ（ｔ）をたどることができれば、正確な干渉チェックが可能である。しかしながら、制動開始から停止までの距離変化を全時間に渡って計算することは、多大な計算負荷がかかることとなる。このため、例えば、実時間で干渉を推定して停止などを考えるときには、全時間に渡って計算することは、計算負荷の観点から現実的ではない。
【００４３】
また、ロボット構成部分間の最近接点は、時々刻々と位置が変化する。このため、有限の最近接距離の履歴Ｌｉｊ（ｔ）から、全時間に渡って最近接点の位置変化を加味してＬｉｊ（ｔ）を正確に推定することは、困難である。
【００４４】
そこで、本発明では、Ｌｉｊの推定には、Ｌｉｊの接近速度値Ｌ（・）ｉｊを考慮して、次のように行っている。ただし、（・）は、（）の前の距離を表す記号の上部に・が付いた速度値を意味している。Ｌｉｊの接近速度値Ｌ（・）ｉｊに関し、最近接点の位置変化分で干渉見落としが発生しないように、２線分の接近に関して接近速度値の上限値Ｌ（・）ｉｊＬｉｍが定義できる。このことから、Ｌ（・）ｉｊＬｉｍの積分値をこれまでのＬｉｊに足しこむ方法で、制動開始から制動完了までのＬｉｊ（ｔ）の接近上限値を推定することができる。
【００４５】
ここで、接近速度についての上限値Ｌ（・）ｉｊＬｉｍの定義について以下に示す。図７は、本発明の実施の形態１における最近接位置における速度よりもリンク端における速度が速度値として大きいことを表す説明図である。図７に示すように、２つのロボット構成部分の線分モデルＬｉｎｋｉとＬｉｎｋｊそれぞれの最近接点付近の速度Ｖｉｃｌｓ、ＶｊｃｌｓとＬｉｊの微分値にあたる接近速度値Ｌ（・）ｉｊの関係は、２つの線分モデルの最近接方向ベクトルをＮｉｊｃｌｓとするとき、下式（５）で表される。
Ｌ（・）ｉｊ＝Ｎｉｊｃｌｓ（Ｖｉｃｌｓ−Ｖｊｃｌｓ） （５）
【００４６】
また、Ｌｉｎｋｉの最近接点付近の速度Ｖｉｃｌｓは、Ｌｉｎｋｉの端点速度Ｖｉｅｄｇ０１とＶｉｅｄｇ０２のどちらかの端点速度（図７中の符号６１、６３、６４参照）よりも必ず遅い。このため、接近方向に対して大きい方の端点速度を、上限値Ｖｉｅｄｇとすることができる。上限値Ｖｉｅｄｇを用いた場合には、Ｌ（・）ｉｊを知る目的でロボット構成部分モデル線分上にある最近接点および最近接距離Ｌｉｊ計算をする必要がないため、計算負荷が少なくてすむ。端点速度から導かれる上限値Ｌ（・）ｉｊｄに基づいて、制動時間ｔｓｔｐ経過するまでのＬｉｊを推定した場合、干渉は起こらない。
【００４７】
以上を踏まえて、制動時間ｔｓｔｐが経過するまでのロボットモデル間距離Ｌｉｊを推定する際には、次のように行なう。制動開始から制動完了までのＬ（・）ｉｊをプロットすることは、計算負荷となる。そこで、ここでは、推定のための有限なｎ個の２つの線分モデルのリンク端速度情報（６３、６４）を参照情報として得て、それを元にＬｉｊの推定を行なう。
【００４８】
図８は、本発明の実施の形態１におけるＬｉｊを推定するロボット間距離推定手段９１６の内部構成図である。図８におけるロボット間距離推定手段９１６は、サンプリング時間決定手段９１２、ロボット位置計算手段９１３、端点速度計算手段９１４、およびロボット間距離推定値計算手段９１５で構成される。
【００４９】
サンプリング時間決定手段９１２は、この参照情報を得る時刻を決定する。サンプリング時間決定手段９１２は、ロボットが軌道通りに進んで停止するとして、制動を開始してから停止するまでの制動時間ｔｓｔｐ（９０３）をｎ分割（ｎは自然数）することで、サンプリングする時間ｔｓｍｐｋ（９０４）を、例えば、下式（６）として決定する。
ｔｓｍｐｋ＝ｔｎｏｗ＋ｔｓｔｐ＊ｋ／ｎ（ｋ＝１、２、・・・ｎ−１） （６）
【００５０】
ここで、添字ｋについては、サンプリング時間の通し番号とする。次に、サンプリングする時間ｔｓｍｐｋの各時刻において、ロボット位置計算手段９１３は、ロボット構成部分の端点位置（９０５）を計算する。また、ロボット位置計算手段９１３は、１計算周期にかかる時間をｄｔとした場合、ｄｔ後の時刻（ｔｓｍｐｋ＋ｄｔ）におけるロボット構成部分の端点位置（９０５）も、同様に計算しておく。
【００５１】
リンク端速度情報（図７における６３、６４参照）を計算する方法として、ここでは、例えば、ロボット構成部分モデルにおける２つの端点（以下の説明では、ｅｄｇ０１とｅｄｇ０２で区別する）の時刻ｔｓｍｐｋにおける位置ｐｅｄｇ０１（ｔｓｍｐｋ）とｐｅｄｇ０２（ｔｓｍｐｋ）、時刻ｔｓｍｐｋ＋ｄｔにおけるｐｅｄｇ０２（ｔｓｍｐｋ＋ｄｔ）とｐｅｄｇ０２（ｔｓｍｐｋ＋ｄｔ）の差分から計算する方法をとる。
【００５２】
これによって、端点速度計算手段９１４は、サンプリングする時刻ｔｓｍｐｋ（ｋ＝１、２、・・・、ｎ−１）における各ロボット構成部分の端点速度Ｖｉｅｄｇ０１（ｔｓｍｐｋ）、Ｖｉｅｄｇ０２（ｔｓｍｐｋ）、Ｖｊｅｄｇ０１（ｔｓｍｐｋ）、Ｖｊｅｄｇ０２（ｔｓｍｐｋ）（９０６）を得ることができる。
【００５３】
この後、Ｌｉｊの推定値を計算するロボット間距離推定値計算手段９１５は、上述したように、Ｌｉｎｋｉの最近接点付近の速度ＶｉｃｌｓがＬｉｎｋｉの端点速度Ｖｉｅｄｇ０１とＶｉｅｄｇ０２のどちらかの端点速度（６３，６４）よりも必ず遅いため、接近方向に対して大きい方の端点速度を上限値Ｖｉｅｄｇとすることができる。ここでは、この上限値Ｖｉｅｄｇを構成部分の速度上限値としている。
【００５４】
２つの端点速度について、それぞれｔｓｍｐｋにおけるＶｉｅｄｇ （ｔｓｍｐｋ）、Ｖｊｅｄｇ（ｔｓｍｐｋ）が分かっているので、上限値Ｌ（・）ｉｊｄ（５０２）は、最近接方向ベクトルをＮｉｊｃｌｓとして、下式（７）という形で求められる。
Ｌ（・）ｉｊｄ＝Ｎｉｊｃｌｓ・（Ｖｉｅｄｇ（ｔｓｍｐｋ）−Ｖｊｅｄｇ（ｔｓｍｐｋ）） （７）
【００５５】
ここで得られる端点ごとの２つのＬ（・）ｉｊｄのうち、接近方向に対してより大きい方を上限値Ｌ（・）ｉｊｄと定める。また、ここでは、簡単の為に、ｎ＝３としてサンプリングし、各サンプリング間を台形近似してΔＬｉｊｄを計算する。図９は、本発明の実施の形態１におけるロボット構成部分の端点速度を用いて最近接点間距離を計算する場合の、速度値から距離を計算するための台形近似を表す説明図である。
【００５６】
この図９に示すような台形近似により、下式（８）を用いることで、サンプリング時刻ごとにおける推定値Ｌｉｊｄを得る。
Ｌｉｊｄ＝Ｌｉｊ（ｔｎｏｗ）＋ΔＬｉｊｄ （８）
【００５７】
干渉判定手段９１７は、干渉判定を行なうための干渉判定領域の閾値をあらかじめ記憶している干渉判定用閾値記憶手段９１８から、チェック対象としているロボット構成部分モデルの干渉判定領域の大きさから定義される干渉判定の閾値ＬｉｊＬｉｍ９０８を読み出す。そして、干渉判定手段９１７は、上述した推定方法を利用してロボット間距離推定手段９１６で求められるロボット間距離の推定値Ｌｉｊｄ９０７と、読み出した干渉判定の閾値ＬｉｊＬｉｍ９０８との比較を、下式（９）により行ない、サンプリング時刻ごとに干渉判定する。
ＬｉｊＬｉｍ≦Ｌｉｊｄ （９）
【００５８】
上式（９）の条件を満たす場合には、干渉判定手段９１７は、干渉は起こらないと判定して指令値生成手段９１９にその旨を伝達する（９０９）。この場合は、移動指令値としてロボット制御装置内で追従する目標値に向かう移動指令が生成される。一方、上式（９）の条件を満たさない場合には、干渉判定手段９１７は、干渉の危険性があると判定して、移動指令値として停止を開始するための指令変更の伝達（９０９）を指令値生成手段９１９に対して行なう。
【００５９】
図１０は、本発明の実施の形態１において、推定結果に基づく干渉判定手段９１７により干渉の危険性があると判定された場合に減速開始を行なう説明図である。この図１０に示すように、判定を行なう現在時刻ｔｎｏｗ２０１における推定結果に基づいて、干渉すると判定されるリミット（６０１）に達した際に干渉の危険性があると判定いた場合には、干渉判定手段９１７は、減速停止を開始するための指令変更の伝達（９０９）を指令値生成手段９１９に対して行なう。
【００６０】
以上のように、実施の形態１によれば、制動開始から制動完了までの間の構成部分同士の距離の変化について、速度上限値に基づいて最近接距離の変化の推定を行っている。そして、ロボット構成部分ごとの距離の変化の推定結果に基づいて、制動開始から制動完了までの間にロボット同士の干渉がないかどうかを判定している。この結果、制動動作中に構成部分同士が干渉するケースを見落とすことがなくなり、近接配置され動作領域が重複するロボット間の干渉問題において、従来方法と比較して、漏れなく干渉状態を発見する効果がある。
【００６１】
実施の形態２．
先の実施の形態１におけるロボット間距離推定手段９１６は、図８に示したように、端点速度計算手段９１４により求めた各サンプリング時刻における端点速度に基づいて、ロボット間距離の推定を行っていた。これに対して、本実施の形態２は、ロボット間距離の推定を行なうための十分な計算能力を備えている場合に、より高精度な推定を行なう場合について説明する。
【００６２】
図１１は、本発明の実施の形態２におけるＬｉｊを推定するロボット間距離推定手段９１６ａの内部構成図である。図１１におけるロボット間距離推定手段９１６ａは、サンプリング時間決定手段９１２、ロボット位置計算手段９１３、最近接点・最近速度計算手段９１４ａ、およびロボット間距離推定値計算手段９１５で構成される。
【００６３】
図１１における最近接点・最近速度計算手段９１４ａは、先の実施の形態１に記載のｎ個のサンプリングそれぞれに対して最近接距離Ｌｉｊと最近接点における接近速度Ｖｉｃｌｓ、Ｖｊｃｌｓの計算（９１４）を行ない、上式（５）を計算することでＬ（・）ｉｊを求める。
【００６４】
図１２は、本発明の実施の形態２における制動開始から停止までの接近速度変化について、最近接距離をもとにしたものとロボット構成部分の端点速度での比較の一例を表す図である。白丸で示したプロットが、先の実施の形態１におけるロボット間距離推定手段９１６による推定結果を示しており、黒丸で示したプロットが、本実施の形態２におけるロボット間距離推定手段９１６ａによる推定結果を示している。
【００６５】
図１２に示すように、実際のロボット構成部分モデル間の最近接距離Ｌｉｊの時間変化で求められる接近速度Ｌ（・）ｉｊ（５０１）を上限値Ｌ（・）ｉｊｄ（５０２）の代わりに利用することで、実際の停止までの速度変化（５１１）をより精度よく予測することができる。
【００６６】
以上のように、実施の形態２によれば、最近接距離と最近接点における接近速度の計算を行なうことにより、制動開始から実際の停止までの速度変化を、より高精度に予測することができる。
【００６７】
実施の形態３．
本実施の形態３は、干渉が発生すると判定され、停止動作を開始した後も、干渉判定を行ない続け、評価式（上式（４）に相当）で干渉がないと判断されれば、動作を再び継続する場合について説明する。
【００６８】
図１３は、本発明の実施の形態３において、制動開始後に、引き続き干渉チェックを行った際に推定値Ｌｉｊｄが干渉判定領域の閾値ＬｉｊＬｉｍよりも大きくなった場合の継続動作を示す説明図である。あるロボット構成部分同士の距離Ｌｉｊについて見るときに、停止動作によって見積もった距離Ｌｉｊは、大きく見積もってしまう場合がある（図１３の６１４参照）。
【００６９】
これに対して、本実施の形態３は、現在時刻（２０１）の干渉判定において干渉の危険性があると判定された場合には減速を行なう。一方で、減速開始後の時刻ｔ' ｎｏｗ（２０２）において干渉の危険性がないと判定された場合には、減速を中止し、動作を継続させる（図１３の６１５参照）。また、それまでに減速を行なっていなければ、軌道通りに動作を継続させるという処理を行なう。
【００７０】
以上のように、実施の形態３によれば、停止終了した位置について、特に、特許文献３に示されるようなリアルタイムで動的に計算された停止指令の生成が必要な場合において、停止動作中にも干渉するか否かの判定を行ない、必要に応じて減速をやめることで、ロボットは動作を続けることができる。高速動作時においては、推定誤差も大きくなるため、このように、停止動作中にも干渉するか否かの判定を行なうことで、制動開始時の推定が大きすぎて停止を開始してしまった場合にも、その後の状況に応じて継続動作が可能となる。
【００７１】
これにより、従来方法で見落とされていた干渉を見落とさずに判定でき、ロボット同士の干渉が生じないとともに、最適な停止位置まで干渉することなく接近を続けることができる効果がある。
【００７２】
なお、本発明は、ロボット同士の場合の干渉チェックに限定されるものではない。一方がロボットであり、他方がモデル化された周辺装置や冶具などを含めた外部環境の場合にも適用可能である。このような外部環境について、あらかじめ形状と位置のデータを登録しておき、ロボットとの相対的な位置関係や動作の相対的な関係の情報が、通信によって共有化できるときに、同様の干渉チェックを適用することができる。
【図面の簡単な説明】
【００７３】
【図１】本発明の実施の形態１における干渉チェック制御装置の構成図である。
【図２】本発明の実施の形態１において干渉チェックを行なうロボットシステムの構成例を表す図である。
【図３】本発明の実施の形態１におけるロボットの線分モデルの例示図である。
【図４】本発明の実施の形態１におけるモデル化された２線分間の最近接点を幾何的に表した図である。
【図５】本発明の実施の形態１における干渉判定領域の説明図である。
【図６】本発明の実施の形態１における２つのロボット間の干渉チェック方法の説明図である。
【図７】本発明の実施の形態１における最近接位置における速度よりもリンク端における速度が速度値として大きいことを表す説明図である。
【図８】本発明の実施の形態１におけるＬｉｊを推定するロボット間距離推定手段の内部構成図である。
【図９】本発明の実施の形態１におけるロボット構成部分の端点速度を用いて最近接点間距離を計算する場合の、速度値から距離を計算するための台形近似を表す説明図である。
【図１０】本発明の実施の形態１において、推定結果に基づく干渉判定手段９１７により干渉の危険性があると判定された場合に減速開始を行なう説明図である。
【図１１】本発明の実施の形態２におけるＬｉｊを推定するロボット間距離推定手段９１６ａの内部構成図である。
【図１２】本発明の実施の形態２における制動開始から停止までの接近速度変化について、最近接距離をもとにしたものとロボット構成部分の端点速度での比較の一例を表す図である。
【図１３】本発明の実施の形態３において、制動開始後に引き続き干渉チェックを行った際に推定値Ｌｉｊｄが干渉判定領域の閾値ＬｉｊＬｉｍよりも大きくなった場合の継続動作を示す説明図である。
【符号の説明】
【００７４】
９１０ ロボット速度・位置記憶手段、９１１ 制動時間計算手段、９１２ サンプリング時間決定手段、９１３ ロボット位置計算手段、９１４ 端点速度計算手段、９１４ａ 最近接点・最近速度計算手段、９１５ ロボット間距離推定値計算手段、９１６、９１６ａ ロボット間距離推定手段、９１７ 干渉判定手段、９１８ 干渉判定用閾値記憶手段、９１９ 指令値生成手段。
【技術分野】
【０００１】
本発明は、近接配置され動作領域が重複している複数のロボットの干渉チェック制御装置および干渉チェック制御方法に関する。
【背景技術】
【０００２】
少なくとも２台以上のロボットの動作領域が重なる状態で同時に作業するロボットシステムにおいて、ロボット同士の干渉チェックを行なう技術が開示されている。一般に、ロボットをいくつかの構成部分に分け、ロボット構成部分を覆うように例えば円筒と半球を用いた近似立体でロボット構成部分をモデル化して、これら近似立体間の距離を計算することから衝突を検知する方法（特許文献１参照）が行われており、例えばロボットを単純な線分でモデル化してその線分間の最近接距離を計算し干渉状態をチェックする方法が行われている。
【０００３】
前記のような干渉チェック方法を用いて各ロボット同士が干渉しないよう停止する技術として、例えば、エンドエフェクタなどの取り付けてある手先部分であるロボットのツールの周りに該ロボットのツールの大きさよりも大きい２重の干渉判定領域を設けてその干渉状態と各ロボット同士の相対的な移動方向の関係にあわせて、片方を停止させるか両方を停止させるか決定する方法（特許文献２参照）がある。また、干渉回避のために停止動作を開始した際の停止完了時の姿勢を計算し、その姿勢に対して各ロボット同士の干渉の有無を判定して、停止完了時の姿勢に対して各ロボット同士の干渉があれば停止必要と判断し停止を開始する方法（特許文献３参照）という先行技術がある。
【０００４】
【特許文献１】特開６０-２１７４１０号公報
【特許文献２】特許第３３５１３３０号公報
【特許文献３】特許第３９０７６４９号公報
【発明の開示】
【発明が解決しようとする課題】
【０００５】
しかしながら、従来技術には、以下のような課題がある。
ロボットの手先軌道が教示軌道に沿って、例えば直線や円弧を描く際には、各ロボット構成部分は、単調動作ではなく複雑に動く場合がある。あるロボット構成部分と他ロボット構成部分が相対的に近づいたり離れたりする動きに該当する。ロボット同士が近接配置され動作領域が重複する際には、該ロボット構成部分の複雑な動作によって各ロボット構成部分同士が干渉する危険性があるというのは大きな問題であり、従来方法ではそのようなロボット構成部分間の干渉チェックができていなかった。
【０００６】
従来方法は、例えば、特許文献２ではロボットのツールに限定されており構成部分同士の距離判定を行なっていないためツール部分同士は干渉していなくてもその停止動作中に別の構成部分同士が干渉する可能性があった。特許文献３では、ロボット制御指令を作る制御計算周期ごとに干渉チェックをする時に、ある時刻で計算されたロボット停止位置と、１制御周期分だけ次の時刻に計算された停止位置の停止位置間隔が、特に高速動作時に大きくなってしまうため、干渉チェックを行なう隣り合った停止位置間においてはロボット構成部分同士が干渉している可能性を判定できない。
【０００７】
本発明は、前記のような課題を解決するためになされたものであり、制動動作中におけるロボットの構成部分同士の干渉の有無を高精度に判定することのできる干渉チェック制御装置および干渉チェック制御方法を得ることを目的とする。
【課題を解決するための手段】
【０００８】
本発明に係る干渉チェック制御装置は、それぞれの動作領域に重複する部分を有するロボット同士の干渉チェック制御装置であって、計算周期ごとに算出されたロボットの速度・位置データを記憶する速度・位置記憶手段と、各ロボット間の干渉判定を行なうための干渉判定領域の閾値をあらかじめ記憶する干渉判定用閾値記憶手段と、速度・位置記憶手段に記憶された速度・位置データに基づいて、ロボットが現時点から制動を開始した際に完全に停止するまでに必要な時間を計算する制動時間計算手段と、制動開始から制動完了までの各ロボット同士の各構成部分間の最近接距離の履歴を、接近する各構成部分の速度に基づいて推定するロボット間距離推定手段と、推定された各構成部分間の最近接距離の履歴と、干渉判定用閾値記憶手段にあらかじめ記憶された干渉判定領域の閾値とを比較し、制動開始から制動完了までの間の干渉の有無を判定する干渉判定手段と、干渉判定手段による干渉判定結果に応じて、計算周期ごとに各ロボットに対する移動指令値を作り出すとともに、現在の計算周期におけるロボットの速度・位置データを速度・位置記憶手段に記憶させる指令値生成手段とを備えるものである。
【０００９】
本発明に係る干渉チェック制御方法は、それぞれの動作領域に重複する部分を有するロボット同士の干渉チェック制御方法であって、第１の記憶部に記憶された速度・位置データに基づいて、ロボットが現時点から制動を開始した際に完全に停止するまでに必要な時間を計算する制動時間計算ステップと、制動開始から制動完了までの各ロボット同士の各構成部分間の最近接距離の履歴を、接近する各構成部分の速度に基づいて推定するロボット間距離推定ステップと、推定された各構成部分間の最近接距離の履歴と、第２の記憶部にあらかじめ記憶された干渉判定領域の閾値とを比較し、制動開始から制動完了までの間の干渉の有無を判定する干渉判定ステップと、干渉判定ステップによる干渉判定結果に応じて、計算周期ごとに各ロボットに対する移動指令値を作り出すとともに、現在の計算周期におけるロボットの速度・位置データを第１の記憶部に記憶させる指令値生成ステップとを備えるものである。
【発明の効果】
【００１０】
本発明に係る干渉チェック制御装置および干渉チェック制御方法によれば、制動開始から制動完了までの間の構成部分同士の最近接距離の変化を、接近する各構成部分の速度に基づいて推定することにより、制動動作中におけるロボットの構成部分同士の干渉の有無を高精度に判定することのできる干渉チェック制御装置および干渉チェック制御方法を得ることができる。
【発明を実施するための最良の形態】
【００１１】
以下、本発明の干渉チェック制御装置および干渉チェック制御方法の好適な実施の形態につき図面を用いて説明する。
本発明は、制動開始から制動完了までにロボット構成部分同士の最近接距離の履歴を、接近する各構成部分の速度に基づいて推定し、制動開始から制動完了までにロボット同士の干渉があるか否かを、ロボット構成部分ごとの最近接距離の変化の推定結果に基づいて判定している。この結果、制動動作中に構成部分同士が干渉するケースを見落とすことがなくなり、近接配置され動作領域が重複するロボット間の干渉問題において、従来方法よりももれなく干渉状態を発見し、ロボットの破損などを防ぐ効果がある。
【００１２】
また、以下の説明中での制動とは、停止をするために各可動部分の駆動源に減速指令をかけて、現在速度と出力可能な減速のための加速度によって規定される時間後に停止する動作のことである。
【００１３】
実施の形態１．
図１は、本発明の実施の形態１における干渉チェック制御装置の構成図である。この図１の干渉チェック制御装置は、ロボット速度・位置記憶手段９１０、制動時間計算手段９１１、ロボット間距離推定手段９１６、干渉判定手段９１７、干渉判定用閾値記憶手段９１８、および指令値生成手段９１９を備えている。
【００１４】
各構成要件を説明する前に、本実施の形態１において行われる干渉チェックについて説明する。まず、ロボットの可動範囲によって形成される、ロボットが通過しうる空間を動作領域と表現する。２台以上のロボット同士で動作領域が交差する場合は、ロボット同士の衝突の危険性が出てくる。このようなロボット同士の衝突を表す表現として、ロボットの占めている空間同士が交差している状態に干渉という表現を用いる。そのほか、後述の干渉判定領域同士が交差する状態にも干渉という表現を用いる。また、このような干渉が生じるかどうかを確認することを干渉チェックと呼ぶ。図２は、本発明の実施の形態１において干渉チェックを行なうロボットシステムの構成例を表す図である。この図２で表されるようなロボットシステムにおいて、各ロボットのロボット制御装置のそれぞれに含まれている干渉チェック制御装置３Ａ、３Ｂは、計算周期毎にロボット同士の干渉チェックを行なう。
【００１５】
また、干渉チェック制御装置３Ａ、３Ｂによって、２台それぞれのロボットの位置と速度情報が通信されることで、情報が共有化されている。この情報は、それぞれの干渉チェック制御装置３Ａ、３Ｂによって計算され、例えば、Ｅｔｈｅｒｎｅｔ（登録商標）やＲＳ２３２Ｃによって通信がおこなわれ、共有化される。
【００１６】
ロボット間の干渉チェックを行なう際には、接近状態を表す指標として、ロボット間の距離情報を計算する必要がある。そこで、本実施の形態１では、ロボット構成部分ごとに線分でモデル化して、線分同士の距離計算を行なうことで、ロボット間距離を求めている。
【００１７】
図３は、本発明の実施の形態１におけるロボットの線分モデルの例示図である。それぞれの構成部分が、線分２０の集合として表される。また、その際に、ロボット構成部分モデルに対して付加されている添字ｉとｊは、ロボット構成部分を根元から１、２、・・・と数えたときの通し番号とする。
【００１８】
本実施の形態１において、ロボット動作を制御するロボット制御装置は、通常、計算周期毎に指令値生成手段９１９により生成される指令に基づいて、ロボットを制御する。ここで、ロボット動作を制御するために生成されるロボットの指令値とは、例えば、ある開始点と目標点を与えたときに、ロボットのエンドエフェクタが直線的な軌道を描いて開始点から目標点に対して移動するようにロボットに備えられたモータなど各駆動源の、現在時刻における目標値を決定することを指す。
【００１９】
本発明では、その計算周期で行なわれる指令値計算をするたびに干渉判定を行なう。そのために、図１に示すように、制動時間計算手段９１１は、現在のロボットの位置と姿勢を含む位置情報９０１、および現在のロボットの動作速度を含む速度情報９０２を、ロボット速度・位置記憶手段９１０から読み込み、現時点から制動を開始したときのロボットの制動時間ｔｓｔｐを計算する。
【００２０】
次に、ロボット間距離推定手段９１６は、制動時間ｔｓｔｐ９０３を制動時間計算手段９１１から読み込み、制動開始から制動完了までのロボット同士の各構成部分間の距離を推定計算する。より具体的には、ロボット間距離推定手段９１６は、制動開始から制動完了までに渡り、２台のロボットについて１つずつ選んだロボット構成部分ＬｉｎｋｉとＬｉｎｋｊのロボットモデル間の距離Ｌｉｊを、全ての組合せについて推定計算する。
【００２１】
次に、干渉判定手段９１７は、ロボット間距離推定手段９１６により推定計算されたロボットモデル間距離Ｌｉｊが、制動開始から制動完了までの間、干渉をしないための許容値に収まるか否かを比較する。そして、指令値生成手段９１９は、干渉判定手段９１７による比較判定結果を反映した指令値を生成し、ロボット間の干渉を防止する。
【００２２】
なお、ここで、ロボット構成部分とは、ロボットが複数の可動部からなっている場合における、干渉判定を行なうために定義したロボットを構成する一部分のことである。例えば、ロボットごとに見ると、リスト部、アーム部、ベース部といったものが、このロボット構成部分に相当する。
【００２３】
次に、先の図１に示した本実施の形態１における各構成要素について、順に、詳細に説明する。
ロボット速度・位置記憶手段９１０は、少なくとも１ステップ前の計算ステップまでに、指令値生成手段で生成された、ロボットの位置や速度に関する情報９００を記憶しておく記憶手段である。
【００２４】
制動時間計算手段９１１は、ロボット速度・位置記憶手段９１０から位置情報９０１、速度情報９０２を取得し、ロボットが制動開始した際に完全に停止するまでに必要な時間を計算する。
【００２５】
ここで、位置情報９０１は、ロボットの位置に関する情報であり、例えば、関節角度、エンドエフェクタの位置姿勢およびロボットを構成するロボット構成部分モデル間の距離計算をするのに必要なロボット構成部分の端点位置情報に相当する。また、速度情報９０２は、それぞれロボットの位置に関する情報の前回計算ステップと今回計算ステップとの差分によって求められる速度に関する情報に相当する。また、ロボットの関節角度情報からエンドエフェクタの位置姿勢および定義されたロボット構成部分の端点位置情報は、運動学計算によって計算できるので、単に位置情報９０１を関節角度情報とすることもできる。また、ロボットの関節角速度情報を用いて運動学を計算することでも、ロボットの速度に関する情報を計算することができる。
【００２６】
なお、ここでの計算ステップとは、所定の制御周期ごとに行なわれ、ロボットを動作させるための１制御周期分の指令計算を行なうための一連の計算過程の集合を意味する。これは、例えば、図１に示すような一連の計算群を１回分処理する過程に相当する。
【００２７】
制動時間計算手段９１１は、例えば、ロボットの指令生成の方法として、各関節角度目標値に向かって追従していく関節補間指令中であったときには、減速停止するための各ロボット構成部分の可動部の各加速度に応じて制動時間ｔｓｔｐを決定する。例えば、添字ｍを関節番号（ｍ＝１、２、・・・）とするとき、現在の関節角速度ｖｎｏｗｍで停止のための各関節によって大きさの異なる加速度ａｓｔｐｍが関節毎では一定値である場合には、各関節の制動時間ｔｓｔｐｍは、下式（１）となる。
ｔｓｔｐｍ＝|ｖｎｏｗｍ／ａｓｔｐｍ| （１）
【００２８】
制動時間計算手段９１１は、それぞれの関節ｍについて制動開始から制動完了までに必要な時間ｔｓｔｐｍを計算でき、さらに、そのｔｓｔｐｍの中から最も大きなｔｓｔｐｍを求め、これをｔｓｔｐとおくことができる。この際、制動時間計算手段９１１は、ｔｓｔｐに応じたそれぞれの関節の新たな加速度ａｓｔｐｍを、制動時間ｔｓｔｐが経過したと同時に停止をするように、ｔｓｔｐｍ＝ｔｓｔｐとして、上式（１）を用いて、再計算する。また、複数のロボットが存在する場合には、それぞれのｔｓｔｐで最も大きなｔｓｔｐを全体のｔｓｔｐとして選び、各ロボットの制動時間をｔｓｔｐｍ＝ｔｓｔｐとして、上式（１）を用いて、再計算する。
【００２９】
次に、現在時刻ｔｎｏｗから制動を開始した場合に、制動時間ｔｓｔｐの間にロボット同士が干渉しないことを保証する必要がある。そこで、次に、ロボット同士が制動時間ｔｓｔｐの間に干渉がないことを保証するための干渉チェック方法について述べる。
【００３０】
その準備として、まず、ロボットのモデル化について説明する。
ロボットは、例えば、先の図３に示したように、線分２０でモデル化され、ロボット同士の干渉チェックを行なうために、ロボットを表す線分同士の距離計算が行なわれる。
【００３１】
図４は、本発明の実施の形態１におけるモデル化された２線分間の最近接点を幾何的に表した図である。例えば、ロボットモデル間距離を計測するための２つの線分をモデルＬｉｎｋｉとＬｉｎｋｊとすると、この２線分間の最近接点３１、３２（４１、４２）からモデル間距離Ｌｉｊ３３（４３）が導かれる。最近接点の計算方法は、例えば、特許文献１に開示されているような手法を用いることができる。
【００３２】
そして、ロボットが干渉するか否かの判定は、ロボットを十分に包む円筒と半球とからなるロボット構成部分間の干渉判定領域を使って行われる。図５は、本発明の実施の形態１における干渉判定領域の説明図である。図５に示すように、線分モデルから距離ｒという大きさでロボットを円筒および半球で包み、これをロボット干渉判定領域と定義する。そして、干渉判定領域同士が干渉する場合に、ロボット同士が干渉すると判定する。
【００３３】
例えば、干渉判定領域の大きさが、全てｒであるとき（すなわち、図５において、線分２１からの距離がｒであるとして規定される干渉判定領域の場合）、現在時刻ｔｎｏｗから制動時間ｔｓｔｐが経過するまでの間、モデル間距離Ｌｉｊに対して下式（２）が成り立っている場合には、干渉がないものとする。
２ｒ≦Ｌｉｊ（ｔ） （ｔｎｏｗ≦ｔ≦ｔｎｏｗ＋ｔｓｔｐ） （２）
【００３４】
なお、距離を計算するのに用いるモデルは、線分だけでなく、ポリゴンモデルや詳細なＣＡＤモデルを利用することもできるが、ここでは、簡単のため、線分を取り扱っている。このようなモデル化されたロボット同士が干渉しないように制御を行なえば、実際のロボットも干渉しないことになる。
【００３５】
上述したような、モデル化されたロボット構成部分間の干渉チェック方法を踏まえ、ロボットが現在時刻ｔｎｏｗから制動完了時刻ｔｎｏｗ＋ｔｓｔｐまでに干渉を起こすか否かを確認する必要がある。従来技術では、解決すべき課題で述べたように、ロボットが動作中に制動をかけた場合に、全ロボット構成部分が制動完了するまでの間に干渉するか否かをチェックできていない場合があった。
【００３６】
そこで、本発明では、ロボット間の干渉を見るときに、それぞれのロボットのＬｉｎｋのすべての組み合わせについて、干渉チェックを行っている。図６は、本発明の実施の形態１における２つのロボット間の干渉チェック方法の説明図である。この図６の例では、第１のロボットであるロボットＡと第２のロボットであるロボットＢは、それぞれ４つのＬｉｎｋで構成されている。
【００３７】
そして、ロボットＡとロボットＢとの干渉を見る際には、ロボットＡのＬｉｎｋ１Ａ〜Ｌｉｎｋ４Ａと、ロボットＢのＬｉｎｋ１Ｂ〜Ｌｉｎｋ４Ｂとのすべての組み合わせについて、干渉チェックを行なう。ただし、衝突危険性のないＬｉｎｋ間（例えば、図６におけるＬｉｎｋ１ＡとＬｉｎｋ１Ｂ）は、干渉チェックを省略することができる。
【００３８】
さらに、図６では、干渉判定領域サイズがＬ３ＡＬｉｍの大きさを持つＬｉｎｋ３Ａと、干渉判定領域サイズがＬ３ＢＬｉｍの大きさをもつＬｉｎｋ３Ｂとの最近接距離について比較を行っている例を示している。干渉危険性のあるロボット構成部分同士の距離Ｌｉｊ（先の図４における３３に相当）の変化に注目して、これが干渉しないかどうかを次のように判定する。
【００３９】
各符号を次のように定義する。
Ｌｉｎｋｉ ：ロボットＡのロボット構成部分モデル（ｉ＝１，２、３、４）
Ｌｉｎｋｊ ：ロボットＢのロボット構成部分モデル（ｊ＝１，２、３、４）
Ｌｉｊ ：ＬｉｎｋｉとＬｉｎｋｊの距離
ＬｉＬｉｍ、ＬｊＬｉｍ ：干渉判定領域となるロボット構成部分モデルごとの線分モデ
ルからの距離
この場合、ロボットＡのＬｉｎｋｉと、ロボットＢのＬｉｎｋｊとの間の干渉判定領域の大きさＬｉｊＬｉｍは、下式（３）となる。
ＬｉｊＬｉｍ＝ＬｉＬｉｍ＋ＬｊＬｉｍ （３）
【００４０】
そこで、上式で規定されるＬｉｊＬｉｍを干渉判定用の閾値として用いることで、下式（４）の不等式を満たさない場合に、干渉が起こると判断できる。
ＬｉｊＬｉｍ≦Ｌｉｊ （４）
【００４１】
現在時刻から制動を開始した際に、停止までのロボット構成部分モデル間距離の変化曲線Ｌｉｊ（ｔ）が、ＬｉｊＬｉｍに１度でも達するか否かを推定し、曲線Ｌｉｊ（ｔ）がＬｉｊＬｉｍに達することがなければ、干渉がないと判定可能である。そこで、次に、ロボット構成部分モデル間距離Ｌｉｊの推定方法について説明する。
【００４２】
まず、２つのロボット構成部分モデル間の接近について考える。モデル間距離は、上述したように、ロボット構成部分モデル間の最近接距離Ｌｉｊで定義される。従って、ロボット構成部分モデル間の最近接距離Ｌｉｊの履歴Ｌｉｊ（ｔ）をたどることができれば、正確な干渉チェックが可能である。しかしながら、制動開始から停止までの距離変化を全時間に渡って計算することは、多大な計算負荷がかかることとなる。このため、例えば、実時間で干渉を推定して停止などを考えるときには、全時間に渡って計算することは、計算負荷の観点から現実的ではない。
【００４３】
また、ロボット構成部分間の最近接点は、時々刻々と位置が変化する。このため、有限の最近接距離の履歴Ｌｉｊ（ｔ）から、全時間に渡って最近接点の位置変化を加味してＬｉｊ（ｔ）を正確に推定することは、困難である。
【００４４】
そこで、本発明では、Ｌｉｊの推定には、Ｌｉｊの接近速度値Ｌ（・）ｉｊを考慮して、次のように行っている。ただし、（・）は、（）の前の距離を表す記号の上部に・が付いた速度値を意味している。Ｌｉｊの接近速度値Ｌ（・）ｉｊに関し、最近接点の位置変化分で干渉見落としが発生しないように、２線分の接近に関して接近速度値の上限値Ｌ（・）ｉｊＬｉｍが定義できる。このことから、Ｌ（・）ｉｊＬｉｍの積分値をこれまでのＬｉｊに足しこむ方法で、制動開始から制動完了までのＬｉｊ（ｔ）の接近上限値を推定することができる。
【００４５】
ここで、接近速度についての上限値Ｌ（・）ｉｊＬｉｍの定義について以下に示す。図７は、本発明の実施の形態１における最近接位置における速度よりもリンク端における速度が速度値として大きいことを表す説明図である。図７に示すように、２つのロボット構成部分の線分モデルＬｉｎｋｉとＬｉｎｋｊそれぞれの最近接点付近の速度Ｖｉｃｌｓ、ＶｊｃｌｓとＬｉｊの微分値にあたる接近速度値Ｌ（・）ｉｊの関係は、２つの線分モデルの最近接方向ベクトルをＮｉｊｃｌｓとするとき、下式（５）で表される。
Ｌ（・）ｉｊ＝Ｎｉｊｃｌｓ（Ｖｉｃｌｓ−Ｖｊｃｌｓ） （５）
【００４６】
また、Ｌｉｎｋｉの最近接点付近の速度Ｖｉｃｌｓは、Ｌｉｎｋｉの端点速度Ｖｉｅｄｇ０１とＶｉｅｄｇ０２のどちらかの端点速度（図７中の符号６１、６３、６４参照）よりも必ず遅い。このため、接近方向に対して大きい方の端点速度を、上限値Ｖｉｅｄｇとすることができる。上限値Ｖｉｅｄｇを用いた場合には、Ｌ（・）ｉｊを知る目的でロボット構成部分モデル線分上にある最近接点および最近接距離Ｌｉｊ計算をする必要がないため、計算負荷が少なくてすむ。端点速度から導かれる上限値Ｌ（・）ｉｊｄに基づいて、制動時間ｔｓｔｐ経過するまでのＬｉｊを推定した場合、干渉は起こらない。
【００４７】
以上を踏まえて、制動時間ｔｓｔｐが経過するまでのロボットモデル間距離Ｌｉｊを推定する際には、次のように行なう。制動開始から制動完了までのＬ（・）ｉｊをプロットすることは、計算負荷となる。そこで、ここでは、推定のための有限なｎ個の２つの線分モデルのリンク端速度情報（６３、６４）を参照情報として得て、それを元にＬｉｊの推定を行なう。
【００４８】
図８は、本発明の実施の形態１におけるＬｉｊを推定するロボット間距離推定手段９１６の内部構成図である。図８におけるロボット間距離推定手段９１６は、サンプリング時間決定手段９１２、ロボット位置計算手段９１３、端点速度計算手段９１４、およびロボット間距離推定値計算手段９１５で構成される。
【００４９】
サンプリング時間決定手段９１２は、この参照情報を得る時刻を決定する。サンプリング時間決定手段９１２は、ロボットが軌道通りに進んで停止するとして、制動を開始してから停止するまでの制動時間ｔｓｔｐ（９０３）をｎ分割（ｎは自然数）することで、サンプリングする時間ｔｓｍｐｋ（９０４）を、例えば、下式（６）として決定する。
ｔｓｍｐｋ＝ｔｎｏｗ＋ｔｓｔｐ＊ｋ／ｎ（ｋ＝１、２、・・・ｎ−１） （６）
【００５０】
ここで、添字ｋについては、サンプリング時間の通し番号とする。次に、サンプリングする時間ｔｓｍｐｋの各時刻において、ロボット位置計算手段９１３は、ロボット構成部分の端点位置（９０５）を計算する。また、ロボット位置計算手段９１３は、１計算周期にかかる時間をｄｔとした場合、ｄｔ後の時刻（ｔｓｍｐｋ＋ｄｔ）におけるロボット構成部分の端点位置（９０５）も、同様に計算しておく。
【００５１】
リンク端速度情報（図７における６３、６４参照）を計算する方法として、ここでは、例えば、ロボット構成部分モデルにおける２つの端点（以下の説明では、ｅｄｇ０１とｅｄｇ０２で区別する）の時刻ｔｓｍｐｋにおける位置ｐｅｄｇ０１（ｔｓｍｐｋ）とｐｅｄｇ０２（ｔｓｍｐｋ）、時刻ｔｓｍｐｋ＋ｄｔにおけるｐｅｄｇ０２（ｔｓｍｐｋ＋ｄｔ）とｐｅｄｇ０２（ｔｓｍｐｋ＋ｄｔ）の差分から計算する方法をとる。
【００５２】
これによって、端点速度計算手段９１４は、サンプリングする時刻ｔｓｍｐｋ（ｋ＝１、２、・・・、ｎ−１）における各ロボット構成部分の端点速度Ｖｉｅｄｇ０１（ｔｓｍｐｋ）、Ｖｉｅｄｇ０２（ｔｓｍｐｋ）、Ｖｊｅｄｇ０１（ｔｓｍｐｋ）、Ｖｊｅｄｇ０２（ｔｓｍｐｋ）（９０６）を得ることができる。
【００５３】
この後、Ｌｉｊの推定値を計算するロボット間距離推定値計算手段９１５は、上述したように、Ｌｉｎｋｉの最近接点付近の速度ＶｉｃｌｓがＬｉｎｋｉの端点速度Ｖｉｅｄｇ０１とＶｉｅｄｇ０２のどちらかの端点速度（６３，６４）よりも必ず遅いため、接近方向に対して大きい方の端点速度を上限値Ｖｉｅｄｇとすることができる。ここでは、この上限値Ｖｉｅｄｇを構成部分の速度上限値としている。
【００５４】
２つの端点速度について、それぞれｔｓｍｐｋにおけるＶｉｅｄｇ （ｔｓｍｐｋ）、Ｖｊｅｄｇ（ｔｓｍｐｋ）が分かっているので、上限値Ｌ（・）ｉｊｄ（５０２）は、最近接方向ベクトルをＮｉｊｃｌｓとして、下式（７）という形で求められる。
Ｌ（・）ｉｊｄ＝Ｎｉｊｃｌｓ・（Ｖｉｅｄｇ（ｔｓｍｐｋ）−Ｖｊｅｄｇ（ｔｓｍｐｋ）） （７）
【００５５】
ここで得られる端点ごとの２つのＬ（・）ｉｊｄのうち、接近方向に対してより大きい方を上限値Ｌ（・）ｉｊｄと定める。また、ここでは、簡単の為に、ｎ＝３としてサンプリングし、各サンプリング間を台形近似してΔＬｉｊｄを計算する。図９は、本発明の実施の形態１におけるロボット構成部分の端点速度を用いて最近接点間距離を計算する場合の、速度値から距離を計算するための台形近似を表す説明図である。
【００５６】
この図９に示すような台形近似により、下式（８）を用いることで、サンプリング時刻ごとにおける推定値Ｌｉｊｄを得る。
Ｌｉｊｄ＝Ｌｉｊ（ｔｎｏｗ）＋ΔＬｉｊｄ （８）
【００５７】
干渉判定手段９１７は、干渉判定を行なうための干渉判定領域の閾値をあらかじめ記憶している干渉判定用閾値記憶手段９１８から、チェック対象としているロボット構成部分モデルの干渉判定領域の大きさから定義される干渉判定の閾値ＬｉｊＬｉｍ９０８を読み出す。そして、干渉判定手段９１７は、上述した推定方法を利用してロボット間距離推定手段９１６で求められるロボット間距離の推定値Ｌｉｊｄ９０７と、読み出した干渉判定の閾値ＬｉｊＬｉｍ９０８との比較を、下式（９）により行ない、サンプリング時刻ごとに干渉判定する。
ＬｉｊＬｉｍ≦Ｌｉｊｄ （９）
【００５８】
上式（９）の条件を満たす場合には、干渉判定手段９１７は、干渉は起こらないと判定して指令値生成手段９１９にその旨を伝達する（９０９）。この場合は、移動指令値としてロボット制御装置内で追従する目標値に向かう移動指令が生成される。一方、上式（９）の条件を満たさない場合には、干渉判定手段９１７は、干渉の危険性があると判定して、移動指令値として停止を開始するための指令変更の伝達（９０９）を指令値生成手段９１９に対して行なう。
【００５９】
図１０は、本発明の実施の形態１において、推定結果に基づく干渉判定手段９１７により干渉の危険性があると判定された場合に減速開始を行なう説明図である。この図１０に示すように、判定を行なう現在時刻ｔｎｏｗ２０１における推定結果に基づいて、干渉すると判定されるリミット（６０１）に達した際に干渉の危険性があると判定いた場合には、干渉判定手段９１７は、減速停止を開始するための指令変更の伝達（９０９）を指令値生成手段９１９に対して行なう。
【００６０】
以上のように、実施の形態１によれば、制動開始から制動完了までの間の構成部分同士の距離の変化について、速度上限値に基づいて最近接距離の変化の推定を行っている。そして、ロボット構成部分ごとの距離の変化の推定結果に基づいて、制動開始から制動完了までの間にロボット同士の干渉がないかどうかを判定している。この結果、制動動作中に構成部分同士が干渉するケースを見落とすことがなくなり、近接配置され動作領域が重複するロボット間の干渉問題において、従来方法と比較して、漏れなく干渉状態を発見する効果がある。
【００６１】
実施の形態２．
先の実施の形態１におけるロボット間距離推定手段９１６は、図８に示したように、端点速度計算手段９１４により求めた各サンプリング時刻における端点速度に基づいて、ロボット間距離の推定を行っていた。これに対して、本実施の形態２は、ロボット間距離の推定を行なうための十分な計算能力を備えている場合に、より高精度な推定を行なう場合について説明する。
【００６２】
図１１は、本発明の実施の形態２におけるＬｉｊを推定するロボット間距離推定手段９１６ａの内部構成図である。図１１におけるロボット間距離推定手段９１６ａは、サンプリング時間決定手段９１２、ロボット位置計算手段９１３、最近接点・最近速度計算手段９１４ａ、およびロボット間距離推定値計算手段９１５で構成される。
【００６３】
図１１における最近接点・最近速度計算手段９１４ａは、先の実施の形態１に記載のｎ個のサンプリングそれぞれに対して最近接距離Ｌｉｊと最近接点における接近速度Ｖｉｃｌｓ、Ｖｊｃｌｓの計算（９１４）を行ない、上式（５）を計算することでＬ（・）ｉｊを求める。
【００６４】
図１２は、本発明の実施の形態２における制動開始から停止までの接近速度変化について、最近接距離をもとにしたものとロボット構成部分の端点速度での比較の一例を表す図である。白丸で示したプロットが、先の実施の形態１におけるロボット間距離推定手段９１６による推定結果を示しており、黒丸で示したプロットが、本実施の形態２におけるロボット間距離推定手段９１６ａによる推定結果を示している。
【００６５】
図１２に示すように、実際のロボット構成部分モデル間の最近接距離Ｌｉｊの時間変化で求められる接近速度Ｌ（・）ｉｊ（５０１）を上限値Ｌ（・）ｉｊｄ（５０２）の代わりに利用することで、実際の停止までの速度変化（５１１）をより精度よく予測することができる。
【００６６】
以上のように、実施の形態２によれば、最近接距離と最近接点における接近速度の計算を行なうことにより、制動開始から実際の停止までの速度変化を、より高精度に予測することができる。
【００６７】
実施の形態３．
本実施の形態３は、干渉が発生すると判定され、停止動作を開始した後も、干渉判定を行ない続け、評価式（上式（４）に相当）で干渉がないと判断されれば、動作を再び継続する場合について説明する。
【００６８】
図１３は、本発明の実施の形態３において、制動開始後に、引き続き干渉チェックを行った際に推定値Ｌｉｊｄが干渉判定領域の閾値ＬｉｊＬｉｍよりも大きくなった場合の継続動作を示す説明図である。あるロボット構成部分同士の距離Ｌｉｊについて見るときに、停止動作によって見積もった距離Ｌｉｊは、大きく見積もってしまう場合がある（図１３の６１４参照）。
【００６９】
これに対して、本実施の形態３は、現在時刻（２０１）の干渉判定において干渉の危険性があると判定された場合には減速を行なう。一方で、減速開始後の時刻ｔ' ｎｏｗ（２０２）において干渉の危険性がないと判定された場合には、減速を中止し、動作を継続させる（図１３の６１５参照）。また、それまでに減速を行なっていなければ、軌道通りに動作を継続させるという処理を行なう。
【００７０】
以上のように、実施の形態３によれば、停止終了した位置について、特に、特許文献３に示されるようなリアルタイムで動的に計算された停止指令の生成が必要な場合において、停止動作中にも干渉するか否かの判定を行ない、必要に応じて減速をやめることで、ロボットは動作を続けることができる。高速動作時においては、推定誤差も大きくなるため、このように、停止動作中にも干渉するか否かの判定を行なうことで、制動開始時の推定が大きすぎて停止を開始してしまった場合にも、その後の状況に応じて継続動作が可能となる。
【００７１】
これにより、従来方法で見落とされていた干渉を見落とさずに判定でき、ロボット同士の干渉が生じないとともに、最適な停止位置まで干渉することなく接近を続けることができる効果がある。
【００７２】
なお、本発明は、ロボット同士の場合の干渉チェックに限定されるものではない。一方がロボットであり、他方がモデル化された周辺装置や冶具などを含めた外部環境の場合にも適用可能である。このような外部環境について、あらかじめ形状と位置のデータを登録しておき、ロボットとの相対的な位置関係や動作の相対的な関係の情報が、通信によって共有化できるときに、同様の干渉チェックを適用することができる。
【図面の簡単な説明】
【００７３】
【図１】本発明の実施の形態１における干渉チェック制御装置の構成図である。
【図２】本発明の実施の形態１において干渉チェックを行なうロボットシステムの構成例を表す図である。
【図３】本発明の実施の形態１におけるロボットの線分モデルの例示図である。
【図４】本発明の実施の形態１におけるモデル化された２線分間の最近接点を幾何的に表した図である。
【図５】本発明の実施の形態１における干渉判定領域の説明図である。
【図６】本発明の実施の形態１における２つのロボット間の干渉チェック方法の説明図である。
【図７】本発明の実施の形態１における最近接位置における速度よりもリンク端における速度が速度値として大きいことを表す説明図である。
【図８】本発明の実施の形態１におけるＬｉｊを推定するロボット間距離推定手段の内部構成図である。
【図９】本発明の実施の形態１におけるロボット構成部分の端点速度を用いて最近接点間距離を計算する場合の、速度値から距離を計算するための台形近似を表す説明図である。
【図１０】本発明の実施の形態１において、推定結果に基づく干渉判定手段９１７により干渉の危険性があると判定された場合に減速開始を行なう説明図である。
【図１１】本発明の実施の形態２におけるＬｉｊを推定するロボット間距離推定手段９１６ａの内部構成図である。
【図１２】本発明の実施の形態２における制動開始から停止までの接近速度変化について、最近接距離をもとにしたものとロボット構成部分の端点速度での比較の一例を表す図である。
【図１３】本発明の実施の形態３において、制動開始後に引き続き干渉チェックを行った際に推定値Ｌｉｊｄが干渉判定領域の閾値ＬｉｊＬｉｍよりも大きくなった場合の継続動作を示す説明図である。
【符号の説明】
【００７４】
９１０ ロボット速度・位置記憶手段、９１１ 制動時間計算手段、９１２ サンプリング時間決定手段、９１３ ロボット位置計算手段、９１４ 端点速度計算手段、９１４ａ 最近接点・最近速度計算手段、９１５ ロボット間距離推定値計算手段、９１６、９１６ａ ロボット間距離推定手段、９１７ 干渉判定手段、９１８ 干渉判定用閾値記憶手段、９１９ 指令値生成手段。
【特許請求の範囲】
【請求項１】
それぞれの動作領域に重複する部分を有するロボット同士の干渉チェック制御装置であって、
計算周期ごとに算出されたロボットの速度・位置データを記憶する速度・位置記憶手段と、
各ロボット間の干渉判定を行なうための干渉判定領域の閾値をあらかじめ記憶する干渉判定用閾値記憶手段と、
前記速度・位置記憶手段に記憶された前記速度・位置データに基づいて、ロボットが現時点から制動を開始した際に完全に停止するまでに必要な時間を計算する制動時間計算手段と、
制動開始から制動完了までの各ロボット同士の各構成部分間の最近接距離の履歴を、接近する各構成部分の速度に基づいて推定するロボット間距離推定手段と、
推定された各構成部分間の前記最近接距離の履歴と、前記干渉判定用閾値記憶手段にあらかじめ記憶された干渉判定領域の前記閾値とを比較し、制動開始から制動完了までの間の干渉の有無を判定する干渉判定手段と、
前記干渉判定手段による干渉判定結果に応じて、計算周期ごとに各ロボットに対する移動指令値を作り出すとともに、現在の計算周期におけるロボットの速度・位置データを前記速度・位置記憶手段に記憶させる指令値生成手段と
を備えることを特徴とする干渉チェック制御装置。
【請求項２】
請求項１に記載の干渉チェック制御装置において、
前記ロボット間距離推定手段は、制動開始から制動完了までの各ロボット同士の各構成部分間の最近接距離の履歴を、接近する各構成部分の端点速度から導かれる速度上限値に基づいて推定することを特徴とする干渉チェック制御装置。
【請求項３】
請求項１に記載の干渉チェック制御装置において、
前記ロボット間距離推定手段は、制動開始から制動完了までの各ロボット同士の各構成部分間の最近接距離の履歴を、接近する各構成部分の最近接点における接近速度に基づいて推定することを特徴とする干渉チェック制御装置。
【請求項４】
それぞれの動作領域に重複する部分を有するロボット同士の干渉チェック制御方法であって、
第１の記憶部に記憶された速度・位置データに基づいて、ロボットが現時点から制動を開始した際に完全に停止するまでに必要な時間を計算する制動時間計算ステップと、
制動開始から制動完了までの各ロボット同士の各構成部分間の最近接距離の履歴を、接近する各構成部分の速度に基づいて推定するロボット間距離推定ステップと、
推定された各構成部分間の前記最近接距離の履歴と、第２の記憶部にあらかじめ記憶された干渉判定領域の閾値とを比較し、制動開始から制動完了までの間の干渉の有無を判定する干渉判定ステップと、
前記干渉判定ステップによる干渉判定結果に応じて、計算周期ごとに各ロボットに対する移動指令値を作り出すとともに、現在の計算周期におけるロボットの速度・位置データを前記第１の記憶部に記憶させる指令値生成ステップと
を備えることを特徴とする干渉チェック制御方法。
【請求項１】
それぞれの動作領域に重複する部分を有するロボット同士の干渉チェック制御装置であって、
計算周期ごとに算出されたロボットの速度・位置データを記憶する速度・位置記憶手段と、
各ロボット間の干渉判定を行なうための干渉判定領域の閾値をあらかじめ記憶する干渉判定用閾値記憶手段と、
前記速度・位置記憶手段に記憶された前記速度・位置データに基づいて、ロボットが現時点から制動を開始した際に完全に停止するまでに必要な時間を計算する制動時間計算手段と、
制動開始から制動完了までの各ロボット同士の各構成部分間の最近接距離の履歴を、接近する各構成部分の速度に基づいて推定するロボット間距離推定手段と、
推定された各構成部分間の前記最近接距離の履歴と、前記干渉判定用閾値記憶手段にあらかじめ記憶された干渉判定領域の前記閾値とを比較し、制動開始から制動完了までの間の干渉の有無を判定する干渉判定手段と、
前記干渉判定手段による干渉判定結果に応じて、計算周期ごとに各ロボットに対する移動指令値を作り出すとともに、現在の計算周期におけるロボットの速度・位置データを前記速度・位置記憶手段に記憶させる指令値生成手段と
を備えることを特徴とする干渉チェック制御装置。
【請求項２】
請求項１に記載の干渉チェック制御装置において、
前記ロボット間距離推定手段は、制動開始から制動完了までの各ロボット同士の各構成部分間の最近接距離の履歴を、接近する各構成部分の端点速度から導かれる速度上限値に基づいて推定することを特徴とする干渉チェック制御装置。
【請求項３】
請求項１に記載の干渉チェック制御装置において、
前記ロボット間距離推定手段は、制動開始から制動完了までの各ロボット同士の各構成部分間の最近接距離の履歴を、接近する各構成部分の最近接点における接近速度に基づいて推定することを特徴とする干渉チェック制御装置。
【請求項４】
それぞれの動作領域に重複する部分を有するロボット同士の干渉チェック制御方法であって、
第１の記憶部に記憶された速度・位置データに基づいて、ロボットが現時点から制動を開始した際に完全に停止するまでに必要な時間を計算する制動時間計算ステップと、
制動開始から制動完了までの各ロボット同士の各構成部分間の最近接距離の履歴を、接近する各構成部分の速度に基づいて推定するロボット間距離推定ステップと、
推定された各構成部分間の前記最近接距離の履歴と、第２の記憶部にあらかじめ記憶された干渉判定領域の閾値とを比較し、制動開始から制動完了までの間の干渉の有無を判定する干渉判定ステップと、
前記干渉判定ステップによる干渉判定結果に応じて、計算周期ごとに各ロボットに対する移動指令値を作り出すとともに、現在の計算周期におけるロボットの速度・位置データを前記第１の記憶部に記憶させる指令値生成ステップと
を備えることを特徴とする干渉チェック制御方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【公開番号】特開２０１０−５２１１６（Ｐ２０１０−５２１１６Ａ）
【公開日】平成２２年３月１１日（２０１０．３．１１）
【国際特許分類】
【出願番号】特願２００８−２２２１１１（Ｐ２００８−２２２１１１）
【出願日】平成２０年８月２９日（２００８．８．２９）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
【公開日】平成２２年３月１１日（２０１０．３．１１）
【国際特許分類】
【出願日】平成２０年８月２９日（２００８．８．２９）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
[ Back to top ]