JP6378783B2 - アーム型のロボットの障害物自動回避方法及び制御装置 - Google Patents

Description

本発明は、アーム型のロボットの障害物自動回避方法及び制御装置に関する。

従来から停止したロボットを障害物との干渉を自動的に回避して、所定の姿勢に到達させる方法が知られている。

特許文献１には、経路を遡行して作業原点まで復帰する方法が開示されている。具体的には、移動コマンドを含んで記述された制御プログラムを順次実行することにより所望の経路で一連の動作を行うロボットを停止位置から作業原点に復帰させる制御方法であって、既に実行された制御プログラムを順次逆実行するに際し、各移動コマンドをその一つ前の移動コマンドの位置引数を用いて実行する。

特許文献２には、移動経路の外でロボットが異常停止した場合でも原点に復帰することができるロボットが開示されている。具体的には、ロボットが動作する動作エリアを少なくとも含むエリアマップを、所定の領域を持つエリアブロックごとに区画するエリアブロック作成工程と、区画されたエリアブロックごとにロボットの復帰方向を設定する方向設定工程と、を備える。これによって、障害物との干渉を回避することができる。

特開平７−２８５２０号公報 特開２００９−９０３８３号公報

特許文献１に記載の方法では、移動経路の外でロボットが異常停止した場合には、作業原点に復帰できない。一方、特許文献２の方法ではそれが可能である。しかし、特許文献２に記載の方法は、予めエリアブロックごとにロボットの復帰方向を設定する必要があり、設定作業を行う作業者の作業量が増大するという問題があった。また、復帰方向の決定も作業者の経験則による判断を要し、作業者の確保が困難であるという問題があった。更に、エリアブロックごとにロボットの復帰方向を設定したデータはデータ量が膨大になるという問題があった。換言すると、ロボットの復帰方向に関して複雑な設定が必要であるという問題があった。

本発明は、上記課題に鑑みてなされたものであり、ロボットがどの位置で停止しても、単純な設定で安全に所定の姿勢に到達させることが可能なロボットの障害物自動回避方法及び制御装置を提供することを目的とする。

上記課題を解決するため、本発明のある態様に係るロボットの障害物自動回避方法は、１以上のリンクが関節によって連結され、先端部にハンドが設けられたロボットアームを備えるアーム型のロボットの障害物自動回避方法であって、前記ロボットを、幾何形状を有するようモデリングして幾何モデルとして表現する幾何モデル表現ステップと、前記幾何モデルが進入してはいけない進入禁止領域と、当該進入禁止領域によって規定され、前記幾何モデルが動作する動作領域とを設定する領域設定ステップと、前記ロボットの最終姿勢を決定する最終姿勢決定ステップと、前記ロボットが現在姿勢から前記最終姿勢に向かって変化する場合における前記ハンドの初期軌道を決定する初期軌道決定ステップと、前記初期軌道上の所定の点に対応する前記ロボットの仮想姿勢を算出する仮想姿勢算出ステップと、前記仮想姿勢における前記幾何モデルが前記進入禁止領域と干渉するか否かを判定する干渉判定ステップと、前記干渉判定ステップにおいて干渉しないと判定した場合、前記仮想姿勢を経由姿勢として決定し、干渉すると判定した場合、前記進入禁止領域の干渉部分と前記幾何モデルの干渉部分とが相対的に反発する反発力を仮想的に発生させ、仮想反発力によって前記幾何モデルの干渉部分が前記進入禁止領域から前記動作領域に押し出された状態の姿勢を算出し、該算出した姿勢を経由姿勢として決定する経由姿勢決定ステップと、前記現在姿勢から前記経由姿勢を経由して前記最終姿勢に変化する場合における前記ハンドの軌道を更新軌道として決定する更新軌道決定ステップと、最新の経由姿勢決定ステップにおいて決定された経由姿勢を前記初期軌道決定ステップにおける前記現在姿勢と仮定して前記初期軌道決定ステップ、前記仮想姿勢算出ステップ、前記干渉判定ステップ、前記経由姿勢決定ステップ、及び前記更新軌道決定ステップを繰り返して実行するステップと、を含むよう構成されている。

上記構成によれば、ロボットが現在姿勢から最終姿勢に向かって変化する場合におけるハンドの初期軌道上の所定の点に対応するロボットの仮想姿勢を算出し、仮想姿勢におけるロボットの幾何モデルが進入禁止領域（障害物）と干渉するか否かを判定する。そして、干渉しないと判定した場合には仮想姿勢を、現在姿勢から最終姿勢に向かって変化する場合に経由する経由姿勢として決定し、干渉すると判定した場合には、仮想反発力によって幾何モデルの干渉部分が進入禁止領域から動作領域に押し出された状態の姿勢を算出し、その姿勢を経由姿勢として決定する。従って、ロボットの幾何モデルが進入禁止領域と干渉する場合には、進入禁止領域に干渉しない経由姿勢が与えられるので、障害物との衝突が回避される。但し、この場合には当初の初期軌道から逸れるので最終姿勢に近づくことは担保されない。一方、ロボットの幾何モデルが進入禁止領域と干渉しない場合には、初期軌道上の点に対応する経由姿勢が与えられるので、最終姿勢に近づく。そして、上記構成によれば、新たに与えられた経由姿勢を現在姿勢としてこの手順を繰り返すので、ロボットの幾何モデルが進入禁止領域と干渉しない手順が出現する都度、その手順における初期軌道上の点に対応する経由姿勢が与えられて最終姿勢に近づく。従って、試行錯誤を繰り返すことにより、最終的に、ロボットが、障害物との衝突を回避しながら現在姿勢から最終姿勢に変化する場合におけるハンドの更新軌道を得ることができる。

その結果、ロボットの幾何モデル、進入禁止領域、及び動作領域の設定という単純な設定で、ロボットがどの位置で停止しても障害物との干渉を自動で回避して、安全にロボットを所定の姿勢（例えば、退避姿勢）に到達させることができる。

前記最終姿勢決定ステップは、既知の軌道上に設定される前記ロボットの複数の最終候補姿勢のうち、前記ロボットの現在姿勢と前記最終候補姿勢との前記各関節の角度の変位の絶対値の総和が最小の値である最終候補姿勢を前記最終姿勢として選択し、決定してもよい。

この構成によれば、ロボットの現在姿勢に近い姿勢を最終姿勢として選択することができる。よって、最終姿勢に到達するまでに要する時間を短縮することができる。

前記経由姿勢決定ステップは、前記仮想反発力によってある仮想姿勢における前記ロボットの前記各関節の回動軸周りに生じるトルクを算出し、更に該トルクの影響によって変化する前記ロボットの所定時間経過後の仮想姿勢を算出する演算を前記仮想姿勢を起点として繰り返し行うことによって前記ロボットの仮想姿勢の経時変化を算出し、前記ロボットの仮想姿勢の経時変化が収束したときの前記ロボットの仮想姿勢に基づいて前記経由姿勢を決定してもよい。

この構成によれば、ロボットと進入禁止領域との干渉を回避した経由姿勢を適切に与えることができる。

前記仮想反発力は、前記幾何モデルが進入禁止領域に進入した距離に比例して大きくなるように構成されていてもよい。

この構成によれば、ロボットの幾何モデルが進入禁止領域に深く進入した場合であっても、速やかに進入禁止領域の外に押し出すことができる。

