JP5144785B2 - ロボットの着目部位と周辺物との干渉を予測する方法及び装置 - Google Patents

Description

本発明は、ロボットの手先に搭載されているツールやセンサ等の着目部位がロボットの動作に伴って移動する場合において、その移動経路上での着目部位と、ロボットの周りに固定設置されている周辺機器や柵等の周辺物との干渉を予測する方法及び装置に関する。

視覚センサを用いてロボットがワークピースを取出すピッキング作業の場合、視覚センサによって位置決めされていないワークピースの位置姿勢を計測し、その計測結果を基にワークピースを把持できる位置姿勢へのロボットの移動経路を生成することが行われている。ワークピースの位置姿勢が変化するとロボットの移動経路も変化するため、ロボットの手先に搭載されているツールやセンサ等の着目部位と、ロボットの周りに固定設置されている周辺機器や柵等の周辺物との移動経路上での干渉の有無も変化する。

複数のワークピースがバラ積みされている場合、視覚センサによって計測したそれぞれのワークピースに対して、ロボットの移動経路が生成される。生成された複数の移動経路について干渉予測を行い、干渉しないと判断された移動経路に対応するワークピースをロボットに把持させることが好ましい。ここで、干渉を予測する従来技術としては、移動経路の移動終了位置でのロボットの着目部位と周辺物との干渉の確認を行うものがある。例えば特許文献１には、スカラ型ロボットの干渉チェック方法が開示されており、ここでは、ロボットの目標位置についての指令を各関節軸のストローク値に変換し、各関節軸のストローク限についてのチェックを行うとともに、ロボットの目標位置についての指令を直交座標空間上の位置に変換し、直交座標空間上において前記干渉領域についてのチェックを行い、目標位置が前記ストローク限及び前記干渉領域についての制限を満足することが確認された場合に、各関節軸に動作指令を与える、とされている。

また特許文献２には、ロボットと他の物体のとの干渉を回避するための干渉回避装置が開示されており、その〔００２４〕には、ツール３の形状情報及び収納箱４の形状情報と位置姿勢情報、更に、教示時のワークピース１０の位置姿勢Ｗｎ、ツールの位置姿勢Ｔｎをロボット制御装置６のメモリに設定記憶させておき、上記１式で求められたツールの位置姿勢Ｔａのツール形状情報と、収納箱４の形状情報と位置姿勢情報に基づいて、ツール３と収納箱４が干渉するか否かを判断することができる、と記載されている。

干渉を予測する他の従来技術として、特許文献３には、教示作業が自動化されているロボットの制御装置及び制御システムが開示されており、ここでは、所定作業に対するロボットの動作をシミュレーションして作業経路における部材との干渉の有無をチェックし、ロボットアームあるいはその先端に装着されたツールが部材と干渉すると判定された場合において、干渉区間長が所定値を超えていなければ、干渉個所でロボットの作業経路を分断し、その分断された作業経路内においてロボットを動作させるようにする、とされている。

また他の従来技術として、特許文献４には、非凸多面体の凸包を生成し、該凸包と他の物体間の干渉チェックを行い、干渉し始めた時、凸包を解除して前記非凸多面体と他の物体間の干渉チェックを行う干渉チェック方法が開示されており、該方法は、非凸多面体を構成する頂点を、第１軸（例えばＸ軸）の座標値の差が零あるいは微小量ε以内の複数の頂点群に分割するステップ、各頂点群について第２−第３軸平面（例えばＹＺ平面）上で２次元凸包を生成するステップ、隣接する２次元凸包を併合して３次元凸包を生成し、以後、隣接する３次元凸包を順次併合して目的とする非凸多面体の凸包を生成するステップ、を有する、とされている。

さらなる他の従来技術として、特許文献５には、複数のロボットの動作をシミュレートするシミュレーション装置が開示されており、該シミュレーション装置は、１台ごとのロボットモデルの一定時間動作した軌跡の形状を表す複数の多面体の座標値とシミュレーション時刻を記録する軌跡情報記録手段と、前記複数のロボットの軌跡の形状を表す複数の多面体の、三次元座標値と時刻とを、それぞれスプレッドシートに入力し、複数シート間の同じ時刻で空間的に干渉する多面体を計算により検出する干渉検出手段とを備えるとされている。

