JP2019025621A - 干渉判定方法、干渉判定システム及びコンピュータプログラム - Google Patents

Abstract

【課題】ロボットの動作経路と周辺物との干渉の有無を高速に演算することを可能とする干渉判定方法を提供する。【解決手段】第１の位置から第２の位置へと移動する動作経路上に、少なくとも１つの中間位置を設定し、各位置に相当する複数の姿勢のロボット近似体の少なくとも２つを組み合わせて構成される第１の組み合わせ近似体を複数生成し、さらに、少なくとも一つの着目部位について、これら第１の組み合わせ近似体の少なくとも２つを組み合わせて構成される第２の組み合わせ近似体を生成し、この第２の組み合わせ近似体と、前記周辺物近似体とが干渉するか判定する。【選択図】図１０

Description

本発明は、干渉判定方法、干渉判定システム及びコンピュータプログラムに関し、特にロボットアームの経路を生成する際の経路生成手法及び経路生成装置において、障害物の干渉範囲を高速に演算するための手法に関する。

産業用ロボットに所定の作業を行わせる際、作業を実行するロボットアームの動作経路を、プログラミングや、実機を実際に動作させる教示により決定する場合、多くの時間と労力を要する。

これに対し、動作開始点（始点）及び動作終了点（終点）を入力すると、始点と終点間の経路を自動的に生成するアルゴリズムを用いることができれば、産業用ロボットを教示する時間を大幅に短縮することが期待できる。

産業用ロボットのロボットアームの動作を制御する際に周辺物との干渉の有無をチェックする方法として、特許文献１がある。この文献には、始点と終点との間を結ぶロボットの経路について、ロボットがその経路を通過する離散時間毎に区分して、ロボットの占有領域を計算し、隣り合う離散時間同士の占有領域を組み合わせて複数の凸包を計算し、これら凸包と、周辺の障害物を多面体で近似した凸多面体とが共有領域を有するか否かを計算し、共有領域がある場合は干渉が生じている、とする干渉予測方法が開示されている。

このようなアルゴリズムを用いて経路を生成する際に、産業用ロボットが配置される周辺環境、たとえば、ベルトコンベア等の障害物を考慮した経路生成が可能になれば、より実際の環境に適合した経路が生成されることとなり好ましい。

特開２０１２−２２３８４５号公報

そして、このような経路の自動生成に求められる性能の１つとして、経路演算処理の高速化がある。たとえば、ベルトコンベア上を不規則に流れてくる複数の把持対象物をロボットアームでそれぞれ把持しなければならない場合は、把持対象物ごとに経路を演算しなければならないため、演算処理の高速化を図る必要がある。演算時間の長短は、ベルトコンベアの可動速度、ひいては、生産効率に直結するからである。

特許文献１に記載の干渉判定方法では、隣接する離散時間におけるロボットの占有領域を組み合わせた凸包の全てと、ロボット周辺の障害物を近似した凸多面体について共有領域の有無を判定しているため、干渉有無の判定時間が長いという課題があった。

そこで本発明は、動作経路に従って動作するロボットと周辺物との干渉の有無を高速に演算することを可能とする干渉判定方法、干渉判定システム及びコンピュータプログラムを提供することを目的とする。

本発明の一態様に係る干渉判定方法は、取得部、記憶部及び演算部を備える演算装置を用いて、複数の可動軸を有するロボットが第１の位置から第２の位置に移動する際に、ロボットの周辺に存在する周辺物と干渉する可能性を判定する方法である。演算部は、記憶部にロボットの形状を着目部位毎に多面体で近似したロボットデータを記憶させ、記憶部に周辺物の形状を近似した周辺物近似体のデータを記憶させる。さらに取得部は、第１の位置及び第２の位置を取得する。そして演算部は、第１の位置から第２の位置へと移動する動作経路上に、少なくとも１つの中間位置を設定し、ロボットデータに基づいて、第１の位置、第２の位置及び中間位置におけるロボットの形状を少なくとも一つの着目部位について多面体で近似した複数のロボット近似体を生成する。さらに、少なくとも一つの着目部位について、第１の位置、第２の位置及び中間位置に相当する複数のロボット近似体の少なくとも２つを組み合わせて構成される第１の組み合わせ近似体を複数生成し、これら第１の組み合わせ近似体の少なくとも２つを組み合わせて構成される第２の組み合わせ近似体を生成する。そして第２の組み合わせ近似体と周辺物近似体が干渉するか判定する。

この態様によれば、ロボットがとり得る姿勢に対応するロボット近似体の少なくとも２つを組み合わせて第１の組み合わせ近似体を生成し、この第１の組み合わせ近似体を複数組み合わせて構成される第２の組み合わせ近似体を用いて周辺物との干渉可能性を判定することができるので、ロボットがとり得る全ての姿勢に対して常に判定処理を行わずとも、干渉の可能性を判定することが可能になるので、ロボットの動作経路と周辺物との干渉の有無を高速に演算する干渉判定方法を提供することができる。

上記態様において、第２の組み合わせ近似体と周辺物近似体が干渉すると判定された場合は、第２の組み合わせ近似体に含まれる第１の組み合わせ近似体を用いて、周辺物近似体と干渉するか判定を実行するようにしてもよい。

この態様によれば、複数の第１の組み合わせ近似体を用いて干渉の有無を判定するから、判定精度を向上することができる。すなわち、干渉する可能性のある第２の組み合わせ近似体に含まれる、第１の組み合わせ近似体のみと比較すればよいので、少ない演算量でより精度の高い干渉判定ができる。

上記態様において、複数の第１の組み合わせ近似体は、それぞれ、動作経路上で隣接する２つのロボット近似体を組み合わせて構成してもよい。

この態様によれば、隣接する２つのロボット近似体の間で干渉位置を特定することが可能となるため、近似体を用いて干渉有無を特定するための処理と比較して、相対的に処理負荷の大きい、詳細な干渉位置を特定するための処理を実行する範囲を限定することができる。したがって、詳細な干渉位置を特定するための処理を高速に実施することが可能になる。

上記態様において、着目部位について、複数のロボット近似体のうち、隣り合う任意のロボット近似体の間の領域は、少なくとも一つの第１の組み合わせ近似体により包含されるように構成してもよい。

この態様によれば、隣り合う任意のロボット近似体の領域は、少なくとも一つの第１の組み合わせ近似体により包含されているから、着目部位の動作経路が間隙なく第１の組み合わせ近似体に包含されることとなり、判定精度を向上させることが可能になる。

上記態様において、第２の組み合わせ近似体は、着目部位について、生成された全てのロボット近似体を組み合わせて構成されるようにしてもよい。

この態様によれば、第２の組み合わせ近似体を用いて干渉判定をした結果、干渉可能性がないと判定された場合は、その内部に包含される領域についてさらなる干渉判定処理が必要なくなるから、判定処理の高速化を図ることが可能になる。

上記態様において、複数の第１の組み合わせ近似体と、周辺物近似体とが干渉するかそれぞれ判定するステップにおいて、第１の組み合わせ近似体と、周辺物近似体とが干渉すると判定された場合は、その第１の組み合わせ近似体に含まれる隣り合う二つのロボット近似体にそれぞれ相当する着目部位の二つの姿勢の間で、この着目部位が周辺物近似体と衝突した点を抽出し、その際のロボットの位置及び姿勢のパラメータを生成するステップを更に備えてもよい。

この態様によれば、第１の組み合わせ近似体と、周辺物近似体とが干渉すると判定された場合は、その近似体に含まれる領域内で、衝突点を抽出し、その際のロボットの位置及び姿勢のパラメータを生成するようにしているため、処理時間を要する衝突点の抽出処理を限定的な領域内でのみ実施することが可能となり、判定処理を最適化し、演算の高速性の向上を図ることができる。

上記態様において、演算部が、記憶部に、周辺物の一部を構成する第１の着目部位の形状を近似する近似体であって、周辺物近似体より小さい第１の周辺物近似体のデータを記憶させるステップをさらに備え、第１又は第２の組み合わせ近似体が、周辺物近似体と干渉すると判定された場合は、その第１又は第２の組み合わせ近似体と、前記第１の周辺物近似体データとが干渉するか判定するステップを更に備えるようにしてもよい。

この態様によれば、周辺物の一部を構成する着目部位について第１又は第２の組み合わせ近似体との干渉判定をすることが可能になるから、干渉可能性のある特定の部位に対する干渉判定を行うことができ、周辺物の干渉可能性のある部位をより正確に特定することが可能になる。

上記態様において、周辺物近似体、第１の組み合わせ近似体及び第２の組み合わせ近似体は、いずれも第１の直方体であって、互いに直交する３つの軸により構成される第１の座標系の各軸に平行な線分により構成される第１の直方体であり、第１又は第２の組み合わせ近似体と周辺物近似体が干渉すると判定された場合は、その第１又は第２の組み合わせ近似体と、周辺物を近似する第２の直方体であって、前記周辺物の形状に応じて、前記第１の座標系を所定の角度で回転させた第２の座標系の各軸に平行な線分により構成され、前記第１の直方体より小さい第２の直方体の近似体と、が干渉するか判定するようにしてもよい。

この態様によれば、まずは周辺物を近似する第１の直方体で干渉判定を実施し、干渉した場合に、さらに周辺物を近似する第２の直方体で干渉判定を実施するようにしたから、第１の直方体を用いた場合と比較して、相対的に時間を要する第２の直方体を用いた判定処理を必要な場合にのみ実施するように構成することが可能となり、干渉が生じ得る範囲の絞込み精度を向上させつつ判定処理の効率化を促進することができる。

また、本発明の他の態様に係る干渉判定システムは、複数の可動軸を有するロボットが第１の位置から第２の位置に移動する際に、ロボットの周辺に存在する周辺物と干渉する可能性を判定する。この干渉判定システムは、ロボットの形状を複数の着目部位毎に多面体で近似したロボットデータ及び周辺物の形状を近似した周辺物近似体のデータを記憶するための記憶部を備え、さらに、第１の位置及び第２の位置を取得するための取得部を備える。また、演算部は、第１の位置から第２の位置へと移動する動作経路上に、少なくとも１つの中間位置を設定し、ロボットデータに基づいて、第１の位置、第２の位置及び中間位置におけるロボットの形状を、少なくとも一つの着目部位について多面体で近似した複数のロボット近似体を生成し、少なくとも一つの着目部位について第１の位置、第２の位置及び中間位置に相当する複数のロボット近似体の少なくとも２つを組み合わせて構成される第１の組み合わせ近似体を複数生成し、これら第１の組み合わせ近似体の少なくとも２つを組み合わせて構成される第２の組み合わせ近似体を生成し、この第２の組み合わせ近似体と、周辺物近似体とが干渉するように構成される。

この態様によれば、ロボット近似体の少なくとも２つを組み合わせて構成される第１の組み合わせ近似体と、この第１の組み合わせ近似体を複数組み合わせて構成される第２の組み合わせ近似体を用いて周辺物との干渉可能性を判定することができるので、ロボットの動作経路と周辺物との干渉の有無を高速に演算する干渉判定システムを提供することができる。

上記態様において、第２の組み合わせ近似体と、周辺物近似体とが干渉すると判定された場合は、第２の組み合わせ近似体に含まれる第１の組み合わせ近似体を用いて、周辺物近似体と干渉するか判定するように構成してもよい。

この態様によれば、複数の第１の組み合わせ近似体を用いて干渉判定するから、判定精度を向上することが可能になる。

上記態様において、複数の第１の組み合わせ近似体は、それぞれ、動作経路上で隣接する２つのロボット近似体を組み合わせて構成してもよい。

この態様によれば、隣接する２つのロボット近似体の間で干渉位置を特定することが可能となるため、干渉位置を特定するための処理を高速に実施することが可能になる。

上記態様において、着目部位について、複数のロボット近似体のうち、隣り合う任意のロボット近似体の間の領域は、少なくとも一つの第１の組み合わせ近似体により包含されるように構成してもよい。

この態様によれば、隣り合う任意のロボット近似体の領域は、少なくとも一つの第１の組み合わせ近似体により包含されているから、着目部位の動作経路が間隙なく第１の組み合わせ近似体に包含されることとなり、判定精度を向上させる、特に、干渉が生じ得るにも関わらずこれを非干渉と判定するような誤判定を、より確実に防止することが可能になる。換言すれば、着目部位について、初期姿勢（位置）から目標姿勢（位置）に至るまでの複数の姿勢について生成されたロボット近似体が、互いに共有する領域を有することになる。したがって、着目部位の動作経路を、第１の組合せ近似体によって間隙なく包含させることができるので、判定精度を向上させる（誤判定等を防止する）ことが可能になる。

上記態様において、第２の組み合わせ近似体は、着目部位について生成された全てのロボット近似体を組み合わせて構成されるようにしてもよい。

この態様によれば、第２の組み合わせ近似体を用いて干渉判定をした結果、干渉可能性がないと判定された場合は、その内部に包含される領域についてさらなる干渉判定処理が必要なくなるから、判定処理の高速化を図ることが可能になる。

上記態様において、複数の第１の組み合わせ近似体と、周辺物近似体とが干渉するかそれぞれ判定するステップにおいて、第１の組み合わせ近似体と、周辺物近似体とが干渉すると判定された場合は、その第１の組み合わせ近似体に含まれる隣り合う二つのロボット近似体にそれぞれ相当する着目部位の二つの姿勢の間で、この着目部位が周辺物近似体と衝突した点を抽出し、その際のロボットの位置及び姿勢のパラメータを生成するステップを更に備えてもよい。

この態様によれば、第１の組み合わせ近似体と、周辺物近似体とが干渉すると判定された場合は、その近似体に含まれる領域内で、衝突点を抽出し、その際のロボットの位置及び姿勢のパラメータを生成するようにしているため、処理時間を要する衝突点の抽出処理を限定的な領域内でのみ実施することが可能となり、判定処理を最適化し、演算の高速性の向上を図ることができる。

