JP2000181539A - ロボットの大域動作経路計画方法とその制御装置 - Google Patents

Abstract

(57)【要約】 【課題】 経路が高速で求まる実用的な経路計画方法
とその方法を実施する装置を提供する。 【解決手段】ロボットのスタート配置とゴール配置が与
えられたとき、計算機上の幾何学モデル手段と干渉検出
手段とを用い、次の２段階の経路探索を行う。ステップ
１（Ｓ２）：配置空間または作業空間を大まかに離散化
した格子点を持つ空間を形成し、グラフ探索手法を用い
て、スタート配置から前記格子点を経由してゴール配置
へ至るサブゴール系列を求める。ステップ２（Ｓ６）：
ステップ１で求められたサブゴール系列に含まれる隣接
するサブゴール間を包含する空間において、ステップ１
より微少間隔で離散化した格子点を持つ空間を形成し、
グラフ探索手法を用いて、前記隣接サブゴール間のロー
カルパスを求める。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、ロボットの作業環
境にある物体の幾何学的形状と、その配置を記述した計
算機上の幾何モデルを使ってロボットの動作経路を計画
する方法に関するものであり、特に、ロボットのスター
ト配置とゴール配置が与えられたとき、モデル同士の干
渉を検査する干渉検査手段を利用してロボットと作業環
境内の障害物とが干渉しないようにするロボットの大域
動作経路計画方法とその方法を実施する装置に関する。

【０００２】

【従来の技術】ロボットの障害物回避に係る経路計画問
題は、配置空間（Ｃ空間：Configuration space）にお
ける点の移動経路計画問題という形式に一般化すること
ができる。Ｃ空間は、ロボットの配置（Configuratio
n）を一意に決定するパラメータ空間であり、多関節マ
ニピュレータの場合は、関節空間に一致する。２次元作
業空間内における移動ロボットの場合は、作業空間座標
系を基準にしたロボット座標原点の位置（Ｘ、Ｙ）と、
ロボットの向きθの３自由度空間（Ｘ、Ｙ、θ）とな
る。Ｃ空間では、全てのロボットの任意の配置が点で表
される。Ｃ空間において、障害物と干渉しない経路を求
めるためには、通常、Ｃ空間の空間記述が必要になる。
すなわち、作業空間におけるロボットと障害物の幾何学
的形状およびその位置関係を用いて、Ｃ空間における障
害物（Ｃ障害物）を記述し、Ｃ空間内の自由（非干渉）
空間（Ｃ自由空間）を求める必要がある。

【０００３】Ｃ空間記述に基づくロボットの経路計画方
法は、ロードマップ法、セル分解法、Ｃ空間離散化法、
ポテンシャルフィールド法に大別される。ロードマップ
法と、セル分解法、Ｃ空間離散化法の相違点は、Ｃ空間
の記述法と、その記述に基づく探索手法にある。ロード
マップ法やセル分解法では、通常、Ｃ空間の陽な記述が
必要になる。２次元作業空間における２自由度移動ロボ
ットの場合は、Ｃ自由空間を陽に記述することができ
る。すなわち、Ｃ障害物の境界を陽に求めることができ
る。多関節マニピュレータの場合は、Ｃ障害物を解析的
に求める一般的手法は存在せず、Slice Projection（Ne
edle）法、Swept Volume法、Point Evaluation法などの
Ｃ空間の離散化に基づく近似手法によりＣ空間記述を求
めるのが一般的である。その意味から、ロードマップ法
やセル分解法は２次元作業空間における移動ロボットの
経路計画問題に適しており、Ｃ空間離散化法は多関節マ
ニピュレータの経路計画問題に適している。ポテンシャ
ルフィールド法は、ゴール配置からロボットに作用する
引力、障害物からロボットに作用する斥力、ロボットを
動作範囲内に留める斥力の和からなる人工ポテンシャル
関数を定義し、ポテンシャル関数のＣ空間における最急
勾配方向に経路を探索する方法である。多関節マニピュ
レータの場合は、Ｃ障害物を陽に記述するのは困難なた
め、リンクにいくつかの代表点を設け、各代表点から一
定距離内にある障害物からそれぞれの代表点に作用する
斥力を作業空間で定義する。ゴール配置以外のポテンシ
ャル極小点を如何に避けるか、現実的に計算可能なポテ
ンシャル関数を如何に導出するかが課題となる。

