JP5857803B2 - 産業用機械の干渉判定装置、干渉判定方法、コンピュータプログラムおよび記録媒体 - Google Patents

Description

本発明は、産業用機械と障害物との干渉を判定する技術に関する。

ロボットの実働時や教示時あるいは動作シミュレーション時に、ロボットと障害物（設備）とが干渉することを防止するため、種々の技術が提案されている。例えば、特許文献１には、水平多関節ロボットの３次元モデルと障害物の３次元モデルとをそれぞれ水平面に射影し、その水平面上でロボットと障害物との干渉を判定する技術が記載されている。このように、物体間（ロボットと障害物間）の干渉を判定するためのアルゴリズムとして、その他、例えば、物体間の距離に基づいて干渉を判定する方法や、物体を直方体に分割してその直方体同士で干渉の判定を行う方法、物体表面をポリゴンで表し、ポリゴン同士で干渉を判定する方法、物体と物体とを分離軸と呼ばれる直線上に射影してその分離軸上での像の重なりに応じて干渉の判定を行う方法などがある。

特開２００４−２０２６２７号公報

しかし、特許文献１に記載の技術では、ロボットの形態が水平多関節型に限定されてしまうため、複雑な構成のロボットに適用することができないという問題があった。また、物体間の距離に基づいて干渉を判定する方法や、直方体同士で干渉を判定する方法、ポリゴン同士で干渉を判定する方法では、ロボットや設備の形状が複雑になるほど処理負担が増大し、干渉の判定に時間がかかるという問題があった。また、分離軸による判定では、物体の形状によっては、最適な分離軸を決定することが困難な場合があった。これらの問題は、ロボットに限らず、可動部を有する産業用機械と障害物との干渉を判定する際に共通した問題であった。

これらの問題を考慮し、本発明が解決しようとする課題は、産業用機械の可動部と障害物との干渉を、迅速かつ高精度に判定可能な技術を提供することである。

本発明は、上述の課題の少なくとも一部を解決するためになされたものであり、以下の形態又は適用例として実現することが可能である。

［適用例１］可動部を有する産業用機械と障害物との干渉を判定する干渉判定装置であって、前記可動部の少なくとも一部の形状と前記障害物の少なくとも一部の形状とのうち、一方の形状を直円柱によって表した円柱モデルのデータと、他方の形状を直方体によって表した直方体モデルのデータと、が記憶された記憶部と、前記円柱モデルが表す形状の位置と、前記直方体モデルが表す形状の位置と、を取得する位置取得部と、前記円柱モデルの上面または下面の法線ベクトルと、前記直方体モデルの面法線ベクトルまたは辺ベクトルのうちのいずれか１のベクトルとを含む第１の平面を求め、該第１の平面上における前記円柱モデルの側面の法線ベクトルに平行な直線を、第１の分離軸として決定する分離軸決定部と、前記第１の平面に前記円柱モデルと前記直方体モデルとを射影し、更に、前記第１の平面上の前記第１の分離軸に、前記第１の平面に射影された前記円柱モデルの像と前記直方体モデルの像とを射影し、前記第１の分離軸に射影された前記円柱モデルの像と前記直方体モデルの像との重なりの有無に基づいて、前記可動部と前記障害物との干渉を判定する干渉判定部と、を備える干渉判定装置。

このような構成の干渉判定装置では、円柱モデルの上面または下面の法線ベクトルと直方体モデルの面法線ベクトル（または辺ベクトル）のうちのいずれか１のベクトルとを含む第１の平面を求め、この第１の平面上における円柱モデルの側面の法線ベクトルに平行な直線を、第１の分離軸として求める。そのため、３６０°に亘って無数に存在する円柱モデルの側面の法線ベクトルの中から、分離軸として用いる法線ベクトルを確実に特定することができ、これにより効率的に直方体モデルと円柱モデルの側面との干渉を判定することができる。このように、上記構成の干渉判定装置では、円柱モデルを用いた場合であっても、迅速に干渉の判定を行うことができるので、産業用機械の可動部と障害物とのリアルタイムの干渉判定が容易となる。また、上記構成によれば、円柱モデルを用いた場合であっても、迅速に干渉の判定を行うことができるので、産業用機械の可動部の外形形状を、直方体モデルなどの単純なモデルだけではなく、円柱モデルを用いて精度良くモデリングすることができる。よって、実際の産業用機械の形状に即した干渉の判定が可能となるので、狭小空間での干渉の判定など、精度が求められる干渉の判定も容易に行うことが可能になる。また、上記構成の干渉判定装置では、第１の平面に対して円柱モデルと直方体モデルとを射影し、更に、第１の平面上の第１の分離軸に第１の平面に対して射影された円柱モデルの像と直方体モデルの像とを射影し、その射影の結果に基づいて、産業用機械の可動部と障害物との干渉を判定する。つまり、３次元のモデルを、２次元の平面を介して１次元の分離軸に射影するため、分離軸への各モデルの射影を精度良く行うことが可能になる。

［適用例２］適用例１に記載の干渉判定装置であって、前記分離軸決定部は、更に、前記円柱モデルの上面または下面の法線ベクトルと、前記直方体モデルの辺ベクトルまたは面法線ベクトルのうちのいずれか１のベクトルとの外積ベクトルを求め、該外積ベクトルと前記円柱モデルの上面または下面の法線ベクトルとを含む第２の平面を求め、該第２の平面上における前記円柱モデルの側面の法線ベクトルに平行な直線を、第２の分離軸として決定し、前記干渉判定部は、更に、前記第２の平面に前記円柱モデルと前記直方体モデルとを射影し、更に、前記第２の平面上の前記第２の分離軸に、前記第２の平面に射影された前記円柱モデルの像と前記直方体モデルの像とを射影し、前記第２の分離軸に射影された前記円柱モデルの像と前記直方体モデルの像との重なりの有無に基づいて、前記可動部と前記障害物との干渉を判定する、干渉判定装置。

このような構成の干渉判定装置によれば、円柱モデルの上面または下面の法線ベクトルと、直方体モデルの辺ベクトル（または面法線ベクトル）のうちのいずれか１のベクトルとの外積ベクトルを求めるとともに、この外積ベクトルと円柱モデルの上面または下面の法線ベクトルとを含む第２の平面を求め、この第２の平面上における円柱モデルの側面の法線ベクトルに平行な第２の分離軸に対して円柱モデルと直方体モデルとを射影する。そのため、第１の分離軸とは異なる観点から、円柱モデルによって表された可動部の側面と直方体モデルによって表された障害物との干渉を判定することができる。

［適用例３］適用例１または適用例２に記載の干渉判定装置であって、前記分離軸決定部は、更に、前記円柱モデルの上面または下面の法線ベクトルと、前記直方体モデルの辺ベクトルまたは面法線ベクトルのうちのいずれか１のベクトルとの外積ベクトルを求め、該外積ベクトルに平行な直線を、第３の分離軸として決定し、前記干渉判定部は、更に、前記第３の分離軸に前記円柱モデルと前記直方体モデルとを射影し、前記第３の分離軸に射影された前記円柱モデルの像と前記直方体モデルの像との重なりの有無に基づいて、前記可動部と前記障害物との干渉を判定する、干渉判定装置。

このような構成の干渉判定装置では、円柱モデルの上面または下面の法線ベクトルと、直方体モデルの辺ベクトル（または面法線ベクトル）のうちのいずれか１のベクトルとの外積ベクトルを求め、この外積ベクトルに平行な第３の分離軸に対して円柱モデルと直方体モデルとを射影する。そのため、円柱モデルによって表された可動部の外形と直方体モデルによって表された障害物の外形との干渉を判定することができる。