上記態様において、演算部は、記憶部に、周辺物の一部を構成する第１の着目部位の形状を近似する近似体であって、周辺物近似体より小さい第１の周辺物近似体のデータを記憶させるステップをさらに備え、第１又は第２の組み合わせ近似体が、周辺物近似体と干渉すると判定された場合は、その第１又は第２の組み合わせ近似体と、前記第１の周辺物近似体データとが干渉するか判定するステップを更に備えるようにしてもよい。

この態様によれば、周辺物の一部を構成する着目部位について第１又は第２の組み合わせ近似体との干渉判定をすることが可能になるから、干渉可能性のある特定の部位に対する干渉判定を行うことができ、周辺物の干渉可能性のある部位をより正確に特定することが可能になる。

上記態様において、周辺物近似体、第１の組み合わせ近似体及び第２の組み合わせ近似体は、いずれも第１の座標系を構成し、互いに直行する３つの軸のそれぞれに平行な線分により構成される第１の直方体であり、第１又は第２の組み合わせ近似体と周辺物近似体が干渉すると判定された場合は、その第１又は第２の組み合わせ近似体と、周辺物を近似する第２の直方体であって、前記周辺物の形状に応じて、前記第１の座標系を所定の角度で回転させた第２の座標系の各軸に平行な線分により構成され、前記第１の直方体より小さい第２の直方体の近似体とが干渉するか判定するようにしてもよい。

この態様によれば、まずは周辺物を近似する第１の直方体で干渉判定を実施し、干渉した場合に、さらに周辺物を近似する第２の直方体で干渉判定を実施するようにしたから、第１の直方体を用いた場合と比較して、相対的に時間を要する第２の直方体を用いた判定処理を必要な場合にのみ実施するように構成することが可能となり、干渉可能性範囲の絞込み精度を向上させつつ判定処理の効率化を促進することができる。

また、本発明の一態様に係るコンピュータプログラムは、複数の可動軸を有するロボットが第１の位置から第２の位置に移動する際に、前記ロボットの周辺に存在する周辺物と干渉する可能性を判定するためのコンピュータプログラムであって、コンピュータに、ロボットの形状を、複数の着目部位毎に多面体で近似したロボットデータを記憶させるステップと、周辺物の形状を近似した周辺物近似体のデータを記憶させるステップと、第１の位置及び前記第２の位置を取得するステップと、第１の位置から第２の位置へと移動する動作経路上に、少なくとも１つの中間位置を設定するステップと、ロボットデータに基づいて、第１の位置、第２の位置及び中間位置におけるロボットの形状を、少なくとも一つの着目部位について多面体で近似した複数のロボット近似体を生成するステップと、少なくとも一つの着目部位について、第１の位置、第２の位置及び中間位置に相当する複数のロボット近似体の少なくとも２つを組み合わせて構成される第１の組み合わせ近似体を複数生成し、さらに、これら第１の組み合わせ近似体の少なくとも２つを組み合わせて構成される第２の組み合わせ近似体を生成するステップと、この第２の組み合わせ近似体と、周辺物近似体とが干渉するか判定するステップと、を実行させるように構成される。

この態様によれば、ロボット近似体の少なくとも２つを組み合わせて構成される第１の組み合わせ近似体と、この第１の組み合わせ近似体を複数組み合わせて構成される第２の組み合わせ近似体を用いて周辺物との干渉可能性を判定することができるので、ロボットがとり得る全ての姿勢に対して常に判定処理を行わずとも、干渉の可能性を判定することが可能になる。従って、ロボットの動作経路と周辺物との干渉の有無を高速に演算するコンピュータプログラムを提供することができる。

また、本発明の一態様に係る干渉判定方法は、取得部、記憶部及び演算部を備える演算装置を用いて、複数の可動軸を有するロボットが第１の位置から第２の位置に移動する際に、ロボットの周辺に存在する周辺物と干渉する可能性を判定する方法である。そして、演算部は、記憶部に、ロボットの形状を、複数の着目部位ごとに多面体で近似したロボットデータを記憶させ、記憶部に、周辺物の形状を近似した周辺物近似体のデータを記憶させ、取得部を用いて、第１の位置及び第２の位置を取得し、第１の位置から第２の位置へと移動する動作経路上に、少なくとも１つの中間位置を設定し、ロボットデータに基づいて、第１の位置、第２の位置及び中間位置におけるロボットの形状を複数の着目部位毎に多面体で近似した複数のロボット近似体を生成し、生成された全てのロボット近似体を組み合わせて構成される総組み合わせ近似体を生成し、この総組み合わせ近似体と、周辺物近似体とが干渉するか判定し、干渉すると判定された場合は、複数の着目部位毎に、第１の位置、第２の位置及び中間位置に相当する複数のロボット近似体の少なくとも２つを組み合わせて構成される複数の部分組み合わせ近似体を生成し、複数の部分組み合わせ近似体と、周辺物近似体とが干渉するかそれぞれ判定する。

この態様によれば、複数のロボット近似体から構成される総組み合わせ近似体を用いて周辺物と干渉するか判定し、干渉すると判定された場合は、さらに部分組み合わせ近似体を用いて周辺物と干渉判定する一方で、干渉しないと判定された場合はその領域内の干渉判定処理を省略することが可能になるため、より高速な干渉判定処理を実現することが可能になる。

また、本発明の一態様に係る干渉判定システムは、複数の可動軸を有するロボットが第１の位置から第２の位置に移動する際に、ロボットの周辺に存在する周辺物と干渉する可能性を判定するための干渉判定システムである。そして、ロボットの形状を複数の着目部位毎に多面体で近似したロボットデータ及び周辺物の形状を近似した周辺物近似体のデータを記憶するための記憶部と、第１の位置及び第２の位置を取得するための取得部と、第１の位置から第２の位置へと移動する動作経路上に、少なくとも１つの中間位置を設定し、ロボットデータに基づいて、第１の位置、第２の位置及び中間位置におけるロボットの形状を複数の着目部位毎に多面体で近似した複数のロボット近似体を生成し、全てのロボット近似体を組み合わせて構成される総組み合わせ近似体を生成し、この総組み合わせ近似体と、周辺物近似体とが干渉するか判定し、干渉すると判定した場合は、複数の着目部位毎に、第１の位置、第２の位置及び中間位置に相当する複数のロボット近似体の少なくとも２つを組み合わせて構成される複数の部分組み合わせ近似体を生成し、複数の部分組み合わせ近似体と、周辺物近似体とが干渉するか判定するように構成される演算部と、を備えるように構成される。

この態様によれば、複数のロボット近似体から構成される総組み合わせ近似体を用いて周辺物と干渉するか判定し、干渉すると判定された場合は、さらに部分組み合わせ近似体を用いて周辺物と干渉判定する一方で、干渉しないと判定された場合はその領域内の干渉判定処理を省略することが可能になるため、より高速な干渉判定処理を実現することが可能になる。

また、本発明の一態様に係るコンピュータプログラムは、複数の可動軸を有するロボットが第１の位置から第２の位置に移動する際に、ロボットの周辺に存在する周辺物と干渉する可能性を判定するためのコンピュータプログラムであって、コンピュータに、ロボットの形状を、複数の着目部位ごとに多面体で近似したロボットデータを記憶させるステップと、周辺物の形状を近似した周辺物近似体のデータを記憶させるステップと、第１の位置及び第２の位置を取得するステップと、第１の位置から第２の位置へと移動する動作経路上に、少なくとも１つの中間位置を設定するステップと、ロボットデータに基づいて、第１の位置、第２の位置及び中間位置におけるロボットの形状を複数の着目部位毎に多面体で近似した複数のロボット近似体を生成するステップと、ステップで生成された全てのロボット近似体を組み合わせて構成される総組み合わせ近似体を生成するステップと、この総組み合わせ近似体と、周辺物近似体とが干渉するか判定するステップと、このステップにおいて干渉すると判定された場合は、複数の着目部位毎に、第１の位置、第２の位置及び中間位置に相当する複数のロボット近似体の少なくとも２つを組み合わせて構成される複数の部分組み合わせ近似体を生成するステップと、複数の部分組み合わせ近似体と、周辺物近似体とが干渉するかそれぞれ判定するステップと、を実行させるように構成される。

この態様によれば、複数のロボット近似体から構成される総組み合わせ近似体を用いて周辺物と干渉するか判定し、干渉すると判定された場合は、さらに部分組み合わせ近似体を用いて周辺物と干渉判定する一方で、干渉しないと判定された場合はその領域内の干渉判定処理を省略することが可能になるため、より高速な干渉判定処理を実現することが可能になる。

本発明によれば、ロボットの動作経路と周辺物との干渉の有無の高速演算を可能とする手法を提供することができる。

第１実施形態に係る干渉判定システム１０のハードウェアブロック図。 第１実施形態に係る干渉判定方法の事前処理を示すフローチャート。 オブジェクトを近似するバウンディング・ボリュームの一例。 オブジェクトの近似体の判定精度及び生成コスト等を比較した図。 オブジェクトのメッシュモデルに基づき生成された近似体の階層構造を示した図。 第１の直方体の生成方法を示した図。 障害物の第１直方体（オブジェクトレベル及び空間レベル）を示した図。 第１実施形態に係るロボットアーム１６を示した図。 第１実施形態に係るロボットアーム１６を複数の第２の直方体で近似した図。 第１実施形態に係る干渉判定方法のフローチャート。 第１実施形態に係るロボットアーム１６の近似体の初期及び目標姿勢を示した図。 第１実施形態における中間姿勢数の決定方法を示した図。 第１実施形態における中間姿勢数の決定方法の変形例を示した図。 第１実施形態に係るロボットアーム１６の各リンクＬの初期姿勢、中間姿勢、目標姿勢を第２の直方体の近似体で示した図。 第１実施形態に係るロボットアーム１６のリンクＬ５の初期姿勢、中間姿勢、目標姿勢を第２の直方体の近似体で示した図。 図１５の図に基づき第２の直方体を包含する第１の直方体を示した図。 第１の直方体の干渉判定方法の説明図。 第１実施形態に係るロボットアーム１６のリンクＬ５と第１の直方体の干渉を示した図。 凸包の生成方法の説明図。 凸包に基づく誤判定の説明図。 第１の直方体に基づく誤判定の説明図。 第２実施形態にかかる干渉判定方法のフローチャート。 第２実施形態にかかる干渉判定に用いられるロボットのツリー構造。

以下、添付図面を参照して、本発明の好適な実施形態について説明する（なお、各図において、同一の符号を付したものは、同一又は同様の構成を有する）。

［第１実施形態］
図１は、本発明の第１実施形態に係る干渉判定システム１０のハードウェアブロック図である。この図に示されるように、この干渉判定システム１０は、情報を入力するための入力部１２と、情報を処理するための演算装置１４と、演算結果に基づいて制御されるロボットアーム１６ならびに結果を表示するためのモニタ１８を備える出力部２０を有する。なお、干渉判定システム１０は、演算装置１４で実行された演算結果に基づいて、制御対象であるロボットアーム１６の干渉の可能性を判定するように構成されればよい。したがって、干渉判定の演算結果を利用する出力部２０を必ずしも備えなくてもよい。

入力部１２は、演算装置１４に情報を入力するためのインターフェースとして機能し、入力データであるロボットアームの初期姿勢、目標姿勢、ロボットに関する情報ならびに障害物の情報等を入力するための図示しないキーボード等を備えるが、これに限られるものではなく、演算処理の対象となる情報を入力するためのいかなる構成を採用することができる。たとえば、ＤＶＤやＣＤ等の記憶媒体に演算処理となる情報を記憶させ、入力部としてこれら情報を読み出すためのドライブを設けてもよい。また、ネットワーク等を介して演算装置１４に情報を入力する構成を採用することも可能である。ここで、ロボットに関する情報は、ロボットの３次元形状を仮想空間上に表現するためのロボットモデルを含むことが好ましく、障害物の情報は、障害物（周辺物）の３次元形状を仮想空間上に表現するための障害物モデルを含むことが好ましい。本実施形態では、入力部１２を介して入力されるロボットモデル及び障害物モデルは、メッシュにより表現されるメッシュモデルの例を示すが、モデルの表現形式は、メッシュモデルには特に限定されない。なお、ロボットモデルは、後述するロボットアームの３次元形状を仮想空間上に表現するためのモデルと、ロボットアームの先端に設けられるエンドエフェクタの３次元形状を仮想空間上に表現するためのモデルと、を含んでいてよい。

ここで、ロボットアーム１６の姿勢とは、ロボットアーム１６が有する複数の駆動軸が、それぞれ所定の角度であるときの、ロボットアーム１６の体勢を指す。ロボットアーム１６の姿勢は、ロボットアーム１６が有する複数の駆動軸の角度を定めることにより一意に定まる。また、ロボットアーム１６が有する複数の駆動軸の角度を定めることで、順運動学計算により、ロボットアーム１６の先端の位置を算出することができる。さらに、ロボットアーム１６の先端の位置と、ロボットアームの先端に取りつけられるエンドエフェクタの形状データと、に基づいて、エンドエフェクタの所定の位置を算出することができる。一方、ロボットアーム１６の先端の位置が与えられた場合には、逆運動学計算により、ロボットアーム１６の先端を当該位置にするために必要な、複数の駆動軸のそれぞれの角度、すなわちロボットアーム１６の姿勢を算出することができる。このように、ロボットアーム１６の先端の位置と姿勢とは相互に関連し、少なくとも、位置と姿勢とのいずれか一方に関する情報が与えられれば、運動学計算に基づいて、他方の情報を算出することができる。