【０００４】これら従来方式は、文献「Y.K.Hwang and
N.Ahuja : Gross Motion Planning？ A Survey, ACM Co
mputing Surveys, Vol.24, No.3, pp.219-291, 1992」
に詳しくまとめられている。経路計画問題は、計算の複
雑性理論(Computational Complexity Thoery) によれ
ば、NP(Nondeterministic Polymonial)--hard と呼ばれ
る解の導出が非常に困難な問題に属する。ロボットの自
由度、障害物の数、障害物の面の数などが大きくなるに
従い、現実的な時間内では障害物回避経路を求めること
ができなくなる。上記従来法を現状の100MIPS程度の計
算機を用いて数分以内に経路が求まるという基準で評価
すると、ロードマップ法やセル分解法は２次元作業空間
内における移動ロボット、Ｃ空間離散化法（局所探索）
やポテンシャルフィールド法は３次元作業空間内におけ
る６自由度マニピュレータという具合に、適用可能範囲
が限られて来る。

【０００５】Ｃ空間の離散化記述は、微少間隔で離散化
された各Ｃ空間領域（セル)をノードとし、それらセル
の隣接関係をアークとする連結度グラフで表される。各
ノードは、自由空間か、干渉空間かという情報を持つ。
例えば、Point Evaluation法では、各離散化セルの代表
点（通常、幾何学的重心)においてロボットと作業環境
との干渉を、幾何モデラと干渉チェッカを用いて検査
し、代表点で干渉が検出されなければ、そのセルを自由
空間とし、干渉が検出されれば、そのセルを障害物空間
とする。この連結度グラフ上のスタート配置ノードから
ゴール配置ノードへ至る経路を、縦型、横型、山登り、
修正Ａ＊探索などのグラフ探索手法を用いて求めるのが
Ｃ空間離散化法の基本的なアプローチである。Ｃ空間離
散化法には、予めＣ空間全域の離散化記述を求めておく
方式と、予め離散化記述を求めず、探索の過程で逐次離
散化セルにおける干渉を検査する方式がある。作業環境
がダイナミックに変化する場合には、後者の方式を用い
る。

【０００６】

【発明が解決しようとする課題】Ｃ空間離散化法は、ロ
ボットの自由度、障害物の面の総数などに対して、計算
量、記憶領域が指数関数的に増加してしまう。そのた
め、６自由度以上のマニピュレータに対して、Ｃ空間離
散化法の基本アプローチをそのまま適用するのは現実的
でなく、探索空間を制限したり（局所探索）、探索効率
を高めるための工夫が必要となる。様々なヒューリステ
ィックスを用いて探索空間削減、探索効率化をする方式
が種々提案されているが、経路が高速で求まる実用的な
方式が少ないという問題がある。本発明は、上記技術的
問題に鑑みてなされたものであり、３次元作業空間内に
おける自由度の大きな多関節マニピュレータに対して
も、経路が高速で求まる実用的な経路計画方法とその方
法を実施する装置を提供することを目的としている。

【０００７】

