JP6069923B2 - ロボットシステム、ロボット、ロボット制御装置 - Google Patents

Description

本発明は、ロボットシステム、ロボット、ロボット制御装置等に関する。

物体間の衝突や近接の有無を判定することは多くの分野で必要とされている。例えばロボットなどの分野において、衝突の発生は非常に大きな問題となる。このため、従来より、実際に衝突が発生する以前に、衝突の有無や、許容された以上の近接を、コンピューターによる計算により判定する手法が研究、開発されている。このような衝突判定手法の従来技術としては、例えば特許文献１等に開示される技術が知られている。

特許文献１の手法では、物体がポリゴンデータにより表されており、そのポリゴンデータの各ポリゴンを所定半径の球で覆い、それらの球をより大きな半径の球に統合していき、それらの球のデータを、球の統合関係を表す二分木構造のデータとして構成する。そして、その二分木構造のデータを階層毎に逐次的に衝突判定することにより、物体間の衝突判定を行う。

特開平１１−２５０１２２号公報

特許文献１のようにポリゴンデータを用いて衝突検出を行う手法では、検出対象となる物体のＣＡＤ（Computer Aided Design）データが必要となる。しかしながら、現実的には、ＣＡＤデータが存在しない物体やＣＡＤデータを入手できない物体は多数存在しているため、そのような物体に上記手法を適用することは困難であるという課題がある。

本発明の幾つかの態様によれば、物体のポリゴンデータを入手できない場合等であっても衝突検出可能な衝突検出システム、衝突検出用データ生成装置、ロボットシステム、ロボット、衝突検出用データ生成方法及びプログラム等を提供できる。

本発明の一態様は、物体の衝突検出用データとして、第１の物体に対応する第１の衝突検出用データと第２の物体に対応する第２の衝突検出用データとを記憶する記憶部と、前記第１の衝突検出用データと前記第２の衝突検出用データとに基づいて、ワールド座標系における前記第１の物体と前記第２の物体との衝突判定を行う処理部と、を含み、前記記憶部は、前記物体のモデル座標系において所定の視点から前記物体を見たときの深度マップデータを前記モデル座標系に設定された立方体領域により離散化して得られた代表点データを、前記衝突検出用データとして記憶する衝突検出システムに関係する。

このようにすれば、モデル座標系に設定された立方体領域により物体の深度マップデータが離散化されて得られた代表点データが、衝突検出用データとして記憶部に記憶され、その衝突検出用データに基づいて第１の物体と第２の物体の衝突検出が行われる。これにより、物体のポリゴンデータを入手できない場合等であっても衝突検出を行うことが可能になる。

また本発明の一態様では、前記記憶部は、前記物体を内包するバウンディングボックスの前記代表点データと、前記バウンディングボックスを分割した立方体領域により前記深度マップデータを離散化して得られた前記代表点データである分割代表点データと、を前記衝突検出用データとして記憶し、前記処理部は、前記第１の物体を内包する第１のバウンディングボックスと前記第２の物体を内包する第２のバウンディングボックスとが衝突すると判定した場合に、前記第１の物体の前記分割代表点データと前記第２の物体の前記分割代表点データとに基づいて前記第１の物体と前記第２の物体との衝突判定を行ってもよい。

このようにすれば、バウンディングボックスが非衝突と判定された段階で、第１の物体と第２の物体が非衝突であると確定判定できるため、より小さな立方体領域での衝突判定を省略して処理を簡素化できる。

また本発明の一態様では、記記憶部は、前記衝突検出用データとして木構造のデータを記憶し、前記木構造のデータは、親ノードの立方体領域を分割した複数の立方体領域に対応する前記代表点データを、前記親ノードから分岐する子ノードの前記代表点データとして有してもよい。

このようにすれば、木構造の上位階層のノードから下位階層のノードに向かって階層ごとに順次衝突検出を行うことが可能となる。このような再帰的な処理により、衝突検出を並列処理化できる。

また本発明の一態様では、前記親ノードの立方体領域を分割した前記子ノードの前記複数の立方体領域は、前記所定の視点から見たときの前記親ノードの立方体領域を、２×２領域に分割するとともに、前記２×２領域の各領域を、前記所定の視点の奥行き方向に２分割することで得られた２×２×２個の立方体領域であり、前記木構造のデータは、前記所定の視点から見たときの前記２×２領域の各領域に対応して前記子ノードが設定された４分木構造のデータであり、前記４分木構造における前記子ノードのデータは、前記２×２領域の前記各領域において、前記奥行き方向の２つの立方体領域のうち少なくとも一方に存在する前記代表点データであってもよい。

このような４分木構造のデータに基づいて衝突検出を行うことにより、各階層でのノード対の組み合わせを４×４＝１６個にすることができ、例えばスレッド数が数十程度のＣＰＵ（Central Processing Unit）等を用いて衝突検出システムを実現することが可能になる。

また本発明の一態様では、前記処理部は、前記親ノードのデータに基づく前記衝突判定において、衝突すると判定された前記親ノードが存在する場合には、前記衝突すると判定された前記親ノードから分岐する前記子ノードのデータに基づいて前記衝突判定を行い、前記親ノードのデータに基づく前記衝突判定において、衝突すると判定された前記親ノードが存在しない場合には、前記第１の物体と前記第２の物体が非衝突であると確定判定してもよい。

このようにすれば、木構造の上位階層のノードから下位階層のノードに向かって階層ごとに順次衝突検出を行う再帰的な衝突検出処理を実現することができる。また、ノード対の全ての組み合わせについて非衝突と判定された階層がある場合には、その階層を処理した段階で第１の物体と第２の物体が非衝突であると確定判定できるので、処理を簡素化できる。

また本実施形態では、前記深度マップデータは、前記物体の３次元情報を測定する３次元情報測定装置により生成された深度マップデータであってもよい。

また本発明の他の態様は、物体のモデル座標系における所定の視点から前記物体を見たときの深度マップデータを取得する深度マップデータ取得部と、前記物体の前記モデル座標系における代表点データを前記衝突検出用データとして生成する衝突検出用データ生成部と、を含み、前記衝突検出用データ生成部は、前記物体の前記モデル座標系に設定された立方体領域により前記深度マップデータを離散化することにより、前記代表点データを生成する衝突検出用データ生成装置に関係する。

本発明の他の態様によれば、モデル座標系に設定された立方体領域により物体の深度マップデータが離散化され、その離散化により衝突検出用データとして代表点データが生成される。このようにして衝突検出用データを生成することで、物体のポリゴンデータを入手できない場合であっても衝突検出を行うことが可能になる。

また本発明の他の態様では、前記衝突検出用データ生成部は、親ノードの立方体領域を分割した複数の立方体領域に対応するノードを、子ノードとして前記親ノードに接続し、前記衝突検出用データとして木構造のデータを生成してもよい。

このようにすれば、深度マップデータを離散化した代表点データから木構造のデータを構成できる。また、このような木構造のデータを生成することで、衝突検出において再帰的な並列処理を行うことができる。

また本発明の他の態様では、前記親ノードの立方体領域を分割した前記子ノードの前記複数の立方体領域は、前記所定の視点から見たときの前記親ノードの立方体領域を、２×２領域に分割するとともに、前記２×２領域の各領域を、前記所定の視点の奥行き方向に２分割することで得られた２×２×２個の立方体領域であり、前記衝突検出用データ生成部は、前記所定の視点から見たときの前記２×２領域の各領域に対応して前記子ノードが設けられた４分木構造のデータを、前記木構造のデータとして生成し、前記４分木構造のデータにおける前記子ノードのデータは、前記２×２領域の前記各領域において、前記奥行き方向の２つの立方体領域のうち少なくとも一方に存在する前記代表点データであってもよい。

このようにすれば、所定の視点から見たときの２×２領域の各領域に対応して子ノードを設定することで、深度マップデータを立方体領域により離散化した代表点データから４分木構造のデータを構成することができる。

また本発明の他の態様では、前記衝突検出用データ生成部は、処理対象の代表点と、前記処理対象の代表点の周囲２６近傍の立方体領域の外側に存在する代表点との間に、前記代表点データが欠落する立方体領域があると判断した場合に、前記代表点データが欠落する立方体領域に、前記代表点データを補完してもよい。

このようにして衝突検出用データを生成することで、代表点データが欠落する立方体領域において実際には衝突可能性があるにも関わらず非衝突と判定されてしまうような誤検出を抑制することが可能になる。

また本発明の他の態様では、前記所定の視点の奥行き方向に遠ざかるほど深度値が大きくなる場合に、前記衝突検出用データ生成部は、前記処理対象の代表点の代表深度値から、前記処理対象の代表点の周囲に存在する代表点の代表深度値を減算した差分値が、負であると判定した場合に、前記処理対象の代表点に対して前記奥行き方向側の立方体領域に、前記代表点データを補完してもよい。

このようにすれば、差分値が負である場合には、その代表点が処理対象の代表点よりも奥行き方向側に存在することになるため、処理対象の代表点データに対して奥行き方向側の立方体領域に代表点データを補完できる。

また本発明の他の態様では、前記深度マップデータは、前記物体の３次元情報を測定する３次元情報測定装置により生成された深度マップデータであってもよい。

