JP7429807B2 - 経路生成装置、経路生成方法及び経路生成プログラム - Google Patents

Description

本開示は、経路生成装置、経路生成方法及び経路生成プログラムに関する。

従来より、動作装置の経路を生成する技術が知られている。例えば、特許文献１には、ロボットと障害物との干渉を判定しつつ、ロボットの経路を計画する装置が開示されている。

特開２０１３－２４６５５３号公報

動作装置の経路を生成する場合には、特許文献１の装置のように、障害物等の物体と動作装置との干渉を判定する必要がある。干渉判定は、経路を生成する上で何度も行われる。例えば、所定の開始位置から終了位置に至る経路を生成する場合には、その経路上の複数個所で行われる。また、干渉判定によって物体と動作装置とが干渉すると判定された場合には、経路を変更して再び干渉判定が行われる。通常、干渉判定は、物体と動作装置との距離を演算することによって行われる。前述の如く、経路生成においては干渉判定が何度も行われるので、物体と動作装置との距離の演算の時間がかかると、経路生成の時間が長期化することになる。その一方で、経路生成の時間を短縮するために、物体と動作装置との距離の演算を簡略化すると、物体と動作装置との干渉判定の精度が低下する。

本開示は、かかる点に鑑みてなされたものであり、その目的とするところは、干渉判定の時間の短縮と精度の向上とを両立させることにある。

本開示の経路生成装置は、動作装置の経路を生成する経路生成装置であって、前記動作装置が配置された配置空間に含まれる物体のモデルである物体モデルを設定する物体設定部と、前記配置空間における前記動作装置のモデルである装置モデルを設定する装置設定部と、前記動作装置の経路を段階的に生成する経路生成部と、前記物体モデルと前記装置モデルとの距離に基づいて、前記物体モデルと前記経路に沿って移動した後の前記装置モデルとの干渉判定を行う干渉判定部とを備え、前記物体モデル及び前記装置モデルの少なくとも一方は、点群で形成された点群モデルであり、干渉の可能性があると前記干渉判定部によって判定された場合には、前記物体設定部及び前記装置設定部の少なくとも一方は、前記点群モデルの前記点群の密度を高くし、前記干渉判定部は、前記点群の密度が高くなった前記点群モデルを用いて前記干渉判定を再び行い、前記経路生成部は、干渉の可能性がないと前記干渉判定部によって判定された場合には次の経路を生成する。

本開示の経路生成方法は、動作装置の経路を生成する経路生成方法であって、前記動作装置が配置された配置空間に含まれる物体のモデルである物体モデルを設定することと、前記配置空間における前記動作装置のモデルである装置モデルを設定することと、前記動作装置の経路を段階的に生成することと、前記物体モデルと前記装置モデルとの距離に基づいて、前記物体モデルと前記経路に沿って移動した後の前記装置モデルとの干渉判定を行うこととを含み、前記物体モデル及び前記装置モデルの少なくとも一方は、点群で形成された点群モデルであり、干渉の可能性があると前記干渉判定によって判定された場合には、前記物体モデルを設定すること及び前記装置モデルを設定することの少なくとも一方において、前記点群モデルの前記点群の密度を高くし、前記干渉判定を行うことにおいて、前記点群の密度が高くなった前記点群モデルを用いて前記干渉判定を再び行い、前記経路を生成することにおいては、干渉の可能性がないと前記干渉判定によって判定された場合には次の経路を生成する。

本開示の経路生成プログラムは、動作装置の動作を生成するためにコンピュータに、前記動作装置が配置された配置空間に含まれる物体のモデルである物体モデルを設定する機能と、前記配置空間における前記動作装置のモデルである装置モデルを設定する機能と、前記動作装置の経路を段階的に生成する機能と、前記物体モデルと前記装置モデルとの距離に基づいて、前記物体モデルと前記経路に沿って移動した後の前記装置モデルとの干渉判定を行う機能とを実現させ、前記物体モデル及び前記装置モデルの少なくとも一方は、点群で形成された点群モデルであり、干渉の可能性があると前記干渉判定によって判定された場合には、前記物体モデルを設定する機能及び前記装置モデルを設定する機能の少なくとも一方は、前記点群モデルの前記点群の密度を高くし、前記干渉判定を行う機能は、前記点群の密度が高くなった前記点群モデルを用いて前記干渉判定を再び行い、前記経路を生成する機能は、干渉の可能性がないと前記干渉判定によって判定された場合には次の経路を生成する。

前記経路生成装置によれば、干渉判定の時間の短縮と精度の向上とを両立させることができる。

前記経路生成方法によれば、干渉判定の時間の短縮と精度の向上とを両立させることができる。

前記経路生成プログラムによれば、干渉判定の時間の短縮と精度の向上とを両立させることができる。

図１は、ロボットシステムの構成を示す模式図である。 図２は、ロボット制御装置及び制御装置の概略的なハードウェア構成を示す図である。 図３は、制御部の制御系統の構成を示すブロック図である。 図４は、物体モデル及び装置モデルの一例を示す図である。 図５は、制御装置によるロボットアームの動作制御を示すフローチャートである。 図６は、制御装置によるロボットアームの経路生成処理を示すフローチャートである。 図７は、第１装置モデルが第１物体モデルに接近した状態を示す模式図である。 図８は、図７における第１物体モデル及び第１装置モデルが第２物体モデル及び第２装置モデルに変更された状態を示す模式図である。 図９は、図８における第２物体モデル及び第２装置モデルが第３物体モデル及び第３装置モデルに変更された状態を示す模式図である。

以下、例示的な実施形態を図面に基づいて詳細に説明する。図１は、ロボットシステム１００の構成を示す模式図である。

ロボットシステム１００は、ロボット１と、ロボット１の経路を生成する経路生成装置３とを備えている。経路生成装置３は、制御装置５を備えている。経路生成装置３は、ロボット１が配置された配置空間の情報を取得する三次元ビジョンセンサ４を備えていてもよい。ロボットシステム１００では、制御装置５が三次元ビジョンセンサ４の検出結果を参照してロボット１の経路を生成すると共に、ロボット１の動作を制御する。

この例では、ロボット１は、コンテナ９１内の物体を搬出したり、コンテナ９１内へ物体を搬入したりする。

ロボット１は、この例では、産業用ロボットである。ロボット１は、ロボットアーム１２と、ロボット１の全体を制御するロボット制御装置２とを有している。ロボットアーム１２は、エンドエフェクタとしてのハンド１４を有している。ロボットアーム１２は、ハンド１４によって物体を把持する。ロボット１は、動作装置の一例であり、より具体的には、ロボットアーム１２が動作装置の一例である。

ロボット１が配置される空間には、直交３軸のロボット座標系が規定されている。例えば、上下方向にＺ軸が設定され、水平方向に互いに直交するＸ軸及びＹ軸が設定される。

ロボットアーム１２は、三次元状に動作するように構成されている。具体的には、ロボットアーム１２は、少なくとも３自由度の並進を含む動作を行うように構成されている。この例では、ロボットアーム１２は、垂直多関節型のロボットアームである。ロボットアーム１２は、ベース１０に支持されている。ロボットアーム１２は、複数のリンクと、複数のリンクを連結する複数の関節とを有している。

詳しくは、ロボットアーム１２は、ベース１０に連結された第１リンク１２ａと、第１リンク１２ａに連結された第２リンク１２ｂと、第２リンク１２ｂに連結された第３リンク１２ｃと、第３リンク１２ｃに連結された第４リンク１２ｄと、第４リンク１２ｄに連結された第５リンク１２ｅとを有している。尚、ハンド１４も、ロボットアーム１２の１つのリンクとみなすことができる。

詳しくは、ベース１０と第１リンク１２ａとは、鉛直方向に延びる軸回りに回転可能な第１関節１３ａを介して互いに連結されている。第１リンク１２ａと第２リンク１２ｂとは、水平方向に延びる軸回りに回転可能な第２関節１３ｂを介して互いに連結されている。第２リンク１２ｂと第３リンク１２ｃとは、水平方向に延びる軸回りに回転可能な第３関節１３ｃを介して互いに連結されている。第３リンク１２ｃと第４リンク１２ｄとは、第４リンク１２ｄの軸心（即ち、第４リンク１２ｄが延びる方向）回りに回転可能な第４関節１３ｄを介して互いに連結されている。第４リンク１２ｄと第５リンク１２ｅとは、第４リンク１２ｄの軸心と直交する軸回りに回転可能な第５関節１３ｅを介して互いに連結されている。

ロボットアーム１２の先端部、即ち、第５リンク１２ｅには、ハンド１４が連結されている。第５リンク１２ｅとハンド１４とは、所定の軸回りに回転可能に第６関節１３ｆを介して互いに連結されている。ハンド１４は、開閉するように構成された２本の指１４ａを有している。２本の指１４ａは、エアシリンダ等のアクチュエータによって開閉駆動される。

ロボットアーム１２は、各関節を回転駆動するサーボモータ１５（図２参照）を有している。各サーボモータ１５は、エンコーダ１５ａ（図２参照）を有している。

図２は、ロボット制御装置２及び制御装置５の概略的なハードウェア構成を示す図である。ロボット制御装置２は、制御装置５と信号及び指令等の送受信を行う。ロボット制御装置２は、ロボットアーム１２のサーボモータ１５及びハンド１４を制御する。例えば、ロボット制御装置２は、制御装置５の指令に応じて、サーボモータ１５に電流を供給する。このとき、ロボット制御装置２は、エンコーダ１５ａの出力に基づいて供給電流をフィードバック制御する。また、ロボット制御装置２は、ハンド１４のアクチュエータを制御することによって２本の指１４ａを開閉させる。

ロボット制御装置２は、制御部２１と、記憶部２２と、メモリ２３とを有している。

制御部２１は、ロボット制御装置２の全体を制御する。制御部２１は、各種の演算処理を行う。例えば、制御部２１は、ＣＰＵ（Central Processing Unit）等のプロセッサで形成されている。制御部２１は、ＭＣＵ（Micro Controller Unit）、ＭＰＵ（Micro Processor Unit）、ＦＰＧＡ（Field Programmable Gate Array）、ＰＬＣ（Programmable Logic Controller）、システムＬＳＩ等で形成されていてもよい。

記憶部２２は、制御部２１で実行されるプログラム及び各種データを格納している。記憶部２２は、不揮発性メモリ、ＨＤＤ（Hard Disc Drive）又はＳＳＤ（Solid State Drive）等で形成される。

メモリ２３は、データ等を一時的に格納する。例えば、メモリ２３は、揮発性メモリで形成される。

三次元ビジョンセンサ４は、ロボットアーム１２、即ち、ロボット１が配置された配置空間の空間情報を取得する。空間情報は、配置空間における物体の位置及び形状の情報を含む。三次元ビジョンセンサ４は、三次元の空間情報を取得する。例えば、三次元ビジョンセンサ４は、コンテナ９１を上方から、コンテナ９１及びコンテナ９１内の物体の位置及び形状を計測する。三次元ビジョンセンサ４は、コンテナ９１及びコンテナ９１内の物体の位置及び形状を点群データの形式で出力する。三次元ビジョンセンサ４は、計測結果を空間情報として制御装置５へ出力する。

制御装置５は、ロボットアーム１２の経路を生成すると共に、ロボットアーム１２を経路に沿って移動させる指令をロボット制御装置２へ出力する。この例では、ロボットアーム１２が他の物体との干渉を回避しながら所定の開始位置から目標位置まで移動する経路を生成する。より具体的には、ロボットアーム１２がコンテナ９１内から所定の物体Ａを取り出し、その物体Ａを載置台（図示省略）まで搬送するピックアンドプレース動作を制御装置５がロボットアーム１２に実行させる。

制御装置５は、ロボット制御装置２と信号及び情報等の送受信を行う。制御装置５には、三次元ビジョンセンサ４の検出結果が入力される。制御装置５は、図２に示すように、制御部５１と、記憶部５２と、メモリ５３とを有している。尚、図示を省略するが、制御装置５は、動作生成に関する設定等を行うためにユーザが操作する入力操作部と、設定内容を表示するディスプレイとをさらに有していてもよい。

制御部５１は、制御装置５の全体を制御する。制御部５１は、各種の演算処理を行う。例えば、制御部５１は、ＣＰＵ（Central Processing Unit）等のプロセッサで形成されている。制御部５１は、ＭＣＵ（Micro Controller Unit）、ＭＰＵ（Micro Processor Unit）、ＦＰＧＡ（Field Programmable Gate Array）、ＰＬＣ（Programmable Logic Controller）、システムＬＳＩ等で形成されていてもよい。