【課題を解決するための手段】上記課題を解決するた
め、本発明のロボットの大域動作経路計画方法は、ロボ
ットのスタート配置とゴール配置が与えられたとき、ロ
ボットとその作業環境にある障害物の幾何学的形状とそ
れらの配置を幾何学モデルにする計算機上の幾何学モデ
ル手段と、モデル同士の干渉を検出する計算機上の干渉
検出手段とを用い、ロボットと障害物が干渉を起こさな
いように次の２段階の経路（パス）探索を行うことを特
徴としている。ステップ１：配置空間（Ｃ空間）または
作業空間を大まかに離散化した格子点（サブゴール候
補）を持つ空間（離散化Ｃ空間）を形成し、グラフ（状
態空間）探索手法を用いて、スタート配置から前記格子
点を経由してゴール配置へ至るサブゴール系列を求め
る。ステップ２：ステップ１で求められたサブゴール系
列に含まれる隣接するサブゴールを包含する空間におい
て、ステップ１より微少間隔で離散化した格子点を持つ
空間（離散化Ｃ空間）を形成し、グラフ（状態空間）探
索手法を用いて、前記隣接サブゴール間の微少間隔格子
点列すなわちローカルパス（局所微少間隔経路）を求め
る。請求項２記載のロボットの大域動作経路計画方法
は、前記ステップ２で、サブゴール系列に含まれる隣接
サブゴールsg#i, sg#i+1間のローカルパス探索に失敗し
た場合、ステップ１に戻り、サブゴールsg#iからsg#i+1
へのアークを経由しない別のサブゴール系列を求め直
し、再びステップ２に移る処理を、スタート配置からゴ
ール配置へ至る連続したローカルパスが見つかるまで、
あるいはサブゴール系列が存在しないと判明するまで、
あるいは予め定められた時間、繰り返すバックトラッキ
ング(後戻り)を行うことを特徴としている。請求項３記
載のロボットの大域動作経路計画方法は、前記ステップ
２において、隣接サブゴール間のローカルパスを求める
際、探索範囲を２つの隣接サブゴールを包含する所定の
局所空間に限定することを特徴としている。請求項４記
載のロボットの大域動作経路計画方法は、前記ステップ
２において、隣接サブゴールsg#i,sg#i+1を結ぶローカ
ルパスを求める際、離散化Ｃ空間上の２点ｎ,ｍ間の距
離をd(n,m)、aを１より大きなパラメータとしたとき、
探索空間Ｓspaceを Ｓspace＝｛n|d(sg#i,n)＋d(n,sg#i+1)＜d(sg#i,sg#i+
1)×a｝ により制限することを特徴としている。請求項５記載の
ロボットの大域動作経路計画方法は、前記ステップ１で
サブゴール系列を求める際、展開された全てのサブゴー
ル候補を重複を省いて、計算機上に有向グラフの形式で
記憶し、前記ステップ２の実行結果によってサブゴール
有向グラフのデータを更新し、前記ステップ１、２とも
に最新のサブゴール有向グラフのデータを反映して探索
を行うことを特徴としている。請求項６記載のロボット
の大域動作経路計画方法は、前記ローカルパスを求める
際の隣接する前記格子間に、さらに微少な間隔で離散化
した格子点を持つ空間（離散化Ｃ空間）を形成し、グラ
フ（状態空間）探索手法を用いて、ローカルパスに含ま
れる隣接格子点間のさらに細分化されたサブローカルパ
スを求めることを特徴としている。請求項７記載のロボ
ットの大域動作経路計画制御装置は、ロボットとその作
業環境の幾何モデルを入力するロボット、環境モデル入
力手段と、前記ロボット、環境モデル入力手段が出力す
る前記幾何モデルを記憶する幾何モデル記憶手段と、ロ
ボットのスタート配置とゴール配置やその他パラメータ
を入力するスタート、ゴール配置、パラメータ入力手段
と、前記幾何モデル記憶手段の情報を利用してロボット
と環境との干渉を検査する干渉検査手段と、前記干渉検
査手段の干渉検査結果を利用してサブゴールの系列を探
索するサブゴール系列探索手段と、前記サブゴール系列
探索手段によって展開されたサブゴールを有向グラフの
形式で記憶するサブゴール有向グラフ記憶手段と、前記
サブゴール系列探索手段によって得られたサブゴール系
列を記憶するサブゴール系列記憶手段と、前記サブゴー
ル系列記憶手段が記憶している情報から２つの隣接サブ
ゴールを選択する隣接サブゴール選択手段と、前記隣接
サブゴール選択手段から得られる２つの隣接サブゴール
間のローカルパスを探索する隣接サブゴール間ローカル
パス探索手段と、前記隣接サブゴール間ローカルパス探
索手段が前記ローカルパスを探索するときの探索範囲を
制限する探索領域制限手段と、前記隣接サブゴール間ロ
ーカルパス探索手段によって得られる隣接サブゴール間
ローカルパスを記憶する隣接サブゴール間ローカルパス
一時記憶手段と、前記サブゴール系列記憶手段に記憶さ
れている情報を受けて前記スタート配置から前記ゴール
配置に至る連続したローカルパスを出力する経路出力手
段とを備えたことを特徴としている。このようになって
いるため、経路を探索する際の探索空間が大幅に抑制さ
れ、探索効率が向上するのである。

【０００８】

【発明の実施の形態】以下、本発明の実施の形態につい
て図を参照して説明する。図１は本発明の方法で制御す
るロボットが置かれている座標系を示している。図中、
（ａ）は２次元作業空間内に置かれた２自由度マニピュ
レータの場合であり、２自由度マニピュレータとその周
辺の障害物の位置関係を表わしている。（ｂ)は２次元
作業空間内に置かれた２自由度移動ロボットの場合であ
り、２自由度移動ロボットとその周辺の障害物の位置関
係を表わしている。（ｃ）はマニピュレータまたは移動
ロボットのスタート配置とゴール配置を表している。図
において、１は２自由度マニピュレータ、２はそのエン
ドエフェクタ、３は２自由度移動ロボットを示してお
り、４Ａ、４Ｂ、４Ｃは作業空間における障害物であ
る。ここで２自由度ロボットのエンドエフェクタ２の位
置と、別の移動ロボット３の位置をｐ(x1, x2)と表し、
マニピュレータ１の第１及び第２関節の角度をそれぞれ
ｑ1,ｑ2 と表すと、２自由度マニピュレータ１のＣ空間
は(q1, q2)、２自由度移Ｖ動ロボットのＣ空間は(x1, x
2)と表される。