［適用例４］適用例１から適用例３までのいずれか一項に記載の干渉判定装置であって、前記分離軸決定部は、更に、前記直方体モデルの面法線ベクトルまたは辺ベクトルのうちのいずれか１のベクトルに平行な直線を、第４の分離軸として決定し、前記干渉判定部は、更に、前記第４の分離軸に前記円柱モデルと前記直方体モデルとを射影し、前記第４の分離軸に射影された前記円柱モデルの像と前記直方体モデルの像との重なりの有無に基づいて、前記可動部と前記障害物との干渉を判定する、干渉判定装置。

このような構成の干渉判定装置では、直方体モデルの面法線ベクトル（または辺ベクトル）のうちのいずれか１のベクトルに平行な第４の分離軸に対して円柱モデルと直方体モデルとを射影する。そのため、円柱モデルによって表された可動部と直方体モデルによって表された障害物の面との干渉を判定することができる。

［適用例５］適用例１から適用例４までのいずれか一項に記載の干渉判定装置であって、前記分離軸決定部は、更に、前記円柱モデルの上面または下面の法線ベクトルに平行な直線を第５の分離軸として決定し、前記干渉判定部は、更に、前記第５の分離軸に前記円柱モデルと前記直方体モデルとを射影し、前記第５の分離軸に射影された前記円柱モデルの像と前記直方体モデルの像との重なりの有無に基づいて、前記可動部と前記障害物との干渉を判定する、干渉判定装置。

このような構成の干渉判定装置では、円柱モデルの上面または下面の法線ベクトルに平行な第５の分離軸に対して円柱モデルと直方体モデルとを射影する。そのため、円柱モデルによって表された可動部の上面や下面と直方体モデルによって表された障害物の外形との干渉を判定することができる。

［適用例６］適用例１を引用する適用例２、を引用する適用例３、を引用する適用例４、を引用する適用例５に記載の干渉判定装置であって、前記分離軸決定部は、前記第５の分離軸、前記第４の分離軸、前記第１の分離軸、前記第２の分離軸、前記第３の分離軸、の順序で分離軸の決定を行い、前記干渉判定部は、前記可動部と前記障害物とが干渉していないと判定された分離軸以降の順序の分離軸を用いた干渉の判定を行わず、前記分離軸決定部は、前記可動部と前記障害物とが干渉していないと判定された分離軸以降の順序の分離軸の決定を行わない、干渉判定装置。

このような構成の干渉判定装置によれば、分離軸を決定するための計算量の少ない順に、干渉の判定に用いられる分離軸が決定され、しかも、可動部と障害物とが干渉していないと一旦判定された場合には、それ以降の分離軸の決定と干渉の判定が行われない。そのため、高速に干渉の判定を行うことができる。

本発明は、上述した干渉判定装置としての構成のほか、干渉判定方法や、コンピュータプログラムとしても構成することができる。かかるコンピュータプログラムは、コンピュータが読取可能な記録媒体に記録されていてもよい。

干渉判定装置を含むロボットシステムの概略構成を示す説明図である。 干渉判定装置によって行われる分離軸判定の概要を示す説明図である。 干渉判定装置の概略構成を示す説明図である。 干渉判定処理のフローチャートである。 第１サブ判定処理の詳細なフローチャートである。 第１サブ判定処理によって決定される分離軸の例を示す図である。 第２サブ判定処理の詳細なフローチャートである。 第２サブ判定処理において、直方体モデルのＸ面の法線ベクトルに平行な直線が分離軸として決定された例を示す図である。 第２サブ判定処理において、直方体モデルのＹ面の法線ベクトルに平行な直線が分離軸として決定された例を示す図である。 第２サブ判定処理において、直方体モデルのＺ面の法線ベクトルに平行な直線が分離軸として決定された例を示す図である。 第３サブ判定処理の詳細なフローチャートである。 第３サブ判定処理で直方体モデルのＸ面の法線ベクトルが選択された場合における分離軸の例を示す図である。 第３サブ判定処理で直方体モデルのＹ面の法線ベクトルが選択された場合における分離軸の例を示す図である。 第３サブ判定処理で直方体モデルのＺ面の法線ベクトルが選択された場合における分離軸の例を示す図である。 第３サブ判定処理において円柱モデルと直方体モデルとが分離軸に射影される概念を示す図である。 円柱モデルの側面の法線ベクトルを示す図である。 第４サブ判定処理の詳細なフローチャートである。 第４サブ判定処理において円柱モデルと直方体モデルとが分離軸に射影される概念を示す図である。 第５サブ判定処理の詳細なフローチャートである。 第５サブ判定処理において円柱モデルと直方体モデルとが分離軸に射影される概念を示す図である。 第３サブ判定処理以降の処理が必要となる円柱モデルと直方体モデルとの位置関係を示す図である。

Ａ．装置構成：
図１は、本発明の一実施形態としての干渉判定装置４００を含むロボットシステム１０の概略構成を示す説明図である。ロボットシステム１０は、多関節型の産業用ロボット１００と、ロボット１００の動作を制御する制御装置２００と、ロボット１００の動作を制御装置２００に教示する教示装置３００と、ロボット１００と障害物５００との干渉を判定する干渉判定装置４００と、を含んでいる。ロボット１００と教示装置３００と干渉判定装置４００とは、それぞれ、所定のケーブルによって制御装置２００に接続されている。

ロボット１００は、工場内の設備に固定されるベース部１０１を備えている。ベース部１０１には、水平方向に旋回可能な関節軸によってベース部１０１に支持されたショルダ部１０２と、鉛直方向に旋回可能な関節軸によってショルダ部１０２に下端が支持された下アーム１０３と、鉛直方向に旋回可能な関節軸によって下アーム１０３の先端に略中央部が支持された上アーム１０４と、鉛直方向に旋回可能な関節軸によって上アーム１０４の先端に支持された手首１０５と、手首１０５の先端に設けられた回転可能なフランジ部１０６と、フランジ部１０６に取り付けられたエンドエフェクタ１０７と、が可動部として備えられている。

図２は、干渉判定装置４００によって行われる干渉判定処理の概要を示す説明図である。本実施形態の干渉判定装置４００は、分離軸判定という手法で干渉の判定を行う。図２（Ａ）に示されているように、分離軸判定では、２つの物体が干渉しているか否かを判定するために、まず、ある分離軸が決定され、その分離軸に２つの物体が射影される。そして、その射影によって分離軸上に表れる２つの像が重ならなければ、２つの物体は干渉していないと判定される。一方、図２（Ｂ）に示されているように、２つの像が重なれば、その分離軸では、２つの物体が干渉していないと判定することができない。そこで、他の分離軸が選択され、選択された他の分離軸への射影の結果に基づいて、再度、干渉の判定が行われる。分離軸判定では、選択された分離軸うち、２つの像が重ならない分離軸が１つでも存在すれば、２つの物体は干渉していないと判定される。これに対して、選択されたすべての分離軸で２つの像が重なれば、２つの物体は干渉していると判定される。

図３は、干渉判定装置４００の概略構成を示す説明図である。干渉判定装置４００は、ＣＰＵ４１０と、ＲＡＭ４２０と、ＨＤＤ（ハードディスクドライブ）４３０と、モニタ４４０と、インタフェース４５０と、を備えている。これらは、バス４６０を介してそれぞれ相互に接続されている。インタフェース４５０には、制御装置２００が接続される。干渉判定装置４００は、例えば、パーソナルコンピュータによって構成することができる。

ＨＤＤ４３０には、ロボット１００および障害物５００の外形形状を表すモデルデータ４３２が記憶されている。本実施形態では、ロボット１００の可動部の少なくとも一部の外形形状が、直円柱形状を表す円柱モデルによって表されている。また、障害物５００の外形形状は、直方体の形状を表す直方体モデルによって表されている。また、ＨＤＤ４３０には、ロボット１００と障害物５００との干渉の判定を実現するための干渉判定プログラム４３４が記憶されている。なお、これらのモデルデータ４３２および干渉判定プログラム４３４は、ＨＤＤ４３０に限られず、フレキシブルディスクやＣＤ−ＲＯＭ、ＤＶＤ−ＲＯＭ、光磁気ディスク、フラッシュメモリ等の記録媒体に記録されていてもよい。