したがって、ロボットアームの初期姿勢に関する情報は、ロボットアームを初期姿勢から目標姿勢に移動させるときの初期位置に関する情報であってもよく、また、初期位置に関する情報を含んでいてよい。ここで、初期位置とは、ロボットアームの動作経路を演算するときの始点である第１の位置として用いることができる。初期位置（第１の位置）は、ロボットアームの先端、または、ロボットアームの先端に取りつけられるエンドエフェクタの所定の位置、を示す座標により表現することができる。また、ロボットアームの目標姿勢に関する情報は、ロボットアームを初期姿勢から目標姿勢に移動させるときの目標位置に関する情報であってもよく、また、目標位置に関する情報を含んでいてよい。ここで、目標位置とは、ロボットアームの動作経路を演算するときの終点である第２の位置として用いることができる。目標位置（第２の位置）は、ロボットアームの先端、または、ロボットアームの先端に取りつけられるエンドエフェクタの所定の位置、を示す座標であることが好ましい。演算装置１４は、例えば、後述する記憶装置２６に格納された逆運動学の演算プログラムを用いて、初期位置に基づいて初期姿勢を算出し、目標位置に基づいて目標姿勢を算出することができる。

なお、演算装置１４は、ロボットアームの初期位置（初期姿勢）に関する情報、および、目標位置（目標姿勢）に関する情報の少なくともいずれかを、ロボットアームの周辺環境の情報に基づいて、算出してもよい。例えば、演算装置１４は、入力部１２を用いて、ロボットアームの周辺環境を撮像する画像センサが計測した画像情報を取得し、取得されたセンサ情報（画像情報）に基づく演算により、初期位置（初期姿勢）に関する情報、および、目標位置（目標姿勢）に関する情報を算出してもよい。また、演算装置１４は、ロボットアームの周辺環境の情報に基づいて算出された初期位置（初期姿勢）に関する情報、および、目標位置（目標姿勢）に関する情報の少なくともいずれかを、入力部１２を介して取得してもよい。

演算装置１４は、典型的にはパーソナル・コンピュータ（ＰＣ）２２で実現することが可能であり、本実施の形態に示される種々の演算処理を実施するための演算部として機能するハードウェアプロセッサ（例えば、ＣＰＵ２４）と、経路演算のためのコンピュータプログラムならびに本実施形態に開示される衝突（干渉）判定の各処理を実行するためのコンピュータプログラム等を不揮発的に記憶するためのＨＤＤ、ＳＳＤ、フラッシュメモリ等の記憶装置２６と、一時記憶するためのＳＲＡＭ、ＤＲＡＭ等から構成されるメモリ２８を備える。これらメモリ２８と記憶装置２６が、処理対象となる種々の情報が記憶される記憶部として機能する。また、演算部は、ハードウェアプロセッサが所定の演算処理のためのプログラムを実行することで、その機能が実現される。なお、演算部として、特定の処理を実施するためのグラフィック用のＧＰＵ（Graphic Processing Unit）、ＦＰＧＡ（Field Programmable Gate Array）、ＤＳＰ（Digital Signal Processor）、ＡＳＩＣ（Application Specific Integrated Circuit）をＣＰＵ２４と共に、あるいは、単独で用いてもよい。また、演算部及び記憶部は、一体的に構成されなくてもよい。

出力部２０は、制御対象であるロボットアーム１６と、判定結果を表示するためのモニタ１８（表示装置）を備える。ロボットアーム１６は、演算装置１４と図示しない外部インターフェースで接続されており、演算装置１４から出力される制御情報に基づいて制御可能に構成される。制御対象は、任意の経路に従って動作をする際に周囲の障害物と干渉を判定する必要があるロボットであればよく、ロボットアーム１６に限られない。

図２は、判定処理に必要なデータを生成するための事前処理のフローチャートである。事前処理は、障害物を仮想空間上に表現するために近似したモデル、すなわち、近似体を生成する処理、及び、ロボットを仮想空間上に表現するとともに干渉判定を行うために近似したモデル、すなわち、ロボットデータを生成する処理を含む。この事前処理は、障害物や、制御対象物であるロボットが変更されるたびに実施される処理である。なお、これら各処理ならびに各実施形態で実行される各処理は、記憶装置２６に記憶されるコンピュータプログラムに従って、ＣＰＵ２４により実行される。なお、本実施形態においては、演算装置１４は、ステップＳ１１において、メッシュにより表現された障害物モデルを、境界球や直方体、凸包等の、後述する所定の方法、または、その他の公知の手法を用いて近似した障害物モデル（周辺物近似体）を生成する。また、演算装置１４は、ステップＳ１２において、メッシュにより表現されたロボットモデルを、境界球や直方体、凸包等の、後述する所定の方法、また、その他の公知の手法を用いて近似したロボットモデル（ロボットデータ）を生成する。メッシュ等により表現されたロボットモデルおよび障害物のモデルは、ロボットおよび障害物のそれぞれについて設定される着目部位毎に近似される。着目部位とは、障害物またはロボットの全部または一部を含む部位であって、障害物の形状や、ロボットの構造に基づいて、後述のように適宜設定される。

まず、ステップＳ１１に示されるように、入力部１２より入力され、メッシュにより表現された障害物モデルに基づいて、境界球や直方体、凸包等の後述する所定の方法、または、その他の公知の手法を用いて近似した障害物モデルを生成する。本実施の形態では、メッシュにより表現された障害物モデルに基づいて、ＢＶＨ （Bounding Volume Hierarchy）を構成する障害物モデルが生成される。ここで障害物は、１つに限られず、複数でもよい。また、障害物は、静止物に限られず移動するものでもよい。障害物が移動物体である場合には、後述する干渉判定の演算処理において、移動物体の移動経路を考慮するとともに、ロボットの動作経路と、移動物体の移動経路との時系列を加味した干渉判定を行うようにする。ここで、ＢＶ（Bounding Volume）とは、近似する対象であるオブジェクトを包含するように近似する単純形状のことをいい、ＢＶＨとは、ＢＶを構成要素とする階層構造のことをいう。以下、ＢＶならびにＢＶＨの生成方法を、図面を用いて説明する。なお、本実施の形態では、オブジェクトを複数の構成要素（部分）に分けて近似し、各構成要素（部分）を階層構造にする手法としてＢＶＨを用いた例について説明するが、階層構造にする手法は、ＢＶＨに限定されない。すなわち、入力部１２より入力された、障害物モデルに基づいて、障害物モデルを着目部位毎に分離し、着目部位毎に分離した障害物モデルを階層的に整理することにより、木構造化（階層構造化）された障害物モデルが生成されればよい。障害物モデルを着目部位毎に分離して階層的に整理する方法としては、例えば、障害物モデルを、所定の座標軸の一つに垂直な複数の平面によって分割し、分割された障害物モデルを階層構造にする方法を使用することができる。

図３は、ＢＶの一例である。たとえば図３（ａ）は、凸型が傾いた形状のオブジェクトに対して、それを包含する球（Bounding Sphereまたは境界球）のＢＶを示している。この境界球は、球の位置ならびに半径によって特定することができる。例えば、本実施例では、境界球同志の干渉判定に要する時間は、１ナノ秒のオーダーという短時間で実行可能である。なお、ＢＶは、包含されるオブジェクトと接するものに限られず、オブジェクトを包含するように所定のマージンをもってオブジェクトより大きく生成することも可能である。

図３（ｂ）は、同じオブジェクトを対象とし、境界球とは異なるＢＶにより近似した例について説明する概念図である。図３（ｂ）に示すＢＶは、所定の基準座標系（例えば、水平方向をＸ軸とＹ軸、鉛直方向をＺ軸、として定義した、互いに直交するＸＹＺ軸を有する絶対座標系）の各軸に平行な線分により構成され、近似する対象となるオブジェクトを包含する第１の直方体のＢＶである。換言すると、第１の直方体（または「第１直方体」）とは、基本となる座標軸（第１の座標系の各軸）に平行な線分で構成される直方体からなるＢＶのことをいい、三次元の直交座標系の場合は、座標軸であるＸ軸、これと直交するＹ軸及びこれらと直交するＺ軸に平行であって、オブジェクトを包含する直方体となる。第１の直方体は、各軸の位置および各辺の長さにより特定可能である。また、第１の直方体は、基準座標系の各軸の最小の座標と最大の座標によって特定可能であり、例えば、本実施例では、第１の直方体同士の干渉判定に要する時間は、１ナノ秒のオーダーである。

図３（ｃ）は、同じオブジェクトを対象とし、境界球、および、第１の直方体とは異なるＢＶにより近似した例について説明する概念図である。図３（ｃ）に示すＢＶは、所定の基準座標系を、オブジェクトの形状等に応じて、所定の角度で回転させた回転座標系（第２の座標系）の各軸に平行な線分により構成され、近似する対象となるオブジェクトを包含する第２の直方体のＢＶである。換言すると、第２の直方体（または「第２直方体」）は、基本となる座標系（第１の座標系）の座標軸から傾いた直方体からなるＢＶであり、基準となる座標系が三次元の直交座標系の場合は、座標軸であるＸ軸、これと直交するＹ軸及びこれらと直交するＺ軸から傾いており、オブジェクトを包含する直方体から構成される。第２の直方体は、各軸の位置および各辺の長さに加えて、各軸に対する傾きにより特定される。例えば、本実施例では、干渉判定に要する時間は、１００ナノ秒のオーダーであり、第１の直方体の場合より多くの時間を要する。なお、第２の直方体は、第１の直方体と比較して、ＢＶ内のオブジェクトが存在しない領域の面積、または、体積が小さいため、判定精度が向上する場合がある。第２の直方体は、近似対象のオブジェクトの形状等に応じて傾斜させた座標系の各軸に平行な線分により構成され、第１の直方体と比較して、オブジェクトの形状により近い形状となる場合が多いためである。

図３（ｄ）は、同じオブジェクトを対象とし、凸包により近似した例を示している。凸包とは、その座標系に存在する複数の点を包含する最小の凸多角形のことをいう。所定の凸包を特定するためには、第１の直方体や第２の直方体より極めて多くのパラメータが必要であるため、干渉判定に要する時間は、点の数に応じて指数関数的に増加する。凸包については、後に詳述する。本実施形態において、オブジェクトの近似は、第１の直方体だけに限られず、上述した境界球（Bounding Sphere）、第２の直方体、凸包またはその他の公知の方法を用いることができる。また、オブジェクトの近似や干渉判定の際に用いられる座標系も、二次元もしくは三次元の直交座標系に限られず、極座標系や、斜交座標系を用いることもできる。

図４は、これらオブジェクトの近似体を、近似体の生成コスト（生成に要する時間）ならびに衝突（干渉）判定コスト（判定に要する時間）及び判定精度の観点から比較した図である。この図に示されるように、少ないパラメータにより特定することができる境界球ならびに第１の直方体は、生成コストならびに判定コストともに小さい。ただし、近似体とオブジェクトの差分となる領域が大きいため判定精度が悪く、実際のオブジェクトは衝突しないにもかかわらず、衝突すると誤判定してしまう可能性が高くなるケースがある。一方で、凸包は、オブジェクトの境界から抽出される複数の点によりオブジェクトを精度良く近似することができるため、判定精度が第１の直方体等より高くなる場合がある一方で、近似データの生成コストや衝突判定コストが、第１の直方体等より飛躍的に大きくなるというデメリットがある。第２の直方体は、第１の直方体等と凸包体の中間的な位置づけであり、近似体の生成コスト及び衝突判定コストは、第１の直方体や境界球の場合より大きいが、凸包よりは小さい。一方で判定精度については、第１の直方体等より優れているが、凸包に劣る。

さらに、オブジェクトの判定精度は、オブジェクトの近似方法のみにより特定されるものではなく、近似しようとするオブジェクトの形状にも影響を受ける。すなわち、図４に示されるとおり、オブジェクトが斜めに傾いている長方形の場合は、それを包含するように近似する境界球（または境界円）よりも、第１の直方体の場合の方が、判定精度は向上する。これは、第１の直方体の方が、オブジェクトが存在しない領域の面積が小さく精度良く近似できるためである。また、同じ第１の直方体でも、楕円よりも円のほうが、ＢＶ内のオブジェクトが存在しない領域の面積、または、体積が小さいため、判定精度が向上する場合がある。

図５は、近似体を階層構造にする例を説明するための図である。本実施例では、相対的に下位層の近似体を包含するように相対的に上位層の近似体を生成することで、近似体を階層構造にする。より具体的には、図５は、階層構造の一つとしての木構造を作る手法としてＢＶＨ（Bounding Volume Hierarchy）を採用した例を示している。一例として、テーブルと、そのテーブル上のビンからなるオブジェクトのメッシュモデルを第１の直方体で近似する場合について説明する。図５（ａ）は、このオブジェクトの全体を近似した第１の直方体１１を示している。図５（ｂ）は、このオブジェクトを、上半分の着目部位と、下半分の着目部位に分離し、上半分の着目部位を包含する第１の直方体２１からなる近似体と、下半分の着目部位を包含する第１の直方体２２からなる近似体を示している。図５（ｃ）は、第１の直方体２１を、その上半分の着目部位を包含する第１の直方体３１と、下半分の着目部位を包含する第１の直方体３２で近似するとともに、第１の直方体２２を、上側と下側の着目部位に分離し、それぞれ第１の直方体３３と ４つの第１の直方体３４−１〜３４−４で近似した例を示す。図５（ｄ）は、さらに図５（ｃ）の着目部位を細分化し、合計１６個の第１の直方体の近似体（第１の直方体４１〜４６、４７−１〜４７−４、４８−１〜４８−４及び４９−１〜４９−２）で近似した例である。なお、着目部位は、任意に設定することができる。例えば、周辺物（障害物）を所定の座標軸の一つに垂直な複数の平面によって分割した部位のそれぞれを、着目部位とすることができる。また、例えば、周辺物（障害物）の着目部位は、当該周辺物が占有する体積に応じて設定することができる。また、周辺物の形状に応じて設定することができる。例えば、周辺物が、胴部と、胴部から突出した突出形状とを、有する場合には、当該胴部と、突出形状とを、それぞれ着目部位とすることができる。なお、複数の可動軸を有するロボットアームに対して着目部位を適用する場合には、当該可動軸に対応して設けられるリンク形状が着目部位として設定されることが好ましい。