また本発明の更に他の態様は、可動部を有するロボットと、物体の衝突検出用データとして、第１の物体に対応する第１の衝突検出用データと第２の物体に対応する第２の衝突検出用データとを記憶する記憶部と、前記第１の衝突検出用データと前記第２の衝突検出用データとに基づいて、ワールド座標系における前記第１の物体と前記第２の物体との衝突判定を行う処理部と、前記処理部により行われた前記衝突判定の結果に基づいて前記可動部の動作を制御する制御部と、を含み、前記記憶部は、前記物体のモデル座標系において所定の視点から前記物体を見たときの深度マップデータを前記モデル座標系に設定された立方体領域により離散化して得られた代表点データを、前記衝突検出用データとして記憶するロボットシステムに関係する。

また本発明の更に他の態様は、可動部と、物体の衝突検出用データとして、第１の物体に対応する第１の衝突検出用データと第２の物体に対応する第２の衝突検出用データとを記憶する記憶部と、前記第１の衝突検出用データと前記第２の衝突検出用データとに基づいて、ワールド座標系における前記第１の物体と前記第２の物体との衝突判定を行う処理部と、前記処理部により行われた前記衝突判定の結果に基づいて前記可動部の動作を制御する制御部と、を含み、前記記憶部は、前記物体のモデル座標系において所定の視点から前記物体を見たときの深度マップデータを前記モデル座標系に設定された立方体領域により離散化して得られた代表点データを、前記衝突検出用データとして記憶するロボットに関係する。

また本発明の更に他の態様は、物体のモデル座標系における所定の視点から前記物体を見たときの深度マップデータを取得し、前記物体の前記モデル座標系に設定された立方体領域により前記深度マップデータを離散化し、前記離散化して得られる代表点データを前記衝突検出用データとして生成する衝突検出用データ生成方法に関係する。

また本発明の更に他の態様は、物体のモデル座標系における所定の視点から前記物体を見たときの深度マップデータを取得する深度マップデータ取得部と、前記物体の前記モデル座標系における代表点データを前記衝突検出用データとして生成する衝突検出用データ生成部として、コンピューターを機能させ、前記衝突検出用データ生成部は、前記物体の前記モデル座標系に設定された立方体領域により前記深度マップデータを離散化することにより、前記代表点データを生成するプログラムに関係する。

図１（Ａ）は、本実施形態の衝突検出用データ生成装置の構成例。図１（Ｂ）は、本実施形態の衝突検出システムの構成例。 図２（Ａ）は、本実施形態の衝突検出システムを含むロボットシステムの例。図２（Ｂ）は、本実施形態の衝突検出システムを含むロボットの例。 衝突検出用データの生成手法についての説明図。 衝突検出用データの生成手法についての説明図。 衝突検出用データの生成手法についての説明図。 衝突検出用データの生成手法についての説明図。 衝突検出用データの生成手法についての説明図。 衝突検出用データの生成手法についての説明図。 図９（Ａ）〜図９（Ｃ）は、本実施形態の衝突検出用データ生成装置が生成する４分木構造のデータの例。 図１０（Ａ）、図１０（Ｂ）は、本実施形態の衝突検出用データ生成装置が生成する４分木構造のデータの例。 図１１（Ａ）〜図１１（Ｃ）は、本実施形態の衝突検出用データ生成装置が生成する４分木構造のデータの例。 衝突検出の手法についての説明図。 図１３（Ａ）〜図１３（Ｃ）は、衝突検出の手法についての説明図。 衝突検出の手法についての説明図。 衝突検出の手法についての説明図。 衝突検出の手法についての説明図。 本実施形態の衝突検出用データ生成装置の詳細な構成例。 衝突検出用データ生成処理のフローチャート。 １階層分のデータ生成処理の詳細なフローチャート。 ４分木構造生成処理の詳細なフローチャート。 本実施形態の衝突検出システムの詳細な構成例。 衝突検出処理のフローチャート。 再帰的ノード対衝突検出処理の詳細なフローチャート。

以下、本発明の好適な実施の形態について詳細に説明する。なお以下に説明する本実施形態は特許請求の範囲に記載された本発明の内容を不当に限定するものではなく、本実施形態で説明される構成の全てが本発明の解決手段として必須であるとは限らない。

１．構成
ロボット（マニピュレーター）の動作においては、周辺構造物や周辺機器への衝突や、自己衝突や、他のロボットへの衝突は非常に大きな問題となる。本実施形態の衝突検出手法では、このような衝突をシミュレーションにより事前に検出する。

このような本実施形態の衝突検出手法が用いられる形態としては、大別して、オフラインでの使用（事前確認）と、ラインタイムでの使用（予測、先読み）が考えられる。オフラインの使用では、周辺環境などが既知且つ静的であり、ロボットの動作が既知である場合に、システムの作成時に、その衝突を検証する。一方、ラインタイムの使用では、周辺環境などが動的に変化する場合（例えば周囲に複数のロボットが存在したり、作業者が存在する場合）に、ロボットの実際の動作に先行して、シミュレーションにより衝突を検出する。

従来では、このようなロボットにおける衝突検出手法として、物体（オブジェクト）のポリゴンデータを入手可能であることを前提としたアルゴリズムを使用する場合が多かった。ポリゴンデータは、物体の構造を設計する際等に物体の形状を多数のポリゴンの組み合わせによって表現したＣＡＤデータである。しかしながら、衝突検出の対象となる物体は多種多様であり、それらの全てについてＣＡＤデータを入手することは、現実的には困難であるという課題がある。

また、ポリゴンデータを使用する従来の衝突検出手法には、上述した特許文献１のように、各ポリゴンについて二分木構造の球データを生成し、そのデータを使用して衝突検出を行う手法がある。しかしながら、このような手法では、大量の無駄な衝突検出用データや無駄な衝突検出処理が発生するため、効率的な衝突検出を行うことが困難である。

具体的には、現実の物体を記述したポリゴンデータは、必ずしも、衝突検出において重要な物体表面のポリゴンデータのみを含むわけではない。衝突検出において重要でないポリゴンデータとして、物体内部を表現するポリゴンデータや、物体の外部からは見えない部分を表現するポリゴンデータ等を大量に含んでいる。このようなポリゴンデータからポリゴン単位で球データを生成すると、二分木構造のデータには、衝突検出に関係しない球データが大量に含まれることになる。特許文献１では、このような衝突検出において重要でないポリゴンデータへの対処法について無言であり、衝突検出に関係しない球データについても衝突検出処理が行われることで非効率な処理となってしまう。

また、現実の物体を記述したポリゴンデータには、非常に多岐にわたる大きさのポリゴンが含まれており、これらを同一のアルゴリズムで扱うことは効率的ではない。例えば、ロボットのパーツや、ロボットが取り扱う道具類には細い棒上の物体が多く、このような物体は微小なポリゴンで表現される。また、物体の複雑な形状等にも、多数の微小なポリゴンが使用されている。ポリゴンを覆う球の大きさは近接の許容範囲程度の大きさがあればよいと考えられるので、それよりも小さいポリゴンで表現される構造を木構造のデータで表現した場合、データが非常に冗長となる。そうすると、微小な構造が衝突検出で重要となることは少ないにも関わらず、微小な構造によって無駄な衝突検出処理が大量に発生してしまう。

図１（Ａ）に、以上のような問題を解決できる本実施形態の衝突検出用データ生成装置の構成例を示す。なお本実施形態の衝突検出用データ生成装置の構成は図１（Ａ）の構成には限定されず、その一部の構成要素（例えば操作部、外部Ｉ／Ｆ部等）を省略したり、他の構成要素を追加するなどの種々の変形実施が可能である。

この衝突検出用データ生成装置は、処理部１１０と、記憶部１５０を含む。また衝突検出用データ生成装置は、操作部１７０、外部Ｉ／Ｆ（インターフェース）部１８０、情報記憶媒体１９０を含むことができる。この衝突検出用データ生成装置は例えば情報処理装置によって構成され、その情報処理装置のハードウェアーやプログラムにより衝突検出用データ生成装置が実現される。

処理部１１０は、各種のデータ生成処理や制御処理などを行うものであり、例えばＣＰＵ等の各種のプロセッサー、専用回路（ＡＳＩＣ）などのハードウェアーや、プロセッサー上で実行されるプログラムなどにより実現できる。処理部１１０は、深度マップデータ取得部１１２、衝突検出用データ生成部１１４を含む。

深度マップデータ取得部１１２は、衝突検出用データを生成するための深度マップデータを取得する処理を行う。ここで深度マップとは、所定の視点（例えば無限遠の視点）から見た場合における物体の深度値により表されるマップであり、ピクセル単位の深度値がマトリクス配列されたマップである。深度マップデータ取得部１１２には、例えば情報記憶媒体１９０に予め保存されたＣＡＤデータが入力され、あるいは外部Ｉ／Ｆ部１８０を介して不図示の三次元情報測定装置（例えば３Ｄスキャナー）からの測定情報が入力される。そして、深度マップデータ取得部１１２は、入力されたＣＡＤデータや測定情報から深度マップデータを生成する。

衝突検出用データ生成部１１４は、衝突検出処理に用いるためのデータを深度マップデータから生成する処理を行う。具体的には、図３等で後述するように、深度マップデータにおける位置及び深度値を立方体領域で離散化し、物体の表面を覆う離散化された代表値データを衝突検出用データとして生成する。衝突検出用データ生成部１１４は、立方体領域のサイズを順次分割（又は統合）しながら代表点データを生成し、その分割（又は統合）の従属関係を表した木構造のデータを構成する。生成された衝突検出用データは、情報記憶媒体１９０に格納される。