特開２０００−３２６２８０号公報 特開２００２−３３１４８０号公報 特開２００２−２７３６７５号公報 特開平９−２７０４６号公報 特開２００４−２８０６３５号公報

特許文献１又は２に記載の技術では、移動終了位置での干渉は確認できても、移動経路上での干渉を確認・予測することは難しい。例えば、特許文献２の図２は、収納箱４の中にあるワークピース７を取出す時のツール３の移動経路を示すものであるが、移動終了時刻Ｔ４でのツール３と収納箱４とは干渉しないが、移動中の時刻Ｔ２とＴ３の間にツール３と周辺物４とが干渉してしまう場合がある。

一方、特許文献３に記載の技術は、生成された移動経路上での干渉を高い精度で予測するために、移動経路上の全補間位置について、逐次的にロボットの着目部位と周辺物とが干渉するか否かを調べるものと解される。しかしこの技術では、計算時間が過度に長くなってしまい、ロボット動作中にリアルタイムで干渉を予測するには適さないと思われる。

特許文献４に記載の技術は、干渉予測に際しいわゆる凸包を利用するものであるが、これは静止状態にある多面体同士の干渉を予測するものであり、移動する多面体には適用できない。

また特許文献５に記載の技術は、複数のロボットや加工機械・治具が同時に動作する際に、該ロボット間等の干渉判定を、それぞれの動作領域を覆う多面体を計算し、該多面体同士の干渉判定に置き換えるものと解される。しかし、動作領域に対応する多面体が凸多面体に限定されないため、干渉判定計算が複雑で時間が掛かる場合があり、ロボット動作中にリアルタイムで干渉判定を行うには適さない。

そこで本発明は、ロボットの手先に搭載されているツールやセンサ等の着目部位がロボットの動作に伴って移動する場合に、移動経路上での着目部位とロボットの周りに固定設置されている周辺機器や柵等の周辺物との干渉を、実用的な精度及び計算速度にて予測する干渉予測方法及び干渉予測装置を提供することを目的とする。

上記目的を達成するために、請求項１に記載の発明は、空間に固定されている周辺物の形状を多面体で近似した周辺物近似多面体が前記空間を占有する領域である周辺物占有領域と、ロボットの着目部位の形状を多面体で近似した着目部位近似多面体とを記憶し、前記ロボットの動作に伴って、前記着目部位近似多面体が複数の時刻の各々において前記空間を占有する領域である、複数の着目部位占有領域を計算し、前記複数の着目部位占有領域と前記周辺物占有領域とに基づいて、前記ロボットの着目部位と前記周辺物との干渉の有無を予測する、干渉予測方法であって、前記複数の着目部位占有領域を前記複数の時刻の順に並べたときに、隣り合う着目部位占有領域同士の組合せの凸包を全て計算し、前記周辺物占有領域を凸多面体の和として計算し、全ての前記凸包と前記凸多面体との共有領域の有無を、前記ロボットの着目部位と前記周辺物との干渉の有無として判定する、干渉予測方法を提供する。

請求項２に記載の発明は、空間に固定されている周辺物の形状を多面体で近似した周辺物近似多面体が前記空間を占有する領域である周辺物占有領域と、ロボットの着目部位の形状を多面体で近似した着目部位近似多面体とを記憶する記憶部と、前記ロボットの動作に伴って、前記着目部位近似多面体が複数の時刻の各々において前記空間を占有する領域である、複数の着目部位占有領域を計算する着目部位占有領域計算部と、前記複数の着目部位占有領域と前記周辺物占有領域とに基づいて、前記ロボットの着目部位と前記周辺物との干渉の有無を予測する干渉予測部と、を備える干渉予測装置であって、前記干渉予測部は、前記複数の着目部位占有領域を前記複数の時刻の順に並べたときに、隣り合う着目部位占有領域同士の組合せの凸包を全て計算する凸包計算部と、前記周辺物占有領域を凸多面体の和として計算する周辺物占有領域計算部と、を有し、前記干渉予測部は、全ての前記凸包と前記凸多面体との共有領域の有無を、前記ロボットの着目部位と前記周辺物との干渉の有無として判定する、干渉予測装置を提供する。