ＣＰＵ４１０は、ＨＤＤ４３０に記憶された干渉判定プログラム４３４をＲＡＭ４２０にロードして実行することで、位置取得部４１２、分離軸決定部４１４、干渉判定部４１６として機能する。

位置取得部４１２は、制御装置２００からロボット１００の各可動部の位置を取得するとともに、予めＨＤＤ４３０に設定された障害物５００の位置を取得する機能を有する。

分離軸決定部４１４は、ロボット１００の可動部と障害物５００との干渉を検出するための分離軸の決定を行う機能を有する。具体的な分離軸の決定方法については後述する。

干渉判定部４１６は、分離軸決定部４１４によって決定された分離軸にロボット１００のモデル（円柱モデル）と障害物のモデル（直方体モデル）とを射影して、ロボット１００の可動部と障害物５００との干渉を判定する機能を有する。具体的な干渉の判定方法については後述する。

なお、本実施形態では、干渉判定装置４００と制御装置２００とをそれぞれ異なる装置として構成としたが、干渉判定装置４００の機能をすべて制御装置２００に組み込むこととしてもよい。

Ｂ．干渉判定処理：
図４は、ＣＰＵ４１０によって干渉判定プログラム４３４が実行されることによって繰り返し実行される干渉判定処理のフローチャートである。ＣＰＵ４１０によって干渉判定プログラム４３４が実行されると、まず、位置取得部４１２が、ロボット１００の各軸の回転角度を制御装置２００から取得する（ステップＳ１０）。位置取得部４１２は、ロボット１００の各軸の回転角度を取得すると、その回転角度から、各リンク座標系、ツール座標系におけるロボット１００の各可動部の位置および姿勢を計算する（ステップＳ１２）。ロボット１００の位置および姿勢を計算すると、位置取得部４１２は、干渉判定の対象となる可動部および障害物を特定する（ステップＳ１４）。判定対象を特定すると、位置取得部４１２は、特定された判定対象の中心座標系における判定対象の位置および姿勢を計算する（ステップＳ１６）。

判定対象の位置および姿勢を計算すると、位置取得部４１２は、判定対象の可動部が円柱モデルであり、かつ、障害物が直方体モデルであるかを判断する（ステップＳ１８）。判定対象が円柱モデルおよび直方体モデルでなければ（ステップＳ１８：ＮＯ）、ＣＰＵ４１０は、判定対象の種類に応じた判定処理を行う（ステップＳ２０）。例えば、判定対象が、点モデルと角柱モデルとであれば、角柱に点が内包されるか否かに基づき干渉の判定が行われ、判定対象が、直方体モデルと角柱モデルとであれば、直方体が角柱の内側に存在するか否かに基づき干渉の判定が行われる。

判定対象の可動部が円柱モデルであり、障害物が直方体モデルであると判断された場合には（ステップＳ１８：ＹＥＳ）、分離軸決定部４１４および干渉判定部４１６が、第１サブ判定処理（ステップＳ２２）と、第２サブ判定処理（ステップＳ２４）と、第３サブ判定処理（ステップＳ２６）と、第４サブ判定処理（ステップＳ２８）と、第５サブ判定処理（ステップＳ３０）と、をこの順で順番に実行する。第１サブ判定処理から第５サブ判定処理までのすべての判定処理において、「干渉なし」と判定されなければ、干渉判定部４１６は、「干渉あり」と判定し、モニタ４４０にその判定結果（干渉あり）を表示する（ステップＳ３２）。これに対して、詳細は後述するが、第１サブ判定処理から第５サブ判定処理までの判定処理のうち、いずれかの判定処理で「干渉なし」と判定されれば、それ以降の判定処理は実行されることなく、干渉判定部４１６により、モニタ４４０にその判定結果（干渉なし）が表示され、当該干渉判定処理は終了する。

Ｃ．第１サブ判定処理：
図５は、第１サブ判定処理の詳細なフローチャートである。第１サブ判定処理は、円柱モデルの上面および下面と、直方体の外形（辺）との干渉を判定するための処理である。第１サブ判定処理が開始されると、分離軸決定部４１４は、円柱モデルの上面の法線ベクトルに平行な直線を分離軸として決定する（ステップＳ１００。図６（Ｄ）参照）。第１サブ判定処理によって決定された分離軸は、特許請求の範囲における「第５の分離軸」に相当する。

分離軸が決定されると、干渉判定部４１６は、決定された分離軸に、円柱モデルと直方体モデルとを射影し（ステップＳ１０２）、分離軸上に射影された円柱モデルの像と直方体モデルの像とが重なるか否かを判断する（ステップＳ１０４）。

分離軸上で円柱モデルの像と直方体モデルの像とが重なる場合には（ステップＳ１０４：ＹＥＳ）、円柱モデルの上面または下面と、直方体の外形とが干渉している可能性があるため、更に詳しく干渉の判定を行うために、干渉判定部４１６は、当該第１サブ判定処理を終了させ、図４に示した第２サブ判定処理（ステップＳ２４）に処理を移行させる。これに対して、分離軸上で円柱モデルの像と直方体モデルの像とが重ならない場合には（ステップＳ１０４：ＮＯ）、図４に示した第２〜５サブ判定処理を実行しなくても、円柱モデルと直方体モデルとが干渉しないことが明らかである。そのため、干渉判定部４１６は、そのまま、「干渉なし」と判断して、モニタ４４０にその判定結果（干渉なし）を表示させる（ステップＳ１０６）。モニタ４４０に判定結果を表示させると、干渉判定部４１６は、図４に示した一連の干渉判定処理を終了させる。

図６は、上述した第１サブ判定処理によって決定される分離軸の例を示す図である。（図６（Ａ）〜（Ｄ）には、円柱モデルと直方体モデルの位置関係を異なる視点から観察した様子が示されている。図６（Ａ）には直方体モデルを＋Ｘ方向側から見た図が、図６（Ｂ）には直方体モデルを＋Ｙ方向から見た図が、図６（Ｃ）には直方体モデルを＋Ｚ方向から見た図が、それぞれ示されている。図６（Ｄ）には、円柱モデルの上面の法線ベクトルに平行な分離軸ＡＸ１に、円柱モデルと直方体モデルとが射影された様子が示されている。図６（Ｄ）に示された例では、分離軸ＡＸ１上に射影された像同士が重なっていないため、「干渉なし」と判定される。

Ｄ．第２サブ判定処理：
図７は、第２サブ判定処理の詳細なフローチャートである。第２サブ判定処理は、第１サブ判定処理において「干渉なし」と判定されなかった場合に実行される処理であり、直方体モデルの面と円柱モデルの外形との干渉を判定するための処理である。第２サブ判定処理が開始されると、分離軸決定部４１４は、直方体モデルの３種類の面法線ベクトルの中から１つの面法線ベクトルを選択し（ステップＳ２００）、選択された面法線ベクトルと円柱モデルの上面の法線ベクトルとを含む平面を作成する（ステップＳ２０２）。このような平面を作成すると、分離軸決定部４１４は、作成された平面上で、選択された面法線ベクトルに平行な直線を分離軸として決定する（ステップＳ２０４。図８（Ｂ）、図９（Ａ）、図１０（Ｂ）参照）。第２サブ判定処理で決定された分離軸は、特許請求の範囲における「第４の分離軸」に相当する。なお、直方体モデルの３種類の面法線ベクトルとは、直方体モデルの計６本の面法線ベクトルのうち、逆向きの２本の面法線ベクトルを１種類と捉えたものである。