前記ロボットの姿勢が前記最終姿勢に近づくように経時変化せず、停留状態に陥っているか否かを判定する停留状態判定ステップと、前記停留状態判定ステップにおいて停留状態に陥っていると判定された場合、前記ロボットの前記仮想反発力によって押し出された状態の姿勢以外の経由候補姿勢を生成する経由候補姿勢生成ステップと、前記経由候補姿勢が前記最終姿勢に近づく姿勢であるか否かを判定する第１判定ステップと、前記第１判定ステップにおいて、前記経由候補姿勢が前記最終姿勢に近づく方向に変化する姿勢であると判定された場合、該経由候補姿勢に基づいて前記経由姿勢を決定する第１決定ステップとを有していてもよい。

この構成によれば、例えば、反発力により押し戻す方向が初期軌道の方向と一致する場合には、動作領域に押し戻された姿勢が初期軌道上の点に対応するので、進入禁止領域に対する幾何モデルの干渉が繰り返されて停留状態に陥り、ロボットが最終姿勢に近づかない。しかし、上記構成によれば、初期軌道上にない点に対応する経由姿勢が得られるので、このような停留状態に陥った場合であっても、最終姿勢に近づく別の経由姿勢を経由して、最終姿勢に到達することができる。

前記第１判定ステップにおいて、前記経由候補姿勢が前記最終姿勢から遠ざかる方向に変化する姿勢であると判定された場合、確率値に基づいて前記経由候補姿勢を選択するか否かを判定する第２判定ステップと、前記第２判定ステップにおいて選択すると判定されると、当該経由候補姿勢に基づいて前記経由姿勢を決定する第２決定ステップとを有していてもよい。

この構成によれば、最終姿勢に近づく別の姿勢を生成できない場合であっても、別の経由姿勢を経由して最終姿勢に到達することができるか否かを試みることができる。これによって、現在の最終姿勢に到達できない状態を好適に脱することができる。

上記課題を解決するため、本発明のある態様に係るアーム型のロボットの制御装置は、１以上のリンクが関節によって連結され、先端部にハンドが設けられたロボットアームを備えるアーム型のロボットの制御装置であって、前記ロボットを、幾何形状を有するようモデリングして幾何モデルとして表現する幾何モデル表現部と、前記幾何モデルが進入してはいけない進入禁止領域と、当該進入禁止領域によって規定され、前記幾何モデルが動作する動作領域とを設定する領域設定部と、前記ロボットの最終姿勢を決定する最終姿勢決定部と、前記ロボットが現在姿勢から前記最終姿勢に向かって変化する場合における前記ハンドの初期軌道を決定する初期軌道決定部と、前記初期軌道上の所定の点に対応する前記ロボットの仮想姿勢を算出する仮想姿勢算出部と、前記仮想姿勢における前記幾何モデルが前記進入禁止領域と干渉するか否かを判定する干渉判定部と、経由姿勢を決定する経由姿勢決定部と、更新軌道決定部と、を備え、前記経由姿勢決定部は、前記干渉判定部が干渉しないと判定した場合、前記仮想姿勢を経由姿勢として決定し、干渉すると判定した場合、前記進入禁止領域の干渉部分と前記幾何モデルの干渉部分とが相対的に反発する反発力を仮想的に発生させ、仮想反発力によって前記幾何モデルの干渉部分が前記進入禁止領域から前記動作領域に押し出された状態の姿勢を算出し、該算出した姿勢を経由姿勢として決定し、前記更新軌道決定部は、前記現在姿勢から前記経由姿勢を経由して前記最終姿勢に変化する場合における前記ハンドの軌道を更新軌道として決定し、前記経由姿勢決定部が決定した最新の経由姿勢を前記初期軌道決定部の現在姿勢と仮定して、前記初期軌道決定部、前記仮想姿勢算出部、前記干渉判定部、前記経由姿勢決定部、及び前記更新軌道決定部が繰り返し処理するよう構成されている。

上記構成によれば、ロボットが現在姿勢から最終姿勢に向かって変化する場合におけるハンドの初期軌道上の所定の点に対応するロボットの仮想姿勢を算出し、仮想姿勢におけるロボットの幾何モデルが進入禁止領域（障害物）と干渉するか否かを判定する。そして、干渉しないと判定した場合には仮想姿勢を、現在姿勢から最終姿勢に向かって変化する場合に経由する経由姿勢として決定し、干渉すると判定した場合には、仮想反発力によって幾何モデルの干渉部分が進入禁止領域から動作領域に押し出された状態の姿勢を算出し、その姿勢を経由姿勢として決定する。従って、ロボットの幾何モデルが進入禁止領域と干渉する場合には、進入禁止領域に干渉しない経由姿勢が与えられるので、障害物との衝突が回避される。但し、この場合には当初の初期軌道から逸れるので最終姿勢に近づくことは担保されない。一方、ロボットの幾何モデルが進入禁止領域と干渉しない場合には、初期軌道上の点に対応する経由姿勢が与えられるので、最終姿勢に近づく。そして、上記構成によれば、新たに与えられた経由姿勢を現在姿勢としてこの手順を繰り返すので、ロボットの幾何モデルが進入禁止領域と干渉しない手順が出現する都度、その手順における初期軌道上の点に対応する経由姿勢が与えられて最終姿勢に近づく。従って、試行錯誤を繰り返すことにより、最終的に、ロボットが、障害物との衝突を回避しながら現在姿勢から最終姿勢に変化する場合におけるハンドの更新軌道を得ることができる。

本発明は、単純な設定で、ロボットがどの位置で停止しても障害物との干渉を自動で回避して、安全にロボットを所定の姿勢に到達させることができるという効果を奏する。

本発明の実施の形態１に係るロボットを備える設備の構成例を示す平面図である。 図１のロボットの制御系統の構成例を概略的に示すブロック図である 図１のロボットの最終姿勢の選択方法を示す説明図である。 図１のロボットの経由姿勢の算出方法を示すフローチャートである。 図１のロボットの経由姿勢の算出方法を示す説明図である。 図１のロボットの経由姿勢の算出方法を示す説明図である。 図１のロボットの経由姿勢の算出方法を示す説明図である。 図１のロボットの動作例を示すフローチャートである。 図１のロボットの動作例を示す図である。 図１のロボットの動作例を示す図である。 図１のロボットの動作例を示す図である。 図１のロボットの動作例を示す図である。 図１のロボットの動作例を示す図である。 図１のロボットの動作例を示す図である。 本発明の実施の形態２に係るロボットの動作例を示すフローチャートである。 図８のロボットの処理において用いる遷移確率を示すグラフである。 図８のロボットのシミュレーション条件を示す図である。 図８のロボットのシミュレーション条件を示す図である。 図８のロボットのシミュレーション結果を示す図である。

以下、本発明の実施の形態について、図面を参照しながら説明する。なお、本実施の形態によって本発明が限定されるものではない。また、以下では、全ての図を通じて、同一又は相当する要素には同一の参照符号を付して、その重複する説明を省略する。

（実施の形態１）
図１は、本発明の実施の形態１に係るロボット１００を備える設備１１０の構成例を示す平面図である。

設備１１０は、例えば基板の処理を行う設備であり、内部にロボット１００が設置され、例えば、チャンバ１１３を囲む壁１１１と、チャンバ内部において壁１１１から内側に突出している壁１１２とを有する。これら壁１１１及び壁１１２は、ロボット１００の動作における障害物を構成する。

［ロボットの構成］
ロボット１００は、例えば半導体ウェハ、ガラスウェハ等の基板を搬送するロボットである。

図１に示すように、ロボット１００は、１以上のリンクが関節によって連結され、先端部にハンドが設けられたロボットアームを備えるアーム型のロボットである。更に具体的には、ロボット１００は、水平多関節ロボットであり、基台１と、第１アーム２と、第２アーム３と、ハンド４と、を備え、これらが直鎖状に連結されている。本実施の形態において、第１アーム２、第２アーム３、ハンド４がロボットアームを構成する。また、ロボット１００は、ロボット１００の動作を制御する制御器５（図２参照）を備える。