本発明によれば、動作直前に実用的な精度及び計算速度で自動的に干渉を予測できるので、バラ積みされたワークピースの取出し等の用途において、干渉を避けながらロボットを動作させることができる。また本願発明では、ロボットの着目部位の隣り合う補間位置における領域同士の凸包（凸多面体の範疇）を求め、周辺物を凸多面体の和で表し、干渉判定を凸多面体同士のそれに置き換え、計算の簡素化・高速化を図り、ロボット動作中のリアルタイムでの干渉判定を実現している。

本発明に係る干渉予測装置の一実施形態の概略構成を示す図である。 ツール及び収納箱を多面体で近似した例を示す図である。 凸多面体の例である正四面体にインデックスを割り当てた例を示す図である。 本発明に係る干渉予測装置又は干渉予測方法による、干渉予測手順を示すフローチャートである。 分割数が４である場合の各時点でのツールの位置姿勢と、収納箱と、ワークピースとの位置関係を表した図である。 各時点での多面体の頂点の直線度を求める例を説明する図である。 多面体領域から凸包を求める例を説明する図である。 多角形を複数の三角形に分割した例を示す図である。 凸包と周辺物を示す多面体との位置関係を示す図である。

図１は、本発明に係る干渉予測装置の一実施形態の概要図である。なお、本願発明における「ロボットの着目部位」とは、ロボットアームの本体又は付属物のうち、動作中に周辺機器と干渉する可能性のある部位をいう。本実施形態では、ロボット制御装置６内に干渉予測のためのソフトウエアを設けることによって、干渉予測装置を構成している。ロボット５の手首先端には、ワークピース７を取出すためのツール３、ワークピース７を撮像するためのカメラ２、及びワークピース７にレーザ光を投光するレーザ投光器１が取り付けられている。カメラ２及びレーザ投光器１によって、ワークピース７の位置及び姿勢（以降、位置姿勢と称する）を検出する３次元視覚センサが構成される。本実施形態では、ワークピース７は収納箱４内に収納されている。この収納箱４には複数のワークピース７がバラ積み状態で収納されており、故に各々のワークピース７はその位置姿勢が不規則な状態である。

ロボット５、レーザ投光器１、カメラ２及びツール３は従来周知のものでよく、故に詳細な説明は省略する。ロボット制御装置６は、プロセッサ、ＲＯＭ、ＲＡＭ、不揮発性ＲＡＭ、表示装置付き入力手段、入出力インタフェース、カメラインタフェース、ロボット各関節軸のサーボモータ等を駆動制御するサーボ制御器等を備える点では、従来のロボット制御装置と同様でよい。図示例のロボット５の関節軸の数は６つであり、ツール３の位置姿勢が命令されると、ロボット制御装置６は各関節軸の回転角を逆算し、ツール３がその位置姿勢となるようにロボット５の各軸の動作を制御する。また、入出力インタフェースにはレーザ投光器１及びツール３が接続され、カメラインタフェースにはカメラ２が接続される。

ロボット５の手首先端に対するツール３の位置姿勢と、ツール３の形状と、収納箱４の位置姿勢と、収納箱４の形状と、教示時のワークピース７の位置姿勢と、その位置姿勢に対してワークピース７を把持する際のツールの位置姿勢とは、ロボット制御装置６のメモリに予め設定・記憶させておくものとする。その際、記憶するツール３及び収納箱４の形状に関して、図２に示すように、ツール３は２つの直方体を組み合わせて多面体で近似した領域Ｄ_１で表し、収納箱４は直方体の上側から少し小さめの直方体をくり抜いた多面体で近似した領域Ｄ_２で表すものとする。

領域Ｄ_１、Ｄ_２は共に複数の直方体領域の和として表現することができるが、干渉予測の計算が簡単になるように、近似する領域は凸多面体の和で表せるようにする。具体的には、それぞれの多面体領域の持つ頂点、辺の各々にインデックスを割り振り、各頂点の空間上の座標値、各辺の両端にある頂点のインデックス、各面の端にある辺のインデックスを記憶する。例えば、近似された領域が、図３に示すような各辺の長さが３である正四面体（凸多面体）の場合、各頂点及び各辺にインデックスを割り振り、ある座標系における頂点Ｖ_１、頂点Ｖ_２、頂点Ｖ_３、頂点Ｖ_４それぞれの座標値（0.000, 0.000, 2.449）、（1.732, 0.000, 0.000）、（-0.866, 1.500, 0.000）、（-0.866, -1.500, 0.000）と、辺Ｅ_１、辺Ｅ_２、辺Ｅ_３、辺Ｅ_４、辺Ｅ_５、辺Ｅ_６それぞれの両端の頂点のインデックス（Ｖ_１, Ｖ_２）、（Ｖ_１, Ｖ₄）、（Ｖ_１, Ｖ₃）、（Ｖ_２, Ｖ₄）、（Ｖ_３, Ｖ_４）、（Ｖ_２, Ｖ₃）と、面Ｓ_１、面Ｓ_２、面Ｓ_３、面Ｓ_４それぞれの端にある辺のインデックス（Ｅ_１, Ｅ_２, Ｅ_４）、（Ｅ_２, Ｅ_３, Ｅ_５）、（Ｅ_１, Ｅ_３, Ｅ_６）、（Ｅ_４, Ｅ_５, Ｅ_６）を記憶することになる。