記憶部１５０は、処理部１１０などのワーク領域となるもので、ＲＡＭ（ＳＲＡＭ、ＤＲＡＭ等）などのメモリーにより実現できる。操作部１７０は、使用者が各種の操作情報を入力するためのものである。外部Ｉ／Ｆ部１８０は、外部との間で有線や無線で情報の通信処理等を行うものである。情報記憶媒体１９０は（コンピューターにより読み取り可能な媒体）は、プログラムやデータなどを格納するものであり、その機能は、光ディスク、ＨＤＤ、或いはメモリーなどにより実現できる。処理部１１０は、情報記憶媒体１９０に格納されるプログラム（データ）に基づいて本実施形態の種々の処理を行う。即ち情報記憶媒体１９０には、本実施形態の各部としてコンピューター（操作部、処理部、記憶部、出力部を備える装置）を機能させるためのプログラム（各部の処理をコンピューターに実行させるためのプログラム）が記憶される。

図１（Ｂ）に、上述した問題を解決できる本実施形態の衝突検出システムの構成例を示す。なお本実施形態の衝突検出システムの構成は図１（Ｂ）の構成には限定されず、その一部の構成要素（例えば操作部、外部Ｉ／Ｆ部等）を省略したり、他の構成要素を追加するなどの種々の変形実施が可能である。

この衝突検出システムは、処理部１０と、記憶部５０を含む。また衝突検出システムは、操作部７０、外部Ｉ／Ｆ（インターフェース）部８０、情報記憶媒体９０を含むことができる。

記憶部５０は、処理部１０などのワーク領域となるもので、ＲＡＭ（ＳＲＡＭ、ＤＲＡＭ等）などのメモリーにより実現できる。この記憶部５０は、代表点データ記憶部５２を含む。

代表点データ記憶部５２は、衝突検出用データ生成装置により生成された衝突検出用データを記憶する。例えば、衝突検出用データ生成装置は衝突検出システムと別体に構成され、衝突検出用データは外部Ｉ／Ｆ部８０を介して情報記憶媒体９０に記憶される。そして、衝突検出処理を実行する際には、処理部１０は、情報記憶媒体９０の衝突検出用データを記憶部５０のＲＡＭに展開し、そのＲＡＭ上のデータを参照して衝突検出処理を行う。なお、衝突検出用データ生成装置は衝突検出システムと一体に構成されてもよい。この場合、深度マップデータ取得部１１２と衝突検出用データ生成部１１４は処理部１０に含まれ、処理部１０により生成された衝突検出用データが情報記憶媒体９０に格納される。

処理部１０は、各種の判定処理や制御処理などを行うものであり、例えばＣＰＵ等の各種のプロセッサー、専用回路（ＡＳＩＣ）などのハードウェアーや、プロセッサー上で実行されるプログラムなどにより実現できる。処理部１０は、オブジェクト空間設定部１２、衝突判定部１４を含む。

オブジェクト空間設定部１２は、複数の物体をオブジェクト空間に配置設定する処理等を行う。具体的には、ワールド座標系での物体の位置や回転角度を決定し、その位置にその回転角度で物体を配置する。ここでワールド座標系とは、衝突検出処理を行う空間に設定された座標系であり、衝突検出対象の物体に対して共通に設定された座標系である。また物体（オブジェクト）とは、ロボット等の衝突検出対象物や、周辺構造物や周辺機器などの被衝突検出対象物を、モデル化したものである。本実施形態では、各物体に対してそれぞれモデル座標系が設定されており、そのモデル座標系における代表点データにより物体が表現される。オブジェクト空間設定部１２は、このモデル座標系における代表点データの座標を、ワールド座標系における座標に変換することで、ワールド座標系に複数の物体を配置する。

衝突判定部１４は、衝突検出対象の物体（第１の物体）と被衝突検出対象の物体（第２の物体）との間の衝突判定処理を行う。具体的には、図８以降で後述するように、立方体領域のサイズが大きい上位階層の代表点データで衝突判定を行い、衝突する（衝突の可能性がある）と判定されたノードが存在する場合には、そのノードの子ノードの代表点データで衝突判定を行う。最下位階層で衝突すると判定されたノードが存在する場合には、衝突と確定判定し、最下位階層よりも上位の階層で非衝突（衝突の可能性が無い）と判定された場合には、その非衝突と判定された階層よりも下位階層の衝突判定を行わず、非衝突と確定判定する。

操作部７０は、使用者が各種の操作情報を入力するためのものである。外部Ｉ／Ｆ部８０は、外部との間で有線や無線で情報の通信処理等を行うものである。情報記憶媒体９０は（コンピューターにより読み取り可能な媒体）は、プログラムやデータなどを格納するものであり、その機能は、光ディスク、ＨＤＤ、或いはメモリーなどにより実現できる。処理部１０は、情報記憶媒体９０に格納されるプログラム（データ）に基づいて本実施形態の種々の処理を行う。即ち情報記憶媒体９０には、本実施形態の各部としてコンピューター（操作部、処理部、記憶部、出力部を備える装置）を機能させるためのプログラム（各部の処理をコンピューターに実行させるためのプログラム）が記憶される。

以上のように、深度マップデータを立方体領域で離散化して衝突検出用データを生成することで、ＣＡＤデータを入手できない物体であっても衝突検出が可能となる。また、代表点データは、衝突検出に重要な物体表面を表すデータのみで構成されるため、効率的な衝突検出処理が可能となる。また、代表点データは、ポリゴンの大きさに左右されない非冗長なデータであるため、無駄な衝突検出処理を抑制できる。

図２（Ａ）に本実施形態の衝突検出システムを含むロボットシステムの例を示す。このロボットシステムは、制御装置３００（情報処理装置）とロボット３１０とを含む。制御装置３００はロボット３１０の制御処理を行う。具体的には、動作シーケンス情報（シナリオ情報）に基づいてロボット３１０を動作させる制御を行う。ロボット３１０は、アーム３２０及びハンド（把持部）３３０等の可動部を有する。そして制御装置３００からの動作指示にしたがって可動部が動作する。例えば、図示しないパレットに載せられたワークを把持したり、移動するなどの動作を行う。また、図示しない撮像装置で取得された撮像画像情報に基づいて、ロボットの姿勢やワークの位置などの情報が検出され、検出された情報が制御装置３００に送られる。

ここで可動部とは、可動部が動作することにより物体間の相対的な距離や姿勢を変化（又は移動）させるものである。例えば、本実施形態のロボット３１０はロボットアーム３２０やハンド３３０を有しており、そのロボットアーム３２０やハンド３３０を動かして作業を行う場合、そのロボットアーム３２０やハンド３３０を構成する各パーツや、そのパーツをジョイントする関節が可動部に相当する。この例では、例えばハンド３３０が物体を保持（或は把持、吸着等）しており、ロボットアーム３２０やハンド３３０が動くことによって、ハンド３３０が保持する物体とロボット３１０周辺の物体（例えば構造物や設置物、部品等）とが相対的に移動する。或は、ロボットアーム３２０やハンド３３０が動くことによって、ロボットアーム３２０やハンド３３０を構成するパーツとロボット３１０周辺の物体とが相対的に移動したり、或は、ロボットアーム３２０やハンド３３０の関節でジョイントされたパーツとパーツとが相対的に移動する。本実施形態では、このように可動部によって移動された物体間の衝突を検出している。

本実施形態の衝突検出システムは例えば図２（Ａ）の制御装置３００に設けられ、例えば制御装置３００のハードウェアーやプログラムにより衝突検出システムが実現される。そして、ラインタイムの使用では、周辺環境などが動的に変化する場合に、ロボット３１０の実際の動作に先行して、本実施形態の衝突検出システムがシミュレーションにより衝突の判定処理を行う。そして制御装置３００は、ロボット３１０が周辺構造物や周辺機器などに衝突しないように、判定処理の結果に基づいてロボット３１０の制御を行う。一方、オフラインの使用では、本実施形態の衝突検出システムにより、動作シーケンス情報等の作成時にシミュレーションにより衝突を検証する。そして制御装置３００は、衝突が起こらないように作成された動作シーケンス情報（シナリオ情報）に基づいて、ロボット３１０を制御する。

なお図２（Ａ）はロボット３１０と制御装置３００とが別体として存在するロボットシステムの例であるが、本実施形態では、制御装置３００がロボット３１０に内蔵されたロボットであってもよい。

図２（Ｂ）に、本実施形態の衝突検出システムを含むロボットの例を示す。このロボットは、ロボット本体３１０（アーム３２０及びハンド３３０を有する）と、ロボット本体３１０を支えるベースユニット部と、を含み、そのベースユニット部に制御装置３００が格納される。図２（Ｂ）のロボットでは、ベースユニット部に車輪等が設けられ、ロボット全体が移動可能な構成となっている。なお、図２（Ａ）は単腕型の例であるが、ロボットは図２（Ｂ）に示すように双腕型等の多腕型のロボットであってもよい。なお、ロボットの移動は人手で行われてもよいし、車輪を駆動するモーターを設け、そのモーターを制御装置３００により制御することで行なってもよい。

２．衝突検出用データの生成手法
次に、本実施形態における衝突検出用データの生成手法について説明する。なお図３〜図７では、モデル座標系は物体ＯＢに対して設定されており、そのモデル座標系を右手系の直交ＸＹＺ座標で表すものとする。