【０００９】本発明の方法は、（ａ）、（ｂ）のような
ロボットと作業環境の幾何学情報、スタート配置Ｓ、ゴ
ール配置Ｇが与えられたとき、（ｃ）のスタート配置Ｓ
からゴール配置Ｇへ至る非干渉経路を計画する方法であ
り、その手順を図２のフローチャートを用いて説明す
る。まず初めにロボットのスタート配置Ｓとゴール配置
Ｇなど入力パラメータが設定される（Ｓ１）。パラメー
タが設定されると、設定されたパラメータとサブゴール
有向グラフとからSTEP1のサブゴール系列探索が行われ
る（Ｓ２）。その結果、サブゴール系列が存在するかど
うかが判断され（Ｓ３）、サブゴールが有ればそのサブ
ゴールがメモリーに格納され（Ｓ４）、無ければ探索失
敗として一連の処理を終了する。サブゴールがメモリー
に格納されるとサブゴール有向グラフを参照してそのサ
ブゴールに隣接したサブゴールを選択し（Ｓ５）、STEP
2のサブゴール間ローカルパス（微少間隔経路）探索を
する（Ｓ６）。その際、結果として得られた経路をサブ
ゴール有向グラフに書込んで変更する。探索の結果、経
路があるかどうかが判断され（Ｓ７）、経路があること
がわかればゴール配置かどうかが判断される（Ｓ８）。
経路が無ければ所定時間で処理されたかどうかが判断さ
れ（Ｓ１０）、時間切れであれば探索失敗となって一連
の処理を終了する。時間切れで無ければSTEP1の探索に
戻って上記手順を繰り返す（Ｓ２）。ゴール配置であれ
ば全経路が求まり、メモリーに格納して（Ｓ９）一連の
処理を終了する。ゴール配置でなければ他の隣接サブゴ
ール選択を行い（Ｓ５）、以下同様の手順を繰り返す。

【００１０】次にSTEP1のサブゴール系列探索（Ｓ２）
について詳しく説明する。図３の（ａ）、（ｂ）は２次
元Ｃ空間(x1, x2)を表わす２つの例を示しており、図中
Ｓはスタート配置、Ｇはゴール配置を表している。それ
ぞれの図において新たな座標軸 x1', x2' を定義し、こ
の座標軸に沿って大まかな距離間隔で離散化した格子点
の空間すなわち離散化Ｃ空間(x1', x2')を定義する。こ
の離散化Ｃ空間ではスタート配置Ｓは（０、０）と表さ
れ、（ａ）の場合、ゴール配置Ｇは（６、０）、（ｂ）
の場合、ゴール配置Ｇは（４、５）と表される。本発明
の方法によれば、この例のようにスタート配置を離散化
Ｃ空間の原点とする必要はなく、また、座標軸が直交し
ている必要もない。さらには、(x1, x2)から(x1', x2')
に非線形変換しても良く、離散化Ｃ空間の定義の仕方は
任意である。

【００１１】離散化Ｃ空間(x1', x2')の各点（格子点）
をサブゴールの候補にすると、離散化Ｃ空間(x1', x2')
で、スタート配置Ｓからゴール配置Ｇへ至る経路を修正
Ａ＊、山登り法などの一般的グラフ（状態空間)探索方
式を適用して求める。図４にサブゴール系列の探索結果
例を示す。図４は２次元Ｃ空間を表わしており、Ｓ、
Ｇ、４１はそれぞれスタート配置、ゴール配置、Ｃ空間
障害物である。スタート配置Ｓ、ゴール配置Ｇを含めａ
〜ｅが求まったサブゴール系列を示している。４３はサ
ブゴール系列以外の展開されたサブゴール候補、４２は
障害物との干渉が検出されたサブゴール候補を示してい
る。修正Ａ＊、山登り法などのグラフ探索手法について
は、公知の技術につき説明を省略する。