図４は、ロボット５を用いたワークピース７の取出し作業において、ツール３と収納箱４との干渉が起こるか否かを判断する処理手順を示すフローチャートである。処理手順では、まず、ロボット制御装置６からの指令により、ロボット５を収納箱４内のワークピース７を撮像できる位置姿勢に移動させた後、レーザ投光器１及びカメラ２を用いて、収納箱４内のワークピース７の位置姿勢を計測する（ステップ１０１）。

次に、この計測したワークピース７の位置姿勢と、予め記憶しておいた教示時のワークピース７の位置姿勢と、予め記憶しておいたワークピースを把持する際のツールの位置姿勢とを合わせて、計測したワークピース７をツール３で把持して収納箱から取出すことのできるツール３の位置姿勢を算出する。そして、ツール３に関して、ワークピース７を計測した時の位置姿勢を移動開始時の位置姿勢とし、計測したワークピース７をツール３で把持して収納箱から取出すことのできる位置姿勢を移動終了時の位置姿勢とした、ツール３の移動経路を生成する（ステップ１０２）。移動経路の生成方法には様々なものがあるが、例えば、ロボット各関節軸の回転速度が常に一定となるような移動経路の生成方法がある。即ち、移動開始時のロボット５の各関節軸ｋ（ｋ＝１，２，・・・，６）の回転角の値をθ_ｋとし、移動終了時の各関節軸ｋの回転角の値をφ_ｋとし、移動開始時刻を０（ゼロ）とし、移動終了時刻をＴとした場合に、時刻ｔ（０≦ｔ≦Ｔ）における各関節軸ｋの回転角がθ_ｋ＋Δ_ｋ×ｔとなるような移動経路を生成する方法である。但し、Δ_ｋ＝（φ_ｋ−θ_ｋ）／Ｔである。但しこのような移動経路の生成方法は一例であって、ここで述べる干渉予測の適用は、移動経路の生成方法に依るものではない。

次に、移動開始から移動終了までの移動時間をｎ等分し、得られたｎ＋１個の時刻（時点）をＴ_０, Ｔ_１,・・・, Ｔ_ｎ とする（ステップ１０３）。Ｔ_０ および Ｔ_ｎ はそれぞれ移動開始時刻と移動終了時刻である。分割数ｎは、干渉予測の粗精や処理時間を考慮して、予め与えておいてもよいし、移動時間及び移動経路長とから逐次決定してもよい。分割数を大きくすれば干渉予測の精度は向上するが、処理時間は分割数にほぼ比例して大きくなる。リアルタイムで干渉予測の処理を行わなければならないため、処理時間はできるだけ短い方がよい。

図５は、分割数ｎ＝４である場合の各時点でのツール３の位置姿勢と、収納箱４と、ワークピース７との位置関係を表したものである。ここではツール３の移動時間を2.0秒、移動経路長を400mmとする。移動時間を４等分した場合、移動開始時より、0.0秒、0.5秒、1.0秒、1.5秒、2.0秒の５つの時点が生成されることになり、隣接する時点間でのロボットの移動経路長は100mmになる。

次に、変数ｊに２を設定（ｊ←２）し、ツールの凸包Ｄ'_１の計算を行う（ステップ１０４）。先ず、ツール３が占有する領域Ｄ_１に対し、その凸包Ｄ'_１の頂点を計算する。凸包とは与えられた集合の任意の２点を結ぶ線分を含む（集合の包含関係の意味で）最小の集合のことであり、換言すれば点の集合体を全て含む最小の凸形状、すなわち点の集合体のうち最も外側の点を直線で結ぶことによって形成される多面体をいう。従って領域Ｄ_１が多面体であれば凸包Ｄ'_１も凸多面体となる。凸包Ｄ'_１の頂点は、例えばQuickHull（“The Quickhull Algorithm for Convex Hull” Geometry Center Technical Report GCG53, July 1993）と呼ばれるアルゴリズムを用いて、多面体領域Ｄ_１の各頂点から求めることができる。

