ロボットの着目部位と周辺物との干渉を予測する方法及び装置
【課題】ロボットの手先に搭載されているツールやセンサ等の着目部位がロボットの動作に伴って移動する場合に、移動経路上での着目部位とロボットの周りに固定設置されている周辺物との干渉を、実用的な精度及び計算速度にて予測する方法及び装置の提供。
【解決手段】各時点Ｔｊでツール３が占有する領域をＤ１,ｊとして、隣接する時点Ｔｊ−１、Ｔｊの領域Ｄ１,ｊ−１、Ｄ１,ｊの凸包Ｃｊを計算する（ステップ１１１）。凸包Ｃｊと多面体領域Ｄ２との共通領域の有無を調べる（ステップ１１２）。凸包Ｃｊと多面体領域Ｄ２との共通領域が有る場合は、移動経路中でツール３と収納箱４とが干渉すると判断（ステップ１１３）して処理を終了し、共通領域が無い場合は、ｊ＜ｎ（ステップ１１４）か否かを調べ、ｊ≧ｎならば、移動経路中でツール３と収納箱４とが干渉しないと判断（ステップ１１５）して処理を終了する。
【解決手段】各時点Ｔｊでツール３が占有する領域をＤ１,ｊとして、隣接する時点Ｔｊ−１、Ｔｊの領域Ｄ１,ｊ−１、Ｄ１,ｊの凸包Ｃｊを計算する（ステップ１１１）。凸包Ｃｊと多面体領域Ｄ２との共通領域の有無を調べる（ステップ１１２）。凸包Ｃｊと多面体領域Ｄ２との共通領域が有る場合は、移動経路中でツール３と収納箱４とが干渉すると判断（ステップ１１３）して処理を終了し、共通領域が無い場合は、ｊ＜ｎ（ステップ１１４）か否かを調べ、ｊ≧ｎならば、移動経路中でツール３と収納箱４とが干渉しないと判断（ステップ１１５）して処理を終了する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットの手先に搭載されているツールやセンサ等の着目部位がロボットの動作に伴って移動する場合において、その移動経路上での着目部位と、ロボットの周りに固定設置されている周辺機器や柵等の周辺物との干渉を予測する方法及び装置に関する。
【背景技術】
【０００２】
視覚センサを用いてロボットがワークピースを取出すピッキング作業の場合、視覚センサによって位置決めされていないワークピースの位置姿勢を計測し、その計測結果を基にワークピースを把持できる位置姿勢へのロボットの移動経路を生成することが行われている。ワークピースの位置姿勢が変化するとロボットの移動経路も変化するため、ロボットの手先に搭載されているツールやセンサ等の着目部位と、ロボットの周りに固定設置されている周辺機器や柵等の周辺物との移動経路上での干渉の有無も変化する。
【０００３】
複数のワークピースがバラ積みされている場合、視覚センサによって計測したそれぞれのワークピースに対して、ロボットの移動経路が生成される。生成された複数の移動経路について干渉予測を行い、干渉しないと判断された移動経路に対応するワークピースをロボットに把持させることが好ましい。ここで、干渉を予測する従来技術としては、移動経路の移動終了位置でのロボットの着目部位と周辺物との干渉の確認を行うものがある。例えば特許文献１には、スカラ型ロボットの干渉チェック方法が開示されており、ここでは、ロボットの目標位置についての指令を各関節軸のストローク値に変換し、各関節軸のストローク限についてのチェックを行うとともに、ロボットの目標位置についての指令を直交座標空間上の位置に変換し、直交座標空間上において前記干渉領域についてのチェックを行い、目標位置が前記ストローク限及び前記干渉領域についての制限を満足することが確認された場合に、各関節軸に動作指令を与える、とされている。
【０００４】
また特許文献２には、ロボットと他の物体のとの干渉を回避するための干渉回避装置が開示されており、その〔００２４〕には、ツール３の形状情報及び収納箱４の形状情報と位置姿勢情報、更に、教示時のワークピース１０の位置姿勢Ｗｎ、ツールの位置姿勢Ｔｎをロボット制御装置６のメモリに設定記憶させておき、上記１式で求められたツールの位置姿勢Ｔａのツール形状情報と、収納箱４の形状情報と位置姿勢情報に基づいて、ツール３と収納箱４が干渉するか否かを判断することができる、と記載されている。
【０００５】
干渉を予測する他の従来技術として、特許文献３には、教示作業が自動化されているロボットの制御装置及び制御システムが開示されており、ここでは、所定作業に対するロボットの動作をシミュレーションして作業経路における部材との干渉の有無をチェックし、ロボットアームあるいはその先端に装着されたツールが部材と干渉すると判定された場合において、干渉区間長が所定値を超えていなければ、干渉個所でロボットの作業経路を分断し、その分断された作業経路内においてロボットを動作させるようにする、とされている。
【０００６】
また他の従来技術として、特許文献４には、非凸多面体の凸包を生成し、該凸包と他の物体間の干渉チェックを行い、干渉し始めた時、凸包を解除して前記非凸多面体と他の物体間の干渉チェックを行う干渉チェック方法が開示されており、該方法は、非凸多面体を構成する頂点を、第１軸（例えばＸ軸）の座標値の差が零あるいは微小量ε以内の複数の頂点群に分割するステップ、各頂点群について第２−第３軸平面（例えばＹＺ平面）上で２次元凸包を生成するステップ、隣接する２次元凸包を併合して３次元凸包を生成し、以後、隣接する３次元凸包を順次併合して目的とする非凸多面体の凸包を生成するステップ、を有する、とされている。
【０００７】
さらなる他の従来技術として、特許文献５には、複数のロボットの動作をシミュレートするシミュレーション装置が開示されており、該シミュレーション装置は、１台ごとのロボットモデルの一定時間動作した軌跡の形状を表す複数の多面体の座標値とシミュレーション時刻を記録する軌跡情報記録手段と、前記複数のロボットの軌跡の形状を表す複数の多面体の、三次元座標値と時刻とを、それぞれスプレッドシートに入力し、複数シート間の同じ時刻で空間的に干渉する多面体を計算により検出する干渉検出手段とを備えるとされている。
【先行技術文献】
【特許文献】
【０００８】
【特許文献１】特開２０００−３２６２８０号公報
【特許文献２】特開２００２−３３１４８０号公報
【特許文献３】特開２００２−２７３６７５号公報
【特許文献４】特開平９−２７０４６号公報
【特許文献５】特開２００４−２８０６３５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００９】
特許文献１又は２に記載の技術では、移動終了位置での干渉は確認できても、移動経路上での干渉を確認・予測することは難しい。例えば、特許文献２の図２は、収納箱４の中にあるワークピース７を取出す時のツール３の移動経路を示すものであるが、移動終了時刻Ｔ４でのツール３と収納箱４とは干渉しないが、移動中の時刻Ｔ２とＴ３の間にツール３と周辺物４とが干渉してしまう場合がある。
【００１０】
一方、特許文献３に記載の技術は、生成された移動経路上での干渉を高い精度で予測するために、移動経路上の全補間位置について、逐次的にロボットの着目部位と周辺物とが干渉するか否かを調べるものと解される。しかしこの技術では、計算時間が過度に長くなってしまい、ロボット動作中にリアルタイムで干渉を予測するには適さないと思われる。
【００１１】
特許文献４に記載の技術は、干渉予測に際しいわゆる凸包を利用するものであるが、これは静止状態にある多面体同士の干渉を予測するものであり、移動する多面体には適用できない。
【００１２】
また特許文献５に記載の技術は、複数のロボットや加工機械・治具が同時に動作する際に、該ロボット間等の干渉判定を、それぞれの動作領域を覆う多面体を計算し、該多面体同士の干渉判定に置き換えるものと解される。しかし、動作領域に対応する多面体が凸多面体に限定されないため、干渉判定計算が複雑で時間が掛かる場合があり、ロボット動作中にリアルタイムで干渉判定を行うには適さない。
【００１３】
そこで本発明は、ロボットの手先に搭載されているツールやセンサ等の着目部位がロボットの動作に伴って移動する場合に、移動経路上での着目部位とロボットの周りに固定設置されている周辺機器や柵等の周辺物との干渉を、実用的な精度及び計算速度にて予測する干渉予測方法及び干渉予測装置を提供することを目的とする。
【課題を解決するための手段】
【００１４】
上記目的を達成するために、請求項１に記載の発明は、空間に固定されている周辺物の形状を多面体で近似した周辺物近似多面体が前記空間を占有する領域である周辺物占有領域と、ロボットの着目部位の形状を多面体で近似した着目部位近似多面体とを記憶し、前記ロボットの動作に伴って、前記着目部位近似多面体が複数の時刻の各々において前記空間を占有する領域である、複数の着目部位占有領域を計算し、前記複数の着目部位占有領域と前記周辺物占有領域とに基づいて、前記ロボットの着目部位と前記周辺物との干渉の有無を予測する、干渉予測方法であって、前記複数の着目部位占有領域を前記複数の時刻の順に並べたときに、隣り合う着目部位占有領域同士の組合せの凸包を全て計算し、前記周辺物占有領域を凸多面体の和として計算し、全ての前記凸包と前記凸多面体との共有領域の有無を、前記ロボットの着目部位と前記周辺物との干渉の有無として判定する、干渉予測方法を提供する。
【００１５】
請求項２に記載の発明は、空間に固定されている周辺物の形状を多面体で近似した周辺物近似多面体が前記空間を占有する領域である周辺物占有領域と、ロボットの着目部位の形状を多面体で近似した着目部位近似多面体とを記憶する記憶部と、前記ロボットの動作に伴って、前記着目部位近似多面体が複数の時刻の各々において前記空間を占有する領域である、複数の着目部位占有領域を計算する着目部位占有領域計算部と、前記複数の着目部位占有領域と前記周辺物占有領域とに基づいて、前記ロボットの着目部位と前記周辺物との干渉の有無を予測する干渉予測部と、を備える干渉予測装置であって、前記干渉予測部は、前記複数の着目部位占有領域を前記複数の時刻の順に並べたときに、隣り合う着目部位占有領域同士の組合せの凸包を全て計算する凸包計算部と、前記周辺物占有領域を凸多面体の和として計算する周辺物占有領域計算部と、を有し、前記干渉予測部は、全ての前記凸包と前記凸多面体との共有領域の有無を、前記ロボットの着目部位と前記周辺物との干渉の有無として判定する、干渉予測装置を提供する。
【発明の効果】
【００１６】
本発明によれば、動作直前に実用的な精度及び計算速度で自動的に干渉を予測できるので、バラ積みされたワークピースの取出し等の用途において、干渉を避けながらロボットを動作させることができる。また本願発明では、ロボットの着目部位の隣り合う補間位置における領域同士の凸包（凸多面体の範疇）を求め、周辺物を凸多面体の和で表し、干渉判定を凸多面体同士のそれに置き換え、計算の簡素化・高速化を図り、ロボット動作中のリアルタイムでの干渉判定を実現している。
【図面の簡単な説明】
【００１７】
【図１】本発明に係る干渉予測装置の一実施形態の概略構成を示す図である。
【図２】ツール及び収納箱を多面体で近似した例を示す図である。
【図３】凸多面体の例である正四面体にインデックスを割り当てた例を示す図である。
【図４】本発明に係る干渉予測装置又は干渉予測方法による、干渉予測手順を示すフローチャートである。
【図５】分割数が４である場合の各時点でのツールの位置姿勢と、収納箱と、ワークピースとの位置関係を表した図である。
【図６】各時点での多面体の頂点の直線度を求める例を説明する図である。
【図７】多面体領域から凸包を求める例を説明する図である。
【図８】多角形を複数の三角形に分割した例を示す図である。
【図９】凸包と周辺物を示す多面体との位置関係を示す図である。
【発明を実施するための形態】
【００１８】
図１は、本発明に係る干渉予測装置の一実施形態の概要図である。なお、本願発明における「ロボットの着目部位」とは、ロボットアームの本体又は付属物のうち、動作中に周辺機器と干渉する可能性のある部位をいう。本実施形態では、ロボット制御装置６内に干渉予測のためのソフトウエアを設けることによって、干渉予測装置を構成している。ロボット５の手首先端には、ワークピース７を取出すためのツール３、ワークピース７を撮像するためのカメラ２、及びワークピース７にレーザ光を投光するレーザ投光器１が取り付けられている。カメラ２及びレーザ投光器１によって、ワークピース７の位置及び姿勢（以降、位置姿勢と称する）を検出する３次元視覚センサが構成される。本実施形態では、ワークピース７は収納箱４内に収納されている。この収納箱４には複数のワークピース７がバラ積み状態で収納されており、故に各々のワークピース７はその位置姿勢が不規則な状態である。
【００１９】
ロボット５、レーザ投光器１、カメラ２及びツール３は従来周知のものでよく、故に詳細な説明は省略する。ロボット制御装置６は、プロセッサ、ＲＯＭ、ＲＡＭ、不揮発性ＲＡＭ、表示装置付き入力手段、入出力インタフェース、カメラインタフェース、ロボット各関節軸のサーボモータ等を駆動制御するサーボ制御器等を備える点では、従来のロボット制御装置と同様でよい。図示例のロボット５の関節軸の数は６つであり、ツール３の位置姿勢が命令されると、ロボット制御装置６は各関節軸の回転角を逆算し、ツール３がその位置姿勢となるようにロボット５の各軸の動作を制御する。また、入出力インタフェースにはレーザ投光器１及びツール３が接続され、カメラインタフェースにはカメラ２が接続される。
【００２０】
ロボット５の手首先端に対するツール３の位置姿勢と、ツール３の形状と、収納箱４の位置姿勢と、収納箱４の形状と、教示時のワークピース７の位置姿勢と、その位置姿勢に対してワークピース７を把持する際のツールの位置姿勢とは、ロボット制御装置６のメモリに予め設定・記憶させておくものとする。その際、記憶するツール３及び収納箱４の形状に関して、図２に示すように、ツール３は２つの直方体を組み合わせて多面体で近似した領域Ｄ１で表し、収納箱４は直方体の上側から少し小さめの直方体をくり抜いた多面体で近似した領域Ｄ２で表すものとする。
【００２１】
領域Ｄ１、Ｄ２は共に複数の直方体領域の和として表現することができるが、干渉予測の計算が簡単になるように、近似する領域は凸多面体の和で表せるようにする。具体的には、それぞれの多面体領域の持つ頂点、辺の各々にインデックスを割り振り、各頂点の空間上の座標値、各辺の両端にある頂点のインデックス、各面の端にある辺のインデックスを記憶する。例えば、近似された領域が、図３に示すような各辺の長さが３である正四面体（凸多面体）の場合、各頂点及び各辺にインデックスを割り振り、ある座標系における頂点Ｖ１、頂点Ｖ２、頂点Ｖ３、頂点Ｖ４それぞれの座標値（0.000, 0.000, 2.449）、（1.732, 0.000, 0.000）、（-0.866, 1.500, 0.000）、（-0.866, -1.500, 0.000）と、辺Ｅ１、辺Ｅ２、辺Ｅ３、辺Ｅ４、辺Ｅ５、辺Ｅ６それぞれの両端の頂点のインデックス（Ｖ１, Ｖ２）、（Ｖ１, Ｖ4）、（Ｖ１, Ｖ3）、（Ｖ２, Ｖ4）、（Ｖ３, Ｖ４）、（Ｖ２, Ｖ3）と、面Ｓ１、面Ｓ２、面Ｓ３、面Ｓ４それぞれの端にある辺のインデックス（Ｅ１, Ｅ２, Ｅ４）、（Ｅ２, Ｅ３, Ｅ５）、（Ｅ１, Ｅ３, Ｅ６）、（Ｅ４, Ｅ５, Ｅ６）を記憶することになる。
【００２２】
図４は、ロボット５を用いたワークピース７の取出し作業において、ツール３と収納箱４との干渉が起こるか否かを判断する処理手順を示すフローチャートである。処理手順では、まず、ロボット制御装置６からの指令により、ロボット５を収納箱４内のワークピース７を撮像できる位置姿勢に移動させた後、レーザ投光器１及びカメラ２を用いて、収納箱４内のワークピース７の位置姿勢を計測する（ステップ１０１）。
【００２３】
次に、この計測したワークピース７の位置姿勢と、予め記憶しておいた教示時のワークピース７の位置姿勢と、予め記憶しておいたワークピースを把持する際のツールの位置姿勢とを合わせて、計測したワークピース７をツール３で把持して収納箱から取出すことのできるツール３の位置姿勢を算出する。そして、ツール３に関して、ワークピース７を計測した時の位置姿勢を移動開始時の位置姿勢とし、計測したワークピース７をツール３で把持して収納箱から取出すことのできる位置姿勢を移動終了時の位置姿勢とした、ツール３の移動経路を生成する（ステップ１０２）。移動経路の生成方法には様々なものがあるが、例えば、ロボット各関節軸の回転速度が常に一定となるような移動経路の生成方法がある。即ち、移動開始時のロボット５の各関節軸ｋ（ｋ＝１，２，・・・，６）の回転角の値をθｋとし、移動終了時の各関節軸ｋの回転角の値をφｋとし、移動開始時刻を０（ゼロ）とし、移動終了時刻をＴとした場合に、時刻ｔ（０≦ｔ≦Ｔ）における各関節軸ｋの回転角がθｋ＋Δｋ×ｔとなるような移動経路を生成する方法である。但し、Δｋ＝（φｋ−θｋ）／Ｔである。但しこのような移動経路の生成方法は一例であって、ここで述べる干渉予測の適用は、移動経路の生成方法に依るものではない。
【００２４】
次に、移動開始から移動終了までの移動時間をｎ等分し、得られたｎ＋１個の時刻（時点）をＴ０, Ｔ１,・・・, Ｔｎ とする（ステップ１０３）。Ｔ０ および Ｔｎ はそれぞれ移動開始時刻と移動終了時刻である。分割数ｎは、干渉予測の粗精や処理時間を考慮して、予め与えておいてもよいし、移動時間及び移動経路長とから逐次決定してもよい。分割数を大きくすれば干渉予測の精度は向上するが、処理時間は分割数にほぼ比例して大きくなる。リアルタイムで干渉予測の処理を行わなければならないため、処理時間はできるだけ短い方がよい。
【００２５】
図５は、分割数ｎ＝４である場合の各時点でのツール３の位置姿勢と、収納箱４と、ワークピース７との位置関係を表したものである。ここではツール３の移動時間を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等により求める。次に、凸包Ｃｊの各辺は、凸包Ｃｊの全ての２頂点の組合せ（Ｖ1，Ｖ2）に対して、線分Ｖ1Ｖ2が凸包Ｃｊの辺であるかの判断を以下の手順で行って求める。空間上の直線Ｖ1Ｖ2に垂直な平面Πに凸包Ｃｊの全ての頂点を正射影する。この時、頂点Ｖ1，Ｖ2は平面Π上の同一点Ｐに射影される。平面Π上において、正射影した全ての点の凸包の頂点か辺の上に点Ｐがあるならば、線分Ｖ1Ｖ2は凸包Ｃｊの辺であると判断できる。平面上の凸包の頂点は同様にQuickHullにより計算可能である。
【００３３】
また、平面上において、凸包の各頂点Ｐ1，Ｐ2を結ぶ線分Ｐ1Ｐ2がその凸包の辺であるか否かは、以下のようにして判断する。線分Ｐ1Ｐ2に垂直な直線λ上にその凸包の全ての頂点を正射影し、直線λ上において、それらの点の凸包である線分の両端のどちらかに線分Ｐ1Ｐ2を正射影した点があるならば、線分Ｐ1Ｐ2は辺であると判断できる。また、凸包Ｃｊの各面は、凸包Ｃｊの３辺の組合せで三角形を成す全ての組合せに対して、その三角形を含む平面に垂直な直線Λに凸包Ｃｊの全ての頂点を正射影し、直線Λ上において、それらの点の凸包である線分の両端のどちらかにその三角形を正射影した点があるならば、その三角形は凸包Ｃｊの面と判断できる。図７に、領域Ｄ１,ｊ−１、Ｄ１,ｊとそれらから計算された凸包Ｃｊの例を示す。領域Ｄ１,ｊ−１、Ｄ１,ｊが実線で示された各時刻でのツール３に実質相当する領域であるとすると、凸包Ｃｊは、領域Ｄ１,ｊ−１、Ｄ１,ｊに破線で示された領域を加えたものとなる。
【００３４】
次に、凸包Ｃｊと多面体領域Ｄ２との共通領域の有無を調べる（ステップ１１２）。多面体領域Ｄ２は複数の凸多面体領域Ｄ２,ｊの和で表されるので、凸包Ｃｊと凸多面体Ｄ２,ｊの組み合わせで共通領域を持つものがあれば、凸包Ｃｊと多面体領域Ｄ２に共通領域があると言える。凸包Ｃｊと凸多面体領域Ｄ２,ｊの何れの面も多角形なので、図８のように複数の三角形への分割（図８では六角形を４つの三角形Ｉ〜IVに分割）が可能である。すると、凸包Ｃｊと凸多面体領域Ｄ２,ｊの何れも三角形のみで覆われた閉領域となり、凸包Ｃｊを覆う三角形と凸多面体領域Ｄ２,ｊを覆う三角形の辺（線分）とが交差する組合せが１つ以上あるか、凸包Ｃｊを覆う三角形の辺（線分）と凸多面体領域Ｄ２,ｊを覆う三角形とが交差する組合せが１つ以上あるか、もしくは凸包Ｃｊと凸多面体領域Ｄ２,ｊのどちらかの領域がもう一方の領域を完全に含むか、これらの３つの条件の少なくとも１つが成立するか否かで、共通領域の有無の判断を行うことができる。
【００３５】
線分と三角形の交差は、以下のように判断する。空間上の線分と三角形について、線分の両端の点Ａ１、Ａ２の空間上の座標値をそれぞれα１、α２とし、三角形の頂点Ｂ１、Ｂ２、Ｂ３の空間上の座標値をβ１、β２、β３とする。α１、α２、β１、β２、β３は３次元ベクトルで表される。まず、３×３行列（α１−α２、β１−β３、β２−β３）が正則行列であるか否かを計算し、正則である場合は線分Ａ１Ａ２と三角形Ｂ１Ｂ２Ｂ３が平行ではなく、正則ではない場合は線分Ａ１Ａ２と三角形Ｂ１Ｂ２Ｂ３が平行である、と判断する。
【００３６】
線分Ａ１Ａ２と三角形Ｂ１Ｂ２Ｂ３が平行ではない場合は、線分Ａ１Ａ２を含む空間上の直線と三角形Ｂ１Ｂ２Ｂ３を含む空間上の平面との交点の空間上の座標値をｘとすると、
ｘ＝ｓ１α１＋ｓ２α２、ｘ＝ｔ１β１＋ｔ２β２＋ｔ３β３
と一意に表すことができる。但し、ｓ１、ｓ２、ｔ１、ｔ２、ｔ３は
ｓ１＋ｓ２＝１、ｔ１＋ｔ２＋ｔ３＝１
を満たす実数値である。計算したｓ１、ｓ２、ｔ１、ｔ２、ｔ３の５つの全ての値が０以上かつ１以下である場合は、線分Ａ１Ａ２と三角形Ｂ１Ｂ２Ｂ３は交差するものと判断できる。一方、線分Ａ１Ａ２と三角形Ｂ１Ｂ２Ｂ３が平行である場合は点Ａ１と平面Ｂ１Ｂ２Ｂ３の距離を計算し、その距離が０ならば、線分Ａ１Ａ２と三角形Ｂ１Ｂ２Ｂ３が同じ平面上にあることになる。
【００３７】
線分Ａ１Ａ２と三角形Ｂ１Ｂ２Ｂ３が同じ平面上にある場合は、２×２行列（α１−α２、β１−β２）が正則行列ならば、線分Ａ１Ａ２と三角形Ｂ１Ｂ２Ｂ３の辺Ｂ１Ｂ２とは平行ではないので、線分Ａ１Ａ２を含む空間上の直線と辺Ｂ１Ｂ２を含む空間上の直線との交点の座標値をｙとすると、
ｙ＝ｓ１α１＋ｓ２α２、ｘ＝ｔ１β１＋ｔ２β２
と一意に表すことができる。但し、ｓ１、ｓ２、ｔ１、ｔ２は
ｓ１＋ｓ２＝１、ｔ１＋ｔ２＝１
を満たす実数値である。計算したｓ１、ｓ２、ｔ１、ｔ２の４つの全ての値が０以上かつ１以下である場合は、線分Ａ１Ａ２と辺Ｂ１Ｂ２が交差するので、線分Ａ１Ａ２と三角形Ｂ１Ｂ２Ｂ３は交差するものと判断できる。線分Ａ１Ａ２と三角形Ｂ１Ｂ２Ｂ３が同じ平面上にある場合は、辺Ｂ２Ｂ３と辺Ｂ３Ｂ１についても辺Ｂ１Ｂ２と同様に、線分Ａ１Ａ２との交差を調べ、線分Ａ１Ａ２と三角形Ｂ１Ｂ２Ｂ３の交差を判断する。何れの交差の判断でも線分Ａ１Ａ２と三角形Ｂ１Ｂ２Ｂ３が交差するとの判断が得られなければ、線分Ａ１Ａ２と三角形Ｂ１Ｂ２Ｂ３は交差しないと判断する。
【００３８】
凸包Ｃｊが凸多面体領域Ｄ２,ｊを完全に含むか否かは、凸多面体領域Ｄ２,ｊの全頂点に関して凸包Ｃｊの重心とその頂点を結ぶ線分が凸包Ｃｊの表面と交わらないか、そうでないかで判断することができる。凸多面体領域Ｄ２,ｊが凸包Ｃｊを完全に含むか否かも、同様にして判断することができる。
【００３９】
上述のような共通領域の有無の判断の結果、凸包Ｃｊと多面体領域Ｄ２との共通領域が有る場合は、移動経路中でツール３と収納箱４とが干渉すると判断（ステップ１１３）して処理を終了し、共通領域が無い場合は、ｊ＜ｎ（ステップ１１４）か否かを調べる。
【００４０】
ステップ１１４において、ｊ＜ｎならば、ステップ１０９に進んでｊに（ｊ＋１）を設定し（ｊ←ｊ＋１）、ステップ１０５に戻る。一方、ステップ１１４においてｊ≧ｎならば、移動経路中でツール３と収納箱４とが干渉しないと判断（ステップ１１５）して処理を終了する。
【００４１】
図９は、図５に示した時点Ｔ２と時点Ｔ３の間の部分を拡大したものであり、時点Ｔ２、Ｔ３の各々でツール３が占有する２つの領域の凸包Ｃ３と多面体領域Ｄ２との共通領域が有ることになる。よって、図５の移動経路の場合、干渉予測装置はツール３と収納箱４とが干渉すると予測・判断する。
【００４２】
凸包Ｃｊと領域Ｄ２の共通領域の有無の判断の計算に要する時間、２つの領域Ｄ１,ｊ−１、Ｄ１,ｊから凸包Ｃｊを計算するのに要する時間は、それぞれ１〜２ミリ秒（ｍｓ）を要する。図５の移動経路の場合、ロボットの移動時間の分割数ｎが４なので、干渉予測に要する計算時間は、最大で（２＋２）×４＝１６ｍｓ程度となり、ロボットの動作に差し支える程の時間にはならない。先に、直線度Ｌｊの値が１に近く、ツール３が占有する領域Ｄ１の凸包の各頂点の移動を表す折れ線Ｐｊ−１ＰｊＰｊ＋１が直線に近い場合、ステップ１０７で時点の列から時点Ｔｊを削除しても、干渉予測の精度は殆ど低下しないと述べたが、それは以下の理由に依る。折れ線Ｐｊ−１ＰｊＰｊ＋１が直線に近い場合は、領域Ｄ１,ｊ−１、Ｄ１,ｊの凸包と領域Ｄ１,ｊ、Ｄ１,ｊ＋１の凸包との和が、領域Ｄ１,ｊ−１、Ｄ１,ｊ＋１の凸包とほぼ一致する。したがって、凸包と多面体領域Ｄ２との共通領域の有無により干渉を判断する場合においては、時点の列から時点Ｔｊを削除して領域Ｄ１,ｊの計算を省いても、干渉予測の精度は殆ど低下しない。
【００４３】
以上述べてきたように、本発明では、移動経路上の幾つかの補間位置において着目部位が占有する領域を求め、隣り合う補間位置における領域同士の凸包を全て求め、周辺物を凸多面体の和で表し、それら全ての凸包と全ての凸多面体との干渉を調べることによって、移動経路上での着目部位と周辺物との干渉を予測する。
【符号の説明】
【００４４】
１ レーザ投光器
２ カメラ
３ ツール
４ 収納箱
５ ロボット
６ ロボット制御装置
７ ワークピース
【技術分野】
【０００１】
本発明は、ロボットの手先に搭載されているツールやセンサ等の着目部位がロボットの動作に伴って移動する場合において、その移動経路上での着目部位と、ロボットの周りに固定設置されている周辺機器や柵等の周辺物との干渉を予測する方法及び装置に関する。
【背景技術】
【０００２】
視覚センサを用いてロボットがワークピースを取出すピッキング作業の場合、視覚センサによって位置決めされていないワークピースの位置姿勢を計測し、その計測結果を基にワークピースを把持できる位置姿勢へのロボットの移動経路を生成することが行われている。ワークピースの位置姿勢が変化するとロボットの移動経路も変化するため、ロボットの手先に搭載されているツールやセンサ等の着目部位と、ロボットの周りに固定設置されている周辺機器や柵等の周辺物との移動経路上での干渉の有無も変化する。
【０００３】
複数のワークピースがバラ積みされている場合、視覚センサによって計測したそれぞれのワークピースに対して、ロボットの移動経路が生成される。生成された複数の移動経路について干渉予測を行い、干渉しないと判断された移動経路に対応するワークピースをロボットに把持させることが好ましい。ここで、干渉を予測する従来技術としては、移動経路の移動終了位置でのロボットの着目部位と周辺物との干渉の確認を行うものがある。例えば特許文献１には、スカラ型ロボットの干渉チェック方法が開示されており、ここでは、ロボットの目標位置についての指令を各関節軸のストローク値に変換し、各関節軸のストローク限についてのチェックを行うとともに、ロボットの目標位置についての指令を直交座標空間上の位置に変換し、直交座標空間上において前記干渉領域についてのチェックを行い、目標位置が前記ストローク限及び前記干渉領域についての制限を満足することが確認された場合に、各関節軸に動作指令を与える、とされている。
【０００４】
また特許文献２には、ロボットと他の物体のとの干渉を回避するための干渉回避装置が開示されており、その〔００２４〕には、ツール３の形状情報及び収納箱４の形状情報と位置姿勢情報、更に、教示時のワークピース１０の位置姿勢Ｗｎ、ツールの位置姿勢Ｔｎをロボット制御装置６のメモリに設定記憶させておき、上記１式で求められたツールの位置姿勢Ｔａのツール形状情報と、収納箱４の形状情報と位置姿勢情報に基づいて、ツール３と収納箱４が干渉するか否かを判断することができる、と記載されている。
【０００５】
干渉を予測する他の従来技術として、特許文献３には、教示作業が自動化されているロボットの制御装置及び制御システムが開示されており、ここでは、所定作業に対するロボットの動作をシミュレーションして作業経路における部材との干渉の有無をチェックし、ロボットアームあるいはその先端に装着されたツールが部材と干渉すると判定された場合において、干渉区間長が所定値を超えていなければ、干渉個所でロボットの作業経路を分断し、その分断された作業経路内においてロボットを動作させるようにする、とされている。
【０００６】
また他の従来技術として、特許文献４には、非凸多面体の凸包を生成し、該凸包と他の物体間の干渉チェックを行い、干渉し始めた時、凸包を解除して前記非凸多面体と他の物体間の干渉チェックを行う干渉チェック方法が開示されており、該方法は、非凸多面体を構成する頂点を、第１軸（例えばＸ軸）の座標値の差が零あるいは微小量ε以内の複数の頂点群に分割するステップ、各頂点群について第２−第３軸平面（例えばＹＺ平面）上で２次元凸包を生成するステップ、隣接する２次元凸包を併合して３次元凸包を生成し、以後、隣接する３次元凸包を順次併合して目的とする非凸多面体の凸包を生成するステップ、を有する、とされている。
【０００７】
さらなる他の従来技術として、特許文献５には、複数のロボットの動作をシミュレートするシミュレーション装置が開示されており、該シミュレーション装置は、１台ごとのロボットモデルの一定時間動作した軌跡の形状を表す複数の多面体の座標値とシミュレーション時刻を記録する軌跡情報記録手段と、前記複数のロボットの軌跡の形状を表す複数の多面体の、三次元座標値と時刻とを、それぞれスプレッドシートに入力し、複数シート間の同じ時刻で空間的に干渉する多面体を計算により検出する干渉検出手段とを備えるとされている。
【先行技術文献】
【特許文献】
【０００８】
【特許文献１】特開２０００−３２６２８０号公報
【特許文献２】特開２００２−３３１４８０号公報
【特許文献３】特開２００２−２７３６７５号公報
【特許文献４】特開平９−２７０４６号公報
【特許文献５】特開２００４−２８０６３５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００９】
特許文献１又は２に記載の技術では、移動終了位置での干渉は確認できても、移動経路上での干渉を確認・予測することは難しい。例えば、特許文献２の図２は、収納箱４の中にあるワークピース７を取出す時のツール３の移動経路を示すものであるが、移動終了時刻Ｔ４でのツール３と収納箱４とは干渉しないが、移動中の時刻Ｔ２とＴ３の間にツール３と周辺物４とが干渉してしまう場合がある。
【００１０】
一方、特許文献３に記載の技術は、生成された移動経路上での干渉を高い精度で予測するために、移動経路上の全補間位置について、逐次的にロボットの着目部位と周辺物とが干渉するか否かを調べるものと解される。しかしこの技術では、計算時間が過度に長くなってしまい、ロボット動作中にリアルタイムで干渉を予測するには適さないと思われる。
【００１１】
特許文献４に記載の技術は、干渉予測に際しいわゆる凸包を利用するものであるが、これは静止状態にある多面体同士の干渉を予測するものであり、移動する多面体には適用できない。
【００１２】
また特許文献５に記載の技術は、複数のロボットや加工機械・治具が同時に動作する際に、該ロボット間等の干渉判定を、それぞれの動作領域を覆う多面体を計算し、該多面体同士の干渉判定に置き換えるものと解される。しかし、動作領域に対応する多面体が凸多面体に限定されないため、干渉判定計算が複雑で時間が掛かる場合があり、ロボット動作中にリアルタイムで干渉判定を行うには適さない。
【００１３】
そこで本発明は、ロボットの手先に搭載されているツールやセンサ等の着目部位がロボットの動作に伴って移動する場合に、移動経路上での着目部位とロボットの周りに固定設置されている周辺機器や柵等の周辺物との干渉を、実用的な精度及び計算速度にて予測する干渉予測方法及び干渉予測装置を提供することを目的とする。
【課題を解決するための手段】
【００１４】
上記目的を達成するために、請求項１に記載の発明は、空間に固定されている周辺物の形状を多面体で近似した周辺物近似多面体が前記空間を占有する領域である周辺物占有領域と、ロボットの着目部位の形状を多面体で近似した着目部位近似多面体とを記憶し、前記ロボットの動作に伴って、前記着目部位近似多面体が複数の時刻の各々において前記空間を占有する領域である、複数の着目部位占有領域を計算し、前記複数の着目部位占有領域と前記周辺物占有領域とに基づいて、前記ロボットの着目部位と前記周辺物との干渉の有無を予測する、干渉予測方法であって、前記複数の着目部位占有領域を前記複数の時刻の順に並べたときに、隣り合う着目部位占有領域同士の組合せの凸包を全て計算し、前記周辺物占有領域を凸多面体の和として計算し、全ての前記凸包と前記凸多面体との共有領域の有無を、前記ロボットの着目部位と前記周辺物との干渉の有無として判定する、干渉予測方法を提供する。
【００１５】
請求項２に記載の発明は、空間に固定されている周辺物の形状を多面体で近似した周辺物近似多面体が前記空間を占有する領域である周辺物占有領域と、ロボットの着目部位の形状を多面体で近似した着目部位近似多面体とを記憶する記憶部と、前記ロボットの動作に伴って、前記着目部位近似多面体が複数の時刻の各々において前記空間を占有する領域である、複数の着目部位占有領域を計算する着目部位占有領域計算部と、前記複数の着目部位占有領域と前記周辺物占有領域とに基づいて、前記ロボットの着目部位と前記周辺物との干渉の有無を予測する干渉予測部と、を備える干渉予測装置であって、前記干渉予測部は、前記複数の着目部位占有領域を前記複数の時刻の順に並べたときに、隣り合う着目部位占有領域同士の組合せの凸包を全て計算する凸包計算部と、前記周辺物占有領域を凸多面体の和として計算する周辺物占有領域計算部と、を有し、前記干渉予測部は、全ての前記凸包と前記凸多面体との共有領域の有無を、前記ロボットの着目部位と前記周辺物との干渉の有無として判定する、干渉予測装置を提供する。
【発明の効果】
【００１６】
本発明によれば、動作直前に実用的な精度及び計算速度で自動的に干渉を予測できるので、バラ積みされたワークピースの取出し等の用途において、干渉を避けながらロボットを動作させることができる。また本願発明では、ロボットの着目部位の隣り合う補間位置における領域同士の凸包（凸多面体の範疇）を求め、周辺物を凸多面体の和で表し、干渉判定を凸多面体同士のそれに置き換え、計算の簡素化・高速化を図り、ロボット動作中のリアルタイムでの干渉判定を実現している。
【図面の簡単な説明】
【００１７】
【図１】本発明に係る干渉予測装置の一実施形態の概略構成を示す図である。
【図２】ツール及び収納箱を多面体で近似した例を示す図である。
【図３】凸多面体の例である正四面体にインデックスを割り当てた例を示す図である。
【図４】本発明に係る干渉予測装置又は干渉予測方法による、干渉予測手順を示すフローチャートである。
【図５】分割数が４である場合の各時点でのツールの位置姿勢と、収納箱と、ワークピースとの位置関係を表した図である。
【図６】各時点での多面体の頂点の直線度を求める例を説明する図である。
【図７】多面体領域から凸包を求める例を説明する図である。
【図８】多角形を複数の三角形に分割した例を示す図である。
【図９】凸包と周辺物を示す多面体との位置関係を示す図である。
【発明を実施するための形態】
【００１８】
図１は、本発明に係る干渉予測装置の一実施形態の概要図である。なお、本願発明における「ロボットの着目部位」とは、ロボットアームの本体又は付属物のうち、動作中に周辺機器と干渉する可能性のある部位をいう。本実施形態では、ロボット制御装置６内に干渉予測のためのソフトウエアを設けることによって、干渉予測装置を構成している。ロボット５の手首先端には、ワークピース７を取出すためのツール３、ワークピース７を撮像するためのカメラ２、及びワークピース７にレーザ光を投光するレーザ投光器１が取り付けられている。カメラ２及びレーザ投光器１によって、ワークピース７の位置及び姿勢（以降、位置姿勢と称する）を検出する３次元視覚センサが構成される。本実施形態では、ワークピース７は収納箱４内に収納されている。この収納箱４には複数のワークピース７がバラ積み状態で収納されており、故に各々のワークピース７はその位置姿勢が不規則な状態である。
【００１９】
ロボット５、レーザ投光器１、カメラ２及びツール３は従来周知のものでよく、故に詳細な説明は省略する。ロボット制御装置６は、プロセッサ、ＲＯＭ、ＲＡＭ、不揮発性ＲＡＭ、表示装置付き入力手段、入出力インタフェース、カメラインタフェース、ロボット各関節軸のサーボモータ等を駆動制御するサーボ制御器等を備える点では、従来のロボット制御装置と同様でよい。図示例のロボット５の関節軸の数は６つであり、ツール３の位置姿勢が命令されると、ロボット制御装置６は各関節軸の回転角を逆算し、ツール３がその位置姿勢となるようにロボット５の各軸の動作を制御する。また、入出力インタフェースにはレーザ投光器１及びツール３が接続され、カメラインタフェースにはカメラ２が接続される。
【００２０】
ロボット５の手首先端に対するツール３の位置姿勢と、ツール３の形状と、収納箱４の位置姿勢と、収納箱４の形状と、教示時のワークピース７の位置姿勢と、その位置姿勢に対してワークピース７を把持する際のツールの位置姿勢とは、ロボット制御装置６のメモリに予め設定・記憶させておくものとする。その際、記憶するツール３及び収納箱４の形状に関して、図２に示すように、ツール３は２つの直方体を組み合わせて多面体で近似した領域Ｄ１で表し、収納箱４は直方体の上側から少し小さめの直方体をくり抜いた多面体で近似した領域Ｄ２で表すものとする。
【００２１】
領域Ｄ１、Ｄ２は共に複数の直方体領域の和として表現することができるが、干渉予測の計算が簡単になるように、近似する領域は凸多面体の和で表せるようにする。具体的には、それぞれの多面体領域の持つ頂点、辺の各々にインデックスを割り振り、各頂点の空間上の座標値、各辺の両端にある頂点のインデックス、各面の端にある辺のインデックスを記憶する。例えば、近似された領域が、図３に示すような各辺の長さが３である正四面体（凸多面体）の場合、各頂点及び各辺にインデックスを割り振り、ある座標系における頂点Ｖ１、頂点Ｖ２、頂点Ｖ３、頂点Ｖ４それぞれの座標値（0.000, 0.000, 2.449）、（1.732, 0.000, 0.000）、（-0.866, 1.500, 0.000）、（-0.866, -1.500, 0.000）と、辺Ｅ１、辺Ｅ２、辺Ｅ３、辺Ｅ４、辺Ｅ５、辺Ｅ６それぞれの両端の頂点のインデックス（Ｖ１, Ｖ２）、（Ｖ１, Ｖ4）、（Ｖ１, Ｖ3）、（Ｖ２, Ｖ4）、（Ｖ３, Ｖ４）、（Ｖ２, Ｖ3）と、面Ｓ１、面Ｓ２、面Ｓ３、面Ｓ４それぞれの端にある辺のインデックス（Ｅ１, Ｅ２, Ｅ４）、（Ｅ２, Ｅ３, Ｅ５）、（Ｅ１, Ｅ３, Ｅ６）、（Ｅ４, Ｅ５, Ｅ６）を記憶することになる。
【００２２】
図４は、ロボット５を用いたワークピース７の取出し作業において、ツール３と収納箱４との干渉が起こるか否かを判断する処理手順を示すフローチャートである。処理手順では、まず、ロボット制御装置６からの指令により、ロボット５を収納箱４内のワークピース７を撮像できる位置姿勢に移動させた後、レーザ投光器１及びカメラ２を用いて、収納箱４内のワークピース７の位置姿勢を計測する（ステップ１０１）。
【００２３】
次に、この計測したワークピース７の位置姿勢と、予め記憶しておいた教示時のワークピース７の位置姿勢と、予め記憶しておいたワークピースを把持する際のツールの位置姿勢とを合わせて、計測したワークピース７をツール３で把持して収納箱から取出すことのできるツール３の位置姿勢を算出する。そして、ツール３に関して、ワークピース７を計測した時の位置姿勢を移動開始時の位置姿勢とし、計測したワークピース７をツール３で把持して収納箱から取出すことのできる位置姿勢を移動終了時の位置姿勢とした、ツール３の移動経路を生成する（ステップ１０２）。移動経路の生成方法には様々なものがあるが、例えば、ロボット各関節軸の回転速度が常に一定となるような移動経路の生成方法がある。即ち、移動開始時のロボット５の各関節軸ｋ（ｋ＝１，２，・・・，６）の回転角の値をθｋとし、移動終了時の各関節軸ｋの回転角の値をφｋとし、移動開始時刻を０（ゼロ）とし、移動終了時刻をＴとした場合に、時刻ｔ（０≦ｔ≦Ｔ）における各関節軸ｋの回転角がθｋ＋Δｋ×ｔとなるような移動経路を生成する方法である。但し、Δｋ＝（φｋ−θｋ）／Ｔである。但しこのような移動経路の生成方法は一例であって、ここで述べる干渉予測の適用は、移動経路の生成方法に依るものではない。
【００２４】
次に、移動開始から移動終了までの移動時間をｎ等分し、得られたｎ＋１個の時刻（時点）をＴ０, Ｔ１,・・・, Ｔｎ とする（ステップ１０３）。Ｔ０ および Ｔｎ はそれぞれ移動開始時刻と移動終了時刻である。分割数ｎは、干渉予測の粗精や処理時間を考慮して、予め与えておいてもよいし、移動時間及び移動経路長とから逐次決定してもよい。分割数を大きくすれば干渉予測の精度は向上するが、処理時間は分割数にほぼ比例して大きくなる。リアルタイムで干渉予測の処理を行わなければならないため、処理時間はできるだけ短い方がよい。
【００２５】
図５は、分割数ｎ＝４である場合の各時点でのツール３の位置姿勢と、収納箱４と、ワークピース７との位置関係を表したものである。ここではツール３の移動時間を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等により求める。次に、凸包Ｃｊの各辺は、凸包Ｃｊの全ての２頂点の組合せ（Ｖ1，Ｖ2）に対して、線分Ｖ1Ｖ2が凸包Ｃｊの辺であるかの判断を以下の手順で行って求める。空間上の直線Ｖ1Ｖ2に垂直な平面Πに凸包Ｃｊの全ての頂点を正射影する。この時、頂点Ｖ1，Ｖ2は平面Π上の同一点Ｐに射影される。平面Π上において、正射影した全ての点の凸包の頂点か辺の上に点Ｐがあるならば、線分Ｖ1Ｖ2は凸包Ｃｊの辺であると判断できる。平面上の凸包の頂点は同様にQuickHullにより計算可能である。
【００３３】
また、平面上において、凸包の各頂点Ｐ1，Ｐ2を結ぶ線分Ｐ1Ｐ2がその凸包の辺であるか否かは、以下のようにして判断する。線分Ｐ1Ｐ2に垂直な直線λ上にその凸包の全ての頂点を正射影し、直線λ上において、それらの点の凸包である線分の両端のどちらかに線分Ｐ1Ｐ2を正射影した点があるならば、線分Ｐ1Ｐ2は辺であると判断できる。また、凸包Ｃｊの各面は、凸包Ｃｊの３辺の組合せで三角形を成す全ての組合せに対して、その三角形を含む平面に垂直な直線Λに凸包Ｃｊの全ての頂点を正射影し、直線Λ上において、それらの点の凸包である線分の両端のどちらかにその三角形を正射影した点があるならば、その三角形は凸包Ｃｊの面と判断できる。図７に、領域Ｄ１,ｊ−１、Ｄ１,ｊとそれらから計算された凸包Ｃｊの例を示す。領域Ｄ１,ｊ−１、Ｄ１,ｊが実線で示された各時刻でのツール３に実質相当する領域であるとすると、凸包Ｃｊは、領域Ｄ１,ｊ−１、Ｄ１,ｊに破線で示された領域を加えたものとなる。
【００３４】
次に、凸包Ｃｊと多面体領域Ｄ２との共通領域の有無を調べる（ステップ１１２）。多面体領域Ｄ２は複数の凸多面体領域Ｄ２,ｊの和で表されるので、凸包Ｃｊと凸多面体Ｄ２,ｊの組み合わせで共通領域を持つものがあれば、凸包Ｃｊと多面体領域Ｄ２に共通領域があると言える。凸包Ｃｊと凸多面体領域Ｄ２,ｊの何れの面も多角形なので、図８のように複数の三角形への分割（図８では六角形を４つの三角形Ｉ〜IVに分割）が可能である。すると、凸包Ｃｊと凸多面体領域Ｄ２,ｊの何れも三角形のみで覆われた閉領域となり、凸包Ｃｊを覆う三角形と凸多面体領域Ｄ２,ｊを覆う三角形の辺（線分）とが交差する組合せが１つ以上あるか、凸包Ｃｊを覆う三角形の辺（線分）と凸多面体領域Ｄ２,ｊを覆う三角形とが交差する組合せが１つ以上あるか、もしくは凸包Ｃｊと凸多面体領域Ｄ２,ｊのどちらかの領域がもう一方の領域を完全に含むか、これらの３つの条件の少なくとも１つが成立するか否かで、共通領域の有無の判断を行うことができる。
【００３５】
線分と三角形の交差は、以下のように判断する。空間上の線分と三角形について、線分の両端の点Ａ１、Ａ２の空間上の座標値をそれぞれα１、α２とし、三角形の頂点Ｂ１、Ｂ２、Ｂ３の空間上の座標値をβ１、β２、β３とする。α１、α２、β１、β２、β３は３次元ベクトルで表される。まず、３×３行列（α１−α２、β１−β３、β２−β３）が正則行列であるか否かを計算し、正則である場合は線分Ａ１Ａ２と三角形Ｂ１Ｂ２Ｂ３が平行ではなく、正則ではない場合は線分Ａ１Ａ２と三角形Ｂ１Ｂ２Ｂ３が平行である、と判断する。
【００３６】
線分Ａ１Ａ２と三角形Ｂ１Ｂ２Ｂ３が平行ではない場合は、線分Ａ１Ａ２を含む空間上の直線と三角形Ｂ１Ｂ２Ｂ３を含む空間上の平面との交点の空間上の座標値をｘとすると、
ｘ＝ｓ１α１＋ｓ２α２、ｘ＝ｔ１β１＋ｔ２β２＋ｔ３β３
と一意に表すことができる。但し、ｓ１、ｓ２、ｔ１、ｔ２、ｔ３は
ｓ１＋ｓ２＝１、ｔ１＋ｔ２＋ｔ３＝１
を満たす実数値である。計算したｓ１、ｓ２、ｔ１、ｔ２、ｔ３の５つの全ての値が０以上かつ１以下である場合は、線分Ａ１Ａ２と三角形Ｂ１Ｂ２Ｂ３は交差するものと判断できる。一方、線分Ａ１Ａ２と三角形Ｂ１Ｂ２Ｂ３が平行である場合は点Ａ１と平面Ｂ１Ｂ２Ｂ３の距離を計算し、その距離が０ならば、線分Ａ１Ａ２と三角形Ｂ１Ｂ２Ｂ３が同じ平面上にあることになる。
【００３７】
線分Ａ１Ａ２と三角形Ｂ１Ｂ２Ｂ３が同じ平面上にある場合は、２×２行列（α１−α２、β１−β２）が正則行列ならば、線分Ａ１Ａ２と三角形Ｂ１Ｂ２Ｂ３の辺Ｂ１Ｂ２とは平行ではないので、線分Ａ１Ａ２を含む空間上の直線と辺Ｂ１Ｂ２を含む空間上の直線との交点の座標値をｙとすると、
ｙ＝ｓ１α１＋ｓ２α２、ｘ＝ｔ１β１＋ｔ２β２
と一意に表すことができる。但し、ｓ１、ｓ２、ｔ１、ｔ２は
ｓ１＋ｓ２＝１、ｔ１＋ｔ２＝１
を満たす実数値である。計算したｓ１、ｓ２、ｔ１、ｔ２の４つの全ての値が０以上かつ１以下である場合は、線分Ａ１Ａ２と辺Ｂ１Ｂ２が交差するので、線分Ａ１Ａ２と三角形Ｂ１Ｂ２Ｂ３は交差するものと判断できる。線分Ａ１Ａ２と三角形Ｂ１Ｂ２Ｂ３が同じ平面上にある場合は、辺Ｂ２Ｂ３と辺Ｂ３Ｂ１についても辺Ｂ１Ｂ２と同様に、線分Ａ１Ａ２との交差を調べ、線分Ａ１Ａ２と三角形Ｂ１Ｂ２Ｂ３の交差を判断する。何れの交差の判断でも線分Ａ１Ａ２と三角形Ｂ１Ｂ２Ｂ３が交差するとの判断が得られなければ、線分Ａ１Ａ２と三角形Ｂ１Ｂ２Ｂ３は交差しないと判断する。
【００３８】
凸包Ｃｊが凸多面体領域Ｄ２,ｊを完全に含むか否かは、凸多面体領域Ｄ２,ｊの全頂点に関して凸包Ｃｊの重心とその頂点を結ぶ線分が凸包Ｃｊの表面と交わらないか、そうでないかで判断することができる。凸多面体領域Ｄ２,ｊが凸包Ｃｊを完全に含むか否かも、同様にして判断することができる。
【００３９】
上述のような共通領域の有無の判断の結果、凸包Ｃｊと多面体領域Ｄ２との共通領域が有る場合は、移動経路中でツール３と収納箱４とが干渉すると判断（ステップ１１３）して処理を終了し、共通領域が無い場合は、ｊ＜ｎ（ステップ１１４）か否かを調べる。
【００４０】
ステップ１１４において、ｊ＜ｎならば、ステップ１０９に進んでｊに（ｊ＋１）を設定し（ｊ←ｊ＋１）、ステップ１０５に戻る。一方、ステップ１１４においてｊ≧ｎならば、移動経路中でツール３と収納箱４とが干渉しないと判断（ステップ１１５）して処理を終了する。
【００４１】
図９は、図５に示した時点Ｔ２と時点Ｔ３の間の部分を拡大したものであり、時点Ｔ２、Ｔ３の各々でツール３が占有する２つの領域の凸包Ｃ３と多面体領域Ｄ２との共通領域が有ることになる。よって、図５の移動経路の場合、干渉予測装置はツール３と収納箱４とが干渉すると予測・判断する。
【００４２】
凸包Ｃｊと領域Ｄ２の共通領域の有無の判断の計算に要する時間、２つの領域Ｄ１,ｊ−１、Ｄ１,ｊから凸包Ｃｊを計算するのに要する時間は、それぞれ１〜２ミリ秒（ｍｓ）を要する。図５の移動経路の場合、ロボットの移動時間の分割数ｎが４なので、干渉予測に要する計算時間は、最大で（２＋２）×４＝１６ｍｓ程度となり、ロボットの動作に差し支える程の時間にはならない。先に、直線度Ｌｊの値が１に近く、ツール３が占有する領域Ｄ１の凸包の各頂点の移動を表す折れ線Ｐｊ−１ＰｊＰｊ＋１が直線に近い場合、ステップ１０７で時点の列から時点Ｔｊを削除しても、干渉予測の精度は殆ど低下しないと述べたが、それは以下の理由に依る。折れ線Ｐｊ−１ＰｊＰｊ＋１が直線に近い場合は、領域Ｄ１,ｊ−１、Ｄ１,ｊの凸包と領域Ｄ１,ｊ、Ｄ１,ｊ＋１の凸包との和が、領域Ｄ１,ｊ−１、Ｄ１,ｊ＋１の凸包とほぼ一致する。したがって、凸包と多面体領域Ｄ２との共通領域の有無により干渉を判断する場合においては、時点の列から時点Ｔｊを削除して領域Ｄ１,ｊの計算を省いても、干渉予測の精度は殆ど低下しない。
【００４３】
以上述べてきたように、本発明では、移動経路上の幾つかの補間位置において着目部位が占有する領域を求め、隣り合う補間位置における領域同士の凸包を全て求め、周辺物を凸多面体の和で表し、それら全ての凸包と全ての凸多面体との干渉を調べることによって、移動経路上での着目部位と周辺物との干渉を予測する。
【符号の説明】
【００４４】
１ レーザ投光器
２ カメラ
３ ツール
４ 収納箱
５ ロボット
６ ロボット制御装置
７ ワークピース
【特許請求の範囲】
【請求項１】
空間に固定されている周辺物の形状を多面体で近似した周辺物近似多面体が前記空間を占有する領域である周辺物占有領域と、ロボットの着目部位の形状を多面体で近似した着目部位近似多面体とを記憶し、
前記ロボットの動作に伴って、前記着目部位近似多面体が複数の時刻の各々において前記空間を占有する領域である、複数の着目部位占有領域を計算し、
前記複数の着目部位占有領域と前記周辺物占有領域とに基づいて、前記ロボットの着目部位と前記周辺物との干渉の有無を予測する、干渉予測方法であって、
前記複数の着目部位占有領域を前記複数の時刻の順に並べたときに、隣り合う着目部位占有領域同士の組合せの凸包を全て計算し、
前記周辺物占有領域を凸多面体の和として計算し、
全ての前記凸包と前記凸多面体との共有領域の有無を、前記ロボットの着目部位と前記周辺物との干渉の有無として判定する、干渉予測方法。
【請求項２】
空間に固定されている周辺物の形状を多面体で近似した周辺物近似多面体が前記空間を占有する領域である周辺物占有領域と、ロボットの着目部位の形状を多面体で近似した着目部位近似多面体とを記憶する記憶部と、
前記ロボットの動作に伴って、前記着目部位近似多面体が複数の時刻の各々において前記空間を占有する領域である、複数の着目部位占有領域を計算する着目部位占有領域計算部と、
前記複数の着目部位占有領域と前記周辺物占有領域とに基づいて、前記ロボットの着目部位と前記周辺物との干渉の有無を予測する干渉予測部と、
を備える干渉予測装置であって、
前記干渉予測部は、
前記複数の着目部位占有領域を前記複数の時刻の順に並べたときに、隣り合う着目部位占有領域同士の組合せの凸包を全て計算する凸包計算部と、
前記周辺物占有領域を凸多面体の和として計算する周辺物占有領域計算部と、を有し、
前記干渉予測部は、全ての前記凸包と前記凸多面体との共有領域の有無を、前記ロボットの着目部位と前記周辺物との干渉の有無として判定する、干渉予測装置。
【請求項１】
空間に固定されている周辺物の形状を多面体で近似した周辺物近似多面体が前記空間を占有する領域である周辺物占有領域と、ロボットの着目部位の形状を多面体で近似した着目部位近似多面体とを記憶し、
前記ロボットの動作に伴って、前記着目部位近似多面体が複数の時刻の各々において前記空間を占有する領域である、複数の着目部位占有領域を計算し、
前記複数の着目部位占有領域と前記周辺物占有領域とに基づいて、前記ロボットの着目部位と前記周辺物との干渉の有無を予測する、干渉予測方法であって、
前記複数の着目部位占有領域を前記複数の時刻の順に並べたときに、隣り合う着目部位占有領域同士の組合せの凸包を全て計算し、
前記周辺物占有領域を凸多面体の和として計算し、
全ての前記凸包と前記凸多面体との共有領域の有無を、前記ロボットの着目部位と前記周辺物との干渉の有無として判定する、干渉予測方法。
【請求項２】
空間に固定されている周辺物の形状を多面体で近似した周辺物近似多面体が前記空間を占有する領域である周辺物占有領域と、ロボットの着目部位の形状を多面体で近似した着目部位近似多面体とを記憶する記憶部と、
前記ロボットの動作に伴って、前記着目部位近似多面体が複数の時刻の各々において前記空間を占有する領域である、複数の着目部位占有領域を計算する着目部位占有領域計算部と、
前記複数の着目部位占有領域と前記周辺物占有領域とに基づいて、前記ロボットの着目部位と前記周辺物との干渉の有無を予測する干渉予測部と、
を備える干渉予測装置であって、
前記干渉予測部は、
前記複数の着目部位占有領域を前記複数の時刻の順に並べたときに、隣り合う着目部位占有領域同士の組合せの凸包を全て計算する凸包計算部と、
前記周辺物占有領域を凸多面体の和として計算する周辺物占有領域計算部と、を有し、
前記干渉予測部は、全ての前記凸包と前記凸多面体との共有領域の有無を、前記ロボットの着目部位と前記周辺物との干渉の有無として判定する、干渉予測装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【公開番号】特開２０１２−２２３８４５（Ｐ２０１２−２２３８４５Ａ）
【公開日】平成２４年１１月１５日（２０１２．１１．１５）
【国際特許分類】
【出願番号】特願２０１１−９２４１３（Ｐ２０１１−９２４１３）
【出願日】平成２３年４月１８日（２０１１．４．１８）
【出願人】（３９０００８２３５）ファナック株式会社 (1,110)
【Ｆターム（参考）】
【公開日】平成２４年１１月１５日（２０１２．１１．１５）
【国際特許分類】
【出願日】平成２３年４月１８日（２０１１．４．１８）
【出願人】（３９０００８２３５）ファナック株式会社 (1,110)
【Ｆターム（参考）】
[ Back to top ]