記憶部５２は、制御部５１で実行されるプログラム及び各種データを格納している。例えば、記憶部５２は、ロボットアーム１２の動作を制御するための動作制御プログラム６１及びロボットアーム１２の経路を生成するための経路生成プログラム６２が格納されている。記憶部５２は、不揮発性メモリ、ＨＤＤ（Hard Disc Drive）又はＳＳＤ（Solid State Drive）等で形成される。

メモリ５３は、データ等を一時的に格納する。例えば、メモリ５３は、揮発性メモリで形成される。

図３は、制御部５１の制御系統の構成を示すブロック図である。制御部５１は、記憶部５２から経路生成プログラム６２をメモリ５３に読み出して展開することによって、各種機能を実現する。具体的には、制御部５１は、物体設定部５４と装置設定部５５と経路生成部５６と干渉判定部５７と動作指令部５８として機能する。

物体設定部５４は、ロボットアーム１２が配置された配置空間に含まれる物体のモデルである物体モデル７１を設定する。この例では、物体モデル７１は、点群で形成された点群モデルである。また、この例では、コンテナ９１、載置台及び、コンテナ９１内の物体が配置空間に含まれる物体に相当する。物体設定部５４は、コンテナ９１等のそれぞれの物体について物体モデル７１を生成する。物体設定部５４は、物体ごとに、物体モデル７１の点群の密度を変更することができる。以下、「密度」は、特段の断りが無い限り、点群の密度を意味する。例えば、物体設定部５４は、三次元ビジョンセンサ４から出力される点群データをダウンサンプリングして物体モデル７１を生成することによって、点群の密度が異なる物体モデル７１を生成することができる。物体設定部５４は、物体モデル７１の一部の点群の密度を変更することができる。物体設定部５４は、物体モデル７１の全部の点群の密度を変更することもできる。

この例では、物体設定部５４は、所定の第１密度と第１密度よりも高い第２密度と第２密度よりも高い第３密度との３段階で、物体モデル７１の点群の密度を変更する。また、物体設定部５４は、物体モデル７１の一部の点群の密度を変更する。以下、最も高い点群の密度が第１密度の物体モデル７１を「第１物体モデル７１Ａ」と称し、最も高い点群の密度が第２密度の物体モデル７１を「第２物体モデル７１Ｂ」と称し、最も高い点群の密度が第３密度の物体モデル７１を「第３物体モデル７１Ｃ」と称する。第１物体モデル７１Ａ、第２物体モデル７１Ｂ及び第３物体モデル７１Ｃのそれぞれを区別しない場合には、単に「物体モデル７１」と称する。図４は、物体モデル７１及び装置モデル７２の一例を示す図である。図４では、物体モデル７１のうちコンテナ９１の物体モデル７１を示している。図４は、コンテナ９１の第１物体モデル７１Ａを示している。

経路生成部５６は、ロボットアーム１２の経路を段階的に生成する。この例では、経路生成部５６は、ロボットアーム１２の開始位置と目標位置との間の１又は複数の中間位置（即ち、ノード）を生成することによって、１又は複数の中間位置を経由した開始位置から目標位置までの経路を生成する。ここで、ロボットアーム１２は複数の自由度を有しているので、ロボットアーム１２の位置は、各関節及びハンド１４の指先の位置によって一義的に決まる位置であり、ロボットアーム１２の姿勢とみなすこともできる（つまり、開始位置は開始姿勢、目標位置は目標姿勢、及び、中間位置は中間姿勢とみなすこともできる）。また、ロボットアーム１２の経路は、複数の関節の位置及びハンド１４の指先の位置によって定義され、ひいては、各関節の回転角によって定義される。

例えば、経路生成部５６は、ロボットアーム１２の中間位置を開始位置から目標位置へ向かって順番に生成する。中間位置の生成は、中間位置までの経路の生成とみなすことができる。経路生成部５６は、新たな中間位置をランダム又は所定のルール（制約）に従って生成する。所定のルールとしては、例えば、ロボットアーム１２が目標位置へ近づくこと、ロボットアーム１２が物体と干渉する場合にはロボットアーム１２が物体から遠ざかること、又は、複数の関節及びハンド１４の指先のそれぞれの移動量の上限値等が挙げられる。経路生成部５６は、これらのルールを総合的に判断して、中間位置を生成してもよい。

生成された中間位置、即ち、経路に対して、干渉判定部５７による干渉判定が行われる。経路生成部５６は、干渉判定の結果に応じて、中間位置の変更（即ち、経路の変更）及び次の中間位置の生成（即ち、次の経路の生成）を選択的に行う。

装置設定部５５は、配置空間におけるロボットアーム１２のモデルである装置モデル７２を設定する。この例では、装置モデル７２は、点群で形成された点群モデルである。装置設定部５５は、経路生成部５６によって生成された中間位置に位置するロボットアーム１２の装置モデル７２を生成する。

装置設定部５５は、装置モデル７２の点群の密度を変更することができる。装置設定部５５は、装置モデル７２の一部の点群の密度を変更することができる。装置設定部５５は、装置モデル７２の全体の点群の密度を変更することもできる。

記憶部５２には、ロボットアーム１２の点群データが記憶されている。ロボットアーム１２の点群データは、ロボットアーム１２の形状、少なくとも外形を表す。具体的には、ロボットアーム１２の点群データには、各リンクの点群データが含まれている。さらに、記憶部５２には、密度が異なるロボットアーム１２の点群データが記憶されている。具体的には、所定の第１密度のロボットアーム１２の点群データと、第１密度よりも高い第２密度のロボットアーム１２の点群データと、第２密度よりも高い第３密度のロボットアーム１２の点群データとが記憶部５２に記憶されている。尚、ロボットアーム１２の点群データの第１密度、第２密度及び第３密度はそれぞれ、第１物体モデル７１Ａ、第２物体モデル７１Ｂ及び第３物体モデル７１Ｃの第１密度、第２密度及び第３密度と略同じである。

装置設定部５５は、記憶部５２からロボットアーム１２の点群データを読み出し、読み出された点群データを、経路生成部５６によって生成された中間位置に配置する。こうして、装置設定部５５は、ロボットアーム１２の装置モデル７２を設定する。このとき、装置設定部５５は、記憶部５２から読み出す点群データを変更することによって、装置モデル７２の密度を変更することができる。さらに、装置設定部５５は、ロボットアーム１２の部分ごとに異なる密度の点群データを読み出すことができる。装置設定部５５は、ロボットアーム１２の全体に亘って密度が均一な装置モデル７２を生成することもできる。以下、最も高い点群の密度が第１密度の装置モデル７２を「第１装置モデル７２Ａ」と称し、最も高い点群の密度が第２密度の装置モデル７２を「第２装置モデル７２Ｂ」と称し、最も高い点群の密度が第３密度の装置モデル７２を「第３装置モデル７２Ｃ」と称する。第１装置モデル７２Ａ、第２装置モデル７２Ｂ及び第３装置モデル７２Ｃのそれぞれを区別しない場合には、単に「装置モデル７２」と称する。図４は、ロボットアーム１２の全体が第１密度の点群データで形成された第１装置モデル７２Ａを示している。

干渉判定部５７は、物体モデル７１と装置モデル７２との距離に基づいて、経路に沿って移動した後の（即ち、生成された中間位置に位置する）装置モデル７２と物体モデル７１との干渉判定を行う。ここで、物体モデル７１及び装置モデル７２はそれぞれ点群モデルなので、物体モデル７１と装置モデル７２との距離は、物体モデル７１の点群に含まれる点と装置モデル７２の点群に含まれる点との距離となる。干渉判定部５７は、密度が低い点群モデルから順番に用いて干渉判定を行う。干渉判定部５７は、密度が低い物体モデル７１及び装置モデル７２を用いて干渉判定を行い、干渉の可能性が無ければ干渉判定を終了し、干渉の可能性があると判定した場合には物体モデル７１及び装置モデル７２の少なくとも一方の点群の密度を高くして、再び干渉判定を行う。

具体的には、干渉判定部５７は、物体モデル７１と装置モデル７２との最短距離であるモデル間距離が所定の安全閾値以下の場合に、物体モデル７１と装置モデル７２との干渉の可能性があると判定する。一方、干渉判定部５７は、モデル間距離が安全閾値よりも大きい場合に、物体モデル７１と装置モデル７２との干渉の可能性がないと判定する。さらに、干渉判定部５７は、干渉の可能性がないと判定した場合、モデル間距離から安全閾値を減じた値である余裕距離を求める。

ここで、物体モデル７１及び装置モデル７２のそれぞれにおいて互いに最も接近する部分がモデル間距離、即ち、最短距離を形成する部分である。この例では、物体モデル７１及び装置モデル７２は点群モデルであるので、物体モデル７１及び装置モデル７２のそれぞれにおいて互いに最も接近する２つの点がモデル間距離を形成する。これらの点を「最接近点」と称する。つまり、物体モデル７１の最接近点と装置モデル７２の最接近点とでモデル間距離が形成される。

さらに、モデル間距離が安全閾値以下の場合には、干渉判定部５７は、モデル間距離が安全閾値よりも小さな干渉閾値以下か否かを判定する。干渉判定部５７は、モデル間距離が干渉閾値以下の場合に、物体モデル７１と装置モデル７２とが干渉すると判定する。

つまり、干渉判定部５７は、物体モデル７１と装置モデル７２との距離に基づいて、干渉の可能性あり、干渉の可能性なし、及び、干渉するの何れであるかを判定する。そして、干渉の可能性ありの場合には、干渉判定部５７は、物体モデル７１及び装置モデル７２の少なくとも一方の点群の密度を高くして、同様の干渉判定を行う。

例えば、干渉判定部５７は、まず第１物体モデル７１Ａと第１装置モデル７２Ａとの干渉判定を行う。干渉の可能性があれば、干渉判定部５７は、次に第２物体モデル７１Ｂと第２装置モデル７２Ｂとの干渉判定を行う。それでも干渉の可能性があれば、干渉判定部５７は、さらに第３物体モデル７１Ｃと第３装置モデル７２Ｃとの干渉判定を行う。

このとき、第２物体モデル７１Ｂ及び第２装置モデル７２Ｂのそれぞれは、全体が第２密度となっていなくてもよい。つまり、第２物体モデル７１Ｂのうち最接近点を含む一部の点群だけが第２密度となり、他の部分は第１密度であってもよい。第２装置モデル７２Ｂのうち最接近点を含む一部の点群だけが第２密度となり、他の部分は第１密度であってもよい。同様に、第３物体モデル７１Ｃと第３装置モデル７２Ｃとの干渉判定においても、第３物体モデル７１Ｃのうち最接近点を含む一部の点群だけが第３密度となり、他の部分は第１密度又は第２密度であってもよい。第３装置モデル７２Ｃのうち最接近点を含む一部の点群だけが第３密度となり、他の部分は第１密度又は第２密度であってもよい。

尚、干渉判定部５７は、第１物体モデル７１Ａと第１装置モデル７２Ａとの干渉判定において、干渉の可能性がない、又は、干渉すると判定した場合には、干渉判定を終了する。第２物体モデル７１Ｂと第２装置モデル７２Ｂとの干渉判定においても同様である。第３物体モデル７１Ｃと第３装置モデル７２Ｃとの干渉判定においても同様である。

また、最も密度が高い物体モデル７１（即ち、第３物体モデル７１Ｃ）と装置モデル７２と（即ち、第３装置モデル７２Ｃ）の干渉判定においては、干渉判定部５７は、干渉閾値による判定のみを行う。すなわち、干渉判定部５７は、第３物体モデル７１Ｃと第３装置モデル７２Ｃとのモデル間距離が干渉閾値以下の場合には干渉すると判定し、第３物体モデル７１Ｃと第３装置モデル７２Ｃとのモデル間距離が干渉閾値よりも大きい場合には干渉の可能性なしと判定する。

さらに、干渉判定のための安全閾値は、点群モデルに応じて、即ち、点群の密度に応じて変更され得る。例えば、点群モデルの点群の密度が高いほど、安全閾値が小さく設定される。この例では、第１物体モデル７１Ａと第１装置モデル７２Ａの干渉判定のための安全閾値である第１安全閾値は、第２物体モデル７１Ｂと第２装置モデル７２Ｂの干渉判定のための安全閾値である第２安全閾値よりも大きい。

