JP4244443B2 - ロボットの大域動作経路計画方法とその制御装置 - Google Patents
ロボットの大域動作経路計画方法とその制御装置 Download PDFInfo
- Publication number
- JP4244443B2 JP4244443B2 JP14666799A JP14666799A JP4244443B2 JP 4244443 B2 JP4244443 B2 JP 4244443B2 JP 14666799 A JP14666799 A JP 14666799A JP 14666799 A JP14666799 A JP 14666799A JP 4244443 B2 JP4244443 B2 JP 4244443B2
- Authority
- JP
- Japan
- Prior art keywords
- robot
- sequence
- search
- subgoal
- route
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Images
Landscapes
- Numerical Control (AREA)
- Manipulator (AREA)
Description
【０００１】
【発明の属する技術分野】
本発明は、ロボットの作業環境にある物体の幾何学的形状と、その配置を記述した計算機上の幾何モデルを使ってロボットの動作経路を計画する方法に関するものであり、特に、ロボットのスタート配置とゴール配置が与えられたとき、モデル同士の干渉を検査する干渉検査手段を利用してロボットと作業環境内の障害物とが干渉しないようにするロボットの大域動作経路計画方法とその方法を実施する装置に関する。
【０００２】
【従来の技術】
ロボットの障害物回避に係る経路計画問題は、配置空間（Ｃ空間：Configurationspace）における、点の移動経路計画問題という形式に一般化することができる。Ｃ空間はロボットの配置（Configuration ）を一意に決定するパラメータ空間である。
図１２は従来のロボットが置かれている座標系およびロボットの配置空間（Ｃ空間）を示す図であり、この図１２を参照してＣ空間について説明する。図１２（ａ）、（ｂ）はそれぞれ２次元作業空間１０３における３自由度マニピュレータ１０１、３自由度移動ロボット１０２を示している。
３自由度マニピュレータ１０１の場合、図１２（ｃ）に示した関節空間（q1,q2,q3） はＣ空間となる。２次元作業空間内における移動ロボットの場合、作業空間座標系を基準にしたロボット座標原点の位置 (X,Y)とロボットの向きθの３自由度空間（X,Y,θ）はＣ空間となる。Ｃ空間において障害物と干渉しない経路を求めるためには、通常Ｃ空間の空間記述が必要になる。即ち、作業空間１０３における障害物１０４ａ、１０４ｂとロボット１０１の幾何学的形状およびその位置関係を用いてＣ空間における障害物１０５ａ、１０５ｂ（Ｃ空間障害物）を記述し、Ｃ空間内の自由（非干渉）空間（Ｃ自由空間）を求める必要がある。
Ｃ空間記述に基づくロボットの経路計画方法は、ロードマップ法、セル分解法、Ｃ空間離散化法、ポテンシャルフィールド法に大別される。
ロードマップ法と、セル分解法、Ｃ空間離散化法の相違点は、Ｃ空間の記述法と、その記述に基づく探索手法にある。ロードマップ法やセル分解法では、通常、Ｃ空間の陽な記述が必要になる。２次元作業空間における２自由度移動ロボットの場合は、Ｃ自由空間を陽に記述することができる。すなわち、Ｃ空間障害物の境界を陽に求めることができる。多関節マニピュレータの場合は、Ｃ障害物を解析的に求める一般的手法は存在せず、Slice Projection（Needle）法、Swept Volume法、Point Evaluation法などのＣ空間の離散化に基づく近似手法によりＣ空間記述を求めるのが一般的である。ロードマップ法やセル分解法は２次元作業空間における移動ロボットの経路計画問題に適しており、Ｃ空間離散化法は多関節マニピュレータの経路計画問題に適している。
ポテンシャルフィールド法は、ゴール配置からロボットに作用する引力、障害物からロボットに作用する斥力、ロボットを動作範囲内に留める斥力の和からなる人工ポテンシャル関数を定義し、ポテンシャル関数のＣ空間における最急勾配方向に経路を探索する方法である。多関節マニピュレータの場合は、Ｃ障害物を陽に記述するのは困難なため、リンクにいくつかの代表点を設け、各代表点から一定距離内にある障害物からそれぞれの代表点に作用する斥力を作業空間で定義する。ゴール配置以外のポテンシャル極小点を如何に避けるか、現実的に計算可能なポテンシャル関数を如何に導出するかが課題となる。
これら従来方式は、文献「Y.K.Hwang and N.Ahuja : Gross Motion Planning -A Survey, ACM Computing Surveys, Vol.24, No.3, pp.219-291, 1992」に詳しくまとめられているので、詳細は省略する。
経路計画問題は、計算の複雑性理論（Computational Complexity Thoery ）によれば、NP（Nondeterministic Polymonial ）--hardと呼ばれる解の導出が非常に困難な問題に属する。ロボットの自由度、障害物の数、障害物の面の数などが大きくなるに従って、現実的な時間内では障害物回避経路を求めることができなくなる。上の従来法を現状の100MIPS 程度の計算機を用いて数十分以内に経路が求まるという基準で評価すると、ロードマップ法やセル分解法は、２次元作業空間内における移動ロボット、Ｃ空間離散化法やポテンシャルフィールド法は、３次元作業空間内における６自由度マニピュレータという具合に適用可能範囲が限られて来る。
【０００３】
Point Evaluation法に基づくＣ空間離散化法の基本アプローチについて、図１３に示す従来の２次元離散化Ｃ空間と経路探索の進行図、を参照して説明する。図１３（ａ）の２０１は２次元Ｃ空間（q1,q2)を示している。２０３、２０４はそれぞれスタート配置Ｓ、ゴール配置Ｇを示している。２０５ａ〜ｂはＣ空間障害物である。Point Evaluation法に基づくＣ空間離散化法では、図１３（ｂ）の２０２のようにＣ空間２０１を微小間隔で離散化した格子点の空間、すなわち、離散化Ｃ空間（d-q1,d-q2)を考える。離散化Ｃ空間２０２は各格子点をノードとして、それらの隣接関係をアークとする連結度グラフと見なすことができる。
この連結度グラフ上のスタート配置Ｓから隣接する格子点を辿ってゴール配置Ｇへ至る経路（格子点列）を求めるのであるが、各格子点においてのみ、ロボットと作業環境との干渉の有無を幾何モデルと干渉チェッカを用いて検査する。
▲１▼ 予め全格子点での干渉の有無を求めておく方式と、▲２▼ 探索の過程で逐次各格子点における干渉の有無を検査する方式がある。
作業環境がダイナミックに変化する場合には、後者の方式を用いる。
【０００４】
格子点の連結度グラフ上での経路探索には、縦型、横型、山登り、最良優先、Ａ* 探索などのグラフ（状態空間）探索手法を用いる。図１３（ｃ）に離散化Ｃ空間においてグラフ探索が進行する様子を示す。
グラフ探索は、スタートノードＳの子ノード（隣接ノード）を生成することから始まる。1-1,1-2,1-3,1-4 が生成されたＳの子ノードである。子ノードが生成されると、ある基準に基づいてそのノードでの評価関数値が計算されノードに記憶される。また、各ノードには親ノードへのリンク（ポインタ）も記憶される。
あるノードの子ノードを生成することを、そのノードを展開するという。
生成されたノード1-1,1-2,1-3,1-4 の中から、評価関数値に基づいて次にどのノードを展開すべきかが選ばれる。
図１３（ｃ）の例では、ノード1-2 が選択されて展開されている。
図１３（ｄ）は図１３（ｃ）のグラフ探索進行状況をツリー構造（探索木と呼ばれる）で表現したものである。
図１３（ｃ）の探索済み領域の波面（境界）２０７ａ〜ｅおよび２０８ａ〜ｂが図１３（ｄ）の探索木の葉（直系の子孫を持たないノード）に対応している。
２０８ａ〜ｂは探索木の葉の中の障害物と干渉が生じたノードを示している。
スタート配置２０３および２０６ａ〜ｂは展開済みノード（子を持つノード）を示している。
また、探索木の枝が親ノードへのポインタに相当する。グラフ探索は、探索木の葉の中から評価関数値に基づいて次に展開する（干渉を生じない）ノードを選び、そのノードの子ノードを重複せずに生成する処理を、葉からゴール配置が選択される、あるいは選択すべき葉がなくなるまで繰り返す。
葉からゴール配置が選択された場合、ゴール配置から親ノードへのポインタを辿っていくことにより経路が得られる。
上述したように、Point Evaluation法に基づくＣ空間離散化法では格子点においてのみ干渉チェックを行うため、非干渉ノード間を結ぶアーク上での非干渉性は保証されない。したがって、離散化分解能を十分高くする必要がある。
【０００５】
【発明が解決しようとする課題】
しかしながら、上述の従来のＣ空間離散化法では、離散化分解能、ロボットの自由度、障害物の面の総数などの増加に対して、計算量、記憶領域が指数関数的に増加してしまう。そのため、６自由度以上のマニピュレータに対して、Ｃ空間離散化法の基本アプローチをそのまま適用するのは現実的でなく、探索空間を制限したり（局所探索）、探索効率を高めるための工夫が必要となる。様々なヒューリスティックス（発見的手法）を用いて探索空間削減、探索効率化をする方式が種々提案されているが、経路を高速に求られる実用的な方式が少ないという問題があった。
探索効率を高める方式としては、例えば、特開平１０−９７３１６、「多数の移動物体を伴うシステムの移動計画及び制御方法」、に開示されているロボット経路の計画法等がある。
図１４はその従来方式のブロック図であり、ロボットの配置空間を第１の大きなスケールでセルに分割したセル空間データベース（フリー《衝突無し》の情報、フル《衝突の危険あり》の情報、ミックス《それ以外》の情報を持つ。）を利用して、中間経由点を直線的に結んだセグメント列を求める線路計画モジュール９１２と、次に、線路計画モジュール９１２により得られたセグメント列の中間点リストより、微細な時間分解能で加減速を考慮した、第２の更に微小時間間隔のトラジェクトリ（軌跡）データを求め、それをロボットマニピュレータ・システム９１６の位置制御系に直接指令を与えて、実際にロボットを動作させ、同時に、人口ポテンシャル法を利用して（フィードバック制御を用い）衝突を動作段階で確実に回避できるように、先のトラジェクトリを修正するものであるが、経路探索という意味からは第１と第２のトラジェクトリ間でのバックトラッキングも、再探索も行われず、微細な第２のトラジェクトリに関しても、空間配置ではない時間間隔を微小間隔に分割して経路補完を行っているに過ぎないので、充分に経路探索が尽くされてはいない。それに、実際のロボットの動作段階で人口ポテンシャル方式による修正により、最終的な衝突回避を行っているので、形式的には２段階経路探索方式となってはいるが、経路の探索精度としては充分ではなく、存在する経路を見落としやすいという問題があった。
また、特願平１０−３５６７１９に、サブゴール系列（中間経由点）探索と隣接サブゴール間局所経路探索を繰り返す２段階経路計画方式が記載されている。この方式によれば、有向グラフを用いた２段階探索が行われるので経路計算の高速化が可能とはなるが、存在する経路を見落としやすいという問題があった。
本発明は、上記問題に鑑みてなされたものであり、３次元作業空間内における自由度の大きな多関節マニピュレータに対しても経路を高速に求めることが可能で、なおかつ、経路を見落とす可能性も小さいロボットの大域動作経路計画方法とその制御装置をを提供することを目的としている。
【０００６】
【課題を解決するための手段】
上記目的を達成するため、請求項１記載のロボットの大域動作経路計画方法は、ロボットのスタート配置とゴール配置とロボットの作業環境の幾何学情報と前記作業環境にある障害物の幾何学的形状及び配置とに基づいて前記ロボットと障害物とが干渉を起こさないように前記スタート配置から前記ゴール配置へ至る非干渉経路を計画するロボットの大域動作経路計画方法において、ステップ１として、前記ロボットの配置空間に、第１間隔で離散した複数の格子点と前記スタート配置および前記ゴール配置とで構成される第１経由点候補を形成し、前記障害物とロボットが干渉しない前記第１経由点候補の中から、前記スタート配置から前記ゴール配置へ至る前記第１経由点候補の経路を第１経由点系列として求め、ステップ２として、前記ステップ１で求められた前記第１経由点系列において、互いに隣接する一対の第１経由点について、前記一対の第１経由点を包含する探索範囲を設定し、前記探索範囲において前記第１間隔よりも小さい微小な第２間隔で離散した複数の格子点である第２経由点候補を形成し、前記障害物とロボットが干渉しない前記第２経由点候補の中から、前記一対の第１経由点の一方から他方へ至る前記第２経由点候補の経路を第２経由点系列として求め、ステップ３として、前記ステップ２において、前記探索範囲に前記第２経由点系列が存在しない場合は、前記ステップ１に戻り、前記ステップ２において前記第２経由点系列が存在しなかった前記一対の第１経由点を含まない新たな第１経由点系列を求め、再びステップ２に移り、前記ステップ１から前記ステップ３までの処理を前記第１経由点系列の全ての前記一対の第１経由点について前記第２経由点系列が求まるまで繰り返し、スタート配置からゴール配置に至る前記第２経由点列を前記非干渉経路とすることを特徴としている。
また、請求項２記載のロボットの大域動作経路計画方法は、前記ステップ２の前記探索範囲は、前記一対の第１経由点を包含する楕円体領域に含まれることを特徴としている。
また、請求項３記載のロボットの大域動作経路計画方法は、前記ステップ１で求めた前記第１経由点系列を全てリストに保存しておき、ステップ１の経由点系列探索に成功した場合は、その経由点系列に対して前記ステップ２を実行し、ステップ１の経由点系列探索に失敗した場合は、経由点系列のリストの中から経由点系列を適当に１つ選び出し、過去のステップ２において前記探索範囲に前記第２経由点系列が存在しなかった場合は、前回よりも前記探索範囲を広く設定することを特徴としている。
また、請求項４記載のロボットの大域動作経路計画方法は、前記第１経由点系列の前記リストから前記第１経由点系列を１つ選択する際に、最も前記ゴール配置近くまで前記ステップ２の前記第２経由点系列が求められ、かつ最もその経路長が短いものを優先的に選択することを特徴としている。
また、請求項５記載のロボットの大域動作経路計画方法は、前記ステップ２において、前記探索範囲を予め設定した最大範囲にした状態で前記探索範囲に前記第２経由点系列が存在しなかった場合、当該経由点系列を前記第１経由点系列の前記リストから除外することを特徴としている。
また、請求項６記載のロボットの大域動作経路計画方法は、前記第１経由点系列の前記リストに記憶された前記第１経由点系列の全てにおいて前記非干渉経路を求められない場合、前記第１間隔を整数個に分割してより小さい間隔をあらたな第１間隔として第１経由点候補を形成して前記ステップ１を実行することを特徴としている。
また、請求項７記載のロボットの大域動作経路計画装置は、請求項１記載のロボットの大域動作経路計画方法を実施するためのロボットの大域動作経路計画装置であって、前記ロボットの前記スタート配置と、前記ゴール配置と、前記ロボットの作業環境の前記幾何学情報と、前記作業環境にある前記障害物の幾何学的形状及び配置と、を入力するための入力手段と、前記入力手段により入力された情報に基づいて前記ステップ１から前記ステップ３までの処理を実行する計算機とを備えていることを特徴としている。
【０００７】
このロボットの大域動作経路計画方法とその制御装置では、基本的に次のようなステップ１とステップ２による２段階経路探索が実行される。
（１）、先ず、ステップ１の処理として、配置空間（Ｃ空間）または作業空間を大まかに離散化した格子点（サブゴールド候補、例えば、パラメータｄｉｓＰａｒａによる）を持つ空間（離散化Ｃ空間）を形成し、グラフ（状態空間）探索手法を用いて、スタート配置あるいはゴール配置から格子点を経由してゴール配置あるいはスタート配置へ至るサブゴール系列を求める。
（２）、次に、ステップ２の処理として、ステップ１の処理で求めたサブゴール系列から隣接するサブゴールを２つづつ取出し、ステップ１のパラメータｄｉｓＰａｒａよりも微小間隔で離散化した格子点を持つ空間（離散化Ｃ空間）を形成し、グラフ（状態空間）探索手法を用いて隣接サブゴール間の微小間隔格子点列の局所微小間隔経路を求める処理を繰り返し行う。
（３）、ステップ２の探索が失敗した場合の処理は、ステップ２でサブゴール系列に含まれる隣接サブゴールをＳｇｉ、Ｓｇｉ＋１とし、Ｓｇｉ→Ｓｇｉ＋１間の局所経路探索に失敗したとすると、バックトラッキング及び再探索を次のように実施する、ステップ１に戻り、サブゴールＳｇｉからＳｇｉ＋１へのアークを経由しない別のサブゴール系列を求め直し、再び、ステップ２に移る処理を繰り返し、スタート配置からゴール配置までの連続した微小間隔経路が求められた場合に、繰り返し処理を終了する。
（４）、ステップ１の探索が成功の場合は、求めたサブゴール系列を全てリストに保存し、そのサブゴール系列全てについてステップ２を実行する。その場合パラメータはｒＰａｒａＭｉｎ（最低微小間隔）を用いる。
また、ステップ１の探索が失敗した場合は、サブゴール系列リストから１つ選び出し、過去ステップ２で局所経路探索に失敗した隣接サブゴール間に対して前回よりも探索範囲を広く（例えば、パラメータをｒＰａｒａＭａｘとして）してステップ２を実行し、局所経路探索に成功したらパラメータを元に戻して続行する。
（５）、先のサブゴール系列リストからのサブゴール系列の選択ができなくなったら、大まかなステップ１の間隔（パラメータｄｉｓＰａｒａ）を更に整数個に分割して離散化Ｃ空間を形成し直してステップ１から実行し直すようにする。
以上のように構成することによって、経路を探索する際の探索空間が大幅に抑制され、探索効率が向上して、３次元空間内における自由度の大きい多関節マニピュレータに対しても、経路を高速で求めることが可能になる。
また、ステップ１とステップ２のバックトラッキング、再探索等が繰返し実行され、かつ、局所経路探索の探索範囲をｒＰａｒａの切換え等により動的に変更することによって、従来方式に比較すると経路探索が充分に尽くされ、探索精度が高くなるので、存在する経路を見落とす可能性も小さくなる。
【０００８】
【発明の実施の形態】
以下、本発明の実施の形態について図を参照して説明する。
図１は本発明の第１の実施の形態に係るロボットの大域動作経路計画方法の処理のフローチャートである。
図２は図１に示す離散化Ｃ空間構成法とサブゴール系列探索結果を示す図である。
本発明のロボットの大域動作経路計画方法は、図１２（ａ）、（ｂ）のようなロボットと作業環境の幾何学情報、スタート配置Ｓ、ゴール配置Ｇが与えられたとき、図１２（ｃ）のようなＣ空間においてスタート配置Ｓからゴール配置Ｇへ至る非干渉経路を計画する方法である。
以降、簡単のため２次元Ｃ空間を例にして説明を進めるが、本発明はもちろんこれに限定されるものではなく、３次元以上のＣ空間に対してもそのまま適用できることはいうまでもない。
スタート配置Ｓ、ゴール配置Ｇは、図１３（ｂ）に示した微小間隔で離散化されたＣ空間（d-q1,d-q2 ）上の座標値で与えられるものとする。又、図１３（ｂ）の離散化Ｃ空間を、以降、ｄ−Ｃ−Ｈｒｅｓ（discrete C-space with High resolution の意味）と表す。
以下本発明の方法の手順を図１に示したフローチャートを用いて説明する。
先ず、ｄ−Ｃ−Ｈｒｅｓの離散化Ｃ空間の設定と（Ｓ３００）、スタート配置Ｓ、ゴール配置Ｇを入力した後（Ｓ３０１）。
大まかな離散化刻み幅、ｄｉｓＰａｒａ（図２の４０３）、を設定し（Ｓ３０２）、図２（ａ）、（ｂ）のように離散化Ｃ空間（d-q1',d-q2')を形成する（Ｓ３０３）。 図２（ａ）、（ｂ）の４０３ａ、４０３ｂに示したように、ｄｉｓＰａｒａはｄ−Ｃ−Ｈｒｅｓの刻み幅に比べて十分大きな値とする。離散化Ｃ空間（d-q1',d-q2')は大まかな間隔で離散化されたＣ空間であり、ｄ−Ｃ−Ｌｒｅｓと表す（discrete C-space with Low resolutionの意味）。図２（ａ）ではｄ−Ｃ−Ｌｒｅｓのスタート配置Ｓ４０１、ゴール配置Ｇ４０２の座標値はそれぞれ（0,0), （5,0)となる。同様に図２（ｂ）ではそれぞれ（0,0),(4,3)となる。本発明の方法によれば、この例のようにスタート配置を離散化Ｃ空間ｄ−Ｃ−Ｌｒｅｓの原点とする必要はなく、また、座標軸が直交している必要もない。さらには、（d-q1,d-q2)から（d-q1',d-q2')に非線形変換しても良く、ｄ−Ｃ−Ｌｒｅｓの定義の仕方は任意である。
【０００９】
つぎに、STEP1 のサブゴール系列探索が行われる（Ｓ３０４）。
図３は図１に示すSTEP1 の処理の内部フローチャートである。
この図３は、ｄ−Ｃ−Ｌｒｅｓでのスタート配置Ｓからゴール配置Ｇまでの経路探索に等しい。前述した最良優先、Ａ* 探索などのグラフ探索手法を適用してサブゴール系列を求める。グラフ探索を行うためには、図１３（ｄ）に示した探索木のデータ構造を計算機内部に持つ必要がある。探索木の葉とそれ以外のノードを別々の連結リストに格納する。
図４は図３に示すグラフ探索に用いる連結リストのデータ構造の例を示す図である。
図４（ａ）は連結リスト６０１とそのセル６０２の基本クラスであり、図４（ｂ）のグラフ探索に用いる連結リスト６０３とそのセル６０４のクラスは、図４（ａ）の派生クラスとして定義している。探索木の葉を格納する連結リストをOPENLIST、それ以外のノードを格納するリストをCLOSEDLISTと呼ぶ。OPENLIST内の干渉が生じるノードを別のリスト（COLLISIONLIST)に格納しても良い。又、グラフ探索手法は公知の技術であるため、そのアルゴリズムの詳細については説明を省略する。
スタート配置Ｓ、ゴール配置Ｇを含め、STEP1 で生成された全てのノード（サブゴールおよびその候補）は重複を省いて、有向グラフの形式で計算機上に記憶される。
【００１０】
図５は図３に示すサブゴール有向グラフのデータ構造を示す図である。
図中、７０１は全ての生成済みサブゴール候補を格納する連結リストであり、データ型（クラス）とオブジェクト（変数）名を CLListForSgGraph SgGraph;と定義する。連結リスト７０１の各セル７０２には、サブゴール候補の座標値や干渉の有無のデータに加え、隣接するサブゴール候補に関するデータを格納するリスト７０３が格納される。そのデータ型（クラス）とオブジェクト（変数）名を CLListForAdjSGs AdjSGList;と定義する。７０４はAdjSGList の各セルである。
図２（ｃ）にSTEP１により探索されたサブゴール系列の例を示す。同図においてS-Sg1-Sg2-Sg3-Sg4-G が求まったサブゴール系列である。
図１のフローチャートに戻って、STEP1 のサブゴール系列探索に成功した場合（TRUE）は、そのサブゴール系列SgSeq がサブゴール系列リストListOfSgSeq に記憶される（Ｓ３０５）。
【００１１】
図６は図１に示すサブゴール系列リストのデータ構造を示す図である。
図中、８０１がサブゴール系列を格納する連結リストであり、データ型（クラス）とオブジェクト（変数）名を CLListForSGSeq ListOfSgSeq;と定義する。連結リスト８０１の各セル８０２には、サブゴール系列８０３（サブゴールの連結リスト）とSTEP2 の実行結果に関するBOOL型のフラグが複数格納されている。サブゴール系列８０３の各セル８０４には、各サブゴールのｄ−Ｃ−Ｌｒｅｓにおける座標値などのデータが格納されている。
つぎに、サブゴール系列リスト８０１から何らかの基準に従って１つのサブゴール系列が選択される（Ｓ３０６）。STEP1 に失敗してサブゴール系列が求まらなかった場合（FALSE)は、Ｓ３０５をスキップしてＳ３０６が実行される。
Ｓ３０６で選択されたサブゴール系列に対して、後述するパラメータｒＰａｒａが設定されて、STEP2 の隣接サブゴール間局所経路探索が行われる（Ｓ３０７）。
図７は図１に示すSTEP2 の内部フローチャートである。
図７に移って、STEP2 の処理については、図１のＳ３０６で選択されたサブゴール系列から、隣接する２つのサブゴールを選択して（Ｓ９０１）、パラメータのｒＰａｒａを設定し（Ｓ９０２）、ｄ−Ｃ−Ｈｒｅｓ上で局所経路探索が行われる（Ｓ９０３）。ここでも、STEP1 と同様に最良優先、Ａ* 探索などのグラフ探索手法が適用される。Ｓ９０３の局所経路探索に成功した場合、ゴール配置Ｇまで局所経路が求まったかを判断する（Ｓ９０４）。ゴール配置Ｇまで到達していない場合、Ｓ９０１に戻ってつぎの隣接する２つのサブゴールが選択される。ゴール配置Ｇに到達した場合、TRUEを返してSTEP2 を終了する。途中でＳ９０３に失敗した場合、FALSE を返してSTEP2 を終了する。
Ｓ９０２で設定されるパラメータｒＰａｒａは、隣接サブゴール間局所経路探索の探索範囲を制限するパラメータである。
図８は図７に示す経路探索範囲を制限する局所領域を示す図である。
図８において、１００１ａ、１００１ｂは共に隣接する２つのサブゴールを包含する楕円体領域である。１００２、１００３はそれぞれサブゴールＳｇ１、Ｓｇ２、１００４はＣ空間障害物である。
この楕円体領域を用いて探索範囲を制限する場合、以下の式によって探索範囲Ｓrng を定義する。但し、ｄ（ｎ、ｍ）は離散化Ｃ空間ｄ−Ｃ−Ｈｒｅｓにおける２点ｎ，ｍ間の距離を表す。
Ｓrng=｛ n｜d(Sg1,n)＋d(n,Sg2 ） <=d(Sg-i,Sg-i+1）×rPara,rPara>=1｝
上式より、パラメータｒＰａｒａを小さくするほど探索範囲Ｓrng が制限されることがわかる。ｒＰａｒａ＝１のときは、隣接サブゴールＳｇ１、Ｓｇ２を結ぶ線分の最近傍集合とする。
なお、図８（ａ）は楕円体領域１００１ａ内で局所経路が求まる場合、
図８（ｂ）は楕円体領域１００１ｂ内で局所経路が求まらない場合を示している。
図８において、サブゴールＳｇ１から同Ｓｇ２への局所微小間隔経路を探索するとき、図８（ａ）の場合は、修正Ａ* アルゴリズムなどを適用することによって、最短経路１００５が求まる。
図８（ｂ）の場合は、楕円体領域１００１ｂ内では経路が存在しないので、楕円体領域１００１ｂとＣ空間障害物１００４に囲まれる領域１００６内全ての格子点を展開した後、経路探索に失敗したと判断される。
又、楕円体領域以外の閉領域を用いて探索範囲を制限してもよい。探索範囲の設定の仕方は任意で限定されない。
STEP2 の処理において、Ｓ→Ｇに至る全ての隣接サブゴール間局所経路探索に成功した場合（TRUE）は、その経路をストア（Ｓ３０８）して、本発明の経路計画方法は経路探索を終了する。
【００１２】
次に、改めてバックトラッキング（再探索）により経路の見落としを無くし、経路探索の精度を高める本発明の独特の処理について説明する。
STEP2 の処理で、隣接サブゴール間局所経路探索に途中で失敗した場合（FALSE)は、STEP1 のサブゴール系列探索に戻る（Ｓ３０４）。例えば、図２（ｃ）のサブゴール系列Ｓ→Ｓｇ１→Ｓｇ２→Ｓｇ３→Ｓｇ４→Ｇ内の局所経路探索において、図８（ｂ）のようにＳｇ１とＳｇ２間の局所経路探索に失敗した場合、STEP1 に戻り図２（ｄ）のようにＳｇ１からＳｇ２へのアークを経由しない新たなサブゴール系列Ｓ→Ｓｇ１’→Ｓｇ２→Ｓｇ３→Ｓｇ４→Ｇを再探索する。
そのために、図３、図７のフローチャートに示したようにSTEP1 、STEP2 共にサブゴール有向グラフ（図５）のデータを参照しながら探索を行い、探索の結果に応じてそのデータを変更する。
又、図７に示すSTEP2 が、隣接サブゴールＳｇ１、Ｓｇ２間局所経路探索Ｓ９０３に成功した場合は、求められた経路は図５に示したサブゴール有向グラフ SgGraphの、７０３の、SGgraph.Cell（Sg1 ）.AdjSGList.Cell （Arc(Sg2-Sg1)）.PathToAdjSG、に格納される。
逆に、Ｓｇ１→Ｓｇ２間局所経路探索Ｓ９０３に失敗した場合は、SgGraph.Cell（Sg1 ）.AdjSGList.Cell （Arc(Sg2-Sg1)).ExistanceOfPathToAdjSG1= FALSE、と変更される。
又、STEP1 では常に、SgGraph.Cell（Sg1).AdjSGList.Cell （Arc(Sg2-Sg1).ExistanceOfPathToAdjSG1、の状態を参照し、このフラグがFALSE であれば、Ｓｇ１からＳｇ２へは探索を進めない。これによって、効率化を妨げないように考慮している。
又、このサブゴール有向グラフには、前回までに生成したサブゴール及びその候補に関する情報が記憶されているので、そのデータを参照することにより、再度、始めから幾何モデラを用いてロボットと作業環境との干渉を検査するような無駄を省き、探索効率の向上を図っている。STEP2 のＳ９０１の選択処理でも、SgGraph が参照され、
SgGraph.Cell（Sg1).AdjSGList.Cell(Arc(Sg2-Sg1)).PathToAdjSG が空ではない（つまり、既に局所経路を求めた）隣接サブゴールＳｇ１、Ｓｇ２間はスキップされ、次の隣接サブゴールＳｇ２、Ｓｇ３が選択される。
又、本発明の経路計画方法では、図１のフローチャートのＳ３０６の処理で、図６のようなサブゴール系列リストListOfSgSeq を選択する処理に失敗（FALSE ）した場合、所定時間経過したかどうか判断される（Ｓ３０９）。時間切れであれば、探索失敗として終了する。時間切れで無ければ、離散化刻み幅ｄｉｓＰａｒａをＮ（整数）分割し（Ｓ３１０）、ｄ−Ｃ−Ｌｒｅｓの再形成を行って（Ｓ３０３）、一連の処理を継続する。
【００１３】
次に、図１のフローチャートのサブゴール系列リストListOfSgSeq から、サブゴール系列SgSeq を選択する処理（Ｓ３０６）。図７のパラメータｒＰａｒａの設定処理（Ｓ９０２）。サブゴール有向グラフ（図５）及びサブゴール系列リスト（図６）のデータとの関係について詳細に説明する。
STEP1 で求めたサブゴール系列SgSeq を、図６に示したサブゴール系列リストListOfSgSeq(８０１）の先頭に挿入する。挿入された時点では、STEP2 の実行結果に関するフラグBOOL SearchResult1、SearchResult2 、は共にTRUEとなっている。図１のＳ３０６の処理では、ListOfSgSeq の先頭要素のSearchResult1 がまずチェックされる。これがTRUEの場合は、そのサブゴール系列が選択される。つまり、STEP1 で新しいサブゴール系列を求めた場合は、必ず、それを選択する。
図７のＳ９０１の処理で、隣接サブゴールを選択する際に、隣接サブゴールへの経路の存在性を示すフラグExistanceOfPathToAdjSg1 、（図５：サブゴール有向グラフ：７０４）がチェックされ、このフラグがTRUEの場合、Ｓ９０２の処理において、パラメータのｒＰａｒａは予め設定した値ｒＰａｒａＭｉｎに設定する。
次に、Ｓ９０３のSTEP2 での経路探索処理で、局所経路探索に失敗した場合、先述したようにフラグExistanceOfPathToAdjSG1 をFALSE に書換え、更に、SearchResult1 もFALSE に書換える。
STEP1(Ｓ３０４の処理) でサブゴール系列探索に失敗した場合、図６のサブゴール系列リストListSgSeq にストアされているサブゴール系列のフラグSearchResult1(図６：８０２）は全てFALSE になっている。この場合は、Ｓ３０６にスキップしてSearchResult2 がTRUEのサブゴール系列を選択する。このサブゴール系列は過去にSTEP2 が実行されたものであり、隣接サブゴール間のいずれかにおいてＳ９０３のSTEP2 の経路探索処理に失敗している。
SearchResult1=FALSE 、SearchResult2=TRUEのサブゴール系列を選択する時は、最もゴール配置Ｇ近くまで局所経路探索（Ｓ９０３）に成功し、なおかつ、その経路長が最も短いものを選択する。このようにして選択されたサブゴール系列に対して、図７のＳ９０１の処理で隣接サブゴールを選択する際、ExistanceOfPathToAdjSG1=FALSE のものが選択される。このとき同図のＳ９０２の処理で、パラメータｒＰａｒａは予め設定した値、ｒＰａｒａＭａｘに設定され、Ｓ９０３が再実行される。
このＳ９０３の再探索に成功したら、サブゴール有向グラフの該当するExistanceOfPathToAdjSG1 をTRUEに書換える。又、Ｓ９０３の探索処理に失敗した場合は、サブゴール有向グラフが該当するExistanceOfPathToAdjSG2 をFALSE に書換え、図６のサブゴール系列リストのSearchResult2 もFALSE に書換える。
又、Ｓ３０６の処理でSearchResult1=FALSE 、SearchResult2=TRUEのサブゴール系列を選択する際、各隣接サブゴール間に対してサブゴール有向グラフの該当するExistanceOfPathToAdjSG2 をチェックし、これがFALSE のものは選択しないようにする。そしてＳ３０６は、サブゴール系列リストからサブゴールの選択に成功した場合、TRUEを返し、SearchResult2 が全てFALSE のため選択に失敗した場合はFALSE を返す。
以上、本実施の形態では、サブゴール有向グラフの隣接サブゴール間経路の存在性を示すフラグExistanceOfPathToAdjSGと、サブゴール系列リストのSTEP2 実行結果を示すフラグSearchResultをそれぞれ２つづつ使用したが、これに限定するものではなく、それらを更に多数用意することにより、ｒＰａｒａを多段階に変更してSTEP2 の隣接サブゴール間局所経路探索Ｓ９０３を実行してもよい。
【００１４】
次に、図９は図１に示す経路計画方法を計算機に実装した探索結果を示す図であり、本実施の形態による効果を実証するために、２次元Ｃ空間において経路探索を実行した例を示したものである。
この例では、ｄ−Ｃ−Ｈｒｅｓの分解能は１００（各軸０〜９９の整数値）である。又、ｄｉｓＰａｒａ＝１８．３８５としてｄ−Ｃ−Ｌｒｅｓを形成している。又、ｒＰａｒａの設定値はｒＰａｒａＭｉｎ＝１、ｒＰａｒａＭａｘ＝１．２としている。但し、これらの比は限定ではない。
図９（ａ）では、自由空間が広くなっているため、STEP1 のサブゴール系列探索に失敗することなく、ｒＰａｒａＭｉｎのまま、STEP2 が実行され１３回目のSTEP2 において、スタート配置Ｓからコール配置Ｇに至る経路を求められている様子を示している。
一方、図９（ｂ）の場合は、自由空間が局所的に狭いケースなので、途中でSTEP1 （Ｓｇ１→Ｓｇ２）に失敗し、ｒＰａｒａ＝ｒＰａｒａＭａｘに変更してSTEP2 の隣接サブゴール間局所経路探索Ｓ９０３が実行されている。Ｓｇ１からＳｇ２までの局所経路探索に成功した後は、STEP1 に失敗することもなくＳｇ２→Ｇまで探索が進行している。なお、STEP1 ではｄ−Ｃ−Ｌｒｅｓの対角線上の格子点を隣接格子点としてＡ* 探索を、STEP2 のＳ９０３では、ｄ−Ｃ−Ｈｒｅｓの対角線上の格子点を隣接格子点とはせずに、修正Ａ* 探索を適用している。
このような本発明の経路計画法によれば、局所経路探索の探索範囲を指定するパラメータｒＰａｒａを動的に変更しているため、スタート配置やゴール配置が障害物に囲まれ、非常に狭い領域を通過しないとゴール配置に到達できないような所でも、見落としがなく経路を発見することができる。
図１０は比較のための従来方式の図であるが、図はパラメータｄ−Ｃ−Ｈｒｅｓによる従来のＣ空間離散化法（修正Ａ* 探索）をそのまま適用した探索結果を示す。同図の（ａ）、（ｂ）の障害物形状は、それぞれ図９（ａ）、（ｂ）の場合と同じである。
図９と図１０を比較すると、本発明の経路計画方法は従来の方法に比べて探索領域が大幅に削減されていることが分かる。したがって、本発明の経路計画方法によれば、探索効率が向上し、探索精度が上がり、経路を高速に求めることができる。
また、ここまでは説明の都合上、２次元Ｃ空間を例にとって説明してきたが、勿論、本発明の経路計画方法は３次元以上のＣ空間に対してもそのまま適用可能である。
【００１５】
次に、本発明の第２の実施の形態について図を参照して説明する。
図１１は本発明の第２の実施の形態に係るロボットの大域動作経路計画制御装置のブロック図である。
図１１において、１３０１はロボットとその作業環境の幾何モデルを入力するロボット・環境モデル入力手段であり、１３０２はロボット・環境モデル入力手段１３０１がが出力する幾何モデルを記憶する幾何モデル記憶手段である。１３０３は幾何モデル記憶手段１３０２の情報を利用してロボットと環境との干渉を検査する干渉検査手段である。
１３０４はロボットのスタート配置とゴール配置およびその他のパラメータを入力するスタート・ゴール配置パラメータ入力手段である。１３０５はパラメータ入力手段１３０４が出力するパラメータを設定、あるいは探索の状況に応じて変更するパラメータ設定・変更手段であり、１３０６はパラメータ設定・変更手段１３０５によって設定されたパラメータを用いて、サブゴール系列探索のための離散化配置空間を設定する離散化配置空間設定手段である。１３０７は離散化配置空間設定手段１３０６によって決められた離散化配置空間において、干渉検査手段１３０３の干渉検査結果をを利用してサブゴールの系列を探索するサブゴール系列探索手段である。
１３０８はサブゴール系列探索手段１３０７によって生成されたサブゴールおよびその候補を有向グラフの形式で記憶するサブゴール有向グラフ記憶手段である。１３０９はサブゴール系列探索手段１３０７によって得られたサブゴール系列をリストの形式で記憶するサブゴール系列リスト記憶手段である。１３１０はサブゴール系列リスト記憶手段１３０９が記憶している情報から１つのサブゴール系列を選択するサブゴール系列選択手段である。１３１１はサブゴール系列選択手段１３１０によって選ばれたサブゴール系列を一時的に記憶しておくサブゴール系列一時記憶手段である。１３１２はサブゴール系列選択手段１３１０によって選ばれ、サブゴール系列一時記憶手段１３１１に記憶されているサブゴール系列から２つの隣接サブゴールを選択する隣接サブゴール選択手段である。
１３１３は隣接サブゴール選択手段１３１２から得られる２つの隣接サブゴール間の局所経路を探索する隣接サブゴール間経路探索手段であり、１３１４は隣接サブゴール間経路探索手段１３１３が経路を探索する時に探索範囲を制限する探索範囲制限手段である。１３１５はサブゴール有向グラフ記憶手段１３０８に記憶されている情報とサブゴール系列一時記憶手段１３１１の情報を受けて、スタート配置からゴール位置に至る連続した経路を記憶する経路記憶手段であり、１３１６は経路記憶手段１３１５が記憶している経路を出力する経路出力手段である。
【００１６】
つぎに以上のような構成の制御装置の動作について説明する。
制御装置全体の動作については、前実施の形態で示した図１のフローチャートの処理のSTEP1 、STEP2 による経路探索が行われるが、以下、その大略要点について簡単に説明するる。
スタート・ゴール配置パラメータ入力手段１３０４、パラメータ設定・変更手段１３０５、離散化配置空間設定手段１３０６、において、スタート配置Ｓ、ゴール配置Ｇに加え、STEP1 の場合のパラメータｄｉｓＰａｒａ、STEP2 において探索空間を制限するパラメータｒＰａｒａＭｉｎ、ｒＰａｒａＭａｘ等の設定が行われ、サブゴール系列探索手段１３０７によりSTEP1 による経路探索が実行される。
サブゴール系列探索手段１３０７により探索されたサブゴール系列および候補は、サブゴール有向グラフ記憶手段に、図５に示したようなデータ構造で記憶される。
又、探索されたサブゴール系列をリストの形式で、図６に示すようなデータ構造としてサブゴール系列リスト記憶手段１３０９に記憶される。
次に、サブゴールド系列リスト記憶手段に記憶している情報から１つのサブゴール系列を、サブゴール系列選択手段で選択してサブゴール系列一時記憶手段１３１１に記憶させ、そこから隣接サブゴール選択手段１３１２が２つの隣接サブゴールを選択する。
選択された２つの隣接サブゴール間の局所経路を、隣接サブゴール間経路探索手段１３１３がSTEP2 により探索する。この場合に探索範囲制限手段１３１４はパラメータ設定手段１３０５よりパラメータｒＰａｒａ等を参照し、図８に示すような楕円体領域を用いて探索範囲Srng等を演算して探索範囲を制限する。
隣接サブゴール間経路探索手段１３１３による探索結果は、サブゴール有向グラフ記憶手段１３０８に記憶される。又、この間の結果によっては、バックトラッキングや再探索等の繰り返し処理も行われる。
最後に、サブゴール有向グラフ記憶手段１３０８の情報と、サブゴール系列一時記憶手段１３１１の情報から、スタート配置Ｓ→ゴール配置Ｇに至る連続経路を記憶している経路記憶手段１３１５より経路出力手段１３１６へ経路情報を出力してロボットを障害物と干渉しないように動作させる。
このように、第２の実施の形態によれば、動作については説明の都合上２次元空間経路探索を例に説明したが、勿論３次元以上のＣ空間についてもそのまま適用可能であり、経路の探索効率が向上して経路の見落としが少なくなり、かつ、ロボット経路を高速で求めることが可能になる。
【００１７】
【発明の効果】
以上説明したように、本発明によれば、ロボットのスタート配置とゴール配置が与えられた時、その間を大まかに分割するSTEP1 の探索処理により離散化Ｃ空間を形成してグラフ探索手法を用いてサブゴール系列を探索し、求められたサブゴール間に、STEP2 の探索処理による、更に、微小な間隔の格子点を持つ離散化Ｃ空間を形成してグラフ探索手法を用いて局所経路を求めるという２段階の経路探索を実行するので、探索空間が大幅に抑制されて探索効率が向上し、ロボットの制御に関するリソース消費を抑えて全体として性能を向上させるという効果がある。
更に、経路探索処理についても、STEP1 による探索結果をサブゴール系列リストに格納し、それを用いてSTEP2 の局所経路探索を実行し結果をサブゴール有向グラフに反映させて、無駄を省いた局所経路探索に必要最小限な情報をストアすることでデータ処理の効率化を図り、STEP2 の探索が失敗した場合にも、バックトラッキング（再探索）を繰り返し実行するので、探索精度が向上し、３次元作業空間で自由度の大きな多関節マニュピレータに対しても、高速で実用的な経路計画が実現できるという効果がある。
更に、STEP1 の探索が成功した場合は、求めたサブゴール系列をサブゴール系列リストに保存してパラメータｒＰａｒａＭｉｎを用いてSTEP2 を実行し、STEP1 の探索が失敗した場合には、探索範囲を広くするパラメータｒＰａｒａＭａｘによりSTEP2 を実行するというように、局所経路探索の探索範囲を動的に変更する探索を行っているので、自由空間が非常に狭くなっている場合でも、経路を見落とす可能性が小さくなるという効果がある。
【図面の簡単な説明】
【図１】本発明の第１の実施の形態に係るロボットの大域動作経路計画方法の処理のフローチャートである。
【図２】図１に示す離散化Ｃ空間構成法とサブゴール系列探索結果を示す図である。
【図３】図１に示すSTEP1 の処理の内部フローチャートである。
【図４】図３に示すグラフ探索に用いる連結リストのデータ構造例を示す図である。
【図５】図３に示すサブゴール有向グラフのデータ構造を示す図である。
【図６】図１に示すサブゴール系列リストのデータ構造を示す図である。
【図７】図１に示すSTEP2 の内部フローチャートである。
【図８】図７に示す経路探索範囲を制限する局所領域を示す図である。
【図９】図１に示す経路計画方法を計算機に実装した探索結果を示す図である。
【図１０】図９に示す経路探索結果の比較図である。
【図１１】本発明の第２の実施の形態に係るロボットの大域動作経路計画制御装置のブロック図である。
【図１２】従来のロボットが置かれている座標系およびロボットの配置空間を示す図である。
【図１３】従来の２次元離散化Ｃ空間と経路探索の進行図である。
【図１４】従来の多数の移動物体を伴うシステムの移動計画および制御方法のシステム構成ブロック図である。
【符号の説明】
１０１ ３自由度マニピュレータ
１０２ ３自由度移動ロボット
１０３ ２次元作業空間
１０４ ２次元作業空間の障害物
１０５、２０５、４０４、１００４ Ｃ空間の障害物
２０１ ２次元Ｃ空間
２０２ ２次元離散化Ｃ空間
２０３、４０１ スタート配置
２０４、４０２ ゴール配置
２０６ 展開されたノード
２０７ 未展開ノード
２０８ ロボットと障害物が干渉した未展開ノード
４０３ 離散化間隔
４０５ サブゴール
６０１ 連結リスト
６０２ 連結リストのセル
６０３ グラフ探索に用いる連結リスト
６０４ グラフ探索に用いる連結リストのセル
７０１ 生成されたブゴールおよび候補を格納する連結リスト
７０２ サブゴールおよびその候補を格納する連結リストのセル
７０３ 隣接サブゴールに関する情報を格納する連結リストのセル
７０４ 隣接サブゴールに関する情報を格納する連結リストのセル
８０１ サブゴール系列リストの連結リストによる実現
８０２ サブゴール系列リストのセル
８０３ サブゴール系列の連結リストによる実現
８０４ サブゴール系列のセル
１００１ 探索範囲を制限する楕円体領域
１００２ サブゴールＳｇ１
１００３ サブゴールＳｇ２
１００５ 隣接サブゴール間局所経路
１００６ 楕円体領域Ｃ空間障害物で囲まれた閉領域
１３０１ ロボット・環境モデル入力手段
１３０２ 幾何モデル記憶手段
１３０３ 干渉検査手段
１３０４ スタート・ゴール配置パラメータ入力手段
１３０５ パラメータ設定・変更手段
１３０６ 離散化配置空間設定手段
１３０７ サブゴール系列探索手段
１３０８ サブゴール有向グラフ記憶手段
１３０９ サブゴール系列リスト記憶手段
１３１０ サブゴール系列選択手段
１３１１ サブゴール系列一時記憶手段
１３１２ 隣接サブゴール選択手段
１３１３ 隣接サブゴール間経路探索手段
１３１４ 探索範囲制限手段
１３１５ 経路記憶手段
１３１６ 経路出力手段
【発明の属する技術分野】
本発明は、ロボットの作業環境にある物体の幾何学的形状と、その配置を記述した計算機上の幾何モデルを使ってロボットの動作経路を計画する方法に関するものであり、特に、ロボットのスタート配置とゴール配置が与えられたとき、モデル同士の干渉を検査する干渉検査手段を利用してロボットと作業環境内の障害物とが干渉しないようにするロボットの大域動作経路計画方法とその方法を実施する装置に関する。
【０００２】
【従来の技術】
ロボットの障害物回避に係る経路計画問題は、配置空間（Ｃ空間：Configurationspace）における、点の移動経路計画問題という形式に一般化することができる。Ｃ空間はロボットの配置（Configuration ）を一意に決定するパラメータ空間である。
図１２は従来のロボットが置かれている座標系およびロボットの配置空間（Ｃ空間）を示す図であり、この図１２を参照してＣ空間について説明する。図１２（ａ）、（ｂ）はそれぞれ２次元作業空間１０３における３自由度マニピュレータ１０１、３自由度移動ロボット１０２を示している。
３自由度マニピュレータ１０１の場合、図１２（ｃ）に示した関節空間（q1,q2,q3） はＣ空間となる。２次元作業空間内における移動ロボットの場合、作業空間座標系を基準にしたロボット座標原点の位置 (X,Y)とロボットの向きθの３自由度空間（X,Y,θ）はＣ空間となる。Ｃ空間において障害物と干渉しない経路を求めるためには、通常Ｃ空間の空間記述が必要になる。即ち、作業空間１０３における障害物１０４ａ、１０４ｂとロボット１０１の幾何学的形状およびその位置関係を用いてＣ空間における障害物１０５ａ、１０５ｂ（Ｃ空間障害物）を記述し、Ｃ空間内の自由（非干渉）空間（Ｃ自由空間）を求める必要がある。
Ｃ空間記述に基づくロボットの経路計画方法は、ロードマップ法、セル分解法、Ｃ空間離散化法、ポテンシャルフィールド法に大別される。
ロードマップ法と、セル分解法、Ｃ空間離散化法の相違点は、Ｃ空間の記述法と、その記述に基づく探索手法にある。ロードマップ法やセル分解法では、通常、Ｃ空間の陽な記述が必要になる。２次元作業空間における２自由度移動ロボットの場合は、Ｃ自由空間を陽に記述することができる。すなわち、Ｃ空間障害物の境界を陽に求めることができる。多関節マニピュレータの場合は、Ｃ障害物を解析的に求める一般的手法は存在せず、Slice Projection（Needle）法、Swept Volume法、Point Evaluation法などのＣ空間の離散化に基づく近似手法によりＣ空間記述を求めるのが一般的である。ロードマップ法やセル分解法は２次元作業空間における移動ロボットの経路計画問題に適しており、Ｃ空間離散化法は多関節マニピュレータの経路計画問題に適している。
ポテンシャルフィールド法は、ゴール配置からロボットに作用する引力、障害物からロボットに作用する斥力、ロボットを動作範囲内に留める斥力の和からなる人工ポテンシャル関数を定義し、ポテンシャル関数のＣ空間における最急勾配方向に経路を探索する方法である。多関節マニピュレータの場合は、Ｃ障害物を陽に記述するのは困難なため、リンクにいくつかの代表点を設け、各代表点から一定距離内にある障害物からそれぞれの代表点に作用する斥力を作業空間で定義する。ゴール配置以外のポテンシャル極小点を如何に避けるか、現実的に計算可能なポテンシャル関数を如何に導出するかが課題となる。
これら従来方式は、文献「Y.K.Hwang and N.Ahuja : Gross Motion Planning -A Survey, ACM Computing Surveys, Vol.24, No.3, pp.219-291, 1992」に詳しくまとめられているので、詳細は省略する。
経路計画問題は、計算の複雑性理論（Computational Complexity Thoery ）によれば、NP（Nondeterministic Polymonial ）--hardと呼ばれる解の導出が非常に困難な問題に属する。ロボットの自由度、障害物の数、障害物の面の数などが大きくなるに従って、現実的な時間内では障害物回避経路を求めることができなくなる。上の従来法を現状の100MIPS 程度の計算機を用いて数十分以内に経路が求まるという基準で評価すると、ロードマップ法やセル分解法は、２次元作業空間内における移動ロボット、Ｃ空間離散化法やポテンシャルフィールド法は、３次元作業空間内における６自由度マニピュレータという具合に適用可能範囲が限られて来る。
【０００３】
Point Evaluation法に基づくＣ空間離散化法の基本アプローチについて、図１３に示す従来の２次元離散化Ｃ空間と経路探索の進行図、を参照して説明する。図１３（ａ）の２０１は２次元Ｃ空間（q1,q2)を示している。２０３、２０４はそれぞれスタート配置Ｓ、ゴール配置Ｇを示している。２０５ａ〜ｂはＣ空間障害物である。Point Evaluation法に基づくＣ空間離散化法では、図１３（ｂ）の２０２のようにＣ空間２０１を微小間隔で離散化した格子点の空間、すなわち、離散化Ｃ空間（d-q1,d-q2)を考える。離散化Ｃ空間２０２は各格子点をノードとして、それらの隣接関係をアークとする連結度グラフと見なすことができる。
この連結度グラフ上のスタート配置Ｓから隣接する格子点を辿ってゴール配置Ｇへ至る経路（格子点列）を求めるのであるが、各格子点においてのみ、ロボットと作業環境との干渉の有無を幾何モデルと干渉チェッカを用いて検査する。
▲１▼ 予め全格子点での干渉の有無を求めておく方式と、▲２▼ 探索の過程で逐次各格子点における干渉の有無を検査する方式がある。
作業環境がダイナミックに変化する場合には、後者の方式を用いる。
【０００４】
格子点の連結度グラフ上での経路探索には、縦型、横型、山登り、最良優先、Ａ* 探索などのグラフ（状態空間）探索手法を用いる。図１３（ｃ）に離散化Ｃ空間においてグラフ探索が進行する様子を示す。
グラフ探索は、スタートノードＳの子ノード（隣接ノード）を生成することから始まる。1-1,1-2,1-3,1-4 が生成されたＳの子ノードである。子ノードが生成されると、ある基準に基づいてそのノードでの評価関数値が計算されノードに記憶される。また、各ノードには親ノードへのリンク（ポインタ）も記憶される。
あるノードの子ノードを生成することを、そのノードを展開するという。
生成されたノード1-1,1-2,1-3,1-4 の中から、評価関数値に基づいて次にどのノードを展開すべきかが選ばれる。
図１３（ｃ）の例では、ノード1-2 が選択されて展開されている。
図１３（ｄ）は図１３（ｃ）のグラフ探索進行状況をツリー構造（探索木と呼ばれる）で表現したものである。
図１３（ｃ）の探索済み領域の波面（境界）２０７ａ〜ｅおよび２０８ａ〜ｂが図１３（ｄ）の探索木の葉（直系の子孫を持たないノード）に対応している。
２０８ａ〜ｂは探索木の葉の中の障害物と干渉が生じたノードを示している。
スタート配置２０３および２０６ａ〜ｂは展開済みノード（子を持つノード）を示している。
また、探索木の枝が親ノードへのポインタに相当する。グラフ探索は、探索木の葉の中から評価関数値に基づいて次に展開する（干渉を生じない）ノードを選び、そのノードの子ノードを重複せずに生成する処理を、葉からゴール配置が選択される、あるいは選択すべき葉がなくなるまで繰り返す。
葉からゴール配置が選択された場合、ゴール配置から親ノードへのポインタを辿っていくことにより経路が得られる。
上述したように、Point Evaluation法に基づくＣ空間離散化法では格子点においてのみ干渉チェックを行うため、非干渉ノード間を結ぶアーク上での非干渉性は保証されない。したがって、離散化分解能を十分高くする必要がある。
【０００５】
【発明が解決しようとする課題】
しかしながら、上述の従来のＣ空間離散化法では、離散化分解能、ロボットの自由度、障害物の面の総数などの増加に対して、計算量、記憶領域が指数関数的に増加してしまう。そのため、６自由度以上のマニピュレータに対して、Ｃ空間離散化法の基本アプローチをそのまま適用するのは現実的でなく、探索空間を制限したり（局所探索）、探索効率を高めるための工夫が必要となる。様々なヒューリスティックス（発見的手法）を用いて探索空間削減、探索効率化をする方式が種々提案されているが、経路を高速に求られる実用的な方式が少ないという問題があった。
探索効率を高める方式としては、例えば、特開平１０−９７３１６、「多数の移動物体を伴うシステムの移動計画及び制御方法」、に開示されているロボット経路の計画法等がある。
図１４はその従来方式のブロック図であり、ロボットの配置空間を第１の大きなスケールでセルに分割したセル空間データベース（フリー《衝突無し》の情報、フル《衝突の危険あり》の情報、ミックス《それ以外》の情報を持つ。）を利用して、中間経由点を直線的に結んだセグメント列を求める線路計画モジュール９１２と、次に、線路計画モジュール９１２により得られたセグメント列の中間点リストより、微細な時間分解能で加減速を考慮した、第２の更に微小時間間隔のトラジェクトリ（軌跡）データを求め、それをロボットマニピュレータ・システム９１６の位置制御系に直接指令を与えて、実際にロボットを動作させ、同時に、人口ポテンシャル法を利用して（フィードバック制御を用い）衝突を動作段階で確実に回避できるように、先のトラジェクトリを修正するものであるが、経路探索という意味からは第１と第２のトラジェクトリ間でのバックトラッキングも、再探索も行われず、微細な第２のトラジェクトリに関しても、空間配置ではない時間間隔を微小間隔に分割して経路補完を行っているに過ぎないので、充分に経路探索が尽くされてはいない。それに、実際のロボットの動作段階で人口ポテンシャル方式による修正により、最終的な衝突回避を行っているので、形式的には２段階経路探索方式となってはいるが、経路の探索精度としては充分ではなく、存在する経路を見落としやすいという問題があった。
また、特願平１０−３５６７１９に、サブゴール系列（中間経由点）探索と隣接サブゴール間局所経路探索を繰り返す２段階経路計画方式が記載されている。この方式によれば、有向グラフを用いた２段階探索が行われるので経路計算の高速化が可能とはなるが、存在する経路を見落としやすいという問題があった。
本発明は、上記問題に鑑みてなされたものであり、３次元作業空間内における自由度の大きな多関節マニピュレータに対しても経路を高速に求めることが可能で、なおかつ、経路を見落とす可能性も小さいロボットの大域動作経路計画方法とその制御装置をを提供することを目的としている。
【０００６】
【課題を解決するための手段】
上記目的を達成するため、請求項１記載のロボットの大域動作経路計画方法は、ロボットのスタート配置とゴール配置とロボットの作業環境の幾何学情報と前記作業環境にある障害物の幾何学的形状及び配置とに基づいて前記ロボットと障害物とが干渉を起こさないように前記スタート配置から前記ゴール配置へ至る非干渉経路を計画するロボットの大域動作経路計画方法において、ステップ１として、前記ロボットの配置空間に、第１間隔で離散した複数の格子点と前記スタート配置および前記ゴール配置とで構成される第１経由点候補を形成し、前記障害物とロボットが干渉しない前記第１経由点候補の中から、前記スタート配置から前記ゴール配置へ至る前記第１経由点候補の経路を第１経由点系列として求め、ステップ２として、前記ステップ１で求められた前記第１経由点系列において、互いに隣接する一対の第１経由点について、前記一対の第１経由点を包含する探索範囲を設定し、前記探索範囲において前記第１間隔よりも小さい微小な第２間隔で離散した複数の格子点である第２経由点候補を形成し、前記障害物とロボットが干渉しない前記第２経由点候補の中から、前記一対の第１経由点の一方から他方へ至る前記第２経由点候補の経路を第２経由点系列として求め、ステップ３として、前記ステップ２において、前記探索範囲に前記第２経由点系列が存在しない場合は、前記ステップ１に戻り、前記ステップ２において前記第２経由点系列が存在しなかった前記一対の第１経由点を含まない新たな第１経由点系列を求め、再びステップ２に移り、前記ステップ１から前記ステップ３までの処理を前記第１経由点系列の全ての前記一対の第１経由点について前記第２経由点系列が求まるまで繰り返し、スタート配置からゴール配置に至る前記第２経由点列を前記非干渉経路とすることを特徴としている。
また、請求項２記載のロボットの大域動作経路計画方法は、前記ステップ２の前記探索範囲は、前記一対の第１経由点を包含する楕円体領域に含まれることを特徴としている。
また、請求項３記載のロボットの大域動作経路計画方法は、前記ステップ１で求めた前記第１経由点系列を全てリストに保存しておき、ステップ１の経由点系列探索に成功した場合は、その経由点系列に対して前記ステップ２を実行し、ステップ１の経由点系列探索に失敗した場合は、経由点系列のリストの中から経由点系列を適当に１つ選び出し、過去のステップ２において前記探索範囲に前記第２経由点系列が存在しなかった場合は、前回よりも前記探索範囲を広く設定することを特徴としている。
また、請求項４記載のロボットの大域動作経路計画方法は、前記第１経由点系列の前記リストから前記第１経由点系列を１つ選択する際に、最も前記ゴール配置近くまで前記ステップ２の前記第２経由点系列が求められ、かつ最もその経路長が短いものを優先的に選択することを特徴としている。
また、請求項５記載のロボットの大域動作経路計画方法は、前記ステップ２において、前記探索範囲を予め設定した最大範囲にした状態で前記探索範囲に前記第２経由点系列が存在しなかった場合、当該経由点系列を前記第１経由点系列の前記リストから除外することを特徴としている。
また、請求項６記載のロボットの大域動作経路計画方法は、前記第１経由点系列の前記リストに記憶された前記第１経由点系列の全てにおいて前記非干渉経路を求められない場合、前記第１間隔を整数個に分割してより小さい間隔をあらたな第１間隔として第１経由点候補を形成して前記ステップ１を実行することを特徴としている。
また、請求項７記載のロボットの大域動作経路計画装置は、請求項１記載のロボットの大域動作経路計画方法を実施するためのロボットの大域動作経路計画装置であって、前記ロボットの前記スタート配置と、前記ゴール配置と、前記ロボットの作業環境の前記幾何学情報と、前記作業環境にある前記障害物の幾何学的形状及び配置と、を入力するための入力手段と、前記入力手段により入力された情報に基づいて前記ステップ１から前記ステップ３までの処理を実行する計算機とを備えていることを特徴としている。
【０００７】
このロボットの大域動作経路計画方法とその制御装置では、基本的に次のようなステップ１とステップ２による２段階経路探索が実行される。
（１）、先ず、ステップ１の処理として、配置空間（Ｃ空間）または作業空間を大まかに離散化した格子点（サブゴールド候補、例えば、パラメータｄｉｓＰａｒａによる）を持つ空間（離散化Ｃ空間）を形成し、グラフ（状態空間）探索手法を用いて、スタート配置あるいはゴール配置から格子点を経由してゴール配置あるいはスタート配置へ至るサブゴール系列を求める。
（２）、次に、ステップ２の処理として、ステップ１の処理で求めたサブゴール系列から隣接するサブゴールを２つづつ取出し、ステップ１のパラメータｄｉｓＰａｒａよりも微小間隔で離散化した格子点を持つ空間（離散化Ｃ空間）を形成し、グラフ（状態空間）探索手法を用いて隣接サブゴール間の微小間隔格子点列の局所微小間隔経路を求める処理を繰り返し行う。
（３）、ステップ２の探索が失敗した場合の処理は、ステップ２でサブゴール系列に含まれる隣接サブゴールをＳｇｉ、Ｓｇｉ＋１とし、Ｓｇｉ→Ｓｇｉ＋１間の局所経路探索に失敗したとすると、バックトラッキング及び再探索を次のように実施する、ステップ１に戻り、サブゴールＳｇｉからＳｇｉ＋１へのアークを経由しない別のサブゴール系列を求め直し、再び、ステップ２に移る処理を繰り返し、スタート配置からゴール配置までの連続した微小間隔経路が求められた場合に、繰り返し処理を終了する。
（４）、ステップ１の探索が成功の場合は、求めたサブゴール系列を全てリストに保存し、そのサブゴール系列全てについてステップ２を実行する。その場合パラメータはｒＰａｒａＭｉｎ（最低微小間隔）を用いる。
また、ステップ１の探索が失敗した場合は、サブゴール系列リストから１つ選び出し、過去ステップ２で局所経路探索に失敗した隣接サブゴール間に対して前回よりも探索範囲を広く（例えば、パラメータをｒＰａｒａＭａｘとして）してステップ２を実行し、局所経路探索に成功したらパラメータを元に戻して続行する。
（５）、先のサブゴール系列リストからのサブゴール系列の選択ができなくなったら、大まかなステップ１の間隔（パラメータｄｉｓＰａｒａ）を更に整数個に分割して離散化Ｃ空間を形成し直してステップ１から実行し直すようにする。
以上のように構成することによって、経路を探索する際の探索空間が大幅に抑制され、探索効率が向上して、３次元空間内における自由度の大きい多関節マニピュレータに対しても、経路を高速で求めることが可能になる。
また、ステップ１とステップ２のバックトラッキング、再探索等が繰返し実行され、かつ、局所経路探索の探索範囲をｒＰａｒａの切換え等により動的に変更することによって、従来方式に比較すると経路探索が充分に尽くされ、探索精度が高くなるので、存在する経路を見落とす可能性も小さくなる。
【０００８】
【発明の実施の形態】
以下、本発明の実施の形態について図を参照して説明する。
図１は本発明の第１の実施の形態に係るロボットの大域動作経路計画方法の処理のフローチャートである。
図２は図１に示す離散化Ｃ空間構成法とサブゴール系列探索結果を示す図である。
本発明のロボットの大域動作経路計画方法は、図１２（ａ）、（ｂ）のようなロボットと作業環境の幾何学情報、スタート配置Ｓ、ゴール配置Ｇが与えられたとき、図１２（ｃ）のようなＣ空間においてスタート配置Ｓからゴール配置Ｇへ至る非干渉経路を計画する方法である。
以降、簡単のため２次元Ｃ空間を例にして説明を進めるが、本発明はもちろんこれに限定されるものではなく、３次元以上のＣ空間に対してもそのまま適用できることはいうまでもない。
スタート配置Ｓ、ゴール配置Ｇは、図１３（ｂ）に示した微小間隔で離散化されたＣ空間（d-q1,d-q2 ）上の座標値で与えられるものとする。又、図１３（ｂ）の離散化Ｃ空間を、以降、ｄ−Ｃ−Ｈｒｅｓ（discrete C-space with High resolution の意味）と表す。
以下本発明の方法の手順を図１に示したフローチャートを用いて説明する。
先ず、ｄ−Ｃ−Ｈｒｅｓの離散化Ｃ空間の設定と（Ｓ３００）、スタート配置Ｓ、ゴール配置Ｇを入力した後（Ｓ３０１）。
大まかな離散化刻み幅、ｄｉｓＰａｒａ（図２の４０３）、を設定し（Ｓ３０２）、図２（ａ）、（ｂ）のように離散化Ｃ空間（d-q1',d-q2')を形成する（Ｓ３０３）。 図２（ａ）、（ｂ）の４０３ａ、４０３ｂに示したように、ｄｉｓＰａｒａはｄ−Ｃ−Ｈｒｅｓの刻み幅に比べて十分大きな値とする。離散化Ｃ空間（d-q1',d-q2')は大まかな間隔で離散化されたＣ空間であり、ｄ−Ｃ−Ｌｒｅｓと表す（discrete C-space with Low resolutionの意味）。図２（ａ）ではｄ−Ｃ−Ｌｒｅｓのスタート配置Ｓ４０１、ゴール配置Ｇ４０２の座標値はそれぞれ（0,0), （5,0)となる。同様に図２（ｂ）ではそれぞれ（0,0),(4,3)となる。本発明の方法によれば、この例のようにスタート配置を離散化Ｃ空間ｄ−Ｃ−Ｌｒｅｓの原点とする必要はなく、また、座標軸が直交している必要もない。さらには、（d-q1,d-q2)から（d-q1',d-q2')に非線形変換しても良く、ｄ−Ｃ−Ｌｒｅｓの定義の仕方は任意である。
【０００９】
つぎに、STEP1 のサブゴール系列探索が行われる（Ｓ３０４）。
図３は図１に示すSTEP1 の処理の内部フローチャートである。
この図３は、ｄ−Ｃ−Ｌｒｅｓでのスタート配置Ｓからゴール配置Ｇまでの経路探索に等しい。前述した最良優先、Ａ* 探索などのグラフ探索手法を適用してサブゴール系列を求める。グラフ探索を行うためには、図１３（ｄ）に示した探索木のデータ構造を計算機内部に持つ必要がある。探索木の葉とそれ以外のノードを別々の連結リストに格納する。
図４は図３に示すグラフ探索に用いる連結リストのデータ構造の例を示す図である。
図４（ａ）は連結リスト６０１とそのセル６０２の基本クラスであり、図４（ｂ）のグラフ探索に用いる連結リスト６０３とそのセル６０４のクラスは、図４（ａ）の派生クラスとして定義している。探索木の葉を格納する連結リストをOPENLIST、それ以外のノードを格納するリストをCLOSEDLISTと呼ぶ。OPENLIST内の干渉が生じるノードを別のリスト（COLLISIONLIST)に格納しても良い。又、グラフ探索手法は公知の技術であるため、そのアルゴリズムの詳細については説明を省略する。
スタート配置Ｓ、ゴール配置Ｇを含め、STEP1 で生成された全てのノード（サブゴールおよびその候補）は重複を省いて、有向グラフの形式で計算機上に記憶される。
【００１０】
図５は図３に示すサブゴール有向グラフのデータ構造を示す図である。
図中、７０１は全ての生成済みサブゴール候補を格納する連結リストであり、データ型（クラス）とオブジェクト（変数）名を CLListForSgGraph SgGraph;と定義する。連結リスト７０１の各セル７０２には、サブゴール候補の座標値や干渉の有無のデータに加え、隣接するサブゴール候補に関するデータを格納するリスト７０３が格納される。そのデータ型（クラス）とオブジェクト（変数）名を CLListForAdjSGs AdjSGList;と定義する。７０４はAdjSGList の各セルである。
図２（ｃ）にSTEP１により探索されたサブゴール系列の例を示す。同図においてS-Sg1-Sg2-Sg3-Sg4-G が求まったサブゴール系列である。
図１のフローチャートに戻って、STEP1 のサブゴール系列探索に成功した場合（TRUE）は、そのサブゴール系列SgSeq がサブゴール系列リストListOfSgSeq に記憶される（Ｓ３０５）。
【００１１】
図６は図１に示すサブゴール系列リストのデータ構造を示す図である。
図中、８０１がサブゴール系列を格納する連結リストであり、データ型（クラス）とオブジェクト（変数）名を CLListForSGSeq ListOfSgSeq;と定義する。連結リスト８０１の各セル８０２には、サブゴール系列８０３（サブゴールの連結リスト）とSTEP2 の実行結果に関するBOOL型のフラグが複数格納されている。サブゴール系列８０３の各セル８０４には、各サブゴールのｄ−Ｃ−Ｌｒｅｓにおける座標値などのデータが格納されている。
つぎに、サブゴール系列リスト８０１から何らかの基準に従って１つのサブゴール系列が選択される（Ｓ３０６）。STEP1 に失敗してサブゴール系列が求まらなかった場合（FALSE)は、Ｓ３０５をスキップしてＳ３０６が実行される。
Ｓ３０６で選択されたサブゴール系列に対して、後述するパラメータｒＰａｒａが設定されて、STEP2 の隣接サブゴール間局所経路探索が行われる（Ｓ３０７）。
図７は図１に示すSTEP2 の内部フローチャートである。
図７に移って、STEP2 の処理については、図１のＳ３０６で選択されたサブゴール系列から、隣接する２つのサブゴールを選択して（Ｓ９０１）、パラメータのｒＰａｒａを設定し（Ｓ９０２）、ｄ−Ｃ−Ｈｒｅｓ上で局所経路探索が行われる（Ｓ９０３）。ここでも、STEP1 と同様に最良優先、Ａ* 探索などのグラフ探索手法が適用される。Ｓ９０３の局所経路探索に成功した場合、ゴール配置Ｇまで局所経路が求まったかを判断する（Ｓ９０４）。ゴール配置Ｇまで到達していない場合、Ｓ９０１に戻ってつぎの隣接する２つのサブゴールが選択される。ゴール配置Ｇに到達した場合、TRUEを返してSTEP2 を終了する。途中でＳ９０３に失敗した場合、FALSE を返してSTEP2 を終了する。
Ｓ９０２で設定されるパラメータｒＰａｒａは、隣接サブゴール間局所経路探索の探索範囲を制限するパラメータである。
図８は図７に示す経路探索範囲を制限する局所領域を示す図である。
図８において、１００１ａ、１００１ｂは共に隣接する２つのサブゴールを包含する楕円体領域である。１００２、１００３はそれぞれサブゴールＳｇ１、Ｓｇ２、１００４はＣ空間障害物である。
この楕円体領域を用いて探索範囲を制限する場合、以下の式によって探索範囲Ｓrng を定義する。但し、ｄ（ｎ、ｍ）は離散化Ｃ空間ｄ−Ｃ−Ｈｒｅｓにおける２点ｎ，ｍ間の距離を表す。
Ｓrng=｛ n｜d(Sg1,n)＋d(n,Sg2 ） <=d(Sg-i,Sg-i+1）×rPara,rPara>=1｝
上式より、パラメータｒＰａｒａを小さくするほど探索範囲Ｓrng が制限されることがわかる。ｒＰａｒａ＝１のときは、隣接サブゴールＳｇ１、Ｓｇ２を結ぶ線分の最近傍集合とする。
なお、図８（ａ）は楕円体領域１００１ａ内で局所経路が求まる場合、
図８（ｂ）は楕円体領域１００１ｂ内で局所経路が求まらない場合を示している。
図８において、サブゴールＳｇ１から同Ｓｇ２への局所微小間隔経路を探索するとき、図８（ａ）の場合は、修正Ａ* アルゴリズムなどを適用することによって、最短経路１００５が求まる。
図８（ｂ）の場合は、楕円体領域１００１ｂ内では経路が存在しないので、楕円体領域１００１ｂとＣ空間障害物１００４に囲まれる領域１００６内全ての格子点を展開した後、経路探索に失敗したと判断される。
又、楕円体領域以外の閉領域を用いて探索範囲を制限してもよい。探索範囲の設定の仕方は任意で限定されない。
STEP2 の処理において、Ｓ→Ｇに至る全ての隣接サブゴール間局所経路探索に成功した場合（TRUE）は、その経路をストア（Ｓ３０８）して、本発明の経路計画方法は経路探索を終了する。
【００１２】
次に、改めてバックトラッキング（再探索）により経路の見落としを無くし、経路探索の精度を高める本発明の独特の処理について説明する。
STEP2 の処理で、隣接サブゴール間局所経路探索に途中で失敗した場合（FALSE)は、STEP1 のサブゴール系列探索に戻る（Ｓ３０４）。例えば、図２（ｃ）のサブゴール系列Ｓ→Ｓｇ１→Ｓｇ２→Ｓｇ３→Ｓｇ４→Ｇ内の局所経路探索において、図８（ｂ）のようにＳｇ１とＳｇ２間の局所経路探索に失敗した場合、STEP1 に戻り図２（ｄ）のようにＳｇ１からＳｇ２へのアークを経由しない新たなサブゴール系列Ｓ→Ｓｇ１’→Ｓｇ２→Ｓｇ３→Ｓｇ４→Ｇを再探索する。
そのために、図３、図７のフローチャートに示したようにSTEP1 、STEP2 共にサブゴール有向グラフ（図５）のデータを参照しながら探索を行い、探索の結果に応じてそのデータを変更する。
又、図７に示すSTEP2 が、隣接サブゴールＳｇ１、Ｓｇ２間局所経路探索Ｓ９０３に成功した場合は、求められた経路は図５に示したサブゴール有向グラフ SgGraphの、７０３の、SGgraph.Cell（Sg1 ）.AdjSGList.Cell （Arc(Sg2-Sg1)）.PathToAdjSG、に格納される。
逆に、Ｓｇ１→Ｓｇ２間局所経路探索Ｓ９０３に失敗した場合は、SgGraph.Cell（Sg1 ）.AdjSGList.Cell （Arc(Sg2-Sg1)).ExistanceOfPathToAdjSG1= FALSE、と変更される。
又、STEP1 では常に、SgGraph.Cell（Sg1).AdjSGList.Cell （Arc(Sg2-Sg1).ExistanceOfPathToAdjSG1、の状態を参照し、このフラグがFALSE であれば、Ｓｇ１からＳｇ２へは探索を進めない。これによって、効率化を妨げないように考慮している。
又、このサブゴール有向グラフには、前回までに生成したサブゴール及びその候補に関する情報が記憶されているので、そのデータを参照することにより、再度、始めから幾何モデラを用いてロボットと作業環境との干渉を検査するような無駄を省き、探索効率の向上を図っている。STEP2 のＳ９０１の選択処理でも、SgGraph が参照され、
SgGraph.Cell（Sg1).AdjSGList.Cell(Arc(Sg2-Sg1)).PathToAdjSG が空ではない（つまり、既に局所経路を求めた）隣接サブゴールＳｇ１、Ｓｇ２間はスキップされ、次の隣接サブゴールＳｇ２、Ｓｇ３が選択される。
又、本発明の経路計画方法では、図１のフローチャートのＳ３０６の処理で、図６のようなサブゴール系列リストListOfSgSeq を選択する処理に失敗（FALSE ）した場合、所定時間経過したかどうか判断される（Ｓ３０９）。時間切れであれば、探索失敗として終了する。時間切れで無ければ、離散化刻み幅ｄｉｓＰａｒａをＮ（整数）分割し（Ｓ３１０）、ｄ−Ｃ−Ｌｒｅｓの再形成を行って（Ｓ３０３）、一連の処理を継続する。
【００１３】
次に、図１のフローチャートのサブゴール系列リストListOfSgSeq から、サブゴール系列SgSeq を選択する処理（Ｓ３０６）。図７のパラメータｒＰａｒａの設定処理（Ｓ９０２）。サブゴール有向グラフ（図５）及びサブゴール系列リスト（図６）のデータとの関係について詳細に説明する。
STEP1 で求めたサブゴール系列SgSeq を、図６に示したサブゴール系列リストListOfSgSeq(８０１）の先頭に挿入する。挿入された時点では、STEP2 の実行結果に関するフラグBOOL SearchResult1、SearchResult2 、は共にTRUEとなっている。図１のＳ３０６の処理では、ListOfSgSeq の先頭要素のSearchResult1 がまずチェックされる。これがTRUEの場合は、そのサブゴール系列が選択される。つまり、STEP1 で新しいサブゴール系列を求めた場合は、必ず、それを選択する。
図７のＳ９０１の処理で、隣接サブゴールを選択する際に、隣接サブゴールへの経路の存在性を示すフラグExistanceOfPathToAdjSg1 、（図５：サブゴール有向グラフ：７０４）がチェックされ、このフラグがTRUEの場合、Ｓ９０２の処理において、パラメータのｒＰａｒａは予め設定した値ｒＰａｒａＭｉｎに設定する。
次に、Ｓ９０３のSTEP2 での経路探索処理で、局所経路探索に失敗した場合、先述したようにフラグExistanceOfPathToAdjSG1 をFALSE に書換え、更に、SearchResult1 もFALSE に書換える。
STEP1(Ｓ３０４の処理) でサブゴール系列探索に失敗した場合、図６のサブゴール系列リストListSgSeq にストアされているサブゴール系列のフラグSearchResult1(図６：８０２）は全てFALSE になっている。この場合は、Ｓ３０６にスキップしてSearchResult2 がTRUEのサブゴール系列を選択する。このサブゴール系列は過去にSTEP2 が実行されたものであり、隣接サブゴール間のいずれかにおいてＳ９０３のSTEP2 の経路探索処理に失敗している。
SearchResult1=FALSE 、SearchResult2=TRUEのサブゴール系列を選択する時は、最もゴール配置Ｇ近くまで局所経路探索（Ｓ９０３）に成功し、なおかつ、その経路長が最も短いものを選択する。このようにして選択されたサブゴール系列に対して、図７のＳ９０１の処理で隣接サブゴールを選択する際、ExistanceOfPathToAdjSG1=FALSE のものが選択される。このとき同図のＳ９０２の処理で、パラメータｒＰａｒａは予め設定した値、ｒＰａｒａＭａｘに設定され、Ｓ９０３が再実行される。
このＳ９０３の再探索に成功したら、サブゴール有向グラフの該当するExistanceOfPathToAdjSG1 をTRUEに書換える。又、Ｓ９０３の探索処理に失敗した場合は、サブゴール有向グラフが該当するExistanceOfPathToAdjSG2 をFALSE に書換え、図６のサブゴール系列リストのSearchResult2 もFALSE に書換える。
又、Ｓ３０６の処理でSearchResult1=FALSE 、SearchResult2=TRUEのサブゴール系列を選択する際、各隣接サブゴール間に対してサブゴール有向グラフの該当するExistanceOfPathToAdjSG2 をチェックし、これがFALSE のものは選択しないようにする。そしてＳ３０６は、サブゴール系列リストからサブゴールの選択に成功した場合、TRUEを返し、SearchResult2 が全てFALSE のため選択に失敗した場合はFALSE を返す。
以上、本実施の形態では、サブゴール有向グラフの隣接サブゴール間経路の存在性を示すフラグExistanceOfPathToAdjSGと、サブゴール系列リストのSTEP2 実行結果を示すフラグSearchResultをそれぞれ２つづつ使用したが、これに限定するものではなく、それらを更に多数用意することにより、ｒＰａｒａを多段階に変更してSTEP2 の隣接サブゴール間局所経路探索Ｓ９０３を実行してもよい。
【００１４】
次に、図９は図１に示す経路計画方法を計算機に実装した探索結果を示す図であり、本実施の形態による効果を実証するために、２次元Ｃ空間において経路探索を実行した例を示したものである。
この例では、ｄ−Ｃ−Ｈｒｅｓの分解能は１００（各軸０〜９９の整数値）である。又、ｄｉｓＰａｒａ＝１８．３８５としてｄ−Ｃ−Ｌｒｅｓを形成している。又、ｒＰａｒａの設定値はｒＰａｒａＭｉｎ＝１、ｒＰａｒａＭａｘ＝１．２としている。但し、これらの比は限定ではない。
図９（ａ）では、自由空間が広くなっているため、STEP1 のサブゴール系列探索に失敗することなく、ｒＰａｒａＭｉｎのまま、STEP2 が実行され１３回目のSTEP2 において、スタート配置Ｓからコール配置Ｇに至る経路を求められている様子を示している。
一方、図９（ｂ）の場合は、自由空間が局所的に狭いケースなので、途中でSTEP1 （Ｓｇ１→Ｓｇ２）に失敗し、ｒＰａｒａ＝ｒＰａｒａＭａｘに変更してSTEP2 の隣接サブゴール間局所経路探索Ｓ９０３が実行されている。Ｓｇ１からＳｇ２までの局所経路探索に成功した後は、STEP1 に失敗することもなくＳｇ２→Ｇまで探索が進行している。なお、STEP1 ではｄ−Ｃ−Ｌｒｅｓの対角線上の格子点を隣接格子点としてＡ* 探索を、STEP2 のＳ９０３では、ｄ−Ｃ−Ｈｒｅｓの対角線上の格子点を隣接格子点とはせずに、修正Ａ* 探索を適用している。
このような本発明の経路計画法によれば、局所経路探索の探索範囲を指定するパラメータｒＰａｒａを動的に変更しているため、スタート配置やゴール配置が障害物に囲まれ、非常に狭い領域を通過しないとゴール配置に到達できないような所でも、見落としがなく経路を発見することができる。
図１０は比較のための従来方式の図であるが、図はパラメータｄ−Ｃ−Ｈｒｅｓによる従来のＣ空間離散化法（修正Ａ* 探索）をそのまま適用した探索結果を示す。同図の（ａ）、（ｂ）の障害物形状は、それぞれ図９（ａ）、（ｂ）の場合と同じである。
図９と図１０を比較すると、本発明の経路計画方法は従来の方法に比べて探索領域が大幅に削減されていることが分かる。したがって、本発明の経路計画方法によれば、探索効率が向上し、探索精度が上がり、経路を高速に求めることができる。
また、ここまでは説明の都合上、２次元Ｃ空間を例にとって説明してきたが、勿論、本発明の経路計画方法は３次元以上のＣ空間に対してもそのまま適用可能である。
【００１５】
次に、本発明の第２の実施の形態について図を参照して説明する。
図１１は本発明の第２の実施の形態に係るロボットの大域動作経路計画制御装置のブロック図である。
図１１において、１３０１はロボットとその作業環境の幾何モデルを入力するロボット・環境モデル入力手段であり、１３０２はロボット・環境モデル入力手段１３０１がが出力する幾何モデルを記憶する幾何モデル記憶手段である。１３０３は幾何モデル記憶手段１３０２の情報を利用してロボットと環境との干渉を検査する干渉検査手段である。
１３０４はロボットのスタート配置とゴール配置およびその他のパラメータを入力するスタート・ゴール配置パラメータ入力手段である。１３０５はパラメータ入力手段１３０４が出力するパラメータを設定、あるいは探索の状況に応じて変更するパラメータ設定・変更手段であり、１３０６はパラメータ設定・変更手段１３０５によって設定されたパラメータを用いて、サブゴール系列探索のための離散化配置空間を設定する離散化配置空間設定手段である。１３０７は離散化配置空間設定手段１３０６によって決められた離散化配置空間において、干渉検査手段１３０３の干渉検査結果をを利用してサブゴールの系列を探索するサブゴール系列探索手段である。
１３０８はサブゴール系列探索手段１３０７によって生成されたサブゴールおよびその候補を有向グラフの形式で記憶するサブゴール有向グラフ記憶手段である。１３０９はサブゴール系列探索手段１３０７によって得られたサブゴール系列をリストの形式で記憶するサブゴール系列リスト記憶手段である。１３１０はサブゴール系列リスト記憶手段１３０９が記憶している情報から１つのサブゴール系列を選択するサブゴール系列選択手段である。１３１１はサブゴール系列選択手段１３１０によって選ばれたサブゴール系列を一時的に記憶しておくサブゴール系列一時記憶手段である。１３１２はサブゴール系列選択手段１３１０によって選ばれ、サブゴール系列一時記憶手段１３１１に記憶されているサブゴール系列から２つの隣接サブゴールを選択する隣接サブゴール選択手段である。
１３１３は隣接サブゴール選択手段１３１２から得られる２つの隣接サブゴール間の局所経路を探索する隣接サブゴール間経路探索手段であり、１３１４は隣接サブゴール間経路探索手段１３１３が経路を探索する時に探索範囲を制限する探索範囲制限手段である。１３１５はサブゴール有向グラフ記憶手段１３０８に記憶されている情報とサブゴール系列一時記憶手段１３１１の情報を受けて、スタート配置からゴール位置に至る連続した経路を記憶する経路記憶手段であり、１３１６は経路記憶手段１３１５が記憶している経路を出力する経路出力手段である。
【００１６】
つぎに以上のような構成の制御装置の動作について説明する。
制御装置全体の動作については、前実施の形態で示した図１のフローチャートの処理のSTEP1 、STEP2 による経路探索が行われるが、以下、その大略要点について簡単に説明するる。
スタート・ゴール配置パラメータ入力手段１３０４、パラメータ設定・変更手段１３０５、離散化配置空間設定手段１３０６、において、スタート配置Ｓ、ゴール配置Ｇに加え、STEP1 の場合のパラメータｄｉｓＰａｒａ、STEP2 において探索空間を制限するパラメータｒＰａｒａＭｉｎ、ｒＰａｒａＭａｘ等の設定が行われ、サブゴール系列探索手段１３０７によりSTEP1 による経路探索が実行される。
サブゴール系列探索手段１３０７により探索されたサブゴール系列および候補は、サブゴール有向グラフ記憶手段に、図５に示したようなデータ構造で記憶される。
又、探索されたサブゴール系列をリストの形式で、図６に示すようなデータ構造としてサブゴール系列リスト記憶手段１３０９に記憶される。
次に、サブゴールド系列リスト記憶手段に記憶している情報から１つのサブゴール系列を、サブゴール系列選択手段で選択してサブゴール系列一時記憶手段１３１１に記憶させ、そこから隣接サブゴール選択手段１３１２が２つの隣接サブゴールを選択する。
選択された２つの隣接サブゴール間の局所経路を、隣接サブゴール間経路探索手段１３１３がSTEP2 により探索する。この場合に探索範囲制限手段１３１４はパラメータ設定手段１３０５よりパラメータｒＰａｒａ等を参照し、図８に示すような楕円体領域を用いて探索範囲Srng等を演算して探索範囲を制限する。
隣接サブゴール間経路探索手段１３１３による探索結果は、サブゴール有向グラフ記憶手段１３０８に記憶される。又、この間の結果によっては、バックトラッキングや再探索等の繰り返し処理も行われる。
最後に、サブゴール有向グラフ記憶手段１３０８の情報と、サブゴール系列一時記憶手段１３１１の情報から、スタート配置Ｓ→ゴール配置Ｇに至る連続経路を記憶している経路記憶手段１３１５より経路出力手段１３１６へ経路情報を出力してロボットを障害物と干渉しないように動作させる。
このように、第２の実施の形態によれば、動作については説明の都合上２次元空間経路探索を例に説明したが、勿論３次元以上のＣ空間についてもそのまま適用可能であり、経路の探索効率が向上して経路の見落としが少なくなり、かつ、ロボット経路を高速で求めることが可能になる。
【００１７】
【発明の効果】
以上説明したように、本発明によれば、ロボットのスタート配置とゴール配置が与えられた時、その間を大まかに分割するSTEP1 の探索処理により離散化Ｃ空間を形成してグラフ探索手法を用いてサブゴール系列を探索し、求められたサブゴール間に、STEP2 の探索処理による、更に、微小な間隔の格子点を持つ離散化Ｃ空間を形成してグラフ探索手法を用いて局所経路を求めるという２段階の経路探索を実行するので、探索空間が大幅に抑制されて探索効率が向上し、ロボットの制御に関するリソース消費を抑えて全体として性能を向上させるという効果がある。
更に、経路探索処理についても、STEP1 による探索結果をサブゴール系列リストに格納し、それを用いてSTEP2 の局所経路探索を実行し結果をサブゴール有向グラフに反映させて、無駄を省いた局所経路探索に必要最小限な情報をストアすることでデータ処理の効率化を図り、STEP2 の探索が失敗した場合にも、バックトラッキング（再探索）を繰り返し実行するので、探索精度が向上し、３次元作業空間で自由度の大きな多関節マニュピレータに対しても、高速で実用的な経路計画が実現できるという効果がある。
更に、STEP1 の探索が成功した場合は、求めたサブゴール系列をサブゴール系列リストに保存してパラメータｒＰａｒａＭｉｎを用いてSTEP2 を実行し、STEP1 の探索が失敗した場合には、探索範囲を広くするパラメータｒＰａｒａＭａｘによりSTEP2 を実行するというように、局所経路探索の探索範囲を動的に変更する探索を行っているので、自由空間が非常に狭くなっている場合でも、経路を見落とす可能性が小さくなるという効果がある。
【図面の簡単な説明】
【図１】本発明の第１の実施の形態に係るロボットの大域動作経路計画方法の処理のフローチャートである。
【図２】図１に示す離散化Ｃ空間構成法とサブゴール系列探索結果を示す図である。
【図３】図１に示すSTEP1 の処理の内部フローチャートである。
【図４】図３に示すグラフ探索に用いる連結リストのデータ構造例を示す図である。
【図５】図３に示すサブゴール有向グラフのデータ構造を示す図である。
【図６】図１に示すサブゴール系列リストのデータ構造を示す図である。
【図７】図１に示すSTEP2 の内部フローチャートである。
【図８】図７に示す経路探索範囲を制限する局所領域を示す図である。
【図９】図１に示す経路計画方法を計算機に実装した探索結果を示す図である。
【図１０】図９に示す経路探索結果の比較図である。
【図１１】本発明の第２の実施の形態に係るロボットの大域動作経路計画制御装置のブロック図である。
【図１２】従来のロボットが置かれている座標系およびロボットの配置空間を示す図である。
【図１３】従来の２次元離散化Ｃ空間と経路探索の進行図である。
【図１４】従来の多数の移動物体を伴うシステムの移動計画および制御方法のシステム構成ブロック図である。
【符号の説明】
１０１ ３自由度マニピュレータ
１０２ ３自由度移動ロボット
１０３ ２次元作業空間
１０４ ２次元作業空間の障害物
１０５、２０５、４０４、１００４ Ｃ空間の障害物
２０１ ２次元Ｃ空間
２０２ ２次元離散化Ｃ空間
２０３、４０１ スタート配置
２０４、４０２ ゴール配置
２０６ 展開されたノード
２０７ 未展開ノード
２０８ ロボットと障害物が干渉した未展開ノード
４０３ 離散化間隔
４０５ サブゴール
６０１ 連結リスト
６０２ 連結リストのセル
６０３ グラフ探索に用いる連結リスト
６０４ グラフ探索に用いる連結リストのセル
７０１ 生成されたブゴールおよび候補を格納する連結リスト
７０２ サブゴールおよびその候補を格納する連結リストのセル
７０３ 隣接サブゴールに関する情報を格納する連結リストのセル
７０４ 隣接サブゴールに関する情報を格納する連結リストのセル
８０１ サブゴール系列リストの連結リストによる実現
８０２ サブゴール系列リストのセル
８０３ サブゴール系列の連結リストによる実現
８０４ サブゴール系列のセル
１００１ 探索範囲を制限する楕円体領域
１００２ サブゴールＳｇ１
１００３ サブゴールＳｇ２
１００５ 隣接サブゴール間局所経路
１００６ 楕円体領域Ｃ空間障害物で囲まれた閉領域
１３０１ ロボット・環境モデル入力手段
１３０２ 幾何モデル記憶手段
１３０３ 干渉検査手段
１３０４ スタート・ゴール配置パラメータ入力手段
１３０５ パラメータ設定・変更手段
１３０６ 離散化配置空間設定手段
１３０７ サブゴール系列探索手段
１３０８ サブゴール有向グラフ記憶手段
１３０９ サブゴール系列リスト記憶手段
１３１０ サブゴール系列選択手段
１３１１ サブゴール系列一時記憶手段
１３１２ 隣接サブゴール選択手段
１３１３ 隣接サブゴール間経路探索手段
１３１４ 探索範囲制限手段
１３１５ 経路記憶手段
１３１６ 経路出力手段
Claims (7)
- ロボットのスタート配置とゴール配置とロボットの作業環境の幾何学情報と前記作業環境にある障害物の幾何学的形状及び配置とに基づいて前記ロボットと障害物とが干渉を起こさないように前記スタート配置から前記ゴール配置へ至る非干渉経路を計画するロボットの大域動作経路計画方法において、
ステップ１として、
前記ロボットの配置空間に、第１間隔で離散した複数の格子点と前記スタート配置および前記ゴール配置とで構成される第１経由点候補を形成し、
前記障害物とロボットが干渉しない前記第１経由点候補の中から、前記スタート配置から前記ゴール配置へ至る前記第１経由点候補の経路を第１経由点系列として求め、
ステップ２として、
前記ステップ１で求められた前記第１経由点系列において、互いに隣接する一対の第１経由点について、前記一対の第１経由点を包含する探索範囲を設定し、
前記探索範囲において前記第１間隔よりも小さい微小な第２間隔で離散した複数の格子点である第２経由点候補を形成し、
前記障害物とロボットが干渉しない前記第２経由点候補の中から、前記一対の第１経由点の一方から他方へ至る前記第２経由点候補の経路を第２経由点系列として求め、
ステップ３として、
前記ステップ２において、前記探索範囲に前記第２経由点系列が存在しない場合は、前記ステップ１に戻り、前記ステップ２において前記第２経由点系列が存在しなかった前記一対の第１経由点を含まない新たな第１経由点系列を求め、再びステップ２に移り、
前記ステップ１から前記ステップ３までの処理を前記第１経由点系列の全ての前記一対の第１経由点について前記第２経由点系列が求まるまで繰り返し、スタート配置からゴール配置に至る前記第２経由点列を前記非干渉経路とする、
ことを特徴とするロボットの大域動作経路計画方法。 - 前記ステップ２の前記探索範囲は、前記一対の第１経由点を包含する楕円体領域に含まれる
ことを特徴とする請求項１記載のロボットの大域動作経路計画方法。 - 前記ステップ１で求めた前記第１経由点系列を全てリストに保存しておき、
ステップ１の経由点系列探索に成功した場合は、その経由点系列に対して前記ステップ２を実行し、
ステップ１の経由点系列探索に失敗した場合は、経由点系列のリストの中から経由点系列を適当に１つ選び出し、
過去のステップ２において前記探索範囲に前記第２経由点系列が存在しなかった場合は、前回よりも前記探索範囲を広く設定する
ことを特徴とする請求項１又は２記載のロボットの大域動作経路計画方法。 - 前記第１経由点系列の前記リストから前記第１経由点系列を１つ選択する際に、最も前記ゴール配置近くまで前記ステップ２の前記第２経由点系列が求められ、かつ最もその経路長が短いものを優先的に選択する
ことを特徴とする請求項３記載のロボットの大域動作経路計画方法。 - 前記ステップ２において、前記探索範囲を予め設定した最大範囲にした状態で前記探索範囲に前記第２経由点系列が存在しなかった場合、当該経由点系列を前記第１経由点系列の前記リストから除外する
ことを特徴とする請求項３又は４記載のロボットの大域動作経路計画方法。 - 前記第１経由点系列の前記リストに記憶された前記第１経由点系列の全てにおいて前記非干渉経路を求められない場合、
前記第１間隔を整数個に分割してより小さい間隔をあらたな第１間隔として第１経由点候補を形成して前記ステップ１を実行する
ことを特徴とする請求項３〜５のいずれか１項記載のロボットの大域動作経路計画方法。 - 請求項１記載のロボットの大域動作経路計画方法を実施するためのロボットの大域動作経路計画装置であって、
前記ロボットの前記スタート配置と、前記ゴール配置と、前記ロボットの作業環境の前記幾何学情報と、前記作業環境にある前記障害物の幾何学的形状及び配置と、を入力するための入力手段と、
前記入力手段により入力された情報に基づいて前記ステップ１から前記ステップ３までの処理を実行する計算機とを備えている
ことを特徴とするロボットの大域動作経路計画制御装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP14666799A JP4244443B2 (ja) | 1999-05-26 | 1999-05-26 | ロボットの大域動作経路計画方法とその制御装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP14666799A JP4244443B2 (ja) | 1999-05-26 | 1999-05-26 | ロボットの大域動作経路計画方法とその制御装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2000339012A JP2000339012A (ja) | 2000-12-08 |
| JP4244443B2 true JP4244443B2 (ja) | 2009-03-25 |
Family
ID=15412905
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP14666799A Expired - Fee Related JP4244443B2 (ja) | 1999-05-26 | 1999-05-26 | ロボットの大域動作経路計画方法とその制御装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP4244443B2 (ja) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112238452A (zh) * | 2019-07-19 | 2021-01-19 | 顺丰科技有限公司 | 机械臂路径规划方法、装置、工控设备及存储介质 |
Families Citing this family (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP5361004B2 (ja) * | 2010-09-13 | 2013-12-04 | 独立行政法人国立高等専門学校機構 | 円滑経路生成装置および円滑経路生成方法 |
| KR101096592B1 (ko) | 2010-09-29 | 2011-12-20 | 국방과학연구소 | 장애물격자지도를 활용하는 무인차량의 자율주행성능 향상 장치 및 방법 |
| JP5715840B2 (ja) * | 2011-02-04 | 2015-05-13 | 本田技研工業株式会社 | 軌道計画方法、軌道制御方法、軌道計画システム及び軌道計画・制御システム |
| JP7417500B2 (ja) | 2020-09-14 | 2024-01-18 | 株式会社東芝 | 情報処理装置、情報処理方法、コンピュータプログラム及び走行管理システム |
| CN112229419B (zh) * | 2020-09-30 | 2023-02-17 | 隶元科技发展(山东)有限公司 | 一种动态路径规划导航方法及系统 |
| CN113791616A (zh) * | 2021-08-25 | 2021-12-14 | 深圳优地科技有限公司 | 路径规划方法、装置、机器人和存储介质 |
| KR102555708B1 (ko) * | 2022-04-19 | 2023-07-13 | 호서대학교 산학협력단 | 타일 격자무늬 추적 자율주행 로봇을 위한 위치 인식 및 주행 제어 방법 |
Family Cites Families (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH0719175B2 (ja) * | 1986-07-25 | 1995-03-06 | 理化学研究所 | ロボツトのア−ム軌道計画法と波面伝播装置 |
| JPS63200208A (ja) * | 1987-02-14 | 1988-08-18 | Fujitsu Ltd | 移動経路探索方法 |
| JPS63200207A (ja) * | 1987-02-14 | 1988-08-18 | Fujitsu Ltd | 移動経路探索方法 |
| JPH03116205A (ja) * | 1989-09-29 | 1991-05-17 | Toshiba Corp | 移動経路探索のための自由空間算出装置及び方法 |
| JPH04286005A (ja) * | 1991-03-15 | 1992-10-12 | Fujitsu Ltd | 移動物と障害物との干渉チェック方法 |
| JP3150018B2 (ja) * | 1993-08-27 | 2001-03-26 | 株式会社アイ・エイチ・アイ・エアロスペース | 無人移動探査機の経路決定方法 |
| JPH07281748A (ja) * | 1994-04-15 | 1995-10-27 | Nippondenso Co Ltd | 自走体の運行方法、及び自走体の運行システム |
| JPH09257503A (ja) * | 1996-03-19 | 1997-10-03 | Japan Radio Co Ltd | 経路探索方法 |
| US6004016A (en) * | 1996-08-06 | 1999-12-21 | Trw Inc. | Motion planning and control for systems with multiple mobile objects |
| JP2000181539A (ja) * | 1998-12-15 | 2000-06-30 | Yaskawa Electric Corp | ロボットの大域動作経路計画方法とその制御装置 |
-
1999
- 1999-05-26 JP JP14666799A patent/JP4244443B2/ja not_active Expired - Fee Related
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112238452A (zh) * | 2019-07-19 | 2021-01-19 | 顺丰科技有限公司 | 机械臂路径规划方法、装置、工控设备及存储介质 |
| CN112238452B (zh) * | 2019-07-19 | 2022-06-03 | 顺丰科技有限公司 | 机械臂路径规划方法、装置、工控设备及存储介质 |
Also Published As
| Publication number | Publication date |
|---|---|
| JP2000339012A (ja) | 2000-12-08 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP2002073130A (ja) | ロボットの大域動作経路計画方法とその制御装置 | |
| JP4103057B2 (ja) | ロボットの動作経路計画方法およびその装置 | |
| Chen et al. | SANDROS: a dynamic graph search algorithm for motion planning | |
| Kallmann et al. | Geometric and discrete path planning for interactive virtual worlds | |
| Kallmann et al. | Navigation meshes and real-time dynamic planning for virtual worlds | |
| Foskey et al. | A Voronoi-based hybrid motion planner | |
| Kallmann | Dynamic and robust local clearance triangulations | |
| Tsianos et al. | Sampling-based robot motion planning: Towards realistic applications | |
| Saha et al. | Planning tours of robotic arms among partitioned goals | |
| Ben-Shahar et al. | Practical pushing planning for rearrangement tasks | |
| Jacobs et al. | A scalable method for parallelizing sampling-based motion planning algorithms | |
| Gipson et al. | Resolution independent density estimation for motion planning in high-dimensional spaces | |
| JP4244443B2 (ja) | ロボットの大域動作経路計画方法とその制御装置 | |
| Starek et al. | Bidirectional fast marching trees: An optimal sampling-based algorithm for bidirectional motion planning | |
| Hwang et al. | A heuristic and complete planner for the classical mover's problem | |
| JP2000181539A (ja) | ロボットの大域動作経路計画方法とその制御装置 | |
| Kim et al. | Simple global path planning algorithm using a ray-casting and tracking method | |
| Doyle et al. | A tangent based method for robot path planning | |
| Fitch et al. | 3D rectilinear motion planning with minimum bend paths | |
| Miao | Robot path planning in dynamic environments using a simulated annealing based approach | |
| González | Fast Marching Methods in path and motion planning: improvements and high-level applications | |
| Steinbach et al. | Efficient collision and self-collision detection for humanoids based on sphere trees hierarchies | |
| Saddem et al. | Decomposing the model-checking of mobile robotics actions on a grid | |
| Lingas et al. | Optimal parallel algorithms for rectilinear link-distance problems | |
| Lee et al. | Adapting computational protein folding logic for growth-based, assembly-driven spatial truss design |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| RD04 | Notification of resignation of power of attorney |
Free format text: JAPANESE INTERMEDIATE CODE: A7424 Effective date: 20060324 |
|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20060418 |
|
| RD04 | Notification of resignation of power of attorney |
Free format text: JAPANESE INTERMEDIATE CODE: A7424 Effective date: 20071129 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20080625 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20080730 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20080926 |
|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20081216 |
|
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20081229 |
|
| R150 | Certificate of patent or registration of utility model |
Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120116 Year of fee payment: 3 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120116 Year of fee payment: 3 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20130116 Year of fee payment: 4 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20150116 Year of fee payment: 6 |
|
| LAPS | Cancellation because of no payment of annual fees |