図３に示すように、深度マップデータ取得部１１２は、物体ＯＢを所定の視点（視線方向）から見たときの深度マップデータＺＤを取得する。所定の視点は、例えば、＋Ｚ方向側、−Ｚ方向側、＋Ｘ方向側、−Ｘ方向側、＋Ｙ方向側、−Ｙ方向側から物体ＯＢを見る６つの視点（視線方向）である。深度マップデータ取得部１１２は、その６つの視点のそれぞれについて深度マップデータを取得する。図３には、例として＋Ｚ方向側の視点から見たときの深度マップデータＺＤのＸＺ平面における断面図を示す。この例では、モデル座標系のＺ軸に沿って深度マップデータの深度値が変化し、モデル座標系のＸ軸、Ｙ軸に沿って深度マップデータ上の位置（ピクセル位置）が変化する。

衝突検出用データ生成部１１４は、Ｘ座標、Ｙ座標、Ｚ座標を所定の間隔で離散化し、モデル座標系が設定された空間（以下では適宜、モデル空間と呼ぶ）を、立方体領域ＣＡに離散化する。この立方体領域ＣＡは、モデル空間及び物体ＯＢに対して固定された領域であり、上記の６つの視点のいずれから見た場合にも同一位置に設定されている。

衝突検出用データ生成部１１４は、離散化したモデル空間を方向ＤＳ（例えば＋Ｘ方向）に沿ってスキャンし、物体ＯＢの表面（外郭）に対応する立方体領域ＣＡＨに代表点ＰＡを設定する。具体的には、同一ＸＹ座標の立方体領域の列ＤＩＲに注目した場合、Ａ１に示すように、列ＤＩＲを視点から見たときに最初に物体ＯＢに交差する立方体領域に代表点を設定する。そして、この処理を方向ＤＳに沿って移動しながら順次行い、視点から見たときの物体ＯＢの表面に代表点ＰＡを設定する。代表点は、例えば立方体領域の重心に設定され、その重心のＸＹＺ座標によって表される。深度マップデータにおいて最背面に対応する領域には、代表点ＰＡを設定しない。最背面は、深度値で表現できる深度範囲において視点から最遠の深度となる面である。

ここでモデル空間での離散化は、深度マップデータにおいて深度値とその位置を離散化することに相当し、モデル空間の代表点に対応して、深度マップデータにおける代表深度値と代表位置が決定されることになる。図３の例では、代表点のＺ座標と代表深度値とが対応し、代表点のＸＹ座標と代表位置とが対応する。

図４に示すように、衝突検出用データ生成部１１４は、＋Ｘ方向のスキャンを例えば＋Ｙ方向に順次移動しながら行い、離散化されたＸＹ座標の全てについて代表点ＰＡの設定処理を行う。図４には、設定した代表点ＰＡの一部のみを図示しているが、スキャンが終了すると物体ＯＢを覆うように代表点ＰＡが設定される。

さて、図５のＢ１に示すように、上述した代表点の設定処理を行っても物体ＯＢの表面を代表点で覆いきれていない場合がある。これは、同一のＸＹ座標をもつ立方体領域のうち１つの立方体領域にのみ代表点を設定するためである。このような代表点が欠落したデータを用いて衝突判定を行った場合、Ｂ１に示す立方体領域に対して−Ｘ方向側から他の物体が接近すると、衝突の可能性を正しく検出できない可能性がある。

そのため図６のＥ１に示すように、衝突検出用データ生成部１１４は、物体ＯＢの表面が連続的に覆われるように代表点を補完（補充）する処理を行う。具体的には、図５のＢ２に示す代表点と立方体領域に注目した（処理対象とする）場合、Ｂ３に示す周囲２６近傍の立方体領域で代表点が欠落しているか否かを判定する。即ちＢ４に示すように、視点から周囲２６近傍の立方体領域を見たときに、その周囲２６近傍の立方体領域よりも遠方側（奥行き方向側、図５では−Ｚ方向側）に代表点が存在する場合、Ｂ２に示す立方体領域の遠方側に隣接するＢ１に示す立方体領域に代表点を補完する。ここで周囲２６近傍の立方体領域とは、注目する立方体領域を囲む直近の２６（＝３×３×３−１）個の立方体領域である。図５では、２次元の断面図を図示しているため８近傍となっているが、３次元では、注目する立方体領域に対して±Ｙ方向側に存在する立方体領域を合わせて２６近傍となる。

衝突検出用データ生成部１１４は、上記のような補完処理を方向ＤＳ（例えば＋Ｘ方向）にスキャンしながら行い、図５のＢ５、Ｂ６に示すように代表点が欠落した立方体領域に対して、図６のＥ２、Ｅ３に示すように代表点を補完する。そして、方向ＤＳのスキャンを例えば＋Ｙ方向に順次移動しながら行い、離散化されたＸＹ座標の全てについて補完処理を行う。

以上のようにして、最終的に物体ＯＢの表面が連続的に覆われた代表点データを生成することで、物体ＯＢに対する衝突可能性を正しく検出できるようになる。なお、注目する代表点（図５のＢ２）の遠方側（Ｂ１）に代表点を補完するのは、物体ＯＢの凸部を太らせないためである。仮に注目する代表点よりも視点に近い側に代表点を補完したとすると、例えばＢ６に示す立方体領域ではなくＢ７に示す立方体領域に代表点が補完される。そうすると、実際の凸部よりも、代表点が表現する凸部の方が無駄に太くなってしまい、実際には凸部に衝突しないにも関わらず衝突可能性があると判定される可能性がある。本実施形態では注目する代表点の遠方側に代表点を補完するので、凸部が太らず、正確な衝突判定が可能である。

ここで図５では、Ｂ２の立方体領域とＢ４の立方体領域の間隔が立方体領域１個分である場合を例に説明したが、間隔が立方体領域２個分以上である場合には、その２個分以上の立方体領域に対して代表点を補完する。例えばＢ４に示す代表点が仮にＢ８に示す位置に存在した場合、Ｂ１に示す立方体領域に加えてＢ９に示す立方体領域にも代表点を補完する。

図７には、＋Ｘ方向側の視点から見たときの深度マップデータＺＤ’のＸＺ平面における断面図である。図７では、モデル座標系のＸ軸に沿って深度マップデータの深度値が変化し、モデル座標系のＹ軸、Ｚ軸に沿って深度マップデータにおける位置（ピクセル位置）が変化する。

図７に示すように、衝突検出用データ生成部１１４は、＋Ｚ方向側から見た視点以外の視点についても、上述した手法により深度マップデータＺＤ’から代表点データを生成する。このとき、図７のＦ１に示す代表点のように、図６のＥ４に示す他の視点での代表点と重複する場合がある。衝突検出用データ生成部１１４は、Ｆ１に示すような重複する代表点を削除し、＋Ｘ方向側から見た視点での最終的な代表点データを生成する。そして、６つの視点で生成した代表点データを合わせて、あらゆる視点から見て物体ＯＢの表面が代表点で覆われるような衝突検出用データを生成する。

図８に示すように、衝突検出用データ生成部１１４は、立方体領域のサイズを変えてモデル空間を離散化し、その立方体領域ＣＢに対応する代表点ＰＢのデータを生成する。立方体領域ＣＢのサイズは、立方体領域ＣＡの辺の長さを２倍にしたサイズであり、立方体領域ＣＢを２×２×２分割した各領域が立方体領域ＣＡに対応している。衝突検出用データ生成部１１４は、このようにして立方体領域のサイズを順次大きくしてデータ生成処理を行い、各サイズに対応する代表点データを生成し、それらを統合して４分木構造のデータを生成する。４分木において最上位の立方体領域は、物体ＯＢを内包する立方体領域（バウンディングボックス）である。また４分木において最下位階層の立方体領域のサイズは、例えばロボット等の衝突検出での許容誤差程度（例えば数ｃｍ）にすればよい。

以上で説明した階層毎の処理において１階層分の処理は、深度マップを３次元ベクトル量子化することに相当する。また、衝突検出用データは、３次元ベクトル量子化の量子化刻みが異なるデータを、４分木構造に統合したものに相当する。３次元ベクトル量子化における量子化刻みは、立方体領域のサイズに対応しており、量子化刻みが同じデータを同一階層のデータとして４分木構造に統合する。

なお、立方体領域ＣＡ、ＣＢのサイズ比は２倍に限定されず、例えば３倍や４倍等であってもよい。また上記では、立方体領域のサイズを順次大きくしてデータ生成する場合を例に説明したが、本実施形態はこれに限定されず、立方体領域のサイズを順次小さく（例えば１／２倍に）しながらデータ生成してもよい。

３．データ構成例
図９（Ａ）〜図１１（Ｃ）に、本実施形態の衝突検出用データ生成装置が生成する４分木構造のデータの例。なお以下では、＋Ｚ方向側から見た視点を所定の視点として３階層のデータを生成する場合を例に説明する。

図９（Ａ）は、＋Ｚ方向側から見たときの立方体領域の分割関係を表した図である。領域Ａ〜Ｖは、立方体領域をＸＹ平面に対して垂直に投影した領域を表す。領域Ａは、物体ＯＢを内包する立方体領域に対応している。そして、物体ＯＢを内包する立方体領域が２×２×２分割され、その各立方体領域が更に２×２×２分割されるに従って、領域Ａが領域Ｂ〜Ｄに２×２分割され、領域Ｂ〜Ｄがそれぞれ領域Ｆ〜Ｉ、Ｊ〜Ｍ、Ｏ〜Ｒ、Ｓ〜Ｖに２×２分割される。