次にステップ１０５において、時点Ｔ_ｊでの直線度Ｌ_jを求める。具体的には、ステップ１０４で求めた凸包Ｄ'_１の各頂点Ｖに対し、図６に示すように、各頂点Ｖについて時点Ｔ_ｊ−１、Ｔ_ｊ、Ｔ_ｊ＋１でのその頂点の位置をそれぞれＰ_ｊ−１、Ｐ_ｊ、Ｐ_ｊ＋１とし、点Ｐ_ｊから線分Ｐ_ｊ−１Ｐ_ｊ＋１へ下した垂線の足をＨ_ｊとして、頂点Ｖの時点Ｔ_ｊでの直線度Ｌ_v,jを以下の式（１）から計算する。

さらに、全ての頂点の時点Ｔ_ｊでの直線度Ｌ_{v , j}のうちの最小値を、ツール３の時点Ｔ_ｊでの直線度Ｌ_jとする（ステップ１０５）。直線度Ｌ_{v , j}は範囲０〜１の値を取り得るが、値が１に近いほど、折れ線Ｐ_ｊ−１Ｐ_ｊＰ_ｊ＋１が直線に近いと言える。そして、ロボット制御装置に予め記憶させておいた０に近い正値εに対して、ツール３の直線度Ｌ_ｊと（１−ε）との大小関係を判定し（ステップ１０６）、Ｌ_ｊが（１−ε）より大きい場合は、Ｔ_ｊ←Ｔ_ｊ＋１、Ｔ_ｊ＋１←Ｔ_{ｊ＋２、・・・}、Ｔ_ｎー１←Ｔ_ｎ、ｎ←ｎ−１とすることによって、時点の列から時点Ｔ_ｊを削除する（ステップ１０７）。これにより、干渉予測の精度をほとんど低下させることなく、後のステップで計算する凸包の個数を１つ減じて、計算にかかる時間を削減することができる。干渉予測の精度がほとんど低下しない理由に関しては、後述する。

ステップ１０８では、ｊと（ｎ−１）との大小関係を判定し、ｊ＜ｎ−１であるうちはｊ←ｊ＋１として（ステップ１０９）、ステップ１０５に戻り、ｊ＜ｎ−１が成り立たなくなるまで各時点での直線度Ｌ_ｊの値のチェックを行う。

次に、ｊ←１として（ステップ１１０）、各時点Ｔ_ｊ（ｊ＝１，２，_・・・，ｎ）でツール３が占有する領域をＤ_１,ｊとして、隣接する時点Ｔ_ｊ−１、Ｔ_ｊの領域Ｄ_{１,ｊ−１}、Ｄ_１,ｊの凸包Ｃ_ｊを計算する（ステップ１１１）。領域Ｄ_１,ｊは、各時点でＴ_ｊのツール３の移動量に応じて領域Ｄ_１を３次元空間で剛体変換させた領域である。領域Ｄ_１は多面体なので、領域Ｄ_１,ｊも多面体であり、領域Ｄ_{１,ｊ−１}、Ｄ_１,ｊの凸包Ｃ_ｊも凸多面体となる。

ステップ１１１の詳細について説明する。先ず、凸包Ｃ_ｊの各頂点は、多面体領域Ｄ_{１,ｊ−１}、Ｄ_１,ｊの各頂点から上述のQuickHull等により求める。次に、凸包Ｃ_ｊの各辺は、凸包Ｃ_ｊの全ての２頂点の組合せ（Ｖ₁，Ｖ₂）に対して、線分Ｖ₁Ｖ₂が凸包Ｃ_ｊの辺であるかの判断を以下の手順で行って求める。空間上の直線Ｖ₁Ｖ₂に垂直な平面Πに凸包Ｃ_ｊの全ての頂点を正射影する。この時、頂点Ｖ₁，Ｖ₂は平面Π上の同一点Ｐに射影される。平面Π上において、正射影した全ての点の凸包の頂点か辺の上に点Ｐがあるならば、線分Ｖ₁Ｖ₂は凸包Ｃ_ｊの辺であると判断できる。平面上の凸包の頂点は同様にQuickHullにより計算可能である。