基台１は、例えば、中空の円筒状部材である。基台１の内部には、サーボモータを含む第１アーム駆動部１２（図２参照）が配設されている。第１アーム駆動部１２は、後述する第１関節軸２ａを回動させるように構成されている。

第１アーム２は、例えば、中空の板状部材であり、平面視において大略短冊状に形成されている。第１アーム２は、第１アーム２の基端部の底面から下方に突出するように第１関節軸２ａが形成されている。そして、第１関節軸２ａは、ｚ方向に延びる回動軸線Ｌ１を中心に回動可能に基台１に取り付けられ、この部分が第１関節を構成している。従って、第１アーム２は、ｘｙ平面において回動するように構成されている。なお、本実施の形態において、回動軸線Ｌ１がｘｙ平面上における基準点Ｏを構成する。

第１アーム２の内部には、サーボモータを含む第２アーム駆動部１３（図２参照）が配設されている。第２アーム駆動部１３は、後述する第２関節軸３ａを回動させるように構成されている。

そして、第１アーム２の基台１に対する回動軸線Ｌ１周りの相対的な角度位置ｑ１（図３参照）は、第１アーム駆動部１２のサーボモータのエンコーダによって検出される。このエンコーダが第１アーム角度位置検出部１５（図２参照）を構成する。

なお、第１アーム２は１番目のリンクということがあり、また第１関節軸２ａを１軸ということがある。

第２アーム３は、例えば、中空の板状部材であり、平面視において大略短冊状に形成されている。第２アーム３は、第２アーム３の基端部の底面から下方に突出するように第２関節軸３ａが設けられている。そして、第２関節軸３ａは、回動軸線Ｌ１と平行に延びる回動軸線Ｌ２を中心に回動可能に第１アーム２に取り付けられ、この部分が第２関節を構成している。従って、第２アーム３は、ｘｙ平面上を回動するように構成されている。

第２アーム３の内部には、サーボモータを含むハンド駆動部１４（図２参照）が配設されている。ハンド駆動部１４は、後述する第３関節軸４ａを回動させるように構成されている。

そして、第２アーム３の第１アーム２に対する回動軸線Ｌ２周りの相対的な角度位置ｑ２（図３参照）は、第２アーム駆動部１３のサーボモータのエンコーダによって検出される。このエンコーダが第２アーム角度位置検出部１６（図２参照）を構成する。

なお、第２アーム３は２番目のリンクということがあり、また第２関節軸３ａを２軸ということがある。

ハンド４は、基板を保持するものであり、平板状に形成されている。ハンド４は、その基端部の底面から下方に突出するように形成された第３関節軸４ａを有している。そして、第３関節軸４ａは、回動軸線Ｌ１，Ｌ２と平行に延びる回動軸線Ｌ３を中心に回動可能に第２アーム３に取り付けられ、この部分が第３関節を構成している。従って、ハンド４は、ｘｙ平面において回動するように構成されている。

そして、ハンド４の第２アーム３に対する回動軸線Ｌ３周りの相対的な角度位置ｑ３（図３参照）は、ハンド駆動部１４のサーボモータのエンコーダによって検出される。このエンコーダがハンド角度位置検出部１７（図２参照）を構成する。

なお、ハンド４は３番目のリンクということがあり、また第３関節軸４ａを３軸ということがある。

［制御部］
図２は、ロボット１００の制御系統の構成例を概略的に示すブロック図である。

ロボット１００が備える制御器５は、例えば、ＣＰＵ等の演算器を有する制御部２１と、ＲＯＭ及びＲＡＭ等のメモリを有する記憶部２２とを備えている。制御器５は、集中制御する単独の制御器で構成されていてもよく、互いに協働して分散制御する複数の制御器で構成されてもよい。制御部２１は、第１アーム制御部３１，第２アーム制御部３２，ハンド制御部３３，最終姿勢決定部３４，初期軌道決定部３５，仮想姿勢算出部３６，幾何モデル表現部３７，領域設定部４０，干渉判定部３８，経由姿勢決定部３９，更新軌道決定部４１を含む。これらの機能部３１〜４１は、記憶部２２に格納された所定の制御プログラムを制御部２１が実行することにより実現される機能ブロック（機能モジュール）である。

第１アーム制御部３１は、第１アーム駆動部１２を制御し、ｘｙ平面において回動軸線Ｌ１周りに第１アーム２を回動させる。

第２アーム制御部３２は、第２アーム駆動部１３を制御し、ｘｙ平面において回動軸線Ｌ２周りに第２アーム３を回動させる。

ハンド制御部３３は、ハンド駆動部１４を制御し、ｘｙ平面において回動軸線Ｌ３周りにハンド４を回動させる。

なお、本実施の形態では、例えば、制御部２１の主制御部（図示せず）が、ロボット１００の第１アーム角度位置検出部１５、第２アーム角度位置検出部１６、及びハンド角度位置検出部１７がそれぞれ検出する角度位置と、最終姿勢とに基づいて、第１アーム制御部３１，第２アーム制御部３２，及びハンド制御部３３に最終角度位置をそれぞれ出力する。第１アーム制御部３１，第２アーム制御部３２，及びハンド制御部３３は、それぞれ、対応する制御対象物（第１関節軸２ａ、第２関節軸３ａ、及び第３関節軸４ａ）の角度位置が最終姿勢に対応する最終角度位置になるように、対応する角度位置検出部１５，１６，１７が検出する回動軸の角度位置に基づいて、対応する第１アーム駆動部１２，第２アーム駆動部１３，及びハンド駆動部１４をそれぞれフィードバック制御する。

幾何モデル表現部３７は、ロボット１００を、幾何形状を有するようモデリングして幾何モデルＭとして表現する。

本実施の形態において、幾何モデル表現部３７は、第１アーム２，第２アーム３，及びハンド４を幾何モデルＭ１，Ｍ２，及びＭ３で表現する。

第１アーム２の幾何モデルＭ１は、図１に示すように、例えば、長円形のモデルである。幾何モデルＭ１は、第１アーム２をｘｙ平面に投影した図形の外側において、この図形と離間するように表現される。

第２アーム３の幾何モデルＭ２は、例えば、長円形のモデルである。幾何モデルＭ２は、第２アーム３をｘｙ平面に投影した図形の外側において、この図形と離間するように表現される。

ハンド４の幾何モデルＭ３は、例えば、水滴型のモデルである。幾何モデルＭ３は、基板を保持するハンド４をｘｙ平面に投影した図形の外側において、この図形と離間するように規定（画定）される。

図１及び２に示すように、領域設定部４０は、幾何モデルＭ１〜Ｍ３が進入してはいけない進入禁止領域Ｓ１と、進入禁止領域Ｓ１によって規定（画定）され、幾何モデルＭ１〜Ｍ３が動作する動作領域Ｓ２とを設定する。

本実施の形態において、進入禁止領域Ｓ１及び動作領域Ｓ２は、記憶部２２に格納されており、領域設定部４０が記憶部２２から進入禁止領域Ｓ１及び動作領域Ｓ２を読み出すことにより、進入禁止領域Ｓ１及び動作領域Ｓ２を設定する。

また、進入禁止領域Ｓ１は、設備１１０のチャンバ１１３の内壁面（壁１１１及び壁１１２の壁面）よりも内側の領域も含むように設定される。また、進入禁止領域Ｓ１と動作領域Ｓ２の境界に沿って延びる仮想線が境界線Ｂを構成する。

最終姿勢決定部３４は、ロボット１００の最終姿勢Ｐｔを決定する。