図９（Ｂ）に示すように、領域Ａ〜Ｖの分割関係に従って代表点データを４分木構造に構成する。即ち、４分木構造の最上位（ルート）のノードＡには、図９（Ａ）の領域Ａに対応した物体ＯＢを内包する立方体領域の代表点データを設定する。そして、ノードＡを親ノードとする子ノードＢ〜Ｅには、図９（Ａ）の領域Ｂ〜Ｅに存在する代表点データを設定する。ここで、領域Ｂ〜Ｅには、それぞれＺ方向に２つの立方体領域が並んでおり、そのＺ方向に並んだ２つの立方体領域のうち基本的には一方に代表点が設定されている。例えば、図８のＧ１に示す代表点が設定された立方体領域を分割すると、図３のＡ２に示す立方体領域となる。図３のＡ２に示す立方体領域を視点から見たとき、Ｚ方向に並ぶ２つの立方体領域のうち一方に代表点が設定されている。ノードＢ〜Ｅには、基本的には、このような２つの立方体領域のうち一方に存在する代表点データが設定される。同様にしてノードＢ〜Ｅをそれぞれ親ノードとする子ノードＦ〜Ｉ、Ｊ〜Ｍ、Ｏ〜Ｒ、Ｓ〜Ｖには、図９（Ａ）の領域Ｆ〜Ｉ、Ｊ〜Ｍ、Ｏ〜Ｒ、Ｓ〜Ｖに存在する代表点データを設定する。

図９（Ｃ）に、各ノードのデータ構成例を示す。なお簡単のためノードＣ〜Ｅの子ノードの図示を省略した。図９（Ｃ）に示すように、ノードＡ〜Ｖは、ノードＡｘｙ〜Ｖｘｙと、そのノードＡｘｙ〜Ｖｘｙに従属するサブノードＡｚ〜Ｖｚにより構成される。ノードＡｘｙ〜Ｖｘｙには、代表点のＸＹ座標が記憶され、サブノードＡｚ〜Ｖｚには、代表点の深度値（Ｚ値）が記憶される。±Ｘ、±Ｙ方向側から見た視点での代表点データの場合には、深度値はＸ、Ｙ方向に変化するが、深度値がＺ方向に変化するように適当な座標変換を行って４分木を構成する。あるいは、±Ｘ、±Ｙ方向側から見た視点での代表点データにおいて、ノードＡｘｙ〜Ｖｘｙに代表点のＹＺ、ＺＸ座標を記憶し、サブノードＡｚ〜ＶｚにＸ、Ｙ方向における深度値を記憶してもよい。

ここで、上述のような親ノードに対して複数の子ノードが接続される分木構造において、そのノードの親子関係における世代をデータの階層と呼ぶ。即ち、ノードの親子関係において同一世代のノードは、同一階層のノードとなる。例えば図９（Ｂ）の例では、ルートノードＡが１つの階層を構成し、そのルートノードＡの子ノードＢ〜Ｅが１つの階層を構成する。そして、子ノードＢ〜Ｅを親ノードとする子ノードＦ〜Ｖ（ルートノードＡからみて孫世代のノード）が、更に１つの階層を構成する。

図１０（Ａ）に、図６等で説明した代表点補完処理を行った場合のデータ構成例を示す。例えばノードＣの代表点Ｃａに対して、同一ＸＹ座標で深度値が異なる２つの代表点Ｃｂ、Ｃｃが補完されているとする。この場合、それら同一ＸＹ座標の３つの代表点データＣａ、Ｃｂ、Ｃｃを１つのノードＣに設定する。

具体的には、図１０（Ｂ）に示すように、代表点データＣａ、Ｃｂ、Ｃｃの共通のＸＹ座標をノードＣｘｙに記憶し、そのノードＣｘｙに従属するサブノードＣａｚ、Ｃｂｚ、Ｃｃｚにそれぞれ代表点データＣａ、Ｃｂ、Ｃｃの深度値を記憶する。ノードＣｘｙに接続されるサブノードＣａｚ、Ｃｂｚ、Ｃｃｚは、例えばリスト構造で構成することが想定される。このようにデータを構成すれば、ノードＡｘｙ〜Ｖｘｙは必ず４分木で接続されるので、補完データが存在しても４分木構造のデータを構成できる。なお図１０（Ｂ）では簡単のためノードＣ〜Ｅの子ノードの図示を省略した。

図１１（Ａ）は、＋Ｚ方向側から見たときの物体ＯＢＸと立方体領域を表した図である。図１１（Ａ）の例では、領域Ｅを分割した領域Ｓ〜Ｖのうち、物体ＯＢＸに交差する領域はＳのみである。そのため図１１（Ｂ）に示すように、ノードＥに接続される子ノードＳ〜Ｖのうち、代表点が存在するのはノードＳのみである。

この場合、図１１（Ｃ）に示すように、ノードＳｘｙ、サブノードＳｚにはそれぞれ代表点のＸＹ座標、深度値が記憶される。ノードＴｘｙ〜Ｖｘｙには代表点は存在しないが、代表点が設定される位置のＸＹ座標が記憶される。サブノードＴｚ〜Ｖｚには深度値が記憶されず、ノードＴｘｙ〜ＶｘｙにはＮＵＬＬリストが接続されることになる。例えば、図１１（Ｃ）においてノードＡｘｙ〜ＶｘｙからサブノードＡｚ〜Ｖｚを指す矢印が、実装において例えばポインターで実現される場合、サブノードＴｚ〜Ｖｚを指す矢印は例えばＮＵＬＬポインターで実現される。このようにデータを構成すれば、ノードＡｘｙ〜Ｖｘｙは必ず４分木で接続されるので、代表点が存在しないノードが存在しても４分木構造のデータを構成できる。なお図１１（Ｃ）では簡単のためノードＣ〜Ｅの子ノードの図示を省略した。

４．衝突検出の手法
次に、図１（Ｂ）で説明した衝突検出システムが行う衝突検出の手法について説明する。図１２に、衝突判定を行う対象である第１の物体ＯＢ１と第２の物体ＯＢ２の、ワールド座標系における断面図を示す。物体ＯＢ１、ＯＢ２は、例えばロボット等のパーツや、そのパーツ間を接続する関節部分や、ロボット等の作業空間に配置された構造物などである。

図１２に示すように、衝突判定部１４は、まず衝突検出用データのうち立方体領域のサイズが最大の代表点データ（即ち、４分木構造のルートデータ）を用いて衝突判定を行う。具体的には、オブジェクト空間設定部１２が、４分木構造のルートの代表点と立方体領域を、物体ＯＢ１、ＯＢ２のモデル座標系からワールド座標系に座標変換する。そして、衝突判定部１４は、物体ＯＢ１、ＯＢ２の立方体領域ＢＡ１、ＢＡ２がワールド座標系において交差するか否かを判断する。

具体的には図１３（Ａ）に示すように、衝突判定部１４は、物体ＯＢ１の代表点ＤＰ１と物体ＯＢ２の代表点ＤＰ２との距離ＤＳを求める。ここで、物体ＯＢ１の立方体領域ＢＡ１の一辺の長さをＳＩ１とし、物体ＯＢ２の立方体領域ＢＡ２の一辺の長さをＳＩ２とする。衝突判定部１４は、ＤＳ＞√３×（ＳＩ１＋ＳＩ２）を満たすと判定した場合には、立方体領域ＢＡ１を内包する球ＫＹ１と立方体領域ＢＡ２を内包する球ＫＹ２とが非交差であると判断する。この場合、立方体領域ＢＡ１、ＢＡ２は非衝突であると確定判定する。

一方、図１３（Ｂ）に示すように、ＤＳ＞√３×（ＳＩ１＋ＳＩ２）を満たさないと判定した場合には、球ＫＹ１、ＫＹ２が交差していると判断し、立方体領域ＢＡ１、ＢＡ２が交差しているか否かを判定する。具体的には図１３（Ｃ）に示すように、衝突判定部１４は、立方体領域ＢＡ１、ＢＡ２の相対的な位置と相対的な回転角度に基づいて交差判定を行う。この相対的な位置と相対的な回転角度は、例えばワールド座標系における立方体領域ＢＡ１、ＢＡ２の位置と姿勢から知ることができ、例えば立方体領域ＢＡ２を基準として立方体領域ＢＡ１の位置と回転角度を求めればよい。

このように、球の交差判定と立方体領域の交差判定を組み合わせることで、処理を簡素にできる。即ち、球が交差していない場合には、立方体領域の交差判定よりも比較的簡素な処理である球の交差判定のみで終了できる。なお、上記では球が交差すると判定した場合に立方体領域が交差するか否かを判定したが、本実施形態では球が交差すると判定した時点で立方体領域が衝突すると確定判定してもよい。この場合、更に処理を簡素化できる。

上記の交差判定において立方体領域ＢＡ１、ＢＡ２が交差すると判定した場合、衝突判定部１４は、立方体領域ＢＡ１、ＢＡ２よりも小さいサイズの立方体領域で衝突判定を行う。即ち図１４に示すように、立方体領域ＢＡ１を分割した立方体領域ＢＢ１〜ＢＧ１と、立方体領域ＢＡ２を分割した立方体領域ＢＢ２〜ＢＧ２との交差判定を、全ての組み合わせについて行う。例えば立方体領域ＢＢ１と立方体領域ＢＢ２、ＢＣ２とが交差すると判定した場合、図１５に示すように、立方体領域ＢＢ１、ＢＢ２、ＢＣ２を更に分割した立方体領域で交差判定を行う。

