JP3975341B2 - ロボットの干渉チェック方法 - Google Patents
ロボットの干渉チェック方法 Download PDFInfo
- Publication number
- JP3975341B2 JP3975341B2 JP2002140237A JP2002140237A JP3975341B2 JP 3975341 B2 JP3975341 B2 JP 3975341B2 JP 2002140237 A JP2002140237 A JP 2002140237A JP 2002140237 A JP2002140237 A JP 2002140237A JP 3975341 B2 JP3975341 B2 JP 3975341B2
- Authority
- JP
- Japan
- Prior art keywords
- approximate
- interference
- distance
- solid
- size
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Images
Landscapes
- Numerical Control (AREA)
- Manipulator (AREA)
Description
【０００１】
【発明の属する技術分野】
本発明は、複数のロボット間の干渉をチェックする方法、特に、好適に干渉をチェックするロボットの干渉チェック方法に関する。
【０００２】
【従来の技術】
少なくとも２台以上のロボットが同時に作業することを想定し、これらロボットの衝突を防ぐために、ロボット同士の干渉をチェックする方法がいくつか開示されている。例えば、特開昭６０−２１７４１０号公報に開示の「協調作業教示におけるロボット干渉検出方式」では、ロボットの各部を円筒と半球の近似立体で覆い、これら近似立体間の干渉をチェックすることでロボット同士の干渉を検出している。そして、ロボットの干渉チェックはロボット各部に設定した近似立体間の距離を演算し、それと近似立体のサイズとの比較により干渉の有無を判定することで行われている。
【０００３】
図１２に従来のロボットの干渉チェック方法の構成を表すブロック図を示す。図１２において、９１は近似立体間距離演算手段で、ある時刻ｔでの第１の近似立体位置（ｔ）９０４と第２の近似立体位置（ｔ）９０５とを入力し、それら近似立体間の距離を演算し、近似立体間の距離（ｔ）９０６として出力する。第１の近似立体位置（ｔ）９０４または第２の近似立体位置（ｔ）９０５は、例えば近似立体が半球であれば半球の中心位置、円筒であれば円筒の両端点位置である。また、第１の近似立体位置（ｔ）９０４が一方のロボットに対するもの、第２の近似立体位置（ｔ）９０５が他方のロボットに対するものと想定している。
【０００４】
９２は干渉判定手段で、近似立体間の距離（ｔ）９０６と予め近似立体サイズファイル９３に設定されている近似立体サイズ９３とを入力し、それらを比較し、干渉の有無信号８１０を出力する。例えば、近似立体が２つの球であれば、近似立体間の距離（ｔ）９０６である球間の距離ＬSSと、近似立体サイズ９３である一方の球半径ＲSAと他方の球半径ＲSBとから式（１）の判定を行い
ＬSS≦ＲSA＋ＲSB・・・・・・（１）
式（１）を満たす場合干渉有り、満たさない場合干渉無しとする。ところで、図１２を用いて説明した従来のロボットの干渉チェック方法は、ロボットの制御周期ΔＴ毎に演算が行われる。つまり、ある時刻ｔで干渉チェックの演算を行った後、時刻ｔ＋ΔＴの時、次の干渉チェックの演算を行う。
【０００５】
【発明が解決しようとする課題】
ところが、従来のロボットの干渉チェック方法では、衝突を回避できないケースが存在する。図１３は近似立体が干渉するケースを表す図を示している。図１３において、（ａ）は球と球の干渉のケース、（ｂ）は円筒と球の干渉のケース、（ｃ）は円筒と円筒の干渉のケースである。尚、従来の技術では円筒と半球を扱ったが、理解をし易くするためここでは半球を球として扱う。球と球の干渉ケース（ａ）では、第１の球４が固定で、第２の球５（図で左）が制御周期ΔＴの間に矢印の方向に球５（図で右）まで移動するとする。この時、時刻ｔの時に第１の球４と第２の球５は干渉していない。また、次の時刻ｔ＋ΔＴの時に第１の球４と第２の球５は干渉していない。ところが、制御周期ΔＴの間、第２の球５の移動に伴い、第２の球５は、第１の球４と干渉してしまう。
【０００６】
円筒と球の干渉ケース（ｂ）では、第１の円筒６が固定で、第２の球５が制御周期ΔＴの間に矢印の方向に移動するとする。この時、時刻ｔの時に第１の円筒６と第２の球５は干渉していない。また、次の時刻ｔ＋ΔＴの時に第１の円筒６と第２の球５は干渉していない。ところが、制御周期ΔＴの間、第２の球５の移動に伴い、第２の球５は、第１の円筒６と干渉してしまう。
【０００７】
円筒と円筒の干渉ケース（ｃ）では、第１の円筒６が固定で、第２の円筒７が制御周期ΔＴの間に矢印の方向に移動するとする。この時、時刻ｔの時に第１の円筒６と第２の円筒７は干渉していない。また、次の時刻ｔ＋ΔＴの時に第１の円筒６と第２の円筒７は干渉していない。ところが、制御周期ΔＴの間、第２の円筒７の移動に伴い、第２の円筒７は、第１の円筒６と干渉してしまう。これらの現象は、ロボットの動作速度が極めて高速で、ロボットに設定された近似立体の、制御周期ΔＴの間に移動する距離が大きくなる場合に、制御周期ΔＴの間、図１２に示すような干渉チェックの演算が行えないことに起因する。近似立体が干渉してしまうと、近似立体が設定されたロボットが衝突してしまう危険性が高い。そのため、従来の技術では、ロボットが衝突し、ロボットや工具を破壊する危険性が生じるという問題があった。また、ロボットや工具の破壊による飛散物のため、人に対する安全性が低下するという問題があった。
【０００８】
そこで、本発明の第１の実施形態では、ロボットや工具の干渉を避けることによりそれらを衝突させないようにし、結果としてロボットや工具の破壊の危険性をなくし、人に対する安全性を高めたロボットの干渉チェック方法を提供することを第１の目的とする。
【０００９】
また、本発明の第２の実施形態では、本発明の第１の実施形態では防ぎにくかった課題をさらに解決することにある。例えば、第１の実施形態における干渉予測手段内の、距離推定手段で演算する時刻ｔ＋ΔＴでの第１の近似立体位置または第２の近似立体位置はあくまで推定値であること、距離近似手段で演算する近似曲線は、あくまで近似値であることから、必ずしも完全に実際の状況を反映しているとは限らないので、これら推定値や近似値が要因となって、近似立体が干渉してしまう場合があったり、また、２台以上のロボットの制御周期の同期が取れず、または、同期がずれた場合、一方のロボットの位置と他方のロボットの位置の双方の払い出しのタイミングがずれ、近似立体が干渉してしまう場合もあった。近似立体が干渉してしまうと、近似立体が設定されたロボットが衝突してしまう危険性が高い。そのため、第１実施形態では、ロボットが衝突してロボットや工具を損傷する怖れが若干あった。
そこで、本発明の第２の実施形態では、第１実施形態のロボットの干渉チェック方法の機能を向上させ、ロボットや工具の干渉を確実に避けることにより、それらを衝突させない、結果として、ロボットや工具の破壊の危険性をなくした、人に対する安全性を高めたロボットの干渉チェック方法を提供することを第２の目的とする。
【００１０】
【課題を解決するための手段】
上記問題を解決するため、請求項１記載のロボットの干渉チェック方法は、複数のロボットの各部の形状を覆って近似する近似立体を予め設定し、動作中の前記複数のロボットにおける一方の前記ロボットに設定した前記近似立体と、他方の前記ロボットに設定した前記近似立体との距離を所定の制御周期ごとに演算する近似立体間距離演算手段と、前記近似立体のサイズと前記近似立体間距離演算手段で演算された距離とを比較する干渉判定手段と、を有して成るロボットの干渉をチェックする方法において、前記近似立体間距離演算手段で演算された距離のうち少なくとも現在から制御周期１回分遡った時刻（以降、直前の時刻）での距離を記憶する距離記憶手段と、前記制御周期ごとに取得される前記近似立体の位置のうち少なくとも直前の時刻での位置を記憶する位置記憶手段と、干渉予測手段とを有し、前記干渉予測手段は、直前の時刻での前記近似立体の位置と現在の時刻での位置との差分を現在の時刻での位置に加えることで、現在から制御周期１回分経った時刻（以降、次の時刻）での前記近似立体の位置を推定し、次の時刻での前記近似立体間の距離を演算する距離推定手段と、前記距離記憶手段に記憶されている直前の時刻での前記近似立体間の距離と現在の時刻での距離と前記距離推定手段で演算された次の時刻での距離とから、前記近似立体間の距離の時間変化を表す２次以上の近似曲線を演算する距離近似手段と、現在の時刻から次の時刻までの間における前記近似曲線の最小値を求める最小値取得手段と、前記近似曲線の最小値と前記近似立体のサイズとを比較することにより干渉を判定する干渉推定手段と、から構成されることを特徴とする。
【００１１】
請求項１記載のロボットの干渉チェック方法によれば、次の時刻での近似立体の位置を推定し、次の時刻までの干渉の有無が予測できる干渉予測手段を設けているので、従来の干渉チェック方法に合わせ、近似立体間の干渉が確実にチェックでき、ロボットや工具の衝突を防止することができるという効果がある。
【００１２】
また、請求項２記載の発明は、請求項１記載のロボットの干渉チェック方法において、前記干渉予測手段の前段に干渉予測判定手段を設け、前記干渉予測判定手段は、前記近似立体のサイズより大きく設定された距離閾値と前記近似立体間距離演算手段が演算した現在の時刻での前記近似立体間の距離とを比較し、前記干渉予測手段は、前記距離が前記距離閾値以下の場合のみ演算を行うことを特徴とする。
請求項２記載のロボットの干渉チェック方法によれば、請求項１記載のロボットの干渉チェック方法と同様、干渉予測手段を設けているので、従来の干渉チェック方法に合わせ、近似立体間の干渉が確実にチェックでき、ロボットや工具の衝突を防止することができるという効果がある。また、干渉予測判定手段を設けて、近似立体サイズより大きめの距離閾値に基づいて干渉予測手段の演算の実行を決定することができるので、常時干渉予測手段の演算を実行する必要がなく、干渉チェックに要する演算が高速に行えるという効果がある。
【００１３】
また、本発明の請求項３記載の発明は、請求項１記載のロボットの干渉チェック方法において、前記干渉予測手段への入力として前記近似立体のサイズより大きい近似立体拡張サイズを予め設定し、前記干渉推定手段は、前記近似立体のサイズに代えて前記近似曲線の最小値と前記近似立体拡張サイズとを比較することにより干渉を判定することを特徴とする。
請求項３記載のロボットの干渉チェック方法によれば、近似立体拡張サイズを利用して干渉の有無を判定する干渉予測手段を設けて、近似立体サイズより大きめの近似立体拡張サイズに基づいて干渉チェックを行うので、干渉を判定する距離が長くなり、請求項１記載の干渉の可能性を低減させ、より確実にロボットや工具の衝突を防止することができるという効果がある。
【００１４】
また、本発明の請求項４記載の発明は、請求項１記載のロボットの干渉チェック方法において、前記近似立体のサイズを拡張して前記干渉予測手段への入力とする近似立体サイズ調整手段を設け、前記近似立体サイズ調整手段は、現在の時刻での前記近似立体間の距離が所定の判定値より小さい場合には前記距離が小さくなるに従って前記近似立体のサイズを拡張した近似立体拡張サイズを出力し、
前記干渉推定手段は、前記近似立体のサイズに代えて前記近似曲線の最小値と前記近似立体拡張サイズとを比較することにより干渉を判定することを特徴とする。
請求項４記載のロボットの干渉チェック方法によれば、近似立体間の距離に応じて近似立体拡張サイズを決定する近似立体サイズ調整手段を設けて、近似立体が接近するに従って近似立体拡張サイズを大きくするようにするので、請求項１記載の干渉の可能性を低減させ、より確実にロボットや工具の衝突を防止することができるという効果がある。
【００１５】
また、本発明の請求項５記載の発明は、請求項１記載のロボットの干渉チェック方法において、前記位置記憶手段に記憶されている直前の時刻での前記近似立体の位置と現在の時刻での位置とから前記近似立体の速度を演算する速度演算手段と、前記近似立体のサイズを拡張して前記干渉予測手段への入力とする近似立体サイズ調整手段を設け、前記近似立体サイズ調整手段は、前記近似立体の速度が所定の判定値より大きい場合には前記速度が大きくなるに従って前記近似立体のサイズを拡張した近似立体拡張サイズを出力し、前記干渉推定手段は、前記近似立体のサイズに代えて前記近似曲線の最小値と前記近似立体拡張サイズとを比較することにより干渉を判定することを特徴とする。
請求項５記載のロボットの干渉チェック方法によれば、近似立体の速度に応じて近似立体拡張サイズを決定する近似立体サイズ調整手段を設けて、近似立体が高速で接近するに従って近似立体拡張サイズを大きくするようにするので、従来の干渉の可能性を低減させ、より確実にロボットや工具の衝突を防止することができるという効果がある。
【００１６】
また、本発明の請求項６記載の発明は、請求項３〜５のいずれか１項記載のロボットの干渉チェック方法において、前記干渉予測手段の前段に干渉予測判定手段を設け、前記干渉予測判定手段は、前記近似立体拡張サイズより大きく設定された距離閾値と前記近似立体間距離演算手段が演算した現在の時刻での前記近似立体間の距離とを比較し、前記干渉予測手段は、前記距離が前記距離閾値以下の場合のみ演算を行うことを特徴とする。
請求項６記載のロボットの干渉チェック方法によれば、請求項３〜５のいずれか１項記載のロボットの干渉チェック方法と同様、干渉予測手段を設けて、近似立体サイズより大きめの近似立体拡張サイズに基づいて干渉チェックを行うので、干渉を判定する距離が長くなり、従来の干渉の可能性を低減させ、より確実にロボットや工具の衝突を防止することができるという効果がある。また、干渉予測判定手段を設けて、近似立体拡張サイズより大きめの距離閾値に基づいて干渉予測手段の演算の実行を決定することができるので、常時干渉予測手段の演算を実行する必要がなく、干渉チェックに要する演算が高速に行えるという効果がある。
【００１７】
以上、全ての請求項記載のロボットの干渉チェック方法によれば、ロボットや工具の衝突を防止することができるので、ロボットや工具の破壊の危険性をなくし、それに伴う人に対する安全性を向上させることができるという効果がある。
【００１８】
【発明の実施の形態】
本発明の第１の実施の形態のロボットの干渉チェック方法について、図１〜図３を用いて説明する。本実施の形態は、請求項１記載の内容に相当する。図１は本発明の第１の実施の形態に関わるロボットの干渉チェック方法の構成を表すブロック図である。尚、図１２で説明した構成要素のうち同一の構成要素については同一の符号を付して説明を割愛する。図１において、１１は位置記憶手段で、制御周期ΔＴ分、１つ前の時刻ｔ−ΔＴでの第１の近似立体位置（ｔ−ΔＴ）１０４と第２の近似立体位置（ｔ−ΔＴ）１０５とを記憶している。１２は距離記憶手段で、制御周期ΔＴ分、１つ前の時刻ｔ−ΔＴでの近似立体間の距離（ｔ−ΔＴ）１０６を記憶している。１３は干渉予測手段で、近似立体間の距離（ｔ−ΔＴ）１０６と、近似立体間の距離（ｔ）９０６と、第１の近似立体位置（ｔ−ΔＴ）１０４と、第２の近似立体位置（ｔ−ΔＴ）１０５と、第１の近似立体位置（ｔ）９０４と、第２の近似立体位置（ｔ）９０５と、近似立体サイズ９３とを入力し、制御周期ΔＴの間の干渉の有無を予測し、干渉の有無信号８１０を出力する。
【００１９】
干渉予測手段１３の具体的内容を、図２および図３を用いて説明する。図２は本発明の干渉予測手段の構成を表すブロック図である。尚、前出の図を用いて説明した同一構成要素については、同一の符号を付して説明を割愛する。図２において、５１は距離推定手段で、第１の近似立体位置（ｔ−ΔＴ）１０４と、第２の近似立体位置（ｔ−ΔＴ）１０５と、第１の近似立体位置（ｔ）９０４と、第２の近似立体位置（ｔ）９０５とを入力し、時刻ｔ＋ΔＴでの近似立体間の距離を推定し、近似立体間の距離（ｔ＋ΔＴ）５０５として出力する。５２は距離近似手段で、近似立体間の距離（ｔ−ΔＴ）１０６と、近似立体間の距離（ｔ）９０６と、近似立体間の距離（ｔ＋ΔＴ）５０５とを入力し、距離と時間との関係から近似曲線を作成し、近似曲線５０６として出力する。５３は最小値取得手段で、近似曲線５０６を入力し、近似曲線５０６の最小値を求め、距離最小値５０７として出力する。
【００２０】
５４は干渉推定手段で、距離最小値５０７と、近似立体サイズ９３とを入力し、それらを比較し、干渉の有無信号８１０を出力する。例えば、近似立体が２つの球であれば、距離最小値５０７をＬSSMINとし、近似立体サイズ９３である一方の球半径ＲSAと他方の球半径ＲSBとから式（２）の判定を行い、
ＬSSMIN≦ＲSA＋ＲSB・・・・・（２）
式（２）を満たす場合干渉有り、満たさない場合干渉無しとする。次に、これらの距離推定手段５１、距離近似手段５２、最小値取得手段５３、干渉推定手段５４の各機能を、図３を用いて詳しく説明する。
【００２１】
図３は、本発明の干渉予測手段の機能を説明する図である。尚、前出の図を用いて説明した同一の構成要素については、同一の符号を付して説明を割愛する。ここでは、図１３（ａ）に示した球と球の干渉ケースを扱う。図３（ａ）において、図１３（ａ）と同様、第１の球４は固定で、第２の球５が移動するとする。従って、第１の近似立体位置（ｔ−ΔＴ）１０４と、第１の近似立体位置（ｔ）９０４と、第１の近似立体位置（ｔ＋ΔＴ）７０１は、全て同じ位置となる。図３（ａ）に示すように、今、時刻ｔ−ΔＴから時刻ｔへの経過に伴い、第１の球４と第２の球５とが接近してきている場合を考える。
【００２２】
距離推定手段５１（図２）は、まず、時刻ｔ−ΔＴの第２の近似立体位置（ｔ−ΔＴ）１０５と、時刻ｔの第２の近似立体位置（ｔ）９０５とから、時刻ｔ＋ΔＴの第２の近似立体位置（ｔ＋ΔＴ）７０２を推定する。この推定は、例えば、第２の近似立体位置（ｔ−ΔＴ）１０５と第２の近似立体位置（ｔ）９０５を直線で結び、その延長線上、同じ移動量の位置とするような方法を用いれば良い。一般に、制御周期ΔＴ間で移動する距離は、全体の移動距離と比較して微小であるため、第２の近似立体位置（ｔ＋ΔＴ）７０２は、直線上にあると見なして問題ない。また、距離推定手段５１は、次に、推定された第２の近似立体位置（ｔ＋ΔＴ）７０２と、第１の近似立体位置（ｔ＋ΔＴ）７０１との距離を演算し、近似立体間の距離（ｔ＋ΔＴ）５０５として出力する。
【００２３】
距離近似手段５２（図２）は、まず、図３（ｂ）のような距離−時間グラフを作成する。図３（ｂ）の距離−時間グラフは、近似立体間の距離（ｔ−ΔＴ）１０６と、近似立体間の距離（ｔ）９０６と、近似立体間の距離（ｔ＋ΔＴ）５０５を時刻に応じてプロットしたものである。距離近似手段５２は、次に、これらプロットしたデータを２次以上の多項式で近似し、近似曲線５０６として出力する。図３（ｂ）の距離−時間グラフでは、２次の多項式で近似している。近似には、例えば、最小自乗法を用いれば良い。
【００２４】
最小値取得手段５３（図２）は、近似曲線５０６（具体的には近似曲線の係数）を入力し、近似曲線５０６の最小値を求め、距離最小値５０７として出力する。
【００２５】
干渉推定手段５４（図２）は、距離最小値５０７と近似立体サイズ９３とを入力し、例えば式（２）を用いて干渉の有無信号８１０を判定し、出力する。図３（ｂ）の距離−時間グラフでは、式（２）を満たす場合、すなわち、干渉有りの場合を示している。以上説明したように、本発明の第１の実施の形態（請求項１記載の例）は、時刻ｔ＋ΔＴでの近似立体位置（ｔ＋ΔＴ）７０１及び７０２を推定し、時刻ｔから時刻ｔ＋ΔＴまでの干渉の有無信号８１０が予測できる干渉予測手段１３を設けているので、従来の干渉判定手段９２による干渉チェックに合わせ、近似立体間の干渉が確実にチェックでき、ロボットや工具の衝突を防止することができるという効果がある。
【００２６】
尚、本実施の形態は、球と球の干渉ケース図３（ａ）についてのみ説明したが、図１３で示したように球と球の干渉ケース（図１３（ａ））以外にも、円筒と球の干渉ケース（図１３（ｂ））や円筒と円筒の干渉ケース（図１３（ｃ））についても、球と球の干渉ケース図３（ａ）と同様な方法を適用すればロボットの干渉チェックが実行できる。
【００２７】
また、本実施の形態は、第２の近似立体位置（ｔ＋ΔＴ）７０２を、第２の近似立体位置（ｔ−ΔＴ）１０５と第２の近似立体位置（ｔ）９０５とを結ぶ直線上にあるとしたが、本発明はこれに限るものではなく、予め決められた次数を持つ曲線上にあるとしても良い。また、本実施の形態は、近似立体位置を時刻ｔ−ΔＴ、時刻ｔ、時刻ｔ＋ΔＴに対応する３点のみを対象としたが、本発明はこれに限るものではなく、４点以上を対象として、干渉の予測性能を向上させても良い。
【００２８】
また、本実施の形態は、距離近似手段５２（図２）での近似曲線５０６を２次の多項式としたが、本発明これに限るものではなく、３次以上の多項式として、近似性能を向上させても良い。また、本実施の形態は、一方の近似立体（第１の球４）の位置を固定していたが、本発明はこれに限るものではなく、２つの近似立体が移動していても同様な方法でロボットの干渉チェックが実行できる。
【００２９】
本発明の第１の実施の形態の変形例としてのロボットの干渉チェック方法について、図４を用いて説明する。本変形例は請求項２記載の内容に相当するもので、図４は本変形例に係るロボットの干渉チェック方法の構成を表すブロック図である。尚、前出の図を用いて説明した同一の構成要素については、同一の符号を付して説明を割愛する。図４において、２１は干渉予測判定手段、２２は距離閾値ファイルである。図１を用いて説明した第１の実施の形態との違いは、新たに干渉予測判定手段２１と距離閾値ファイル２２とが加わっている点である。距離閾値ファイルの距離閾値２２は、近似立体サイズ９３より大きめの値を設定しておく。干渉予測判定手段２１は、時刻ｔでの近似立体間の距離（ｔ）９０６と、予め設定されている距離閾値２２とを入力し、それらを比較し、干渉予測手段１３の演算の実行を決定する。例えば、近似立体が２つの球であれば、近似立体間の距離（ｔ）９０６である球間の距離ＬSSと、距離閾値２２のＬth（＞ＲSA＋ＲSB）とから式（３）の判定を行い、
ＬSS≦Ｌth・・・・・（３）
式（３）を満たす場合、干渉予測手段１３の演算を実行し、満たさない場合、干渉予測手段１３の演算を実行しない。
【００３０】
演算を実行する場合には、近似立体間の距離（ｔ−ΔＴ）１０６と、近似立体間の距離（ｔ）９０６と、第１の近似立体位置（ｔ−ΔＴ）１０４と、第２の近似立体位置（ｔ−ΔＴ）１０５と、第１の近似立体位置（ｔ）９０４と、第２の近似立体位置（ｔ）９０５と、近似立体サイズ９３とを干渉予測手段１３に出力する。干渉予測手段１３の機能は、図１及び図２を用いて説明したものと全く同様である。以上説明したように、本発明の第１の実施形態の変形例（請求項２記載の例）は、第１の実施の形態と同様、干渉予測手段１３を設けているので、従来の干渉判定手段９２による干渉チェックに合わせ、近似立体間の干渉が確実にチェックでき、ロボットや工具の衝突を防止することができるという効果がある。また、干渉予測判定手段２１を設けて、近似立体サイズ９３より大きめの距離閾値２２に基づいて干渉予測手段１３の演算の実行を決定することができるので、常時干渉予測手段１３の演算を実行する必要がなく、干渉チェックに要する演算が高速に行えるという効果がある。
【００３１】
次に、本発明の第２の実施の形態のロボットの干渉チェック方法について、図５および図６を用いて説明する。本実施の形態は、請求項３記載の内容に相当する。図５は、本発明の第２の実施の形態に関わるロボットの干渉チェック方法の構成を表すブロック図である。尚、前出の図を用いて説明した同一の構成要素については、同一の符号を付して説明を割愛する。図５において、１４は干渉予測手段、９４は近似立体拡張サイズファイルである。図１を用いて説明した第１の実施形態の干渉チェック方法との違いは、第１の実施形態の干渉予測手段１３が第２の実施形態の干渉予測手段１４に替わっている点、近似立体拡張サイズファイル９４が加わっている点である。近似立体拡張サイズ９４の値は、近似立体サイズ９３より大きめの値を予め設定しておく。干渉予測手段１４は、距離記憶手段１２からの近似立体間の距離（ｔ−ΔＴ）１０６と、近似立体間距離演算手段９１からの近似立体間の距離（ｔ）９０６と、位置記憶手段１１からの第１の近似立体位置（ｔ−ΔＴ）１０４と第２の近似立体位置（ｔ−ΔＴ）１０５と、第１の近似立体位置（ｔ）９０４と、第２の近似立体位置（ｔ）９０５と、近似立体拡張サイズ９４とを入力し、制御周期ΔＴの間の干渉の有無を予測し、干渉の有無信号８１０を出力する。
【００３２】
干渉予測手段１４の具体的内容を、図６を用いて説明する。図６は本発明の干渉予測手段の構成を表すブロック図である。尚、前出の図を用いて説明した同一の構成要素については、同一の符号を付して説明を割愛する。図６において５４'は干渉推定手段である。図２を用いて説明した第１の実施形態の干渉予測手段１３とこの干渉予測手段１４との違いは、第１の実施形態の干渉推定手段５４が本発明の干渉推定手段５４'に、そしてその入力となる近似立体サイズ９３（第１実施形態）がここでは近似立体拡張サイズ９４に、それぞれ替わっている点である。干渉推定手段５４'は、距離最小値５０７と近似立体拡張サイズ９４とを入力し、それらを比較し、干渉の有無信号８１０を出力する。例えば、近似立体が２つの球であれば、距離最小値５０７をＬSSMINとし、近似立体拡張サイズ９４である一方の球半径ＲSA'（＞ＲSA）と他方の球半径ＲSB'（＞ＲSB）とから式（４）の判定を行い、
ＬSSMIN≦ＲSA'＋ＲSB' ・・・・・（４）
式（４）を満たす場合干渉有り、満たさない場合干渉無しとする。以上説明したように、本発明の第２の実施の形態（請求項３記載の例）は、近似立体拡張サイズ９４を利用して干渉の有無を出力する干渉推定手段５４'を含む干渉予測手段１４を設けて、近似立体サイズ９３より大きめの近似立体拡張サイズ９４に基づいて干渉チェックを行うので、干渉を判定する距離が長くなり、第１の実施形態の干渉の可能性を低減させ、より確実にロボットや工具の衝突を防止することができるという効果がある。
【００３３】
さらに、本発明の第２の実施の形態の変形例としてのロボットの干渉チェック方法について、図７および図８を用いて説明する。本変形例は、請求項４記載の内容に相当するもので、図７は本変形例に係るロボットの干渉チェック方法の構成を表すブロック図である。尚、前出の図を用いて説明した同一の構成要素については、同一の符号を付して説明を割愛する。図７において、７１は第１の近似立体サイズ調整手段である。図５を用いて説明した第２の実施の形態との違いは、干渉予測手段１４に加わる入力の１つが、近似立体拡張サイズ９４から近似立体サイズ調整手段７１の出力に替わっている点である。第１の近似立体サイズ調整手段７１は、近似立体サイズ９３と近似立体間の距離（ｔ）９０６を入力し、近似立体間の距離（ｔ）９０６に応じて近似立体のサイズを決定し、近似立体拡張サイズ９４'として出力する。
【００３４】
第１の近似立体サイズ調整手段７１の具体的内容を、図８を用いて説明する。図８は、本発明の第１の近似立体サイズ調整手段の機能を説明する図である。尚、前出の図を用いて説明した同一の構成要素については、同一の符号を付して説明を割愛する。図８において、６０１は距離判定値、６０２は距離の近似立体拡張直線である。近似立体間の距離（ｔ）９０６が距離判定値６０１より小さくなった場合、距離の近似立体拡張直線６０２に従い、近似立体拡張サイズ９４'を大きくする。これにより、近似立体間の距離（ｔ）９０６が小さくなるに従って、近似立体拡張サイズ９４'（近似立体サイズ９３より大きい）が大きくなって行く。
以上説明したように、本発明の第２の実施形態の変形例（請求項４記載の例）は、近似立体間の距離（ｔ）９０６に応じて近似立体拡張サイズ９４'を決定する第１の近似立体サイズ調整手段７１を設けて、近似立体が接近するに従って近似立体拡張サイズ９４'を大きくするようにするので、第１の実施形態の干渉の可能性を低減させ、より確実にロボットや工具の衝突を防止することができるという効果がある。
尚、本第１の変形例は、近似立体拡張サイズ９４'を決定する方法として、距離の近似立体拡張直線６０２を用いたが、本発明はこれに限るものではなく、曲線を用いて近似立体拡張サイズ９４'を決定するようにしても良い。
【００３５】
本発明の第２の実施の形態の第２の変形例としてのロボットの干渉チェック方法について、図９および図１０を用いて説明する。本実施形態の第２の変形例は請求項５記載の内容に相当するもので、図９は本第２の変形例に係るロボットの干渉チェック方法の構成を表すブロック図である。尚、前出の図を用いて説明した同一の構成要素については、同一の符号を付して説明を割愛する。図９において、３１は速度演算手段、７１'は第２の近似立体サイズ調整手段である。本第２の変形例と第２の実施の形態（図５）との違いは、干渉予測手段１４に加わる入力の１つが、近似立体拡張サイズ９４から第２の近似立体サイズ調整手段７１'の出力に替わっている点、および速度演算手段３１が加わっている点である。速度演算手段３１は、位置記憶手段１１からの時刻ｔ−ΔＴでの第１の近似立体位置（ｔ−ΔＴ）１０４と第２の第２の近似立体位置（ｔ−ΔＴ）１０５と、時刻ｔでの第１の近似立体位置（ｔ）９０４と第２の近似立体位置（ｔ）９０５と、から制御周期ΔＴを用いて、第１の近似立体速度（ｔ）３０３と第２の近似立体速度（ｔ）３０４を演算し、第２の近似立体サイズ調整手段７１'へ出力する。
【００３６】
第２の近似立体サイズ調整手段７１'は、第１の近似立体速度（ｔ）３０３、第２の近似立体速度（ｔ）３０４を入力し、近似立体の速度（第１の近似立体速度（ｔ）３０３または第２の近似立体速度（ｔ）３０４）に応じて近似立体のサイズを決定し、近似立体拡張サイズ９４"として出力する。
【００３７】
第２の近似立体サイズ調整手段７１'の具体的内容を、図１０を用いて説明する。図１０は、本発明の第２の近似立体サイズ調整手段の機能を説明する図である。尚、前出の図を用いて説明した同一の構成要素については、同一の符号を付して説明を割愛する。図１０において、２０１は速度判定値、２０２は速度の近似立体拡張直線である。近似立体の速度（第１の近似立体速度（ｔ）３０３または第２の近似立体速度（ｔ）３０４）が速度判定値２０１より大きくなった場合、速度の近似立体拡張直線２０２に従い、近似立体拡張サイズ９４"を大きくする。これにより、近似立体の速度（第１の近似立体速度（ｔ）３０３または第２の近似立体速度（ｔ）３０４）が大きくなるに従って、近似立体拡張サイズ９４"（近似立体サイズ９３より大きい）が大きくなって行く。以上説明したように、本発明の第２の実施の形態の第２変形例（請求項５記載の例）は、近似立体の速度（第１の近似立体速度（ｔ）３０３または第２の近似立体速度（ｔ）３０４）に応じて近似立体拡張サイズ９４"を決定する第２の近似立体サイズ調整手段７１'を設けて、近似立体が高速で接近するに従って近似立体拡張サイズ９４"を大きくするようにするので、第１の実施形態の干渉の可能性を低減させ、より確実にロボットや工具の衝突を防止することができるという効果がある。尚、本第２の変形例は、近似立体拡張サイズ９４"を決定する方法として、速度の近似立体拡張直線２０２を用いたが、本発明はこれに限るものではなく、曲線を用いて近似立体拡張サイズ９４"を決定するようにしても良い。
【００３８】
本発明の第２の実施の形態の第３の変形例としてのロボットの干渉チェック方法について、図１１を用いて説明する。本第３の変形例は請求項６記載の内容に相当する。図１１は本第３の変形例に係るロボットの干渉チェック方法の構成を表すブロック図である。尚、前出の図を用いて説明した同一の構成要素については、同一の符号を付して説明を割愛する。図５を用いて説明した第２の実施の形態との違いは、干渉予測判定手段２１と距離閾値ファイル２２とが加わっている点である。
【００３９】
距離閾値ファイル２２の値は、近似立体拡張サイズ９４より大きめの値を設定しておく。干渉予測判定手段２１は、時刻ｔでの近似立体間の距離（ｔ）９０６と、予め設定されている距離閾値２２とを入力し、それらを比較し、干渉予測手段１４の演算の実行を決定する。例えば、近似立体が２つの球であれば、近似立体間の距離（ｔ）９０６である球間の距離ＬSSと、距離閾値２２のＬth（＞ＲSA＋ＲSB）とから式（３）の判定を行い、式（３）を満たす場合、干渉予測手段１４の演算を実行し、満たさない場合、干渉予測手段１４の演算を実行しない。演算を実行する場合には、近似立体間の距離（ｔ−ΔＴ）１０６と、近似立体間の距離（ｔ）９０６と、第１の近似立体位置（ｔ−ΔＴ）１０４と、第２の近似立体位置（ｔ−ΔＴ）１０５と、第１の近似立体位置（ｔ）９０４と、第２の近似立体位置（ｔ）９０５と、近似立体拡張サイズ９４とを干渉予測手段１４に出力する。以上説明したように、本第３の変形例（請求項６記載の例）は、第２の実施の形態と同様、干渉予測手段１４を設けて、近似立体サイズ９３より大きめの近似立体拡張サイズ９４に基づいて干渉チェックを行うので、干渉を判定する距離が長くなり、第２の実施形態の干渉の可能性を低減させ、より確実にロボットや工具の衝突を防止することができるという効果がある。
【００４０】
また、干渉予測判定手段２１を設けて、近似立体拡張サイズ９４より大きめの距離閾値２２に基づいて干渉予測手段１４の演算の実行を決定することができるので、常時干渉予測手段１４の演算を実行する必要がなく、干渉チェックに要する演算が高速に行えるという効果がある。尚、本実施の形態は、図５を用いて説明した第２の実施の形態の干渉予測手段１４の前段に干渉予測判定手段２１と距離閾値２２とを設けるようにしたが、本発明はこれに限るものではなく、図７、図９を用いて説明した第２の実施の形態の第１、第２の変形例の干渉予測手段１４の前段に干渉予測判定手段２１と距離閾値２２とを設けるようにしても良い。以上説明した全ての実施の形態は、ロボットや工具の衝突を防止することができるので、ロボットや工具の破壊の危険性をなくし、それに伴う人に対する安全性を向上させることができるという効果がある。
【００４１】
【発明の効果】
以上説明したように、本発明の請求項１記載のロボットの干渉チェック方法によれば、次の時刻での近似立体の位置を推定し、次の時刻までの干渉の有無が予測できる干渉予測手段を設けているので、従来の干渉チェック方法に合わせ、近似立体間の干渉が確実にチェックでき、ロボットや工具の衝突を防止することができるという効果がある。本発明の請求項２記載のロボットの干渉チェック方法によれば、請求項１記載のロボットの干渉チェック方法と同様、干渉予測手段を設けているので、従来の干渉チェック方法に合わせ、近似立体間の干渉が確実にチェックでき、ロボットや工具の衝突を防止することができるという効果がある。また、干渉予測判定手段を設けて、近似立体サイズより大きめの距離閾値に基づいて干渉予測手段の演算の実行を決定することができるので、常時干渉予測手段の演算を実行する必要がなく、干渉チェックに要する演算が高速に行えるという効果がある。
【００４２】
また、本発明の請求項３記載のロボットの干渉チェック方法によれば、近似立体拡張サイズを利用して干渉の有無を判定する干渉予測手段を設けて、近似立体サイズより大きめの近似立体拡張サイズに基づいて干渉チェックを行うので、干渉を判定する距離が長くなり、請求項１記載の干渉の可能性を低減させ、より確実にロボットや工具の衝突を防止することができるという効果がある。
本発明の請求項４記載のロボットの干渉チェック方法によれば、近似立体間の距離に応じて近似立体拡張サイズを決定する近似立体サイズ調整手段を設けて、近似立体が接近するに従って近似立体拡張サイズを大きくするようにするので、請求項１記載の干渉の可能性を低減させ、より確実にロボットや工具の衝突を防止することができるという効果がある。
本発明の請求項５記載のロボットの干渉チェック方法によれば、近似立体の速度に応じて近似立体拡張サイズを決定する近似立体サイズ調整手段を設けて、近似立体が高速で接近するに従って近似立体拡張サイズを大きくするようにするので、請求項１記載の干渉の可能性を低減させ、より確実にロボットや工具の衝突を防止することができるという効果がある。
本発明の請求項６記載のロボットの干渉チェック方法によれば、請求項３〜５のいずれか１項記載のロボットの干渉チェック方法と同様、干渉予測手段を設けて、近似立体サイズより大きめの近似立体拡張サイズに基づいて干渉チェックを行うので、干渉を判定する距離が長くなり、請求項１記載の干渉の可能性を低減させ、より確実にロボットや工具の衝突を防止することができるという効果がある。また、干渉予測判定手段を設けて、近似立体サイズより大きめの距離閾値に基づいて干渉予測手段の演算の実行を決定することができるので、常時干渉予測手段の演算を実行する必要がなく、干渉チェックに要する演算が高速に行えるという効果がある。
本発明の全ての請求項に記載のロボットの干渉チェック方法によれば、ロボットや工具の衝突を防止することができるので、ロボットや工具の破壊の危険性をなくし、それに伴う人に対する安全性を向上させることができるという効果がある。
【図面の簡単な説明】
【図１】本発明の第１の実施の形態に関わるロボットの干渉チェック方法の構成を表すブロック図である。
【図２】本発明の第１の実施の形態に関わる干渉予測手段の構成を表すブロック図である。
【図３】本発明の干渉予測手段の機能を説明する図である。
【図４】本発明の第１の実施の形態の変形例に関わるロボットの干渉チェック方法の構成を表すブロック図である。
【図５】本発明の第２の実施の形態に関わるロボットの干渉チェック方法の構成を表すブロック図である。
【図６】本発明の第２の実施の形態に関わる干渉予測手段の構成を表すブロック図である。
【図７】本発明の第２の実施の形態の変形例に関わるロボットの干渉チェック方法の構成を表すブロック図である。
【図８】本発明の第１の近似立体サイズ調整手段の機能を説明する図である。
【図９】本発明の第２の実施の形態の第２の変形例に関わるロボットの干渉チェック方法の構成を表すブロック図である。
【図１０】本発明の第２の近似立体サイズ調整手段の機能を説明する図である。
【図１１】本発明の第２の実施の形態の第３の変形例に関わるロボットの干渉チェック方法の構成を表すブロック図である。
【図１２】従来のロボットの干渉チェック方法の構成を表すブロック図である。
【図１３】近似立体が干渉するケースを表す図である。
【符号の説明】
４ 第１の球
５ 第２の球
６ 第１の円筒
７ 第２の円筒
１１ 位置記憶手段
１２ 距離記憶手段
１３ 干渉予測手段
１４、１４' 干渉予測手段
２１ 干渉予測判定手段
２２ 距離閾値ファイル（その値）
３１ 速度演算手段
５１ 距離推定手段
５２ 距離近似手段
５３ 最小値取得手段
５４、５４' 干渉推定手段
７１ 第１の近似立体サイズ調整手段
７１'第２の近似立体サイズ調整手段
９１ 近似立体間距離演算手段
９２ 干渉判定手段
９３ 近似立体サイズファイル（その値）
９４，９４'、９４" 近似立体拡張サイズファイル（その値）
１０４ 第１の近似立体位置（ｔ−ΔＴ）
１０５ 第２の近似立体位置（ｔ−ΔＴ）
１０６ 近似立体間の距離（ｔ−ΔＴ）
２０１ 速度判定値
２０２ 速度の近似立体拡張直線
３０３ 第１の近似立体速度（ｔ）
３０４ 第２の近似立体速度（ｔ）
５０５ 近似立体間の距離（ｔ＋ΔＴ）
５０６ 近似曲線
５０７ 距離最小値
６０１ 距離判定値
６０２ 距離の近似立体拡張直線
７０１ 第１の近似立体位置（ｔ＋ΔＴ）
７０２ 第２の近似立体位置（ｔ＋ΔＴ）
８１０ 干渉の有無信号
９０４ 第１の近似立体位置（ｔ）
９０５ 第２の近似立体位置（ｔ）
９０６ 近似立体間の距離（ｔ）
【発明の属する技術分野】
本発明は、複数のロボット間の干渉をチェックする方法、特に、好適に干渉をチェックするロボットの干渉チェック方法に関する。
【０００２】
【従来の技術】
少なくとも２台以上のロボットが同時に作業することを想定し、これらロボットの衝突を防ぐために、ロボット同士の干渉をチェックする方法がいくつか開示されている。例えば、特開昭６０−２１７４１０号公報に開示の「協調作業教示におけるロボット干渉検出方式」では、ロボットの各部を円筒と半球の近似立体で覆い、これら近似立体間の干渉をチェックすることでロボット同士の干渉を検出している。そして、ロボットの干渉チェックはロボット各部に設定した近似立体間の距離を演算し、それと近似立体のサイズとの比較により干渉の有無を判定することで行われている。
【０００３】
図１２に従来のロボットの干渉チェック方法の構成を表すブロック図を示す。図１２において、９１は近似立体間距離演算手段で、ある時刻ｔでの第１の近似立体位置（ｔ）９０４と第２の近似立体位置（ｔ）９０５とを入力し、それら近似立体間の距離を演算し、近似立体間の距離（ｔ）９０６として出力する。第１の近似立体位置（ｔ）９０４または第２の近似立体位置（ｔ）９０５は、例えば近似立体が半球であれば半球の中心位置、円筒であれば円筒の両端点位置である。また、第１の近似立体位置（ｔ）９０４が一方のロボットに対するもの、第２の近似立体位置（ｔ）９０５が他方のロボットに対するものと想定している。
【０００４】
９２は干渉判定手段で、近似立体間の距離（ｔ）９０６と予め近似立体サイズファイル９３に設定されている近似立体サイズ９３とを入力し、それらを比較し、干渉の有無信号８１０を出力する。例えば、近似立体が２つの球であれば、近似立体間の距離（ｔ）９０６である球間の距離ＬSSと、近似立体サイズ９３である一方の球半径ＲSAと他方の球半径ＲSBとから式（１）の判定を行い
ＬSS≦ＲSA＋ＲSB・・・・・・（１）
式（１）を満たす場合干渉有り、満たさない場合干渉無しとする。ところで、図１２を用いて説明した従来のロボットの干渉チェック方法は、ロボットの制御周期ΔＴ毎に演算が行われる。つまり、ある時刻ｔで干渉チェックの演算を行った後、時刻ｔ＋ΔＴの時、次の干渉チェックの演算を行う。
【０００５】
【発明が解決しようとする課題】
ところが、従来のロボットの干渉チェック方法では、衝突を回避できないケースが存在する。図１３は近似立体が干渉するケースを表す図を示している。図１３において、（ａ）は球と球の干渉のケース、（ｂ）は円筒と球の干渉のケース、（ｃ）は円筒と円筒の干渉のケースである。尚、従来の技術では円筒と半球を扱ったが、理解をし易くするためここでは半球を球として扱う。球と球の干渉ケース（ａ）では、第１の球４が固定で、第２の球５（図で左）が制御周期ΔＴの間に矢印の方向に球５（図で右）まで移動するとする。この時、時刻ｔの時に第１の球４と第２の球５は干渉していない。また、次の時刻ｔ＋ΔＴの時に第１の球４と第２の球５は干渉していない。ところが、制御周期ΔＴの間、第２の球５の移動に伴い、第２の球５は、第１の球４と干渉してしまう。
【０００６】
円筒と球の干渉ケース（ｂ）では、第１の円筒６が固定で、第２の球５が制御周期ΔＴの間に矢印の方向に移動するとする。この時、時刻ｔの時に第１の円筒６と第２の球５は干渉していない。また、次の時刻ｔ＋ΔＴの時に第１の円筒６と第２の球５は干渉していない。ところが、制御周期ΔＴの間、第２の球５の移動に伴い、第２の球５は、第１の円筒６と干渉してしまう。
【０００７】
円筒と円筒の干渉ケース（ｃ）では、第１の円筒６が固定で、第２の円筒７が制御周期ΔＴの間に矢印の方向に移動するとする。この時、時刻ｔの時に第１の円筒６と第２の円筒７は干渉していない。また、次の時刻ｔ＋ΔＴの時に第１の円筒６と第２の円筒７は干渉していない。ところが、制御周期ΔＴの間、第２の円筒７の移動に伴い、第２の円筒７は、第１の円筒６と干渉してしまう。これらの現象は、ロボットの動作速度が極めて高速で、ロボットに設定された近似立体の、制御周期ΔＴの間に移動する距離が大きくなる場合に、制御周期ΔＴの間、図１２に示すような干渉チェックの演算が行えないことに起因する。近似立体が干渉してしまうと、近似立体が設定されたロボットが衝突してしまう危険性が高い。そのため、従来の技術では、ロボットが衝突し、ロボットや工具を破壊する危険性が生じるという問題があった。また、ロボットや工具の破壊による飛散物のため、人に対する安全性が低下するという問題があった。
【０００８】
そこで、本発明の第１の実施形態では、ロボットや工具の干渉を避けることによりそれらを衝突させないようにし、結果としてロボットや工具の破壊の危険性をなくし、人に対する安全性を高めたロボットの干渉チェック方法を提供することを第１の目的とする。
【０００９】
また、本発明の第２の実施形態では、本発明の第１の実施形態では防ぎにくかった課題をさらに解決することにある。例えば、第１の実施形態における干渉予測手段内の、距離推定手段で演算する時刻ｔ＋ΔＴでの第１の近似立体位置または第２の近似立体位置はあくまで推定値であること、距離近似手段で演算する近似曲線は、あくまで近似値であることから、必ずしも完全に実際の状況を反映しているとは限らないので、これら推定値や近似値が要因となって、近似立体が干渉してしまう場合があったり、また、２台以上のロボットの制御周期の同期が取れず、または、同期がずれた場合、一方のロボットの位置と他方のロボットの位置の双方の払い出しのタイミングがずれ、近似立体が干渉してしまう場合もあった。近似立体が干渉してしまうと、近似立体が設定されたロボットが衝突してしまう危険性が高い。そのため、第１実施形態では、ロボットが衝突してロボットや工具を損傷する怖れが若干あった。
そこで、本発明の第２の実施形態では、第１実施形態のロボットの干渉チェック方法の機能を向上させ、ロボットや工具の干渉を確実に避けることにより、それらを衝突させない、結果として、ロボットや工具の破壊の危険性をなくした、人に対する安全性を高めたロボットの干渉チェック方法を提供することを第２の目的とする。
【００１０】
【課題を解決するための手段】
上記問題を解決するため、請求項１記載のロボットの干渉チェック方法は、複数のロボットの各部の形状を覆って近似する近似立体を予め設定し、動作中の前記複数のロボットにおける一方の前記ロボットに設定した前記近似立体と、他方の前記ロボットに設定した前記近似立体との距離を所定の制御周期ごとに演算する近似立体間距離演算手段と、前記近似立体のサイズと前記近似立体間距離演算手段で演算された距離とを比較する干渉判定手段と、を有して成るロボットの干渉をチェックする方法において、前記近似立体間距離演算手段で演算された距離のうち少なくとも現在から制御周期１回分遡った時刻（以降、直前の時刻）での距離を記憶する距離記憶手段と、前記制御周期ごとに取得される前記近似立体の位置のうち少なくとも直前の時刻での位置を記憶する位置記憶手段と、干渉予測手段とを有し、前記干渉予測手段は、直前の時刻での前記近似立体の位置と現在の時刻での位置との差分を現在の時刻での位置に加えることで、現在から制御周期１回分経った時刻（以降、次の時刻）での前記近似立体の位置を推定し、次の時刻での前記近似立体間の距離を演算する距離推定手段と、前記距離記憶手段に記憶されている直前の時刻での前記近似立体間の距離と現在の時刻での距離と前記距離推定手段で演算された次の時刻での距離とから、前記近似立体間の距離の時間変化を表す２次以上の近似曲線を演算する距離近似手段と、現在の時刻から次の時刻までの間における前記近似曲線の最小値を求める最小値取得手段と、前記近似曲線の最小値と前記近似立体のサイズとを比較することにより干渉を判定する干渉推定手段と、から構成されることを特徴とする。
【００１１】
請求項１記載のロボットの干渉チェック方法によれば、次の時刻での近似立体の位置を推定し、次の時刻までの干渉の有無が予測できる干渉予測手段を設けているので、従来の干渉チェック方法に合わせ、近似立体間の干渉が確実にチェックでき、ロボットや工具の衝突を防止することができるという効果がある。
【００１２】
また、請求項２記載の発明は、請求項１記載のロボットの干渉チェック方法において、前記干渉予測手段の前段に干渉予測判定手段を設け、前記干渉予測判定手段は、前記近似立体のサイズより大きく設定された距離閾値と前記近似立体間距離演算手段が演算した現在の時刻での前記近似立体間の距離とを比較し、前記干渉予測手段は、前記距離が前記距離閾値以下の場合のみ演算を行うことを特徴とする。
請求項２記載のロボットの干渉チェック方法によれば、請求項１記載のロボットの干渉チェック方法と同様、干渉予測手段を設けているので、従来の干渉チェック方法に合わせ、近似立体間の干渉が確実にチェックでき、ロボットや工具の衝突を防止することができるという効果がある。また、干渉予測判定手段を設けて、近似立体サイズより大きめの距離閾値に基づいて干渉予測手段の演算の実行を決定することができるので、常時干渉予測手段の演算を実行する必要がなく、干渉チェックに要する演算が高速に行えるという効果がある。
【００１３】
また、本発明の請求項３記載の発明は、請求項１記載のロボットの干渉チェック方法において、前記干渉予測手段への入力として前記近似立体のサイズより大きい近似立体拡張サイズを予め設定し、前記干渉推定手段は、前記近似立体のサイズに代えて前記近似曲線の最小値と前記近似立体拡張サイズとを比較することにより干渉を判定することを特徴とする。
請求項３記載のロボットの干渉チェック方法によれば、近似立体拡張サイズを利用して干渉の有無を判定する干渉予測手段を設けて、近似立体サイズより大きめの近似立体拡張サイズに基づいて干渉チェックを行うので、干渉を判定する距離が長くなり、請求項１記載の干渉の可能性を低減させ、より確実にロボットや工具の衝突を防止することができるという効果がある。
【００１４】
また、本発明の請求項４記載の発明は、請求項１記載のロボットの干渉チェック方法において、前記近似立体のサイズを拡張して前記干渉予測手段への入力とする近似立体サイズ調整手段を設け、前記近似立体サイズ調整手段は、現在の時刻での前記近似立体間の距離が所定の判定値より小さい場合には前記距離が小さくなるに従って前記近似立体のサイズを拡張した近似立体拡張サイズを出力し、
前記干渉推定手段は、前記近似立体のサイズに代えて前記近似曲線の最小値と前記近似立体拡張サイズとを比較することにより干渉を判定することを特徴とする。
請求項４記載のロボットの干渉チェック方法によれば、近似立体間の距離に応じて近似立体拡張サイズを決定する近似立体サイズ調整手段を設けて、近似立体が接近するに従って近似立体拡張サイズを大きくするようにするので、請求項１記載の干渉の可能性を低減させ、より確実にロボットや工具の衝突を防止することができるという効果がある。
【００１５】
また、本発明の請求項５記載の発明は、請求項１記載のロボットの干渉チェック方法において、前記位置記憶手段に記憶されている直前の時刻での前記近似立体の位置と現在の時刻での位置とから前記近似立体の速度を演算する速度演算手段と、前記近似立体のサイズを拡張して前記干渉予測手段への入力とする近似立体サイズ調整手段を設け、前記近似立体サイズ調整手段は、前記近似立体の速度が所定の判定値より大きい場合には前記速度が大きくなるに従って前記近似立体のサイズを拡張した近似立体拡張サイズを出力し、前記干渉推定手段は、前記近似立体のサイズに代えて前記近似曲線の最小値と前記近似立体拡張サイズとを比較することにより干渉を判定することを特徴とする。
請求項５記載のロボットの干渉チェック方法によれば、近似立体の速度に応じて近似立体拡張サイズを決定する近似立体サイズ調整手段を設けて、近似立体が高速で接近するに従って近似立体拡張サイズを大きくするようにするので、従来の干渉の可能性を低減させ、より確実にロボットや工具の衝突を防止することができるという効果がある。
【００１６】
また、本発明の請求項６記載の発明は、請求項３〜５のいずれか１項記載のロボットの干渉チェック方法において、前記干渉予測手段の前段に干渉予測判定手段を設け、前記干渉予測判定手段は、前記近似立体拡張サイズより大きく設定された距離閾値と前記近似立体間距離演算手段が演算した現在の時刻での前記近似立体間の距離とを比較し、前記干渉予測手段は、前記距離が前記距離閾値以下の場合のみ演算を行うことを特徴とする。
請求項６記載のロボットの干渉チェック方法によれば、請求項３〜５のいずれか１項記載のロボットの干渉チェック方法と同様、干渉予測手段を設けて、近似立体サイズより大きめの近似立体拡張サイズに基づいて干渉チェックを行うので、干渉を判定する距離が長くなり、従来の干渉の可能性を低減させ、より確実にロボットや工具の衝突を防止することができるという効果がある。また、干渉予測判定手段を設けて、近似立体拡張サイズより大きめの距離閾値に基づいて干渉予測手段の演算の実行を決定することができるので、常時干渉予測手段の演算を実行する必要がなく、干渉チェックに要する演算が高速に行えるという効果がある。
【００１７】
以上、全ての請求項記載のロボットの干渉チェック方法によれば、ロボットや工具の衝突を防止することができるので、ロボットや工具の破壊の危険性をなくし、それに伴う人に対する安全性を向上させることができるという効果がある。
【００１８】
【発明の実施の形態】
本発明の第１の実施の形態のロボットの干渉チェック方法について、図１〜図３を用いて説明する。本実施の形態は、請求項１記載の内容に相当する。図１は本発明の第１の実施の形態に関わるロボットの干渉チェック方法の構成を表すブロック図である。尚、図１２で説明した構成要素のうち同一の構成要素については同一の符号を付して説明を割愛する。図１において、１１は位置記憶手段で、制御周期ΔＴ分、１つ前の時刻ｔ−ΔＴでの第１の近似立体位置（ｔ−ΔＴ）１０４と第２の近似立体位置（ｔ−ΔＴ）１０５とを記憶している。１２は距離記憶手段で、制御周期ΔＴ分、１つ前の時刻ｔ−ΔＴでの近似立体間の距離（ｔ−ΔＴ）１０６を記憶している。１３は干渉予測手段で、近似立体間の距離（ｔ−ΔＴ）１０６と、近似立体間の距離（ｔ）９０６と、第１の近似立体位置（ｔ−ΔＴ）１０４と、第２の近似立体位置（ｔ−ΔＴ）１０５と、第１の近似立体位置（ｔ）９０４と、第２の近似立体位置（ｔ）９０５と、近似立体サイズ９３とを入力し、制御周期ΔＴの間の干渉の有無を予測し、干渉の有無信号８１０を出力する。
【００１９】
干渉予測手段１３の具体的内容を、図２および図３を用いて説明する。図２は本発明の干渉予測手段の構成を表すブロック図である。尚、前出の図を用いて説明した同一構成要素については、同一の符号を付して説明を割愛する。図２において、５１は距離推定手段で、第１の近似立体位置（ｔ−ΔＴ）１０４と、第２の近似立体位置（ｔ−ΔＴ）１０５と、第１の近似立体位置（ｔ）９０４と、第２の近似立体位置（ｔ）９０５とを入力し、時刻ｔ＋ΔＴでの近似立体間の距離を推定し、近似立体間の距離（ｔ＋ΔＴ）５０５として出力する。５２は距離近似手段で、近似立体間の距離（ｔ−ΔＴ）１０６と、近似立体間の距離（ｔ）９０６と、近似立体間の距離（ｔ＋ΔＴ）５０５とを入力し、距離と時間との関係から近似曲線を作成し、近似曲線５０６として出力する。５３は最小値取得手段で、近似曲線５０６を入力し、近似曲線５０６の最小値を求め、距離最小値５０７として出力する。
【００２０】
５４は干渉推定手段で、距離最小値５０７と、近似立体サイズ９３とを入力し、それらを比較し、干渉の有無信号８１０を出力する。例えば、近似立体が２つの球であれば、距離最小値５０７をＬSSMINとし、近似立体サイズ９３である一方の球半径ＲSAと他方の球半径ＲSBとから式（２）の判定を行い、
ＬSSMIN≦ＲSA＋ＲSB・・・・・（２）
式（２）を満たす場合干渉有り、満たさない場合干渉無しとする。次に、これらの距離推定手段５１、距離近似手段５２、最小値取得手段５３、干渉推定手段５４の各機能を、図３を用いて詳しく説明する。
【００２１】
図３は、本発明の干渉予測手段の機能を説明する図である。尚、前出の図を用いて説明した同一の構成要素については、同一の符号を付して説明を割愛する。ここでは、図１３（ａ）に示した球と球の干渉ケースを扱う。図３（ａ）において、図１３（ａ）と同様、第１の球４は固定で、第２の球５が移動するとする。従って、第１の近似立体位置（ｔ−ΔＴ）１０４と、第１の近似立体位置（ｔ）９０４と、第１の近似立体位置（ｔ＋ΔＴ）７０１は、全て同じ位置となる。図３（ａ）に示すように、今、時刻ｔ−ΔＴから時刻ｔへの経過に伴い、第１の球４と第２の球５とが接近してきている場合を考える。
【００２２】
距離推定手段５１（図２）は、まず、時刻ｔ−ΔＴの第２の近似立体位置（ｔ−ΔＴ）１０５と、時刻ｔの第２の近似立体位置（ｔ）９０５とから、時刻ｔ＋ΔＴの第２の近似立体位置（ｔ＋ΔＴ）７０２を推定する。この推定は、例えば、第２の近似立体位置（ｔ−ΔＴ）１０５と第２の近似立体位置（ｔ）９０５を直線で結び、その延長線上、同じ移動量の位置とするような方法を用いれば良い。一般に、制御周期ΔＴ間で移動する距離は、全体の移動距離と比較して微小であるため、第２の近似立体位置（ｔ＋ΔＴ）７０２は、直線上にあると見なして問題ない。また、距離推定手段５１は、次に、推定された第２の近似立体位置（ｔ＋ΔＴ）７０２と、第１の近似立体位置（ｔ＋ΔＴ）７０１との距離を演算し、近似立体間の距離（ｔ＋ΔＴ）５０５として出力する。
【００２３】
距離近似手段５２（図２）は、まず、図３（ｂ）のような距離−時間グラフを作成する。図３（ｂ）の距離−時間グラフは、近似立体間の距離（ｔ−ΔＴ）１０６と、近似立体間の距離（ｔ）９０６と、近似立体間の距離（ｔ＋ΔＴ）５０５を時刻に応じてプロットしたものである。距離近似手段５２は、次に、これらプロットしたデータを２次以上の多項式で近似し、近似曲線５０６として出力する。図３（ｂ）の距離−時間グラフでは、２次の多項式で近似している。近似には、例えば、最小自乗法を用いれば良い。
【００２４】
最小値取得手段５３（図２）は、近似曲線５０６（具体的には近似曲線の係数）を入力し、近似曲線５０６の最小値を求め、距離最小値５０７として出力する。
【００２５】
干渉推定手段５４（図２）は、距離最小値５０７と近似立体サイズ９３とを入力し、例えば式（２）を用いて干渉の有無信号８１０を判定し、出力する。図３（ｂ）の距離−時間グラフでは、式（２）を満たす場合、すなわち、干渉有りの場合を示している。以上説明したように、本発明の第１の実施の形態（請求項１記載の例）は、時刻ｔ＋ΔＴでの近似立体位置（ｔ＋ΔＴ）７０１及び７０２を推定し、時刻ｔから時刻ｔ＋ΔＴまでの干渉の有無信号８１０が予測できる干渉予測手段１３を設けているので、従来の干渉判定手段９２による干渉チェックに合わせ、近似立体間の干渉が確実にチェックでき、ロボットや工具の衝突を防止することができるという効果がある。
【００２６】
尚、本実施の形態は、球と球の干渉ケース図３（ａ）についてのみ説明したが、図１３で示したように球と球の干渉ケース（図１３（ａ））以外にも、円筒と球の干渉ケース（図１３（ｂ））や円筒と円筒の干渉ケース（図１３（ｃ））についても、球と球の干渉ケース図３（ａ）と同様な方法を適用すればロボットの干渉チェックが実行できる。
【００２７】
また、本実施の形態は、第２の近似立体位置（ｔ＋ΔＴ）７０２を、第２の近似立体位置（ｔ−ΔＴ）１０５と第２の近似立体位置（ｔ）９０５とを結ぶ直線上にあるとしたが、本発明はこれに限るものではなく、予め決められた次数を持つ曲線上にあるとしても良い。また、本実施の形態は、近似立体位置を時刻ｔ−ΔＴ、時刻ｔ、時刻ｔ＋ΔＴに対応する３点のみを対象としたが、本発明はこれに限るものではなく、４点以上を対象として、干渉の予測性能を向上させても良い。
【００２８】
また、本実施の形態は、距離近似手段５２（図２）での近似曲線５０６を２次の多項式としたが、本発明これに限るものではなく、３次以上の多項式として、近似性能を向上させても良い。また、本実施の形態は、一方の近似立体（第１の球４）の位置を固定していたが、本発明はこれに限るものではなく、２つの近似立体が移動していても同様な方法でロボットの干渉チェックが実行できる。
【００２９】
本発明の第１の実施の形態の変形例としてのロボットの干渉チェック方法について、図４を用いて説明する。本変形例は請求項２記載の内容に相当するもので、図４は本変形例に係るロボットの干渉チェック方法の構成を表すブロック図である。尚、前出の図を用いて説明した同一の構成要素については、同一の符号を付して説明を割愛する。図４において、２１は干渉予測判定手段、２２は距離閾値ファイルである。図１を用いて説明した第１の実施の形態との違いは、新たに干渉予測判定手段２１と距離閾値ファイル２２とが加わっている点である。距離閾値ファイルの距離閾値２２は、近似立体サイズ９３より大きめの値を設定しておく。干渉予測判定手段２１は、時刻ｔでの近似立体間の距離（ｔ）９０６と、予め設定されている距離閾値２２とを入力し、それらを比較し、干渉予測手段１３の演算の実行を決定する。例えば、近似立体が２つの球であれば、近似立体間の距離（ｔ）９０６である球間の距離ＬSSと、距離閾値２２のＬth（＞ＲSA＋ＲSB）とから式（３）の判定を行い、
ＬSS≦Ｌth・・・・・（３）
式（３）を満たす場合、干渉予測手段１３の演算を実行し、満たさない場合、干渉予測手段１３の演算を実行しない。
【００３０】
演算を実行する場合には、近似立体間の距離（ｔ−ΔＴ）１０６と、近似立体間の距離（ｔ）９０６と、第１の近似立体位置（ｔ−ΔＴ）１０４と、第２の近似立体位置（ｔ−ΔＴ）１０５と、第１の近似立体位置（ｔ）９０４と、第２の近似立体位置（ｔ）９０５と、近似立体サイズ９３とを干渉予測手段１３に出力する。干渉予測手段１３の機能は、図１及び図２を用いて説明したものと全く同様である。以上説明したように、本発明の第１の実施形態の変形例（請求項２記載の例）は、第１の実施の形態と同様、干渉予測手段１３を設けているので、従来の干渉判定手段９２による干渉チェックに合わせ、近似立体間の干渉が確実にチェックでき、ロボットや工具の衝突を防止することができるという効果がある。また、干渉予測判定手段２１を設けて、近似立体サイズ９３より大きめの距離閾値２２に基づいて干渉予測手段１３の演算の実行を決定することができるので、常時干渉予測手段１３の演算を実行する必要がなく、干渉チェックに要する演算が高速に行えるという効果がある。
【００３１】
次に、本発明の第２の実施の形態のロボットの干渉チェック方法について、図５および図６を用いて説明する。本実施の形態は、請求項３記載の内容に相当する。図５は、本発明の第２の実施の形態に関わるロボットの干渉チェック方法の構成を表すブロック図である。尚、前出の図を用いて説明した同一の構成要素については、同一の符号を付して説明を割愛する。図５において、１４は干渉予測手段、９４は近似立体拡張サイズファイルである。図１を用いて説明した第１の実施形態の干渉チェック方法との違いは、第１の実施形態の干渉予測手段１３が第２の実施形態の干渉予測手段１４に替わっている点、近似立体拡張サイズファイル９４が加わっている点である。近似立体拡張サイズ９４の値は、近似立体サイズ９３より大きめの値を予め設定しておく。干渉予測手段１４は、距離記憶手段１２からの近似立体間の距離（ｔ−ΔＴ）１０６と、近似立体間距離演算手段９１からの近似立体間の距離（ｔ）９０６と、位置記憶手段１１からの第１の近似立体位置（ｔ−ΔＴ）１０４と第２の近似立体位置（ｔ−ΔＴ）１０５と、第１の近似立体位置（ｔ）９０４と、第２の近似立体位置（ｔ）９０５と、近似立体拡張サイズ９４とを入力し、制御周期ΔＴの間の干渉の有無を予測し、干渉の有無信号８１０を出力する。
【００３２】
干渉予測手段１４の具体的内容を、図６を用いて説明する。図６は本発明の干渉予測手段の構成を表すブロック図である。尚、前出の図を用いて説明した同一の構成要素については、同一の符号を付して説明を割愛する。図６において５４'は干渉推定手段である。図２を用いて説明した第１の実施形態の干渉予測手段１３とこの干渉予測手段１４との違いは、第１の実施形態の干渉推定手段５４が本発明の干渉推定手段５４'に、そしてその入力となる近似立体サイズ９３（第１実施形態）がここでは近似立体拡張サイズ９４に、それぞれ替わっている点である。干渉推定手段５４'は、距離最小値５０７と近似立体拡張サイズ９４とを入力し、それらを比較し、干渉の有無信号８１０を出力する。例えば、近似立体が２つの球であれば、距離最小値５０７をＬSSMINとし、近似立体拡張サイズ９４である一方の球半径ＲSA'（＞ＲSA）と他方の球半径ＲSB'（＞ＲSB）とから式（４）の判定を行い、
ＬSSMIN≦ＲSA'＋ＲSB' ・・・・・（４）
式（４）を満たす場合干渉有り、満たさない場合干渉無しとする。以上説明したように、本発明の第２の実施の形態（請求項３記載の例）は、近似立体拡張サイズ９４を利用して干渉の有無を出力する干渉推定手段５４'を含む干渉予測手段１４を設けて、近似立体サイズ９３より大きめの近似立体拡張サイズ９４に基づいて干渉チェックを行うので、干渉を判定する距離が長くなり、第１の実施形態の干渉の可能性を低減させ、より確実にロボットや工具の衝突を防止することができるという効果がある。
【００３３】
さらに、本発明の第２の実施の形態の変形例としてのロボットの干渉チェック方法について、図７および図８を用いて説明する。本変形例は、請求項４記載の内容に相当するもので、図７は本変形例に係るロボットの干渉チェック方法の構成を表すブロック図である。尚、前出の図を用いて説明した同一の構成要素については、同一の符号を付して説明を割愛する。図７において、７１は第１の近似立体サイズ調整手段である。図５を用いて説明した第２の実施の形態との違いは、干渉予測手段１４に加わる入力の１つが、近似立体拡張サイズ９４から近似立体サイズ調整手段７１の出力に替わっている点である。第１の近似立体サイズ調整手段７１は、近似立体サイズ９３と近似立体間の距離（ｔ）９０６を入力し、近似立体間の距離（ｔ）９０６に応じて近似立体のサイズを決定し、近似立体拡張サイズ９４'として出力する。
【００３４】
第１の近似立体サイズ調整手段７１の具体的内容を、図８を用いて説明する。図８は、本発明の第１の近似立体サイズ調整手段の機能を説明する図である。尚、前出の図を用いて説明した同一の構成要素については、同一の符号を付して説明を割愛する。図８において、６０１は距離判定値、６０２は距離の近似立体拡張直線である。近似立体間の距離（ｔ）９０６が距離判定値６０１より小さくなった場合、距離の近似立体拡張直線６０２に従い、近似立体拡張サイズ９４'を大きくする。これにより、近似立体間の距離（ｔ）９０６が小さくなるに従って、近似立体拡張サイズ９４'（近似立体サイズ９３より大きい）が大きくなって行く。
以上説明したように、本発明の第２の実施形態の変形例（請求項４記載の例）は、近似立体間の距離（ｔ）９０６に応じて近似立体拡張サイズ９４'を決定する第１の近似立体サイズ調整手段７１を設けて、近似立体が接近するに従って近似立体拡張サイズ９４'を大きくするようにするので、第１の実施形態の干渉の可能性を低減させ、より確実にロボットや工具の衝突を防止することができるという効果がある。
尚、本第１の変形例は、近似立体拡張サイズ９４'を決定する方法として、距離の近似立体拡張直線６０２を用いたが、本発明はこれに限るものではなく、曲線を用いて近似立体拡張サイズ９４'を決定するようにしても良い。
【００３５】
本発明の第２の実施の形態の第２の変形例としてのロボットの干渉チェック方法について、図９および図１０を用いて説明する。本実施形態の第２の変形例は請求項５記載の内容に相当するもので、図９は本第２の変形例に係るロボットの干渉チェック方法の構成を表すブロック図である。尚、前出の図を用いて説明した同一の構成要素については、同一の符号を付して説明を割愛する。図９において、３１は速度演算手段、７１'は第２の近似立体サイズ調整手段である。本第２の変形例と第２の実施の形態（図５）との違いは、干渉予測手段１４に加わる入力の１つが、近似立体拡張サイズ９４から第２の近似立体サイズ調整手段７１'の出力に替わっている点、および速度演算手段３１が加わっている点である。速度演算手段３１は、位置記憶手段１１からの時刻ｔ−ΔＴでの第１の近似立体位置（ｔ−ΔＴ）１０４と第２の第２の近似立体位置（ｔ−ΔＴ）１０５と、時刻ｔでの第１の近似立体位置（ｔ）９０４と第２の近似立体位置（ｔ）９０５と、から制御周期ΔＴを用いて、第１の近似立体速度（ｔ）３０３と第２の近似立体速度（ｔ）３０４を演算し、第２の近似立体サイズ調整手段７１'へ出力する。
【００３６】
第２の近似立体サイズ調整手段７１'は、第１の近似立体速度（ｔ）３０３、第２の近似立体速度（ｔ）３０４を入力し、近似立体の速度（第１の近似立体速度（ｔ）３０３または第２の近似立体速度（ｔ）３０４）に応じて近似立体のサイズを決定し、近似立体拡張サイズ９４"として出力する。
【００３７】
第２の近似立体サイズ調整手段７１'の具体的内容を、図１０を用いて説明する。図１０は、本発明の第２の近似立体サイズ調整手段の機能を説明する図である。尚、前出の図を用いて説明した同一の構成要素については、同一の符号を付して説明を割愛する。図１０において、２０１は速度判定値、２０２は速度の近似立体拡張直線である。近似立体の速度（第１の近似立体速度（ｔ）３０３または第２の近似立体速度（ｔ）３０４）が速度判定値２０１より大きくなった場合、速度の近似立体拡張直線２０２に従い、近似立体拡張サイズ９４"を大きくする。これにより、近似立体の速度（第１の近似立体速度（ｔ）３０３または第２の近似立体速度（ｔ）３０４）が大きくなるに従って、近似立体拡張サイズ９４"（近似立体サイズ９３より大きい）が大きくなって行く。以上説明したように、本発明の第２の実施の形態の第２変形例（請求項５記載の例）は、近似立体の速度（第１の近似立体速度（ｔ）３０３または第２の近似立体速度（ｔ）３０４）に応じて近似立体拡張サイズ９４"を決定する第２の近似立体サイズ調整手段７１'を設けて、近似立体が高速で接近するに従って近似立体拡張サイズ９４"を大きくするようにするので、第１の実施形態の干渉の可能性を低減させ、より確実にロボットや工具の衝突を防止することができるという効果がある。尚、本第２の変形例は、近似立体拡張サイズ９４"を決定する方法として、速度の近似立体拡張直線２０２を用いたが、本発明はこれに限るものではなく、曲線を用いて近似立体拡張サイズ９４"を決定するようにしても良い。
【００３８】
本発明の第２の実施の形態の第３の変形例としてのロボットの干渉チェック方法について、図１１を用いて説明する。本第３の変形例は請求項６記載の内容に相当する。図１１は本第３の変形例に係るロボットの干渉チェック方法の構成を表すブロック図である。尚、前出の図を用いて説明した同一の構成要素については、同一の符号を付して説明を割愛する。図５を用いて説明した第２の実施の形態との違いは、干渉予測判定手段２１と距離閾値ファイル２２とが加わっている点である。
【００３９】
距離閾値ファイル２２の値は、近似立体拡張サイズ９４より大きめの値を設定しておく。干渉予測判定手段２１は、時刻ｔでの近似立体間の距離（ｔ）９０６と、予め設定されている距離閾値２２とを入力し、それらを比較し、干渉予測手段１４の演算の実行を決定する。例えば、近似立体が２つの球であれば、近似立体間の距離（ｔ）９０６である球間の距離ＬSSと、距離閾値２２のＬth（＞ＲSA＋ＲSB）とから式（３）の判定を行い、式（３）を満たす場合、干渉予測手段１４の演算を実行し、満たさない場合、干渉予測手段１４の演算を実行しない。演算を実行する場合には、近似立体間の距離（ｔ−ΔＴ）１０６と、近似立体間の距離（ｔ）９０６と、第１の近似立体位置（ｔ−ΔＴ）１０４と、第２の近似立体位置（ｔ−ΔＴ）１０５と、第１の近似立体位置（ｔ）９０４と、第２の近似立体位置（ｔ）９０５と、近似立体拡張サイズ９４とを干渉予測手段１４に出力する。以上説明したように、本第３の変形例（請求項６記載の例）は、第２の実施の形態と同様、干渉予測手段１４を設けて、近似立体サイズ９３より大きめの近似立体拡張サイズ９４に基づいて干渉チェックを行うので、干渉を判定する距離が長くなり、第２の実施形態の干渉の可能性を低減させ、より確実にロボットや工具の衝突を防止することができるという効果がある。
【００４０】
また、干渉予測判定手段２１を設けて、近似立体拡張サイズ９４より大きめの距離閾値２２に基づいて干渉予測手段１４の演算の実行を決定することができるので、常時干渉予測手段１４の演算を実行する必要がなく、干渉チェックに要する演算が高速に行えるという効果がある。尚、本実施の形態は、図５を用いて説明した第２の実施の形態の干渉予測手段１４の前段に干渉予測判定手段２１と距離閾値２２とを設けるようにしたが、本発明はこれに限るものではなく、図７、図９を用いて説明した第２の実施の形態の第１、第２の変形例の干渉予測手段１４の前段に干渉予測判定手段２１と距離閾値２２とを設けるようにしても良い。以上説明した全ての実施の形態は、ロボットや工具の衝突を防止することができるので、ロボットや工具の破壊の危険性をなくし、それに伴う人に対する安全性を向上させることができるという効果がある。
【００４１】
【発明の効果】
以上説明したように、本発明の請求項１記載のロボットの干渉チェック方法によれば、次の時刻での近似立体の位置を推定し、次の時刻までの干渉の有無が予測できる干渉予測手段を設けているので、従来の干渉チェック方法に合わせ、近似立体間の干渉が確実にチェックでき、ロボットや工具の衝突を防止することができるという効果がある。本発明の請求項２記載のロボットの干渉チェック方法によれば、請求項１記載のロボットの干渉チェック方法と同様、干渉予測手段を設けているので、従来の干渉チェック方法に合わせ、近似立体間の干渉が確実にチェックでき、ロボットや工具の衝突を防止することができるという効果がある。また、干渉予測判定手段を設けて、近似立体サイズより大きめの距離閾値に基づいて干渉予測手段の演算の実行を決定することができるので、常時干渉予測手段の演算を実行する必要がなく、干渉チェックに要する演算が高速に行えるという効果がある。
【００４２】
また、本発明の請求項３記載のロボットの干渉チェック方法によれば、近似立体拡張サイズを利用して干渉の有無を判定する干渉予測手段を設けて、近似立体サイズより大きめの近似立体拡張サイズに基づいて干渉チェックを行うので、干渉を判定する距離が長くなり、請求項１記載の干渉の可能性を低減させ、より確実にロボットや工具の衝突を防止することができるという効果がある。
本発明の請求項４記載のロボットの干渉チェック方法によれば、近似立体間の距離に応じて近似立体拡張サイズを決定する近似立体サイズ調整手段を設けて、近似立体が接近するに従って近似立体拡張サイズを大きくするようにするので、請求項１記載の干渉の可能性を低減させ、より確実にロボットや工具の衝突を防止することができるという効果がある。
本発明の請求項５記載のロボットの干渉チェック方法によれば、近似立体の速度に応じて近似立体拡張サイズを決定する近似立体サイズ調整手段を設けて、近似立体が高速で接近するに従って近似立体拡張サイズを大きくするようにするので、請求項１記載の干渉の可能性を低減させ、より確実にロボットや工具の衝突を防止することができるという効果がある。
本発明の請求項６記載のロボットの干渉チェック方法によれば、請求項３〜５のいずれか１項記載のロボットの干渉チェック方法と同様、干渉予測手段を設けて、近似立体サイズより大きめの近似立体拡張サイズに基づいて干渉チェックを行うので、干渉を判定する距離が長くなり、請求項１記載の干渉の可能性を低減させ、より確実にロボットや工具の衝突を防止することができるという効果がある。また、干渉予測判定手段を設けて、近似立体サイズより大きめの距離閾値に基づいて干渉予測手段の演算の実行を決定することができるので、常時干渉予測手段の演算を実行する必要がなく、干渉チェックに要する演算が高速に行えるという効果がある。
本発明の全ての請求項に記載のロボットの干渉チェック方法によれば、ロボットや工具の衝突を防止することができるので、ロボットや工具の破壊の危険性をなくし、それに伴う人に対する安全性を向上させることができるという効果がある。
【図面の簡単な説明】
【図１】本発明の第１の実施の形態に関わるロボットの干渉チェック方法の構成を表すブロック図である。
【図２】本発明の第１の実施の形態に関わる干渉予測手段の構成を表すブロック図である。
【図３】本発明の干渉予測手段の機能を説明する図である。
【図４】本発明の第１の実施の形態の変形例に関わるロボットの干渉チェック方法の構成を表すブロック図である。
【図５】本発明の第２の実施の形態に関わるロボットの干渉チェック方法の構成を表すブロック図である。
【図６】本発明の第２の実施の形態に関わる干渉予測手段の構成を表すブロック図である。
【図７】本発明の第２の実施の形態の変形例に関わるロボットの干渉チェック方法の構成を表すブロック図である。
【図８】本発明の第１の近似立体サイズ調整手段の機能を説明する図である。
【図９】本発明の第２の実施の形態の第２の変形例に関わるロボットの干渉チェック方法の構成を表すブロック図である。
【図１０】本発明の第２の近似立体サイズ調整手段の機能を説明する図である。
【図１１】本発明の第２の実施の形態の第３の変形例に関わるロボットの干渉チェック方法の構成を表すブロック図である。
【図１２】従来のロボットの干渉チェック方法の構成を表すブロック図である。
【図１３】近似立体が干渉するケースを表す図である。
【符号の説明】
４ 第１の球
５ 第２の球
６ 第１の円筒
７ 第２の円筒
１１ 位置記憶手段
１２ 距離記憶手段
１３ 干渉予測手段
１４、１４' 干渉予測手段
２１ 干渉予測判定手段
２２ 距離閾値ファイル（その値）
３１ 速度演算手段
５１ 距離推定手段
５２ 距離近似手段
５３ 最小値取得手段
５４、５４' 干渉推定手段
７１ 第１の近似立体サイズ調整手段
７１'第２の近似立体サイズ調整手段
９１ 近似立体間距離演算手段
９２ 干渉判定手段
９３ 近似立体サイズファイル（その値）
９４，９４'、９４" 近似立体拡張サイズファイル（その値）
１０４ 第１の近似立体位置（ｔ−ΔＴ）
１０５ 第２の近似立体位置（ｔ−ΔＴ）
１０６ 近似立体間の距離（ｔ−ΔＴ）
２０１ 速度判定値
２０２ 速度の近似立体拡張直線
３０３ 第１の近似立体速度（ｔ）
３０４ 第２の近似立体速度（ｔ）
５０５ 近似立体間の距離（ｔ＋ΔＴ）
５０６ 近似曲線
５０７ 距離最小値
６０１ 距離判定値
６０２ 距離の近似立体拡張直線
７０１ 第１の近似立体位置（ｔ＋ΔＴ）
７０２ 第２の近似立体位置（ｔ＋ΔＴ）
８１０ 干渉の有無信号
９０４ 第１の近似立体位置（ｔ）
９０５ 第２の近似立体位置（ｔ）
９０６ 近似立体間の距離（ｔ）
Claims (6)
- 複数のロボットの各部の形状を覆って近似する近似立体を予め設定し、
動作中の前記複数のロボットにおける一方の前記ロボットに設定した前記近似立体と、他方の前記ロボットに設定した前記近似立体との距離を所定の制御周期ごとに演算する近似立体間距離演算手段と、
前記近似立体のサイズと前記近似立体間距離演算手段で演算された距離とを比較する干渉判定手段と、を有して成るロボットの干渉をチェックする方法において、
前記近似立体間距離演算手段で演算された距離のうち少なくとも現在から制御周期１回分遡った時刻（以降、直前の時刻）での距離を記憶する距離記憶手段と、
前記制御周期ごとに取得される前記近似立体の位置のうち少なくとも直前の時刻での位置を記憶する位置記憶手段と、
干渉予測手段とを有し、
前記干渉予測手段は、直前の時刻での前記近似立体の位置と現在の時刻での位置との差分を現在の時刻での位置に加えることで、現在から制御周期１回分経った時刻（以降、次の時刻）での前記近似立体の位置を推定し、次の時刻での前記近似立体間の距離を演算する距離推定手段と、
前記距離記憶手段に記憶されている直前の時刻での前記近似立体間の距離と現在の時刻での距離と前記距離推定手段で演算された次の時刻での距離とから、前記近似立体間の距離の時間変化を表す２次以上の近似曲線を演算する距離近似手段と、
現在の時刻から次の時刻までの間における前記近似曲線の最小値を求める最小値取得手段と、
前記近似曲線の最小値と前記近似立体のサイズとを比較することにより干渉を判定する干渉推定手段と、から構成されることを特徴とするロボットの干渉チェック方法。 - 前記干渉予測手段の前段に干渉予測判定手段を設け、
前記干渉予測判定手段は、前記近似立体のサイズより大きく設定された距離閾値と前記近似立体間距離演算手段が演算した現在の時刻での前記近似立体間の距離とを比較し、
前記干渉予測手段は、前記距離が前記距離閾値以下の場合のみ演算を行うことを特徴とする請求項１記載のロボットの干渉チェック方法。 - 前記干渉予測手段への入力として前記近似立体のサイズより大きい近似立体拡張サイズを予め設定し、
前記干渉推定手段は、前記近似立体のサイズに代えて前記近似曲線の最小値と前記近似立体拡張サイズとを比較することにより干渉を判定することを特徴とする請求項１記載のロボットの干渉チェック方法。 - 前記近似立体のサイズを拡張して前記干渉予測手段への入力とする近似立体サイズ調整手段を設け、
前記近似立体サイズ調整手段は、現在の時刻での前記近似立体間の距離が所定の判定値より小さい場合には前記距離が小さくなるに従って前記近似立体のサイズを拡張した近似立体拡張サイズを出力し、
前記干渉推定手段は、前記近似立体のサイズに代えて前記近似曲線の最小値と前記近似立体拡張サイズとを比較することにより干渉を判定することを特徴とする請求項１記載のロボットの干渉チェック方法。 - 前記位置記憶手段に記憶されている直前の時刻での前記近似立体の位置と現在の時刻での位置とから前記近似立体の速度を演算する速度演算手段と、
前記近似立体のサイズを拡張して前記干渉予測手段への入力とする近似立体サイズ調整手段を設け、
前記近似立体サイズ調整手段は、前記近似立体の速度が所定の判定値より大きい場合には前記速度が大きくなるに従って前記近似立体のサイズを拡張した近似立体拡張サイズを出力し、
前記干渉推定手段は、前記近似立体のサイズに代えて前記近似曲線の最小値と前記近似立体拡張サイズとを比較することにより干渉を判定することを特徴とする請求項１記載のロボットの干渉チェック方法。 - 前記干渉予測手段の前段に干渉予測判定手段を設け、
前記干渉予測判定手段は、前記近似立体拡張サイズより大きく設定された距離閾値と前記近似立体間距離演算手段が演算した現在の時刻での前記近似立体間の距離とを比較し、
前記干渉予測手段は、前記距離が前記距離閾値以下の場合のみ演算を行うことを特徴とする請求項３〜５のいずれか１項記載のロボットの干渉チェック方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2002140237A JP3975341B2 (ja) | 2002-05-15 | 2002-05-15 | ロボットの干渉チェック方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2002140237A JP3975341B2 (ja) | 2002-05-15 | 2002-05-15 | ロボットの干渉チェック方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2003334777A JP2003334777A (ja) | 2003-11-25 |
| JP3975341B2 true JP3975341B2 (ja) | 2007-09-12 |
Family
ID=29701166
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2002140237A Expired - Fee Related JP3975341B2 (ja) | 2002-05-15 | 2002-05-15 | ロボットの干渉チェック方法 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP3975341B2 (ja) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP4569390B2 (ja) * | 2005-06-02 | 2010-10-27 | トヨタ自動車株式会社 | 物体間衝突判定装置及び方法 |
| JP4955371B2 (ja) * | 2006-11-24 | 2012-06-20 | 三菱電機株式会社 | 数値制御システム |
| JP5250858B2 (ja) * | 2007-05-08 | 2013-07-31 | 株式会社Ｉｈｉ | 複数ロボットの衝突検知方法及びロボット装置 |
| KR101012742B1 (ko) * | 2007-10-31 | 2011-02-09 | 한국기계연구원 | 양팔 로봇 시스템의 충돌제어 방법 |
| JP2012216151A (ja) * | 2011-04-01 | 2012-11-08 | Mitsubishi Electric Corp | 干渉回避制御装置 |
| JP7151072B2 (ja) * | 2017-11-15 | 2022-10-12 | セイコーエプソン株式会社 | ロボット |
Family Cites Families (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS60217410A (ja) * | 1984-04-13 | 1985-10-31 | Hitachi Ltd | 協調作業教示におけるロボツト干渉検出方式 |
| JPH07107648B2 (ja) * | 1987-09-09 | 1995-11-15 | インターナシヨナル・ビジネス・マシーンズ・コーポレーシヨン | 物体間衝突検出装置 |
| JP2678005B2 (ja) * | 1988-03-04 | 1997-11-17 | ファナック株式会社 | ロボットの干渉チェック方法 |
| JPH085028B2 (ja) * | 1990-02-28 | 1996-01-24 | 株式会社日立製作所 | 移動体の衝突判定方法並びに衝突判定装置 |
| JPH0748161B2 (ja) * | 1991-06-10 | 1995-05-24 | 動力炉・核燃料開発事業団 | ロボットとワークの干渉チェック方式 |
| JP2785086B2 (ja) * | 1992-06-17 | 1998-08-13 | ファナック株式会社 | ロボットの手動送り方法 |
| JPH06226666A (ja) * | 1993-02-05 | 1994-08-16 | Yaskawa Electric Corp | 双腕ロボットマニピュレータの衝突防止方法 |
| JPH06259506A (ja) * | 1993-03-10 | 1994-09-16 | Toshiba Corp | ３次元形状干渉チェック装置 |
| JPH06315886A (ja) * | 1993-05-07 | 1994-11-15 | Yaskawa Electric Corp | 双腕ロボットマニピュレータの干渉検出装置 |
| JP3316092B2 (ja) * | 1993-09-14 | 2002-08-19 | 富士通株式会社 | 最接近点探索装置 |
| JPH07271415A (ja) * | 1994-03-30 | 1995-10-20 | Nec Corp | 協調ロボット制御方法 |
| JPH0890468A (ja) * | 1994-09-27 | 1996-04-09 | Nissan Motor Co Ltd | ロボットのティーチング装置 |
| JP3512049B2 (ja) * | 1996-03-15 | 2004-03-29 | 株式会社安川電機 | 産業用ロボットのツール干渉チェック方法 |
-
2002
- 2002-05-15 JP JP2002140237A patent/JP3975341B2/ja not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| JP2003334777A (ja) | 2003-11-25 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN101512453B (zh) | 避免工业机器人与物体之间碰撞的方法和设备 | |
| JP3907649B2 (ja) | ロボット間の干渉防止制御装置 | |
| US6678582B2 (en) | Method and control device for avoiding collisions between cooperating robots | |
| Althoff et al. | Safety assessment of trajectories for navigation in uncertain and dynamic environments | |
| US10478969B2 (en) | Method for the improved detection of the collision of a robot with its environment, system and computer program product implementing said method | |
| US6581007B2 (en) | System, method, and program for detecting approach to object | |
| CN107548336B (zh) | 控制和/或调节机器人的电机 | |
| JPWO2020247207A5 (ja) | ||
| US20150183431A1 (en) | Collision prediction apparatus | |
| US20110153080A1 (en) | Method and apparatus for industrial robotic pathscycle time optimization using fly by | |
| EP3587042A1 (en) | Method, apparatus and system for determining a trajectory of a robot's end effector | |
| US9552733B2 (en) | Course evaluation apparatus and course evaluation method | |
| JP2019087096A (ja) | 行動決定システム及び自動運転制御装置 | |
| JP3975341B2 (ja) | ロボットの干渉チェック方法 | |
| JP7494382B2 (ja) | 制御命令の識別方法及び識別装置、不揮発性記憶媒体、プロセッサ、電子装置、多関節ロボット | |
| Stellet et al. | Analytical derivation of performance bounds of autonomous emergency brake systems | |
| CN110000815A (zh) | 一种碰撞检测方法、装置、电子设备及存储介质 | |
| Muntzinger et al. | Reliable automotive pre-crash system with out-of-sequence measurement processing | |
| CN110238841B (zh) | 障碍物的避让方法及装置 | |
| EP2998078A1 (en) | Method for improving efficiency of industrial robotic energy consumption and cycle time by handling orientation at task location | |
| Hamasaki et al. | Prediction of human's movement for collision avoidance of mobile robot | |
| JP2024096877A (ja) | ロボット動作に使用される安全システム及び方法 | |
| CN114310895B (zh) | 机器人碰撞检测方法、装置、电子设备及存储介质 | |
| JP2012216151A (ja) | 干渉回避制御装置 | |
| KR102555435B1 (ko) | 장애물에 의한 이동 로봇의 제어 시스템 및 그 제어 방법 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20050418 |
|
| RD04 | Notification of resignation of power of attorney |
Free format text: JAPANESE INTERMEDIATE CODE: A7424 Effective date: 20060324 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20061004 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20061201 |
|
| A02 | Decision of refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A02 Effective date: 20070221 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20070319 |
|
| A911 | Transfer of reconsideration by examiner before appeal (zenchi) |
Free format text: JAPANESE INTERMEDIATE CODE: A911 Effective date: 20070424 |
|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20070523 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20070605 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20100629 Year of fee payment: 3 |
|
| R150 | Certificate of patent or registration of utility model |
Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20110629 Year of fee payment: 4 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120629 Year of fee payment: 5 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20130629 Year of fee payment: 6 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20140629 Year of fee payment: 7 |
|
| LAPS | Cancellation because of no payment of annual fees |