本実施の形態において、最終姿勢決定部３４は、記憶部２２に記憶されている複数の最終候補姿勢Ｐｔｃから一の最終姿勢Ｐｔを選択し、決定する。

最終候補姿勢Ｐｔｃは、進入禁止領域Ｓ１と干渉しない既知の軌道上に設定される姿勢である。よって、ロボット１００は、何れか一の姿勢に到達することができれば、以降は既知の軌道を辿り、進入禁止領域Ｓ１と干渉せずに動作することができる。

図３は、ロボット１００の最終姿勢Ｐｔの選択方法を示す説明図である。

図３に示すように、最終姿勢決定部３４が複数の最終候補姿勢Ｐｔｃ（図３ではそのうち一を示す）から一の最終姿勢Ｐｔを選択するときは、まず、現在姿勢Ｐｓと最終候補姿勢Ｐｔｃとの各関節軸の関節角度の変位の絶対値の総和Ｅを以下の式に基づいて算出する。

次に、Ｅが最小の値である最終候補姿勢Ｐｔｃ選択し、これを最終姿勢Ｐｔとして決定する。

初期軌道決定部３５は、ロボット１００が現在姿勢Ｐｓから最終姿勢決定部３４において決定された最終姿勢Ｐｔに向かって変化する場合におけるハンド４の初期軌道Ｔａを決定する。本実施の形態において、初期軌道Ｔａは、現在姿勢Ｐｓ及び最終姿勢Ｐｔが与えられることによって一義的に決まる軌道である。

仮想姿勢算出部３６は、初期軌道Ｔａ上の所定の点におけるロボット１００の仮想姿勢Ｐｐを算出する。

干渉判定部３８は、仮想姿勢Ｐｐにおけるロボット１００の幾何モデルＭ１〜Ｍ３が進入禁止領域Ｓ１と干渉するか否かをそれぞれ判定する。

経由姿勢決定部３９は、現在姿勢Ｐｓから最終姿勢Ｐｔに向かって変化する場合に経由する経由姿勢Ｐｖを算出する。

更に、経由姿勢決定部３９は、干渉判定部３８がロボット１００の幾何モデルＭ１〜Ｍ３と進入禁止領域Ｓ１とが全て干渉しないと判定した場合、仮想姿勢Ｐｐを経由姿勢Ｐｖとして決定する。

一方、経由姿勢決定部３９は、幾何モデルＭ１〜Ｍ３のうち少なくとも一の幾何モデルと進入禁止領域Ｓ１とが干渉すると判定した場合、進入禁止領域Ｓ１の干渉部分と幾何モデルの干渉部分とが相対的に反発する反発力ｆｉを仮想的に発生させ、仮想反発力ｆｉによって幾何モデルの干渉部分が進入禁止領域Ｓ１から動作領域Ｓ２に押し出された状態の姿勢を算出し、この算出した姿勢を経由姿勢Ｐｖとして決定する。

図４は、ロボット１００の経由姿勢Ｐｖの算出方法を示すフローチャートである。

図５Ａ〜５Ｃは、ロボット１００の経由姿勢Ｐｖの算出方法を示す説明図である。

経由姿勢Ｐｖを算出するために経由姿勢決定部３９が行う処理について以下に詳しく説明する。なお、本実施の形態において、ロボット１００は、３個のリンクを有するロボットであるが、以下ではｎ個のリンクを有するロボットとして一般化した場合の考え方を示す。

まず、ロボット１００の幾何モデルが進入禁止領域Ｓ１に進入した距離ｄｉを決定する（ステップＳ７１）。距離ｄｉの算出は、仮想姿勢Ｐｐにおける各幾何モデルと進入禁止領域Ｓ１との干渉形態に応じて行う。

例えば、図５Ａに示す場合、まず、幾何モデルＭｉの干渉部分（図５Ａにおいて薄墨を付した部分）において、境界線Ｂとの距離が最大となる幾何モデルＭｉ上の点Ｐ（ｘ，ｙ）を算出する。そして、境界線Ｂの法線方向において、点Ｐと境界線Ｂとの距離を距離ｄｉに設定する。

また、図５Ｂに示す場合、まず、幾何モデルＭｉの干渉部分（図５Ｂにおいて薄墨を付した部分）において、境界線Ｂとの距離が最大となる幾何モデルＭｉ上の点Ｐ（ｘ，ｙ）を算出する。そして、点Ｐと境界線Ｂの頂点Ｑとの距離を距離ｄｉに設定する。

更に、図５Ｃに示す場合、まず、幾何モデルＭｉの干渉部分（図５Ｃにおいて薄墨を付した部分）において、境界線Ｂの頂点Ｑから幾何モデルと境界線Ｂとの一方の交点Ｒに向かう側に位置する、境界線Ｂとの距離が最大となる幾何モデルＭｉ上の点Ｐ１（ｘ，ｙ）を算出する。そして、線分ＱＲの法線方向において、点Ｐ１と境界線Ｂとの距離を距離ｄｉ１に設定する。

また、幾何モデルＭｉの干渉部分において、境界線Ｂの頂点Ｑから幾何モデルと境界線Ｂとの他方の交点Ｒ’に向かう側に位置する、境界線Ｂとの距離が最大となる幾何モデル上の点Ｐ２（ｘ，ｙ）を算出する。そして、線分ＱＲ’の法線方向において、点Ｐ２と境界線Ｂとの距離を距離ｄｉ２に設定する。

このように、距離ｄｉの算出は、仮想姿勢Ｐｐにおける幾何モデルと進入禁止領域Ｓ１との干渉形態に応じて行うように構成されるが、上記の構成に限定されるものではない。

次に、決定した距離ｄｉに基づき、以下の式（２）に基づいて仮想反発力ｆｉをそれぞれのリンクについて算出する（ステップＳ７２）。

なお、図５Ｃに示す場合は、決定した距離ｄｉ１及びｄｉ２に基づき、式（２）に基づいてそれぞれ仮想反発力ｆｉ１及びｆｉ２を算出した上で、仮想反発力ｆｉ１及びｆｉ２を合成し、仮想反発力ｆｉを算出する。

すなわち、進入禁止領域Ｓ１と干渉していると判定されたｉ番目のリンクに対し、幾何モデルが進入禁止領域Ｓ１に進入した距離に比例する仮想反発力ｆｉを仮想的に発生させる。これによって、幾何モデルが進入禁止領域Ｓ１に深く進入した場合は、強く押しだすようになっている。したがって、幾何モデルが押し出されるまでに要する時間を短縮することができる。

一方、進入禁止領域Ｓ１と干渉していないと判定されたｉ番目のリンクには、仮想反発力ｆｉを発生させないように構成している。

次に、仮想反発力ｆｉによって生じるロボット１００の各軸周りに生じる仮想のトルクτを算出する（ステップＳ７３）。

すなわち、境界線Ｂと干渉していると判定されたｉ番目のリンクにおいて発生する仮想反発力ｆｉは、ｉ番目のリンクのみならず、ｉ＜ｎ番目のリンクにも影響を与えるように構成されている。これによって、ｉ番目のリンクに仮想反発力ｆｉを発生させた点が当該リンクの死点である場合であっても、当該仮想反発力ｆｉは他のリンクに影響を与えトルクを発生させる。よって、当該トルクによって他のリンクが回動し、仮想姿勢Ｐｐとは異なる経由姿勢Ｐｖを生成することができる。

また、角度ψｉの算出は、各幾何モデルと進入禁止領域Ｓ１との干渉形態に応じて行う。

例えば、図５Ａに示す場合、角度ψｉは、ｘｙ平面において境界線Ｂの法線方向とｘ軸のなす角度である。