【００１２】スタート配置Ｓ、ゴール配置Ｇを含め、ST
EP1で展開された全てのサブゴール候補は重複を省い
て、有向グラフとして計算機上に記憶される。サブゴー
ル有向グラフのデータ構造の例を図５に示す。図中、５
１は全ての展開済みサブゴール候補を格納する連結リス
トであり、データ型（クラス）とオブジェクト（変数）
名を CLListForSGs SGList; と定義する。展開された連
結リスト５１の各セル５２（データ型：CLListSGcell）
は以下のデータを有する。 ・サブゴール候補の座標値 long SGconfig[dim];（di
m：Ｃ空間の次元） ・ロボットと作業環境との干渉の有無 BOOL IsEmpty;
（Default TRUE） ・隣接サブゴール候補に関する情報を格納する連結リス
トCLListForAdjSGs AdjSGList;あるいは、そのリストの
ヘッダへのポインタ CLListAdjSGcell *ptrHead;５３
は各サブゴール候補の隣接サブゴール候補に関する情報
を格納する連結リストである。データ型（クラス）とオ
ブジェクト（変数）名を CLListForAdjSGs AdjSGList;
と定義する。この隣接サブゴール連結リスト５３の各セ
ル５４（データ型：CLListAdjSGcell）は以下のデータ
を有する。 ・隣接サブゴール候補へのポインタ CLListSGcell *pt
rAdjSG; あるいは隣接サブゴール候補へのアークベクト
ル long ArcToAdjSG[dim]; ・隣接サブゴール候補までの経路を格納するリスト CLL
istPath PathToAdjSG; ・隣接サブゴール候補までの経路の存在性BOOL Existan
ceOfPathToAdjSG;（Default TRUE） また、サブゴール有向グラフのデータ構造は、連結リス
トを用いずに配列などにより実現しても良い。

【００１３】つぎに、STEP2の隣接サブゴール間ローカ
ルパス（微少間隔経路）探索（Ｓ６）、STEP1とSTEP2間
のバックトラッキング（後戻り）処理、およびサブゴー
ル有向グラフとの関連について説明する。STEP1のサブ
ゴール系列探索（Ｓ２）によりサブゴール系列が得られ
た場合、それらは一旦、連結リストにストアされる（Ｓ
４）。そしてサブゴール系列連結リストの先頭から順に
２つの隣接サブゴールが取り出され（Ｓ５）、STEP2
で、それらを結ぶ経路がSTEP1よりも微少間隔で離散化
されたＣ空間において、STEP1と同様に一般的なグラフ
探索方式を用いて探索される（Ｓ６）。ただし、その
際、次に説明する図６のように２つの隣接サブゴールを
包含する局所空間に探索範囲が限定される。

【００１４】次に２つの隣接サブゴールを包含する局所
空間について説明する。図６において、６１、６２はそ
れぞれ隣接する２つのサブゴールを包含する楕円体領域
と多面体領域であり、６３、６４、６５はそれぞれ、サ
ブゴールsg#i, sg#i+1と、Ｃ空間障害物である。楕円体
領域６１と多面体領域６２のどちらを用いて探索範囲を
制限しても良く、楕円体領域６１と多面体領域６２以外
の局所空間により探索範囲を制限しても良い。局所空間
の設定の仕方は任意であるが、（ａ）は楕円体領域６１
内でローカルパスが求まる場合、（ｂ）は楕円体領域６
１内でローカルパスが求まらない場合を例示している。
楕円体領域６１を用いて探索空間を制限する場合は、以
下の式によって探索空間Ｓspaceを定義すれば良い。た
だし、d(n,m)は離散化Ｃ空間における２点ｎ,ｍ間の距
離を表す。 Ｓspace＝｛n|d(sg#i,n)＋d(n,sg#i+1)＜d(sg#i,sg#i+
1)×a,a>1｝ 図において、サブゴールsg#iから同sg#i+1へのローカル
パス（微少間隔経路）を探索するとき、（ａ）の場合
は、修正Ａ＊アルゴリズムなどを適用することによって
最短経路６６が求まる。（ｂ）の場合は、楕円体領域６
１内では経路が存在しないので、楕円体領域６１とＣ空
間障害物６５に囲まれる領域６７内の全ての格子点を展
開した後、経路探索に失敗したと判断される。