一方、干渉判定のための干渉閾値は、点群の密度が変わっても同じであってもよい。この例では、第１物体モデル７１Ａと第１装置モデル７２Ａとの干渉判定、第２物体モデル７１Ｂと第２装置モデル７２Ｂとの干渉判定、及び、第３物体モデル７１Ｃと第３装置モデル７２Ｃとの干渉判定のそれぞれにおける干渉閾値は、同じである。

尚、干渉判定において点群モデルの密度を変更する際には、物体モデル７１及び装置モデル７２の一方だけの密度を変更してもよい。例えば、第１物体モデル７１Ａ及び第１装置モデル７２Ａから点群モデルの密度を上げる場合に、第１物体モデル７１Ａだけを第２物体モデル７１Ｂに変更して、第１装置モデル７２Ａはそのままであってもよい。物体モデル７１及び装置モデル７２の少なくとも一方の点群の密度を高くする限り、様々な密度の物体モデル７１と装置モデル７２との組み合わせで干渉判定を行ってもよい。

経路生成部５６は、干渉の可能性がないと干渉判定部５７によって判定された場合に、次の中間位置、即ち、次の経路を生成する。一方、経路生成部５６は、干渉すると干渉判定部５７によって判定された場合には、中間位置、即ち、経路を変更する。経路生成部５６は、次の中間位置又は変更後の中間位置に対しても干渉判定部５７による干渉判定を受ける。

さらに、経路生成部５６は、中間位置（即ち、経路）を生成する場合、今回の中間位置に繋がる前の中間位置の余裕距離が今回の中間位置の装置モデル７２の移動量よりも大きいときには、今回の中間位置に対する干渉判定を省略して、即ち、スキップして、次の中間位置を生成する。つまり、前の中間位置の干渉判定におけるモデル間距離が今回の中間位置への装置モデル７２の移動量に比べて十分に大きい場合には、経路生成部５６は、今回の中間位置について干渉判定部５７による干渉判定を受けることなく、次の中間位置を生成する。

経路生成部５６は、このような処理を繰り返すことによって、開始位置から目標位置までの中間位置、即ち、経路を段階的に生成する。経路生成部５６は、開始位置から目標位置までの経路の生成が完了すると、生成された経路の情報を動作指令部５８へ出力する。

動作指令部５８は、経路生成部５６から出力される経路の情報に応じた指令値を作成し、作成された指令値をロボット制御装置２へ出力する。

ロボット制御装置２は、動作指令部５８からの指令値に基づいてサーボモータ１５を駆動する。このとき、ロボット制御装置２は、エンコーダ１５ａの検出結果に基づいてサーボモータ１５への供給電流をフィードバック制御する。これにより、ロボットアーム１２が開始位置から目標位置まで、経路生成部５６によって生成された経路に沿って移動する。

続いて、このように構成されたロボットシステム１００の動作について説明する。図５は、制御装置５によるロボットアーム１２の動作制御を示すフローチャートである。ロボットアーム１２がコンテナ９１内から所定の物体Ａを取り出し、その物体Ａを載置台まで搬送するピックアンドプレース動作について説明する。制御装置５は、ピックアンドプレース動作をロボットアーム１２が物体Ａを取りに行くまでの第１動作とロボットアーム１２が物体Ａをコンテナ９１から載置台まで搬出する第２動作とに分けてロボットアーム１２に実行させる。この動作制御は、制御部５１が、記憶部５２から動作制御プログラム６１をメモリ５３に読み出して展開することによって実行される。

まず、制御部５１は、ロボットアーム１２が物体Ａを取りに行くまでの第１動作を実行させる。制御部５１は、ステップＳ１において、三次元ビジョンセンサ４を介して空間情報を取得する。三次元ビジョンセンサ４は、コンテナ９１及び載置台の上方から、コンテナ９１、載置台及びコンテナ９１内の物体の位置及び形状を計測する。三次元ビジョンセンサ４は、コンテナ９１、載置台及びコンテナ９１内の物体の位置及び形状を点群データの形式で制御装置５へ出力する。

次に、ステップＳ２において、制御部５１は、ロボットアーム１２の目標位置を取得する。具体的には、経路生成部５６は、まず三次元ビジョンセンサ４の空間情報に基づいて物体Ａの位置を検出する。そして、経路生成部５６は、検出された位置に配置された物体Ａを他の物体に干渉することなく把持する状態のロボットアーム１２の位置を求め、その位置を目標位置として設定する。

続いて、ステップＳ３において、制御部５１は、ロボットアーム１２の開始位置を取得する。具体的には、経路生成部５６は、各エンコーダ１５ａの検出結果に基づいてロボットアーム１２の現在位置を求め、その現在位置を開始位置として設定する。経路生成部５６は、各エンコーダ１５ａの検出結果から各関節の回転角を得ることができる。ロボットアーム１２の各リンク及びハンドの寸法は既知である。そのため、経路生成部５６は、各関節の回転角と各リンク及びハンドの寸法とに基づいて、ロボットアーム１２の各部分の位置を求めることができる。

次に、ステップＳ４において、経路生成部５６は、開始位置から目標位置までのロボットアーム１２の経路を生成する。ステップＳ４では、経路生成処理が実行され、経路が生成される。

ステップＳ５において、制御部５１は、ロボットアーム１２に動作を実行させる。具体的には、動作指令部５８は、経路生成部５６から出力された経路に沿ってロボットアーム１２を移動させるための指令値に生成し、その指令値をロボット制御装置２へ出力する。ロボット制御装置２は、指令値に基づいて、サーボモータ１５を制御して、ロボットアーム１２を動作させる。ロボットアーム１２は、経路に沿って移動する。

制御部５１は、ステップＳ６において、目標位置が最終目標位置か否かを判定する。つまり、ピックアンドプレース動作のように、動作が分割され、複数の目標位置が含まれている場合がある。ステップＳ６では、今回の目標位置が最終目標位置であるか否か、即ち、次に設定されるべき目標位置が残っているか否かが判定される。今回の処理では第１動作の完了後には第２動作が残っているので、今回の目標位置は最終目標位置ではないと判定される。

今回の目標位置が最終目標位置の場合には、制御部５１は、ロボットアーム１２の動作制御を終了する。尚、目標位置が１つだけの場合には、当然ながら、今回の目標位置が最終目標位置となる。

今回の目標位置が最終目標位置でない場合、制御部５１は、ステップＳ２へ戻り、ステップＳ２からの処理を繰り返す。ステップＳ２において、次の動作の目標位置が新たに設定され、ステップＳ３以降の処理が実行される。今回の処理では、制御部５１は、第２動作のロボットアーム１２の目標位置を新たな目標位置として設定する。具体的には、経路生成部５６は、他の物体に干渉することなく物体Ａを載置台に載置する状態のロボットアーム１２の位置を求め、その位置を新たな目標位置として設定する。

こうして、ロボットアーム１２の目標位置が更新される。そして、新たな目標位置に関して、ステップＳ３からステップＳ６の処理が繰り返される。今回の処理では、ロボットアーム１２は、最終的に第２動作の目標位置まで移動し、物体Ａを載置台に載置する。

ロボットアーム１２が新たな目標位置に到達した場合には、制御部５１は、再びステップＳ７において、目標位置が最終目標位置か否かを判定する。目標位置が最終目標位置となるまで、前述の処理が繰り返される。今回の処理では、第２動作の目標位置が最終目標位置であるので、制御部５１は、ロボットアーム１２の動作制御を終了する。

続いて、経路生成処理について説明する。図６は、制御装置５によるロボットアーム１２の経路生成処理を示すフローチャートである。経路生成処理は、制御部５１が、記憶部５２から経路生成プログラム６２をメモリ５３に読み出して展開することによって実行される。

まず、経路生成部５６は、ステップｓｂ１において、ロボットアーム１２の経路を生成する。経路生成処理における最初のステップｓｂ１においては、経路生成部５６は、開始位置から最初に経由する中間位置を生成する。経路生成部５６は、前述の如く、中間位置をランダム又は所定のルール（制約）に従って生成する。中間位置の生成は、中間位置までの経路の生成とみなすことができる。以下、「今回の」中間位置又は経路という場合には、このステップｓｂ１において生成された最新の中間位置及び経路を指す。

次に、物体設定部５４は、ステップｓｂ２において、ロボットアーム１２の配置空間に含まれる物体の物体モデル７１であって最も密度の低い物体モデル７１である第１物体モデル７１Ａを生成する。第１物体モデル７１Ａの全てが第１密度の点群で形成されている。この例では、物体設定部５４は、コンテナ９１、載置台及びコンテナ９１内の物体のそれぞれの第１物体モデル７１Ａを生成する。

また、装置設定部５５は、ステップｓｂ３において、配置空間におけるロボットアーム１２の装置モデル７２であって最も密度の低い装置モデル７２である第１装置モデル７２Ａを生成する。第１装置モデル７２Ａの全てが第１密度の点群で形成されている。このとき、装置設定部５５は、今回の中間位置に位置するロボットアーム１２の装置モデル７２を生成する。

次に、ステップｓｂ４において、干渉判定部５７は、干渉判定をスキップするか否かを判定する。具体的には、干渉判定部５７は、今回の中間位置に繋がる前の中間位置の余裕距離が記憶部５２に記憶されている場合には、余裕距離が今回の中間位置の第１装置モデル７２Ａの移動量よりも大きいか否かを判定する。経路生成処理における最初のステップｓｂ４においては、記憶部５２に前の中間位置の余裕距離が記憶されていないので、干渉判定部５７は、干渉判定をスキップしないと判定する。ステップｓｂ４の詳細な処理については後述する。

干渉判定をスキップしない場合には、干渉判定部５７は、ステップｓｂ５において、第１物体モデル７１Ａと第１装置モデル７２Ａとのモデル間距離が第１安全閾値以下であるか否かを判定する。第１安全閾値は、第１物体モデル７１Ａ及び第１装置モデル７２Ａの密度において、第１物体モデル７１Ａと第１装置モデル７２Ａとが干渉しないと推定できるモデル間距離である。基本的には、干渉判定部５７は、第１物体モデル７１Ａに含まれる点と第１装置モデル７２Ａに含まれる点との全ての組み合わせについて２点間の距離を演算し、２点間の最短距離、即ち、モデル間距離を求める。干渉判定部５７は、求めたモデル間距離が第１安全閾値以下か否かを判定する。ただし、何れかの２点間の距離が第１安全閾値以下となる場合には、干渉判定部５７は、全ての２点間の距離の演算が完了していなくても、モデル間距離が第１安全閾値以下であると判定することができる。

このとき、記憶部５２に後述する注目部分が保存されている場合には、干渉判定部５７は、第１物体モデル７１Ａに含まれる点と第１装置モデル７２Ａに含まれる点のうち注目部分に対応する点を優先的に、即ち、注目部分に対応する点から先に２点間の距離を演算する。注目部分は、干渉の可能性がある部分であり、２点間の距離が第１安全閾値以下となる可能性が他の部分に比べて高い。そのため、モデル間距離が第１安全閾値以下か否かの判定が早く終わり、２点間の距離の演算負荷を低減できる場合がある。

この例では、干渉判定部５７は、ロボットアーム１２の複数のリンクのそれぞれについてモデル間距離を求め、リンクごとにモデル間距離と第１安全閾値とを比較する。ここでのリンクには、ハンド１４も含まれる。つまり、第１リンク１２ａ、第２リンク１２ｂ、第３リンク１２ｃ、第４リンク１２ｄ、第５リンク１２ｅ及びハンド１４のそれぞれにおいて、モデル間距離が求められ、モデル間距離が第１安全閾値以下であるか否かが判定される。干渉判定部５７は、複数のリンクの少なくとも１つのリンクのモデル間距離が第１安全閾値以下か否かを判定する。尚、第１安全閾値は、複数のリンクで同じ値であってもよいし、リンクごとに異なっていてもよい。

さらに、干渉判定部５７は、複数のリンクのうちロボットアーム１２の先端側のリンクから順に、モデル間距離を求め、求められたモデル間距離と第１安全閾値とを比較する。この場合、干渉判定部５７は、先端側のリンクから順にモデル間距離と第１安全閾値とを比較し、何れか１つのリンクのモデル間距離が第１安全閾値以下となる場合には、残りのリンクについてモデル間距離の演算等を行わなくても、複数のリンクの少なくとも１つのリンクのモデル間距離が第１安全閾値以下であると判定することができる。