このようにして、第１の直方体１１を階層構造の頂点とし、その一つ下の階層を第１の直方体２１及び第１の直方体２２とし、さらに第１の直方体２１の下の階層を第１の直方体３１と第１の直方体３２、第１の直方体２２の下の階層を第１の直方体３３と４つの第１の直方体３４−１〜３４−４とする階層構造を形成し、さらに最下層として１６個の第１の直方体４１〜４９−２から構成される、合計４層の第１の直方体によって構成される木構造（ＢＶＨ）が形成される。

図６は、第１の直方体の生成方法を、二次元座標系を用いて示している。図６（ａ）は、単一のオブジェクトを包含する第１の直方体（または「第１直方体」）を示しており、図６（ｂ）は、第２の直方体（または「第２直方体」）である第２の直方体１及び第２の直方体２を組み合わせて、これを包含するような第１の直方体を生成する場合を示している。図６（ａ）に示されるように、オブジェクトを構成する各頂点の各座標軸の値から、各座標について最大値及び最小値を抽出することにより、このオブジェクトを包含する第１の直方体を生成することができる。また、複数の第１の直方体を包含する大きな第１の直方体を生成する場合も、複数の第１の直方体の各頂点の中から、各座標軸について最大値と最小値を抽出し、抽出された最大値と最小値とを包含する大きな第１の直方体を生成する。これにより、これら複数の第１の直方体を包含する大きな第１の直方体を生成することができる。三次元座標系においても、同様に、オブジェクトのＸ軸、Ｙ軸、Ｚ軸における最大値と最小値をそれぞれ抽出することにより、これを包含する第１の直方体を生成することができる。

図６（ｂ）に示されるように複数の第２の直方体を包含する第１の直方体は、各第２の直方体の頂点の中から、各座標軸について最大値（ｘ_max, ｙ_max）及び最小値（ｘ_min, ｙ_min）を抽出し、抽出された最大値と最小値とを包含することにより生成することができる。三次元座標系においても、同様に、複数の第２の直方体のそれぞれについて、Ｘ軸、Ｙ軸、Ｚ軸における最大値と最小値をそれぞれ抽出することにより、これを包含する第１の直方体を生成することができる。すなわち、第１の直方体、第２の直方体、凸包、または、その他の方法でオブジェクトを近似した近似体を複数生成し、生成された少なくとも２つの近似体を組み合わせる、または、包含する近似体を生成する処理を繰り返すことで、近似体の階層構造（木構造）を生成することができる。

このようにして生成された複数の第１の直方体をさらに包含する第１の直方体の生成を繰り返すことで、第１の直方体により構成される木構造のＢＶＨを生成することが可能になる。これによれば、図５は、複数の第１の直方体を包含する大きな第１の直方体を生成する処理を繰り返し、第１の直方体により構成される木構造のＢＶＨを生成する方法の概略を示す図として捉えられる。すなわち、図５に示すＢＶＨの生成方法は、１６個の第１の直方体（第１の直方体４１から第１の直方体４９−２）を出発点（最下層）として、最下層の第１の直方体のうち、隣接する複数の第１の直方体を包含する７個の第１の直方体（第１の直方体３１から第１の直方体３４−４）を有する上位層（第３層）を生成する処理を含む。さらに、図５に示すＢＶＨの生成方法は、同様の処理を繰り返して、２個の第１の直方体（第１の直方体２１から第１の直方体２２）を含む第２層、及び、１個の第１の直方体（第１の直方体１１）を含む最上位層を生成し、これら、最下層〜最上位層をＢＶＨとして構成する処理を含む。

なお、ＢＶＨまたはその他の方法により生成される階層構造（木構造）のレイヤーの数は、少なくとも２層あれば足り、たとえば、図５（ｄ）に示される１６個の第１の直方体４１から第１の直方体４９−２を第２層とし、これら１６個の第１の直方体を組み合わせて構成される単一の第１の直方体１１を第１層とする２層構造としてもよい。また、必ずしも隣り合う２個または３個以上の第１の直方体を組み合わせて上層の第１の直方体を生成しなければならないわけではなく、たとえば離間する２個または３個以上の第１の直方体を組み合わせてこれらを包含する一つの第１の直方体を生成してもよい。また、木構造の枝ごとにレイヤーの数が異なっていてもよい。

また、近似体は、第２の直方体、境界球（Bounding Sphere）、凸包その他の形状で近似することも可能である。また、同じ層において隣接する第１の直方体等は、その端部等において重複するように構成してもよい。また、組み合わせて生成される近似体は、必ずしも組み合わせの対象となる近似体を包含しなければならないということはない。たとえば、近似体を凸包として、その凸包の少なくとも一部を包含するように凸包の組み合わせからなる近似体を生成するようにしてもよい。ただし、組み合わせて生成された近似体は、組み合わせられる各近似体よりも大きくなければならない。ここで、近似体が大きい（小さい）とは、その座標系における近似体が占める領域（三次元直交座標系でいえば近似体の体積）が、他の近似体が占める領域より大きい（小さい）ことをいう。

以上述べた方法を用いて、本実施の形態においては、まず各障害物について、第１の直方体により構成される階層構造（木構造）が生成される（ステップＳ１１）。具体的には、入力部１２から入力された障害物情報に基づいて、障害物を近似する第１の直方体（以下、「障害物の第１直方体」、または、「障害物第１直方体」、という）により構成されるＢＶＨが生成される。

図７（ａ）は、前述したオブジェクト（テーブルとそのテーブル上のビン）について、着目部位ごとに第１の直方体を生成し、生成された第１の直方体により構成される階層構造（木構造）の例を示す図である。図７（ｂ）は、障害物情報に含まれる各障害物の位置情報を加味して、複数の障害物（障害物１から５）の第１の直方体により構成される階層構造（木構造）を組み合わせて形成された、空間レベルの第１の直方体により構成される階層構造（木構造）を示している。なお、各障害物については、階層構造（木構造）を生成せずに、単一の第１の直方体で近似し、各障害物に対応した複数の第１の直方体を組み合わせた第１の直方体を生成することで、第１の直方体により構成される障害物の階層構造（木構造）を形成してもよい。このようにして演算部２４により生成された障害物情報を記憶部２６に記憶させてステップＳ１１（図２）が完了する。

次いでステップＳ１２において、入力部１２より入力されたメッシュ等により表現されるロボットモデルに基づいて、第２の直方体により表現されるロボットモデル（ロボットデータ）が作成される。

本実施形態におけるロボットアーム１６は、図８に示される。図８に示すように、ロボットアーム１６は、可動軸として機能するジョイント（Ｊ１〜Ｊ５）と、所定位置に固定され、または、軸の駆動に伴って回転動作を行うリンク（ＢＬ〜Ｌ５）と、を備え、各リンクが、各ジョイントを介して可動自在に接続されて構成されている。具体的には、所定位置に固定されるベースリンクＢＬと、このベースリンクＢＬとジョイントJ１で接続され、図に示されるようにベースリンクＢＬに対して相対的にほぼ垂直方向を軸として回転するリンクＬ１と、このリンクＬ１とジョイントJ２で接続され、ほぼ水平方向を回転軸としてリンクＬ１に対して相対的に回転するリンクＬ２と、このリンクＬ２とジョイントJ３で接続され、ほぼ水平方向を回転軸としてリンクＬ２に対して相対的に回転するリンクＬ３と、このリンクＬ３とジョイントJ４で接続され、ジョイントJ３の回転軸とは直交する方向を回転軸としてリンクＬ３に対して相対的に回転するリンクＬ４と、リンクＬ４とジョイントJ５で接続され、ジョイントJ４の回転軸とは直交する方向を回転軸としてリンクＬ４に対して相対的に回転するリンクＬ５を備え、さらにリンクＬ５の先端部は、ジョイントJ６によりリンクＬ５に対して相対的に回転可能に構成されるロボットアーム１６である。

図９は、ロボットアーム１６のベースリンクＢＬからリンクＬ５の計６個の着目部位について、第２の直方体のロボット近似体を生成した例を示している。なお、本実施形態においては、オブジェクトであるロボットアーム１６を、一体的に移動するリンクＬ毎に分離し、これら各着目部位について、それぞれ第２の直方体近似体を生成した。しかしながら、着目部位は、干渉判定の目的等に応じて自由に設定可能である。たとえば、ベースリンクＢＬのように動作しない部分を無視して、移動する部位のみを着目部位として第２の直方体を生成してもよいし、あるいは、移動する部位の先端であるリンクＬ４及びリンクＬ５のみを着目部位として、第２の直方体を生成し、干渉判定を実行してもよい。また、全ての着目部位を同種のＢＶで近似する必要はない。少なくとも一つの着目部位について多面体で近似されていれば、本実施形態に係る干渉方法を適用することが可能である。

なお、第２の直方体は基準となる座標系から所定角度分について傾斜した直方体により構成されるＢＶであり、第１の直方体と比較して、前記所定角度分の演算が必要となることから、生成コストは大きい。これは、第１の直方体の生成には、メッシュモデルを構成する全頂点のうち、座標系の各軸の最大値と最小値とを探索する処理を行なえば足りるのに対し、第２の直方体の生成には、さらに、所定角度分について傾斜した座標軸についての探索が必要となるからである。一方で、図４に示すように、第２の直方体は、第１の直方体と比較して判定精度が良い。

本実施の形態では、図２に示す事前処理において、ロボットアーム１６の着目部位（リンク）を第２の直方体で近似している。これは、ロボットアーム１６のメッシュモデルに基づいて、第２の直方体を生成するコストが、その他の物体のメッシュモデルに基づいて第２の直方体を生成するコストよりも低い場合があるからである。すなわち、ロボットアーム１６に対して、所定の位置、姿勢を取らせるための運動学、または、逆運動学の演算処理では、ロボットアーム１６が有する可動軸（ジョイント）毎に、基準となる座標系から所定の角度回転した任意の座標が算出される。このように、ロボットアーム１６の姿勢は、第２の直方体を生成するのに利用可能な、任意の座標を演算することにより算出される。したがって、ロボットアーム１６の姿勢を演算するために算出した任意の座標を、第２の直方体を用いた近似体の生成処理にも利用できるから、相対的に、ロボットアーム１６を第２の直方体により近似するときの生成コストは小さい。よって、第１の直方体と比較して判定精度の良い第２の直方体を利用して、ロボットアーム１６を近似することが好ましい。

このようにして、演算部２４により、メッシュ等により表現されたロボットモデルに基づいて、着目部位毎に第２の直方体により近似された第２の直方体のロボットモデル（ロボットデータ）が計算され、記憶装置２６に記憶されることで、ステップＳ１２が完了する。生成されたロボットデータは、後述のように、動作経路上でロボットアーム１６がとり得る所定の姿勢に対応するロボット近似体の生成処理に利用される。なお、本実施の形態では、ロボットアーム１６の着目部位を近似して、ロボットデータを生成する場合について説明したが、ロボットアーム１６に加え、ロボットアーム１６の先端に設けられるエンドエフェクタを着目部位として近似して、ロボットアーム１６の着目部位と、エンドエフェクタの着目部位と、の双方を含むロボットデータを生成してもよい。さらに、エンドエフェクタによって保持される対象物（例えば、把持されるワーク）を着目部位として近似して、ロボットアーム１６の着目部位と、エンドエフェクタの着目部位と、対象物と、を含むロボットデータを生成してもよい。このとき、エンドエフェクタと対象物とは、１つの着目部位として一体に近似されてもよい。このように、ロボットアーム１６の姿勢の変更動作に伴って動作するエンドエフェクタや対象物をロボットデータに含めることで、当該ロボットと障害物との干渉判定の精度を向上させることができる。

なお、本実施形態における各ステップは、その趣旨に逸脱しない範囲で順不同であり、たとえば、ステップＳ１１をステップＳ１２の後、または同時に実行してもよい。

以上の事前処理に続いて、干渉判定が実行される。図１０は、第１実施形態における干渉判定システム１０を用いた干渉判定方法のフローチャートを示している。なお、図１０に示すフローチャートは、障害物（周辺物）について、オブジェクト単位での近似体を生成し、この障害物の近似体と、階層構造化（木構造化）したロボットの近似体と、の干渉判定について説明するが、障害物（周辺物）の近似体を、階層構造（木構造）としてもよい。

まず、ステップＳ２１において、入力部から入力されたロボットアーム１６の第１の位置に相当する初期姿勢（初期位置）ならびに第２の位置に相当する目標姿勢（目標位置）に基づいてロボットアーム１６の中間姿勢（中間位置）が生成される。ここで、初期姿勢（初期位置）は、ロボットアーム１６が静止しているときの姿勢に限られず、動作経路上の途中の地点における姿勢でもよい。同様に、目標姿勢（目標位置）は、ロボットアームが最終的にとるべき姿勢に限られず、動作経路上の途中の地点における姿勢でもよい。

図１１は、ステップＳ１２で得られたロボットアームの初期姿勢の各リンクＬを近似したロボット近似体に相当する第２の直方体と、目標姿勢の各リンクＬを近似したロボット近似体に相当する第２の直方体を示している。