【００１５】STEP2の隣接サブゴール間ローカルパスの
探索が成功した場合、求まった経路は、サブゴール有向
グラフの SGList.cell(sg#i).AdjSGList.cell(arc(sg#i+1,sg#
i)).PathToAdjSG に格納される。そして、サブゴール系列連結リストから
つぎの隣接２サブゴールsg#i+1,sg#i+2が選択され、STE
P2が繰り返される。その際、すでに PathToAdjSGが登録
されているサブゴール間のローカルパス（微少間隔経
路）探索はスキップされる。このようにサブゴール有向
グラフのデータを更新することによって、以前にローカ
ルパスを求めた隣接サブゴール間については、再度、ST
EP2を実行しなくて済む。隣接サブゴール間ローカルパ
スに失敗した場合、同サブゴール有向グラフの SGList.cell(sg#i).AdjSGList.cell(arc(sg#i+1,sg#
i)). ExistanceOfPathToAdjSG がFALSEに書き換えられ、STEP1に戻る。STEP1において
サブゴール系列を探索する際には、サブゴール有向グラ
フのデータを参照し、あるサブゴール候補からExistanc
eOfPathToAdjSGがFALSEとなっている、すなわち、以前
にSTEP2のローカルパス探索に失敗したアーク方向へは
探索を展開しないようにする。また、サブゴール有向グ
ラフには、前回までに展開したサブゴール候補に関する
情報が記憶されているので、そのデータを参照すること
により、再度、幾何モデラを用いてロボットと作業環境
との干渉を検査するような無駄を省くことができる。

【００１６】スタート配置Ｓからゴール配置Ｇに至るサ
ブゴール系列内の全ての隣接サブゴール間ローカルパス
探索に成功した場合、スタート配置Ｓからゴール配置Ｇ
に至る全ローカルパスをストアしてアルゴリズムは正常
終了する。STEP1とSTEP2のバックトラッキングを繰り返
すうちに、STEP1のサブゴール系列探索に失敗した場合
（Ｓ３）、あるいは一定時間以上経過した場合（Ｓ１
０）は探索失敗として一連の処理を終了する。

【００１７】本発明による経路計画方法では、スタート
配置やゴール配置が障害物に囲まれ、非常に狭い領域を
通過しないとゴール配置に到達できないような場合に
は、探索に失敗する場合が多くなる。しかし、そのよう
なケースは稀であり、実用上は、ほとんどの場合に対処
できる。しかも本発明による経路計画方法は、上述した
ように探索空間を大幅に削減しているので、探索効率が
向上し、経路を高速に求めることができる。また、以上
では２次元Ｃ空間を例にとり実施の形態を説明してきた
が、本発明による経路計画方式は３次元以上のＣ空間に
対してもそのまま適用できる。図４の説明では、スター
ト配置Ｓとゴール配置Ｇの間で離散化した格子点を持つ
空間を形成してサブゴール系列を求め、その隣接するサ
ブゴール間のローカルパスを求める時に更に微少間隔で
離散化した格子点を持つ空間を形成して求めるという２
段階の手順をとっていたが、その２段階の手順を更に拡
張して３段階とすることもできる。スタート配置Ｓとゴ
ール配置Ｇの間で離散化した格子点を持つ空間を形成し
てサブゴール系列を求める手順を１段階とし、その隣接
するサブゴール間のローカルパスを求める時に更に微少
間隔で離散化した格子点を持つ空間を形成して求める手
順を２段階とすれば、３段階にする時は、１段階から２
段階に展開したことと同じ仕方を、２段階から３段階に
展開すればよい。これによって経路が局部的に複雑な動
作をするような場合でも、２段階で得られる同様の効果
を得ることができる。

【００１８】次に、前記の方法を実施する制御装置につ
いて図７のブロック図を用いて説明する。図において、
７１はロボットと作業環境の幾何モデルを入力するロボ
ット、環境モデル入力手段であり、７２はロボット、環
境モデル入力手段７１で入力されたデータを記憶する幾
何モデル記憶手段、７３は幾何モデル記憶手段７２を利
用したロボットと環境との干渉を検査する干渉検査手段
である。７４はロボットのスタート配置とゴール配置や
その他のパラメータを入力するスタート、ゴール配置、
パラメータ入力手段、７５は干渉検査手段７３を利用し
てサブゴールを探索するサブゴール系列探索手段、７６
はサブゴール系列探索手段７５によって展開されたサブ
ゴールを有向グラフの形式で記憶しておくサブゴール有
向グラフ記憶手段である。７７はサブゴール系列探索手
段７５によって得られたサブゴール系列を記憶するサブ
ゴール系列記憶手段、７８はサブゴール系列記憶手段７
７から２つの隣接サブゴールを選択する隣接サブゴール
選択手段である。７９は隣接サブゴール選択手段７８か
ら得られる２つの隣接サブゴール間のローカルパス（微
少間隔経路）を探索する隣接サブゴール間ローカルパス
探索手段、８０は隣接サブゴール間ローカルパス探索手
段７９における探索範囲を制限する探索領域制限手段で
ある。８１は隣接サブゴール間ローカルパスを記憶する
隣接サブゴール間ローカルパス一時記憶手段、８２はス
タート配置からゴール配置に至る連続したローカルパス
を出力する経路出力手段である。スタート配置Ｓ、ゴー
ル配置Ｇに加え、STEP1,2 におけるＣ空間の離散化間
隔、STEP2 において探索空間を制限するパラメータa
は、スタート、ゴール配置パラメータ入力手段７４によ
り設定される。また、これらのパラメータは予め固定さ
れていても良い。本発明の制御装置が上記のように構成
されているので、前記の方法と同様の処理をすることが
でき、同じ効果を得ることができるのである。