尚、詳しくは後述するが、干渉判定を省略するリンクが記憶部５２に保存されている場合には、干渉判定部５７は、そのリンクの干渉判定（即ち、モデル間距離の演算及びモデル間距離と安全閾値との比較等）を行わない。

モデル間距離が第１安全閾値よりも大きい場合には、干渉判定部５７は、第１物体モデル７１Ａと第１装置モデル７２Ａとの干渉の可能性がないと判定する。干渉の可能性がないと判定されると、今回の中間位置は、開始位置から目標位置までの間の一の中間位置、即ち、一の経路として確定される。このとき、干渉判定部５７は、モデル間距離から第１安全閾値を減じた値を余裕距離とし、記憶部５２に保存する。これにより、今回の中間位置、即ち、経路に対する干渉判定が完了する。

リンクごとにモデル間距離を求める例では、干渉判定部５７は、リンクごとにモデル間距離と第１安全閾値とを比較しているので、複数のリンクの全てのリンクのモデル間距離が第１安全閾値よりも大きい場合に、干渉の可能性がないと判定する。干渉判定部５７は、リンクのそれぞれについて余裕距離を求め、リンクごとの余裕距離を記憶部５２に保存する。

その後、経路生成部５６は、ステップｓｂ１３において、経路が目標位置に到達したか否かを判定する。具体的には、経路生成部５６は、今回の中間位置が目標位置の１つ手前の中間位置か否かを判定する。今回の中間位置が目標位置の１つ手前の中間位置である場合には、経路が目標位置に到達したと判定して、経路生成部５６は、図５に示すロボットアーム１２の動作制御のフロー（即ち、ステップＳ５）へ戻る。一方、今回の中間位置が目標位置の１つ手前の中間位置でない場合には、経路生成部５６は、ステップｓｂ１へ戻り、次の中間位置を生成する。

ステップｓｂ１では、経路生成部５６は、確定した中間位置（以下、「前の中間位置」と称する）に続く次の中間位置、即ち、経路を生成する。その後、ステップｓｂ２において、第１物体モデル７１Ａが設定される。物体設定部５４は、既に生成されている物体モデル７１が第１物体モデル７１Ａである場合には、その第１物体モデル７１Ａをそのまま用いる。続くステップｓｂ３において、装置設定部５５は、新たな中間位置に位置するロボットアーム１２の第１装置モデル７２Ａを生成する。

次のステップｓｂ４においては、経路生成部５６は、干渉判定をスキップするか否かを判定する。経路生成部５６は、今回の中間位置に繋がる前の中間位置での余裕距離が記憶部５２に記憶されている場合には、余裕距離が今回の中間位置への第１装置モデル７２Ａの移動量よりも大きいか否かを判定する。今回の中間位置への第１装置モデル７２Ａの移動量は、第１装置モデル７２Ａが前の中間位置から今回の中間位置へ移動するときの、前の中間位置における第１装置モデル７２Ａの最接近点（即ち、第１物体モデル７１Ａに最も接近した部分）の移動量である。

余裕距離が第１装置モデル７２Ａの移動量よりも大きい場合には、干渉判定が省略され、即ち、干渉判定がスキップされる。経路生成部５６は、ステップｓｂ１３において、経路が目標位置に到達したか否かを判定する。経路が目標位置に到達した場合には、経路生成部５６は、図５に示すロボットアーム１２の動作制御のフロー（即ち、ステップＳ５）へ戻る。一方、経路が目標位置に到達していない場合には、経路生成部５６は、ステップｓｂ１へ戻り、次の中間位置を生成する。このように、経路生成部５６は、今回の中間位置に対する干渉判定をスキップして、今回の中間位置を確定させ、その後の処理を実行する。

余裕距離が今回の中間位置への第１装置モデル７２Ａの移動量よりも大きい場合には、今回の中間位置に位置する第１装置モデル７２Ａに対する干渉判定においても、モデル間距離が第１安全距離よりも大きくなる可能性が高い。つまり、干渉判定をスキップしない場合には、ステップｓｂ５においてモデル間距離の算出が行われた後にステップｓｂ１３へ進む処理が実行されると予測される。そのため、干渉判定部５７が干渉判定をスキップすることによって、今回の中間位置におけるモデル間距離の算出を省略することができる。

干渉判定をスキップした後に、ステップｓｂ１，ｓｂ２，ｓｂ３を経て再びステップｓｂ４へ戻ってきた場合には、前の中間位置での余裕距離が記憶部５２に記憶されていないので、干渉判定部５７は、ステップｓｂ５において、第１物体モデル７１Ａと新しい中間位置における第１装置モデル７２Ａとのモデル間距離を算出し、モデル間距離が第１安全閾値以下か否かを判定する。モデル間距離が第１安全閾値よりも大きい場合には、前述のように、干渉判定部５７は、干渉の可能性がないと判定し、今回の中間位置を確定させ、今回の余裕距離を記憶部５２に保存する。処理はステップｓｂ１へ戻り、経路生成部５６は、次の中間位置を生成する。

物体から離れた開始位置からロボットアーム１２の経路が生成される場合には、第１装置モデル７２Ａがいずれかの第１物体モデル７１Ａに接近するまでは、前述の処理が繰り返され、開始位置から目標位置に向かって中間位置、即ち、経路が順次生成されていく。やがて、第１装置モデル７２Ａが第１物体モデル７１Ａに接近すると、モデル間距離が第１安全閾値以下となり得る。図７は、第１装置モデル７２Ａが第１物体モデル７１Ａに接近した状態を示す模式図である。図７では、コンテナ９１の第１物体モデル７１Ａ及びロボットアーム１２の第１装置モデル７２Ａが示されている。

尚、リンクごとにモデル間距離を求める例では、経路生成部５６は、ステップｓｂ４において、経路に対する干渉判定を省略するか、即ち、スキップするか否かをリンクごとに判定する。つまり、経路生成部５６は、余裕距離が今回の中間位置への第１装置モデル７２Ａの移動量よりも大きいか否かを複数のリンクのそれぞれについて判定する。今回の中間位置への第１装置モデル７２Ａの移動量は、各リンクの最接近点の移動量である。全てのリンクにおいて余裕距離が第１装置モデル７２Ａの移動量よりも大きい場合には、ロボットアーム１２の全体として干渉判定が省略、即ち、スキップされ、経路生成部５６は、ステップｓｂ１３へ進む。一方、全てのリンクではなく一部のリンクにおいて余裕距離が第１装置モデル７２Ａの移動量よりも大きい場合には、経路生成部５６は、ロボットアーム１２の全体としての干渉判定は省略しない。この場合、経路生成部５６は、干渉判定を省略するリンクを記憶部５２に保存する。その後、ステップｓｂ５において、干渉判定部５７は、干渉判定を省略するリンクが記憶部５２に保存されている場合には、そのリンクの干渉判定（即ち、モデル間距離の演算及びモデル間距離と安全閾値との比較等）を省略する。ロボットアーム１２においては、リンクごとに可動範囲が異なるため、物体と干渉する可能性もリンクごとに異なる。そのため、モデル間距離が十分にあるために物体との干渉の可能性が低いリンクについては干渉判定が省略され、それ以外のリンクについてのみ干渉判定が行われる。これにより、モデル間距離の演算等の負荷が低減される。

ステップｓｂ５においてモデル間距離が第１安全閾値以下の場合には、干渉判定部５７は、第１物体モデル７１Ａと第１装置モデル７２Ａとの干渉の可能性があると判定する。干渉判定部５７は、第１物体モデル７１Ａ及び第１装置モデル７２Ａにおいて、それぞれの最接近点を含む所定の範囲の部分を注目部分として記憶部５２に保存する。つまり、記憶部５２には、第１物体モデル７１Ａにおける注目部分と第１装置モデル７２Ａにおける注目部分との１組の注目部分が保存される。最接近点は、干渉の可能性があると判定された部分とみなすことができる。尚、何れかの２点間の距離が第１安全閾値以下となる場合には、干渉判定部５７は、第１物体モデル７１Ａ及び第１装置モデル７２Ａにおいて、距離が第１安全閾値以下である２点のそれぞれを含む所定の範囲の部分を１組の注目部分として記憶部５２に保存する。

リンクごとにモデル間距離を求める例では、干渉判定部５７は、リンクごとにモデル間距離と第１安全閾値とを比較しているので、複数のリンクの少なくとも１つのリンクのモデル間距離が第１安全閾値以下の場合に、干渉の可能性があると判定する。そして、モデル間距離が第１安全閾値以下となるリンクの最接近点を含む所定の範囲の部分とそれに対応する、第１物体モデル７１Ａの最接近点を含む所定の範囲の部分とが１組の注目部分として記憶部５２に保存される。このとき、モデル間距離が第１安全閾値以下となるリンクが複数存在する場合には、モデル間距離が第１安全閾値以下となる複数のリンクのそれぞれにおける最接近点を含む所定の範囲の部分とそれらに対応する第１物体モデル７１Ａの最接近点を含む所定の範囲の部分とが注目部分として記憶部５２に保存される。つまり、複数組の注目部分が記憶部５２に保存される。

モデル間距離が第１安全閾値以下の場合には、干渉判定部５７は、ステップｓｂ６において、モデル間距離が干渉閾値以下か否かを判定する。干渉閾値は、第１安全閾値よりも小さい値である。干渉閾値は、第２物体モデル７１Ｂ及び第２装置モデル７２Ｂによる干渉判定、並びに、第３物体モデル７１Ｃ及び第３装置モデル７２Ｃによる干渉判定において用いられる干渉閾値と共通である。干渉閾値は、第３物体モデル７１Ｃ及び第３装置モデル７２Ｃの密度において第３物体モデル７１Ｃと第３装置モデル７２Ｃとが干渉すると推定できるモデル間距離である。

モデル間距離が干渉閾値以下である場合には、干渉判定部５７は、第１物体モデル７１Ａと第１装置モデル７２Ａとが干渉すると判定する。尚、何れかの２点間の距離が干渉閾値以下となる場合には、干渉判定部５７は、全ての２点間の距離の演算が完了していなくても、モデル間距離が干渉閾値以下であると判定することができる。これにより、今回の中間位置、即ち、経路に対する干渉判定を終了する。

経路生成部５６は、ステップｓｂ７において、ステップｓｂ１において生成された中間位置を変更する。経路生成部５６は、少なくとも装置モデル７２が物体モデル７１から遠ざかるような中間位置を再生成してもよい。その後、処理は、ステップｓｂ２へ戻る。ステップｓｂ２において、物体設定部５４は、既に生成されている物体モデル７１が第１物体モデル７１Ａである場合には、その第１物体モデル７１Ａをそのまま用いる。続くステップｓｂ３において、装置設定部５５は、変更後の中間位置に位置するロボットアーム１２の第１装置モデル７２Ａを生成する。干渉判定部５７は、ステップｓｂ７の経路変更を経た後の最初のステップｓｂ４においては、干渉判定をスキップしないと判定する。その後、干渉判定部５７は、前述のように、モデル間距離が第１安全閾値以下であるかの判定を行い（ステップｓｂ５）、必要であれば、モデル間距離が干渉閾値以下であるかの判定を行う（ステップｓｂ６）。

一方、モデル間距離が干渉閾値よりも大きい場合には、ステップｓｂ８において、物体設定部５４及び装置設定部５５はそれぞれ、物体モデル７１及び装置モデル７２の密度を１段階だけ上昇させる。ステップｓｂ８に至るのは、モデル間距離が干渉閾値よりも大きく且つ第１安全閾値以下の場合である。つまり、第１物体モデル７１Ａと第１装置モデル７２Ａとが干渉しているとも、干渉していないとも判定できず、干渉の可能性があるとしか判定できない場合である。そのため、干渉判定部５７は、密度が１段階高い物体モデル７１及び装置モデル７２を用いてより詳細に干渉判定を行う。具体的には、物体設定部５４は、第１物体モデル７１Ａをより密度が高い第２物体モデル７１Ｂに変更し、装置設定部５５は、第１装置モデル７２Ａをより密度が高い第２装置モデル７２Ｂに変更する。尚、第２装置モデル７２Ｂの位置は、第１装置モデル７２Ａの位置と同じである。図８は、図７における第１物体モデル７１Ａ及び第１装置モデル７２Ａが第２物体モデル７１Ｂ及び第２装置モデル７２Ｂに変更された状態を示す模式図である。