図１２は、所定の着目部位（リンク５０）が、始点においてとる初期姿勢Ｓから、所定の動作経路を経て移動し、終点においてとる目標姿勢Ｇまでに障害物と干渉するか適切に判定するために設定すべき中間位置（中間姿勢）の数を決定するための方法を示している。なお、始点から終点までの動作経路は、公知の方法により算出されたものでよく、例えば、特開２０１４−０７３５５０に記載されるような、始点から終点まで移動する経路を探索するする手法や、作業者がロボットアーム１６に対して教示した教示点を公知の補間方法により接続して経路を生成する手法を採用することができる。図１２（ａ）は、動作経路に沿ってリンク５０の第２の直方体が移動する様子を示している。図１２（ｂ）は、中間姿勢の数を１個と設定し、この中間姿勢Ｍ１を、始点（初期姿勢Ｓ）と終点（目標姿勢Ｇ）の中間に配置した例を示している。図１２（ｃ）は、中間姿勢の数を３とし、初期姿勢Ｓと中間姿勢Ｍ１の中間に第２中間姿勢Ｍ２を配置し、中間姿勢Ｍ１と目標姿勢Ｇの中間に第３中間姿勢Ｍ３を配置した例を示している。図１２（ｄ）は、中間姿勢の数を７とし、初期姿勢Ｓと中間姿勢Ｍ１の間に中間姿勢Ｍ２、Ｍ４、Ｍ５を、中間姿勢Ｍ１と目標姿勢Ｇの間に間姿勢Ｍ３、Ｍ６、Ｍ７を配置した例を示している。

本実施の形態では、初期姿勢Ｓと目標姿勢Ｇとの間にＡ個（Ａは１以上の自然数）の中間姿勢Ｍを設け、初期姿勢Ｓと目標姿勢Ｇとを、Ａ個の中間姿勢Ｍを介して接続したリンク間距離Ｌａを計算し、続いて、初期姿勢Ｓと目標姿勢Ｇとを、Ａ個よりも多いＢ個の中間姿勢Ｍを介して接続したリンク間距離Ｌｂを計算する。続いて、リンク間距離Ｌａとリンク間距離Ｌｂとを比較して、ＬｂのＬａに対する変化の度合いΔＬ（例えば、変化率（Ｌｂ／Ｌａ）と変化量（Ｌｂ−Ｌａ）との少なくともいずれか一方）が、予め定められた所定の閾値Ｖを下回るか否かを判断する。変化の度合いΔＬが所定の閾値Ｖ未満である場合には、中間姿勢Ｍの数をＡ個に設定する。一方、変化の度合いΔＬが所定の閾値Ｖ以上である場合には、初期姿勢Ｓと目標姿勢Ｇとを、Ｂ個よりも多いＣ個の中間姿勢Ｍを介して接続したリンク間距離Ｌｃを計算し、ＬｃのＬｂに対する変化の度合いが、予め定められた所定の閾値Ｖを下回るか否かを判断する。変化の度合いΔＬが閾値未満である場合には、中間姿勢Ｍの数をＢ個に設定する。一方、変化の度合いが閾値以上である場合には、中間姿勢Ｍの数をＣ個よりも多いＤ個に設定し、リンク間距離Ｌｄを計算する。

以上のように、中間姿勢Ｍの数を増やしてリンク間距離Ｌを計算し、中間姿勢Ｍを増やす前後におけるリンク間距離Ｌ同士を比較して、その変化の度合いが閾値Ｖ未満であるか否かに応じて、中間姿勢Ｍの数を設定する。これによれば、移動経路が十分に反映された中間姿勢Ｍの数を適切に設定することができる。換言すれば、初期姿勢と目標姿勢とを中間姿勢を介して接続するリンク間距離を、中間姿勢の数を変更して算出し、算出されたリンク間距離を比較して、その増減に基づいて、中間姿勢の数を変更する。このように、衝突判定を正確に行うために十分な中間姿勢の取り方の粒度を設定することによって判定の精度を向上させることができる。

なお、中間姿勢の数は、様々な方法で設定することが可能である。たとえば、経路によらず固定値としてもよい。また、図１３（ａ）に示されるとおり、障害物に近いと予めわかっている箇所やリンクの変動量が大きい領域については、密に多数の中間姿勢を設定し、障害物から遠いところやリンクの変動量が小さい領域については粗く少数の中間姿勢を設定することも可能である。また、同図（ｂ）に示されるように、相対的に動作経路の始点側に近い前半部分は姿勢の変化量が小さく、後半部分に姿勢の変化量が大きくなる場合は、たとえば姿勢の変化量が大きい後半部分のみに多数の中間姿勢を設定し、前半部分は中間姿勢の数を少なくしてもよい。また、同図（ｃ）に示されるように、前半部分と後半部分の双方に所定の変化量がある場合は、同数の中間姿勢を設定してもよい。また、初期姿勢、複数の中間姿勢、目標姿勢の間を直線ではなく、所定の次数の曲線により接続してもよい。なお本実施形態において複数の中間位置を設定するとは、中間位置を特定する座標を取得する場合に限定されず、これら複数の中間位置に対応する中間姿勢に相当するデータを生成する場合を含んでいる。

また、作業者によって指定される点であって、例えば、ロボットアーム１６の先端やエンドエフェクタの先端の座標を示す点である教示点を、中間位置（中間姿勢）としてもよい。また、始点と終点とを接続する経路として、任意の線分（曲線、直線の組合せ等）を示す演算式が与えられている場合に、当該演算式を用いて、線分上に任意の点を設定し、これを中間位置（中間姿勢）としてもよい。このとき、始点と終点とを接続する任意の線分は、始点と終点とを、予め作業者によって指定された教示点を経由して接続するようにして設定することができる。

図１４は、上述した方法で決定された中間姿勢の数に基づいて生成された初期姿勢、中間姿勢、目標姿勢の第２の直方体をリンクＬごとに示している。固定部位であるベースリンクＢＬに近いリンクＬほど移動量が小さく、ロボットアーム１６の先端にいくほど、移動量が多いことが示されている。

次いで、着目部位のうち移動リンクであるリンクＬ１からリンクＬ５について、それぞれ、ステップＳ２２からＳ２７が繰り返されるループ処理が実行される。本実施の形態においては、まずリンクＬ５について、第１の直方体により構成される階層構造（木構造）を生成するステップＳ２２が実行される。なお、階層構造（木構造）は、初期姿勢、中間姿勢、目標姿勢における着目部位（リンクＬ）について生成した第２の直方体を含んでいてもよい。このとき、第２の直方体は、階層構造（木構造）の最下層を構成する葉として位置づけられることが好ましい。

次に、図１５および図１６に基づいて、着目部位毎に、初期姿勢、中間姿勢、目標姿勢に対して生成されるロボット近似体から、第１の組み合わせ近似体と、第２の組み合わせ近似体と、を生成する方法について説明する。

前述のように、ロボット近似体は、第２の直方体を用いて着目部位を近似することにより生成される。本実施例において、第１の組み合わせ近似体は、第１の直方体を用いて、複数のロボット近似体を包含するように近似することで生成される。さらに、本実施例において、第２の組み合わせ近似体は、第１の直方体を用いて、複数の第１の組み合わせ近似体を包含するように近似することで生成される。なお、第１の直方体を用いて、相対的に小さな複数の組み合わせ近似体を包含するように近似することで、相対的に大きな組み合わせ近似体を生成する処理を繰り返し、第２の組み合わせ近似体よりもさらに大きな組み合わせ近似体を生成してもよい。なお、近似する方法として、第１の直方体と第２の直方体以外を用いてもよい。

図１５は、リンクＬ５について、初期姿勢、９個の中間姿勢、目標姿勢を近似する合計１１個の第２の直方体の近似体を示している。そして、図１６（ａ）及び（ｂ）は、これら第２の直方体を組み合わせて生成された第１の直方体と、生成されたこれら複数の第１の直方体の階層構造（木構造）の平面図（ＸＺ平面）と、斜視図をそれぞれ示している。

このステップＳ２２では、まず、隣り合う２個の第２の直方体を組み合わせて、これらを包含する第１の直方体が生成される。このとき、任意の隣り合う２個の第２の直方体の間の空間（隙間）が、少なくともいずれか一つの第１の直方体によって包含されるように第１の直方体が生成される。このように第１の直方体を生成することによって、初期姿勢から、中間姿勢を経て目標姿勢に至るまでにリンクＬ５が通過する領域を間隙なく干渉判定することが可能になり、判定精度が向上する。

図１６（ａ）は、隣り合う２個の第２の直方体６０と第２の直方体６１を組み合わせて、これを包含する第１の直方体７０が生成され、同じく隣り合う２個の第２の直方体６１と第２の直方体６２を組み合わせて、これを包含する第１の直方体７１が生成された様子を示している。以下同様に隣り合う２個の第２の直方体を組み合わせて、これを包含する第１の直方体が生成され、階層構造（木構造）のうち、第１の直方体の最下層（第４層）を構成する複数の第１の直方体（第１の組み合わせ近似体）が生成される。なお、隣り合う３個以上の第２の直方体から１個の第１の直方体を生成してもよい。また、第１の直方体は、第２の直方体と接する必要はなく、マージンをもって複数の第２の直方体を包含するように生成してもよい。

次いで、生成された複数の第１の直方体（第１の組み合わせ近似体）のうち、隣接する第１の直方体を組み合わせてその上位層である第３層の第１の直方体（第２の組み合わせ近似体）が生成される。図１６（ａ）に示されるように、第１の直方体７０と第１の直方体７１を組み合わせて第１の直方体８０が生成され、第１の直方体７２と第１の直方体７３を組み合わせて第１の直方体８１等が生成される。このようにして、第３層となる複数の第１の直方体（第２の組み合わせ近似体）が生成される。

次いで、第２層の第１の直方体として、第１の直方体８０と第１の直方体８１を組み合わせて第１の直方体９０等が生成される。さらに最上位層として、第１の直方体９０と第１の直方体９１を組み合わせて第１の直方体１００が生成される。この第１の直方体１００は、着目部位であるリンクＬ５について、１１個全ての第２の直方体を包含する第１の直方体である。以上の処理をＣＰＵ２４ならびにメモリ２８を用いて実行し、上記のように生成された第１の直方体により構成される階層構造（木構造）を記憶装置２６に記憶させることによって、リンクＬ５について、第１の直方体により構成される階層構造（木構造）を生成するステップＳ２２が完了する。

なお、第２層の第１の直方体（第１の直方体９０、第１の直方体９１）、および、第１層の第１の直方体（第１の直方体１００）は、第１直方体を用いて、相対的に小さな複数の組み合わせ近似体を包含するように近似することで生成される、相対的に大きな組み合わせ近似体である。したがって、第２層の第１の直方体と、第１層の第１の直方体は、第２の組み合わせ近似体に該当する。また、第２層の第１の直方体（第１の直方体９０、第１の直方体９１）は、第１直方体を用いて、複数の第２の組み合わせ近似体を包含するように近似することで生成された組み合わせ近似体であるので、第３の組み合わせ近似体に該当する。また、第１層の第１の直方体（第１の直方体１００）は、第１直方体を用いて、複数の第３の組み合わせ近似体を包含するように近似することで生成された組み合わせ近似体であるので、第４の組み合わせ近似体に該当する。

次いで、ステップＳ２３において、ステップＳ２２で生成された第１の直方体により構成される階層構造（木構造）を構成する各第１の直方体と、事前処理にて生成された障害物の第１直方体を比較して干渉可能性の有無が判定される。ここで、ロボットと周辺物の干渉可能性を判定する、とは、ロボットと周辺物の干渉の有無を、ロボット及び周辺物の近似体を用いて仮想空間上で推定することにより判定することをいう。

図１７は、第１の直方体同士の干渉判定方法を、二次元座標系を用いて模式的に示している。本実施の形態では、この干渉判定方法を、ロボットについて、第１の直方体を用いて生成した組み合わせ近似体と、障害物（周辺物）について、第１の直方体を用いて生成した周辺物近似体と、の干渉を判定する方法として用いることができる。この図に示されるように、六角形Ｈを近似する第１の直方体（Ｈ）と、星形Ｓを近似する第１の直方体（Ｓ）との干渉有無は、それぞれの第１の直方体の各軸における最大値と最小値を比較することによって判断することができる。図１７（ａ）において、Ｙ軸については、第１の直方体（Ｈ）のＹ軸上の位置（すなわち、最大値と最小値の間）が第１の直方体（Ｓ）のＹ軸上の位置と重複するため、Ｙ軸のみでは干渉可能性が否定できない。しかしながら、Ｘ軸において、第１の直方体（Ｓ）のＸ軸上の最大値よりも、第１の直方体（Ｈ）のＸ軸上の最小値が大であるため、Ｘ軸で両者は分離しており、よって六角形Ｈと星形Ｓは干渉しないと判断される。一方で、図（ｂ）の場合は、Ｘ軸上においても、Ｙ軸上においても、重複領域が存在しているため第１の直方体同士は干渉しており、結果として六角形Ｈと星形Ｓは干渉している可能性があると判断される。なお、図１７（ｃ）の場合は、六角形Ｈと星形Ｓ自体は干渉していないものの、それを近似する第１の直方体（Ｈ）と第１の直方体（Ｓ）は、重複領域を有するため、六角形Ｈと星形Ｓは干渉可能性ありと判断される。ここで、ロボットアーム１６と周辺物（障害物）との干渉判定においては、安全性をより高めるために、実際には干渉が生じているにも関わらず非干渉であると判定される旨の誤判定を確実に防ぐことが好ましい。この点で、本実施形態における手法は、図１７（ｃ）に示すように、実際には非干渉である場合でも干渉であると判定される旨の誤判定が生じる懸念があるものの、干渉を非干渉と判定する誤判定を確実に防止することが可能になる。以上のように、第１の直方体を用いて、ロボット、周辺物等のオブジェクトを近似することによって、座標軸（三次元直交座標の場合は、Ｘ軸、Ｙ軸及びＺ軸）の各オブジェクトの位置情報のみに基づいて高速に干渉可能性を判定することが可能になる。