具体的には、衝突判定部１４は、立方体領域ＢＢ１、ＢＢ２内で代表点が最も近い立方体領域ＢＨ１、ＢＨ２が非交差であると判定し、且つ、立方体領域ＢＢ１、ＢＣ２内で代表点が最も近い立方体領域ＢＩ１、ＢＩ２が非交差であると判定した場合には、物体ＯＢ１、ＯＢ２が非衝突であると確定判定する。一方、交差すると判定された立方体領域が存在し、且つ、判定している階層が４分木構造の最下位階層である場合には、物体ＯＢ１、ＯＢ２に衝突可能性があると確定判定する。

なお、上述の図１４、図１５では、４分木構造の１階層分の衝突判定を省略し、２階層下の衝突判定を図示しているが、実際には衝突判定は１階層ずつ行われる。

即ち、図１６に示すように、衝突判定部１４は、物体ＯＢ１、ＯＢ２の４分木構造のデータにおいて、まずルートのノードＮＡ１、ＮＡ２の代表点データを用いて衝突判定を行う。そして、ノードＮＡ１、ＮＡ２の立方体領域が交差すると判定した場合、ノードＮＡ１、ＮＡ２の子ノードＮＢ１〜ＮＥ１、ＮＢ２〜ＮＥ２の代表点データを用いて、全ての組み合わせについて衝突判定を行う。例えばノードＮＥ１、ＮＣ２の立方体領域が交差すると判定した場合、ノードＮＥ１の子ノードであるＮＳ１〜ＮＶ１の代表点データとＮＣ２の子ノードであるＮＪ２〜ＮＭ２の代表点データとを用いて、これら代表点データの全ての組み合わせについて衝突判定を行う。一方、立方体領域が非交差であると判定されたノードの子ノードについては、それ以上の衝突判定を行わない。

ノードＮＳ１〜ＮＶ１、ＮＪ２〜ＮＭ２の衝突判定において、例えばノードＮＴ１、ＮＫ２の立方体領域が交差すると判定したとする。図１６の例では、ノードＮＴ１は最下位階層のノードであり、ノードＮＫ２は更に下位の子ノードが存在する。この場合、衝突判定部１４は、ノードＮＴ１の代表点データと、ノードＮＫ２の子ノードＮＷ２〜ＮＺ２の代表点データとを用いて衝突判定を行う。ノードＮＴ１の立方体領域と、最下位階層のノードである例えばノードＮＸ２の立方体領域とが交差すると判定した場合、物体ＯＢ１、ＯＢ２に衝突可能性があると確定判定する。一方、いずれかの階層において全てのノードが非衝突と判定された場合には、その時点で物体ＯＢ１、ＯＢ２が非衝突であると確定判定し、物体ＯＢ１、ＯＢ２についての衝突判定を終了する。

さて上述したように、ポリゴンデータを用いた従来の衝突検出手法では、ＣＡＤデータを入手できない物体が現実的には多数存在するため、そのような物体には適用が困難であるという課題がある。また、ポリゴンの大きさに依存して処理が冗長となるという課題や、衝突検出に重要でないポリゴンデータが多数含まれるため無駄な処理が行われるという課題がある。

この点本実施形態では、図３等で説明したように、衝突検出用データ生成部１１４は、物体ＯＢのモデル座標系に設定された立方体領域ＣＡにより深度マップデータを離散化することにより、物体ＯＢのモデル座標系における代表点ＰＡのデータを衝突検出用データとして生成する。ここでモデル座標系とは、衝突検出対象の個々の物体に対して設定されたモデル空間の座標系である。また立方体領域とは、モデル空間において各辺の長さが同一となる立方体であり、深度マップデータ上では、辺の長さは深度幅及び平面方向の距離に対応する。また代表点データとは、立方体領域の位置を代表する代表点（例えば立方体領域の中心点）を表すデータであり、深度マップデータ上における代表深度値及び代表位置のデータ（あるいはモデル座標系におけるＸＹＺ座標のデータ）である。

また本実施形態では、記憶部５０は、代表点ＰＡのデータを衝突検出用データとして記憶し、処理部１０は、第１の物体に対応する第１の衝突検出用データと第２の物体に対応する第２の衝突検出用データとに基づいて、ワールド座標系における第１の物体と第２の物体との衝突判定を行う。ここでワールド座標系とは、例えばロボットの作業空間等に対応しており、モデル座標系から座標変換を行うことにより衝突検出対象の物体がワールド座標系に配置される。

このように本実施形態では、深度マップデータから衝突検出用データを生成できるため、ＣＡＤデータが無くても衝突検出を行うことができる。例えば、衝突検出対象の物体の中にＣＡＤデータが無い物体がある場合に、３Ｄスキャナー等を用いてその物体の深度マップデータを取得することによって、衝突検出用データを作成できる。

また、モデル空間を立方体領域で離散化するため、代表点データにはポリゴンのような大きさのバラつきが生じず、ポリゴンを球で覆った場合のような冗長な重なりも生じない。このような非冗長なデータを用いることで衝突検出の処理負荷を軽減できる。また、深度マップデータを用いることで、物体の外側表面にだけ代表点を設定することができるため、ポリゴンを用いる場合のような物体内部のデータが生じず、衝突検出に重要でない無駄な処理を無くすことができる。また、衝突検出を行うノード対の数はノード数の二乗に比例することから、冗長なデータや無駄なデータが生じない本実施形態では、処理の高速化が期待できる。

ここでノード対とは、衝突判定の判定対象として選択された１組のノードのことである。図１６で説明した第１のオブジェクトＯＢ１と第２のオブジェクトＯＢ２の間の衝突判定を行う場合、ＯＢ１のデータから選択された１つのノードと、ＯＢ２のデータから選択された１つのノードとの組み合わせがノード対となる。本実施形態では、衝突有りと判定したノードの子ノードについて階層毎に衝突判定を行うので、例えばＯＢ１の第３階層のうちＮＥ１の子ノードＮＳ１〜ＮＶ１から１つのノードを選択し、ＯＢ２の第３階層のうちＮＣ２の子ノードＮＪ２〜ＮＭ２から１つのノードを選択し、ノード対を選択することになる。このように本実施形態では、判定対象の階層や判定対象の子ノードから選択したノードの組み合わせがノード対である。

また本実施形態では、記憶部５０は、物体ＯＢを内包するバウンディングボックス（図９（Ａ）の領域Ａ）の代表点データ（図９（Ｂ）のノードＡ）と、そのバウンディングボックスを分割した立方体領域（領域Ｂ〜Ｅ）により深度マップデータを離散化して得られた分割代表点データ（ノードＢ〜Ｅ）と、を衝突検出用データとして記憶する。処理部１０は、第１の物体ＯＢ１を内包するバウンディングボックス（図１６のノードＮＡ１）と第２の物体ＯＢ２を内包するバウンディングボックス（ノードＮＡ２）とが衝突すると判定した場合に、第１の物体ＯＢ１の分割代表点データ（ノードＮＢ１〜ＮＥ１）と第２の物体ＯＢ２の分割代表点データ（ノードＮＢ２〜ＮＥ２）とに基づいて衝突判定を行う。

このようにすれば、バウンディングボックスが非衝突と判定された段階で、分割代表点データの処理を行うことなく第１の物体と第２の物体が非衝突であると確定判定できるため、処理を簡素化できる。

より具体的には、衝突検出用データ生成部１１４は、親ノード（例えば図９（Ｂ）のノードＢ）の立方体領域（図９（Ａ）の領域Ｂ）を分割した複数の立方体領域（領域Ｆ〜Ｉ）に対応するノード（ノードＦ〜Ｉ）を子ノードとして親ノードに接続し、衝突検出用データとして木構造のデータを生成する。衝突検出システムの記憶部５０は、このような木構造のデータを記憶し、処理部１０は、その木構造のデータに基づいて衝突検出を行う。

このような木構造のデータを用いることで、ノード対の再帰的な衝突検出を行うことが可能となるため、例えばＣＰＵ等による並列処理を容易に実現できる。具体的には、本実施形態では４分木構造のデータが生成され、このような４分木構造を用いた場合、再帰的な衝突検出の各階層では４×４＝１６通りのノード対の衝突検出が行われることになる。そのため、数万〜数十万の並列処理が可能なＧＰＵ（Graphics Processing Unit）を使用しなくても、数十スレッド程度の並列処理を行うＣＰＵを用いて衝突検出システムを実現でき、ＧＰＵ省略によるコスト低減を図ることが可能となる。

なお本実施形態の木構造のデータは４分木構造のデータに限定されず、例えばバウンディングボックスのノードに対して最小立方体領域のノードが直接に接続された（中間階層が無い）データであってもよい。この場合、最小立方体領域のノード対の組み合わせ数が多くなるので、ＧＰＵなどを用いて衝突検出システムを構成することが想定される。

５．衝突検出用データ生成装置の詳細な構成
図１７に、本実施形態の衝突検出用データ生成装置の詳細な構成例を示す。この衝突検出用データ生成装置は、処理部１１０、記憶部１５０を含む。処理部１１０は、深度マップデータ取得部１１２、代表点設定部２００、４分木構造生成部２２０を含む。記憶部１５０は、物体１〜物体Ｎの衝突検出用データを記憶する代表点データ記憶部ＭＡ１〜ＭＡＮを含む。