このとき、物体設定部５４は、物体モデル７１のうち注目部分の点群の密度を高くする。物体モデル７１の注目部分は、ステップｓｂ５の干渉判定において干渉の可能性があると判定された部分である最接近点を含む部分であり、記憶部５２に保存されている。この例では、物体設定部５４は、図８に示すように、コンテナ９１のうち注目部分である、壁の一部９１ａを第２密度の点群で形成し、それ以外の部分を第１密度の点群で形成している。

同様に、装置設定部５５は、装置モデル７２のうち注目部分の点群の密度を高くする。装置モデル７２の注目部分は、ステップｓｂ５の干渉判定において干渉の可能性があると判定された部分である最接近点を含む部分であり、記憶部５２に保存されている。図８の例では、装置設定部５５は、ロボットアーム１２のうち注目部分である、第４リンク１２ｄの一部１２ｆを第２密度の点群で形成し、それ以外の部分を第１密度の点群で形成している。

尚、ロボットアーム１２の複数のリンクにおいてモデル間距離が干渉閾値より大きく且つ第１安全閾値以下である場合には、物体設定部５４は、物体モデル７１のうち複数の注目部分の点群の密度を高くし、装置設定部５５は、装置モデル７２のうち複数の注目部分の点群の密度を高くする。

そして、干渉判定部５７は、ステップｓｂ９において、第２物体モデル７１Ｂと第２装置モデル７２Ｂとのモデル間距離が第２安全閾値以下であるか否かを判定する。

第２安全閾値は、第２物体モデル７１Ｂ及び第２装置モデル７２Ｂの密度において、第２物体モデル７１Ｂと第２装置モデル７２Ｂとが干渉しないと推定できるモデル間距離である。第２安全閾値は、第１安全閾値よりも小さい値であって、干渉閾値よりも大きい値である。つまり、第２安全閾値は、第１物体モデル７１Ａ及び第１装置モデル７２Ａよりも高い密度の第２物体モデル７１Ｂ及び第２装置モデル７２Ｂの干渉の可能性を判定するための閾値なので、より精細な判定を行うべく第１安全閾値よりも小さく設定されている。

干渉判定部５７は、第２物体モデル７１Ｂに含まれる点と第２装置モデル７２Ｂに含まれる点との２点間の距離を演算し、モデル間距離を求める。このとき、第２物体モデル７１Ｂ及び第２装置モデル７２Ｂに含まれる点のうち２点間の距離の演算が行われるのは、第２密度の点群、即ち、注目部分に含まれる点である。第２物体モデル７１Ｂ及び第２装置モデル７２Ｂに含まれる点のうち第２密度でない点群の点については２点間の距離の演算は行われない。第２物体モデル７１Ｂ及び第２装置モデル７２Ｂのうち注目部分は、干渉の可能性があると判定された部分であるためである。前述のような２点間の距離の演算を行うことによって、演算量を低減することができる。この例では、干渉判定部５７は、第２物体モデル７１Ｂのうちコンテナ９１の壁の一部９１ａに含まれる点と第２装置モデル７２Ｂのうち第４リンク１２ｄの一部１２ｆに含まれる点との２点間の距離を演算する。

干渉判定部５７は、第２物体モデル７１Ｂ及び第２装置モデル７２Ｂにおいて、それぞれの最接近点を含む所定の範囲の部分を注目部分として更新して、記憶部５２に保存する。

モデル間距離が第２安全閾値よりも大きい場合には、干渉判定部５７は、第２物体モデル７１Ｂと第２装置モデル７２Ｂとの干渉の可能性がないと判定する。干渉の可能性がないと判定されると、今回の中間位置は、開始位置から目標位置までの間の一の中間位置、即ち、一の経路として確定される。このとき、干渉判定部５７は、モデル間距離から第２安全閾値を減じた値を余裕距離とし、記憶部５２に保存する。これにより、今回の中間位置、即ち、経路に対する干渉判定が完了する。

その後、経路生成部５６は、ステップｓｂ１３において、経路が目標位置に到達したか否かを判定する。経路が目標位置に到達した場合には、経路生成部５６は、図５に示すロボットアーム１２の動作制御のフロー（即ち、ステップＳ５）へ戻る。一方、経路が目標位置に到達していない場合には、経路生成部５６は、ステップｓｂ１へ戻り、次の中間位置を生成する。

一方、モデル間距離が第２安全閾値以下の場合には、干渉判定部５７は、第２物体モデル７１Ｂと第２装置モデル７２Ｂとの干渉の可能性があると判定する。尚、何れかの２点間の距離が第２安全閾値以下となる場合には、干渉判定部５７は、全ての２点間の距離の演算が完了していなくても、モデル間距離が第２安全閾値以下であると判定することができる。そのような場合には、干渉判定部５７は、第２物体モデル７１Ｂ及び第２装置モデル７２Ｂにおいて、距離が第２安全閾値以下である２点のそれぞれを含む所定の範囲の部分を注目部分として更新して記憶部５２に保存する。

モデル間距離が第２安全閾値以下の場合には、干渉判定部５７は、ステップｓｂ１０において、モデル間距離が干渉閾値以下か否かを判定する。干渉閾値は、前述の如く、ステップｓｂ６における判定の場合と同じ値である。

モデル間距離が干渉閾値以下である場合には、干渉判定部５７は、第２物体モデル７１Ｂと第２装置モデル７２Ｂとが干渉すると判定する。尚、何れかの２点間の距離が干渉閾値以下となる場合には、干渉判定部５７は、全ての２点間の距離の演算が完了していなくても、モデル間距離が干渉閾値以下であると判定することができる。これにより、今回の中間位置、即ち、経路に対する干渉判定を終了する。

経路生成部５６は、ステップｓｂ７において、ステップｓｂ１において生成された中間位置を変更する。ステップｓｂ７以降の処理は、第１物体モデル７１Ａ及び第１装置モデル７２Ａの干渉判定においてモデル間距離が干渉閾値以下であった場合と同様である。尚、ステップｓｂ７の後はステップｓｂ２，ｓｂ３へ戻るので、変更後の中間位置に対して、第１物体モデル７１Ａ及び第１装置モデル７２Ａから干渉判定が行われる。

一方、モデル間距離が干渉閾値よりも大きい場合には、ステップｓｂ１１において、物体設定部５４及び装置設定部５５はそれぞれ、物体モデル７１及び装置モデル７２の密度を１段階だけ上昇させる。ステップｓｂ１１に至るのは、モデル間距離が干渉閾値よりも大きく且つ第２安全閾値以下の場合である。つまり、第２物体モデル７１Ｂと第２装置モデル７２Ｂとが干渉しているとも、干渉していないとも判定できず、干渉の可能性があるとしか判定できない場合である。そのため、干渉判定部５７は、密度が１段階高い物体モデル７１及び装置モデル７２を用いて干渉判定を行う。具体的には、物体設定部５４は、第２物体モデル７１Ｂをより密度が高い第３物体モデル７１Ｃに変更し、装置設定部５５は、第２装置モデル７２Ｂをより密度が高い第３装置モデル７２Ｃに変更する。尚、第３装置モデル７２Ｃの位置は、第２装置モデル７２Ｂの位置と同じである。図９は、図８における第２物体モデル７１Ｂ及び第２装置モデル７２Ｂが第３物体モデル７１Ｃ及び第３装置モデル７２Ｃに変更された状態を示す模式図である。

このとき、物体設定部５４は、物体モデル７１のうち注目部分の点群の密度を高くする。物体モデル７１の注目部分は、ステップｓｂ９の干渉判定において干渉の可能性があると判定された部分である最接近点を含む部分であり、記憶部５２に保存されている。この例では、物体設定部５４は、図９に示すように、コンテナ９１のうち注目部分である、壁の一部９１ａを第３密度の点群で形成し、それ以外の部分を第１密度の点群で形成している。

同様に、装置設定部５５は、装置モデル７２のうち注目部分の点群の密度を高くする。装置モデル７２の注目部分は、ステップｓｂ９の干渉判定において干渉の可能性があると判定された部分である最接近点を含む部分であり、記憶部５２に保存されている。図９の例では、装置設定部５５は、ロボットアーム１２のうち注目部分である、第４リンク１２ｄの一部１２ｆを第３密度の点群で形成し、それ以外の部分を第１密度の点群で形成している。

そして、干渉判定部５７は、ステップｓｂ１２において、第３物体モデル７１Ｃと第３装置モデル７２Ｃとのモデル間距離が干渉閾値以下であるか否かを判定する。干渉閾値は、前述の如く、ステップｓｂ６，ｓｂ１０における判定の場合と同じ値である。

干渉判定部５７は、第３物体モデル７１Ｃに含まれる点と第３装置モデル７２Ｃに含まれる点との２点間の距離を演算し、モデル間距離を求める。このとき、第３物体モデル７１Ｃ及び第３装置モデル７２Ｃに含まれる点のうち２点間の距離の演算が行われるのは、第３密度の点群、即ち、注目部分に含まれる点である。第３物体モデル７１Ｃ及び第３装置モデル７２Ｃに含まれる点のうち第３密度でない点群の点については２点間の距離の演算は行われない。第３物体モデル７１Ｃ及び第３装置モデル７２Ｃのうち注目部分は、干渉の可能性があると判定された部分であるためである。前述のような２点間の距離の演算を行うことによって、演算量を低減することができる。この例では、干渉判定部５７は、第３物体モデル７１Ｃのうちコンテナ９１の壁の一部９１ａに含まれる点と第３装置モデル７２Ｃのうち第４リンク１２ｄの一部１２ｆに含まれる点との２点間の距離を演算する。

干渉判定部５７は、第３物体モデル７１Ｃ及び第３装置モデル７２Ｃにおいて、それぞれの最接近点を含む所定の範囲の部分を注目部分として更新して、記憶部５２に保存する。

モデル間距離が干渉閾値よりも大きい場合には、干渉判定部５７は、第３物体モデル７１Ｃと第３装置モデル７２Ｃとの干渉の可能性がないと判定する。干渉の可能性がないと判定されると、今回の中間位置は、開始位置から目標位置までの間の一の中間位置、即ち、一の経路として確定される。このとき、干渉判定部５７は、モデル間距離から干渉閾値を減じた値を余裕距離とし、記憶部５２に保存する。これにより、今回の中間位置、即ち、経路に対する干渉判定が完了する。

その後、経路生成部５６は、ステップｓｂ１３において、経路が目標位置に到達したか否かを判定する。経路が目標位置に到達した場合には、経路生成部５６は、図５に示すロボットアーム１２の動作制御のフロー（即ち、ステップＳ５）へ戻る。一方、経路が目標位置に到達していない場合には、経路生成部５６は、ステップｓｂ１へ戻り、次の中間位置を生成する。

一方、モデル間距離が干渉閾値以下である場合には、干渉判定部５７は、第３物体モデル７１Ｃと第３装置モデル７２Ｃとが干渉すると判定する。尚、何れかの２点間の距離が干渉閾値以下となる場合には、干渉判定部５７は、全ての２点間の距離の演算が完了していなくても、モデル間距離が干渉閾値以下であると判定することができる。これにより、今回の中間位置、即ち、経路に対する干渉判定を終了する。

経路生成部５６は、ステップｓｂ７において、ステップｓｂ１において生成された中間位置を変更する。ステップｓｂ７以降の処理は、第１物体モデル７１Ａ及び第１装置モデル７２Ａの干渉判定においてモデル間距離が干渉閾値以下であった場合と同様である。尚、ステップｓｂ７の後はステップｓｂ２，ｓｂ３へ戻るので、変更後の中間位置に対して、第１物体モデル７１Ａ及び第１装置モデル７２Ａから干渉判定が行われる。

このように、生成された中間位置、即ち、経路に対して密度の低い物体モデル７１及び装置モデル７２から順に干渉判定が行われる。具体的には、まず第１物体モデル７１Ａ及び第１装置モデル７２Ａを用いて、モデル間距離が第１安全閾値よりも大きいか、第１安全閾値以下で且つ干渉閾値よりも大きいか、又は、干渉閾値以下かが判定される。モデル間距離が第１安全閾値よりも大きい場合には、生成された経路では干渉の可能性がないと判定され、次の経路が生成される。モデル間距離が干渉閾値以下の場合には、生成された経路では干渉が生じると判定され、経路が変更される。モデル間距離が第１安全閾値以下で且つ干渉閾値よりも大きい場合には、干渉の可能性があると判定される。