また、図５Ｂに示す場合、角度ψｉは、線分ＰＱとｘ軸のなす角度である。

更に、図５Ｃに示す場合、角度ψｉは、上記仮想反発力の合成ベクトルとｘ軸のなす角度である。

次に、上記仮想トルクτｉの影響によって生じる、ロボット１００の仮想姿勢の経時変化を算出する。

ここでｑ（・）は、ｑの上に符号・（ドット）が一つ付された記号であるとする。

そして、（２）式と（３）式とを繰り返し計算することによって、τ［ｋ］とｑ（・）［ｋ］が０に収束したときのｑ［ｋ］を算出する。このｑ［ｋ］が、第１アーム２、第２アーム３、及びハンド４が進入禁止領域Ｓ１から動作領域Ｓ２に押し出された姿勢である。

そして、ｑ［ｋ］に基づいて、仮想姿勢Ｐｐを設定する（ステップＳ７４）。

更新軌道決定部４１は、現在姿勢Ｐｓから経由姿勢Ｐｖを経由して最終姿勢Ｐｔに変化する場合におけるハンド４の軌道を更新軌道Ｔｂとして決定する。

［動作例］
次に、ロボット１００の動作例を説明する。

図６は、本発明の実施の形態におけるロボット１００の動作例を示すフローチャートである。図７Ａ〜図７Ｆは、本発明の実施の形態におけるロボット１００の動作例を示す図である。

まず、図７Ａに示すように、制御部２１の最終姿勢決定部３４は、ロボット１００の最終姿勢Ｐｔを決定する（最終姿勢決定ステップ）（ステップＳ１０）。本実施の形態において、制御部２１の最終姿勢決定部３４は、４つの最終候補姿勢Ｐｔｃ１〜Ｐｔｃ４についてＥの値を求め、Ｅの値が最小となる最終候補姿勢Ｐｔｃ４を最終姿勢Ｐｔとして選択する。このように、現在姿勢Ｐｓに近い姿勢を最終姿勢Ｐｔとして選択するように構成されているので、現在姿勢Ｐｓから最終姿勢Ｐｔに到達するまでに要する各軸の変位を少なくすることができ、到達までに要する平均時間を短縮することができる。

次に、図７Ｂに示すように、制御部２１の初期軌道決定部３５は、ロボット１００が現在姿勢Ｐｓから最終姿勢Ｐｔに向かって変化する初期軌道Ｔａを決定する（初期軌道決定ステップ）（ステップＳ２０）。

次に、図７Ｃに示すように、制御部２１の仮想姿勢算出部３６は、初期軌道Ｔａにおいて現在姿勢Ｐｓの近傍であって、現在姿勢Ｐｓから最終姿勢Ｐｔに向かう側における所定の点におけるロボット１００の仮想姿勢Ｐｐを算出する（仮想姿勢算出ステップ）（ステップＳ３０）。ここで、近傍とは、現在姿勢から各軸に微小な変更を加えて得られる姿勢をいう。

次に、制御部２１の領域設定部４０は、進入禁止領域Ｓ１及び動作領域Ｓ２を記憶部２２から読み込み、設定する（領域設定ステップ）（ステップＳ３５）。

次に、制御部２１の幾何モデル表現部３７は、仮想姿勢Ｐｐにあるロボット１００の第１アーム２，第２アーム３，及びハンド４をモデリングして、それぞれ幾何モデルＭ１，Ｍ２，及びＭ３で表現する（幾何モデル表現ステップ）（ステップＳ４０）。

幾何モデルＭ１〜Ｍ３は、ロボット１００の第１アーム２、第２アーム３、及びハンド４を簡略化して表したモデルであるので、後述する干渉の判定に要する計算量を削減することができ、干渉の判定を迅速に行うことができる。

また、上述の通り、幾何モデルＭ１からＭ３の仮想線によって描かれる図形の外周は、基板を保持するハンド４をｘｙ平面に投影した図形の外側において、この図形と離間するように規定（画定）される。すなわち、幾何モデルは、第１アーム２、第２アーム３、及びハンド４よりも肉厚に表現されるので、後述するステップＳ５０において制御部２１の干渉判定部３８が幾何モデルと進入禁止領域Ｓ１とが干渉していると判定した場合であっても、ロボット１００と設備１１０が干渉しないようにすることができ、実際にロボット１００又はロボット１００が保持する基板が設備１１０に接触することを防止することができる。

また、進入禁止領域Ｓ１は、設備１１０のチャンバ１１３の内壁面よりも内側の領域も含むように設定されるので、実際にロボット１００又はロボット１００が保持する基板が設備１１０に接触することをより確実に防止することができる。

次に、制御部２１の干渉判定部３８は、仮想姿勢Ｐｐにおける幾何モデルＭ１〜Ｍ３が進入禁止領域Ｓ１と干渉するか否かをそれぞれ判定する（干渉判定ステップ）（ステップＳ５０）。すなわち、幾何モデルと進入禁止領域Ｓ１とが干渉しているとは、幾何モデルの境界線と境界線Ｂとが交差している状態のことである。

なお、本実施の形態において、幾何モデル表現部３７は干渉判定部３８による干渉判定の直前のステップにおいて幾何モデルの生成を行っているが、これに限定されるものではなく、干渉判定部３８による干渉判定より前の任意のステップで実行してもよい。
同様に、領域設定部４０による進入禁止領域Ｓ１及び動作領域Ｓ２の設定も干渉判定部３８による干渉判定より前の任意のステップで実行してもよい。

そして、何れも干渉しないと判定した場合（ステップＳ５０においてＮｏ）、制御部２１の経由姿勢決定部３９は、仮想姿勢Ｐｐを経由姿勢Ｐｖとして決定する（経由姿勢決定ステップ）（ステップＳ６０）。

従って、ロボット１００の幾何モデルが進入禁止領域Ｓ１と干渉しない場合には、初期軌道Ｔａ上の点に対応する経由姿勢が与えられるので、最終姿勢Ｐｔに近づく。

一方、図７Ｄに示すように、ステップＳ５０において、制御部２１の干渉判定部３８が幾何モデルと境界線Ｂとが干渉すると判定した場合（ステップＳ５０においてＹｅｓ）、制御部２１の経由姿勢決定部３９は、図７Ｅに示すように、幾何モデルの干渉部分が進入禁止領域Ｓ１から動作領域Ｓ２に押し出された状態の姿勢を算出し、この算出した姿勢を経由姿勢Ｐｖとして決定する（経由姿勢決定ステップ）（ステップＳ７０）。

従って、ロボット１００の幾何モデルが進入禁止領域Ｓ１と干渉する場合には、進入禁止領域Ｓ１に干渉しない経由姿勢Ｐｖが与えられるので障害物との衝突が回避される。

なお、上記仮想姿勢を経由姿勢Ｐｖに決定する際に、更に、現在姿勢から上記押し出された状態の姿勢に至る軌道が進入禁止領域Ｓ１と干渉しないか否かを判定してもよい。

ステップＳ６０又はステップＳ７０において経由姿勢Ｐｖが決定されると、次に、制御部２１の更新軌道決定部４１は、現在姿勢Ｐｓから経由姿勢Ｐｖを経由して最終姿勢Ｐｔに変化する場合におけるハンド４の軌道を更新軌道Ｔｂとして決定する（更新軌道決定ステップ）（ステップＳ８０）。

次に、制御部２１は、経由姿勢Ｐｖを新たな現在姿勢Ｐｓとして決定（仮定）すると共に、更新軌道Ｔｂを新たな初期軌道Ｔａとして決定（仮定）する（ステップＳ９０）。