また、平面上において、凸包の各頂点Ｐ₁，Ｐ₂を結ぶ線分Ｐ₁Ｐ₂がその凸包の辺であるか否かは、以下のようにして判断する。線分Ｐ₁Ｐ₂に垂直な直線λ上にその凸包の全ての頂点を正射影し、直線λ上において、それらの点の凸包である線分の両端のどちらかに線分Ｐ₁Ｐ₂を正射影した点があるならば、線分Ｐ₁Ｐ₂は辺であると判断できる。また、凸包Ｃ_ｊの各面は、凸包Ｃ_ｊの３辺の組合せで三角形を成す全ての組合せに対して、その三角形を含む平面に垂直な直線Λに凸包Ｃ_ｊの全ての頂点を正射影し、直線Λ上において、それらの点の凸包である線分の両端のどちらかにその三角形を正射影した点があるならば、その三角形は凸包Ｃ_ｊの面と判断できる。図７に、領域Ｄ_{１,ｊ−１}、Ｄ_１,ｊとそれらから計算された凸包Ｃ_ｊの例を示す。領域Ｄ_{１,ｊ−１}、Ｄ_１,ｊが実線で示された各時刻でのツール３に実質相当する領域であるとすると、凸包Ｃ_ｊは、領域Ｄ_{１,ｊ−１}、Ｄ_１,ｊに破線で示された領域を加えたものとなる。

次に、凸包Ｃ_ｊと多面体領域Ｄ_２との共通領域の有無を調べる（ステップ１１２）。多面体領域Ｄ_２は複数の凸多面体領域Ｄ_２,ｊの和で表されるので、凸包Ｃ_ｊと凸多面体Ｄ_２,ｊの組み合わせで共通領域を持つものがあれば、凸包Ｃ_ｊと多面体領域Ｄ_２に共通領域があると言える。凸包Ｃ_ｊと凸多面体領域Ｄ_２,ｊの何れの面も多角形なので、図８のように複数の三角形への分割（図８では六角形を４つの三角形Ｉ〜IVに分割）が可能である。すると、凸包Ｃ_ｊと凸多面体領域Ｄ_２,ｊの何れも三角形のみで覆われた閉領域となり、凸包Ｃ_ｊを覆う三角形と凸多面体領域Ｄ_２,ｊを覆う三角形の辺（線分）とが交差する組合せが１つ以上あるか、凸包Ｃ_ｊを覆う三角形の辺（線分）と凸多面体領域Ｄ_２,ｊを覆う三角形とが交差する組合せが１つ以上あるか、もしくは凸包Ｃ_ｊと凸多面体領域Ｄ_２,ｊのどちらかの領域がもう一方の領域を完全に含むか、これらの３つの条件の少なくとも１つが成立するか否かで、共通領域の有無の判断を行うことができる。

線分と三角形の交差は、以下のように判断する。空間上の線分と三角形について、線分の両端の点Ａ_１、Ａ_２の空間上の座標値をそれぞれα_１、α_２とし、三角形の頂点Ｂ_１、Ｂ_２、Ｂ_３の空間上の座標値をβ_１、β_２、β_３とする。α_１、α_２、β_１、β_２、β_３は３次元ベクトルで表される。まず、３×３行列（α_１−α_２、β_１−β_３、β_２−β_３）が正則行列であるか否かを計算し、正則である場合は線分Ａ_１Ａ_２と三角形Ｂ_１Ｂ_２Ｂ_３が平行ではなく、正則ではない場合は線分Ａ_１Ａ_２と三角形Ｂ_１Ｂ_２Ｂ_３が平行である、と判断する。

