(57)【要約】【課題】設定された干渉領域とロボットの干渉判断方法において、幾何学的に単純な図形を用いて、図形間の距離関係のみで干渉の有無を判断でき、計算を短時間で行える干渉判断方法を提供する。【解決手段】多関節ロボットのロボットアームが、予め設定された干渉領域内に存在することを判断するロボットの干渉判断方法において、面毎にすべての頂点を通る外球５、６、７、８、９、１０を求め（ステップ１）、干渉領域３のすべての頂点を通る外接球１１を求め（ステップ２）、ロボットアーム１と複数の外球との交点を求め（ステップ３）、交点が外接球内１１に存在するかどうか判断し（ステップ４）、存在する場合に干渉状態と判断する（ステップ５）ことを特徴とするロボットの干渉判断方法。【選択図】図１
2004017245.tif 000002
【特許請求の範囲】
【請求項１】
多関節ロボットのロボットアームが、予め設定された干渉領域内に存在することを判断するロボットの干渉判断方法において、
前記干渉領域を構成する複数の面毎にすべての頂点を通る外球を求め、
前記干渉領域のすべての頂点を通る外接球を求め、
前記ロボットアームと前記複数の外球との交点を求め、
前記交点が前記外接球内に存在する場合に干渉状態と判断することを特徴とするロボットの干渉判断方法。
【請求項２】
多関節ロボットのツール先端である制御点が、予め設定された干渉領域内に存在することを判断するロボットの干渉判断方法において、
前記干渉領域を構成する複数の面に分割し、前記面毎にすべての頂点を通る外球を求め、
前記制御点が全ての前記複数の外球内に存在する場合に干渉状態と判断することを特徴とするロボットの干渉判断方法。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明はロボットアームのリアルタイム干渉判断方法に関する。
【０００２】
【従来の技術】
従来、ロボット制御装置によるロボットアームの干渉領域チェック機能は、ロボットアームに取り付けられたハンド先端部が、ロボットアームの動作空間領域で任意の点座標位置へ移動動作をしようとしたとき、干渉領域に侵入するかどうかを判定する機能である。前述のハンド先端部は３次元立体形状ではなく、ポイントとしての干渉領域への侵入を判定するものである。このような場合には、ハンド部の先端部の位置だけがチェックの対象となっており、ロボットアーム自体が干渉領域に入っているかどうかは不明であった。このため、シミュレータなどを使用して、ロボットの軌道と干渉領域をコンピュータに入力してロボットアームの干渉を判断していた。
【０００３】
また、特開２００１−３１５０８７号にロボットアームの干渉チェック方法が開示されている。図９を用いて説明するロボットアームを簡単な四角柱の組み合わせでモデル化し、最初に四角柱２１、２２Ａおよび２３の上面および底面と干渉領域２７の上面および底面とのＺ軸現在位置を比較し、干渉チェック後、干渉の可能性ありと判定された四角柱２１および２３のＸＹ平面投影図である四角柱３１および３３と干渉領域２７のＸＹ平面投影図である四角形３４とのＸＹ座標現在位置を比較し干渉をチェックする方法である。
【０００４】
【発明が解決しようとする課題】
上述したように、オフラインによる干渉チェック方法では、実際のロボットの干渉領域との位置関係をコンピュータに再現することは非常に困難であり、その干渉領域のデータを入力するという作業が必要であった。また、リアルタイムで修正を行うようなセンシング機能を持つロボットシステムでは、オフラインによる干渉チェックは不可能であった。
また、特開２００１−３１５０８７号に開示されている方法では、水平多関節ロボットに関しては有効であるが、垂直多関節ロボットにおいては、各アームをＸＹ平面に投影するなどの処理において、アームとアームの位置関係を求める必要があり、この位置関係は非常に複雑となるためにリアルタイムでの演算は複雑となるという問題があった。
上述した問題を解決するため、本発明は、設定された干渉領域とロボットの干渉判断方法において、干渉領域を複数の球で表現し、アームを直線で表現し、球と直線という幾何学的に単純な図形を用いて、図形間の距離関係のみで干渉の有無を判断でき、計算を短時間で行える干渉判断方法を提供する。
【０００５】
【課題を解決するための手段】
本発明の請求項１記載のロボットの干渉判断方法は、多関節ロボットのロボットアームが、予め設定された干渉領域内に存在することを判断するロボットの干渉判断方法において、前記干渉領域を構成する複数の面毎にすべての頂点を通る外球を求め、前記干渉領域のすべての頂点を通る外接球を求め、　前記ロボットアームと前記複数の外球との交点を求め、前記交点が前記外接球内に存在する場合に干渉状態と判断することを特徴とするものである。
本発明の請求項２記載のロボットの干渉判断方法は、多関節ロボットのツール先端である制御点が、予め設定された干渉領域内に存在することを判断するロボットの干渉判断方法において、前記干渉領域を構成する複数の面に分割し、前記面毎にすべての頂点を通る外球を求め、前記制御点が全ての前記複数の外球内に存在する場合に干渉状態と判断することを特徴とするものである。
【０００６】
【発明の実施の形態】
本発明の第１の実施の形態について、図１、図２、図３を用いて説明する。
図１は、本発明のフローである。まず、干渉領域３を構成する複数の面ごとに各面の頂点を通る外球５、６、７、８、９、１０を求める。干渉領域３を直方体とすると、６つの面が存在するため、６つの球を求める。次に、干渉領域３の６つの頂点を通る外接球を求める。次に、ロボットアームと外球５、６、７、８、９、１０との交点を求める。この交点が外接球１１内にある場合には、干渉状態と判断し、交点が外接球内１１に無い場合には、干渉していないと判断する。以下に、図を用いて詳細に説明する。
図２は、本発明の概要図を示したものである。ロボット１は、ロボット制御装置２によって動作制御される。図３は、球５と干渉領域３の側面図であり、干渉領域３の場合を例に説明する。図４は、図３の幾何学配置を示す図である。図５は、実行時以前の干渉領域の設定フローである。以下に図に基づいて、干渉領域３の設定手順について説明する。
【０００７】
ステップ１では、操作者は、図３に示すＰ１、Ｐ２、Ｐ３、Ｐ４の点を教示する。干渉領域２を定義する点にロボット１を移動させ、ロボット制御装置に４点の位置データを記憶させる。または、ロボット１に教示させたい座標系の数値を入力する方法もある。このとき、入力される座標値の座標系を指定する必要がある。この場合には、操作者が定義した座標系とロボットの固有な座標系との関係は、予め定義しておく必要がある。前述したロボットの固有な座標系とは、ロボットが本来動作するために演算される座標系である。また、直方体の各頂点の位置を教示してもよい。この場合には、各頂点位置には、教示誤差が含まれているので、各辺のもっとも大きくなるものを選択する。
【０００８】
ステップ２では、球５の中心Ｑ５、半径ｒ５を求める。設定されている点Ｐ１、Ｐ２、Ｐ３をＰ１（ｘ１，ｙ１，Ｚ１）、Ｐ２（ｘ２，ｙ２，ｚ２）、Ｐ３（ｘ３，ｙ３，ｚ３）とおく。ここで、球５の中心Ｑ５（ｘｃ，ｙｃ，ｚｃ）は、Ｐ１とＰ２の中点とＰ２とＰ３の中点を通る直線Ｌ１上に存在する。ここで、球５は、点Ｐ１、Ｐ２、Ｐ３で作成される面Ｓと交点をＰＳとするとＰＳから球５の半径方向に距離ｄはなれた点ＰＤを通るものとする。この距離ｄは、ロボット制御装置２に予め設定されているパラメータ、または干渉領域設定時にユーザが設定するパラメータでもよい。図５に基づいて中心Ｑ５を求める。点Ｐ３Ｐ２の距離をａとおくと
【０００９】
【数１】
2004017245.tif 000003
【００１０】
である。角θは、
【００１１】
【数２】
2004017245.tif 000004
【００１２】
となる。△Ｑ５Ｐ１ＰＳに着目するとＱ５ＰＳの距離ｂは
【００１３】
【数３】
2004017245.tif 000005
【００１４】
となる。また半径ｒ５は、
【００１５】
【数４】
2004017245.tif 000006
【００１６】
となる。ここで、点Ｐ１、Ｐ２、Ｐ３の作る平面Ｓの方程式は、
【００１７】
【数５】
2004017245.tif 000007
【００１８】
となる。この平面Ｓの法線ベクトルと直線Ｌ１の方向ベクトルは、等しく、かつ直線Ｌ１は、点ＰＳを通るので直線Ｌ１の方程式は、
【００１９】
【数６】
2004017245.tif 000008
【００２０】
となり、点ＰＤから中心Ｑまでの距離がｂであることから、点Ｑは、
【００２１】
【数７】
2004017245.tif 000009
【００２２】
となる。
【００２３】
ステップ３、４、５、６、７では、ステップ２の処理と同様な方法により球６、７、８、９、１０の中心と半径を求める。
ステップ８では、干渉領域３の各頂点を通る外接球１１の中心と半径を求める。
点Ｐ１、Ｐ２、Ｐ３、Ｐ４により球１１の中心Ｃｇ（ｘｇ、ｙｇ、ｚｇ）、半径ｒｇは、
【００２４】
【数８】
2004017245.tif 000010
【００２５】
となる。
【００２６】
ステップ９では、ロボット制御装置２内の記憶メモリに球の半径と中心位置を記憶する。
【００２７】
次に、干渉チェック方法について、図６のフローに基づいて説明する。図７に干渉判断時の幾何学的な図を用いて説明する。
ステップ１では、ロボットの各アームについて、直線の方程式を求める。ロボット１の上腕アームＡを直線Ｌ２とする。直線Ｌ２の方程式を
【００２８】
【数９】
2004017245.tif 000011
【００２９】
とおく。
【００３０】
ステップ２では、予め求めておいた球５、６、７、８、９、１０、１１の中心と半径を読み出す。
ステップ３では、球５、６、７、８、９、１０の中心から直線Ｌ２へ下ろした垂線の長さを求める。球５の中心Ｑ（ｘｑ、ｙｑ、ｚｑ）とすると、直線Ｌ２に下ろした垂線の長さｈ５は、
【００３１】
【数１０】
2004017245.tif 000012
【００３２】
となる。
【００３３】
ステップ４では、各球の中心から直線Ｌ２に下ろした垂線の長さから各球の半径を減じたものが、ロボットアームの半径より大きいかどうか判定する。
ステップ５では、すべての球から下ろした垂線の長さから各球の半径を減じたものが、ロボットアームの半径より大きいので、干渉なしと判断する。
ステップ６では、球５、６、７、８、９、１０の中心から直線Ｌ２へ下ろした垂線の長さから各球の半径を減じたものが、ロボットアームの半径より小さいものが少なくとも１つある場合である。この場合、その球の半径にロボットアームの半径を加算した球と直線Ｌ２の交点を求める。
【００３４】
ステップ７では、ステップ５で求めた交点が、外接球１１の内部にあるかどうか判断する。ステップ８では、外接球１１の内部に交点がひとつでもあれば、干渉していると判断する。ステップ９では、外接球１１の内部には交点がないので、干渉なしと判断する。
【００３５】
本発明の第２の実施の形態について、図８を用いて説明する。
図８は、説明を簡単にするための平面図である。干渉領域を直方体とした場合、外球は、第１の実施の形態で説明したとおり５、６、７、８、９、１０の外球となる。これらの外球の中心とロボット１の制御点（作業ツールなどの先端の点）との距離が外球５、６、７、８、９、１０の半径より小さいかどうかを判定し、全ての外球の中に存在する場合には、ロボット１が干渉状態であると判断する。
【００３６】
上記のように、本発明によれば、球と直線の幾何学的な距離で干渉を判断できるので、動作指令実行時にリアルタイムでロボットの干渉を検出することが可能である。この干渉判断方法は、ロボット制御装置内で実現される。処理するタイミングは、プレイバック中であれば、ロボットの軌道を作成するタイミングである。これは、外部からのセンサ等の信号によって軌跡が変化する場合のことを考慮して、実際のロボットアームを駆動する位置演算指令を生成するタイミングで処理する。また、ティーチング中に処理するタイミングは、操作者からの指示により、ロボット制御装置が移動指令を受信した後に処理を行う。このようにすることで、実際にロボットが移動する前に干渉を判断し、ロボットを停止させることが可能となる。
また、複雑な干渉領域をもつものについては、直方体を組み合わせることで実現することができる。
【００３７】
【発明の効果】
以上のように本発明では、干渉領域を複数の球とその領域の外接球でモデル化し、複数の球および外接球とロボットアームの位置関係から干渉の有無を判断することができる。また、複雑な干渉領域を設定する場合には、直方体を複数設定することで、干渉の有無を判断することができる。さらに、球と直線という幾何学的に単純な図形を用いて、図形間の距離関係のみで干渉の有無を判断できるため、計算を短時間で行えるという格別の効果を奏するものである。
【図面の簡単な説明】
【図１】本発明のフロー
【図２】本発明の概要図
【図３】干渉領域と球との側面図
【図４】干渉領域の幾何学的な図
【図５】干渉領域設定時フロー
【図６】本発明の干渉判断のフロー
【図７】干渉判断時の幾何学的な図
【図８】第２の実施形態の概要図
【図９】従来技術の説明図
【符号の説明】
１：ロボット
２：ロボット制御装置
３：干渉領域
５、６、７、８、９、１０：面の頂点を通る外球
１１：外接球
【図１】
2004017245.tif 000013
【図２】
2004017245.tif 000014
【図３】
2004017245.tif 000015
【図４】
2004017245.tif 000016
【図５】
2004017245.tif 000017
【図６】
2004017245.tif 000018
【図７】
2004017245.tif 000019
【図８】
2004017245.tif 000020
【図９】
2004017245.tif 000021