上述したような方法で、ステップＳ２３において、ロボットの近似体についての階層構造（木構造）を構成する第１の直方体と、障害物の第１直方体の干渉可能性が判断される。この際、まず、ロボットの近似体についての階層構造（木構造）の最上層を構成する最大の第１の直方体１００と、障害物の第１直方体の干渉有無が判定される。そして、両者が干渉しないと判断された場合は、ロボットが障害物と干渉する可能性が無いと判断できるので、ロボットの近似体についての階層構造（木構造）の下層に属する他の第１の直方体と、障害物の第１直方体との干渉判定を省略することが可能になる。一方で、両者が干渉すると判断された場合は、ロボットの近似体についての階層構造（木構造）の一つ下の階層に属する第１の直方体９０及び第１の直方体９１と、障害物の第１直方体の干渉有無がそれぞれ判断される。なお、前述のように、障害物（周辺物）を所定の方法（第１の直方体、第２の直方体、凸包等）により近似し、これを、階層構造（木構造）としてもよい。その場合には、障害物についても階層を下げて判定を行い、干渉有無の判定を行う。

以下同様に、障害物の第１直方体と干渉すると判断されたロボットの第１の直方体について、当該第１の直方体が属する階層の下層の第１の直方体と、障害物の第１直方体の干渉有無が判断される。その結果、最下層（ロボットについて、第１の直方体により構成される階層構造（木構造）の葉に相当）のいずれの第１の直方体についても、障害物の第１直方体との干渉が無いと判断された場合は、そのリンクＬについてのループ処理を終了する。一方で、最下層のいずれかの第１の直方体について、障害物の第１直方体と干渉すると判断された場合は、干渉すると判断されたロボットの第１の直方体が抽出される（ステップＳ２４）。なお、上述したように、ロボットの近似体についての階層構造（木構造）を２層のみとして、階層構造の最大の第１の直方体で干渉可能性があると判定された場合は、最小の第１の直方体（すなわち、１１個の第２の直方体について隣り合う２つの第２の直方体を組み合わせて生成された１０個の第１の直方体）と、障害物の第１直方体との干渉有無をそれぞれ判断してもよい。

以上のようにして、障害物の第１直方体と衝突したロボットの第１の直方体が抽出される。図１８は、板状の障害物Ｄの第２の直方体に基づいて生成された障害物の第１直方体が、ロボットを第１の直方体によって近似して構成した階層構造（木構造）の最下層に含まれる第１の直方体のうち、各座標軸について重複領域を有する第１の直方体７５、第１の直方体７６、第１の直方体７７と衝突すると判定された様子を示している。

次いで、上記ステップで抽出された第１の直方体７５、第１の直方体７６、第１の直方体７７について、ステップＳ２５からステップＳ２６のループ処理がそれぞれ実行される。

まず、障害物Ｄの第２の直方体と、ロボットについての第１の直方体７５との干渉判定処理が実行される。第１の直方体と第２の直方体との干渉は、たとえば、ＸＹ平面に写像した第１の直方体と、ＸＹ平面に写像した第２の直方体の比較、ならびに、ＸＺ平面に写像した第１の直方体と、ＸＺ平面に写像した第２の直方体を比較し、重複部分の有無により判定可能だが、その他、公知の様々な方法を用いて、判定処理を実行してもよい。図１８（ｂ）及び（ｃ）に示されるように、ロボットについての第１の直方体７５は、ＸＹ平面において重複領域を有するものの、ＸＺ平面において障害物の第２の直方体と分離しているため、第１の直方体７５と障害物第２の直方体は、干渉しないと判定される。一方で、ロボットについての第１の直方体７６及び第１の直方体７７は、両平面において障害物第２の直方体と重複領域を有するため、それぞれ干渉していると判定される。障害物の第２の直方体と干渉すると判定された第１の直方体７６及び第１の直方体７７は、干渉している旨の情報が記憶装置２６に記憶され、また、干渉すると判定された第１の直方体が包含するリンクＬの移動範囲は、衝突可能性があるリンク移動範囲として登録され、記憶装置２６に記憶される（ステップＳ２６）。ステップＳ２５及びＳ２６からなるループ処理の終了後、衝突可能性があるリンク移動範囲の登録の有無が判断され（ステップＳ２７）、登録がない場合は、そのリンクＬについての一連の処理を終了し、次のリンクＬについてステップＳ２２から始まる処理を繰り返す。ステップＳ２７において、衝突可能性があるリンク移動範囲が登録されている場合は、その範囲について詳細判定を実行し、衝突点を特定する。

衝突点の特定は公知の方法も含めた、様々な方法を用いることが可能である。本実施形態においては、衝突可能性があるリンクＬについて、衝突可能性があるリンク移動範囲の始点の姿勢と、終点の姿勢の間の空間において、周辺物と衝突し得る点を公知の方法を用いて抽出し、その際のロボットまたはリンクＬの位置及び姿勢のパラメータ（ｘ、ｙ、ｘ、α、β、γ）を算出する処理が実行される。

なお、上述した実施形態においては、ロボットアーム１６の着目部位を近似する第１の直方体により構成される階層構造（木構造）をＢＶＨによって生成し、これを障害物の第１直方体及び障害物の第２の直方体と比較して干渉判定を実施したが、第１の直方体の代わりに種々の近似体を用いることが可能であり、たとえば、凸包を用いることも可能である。以下では、凸包の生成方法ならびに第１の直方体と凸包を本実施形態に採用した場合の効果の相違について説明する。

図１９は、凸包の生成方法を示している。上述したとおり凸包とは、その座標系に存在する複数の点を包含する最小の凸多角形（または多面体）のことをいう。たとえば、図１９（ａ）に示される点集合の凸包は、たとえば以下の方法で生成される。すなわち、まず任意の二点（点Ａ及び点Ｂ）を通過する直線（または平面）を定義する（図１９（ｂ））。そして、この直線の上側の領域と下側の領域のそれぞれについて、その直線（または平面）との距離が最大となる点（点Ｃ及び点Ｄ）を定義し（図１９（ｃ））、これらの点を稜線（または面）で接続するとともに、その稜線で囲まれた領域内部の点については、処理対象から除外する（図１９（ｄ））。さらに領域外部の点について、稜線（または面）との距離が最大となる点Ｅを特定し、稜線（または面）で接続する（図１９（ｅ））。この処理を繰り返すことにより、凸包を生成することができる（図１９（ｆ））。

したがって、上述した実施の形態において所定の着目部位について隣接する第２の直方体を組み合わせて作成された第１の直方体に替えて、隣接する第２の直方体の頂点を点集合とする凸包を生成し、さらにこれら凸包同士を組み合わせて、さらなる凸包を生成して、凸包の階層構造を生成し、障害物との干渉判定をしてもよい。また、障害物の第１直方体又は障害物の第２の直方体を生成することに替えて障害物の凸包を生成し、障害物の第１直方体又は障害物の第２の直方体による干渉があった場合にのみ、障害物の凸包を用いて精度の良い判定を実現することも可能である。凸包を用いた干渉判定は、公知の様々な方法を利用することが可能である。

上記のような実施の形態に係る判定システムの効果について以下に述べる。まず、ロボットの着目部位の形状を多面体で近似した近似体を、始点、複数の中間点、終点に対して生成した後、その近似体そのものではなく、少なくとも２つの近似体を組み合わせた近似体と、そのような組み合わせの近似体を複数組み合わせたさらに大きな組み合わせ近似体を生成し、これらを用いて段階的に障害物との干渉判定を実施するようにしたから、判定処理の高速化を実現することが可能となる。

換言すれば、組み合わせられる近似体の数が相対的に少ない近似体と、相対的に多い近似体とを生成し、さらに、組み合わせられた近似体の数に応じた階層構造（ツリー構造）を生成したうえで、当該階層構造（ツリー構造）を用いて干渉判定を実施するようにしたから、不要な近似体との干渉判定の処理を除外でき、判定処理の高速化を実現することが可能となる。

特に、第１の直方体を判定に用いる場合に、凸包の場合と比較して以下のような効果が発揮される。

すなわち、図４に示すように、第１の直方体を用いる場合、その生成コストは、凸包を用いる場合と比較して、凸包の特定に用いられる点の数によるものの、小さいことが一般的である。また、凸包を用いた干渉判定の処理には、一般に１ミリ秒以上の時間を要する。このため、凸包を用いた場合に、干渉判定のために要する時間は、少なくとも１ミリ秒以上になる。一方で、第１の直方体は、各座標における位置情報（たとえば、第１の直方体を構成する少なくとも二つの頂点の三次元位置情報）に基づいて生成、干渉判定が可能となるため、その生成コストは、凸包と比較して小さく、また、干渉判定の処理は、ナノ秒のオーダーで実現することができ、飛躍的な判定速度の向上が期待できる。

また、凸包の生成に時間を要する結果、十分な時間間隔で離散的に、凸包の組み合わせの近似体を生成せざるを得ないが、ロボットは、直線運動ではなく曲線運動をする場合もあることから、図２０に示される問題点が発生する可能性がある。図２０（ａ）には、凸包を用いた干渉判定では干渉しないと判定したにもかかわらず、実際のロボットは障害物と干渉してしまう場合を、図２０（ｂ）には、逆に、凸包を用いた干渉判定では干渉すると判定したにもかかわらず、実際のロボットは障害物と干渉しない場合を示している。

図２０（ａ）の左側の図に示されるとおり、２つの姿勢を構成する点集合から形成される凸包（点線）は、障害物Ｄと干渉しないので、ロボットＲと障害物Ｄは、非干渉と判定される。しかしながら、実際のロボットＲが右側の図に示されるように曲線運動をして凸包を超え、その領域に障害物Ｄがあれば、判定結果上は干渉しないと判定されたにもかかわらず、実際は干渉することになる。一方で、図２０（ｂ）の場合、左側の図に示されるように、凸包（点線）と障害物Ｄは干渉するので、判定結果上は干渉すると判定される。しかしながら、実際のロボットが右側の図に示されるように曲線運動をして、障害物を回避するように運動した場合、判定結果上は、干渉すると判定されたにもかかわらず、実際は干渉しないという事態を招く。従って、凸包を用いて干渉判定をする場合、実際は干渉しないにもかかわらず、干渉すると判定される場合と、逆に、実際は干渉するにもかかわらず、干渉しないと判定される場合の双方が発生しうる。

一方で、第１の直方体を用いて判定をする場合、２つの近似体を包含するように第１の直方体を生成することができるから、図２１（ａ）に示されるように、判定上も実際にも干渉する場合のみならず、図２１（ｂ）に示されるように、干渉可能性があると判定されたにもかかわらず実際は干渉しないケースは発生するものの、干渉可能性が無いと判定したにもかかわらず実際は干渉するケースを排除することが可能になる。特に、十分に余裕をもってオブジェクトの各姿勢を包含するように第１の直方体が生成された場合は、干渉可能性が無いと判定したにもかかわらず実際は干渉するケースが発生する可能性を限りなくゼロにすることが可能になる。さらに、第１の直方体を用いて生成された階層構造（木構造）の上位層に含まれる第１の直方体において干渉可能性が無いと判断した後は、その下層の第１の直方体について判断する必要がなくなるから処理の一層の高速化を期待できる。また、その生成の容易性から十分な数の中間位置における中間姿勢を考慮した多数の第１の直方体を生成することも可能になるから、ロボットの曲線運動に対応した干渉判定が可能となる。そして、干渉可能性があると判定された領域についてのみ選択的に、時間を要する詳細な判定処理を実行することで、干渉判定処理の最適化を図ることも可能となる。

なお、本実施形態における各ステップは、その趣旨を逸脱しない範囲で順不同である。たとえば、入力データとして、初期姿勢と目標姿勢の情報を入力するステップは、その情報が用いられる以前のどのタイミングで実施されてもよい。また、情報の入力は、最終的に所定のインターフェースを介して演算部を構成するプロセッサにより処理可能な状態になればよく、たとえば、所定のコンピュータプログラム等を用いて自動的に入力されるように構成されていてもよい。たとえば、初期姿勢と目標姿勢との情報を入力データとして入力するステップは、図示しないセンサが取得する情報を、所定のコンピュータプログラムを用いて、自動的に取得することで実現されていてもよい。具体的には、たとえば、撮像装置が取得した画像情報に基づいて、ロボットアーム１６による作業の対象となる所定のワークの現在位置および／または姿勢と、当該ワークの載置位置および／または姿勢と、を算出し、算出された初期姿勢及び目標姿勢に関する情報を入力データとしての入力するようにしてもよい。

また、障害物の近似体として、障害物を包含する最大の近似体を用いて干渉判定した後に、その障害物を複数の着目部位に分離した近似体（たとえば第１の直方体）を用いて、所定の着目部位あるいは、各着目部位の近似体について、ロボット近似体との干渉判定を実施してもよい。そのようにすることで、干渉可能性のある障害物の部位をより精度良く特定することが可能になる。また、全ての障害物を包含する第１の直方体等の近似体を生成して干渉判定に用いるかわりに、障害物ごとに第１の直方体等の近似体を生成して、各障害物の近似体と、ロボットとの干渉判定を実施してもよい。
［第２実施形態］
図２２は、本発明の第２実施形態に係る判定処理のフローチャートを示している。なお、第１実施形態に係る判定処理と同様の処理については説明を省略する。

ステップＳ３１及びＳ３２は、環境が変更されるたびに実行されるべき処理である。たとえば、周辺の障害物が変更された場合に再実行される。

ここで、ステップＳ３１において、障害物の仮想近似体は、第１の直方体もしくは第２の直方体のいずれか一方のみならず、第１の直方体と第２の直方体との組み合わせや、凸包などその他の近似体で障害物の近似体を生成することができる。

次いで、ステップＳ３２において、ロボットを着目部位であるリンクＬ毎に第２の直方体を表現するロボットデータが生成される。ここで、ロボットの着目部位は、モータ等により定義される軸（回転軸等）から構成される座標系で容易に定義できることから、その座標軸に基づいて、容易で第２の直方体からなる近似体を生成できる。第１の直方体を生成する場合は、さらに回転変換行列を用いた演算処理が必要となり、処理を複雑化させるため、ロボットの着目部位の近似体は、第２の直方体を用いることが好ましい。