干渉の可能性があると判定された場合は、干渉の有無をより詳細に判定するために、密度が１段階高い物体モデル７１及び装置モデル７２を用いて干渉判定が行われる。つまり、第２物体モデル７１Ｂ及び第２装置モデル７２Ｂを用いて、モデル間距離が第２安全閾値よりも大きいか、第２安全閾値以下で且つ干渉閾値よりも大きいか、又は、干渉閾値以下かが判定される。モデル間距離が第２安全閾値よりも大きい場合には、生成された経路では干渉の可能性がないと判定され、次の経路が生成される。モデル間距離が干渉閾値以下の場合には、生成された経路では干渉が生じると判定され、経路が変更される。モデル間距離が第２安全閾値以下で且つ干渉閾値よりも大きい場合には、干渉の可能性があると判定される。

干渉の可能性があると判定された場合、干渉の有無をより詳細に判定するために、密度がさらに１段階高い物体モデル７１及び装置モデル７２を用いて干渉判定が行われる。つまり、第３物体モデル７１Ｃ及び第３装置モデル７２Ｃを用いて、モデル間距離が干渉閾値よりも大きいか、又は、干渉閾値以下かが判定される。第３物体モデル７１Ｃ及び第３装置モデル７２Ｃは、最も密度が高いモデルなので、モデル間距離が干渉閾値以下か否かだけが判定される。モデル間距離が干渉閾値よりも大きい場合には、生成された経路では干渉の可能性がないと判定され、次の経路が生成される。モデル間距離が干渉閾値以下の場合には、生成された経路では干渉が生じると判定され、経路が変更される。

このような干渉判定は、物体モデル７１と装置モデル７２とのモデル間距離に基づいて行われる。モデル間距離は、物体モデル７１に含まれる点と装置モデル７２に含まれる点との距離を演算することによって求められる。そのため、点群モデルに含まれる点の個数が増えるほど演算量が増大し、演算時間が長くなる。

それに対し、経路生成装置３は、生成された経路に対して密度が低い第１物体モデル７１Ａ及び第１装置モデル７２Ａを用いて干渉判定を行い、干渉の可能性が無ければ干渉判定を終了して経路を確定し、干渉の可能性があると判定された場合には密度が高い第２物体モデル７１Ｂ及び第２装置モデル７２Ｂに変更して再び干渉判定を行う。これによれば、密度の低い物体モデル７１及び装置モデル７２から順に用いて干渉判定が行われるため、密度の低い第１物体モデル７１Ａ及び第１装置モデル７２Ａを用いた干渉判定によって干渉の有無がわかると、第１物体モデル７１Ａに含まれる点と第１装置モデル７２Ａに含まれる点との距離の演算量が低減される。その一方で、密度の低い物体モデル７１を用いると、干渉判定の精度が低下する。それに対し、経路生成装置３は、第１物体モデル７１Ａ及び第１装置モデル７２Ａを用いた干渉判定によって干渉の有無が判定できない場合には、干渉の可能性ありと判定して、第１物体モデル７１Ａ及び第１装置モデル７２Ａから密度が高い第２物体モデル７１Ｂ及び第２装置モデル７２Ｂに変更して再び干渉判定を行う。より密度が高い第２物体モデル７１Ｂ及び第２装置モデル７２Ｂを用いることによってより高い精度で干渉判定を行うことができる。このように、経路生成装置３の干渉判定によれば、干渉判定の時間の短縮と精度の向上とを両立させることができる。

この例では、経路生成装置３は、物体モデル７１の密度を３段階に設定している。具体的には、経路生成装置３は、密度の異なる第１物体モデル７１Ａ、第２物体モデル７１Ｂ及び第３物体モデル７１Ｃ、並びに、第１装置モデル７２Ａ、第２装置モデル７２Ｂ及び第３装置モデル７２Ｃを密度の低い順に用いて干渉判定を行う。これにより、干渉判定の時間の短縮と精度の向上とのバランスを段階的に取ることができる。

それに加えて、経路生成装置３においては、物体モデル７１及び装置モデル７２の両方が点群モデルで形成される。これにより、物体モデル７１と装置モデル７２との距離の演算は、２点間の距離の演算になる。その結果、物体モデル７１と装置モデル７２との距離の演算が単純化される。

さらに、経路生成装置３は、装置モデル７２の密度を物体モデル７１の密度に合わせて変更する。具体的には、装置モデル７２は、順番に高くなる第１密度、第２密度及び第３密度の点群モデルによって形成され得る。演算量の低減を優先すべく第１物体モデル７１Ａを用いて干渉判定が行われるときには、最高密度が第１密度の第１装置モデル７２Ａが用いられる。密度が最も低い第１物体モデル７１Ａが用いられることに加え、密度が最も低い第１装置モデル７２Ａが用いられることによって、第１物体モデル７１Ａに含まれる点と第１装置モデル７２Ａに含まれる点との２点間の距離の演算量を低減することができる。干渉判定の精度の向上を優先すべく第３物体モデル７１Ｃを用いて干渉判定が行われるときには、最高密度が第３密度の第３装置モデル７２Ｃが用いられる。密度が最も高い第３物体モデル７１Ｃが用いられることに加え、密度が最も高い第３装置モデル７２Ｃが用いられることによって、第３物体モデル７１Ｃと第３装置モデル７２Ｃとの干渉をより高精度に判定することができる。演算量の低減と干渉判定の精度の向上とのバランスを取るべく第２物体モデル７１Ｂを用いて干渉判定が行われるときには、最高密度が第２密度の第２装置モデル７２Ｂが用いられる。密度が中間の第２物体モデル７１Ｂが用いられることに加え、密度が中間の第２装置モデル７２Ｂが用いられることによって、演算負荷を低減しつつ干渉判定の精度を向上させることができる。このように、装置モデル７２の密度の増減の傾向と物体モデル７１の密度の増減の傾向とを一致させることによって、干渉判定の時間の短縮と精度の向上とを両立が実現しやすくなる。

以上のように、ロボットアーム１２（動作装置）の経路を生成する経路生成装置３は、ロボットアーム１２が配置された配置空間に含まれる物体のモデルである物体モデル７１を設定する物体設定部５４と、配置空間におけるロボットアーム１２のモデルである装置モデル７２を設定する装置設定部５５と、ロボットアーム１２の経路を段階的に生成する経路生成部５６と、物体モデル７１と装置モデル７２との距離に基づいて、物体モデル７１と経路に沿って移動した後の装置モデル７２との干渉判定を行う干渉判定部５７とを備え、物体モデル７１及び装置モデル７２の少なくとも一方は、点群で形成された点群モデルであり、干渉の可能性があると干渉判定部５７によって判定された場合には、物体設定部５４及び装置設定部５５の少なくとも一方は、点群モデルの点群の密度を高くし、干渉判定部５７は、点群の密度が高くなった点群モデルを用いて干渉判定を再び行い、経路生成部５６は、干渉の可能性がないと干渉判定部５７によって判定された場合には次の経路を生成する。

換言すると、ロボットアーム１２（動作装置）の経路を生成する経路生成方法は、ロボットアーム１２が配置された配置空間に含まれる物体のモデルである物体モデル７１を設定することと、配置空間におけるロボットアーム１２のモデルである装置モデル７２を設定することと、ロボットアーム１２の経路を段階的に生成することと、物体モデル７１と装置モデル７２との距離に基づいて、物体モデル７１と経路に沿って移動した後の装置モデル７２との干渉判定を行うこととを含み、物体モデル７１及び装置モデル７２の少なくとも一方は、点群で形成された点群モデルであり、干渉の可能性があると干渉判定によって判定された場合には、物体モデル７１を設定すること及び装置モデル７２を設定することの少なくとも一方において、点群モデルの点群の密度を高くし、干渉判定を行うことにおいて、点群の密度が高くなった点群モデルを用いて干渉判定を再び行い、経路を生成することにおいては、干渉の可能性がないと干渉判定によって判定された場合には次の経路を生成する。

また、経路生成プログラム６２は、ロボットアーム１２（動作装置）の動作を生成するためにコンピュータに、ロボットアーム１２が配置された配置空間に含まれる物体のモデルである物体モデル７１を設定する機能と、配置空間におけるロボットアーム１２のモデルである装置モデル７２を設定する機能と、ロボットアーム１２の経路を段階的に生成する機能と、物体モデル７１と装置モデル７２との距離に基づいて、物体モデル７１と経路に沿って移動した後の装置モデル７２との干渉判定を行う機能とを実現させ、物体モデル７１及び装置モデル７２の少なくとも一方は、点群で形成された点群モデルであり、干渉の可能性があると干渉判定によって判定された場合には、物体モデル７１を設定する機能及び装置モデル７２を設定する機能の少なくとも一方は、点群モデルの点群の密度を高くし、干渉判定を行う機能は、点群の密度が高くなった点群モデルを用いて干渉判定を再び行い、経路を生成する機能は、干渉の可能性がないと前記干渉判定によって判定された場合には次の経路を生成する。

これらの構成によれば、ロボットアーム１２の経路が段階的に生成される。経路が生成されると、物体モデル７１と経路に沿って移動した後の装置モデル７２との干渉判定が行われる。干渉判定は、物体モデル７１と装置モデル７２との距離に基づいて行われる。このとき、物体モデル７１及び装置モデル７２の少なくとも一方は、点群モデルである。干渉判定の結果、干渉の可能性があると判定された場合には、点群モデルの点群の密度が高くされ、干渉判定が再び行われる。干渉の可能性がないと判定された場合には、次の経路が生成される。このように、まずは密度の比較的低い点群モデルを用いて干渉判定が行われる。干渉判定は物体モデル７１と装置モデル７２との距離に基づいて行われるため、密度の低い点群モデルを用いた干渉判定では距離の演算負荷が小さい。密度の低い点群モデルを用いた干渉判定において干渉の可能性がないと判定される場合もあり、そのような場合には距離の演算負荷が低減される。一方、密度の低い点群モデルを用いた干渉判定において干渉の可能性があると判定された場合には、密度の高い点群モデルを用いて干渉判定が再び行われる。点群モデルの密度が高くなることによって、干渉の有無をより詳細に判定することができる。その結果、距離の演算負荷の低減と干渉判定の精度の向上とを両立させることができる。

尚、物体モデル７１及び装置モデル７２の少なくとも一方が点群モデルであるので、物体モデル７１及び装置モデル７２の両方が点群モデルであってもよいし、物体モデル７１だけが点群モデルであってもよいし、装置モデル７２だけが点群モデルであってもよい。

物体設定部５４及び装置設定部５５の少なくとも一方は、干渉の可能性があると干渉判定部５７によって判定された場合には、干渉判定において干渉の可能性があると判定された部分を含む、点群モデルの一部の点群の密度を高くする。

この構成によれば、物体設定部５４及び装置設定部５５の少なくとも一方、即ち、物体設定部５４及び装置設定部５５のうち点群モデルを設定する方は、干渉の可能性があると判定された場合には、点群モデルの全部の点群の密度を高くするのではなく、干渉判定において干渉の可能性があると判定された部分を含む、点群モデルの一部の点群の密度を高くする。これにより、物体設定部５４及び装置設定部５５の少なくとも一方が点群モデルの密度を高くする際の処理量を低減することができる。

また、干渉判定部５７は、物体モデル７１と装置モデル７２との最短距離であるモデル間距離が所定の安全閾値以下の場合に、干渉の可能性があると判定し、モデル間距離が安全閾値よりも大きい場合に、干渉の可能性がないと判定する。

この構成によれば、物体モデル７１と装置モデル７２とのモデル間距離が安全閾値以下か否かによって干渉の可能性の有無が判定される。

さらに、干渉判定部５７は、モデル間距離が安全閾値よりも小さな干渉閾値以下の場合に、干渉すると判定し、経路生成部５６は、干渉すると干渉判定部５７によって判定された場合には経路を変更する。

この構成によれば、干渉判定部５７は、モデル間距離に基づいて、干渉の可能性なし、干渉の可能性あり、及び、干渉するの３つの状態を判定する。具体的には、モデル間距離が安全閾値よりも大きい場合には、干渉の可能性がないと判定され、モデル間距離が安全閾値以下であって且つ干渉閾値よりも大きい場合には、干渉の可能性があると判定され、モデル間距離が干渉閾値以下の場合には、干渉すると判定される。

尚、干渉の可能性がないと判定された場合には次の経路が生成され、干渉すると判定された場合には経路が変更され、干渉の可能性があると判定された場合には密度のより高い点群モデルを用いて干渉判定が再度行われる。このような処理が繰り返されることによって、経路が段階的に生成される。

安全閾値は、点群モデルの点群の密度が高いほど小さくなるように設定される。