分離軸が決定されると、干渉判定部４１６は、ステップＳ２０２で作成された平面に、円柱モデルと直方体モデルとを射影する（ステップＳ２０６）。そして、更に、平面上に射影された円柱モデルの像と直方体モデルの像とを、ステップＳ２０４で決定された分離軸に射影する（ステップＳ２０８）。このように、平面を介して分離軸に段階的に円柱モデルと直方体モデルとが射影されると、干渉判定部４１６は、分離軸に射影された円柱モデルの像と直方体モデルの像とが重なるか否かを判断する（ステップＳ２１０）。分離軸上で円柱モデルの像と直方体モデルの像とが重ならない場合には（ステップＳ２１０：ＮＯ）、図４に示した第３〜５サブ判定処理を実行しなくても、円柱モデルと直方体モデルとが干渉しないことが明らかである。そのため、そのまま、「干渉なし」と判断し、モニタ４４０にその判定結果（干渉なし）を表示させる（ステップＳ２０８）。モニタ４４０に判定結果を表示させると、干渉判定部４１６は、図４に示した一連の干渉判定処理を終了させる。

分離軸上で円柱モデルの像と直方体モデルの像とが重なる場合には（ステップＳ２１０：ＹＥＳ）、干渉判定部４１６は、更に詳しく干渉の判定を行うため、直方体モデルの３種類の全ての面法線ベクトルを用いて当該第２サブ判定処理における干渉の判定を行ったか否かを判断する（ステップＳ２１４）。３種類の全ての面法線ベクトルを用いて干渉の判定を行っていない場合には（ステップＳ２１４：ＮＯ）、干渉判定部４１６は、処理をステップＳ２００に戻す。そうすると、３種類の面法線ベクトルのうちのまだ選択されていない面法線ベクトルを用いた干渉の判定が行われる。これに対して、３種類の全ての面法線ベクトルを用いて干渉の判定を行ったにもかかわらず、「干渉なし」と判定されなかった場合には（ステップＳ２１４：ＹＥＳ）、更に他の分離軸を用いて干渉の判定を行うため、干渉判定部４１６は、当該第２判定処理を終了させて、図４に示した第３サブ判定処理（ステップＳ２６）に処理を移行させる。

なお、本実施形態の第２サブ判定処理では、まず、平面を作成し、その平面に対して各モデルを射影した上で、更に平面上の各モデルの像を分離軸に射影している。これに対して、第２サブ判定処理では、平面を作成することなく、直方体モデルの各面法線ベクトルに平行な分離軸に、直接的に各モデルを射影してもよい。本実施形態のように、平面を作成してその平面上に各モデルを射影すれば、その平面に各モデル（特に、円柱モデル）を精度良く射影することができるので、分離軸に対しても各モデルを精度良く射影することができる。一方、分離軸に対して直接的に各モデルを射影することとすれば、平面を作成する処理が省略されるので、処理時間を短縮することが可能になる。なお、図５に示した第１サブ判定処理において平面を作成しないのは、第１サブ判定処理では、円柱モデルの上面の法線ベクトルに平行な直線、つまり、円柱の中心軸に平行な直線が分離軸として決定されるため、円柱モデルはその分離軸に対して常に精度良く平行に射影することが可能だからである。

図８は、第２サブ判定処理において、直方体モデルのＸ面の法線ベクトルに平行な直線が分離軸として決定された例を示す図である。図８（Ａ）〜（Ｃ）には、同じ位置関係にある円柱モデルと直方体モデルとを異なる視点から観察した様子が示されている。図８（Ａ）には直方体モデルを＋Ｘ方向側から見た図が、図８（Ｂ）には直方体モデルを＋Ｙ方向から見た図が、図８（Ｃ）には直方体モデルを＋Ｚ方向から見た図が、それぞれ示されている。図８（Ｂ）には、直方体モデルのＸ面の面法線ベクトルに平行な分離軸ＡＸ２に、円柱モデルと直方体モデルとが射影された様子が示されている。図８（Ｂ）に示された例では、分離軸ＡＸ２上に射影された像同士が重なっていないため、「干渉なし」と判定される。なお、図８（Ｂ）には、直方体モデルの面法線ベクトルと円柱モデルの上面の法線ベクトルとを含む平面の図示は省略されている。

図９は、第２サブ判定処理において、直方体モデルのＹ面の法線ベクトルに平行な直線が分離軸として決定された例を示す図である。図９（Ａ）〜（Ｃ）には、同じ位置関係にある円柱モデルと直方体モデルとを異なる視点から観察した様子が示されている。図９（Ａ）には直方体モデルを＋Ｘ方向側から見た図が、図９（Ｂ）には直方体モデルを＋Ｙ方向から見た図が、図９（Ｃ）には直方体モデルを＋Ｚ方向から見た図が、それぞれ示されている。図９（Ａ）には、直方体モデルのＹ面の面法線ベクトルに平行な分離軸ＡＸ３に、円柱モデルと直方体モデルとが射影された様子が示されている。図９（Ａ）に示された例では、分離軸ＡＸ３上に射影された像同士が重なっていないため、「干渉なし」と判定される。なお、図９（Ａ）には、直方体モデルの面法線ベクトルと円柱モデルの上面の法線ベクトルとを含む平面の図示は省略されている。

図１０は、第２サブ判定処理において、直方体モデルのＺ面の法線ベクトルに平行な直線が分離軸として決定された例を示す図である。図１０（Ａ）〜（Ｃ）には、同じ位置関係にある円柱モデルと直方体モデルとを異なる視点から観察した様子が示されている。図１０（Ａ）には直方体モデルを＋Ｘ方向側から見た図が、図１０（Ｂ）には直方体モデルを＋Ｙ方向から見た図が、図１０（Ｃ）には直方体モデルを＋Ｚ方向から見た図が、それぞれ示されている。図１０（Ｂ）には、直方体モデルのＺ面の面法線ベクトルに平行な分離軸ＡＸ４に、円柱モデルと直方体モデルとが射影された様子が示されている。図１０（Ｂ）に示された例では、分離軸ＡＸ４上に射影された像同士が重なっていないため、「干渉なし」と判定される。なお、図１０（Ｂ）には、直方体モデルの面法線ベクトルと円柱モデルの上面の法線ベクトルとを含む平面の図示は省略されている。

Ｅ．第３サブ判定処理：
図１１は、第３サブ判定処理の詳細なフローチャートである。第３サブ判定処理は、第２サブ判定処理において「干渉なし」と判定されなかった場合に実行される処理であり、円柱モデルの側面と直方体モデルの外形（辺）との干渉を判定するための処理である。第３サブ判定処理が開始されると、分離軸決定部４１４は、直方体モデルの３種類の面法線ベクトルの中から１つの面法線ベクトルを選択し（ステップＳ３００）、選択された面法線ベクトルと円柱モデルの上面の法線ベクトルとを含む平面を作成する（ステップＳ３０２。図１５の平面ＰＦ１参照）。このような平面を作成すると、分離軸決定部４１４は、作成された平面上で、円柱モデルの側面の法線ベクトルに平行な直線を分離軸として決定する（ステップＳ３０４。図１５の分離軸ＡＸ３参照）。第３サブ判定処理で決定された分離軸は、特許請求の範囲における「第１の分離軸」に相当し、第３サブ判定処理で生成された平面は、特許請求の範囲における「第１の平面」に相当する。