線分Ａ_１Ａ_２と三角形Ｂ_１Ｂ_２Ｂ_３が平行ではない場合は、線分Ａ_１Ａ_２を含む空間上の直線と三角形Ｂ_１Ｂ_２Ｂ_３を含む空間上の平面との交点の空間上の座標値をｘとすると、
ｘ＝ｓ_１α_１＋ｓ_２α_２、ｘ＝ｔ_１β_１＋ｔ_２β_２＋ｔ_３β_３
と一意に表すことができる。但し、ｓ_１、ｓ_２、ｔ_１、ｔ_２、ｔ_３は
ｓ_１＋ｓ_２＝１、ｔ_１＋ｔ_２＋ｔ_３＝１
を満たす実数値である。計算したｓ_１、ｓ_２、ｔ_１、ｔ_２、ｔ_３の５つの全ての値が０以上かつ１以下である場合は、線分Ａ_１Ａ_２と三角形Ｂ_１Ｂ_２Ｂ_３は交差するものと判断できる。一方、線分Ａ_１Ａ_２と三角形Ｂ_１Ｂ_２Ｂ_３が平行である場合は点Ａ_１と平面Ｂ_１Ｂ_２Ｂ_３の距離を計算し、その距離が０ならば、線分Ａ_１Ａ_２と三角形Ｂ_１Ｂ_２Ｂ_３が同じ平面上にあることになる。

線分Ａ_１Ａ_２と三角形Ｂ_１Ｂ_２Ｂ_３が同じ平面上にある場合は、２×２行列（α_１−α_２、β_１−β_２）が正則行列ならば、線分Ａ_１Ａ_２と三角形Ｂ_１Ｂ_２Ｂ_３の辺Ｂ_１Ｂ_２とは平行ではないので、線分Ａ_１Ａ_２を含む空間上の直線と辺Ｂ_１Ｂ_２を含む空間上の直線との交点の座標値をｙとすると、
ｙ＝ｓ_１α_１＋ｓ_２α_２、ｘ＝ｔ_１β_１＋ｔ_２β_２
と一意に表すことができる。但し、ｓ_１、ｓ_２、ｔ_１、ｔ_２は
ｓ_１＋ｓ_２＝１、ｔ_１＋ｔ_２＝１
を満たす実数値である。計算したｓ_１、ｓ_２、ｔ_１、ｔ_２の４つの全ての値が０以上かつ１以下である場合は、線分Ａ_１Ａ_２と辺Ｂ_１Ｂ_２が交差するので、線分Ａ_１Ａ_２と三角形Ｂ_１Ｂ_２Ｂ_３は交差するものと判断できる。線分Ａ_１Ａ_２と三角形Ｂ_１Ｂ_２Ｂ_３が同じ平面上にある場合は、辺Ｂ_２Ｂ_３と辺Ｂ_３Ｂ_１についても辺Ｂ_１Ｂ_２と同様に、線分Ａ_１Ａ_２との交差を調べ、線分Ａ_１Ａ_２と三角形Ｂ_１Ｂ_２Ｂ_３の交差を判断する。何れの交差の判断でも線分Ａ_１Ａ_２と三角形Ｂ_１Ｂ_２Ｂ_３が交差するとの判断が得られなければ、線分Ａ_１Ａ_２と三角形Ｂ_１Ｂ_２Ｂ_３は交差しないと判断する。

凸包Ｃ_ｊが凸多面体領域Ｄ_２,ｊを完全に含むか否かは、凸多面体領域Ｄ_２,ｊの全頂点に関して凸包Ｃ_ｊの重心とその頂点を結ぶ線分が凸包Ｃ_ｊの表面と交わらないか、そうでないかで判断することができる。凸多面体領域Ｄ_２,ｊが凸包Ｃ_ｊを完全に含むか否かも、同様にして判断することができる。

上述のような共通領域の有無の判断の結果、凸包Ｃ_ｊと多面体領域Ｄ_２との共通領域が有る場合は、移動経路中でツール３と収納箱４とが干渉すると判断（ステップ１１３）して処理を終了し、共通領域が無い場合は、ｊ＜ｎ（ステップ１１４）か否かを調べる。

ステップ１１４において、ｊ＜ｎならば、ステップ１０９に進んでｊに（ｊ＋１）を設定し（ｊ←ｊ＋１）、ステップ１０５に戻る。一方、ステップ１１４においてｊ≧ｎならば、移動経路中でツール３と収納箱４とが干渉しないと判断（ステップ１１５）して処理を終了する。

図９は、図５に示した時点Ｔ_２と時点Ｔ_３の間の部分を拡大したものであり、時点Ｔ_２、Ｔ_３の各々でツール３が占有する２つの領域の凸包Ｃ_３と多面体領域Ｄ_２との共通領域が有ることになる。よって、図５の移動経路の場合、干渉予測装置はツール３と収納箱４とが干渉すると予測・判断する。