次に、制御部２１の仮想姿勢算出部３６は、初期軌道Ｔａにおいて現在姿勢Ｐｓの近傍であって、現在姿勢Ｐｓから最終姿勢Ｐｔに向かう側における所定の点における新たな仮想姿勢Ｐｐを算出する（ステップＳ１００）。

次に、ステップＳ１００で算出した仮想姿勢Ｐｐが最終姿勢Ｐｔと一致しているか否かを判定する。そして、一致していないと判定すると（ステップＳ１１０においてＮｏ）は、繰り返し上記ステップＳ４０〜ステップＳ１１０を実行する。すなわち、ステップＳ４０〜ステップＳ１１０は、現在姿勢Ｐｓが最終姿勢Ｐｔが一致するまで実行される。

そして、ステップＳ１００で算出した仮想姿勢Ｐｐが最終姿勢Ｐｔと一致すると（ステップＳ１１０においてＹｅｓ）、処理を終了する。これによって、図７Ｆに示すように、進入禁止領域Ｓ１との干渉を回避した最終的な更新軌道Ｔｂが生成される。

なお、本実施の形態において、ロボット１００は、上記処理が終了した後に、最終的な更新軌道Ｔｂに沿ってロボット１００を現在姿勢Ｐｓから最終姿勢Ｐｔに向かって実際に動かすように構成されている。すなわち、ステップＳ９０において経由姿勢Ｐｖを新たな現在姿勢Ｐｓとして再設定したときに、この現在姿勢Ｐｓに向かって、制御部２１が第１アーム２、第２アーム３、ハンド４を動かさず、決定した更新軌道Ｔｂの履歴を記憶部２２に記憶するように構成されている。しかし、これに限られるものではなく、ステップＳ９０において経由姿勢Ｐｖを新たな現在姿勢Ｐｓとして決定する度に、この現在姿勢Ｐｓに向かって、制御部２１が第１アーム２、第２アーム３、及びハンド４を実際に動かしてもよい。

以上に説明したように、本発明のロボット１００は、ロボット１００が現在姿勢Ｐｓから最終姿勢Ｐｔに向かって変化する場合におけるハンド４の初期軌道Ｔａ上の所定の点に対応するロボット１００の仮想姿勢Ｐｐを算出し、仮想姿勢Ｐｐにおけるロボット１００の幾何モデルＭ１〜Ｍ３が障害物に対応して規定（画定）される進入禁止領域Ｓ１と干渉するか否かを判定する。そして、干渉しないと判定した場合には仮想姿勢Ｐｐを、現在姿勢Ｐｓから最終姿勢Ｐｔに向かって変化する場合に経由する経由姿勢Ｐｖとして決定し、干渉すると判定した場合には、仮想反発力ｆｉによって幾何モデルの干渉部分が進入禁止領域Ｓ１から動作領域Ｓ２に押し出された状態の姿勢を算出し、その姿勢を経由姿勢Ｐｖとして決定する。従って、ロボット１００の幾何モデルが進入禁止領域Ｓ１と干渉する場合には、進入禁止領域Ｓ１に干渉しない経由姿勢Ｐｖが与えられるので、障害物との衝突が回避される。

一方、ロボット１００の幾何モデルが進入禁止領域Ｓ１と干渉しない場合には、初期軌道Ｔａ上の点に対応する経由姿勢Ｐｖが与えられるので、最終姿勢Ｐｔに近づく。そして、上記構成によれば、新たに与えられた経由姿勢Ｐｖを現在姿勢Ｐｓとしてこの手順を繰り返すので、ロボット１００の幾何モデルが進入禁止領域Ｓ１と干渉しない手順が出現する都度、その手順における初期軌道Ｔａ上の点に対応する経由姿勢Ｐｖが与えられて最終姿勢Ｐｔに近づく。従って、試行錯誤を繰り返すことにより、最終的に、ロボット１００が、障害物との衝突を回避しながら現在姿勢Ｐｓから最終姿勢Ｐｔに変化する場合におけるハンド４の更新軌道Ｔｂを得ることができる。

その結果、ロボットの幾何モデル、進入禁止領域、及び動作領域の設定という単純な設定で、ロボットがどの位置で停止しても障害物との干渉を自動で回避して、安全にロボットを所定の姿勢に到達させることができる。

（実施の形態２）
以下では実施の形態２の構成、動作について、実施の形態１との相違点を中心に述べる。

上記実施の形態１において、初期軌道Ｔａは、現在姿勢Ｐｓ及び最終姿勢Ｐｔが与えられることによって一義的に決まる軌道である。また、ステップＳ７０において決定される経由姿勢Ｐｖは、仮想姿勢Ｐｐにおける各幾何モデルと境界線Ｂとの位置関係において一義的に決まる姿勢である。よって、実施の形態１においては、例えば、反発力により押し戻す方向が初期軌道Ｔａの方向と一致する場合には、仮想反発力ｆｉによって押し出された状態の姿勢に基づいて決定された更新軌道Ｔｂが最終姿勢Ｐｔに近づかない場合がある。

本実施の形態は、更新軌道Ｔｂが最終姿勢Ｐｔに近づかない場合を考慮した実施の形態である。

図８は、本発明の実施の形態２に係るロボット１００の動作例を示すフローチャートである。

本実施の形態において、ステップＳ７０の経由姿勢の決定は以下のように実行される。

まず、停留状態に陥っているか否かを判定する（停留状態判定ステップ）（ステップＳ２７１）。本実施の形態において、停留状態に陥っているか否かの判定は、仮想姿勢Ｐｐと最終姿勢Ｐｔとの各関節軸の関節角度の変位の絶対値の総和Ｅを以下の式（５）に基づいて算出する。

そして、Ｅの値が減少していれば、最終姿勢に近づいていると判定する。一方、Ｅの値が変化しない、又は増加していれば、最終姿勢Ｐｔに近づいていない、又は最終姿勢Ｐｔから遠ざかっていると判定する。そして、所定の時間間隔において、Ｅの値が変化しない、又は増加している状態が持続していれば、停留状態に陥っていると判定する。

そして、ステップＳ２７１において、制御部２１が停留状態に陥っていないと判定すると、ステップＳ７１〜Ｓ７４を実行する。ステップＳ７１〜Ｓ７４は、上記実施の形態１のステップＳ７１〜Ｓ７４と同様であるので、その説明を省略する。

一方、ステップＳ２７１において、制御部２１が停留状態に陥っていると判定すると、制御部２１の経由姿勢決定部３９は、現在姿勢Ｐｓの近傍において仮想反発力ｆｉによって押し出された状態の姿勢以外の経由候補姿勢Ｐｖｃを算出する（経由候補姿勢生成ステップ）（ステップＳ２７２）。本実施の形態において、経由候補姿勢Ｐｖｃは、ランダムに生成された姿勢である。しかし、これに限定されるものではない。これに代えて、例えば、現在姿勢Ｐｓの周囲の状況に応じて生成してもよい。

次に、経由候補姿勢Ｐｖｃが現在姿勢Ｐｓよりも最終姿勢Ｐｔに近づく姿勢であるか否かを判定する（第１判定ステップ）（ステップＳ２７３）。本実施の形態において、経由候補姿勢Ｐｖｃが最終姿勢Ｐｔに近づく姿勢であるかの否かの判断は、以下の式（６）に基づいて算出したＥの値と、以下の式（７）に基づいて算出したＥの値とを比較することによって行う。

すなわち、上記式（６）は、経由候補姿勢Ｐｖｃと最終姿勢Ｐｔとの各関節軸の関節角度の変位の絶対値の総和Ｅ（ｑｒ）を求めるものである。また、上記式（７）は、現在姿勢Ｐｓと最終姿勢Ｐｔとの各関節軸の関節角度の変位の絶対値の総和Ｅ（ｑ）を求めるものである。そして、上記式（８）は、Ｅ（ｑｒ）とＥ（ｑ）との値を比較する式である。