分離軸が決定されると、干渉判定部４１６は、ステップＳ３０２で作成された平面に、円柱モデルと直方体モデルとを射影する（ステップＳ３０６）。そして、更に、平面上に射影された円柱モデルの像と直方体モデルの像とを、ステップＳ３０４で決定された分離軸に射影する（ステップＳ３０８）。このように、平面を介して分離軸に段階的に円柱モデルと直方体モデルとが射影されると、干渉判定部４１６は、分離軸に射影された円柱モデルの像と直方体モデルの像とが重なるか否かを判断する（ステップＳ３１０）。分離軸上で円柱モデルの像と直方体モデルの像とが重ならない場合には（ステップＳ３１０：ＮＯ）、図４に示した第４〜５サブ判定処理を実行しなくても、円柱モデルと直方体モデルとが干渉しないことが明らかである。そのため、そのまま、「干渉なし」と判断し、モニタ４４０にその判定結果（干渉なし）を表示させる（ステップＳ３１２）。モニタ４４０に判定結果を表示させると、干渉判定部４１６は、図４に示した一連の干渉判定処理を終了させる。

分離軸上で円柱モデルの像と直方体モデルの像とが重なる場合には（ステップＳ３１０：ＹＥＳ）、干渉判定部４１６は、更に詳しく干渉の判定を行うため、直方体モデルの３つの全ての面法線ベクトルを用いて当該第３サブ判定処理における干渉の判定を行ったか否かを判断する（ステップＳ３１４）。全ての面法線ベクトルを用いて干渉の判定を行っていない場合には（ステップＳ３１４：ＮＯ）、干渉判定部４１６は、処理をステップＳ３００に戻す。そうすると、３種類の面法線ベクトルのうちのまだ選択されていない面法線ベクトルを用いた干渉の判定が行われる。これに対して、３種類の全ての面法線ベクトルを用いて干渉の判定を行ったにもかかわらず、「干渉なし」と判定されなかった場合には（ステップＳ３１４：ＹＥＳ）、更に他の分離軸を用いて干渉の判定を行うため、干渉判定部４１６は、当該第３判定処理を終了させて、図４に示した第４サブ判定処理（ステップＳ２８）に処理を移行させる。

図１２は、第３サブ判定処理のステップＳ３００で直方体モデルのＸ面の法線ベクトルが選択された場合における分離軸の例を示す図である。図１２（Ａ）〜（Ｄ）には、同じ位置関係にある円柱モデルと直方体モデルとを異なる視点から観察した様子が示されている。図１２（Ａ）には直方体モデルを＋Ｘ方向側から見た図が、図１２（Ｂ）には直方体モデルを＋Ｙ方向から見た図が、図１２（Ｃ）には直方体モデルを＋Ｚ方向から見た図が、それぞれ示されている。図１２（Ｄ）には、円柱モデルの上面の法線ベクトルと、直方体モデルのＸ面の法線ベクトルとを含む平面ＰＦ１ａが示されており、また、この平面ＰＦに含まれる円柱モデルの側面の法線ベクトルに平行な分離軸ＡＸ３ａが示されている。

図１３は、第３サブ判定処理のステップＳ３００で直方体モデルのＹ面の法線ベクトルが選択された場合における分離軸の例を示す図である。図１３（Ａ）〜（Ｄ）には、同じ位置関係にある円柱モデルと直方体モデルとを異なる視点から観察した様子が示されている。図１３（Ａ）には直方体モデルを＋Ｘ方向側から見た図が、図１３（Ｂ）には直方体モデルを＋Ｙ方向から見た図が、図１３（Ｃ）には直方体モデルを＋Ｚ方向から見た図が、それぞれ示されている。図１３（Ｄ）には、円柱モデルの上面の法線ベクトルと、直方体モデルのＹ面の法線ベクトルとを含む平面ＰＦ１ｂが示されており、また、この平面ＰＦに含まれる円柱モデルの側面の法線ベクトルに平行な分離軸ＡＸ３ｂが示されている。

図１４は、第３サブ判定処理のステップＳ３００で直方体モデルのＺ面の法線ベクトルが選択された場合における分離軸の例を示す図である。図１４（Ａ）〜（Ｄ）には、同じ位置関係にある円柱モデルと直方体モデルとを異なる視点から観察した様子が示されている。図１４（Ａ）には直方体モデルを＋Ｘ方向側から見た図が、図１４（Ｂ）には直方体モデルを＋Ｙ方向から見た図が、図１４（Ｃ）には直方体モデルを＋Ｚ方向から見た図が、それぞれ示されている。図１４（Ｄ）には、円柱モデルの上面の法線ベクトルと、直方体モデルのＺ面の法線ベクトルとを含む平面ＰＦ１ｃが示されており、また、この平面ＰＦに含まれる円柱モデルの側面の法線ベクトルに平行な分離軸ＡＸ３ｃが示されている。

図１５は、第３サブ判定処理において円柱モデルと直方体モデルとが分離軸に射影される概念を示す図である。図１５には、直方体モデルのＺ面の法線ベクトルＮＶ１と、円柱モデルの上面の法線ベクトルＮＶ２と、これらの法線ベクトルＮＶ１，ＮＶ２を含む平面ＰＦ１とが示されている。また、図１５には、平面ＰＦ１に含まれる円柱モデルの側面の法線ベクトルＮＶ３に平行な分離軸ＡＸ３が示されている。上記ステップＳ３０６では、まず、平面ＰＦ１上に、円柱モデルと直方体モデルとが射影される。図１５には、こうして射影された平面ＰＦ１上の二次元の像がハッチングにより表されている。上記ステップＳ３０８では、この二次元の像から、平面ＰＦ１上の分離軸ＡＸ３に対して更に射影が行われる。このように、上記第３サブ判定処理では、直方体モデルの面法線ベクトルと円柱モデルの上面の法線ベクトルとを含む平面を作成し、その平面上で円柱モデルの側面の法線ベクトルに平行な直線を分離軸として決定する。そのため、図１６に示すように、３６０°に亘って無数に存在する円柱モデルの側面の法線ベクトルの中から、分離軸として用いる法線ベクトルを確実に特定することができるので、効率的に、直方体モデルの面と円柱モデルの側面との干渉を判定することができる。なお、図１５に示された例では、分離軸ＡＸ３上に射影された像同士が重なっていないため、「干渉なし」と判定される。

Ｆ．第４サブ判定処理：
図１７は、第４サブ判定処理の詳細なフローチャートである。第４サブ判定処理は、第３サブ判定処理において「干渉なし」と判定されなかった場合に実行される処理であり、円柱モデルの側面と直方体モデルの外形（辺）との干渉を第３サブ判定処理とは異なる分離軸で判定するための処理である。第４サブ判定処理が開始されると、分離軸決定部４１４は、直方体モデルの１２本の各辺に平行な３種類のベクトル（辺ベクトル）の中から１つ（１種類）の辺ベクトルを選択し（ステップＳ４００）、選択された辺ベクトルと円柱モデルの上面の法線ベクトルとの外積ベクトルを計算する（ステップＳ４０２。図１８の外積ベクトルＣＰ１参照）。外積ベクトルを計算すると、分離軸決定部４１４は、計算された外積ベクトルと円柱モデルの上面の法線ベクトルとを含む平面を作成する（ステップＳ４０４。図１８の平面ＰＦ２参照）。このような平面を作成すると、分離軸決定部４１４は、作成された平面上で、円柱モデルの側面の法線ベクトルに平行な直線を分離軸として決定する（ステップＳ４０６。図１８の分離軸ＡＸ４参照）。第４サブ判定処理で決定された分離軸は、特許請求の範囲における「第２の分離軸」に相当する。