この構成によれば、点群モデルの点群の密度に応じた適切な安全閾値を用いて干渉判定を行うことができる。つまり、点群モデルの密度が低い場合には、点群モデルが精細ではなく、このときには安全閾値が比較的大きく設定される。これにより、干渉するにもかかわらず、干渉しないと誤判定してしまうことを防止することができる。一方、点群モデルの密度が高い場合には、点群モデルが精細であり、このときには安全閾値が比較的小さく設定される。点群モデルが精細なので、安全閾値を比較的小さくしても、干渉の可能性の有無を正確に判定することができる。

また、動作装置としてのロボットアーム１２は、複数のリンクと、複数のリンクを連結する複数の関節とを有し、干渉判定部５７は、複数のリンクのそれぞれについてモデル間距離を求め、複数のリンクの少なくとも１つのリンクのモデル間距離が安全閾値以下の場合に、干渉の可能性があると判定し、複数のリンクの全てのリンクのモデル間距離が安全閾値よりも大きい場合に、干渉の可能性がないと判定する。

この構成によれば、ロボットアーム１２のリンクごとにモデル間距離が求められ、リンクごとにモデル間距離と安全閾値との比較が行われ、リンクごとに干渉の可能性の有無が判定される。

さらに、干渉判定部５７は、複数のリンクのうちロボットアーム１２の先端側のリンクから順に、モデル間距離を求め、求められたモデル間距離と安全閾値とを比較する。

この構成によれば、ロボットアーム１２の先端側に位置するリンクから順に、干渉の可能性の有無が判定される。ロボットアーム１２においては、先端側のリンクほど可動範囲が大きいので、先端側のリンクほど物体との干渉の可能性が高い。干渉判定部５７は、モデル間距離が安全閾値以下のリンクが１つでもあれば、干渉の可能性があると判定する。つまり、複数のリンクにおいてモデル間距離が安全閾値以下のリンクが１つでも見つかれば、残りのリンクについてはモデル間距離の演算等を省略することができる。先端側のリンクから順に干渉の可能性の有無を判定することによって、干渉の可能性があるリンクを早期に発見することができ、モデル間距離の演算等の負荷を低減できる可能性が高まる。

また、干渉判定部５７は、干渉の可能性がないと判定した場合、モデル間距離から安全閾値を減じた値である余裕距離を求め、経路生成部５６は、経路を生成する場合、今回の経路に繋がる前の経路の余裕距離が今回の経路の装置モデル７２の移動量よりも大きいときには、今回の経路に対する干渉判定を省略する。

この構成によれば、経路生成部５６が新たな経路を生成したときに、その経路に沿った装置モデル７２の移動量よりも前の経路に対する干渉判定における余裕距離が大きいときには、今回の経路に対して干渉判定を省略して次の経路を生成する。今回の経路に沿って装置モデル７２が移動しても、物体モデル７１と装置モデル７２とのモデル間距離は安全閾値よりも大きくなる可能性が高い。これにより、干渉判定がスキップされるので、干渉判定に関する演算負荷が低減され、全体的な処理が簡略化される。

さらに、動作装置としてのロボットアーム１２は、複数のリンクと、複数のリンクを連結する複数の関節とを有し、干渉判定部５７は、複数のリンクのそれぞれについてモデル間距離を求め、複数のリンクの少なくとも１つのリンクのモデル間距離が所定の安全閾値以下の場合に、干渉の可能性があると判定し、複数のリンクの全てのリンクのモデル間距離が安全閾値よりも大きい場合に、干渉の可能性がないと判定し、複数のリンクのそれぞれについて余裕距離を求め、経路生成部５６は、経路を生成する場合、今回の経路に対する干渉判定を省略するか否かを複数のリンクごとに判定する。

この構成によれば、干渉判定に関する演算負荷をより低減することができる。つまり、動作装置としてのロボットアーム１２の全体で干渉判定を省略するか否かが判定される構成では、ロボットアーム１２に干渉判定を省略可能なリンクが含まれていても干渉判定を省略可能でないリンクが含まれていれば、ロボットアーム１２としては干渉判定を省略しない。一方、リンクごとに干渉判定を省略するかが判定される構成では、ロボットアーム１２に干渉判定を省略可能なリンクと干渉判定を省略可能でないリンクとが含まれている場合に、干渉判定を省略可能なリンクの干渉判定を省略し、干渉判定を省略可能できないリンクだけを干渉判定することができる。これにより、干渉判定に関する演算負荷をより低減することができる。

また、物体モデル７１及び装置モデル７２のうち少なくとも物体モデル７１は、点群モデルである。

この構成によれば、配置空間における未知な物体に対しても点群モデルを用いた干渉判定を容易に実現できる。詳しくは、通常、装置モデル７２を点群モデルで形成する場合は、装置モデル７２の点群データを予め準備、即ち、保存しておくことが多い。そのため、装置モデル７２を点群モデルで形成する場合には、点群データを予め準備しておく必要がある。それに対し、配置空間における物体の情報は、通常、三次元ビジョンセンサ４等の物体情報を取得する装置によって取得されることが多い。このような装置によれば、物体の点群データを容易に取得することができる。そのため、装置モデル７２を点群モデルで形成する場合には、点群データを予め準備しおく必要がなく、その場で三次元ビジョンセンサ４等によって点群データを取得することができる。これにより、未知な物体に対しても点群モデルを用いた干渉判定を容易に実現できる。

さらに、物体モデル７１及び装置モデル７２は両方とも、点群モデルである。

この構成によれば、物体モデル７１と装置モデル７２との距離は、物体モデル７１に含まれる点と装置モデル７２に含まれる点との距離になる。つまり、距離の演算は、２点間の距離の演算となり、演算自体は簡単となる。その一方で、物体モデル７１及び装置モデル７２に含まれる点の個数が増加すると、演算負荷が増大する。それに対し、前述のように、密度の低い点群モデルから順番に用いて干渉判定を行うことによって、演算負荷が低減される。

さらに、干渉の可能性があると干渉判定部５７によって判定された場合には、物体設定部５４は、物体モデル７１の点群の密度を高くし、装置設定部５５は、装置モデル７２の点群の密度を高くし、干渉判定部５７は、点群の密度が高くなった装置モデル７２と点群の密度が高くなった物体モデル７１との干渉判定を行う。

この構成によれば、干渉判定において干渉の可能性があると判定された場合には、物体モデル７１及び装置モデル７２の両方の点群の密度が高くされる。つまり、演算負荷の低減を図るために物体モデル７１の密度が低ときには装置モデル７２の密度も低いので、演算負荷の低減が促進される。一方、干渉判定の精度の向上を図るために物体モデル７１密度が高いときには装置モデル７２の密度も高いので、干渉判定の精度の向上が促進される。

また、物体設定部５４は、干渉判定において干渉の可能性があると判定された部分を含む、物体モデル７１の一部の点群の密度を高くし、装置設定部５５は、干渉判定において干渉の可能性があると判定された部分を含む、装置モデル７２の一部の点群の密度を高くする。

この構成によれば、物体設定部５４は、干渉の可能性があると判定された場合には、物体モデル７１の全部の点群の密度を高くするのではなく、干渉判定において干渉の可能性があると判定された部分を含む、物体モデル７１の一部の点群の密度を高くする。これにより、物体設定部５４が物体モデル７１の密度を高くする際の処理量を低減することができる。同様に、装置設定部５５は、干渉の可能性があると判定された場合には、装置モデル７２の全部の点群の密度を高くするのではなく、干渉判定において干渉の可能性があると判定された部分を含む、装置モデル７２の一部の点群の密度を高くする。これにより、装置設定部５５が装置モデル７２の密度を高くする際の処理量を低減することができる。

《その他の実施形態》
以上のように、本出願において開示する技術の例示として、前記実施形態を説明した。しかしながら、本開示における技術は、これに限定されず、適宜、変更、置き換え、付加、省略などを行った実施の形態にも適用可能である。また、前記実施形態で説明した各構成要素を組み合わせて、新たな実施の形態とすることも可能である。また、添付図面および詳細な説明に記載された構成要素の中には、課題解決のために必須な構成要素だけでなく、前記技術を例示するために、課題解決のためには必須でない構成要素も含まれ得る。そのため、それらの必須ではない構成要素が添付図面や詳細な説明に記載されていることをもって、直ちに、それらの必須ではない構成要素が必須であるとの認定をするべきではない。

例えば、動作装置は、ロボットアーム１２及びロボット１に限定されない。動作を行う装置であれば、任意の装置が動作装置の対象となり得る。例えば、動作装置は、自走式のロボットなど、ロボットアームを備えないロボットであってもよい。また、動作装置は、ドローン等の移動体であってもよい。また、ロボット１は、産業用ロボットに限定されない。

また、動作装置は、ロボットアーム１２ではなく、ハンド１４の指１４ａであってもよい。

物体設定部５４が生成する物体モデル７１の基となる点群データは、三次元ビジョンセンサ４から出力される点群データに限定されない。物体設定部５４は、例えば、ＲＧＢ－Ｄカメラから出力されるＲＧＢ－Ｄ画像から点群データを生成してもよい。また、物体設定部５４は、ＲＧＢ－Ｄ画像の他に、ステレオカメラから出力されるＲＧＢ画像から点群データを生成してもよいし、デプス画像、ボクセル等に基づいて点群データを生成してもよい。

さらには、物体設定部５４は、センサやカメラ等のような装置から出力されるデータに基づいて点群データを生成する構成に限定されず、予め取得され、記憶部５２又は他の装置に記憶されている点群データに基づいて物体モデル７１を生成してもよい。

物体モデル７１及び装置モデル７２の一方は、点群モデルでなくてもよい。例えば、物体設定部５４は、物体モデル７１を点群モデルで生成する一方、装置設定部５５は、装置モデル７２を点群モデル以外のモデルによって生成してもよい。逆に、物体設定部５４は、物体モデル７１を点群モデル以外のモデルによって生成する一方、装置設定部５５は、装置モデル７２を点群モデルで生成してもよい。点群モデル以外のモデルの例としては、装置モデル７２が点群モデル以外のモデルである場合、装置設定部５５は、実際の動作装置（この例では、ロボットアーム１２）と同様の外表面を有する装置モデル７２を生成してもよい。あるいは、装置設定部５５は、実際の動作装置の外形を円柱や多角形に近似して形成した装置モデル７２、又は、ポリゴンで形成した装置モデル７２等であってもよい。この場合、物体モデルに含まれる点と装置モデルとの距離は、物体モデルに含まれる点と装置モデル７２を形成する面までの距離となる。物体設定部５４が点群モデル以外のモデルを生成する場合も同様である。

物体設定部５４は、物体モデル７１の密度を３段階で変更しているが、これに限定されるものではない。物体設定部５４は、物体モデル７１の密度を２段階、又は、４段階以上に変更してもよい。物体設定部５４は、物体モデル７１の密度を変更する場合に、物体モデル７１のうち注目部分を含む一部だけの密度ではなく、物体モデル７１の全体の密度を変更してもよい。

装置設定部５５は、装置モデル７２の密度を３段階で変更しているが、これに限定されるものではない。装置設定部５５は、装置モデル７２の密度を２段階、又は、４段階以上に変更してもよい。また、装置モデル７２の第１密度、第２密度及び第３密度はそれぞれ、物体モデル７１の第１密度、第２密度及び第３密度と同程度でなくてもよい。装置設定部５５は、装置モデル７２の密度を変更する場合に、装置モデル７２のうち注目部分を含むリンクだけの密度ではなく、装置モデル７２の全体、即ち、ロボットアーム１２の全体の密度を変更してもよい。あるいは、装置設定部５５は、装置モデル７２の密度を変更する場合に、注目部分を含むリンクの全体の密度ではなく、注目部分を含むリンクのうち注目部分を含む一部だけの密度を変更してもよい。

また、干渉判定において干渉の可能性があると判定された場合には、少なくとも物体モデル７１の密度が変更されればよく、装置モデル７２の密度の変更は必須ではない。

動作装置の経路は、前述のピックアンドプレース動作における経路に限定されない。本技術は、動作装置の様々な動作における経路の生成に適用することができる。

経路の開始位置は、エンコーダ１５ａの検出結果によって取得されているが、これに限定されない。目標位置は、三次元ビジョンセンサ４からの空間情報を画像認識技術によって画像認識することによって取得されているが、これに限定されない。開始位置又は目標位置は、外部からの入力を受け付けることによって、制御部５１が取得してもよい。例えば、制御装置５がタッチパネル等の操作入力部を有し、タッチパネルに表示された配置空間の画像からユーザが開始位置及び／又は目標位置を選択してもよい。あるいは、開始位置及び／又は目標位置は、予め決められ、記憶部５２等に記憶されていてもよい。