代表点設定部２００は、モデル空間を離散化して代表点を設定する処理を行うものであり、空間離散化部２０２、代表点選択部２０４、代表点補完部２０６、代表点重複削除部２０８を含む。なお、代表点設定部２００と４分木構造生成部２２０は図１（Ａ）の衝突検出用データ生成部１１４に対応する。

次に、この衝突検出用データ生成装置の詳細な処理例について図１８〜図２０のフローチャートを用いて説明する。

図１８に示すように、データ生成処理が開始されると、深度マップデータ取得部１１２は、複数視点の深度マップデータを取得する（ステップＳ１）。深度マップデータ取得部１１２は、例えばＣＡＤデータ入力部２８０から入力されたＣＡＤデータ（ポリゴンデータ）に基づいて、複数視点から見た物体を描画し、各視点についての深度マップデータを生成する。あるいは、３次元情報測定装置２９０から入力される情報に基づいて深度マップデータを取得する。３次元情報測定装置２９０としては、例えば３Ｄスキャナーやステレオカメラなどが想定される。３Ｄスキャナーの場合、深度マップデータ取得部１１２は、３Ｄスキャナーが生成した深度マップデータを取得する。

次に、空間離散化部２０２は、ルートノードの立方体領域の一辺の長さ（最大離散化値）を離散化値ＳＩに設定する（ステップＳ２）。次に空間離散化部２０２は、離散化値ＳＩが、予め設定された所定の最小値よりも小さいか否かを判定する（ステップＳ３）。所定の最小値としては、例えばロボット等の位置把握精度を考慮して、近接許容誤差よりも小さい値に設定する。離散化値ＳＩが所定の最小値よりも小さい場合には、代表点データ記憶部ＭＡ１〜ＭＡＮのうち対応する記憶部にデータを格納し、データ生成処理を終了する。離散化値ＳＩが所定の最小値以上である場合には、４分木構造の１階層分のデータ生成処理が行われる（ステップＳ４）。１階層分のデータ生成処理の詳細については後述する。

次に、４分木構造生成部２２０は、ステップＳ４でデータ生成された階層の上位階層が存在するか否かを判定する（ステップＳ５）。上位階層が存在する場合には、４分木構造生成部２２０は、ステップＳ４で生成されたデータを４分木構造に構成する処理を行い（ステップＳ６）、空間離散化部２０２がステップＳ７を実行する。４分木構造生成処理の詳細については後述する。上位階層が存在しない場合には、空間離散化部２０２は、離散化値ＳＩを１／２倍の値に更新し（ステップＳ７）、ステップＳ３を再び実行する。

図１９に、ステップＳ４の１階層分のデータ生成処理の詳細なフローチャートを示す。なお図１９では、視点から遠ざかる方向に深度値が大きくなる場合を例に説明するが、本実施形態はこれに限定されない。

この処理が開始されると、空間離散化部２０２はモデル空間の離散化値ＳＩを設定する（ステップＳ２０）。次に空間離散化部２０２は、複数視点の全ての視点について代表点の設定処理を行ったか否かを判定する（ステップＳ２１）。未処理の視点が存在する場合には、未処理の視点の中から１つの視点を選択する（ステップＳ２２）。次に空間離散化部２０２は、１辺の長さが離散化値ＳＩの立方体領域で深度マップデータを離散化する（ステップＳ２３）。

次に代表点選択部２０４は、離散化された深度マップデータをスキャンし、代表点を設定する（ステップＳ２４）。次に代表点選択部２０４は、深度マップにおける最背面の代表点データを削除する（ステップＳ２５）。最背面の代表点データは、とり得る深度値範囲の中で最大（又はその近傍）の深度値をもつ代表点である。

次に代表点補完部２０６は、設定された代表点をスキャンし、代表点と、その代表点の２６近傍より外側の代表点との間で連続的に代表点が設定されていない場合、その代表点の背面側（深度値が大きい側、例えば図５の−Ｚ方向側）に代表点を補完する（ステップＳ２６）。具体的には代表点補完部２０６は、注目する代表点（例えば図５のＢ２）の深度値から、その代表点の近傍の代表位置に存在する代表点（Ｂ４）の深度値を引いた差分ＤＶを求める。ここで近傍の代表位置とは、注目する代表点を視点から見たときに、その注目する代表点の代表位置を中心とする周囲８つの代表位置である。そして、代表点補完部２０６は、差分ＤＶ＜０であり、且つ、｜ＤＶ｜＞ＳＩである場合に、｜ＤＶ｜／ＳＩ個の代表点を、注目する代表点の背面側（Ｂ１）に補完する。ステップＳ２６が終了したら、ステップＳ２０を実行する。

ステップＳ２１において、複数視点の全ての視点について代表点を設定していた場合には、代表点重複削除部２０８は、複数視点の代表点の中で重複する代表点を削除する処理を、全ての代表点について行ったか否かを判定する（ステップＳ２７）。未処理の代表点が存在する場合には、代表点重複削除部２０８は、未処理の代表点の中から１つの代表点を選択する（ステップＳ２８）。代表点重複削除部２０８は、は、選択した代表点と他の全ての代表点とを比較し、同一の代表点が存在する場合には、その同一の代表点を削除し（ステップＳ２９）、ステップＳ２７を実行する。ステップＳ２７において、全ての代表点について処理が終了していた場合には、１階層分のデータ生成処理を終了する。

図２０に、ステップＳ６の４分木構造生成処理の詳細なフローチャートを示す。この処理が開始されると、４分木構造生成部２２０は、ステップＳ４でデータ生成された階層の上位階層の離散化値ＳＩを取得する（ステップＳ４０）。次に、４分木構造生成部２２０は、複数視点の全ての視点について処理を行ったか否かを判定する（ステップＳ４１）。全ての視点について処理を終了していた場合には、４分木構造生成処理を終了する。未処理の視点が存在する場合には、未処理の視点の中から１つの視点を選択する（ステップＳ４２）。

次に４分木構造生成部２２０は、上位階層の全ての代表点について処理を行ったか否かを判定する（ステップＳ４３）。例えば図９（Ｂ）のノードＦ〜Ｖについて処理する場合、その上位階層であるノードＢ〜Ｃの全てについて判定する。上位階層の全ての代表点について処理を終了していた場合には、ステップＳ４１を実行する。未処理の代表点が存在した場合には、４分木構造生成部２２０は、未処理の代表点の中から１つの代表点を選択する（ステップＳ４４）。次に４分木構造生成部２２０は、その選択した代表点を親ノードとして子ノードの代表点を接続し、下位階層の４分木構造を構成する（ステップＳ４５）。例えば図９（Ｃ）のノードＢｘｙを選択している場合、ノードＢｘｙが親ノードであり、そのノードＢｘｙに対して子ノードＦｘｙ〜Ｉｘｙ、Ｆｚ〜Ｉｚを接続する。ノードＦｘｙ〜Ｉｘｙには代表位置のＸＹ座標が設定され、ノードＦｚ〜Ｉｚはこの時点では空のノード（例えばＮＵＬＬノード）である。

次に４分木構造生成部２２０は、４つの子ノードの全てについて代表点データの設定処理を行ったか否かを判定する（ステップＳ４６）。４つの子ノードの全てについて処理を終了していた場合には、ステップＳ４３を実行する。未処理の子ノードが存在した場合には、４分木構造生成部２２０は、未処理の子ノードの中から１つの子ノードを選択する（ステップＳ４７）。次に４分木構造生成部２２０は、選択した子ノードの代表位置に存在する代表点を検出する（ステップＳ４８）。

次に、選択した子ノードの代表位置に代表点が検出されたか否かを判定する（ステップＳ４９）。代表点が検出された場合には、４分木構造生成部２２０は、検出された全ての代表点を子ノードに連結する（ステップＳ５０）。例えば図９（Ｃ）のノードＦｘｙを選択しており、１つの代表点のみが検出された場合、その１つの代表点の代表深度値をノードＦｚとしてノードＦｘｙに接続する。また図１０（Ｂ）のノードＣｘｙのように複数の代表点が検出された場合には、その複数の代表点の代表深度値をリスト構造のノードＣａｚ〜ＣｃｚとしてノードＣｘｙに接続する。ステップＳ４９において代表点が検出されなかった場合には、４分木構造生成部２２０は子ノードに代表点が無い旨の情報を設定する（ステップＳ５１）。例えば図１１（Ｃ）で説明したように、ノードＴｘｙに対してＮＵＬＬノードＴｚを接続する。ステップＳ５０、Ｓ５１が終了したらステップＳ４６を実行する。

６．衝突検出システムの詳細な構成
図２１に、本実施形態の衝突検出システムの詳細な構成例を示す。この衝突検出システムは、処理部１０、記憶部５０を含む。処理部１０は、代表点データ選択部２５０、再帰的ノード対衝突検出部２６０、衝突判定出力部２７０を含む。記憶部５０は、物体１〜物体Ｎの衝突検出用データを記憶する代表点データ記憶部ＭＢ１〜ＭＢＮを含む。

なお、代表点データ選択部２５０、再帰的ノード対衝突検出部２６０、衝突判定出力部２７０は、図１（Ｂ）の衝突判定部１４、オブジェクト空間設定部１２に対応する。ここで、衝突検出システムと衝突検出用データ生成装置を一体に構成する場合には、代表点データ記憶部ＭＢ１〜ＭＢＮと図１７の代表点データ記憶部ＭＡ１〜ＭＡＮが共通化されてもよい。