凸包Ｃ_ｊと領域Ｄ_２の共通領域の有無の判断の計算に要する時間、２つの領域Ｄ_{１,ｊ−１}、Ｄ_１,ｊから凸包Ｃ_ｊを計算するのに要する時間は、それぞれ１〜２ミリ秒（ｍｓ）を要する。図５の移動経路の場合、ロボットの移動時間の分割数ｎが４なので、干渉予測に要する計算時間は、最大で（２＋２）×４＝１６ｍｓ程度となり、ロボットの動作に差し支える程の時間にはならない。先に、直線度Ｌ_ｊの値が１に近く、ツール３が占有する領域Ｄ_１の凸包の各頂点の移動を表す折れ線Ｐ_ｊ−１Ｐ_ｊＰ_ｊ＋１が直線に近い場合、ステップ１０７で時点の列から時点Ｔ_ｊを削除しても、干渉予測の精度は殆ど低下しないと述べたが、それは以下の理由に依る。折れ線Ｐ_ｊ−１Ｐ_ｊＰ_ｊ＋１が直線に近い場合は、領域Ｄ_{１,ｊ−１}、Ｄ_１,ｊの凸包と領域Ｄ_１,ｊ、Ｄ_{１,ｊ＋１}の凸包との和が、領域Ｄ_{１,ｊ−１}、Ｄ_{１,ｊ＋１}の凸包とほぼ一致する。したがって、凸包と多面体領域Ｄ_２との共通領域の有無により干渉を判断する場合においては、時点の列から時点Ｔ_ｊを削除して領域Ｄ_１,ｊの計算を省いても、干渉予測の精度は殆ど低下しない。

以上述べてきたように、本発明では、移動経路上の幾つかの補間位置において着目部位が占有する領域を求め、隣り合う補間位置における領域同士の凸包を全て求め、周辺物を凸多面体の和で表し、それら全ての凸包と全ての凸多面体との干渉を調べることによって、移動経路上での着目部位と周辺物との干渉を予測する。

１ レーザ投光器
２ カメラ
３ ツール
４ 収納箱
５ ロボット
６ ロボット制御装置
７ ワークピース

Claims (2)

1. 空間に固定されている周辺物の形状を多面体で近似した周辺物近似多面体が前記空間を占有する領域である周辺物占有領域と、ロボットの着目部位の形状を多面体で近似した着目部位近似多面体とを記憶し、
   前記ロボットの動作に伴って、前記着目部位近似多面体が複数の時刻の各々において前記空間を占有する領域である、複数の着目部位占有領域を計算し、
   前記複数の着目部位占有領域と前記周辺物占有領域とに基づいて、前記ロボットの着目部位と前記周辺物との干渉の有無を予測する、干渉予測方法であって、
   前記複数の着目部位占有領域を前記複数の時刻の順に並べたときに、隣り合う着目部位占有領域同士の組合せの凸包を全て計算し、
   前記周辺物占有領域を凸多面体の和として計算し、
   全ての前記凸包と前記凸多面体との共有領域の有無を、前記ロボットの着目部位と前記周辺物との干渉の有無として判定する、干渉予測方法。
2. 空間に固定されている周辺物の形状を多面体で近似した周辺物近似多面体が前記空間を占有する領域である周辺物占有領域と、ロボットの着目部位の形状を多面体で近似した着目部位近似多面体とを記憶する記憶部と、
   前記ロボットの動作に伴って、前記着目部位近似多面体が複数の時刻の各々において前記空間を占有する領域である、複数の着目部位占有領域を計算する着目部位占有領域計算部と、
   前記複数の着目部位占有領域と前記周辺物占有領域とに基づいて、前記ロボットの着目部位と前記周辺物との干渉の有無を予測する干渉予測部と、
   を備える干渉予測装置であって、
   前記干渉予測部は、
   前記複数の着目部位占有領域を前記複数の時刻の順に並べたときに、隣り合う着目部位占有領域同士の組合せの凸包を全て計算する凸包計算部と、
   前記周辺物占有領域を凸多面体の和として計算する周辺物占有領域計算部と、を有し、
   前記干渉予測部は、全ての前記凸包と前記凸多面体との共有領域の有無を、前記ロボットの着目部位と前記周辺物との干渉の有無として判定する、干渉予測装置。

Images