安全閾値は、物体モデル７１の点群の密度が高いほど小さくなるように設定されているが、これに限定されない。つまり、第１安全閾値は、第２安全閾値よりも大きいことは必須ではない。例えば、第１安全閾値と第２安全閾値とは、同じ値であってもよい。

干渉閾値は、異なる密度の物体モデル７１の干渉判定において同じ値であるが、これに限定されない。例えば、干渉閾値は、物体モデル７１の点群の密度が高いほど小さくなるように設定されてもよい。

前述の経路生成処理のステップｓｂ４のスキップにおいては、余裕距離が今回の中間位置への第１装置モデル７２Ａの移動量よりも大きい場合には、干渉判定をスキップしている。この今回の中間位置への第１装置モデル７２Ａの移動量は、第１装置モデル７２Ａが前の中間位置から今回の中間位置へ移動するときの、前の第１装置モデル７２Ａの最接近点の移動量であるが、これに限定されない。このスキップの判定における第１装置モデル７２Ａの移動量は、第１装置モデル７２Ａの各部（例えば、複数の関節及びハンド１４の指先）のうちの最大移動量を用いてもよい。あるいは、中間位置を生成するルールとして、装置モデル７２の各部（複数の関節及びハンド１４の指先）のそれぞれの移動量の上限値が設定されている場合には、該上限値を用いてもよい。

また、ステップｓｂ４の干渉判定のスキップは、次の中間位置に対してだけでなく、次回以降の複数の中間位置の干渉判定をスキップしてもよい。例えば、余裕距離が次回以降の複数の中間位置への第１装置モデル７２Ａの移動量よりも大きい場合には、それら複数の中間位置に対する干渉判定をスキップしてもよい。

尚、ステップｓｂ４の干渉判定のスキップの処理は省略されてもよい。

前述の経路生成処理において、物体モデル７１の密度が２段階に変更される場合には、ステップｓｂ９，ｓｂ１１，ｓｂ１２が省略される。つまり、第２物体モデル７１Ｂと第２装置モデル７２Ｂとのモデル間距離が干渉閾値以下か否かが判定される。モデル間距離が第２安全閾値以下か否かは判定されない。モデル間距離が干渉閾値以下の場合には、ステップｓｂ７において中間位置が変更される。モデル間距離が干渉閾値よりも大きい場合には、ステップｓｂ１３において目標位置に到達したか否かが判定される。

また、経路生成処理において、物体モデル７１の密度が４段階以上に変更される場合には、第３物体モデル７１Ｃ及び第３装置モデル７２Ｃの干渉判定においても、モデル間距離が干渉閾値以下か否かの判定の前に、モデル間距離が安全閾値、即ち、第３安全閾値以下か否かの判定がされる。そして、最高密度の物体モデル７１と装置モデル７２との干渉判定においては、モデル間距離が安全閾値以下か否かの判定が行われず、モデル間距離が干渉閾値か否かの判定が行われる。

前述のフローチャートでは、制御装置５は、開始位置から目標位置までの段階的な経路を全て生成してからロボットアーム１２の移動を開始しているが、これに限定されない。制御装置５は、開始位置から目標位置まで段階的に生成される経路の１つ１つが生成され確定したごとにロボットアーム１２をその経路に沿って移動させてもよい。

また、前述のフローチャートでは、所期の処理を実現できる限り、ステップの順番を入れ替えたり、複数のステップを並行に処理したり、ステップを省略したり、ステップを追加したりしてもよい。

本明細書で開示する要素の機能は、開示された機能を実行するよう構成またはプログラムされた汎用プロセッサ、専用プロセッサ、集積回路、ASIC（Application Specific Integrated Circuits）、従来の回路、および/または、それらの組み合わせ、を含む回路または処理回路を使用して実行できる。プロセッサは、トランジスタやその他の回路を含むため、処理回路または回路と見なされる。本開示において、回路、ユニット、または手段は、列挙された機能を実行するハードウェアであるか、または、列挙された機能を実行するようにプログラムされたハードウェアである。ハードウェアは、本明細書に開示されているハードウェアであってもよいし、あるいは、列挙された機能を実行するようにプログラムまたは構成されているその他の既知のハードウェアであってもよい。ハードウェアが回路の一種と考えられるプロセッサである場合、回路、手段、またはユニットはハードウェアとソフトウェアの組み合わせであり、ソフトウェアはハードウェアおよび/またはプロセッサの構成に使用される。

１００ ロボットシステム
１ ロボット
１２ ロボットアーム（動作装置）
３ 経路生成装置
５４ 物体設定部
５５ 装置設定部
５６ 経路生成部
５７ 干渉判定部
６２ 経路生成プログラム
７１ 物体モデル
７２ 装置モデル

Claims (15)

1. 動作装置の経路を生成する経路生成装置であって、
   前記動作装置が配置された配置空間に含まれる物体のモデルである物体モデルを設定する物体設定部と、
   前記配置空間における前記動作装置のモデルである装置モデルを設定する装置設定部と、
   前記動作装置の経路を段階的に生成する経路生成部と、
   前記物体モデルと前記装置モデルとの距離に基づいて、前記物体モデルと前記経路に沿って移動した後の前記装置モデルとの干渉判定を行う干渉判定部とを備え、
   前記物体モデル及び前記装置モデルの少なくとも一方は、点群で形成された点群モデルであり、
   干渉の可能性があると前記干渉判定部によって判定された場合には、
   前記物体設定部及び前記装置設定部の少なくとも一方は、前記点群モデルの前記点群の密度を高くし、
   前記干渉判定部は、前記点群の密度が高くなった前記点群モデルを用いて前記干渉判定を再び行い、
   前記経路生成部は、干渉の可能性がないと前記干渉判定部によって判定された場合には次の経路を生成する経路生成装置。
2. 請求項１に記載の経路生成装置において、
   前記物体設定部及び前記装置設定部の少なくとも一方は、干渉の可能性があると前記干渉判定部によって判定された場合には、前記干渉判定において干渉の可能性があると判定された部分を含む、前記点群モデルの一部の前記点群の密度を高くする経路生成装置。
3. 請求項１又は２に記載の経路生成装置において、
   前記干渉判定部は、
   前記物体モデルと前記装置モデルとの最短距離であるモデル間距離が所定の安全閾値以下の場合に、干渉の可能性があると判定し、
   前記モデル間距離が前記安全閾値よりも大きい場合に、干渉の可能性がないと判定する経路生成装置。
4. 請求項３に記載の経路生成装置において、
   前記干渉判定部は、前記モデル間距離が前記安全閾値よりも小さな干渉閾値以下の場合に、干渉すると判定し、
   前記経路生成部は、干渉すると前記干渉判定部によって判定された場合には経路を変更する経路生成装置。
5. 請求項３又は４に記載の経路生成装置において、
   前記安全閾値は、前記点群モデルの前記点群の密度が高いほど小さくなるように設定される経路生成装置。
6. 請求項３乃至５の何れか１項に記載の経路生成装置において、
   前記動作装置は、複数のリンクと、複数のリンクを連結する複数の関節とを有するロボットアームであって、
   前記干渉判定部は、
   前記複数のリンクのそれぞれについて前記モデル間距離を求め、
   前記複数のリンクの少なくとも１つのリンクの前記モデル間距離が前記安全閾値以下の場合に、干渉の可能性があると判定し、
   前記複数のリンクの全てのリンクの前記モデル間距離が前記安全閾値よりも大きい場合に、干渉の可能性がないと判定する経路生成装置。
7. 請求項６に記載の経路生成装置において、
   前記干渉判定部は、前記複数のリンクのうち前記ロボットアームの先端側のリンクから順に、前記モデル間距離を求め、求められた前記モデル間距離と前記安全閾値とを比較する経路生成装置。
8. 請求項３に記載の経路生成装置において、
   前記干渉判定部は、干渉の可能性がないと判定した場合、前記モデル間距離から前記安全閾値を減じた値である余裕距離を求め、
   前記経路生成部は、前記経路を生成する場合、今回の経路に繋がる前の経路の前記余裕距離が前記今回の経路の前記装置モデルの移動量よりも大きいときには、前記今回の経路に対する前記干渉判定を省略する経路生成装置。
9. 請求項８に記載の経路生成装置において、
   前記動作装置は、複数のリンクと、複数のリンクを連結する複数の関節とを有するロボットアームであって、
   前記干渉判定部は、
   前記複数のリンクのそれぞれについて前記モデル間距離を求め、
   前記複数のリンクの少なくとも１つのリンクの前記モデル間距離が前記安全閾値以下の場合に、干渉の可能性があると判定し、
   前記複数のリンクの全てのリンクの前記モデル間距離が前記安全閾値よりも大きい場合に、干渉の可能性がないと判定し、前記複数のリンクのそれぞれについて前記余裕距離を求め、
   前記経路生成部は、前記経路を生成する場合、前記今回の経路に対する前記干渉判定を省略するか否かを前記複数のリンクごとに判定する経路生成装置。
10. 請求項１乃至７の何れか１項に記載の経路生成装置において、
    前記物体モデル及び前記装置モデルのうち少なくとも前記物体モデルは、前記点群モデルである経路生成装置。
11. 請求項１乃至１０の何れか１項に記載の経路生成装置において、
    前記物体モデル及び前記装置モデルは両方とも、前記点群モデルである経路生成装置。
12. 請求項１１に記載の経路生成装置において、
    干渉の可能性があると前記干渉判定部によって判定された場合には、
    前記物体設定部は、前記物体モデルの前記点群の密度を高くし、
    前記装置設定部は、前記装置モデルの前記点群の密度を高くし、
    前記干渉判定部は、前記点群の密度が高くなった前記装置モデルと前記点群の密度が高くなった前記物体モデルとの干渉判定を行う経路生成装置。
13. 請求項１２に記載の経路生成装置において、
    前記物体設定部は、前記干渉判定において干渉の可能性があると判定された部分を含む、前記物体モデルの一部の前記点群の密度を高くし、
    前記装置設定部は、前記干渉判定において干渉の可能性があると判定された部分を含む、前記装置モデルの一部の前記点群の密度を高くする経路生成装置。
14. 動作装置の経路を生成する経路生成方法であって、
    前記動作装置が配置された配置空間に含まれる物体のモデルである物体モデルを設定することと、
    前記配置空間における前記動作装置のモデルである装置モデルを設定することと、
    前記動作装置の経路を段階的に生成することと、
    前記物体モデルと前記装置モデルとの距離に基づいて、前記物体モデルと前記経路に沿って移動した後の前記装置モデルとの干渉判定を行うこととを含み、
    前記物体モデル及び前記装置モデルの少なくとも一方は、点群で形成された点群モデルであり、
    干渉の可能性があると前記干渉判定によって判定された場合には、
    前記物体モデルを設定すること及び前記装置モデルを設定することの少なくとも一方において、前記点群モデルの前記点群の密度を高くし、
    前記干渉判定を行うことにおいて、前記点群の密度が高くなった前記点群モデルを用いて前記干渉判定を再び行い、
    前記経路を生成することにおいては、干渉の可能性がないと前記干渉判定によって判定された場合には次の経路を生成する経路生成方法。
15. 動作装置の動作を生成するためにコンピュータに、
    前記動作装置が配置された配置空間に含まれる物体のモデルである物体モデルを設定する機能と、
    前記配置空間における前記動作装置のモデルである装置モデルを設定する機能と、
    前記動作装置の経路を段階的に生成する機能と、
    前記物体モデルと前記装置モデルとの距離に基づいて、前記物体モデルと前記経路に沿って移動した後の前記装置モデルとの干渉判定を行う機能とを実現させ、
    前記物体モデル及び前記装置モデルの少なくとも一方は、点群で形成された点群モデルであり、
    干渉の可能性があると前記干渉判定によって判定された場合には、
    前記物体モデルを設定する機能及び前記装置モデルを設定する機能の少なくとも一方は、前記点群モデルの前記点群の密度を高くし、
    前記干渉判定を行う機能は、前記点群の密度が高くなった前記点群モデルを用いて前記干渉判定を再び行い、
    前記経路を生成する機能は、干渉の可能性がないと前記干渉判定によって判定された場合には次の経路を生成する経路生成プログラム。
    
    

Images