【００１９】

【発明の効果】以上述べたように、本発明は、ロボット
のスタート配置とゴール配置が与えられた時、その間を
大きく分割する離散化Ｃ空間を形成してグラフ探索手法
を用いて経路を探索し、求められたその隣接するサブゴ
ール間に、更に微少な間隔の格子点を持つ離散化Ｃ空間
を形成してグラフ探索手法でローカルパスを求めるとい
う２段階の経路探索をしているので、探索空間が大幅に
抑制されて探索効率が向上し、ロボット制御装置のリソ
ース消費を抑えて全体として性能を向上することができ
るという効果がある。また、３次元作業空間における自
由度の大きな多関節マニピュレータに対しても、高速で
実用的な経路計画が実現できるという効果がある。

【図面の簡単な説明】

【図１】ロボットが置かれている座標系を示す図 （ａ）２自由度マニピュレータとその周辺の障害物の位
置関係を示す図 （ｂ）２自由度移動ロボットとその周辺の障害物の位置
関係を示す図 （ｃ）スタート配置とゴール配置を示す図

【図２】本発明の方法の手順を示すフローチャート

【図３】離散化Ｃ空間を示す図

【図４】サブゴール系列の探索結果を説明する図

【図５】サブゴール有向グラフのデータ構造を示す図

【図６】局所探索空間を説明する図

【図７】本発明のロボットの大域動作経路計画制御装置
のブロック図

【符合の説明】

１ ２次元作業空間における２自由度マニピュレータ ２ ２自由度マニピュレータのエンドエフェクタ ３ ２次元作業空間における２自由度移動ロボット ４Ａ、４Ｂ、４Ｃ 障害物 ４１、６５ Ｃ空間障害物 ４２ 干渉が検出されたサブゴール候補点 ４３ 展開されたサブゴール候補点 ５１ 展開されたサブゴールの連結リスト ５２、５４ 連結リストのセル ５３ 隣接サブゴール情報の連結リスト ６１ 楕円体による局所探索空間 ６２ 多面体による局所探索空間 ６３、６４ サブゴール ６６ サブゴール間のローカルパス（微少間隔経路） ６７ 楕円体とＣ空間障害物で囲まれた閉領域 ７１ ロボット、環境モデル入力手段 ７２ 幾何モデル記憶手段 ７３ 干渉検査手段 ７４ スタート、ゴール配置、パラメータ入力手段 ７５ サブゴール系列探索手段 ７６ サブゴール有向グラフ記憶手段 ７７ サブゴール系列記憶手段 ７８ 隣接サブゴール選択手段 ７９ 隣接サブゴール間ローカルパス（微少間隔経路）
探索手段 ８０ 探索領域制限手段 ８１ 隣接サブゴール間ローカルパス（微少間隔経路）
一時記憶手段 ８２ 経路出力手段

Claims (7)

【特許請求の範囲】
1. 【請求項１】ロボットのスタート配置とゴール配置が与
   えられたとき、ロボットとその作業環境にある障害物の
   幾何学的形状とそれらの配置を幾何学モデルにする計算
   機上の幾何学モデル手段と、モデル同士の干渉を検出す
   る計算機上の干渉検出手段とを用い、ロボットと障害物
   が干渉を起こさないように次の２段階の経路（パス）探
   索を行うことを特徴とするロボットの大域動作経路計画
   方法。 ステップ１：配置空間（Ｃ空間）または作業空間を大ま
   かに離散化した格子点（サブゴール候補）を持つ空間
   （離散化Ｃ空間）を形成し、グラフ（状態空間）探索手
   法を用いて、スタート配置から前記格子点を経由してゴ
   ール配置へ至るサブゴール系列を求める。 ステップ２：ステップ１で求められたサブゴール系列に
   含まれる隣接するサブゴールを包含する空間において、
   ステップ１より微少間隔で離散化した格子点を持つ空間
   （離散化Ｃ空間）を形成し、グラフ（状態空間）探索手
   法を用いて、前記隣接サブゴール間の微少間隔格子点列
   すなわちローカルパス（局所微少間隔経路）を求める。