その後、ステップＳ３３以降のステップは、初期姿勢または目標姿勢が変更されるたびに（たとえば、異なる地点間の干渉判定をするたびに）実行される。

まずステップＳ３３において、中間姿勢を生成する。中間姿勢の数（分解能の決定）は、上述したほか、たとえば、以下の（１）から（５）のステップを用いて決定することも可能である。

（１）予め所定の間隔で、異なる地点間（始点と終点間）に複数の中間点を設定する。

（２）次いで、始点から複数の中間点を経て終点に達するまでのリンクの総経路長（Ｌ）を算出する。

（３）この間隔よりもさらに小さい間隔で異なる地点間に複数の中間点を設定する。

（４）次いで始点からこれら複数の中間点を経て終点に達するまでのリンクの経路長（Ｌ'）を算出する。

（５）その後、ＬとＬ'の変動率を算出し、所定の閾値を下回る場合は、処理を終了し（２）における中間点の数を、中間姿勢の数として確定する。下回らない場合は、上記（１）から（４）をさらに間隔を狭めて実行し、閾値が下回った際の間隔に基づいて複数の中間姿勢を決定する。

ステップＳ３４（中間姿勢の第２の直方体のロボット近似体の生成）、ステップＳ３５（隣接する第２の直方体を包含する第１の直方体の生成）、ステップＳ３６（前のステップで生成された複数の第１の直方体を組み合わせる処理を、初期姿勢、中間姿勢、目標姿勢を全て包含する第１の直方体が生成されるまで実行することによる第１の直方体により構成される階層構造（木構造）の生成）は、第１実施形態で述べた実施態様と同様であるため説明を省略する。なお、階層構造（木構造）は、初期姿勢、中間姿勢、目標姿勢における着目部位（リンクＬ）について生成した第２の直方体を含んでいてもよい。このとき、第２の直方体は、階層構造（木構造）の最下層を構成する葉として位置づけられることが好ましい。

次いでステップＳ３７における第１の直方体により構成されたロボットの階層構造（木構造）と障害物の仮想近似体との干渉判定処理について、図２３を用いて説明する。図２３は、ロボットの階層構造（木構造）ツリーを示している。

この図に示されるように、階層構造（木構造）の最上位層は、全ての姿勢の第２の直方体、すなわち始点Ｖ_Sの初期姿勢の第２の直方体、中間点Ｖ_n（ただしｎは自然数）における各中間姿勢の第２の直方体及び終点Ｖ_Gにおける目標姿勢の第２の直方体を包含する第１の直方体である。

そして、最上位層の第１の直方体と障害物が干渉する場合は、その下層の第１の直方体と障害物が干渉するか判定され、干渉すると判定された場合は、その第１の直方体のさらに下層にある第１の直方体について障害物との干渉の判定処理が実行される。

このような処理を繰り返しながら階層構造（木構造）の下層へと移動していき、最下層の第１の直方体（動作経路上において隣接する第２の直方体を組み合わせて生成された第１の直方体であり、図２３における下から２番目のレイヤー）のうち、障害物の近似体と干渉するものがあれば、その第１の直方体（図においては、Ｖ₁とＶ₂における中間姿勢の第２の直方体近似体を包含する第１の直方体）を抽出する（ステップＳ３８）。

その後は、詳細判定が実行され、この第１の直方体に包含される２つの姿勢間の空間で、障害物と衝突すると判定された際のロボットの位置・姿勢のパラメータ（ｘ，ｙ，ｚ，α，β，γ）を特定する処理が実行される（ステップＳ３９）。

このような実施の形態においても、第１実施の形態と同様の効果を期待することができる。また、衝突したロボット第１の直方体を抽出した後に、第２の直方体を用いた干渉判定を実施することなく、詳細判定に移行するので、より実装に適しているといえる。
第１の直方体第１の直方体第１の直方体
このように障害物の近似体として第２の直方体を利用することによって、より精度の高い干渉判定を簡便に実施することが可能となる。特に、障害物の形状によっては、第１の直方体で十分な精度で近似できない場合があり、そのような障害物を第２の直方体で近似することにより、高速に精度の高い干渉判定が可能になる。

なお、ステップＳ３６において、ロボットについて第１の直方体により構成される階層構造（木構造）を生成した後に、ステップＳ３７の干渉判定を実施したが、階層構造（木構造）は、ロボットについての第１の直方体との干渉判定の結果に応じて生成するようにしてもよい。すなわち、初期姿勢、中間姿勢、目標姿勢の第２の直方体がステップＳ３４で生成された後に、これら第２の直方体の全てを包含する総組み合わせ近似体としての第１の直方体（階層構造（木構造）の最上位に位置付けられる第１の直方体）を生成し、ステップＳ３５ならびにＳ３６を実施することなく、総組み合わせ近似体と障害物の近似体との干渉判定が実施される構成を採用してもよい。そして、干渉したと判定された場合にのみ、その下層の第１の直方体として、初期姿勢、中間姿勢、目標姿勢に対応する複数のロボット近似体の少なくとも２つを包含する組み合わせ近似体を部分組み合せ近似体として生成して（たとえば、ステップＳ３５における隣接する中間姿勢を包含する第１の直方体の生成）、障害物との干渉判定をさらに実行するように構成してもよい。また、相対的に多くのロボット近似体が包含されるように生成され、相対的に上位層に含まれる第１の直方体で干渉したと判定された場合に、手段の如何によらず、その第１の直方体に含まれる２つの第１の直方体の間で、離散幅をさらに狭めて、仮想ロボットの位置姿勢を算出し、当該位置姿勢毎に、仮想ロボットと周辺物との干渉を判定して、位置姿勢パラメータを算出するように構成してもよい。このような構成を採用することで、階層構造（木構造）を生成するコストを抑制することが可能になり、一連の処理のより一層の高速化を図ることができる。

以上説明した実施形態は、本発明の理解を容易にするためのものであり、本発明を限定して解釈するためのものではない。実施形態が備える各要素並びにその配置、材料、条件、形状及びサイズ等は、例示したものに限定されるわけではなく適宜変更することができる。また、異なる実施形態で示した構成同士を部分的に置換し又は組み合わせることが可能である。

上記の実施形態の一部又は全部は、以下の付記のようにも記載され得るが、以下には限られない。
（付記１）
取得部、記憶部及び演算部を備える演算装置を用いて、複数の可動軸を有するロボットが第１の位置から第２の位置に移動する際に、前記ロボットの周辺に存在する周辺物と干渉する可能性を判定する方法であって、
前記演算部は、
前記記憶部に、前記ロボットの形状を着目部位毎に多面体で近似したロボットデータを記憶させるステップと、
前記記憶部に、前記周辺物の形状を近似した周辺物近似体のデータを記憶させるステップと、
前記取得部を用いて、前記第１の位置及び第２の位置を取得するステップと、
前記第１の位置から第２の位置へと移動する動作経路上に、少なくとも１つの中間位置を設定するステップと、
前記ロボットデータに基づいて、前記第１の位置、前記第２の位置及び前記中間位置における前記ロボットの形状を少なくとも一つの前記着目部位について前記多面体で近似した複数のロボット近似体を生成するステップと、
少なくとも一つの前記着目部位について、前記第１の位置、前記第２の位置及び前記中間位置に相当する複数の前記ロボット近似体の少なくとも２つを組み合わせて構成される第１の組み合わせ近似体を複数生成し、さらに、これら第１の組み合わせ近似体の少なくとも２つを組み合わせて構成される第２の組み合わせ近似体を生成するステップと、
この第２の組み合わせ近似体と、前記周辺物近似体とが干渉するか判定するステップと
を実行する干渉判定方法。

（付記２）
複数の可動軸を有するロボットが第１の位置から第２の位置に移動する際に、前記ロボットの周辺に存在する周辺物と干渉する可能性を判定するための干渉判定システムであって、
前記ロボットの形状を複数の着目部位毎に多面体で近似したロボットデータ及び前記周辺物の形状を近似した周辺物近似体のデータを記憶するための記憶部と、
前記第１の位置及び第２の位置を取得するための取得部と、
前記第１の位置から前記第２の位置へと移動する動作経路上に、少なくとも１つの中間位置を設定し、
前記ロボットデータに基づいて、前記第１の位置、前記第２の位置及び前記中間位置における前記ロボットの形状を、少なくとも一つの前記着目部位について前記多面体で近似した複数のロボット近似体を生成し、
少なくとも一つの前記着目部位について、前記第１の位置、前記第２の位置及び前記中間位置に相当する複数の前記ロボット近似体の少なくとも２つを組み合わせて構成される第１の組み合わせ近似体を複数生成し、
これら第１の組み合わせ近似体の少なくとも２つを組み合わせて構成される第２の組み合わせ近似体を生成し、
この第２の組み合わせ近似体と、前記周辺物近似体とが干渉するか判定し、
干渉すると判定された場合は、前記複数の第１の組み合わせ近似体と、前記周辺物近似体とが干渉するかそれぞれ判定するように構成される演算部と、
を備える干渉判定システム。

（付記３）
取得部、記憶部及び演算部を備える演算装置を用いて、複数の可動軸を有するロボットが第１の位置から第２の位置に移動する際に、前記ロボットの周辺に存在する周辺物と干渉する可能性を判定する方法であって、
前記演算部は、
前記記憶部に、前記ロボットの形状を、複数の着目部位ごとに多面体で近似したロボットデータを記憶させるステップと、
前記記憶部に、前記周辺物の形状を近似した周辺物近似体のデータを記憶させるステップと、
前記取得部を用いて、前記第１の位置及び第２の位置を取得するステップと、
前記第１の位置から第２の位置へと移動する動作経路上に、少なくとも１つの中間位置を設定するステップと、
前記ロボットデータに基づいて、前記第１の位置、前記第２の位置及び前記中間位置における前記ロボットの形状を前記複数の着目部位毎に前記多面体で近似した複数のロボット近似体を生成するステップと、
前記ステップで生成された全ての前記ロボット近似体を組み合わせて構成される総組み合わせ近似体を生成するステップと、
この第１の組み合わせ近似体と、前記周辺物近似体とが干渉するか判定するステップと、
このステップにおいて干渉すると判定された場合は、前記複数の着目部位毎に、前記第１の位置、前記第２の位置及び前記中間位置に相当する複数の前記ロボット近似体の少なくとも２つを組み合わせて構成される複数の部分組み合わせ近似体を生成し、
複数の前記第２の組み合わせ近似体と、前記周辺物近似体とが干渉するかそれぞれ判定するステップと、
を実行する干渉判定方法。

（付記４）
複数の可動軸を有するロボットが第１の位置から第２の位置に移動する際に、前記ロボットの周辺に存在する周辺物と干渉する可能性を判定するための干渉判定システムであって、
前記ロボットの形状を複数の着目部位毎に多面体で近似したロボットデータ及び前記周辺物の形状を近似した周辺物近似体のデータを記憶するための記憶部と、
前記第１の位置及び前記第２の位置を取得するための取得部と、
前記第１の位置から前記第２の位置へと移動する動作経路上に、少なくとも１つの中間位置を設定し、
前記ロボットデータに基づいて、前記第１の位置、前記第２の位置及び前記中間位置における前記ロボットの形状を前記複数の着目部位毎に前記多面体で近似した複数のロボット近似体を生成し、
全ての前記ロボット近似体を組み合わせて構成される総組み合わせ近似体を生成し、
この総組み合わせ近似体と、前記周辺物近似体とが干渉するか判定し、
干渉すると判定した場合は、前記複数の着目部位毎に、前記第１の位置、前記第２の位置及び前記中間位置に相当する複数の前記ロボット近似体の少なくとも２つを組み合わせて構成される複数の部分組み合わせ近似体を生成し、
複数の前記部分組み合わせ近似体と、前記周辺物近似体とが干渉するか判定するように構成される演算部と、
を備える干渉判定システム。

（付記５）
複数の可動軸を有するロボットが第１の位置から第２の位置に移動する際に、前記ロボットの周辺に存在する周辺物と干渉する可能性を判定するためのコンピュータプログラムであって、
コンピュータに、
前記ロボットの形状を、複数の着目部位ごとに多面体で近似したロボットデータを記憶させるステップと、
前記周辺物の形状を近似した周辺物近似体のデータを記憶させるステップと、
前記第１の位置及び前記第２の位置を取得するステップと、
前記第１の位置から前記第２の位置へと移動する動作経路上に、少なくとも１つの中間位置を設定するステップと、
前記ロボットデータに基づいて、前記第１の位置、前記第２の位置及び前記中間位置における前記ロボットの形状を前記複数の着目部位毎に前記多面体で近似した複数のロボット近似体を生成するステップと、
前記ステップで生成された全ての前記ロボット近似体を組み合わせて構成される総組み合わせ近似体を生成するステップと、
この総組み合わせ近似体と、前記周辺物近似体とが干渉するか判定するステップと、
このステップにおいて干渉すると判定された場合は、前記複数の着目部位毎に、前記第１の位置、前記第２の位置及び前記中間位置に相当する複数の前記ロボット近似体の少なくとも２つを組み合わせて構成される複数の部分組み合わせ近似体を生成するステップと、
複数の前記部分組み合わせ近似体と、前記周辺物近似体とが干渉するかそれぞれ判定するステップと、
を実行させるためのコンピュータプログラム。

１０…干渉判定システム、１２…入力部、１４…演算装置、１６…ロボットアーム、１８…モニタ、２０…出力部、２２…ＰＣ、２４…ＣＰＵ、２６…記憶装置、２８…メモリ。

Claims (20)