分離軸が決定されると、干渉判定部４１６は、ステップＳ４０４で作成された平面に、円柱モデルと直方体モデルとを射影する（ステップＳ４０８）。そして、更に、平面上に射影された円柱モデルの像と直方体モデルの像とを、ステップＳ４０６で決定された分離軸に射影する（ステップＳ４１０）。このように、平面を介して分離軸に段階的に円柱モデルと直方体モデルとが射影されると、干渉判定部４１６は、分離軸に射影された円柱モデルの像と直方体モデルの像とが重なるか否かを判断する（ステップＳ４１２）。分離軸上で円柱モデルの像と直方体モデルの像とが重ならない場合には（ステップＳ４１２：ＮＯ）、図４に示した第５サブ判定処理を実行しなくても、円柱モデルと直方体モデルとが干渉しないことが明らかである。そのため、そのまま、「干渉なし」と判断し、モニタ４４０にその判定結果（干渉なし）を表示させる（ステップＳ４１４）。モニタ４４０に判定結果を表示させると、干渉判定部４１６は、図４に示した一連の干渉判定処理を終了させる。

分離軸上で円柱モデルの像と直方体モデルの像とが重なる場合には（ステップＳ４１２：ＹＥＳ）、干渉判定部４１６は、更に詳しく干渉の判定を行うため、直方体モデルの３種類の全ての辺ベクトルを用いて当該第４サブ判定処理における干渉の判定を行ったか否かを判断する（ステップＳ４１６）。３種類の全ての辺ベクトルを用いて干渉の判定を行っていない場合には（ステップＳ４１６：ＮＯ）、干渉判定部４１６は、処理をステップＳ４００に戻す。そうすると、３種類の辺ベクトルのうちのまだ選択されていない辺ベクトルを用いた干渉の判定が行われる。これに対して、３種類の全ての辺ベクトルを用いて干渉の判定を行ったにもかかわらず、「干渉なし」と判定されなかった場合には（ステップＳ４１６：ＹＥＳ）、更に他の分離軸を用いて干渉の判定を行うため、干渉判定部４１６は、当該第４判定処理を終了させて、図４に示した第５サブ判定処理（ステップＳ３０）に処理を移行させる。

図１８は、第４サブ判定処理において円柱モデルと直方体モデルとが分離軸に射影される概念を示す図である。図１８には、直方体モデルの辺ベクトルＳＶ１と、円柱モデルの上面の法線ベクトルＮＶ２と、辺ベクトルＳＶ１および法線ベクトルＮＶ２から求まる外積ベクトルＣＰ１とが示されている。また、図１８には、外積ベクトルＣＰ１と円柱モデルの上面の法線ベクトルＮＶ２とを含む平面ＰＦ２と、この平面ＰＦ２に含まれる円柱モデルの側面の法線ベクトルＮＶ３に平行な分離軸ＡＸ４が示されている。上記ステップＳ４０８では、まず、平面ＰＦ２上に、円柱モデルと直方体モデルとが射影される。図１８には、こうして射影された平面ＰＦ２上の二次元の像がハッチングにより表されている。上記ステップＳ４１０では、この二次元の像から、平面ＰＦ２上の分離軸ＡＸ４に対して更に射影が行われる。このように、上記第４サブ判定処理では、直方体モデルの辺ベクトルと円柱モデルの上面の法線ベクトルとから外積ベクトルを算出し、この外積ベクトルと円柱モデルの上面の法線ベクトルとを含む平面を作成する。そして、その平面上に存在する円柱モデルの側面の法線ベクトルに平行な直線を分離軸として決定する。そのため、第３サブ判定処理と同様に、３６０°に亘って無数に存在する円柱モデルの側面の法線ベクトルの中から、分離軸として用いる法線ベクトルを確実に特定することができ、これにより、効率的に、直方体モデルの外形と円柱モデルの側面との干渉を判定することができる。なお、図１８に示された例では、分離軸ＡＸ４上に射影された像同士が重なっていないため、「干渉なし」と判定される。

Ｇ．第５サブ判定処理：
図１９は、第５サブ判定処理の詳細なフローチャートである。第５サブ判定処理は、第１〜４サブ判定処理のいずれの処理でも干渉していないことが判定できない位置関係にある直方体モデルの外形と円柱モデルの外形との干渉を判定するための処理である。第５サブ判定処理が開始されると、分離軸決定部４１４は、直方体モデルの３種類の辺ベクトルの中から１つの辺ベクトルを選択し（ステップＳ５００）、選択された辺ベクトルと円柱モデルの上面の法線ベクトルとの外積ベクトルを計算する（ステップＳ５０２。図２０の外積ベクトルＣＰ２参照）。外積ベクトルを計算すると、分離軸決定部４１４は、計算された外積ベクトルと円柱モデルの上面の法線ベクトルとを含む平面を作成する（ステップＳ５０４。図２０の平面ＰＦ３参照）。このような平面を作成すると、分離軸決定部４１４は、作成された平面上の上記外積ベクトルに平行な直線を分離軸として決定する（ステップＳ５０６。図２０の分離軸ＡＸ５参照）。第５サブ判定処理で決定された分離軸は、特許請求の範囲における「第３の分離軸」に相当し、第５サブ判定処理で生成された平面は、特許請求の範囲における「第３の平面」に相当する。

分離軸が決定されると、干渉判定部４１６は、ステップＳ５０４で作成された平面に、円柱モデルと直方体モデルとを射影する（ステップＳ５０８）。そして、更に、平面上に射影された円柱モデルの像と直方体モデルの像とを、ステップＳ５０６で決定された分離軸に射影する（ステップＳ５１０）。このように、平面を介して分離軸に段階的に円柱モデルと直方体モデルとが射影されると、干渉判定部４１６は、分離軸に射影された円柱モデルの像と直方体モデルの像とが重なるか否かを判断する（ステップＳ５１２）。分離軸上で円柱モデルの像と直方体モデルの像とが重ならない場合には（ステップＳ５１２：ＮＯ）、干渉判定部４１６は、「干渉なし」と判断し、モニタ４４０にその判定結果（干渉なし）を表示させる（ステップＳ５１４）。モニタ４４０に判定結果を表示させると、干渉判定部４１６は、図４に示した一連の干渉判定処理を終了させる。

分離軸上で円柱モデルの像と直方体モデルの像とが重なる場合には（ステップＳ５１２：ＹＥＳ）、干渉判定部４１６は、直方体モデルの３種類の全ての辺ベクトルを用いて当該第５サブ判定処理における干渉の判定を行ったか否かを判断する（ステップＳ５１６）。３種類の全ての辺ベクトルを用いて干渉の判定を行っていない場合には（ステップＳ５１６：ＮＯ）、干渉判定部４１６は、処理をステップＳ５００に戻す。そうすると、３種類の辺ベクトルのうちのまだ選択されていない辺ベクトルを用いた干渉の判定が行われる。これに対して、３種類の全ての辺ベクトルを用いて干渉の判定を行ったにもかかわらず、「干渉なし」と判定されなかった場合には（ステップＳ５１６：ＹＥＳ）、干渉判定部４１６は、当該第５サブ判定処理を終了させ、処理を図４に示した干渉判定処理のステップＳ３２に移行させる。そうすると、干渉判定部４１６は、ここではじめて、「干渉あり」と判断し、モニタ４４０に判定結果を表示させる。つまり、本実施形態では、第１サブ判定処理から第５サブ判定処理までのすべての判定処理において、「干渉なし」と判断されなかった場合に、円柱モデルと直方体モデルとが干渉している可能性が高いと判断し、「干渉あり」との判定を行う。

図２０は、第５サブ判定処理において円柱モデルと直方体モデルとが分離軸に射影される概念を示す図である。図２０には、直方体モデルの辺ベクトルＳＶ２と、円柱モデルの上面の法線ベクトルＮＶ２と、辺ベクトルＳＶ１および法線ベクトルＮＶ２から求まる外積ベクトルＣＰ２とが示されている。また、図１５には、外積ベクトルＣＰ２と円柱モデルの上面の法線ベクトルＮＶ２とを含む平面ＰＦ３と、この平面ＰＦ３に含まれる外積ベクトルＣＰ２に平行な分離軸ＡＸ５が示されている。上記ステップＳ５０８では、まず、平面ＰＦ３上に、円柱モデルと直方体モデルとが射影される。図２０には、こうして射影された平面ＰＦ３上の二次元の像がハッチングにより表されている。上記ステップＳ５１０では、この二次元の像から、平面ＰＦ３上の分離軸ＡＸ５に対して更に射影が行われる。図２０に示された例では、分離軸ＡＸ５上に射影された像同士が重なっているため、３種類の全ての辺ベクトルを用いて干渉の判定が行われた場合には、「干渉あり」と判定される。ただし、他のベクトルを用いた干渉の判定において干渉なしと判断されれば、「干渉なし」と判定される。