そして、例えば、上記式（８）において、Δ≦０であれば、すなわち、経由候補姿勢Ｐｖｃに係るＥ（ｑｒ）の値が現在姿勢ＰｓのＥ（ｑ）の値と等しい又は小さければ、最終姿勢Ｐｔに近づく姿勢であると判定する。一方、Δ＞０であれば、すなわち、経由候補姿勢Ｐｖｃに係るＥ（ｑｒ）の値が現在姿勢ＰｓのＥ（ｑ）の値よりも大きければ最終姿勢Ｐｔに近づく姿勢でないと判定する。

そして、制御部２１の経由姿勢決定部３９が最終姿勢Ｐｔに近づく姿勢であると判定すると（ステップＳ２７３においてＹｅｓ）、経由候補姿勢Ｐｖｃに基づいて経由姿勢Ｐｖを決定する（第１決定ステップ）（ステップＳ２７４）。更に、現在姿勢から当該経由姿勢Ｐｖに至る軌道が進入禁止領域Ｓ１と干渉しないか否かを判定する。

一方、制御部２１の経由姿勢決定部３９が最終姿勢Ｐｔに近づく姿勢でないと判定すると（ステップＳ２７３においてＮｏ）、更に経由候補姿勢Ｐｖｃを経由姿勢Ｐｖに選択するか否かを決定する（第２判定ステップ）（ステップＳ２７５）。

この選択は、確率値を用いて行う。本実施の形態において、この選択は、経由候補姿勢Ｐｖｃの評価値に応じた遷移確率Ｐ（Δ，ｔ）を以下の式（９）によって算出し、この確率に従って、経由候補姿勢Ｐｖｃを経由姿勢Ｐｖに選択するか否かを決定する。また、この遷移確率のグラフを図９のａで示す線として例示する。

よって、遷移確率Ｐ（Δ，ｔ）はΔが０に近づくほど、すなわち、経由候補姿勢Ｐｖｃが現在姿勢Ｐｓよりも最終姿勢Ｐｔから乖離する（遠ざかる）度合が少ないほど、経由候補姿勢Ｐｖｃが経由姿勢Ｐｖとして選択される可能性が高まり（１に近づき）、経由候補姿勢Ｐｖｃが現在姿勢Ｐｓよりも最終姿勢Ｐｔから乖離する度合が大きいほど、経由候補姿勢Ｐｖｃが経由姿勢Ｐｖとして選択される可能性が低くなる（０に近づく）。

そして、経由候補姿勢Ｐｖｃが経由姿勢Ｐｖとして選択されると、経由候補姿勢Ｐｖｃに基づいて経由姿勢Ｐｖを決定する（第２決定ステップ）（ステップＳ２７６）。

そして、制御部２１は、上記実施の形態と同様にステップＳ８０以降の処理を実行する。

なお、経由候補姿勢Ｐｖｃが現在姿勢Ｐｓよりも最終姿勢Ｐｔから乖離する度合が少ない姿勢には、停留状態から抜け出すことのできる姿勢が見つかり難い場合がある。このような場合は、図９のグラフにおいてｂで示す線で示すように、停留状態に陥った時に上記（９）式のパラメータｔの値を大きくすることによって、現在姿勢Ｐｓよりも最終姿勢Ｐｔから乖離する度合が比較的大きい経由候補姿勢Ｐｖｃであっても選択され易いように調整してもよい。

また、現在姿勢Ｐｓが最終姿勢Ｐｔに近づくに従って、最終姿勢Ｐｔから乖離する姿勢を選択する確率を下げることが好ましい。したがって、現在姿勢Ｐｓが最終姿勢Ｐｔに近づくに従って、図９のグラフにおいてｂで示す線で示すように、パラメータｔの値を小さくすることによって、最終姿勢Ｐｔから乖離する姿勢を選択する確率を下げてもよい。

このように、本実施の形態において、ロボット１００は、例えば反発力ｆｉにより押し出す方向が初期軌道Ｔａの方向と一致する場合には、動作領域Ｓ２に押し出された姿勢が初期軌道Ｔａ上の点に対応するので、進入禁止領域Ｓ１に対する幾何モデルの干渉が繰り返されて停留状態に陥り、ロボット１００が最終姿勢Ｐｔに近づかない（最終姿勢に到達できない）。このような場合に、初期軌道Ｔａ上にない点に対応する経由姿勢Ｐｖを得ることができ、この姿勢に基づいて更新軌道Ｔｂを決定し、最終姿勢Ｐｔに近づく別の経由姿勢Ｐｖを経由して最終姿勢Ｐｔに到達することができる。

また、別の最終姿勢Ｐｔに近づく姿勢を算出することができない場合であっても、別の姿勢を経由して最終姿勢Ｐｔに到達することができるか否かを試みることができる。これによって、現在の最終姿勢Ｐｔに到達できない状態を好適に脱することができる。そして、最終姿勢Ｐｔと経由候補姿勢Ｐｖｃとの乖離度に比例して低下する確率値に基づいて経由候補姿勢を選択するか否かを判定しているので、現在姿勢Ｐｓとの乖離度が小さい経由候補姿勢Ｐｖｃが経由姿勢Ｐｖとして選択される可能性が高くなる。これによって、より適切に現在の最終姿勢に到達できない状態を好適に脱することができる。

図１０Ａ及び図１０Ｂは、本実施の形態におけるロボット１００のシミュレーション条件を示す図である。図１０Ａに示すように、ロボット１００の現在姿勢Ｐｓ、進入禁止領域Ｓ１、動作領域Ｓ２、幾何モデルＭを設定した。また、図１０Ｂに示すように、ロボット１００の最終姿勢Ｐｔを設定した。なお、最終姿勢Ｐｔにおけるハンドの教示点は、[x, y]=[0, 450]である。

図１０Ｃは、ロボット１００のシミュレーション結果を示す図であり、ハンドの軌跡を示す図である。図１０Ｃに示すように、ロボット１００は、現在姿勢Ｐｓから最終姿勢Ｐｔに到達することがわかる。

＜変形例＞
上記実施の形態においては、ロボット１００を水平多関節ロボットで構成し、幾何モデルをｘｙ平面において表現し、ｚ軸方向における干渉の有無の判定は行っていないがこれに限られるものではない。これに代えて、ロボットを垂直多関節で構成し、幾何モデルを立体的に表現し、干渉の判定を３次元空間で行ってもよい。

また、上記実施の形態においては、仮想姿勢算出部３６は、ロボット１００の仮想姿勢Ｐｐを算出していたが、これに代えて、ロボット１００の幾何モデルの仮想姿勢Ｐｐを算出してもよい。

上記説明から、当業者にとっては、本発明の多くの改良や他の実施形態が明らかである。従って、上記説明は、例示としてのみ解釈されるべきであり、本発明を実行する最良の態様を当業者に教示する目的で提供されたものである。本発明の精神を逸脱することなく、その構造及び／又は機能の詳細を実質的に変更できる。

本件発明は、産業用ロボットに適用することができる。