次に、この衝突検出システムの詳細な処理例について図２２、図２３のフローチャートを用いて説明する。

図２２に示すように、衝突検出処理が開始されると、代表点データ選択部２５０は、全ての物体の組み合わせについて衝突検出処理を行ったか否かを判定する（ステップＳ６０）。未処理の組み合わせが存在する場合には、代表点データ選択部２５０は、未処理の組み合わせの中から１組の物体（第１の物体、第２の物体）を選択する（ステップＳ６１）。

次に再帰的ノード対衝突検出部２６０は、第１の物体の最上位ノードをノードＮ１に設定し、第２の物体の最上位ノードをノードＮ２に設定する（ステップＳ６２）。次に再帰的ノード対衝突検出部２６０は、ノードＮ１、Ｎ２に対して再帰的ノード対衝突検出処理を行う（ステップＳ６３）。この再帰的ノード対衝突検出処理の詳細については後述する。次に衝突判定出力部２７０は、衝突判定結果を出力し、処理部１０は、衝突判定結果に応じた種々の処理を行う（ステップＳ６４）。例えば、処理部１０は、衝突すると判定された場合に、衝突しないように物体の軌道を修正する処理や、動作を停止する処理などを行う。ステップＳ６４が終了したらステップＳ６０を実行する。ステップＳ６０において全ての物体の組み合わせについて処理を終了していた場合には、衝突検出処理を終了する。

図２３に、再帰的ノード対衝突検出処理の詳細なフローチャートを示す。この処理が開始されると、再帰的ノード対衝突検出部２６０は、処理対象のノード対であるノードＮ１、Ｎ２を設定する（ステップＳ８０）。次に、ワールド座標系においてノードＮ１、Ｎ２の立方体領域に重なりがあるか否かを判定する（ステップＳ８１）。重なりが無い場合には、ノードＮ１、Ｎ２が非衝突であると判断して処理を終了する。重なりが有る場合には、再帰的ノード対衝突検出部２６０は、ノードＮ１に子ノードが存在するか否かを判定する（ステップＳ８２）。

ノードＮ１に子ノードが存在する場合には、再帰的ノード対衝突検出部２６０は、ノードＮ２に子ノードが存在するか否かを判定する（ステップＳ８３）。ノードＮ２に子ノードが存在する場合には、ノードＮ１、Ｎ２の子ノードの全ての組み合わせについて、再帰的にノード対の衝突検出を行う（ステップＳ８４）。即ち、ノードＮ１、Ｎ２の子ノードの組み合わせの中に、立方体領域が重なると判定された組み合わせ（例えば図１６のノードＮＥ１、ＮＣ１）が存在した場合、そのノード対を新たにノードＮ１、Ｎ２に設定し、ステップＳ８１以降を再び実行する。この再帰的な衝突検出は、ステップＳ８４で立方体領域が重なると判定された全てのノード対に対して行う。仮に、最下位階層まで立方体領域が重なるノード対が存在すれば、この再帰的な処理が最下位階層まで繰り返されることになる。ステップＳ８３においてノードＮ２に子ノードが存在しなかった場合には、再帰的ノード対衝突検出部２６０は、ノードＮ１の子ノードとノードＮ２との全ての組み合わせについて再帰的なノード対の衝突検出を行う（ステップＳ８５）。

ステップＳ８２においてノードＮ１に子ノードが存在しなかった場合には、再帰的ノード対衝突検出部２６０は、ノードＮ２に子ノードが存在するか否かを判定する（ステップＳ８６）。ノードＮ２に子ノードが存在する場合には、ノードＮ１とノードＮ２の子ノードとの全ての組み合わせについて、再帰的にノード対の衝突検出を行う（ステップＳ８７）。ノードＮ２に子ノードが存在しなかった場合には、ノードＮ１、Ｎ２に衝突可能性があると判断して処理を終了する。

ステップＳ８４、Ｓ８５、Ｓ８７を実行した場合、再帰的ノード対衝突検出部２６０は、その再帰的なノード対衝突検出において最下位階層のノード対に衝突が検出されたか否かを判定し（ステップＳ８８）、その判定結果を出力して処理を終了する。

なお、上記のように本実施形態について詳細に説明したが、本発明の新規事項および効果から実体的に逸脱しない多くの変形が可能であることは当業者には容易に理解できるであろう。従って、このような変形例はすべて本発明の範囲に含まれるものとする。例えば、明細書又は図面において、少なくとも一度、より広義または同義な異なる用語と共に記載された用語は、明細書又は図面のいかなる箇所においても、その異なる用語に置き換えることができる。また本実施形態及び変形例の全ての組み合わせも、本発明の範囲に含まれる。また衝突検出用データ生成装置や衝突検出システムの構成・動作や、衝突検出用データの生成手法や衝突検出の手法等も、本実施形態で説明したものに限定されず、種々の変形実施が可能である。

１０ 処理部、
１２ オブジェクト空間設定部、１４ 衝突判定部、５０ 記憶部、
５２ 代表点データ記憶部、７０ 操作部、８０ 外部Ｉ／Ｆ部、
９０ 情報記憶媒体、１１０ 処理部、１１２ 深度マップデータ取得部、
１１４ 衝突検出用データ生成部、１５０ 記憶部、１７０ 操作部、
１８０ 外部Ｉ／Ｆ部、１９０ 情報記憶媒体、２００ 代表点設定部、
２０２ 空間離散化部、２０４ 代表点選択部、２０６ 代表点補完部、
２０８ 代表点重複削除部、２２０ ４分木構造生成部、
２５０ 代表点データ選択部、２６０ 再帰的ノード対衝突検出部、
２７０ 衝突判定出力部、２８０ ＣＡＤデータ入力部、
２９０ ３次元情報測定装置、３００ 制御装置、３１０ ロボット、
３２０ アーム、
Ａ〜Ｖ ノード、Ａｘｙ〜Ｖｘｙ ノード、Ａｚ〜Ｖｚ サブノード、
ＣＡ，ＣＢ 立方体領域、ＭＡ１〜ＭＡＮ，ＭＢ１-ＭＢＮ 代表点データ記憶部、
ＯＢ 物体、ＯＢ１ 第１の物体、ＯＢ２ 第２の物体、
ＰＡ，ＰＢ 代表点、ＳＩ 離散化値、ＺＤ 深度マップデータ

Claims (4)

1. 可動部を有するロボットと、
   物体の衝突検出用データとして、第１の物体に対応する第１の衝突検出用データと第２の物体に対応する第２の衝突検出用データとを記憶する記憶部と、
   前記第１の衝突検出用データと前記第２の衝突検出用データとに基づいて、ワールド座標系における前記第１の物体と前記第２の物体との衝突判定を行う処理部と、
   前記処理部により行われた前記衝突判定の結果に基づいて前記可動部の動作を制御する制御部と、
   を含み、
   前記記憶部は、
   前記衝突検出用データとして前記物体のモデル座標系において所定の視点から前記物体を見たときの深度マップデータを、親ノードの立方体領域を分割した複数の立方体領域に対応する代表点データである前記親ノードから分岐する子ノードの４分木構造のデータとして記憶することを特徴とするロボットシステム。
2. 請求項１において、
   前記親ノードの立方体領域を分割した前記子ノードの前記複数の立方体領域は、
   前記所定の視点から見たときの前記親ノードの立方体領域を、２×２領域に分割するとともに、前記２×２領域の各領域を、前記所定の視点の奥行き方向に２分割することで得られた２×２×２個の立方体領域であり、
   前記木構造のデータは、
   前記所定の視点から見たときの前記２×２領域の各領域に対応して前記子ノードが設定された４分木構造のデータであり、
   前記４分木構造における前記子ノードのデータは、
   前記２×２領域の前記各領域において、前記奥行き方向の２つの立方体領域のうち少なくとも一方に存在する前記代表点データであることを特徴とするロボットシステム。
3. 可動部と、
   物体の衝突検出用データとして、第１の物体に対応する第１の衝突検出用データと第２の物体に対応する第２の衝突検出用データとを記憶する記憶部と、
   前記第１の衝突検出用データと前記第２の衝突検出用データとに基づいて、ワールド座標系における前記第１の物体と前記第２の物体との衝突判定を行う処理部と、
   前記処理部により行われた前記衝突判定の結果に基づいて前記可動部の動作を制御する制御部と、
   を含み、
   前記記憶部は、
   前記衝突検出用データとして前記物体のモデル座標系において所定の視点から前記物体を見たときの深度マップデータを、親ノードの立方体領域を分割した複数の立方体領域に対応する代表点データである前記親ノードから分岐する子ノードの４分木構造のデータとして記憶することを特徴とするロボット。
4. 物体のモデル座標系における所定の視点から前記物体を見たときの深度マップデータを取得する深度マップデータ取得部と、
   前記物体の前記モデル座標系における代表点データを衝突検出用データとして生成する衝突検出用データ生成部と、を含み
   前記衝突検出用データ生成部は、
   前記衝突検出用データとして前記物体のモデル座標系において前記所定の視点から前記物体を見たときの深度マップデータを、親ノードの立方体領域を分割した複数の立方体領域に対応する代表点データである前記親ノードから分岐する子ノードの４分木構造のデータとして記憶することを特徴とするロボットの制御装置。

Images