1. 取得部、記憶部及び演算部を備える演算装置を用いて、複数の可動軸を有するロボットが第１の位置から第２の位置に移動する際に、前記ロボットの周辺に存在する周辺物と干渉する可能性を判定する方法であって、
   前記演算部は、
   前記記憶部に、前記ロボットの形状を着目部位毎に多面体で近似したロボットデータを記憶させるステップと、
   前記記憶部に、前記周辺物の形状を近似した周辺物近似体のデータを記憶させるステップと、
   前記取得部を用いて、前記第１の位置及び前記第２の位置を取得するステップと、
   前記第１の位置から前記第２の位置へと移動する動作経路上に、少なくとも１つの中間位置を設定するステップと、
   前記ロボットデータに基づいて、前記第１の位置、前記第２の位置及び前記中間位置における前記ロボットの形状を、少なくとも一つの前記着目部位について前記多面体で近似した複数のロボット近似体を生成するステップと、
   少なくとも一つの前記着目部位について、前記第１の位置、前記第２の位置及び前記中間位置に相当する複数の前記ロボット近似体の少なくとも２つを組み合わせて構成される第１の組み合わせ近似体を複数生成し、さらに、これら第１の組み合わせ近似体の少なくとも２つを組み合わせて構成される第２の組み合わせ近似体を生成するステップと、
   この第２の組み合わせ近似体と、前記周辺物近似体とが干渉するか判定するステップと、
   を実行する干渉判定方法。
2. 前記第２の組み合わせ近似体と、前記周辺物近似体とが干渉すると判定された場合は、前記第２の組み合わせ近似体に含まれる前記第１の組み合わせ近似体を用いて、前記周辺物近似体と干渉するか判定するステップを更に備えることを特徴とする請求項１記載の干渉判定方法。
3. 前記複数の第１の組み合わせ近似体は、それぞれ、前記動作経路上で隣接する２つの前記ロボット近似体を組み合わせて構成されることを特徴とする請求項１又は２に記載の干渉判定方法。
4. 前記着目部位について、前記複数のロボット近似体のうち、隣り合う任意の前記ロボット近似体の間の領域は、少なくとも一つの前記第１の組み合わせ近似体により包含されていることを特徴とする請求項１から３のいずれか一項に記載の干渉判定方法。
5. 前記第２の組み合わせ近似体は、前記着目部位について前記のとおり生成された全ての前記ロボット近似体を組み合わせて構成されていることを特徴とする請求項１から４のいずれか一項に記載の干渉判定方法。
6. 前記複数の第１の組み合わせ近似体と、前記周辺物近似体とが干渉するかそれぞれ判定する前記ステップにおいて、所定の前記第１の組み合わせ近似体と、前記周辺物近似体とが干渉すると判定された場合は、
   その第１の組み合わせ近似体に含まれる隣り合う二つの前記ロボット近似体にそれぞれ相当する前記着目部位の二つの姿勢の間で、この着目部位が前記周辺物近似体と衝突した点を抽出し、その際の前記ロボットの位置及び姿勢のパラメータを生成するステップを更に備えることを特徴とする請求項２に記載の干渉判定方法。
7. 前記演算部が、前記記憶部に、前記周辺物の一部を構成する第１の着目部位の形状を近似する近似体であって、前記周辺物近似体より小さい第１の周辺物近似体のデータを記憶させるステップをさらに備え、
   前記第１又は第２の組み合わせ近似体が、前記周辺物近似体と干渉すると判定された場合は、その第１又は第２の組み合わせ近似体と、前記第１の周辺物近似体データとが干渉するか判定するステップを更に備えることを特徴とする請求項１から６のいずれか一項に記載の干渉判定方法。
8. 前記周辺物近似体、前記第１の組み合わせ近似体及び前記第２の組み合わせ近似体は、いずれも第１の直方体であって、互いに直交する３つの軸により構成される第１の座標系の各軸に平行な線分により構成される第１の直方体であり、
   前記第１又は第２の組み合わせ近似体と前記周辺物近似体が干渉すると判定された場合は、
   その第１又は第２の組み合わせ近似体と、前記周辺物を近似する第２の直方体であって、前記周辺物の形状に応じて、前記第１の座標系を所定の角度で回転させた第２の座標系の各軸に平行な線分により構成され、前記第１の直方体より小さい第２の直方体の近似体と、が干渉するか判定するステップを更に備えることを特徴とする請求項１から７のいずれか一項に記載の干渉判定方法。
9. 複数の可動軸を有するロボットが第１の位置から第２の位置に移動する際に、前記ロボットの周辺に存在する周辺物と干渉する可能性を判定するための干渉判定システムであって、
   前記ロボットの形状を複数の着目部位毎に多面体で近似したロボットデータ及び前記周辺物の形状を近似した周辺物近似体のデータを記憶するための記憶部と、
   前記第１の位置及び前記第２の位置を取得するための取得部と、
   前記第１の位置から前記第２の位置へと移動する動作経路上に、少なくとも１つの中間位置を設定し、
   前記ロボットデータに基づいて、前記第１の位置、前記第２の位置及び前記中間位置における前記ロボットの形状を、少なくとも一つの前記着目部位について前記多面体で近似した複数のロボット近似体を生成し、
   少なくとも一つの前記着目部位について、前記第１の位置、前記第２の位置及び前記中間位置に相当する複数の前記ロボット近似体の少なくとも２つを組み合わせて構成される第１の組み合わせ近似体を複数生成し、
   これら第１の組み合わせ近似体の少なくとも２つを組み合わせて構成される第２の組み合わせ近似体を生成し、
   この第２の組み合わせ近似体と、前記周辺物近似体とが干渉するか判定するように構成される演算部と、
   を備える干渉判定システム。
10. 前記演算部は、前記第２の組み合わせ近似体と、前記周辺物近似体とが干渉すると判定した場合は、前記第２の組み合わせ近似体に含まれる前記第１の組み合わせ近似体を用いて、前記周辺物近似体と干渉するか判定するように構成されていることを特徴とする請求項９記載の干渉判定システム。
11. 前記複数の第１の組み合わせ近似体は、それぞれ、前記動作経路上で隣接する２つの前記ロボット近似体を組み合わせて構成されることを特徴とする請求項９又は１０に記載の干渉判定システム。
12. 前記着目部位について、前記複数のロボット近似体のうち、隣り合う任意の前記ロボット近似体の間の領域は、少なくとも一つの前記第１の組み合わせ近似体により包含されているように構成されていることを特徴とする請求項９から１１のいずれか一項に記載の干渉判定システム。
13. 前記第２の組み合わせ近似体は、前記着目部位について前記のとおり生成された全ての前記ロボット近似体を組み合わせて構成されていることを特徴とする請求項９から１２のいずれか一項に記載の干渉判定システム。
14. 前記演算装置は、前記複数の第１の組み合わせ近似体と、前記周辺物近似体とが干渉するかそれぞれ判定する際に、所定の前記第１の組み合わせ近似体と、前記周辺物近似体とが干渉すると判定した場合は、
    その第１の組み合わせ近似体に含まれる隣り合う二つの前記ロボット近似体にそれぞれ相当する前記着目部位の二つの姿勢の間で、この着目部位が前記周辺物近似体と衝突した点を抽出し、その際の前記ロボットの位置及び姿勢のパラメータを生成するように構成されていることを特徴とする請求項１０に記載の干渉判定システム。
15. 前記記憶部は、前記周辺物の一部を構成する第１の着目部位の形状を近似する近似体であって、前記周辺物近似体より小さい第１の周辺物近似体のデータを記憶するように構成され、
    前記演算部は、前記第１又は第２の組み合わせ近似体が、前記周辺物近似体と干渉すると判定した場合は、その第１又は第２の組み合わせ近似体と、前記第１の周辺物近似体データとが干渉するか判定するように構成されていることを特徴とする請求項９から１４のいずれか一項に記載の干渉判定システム。
16. 前記周辺物近似体、前記第１の組み合わせ近似体及び前記第２の組み合わせ近似体は、いずれも第１の座標系を構成し、互いに直交する３つの軸のそれぞれに平行な線分により構成される第１の直方体であり、
    前記演算部は、前記第１又は第２の組み合わせ近似体と前記周辺物近似体が干渉すると判定した場合は、その第１又は第２の組み合わせ近似体と、前記周辺物を近似する第２の直方体であって、前記周辺物の形状に応じて、前記第１の座標系を所定の角度で回転させた第２の座標系の各軸に平行な線分により構成され、前記第１の直方体より小さい第２の直方体の近似体と、が干渉するか判定するように構成されていることを特徴とする請求項９から１５のいずれか一項に記載の干渉判定システム。
17. 複数の可動軸を有するロボットが第１の位置から第２の位置に移動する際に、前記ロボットの周辺に存在する周辺物と干渉する可能性を判定するためのコンピュータプログラムであって、
    コンピュータに、
    前記ロボットの形状を、複数の着目部位毎に多面体で近似したロボットデータを記憶させるステップと、
    前記周辺物の形状を近似した周辺物近似体のデータを記憶させるステップと、
    前記第１の位置及び前記第２の位置を取得するステップと、
    前記第１の位置から前記第２の位置へと移動する動作経路上に、少なくとも１つの中間位置を設定するステップと、
    前記ロボットデータに基づいて、前記第１の位置、前記第２の位置及び前記中間位置における前記ロボットの形状を、少なくとも一つの前記着目部位について前記多面体で近似した複数のロボット近似体を生成するステップと、
    少なくとも一つの前記着目部位について、前記第１の位置、前記第２の位置及び前記中間位置に相当する複数の前記ロボット近似体の少なくとも２つを組み合わせて構成される第１の組み合わせ近似体を複数生成し、さらに、これら第１の組み合わせ近似体の少なくとも２つを組み合わせて構成される第２の組み合わせ近似体を生成するステップと、
    この第２の組み合わせ近似体と、前記周辺物近似体とが干渉するか判定するステップと、
    を実行させるためのコンピュータプログラム。
18. 取得部、記憶部及び演算部を備える演算装置を用いて、複数の可動軸を有するロボットが第１の位置から第２の位置に移動する際に、前記ロボットの周辺に存在する周辺物と干渉する可能性を判定する方法であって、
    前記演算部は、
    前記記憶部に、前記ロボットの形状を、複数の着目部位ごとに多面体で近似したロボットデータを記憶させるステップと、
    前記記憶部に、前記周辺物の形状を近似した周辺物近似体のデータを記憶させるステップと、
    前記取得部を用いて、前記第１の位置及び前記第２の位置を取得するステップと、
    前記第１の位置から前記第２の位置へと移動する動作経路上に、少なくとも１つの中間位置を設定するステップと、
    前記ロボットデータに基づいて、前記第１の位置、前記第２の位置及び前記中間位置における前記ロボットの形状を前記複数の着目部位毎に前記多面体で近似した複数のロボット近似体を生成するステップと、
    前記ステップで生成された全ての前記ロボット近似体を組み合わせて構成される総組み合わせ近似体を生成するステップと、
    この総組み合わせ近似体と、前記周辺物近似体とが干渉するか判定するステップと、
    このステップにおいて干渉すると判定された場合は、前記複数の着目部位毎に、前記第１の位置、前記第２の位置及び前記中間位置に相当する複数の前記ロボット近似体の少なくとも２つを組み合わせて構成される複数の部分組み合わせ近似体を生成し、
    複数の前記部分組み合わせ近似体と、前記周辺物近似体とが干渉するかそれぞれ判定するステップと、
    を実行する干渉判定方法。
19. 複数の可動軸を有するロボットが第１の位置から第２の位置に移動する際に、前記ロボットの周辺に存在する周辺物と干渉する可能性を判定するための干渉判定システムであって、
    前記ロボットの形状を複数の着目部位毎に多面体で近似したロボットデータ及び前記周辺物の形状を近似した周辺物近似体のデータを記憶するための記憶部と、
    前記第１の位置及び前記第２の位置を取得するための取得部と、
    前記第１の位置から前記第２の位置へと移動する動作経路上に、少なくとも１つの中間位置を設定し、
    前記ロボットデータに基づいて、前記第１の位置、前記第２の位置及び前記中間位置における前記ロボットの形状を前記複数の着目部位毎に前記多面体で近似した複数のロボット近似体を生成し、
    全ての前記ロボット近似体を組み合わせて構成される総組み合わせ近似体を生成し、
    この総組み合わせ近似体と、前記周辺物近似体とが干渉するか判定し、
    干渉すると判定した場合は、前記複数の着目部位毎に、前記第１の位置、前記第２の位置及び前記中間位置に相当する複数の前記ロボット近似体の少なくとも２つを組み合わせて構成される複数の部分組み合わせ近似体を生成し、
    複数の前記部分組み合わせ近似体と、前記周辺物近似体とが干渉するか判定するように構成される演算部と、
    を備える干渉判定システム。
20. 複数の可動軸を有するロボットが第１の位置から第２の位置に移動する際に、前記ロボットの周辺に存在する周辺物と干渉する可能性を判定するためのコンピュータプログラムであって、
    コンピュータに、
    前記ロボットの形状を、複数の着目部位ごとに多面体で近似したロボットデータを記憶させるステップと、
    前記周辺物の形状を近似した周辺物近似体のデータを記憶させるステップと、
    前記第１の位置及び前記第２の位置を取得するステップと、
    前記第１の位置から前記第２の位置へと移動する動作経路上に、少なくとも１つの中間位置を設定するステップと、
    前記ロボットデータに基づいて、前記第１の位置、前記第２の位置及び前記中間位置における前記ロボットの形状を前記複数の着目部位毎に前記多面体で近似した複数のロボット近似体を生成するステップと、
    前記ステップで生成された全ての前記ロボット近似体を組み合わせて構成される総組み合わせ近似体を生成するステップと、
    この総組み合わせ近似体と、前記周辺物近似体とが干渉するか判定するステップと、
    このステップにおいて干渉すると判定された場合は、前記複数の着目部位毎に、前記第１の位置、前記第２の位置及び前記中間位置に相当する複数の前記ロボット近似体の少なくとも２つを組み合わせて構成される複数の部分組み合わせ近似体を生成するステップと、
    複数の前記部分組み合わせ近似体と、前記周辺物近似体とが干渉するかそれぞれ判定するステップと、
    を実行させるためのコンピュータプログラム。