なお、本実施形態の第５サブ判定処理では、まず、平面を作成し、その平面に対して各モデルを射影した上で、更にそれらの像を分離軸に射影している。しかし、第５サブ判定処理では、第２サブ判定処理と同様に、平面を作成することなく、外積ベクトルに平行な分離軸に、直接的に各モデルを射影してもよい。

以上で説明した本実施形態の干渉判定装置４００は、上述した第１サブ判定処理を実行することにより、円柱モデルの上面または下面と、直方体モデルの外形との干渉を判定することができ、第２サブ判定処理を実行することにより、直方体モデルの各面と円柱モデルの外形との干渉を判定することができる。更に、第３サブ判定処理を実行することにより、円柱モデルの側面と直方体モデルの外形との干渉を判定することができる。また、第４サブ判定処理や第５サブ判定処理において、円柱モデルの法線ベクトルと直方体の辺ベクトルとの外積を求めることで、更に異なる分離軸により、円柱モデルと直方体モデルとの干渉を判定することができる。

図２１は、第３サブ判定処理以降の処理が必要となる円柱モデルと直方体モデルとの位置関係を示す図である。図２１（Ａ）〜（Ｄ）には、すべて同じ位置関係にある円柱モデルと直方体モデルとを異なる視点から観察した様子が示されている。図２１（Ａ）を参照すれば、円柱モデルと直方体モデルとは明らかに干渉していないが、この位置関係において、第１サブ判定処理を実行すれば、円柱モデルの上面の法線ベクトルに平行な直線が分離軸となり、その分離軸には、円柱モデルと直方体モデルとが重なって射影される。そのため、第２サブ判定処理の実行が必要となる。第２サブ判定処理を実行すれば、直方体の各面の法線ベクトルに平行な直線が分離軸となるため、例えば、図２１（Ｂ）に示すように、直方体モデルのＺ面の法線ベクトルに平行な直線が分離軸として決定される。しかし、図２１（Ｂ）に示す例では、やはり、円柱モデルと直方体モデルとは分離軸に重なって射影される。また、図２１（Ｃ）や図２１（Ｄ）に示すように、分離軸として直方体モデルのＸ面の法線ベクトルに平行な直線やＹ面の法線ベクトルに平行な直線が分離軸として決定されても、やはり、円柱モデルと直方体モデルとはこれらの分離軸に重なって投影される。つまり、図２１に示した円柱モデルと直方体モデルとの位置関係では、明らかに、円柱モデルと直方体モデルとは干渉していないが、第１サブ判定処理や第２サブ判定処理においては、干渉なしと判断されることがない。そのため、本実施形態では、第１サブ判定処理や第２サブ判定処理だけではなく、第３サブ判定処理や第４サブ判定処理、第５サブ判定処理を実行することにより、様々な分離軸が決定されるので、精度良く干渉の判定を行うことが可能になる。

また、本実施形態の第３サブ判定処理や第４サブ判定処理では、それぞれ、２つのベクトルから平面を作成し、その平面上に存在する円柱モデルの側面の法線ベクトルに平行な直線を分離軸として決定する。そのため、３６０°に亘って無数に存在する円柱モデルの側面の法線ベクトルの中から、分離軸として用いる法線ベクトルを確実に特定することができる。この結果、効率的に、直方体モデルと円柱モデルの側面との干渉を判定することができる。このように、本実施形態では、円柱モデルを用いた場合であっても、迅速に干渉の判定を行うことができるので、ロボット１００の可動部と障害物５００とのリアルタイムの干渉判定が容易となる。そのため、例えば、干渉判定装置４００を制御装置２００に組み込んだ構成のように、干渉の判定にＣＰＵリソースの多くを割り当てることができないようなシステムであっても十分に高速な干渉判定を行うことができる。また、上記のように、本実施形態では、円柱モデルを用いた場合であっても、迅速に干渉の判定を行うことができるので、ロボット１００の各可動部の外形形状を、直方体モデルなどの単純なモデルだけではなく、円柱モデルを用いて精度良くモデリングすることができる。よって、実際のロボット１００の形状に即した干渉の判定が可能となるので、狭小空間での干渉の判定など、精度が求められる干渉の判定も容易に行うことが可能になる。

また、本実施形態では、第１サブ判定処理から第５サブ判定処理までのうち、いずれかの判定処理において、干渉なしと判定されれば、その時点で干渉判定処理全体が終了する。そのため、干渉の判定を高速に行うことができる。特に、本実施形態では、第１サブ判定処理の処理内容が最も単純であり、後に実行されるサブ判定処理ほど、平面の作成や外積の計算など、処理内容が複雑になっている。そのため、早い段階で干渉なしと判定されれば、それだけ高速に全体の処理を終了させることができる。よって、円柱モデルを用いてロボット１００の可動部がモデリングされていても、干渉判定の平均的な処理速度を向上させることができる。

以上、本発明の一実施形態について説明したが、本発明はこのような実施形態に限定されず、その趣旨を逸脱しない範囲で種々の構成を採ることができる。例えば、ソフトウェアによって実現した機能を、ハードウェアによって実現するものとしてもよい。その他、以下のような変形が可能である。

上記実施形態における第１サブ判定処理から第５サブ判定処理までの処理は、少なくとも一部を省略してもよい。また、第１サブ判定処理から第５サブ判定処理実行までの処理の実行順序も適宜変更することが可能である。

上記実施形態では、干渉判定装置４００は、ロボット１００の可動部と障害物との干渉を判定しているが、ロボット１００以外の産業用機械と障害物との干渉を判定してもよい。また、ロボット１００の各可動部を円柱モデルと直方体モデルとで表して、可動部同士の干渉の判定を行ってもよい。

上記実施形態では、ロボット１００の可能部の少なくとも一部の外形形状を円柱モデルによって表し、障害物５００の外形形状を直方体モデルによって表している。これに対して、ロボット１００の可動部の少なくとも一部を直方体モデルによって表し、障害物５００の外形形状の少なくとも一部を円柱モデルによって表してもよい。つまり、円柱モデルによって表される物体と直方体モデルによって表される物体との干渉判定ができればよい。

上記実施形態の各サブ判定処理において、円柱モデルの上面の法線ベクトルを用いた処理では、円柱モデルの下面の法線ベクトルを用いてもよい。また、直方体モデルの面法線ベクトルを用いた処理では、面法線ベクトルに替えて辺ベクトルを用いても良く、逆に、辺ベクトルを用いた処理では、辺ベクトルに替えて面法線ベクトルを用いてもよい。

上記実施形態では、干渉判定装置４００は、制御装置２００からロボット１００の可動部の位置を取得している。これに対して、干渉判定装置４００は、干渉判定装置４００のＲＡＭ４２０あるいはＨＤＤ４３０に記録された可動部の位置を取得してもよい。また、干渉判定装置４００は、実際のロボット１００の干渉の判定を行ってもよいし、ロボット１００の教示時や、ロボット１００の動作シミュレーション時において、仮想的に干渉の判定を行ってもよい。