2. 【請求項２】前記ステップ２で、サブゴール系列に含ま
   れる隣接サブゴールsg#i, sg#i+1間のローカルパス探索
   に失敗した場合、ステップ１に戻り、サブゴールsg#iか
   らsg#i+1へのアークを経由しない別のサブゴール系列を
   求め直し、再びステップ２に移る処理を、スタート配置
   からゴール配置へ至る連続したローカルパスが見つかる
   まで、あるいはサブゴール系列が存在しないと判明する
   まで、あるいは予め定められた時間、繰り返すバックト
   ラッキング(後戻り)を行うことを特徴とする請求項１記
   載のロボットの大域動作経路計画方法。
3. 【請求項３】前記ステップ２において、隣接サブゴール
   間のローカルパスを求める際、探索範囲を２つの隣接サ
   ブゴールを包含する所定の局所空間に限定することを特
   徴とする請求項１記載のロボットの大域動作経路計画方
   法。
4. 【請求項４】前記ステップ２において、隣接サブゴール
   sg#i,sg#i+1を結ぶローカルパスを求める際、離散化Ｃ
   空間上の２点ｎ,ｍ間の距離をd(n,m)、aを１より大きな
   パラメータとしたとき、探索空間Ｓspaceを Ｓspace＝｛n|d(sg#i,n)＋d(n,sg#i+1)＜d(sg#i,sg#i+
   1)×a｝ により制限することを特徴とする請求項３記載のロボッ
   トの大域動作経路計画方法。
5. 【請求項５】前記ステップ１でサブゴール系列を求める
   際、展開された全てのサブゴール候補を重複を省いて、
   計算機上に有向グラフの形式で記憶し、前記ステップ２
   の実行結果によってサブゴール有向グラフのデータを更
   新し、前記ステップ１、２ともに最新のサブゴール有向
   グラフのデータを反映して探索を行うことを特徴とする
   請求項１または２記載のロボットの大域動作経路計画方
   法。
6. 【請求項６】前記ローカルパスを求める際の隣接する前
   記格子間に、さらに微少な間隔で離散化した格子点を持
   つ空間（離散化Ｃ空間）を形成し、グラフ（状態空間）
   探索手法を用いて、ローカルパスに含まれる隣接格子点
   間のさらに細分化されたサブローカルパスを求めること
   を特徴とする請求項１記載のロボットの大域動作経路計
   画方法。
7. 【請求項７】ロボットとその作業環境の幾何モデルを入
   力するロボット、環境モデル入力手段と、 前記ロボット、環境モデル入力手段が出力する前記幾何
   モデルを記憶する幾何モデル記憶手段と、 ロボットのスタート配置とゴール配置やその他パラメー
   タを入力するスタート、ゴール配置、パラメータ入力手
   段と、 前記幾何モデル記憶手段の情報を利用してロボットと環
   境との干渉を検査する干渉検査手段と、 前記干渉検査手段の干渉検査結果を利用してサブゴール
   の系列を探索するサブゴール系列探索手段と、 前記サブゴール系列探索手段によって展開されたサブゴ
   ールを有向グラフの形式で記憶するサブゴール有向グラ
   フ記憶手段と、 前記サブゴール系列探索手段によって得られたサブゴー
   ル系列を記憶するサブゴール系列記憶手段と、 前記サブゴール系列記憶手段が記憶している情報から２
   つの隣接サブゴールを選択する隣接サブゴール選択手段
   と、 前記隣接サブゴール選択手段から得られる２つの隣接サ
   ブゴール間のローカルパスを探索する隣接サブゴール間
   ローカルパス探索手段と、 前記隣接サブゴール間ローカルパス探索手段が前記ロー
   カルパスを探索するときの探索範囲を制限する探索領域
   制限手段と、 前記隣接サブゴール間ローカルパス探索手段によって得
   られる隣接サブゴール間ローカルパスを記憶する隣接サ
   ブゴール間ローカルパス一時記憶手段と、 前記サブゴール系列記憶手段に記憶されている情報を受
   けて前記スタート配置から前記ゴール配置に至る連続し
   たローカルパスを出力する経路出力手段とを備えたこと
   を特徴とするロボットの大域動作経路計画制御装置。