Ｂ 境界線
Ｌ１ 回動軸線
Ｌ２ 回動軸線
Ｌ３ 回動軸線
Ｍ１ 幾何モデル
Ｍ２ 幾何モデル
Ｍ３ 幾何モデル
Ｐｐ 仮想姿勢
Ｐｓ 現在姿勢
Ｐｔ 最終姿勢
Ｐｔｃ 最終候補姿勢
Ｐｖ 経由姿勢
Ｐｖｃ 経由候補姿勢
Ｓ１ 進入禁止領域
Ｓ２ 動作領域
Ｔａ 初期軌道
Ｔｂ 更新軌道
１ 基台
２ 第１アーム
２ａ 第１関節軸
３ 第２アーム
３ａ 第２関節軸
４ ハンド
４ａ 第３関節軸
５ 制御器
１２ 第１アーム駆動部
１３ 第２アーム駆動部
１４ ハンド駆動部
１５ 第１アーム角度位置検出部
１６ 第２アーム角度位置検出部
１７ ハンド角度位置検出部
２１ 制御部
２２ 記憶部
３１ 第１アーム制御部
３２ 第２アーム制御部
３３ ハンド制御部
３４ 最終姿勢決定部
３５ 初期軌道決定部
３６ 仮想姿勢算出部
３７ 幾何モデル表現部
３８ 干渉判定部
３９ 経由姿勢決定部
４０ 領域設定部
４１ 更新軌道決定部
１００ ロボット
１１０ 設備

Claims (6)

1. １以上のリンクが関節によって連結され、先端部にハンドが設けられたロボットアームを備え、姿勢が前記関節の角度位置に基づいて規定されるアーム型のロボットの障害物自動回避方法であって、
   前記ロボットを、幾何形状を有するようモデリングして幾何モデルとして表現する幾何モデル表現ステップと、
   前記幾何モデルが進入してはいけない進入禁止領域と、当該進入禁止領域によって規定され、前記幾何モデルが動作する動作領域とを設定する領域設定ステップと、
   前記ロボットの最終姿勢を決定する最終姿勢決定ステップと、
   前記ロボットが現在姿勢から前記最終姿勢に向かって変化する場合における前記ハンドの初期軌道を決定する初期軌道決定ステップと、
   前記初期軌道上の所定の点に対応する前記ロボットの仮想姿勢を算出する仮想姿勢算出ステップと、
   前記仮想姿勢における前記幾何モデルが前記進入禁止領域と干渉するか否かを判定する干渉判定ステップと、
   前記干渉判定ステップにおいて干渉しないと判定した場合、前記仮想姿勢を経由姿勢として決定し、干渉すると判定した場合、前記進入禁止領域の干渉部分と前記幾何モデルの干渉部分とが相対的に反発する反発力（以下、仮想反発力という）を仮想的に発生させ、前記仮想反発力によってある仮想的な姿勢における前記ロボットの前記各関節の回動軸周りに生じるトルクを算出し、更に該トルクの影響によって変化する前記ロボットの所定時間経過後の仮想的な姿勢を算出する演算を前記仮想姿勢を起点として繰り返し行うことによって前記ロボットの仮想的な姿勢の経時変化を算出し、前記ロボットの仮想的な姿勢の経時変化が収束したときの前記ロボットの仮想的な姿勢を経由姿勢として決定する経由姿勢決定ステップと、
   前記現在姿勢から前記経由姿勢を経由して前記最終姿勢に変化する場合における前記ハンドの軌道を更新軌道として決定する更新軌道決定ステップと、
   最新の経由姿勢決定ステップにおいて決定された経由姿勢を前記初期軌道決定ステップにおける前記現在姿勢と仮定して前記初期軌道決定ステップ、前記仮想姿勢算出ステップ、前記干渉判定ステップ、前記経由姿勢決定ステップ、及び前記更新軌道決定ステップを繰り返して実行するステップと、を含む、ロボットの障害物自動回避方法。
2. 前記最終姿勢決定ステップは、既知の軌道上に設定される前記ロボットの複数の最終候補姿勢のうち、前記ロボットの現在姿勢と前記最終候補姿勢との前記各関節の角度の変位の絶対値の総和が最小の値である最終候補姿勢を前記最終姿勢として選択し、決定する、請求項１に記載のロボットの障害物自動回避方法。
3. 前記仮想反発力は、前記幾何モデルが進入禁止領域に進入した距離に比例して大きくなるように構成されている、請求項１又は２の何れかに記載のロボットの障害物自動回避方法。
4. 前記ロボットの姿勢が前記最終姿勢に近づくように経時変化せず、停留状態に陥っているか否かを判定する停留状態判定ステップと、
   前記停留状態判定ステップにおいて停留状態に陥っていると判定された場合、前記ロボットの前記仮想反発力によって押し出された状態の姿勢以外の経由候補姿勢を生成する経由候補姿勢生成ステップと、
   前記経由候補姿勢が前記最終姿勢に近づく姿勢であるか否かを判定する第１判定ステップと、
   前記第１判定ステップにおいて、前記経由候補姿勢が前記最終姿勢に近づく方向に変化する姿勢であると判定された場合、該経由候補姿勢に基づいて前記経由姿勢を決定する第１決定ステップとを有する、請求項１乃至３の何れかに記載のロボットの障害物自動回避方法。
5. 前記第１判定ステップにおいて、前記経由候補姿勢が前記最終姿勢から遠ざかる方向に変化する姿勢であると判定された場合、確率値に基づいて前記経由候補姿勢を選択するか否かを判定する第２判定ステップと、
   前記第２判定ステップにおいて選択すると判定されると、当該経由候補姿勢に基づいて前記経由姿勢を決定する第２決定ステップとを有する、請求項４に記載のロボットの障害物自動回避方法。
6. １以上のリンクが関節によって連結され、先端部にハンドが設けられたロボットアームを備え、姿勢が前記関節の角度位置に基づいて規定されるアーム型のロボットの制御装置であって、
   前記ロボットを、幾何形状を有するようモデリングして幾何モデルとして表現する幾何モデル表現部と、
   前記幾何モデルが進入してはいけない進入禁止領域と、当該進入禁止領域によって規定され、前記幾何モデルが動作する動作領域とを設定する領域設定部と、
   前記ロボットの最終姿勢を決定する最終姿勢決定部と、
   前記ロボットが現在姿勢から前記最終姿勢に向かって変化する場合における前記ハンドの初期軌道を決定する初期軌道決定部と、
   前記初期軌道上の所定の点に対応する前記ロボットの仮想姿勢を算出する仮想姿勢算出部と、
   前記仮想姿勢における前記幾何モデルが前記進入禁止領域と干渉するか否かを判定する干渉判定部と、
   経由姿勢を決定する経由姿勢決定部と、
   更新軌道決定部と、を備え、
   前記経由姿勢決定部は、前記干渉判定部が干渉しないと判定した場合、前記仮想姿勢を経由姿勢として決定し、干渉すると判定した場合、前記進入禁止領域の干渉部分と前記幾何モデルの干渉部分とが相対的に反発する反発力（以下、仮想反発力という）を仮想的に発生させ、前記仮想反発力によってある仮想的な姿勢における前記ロボットの前記各関節の回動軸周りに生じるトルクを算出し、更に該トルクの影響によって変化する前記ロボットの所定時間経過後の仮想的な姿勢を算出する演算を前記仮想姿勢を起点として繰り返し行うことによって前記ロボットの仮想的な姿勢の経時変化を算出し、前記ロボットの仮想的な姿勢の経時変化が収束したときの前記ロボットの仮想的な姿勢を経由姿勢として決定し、
   前記更新軌道決定部は、前記現在姿勢から前記経由姿勢を経由して前記最終姿勢に変化する場合における前記ハンドの軌道を更新軌道として決定し、
   前記経由姿勢決定部が決定した最新の経由姿勢を前記初期軌道決定部の現在姿勢と仮定して、前記初期軌道決定部、前記仮想姿勢算出部、前記干渉判定部、前記経由姿勢決定部、及び前記更新軌道決定部が繰り返し処理する、アーム型のロボットの制御装置。

Images