１０…ロボットシステム
１００…ロボット
１０１…ベース部
１０２…ショルダ部
１０３…下アーム
１０４…上アーム
１０５…手首
１０６…フランジ部
１０７…エンドエフェクタ
２００…制御装置
３００…教示装置
４００…干渉判定装置
４１０…ＣＰＵ
４１２…位置取得部
４１４…分離軸決定部
４１６…干渉判定部
４２０…ＲＡＭ
４３０…ＨＤＤ
４３２…モデルデータ
４３４…干渉判定プログラム
４４０…モニタ
４５０…インタフェース
４６０…バス
５００…障害物

Claims (9)

1. 可動部を有する産業用機械と障害物との干渉を判定する干渉判定装置であって、
   前記可動部の少なくとも一部の形状と前記障害物の少なくとも一部の形状とのうち、一方の形状を直円柱によって表した円柱モデルのデータと、他方の形状を直方体によって表した直方体モデルのデータと、が記憶された記憶部と、
   前記円柱モデルが表す形状の位置と、前記直方体モデルが表す形状の位置と、を取得する位置取得部と、
   前記円柱モデルの上面または下面の法線ベクトルと、前記直方体モデルの面法線ベクトルまたは辺ベクトルのうちのいずれか１のベクトルとを含む第１の平面を求め、該第１の平面上における前記円柱モデルの側面の法線ベクトルに平行な直線を、第１の分離軸として決定する分離軸決定部と、
   前記第１の平面に前記円柱モデルと前記直方体モデルとを射影し、更に、前記第１の平面上の前記第１の分離軸に、前記第１の平面に射影された前記円柱モデルの像と前記直方体モデルの像とを射影し、前記第１の分離軸に射影された前記円柱モデルの像と前記直方体モデルの像との重なりの有無に基づいて、前記可動部と前記障害物との干渉を判定する干渉判定部と、
   を備える干渉判定装置。
2. 請求項１に記載の干渉判定装置であって、
   前記分離軸決定部は、更に、前記円柱モデルの上面または下面の法線ベクトルと、前記直方体モデルの辺ベクトルまたは面法線ベクトルのうちのいずれか１のベクトルとの外積ベクトルを求め、該外積ベクトルと前記円柱モデルの上面または下面の法線ベクトルとを含む第２の平面を求め、該第２の平面上における前記円柱モデルの側面の法線ベクトルに平行な直線を、第２の分離軸として決定し、
   前記干渉判定部は、更に、前記第２の平面に前記円柱モデルと前記直方体モデルとを射影し、更に、前記第２の平面上の前記第２の分離軸に、前記第２の平面に射影された前記円柱モデルの像と前記直方体モデルの像とを射影し、前記第２の分離軸に射影された前記円柱モデルの像と前記直方体モデルの像との重なりの有無に基づいて、前記可動部と前記障害物との干渉を判定する、干渉判定装置。
3. 請求項１または請求項２に記載の干渉判定装置であって、
   前記分離軸決定部は、更に、前記円柱モデルの上面または下面の法線ベクトルと、前記直方体モデルの辺ベクトルまたは面法線ベクトルのうちのいずれか１のベクトルとの外積ベクトルを求め、該外積ベクトルに平行な直線を、第３の分離軸として決定し、
   前記干渉判定部は、更に、前記第３の分離軸に前記円柱モデルと前記直方体モデルとを射影し、前記第３の分離軸に射影された前記円柱モデルの像と前記直方体モデルの像との重なりの有無に基づいて、前記可動部と前記障害物との干渉を判定する、干渉判定装置。
4. 請求項１から請求項３までのいずれか一項に記載の干渉判定装置であって、
   前記分離軸決定部は、更に、前記直方体モデルの面法線ベクトルまたは辺ベクトルのうちのいずれか１のベクトルに平行な直線を、第４の分離軸として決定し、
   前記干渉判定部は、更に、前記第４の分離軸に前記円柱モデルと前記直方体モデルとを射影し、前記第４の分離軸に射影された前記円柱モデルの像と前記直方体モデルの像との重なりの有無に基づいて、前記可動部と前記障害物との干渉を判定する、干渉判定装置。
5. 請求項１から請求項４までのいずれか一項に記載の干渉判定装置であって、
   前記分離軸決定部は、更に、前記円柱モデルの上面または下面の法線ベクトルに平行な直線を第５の分離軸として決定し、
   前記干渉判定部は、更に、前記第５の分離軸に前記円柱モデルと前記直方体モデルとを射影し、前記第５の分離軸に射影された前記円柱モデルの像と前記直方体モデルの像との重なりの有無に基づいて、前記可動部と前記障害物との干渉を判定する、干渉判定装置。
6. 請求項１を引用する請求項２、を引用する請求項３、を引用する請求項４、を引用する請求項５に記載の干渉判定装置であって、
   前記分離軸決定部は、前記第５の分離軸、前記第４の分離軸、前記第１の分離軸、前記第２の分離軸、前記第３の分離軸、の順序で分離軸の決定を行い、
   前記干渉判定部は、前記可動部と前記障害物とが干渉していないと判定された分離軸以降の順序の分離軸を用いた干渉の判定を行わず、
   前記分離軸決定部は、前記可動部と前記障害物とが干渉していないと判定された分離軸以降の順序の分離軸の決定を行わない、干渉判定装置。
7. 可動部を有する産業用機械と障害物との干渉をチェックする干渉判定方法であって、
   前記可動部の少なくとも一部の形状と前記障害物の少なくとも一部の形状とのうち、一方の形状を直円柱によって表した円柱モデルのデータと、他方の形状を直方体によって表した直方体モデルのデータと、を記憶部に記憶させる記憶工程と、
   前記円柱モデルが表す形状の位置と、前記直方体モデルが表す形状の位置と、を取得する位置取得工程と、
   前記円柱モデルの上面または下面の法線ベクトルと、前記直方体モデルの面法線ベクトルまたは辺ベクトルのうちのいずれか１のベクトルとを含む第１の平面を求め、該第１の平面上における前記円柱モデルの側面の法線ベクトルに平行な直線を、第１の分離軸として決定する分離軸決定工程と、
   前記第１の平面に前記円柱モデルと前記直方体モデルとを射影し、更に、前記第１の平面上の前記第１の分離軸に、前記第１の平面に射影された前記円柱モデルの像と前記直方体モデルの像とを射影し、前記第１の分離軸に射影された前記円柱モデルの像と前記直方体モデルの像との重なりの有無に基づいて、前記可動部と前記障害物との干渉を判定する干渉判定工程と、
   を備える干渉判定方法。
8. 可動部を有する産業用機械と障害物との干渉を判定するためのコンピュータプログラムであって、
   前記可動部の少なくとも一部の形状と前記障害物の少なくとも一部の形状とのうち、一方の形状を直円柱によって表した円柱モデルのデータと、他方の形状を直方体によって表した直方体モデルのデータと、をコンピュータに記憶させる記憶機能と、
   前記円柱モデルが表す形状の位置と、前記直方体モデルが表す形状の位置と、を取得する位置取得機能と、
   前記円柱モデルの上面または下面の法線ベクトルと、前記直方体モデルの面法線ベクトルまたは辺ベクトルのうちのいずれか１のベクトルとを含む第１の平面を求め、該第１の平面上における前記円柱モデルの側面の法線ベクトルに平行な直線を、第１の分離軸として決定する分離軸決定機能と、
   前記第１の平面に前記円柱モデルと前記直方体モデルとを射影し、更に、前記第１の平面上の前記第１の分離軸に、前記第１の平面に射影された前記円柱モデルの像と前記直方体モデルの像とを射影し、前記第１の分離軸に射影された前記円柱モデルの像と前記直方体モデルの像との重なりの有無に基づいて、前記可動部と前記障害物との干渉を判定する干渉判定機能と、
   をコンピュータに実現させるコンピュータプログラム。
9. 請求項８に記載のコンピュータプログラムを記録したコンピュータ読取可能な記録媒体。

Images