JP2002073130A - ロボットの大域動作経路計画方法とその制御装置 - Google Patents
ロボットの大域動作経路計画方法とその制御装置Info
- Publication number
- JP2002073130A JP2002073130A JP2001172590A JP2001172590A JP2002073130A JP 2002073130 A JP2002073130 A JP 2002073130A JP 2001172590 A JP2001172590 A JP 2001172590A JP 2001172590 A JP2001172590 A JP 2001172590A JP 2002073130 A JP2002073130 A JP 2002073130A
- Authority
- JP
- Japan
- Prior art keywords
- robot
- search
- subgoal
- subgoals
- space
- 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.)
- Abandoned
Links
Landscapes
- Numerical Control (AREA)
- Manipulator (AREA)
Abstract
(57)【要約】
【課題】 ３次元作業空間内における大自由度の多関節
マニピュレータに対しても経路を高速に求めうるロボッ
トの大域動作経路計画方法を提供する。 【解決手段】 スタート配置とゴール配置が与えられた
とき、ロボットとその作業環境にある障害物の幾何学的
形状とそれらの配置を記述する計算機上の幾何学モデル
手段と、モデル同士の干渉を検出する計算機上の干渉検
出手段とを用い、ロボットと障害物が干渉を起こさない
ように２段階の経路探索を行うロボットの大域動作経路
計画方法において、配置空間を大まかに離散化した格子
点を持つ空間を形成し、グラフ探索手法でスタート配置
から格子点を経由してゴール配置へ至るサブゴール系列
を求め、これから隣接するサブゴールを２つづつ取り出
し、その隣接するサブゴールを包含する空間においてよ
り微少な間隔で離散化した格子点を持つ空間を形成し、
以後同じ処理を繰り返し、経路探索失敗時は最初にもど
り、スタート配置からゴール配置までの連続した微小間
隔経路を求める。
マニピュレータに対しても経路を高速に求めうるロボッ
トの大域動作経路計画方法を提供する。 【解決手段】 スタート配置とゴール配置が与えられた
とき、ロボットとその作業環境にある障害物の幾何学的
形状とそれらの配置を記述する計算機上の幾何学モデル
手段と、モデル同士の干渉を検出する計算機上の干渉検
出手段とを用い、ロボットと障害物が干渉を起こさない
ように２段階の経路探索を行うロボットの大域動作経路
計画方法において、配置空間を大まかに離散化した格子
点を持つ空間を形成し、グラフ探索手法でスタート配置
から格子点を経由してゴール配置へ至るサブゴール系列
を求め、これから隣接するサブゴールを２つづつ取り出
し、その隣接するサブゴールを包含する空間においてよ
り微少な間隔で離散化した格子点を持つ空間を形成し、
以後同じ処理を繰り返し、経路探索失敗時は最初にもど
り、スタート配置からゴール配置までの連続した微小間
隔経路を求める。
Description
【０００１】
【発明の属する技術分野】本発明は、ロボットの作業環
境にある物体の幾何学的形状と、その配置を記述した計
算機上の幾何モデルを使ってロボットの動作経路を計画
する方法に関するものであり、特に、ロボットのスター
ト配置とゴール配置が与えられたとき、モデル同士の干
渉を検査する干渉検査手段を利用してロボットと作業環
境内の障害物とが干渉しないようにするロボットの大域
動作経路計画方法とその方法を実施する装置に関する。
境にある物体の幾何学的形状と、その配置を記述した計
算機上の幾何モデルを使ってロボットの動作経路を計画
する方法に関するものであり、特に、ロボットのスター
ト配置とゴール配置が与えられたとき、モデル同士の干
渉を検査する干渉検査手段を利用してロボットと作業環
境内の障害物とが干渉しないようにするロボットの大域
動作経路計画方法とその方法を実施する装置に関する。
【０００２】
【従来の技術】ロボットの障害物回避に係る経路計画問
題は、配置空間（Ｃ空間：Configuration space）にお
ける、点の移動経路計画問題という形式に一般化するこ
とができる。Ｃ空間はロボットの配置（Configuratio
n）を一意に決定するパラメータ空間である。図２５〜
図２７は従来のロボットが置かれている座標系およびロ
ボットの配置空間（Ｃ空間）を示す図であり、この図２
５〜図２７を参照してＣ空間について説明する。図２５
および図２６はそれぞれ２次元作業空間1703における３
自由度マニピュレータ1701、３自由度移動ロボット1702
を示している。３自由度マニピュレータ1701の場合、図
２７に示した関節空間(q1,q2,q3)はＣ空間となる。２次
元作業空間内における移動ロボットの場合、作業空間座
標系を基準にしたロボット座標原点の位置(X,Y)とロボ
ットの向きθの３自由度空間(X,Y,θ)はＣ空間となる。
Ｃ空間において障害物と干渉しない経路を求めるために
は、通常Ｃ空間の空間記述が必要になる。即ち、作業空
間1703における障害物1704a、1704bとロボット1701の幾
何学的形状およびその位置関係を用いてＣ空間における
障害物1705a、1705b（Ｃ空間障害物）を記述し、Ｃ空間
内の自由（非干渉）空間（Ｃ自由空間）を求める必要が
ある。
題は、配置空間（Ｃ空間：Configuration space）にお
ける、点の移動経路計画問題という形式に一般化するこ
とができる。Ｃ空間はロボットの配置（Configuratio
n）を一意に決定するパラメータ空間である。図２５〜
図２７は従来のロボットが置かれている座標系およびロ
ボットの配置空間（Ｃ空間）を示す図であり、この図２
５〜図２７を参照してＣ空間について説明する。図２５
および図２６はそれぞれ２次元作業空間1703における３
自由度マニピュレータ1701、３自由度移動ロボット1702
を示している。３自由度マニピュレータ1701の場合、図
２７に示した関節空間(q1,q2,q3)はＣ空間となる。２次
元作業空間内における移動ロボットの場合、作業空間座
標系を基準にしたロボット座標原点の位置(X,Y)とロボ
ットの向きθの３自由度空間(X,Y,θ)はＣ空間となる。
Ｃ空間において障害物と干渉しない経路を求めるために
は、通常Ｃ空間の空間記述が必要になる。即ち、作業空
間1703における障害物1704a、1704bとロボット1701の幾
何学的形状およびその位置関係を用いてＣ空間における
障害物1705a、1705b（Ｃ空間障害物）を記述し、Ｃ空間
内の自由（非干渉）空間（Ｃ自由空間）を求める必要が
ある。
【０００３】Ｃ空間記述に基づくロボットの経路計画方
法は、ロードマップ法、セル分解法、Ｃ空間離散化法、
ポテンシャルフィールド法に大別される。ロードマップ
法と、セル分解法、Ｃ空間離散化法の相違点は、Ｃ空間
の記述法と、その記述に基づく探索手法にある。ロード
マップ法やセル分解法では、通常、Ｃ空間の陽な記述が
必要になる。２次元作業空間における２自由度移動ロボ
ットの場合は、Ｃ自由空間を陽に記述することができ
る。すなわち、Ｃ空間障害物の境界を陽に求めることが
できる。多関節マニピュレータの場合は、Ｃ障害物を解
析的に求める一般的手法は存在せず、Slice Projection
（Needle）法、SweptVolume法、Point Evaluation法な
どのＣ空間の離散化に基づく近似手法によりＣ空間記述
を求めるのが一般的である。ロードマップ法やセル分解
法は２次元作業空間における移動ロボットの経路計画問
題に適しており、Ｃ空間離散化法は多関節マニピュレー
タの経路計画問題に適している。ポテンシャルフィール
ド法は、ゴール配置からロボットに作用する引力、障害
物からロボットに作用する斥力、ロボットを動作範囲内
に留める斥力の和からなる人工ポテンシャル関数を定義
し、ポテンシャル関数のＣ空間における最急勾配方向に
経路を探索する方法である。多関節マニピュレータの場
合は、Ｃ障害物を陽に記述するのは困難なため、リンク
にいくつかの代表点を設け、各代表点から一定距離内に
ある障害物からそれぞれの代表点に作用する斥力を作業
空間で定義する。ゴール配置以外のポテンシャル極小点
を如何に避けるか、現実的に計算可能なポテンシャル関
数を如何に導出するかが課題となる。
法は、ロードマップ法、セル分解法、Ｃ空間離散化法、
ポテンシャルフィールド法に大別される。ロードマップ
法と、セル分解法、Ｃ空間離散化法の相違点は、Ｃ空間
の記述法と、その記述に基づく探索手法にある。ロード
マップ法やセル分解法では、通常、Ｃ空間の陽な記述が
必要になる。２次元作業空間における２自由度移動ロボ
ットの場合は、Ｃ自由空間を陽に記述することができ
る。すなわち、Ｃ空間障害物の境界を陽に求めることが
できる。多関節マニピュレータの場合は、Ｃ障害物を解
析的に求める一般的手法は存在せず、Slice Projection
（Needle）法、SweptVolume法、Point Evaluation法な
どのＣ空間の離散化に基づく近似手法によりＣ空間記述
を求めるのが一般的である。ロードマップ法やセル分解
法は２次元作業空間における移動ロボットの経路計画問
題に適しており、Ｃ空間離散化法は多関節マニピュレー
タの経路計画問題に適している。ポテンシャルフィール
ド法は、ゴール配置からロボットに作用する引力、障害
物からロボットに作用する斥力、ロボットを動作範囲内
に留める斥力の和からなる人工ポテンシャル関数を定義
し、ポテンシャル関数のＣ空間における最急勾配方向に
経路を探索する方法である。多関節マニピュレータの場
合は、Ｃ障害物を陽に記述するのは困難なため、リンク
にいくつかの代表点を設け、各代表点から一定距離内に
ある障害物からそれぞれの代表点に作用する斥力を作業
空間で定義する。ゴール配置以外のポテンシャル極小点
を如何に避けるか、現実的に計算可能なポテンシャル関
数を如何に導出するかが課題となる。
【０００４】これら従来方式は、文献「Y.K.Hwang and
N.Ahuja : Gross Motion Planning-A Survey, ACM Comp
uting Surveys, Vol.24, No.3, pp.219-291, 1992」に
詳しくまとめられているので、詳細は省略する。経路計
画問題は、計算の複雑性理論（Computational Complexi
ty Theory）によれば、NP（Non-deterministic Polynom
ial）hardと呼ばれる解の導出が非常に困難な問題に属
する。ロボットの自由度、障害物の数、障害物の面の数
などが大きくなるに従って、現実的な時間内では障害物
回避経路を求めることができなくなる。上の従来法を現
状の100MIPS 程度の計算機を用いて数十分以内に経路が
求まるという基準で評価すると、ロードマップ法やセル
分解法は、２次元作業空間内における移動ロボット、Ｃ
空間離散化法やポテンシャルフィールド法は、３次元作
業空間内における６自由度マニピュレータという具合に
適用可能範囲が限られて来る。
N.Ahuja : Gross Motion Planning-A Survey, ACM Comp
uting Surveys, Vol.24, No.3, pp.219-291, 1992」に
詳しくまとめられているので、詳細は省略する。経路計
画問題は、計算の複雑性理論（Computational Complexi
ty Theory）によれば、NP（Non-deterministic Polynom
ial）hardと呼ばれる解の導出が非常に困難な問題に属
する。ロボットの自由度、障害物の数、障害物の面の数
などが大きくなるに従って、現実的な時間内では障害物
回避経路を求めることができなくなる。上の従来法を現
状の100MIPS 程度の計算機を用いて数十分以内に経路が
求まるという基準で評価すると、ロードマップ法やセル
分解法は、２次元作業空間内における移動ロボット、Ｃ
空間離散化法やポテンシャルフィールド法は、３次元作
業空間内における６自由度マニピュレータという具合に
適用可能範囲が限られて来る。
【０００５】Point Evaluation法に基づくＣ空間離散化
法の基本アプローチについて、図２８〜図３１に示す従
来の２次元離散化Ｃ空間と経路探索の進行図を参照して
説明する。図２８の1801は２次元Ｃ空間(q1,q2)を示し
ている。1803、1804はそれぞれスタート配置S、ゴール
配置Gを示している。1805a〜bはＣ空間障害物である。P
oint Evaluation法に基づくＣ空間離散化法では、図２
９の1802のようにＣ空間1801を微小間隔で離散化した格
子点の空間、すなわち、離散化Ｃ空間(d-q1,d-q2)を考
える。離散化Ｃ空間1802は各格子点をノードとして、そ
れらの隣接関係をアークとする連結度グラフと見なすこ
とができる。この連結度グラフ上のスタート配置Sから
隣接する格子点を辿ってゴール配置Gへ至る経路（格子
点列）を求めるのであるが、各格子点においてのみ、ロ
ボットと作業環境との干渉の有無を幾何モデルと干渉チ
ェッカを用いて検査する。 予め全格子点での干渉の有無を求めておく方式と、
探索の過程で逐次各格子点における干渉の有無を検
査する方式がある。作業環境がダイナミックに変化する
場合には、後者の方式を用いる。
法の基本アプローチについて、図２８〜図３１に示す従
来の２次元離散化Ｃ空間と経路探索の進行図を参照して
説明する。図２８の1801は２次元Ｃ空間(q1,q2)を示し
ている。1803、1804はそれぞれスタート配置S、ゴール
配置Gを示している。1805a〜bはＣ空間障害物である。P
oint Evaluation法に基づくＣ空間離散化法では、図２
９の1802のようにＣ空間1801を微小間隔で離散化した格
子点の空間、すなわち、離散化Ｃ空間(d-q1,d-q2)を考
える。離散化Ｃ空間1802は各格子点をノードとして、そ
れらの隣接関係をアークとする連結度グラフと見なすこ
とができる。この連結度グラフ上のスタート配置Sから
隣接する格子点を辿ってゴール配置Gへ至る経路（格子
点列）を求めるのであるが、各格子点においてのみ、ロ
ボットと作業環境との干渉の有無を幾何モデルと干渉チ
ェッカを用いて検査する。 予め全格子点での干渉の有無を求めておく方式と、
探索の過程で逐次各格子点における干渉の有無を検
査する方式がある。作業環境がダイナミックに変化する
場合には、後者の方式を用いる。
【０００６】格子点の連結度グラフ上での経路探索に
は、縦型、横型、山登り、最良優先、Ａ* 探索などのグ
ラフ（状態空間）探索手法を用いる。図３０に離散化Ｃ
空間においてグラフ探索が進行する様子を示す。グラフ
探索は、スタートノードＳの子ノード（隣接ノード）を
生成することから始まる。1-1,1-2,1-3,1-4が生成され
たSの子ノードである。子ノードが生成されると、ある
基準に基づいてそのノードでの評価関数値が計算されノ
ードに記憶される。また、各ノードには親ノードへのリ
ンク（ポインタ）も記憶される。あるノードの子ノード
を生成することを、そのノードを展開するという。生成
されたノード1-1,1-2,1-3,1-4の中から、評価関数値に
基づいて次にどのノードを展開すべきかが選ばれる。
は、縦型、横型、山登り、最良優先、Ａ* 探索などのグ
ラフ（状態空間）探索手法を用いる。図３０に離散化Ｃ
空間においてグラフ探索が進行する様子を示す。グラフ
探索は、スタートノードＳの子ノード（隣接ノード）を
生成することから始まる。1-1,1-2,1-3,1-4が生成され
たSの子ノードである。子ノードが生成されると、ある
基準に基づいてそのノードでの評価関数値が計算されノ
ードに記憶される。また、各ノードには親ノードへのリ
ンク（ポインタ）も記憶される。あるノードの子ノード
を生成することを、そのノードを展開するという。生成
されたノード1-1,1-2,1-3,1-4の中から、評価関数値に
基づいて次にどのノードを展開すべきかが選ばれる。
【０００７】図３０の例では、ノード1-2が選択されて
展開されている。図３１は図３０のグラフ探索進行状況
をツリー構造（探索木と呼ばれる）で表現したものであ
る。図３０の探索済み領域の波面（境界）1807（1-1,1-
3,1-4,2-1,2-3）および1808（3-1,3-2）が図３１の探索
木の葉（直系の子孫を持たないノード）に対応してい
る。1808（3-1,3-2）は探索木の葉の中の障害物と干渉
が生じたノードを示している。 スタート配置1803およ
び1806（1-2,2-2）は展開済みノード（子を持つノー
ド）を示している。また、探索木の枝が親ノードへのポ
インタに相当する。グラフ探索は、探索木の葉の中から
評価関数値に基づいて次に展開する（干渉を生じない）
ノードを選び、そのノードの子ノードを重複せずに生成
する処理を、葉からゴール配置が選択される、あるいは
選択すべき葉がなくなるまで繰り返す。葉からゴール配
置が選択された場合、ゴール配置から親ノードへのポイ
ンタを辿っていくことにより経路が得られる。上述した
ように、Point Evaluation法に基づくＣ空間離散化法で
は格子点においてのみ干渉チェックを行うため、非干渉
ノード間を結ぶアーク上での非干渉性は保証されない。
したがって、離散化分解能を十分高くする必要がある。
展開されている。図３１は図３０のグラフ探索進行状況
をツリー構造（探索木と呼ばれる）で表現したものであ
る。図３０の探索済み領域の波面（境界）1807（1-1,1-
3,1-4,2-1,2-3）および1808（3-1,3-2）が図３１の探索
木の葉（直系の子孫を持たないノード）に対応してい
る。1808（3-1,3-2）は探索木の葉の中の障害物と干渉
が生じたノードを示している。 スタート配置1803およ
び1806（1-2,2-2）は展開済みノード（子を持つノー
ド）を示している。また、探索木の枝が親ノードへのポ
インタに相当する。グラフ探索は、探索木の葉の中から
評価関数値に基づいて次に展開する（干渉を生じない）
ノードを選び、そのノードの子ノードを重複せずに生成
する処理を、葉からゴール配置が選択される、あるいは
選択すべき葉がなくなるまで繰り返す。葉からゴール配
置が選択された場合、ゴール配置から親ノードへのポイ
ンタを辿っていくことにより経路が得られる。上述した
ように、Point Evaluation法に基づくＣ空間離散化法で
は格子点においてのみ干渉チェックを行うため、非干渉
ノード間を結ぶアーク上での非干渉性は保証されない。
したがって、離散化分解能を十分高くする必要がある。
【０００８】
【発明が解決しようとする課題】しかしながら、上述の
従来のＣ空間離散化法では、離散化分解能、ロボットの
自由度、障害物の面の総数などの増加に対して、計算
量、記憶領域が指数関数的に増加してしまう。そのた
め、６自由度以上のマニピュレータに対して、Ｃ空間離
散化法の基本アプローチをそのまま適用するのは現実的
でなく、探索空間を制限したり（局所探索）、探索効率
を高めるための工夫が必要となる。様々なヒューリステ
ィックス（発見的手法）を用いて探索空間削減、探索効
率化をする方式が種々提案されているが、経路を高速に
求られる実用的な方式が少ないという問題があった。探
索効率を高める方式としては、例えば、特開平10−9731
6、「多数の移動物体を伴うシステムの移動計画及び制
御方法」、に開示されているロボット経路の計画法等が
ある。図３２はその従来方式のブロック図であり、ロボ
ットの配置空間を第１の大きなスケールでセルに分割し
たセル空間データベース（フリー《衝突無し》の情報、
フル《衝突の危険あり》の情報、ミックス《それ以外》
の情報を持つ。）を利用して、中間経由点を直線的に結
んだセグメント列を求める線路計画モジュール1912と、
次に、線路計画モジュール1912により得られたセグメン
ト列の中間点リストより、微細な時間分解能で加減速を
考慮した、第２の更に微小時間間隔のトラジェクトリ
（軌跡）データを求め、それをロボットマニピュレータ
・システム1916の位置制御系に直接指令を与えて、実際
にロボットを動作させ、同時に、人工ポテンシャル法を
利用して（フィードバック制御を用い）衝突を動作段階
で確実に回避できるように、先のトラジェクトリを修正
するものであるが、経路探索という意味からは第１と第
２のトラジェクトリ間でのバックトラッキングも、再探
索も行われず、微細な第２のトラジェクトリに関して
も、空間配置ではない時間間隔を微小間隔に分割して経
路補完を行っているに過ぎないので、充分に経路探索が
尽くされてはいない。それに、実際のロボットの動作段
階で人工ポテンシャル方式による修正により、最終的な
衝突回避を行っているので、形式的には２段階経路探索
方式となってはいるが、経路の探索精度としては充分で
なく、存在する経路を見落としやすいという問題があっ
た。
従来のＣ空間離散化法では、離散化分解能、ロボットの
自由度、障害物の面の総数などの増加に対して、計算
量、記憶領域が指数関数的に増加してしまう。そのた
め、６自由度以上のマニピュレータに対して、Ｃ空間離
散化法の基本アプローチをそのまま適用するのは現実的
でなく、探索空間を制限したり（局所探索）、探索効率
を高めるための工夫が必要となる。様々なヒューリステ
ィックス（発見的手法）を用いて探索空間削減、探索効
率化をする方式が種々提案されているが、経路を高速に
求られる実用的な方式が少ないという問題があった。探
索効率を高める方式としては、例えば、特開平10−9731
6、「多数の移動物体を伴うシステムの移動計画及び制
御方法」、に開示されているロボット経路の計画法等が
ある。図３２はその従来方式のブロック図であり、ロボ
ットの配置空間を第１の大きなスケールでセルに分割し
たセル空間データベース（フリー《衝突無し》の情報、
フル《衝突の危険あり》の情報、ミックス《それ以外》
の情報を持つ。）を利用して、中間経由点を直線的に結
んだセグメント列を求める線路計画モジュール1912と、
次に、線路計画モジュール1912により得られたセグメン
ト列の中間点リストより、微細な時間分解能で加減速を
考慮した、第２の更に微小時間間隔のトラジェクトリ
（軌跡）データを求め、それをロボットマニピュレータ
・システム1916の位置制御系に直接指令を与えて、実際
にロボットを動作させ、同時に、人工ポテンシャル法を
利用して（フィードバック制御を用い）衝突を動作段階
で確実に回避できるように、先のトラジェクトリを修正
するものであるが、経路探索という意味からは第１と第
２のトラジェクトリ間でのバックトラッキングも、再探
索も行われず、微細な第２のトラジェクトリに関して
も、空間配置ではない時間間隔を微小間隔に分割して経
路補完を行っているに過ぎないので、充分に経路探索が
尽くされてはいない。それに、実際のロボットの動作段
階で人工ポテンシャル方式による修正により、最終的な
衝突回避を行っているので、形式的には２段階経路探索
方式となってはいるが、経路の探索精度としては充分で
なく、存在する経路を見落としやすいという問題があっ
た。
【０００９】また、特願平10−356719（筆者の先々願特
許）に、サブゴール系列（中間経由点）探索と隣接サブ
ゴール間局所経路探索を繰り返す２段階経路計画方式が
記載されている。この方式によれば、局所経路探索の探
索範囲を所定の局所領域に制限した２段階探索が行われ
るので経路計算の高速化が可能とはなるが、存在する経
路を見落としやすいという問題があった。特願平11−14
6667（筆者の先願特許）に、サブゴール系列探索と隣接
サブゴール間局所経路探索を繰り返し、なおかつ局所経
路探索の探索範囲を動的に制限し、その探索範囲を段階
的に広げながら局所経路を再探索する２段階経路計画方
式が記載されている。この方式によれば、経路計算の高
速化と経路を見落とす可能性の低減の両立が可能にな
る。しかしながら、探索範囲を広げて隣接サブゴール間
局所経路探索を行なう際、前回に失敗した同局所経路探
索時の格子点情報の再利用については考慮していないの
で、前回探索した格子点を再度探索しなければならず、
（サブゴール間）局所経路の再探索回数が増加すると若
干探索効率が低下する。また、サブゴール系列の探索過
程で得られたサブゴールとその隣接関係などを示す情報
を有向グラフ形式で記憶していたため、一方向の検索し
かできず、既に局所経路が求まった隣接サブゴール間に
対して前回とは反対方向から局所経路探索を行なう場
合、最初から探索を行なわなければならない。このよう
に探索情報の再利用性に関して若干の問題があった。ま
た、文献「K.K.Gupta , The Sequential Framework for
Practical MotionPlanning for Manipulator Arms: Al
gorithm and Experiments, Practical Motion Planning
in Robotics: Current Approaches and Future Direct
ions, pp.9-31, John Wiley & Sons Ltd., 1998」に、
多自由度のマニピュレータの障害物回避経路計画問題
を、各リンク（サブロボット）ごとの経路計画サブ問題
（サブロボットの経路探索）に分割し、ベース部のリン
クから手先部のリンクに向かって順番にバックトラッキ
ングを伴って経路を探索する方式（シーケンシャル探
索）が記載されている。図４５および図４６はシーケン
シャル探索の概念図を示すもので、図４５はシーケンシ
ャル探索の概念図の前半、図４６はその後半を示してい
る。図４５および図４６において、3601および3602は作
業空間障害物、3603は３自由度ロボットマニピュレータ
を示している。3604,3606,3608はそれぞれサブロボット
０（link1），サブロボット１（link2），サブロボット
２（link3）を示している。図４５および図４６では、2
次元作業空間（平面）における3自由度マニピュレータ3
603を例にシーケンシャル探索の概要を説明している。
図４５(a),(b)はそれぞれスタート配置、ゴール配置を
示している。図４５(c),(d)は、link2およびlink3を無
視して、link1（サブロボット０）について関節角度q1
の障害物回避経路を探索した様子を示している（link1
のベース側の一端は固定されている）。図４６(e),(f)
は、link1の手先側一端の軌跡3605をt2とし、link2のベ
ース側の一端をt2に拘束してlink2（サブロボット１）
について関節角度q2の障害物回避経路を探索した様子を
示している（探索空間はt2×q2）。図４６(g),(h)は、l
ink2の手先側一端の軌跡3607をt3とし、link3のベース
側の一端をt3に拘束してlink3（サブロボット２）につ
いて関節角度q3の障害物回避経路を探索した様子を示し
ている（探索空間はt3×q3）。また、図４７はシーケン
シャル探索のフローチャートを示している。各サブロボ
ット（リンク）の経路探索では、探索の前処理として、
サブロボット探索空間全域のＣ空間記述を幾何学モデル
と干渉チェッカを用いて求め、得られたビットマップ形
式のＣ空間記述から距離マップ（Ｃ空間記述の各ビット
にＣ空間障害物からの距離を記入したもの）とVoronoi
図（距離マップのスケルトン）を生成し、それらをもと
にポテンシャル場を計算する。サブロボットの経路はこ
のポテンシャル場上でGradient法により探索される。サ
ブロボットの探索空間全域のＣ空間記述やそれをもとに
ポテンシャル場を計算しているが、各サブロボットの探
索空間は２次元なので、この計算量負荷はさほど問題に
はならない。あるサブロボットの経路探索に失敗した場
合は、１つ手前のサブロボットの経路探索に後戻りする
（バックトラッキング）。図４７において、B#Levelは
バックトラッキングの回数を、Max#Levelはバックトラ
ッキングの最大回数を示している。シーケンシャル探索
では、バックトラッキングの前処理として、１つ手前の
サブロボットのＣ空間に仮想的な障害物（Virtual Forb
idden Area）を挿入する処理が行なわれる。仮想的な障
害物を挿入する座標を求めるため、探索に失敗したサブ
ロボットのＣ空間内のスタートおよびゴール配置を含む
閉領域を走査して、スタート配置およびゴール配置を含
むＣ空間自由領域をブロックする個所を検出している。
この方式では、N次元Ｃ空間での経路計画問題を、最短
でN-1回の2次元Ｃ空間上の経路計画問題に分割するの
で、探索空間が劇的に削減され、効率の良い探索が可能
となる。しかしながら、従来のシーケンシャル探索で
は、各サブロボットの自由度が１に固定されているた
め、あるリンクが大きく迂回してしまう場合が生じ、し
たがって経路長が長くなり経路の質が劣化してしまう。
サブロボットの自由度を２以上とすれば経路の質を改善
できる可能性があるが、従来のシーケンシャル探索で
は、各サブロボットの探索空間全域の詳細なＣ空間記述
が必要なため、サブロボットの自由度を２以上にすると
探索効率が劣化してしまうという問題がある。さらに、
１つ手前のサブロボットの経路探索にバックトラッキン
グする際に、１つ手前のサブロボットの探索空間に仮想
的な障害物を挿入するので、特定のサブロボットでの再
探索回数が増加すると経路を見落とし、経路探索に失敗
するという問題もある。
許）に、サブゴール系列（中間経由点）探索と隣接サブ
ゴール間局所経路探索を繰り返す２段階経路計画方式が
記載されている。この方式によれば、局所経路探索の探
索範囲を所定の局所領域に制限した２段階探索が行われ
るので経路計算の高速化が可能とはなるが、存在する経
路を見落としやすいという問題があった。特願平11−14
6667（筆者の先願特許）に、サブゴール系列探索と隣接
サブゴール間局所経路探索を繰り返し、なおかつ局所経
路探索の探索範囲を動的に制限し、その探索範囲を段階
的に広げながら局所経路を再探索する２段階経路計画方
式が記載されている。この方式によれば、経路計算の高
速化と経路を見落とす可能性の低減の両立が可能にな
る。しかしながら、探索範囲を広げて隣接サブゴール間
局所経路探索を行なう際、前回に失敗した同局所経路探
索時の格子点情報の再利用については考慮していないの
で、前回探索した格子点を再度探索しなければならず、
（サブゴール間）局所経路の再探索回数が増加すると若
干探索効率が低下する。また、サブゴール系列の探索過
程で得られたサブゴールとその隣接関係などを示す情報
を有向グラフ形式で記憶していたため、一方向の検索し
かできず、既に局所経路が求まった隣接サブゴール間に
対して前回とは反対方向から局所経路探索を行なう場
合、最初から探索を行なわなければならない。このよう
に探索情報の再利用性に関して若干の問題があった。ま
た、文献「K.K.Gupta , The Sequential Framework for
Practical MotionPlanning for Manipulator Arms: Al
gorithm and Experiments, Practical Motion Planning
in Robotics: Current Approaches and Future Direct
ions, pp.9-31, John Wiley & Sons Ltd., 1998」に、
多自由度のマニピュレータの障害物回避経路計画問題
を、各リンク（サブロボット）ごとの経路計画サブ問題
（サブロボットの経路探索）に分割し、ベース部のリン
クから手先部のリンクに向かって順番にバックトラッキ
ングを伴って経路を探索する方式（シーケンシャル探
索）が記載されている。図４５および図４６はシーケン
シャル探索の概念図を示すもので、図４５はシーケンシ
ャル探索の概念図の前半、図４６はその後半を示してい
る。図４５および図４６において、3601および3602は作
業空間障害物、3603は３自由度ロボットマニピュレータ
を示している。3604,3606,3608はそれぞれサブロボット
０（link1），サブロボット１（link2），サブロボット
２（link3）を示している。図４５および図４６では、2
次元作業空間（平面）における3自由度マニピュレータ3
603を例にシーケンシャル探索の概要を説明している。
図４５(a),(b)はそれぞれスタート配置、ゴール配置を
示している。図４５(c),(d)は、link2およびlink3を無
視して、link1（サブロボット０）について関節角度q1
の障害物回避経路を探索した様子を示している（link1
のベース側の一端は固定されている）。図４６(e),(f)
は、link1の手先側一端の軌跡3605をt2とし、link2のベ
ース側の一端をt2に拘束してlink2（サブロボット１）
について関節角度q2の障害物回避経路を探索した様子を
示している（探索空間はt2×q2）。図４６(g),(h)は、l
ink2の手先側一端の軌跡3607をt3とし、link3のベース
側の一端をt3に拘束してlink3（サブロボット２）につ
いて関節角度q3の障害物回避経路を探索した様子を示し
ている（探索空間はt3×q3）。また、図４７はシーケン
シャル探索のフローチャートを示している。各サブロボ
ット（リンク）の経路探索では、探索の前処理として、
サブロボット探索空間全域のＣ空間記述を幾何学モデル
と干渉チェッカを用いて求め、得られたビットマップ形
式のＣ空間記述から距離マップ（Ｃ空間記述の各ビット
にＣ空間障害物からの距離を記入したもの）とVoronoi
図（距離マップのスケルトン）を生成し、それらをもと
にポテンシャル場を計算する。サブロボットの経路はこ
のポテンシャル場上でGradient法により探索される。サ
ブロボットの探索空間全域のＣ空間記述やそれをもとに
ポテンシャル場を計算しているが、各サブロボットの探
索空間は２次元なので、この計算量負荷はさほど問題に
はならない。あるサブロボットの経路探索に失敗した場
合は、１つ手前のサブロボットの経路探索に後戻りする
（バックトラッキング）。図４７において、B#Levelは
バックトラッキングの回数を、Max#Levelはバックトラ
ッキングの最大回数を示している。シーケンシャル探索
では、バックトラッキングの前処理として、１つ手前の
サブロボットのＣ空間に仮想的な障害物（Virtual Forb
idden Area）を挿入する処理が行なわれる。仮想的な障
害物を挿入する座標を求めるため、探索に失敗したサブ
ロボットのＣ空間内のスタートおよびゴール配置を含む
閉領域を走査して、スタート配置およびゴール配置を含
むＣ空間自由領域をブロックする個所を検出している。
この方式では、N次元Ｃ空間での経路計画問題を、最短
でN-1回の2次元Ｃ空間上の経路計画問題に分割するの
で、探索空間が劇的に削減され、効率の良い探索が可能
となる。しかしながら、従来のシーケンシャル探索で
は、各サブロボットの自由度が１に固定されているた
め、あるリンクが大きく迂回してしまう場合が生じ、し
たがって経路長が長くなり経路の質が劣化してしまう。
サブロボットの自由度を２以上とすれば経路の質を改善
できる可能性があるが、従来のシーケンシャル探索で
は、各サブロボットの探索空間全域の詳細なＣ空間記述
が必要なため、サブロボットの自由度を２以上にすると
探索効率が劣化してしまうという問題がある。さらに、
１つ手前のサブロボットの経路探索にバックトラッキン
グする際に、１つ手前のサブロボットの探索空間に仮想
的な障害物を挿入するので、特定のサブロボットでの再
探索回数が増加すると経路を見落とし、経路探索に失敗
するという問題もある。
【００１０】本発明は、上記問題に鑑みてなされたもの
であり、３次元作業空間内における自由度の大きな多関
節マニピュレータに対しても経路を高速に求めることが
可能で、なおかつ、経路を見落とす可能性も小さく、な
おかつ、探索情報の再利用性を高めることによりさらな
る探索の高速化が可能なロボットの大域動作経路計画方
法とその制御装置をを提供することを目的としている。
であり、３次元作業空間内における自由度の大きな多関
節マニピュレータに対しても経路を高速に求めることが
可能で、なおかつ、経路を見落とす可能性も小さく、な
おかつ、探索情報の再利用性を高めることによりさらな
る探索の高速化が可能なロボットの大域動作経路計画方
法とその制御装置をを提供することを目的としている。
【００１１】
【課題を解決するための手段】上記目的を達成するた
め、請求項１記載のロボットの大域動作経路計画方法
は、ロボットのスタート配置とゴール配置が与えられた
とき、ロボットとその作業環境にある障害物の幾何学的
形状とそれらの配置を記述する計算機上の幾何学モデル
手段と、モデル同士の干渉を検出する計算機上の干渉検
出手段とを用い、ロボットと障害物が干渉を起こさない
ように２段階の経路探索を行うロボットの大域動作経路
計画方法において、ステップ１として、配置空間（Ｃ空
間）または作業空間を大まかに離散化した格子点（サブ
ゴール候補）を持つ空間（離散化Ｃ空間）を形成し、グ
ラフ（状態空間）探索手法を用いて、スタート配置ある
いはゴール配置から前記格子点を経由してゴール配置あ
るいはスタート配置へ至るサブゴール系列を求め、ステ
ップ２として、前記ステップ１で求められたサブゴール
系列から隣接するサブゴールを２つづつ取り出し、その
隣接するサブゴールを包含する空間において前記ステッ
プ１より微少間隔で離散化した格子点を持つ空間（離散
化Ｃ空間）を形成し、グラフ（状態空間）探索手法を用
いて前記隣接サブゴール間の微少間隔格子点列すなわち
局所微少間隔経路を求める処理を繰り返し、前記ステッ
プ２で、サブゴール系列に含まれる隣接サブゴールSg#
m, Sg#n間の局所経路探索に失敗した場合、ステップ１
に戻り（バックトラッキング）、サブゴールSg#mからSg
#nへのアークを経由しない別のサブゴール系列を求め直
し、再びステップ２に移る処理を繰り返し、スタート配
置からゴール配置までの連続した微小間隔経路が求まっ
た場合に、繰り返し処理を終了することを特徴としてい
る。
め、請求項１記載のロボットの大域動作経路計画方法
は、ロボットのスタート配置とゴール配置が与えられた
とき、ロボットとその作業環境にある障害物の幾何学的
形状とそれらの配置を記述する計算機上の幾何学モデル
手段と、モデル同士の干渉を検出する計算機上の干渉検
出手段とを用い、ロボットと障害物が干渉を起こさない
ように２段階の経路探索を行うロボットの大域動作経路
計画方法において、ステップ１として、配置空間（Ｃ空
間）または作業空間を大まかに離散化した格子点（サブ
ゴール候補）を持つ空間（離散化Ｃ空間）を形成し、グ
ラフ（状態空間）探索手法を用いて、スタート配置ある
いはゴール配置から前記格子点を経由してゴール配置あ
るいはスタート配置へ至るサブゴール系列を求め、ステ
ップ２として、前記ステップ１で求められたサブゴール
系列から隣接するサブゴールを２つづつ取り出し、その
隣接するサブゴールを包含する空間において前記ステッ
プ１より微少間隔で離散化した格子点を持つ空間（離散
化Ｃ空間）を形成し、グラフ（状態空間）探索手法を用
いて前記隣接サブゴール間の微少間隔格子点列すなわち
局所微少間隔経路を求める処理を繰り返し、前記ステッ
プ２で、サブゴール系列に含まれる隣接サブゴールSg#
m, Sg#n間の局所経路探索に失敗した場合、ステップ１
に戻り（バックトラッキング）、サブゴールSg#mからSg
#nへのアークを経由しない別のサブゴール系列を求め直
し、再びステップ２に移る処理を繰り返し、スタート配
置からゴール配置までの連続した微小間隔経路が求まっ
た場合に、繰り返し処理を終了することを特徴としてい
る。
【００１２】また、請求項２記載のロボットの大域動作
経路計画方法は、前記ステップ２において、隣接サブゴ
ール間の局所経路を求める際、探索範囲を２つの隣接サ
ブゴールを包含する所定の局所領域に限定することを特
徴としている。また、請求項３記載のロボットの大域動
作経路計画方法は、前記ステップ２において、隣接サブ
ゴールSg#m,Sg#nを結ぶ局所経路を求める際、離散化Ｃ
空間上の２点p,q間の距離をd(p,q)、rPara を１以上の
パラメータとしたとき、探索領域Srng を、 Srng=｛ p｜d(Sg#m,p)＋d(p,Sg#n) <= d(Sg#m,Sg#n)×r
Para ｝ により制限することを特徴としている。また、請求項４
記載のロボットの大域動作経路計画方法は、前記ステッ
プ２において、隣接サブゴール間の局所経路を求める
際、隣接サブゴールを包含する前記探索範囲を動的かつ
多段階に変更することを特徴としている。また、請求項
５記載のロボットの大域動作経路計画方法は、前記ステ
ップ１でサブゴール系列を求める際に、生成された全て
のサブゴールおよびその候補を、重複を省いて計算機上
にグラフ形式（サブゴールグラフ）で記憶し、前記ステ
ップ２の実行結果によってサブゴールグラフのデータを
更新し、前記ステップ１、２ともに最新のサブゴールグ
ラフのデータを反映して、探索を行うことを特徴として
いる。また、請求項６記載のロボットの大域動作経路計
画方法は、前記ステップ１で求められたサブゴール系列
を全てリストに保存しておき、ステップ１のサブゴール
系列探索に成功した場合は、そのサブゴール系列に対し
て前記ステップ２を実行し、ステップ１のサブゴール系
列探索に失敗した場合、サブゴール系列のリストの中か
らサブゴール系列を適当に１つ選び出し、過去ステップ
２において局所経路探索に失敗した隣接サブゴール間に
対して前回よりも前記探索範囲を広くしてステップ２を
実行し、その局所経路探索に成功した場合、それ以降の
未探索の隣接サブゴール間局所経路探索に対しては前記
探索範囲を元に戻すことを特徴としている。
経路計画方法は、前記ステップ２において、隣接サブゴ
ール間の局所経路を求める際、探索範囲を２つの隣接サ
ブゴールを包含する所定の局所領域に限定することを特
徴としている。また、請求項３記載のロボットの大域動
作経路計画方法は、前記ステップ２において、隣接サブ
ゴールSg#m,Sg#nを結ぶ局所経路を求める際、離散化Ｃ
空間上の２点p,q間の距離をd(p,q)、rPara を１以上の
パラメータとしたとき、探索領域Srng を、 Srng=｛ p｜d(Sg#m,p)＋d(p,Sg#n) <= d(Sg#m,Sg#n)×r
Para ｝ により制限することを特徴としている。また、請求項４
記載のロボットの大域動作経路計画方法は、前記ステッ
プ２において、隣接サブゴール間の局所経路を求める
際、隣接サブゴールを包含する前記探索範囲を動的かつ
多段階に変更することを特徴としている。また、請求項
５記載のロボットの大域動作経路計画方法は、前記ステ
ップ１でサブゴール系列を求める際に、生成された全て
のサブゴールおよびその候補を、重複を省いて計算機上
にグラフ形式（サブゴールグラフ）で記憶し、前記ステ
ップ２の実行結果によってサブゴールグラフのデータを
更新し、前記ステップ１、２ともに最新のサブゴールグ
ラフのデータを反映して、探索を行うことを特徴として
いる。また、請求項６記載のロボットの大域動作経路計
画方法は、前記ステップ１で求められたサブゴール系列
を全てリストに保存しておき、ステップ１のサブゴール
系列探索に成功した場合は、そのサブゴール系列に対し
て前記ステップ２を実行し、ステップ１のサブゴール系
列探索に失敗した場合、サブゴール系列のリストの中か
らサブゴール系列を適当に１つ選び出し、過去ステップ
２において局所経路探索に失敗した隣接サブゴール間に
対して前回よりも前記探索範囲を広くしてステップ２を
実行し、その局所経路探索に成功した場合、それ以降の
未探索の隣接サブゴール間局所経路探索に対しては前記
探索範囲を元に戻すことを特徴としている。
【００１３】また、請求項７記載のロボットの大域動作
経路計画方法は、前記ステップ２での各隣接サブゴール
間局所経路探索において、探索した微少離散化間隔の格
子点情報を全て保存し、局所経路探索範囲を段階的に広
げて再探索する（請求項６のの処理）ときに再利用す
ることを特徴としている。また、請求項８記載のロボッ
トの大域動作経路計画方法は、前記ステップ２の各隣接
サブゴール間局所経路探索において、前記請求項２また
は３において制限された探索範囲外にある格子点に対し
てはロボットの可動範囲内にあるかどうかのチェック
（可動範囲チェック）および障害物と干渉するかしない
かのチェック（干渉チェック）を行わず、その探索範囲
外格子点の情報と探索範囲内の格子点情報を区別がつく
ように保存することを特徴としている。また、請求項９
記載のロボットの大域動作経路計画方法は、前記探索範
囲を広げて隣接サブゴール間の微小間隔局所経路を再探
索する（請求項６のの処理）場合に、前回の探索時に
保存しておいた（前回の）探索範囲外格子点に対して可
動範囲チェックおよび干渉チェックを実施し、可動範囲
内および非干渉の格子点を今回の探索開始点の候補とす
ることを特徴としている。また、請求項１０記載のロボ
ットの大域動作経路計画方法は、前記ステップ２の各隣
接サブゴール間局所経路探索において、探索した微少離
散化間隔の前記格子点情報を、ツリー構造を有し１つま
たは複数のリスト形式で保存することを特徴としてい
る。
経路計画方法は、前記ステップ２での各隣接サブゴール
間局所経路探索において、探索した微少離散化間隔の格
子点情報を全て保存し、局所経路探索範囲を段階的に広
げて再探索する（請求項６のの処理）ときに再利用す
ることを特徴としている。また、請求項８記載のロボッ
トの大域動作経路計画方法は、前記ステップ２の各隣接
サブゴール間局所経路探索において、前記請求項２また
は３において制限された探索範囲外にある格子点に対し
てはロボットの可動範囲内にあるかどうかのチェック
（可動範囲チェック）および障害物と干渉するかしない
かのチェック（干渉チェック）を行わず、その探索範囲
外格子点の情報と探索範囲内の格子点情報を区別がつく
ように保存することを特徴としている。また、請求項９
記載のロボットの大域動作経路計画方法は、前記探索範
囲を広げて隣接サブゴール間の微小間隔局所経路を再探
索する（請求項６のの処理）場合に、前回の探索時に
保存しておいた（前回の）探索範囲外格子点に対して可
動範囲チェックおよび干渉チェックを実施し、可動範囲
内および非干渉の格子点を今回の探索開始点の候補とす
ることを特徴としている。また、請求項１０記載のロボ
ットの大域動作経路計画方法は、前記ステップ２の各隣
接サブゴール間局所経路探索において、探索した微少離
散化間隔の前記格子点情報を、ツリー構造を有し１つま
たは複数のリスト形式で保存することを特徴としてい
る。
【００１４】また、請求項１１記載のロボットの大域動
作経路計画方法は、前記ステップ２の各隣接サブゴール
間局所経路探索において、探索した微少離散化間隔の前
記格子点情報を、前記請求項５のサブゴールグラフの該
当するアークに関連付けて保存することを特徴としてい
る。また、請求項１２記載のロボットの大域動作経路計
画方法は、前記ステップ１において、前記請求項５のサ
ブゴールグラフに既に登録済みのサブゴールSg#mを親ノ
ードとして、前記サブゴールグラフに新規サブゴールSg
#nを追加する際、前記サブゴールSg#mから前記サブゴー
ルSg#nへのアークだけでなく、Sg#nからSg#ｍへの反対
方向へのアークも登録することを特徴としている。ま
た、請求項１３記載のロボットの大域動作経路計画方法
は、前記請求項５のサブゴールグラフに登録されたサブ
ゴールSg#mからSg#nへのアークに、過去に反対方向Sg#n
からSg#mに向かって前記ステップ２の局所経路探索を実
行したかどうかを示すフラグを保存し、前記ステップ２
においてSg#mからSg#nへの局所経路探索を行う際、常に
そのフラグをチェックし、フラグが立っていなければ
（過去に反対方向Sg#nからSg#mに局所経路探索を行って
いない）、サブゴールグラフのSg#nからSg#mへのアーク
の該当するフラグを立てSg#mからSg#nへの局所経路探索
を実行し、前記フラグが立っていれば（過去に反対方向
Sg#nからSg#mに局所経路探索を行った）、Sg#nからSg#m
への局所経路探索の全格子点情報を利用して必要であれ
ば（Sg#m,Sg#n間の局所経路がまだ求まっていなけれ
ば）Sg#nからSg#mへの局所経路探索を、前記探索範囲を
広げて行うことを特徴としている。また、請求項１４記
載のロボットの大域動作経路計画方法は、スタート配置
またはゴール配置が変更された場合、前記幾何学モデル
の障害物個数、形状、配置が変更されなければ、前記請
求項５のサブゴールグラフとそのアークに関連付けられ
た前記ステップ２の局所経路探索実行結果に関する全て
の情報を再利用して、ステップ１およびステップ２を実
行することを特徴としている。
作経路計画方法は、前記ステップ２の各隣接サブゴール
間局所経路探索において、探索した微少離散化間隔の前
記格子点情報を、前記請求項５のサブゴールグラフの該
当するアークに関連付けて保存することを特徴としてい
る。また、請求項１２記載のロボットの大域動作経路計
画方法は、前記ステップ１において、前記請求項５のサ
ブゴールグラフに既に登録済みのサブゴールSg#mを親ノ
ードとして、前記サブゴールグラフに新規サブゴールSg
#nを追加する際、前記サブゴールSg#mから前記サブゴー
ルSg#nへのアークだけでなく、Sg#nからSg#ｍへの反対
方向へのアークも登録することを特徴としている。ま
た、請求項１３記載のロボットの大域動作経路計画方法
は、前記請求項５のサブゴールグラフに登録されたサブ
ゴールSg#mからSg#nへのアークに、過去に反対方向Sg#n
からSg#mに向かって前記ステップ２の局所経路探索を実
行したかどうかを示すフラグを保存し、前記ステップ２
においてSg#mからSg#nへの局所経路探索を行う際、常に
そのフラグをチェックし、フラグが立っていなければ
（過去に反対方向Sg#nからSg#mに局所経路探索を行って
いない）、サブゴールグラフのSg#nからSg#mへのアーク
の該当するフラグを立てSg#mからSg#nへの局所経路探索
を実行し、前記フラグが立っていれば（過去に反対方向
Sg#nからSg#mに局所経路探索を行った）、Sg#nからSg#m
への局所経路探索の全格子点情報を利用して必要であれ
ば（Sg#m,Sg#n間の局所経路がまだ求まっていなけれ
ば）Sg#nからSg#mへの局所経路探索を、前記探索範囲を
広げて行うことを特徴としている。また、請求項１４記
載のロボットの大域動作経路計画方法は、スタート配置
またはゴール配置が変更された場合、前記幾何学モデル
の障害物個数、形状、配置が変更されなければ、前記請
求項５のサブゴールグラフとそのアークに関連付けられ
た前記ステップ２の局所経路探索実行結果に関する全て
の情報を再利用して、ステップ１およびステップ２を実
行することを特徴としている。
【００１５】また、請求項１５記載のロボットの大域動
作経路計画方法は、前記サブゴール系列リストからサブ
ゴール系列を１つ選択する際に、最もゴール配置近くま
で前記ステップ２の隣接サブゴール間局所経路探索に成
功し、かつ最もその経路長が短いサブゴール系列から優
先的に選択することを特徴としている。また、請求項１
６記載のロボットの大域動作経路計画方法は、前記ステ
ップ２において、隣接サブゴール間局所経路探索を行う
際、前記探索範囲を予め設定した最大範囲にして探索に
失敗した場合、そのサブゴール系列を前記サブゴール系
列のリストから削除あるいはラベル付けし、以降選択さ
れないようにすることを特徴としている。また、請求項
１７記載のロボットの大域動作経路計画方法は、前記サ
ブゴール系列のリストが空になったあるいはラベル付け
によりサブゴール系列の選択に失敗した場合、サブゴー
ル系列探索のために大まかな間隔で離散化して形成した
前記離散化Ｃ空間の各格子点間を整数個に分割して細か
く離散化した離散化Ｃ空間を形成し直し、その新しい離
散化Ｃ空間においてステップ1 を実行し、以下同様の処
理をスタート配置からゴール配置までの連続した微小間
隔経路が求まるまで、あるいは予め定めた一定時間内で
再帰的に繰り返すことを特徴としている。また、請求項
１８記載のロボットの大域動作経路計画装置は、ロボッ
トとその作業環境の幾何モデルを入力するロボット・環
境モデル入力手段と、前記ロボット・環境モデル入力手
段が出力する前記幾何モデルを記憶する幾何モデル記憶
手段と、前記幾何モデル記憶手段の情報を利用してロボ
ットと環境との干渉を検査する干渉検査手段と、ロボッ
トのスタート配置とゴール配置およびその他パラメータ
を入力するスタート・ゴール配置パラメータ入力手段
と、前記パラメータ入力手段が出力するパラメータを設
定、あるいは探索の状況に応じて変更するパラメータ設
定・変更手段と、前記パラメータ設定・変更手段によっ
て設定されたパラメータを用いて、サブゴール系列探索
のための離散化配置空間を設定する離散化配置空間設定
手段と、前記離散化配置空間設定手段によって定められ
た離散化配置空間において、前記干渉検査手段の干渉検
査結果を利用してサブゴールの系列を探索するサブゴー
ル系列探索手段と、前記サブゴール系列探索手段によっ
て生成されたサブゴールおよびその候補をグラフ形式で
記憶するサブゴールグラフ記憶手段と、前記サブゴール
系列探索手段によって得られたサブゴール系列をリスト
の形式で記憶するサブゴール系列リスト記憶手段と、前
記サブゴール系列リスト記憶手段が記憶している情報か
ら１つのサブゴール系列を選択するサブゴール系列選択
手段と、前記サブゴール系列選択手段によって選ばれた
サブゴール系列を一時的に記憶しておくサブゴール系列
一時記憶手段と、前記サブゴール系列選択手段によって
選ばれ、前記サブゴール系列一時記憶手段に記憶されて
いるサブゴール系列から２つの隣接サブゴールを選択す
る隣接サブゴール選択手段と、前記隣接サブゴール選択
手段から得られる２つの隣接サブゴール間の局所経路を
探索する隣接サブゴール間経路探索手段と、前記隣接サ
ブゴール間経路探索手段が前記経路を探索するときに探
索範囲を制限する探索範囲制限手段と、前記隣接サブゴ
ール間経路探索手段が探索した全格子点データをサブゴ
ールグラフの該当アークに関連付けて保存する探索済み
格子点情報保存手段と、同一の隣接サブゴール間に対し
て前記隣接サブゴール間経路探索を再実行する場合に前
記探索済み格子点情報保存手段が保存した格子点情報を
取得・変換して再利用させる探索済み格子点情報取得・
変換手段と、前記サブゴールグラフ記憶手段に記憶され
ている情報と前記サブゴール系列一時記憶手段に記憶さ
れている情報を受けて、前記スタート配置から前記ゴー
ル配置に至る連続した経路を記憶する経路記憶手段と、
前記経路記憶手段から前記経路を出力する経路出力手段
とを備えたことを特徴としている。
作経路計画方法は、前記サブゴール系列リストからサブ
ゴール系列を１つ選択する際に、最もゴール配置近くま
で前記ステップ２の隣接サブゴール間局所経路探索に成
功し、かつ最もその経路長が短いサブゴール系列から優
先的に選択することを特徴としている。また、請求項１
６記載のロボットの大域動作経路計画方法は、前記ステ
ップ２において、隣接サブゴール間局所経路探索を行う
際、前記探索範囲を予め設定した最大範囲にして探索に
失敗した場合、そのサブゴール系列を前記サブゴール系
列のリストから削除あるいはラベル付けし、以降選択さ
れないようにすることを特徴としている。また、請求項
１７記載のロボットの大域動作経路計画方法は、前記サ
ブゴール系列のリストが空になったあるいはラベル付け
によりサブゴール系列の選択に失敗した場合、サブゴー
ル系列探索のために大まかな間隔で離散化して形成した
前記離散化Ｃ空間の各格子点間を整数個に分割して細か
く離散化した離散化Ｃ空間を形成し直し、その新しい離
散化Ｃ空間においてステップ1 を実行し、以下同様の処
理をスタート配置からゴール配置までの連続した微小間
隔経路が求まるまで、あるいは予め定めた一定時間内で
再帰的に繰り返すことを特徴としている。また、請求項
１８記載のロボットの大域動作経路計画装置は、ロボッ
トとその作業環境の幾何モデルを入力するロボット・環
境モデル入力手段と、前記ロボット・環境モデル入力手
段が出力する前記幾何モデルを記憶する幾何モデル記憶
手段と、前記幾何モデル記憶手段の情報を利用してロボ
ットと環境との干渉を検査する干渉検査手段と、ロボッ
トのスタート配置とゴール配置およびその他パラメータ
を入力するスタート・ゴール配置パラメータ入力手段
と、前記パラメータ入力手段が出力するパラメータを設
定、あるいは探索の状況に応じて変更するパラメータ設
定・変更手段と、前記パラメータ設定・変更手段によっ
て設定されたパラメータを用いて、サブゴール系列探索
のための離散化配置空間を設定する離散化配置空間設定
手段と、前記離散化配置空間設定手段によって定められ
た離散化配置空間において、前記干渉検査手段の干渉検
査結果を利用してサブゴールの系列を探索するサブゴー
ル系列探索手段と、前記サブゴール系列探索手段によっ
て生成されたサブゴールおよびその候補をグラフ形式で
記憶するサブゴールグラフ記憶手段と、前記サブゴール
系列探索手段によって得られたサブゴール系列をリスト
の形式で記憶するサブゴール系列リスト記憶手段と、前
記サブゴール系列リスト記憶手段が記憶している情報か
ら１つのサブゴール系列を選択するサブゴール系列選択
手段と、前記サブゴール系列選択手段によって選ばれた
サブゴール系列を一時的に記憶しておくサブゴール系列
一時記憶手段と、前記サブゴール系列選択手段によって
選ばれ、前記サブゴール系列一時記憶手段に記憶されて
いるサブゴール系列から２つの隣接サブゴールを選択す
る隣接サブゴール選択手段と、前記隣接サブゴール選択
手段から得られる２つの隣接サブゴール間の局所経路を
探索する隣接サブゴール間経路探索手段と、前記隣接サ
ブゴール間経路探索手段が前記経路を探索するときに探
索範囲を制限する探索範囲制限手段と、前記隣接サブゴ
ール間経路探索手段が探索した全格子点データをサブゴ
ールグラフの該当アークに関連付けて保存する探索済み
格子点情報保存手段と、同一の隣接サブゴール間に対し
て前記隣接サブゴール間経路探索を再実行する場合に前
記探索済み格子点情報保存手段が保存した格子点情報を
取得・変換して再利用させる探索済み格子点情報取得・
変換手段と、前記サブゴールグラフ記憶手段に記憶され
ている情報と前記サブゴール系列一時記憶手段に記憶さ
れている情報を受けて、前記スタート配置から前記ゴー
ル配置に至る連続した経路を記憶する経路記憶手段と、
前記経路記憶手段から前記経路を出力する経路出力手段
とを備えたことを特徴としている。
【００１６】このロボットの大域動作経路計画方法とそ
の制御装置では、基本的に次のようなステップ１とステ
ップ２による２段階経路探索が実行される。 （１）、先ず、ステップ１の処理として、配置空間（Ｃ
空間）または作業空間を大まかに離散化した格子点（サ
ブゴール候補、例えば、パラメータdisParaによる）を
持つ空間（離散化Ｃ空間）を形成し、グラフ（状態空
間）探索手法を用いて、スタート配置あるいはゴール配
置から格子点を経由してゴール配置あるいはスタート配
置へ至るサブゴール系列を求める。また、ステップ１で
探索されたサブゴールおよびその候補に関する情報は重
複を省いてグラフ形式（サブゴールグラフ）で保存され
る。 （２）、次に、ステップ２の処理として、ステップ１の
処理で求めたサブゴール系列から隣接するサブゴールを
２つづつ取出し、ステップ１のパラメータdisParaより
も微小間隔で離散化した格子点を持つ空間（離散化Ｃ空
間）を形成し、グラフ（状態空間）探索手法を用いて隣
接サブゴール間の微小間隔格子点列の局所微小間隔経路
を求める処理を繰り返し行う。また、隣接サブゴール間
局所経路探索の結果（成功または失敗）と探索した全て
の格子点情報は、サブゴールグラフの該当するアークに
関連付けられて保存される。 （３）、ステップ２の探索が失敗した場合の処理は、ス
テップ２でサブゴール系列に含まれる隣接サブゴールを
Sg#m，Sg#nとし、Sg#mからSg#n間の局所経路探索に失敗
したとすると、バックトラッキング及び再探索を次のよ
うに実施する、ステップ１に戻り、サブゴールSg#mから
Sg#nへのアークを経由しない別のサブゴール系列を求め
直し、再び、ステップ２に移る処理を繰り返し、スター
ト配置からゴール配置までの連続した微小間隔経路が求
められた場合に、繰り返し処理を終了する。これは、常
に最新のサブゴールグラフの情報を参照することにより
可能となる。 （４）、ステップ１の探索が成功の場合は、求めたサブ
ゴール系列を全てリストに保存し、そのサブゴール系列
全てについてステップ２を実行する。その場合、隣接サ
ブゴール間局所経路探索範囲を指定するパラメータはrP
ara=1（最小探索範囲）を用いる。また、ステップ１の
探索が失敗した場合は、サブゴール系列リストからサブ
ゴール系列を適当に１つ選び出し、過去ステップ２で局
所経路探索に失敗した隣接サブゴール間に対して前回よ
りも探索範囲を１段階広く（例えば、パラメータをrPar
a[i+1]>rPara[i]>...>rPara[0]=1として）してステップ
２を実行し、局所経路探索に成功したらパラメータを元
に戻して続行する。隣接サブゴール間局所経路を再探索
する場合は、サブゴールグラフの該当アークに関連付け
て保存しておいた前回探索時の探索済み全格子点情報を
再利用して探索効率の低下を防ぐ。
の制御装置では、基本的に次のようなステップ１とステ
ップ２による２段階経路探索が実行される。 （１）、先ず、ステップ１の処理として、配置空間（Ｃ
空間）または作業空間を大まかに離散化した格子点（サ
ブゴール候補、例えば、パラメータdisParaによる）を
持つ空間（離散化Ｃ空間）を形成し、グラフ（状態空
間）探索手法を用いて、スタート配置あるいはゴール配
置から格子点を経由してゴール配置あるいはスタート配
置へ至るサブゴール系列を求める。また、ステップ１で
探索されたサブゴールおよびその候補に関する情報は重
複を省いてグラフ形式（サブゴールグラフ）で保存され
る。 （２）、次に、ステップ２の処理として、ステップ１の
処理で求めたサブゴール系列から隣接するサブゴールを
２つづつ取出し、ステップ１のパラメータdisParaより
も微小間隔で離散化した格子点を持つ空間（離散化Ｃ空
間）を形成し、グラフ（状態空間）探索手法を用いて隣
接サブゴール間の微小間隔格子点列の局所微小間隔経路
を求める処理を繰り返し行う。また、隣接サブゴール間
局所経路探索の結果（成功または失敗）と探索した全て
の格子点情報は、サブゴールグラフの該当するアークに
関連付けられて保存される。 （３）、ステップ２の探索が失敗した場合の処理は、ス
テップ２でサブゴール系列に含まれる隣接サブゴールを
Sg#m，Sg#nとし、Sg#mからSg#n間の局所経路探索に失敗
したとすると、バックトラッキング及び再探索を次のよ
うに実施する、ステップ１に戻り、サブゴールSg#mから
Sg#nへのアークを経由しない別のサブゴール系列を求め
直し、再び、ステップ２に移る処理を繰り返し、スター
ト配置からゴール配置までの連続した微小間隔経路が求
められた場合に、繰り返し処理を終了する。これは、常
に最新のサブゴールグラフの情報を参照することにより
可能となる。 （４）、ステップ１の探索が成功の場合は、求めたサブ
ゴール系列を全てリストに保存し、そのサブゴール系列
全てについてステップ２を実行する。その場合、隣接サ
ブゴール間局所経路探索範囲を指定するパラメータはrP
ara=1（最小探索範囲）を用いる。また、ステップ１の
探索が失敗した場合は、サブゴール系列リストからサブ
ゴール系列を適当に１つ選び出し、過去ステップ２で局
所経路探索に失敗した隣接サブゴール間に対して前回よ
りも探索範囲を１段階広く（例えば、パラメータをrPar
a[i+1]>rPara[i]>...>rPara[0]=1として）してステップ
２を実行し、局所経路探索に成功したらパラメータを元
に戻して続行する。隣接サブゴール間局所経路を再探索
する場合は、サブゴールグラフの該当アークに関連付け
て保存しておいた前回探索時の探索済み全格子点情報を
再利用して探索効率の低下を防ぐ。
【００１７】（５）、先のサブゴール系列リストからの
サブゴール系列の選択ができなくなったら、大まかなス
テップ１の間隔（パラメータdisPara）を更に整数個に
分割して離散化Ｃ空間を形成し直してステップ１から実
行し直すようにする。以上のように構成することによっ
て、経路を探索する際の探索空間が大幅に抑制され、探
索効率が向上して、３次元空間内における自由度の大き
い多関節マニピュレータに対しても、経路を高速で求め
ることが可能になる。また、ステップ１とステップ２の
バックトラッキング、再探索等が繰返し実行され、か
つ、局所経路探索の探索範囲をrParaの切換え等により
動的に変更することによって、従来方式に比較すると経
路探索が充分に尽くされ、存在する経路を見落とす可能
性も小さくなる。さらに、サブゴールグラフは、隣接サ
ブゴール間局所経路探索の実行結果と探索済み全格子点
の情報を該当アークに保存し、その情報を両方向から検
索可能である。そのため、探索情報の再利用性に優れ、
経路探索がさらに高速化される。
サブゴール系列の選択ができなくなったら、大まかなス
テップ１の間隔（パラメータdisPara）を更に整数個に
分割して離散化Ｃ空間を形成し直してステップ１から実
行し直すようにする。以上のように構成することによっ
て、経路を探索する際の探索空間が大幅に抑制され、探
索効率が向上して、３次元空間内における自由度の大き
い多関節マニピュレータに対しても、経路を高速で求め
ることが可能になる。また、ステップ１とステップ２の
バックトラッキング、再探索等が繰返し実行され、か
つ、局所経路探索の探索範囲をrParaの切換え等により
動的に変更することによって、従来方式に比較すると経
路探索が充分に尽くされ、存在する経路を見落とす可能
性も小さくなる。さらに、サブゴールグラフは、隣接サ
ブゴール間局所経路探索の実行結果と探索済み全格子点
の情報を該当アークに保存し、その情報を両方向から検
索可能である。そのため、探索情報の再利用性に優れ、
経路探索がさらに高速化される。
【００１８】また、請求項１９に記載のロボットの大域
動作経路計画制御装置は、複数の関節軸自由度を有する
ロボットにおいて、ロボットの大域動作経路計画を行な
う場合に、Ａ軸群探索値決定手段として請求項１〜18に
記載されたいずれか、またはいくつかの組み合わせの探
索法を用いて探索する軸群（A軸群）とその他の軸群（B
軸群）とを区別する区別手段を有し、B軸群について
は、ロボットのスタート配置からゴール配置までの途中
の中間値を探索処理毎に一定のルールによって逐次演算
するB軸群チェック値演算手段を有し、サブゴール探索
時にＡ軸群探索値決定手段で得たA軸群の各軸の探索値
と、B軸群チェック値演算手段によって得たB軸群の各軸
のチェック値とを用いてロボットの幾何学モデルを求
め、障害物との干渉を検出することを特徴とする。請求
項１９に記載のロボットの大域動作経路計画装置によれ
ば、前記B軸群の軸に対しては、一定のルールに基づ
き、チェック値が一意に導出されるため、探索アルゴリ
ズムを必要としない。このため探索手法の適用はA軸群
だけになり、全ての軸に探索アルゴリズムを適用した場
合に比べ、大幅に経路探索時間を短縮できるという効果
がある。また、請求項２０に記載のロボットの大域動作
経路計画装置は、請求項１９に記載したB軸群チェック
値演算手段として、予めロボットのスタート配置からゴ
ール配置までのA軸群の各軸の移動量が最大の軸（M軸）
の移動量（M#DLT）を求め、探索処理時にはM軸の探索値
からM軸のスタート配置を差し引いた値（M#VAL）とM#DL
Tの比（M#VAL／M#DLT）をロボットのスタート配置から
ゴール配置までのB軸群の各軸の移動量に乗算し、B軸群
の各軸のチェック値とすることを特徴としている。請求
項２０に記載のロボットの大域動作経路計画装置によれ
ば、簡単な四則演算でB軸群のチェック値を算出するこ
とができ、経路探索時間を短縮できる効果がある。ま
た、請求項２１記載の発明は、請求項1〜１７のいずれ
か１項記載のロボットの大域動作経路計画方法における
前記ステップ１およびステップ２において、ロボットと
その作業環境にある障害物の幾何学的形状とそれらの配
置を記述する計算機上の幾何学モデル手段と、モデル同
士の干渉を検出する計算機上の干渉検出手段とを用い
て、検査対象である離散化配置空間上の格子点関節角座
標値に対するロボットと障害物との干渉を検査する際
に、ロボットと障害物が干渉する場合は、ロボットのど
の部位が干渉するかという情報を前記干渉検出手段から
受取り、前記ロボット部位と関連付けて干渉を起こすロ
ボットの前記関節角座標値を保存することを特徴とす
る。請求項２２記載の発明は、請求項１８又は１９記載
のロボットの大域動作経路計画制御装置において、 ロ
ボットとその作業環境にある障害物の幾何学的形状とそ
れらの配置を記述する計算機上の幾何学モデル手段と、
モデル同士の干渉を検出する計算機上の干渉検出手段
と、検査対象である離散化配置空間上の格子点関節角座
標値に対するロボットと障害物との干渉を検査する際
に、ロボットと障害物が干渉する場合は、ロボットのど
の部位が干渉するかという情報を前記干渉検出手段から
受取り、前記ロボット部位と関連付けて干渉を起こすロ
ボットの前記関節角座標値を保存するロボットの関節角
座標値保存手段と、を備えたことを特徴とする。請求項
２３記載の発明は、請求項２１記載のロボットの大域動
作経路計画方法における前記ステップ１およびステップ
２において、前記幾何学モデル手段と、前記モデル同士
の干渉検出手段とを用いて、検査対象である離散化配置
空間上の格子点関節角座標値に対するロボットと障害物
との干渉を検査する前に、ロボット部位に関連付けて保
存された干渉を起こす複数のロボットの関節角座標値と
検査対象である関節角座標値を比較し、前記複数のロボ
ットの関節角座標値のうち少なくとも１つが検査対象で
ある関節角座標値に一致する場合は、前記幾何学モデル
手段と前記モデル同士の干渉検出手段を用いた干渉検査
を行なわずに、ロボットと障害物が干渉すると判断する
ことを特徴とする。請求項２４記載の発明は、請求項２
２記載のロボットの大域動作経路計画制御装置におい
て、前記幾何学モデル手段と、前記モデル同士の干渉検
出手段とを用いて、検査対象である離散化配置空間上の
格子点関節角座標値に対するロボットと障害物との干渉
を検査する前に、ロボット部位に関連付けて保存された
干渉を起こす複数のロボットの関節角座標値と検査対象
である関節角座標値を比較し、前記複数のロボットの関
節角座標値のうち少なくとも１つが検査対象である関節
角座標値に一致する場合は、前記幾何学モデル手段と前
記モデル同士の干渉検出手段を用いた干渉検査を行なわ
ずに、ロボットと障害物が干渉すると判断することを特
徴とする。請求項２１〜２４記載の発明によれば、前記
ステップ１およびステップ２において、ロボットとその
作業環境にある障害物の幾何学的形状とそれらの配置を
記述する計算機上の幾何学モデル手段と、モデル同士の
干渉を検出する計算機上の干渉検出手段とを用いて、検
査対象である離散化配置空間上の格子点関節角座標値に
対するロボットと障害物との干渉を検査する際に、ロボ
ットのどの部位が障害物と干渉したかという情報を干渉
検出手段から受取り、そのロボット部位と関連付けて関
節角座標値を記憶し、別の格子点関節角座標値に対する
ロボットと障害物との干渉を検査する際に、その格子点
関節角座標値を記憶した関節角座標値の情報と照らし合
わせることにより、幾何学モデル手段とモデル同士の干
渉検出手段を用いた計算量の多い干渉検査の回数を大幅
に減少できるため、探索がさらに高速化される。請求項
２５記載の発明は、請求項１〜１７、２１，２３のいず
れか１項記載のロボットの大域動作経路計画方法におい
て、ロボットを駆動軸とそれに付随して運動する機構に
基づいて複数サブロボットへ分割し、各サブロボットの
経路を、ベース部のサブロボットから先端のサブロボッ
トに向かって順番に、１つ手前までのサブロボットの探
索済み経路に拘束された空間上で探索し、あるサブロボ
ットの経路探索に失敗した場合に、１つ手前のサブロボ
ットの経路を再探索してロボット全体経路を求めるシー
ケンシャル探索において、サブロボットの自由度を１あ
るいは２以上とし、各サブロボットの経路を前記ステッ
プ１および前記ステップ２に基く２段階経路探索により
求めることを特徴とする。請求項２６記載の発明は、請
求項１８〜２０、２２記載のロボットの大域動作経路計
画制御装置において、ロボットを駆動軸とそれに付随し
て運動する機構に基づいて複数サブロボットへ分割し、
各サブロボットの経路を、ベース部のサブロボットから
先端のサブロボットに向かって順番に、１つ手前までの
サブロボットの探索済み経路に拘束された空間上で探索
し、あるサブロボットの経路探索に失敗した場合に、１
つ手前のサブロボットの経路を再探索してロボット全体
経路を求めるシーケンシャル探索において、サブロボッ
トの自由度を１あるいは２以上とし、各サブロボットの
経路を前記ステップ１および前記ステップ２に基く２段
階経路探索により求めることを特徴とする。請求項２７
記載の発明は、請求項２５記載のロボットの大域動作経
路計画方法において、前記シーケンシャル探索における
各サブロボットの２段階経路探索において、サブロボッ
トをロボットのベース部に近い駆動軸に対応するサブロ
ボットから0，1，2と番号付けしたとき、サブロボットk
での２段階経路探索に失敗した場合に、サブロボットk-
1のサブゴールグラフのどのサブゴールあるいはサブゴ
ール間アークが原因でサブロボットkの２段階経路探索
に失敗したかを推定し、サブロボットk-1の該当するサ
ブゴールあるいはサブゴール間アークに重み付けをし、
サブロボットk-1の２段階経路探索に後戻りした際に、
その重み付けの効果によって前回とは別のサブゴール系
列および隣接サブゴール間局所経路を探索することを特
徴とする。請求項２８記載の発明は、請求項２６記載の
ロボットの大域動作経路計画制御装置において、前記シ
ーケンシャル探索における各サブロボットの２段階経路
探索において、サブロボットをロボットのベース部に近
い駆動軸に対応するサブロボットから0，1，2と番号付
けしたとき、サブロボットkでの２段階経路探索に失敗
した場合に、サブロボットk-1のサブゴールグラフのど
のサブゴールあるいはサブゴール間アークが原因でサブ
ロボットkの２段階経路探索に失敗したかを推定し、サ
ブロボットk-1の該当するサブゴールあるいはサブゴー
ル間アークに重み付けをし、サブロボットk-1の２段階
経路探索に後戻りした際に、その重み付けの効果によっ
て前回とは別のサブゴール系列および隣接サブゴール間
局所経路を探索することを特徴とする。請求項２９記載
の発明は、請求項２７記載のロボットの大域動作経路計
画方法において、前記重み付けが、同一のサブゴールあ
るいはサブゴール間アークに対して複数回行われる際
に、サブゴールあるいはサブゴール間アークへの重みを
前記回数分だけ加算あるいは積算することを特徴とす
る。請求項３０記載の発明は、請求項２８記載のロボッ
トの大域動作経路計画制御装置において、前記重み付け
が、同一のサブゴールあるいはサブゴール間アークに対
して複数回行われる際に、サブゴールあるいはサブゴー
ル間アークへの重みを前記回数分だけ加算あるいは積算
することを特徴とする。請求項２５〜３０記載の発明に
よれば、シーケンシャル探索において、サブロボットの
自由度を１あるいは２以上とし、各サブロボットの経路
を前記ステップ１および前記ステップ２に基く２段階経
路探索により求めるので、経路の質が良く、経路探索が
さらに高速化される。また、サブロボットkからサブロ
ボットk-1にバックトラッキングする際の前処理とし
て、サブロボットk-1のサブゴールグラフのどのサブゴ
ールあるいはサブゴール間アークが原因でサブロボット
kの２段階経路探索に失敗したかを推定し、サブロボッ
トk-1の該当するサブゴールあるいはサブゴール間アー
クに重み付けをし、その重み付けの効果によって前回と
は別のサブゴール系列および隣接サブゴール間局所経路
を探索し、前記重み付けが、同一のサブゴールあるいは
サブゴール間アークに対して複数回行われる際に、サブ
ゴールあるいはサブゴール間アークへの重みを前記回数
分だけ加算あるいは積算するので、再探索回数が増加し
ても経路探索に失敗しにくく、経路の見落としが防止さ
れる。
動作経路計画制御装置は、複数の関節軸自由度を有する
ロボットにおいて、ロボットの大域動作経路計画を行な
う場合に、Ａ軸群探索値決定手段として請求項１〜18に
記載されたいずれか、またはいくつかの組み合わせの探
索法を用いて探索する軸群（A軸群）とその他の軸群（B
軸群）とを区別する区別手段を有し、B軸群について
は、ロボットのスタート配置からゴール配置までの途中
の中間値を探索処理毎に一定のルールによって逐次演算
するB軸群チェック値演算手段を有し、サブゴール探索
時にＡ軸群探索値決定手段で得たA軸群の各軸の探索値
と、B軸群チェック値演算手段によって得たB軸群の各軸
のチェック値とを用いてロボットの幾何学モデルを求
め、障害物との干渉を検出することを特徴とする。請求
項１９に記載のロボットの大域動作経路計画装置によれ
ば、前記B軸群の軸に対しては、一定のルールに基づ
き、チェック値が一意に導出されるため、探索アルゴリ
ズムを必要としない。このため探索手法の適用はA軸群
だけになり、全ての軸に探索アルゴリズムを適用した場
合に比べ、大幅に経路探索時間を短縮できるという効果
がある。また、請求項２０に記載のロボットの大域動作
経路計画装置は、請求項１９に記載したB軸群チェック
値演算手段として、予めロボットのスタート配置からゴ
ール配置までのA軸群の各軸の移動量が最大の軸（M軸）
の移動量（M#DLT）を求め、探索処理時にはM軸の探索値
からM軸のスタート配置を差し引いた値（M#VAL）とM#DL
Tの比（M#VAL／M#DLT）をロボットのスタート配置から
ゴール配置までのB軸群の各軸の移動量に乗算し、B軸群
の各軸のチェック値とすることを特徴としている。請求
項２０に記載のロボットの大域動作経路計画装置によれ
ば、簡単な四則演算でB軸群のチェック値を算出するこ
とができ、経路探索時間を短縮できる効果がある。ま
た、請求項２１記載の発明は、請求項1〜１７のいずれ
か１項記載のロボットの大域動作経路計画方法における
前記ステップ１およびステップ２において、ロボットと
その作業環境にある障害物の幾何学的形状とそれらの配
置を記述する計算機上の幾何学モデル手段と、モデル同
士の干渉を検出する計算機上の干渉検出手段とを用い
て、検査対象である離散化配置空間上の格子点関節角座
標値に対するロボットと障害物との干渉を検査する際
に、ロボットと障害物が干渉する場合は、ロボットのど
の部位が干渉するかという情報を前記干渉検出手段から
受取り、前記ロボット部位と関連付けて干渉を起こすロ
ボットの前記関節角座標値を保存することを特徴とす
る。請求項２２記載の発明は、請求項１８又は１９記載
のロボットの大域動作経路計画制御装置において、 ロ
ボットとその作業環境にある障害物の幾何学的形状とそ
れらの配置を記述する計算機上の幾何学モデル手段と、
モデル同士の干渉を検出する計算機上の干渉検出手段
と、検査対象である離散化配置空間上の格子点関節角座
標値に対するロボットと障害物との干渉を検査する際
に、ロボットと障害物が干渉する場合は、ロボットのど
の部位が干渉するかという情報を前記干渉検出手段から
受取り、前記ロボット部位と関連付けて干渉を起こすロ
ボットの前記関節角座標値を保存するロボットの関節角
座標値保存手段と、を備えたことを特徴とする。請求項
２３記載の発明は、請求項２１記載のロボットの大域動
作経路計画方法における前記ステップ１およびステップ
２において、前記幾何学モデル手段と、前記モデル同士
の干渉検出手段とを用いて、検査対象である離散化配置
空間上の格子点関節角座標値に対するロボットと障害物
との干渉を検査する前に、ロボット部位に関連付けて保
存された干渉を起こす複数のロボットの関節角座標値と
検査対象である関節角座標値を比較し、前記複数のロボ
ットの関節角座標値のうち少なくとも１つが検査対象で
ある関節角座標値に一致する場合は、前記幾何学モデル
手段と前記モデル同士の干渉検出手段を用いた干渉検査
を行なわずに、ロボットと障害物が干渉すると判断する
ことを特徴とする。請求項２４記載の発明は、請求項２
２記載のロボットの大域動作経路計画制御装置におい
て、前記幾何学モデル手段と、前記モデル同士の干渉検
出手段とを用いて、検査対象である離散化配置空間上の
格子点関節角座標値に対するロボットと障害物との干渉
を検査する前に、ロボット部位に関連付けて保存された
干渉を起こす複数のロボットの関節角座標値と検査対象
である関節角座標値を比較し、前記複数のロボットの関
節角座標値のうち少なくとも１つが検査対象である関節
角座標値に一致する場合は、前記幾何学モデル手段と前
記モデル同士の干渉検出手段を用いた干渉検査を行なわ
ずに、ロボットと障害物が干渉すると判断することを特
徴とする。請求項２１〜２４記載の発明によれば、前記
ステップ１およびステップ２において、ロボットとその
作業環境にある障害物の幾何学的形状とそれらの配置を
記述する計算機上の幾何学モデル手段と、モデル同士の
干渉を検出する計算機上の干渉検出手段とを用いて、検
査対象である離散化配置空間上の格子点関節角座標値に
対するロボットと障害物との干渉を検査する際に、ロボ
ットのどの部位が障害物と干渉したかという情報を干渉
検出手段から受取り、そのロボット部位と関連付けて関
節角座標値を記憶し、別の格子点関節角座標値に対する
ロボットと障害物との干渉を検査する際に、その格子点
関節角座標値を記憶した関節角座標値の情報と照らし合
わせることにより、幾何学モデル手段とモデル同士の干
渉検出手段を用いた計算量の多い干渉検査の回数を大幅
に減少できるため、探索がさらに高速化される。請求項
２５記載の発明は、請求項１〜１７、２１，２３のいず
れか１項記載のロボットの大域動作経路計画方法におい
て、ロボットを駆動軸とそれに付随して運動する機構に
基づいて複数サブロボットへ分割し、各サブロボットの
経路を、ベース部のサブロボットから先端のサブロボッ
トに向かって順番に、１つ手前までのサブロボットの探
索済み経路に拘束された空間上で探索し、あるサブロボ
ットの経路探索に失敗した場合に、１つ手前のサブロボ
ットの経路を再探索してロボット全体経路を求めるシー
ケンシャル探索において、サブロボットの自由度を１あ
るいは２以上とし、各サブロボットの経路を前記ステッ
プ１および前記ステップ２に基く２段階経路探索により
求めることを特徴とする。請求項２６記載の発明は、請
求項１８〜２０、２２記載のロボットの大域動作経路計
画制御装置において、ロボットを駆動軸とそれに付随し
て運動する機構に基づいて複数サブロボットへ分割し、
各サブロボットの経路を、ベース部のサブロボットから
先端のサブロボットに向かって順番に、１つ手前までの
サブロボットの探索済み経路に拘束された空間上で探索
し、あるサブロボットの経路探索に失敗した場合に、１
つ手前のサブロボットの経路を再探索してロボット全体
経路を求めるシーケンシャル探索において、サブロボッ
トの自由度を１あるいは２以上とし、各サブロボットの
経路を前記ステップ１および前記ステップ２に基く２段
階経路探索により求めることを特徴とする。請求項２７
記載の発明は、請求項２５記載のロボットの大域動作経
路計画方法において、前記シーケンシャル探索における
各サブロボットの２段階経路探索において、サブロボッ
トをロボットのベース部に近い駆動軸に対応するサブロ
ボットから0，1，2と番号付けしたとき、サブロボットk
での２段階経路探索に失敗した場合に、サブロボットk-
1のサブゴールグラフのどのサブゴールあるいはサブゴ
ール間アークが原因でサブロボットkの２段階経路探索
に失敗したかを推定し、サブロボットk-1の該当するサ
ブゴールあるいはサブゴール間アークに重み付けをし、
サブロボットk-1の２段階経路探索に後戻りした際に、
その重み付けの効果によって前回とは別のサブゴール系
列および隣接サブゴール間局所経路を探索することを特
徴とする。請求項２８記載の発明は、請求項２６記載の
ロボットの大域動作経路計画制御装置において、前記シ
ーケンシャル探索における各サブロボットの２段階経路
探索において、サブロボットをロボットのベース部に近
い駆動軸に対応するサブロボットから0，1，2と番号付
けしたとき、サブロボットkでの２段階経路探索に失敗
した場合に、サブロボットk-1のサブゴールグラフのど
のサブゴールあるいはサブゴール間アークが原因でサブ
ロボットkの２段階経路探索に失敗したかを推定し、サ
ブロボットk-1の該当するサブゴールあるいはサブゴー
ル間アークに重み付けをし、サブロボットk-1の２段階
経路探索に後戻りした際に、その重み付けの効果によっ
て前回とは別のサブゴール系列および隣接サブゴール間
局所経路を探索することを特徴とする。請求項２９記載
の発明は、請求項２７記載のロボットの大域動作経路計
画方法において、前記重み付けが、同一のサブゴールあ
るいはサブゴール間アークに対して複数回行われる際
に、サブゴールあるいはサブゴール間アークへの重みを
前記回数分だけ加算あるいは積算することを特徴とす
る。請求項３０記載の発明は、請求項２８記載のロボッ
トの大域動作経路計画制御装置において、前記重み付け
が、同一のサブゴールあるいはサブゴール間アークに対
して複数回行われる際に、サブゴールあるいはサブゴー
ル間アークへの重みを前記回数分だけ加算あるいは積算
することを特徴とする。請求項２５〜３０記載の発明に
よれば、シーケンシャル探索において、サブロボットの
自由度を１あるいは２以上とし、各サブロボットの経路
を前記ステップ１および前記ステップ２に基く２段階経
路探索により求めるので、経路の質が良く、経路探索が
さらに高速化される。また、サブロボットkからサブロ
ボットk-1にバックトラッキングする際の前処理とし
て、サブロボットk-1のサブゴールグラフのどのサブゴ
ールあるいはサブゴール間アークが原因でサブロボット
kの２段階経路探索に失敗したかを推定し、サブロボッ
トk-1の該当するサブゴールあるいはサブゴール間アー
クに重み付けをし、その重み付けの効果によって前回と
は別のサブゴール系列および隣接サブゴール間局所経路
を探索し、前記重み付けが、同一のサブゴールあるいは
サブゴール間アークに対して複数回行われる際に、サブ
ゴールあるいはサブゴール間アークへの重みを前記回数
分だけ加算あるいは積算するので、再探索回数が増加し
ても経路探索に失敗しにくく、経路の見落としが防止さ
れる。
【００１９】
【発明の実施の形態】以下、本発明の実施の形態につい
て図を参照して説明する。本発明のロボットの大域動作
経路計画方法は、図２５および図２６のようなロボット
と作業環境の幾何学情報、スタート配置S、ゴール配置G
が与えられたとき、図２７のようなＣ空間においてスタ
ート配置Sからゴール配置Gへ至る非干渉経路を計画する
方法である。以降、簡単のため２次元Ｃ空間を例にして
説明を進めるが、本発明はもちろんこれに限定されるも
のではなく、３次元以上のＣ空間に対してもそのまま適
用できることはいうまでもない。スタート配置S、ゴー
ル配置Gは、図２９に示した微小間隔で離散化されたＣ
空間(d-q1,d-q2)上の座標値で与えられるものとする。
又、図２９の離散化Ｃ空間を、以降、d-C-Hres（discre
te C-space with High resolutionの意味）と表す。
て図を参照して説明する。本発明のロボットの大域動作
経路計画方法は、図２５および図２６のようなロボット
と作業環境の幾何学情報、スタート配置S、ゴール配置G
が与えられたとき、図２７のようなＣ空間においてスタ
ート配置Sからゴール配置Gへ至る非干渉経路を計画する
方法である。以降、簡単のため２次元Ｃ空間を例にして
説明を進めるが、本発明はもちろんこれに限定されるも
のではなく、３次元以上のＣ空間に対してもそのまま適
用できることはいうまでもない。スタート配置S、ゴー
ル配置Gは、図２９に示した微小間隔で離散化されたＣ
空間(d-q1,d-q2)上の座標値で与えられるものとする。
又、図２９の離散化Ｃ空間を、以降、d-C-Hres（discre
te C-space with High resolutionの意味）と表す。
【００２０】図１は本発明の第１の実施の形態に係るロ
ボットの大域動作経路計画方法の処理のフローチャート
である。本発明方法の処理の主要部分は、サブゴール系
列探索（STEP1）を行う処理（S104）、STEP1で求まった
サブゴール系列をサブゴール系列リストに保存する処理
（S105）、サブゴール系列リストからサブゴール系列を
１つ選択する処理（S106）、および、選択されたサブゴ
ール系列に対して各隣接サブゴール間の局所経路探索を
行う処理（S107）で構成される繰返し処理（ループ）で
ある。以下、先ず、本発明方法の手順の概要を図１に示
したフローチャートに沿って説明する。先ず、d-C-Hres
の離散化Ｃ空間の設定と（S100）、スタート配置S、ゴ
ール配置Gを入力した後（S101）、大まかな離散化刻み
幅、disPara（図２のS203）、を設定し（S102）、図２
および図３のように離散化Ｃ空間(d-q1',d-q2')を形成
する（S103）。図２〜図５は図１のS102およびS103に示
す離散化Ｃ空間構成法とS104に示すサブゴール系列探索
の結果を示す図である。図２および図３の203a、203bに
示したように、disParaはd-C-Hresの刻み幅に比べて十
分大きな値とする。離散化Ｃ空間(d-q1',d-q2')は大ま
かな間隔で離散化されたＣ空間であり、d-C-Lresと表す
（discrete C-space with Low resolutionの意味）。図
２ではd-C-Lresのスタート配置S（201）、ゴール配置G
（202）の座標値はそれぞれ(0,0), (5,0)となる。同様
に図３ではそれぞれ(0,0),(4,3)となる。本発明の方法
によれば、この例のようにスタート配置を離散化Ｃ空間
d-C-Lresの原点とする必要はなく、スタートおよびゴー
ル配置がd-C-Lresの格子点上にある必要もない。また、
座標軸が直交している必要もない。さらには、(d-q1,d-
q2)から(d-q1',d-q2')に非線形変換しても良く、d-C-Lr
esの定義の仕方は任意である。
ボットの大域動作経路計画方法の処理のフローチャート
である。本発明方法の処理の主要部分は、サブゴール系
列探索（STEP1）を行う処理（S104）、STEP1で求まった
サブゴール系列をサブゴール系列リストに保存する処理
（S105）、サブゴール系列リストからサブゴール系列を
１つ選択する処理（S106）、および、選択されたサブゴ
ール系列に対して各隣接サブゴール間の局所経路探索を
行う処理（S107）で構成される繰返し処理（ループ）で
ある。以下、先ず、本発明方法の手順の概要を図１に示
したフローチャートに沿って説明する。先ず、d-C-Hres
の離散化Ｃ空間の設定と（S100）、スタート配置S、ゴ
ール配置Gを入力した後（S101）、大まかな離散化刻み
幅、disPara（図２のS203）、を設定し（S102）、図２
および図３のように離散化Ｃ空間(d-q1',d-q2')を形成
する（S103）。図２〜図５は図１のS102およびS103に示
す離散化Ｃ空間構成法とS104に示すサブゴール系列探索
の結果を示す図である。図２および図３の203a、203bに
示したように、disParaはd-C-Hresの刻み幅に比べて十
分大きな値とする。離散化Ｃ空間(d-q1',d-q2')は大ま
かな間隔で離散化されたＣ空間であり、d-C-Lresと表す
（discrete C-space with Low resolutionの意味）。図
２ではd-C-Lresのスタート配置S（201）、ゴール配置G
（202）の座標値はそれぞれ(0,0), (5,0)となる。同様
に図３ではそれぞれ(0,0),(4,3)となる。本発明の方法
によれば、この例のようにスタート配置を離散化Ｃ空間
d-C-Lresの原点とする必要はなく、スタートおよびゴー
ル配置がd-C-Lresの格子点上にある必要もない。また、
座標軸が直交している必要もない。さらには、(d-q1,d-
q2)から(d-q1',d-q2')に非線形変換しても良く、d-C-Lr
esの定義の仕方は任意である。
【００２１】つぎに、STEP1 のサブゴール系列探索が行
われる（S104）。図６は図１に示すSTEP1の処理の内部
フローチャートである。この図６は、d-C-Lresでのスタ
ート配置Sからゴール配置Gまでの経路探索に等しい。前
述した最良優先、A* 探索などのグラフ探索手法を適用
してサブゴール系列を求める。グラフ探索を行うために
は、図３１に示した探索木のデータ構造を計算機内部に
持つ必要がある。探索木の葉とそれ以外のノードを別々
の連結リストに格納する。図７は図６に示すグラフ探索
に用いる連結リストのデータ構造の例を示す図である。
図７(ａ)は連結リスト401とそのセル402の基本クラスで
あり、図７(ｂ)のグラフ探索に用いる連結リスト403と
そのセル404のクラスは、図７(ａ)の派生クラスとして
定義している。連結リスト403のデータ型（クラス名）
をCLListForGraphSearchと定義する。探索木の葉を格納
する連結リストをOPENLIST、それ以外のノードを格納す
るリストをCLOSEDLISTと呼ぶ。OPENLIST内の干渉が生じ
るノードを別のリスト（COLLISIONLIST)に格納しても良
い。又、グラフ探索手法は公知の技術であるため、その
アルゴリズムの詳細については説明を省略する。 スタ
ート配置S、ゴール配置Gを含め、STEP1で生成された全
てのノード（サブゴールおよびその候補）は重複を省い
て、グラフの形式で計算機上に記憶される。
われる（S104）。図６は図１に示すSTEP1の処理の内部
フローチャートである。この図６は、d-C-Lresでのスタ
ート配置Sからゴール配置Gまでの経路探索に等しい。前
述した最良優先、A* 探索などのグラフ探索手法を適用
してサブゴール系列を求める。グラフ探索を行うために
は、図３１に示した探索木のデータ構造を計算機内部に
持つ必要がある。探索木の葉とそれ以外のノードを別々
の連結リストに格納する。図７は図６に示すグラフ探索
に用いる連結リストのデータ構造の例を示す図である。
図７(ａ)は連結リスト401とそのセル402の基本クラスで
あり、図７(ｂ)のグラフ探索に用いる連結リスト403と
そのセル404のクラスは、図７(ａ)の派生クラスとして
定義している。連結リスト403のデータ型（クラス名）
をCLListForGraphSearchと定義する。探索木の葉を格納
する連結リストをOPENLIST、それ以外のノードを格納す
るリストをCLOSEDLISTと呼ぶ。OPENLIST内の干渉が生じ
るノードを別のリスト（COLLISIONLIST)に格納しても良
い。又、グラフ探索手法は公知の技術であるため、その
アルゴリズムの詳細については説明を省略する。 スタ
ート配置S、ゴール配置Gを含め、STEP1で生成された全
てのノード（サブゴールおよびその候補）は重複を省い
て、グラフの形式で計算機上に記憶される。
【００２２】図８は図６に示すサブゴールグラフのデー
タ構造を示す図である。図中、501は全ての生成済みサ
ブゴール候補を格納する連結リストであり、データ型
（クラス）とオブジェクト（変数）名をCLListForSgGra
ph SgGraph;と定義する。連結リスト501の各セル502に
は、サブゴール候補の座標値や干渉の有無のデータに加
え、隣接するサブゴール候補に関するデータを格納する
リスト503が格納される。そのデータ型（クラス）とオ
ブジェクト（変数）名をCLListForAdjSGs AdjSGList;
と定義する。504はAdjSGListの各セルである。505a,505
bは隣接サブゴール（または候補）へのポインタCCellBa
seOfLList * ptrAdjSG;を示している。AdjSGListの各
セル504の内部には、ptrAdjSGで示された隣接サブゴー
ルへの経路の存在性を示すBOOL型の配列変数ExistenceO
fPathToAdjSG[k](k=0,1,2,...,N-1)、STEP2（図１のS10
7）で求められた隣接サブゴールへの経路を格納する連
結リストCLListForGraphSearch PathToAdjSG;、逆方向
局所経路探索実行の有無を示すBOOL型の変数InvDircSrc
h（507）、局所経路探索での探索済みノードの全データ
を格納する複数の連結リスト（506）が格納されてい
る。これらセル504の内部データと本発明方法の処理と
の関連については後で詳細に述べる。図４にSTEP１によ
り探索されたサブゴール系列の例を示す。同図において
S-Sg1-Sg2-Sg3-Sg4-Gが求まったサブゴール系列であ
る。図１のフローチャートに戻って、STEP1 のサブゴー
ル系列探索に成功した場合（TRUE）は、そのサブゴール
系列SgSeqがサブゴール系列リストListOfSgSeqに記憶さ
れる（S105）。
タ構造を示す図である。図中、501は全ての生成済みサ
ブゴール候補を格納する連結リストであり、データ型
（クラス）とオブジェクト（変数）名をCLListForSgGra
ph SgGraph;と定義する。連結リスト501の各セル502に
は、サブゴール候補の座標値や干渉の有無のデータに加
え、隣接するサブゴール候補に関するデータを格納する
リスト503が格納される。そのデータ型（クラス）とオ
ブジェクト（変数）名をCLListForAdjSGs AdjSGList;
と定義する。504はAdjSGListの各セルである。505a,505
bは隣接サブゴール（または候補）へのポインタCCellBa
seOfLList * ptrAdjSG;を示している。AdjSGListの各
セル504の内部には、ptrAdjSGで示された隣接サブゴー
ルへの経路の存在性を示すBOOL型の配列変数ExistenceO
fPathToAdjSG[k](k=0,1,2,...,N-1)、STEP2（図１のS10
7）で求められた隣接サブゴールへの経路を格納する連
結リストCLListForGraphSearch PathToAdjSG;、逆方向
局所経路探索実行の有無を示すBOOL型の変数InvDircSrc
h（507）、局所経路探索での探索済みノードの全データ
を格納する複数の連結リスト（506）が格納されてい
る。これらセル504の内部データと本発明方法の処理と
の関連については後で詳細に述べる。図４にSTEP１によ
り探索されたサブゴール系列の例を示す。同図において
S-Sg1-Sg2-Sg3-Sg4-Gが求まったサブゴール系列であ
る。図１のフローチャートに戻って、STEP1 のサブゴー
ル系列探索に成功した場合（TRUE）は、そのサブゴール
系列SgSeqがサブゴール系列リストListOfSgSeqに記憶さ
れる（S105）。
【００２３】図９は図１に示すサブゴール系列リストの
データ構造を示す図である。図中、601がサブゴール系
列を格納する連結リストであり、データ型（クラス）と
オブジェクト（変数）名をCLListForSGSeq ListOfSgSe
q;と定義する。連結リスト601の各セル602には、サブゴ
ール系列603（サブゴールの連結リスト）とSTEP2 の実
行結果に関するBOOL型の配列変数SearchResult[k](k=0,
1,2,...,N-1)フラグが複数格納されている。サブゴール
系列603の各セル604には、各サブゴールのd-C-Lresにお
ける座標値などのデータが格納されている。配列変数Se
archResult[.]と本発明方法の処理との関連についても
後で詳細に述べる。つぎに、サブゴール系列リスト601
から何らかの基準に従って１つのサブゴール系列が選択
される（S106）。STEP1 に失敗してサブゴール系列が求
まらなかった場合（FALSE)は、S105をスキップしてS106
が実行される。S106で選択されたサブゴール系列に対し
て、STEP2の隣接サブゴール間局所経路探索が行われる
（S107）。STEP1に失敗した場合は、サブゴール系列リ
スト601から過去既にSTEP2に失敗したサブゴール系列が
選択されるため、STEP2を再実行することになる。STEP2
の再実行処理については後述する。図１０は図１に示す
STEP2の内部フローチャートである。図１０に移って、S
TEP2の処理については、図１のS106で選択されたサブゴ
ール系列から、隣接する２つのサブゴールを選択して
（S701）、パラメータのrParaを設定する（S702）。図
８のサブゴールグラフのAdjSGListのセル504に格納され
ている探索済みノードの全データ（前回探索データ）を
取得・変換し（S703）、d-C-Hres上で局所経路探索が行
われる（S704）。ここでも、STEP1 と同様に最良優先、
A * 探索などのグラフ探索手法が適用される。探索結果
の如何にかかわらず、探索済みノードの全データ（全探
索データ）をサブゴールグラフのAdjSGListのセル504に
書込み、更新する（S705）。S704の局所経路探索に成功
した場合、ゴール配置Gまで局所経路が求まったかを判
断する（S706）。ゴール配置Gまで到達していない場
合、S701に戻ってつぎの隣接する２つのサブゴールが選
択される。ゴール配置Gに到達した場合、TRUEを返してS
TEP2 を終了する。途中でS703に失敗した場合、FALSE
を返してSTEP2 を終了する。S702で設定されるパラメー
タrParaは、隣接サブゴール間局所経路探索の探索範囲
を制限するパラメータである。
データ構造を示す図である。図中、601がサブゴール系
列を格納する連結リストであり、データ型（クラス）と
オブジェクト（変数）名をCLListForSGSeq ListOfSgSe
q;と定義する。連結リスト601の各セル602には、サブゴ
ール系列603（サブゴールの連結リスト）とSTEP2 の実
行結果に関するBOOL型の配列変数SearchResult[k](k=0,
1,2,...,N-1)フラグが複数格納されている。サブゴール
系列603の各セル604には、各サブゴールのd-C-Lresにお
ける座標値などのデータが格納されている。配列変数Se
archResult[.]と本発明方法の処理との関連についても
後で詳細に述べる。つぎに、サブゴール系列リスト601
から何らかの基準に従って１つのサブゴール系列が選択
される（S106）。STEP1 に失敗してサブゴール系列が求
まらなかった場合（FALSE)は、S105をスキップしてS106
が実行される。S106で選択されたサブゴール系列に対し
て、STEP2の隣接サブゴール間局所経路探索が行われる
（S107）。STEP1に失敗した場合は、サブゴール系列リ
スト601から過去既にSTEP2に失敗したサブゴール系列が
選択されるため、STEP2を再実行することになる。STEP2
の再実行処理については後述する。図１０は図１に示す
STEP2の内部フローチャートである。図１０に移って、S
TEP2の処理については、図１のS106で選択されたサブゴ
ール系列から、隣接する２つのサブゴールを選択して
（S701）、パラメータのrParaを設定する（S702）。図
８のサブゴールグラフのAdjSGListのセル504に格納され
ている探索済みノードの全データ（前回探索データ）を
取得・変換し（S703）、d-C-Hres上で局所経路探索が行
われる（S704）。ここでも、STEP1 と同様に最良優先、
A * 探索などのグラフ探索手法が適用される。探索結果
の如何にかかわらず、探索済みノードの全データ（全探
索データ）をサブゴールグラフのAdjSGListのセル504に
書込み、更新する（S705）。S704の局所経路探索に成功
した場合、ゴール配置Gまで局所経路が求まったかを判
断する（S706）。ゴール配置Gまで到達していない場
合、S701に戻ってつぎの隣接する２つのサブゴールが選
択される。ゴール配置Gに到達した場合、TRUEを返してS
TEP2 を終了する。途中でS703に失敗した場合、FALSE
を返してSTEP2 を終了する。S702で設定されるパラメー
タrParaは、隣接サブゴール間局所経路探索の探索範囲
を制限するパラメータである。
【００２４】図１１は図１０に示す経路探索範囲を制限
する局所領域を示す図である。図１１において、801a、
801bは共に隣接する２つのサブゴールを包含する楕円体
領域である。802、803はそれぞれサブゴールSg1、Sg2、
804はＣ空間障害物である。この楕円体領域を用いて探
索範囲を制限する場合、以下の式によって探索範囲Srng
を定義する。但し、d(p,q)は離散化Ｃ空間d-C-Hresに
おける２点p，q間の距離を表す。 Srng=｛ n｜d(Sg1,n)＋d(n,Sg2) <= d(Sg1,Sg2)×rPar
a, rPara>=1｝ 上式より、パラメータrParaを小さくするほど探索範囲S
rng が制限されることがわかる。rPara=1のときは、隣
接サブゴールSg1、Sg2を結ぶ線分の最近傍集合とする。
なお、図１１(ａ)は楕円体領域801a内で局所経路が求ま
る場合、図１１(ｂ)は楕円体領域801b内で局所経路が求
まらない場合を示している。図１１において、サブゴー
ルSg1から同Sg2への局所微小間隔経路を探索するとき、
図１１(ａ)の場合は、修正A* アルゴリズムなどを適用
することによって、最短経路805が求まる。図１１(ｂ)
の場合は、楕円体領域801b内では経路が存在しないの
で、楕円体領域801bとＣ空間障害物804に囲まれる領域8
06内全ての格子点を展開した後、経路探索に失敗したと
判断される。又、楕円体領域以外の閉領域を用いて探索
範囲を制限してもよい。探索範囲の設定の仕方は任意で
限定されない。STEP2 の処理において、SからGに至る全
ての隣接サブゴール間局所経路探索に成功した場合（TR
UE）は、その経路をストア（図１のS108）して、本発明
の経路計画方法は経路探索を終了する。
する局所領域を示す図である。図１１において、801a、
801bは共に隣接する２つのサブゴールを包含する楕円体
領域である。802、803はそれぞれサブゴールSg1、Sg2、
804はＣ空間障害物である。この楕円体領域を用いて探
索範囲を制限する場合、以下の式によって探索範囲Srng
を定義する。但し、d(p,q)は離散化Ｃ空間d-C-Hresに
おける２点p，q間の距離を表す。 Srng=｛ n｜d(Sg1,n)＋d(n,Sg2) <= d(Sg1,Sg2)×rPar
a, rPara>=1｝ 上式より、パラメータrParaを小さくするほど探索範囲S
rng が制限されることがわかる。rPara=1のときは、隣
接サブゴールSg1、Sg2を結ぶ線分の最近傍集合とする。
なお、図１１(ａ)は楕円体領域801a内で局所経路が求ま
る場合、図１１(ｂ)は楕円体領域801b内で局所経路が求
まらない場合を示している。図１１において、サブゴー
ルSg1から同Sg2への局所微小間隔経路を探索するとき、
図１１(ａ)の場合は、修正A* アルゴリズムなどを適用
することによって、最短経路805が求まる。図１１(ｂ)
の場合は、楕円体領域801b内では経路が存在しないの
で、楕円体領域801bとＣ空間障害物804に囲まれる領域8
06内全ての格子点を展開した後、経路探索に失敗したと
判断される。又、楕円体領域以外の閉領域を用いて探索
範囲を制限してもよい。探索範囲の設定の仕方は任意で
限定されない。STEP2 の処理において、SからGに至る全
ての隣接サブゴール間局所経路探索に成功した場合（TR
UE）は、その経路をストア（図１のS108）して、本発明
の経路計画方法は経路探索を終了する。
【００２５】次に、改めてSTEP2からSTEP1へのバックト
ラッキング（再探索）の処理について説明する。STEP2
の処理で、隣接サブゴール間局所経路探索に途中で失敗
した場合（FALSE)は、STEP1 のサブゴール系列探索に戻
る（S104）。例えば、図４のサブゴール系列S→Sg1→Sg
2→Sg3→Sg4→G内の局所経路探索において、図１１(ｂ)
のようにSg1とSg2間の局所経路探索に失敗した場合、ST
EP1 に戻り図５のようにSg1からSg2へのアークを経由し
ない新たなサブゴール系列S→Sg1’→Sg2→Sg3→Sg4→G
を再探索する。そのために、図６、図１０のフローチャ
ートに示したようにSTEP1、STEP2共にサブゴールグラフ
（図８）のデータを参照しながら探索を行い、探索の結
果に応じてそのデータを変更する。又、図１０に示すST
EP2が、隣接サブゴールSg1、Sg2間局所経路探索S704に
成功した場合は、求められた経路は図８に示したサブゴ
ールグラフ SgGraphのSGgraph.Cell(Sg1).AdjSGList.Ce
ll(Arc(Sg1-Sg2)).PathToAdjSGに格納される。ここで、
上式は、SgGraphのSg1に対応するセルに格納されている
AdjSGListのSg1からSg2へのアークを示す（Sg2へのポイ
ンタptrAdjSGを格納する）セルにあるPathToAdjSGを意
味している。以下、同様にデータ参照を記述する。逆
に、Sg1→Sg2間局所経路探索S704に失敗した場合は、Sg
Graph.Cell(Sg1).AdjSGList.Cell(Arc(Sg1-Sg2)).Exist
enceOfPathToAdjSG[0]がFALSEに変更される（局所経路
探索が未実行の場合はTRUEとなっている）。又、STEP1
では常に、SgGraph.Cell(Sg1).AdjSGList.Cell(Arc(Sg1
-Sg2)).ExistanceOfPathToAdjSG[0]の状態を参照し、こ
のフラグがFALSE であれば、Sg1からSg2へは探索を進め
ない。又、このサブゴールグラフには、前回までに生成
したサブゴール及びその候補に関する情報が記憶されて
いるので、そのデータを参照することにより、再度、始
めから幾何モデラを用いてロボットと作業環境との干渉
を検査するような無駄を省き、探索効率の向上を図って
いる。STEP2のS701の選択処理でも、SgGraphが参照さ
れ、SgGraph.Cell(Sg1).AdjSGList.Cell(Arc(Sg1-Sg
2)).PathToAdjSG が空ではない（つまり、既に局所経路
を求めた）隣接サブゴールSg1、Sg2間はスキップされ、
次の隣接サブゴールSg2、Sg3が選択される。又、本発明
の経路計画方法では、図１のフローチャートのS106の処
理で、図９のようなサブゴール系列リストListOfSgSeq
を選択する処理に失敗（FALSE ）した場合、所定時間経
過したかどうか判断される（S109）。時間切れであれば
探索失敗として終了する。時間切れで無ければ、離散化
刻み幅disParaをＬ（整数）分割し（S110）、d-C-Lres
の再形成を行って（S103）、一連の処理を継続する。
ラッキング（再探索）の処理について説明する。STEP2
の処理で、隣接サブゴール間局所経路探索に途中で失敗
した場合（FALSE)は、STEP1 のサブゴール系列探索に戻
る（S104）。例えば、図４のサブゴール系列S→Sg1→Sg
2→Sg3→Sg4→G内の局所経路探索において、図１１(ｂ)
のようにSg1とSg2間の局所経路探索に失敗した場合、ST
EP1 に戻り図５のようにSg1からSg2へのアークを経由し
ない新たなサブゴール系列S→Sg1’→Sg2→Sg3→Sg4→G
を再探索する。そのために、図６、図１０のフローチャ
ートに示したようにSTEP1、STEP2共にサブゴールグラフ
（図８）のデータを参照しながら探索を行い、探索の結
果に応じてそのデータを変更する。又、図１０に示すST
EP2が、隣接サブゴールSg1、Sg2間局所経路探索S704に
成功した場合は、求められた経路は図８に示したサブゴ
ールグラフ SgGraphのSGgraph.Cell(Sg1).AdjSGList.Ce
ll(Arc(Sg1-Sg2)).PathToAdjSGに格納される。ここで、
上式は、SgGraphのSg1に対応するセルに格納されている
AdjSGListのSg1からSg2へのアークを示す（Sg2へのポイ
ンタptrAdjSGを格納する）セルにあるPathToAdjSGを意
味している。以下、同様にデータ参照を記述する。逆
に、Sg1→Sg2間局所経路探索S704に失敗した場合は、Sg
Graph.Cell(Sg1).AdjSGList.Cell(Arc(Sg1-Sg2)).Exist
enceOfPathToAdjSG[0]がFALSEに変更される（局所経路
探索が未実行の場合はTRUEとなっている）。又、STEP1
では常に、SgGraph.Cell(Sg1).AdjSGList.Cell(Arc(Sg1
-Sg2)).ExistanceOfPathToAdjSG[0]の状態を参照し、こ
のフラグがFALSE であれば、Sg1からSg2へは探索を進め
ない。又、このサブゴールグラフには、前回までに生成
したサブゴール及びその候補に関する情報が記憶されて
いるので、そのデータを参照することにより、再度、始
めから幾何モデラを用いてロボットと作業環境との干渉
を検査するような無駄を省き、探索効率の向上を図って
いる。STEP2のS701の選択処理でも、SgGraphが参照さ
れ、SgGraph.Cell(Sg1).AdjSGList.Cell(Arc(Sg1-Sg
2)).PathToAdjSG が空ではない（つまり、既に局所経路
を求めた）隣接サブゴールSg1、Sg2間はスキップされ、
次の隣接サブゴールSg2、Sg3が選択される。又、本発明
の経路計画方法では、図１のフローチャートのS106の処
理で、図９のようなサブゴール系列リストListOfSgSeq
を選択する処理に失敗（FALSE ）した場合、所定時間経
過したかどうか判断される（S109）。時間切れであれば
探索失敗として終了する。時間切れで無ければ、離散化
刻み幅disParaをＬ（整数）分割し（S110）、d-C-Lres
の再形成を行って（S103）、一連の処理を継続する。
【００２６】次に、図１のフローチャートのサブゴール
系列リストListOfSgSeqから、サブゴール系列SgSeq を
選択する処理（S106）、図１０のパラメータrParaの設
定処理（S702）、サブゴールグラフ（図８）及びサブゴ
ール系列リスト（図９）のデータと上記処理との関係に
ついて詳細に説明する。まず、rParaはつぎのような段
階的な値を取るものとする。 rPara = MonoIncFunc[k], k=0,1,2,...,N-1 ここで、NをSTEP2における各隣接サブゴール間局所経路
探索の最大探索（範囲）レベルとする。MonoIncFunc[.]
は最小値1の単調増加関数である。サブゴールグラフの
各アークが持つExitenceOfPathToAdjSG[k](k=0,1,
2,...,N-1)とサブゴール系列リストの各セルが持つSear
chResult[k](k=0,1,2,...,N-1)はともに上記rParaのMon
oIncFunc[k](k=0,1,2,...,N-1)に対応づけられた変数で
ある。すなわち、ExitenceOfPathToAdjSG[k]およびSear
chResult[k]は各隣接サブゴール間局所経路探索の探索
レベルであるMonoIncFunc[k]のkに対する結果を示して
いる。STEP1でサブゴール系列探索に成功した場合、求
まったサブゴール系列SgSeqを図９に示したサブゴール
系列リストListOfSgSeq(601)の先頭に挿入する。挿入さ
れた時点では、STEP2 の実行結果に関するBOOL変数Sear
chResult[k]は全てTRUEとなっている。図１のS106の処
理では、SearchResult[k]=TRUEを満足するkが最小とな
るサブゴール系列が選択される。すなわち、STEP1で新
しいサブゴールが求まった場合は、必ず、それが選択さ
れる。図１０のS702のrPara設定処理では、S701の処理
で選択された隣接サブゴール間のアークのExitenceOfPa
thToAdjSG[k]を参照してつぎのようにrParaを設定す
る。 rPara = MonoIncFunc[m], m = min[ k | ExistenceOfPa
thToAdjSG[k]=TRUE ] すなわち、局所経路が未探索である隣接サブゴール間に
対しては、必ずrPara=1(m=0)に設定される。設定された
rPara=MonoIncFunc[m]に対してS704の（局所）経路探索
処理に失敗した場合、対応するExistenceOfPathToAdjSG
[m]およびSearchResult[m]をFALSEに変更する。STEP1で
サブゴール系列探索に失敗した場合、サブゴール系列リ
ストに登録されている全てのサブゴール系列のSearchRe
sult[0]は全てFALSEである。このときも同様に、Search
Result[k]=TRUEを満足するkが最小となるサブゴール系
列が選択される（kは1以上）。サブゴール系列の候補が
複数ある場合は、最もゴール配置G近くまで局所経路探
索（S704）に成功し、なおかつ、その経路長が最も短い
ものを優先的に選択する。このサブゴール系列は、その
中のいずれかの隣接サブゴール間で過去S704の局所経路
探索に失敗したことを示している。過去局所経路探索に
失敗した隣接サブゴール間に対しても上述した方法でrP
ara=MonoIncFunc[m]を設定し（mは1以上）、局所経路探
索S704が再実行される。局所経路の再探索に成功した場
合は、対応する隣接サブゴール間アークのExistenceOfP
athToAdjSG[0]をTRUEに戻す。再探索に失敗した場合の
処理は前述した通りである。 サブゴール系列リスト内
の全てのサブゴール系列に対するSearchResult[N-1]がF
ALSEとなった場合、図１のサブゴール系列選択処理(S10
6)は、失敗と判断され、FALSEを返す。また、S106にお
いて、SearchResult[m]=TRUEであるサブゴール系列を選
択する際には、そのサブゴール系列内の全て隣接サブゴ
ール間に対してサブゴールグラフ（図８）の該当するEx
istenceOfPathToAdjSG[m]を全てチェックし、いずれか
がFALSEの場合はそれを選択せず、SearchResult[m]=FAL
SEに書き換える。
系列リストListOfSgSeqから、サブゴール系列SgSeq を
選択する処理（S106）、図１０のパラメータrParaの設
定処理（S702）、サブゴールグラフ（図８）及びサブゴ
ール系列リスト（図９）のデータと上記処理との関係に
ついて詳細に説明する。まず、rParaはつぎのような段
階的な値を取るものとする。 rPara = MonoIncFunc[k], k=0,1,2,...,N-1 ここで、NをSTEP2における各隣接サブゴール間局所経路
探索の最大探索（範囲）レベルとする。MonoIncFunc[.]
は最小値1の単調増加関数である。サブゴールグラフの
各アークが持つExitenceOfPathToAdjSG[k](k=0,1,
2,...,N-1)とサブゴール系列リストの各セルが持つSear
chResult[k](k=0,1,2,...,N-1)はともに上記rParaのMon
oIncFunc[k](k=0,1,2,...,N-1)に対応づけられた変数で
ある。すなわち、ExitenceOfPathToAdjSG[k]およびSear
chResult[k]は各隣接サブゴール間局所経路探索の探索
レベルであるMonoIncFunc[k]のkに対する結果を示して
いる。STEP1でサブゴール系列探索に成功した場合、求
まったサブゴール系列SgSeqを図９に示したサブゴール
系列リストListOfSgSeq(601)の先頭に挿入する。挿入さ
れた時点では、STEP2 の実行結果に関するBOOL変数Sear
chResult[k]は全てTRUEとなっている。図１のS106の処
理では、SearchResult[k]=TRUEを満足するkが最小とな
るサブゴール系列が選択される。すなわち、STEP1で新
しいサブゴールが求まった場合は、必ず、それが選択さ
れる。図１０のS702のrPara設定処理では、S701の処理
で選択された隣接サブゴール間のアークのExitenceOfPa
thToAdjSG[k]を参照してつぎのようにrParaを設定す
る。 rPara = MonoIncFunc[m], m = min[ k | ExistenceOfPa
thToAdjSG[k]=TRUE ] すなわち、局所経路が未探索である隣接サブゴール間に
対しては、必ずrPara=1(m=0)に設定される。設定された
rPara=MonoIncFunc[m]に対してS704の（局所）経路探索
処理に失敗した場合、対応するExistenceOfPathToAdjSG
[m]およびSearchResult[m]をFALSEに変更する。STEP1で
サブゴール系列探索に失敗した場合、サブゴール系列リ
ストに登録されている全てのサブゴール系列のSearchRe
sult[0]は全てFALSEである。このときも同様に、Search
Result[k]=TRUEを満足するkが最小となるサブゴール系
列が選択される（kは1以上）。サブゴール系列の候補が
複数ある場合は、最もゴール配置G近くまで局所経路探
索（S704）に成功し、なおかつ、その経路長が最も短い
ものを優先的に選択する。このサブゴール系列は、その
中のいずれかの隣接サブゴール間で過去S704の局所経路
探索に失敗したことを示している。過去局所経路探索に
失敗した隣接サブゴール間に対しても上述した方法でrP
ara=MonoIncFunc[m]を設定し（mは1以上）、局所経路探
索S704が再実行される。局所経路の再探索に成功した場
合は、対応する隣接サブゴール間アークのExistenceOfP
athToAdjSG[0]をTRUEに戻す。再探索に失敗した場合の
処理は前述した通りである。 サブゴール系列リスト内
の全てのサブゴール系列に対するSearchResult[N-1]がF
ALSEとなった場合、図１のサブゴール系列選択処理(S10
6)は、失敗と判断され、FALSEを返す。また、S106にお
いて、SearchResult[m]=TRUEであるサブゴール系列を選
択する際には、そのサブゴール系列内の全て隣接サブゴ
ール間に対してサブゴールグラフ（図８）の該当するEx
istenceOfPathToAdjSG[m]を全てチェックし、いずれか
がFALSEの場合はそれを選択せず、SearchResult[m]=FAL
SEに書き換える。
【００２７】つぎに、STEP2（図１０）の前回探索デー
タ取得・変換処理（S703）、サブゴール間経路探索処理
（S704）および全探索データ保存処理（S705）とサブゴ
ールグラフ（図８）に格納される探索済みノードの全デ
ータとの関係について説明する。図１２において、90
1，902，903は、それぞれ、C空間を微小間隔で離散化し
た離散化C空間d-C-Hres、サブゴールSg1、サブゴールSg
2を示している。904a，904bはC空間障害物、905はロボ
ットの動作領域外を示す空間、906はサブゴール間局所
経路探索の際の探索範囲Srngを示している。図４に示し
たサブゴールSg1，Sg2間の局所経路探索を例に上記の説
明を進める。また、便宜上、S704，S705，S703の順に説
明する。図１３は、図１２のサブゴールSg1付近を拡大
し、局所経路探索（S704）が進行する様子を示してい
る。これは、従来技術の項で既に説明したようにA * 探
索などのグラフ探索が進行する様子である。図１４は図
１３の探索進行状況をツリー構造で示したもの（探索
木）である。図１３および図１４に示した六角形のノー
ドb2は干渉チェックにより、障害物と干渉するノードと
判別されたことを示している。また、四角形のノードb3
およびc1は、探索範囲Srng外のノードであることを示し
ている。図１５は図１４の探索木を計算機上に実現した
データ構造を示している。1001から1005は全て図７(b)
に示したグラフ探索のための連結リストのクラスCLList
ForGraphSearchのオブジェクトである。1001のOPENLIST
および1002のCLOSEDLISTは、それぞれ展開候補のノード
（探索木の葉）を格納するリスト、展開済みノード（子
を持つノード）を格納するリストを示している。グラフ
探索のアルゴリズムやOPENLISTおよびCLOSEDLISTといっ
たデータの格納方法は公知の技術である。
タ取得・変換処理（S703）、サブゴール間経路探索処理
（S704）および全探索データ保存処理（S705）とサブゴ
ールグラフ（図８）に格納される探索済みノードの全デ
ータとの関係について説明する。図１２において、90
1，902，903は、それぞれ、C空間を微小間隔で離散化し
た離散化C空間d-C-Hres、サブゴールSg1、サブゴールSg
2を示している。904a，904bはC空間障害物、905はロボ
ットの動作領域外を示す空間、906はサブゴール間局所
経路探索の際の探索範囲Srngを示している。図４に示し
たサブゴールSg1，Sg2間の局所経路探索を例に上記の説
明を進める。また、便宜上、S704，S705，S703の順に説
明する。図１３は、図１２のサブゴールSg1付近を拡大
し、局所経路探索（S704）が進行する様子を示してい
る。これは、従来技術の項で既に説明したようにA * 探
索などのグラフ探索が進行する様子である。図１４は図
１３の探索進行状況をツリー構造で示したもの（探索
木）である。図１３および図１４に示した六角形のノー
ドb2は干渉チェックにより、障害物と干渉するノードと
判別されたことを示している。また、四角形のノードb3
およびc1は、探索範囲Srng外のノードであることを示し
ている。図１５は図１４の探索木を計算機上に実現した
データ構造を示している。1001から1005は全て図７(b)
に示したグラフ探索のための連結リストのクラスCLList
ForGraphSearchのオブジェクトである。1001のOPENLIST
および1002のCLOSEDLISTは、それぞれ展開候補のノード
（探索木の葉）を格納するリスト、展開済みノード（子
を持つノード）を格納するリストを示している。グラフ
探索のアルゴリズムやOPENLISTおよびCLOSEDLISTといっ
たデータの格納方法は公知の技術である。
【００２８】ここで、本発明の独自技術の１つは、グラ
フ探索手法を用いた隣接サブゴール間局所経路探索（図
１０のS704）の際、探索範囲Srng（906）外のノードに
対してロボットが可動範囲内にあるかどうかのチェック
（可動範囲チェック）および干渉チェックを行なわず
に、OPENLISTおよびCLOSEDLISTとは別の連結リストに格
納するところにある。図１５の例では、探索範囲Srng外
のノードをNextOPENLSIT（1003）に格納している。ま
た、COLLISIONLIST（1004）は干渉が生じたノードを格
納する連結リスト、OUTRANGELIST（1005）はロボットの
可動範囲外のノードを格納するノードである。COLLISIO
NLISTとOUTRANGELISTは必ずしもCLOSEDLISTと区別する
必要はないが、CLOSEDLISTと区別することによって、連
結リストの各セルが持つべきデータ量を若干抑えること
ができる。図１６は図１３から更に局所経路探索（グラ
フ探索）を進めた状況を示している。図１６において、
1101，1102，1104a，1104b，1105，1106はそれぞれ図１
３の901，902，904a，904b，905，906に対応している。
図１６の探索状況では、b3，c1，d1，d2，e1，f1，f3，
g2，g3がNextOPENLISTに、Sg1，a1〜a4，b1，c2がCLOSE
DLISTに、b2，f2，g1がCOLLISIONLISTに格納され、OPEN
LISTは空になっている。したがって、つぎに展開すべき
ノード（格子点）の候補がないため、設定した探索範囲
Srng（1106）内では局所経路探索に失敗したと判断され
る（図１０のS704がFALSEとして終了）。S704が終了す
ると、S705にて、図１６に示した探索済み全ノードの情
報を図１５に示したデータ構造形式のままで図８のサブ
ゴールグラフの該当アークに保存する。これも本発明の
独自技術の１つである。連結リストをコピーする必要は
なく、リストの中身を移し替えれば良い。つぎに、STEP
1とSTEP2のバックトラッキングを繰り返した後、図１２
に示したサブゴールSg1，Sg2間の局所経路探索が再実行
される場合を想定する。このとき、図１０のS703にて、
図８のサブゴールグラフの該当アークに格納されている
CLOSEDLIST，NextOPENLIST，COLLISIONLIST，OUTRANGEL
ISTが取得される。取得したこれら連結リストは図１６
の状態を保持している。ここでも、これら連結リストを
コピーするのではなく、中身を移し替えれば良い。ま
た、隣接サブゴール間局所経路探索再実行の際には、前
述したようにrParaが前回よりも１段階大きな値に設定
され、局所経路の探索範囲Srngは図１２および図１３の
906あるいは図１６の1106に示したものよりも広くなっ
ている。NextOPENLISTに格納されているノードに対して
は、前回の局所経路探索時に可動範囲チェックおよび干
渉チェックを行なっていない。そこでS703では、図１８
に示すように、可動範囲チェックおよび干渉チェックを
行ない可動範囲内で干渉を起こさないノードのみをOPEN
LISTに移し替える変換処理を行なう。図１７は、取得し
たNextOPENLISTをOPENLISTに移し替えた後の状態を示し
ている。図１７において、1107は前回1106より１段階広
く設定された探索範囲Srngを示している。同図におい
て、e1はCOLLISIONLISTにf3はOUTRANGELISTに移され、
残りは全てOPENLISTに移されている。なお、五角形のノ
ードf3はロボットの可動範囲外であることを示してい
る。以上説明した前回探索データ取得・変換処理（S70
3）も本発明の独自技術である。前回探索データ取得・
変換処理（S703）が終了すると、サブゴールSg1，Sg2間
の局所経路探索処理（S704）に移り、１段階広く設定さ
れた探索範囲Srng（1107）に対して、図１７を初期状態
として、グラフ探索手法による経路探索が再実行され
る。図１９はサブゴールSg1，Sg2間の局所経路探索が再
実行され、Sg1からSg2への局所経路が求まった様子を示
している。以上の図１２〜図１９を用いた説明では、探
索範囲Srngを１段階広げたのみで隣接サブゴール間局所
経路探索に成功した例を用いたが、S703，S704およびS7
05に示した本発明による局所経路探索方法はこれに限定
されるわけではなく、その再実行回数（探索範囲Srngが
広げられた段階の数）が何回になろうと動作する。さら
に、探索範囲Srngを設定するパラメータrPara=MonoIncF
unc[m]の値によって局所経路が求まるまでの再実行回数
は異なるわけであるが、局所経路が求まった時点でのrP
araが同じ値であれば、それまでを何段階に分けて局所
経路探索を実行しても可動範囲チェックおよび干渉チェ
ックを行なう回数は変わらない。図１９で説明すれば、
探索範囲Srngを1回目は1306に設定し2回目は1307に設定
した場合と、1回目から1307に設定した場合では、前者
は2回目の局所経路探索で経路が求まり、後者は1回目で
局所経路が求まるが、探索したノードの数に差は生じな
い。
フ探索手法を用いた隣接サブゴール間局所経路探索（図
１０のS704）の際、探索範囲Srng（906）外のノードに
対してロボットが可動範囲内にあるかどうかのチェック
（可動範囲チェック）および干渉チェックを行なわず
に、OPENLISTおよびCLOSEDLISTとは別の連結リストに格
納するところにある。図１５の例では、探索範囲Srng外
のノードをNextOPENLSIT（1003）に格納している。ま
た、COLLISIONLIST（1004）は干渉が生じたノードを格
納する連結リスト、OUTRANGELIST（1005）はロボットの
可動範囲外のノードを格納するノードである。COLLISIO
NLISTとOUTRANGELISTは必ずしもCLOSEDLISTと区別する
必要はないが、CLOSEDLISTと区別することによって、連
結リストの各セルが持つべきデータ量を若干抑えること
ができる。図１６は図１３から更に局所経路探索（グラ
フ探索）を進めた状況を示している。図１６において、
1101，1102，1104a，1104b，1105，1106はそれぞれ図１
３の901，902，904a，904b，905，906に対応している。
図１６の探索状況では、b3，c1，d1，d2，e1，f1，f3，
g2，g3がNextOPENLISTに、Sg1，a1〜a4，b1，c2がCLOSE
DLISTに、b2，f2，g1がCOLLISIONLISTに格納され、OPEN
LISTは空になっている。したがって、つぎに展開すべき
ノード（格子点）の候補がないため、設定した探索範囲
Srng（1106）内では局所経路探索に失敗したと判断され
る（図１０のS704がFALSEとして終了）。S704が終了す
ると、S705にて、図１６に示した探索済み全ノードの情
報を図１５に示したデータ構造形式のままで図８のサブ
ゴールグラフの該当アークに保存する。これも本発明の
独自技術の１つである。連結リストをコピーする必要は
なく、リストの中身を移し替えれば良い。つぎに、STEP
1とSTEP2のバックトラッキングを繰り返した後、図１２
に示したサブゴールSg1，Sg2間の局所経路探索が再実行
される場合を想定する。このとき、図１０のS703にて、
図８のサブゴールグラフの該当アークに格納されている
CLOSEDLIST，NextOPENLIST，COLLISIONLIST，OUTRANGEL
ISTが取得される。取得したこれら連結リストは図１６
の状態を保持している。ここでも、これら連結リストを
コピーするのではなく、中身を移し替えれば良い。ま
た、隣接サブゴール間局所経路探索再実行の際には、前
述したようにrParaが前回よりも１段階大きな値に設定
され、局所経路の探索範囲Srngは図１２および図１３の
906あるいは図１６の1106に示したものよりも広くなっ
ている。NextOPENLISTに格納されているノードに対して
は、前回の局所経路探索時に可動範囲チェックおよび干
渉チェックを行なっていない。そこでS703では、図１８
に示すように、可動範囲チェックおよび干渉チェックを
行ない可動範囲内で干渉を起こさないノードのみをOPEN
LISTに移し替える変換処理を行なう。図１７は、取得し
たNextOPENLISTをOPENLISTに移し替えた後の状態を示し
ている。図１７において、1107は前回1106より１段階広
く設定された探索範囲Srngを示している。同図におい
て、e1はCOLLISIONLISTにf3はOUTRANGELISTに移され、
残りは全てOPENLISTに移されている。なお、五角形のノ
ードf3はロボットの可動範囲外であることを示してい
る。以上説明した前回探索データ取得・変換処理（S70
3）も本発明の独自技術である。前回探索データ取得・
変換処理（S703）が終了すると、サブゴールSg1，Sg2間
の局所経路探索処理（S704）に移り、１段階広く設定さ
れた探索範囲Srng（1107）に対して、図１７を初期状態
として、グラフ探索手法による経路探索が再実行され
る。図１９はサブゴールSg1，Sg2間の局所経路探索が再
実行され、Sg1からSg2への局所経路が求まった様子を示
している。以上の図１２〜図１９を用いた説明では、探
索範囲Srngを１段階広げたのみで隣接サブゴール間局所
経路探索に成功した例を用いたが、S703，S704およびS7
05に示した本発明による局所経路探索方法はこれに限定
されるわけではなく、その再実行回数（探索範囲Srngが
広げられた段階の数）が何回になろうと動作する。さら
に、探索範囲Srngを設定するパラメータrPara=MonoIncF
unc[m]の値によって局所経路が求まるまでの再実行回数
は異なるわけであるが、局所経路が求まった時点でのrP
araが同じ値であれば、それまでを何段階に分けて局所
経路探索を実行しても可動範囲チェックおよび干渉チェ
ックを行なう回数は変わらない。図１９で説明すれば、
探索範囲Srngを1回目は1306に設定し2回目は1307に設定
した場合と、1回目から1307に設定した場合では、前者
は2回目の局所経路探索で経路が求まり、後者は1回目で
局所経路が求まるが、探索したノードの数に差は生じな
い。
【００２９】図３３および図３４は、それぞれ特願平11
−146667に記載されているサブゴール（有向）グラフの
データ構造とSTEP2のフローチャートを示している。図
３３では、隣接サブゴールのデータを格納する連結リス
トAdjSGListの各セル（サブゴール間アーク）に、探索
済みノードを全データ（NextOPENLIST，CLOSEDLIST，CO
LLISIONLIST，OUTRANGELIST）を保存するようになって
いない。また、図３４には、図7の前回探索データ取得
・変換処理（S703）および全探索データ保存処理（S70
4）がないことが分かる。したがって、特願平11−14666
7では、探索範囲Srngを広げて局所経路探索を再実行す
る場合には初めから探索をやり直さなければならず、再
実行回数が増加すると若干探索効率が悪くなってしま
う。一方、本発明による局所経路探索方法では、以上説
明したように前回探索データの再利用を巧みに実現して
いるため、同一の隣接サブゴール間に対する局所経路探
索の再実行回数が増加しても探索効率は劣化しない。
−146667に記載されているサブゴール（有向）グラフの
データ構造とSTEP2のフローチャートを示している。図
３３では、隣接サブゴールのデータを格納する連結リス
トAdjSGListの各セル（サブゴール間アーク）に、探索
済みノードを全データ（NextOPENLIST，CLOSEDLIST，CO
LLISIONLIST，OUTRANGELIST）を保存するようになって
いない。また、図３４には、図7の前回探索データ取得
・変換処理（S703）および全探索データ保存処理（S70
4）がないことが分かる。したがって、特願平11−14666
7では、探索範囲Srngを広げて局所経路探索を再実行す
る場合には初めから探索をやり直さなければならず、再
実行回数が増加すると若干探索効率が悪くなってしま
う。一方、本発明による局所経路探索方法では、以上説
明したように前回探索データの再利用を巧みに実現して
いるため、同一の隣接サブゴール間に対する局所経路探
索の再実行回数が増加しても探索効率は劣化しない。
【００３０】つぎに、図８の507に示した逆方向への局
所経路探索実行の有無を示すBOOL型変数InvDircSrch
（デフォルトはFALSE）の機能について説明する。図８
に示したサブゴールSg5はサブゴールSg2の隣接サブゴー
ルとして、サブゴールグラフSgGraphに格納されてい
る。Sg2の隣接サブゴールとしてSg5を追加する際、Sg2
からSg5へのアークポインタ505aだけでなく、Sg5からSg
2へのアークポインタ505bも同時に登録する。図１０に
示したSTEP2の隣接サブゴール選択処理S701にて、サブ
ゴール系列から2つの隣接サブゴールを選択する際、該
当するアークセル（AdjSGListのセル）に格納されてい
るInvDircSrchをチェックする。サブゴールSg2，Sg5がS
701にて選択されたとすると、Sg2からSg5へのアークセ
ルのInvDircSrchをチェックし、これがFALSEの場合は、
Sg5からSg2へのアークセルのInvDircSrchをTRUEに変更
し、S704にてSg2からSg5への局所経路探索を行なう。Sg
2からSg5へのアークセルのInvDircSrchがTRUEの場合、
これは過去にSg5からSg2への局所経路探索を実行したこ
とを示している。そこで、Sg5からSg2へのアークセルの
PathToAdjSGをチェックし、この連結リストが空でない
場合（つまり局所経路が求まっていない）、S702にて探
索範囲パラメータrParaを設定し、S703にて前回探索デ
ータの取得・変換を行なった後、S704にてSg5からSg2へ
の局所経路探索を再実行する。Sg5からSg2への局所経路
探索再実行の結果は、Sg5からSg2へのアークセルに格納
される。以上のように、サブゴールグラフに隣接サブゴ
ール間のアークを常に両方向に登録し、逆方向への局所
経路探索実行の有無をチェックすることにより、探索結
果データの再利用性を高めることができ、スタート配置
およびゴール配置を変更して大域経路探索を実行する場
合に探索効率を向上させることができる。例えば、スタ
ート配置とゴール配置を入れ替えて経路探索を再度行な
う場合、既に行なった経路探索の結果を逆方向から検索
できるので、再度、経路を求め直す必要がない。図３３
に示した特願平11−146667におけるサブゴール（有向）
グラフのデータ構造では、隣接サブゴール間に両方向の
アークを登録することは明示的に示されておらず、また
明細書においても言及していない。したがって、スター
ト配置とゴール配置を入れ替えて経路探索を再度行なう
場合、既に求まっている経路を逆方向から再度求め直す
ことになる。
所経路探索実行の有無を示すBOOL型変数InvDircSrch
（デフォルトはFALSE）の機能について説明する。図８
に示したサブゴールSg5はサブゴールSg2の隣接サブゴー
ルとして、サブゴールグラフSgGraphに格納されてい
る。Sg2の隣接サブゴールとしてSg5を追加する際、Sg2
からSg5へのアークポインタ505aだけでなく、Sg5からSg
2へのアークポインタ505bも同時に登録する。図１０に
示したSTEP2の隣接サブゴール選択処理S701にて、サブ
ゴール系列から2つの隣接サブゴールを選択する際、該
当するアークセル（AdjSGListのセル）に格納されてい
るInvDircSrchをチェックする。サブゴールSg2，Sg5がS
701にて選択されたとすると、Sg2からSg5へのアークセ
ルのInvDircSrchをチェックし、これがFALSEの場合は、
Sg5からSg2へのアークセルのInvDircSrchをTRUEに変更
し、S704にてSg2からSg5への局所経路探索を行なう。Sg
2からSg5へのアークセルのInvDircSrchがTRUEの場合、
これは過去にSg5からSg2への局所経路探索を実行したこ
とを示している。そこで、Sg5からSg2へのアークセルの
PathToAdjSGをチェックし、この連結リストが空でない
場合（つまり局所経路が求まっていない）、S702にて探
索範囲パラメータrParaを設定し、S703にて前回探索デ
ータの取得・変換を行なった後、S704にてSg5からSg2へ
の局所経路探索を再実行する。Sg5からSg2への局所経路
探索再実行の結果は、Sg5からSg2へのアークセルに格納
される。以上のように、サブゴールグラフに隣接サブゴ
ール間のアークを常に両方向に登録し、逆方向への局所
経路探索実行の有無をチェックすることにより、探索結
果データの再利用性を高めることができ、スタート配置
およびゴール配置を変更して大域経路探索を実行する場
合に探索効率を向上させることができる。例えば、スタ
ート配置とゴール配置を入れ替えて経路探索を再度行な
う場合、既に行なった経路探索の結果を逆方向から検索
できるので、再度、経路を求め直す必要がない。図３３
に示した特願平11−146667におけるサブゴール（有向）
グラフのデータ構造では、隣接サブゴール間に両方向の
アークを登録することは明示的に示されておらず、また
明細書においても言及していない。したがって、スター
ト配置とゴール配置を入れ替えて経路探索を再度行なう
場合、既に求まっている経路を逆方向から再度求め直す
ことになる。
【００３１】次に、図２０および図２１は図１に示す経
路計画方法を計算機に実装した探索結果を示す図であ
り、本実施の形態による効果を実証するために、２次元
Ｃ空間において経路探索を実行した例を示したものであ
る。この例では、d-C-Hresの分解能は100（各軸0〜99の
整数値）である。又、didPara =18.385としてd-C-Lres
を形成している。又、rParaの設定値は、 rPara = MonoIncFunc[k] MonoIncFunc[k] = 1.0,1.05,1.1,1.2,1.3 for k=0,1,2,
3,4 としている。但し、これらの比は限定ではない。図２０
では、自由空間が広くなっているため、STEP1 のサブゴ
ール系列探索に失敗することなく、rPara=1.0のまま、S
TEP2 が実行され１３回目のSTEP2 において、スタート
配置Ｓからコール配置Ｇに至る経路を求められている様
子を示している。一方、図２１の場合は、自由空間が局
所的に狭いケースなので、途中でSTEP1に失敗してい
る。その後、いくつかの隣接サブゴール間に対してrPar
a>1.0としてSTEP2 の隣接サブゴール間局所経路探索S70
3が実行されている。rPara=1.2としてSg1からSg2までの
局所経路探索に成功した後は、STEP1 に失敗することも
なくSg2からGまでrPara=1.0として探索が進行してい
る。なお、STEP1ではd-C-Lresｄの対角線上の格子点を
隣接格子点としてＡ* 探索を、STEP2 のS703では、d-C-
Hresの対角線上の格子点を隣接格子点とはせずに、修正
Ａ* 探索を適用している。
路計画方法を計算機に実装した探索結果を示す図であ
り、本実施の形態による効果を実証するために、２次元
Ｃ空間において経路探索を実行した例を示したものであ
る。この例では、d-C-Hresの分解能は100（各軸0〜99の
整数値）である。又、didPara =18.385としてd-C-Lres
を形成している。又、rParaの設定値は、 rPara = MonoIncFunc[k] MonoIncFunc[k] = 1.0,1.05,1.1,1.2,1.3 for k=0,1,2,
3,4 としている。但し、これらの比は限定ではない。図２０
では、自由空間が広くなっているため、STEP1 のサブゴ
ール系列探索に失敗することなく、rPara=1.0のまま、S
TEP2 が実行され１３回目のSTEP2 において、スタート
配置Ｓからコール配置Ｇに至る経路を求められている様
子を示している。一方、図２１の場合は、自由空間が局
所的に狭いケースなので、途中でSTEP1に失敗してい
る。その後、いくつかの隣接サブゴール間に対してrPar
a>1.0としてSTEP2 の隣接サブゴール間局所経路探索S70
3が実行されている。rPara=1.2としてSg1からSg2までの
局所経路探索に成功した後は、STEP1 に失敗することも
なくSg2からGまでrPara=1.0として探索が進行してい
る。なお、STEP1ではd-C-Lresｄの対角線上の格子点を
隣接格子点としてＡ* 探索を、STEP2 のS703では、d-C-
Hresの対角線上の格子点を隣接格子点とはせずに、修正
Ａ* 探索を適用している。
【００３２】このような本発明の経路計画法によれば、
局所経路探索の探索範囲を指定するパラメータrParaを
動的に変更しているため、スタート配置やゴール配置が
障害物に囲まれ、非常に狭い領域を通過しないとゴール
配置に到達できないような所でも、見落としがなく経路
を発見することができる。図２２および図２３は比較の
ための従来方式による経路探索結果を示す図であるが、
図はd-C-Hresにおける従来のＣ空間離散化法（修正Ａ*
探索）をそのまま適用した探索結果を示す。図２２およ
び図２３の障害物形状は、それぞれ図２０および図２１
の場合と同じである。図２０と図２２、および図２１と
図２３を比較すると、本発明の経路計画方法は従来の方
法に比べて探索領域が大幅に削減されていることが分か
る。したがって、本発明の経路計画方法によれば、探索
効率が向上し、探索精度が上がり、経路を高速に求める
ことができる。また、ここまでは説明の都合上、２次元
Ｃ空間を例にとって説明してきたが、勿論、本発明の経
路計画方法は３次元以上のＣ空間に対してもそのまま適
用可能である。
局所経路探索の探索範囲を指定するパラメータrParaを
動的に変更しているため、スタート配置やゴール配置が
障害物に囲まれ、非常に狭い領域を通過しないとゴール
配置に到達できないような所でも、見落としがなく経路
を発見することができる。図２２および図２３は比較の
ための従来方式による経路探索結果を示す図であるが、
図はd-C-Hresにおける従来のＣ空間離散化法（修正Ａ*
探索）をそのまま適用した探索結果を示す。図２２およ
び図２３の障害物形状は、それぞれ図２０および図２１
の場合と同じである。図２０と図２２、および図２１と
図２３を比較すると、本発明の経路計画方法は従来の方
法に比べて探索領域が大幅に削減されていることが分か
る。したがって、本発明の経路計画方法によれば、探索
効率が向上し、探索精度が上がり、経路を高速に求める
ことができる。また、ここまでは説明の都合上、２次元
Ｃ空間を例にとって説明してきたが、勿論、本発明の経
路計画方法は３次元以上のＣ空間に対してもそのまま適
用可能である。
【００３３】次に、本発明による第２の実施の形態につ
いて図を参照して説明する。図２４は本発明による第２
の実施の形態に係るロボットの大域動作経路計画制御装
置のブロック図である。図２４において、1601はロボッ
トとその作業環境の幾何モデルを入力するロボット・環
境モデル入力手段であり、1602はロボット・環境モデル
入力手段1601がが出力する幾何モデルを記憶する幾何モ
デル記憶手段である。1603は幾何モデル記憶手段1602の
情報を利用してロボットと環境との干渉を検査する干渉
検査手段である。1604はロボットのスタート配置とゴー
ル配置およびその他のパラメータを入力するスタート・
ゴール配置パラメータ入力手段である。1605はパラメー
タ入力手段1604が出力するパラメータを設定、あるいは
探索の状況に応じて変更するパラメータ設定・変更手段
であり、1606はパラメータ設定・変更手段1605によって
設定されたパラメータを用いて、サブゴール系列探索の
ための離散化配置空間を設定する離散化配置空間設定手
段である。1607は離散化配置空間設定手段1606によって
決められた離散化配置空間において、干渉検査手段1603
の干渉検査結果をを利用してサブゴールの系列を探索す
るサブゴール系列探索手段である。1608はサブゴール系
列探索手段1607によって生成されたサブゴールおよびそ
の候補をグラフ形式で記憶するサブゴールグラフ記憶手
段である。1609はサブゴール系列探索手段1607によって
得られたサブゴール系列をリストの形式で記憶するサブ
ゴール系列リスト記憶手段である。1610はサブゴール系
列リスト記憶手段1609が記憶している情報から１つのサ
ブゴール系列を選択するサブゴール系列選択手段であ
る。1611はサブゴール系列選択手段1610によって選ばれ
たサブゴール系列を一時的に記憶しておくサブゴール系
列一時記憶手段である。1612はサブゴール系列選択手段
1610によって選ばれ、サブゴール系列一時記憶手段1611
に記憶されているサブゴール系列から２つの隣接サブゴ
ールを選択する隣接サブゴール選択手段である。1613は
隣接サブゴール選択手段1612から得られる２つの隣接サ
ブゴール間の局所経路を探索する隣接サブゴール間経路
探索手段であり、1614は隣接サブゴール間経路探索手段
1613が経路を探索する時に探索範囲を制限する探索範囲
制限手段である。1615は隣接サブゴール間経路探索手段
1613が探索した全格子点データをサブゴールグラフの該
当アークに関連付けて保存する探索済み格子点情報保存
手段であり、1616は同一の隣接サブゴール間に対して隣
接サブゴール間経路探索を再実行する場合に探索済み格
子点情報保存手段1615が保存した格子点情報を取得・変
換して再利用させる探索済み格子点情報取得・変換手段
である。1617はサブゴールグラフ記憶手段1608に記憶さ
れている情報とサブゴール系列一時記憶手段1611の情報
を受けて、スタート配置からゴール位置に至る連続した
経路を記憶する経路記憶手段であり、1618は経路記憶手
段1617が記憶している経路を出力する経路出力手段であ
る。
いて図を参照して説明する。図２４は本発明による第２
の実施の形態に係るロボットの大域動作経路計画制御装
置のブロック図である。図２４において、1601はロボッ
トとその作業環境の幾何モデルを入力するロボット・環
境モデル入力手段であり、1602はロボット・環境モデル
入力手段1601がが出力する幾何モデルを記憶する幾何モ
デル記憶手段である。1603は幾何モデル記憶手段1602の
情報を利用してロボットと環境との干渉を検査する干渉
検査手段である。1604はロボットのスタート配置とゴー
ル配置およびその他のパラメータを入力するスタート・
ゴール配置パラメータ入力手段である。1605はパラメー
タ入力手段1604が出力するパラメータを設定、あるいは
探索の状況に応じて変更するパラメータ設定・変更手段
であり、1606はパラメータ設定・変更手段1605によって
設定されたパラメータを用いて、サブゴール系列探索の
ための離散化配置空間を設定する離散化配置空間設定手
段である。1607は離散化配置空間設定手段1606によって
決められた離散化配置空間において、干渉検査手段1603
の干渉検査結果をを利用してサブゴールの系列を探索す
るサブゴール系列探索手段である。1608はサブゴール系
列探索手段1607によって生成されたサブゴールおよびそ
の候補をグラフ形式で記憶するサブゴールグラフ記憶手
段である。1609はサブゴール系列探索手段1607によって
得られたサブゴール系列をリストの形式で記憶するサブ
ゴール系列リスト記憶手段である。1610はサブゴール系
列リスト記憶手段1609が記憶している情報から１つのサ
ブゴール系列を選択するサブゴール系列選択手段であ
る。1611はサブゴール系列選択手段1610によって選ばれ
たサブゴール系列を一時的に記憶しておくサブゴール系
列一時記憶手段である。1612はサブゴール系列選択手段
1610によって選ばれ、サブゴール系列一時記憶手段1611
に記憶されているサブゴール系列から２つの隣接サブゴ
ールを選択する隣接サブゴール選択手段である。1613は
隣接サブゴール選択手段1612から得られる２つの隣接サ
ブゴール間の局所経路を探索する隣接サブゴール間経路
探索手段であり、1614は隣接サブゴール間経路探索手段
1613が経路を探索する時に探索範囲を制限する探索範囲
制限手段である。1615は隣接サブゴール間経路探索手段
1613が探索した全格子点データをサブゴールグラフの該
当アークに関連付けて保存する探索済み格子点情報保存
手段であり、1616は同一の隣接サブゴール間に対して隣
接サブゴール間経路探索を再実行する場合に探索済み格
子点情報保存手段1615が保存した格子点情報を取得・変
換して再利用させる探索済み格子点情報取得・変換手段
である。1617はサブゴールグラフ記憶手段1608に記憶さ
れている情報とサブゴール系列一時記憶手段1611の情報
を受けて、スタート配置からゴール位置に至る連続した
経路を記憶する経路記憶手段であり、1618は経路記憶手
段1617が記憶している経路を出力する経路出力手段であ
る。
【００３４】つぎに以上のような構成の制御装置の動作
について説明する。制御装置全体の動作については、前
実施の形態で示した図１のフローチャートのSTEP1，STE
P2による経路探索が行われるが、以下、その大略要点に
ついて簡単に説明する。スタート・ゴール配置パラメー
タ入力手段1604、パラメータ設定・変更手段1605、離散
化配置空間設定手段1606、において、スタート配置S、
ゴール配置Gに加え、STEP1 の場合のパラメータdisPar
a、STEP2 において探索空間を制限するパラメータrPara
等の設定が行われ、サブゴール系列探索手段1607により
STEP1 による経路探索が実行される。サブゴール系列探
索手段1607により探索されたサブゴール系列および候補
は、サブゴールグラフ記憶手段に、図８に示したような
データ構造で記憶される。又、探索されたサブゴール系
列をリストの形式で、図９に示すようなデータ構造とし
てサブゴール系列リスト記憶手段1609に記憶される。次
に、サブゴールド系列リスト記憶手段に記憶している情
報から１つのサブゴール系列を、サブゴール系列選択手
段1610で選択してサブゴール系列一時記憶手段1611に記
憶させ、そこから隣接サブゴール選択手段1612が２つの
隣接サブゴールを選択する。選択された２つの隣接サブ
ゴール間の局所経路を、隣接サブゴール間経路探索手段
1613がSTEP2 により探索する。この場合に探索範囲制限
手段1614はパラメータ設定手段1605よりパラメータrPar
a等を参照し、図１１に示すような楕円体領域を用いて
探索範囲Srng等を演算して探索範囲を制限する。隣接サ
ブゴール間経路探索手段1613による探索結果は、サブゴ
ールグラフ記憶手段1608に記憶される。又、この間の結
果によっては、バックトラッキングや再探索等の繰り返
し処理も行われる。さらに、隣接サブゴール間経路探索
手段1613が探索した全ての格子点情報は、探索済み格子
点情報保存手段1615によってサブゴールグラフ記憶手段
1608の該当アークに記憶される。また、同一の隣接サブ
ゴール間に対して、隣接サブゴール間経路探索手段1613
が再実行される場合には、探索済み格子点情報取得・変
換手段1616が前回の探索済み格子点情報を取得および変
換し、その情報は隣接サブゴール間経路探索手段1613に
て再利用される。最後に、サブゴールグラフ記憶手段16
08の情報と、サブゴール系列一時記憶手段1611の情報か
ら、スタート配置Sからゴール配置Gに至る連続経路を記
憶している経路記憶手段1617より経路出力手段1618へ経
路情報を出力してロボットを障害物と干渉しないように
動作させる。このように、第２の実施の形態によれば、
動作については説明の都合上２次元空間経路探索を例に
説明したが、勿論３次元以上のＣ空間についてもそのま
ま適用可能であり、経路の探索効率が向上して経路の見
落としが少なくなり、かつ、ロボットの経路を高速で求
めることが可能になる。さらには、探索データの再利用
性に優れている。
について説明する。制御装置全体の動作については、前
実施の形態で示した図１のフローチャートのSTEP1，STE
P2による経路探索が行われるが、以下、その大略要点に
ついて簡単に説明する。スタート・ゴール配置パラメー
タ入力手段1604、パラメータ設定・変更手段1605、離散
化配置空間設定手段1606、において、スタート配置S、
ゴール配置Gに加え、STEP1 の場合のパラメータdisPar
a、STEP2 において探索空間を制限するパラメータrPara
等の設定が行われ、サブゴール系列探索手段1607により
STEP1 による経路探索が実行される。サブゴール系列探
索手段1607により探索されたサブゴール系列および候補
は、サブゴールグラフ記憶手段に、図８に示したような
データ構造で記憶される。又、探索されたサブゴール系
列をリストの形式で、図９に示すようなデータ構造とし
てサブゴール系列リスト記憶手段1609に記憶される。次
に、サブゴールド系列リスト記憶手段に記憶している情
報から１つのサブゴール系列を、サブゴール系列選択手
段1610で選択してサブゴール系列一時記憶手段1611に記
憶させ、そこから隣接サブゴール選択手段1612が２つの
隣接サブゴールを選択する。選択された２つの隣接サブ
ゴール間の局所経路を、隣接サブゴール間経路探索手段
1613がSTEP2 により探索する。この場合に探索範囲制限
手段1614はパラメータ設定手段1605よりパラメータrPar
a等を参照し、図１１に示すような楕円体領域を用いて
探索範囲Srng等を演算して探索範囲を制限する。隣接サ
ブゴール間経路探索手段1613による探索結果は、サブゴ
ールグラフ記憶手段1608に記憶される。又、この間の結
果によっては、バックトラッキングや再探索等の繰り返
し処理も行われる。さらに、隣接サブゴール間経路探索
手段1613が探索した全ての格子点情報は、探索済み格子
点情報保存手段1615によってサブゴールグラフ記憶手段
1608の該当アークに記憶される。また、同一の隣接サブ
ゴール間に対して、隣接サブゴール間経路探索手段1613
が再実行される場合には、探索済み格子点情報取得・変
換手段1616が前回の探索済み格子点情報を取得および変
換し、その情報は隣接サブゴール間経路探索手段1613に
て再利用される。最後に、サブゴールグラフ記憶手段16
08の情報と、サブゴール系列一時記憶手段1611の情報か
ら、スタート配置Sからゴール配置Gに至る連続経路を記
憶している経路記憶手段1617より経路出力手段1618へ経
路情報を出力してロボットを障害物と干渉しないように
動作させる。このように、第２の実施の形態によれば、
動作については説明の都合上２次元空間経路探索を例に
説明したが、勿論３次元以上のＣ空間についてもそのま
ま適用可能であり、経路の探索効率が向上して経路の見
落としが少なくなり、かつ、ロボットの経路を高速で求
めることが可能になる。さらには、探索データの再利用
性に優れている。
【００３５】次に第３の実施の形態のロボットの大域動
作経路計画方法に関して、図３５、図３６、図３７、図
３８を用いて説明する。これは請求項１９、２０の実施
例である。この発明は多数の関節軸を有するロボットに
おいて経路探索を高速化する手法である。請求項１〜18
の手法は高速化に関する有効な手段であるが、ロボット
の関節軸全てにこの手段を適用した場合、探索処理時間
が軸数の増加に対して指数関数的に増大することは避け
られない。この発明は、この処理時間の増大を軽減する
ことが目的である。図３５は、本発明の第３の実施の形
態に関するロボットの大域動作経路計画方法の構成を表
すブロック図である。請求項19はロボットの関節軸のい
くつかには請求項１〜１８を適用し（請求項１９のA軸
群）、それ以外の関節軸（請求項のB軸群）はスタート
配置とゴール配置の間の値を簡易なアルゴリズム（B軸
群チェック値演算手段3503）で導出し、その値を探索時
に固定のチェック値として取り扱うものである。また、
請求項２０は、前記アルゴリズム（B軸群チェック値演
算手段3503）例である。図３５を用いて説明すると、ロ
ボットの各軸のスタート配置およびゴール配置の情報35
01は区別手段によりＡ軸群情報とＢ軸群情報に分けられ
る。Ａ軸群情報はＡ軸群探索値決定手段3504にて探索値
の候補が出力される。Ａ軸群探索値決定手段に使用され
るアルゴリズムは請求項１〜１８に記載された方法であ
る。Ｂ軸群情報はB軸群チェック値演算手段3503にてチ
ェック値が演算される。このB軸群チェック値演算手段3
503のアルゴリズムの概要を説明する。これはA軸群の中
でスタート配置とゴール配置間の移動量の最大軸（M
軸）の移動量（M#dlt）と、探索時のM軸の探索値のスタ
ート位置からの距離（M#VAL）の比（M#VAL／M#DLT）を
ロボットのスタート配置からゴール配置までのB軸群の
各軸の移動量に乗算し、B軸群の各軸のチェック値とす
るものである。Ａ軸群の探索候補値およびＢ軸群のチェ
ック値は、ロボットの幾何学モデル導出手段3505にて、
ロボット幾何学モデルに変換され、その後段の干渉チェ
ック手段3506にてロボットモデルと周囲の障害物との干
渉がチェックされる。尚、チェックした結果、干渉する
場合は、再度、Ａ軸群探索値決定手段により次の探索候
補値、およびB軸群チェック値演算手段により次のチェ
ック値が出力され、ロボットの幾何学モデル導出と干渉
チェックが繰り返される。このようにB軸群に関しては
前記アルゴリズムにより探索毎に固定のチェック値を演
算するため探索処理を行わない。このため探索を行う軸
はA軸群だけとなるため探索のための処理は限定され処
理時間が短くなる。以下に、具体例を用いて請求項20に
記載されたB軸群チェック値演算手段を説明する。図36
は実施例におけるロボットの外形図であり一般的な６軸
垂直多関節ロボットである。図37は実施例におけるロボ
ットの機構構成図であり、J1〜J6の６つの関節自由度を
有する。予めA軸群をJ1〜J3とし、B軸群をJ4〜J6と定義
する。図38の処理フローに基づきサブゴール探索処理内
容を以下に示す。 （１）まず、初期処理として以下の処理を行う スタート軸配置を（s1,s2,…,sn,…,s6）、ゴール軸
配置を（g1,g2,…,gn,…,g6）、スタートからゴールま
での全軸の（J1〜J6）の移動量を for (n=1;n++;n<6) DLT[N]=g[n]-s[n]; により求め A軸群の移動量最大の軸をM軸。そのスタート値をsm、
その移動量をM#DLTとする。（S2401） （２）サブゴール探索処理を以下のように行う。 探索するJ1〜J3に関して請求項１〜18の手法により探
索値（VAL[1〜3]）を求め、その中のM軸の値 VAL[m]か
ら- M軸スタート配置smを差し引いた値をM#VALとする。
(S2402) この回のJ4〜J6のチェック値を以下のように求める。
(S2403) for (n=4;n++;n<6) [ VAL[n]=DLT[n]×M#VAL／M#DLT ] 以上で求まった探索値 VAL[1〜3]及びチェック値VAL
[4〜6]を用いてロボット幾何学モデルを求める。(S240
4) 求めたロボット幾何学モデルにより干渉チェック処理
を行う。(S2405) 干渉がない場合サブゴールとする。(S2406) 干渉する場合は〜の処理をサブゴールが求まるま
で繰り返し実施する。
作経路計画方法に関して、図３５、図３６、図３７、図
３８を用いて説明する。これは請求項１９、２０の実施
例である。この発明は多数の関節軸を有するロボットに
おいて経路探索を高速化する手法である。請求項１〜18
の手法は高速化に関する有効な手段であるが、ロボット
の関節軸全てにこの手段を適用した場合、探索処理時間
が軸数の増加に対して指数関数的に増大することは避け
られない。この発明は、この処理時間の増大を軽減する
ことが目的である。図３５は、本発明の第３の実施の形
態に関するロボットの大域動作経路計画方法の構成を表
すブロック図である。請求項19はロボットの関節軸のい
くつかには請求項１〜１８を適用し（請求項１９のA軸
群）、それ以外の関節軸（請求項のB軸群）はスタート
配置とゴール配置の間の値を簡易なアルゴリズム（B軸
群チェック値演算手段3503）で導出し、その値を探索時
に固定のチェック値として取り扱うものである。また、
請求項２０は、前記アルゴリズム（B軸群チェック値演
算手段3503）例である。図３５を用いて説明すると、ロ
ボットの各軸のスタート配置およびゴール配置の情報35
01は区別手段によりＡ軸群情報とＢ軸群情報に分けられ
る。Ａ軸群情報はＡ軸群探索値決定手段3504にて探索値
の候補が出力される。Ａ軸群探索値決定手段に使用され
るアルゴリズムは請求項１〜１８に記載された方法であ
る。Ｂ軸群情報はB軸群チェック値演算手段3503にてチ
ェック値が演算される。このB軸群チェック値演算手段3
503のアルゴリズムの概要を説明する。これはA軸群の中
でスタート配置とゴール配置間の移動量の最大軸（M
軸）の移動量（M#dlt）と、探索時のM軸の探索値のスタ
ート位置からの距離（M#VAL）の比（M#VAL／M#DLT）を
ロボットのスタート配置からゴール配置までのB軸群の
各軸の移動量に乗算し、B軸群の各軸のチェック値とす
るものである。Ａ軸群の探索候補値およびＢ軸群のチェ
ック値は、ロボットの幾何学モデル導出手段3505にて、
ロボット幾何学モデルに変換され、その後段の干渉チェ
ック手段3506にてロボットモデルと周囲の障害物との干
渉がチェックされる。尚、チェックした結果、干渉する
場合は、再度、Ａ軸群探索値決定手段により次の探索候
補値、およびB軸群チェック値演算手段により次のチェ
ック値が出力され、ロボットの幾何学モデル導出と干渉
チェックが繰り返される。このようにB軸群に関しては
前記アルゴリズムにより探索毎に固定のチェック値を演
算するため探索処理を行わない。このため探索を行う軸
はA軸群だけとなるため探索のための処理は限定され処
理時間が短くなる。以下に、具体例を用いて請求項20に
記載されたB軸群チェック値演算手段を説明する。図36
は実施例におけるロボットの外形図であり一般的な６軸
垂直多関節ロボットである。図37は実施例におけるロボ
ットの機構構成図であり、J1〜J6の６つの関節自由度を
有する。予めA軸群をJ1〜J3とし、B軸群をJ4〜J6と定義
する。図38の処理フローに基づきサブゴール探索処理内
容を以下に示す。 （１）まず、初期処理として以下の処理を行う スタート軸配置を（s1,s2,…,sn,…,s6）、ゴール軸
配置を（g1,g2,…,gn,…,g6）、スタートからゴールま
での全軸の（J1〜J6）の移動量を for (n=1;n++;n<6) DLT[N]=g[n]-s[n]; により求め A軸群の移動量最大の軸をM軸。そのスタート値をsm、
その移動量をM#DLTとする。（S2401） （２）サブゴール探索処理を以下のように行う。 探索するJ1〜J3に関して請求項１〜18の手法により探
索値（VAL[1〜3]）を求め、その中のM軸の値 VAL[m]か
ら- M軸スタート配置smを差し引いた値をM#VALとする。
(S2402) この回のJ4〜J6のチェック値を以下のように求める。
(S2403) for (n=4;n++;n<6) [ VAL[n]=DLT[n]×M#VAL／M#DLT ] 以上で求まった探索値 VAL[1〜3]及びチェック値VAL
[4〜6]を用いてロボット幾何学モデルを求める。(S240
4) 求めたロボット幾何学モデルにより干渉チェック処理
を行う。(S2405) 干渉がない場合サブゴールとする。(S2406) 干渉する場合は〜の処理をサブゴールが求まるま
で繰り返し実施する。
【００３６】つぎに、第４の実施の形態である探索効率
をさらに効率化するための干渉検査処理の実施形態につ
いて説明する。図３９は、ステップ１あるいはステップ
２におけるグラフ探索が進行する様子を示している。a
1,a2,…,a4, b1,b2,b3の順にノードが生成され、各ノー
ドの格子点座標値（関節角座標値）に対して幾何学モデ
ル手段とモデル同士の干渉検出手段を用いて干渉検査が
行われたことを示している。また、b1はロボットと障害
物が干渉したことを、破線で示したc1以降のノードは将
来のグラフ探索の進行の様子を示している。従来の干渉
検査処理に基くグラフ探索では、ノードがゴール配置G
（あるいはサブゴール）に到達するまでにc1からe3まで
8回干渉検査を今後に行なうことになる。ここで、ノー
ドb1におけるロボットと障害物との干渉具合が図４０に
示す状態にあったとする。図より、Link1と障害物が干
渉しているため、q1=iに対してLink2の関節角q2を如何
に動かしてもロボットと障害物が（Link1で）干渉を起
こすことは明白である。したがって、図４１のように、
ノードc1,d2,e2に関しては、幾何学モデル手段とモデル
同士の干渉検出手段を用いずとも干渉ノードであること
が分かり、モデル同士の干渉検出手段を用いた正味の干
渉検査回数を8回から5回に減らすことが可能である。従
来の干渉検査処理では、干渉検査結果として、干渉の有
無のみを通知しているため、上述したような効率化は望
めない。本発明では、これを改め、干渉検査処理におい
て、干渉の有無だけでなく、ロボットのどの部位が障害
物と干渉するかという情報も利用することにより探索の
更なる効率化を図る。図４２に本発明による干渉検出処
理のフローチャートを示す。ここでは、ロボットとして
マニピュレータを例に説明する。N自由度マニピュレー
タのベース部の関節角から手先部の関節角に向かって順
番にq(1),q(2),…,q(N)と表し、第k(k=1,2,…,N)番目の
関節角q(k)の変化によって直接駆動されるリンクをLink
(k)と表す。図４２のM3802は、幾何学モデル手段とモデ
ル同士の干渉検出手段を用いた干渉検査処理である。M3
802では、まず、リンクのカウンタkをk=1に初期化し（S
3806）、Link(k)と障害物との干渉を検査する（S380
7）。Link(k)が干渉を起こさない場合は、カウンタkを
１増加（S3809）させてS3807に戻り、同様の処理をk=N
まで繰り返す。Link(1)からLink(N)までの全てのLinkが
非干渉となる場合は、ロボットと障害物は非干渉として
処理を終了する（S3810）。S3807にて、Link(k)が障害
物と干渉する場合は、図４３に示すList(k)に干渉を起
こした関節角座標値(q(1),q(2),…, q(k))を保存し（S3
811）、ロボットと障害物は干渉として処理を終了する
（S3812）。図４２に示したList(1)からList(N)は、図
４２のS3807で検出されたリンクと障害物が干渉する関
節角座標値を全て保存するためのリストであり、Link
(k)が干渉する関節角座標値がList(k)に対応している。
図４２のM3801は、幾何学モデル手段とモデル同士の干
渉検出手段を用いた干渉検査処理M3802の前に行なわれ
る処理であり、検査対象の関節角座標値とList(1)からL
ist(N)に保存されている（干渉する）関節角座標値を比
較して、一致するものがあるかどうかを検索する処理で
ある。M3801では、まず、リンクのカウンタkをk=1に初
期化し（S3801）、List(k)の検索を行なう（S3802）。L
ist(k)の中に検査対象の関節角座標値と一致するものが
ない場合は、カウンタkを１増加（S3804）させてS3802
に戻り、同様の処理をk=Nまで繰り返す。List(1)からLi
st(N)までの全リストの中に検査対象の関節角座標値と
一致するものがない場合は、M3802に移る。List(k)の中
に検査対象の関節角座標値と一致するものがある場合
は、M3801に移らず、ロボットと障害物は干渉として処
理を終了する（S3805）。以上説明したように、本発明
の干渉検査処理により、計算負荷の大きいモデル同士の
干渉検出手段を用いた正味の干渉検査回数（M3802）が
減少し、ステップ１およびステップにおける経路探索が
さらに効率化される。
をさらに効率化するための干渉検査処理の実施形態につ
いて説明する。図３９は、ステップ１あるいはステップ
２におけるグラフ探索が進行する様子を示している。a
1,a2,…,a4, b1,b2,b3の順にノードが生成され、各ノー
ドの格子点座標値（関節角座標値）に対して幾何学モデ
ル手段とモデル同士の干渉検出手段を用いて干渉検査が
行われたことを示している。また、b1はロボットと障害
物が干渉したことを、破線で示したc1以降のノードは将
来のグラフ探索の進行の様子を示している。従来の干渉
検査処理に基くグラフ探索では、ノードがゴール配置G
（あるいはサブゴール）に到達するまでにc1からe3まで
8回干渉検査を今後に行なうことになる。ここで、ノー
ドb1におけるロボットと障害物との干渉具合が図４０に
示す状態にあったとする。図より、Link1と障害物が干
渉しているため、q1=iに対してLink2の関節角q2を如何
に動かしてもロボットと障害物が（Link1で）干渉を起
こすことは明白である。したがって、図４１のように、
ノードc1,d2,e2に関しては、幾何学モデル手段とモデル
同士の干渉検出手段を用いずとも干渉ノードであること
が分かり、モデル同士の干渉検出手段を用いた正味の干
渉検査回数を8回から5回に減らすことが可能である。従
来の干渉検査処理では、干渉検査結果として、干渉の有
無のみを通知しているため、上述したような効率化は望
めない。本発明では、これを改め、干渉検査処理におい
て、干渉の有無だけでなく、ロボットのどの部位が障害
物と干渉するかという情報も利用することにより探索の
更なる効率化を図る。図４２に本発明による干渉検出処
理のフローチャートを示す。ここでは、ロボットとして
マニピュレータを例に説明する。N自由度マニピュレー
タのベース部の関節角から手先部の関節角に向かって順
番にq(1),q(2),…,q(N)と表し、第k(k=1,2,…,N)番目の
関節角q(k)の変化によって直接駆動されるリンクをLink
(k)と表す。図４２のM3802は、幾何学モデル手段とモデ
ル同士の干渉検出手段を用いた干渉検査処理である。M3
802では、まず、リンクのカウンタkをk=1に初期化し（S
3806）、Link(k)と障害物との干渉を検査する（S380
7）。Link(k)が干渉を起こさない場合は、カウンタkを
１増加（S3809）させてS3807に戻り、同様の処理をk=N
まで繰り返す。Link(1)からLink(N)までの全てのLinkが
非干渉となる場合は、ロボットと障害物は非干渉として
処理を終了する（S3810）。S3807にて、Link(k)が障害
物と干渉する場合は、図４３に示すList(k)に干渉を起
こした関節角座標値(q(1),q(2),…, q(k))を保存し（S3
811）、ロボットと障害物は干渉として処理を終了する
（S3812）。図４２に示したList(1)からList(N)は、図
４２のS3807で検出されたリンクと障害物が干渉する関
節角座標値を全て保存するためのリストであり、Link
(k)が干渉する関節角座標値がList(k)に対応している。
図４２のM3801は、幾何学モデル手段とモデル同士の干
渉検出手段を用いた干渉検査処理M3802の前に行なわれ
る処理であり、検査対象の関節角座標値とList(1)からL
ist(N)に保存されている（干渉する）関節角座標値を比
較して、一致するものがあるかどうかを検索する処理で
ある。M3801では、まず、リンクのカウンタkをk=1に初
期化し（S3801）、List(k)の検索を行なう（S3802）。L
ist(k)の中に検査対象の関節角座標値と一致するものが
ない場合は、カウンタkを１増加（S3804）させてS3802
に戻り、同様の処理をk=Nまで繰り返す。List(1)からLi
st(N)までの全リストの中に検査対象の関節角座標値と
一致するものがない場合は、M3802に移る。List(k)の中
に検査対象の関節角座標値と一致するものがある場合
は、M3801に移らず、ロボットと障害物は干渉として処
理を終了する（S3805）。以上説明したように、本発明
の干渉検査処理により、計算負荷の大きいモデル同士の
干渉検出手段を用いた正味の干渉検査回数（M3802）が
減少し、ステップ１およびステップにおける経路探索が
さらに効率化される。
【００３７】つぎに本発明の第５の実施の形態につい
て、図４４を用いて説明する。第５の実施の形態は、以
上説明したサブゴール系列探索に基く２段階経路探索方
式を、シーケンシャル探索に適用した例である。図４４
に示した処理を実行する際には、ロボットをどのように
サブロボットに分割するかを指定しておく。図４５・図
４６にその概念をおよび図４７にそのフローチャートを
示した従来のシーケンシャル探索では、マニピュレータ
を各リンク（１自由度）ごとにサブロボットに分割して
いた。本発明では、例えば、７自由度のマニピュレータ
をハンド（３自由度）、上腕（２自由度）、下腕（３自
由度）に分割するといった任意の分割が可能である。ま
た、ベース部に近いサブロボットから手先部に向かって
順番に、0, 1, 2, … と番号付けする。S3501では、サ
ブロボットのカウンタkがゼロにセットされる。S3502で
は、kがサブロボットの個数NumOfSubRobo未満であるか
どうかが判断される。kがNumOfSubRobo未満である場合
は、S3503に移り、サブロボットkの探索空間の設定が行
なわれる。図４６(e)(g)に示したように、シーケンシャ
ル探索では、サブロボットの探索空間は１つ手前のサブ
ロボットまでの経路（図４６のt2およびt3）によって拘
束される。S3503では、その拘束部分の設定を行なう。
サブロボットkの探索空間を設定した後、S3504にてサブ
ロボットkに対して２段階経路探索を行なう。S3504の2
段階経路探索に成功した場合は、S3505にて求まったサ
ブロボットkの経路を保存し、S3506にてサブロボットの
カウンタkを１だけ増加させ、S3502に戻る。S3504の2段
階経路探索に失敗した場合は、S3507にてサブロボット
のカウンタkを１だけ減少させ、S3508にてサブロボット
のカウンタkがゼロ未満であるかどうかが判断される。
サブロボットのカウンタkがゼロ未満の場合は、探索失
敗として終了する（S3512）。サブロボットのカウンタk
がゼロ以上の場合は、S3509にてバックトラッキングの
前処理を行ない、S3502に戻る。S3509のバックトラッキ
ング前処理では、サブロボットk（S3507を実行する前の
サブロボットk-1）のサブゴール空間のどのサブゴール
あるいはサブゴール間アークが原因でサブロボットk+1
（S3507を実行する前のサブロボットk）の2段階経路探
索に失敗したかを推定し、該当するサブゴールあるいは
サブゴール間アークに重み係数を加算あるいは積算する
処理を行なう。S3509の処理によりサブロボットkに対す
る2段階経路探索S304が再実行された際には、上記重み
係数の加算あるいは積算処理の効果により、前回求めた
サブゴール系列とは異なったものが探索される。ここ
で、サブゴールあるいはサブゴール間アークへの重み付
け処理は、そのサブゴールあるいはサブゴール間アーク
を以後選択禁止にするものではなく、あくまで選択しに
くくするためのものである。したがって、バックトラッ
キング回数が多くなり、サブゴール空間の広範囲に渡っ
て重み付けがなされた場合、以前に重み係数が加算ある
いは積算されたサブゴールが再度選択される余地が残っ
ている。Guptaらが提案しているシーケンシャル探索の
バックトラッキング前処理では、本発明で提案している
重み付けとは異なり、仮想的な障害物を挿入する処理を
行なっているため、バックトラッキング回数が多くなる
と容易に探索が失敗する場合がある。S3502にて、サブ
ロボットのカウンタkがサブロボットの個数NumOfSubRob
o以上になっ場合、すなわち、全てのサブロボットに対
してS3504の2段階経路探索に成功した場合は、S3510に
てロボット全体の経路を取得し、探索成功として処理を
終了する（S3511）。このように、本発明の第５の実施
の形態によれば、シーケンシャル探索におけるサブロボ
ットの自由度を２以上とすることが可能であり、各サブ
ロボットの経路を第１および第２の実施の形態で説明し
た２段階経路探索方式により求めるので、経路の質が良
く、経路探索がさらに高速化される。また、サブロボッ
ト間バックトラッキングの前処理方法として、従来の仮
想障害物の挿入とは異なり、サブロボットのサブゴール
グラフへの重みの加算あるいは積算方式をとっているの
で、特定のサブロボットへのバックトラッキング回数が
増加しても、経路探索に失敗することが無く、経路の見
落としが防止される。
て、図４４を用いて説明する。第５の実施の形態は、以
上説明したサブゴール系列探索に基く２段階経路探索方
式を、シーケンシャル探索に適用した例である。図４４
に示した処理を実行する際には、ロボットをどのように
サブロボットに分割するかを指定しておく。図４５・図
４６にその概念をおよび図４７にそのフローチャートを
示した従来のシーケンシャル探索では、マニピュレータ
を各リンク（１自由度）ごとにサブロボットに分割して
いた。本発明では、例えば、７自由度のマニピュレータ
をハンド（３自由度）、上腕（２自由度）、下腕（３自
由度）に分割するといった任意の分割が可能である。ま
た、ベース部に近いサブロボットから手先部に向かって
順番に、0, 1, 2, … と番号付けする。S3501では、サ
ブロボットのカウンタkがゼロにセットされる。S3502で
は、kがサブロボットの個数NumOfSubRobo未満であるか
どうかが判断される。kがNumOfSubRobo未満である場合
は、S3503に移り、サブロボットkの探索空間の設定が行
なわれる。図４６(e)(g)に示したように、シーケンシャ
ル探索では、サブロボットの探索空間は１つ手前のサブ
ロボットまでの経路（図４６のt2およびt3）によって拘
束される。S3503では、その拘束部分の設定を行なう。
サブロボットkの探索空間を設定した後、S3504にてサブ
ロボットkに対して２段階経路探索を行なう。S3504の2
段階経路探索に成功した場合は、S3505にて求まったサ
ブロボットkの経路を保存し、S3506にてサブロボットの
カウンタkを１だけ増加させ、S3502に戻る。S3504の2段
階経路探索に失敗した場合は、S3507にてサブロボット
のカウンタkを１だけ減少させ、S3508にてサブロボット
のカウンタkがゼロ未満であるかどうかが判断される。
サブロボットのカウンタkがゼロ未満の場合は、探索失
敗として終了する（S3512）。サブロボットのカウンタk
がゼロ以上の場合は、S3509にてバックトラッキングの
前処理を行ない、S3502に戻る。S3509のバックトラッキ
ング前処理では、サブロボットk（S3507を実行する前の
サブロボットk-1）のサブゴール空間のどのサブゴール
あるいはサブゴール間アークが原因でサブロボットk+1
（S3507を実行する前のサブロボットk）の2段階経路探
索に失敗したかを推定し、該当するサブゴールあるいは
サブゴール間アークに重み係数を加算あるいは積算する
処理を行なう。S3509の処理によりサブロボットkに対す
る2段階経路探索S304が再実行された際には、上記重み
係数の加算あるいは積算処理の効果により、前回求めた
サブゴール系列とは異なったものが探索される。ここ
で、サブゴールあるいはサブゴール間アークへの重み付
け処理は、そのサブゴールあるいはサブゴール間アーク
を以後選択禁止にするものではなく、あくまで選択しに
くくするためのものである。したがって、バックトラッ
キング回数が多くなり、サブゴール空間の広範囲に渡っ
て重み付けがなされた場合、以前に重み係数が加算ある
いは積算されたサブゴールが再度選択される余地が残っ
ている。Guptaらが提案しているシーケンシャル探索の
バックトラッキング前処理では、本発明で提案している
重み付けとは異なり、仮想的な障害物を挿入する処理を
行なっているため、バックトラッキング回数が多くなる
と容易に探索が失敗する場合がある。S3502にて、サブ
ロボットのカウンタkがサブロボットの個数NumOfSubRob
o以上になっ場合、すなわち、全てのサブロボットに対
してS3504の2段階経路探索に成功した場合は、S3510に
てロボット全体の経路を取得し、探索成功として処理を
終了する（S3511）。このように、本発明の第５の実施
の形態によれば、シーケンシャル探索におけるサブロボ
ットの自由度を２以上とすることが可能であり、各サブ
ロボットの経路を第１および第２の実施の形態で説明し
た２段階経路探索方式により求めるので、経路の質が良
く、経路探索がさらに高速化される。また、サブロボッ
ト間バックトラッキングの前処理方法として、従来の仮
想障害物の挿入とは異なり、サブロボットのサブゴール
グラフへの重みの加算あるいは積算方式をとっているの
で、特定のサブロボットへのバックトラッキング回数が
増加しても、経路探索に失敗することが無く、経路の見
落としが防止される。
【００３８】
【発明の効果】以上説明したように、本発明によれば、
ロボットのスタート配置とゴール配置が与えられた時、
その間を大まかに分割するSTEP1の探索処理により離散
化Ｃ空間を形成してグラフ探索手法を用いてサブゴール
系列を探索し、求められたサブゴール間に、STEP2 の探
索処理による、更に、微小な間隔の格子点を持つ離散化
Ｃ空間を形成してグラフ探索手法を用いて局所経路を求
めるという２段階の経路探索を実行するので、探索空間
が大幅に抑制されて探索効率が向上し、ロボットの制御
に関するリソース消費を抑えて全体として性能を向上さ
せるという効果がある。更に、経路探索処理について
も、STEP1 による探索結果をサブゴール系列リストに格
納し、それを用いてSTEP2の局所経路探索を実行し結果
をサブゴールグラフに反映させて、無駄を省いた局所経
路探索に必要最小限な情報をストアすることでデータ処
理の効率化を図り、STEP2の探索が失敗した場合にも、
バックトラッキング（再探索）を繰り返し実行するの
で、３次元作業空間で自由度の大きな多関節マニュピレ
ータに対しても、高速で実用的な経路計画が実現できる
という効果がある。更に、局所経路探索の探索範囲を段
階的に広げながら探索を行っているので、自由空間が非
常に狭くなっている場合でも、経路を見落とす可能性が
小さくなるという効果がある。更に、STEP2の各隣接サ
ブゴール間局所経路探索の探索済み全格子点情報をサブ
ゴールグラフの該当するアークに関連付けて保存し、そ
の情報を両方向から検索可能である。そのため、探索情
報の再利用性に優れ、経路探索がさらに高速化されると
いう効果がある。更に、請求項１９に記載のロボットの
大域動作経路計画方法によれば、前記B軸群の軸に対し
ては、一定のルールに基づき、チェック値が一意に導出
されるため、探索アルゴリズムを必要としない。このた
め探索手法の適用はA軸群だけになり、全ての軸に探索
アルゴリズムを適用した場合に比べ、大幅に経路探索時
間を短縮できるという効果がある。また請求項２０に記
載のロボットの大域動作経路計画方法によれば、簡単な
四則演算でB軸群のチェック値を算出することができ、
経路探索時間を短縮できる効果がある。更に、STEP1お
よびSTEP2において、ロボットとその作業環境にある障
害物の幾何学的形状とそれらの配置を記述する計算機上
の幾何学モデル手段と、モデル同士の干渉を検出する計
算機上の干渉検出手段とを用いて、検査対象である離散
化配置空間上の格子点関節角座標値に対するロボットと
障害物との干渉を検査する際に、ロボットのどの部位が
障害物と干渉したかという情報を干渉検出手段から受取
り、そのロボット部位と関連付けて関節角座標値を記憶
し、別の格子点関節角座標値に対するロボットと障害物
との干渉を検査する際に、その格子点関節角座標値を記
憶した関節角座標値の情報と照らし合わせることによ
り、幾何学モデル手段とモデル同士の干渉検出手段を用
いた計算量の多い干渉検査の回数を大幅に検証できるた
め、探索がさらに高速化されるという効果がある。更
に、シーケンシャル探索において、サブロボットの自由
度を１あるいは２以上とし、各サブロボットの経路を前
記ステップ１および前記ステップ２に基く２段階経路探
索により求めるので、経路の質が良く、経路探索が更に
高速化されるという効果がある。更に、シーケンシャル
探索において、１つ手前のサブロボットの経路探索にバ
ックトラッキングする際、そのサブロボットのサブゴー
ルグラフのどのサブゴールあるいはサブゴール間アーク
が原因で経路探索に失敗したかを推定し、そのサブゴー
ルあるいはサブゴール間アークに対して重みを加算ある
いは積算するので、特定のサブロボットへのバックトラ
ッキング回数が増加しても経路探索に失敗すること無
く、経路の見落としが防止されるという効果がある。
ロボットのスタート配置とゴール配置が与えられた時、
その間を大まかに分割するSTEP1の探索処理により離散
化Ｃ空間を形成してグラフ探索手法を用いてサブゴール
系列を探索し、求められたサブゴール間に、STEP2 の探
索処理による、更に、微小な間隔の格子点を持つ離散化
Ｃ空間を形成してグラフ探索手法を用いて局所経路を求
めるという２段階の経路探索を実行するので、探索空間
が大幅に抑制されて探索効率が向上し、ロボットの制御
に関するリソース消費を抑えて全体として性能を向上さ
せるという効果がある。更に、経路探索処理について
も、STEP1 による探索結果をサブゴール系列リストに格
納し、それを用いてSTEP2の局所経路探索を実行し結果
をサブゴールグラフに反映させて、無駄を省いた局所経
路探索に必要最小限な情報をストアすることでデータ処
理の効率化を図り、STEP2の探索が失敗した場合にも、
バックトラッキング（再探索）を繰り返し実行するの
で、３次元作業空間で自由度の大きな多関節マニュピレ
ータに対しても、高速で実用的な経路計画が実現できる
という効果がある。更に、局所経路探索の探索範囲を段
階的に広げながら探索を行っているので、自由空間が非
常に狭くなっている場合でも、経路を見落とす可能性が
小さくなるという効果がある。更に、STEP2の各隣接サ
ブゴール間局所経路探索の探索済み全格子点情報をサブ
ゴールグラフの該当するアークに関連付けて保存し、そ
の情報を両方向から検索可能である。そのため、探索情
報の再利用性に優れ、経路探索がさらに高速化されると
いう効果がある。更に、請求項１９に記載のロボットの
大域動作経路計画方法によれば、前記B軸群の軸に対し
ては、一定のルールに基づき、チェック値が一意に導出
されるため、探索アルゴリズムを必要としない。このた
め探索手法の適用はA軸群だけになり、全ての軸に探索
アルゴリズムを適用した場合に比べ、大幅に経路探索時
間を短縮できるという効果がある。また請求項２０に記
載のロボットの大域動作経路計画方法によれば、簡単な
四則演算でB軸群のチェック値を算出することができ、
経路探索時間を短縮できる効果がある。更に、STEP1お
よびSTEP2において、ロボットとその作業環境にある障
害物の幾何学的形状とそれらの配置を記述する計算機上
の幾何学モデル手段と、モデル同士の干渉を検出する計
算機上の干渉検出手段とを用いて、検査対象である離散
化配置空間上の格子点関節角座標値に対するロボットと
障害物との干渉を検査する際に、ロボットのどの部位が
障害物と干渉したかという情報を干渉検出手段から受取
り、そのロボット部位と関連付けて関節角座標値を記憶
し、別の格子点関節角座標値に対するロボットと障害物
との干渉を検査する際に、その格子点関節角座標値を記
憶した関節角座標値の情報と照らし合わせることによ
り、幾何学モデル手段とモデル同士の干渉検出手段を用
いた計算量の多い干渉検査の回数を大幅に検証できるた
め、探索がさらに高速化されるという効果がある。更
に、シーケンシャル探索において、サブロボットの自由
度を１あるいは２以上とし、各サブロボットの経路を前
記ステップ１および前記ステップ２に基く２段階経路探
索により求めるので、経路の質が良く、経路探索が更に
高速化されるという効果がある。更に、シーケンシャル
探索において、１つ手前のサブロボットの経路探索にバ
ックトラッキングする際、そのサブロボットのサブゴー
ルグラフのどのサブゴールあるいはサブゴール間アーク
が原因で経路探索に失敗したかを推定し、そのサブゴー
ルあるいはサブゴール間アークに対して重みを加算ある
いは積算するので、特定のサブロボットへのバックトラ
ッキング回数が増加しても経路探索に失敗すること無
く、経路の見落としが防止されるという効果がある。
【図１】本発明による第１の実施の形態に係るロボット
の大域動作経路計画方法のフローチャートである。
の大域動作経路計画方法のフローチャートである。
【図２】図１のS102およびS103に示す離散化Ｃ空間構成
法を示す図である。
法を示す図である。
【図３】図１のS102およびS103に示す離散化Ｃ空間構成
法を示す図である。
法を示す図である。
【図４】図１のS104に示すサブゴール系列探索の結果を
示す図である。
示す図である。
【図５】図１に示すサブゴール系列探索結果を示す図で
ある。
ある。
【図６】図１に示すSTEP1の処理の内部フローチャート
である。
である。
【図７】グラフ探索に用いる連結リストのデータ構造例
を示す図である。
を示す図である。
【図８】図６に示すサブゴールグラフのデータ構造を示
す図である。
す図である。
【図９】図１に示すサブゴール系列リストのデータ構造
を示す図である。
を示す図である。
【図１０】図１に示すSTEP2 の内部フローチャートであ
る。
る。
【図１１】図１０に示す経路探索範囲を制限する局所領
域を示す図である。
域を示す図である。
【図１２】隣接サブゴール間局所経路探索の進行状況を
示す図である。
示す図である。
【図１３】図１２のサブゴールSg1付近を拡大し、局所
経路探索が進行する様子を示す図である。
経路探索が進行する様子を示す図である。
【図１４】図１３における隣接サブゴール間局所経路探
索の進行状況のデータ構造をツリー構造で示したもの
（探索木）である。
索の進行状況のデータ構造をツリー構造で示したもの
（探索木）である。
【図１５】図１４の探索木を計算機上に実現したデータ
構造を示している。
構造を示している。
【図１６】隣接サブゴール間局所経路探索において前回
探索データを変換して再利用する様子を示す図で、図１
３から更に局所経路探索（グラフ探索）を進めた状況を
示している。
探索データを変換して再利用する様子を示す図で、図１
３から更に局所経路探索（グラフ探索）を進めた状況を
示している。
【図１７】隣接サブゴール間局所経路探索において前回
探索データを変換して再利用する様子を示す図で、取得
したNextOPENLISTをOPENLISTに移し替えた後の状態を示
している。
探索データを変換して再利用する様子を示す図で、取得
したNextOPENLISTをOPENLISTに移し替えた後の状態を示
している。
【図１８】隣接サブゴール間局所経路探索において前回
探索データを変換する処理のフローチャートである。
探索データを変換する処理のフローチャートである。
【図１９】探索範囲を広くして隣接サブゴール間局所経
路探索を再実行した結果を示す図である。
路探索を再実行した結果を示す図である。
【図２０】図１に示す経路計画方法を計算機に実装した
探索結果を示す図で、自由空間が広くなっているケース
である。
探索結果を示す図で、自由空間が広くなっているケース
である。
【図２１】図１に示す経路計画方法を計算機に実装した
探索結果を示す図で、図２０よりも自由空間が局所的に
狭いケースである。
探索結果を示す図で、図２０よりも自由空間が局所的に
狭いケースである。
【図２２】図２０に示す経路探索結果の比較図である。
【図２３】図２１に示す経路探索結果の比較図である。
【図２４】本発明による第２の実施の形態に係るロボッ
トの大域動作経路計画制御装置のブロック図である。
トの大域動作経路計画制御装置のブロック図である。
【図２５】２次元作業空間1703における３自由度マニピ
ュレータ1701が置かれている座標系およびその配置空間
を示す図である。
ュレータ1701が置かれている座標系およびその配置空間
を示す図である。
【図２６】２次元作業空間1703における３自由度移動ロ
ボット1702が置かれている座標系およびロボットの配置
空間を示す図である。
ボット1702が置かれている座標系およびロボットの配置
空間を示す図である。
【図２７】３自由度マニピュレータ1701の場合、関節空
間(q1,q2,q3)がＣ空間となることを示す図である。
間(q1,q2,q3)がＣ空間となることを示す図である。
【図２８】２次元Ｃ空間1801を示す図である。
【図２９】Ｃ空間1801を1802のように微小間隔で離散化
した格子点の空間すなわち離散化Ｃ空間を示す図であ
る。
した格子点の空間すなわち離散化Ｃ空間を示す図であ
る。
【図３０】２次元離散化Ｃ空間においてノード1-2が選
択されて展開されている状況を示す図である。
択されて展開されている状況を示す図である。
【図３１】図３０のグラフ探索進行状況をツリー構造
（探索木と呼ばれる）で表現した図である。
（探索木と呼ばれる）で表現した図である。
【図３２】特開平10−97316における多数の移動物体を
伴うシステムの移動計画および制御方法のシステム構成
ブロック図である。
伴うシステムの移動計画および制御方法のシステム構成
ブロック図である。
【図３３】特願平11−146667におけるサブゴール有向グ
ラフのデータ構造を示す図である。
ラフのデータ構造を示す図である。
【図３４】特願平11−146667におけるSTEP2のフローチ
ャートである。
ャートである。
【図３５】第３の実施の形態に関するロボットの大域動
作経路計画方法の構成を表すブロック図である
作経路計画方法の構成を表すブロック図である
【図３６】第３の実施の形態のロボットの外形図であ
る。
る。
【図３７】第３の実施の形態のロボットの機構構成図で
ある。
ある。
【図３８】第３の実施の形態のサブゴール探索処理のフ
ローチャートである。
ローチャートである。
【図３９】従来のステップ１あるいはステップ２におい
てグラフ探索が進行する様子を示す図である。
てグラフ探索が進行する様子を示す図である。
【図４０】ロボットと障害物との干渉例を示す図であ
る。
る。
【図４１】本発明によるステップ１あるいはステップ２
においてグラフ探索が進行する様子を示す図である。
においてグラフ探索が進行する様子を示す図である。
【図４２】本発明の第４の実施の形態に係る干渉検査処
理のフローチャートである。
理のフローチャートである。
【図４３】各リンクごとの干渉ノード座標値を格納する
リストである。
リストである。
【図４４】本発明による第５の実施の形態に係るロボッ
トの大域動作経路計画方法のフローチャートである。
トの大域動作経路計画方法のフローチャートである。
【図４５】従来のシーケンシャル探索（Guptaの文献）
の概念の前半を示す図である。
の概念の前半を示す図である。
【図４６】従来のシーケンシャル探索（Guptaの文献）
の概念の後半を示す図である。
の概念の後半を示す図である。
【図４７】従来のシーケンシャル探索（Guptaの文献）
のフローチャートである。
のフローチャートである。
２０１、１８０３ スタート配置 ２０２、１８０４ ゴール配置 ２０３ サブゴール空間の離散化間隔 ２０４、８０４、９０４、１１０４，１３０４、１７０
５、１８０５ C空間の障害物 ２０５ サブゴール ４０１ 連結リストの基本クラス ４０２ 連結リストの基本クラスのセル ４０３ グラフ探索に用いる連結リスト ４０４ グラフ探索に用いる連結リストのセル ５０１ 生成されたサブゴールおよび候補を格納する連
結リスト ５０２ サブゴールおよびその候補を格納する連結リス
トのセル ５０３ 隣接サブゴールに関する情報を格納する連結リ
スト ５０４ 隣接サブゴールに関する情報を格納する連結リ
ストのセル ５０５ 隣接サブゴールおよびその候補へのポインタ ５０６ 探索済み全格子点情報 ５０７ 逆方向局所経路探索実行の有無を示すフラグ ６０１ サブゴール系列リストの連結リストによる実現 ６０２ サブゴール系列リストのセル ６０３ サブゴール系列の連結リストによる実現 ６０４ サブゴール系列のセル ８０１、９０６、１１０６、１１０７、１３０６、１３
０７ 探索範囲を制限する楕円体領域 ８０２、９０２、１１０２、１３０２ サブゴールSg1 ８０３、９０３、１３０３ サブゴールSg2 ８０５ 隣接サブゴール間局所経路 ８０６ 楕円体領域とＣ空間障害物で囲まれた閉領域 ９０１、１１０１、１３０１，１８０２ ２次元離散化
C空間 ９０５、１１０５、１３０５ ロボットの可動範囲外を
示す領域 １００１ 展開候補ノードの連結リストOPENLIST １００２ 展開済みノードの連結リストCLOSEDLIST １００３ 探索範囲外ノードの連結リストNextOPENLIST １００４ 干渉ノードの連結リストCOLLISIONLIST １００５ 可動範囲外ノードの連結リストOUTRANGELIST １６０１ ロボット・環境モデル入力手段 １６０２ 幾何モデル記憶手段 １６０３ 干渉検査手段 １６０４ スタート・ゴール配置パラメータ入力手段 １６０５ パラメータ設定・変更手段 １６０６ 離散化配置空間設定手段 １６０７ サブゴール系列探索手段 １６０８ サブゴールグラフ記憶手段 １６０９ サブゴール系列リスト記憶手段 １６１０ サブゴール系列選択手段 １６１１ サブゴール系列一時記憶手段 １６１２ 隣接サブゴール選択手段 １６１３ 隣接サブゴール間経路探索手段 １６１４ 探索範囲制限手段 １６１５ 探索済み格子点情報保存手段 １６１６ 探索済み格子点情報取得・変換手段 １６１７ 経路記憶手段 １６１８ 経路出力手段 １７０１ ３自由度マニピュレータ １７０２ ３自由度移動ロボット １７０３ ２次元作業空間 １７０４ ２次元作業空間の障害物 １８０１ ２次元Ｃ空間 １８０６ 展開されたノード １８０７ 未展開ノード １８０８ ロボットと障害物が干渉した未展開ノード ３６０１、３６０２ 障害物 ３６０３ ３自由度マニピュレータ ３６０４ ３自由度マニピュレータのサブロボット０
（Link1） ３６０５ リンク１の手先側端点の描く軌跡 ３６０６ ３自由度マニピュレータのサブロボット１
（Link2） ３６０７ リンク２の手先側端点の描く軌跡 ３６０８ ３自由度マニピュレータのサブロボット２
（Link3）
５、１８０５ C空間の障害物 ２０５ サブゴール ４０１ 連結リストの基本クラス ４０２ 連結リストの基本クラスのセル ４０３ グラフ探索に用いる連結リスト ４０４ グラフ探索に用いる連結リストのセル ５０１ 生成されたサブゴールおよび候補を格納する連
結リスト ５０２ サブゴールおよびその候補を格納する連結リス
トのセル ５０３ 隣接サブゴールに関する情報を格納する連結リ
スト ５０４ 隣接サブゴールに関する情報を格納する連結リ
ストのセル ５０５ 隣接サブゴールおよびその候補へのポインタ ５０６ 探索済み全格子点情報 ５０７ 逆方向局所経路探索実行の有無を示すフラグ ６０１ サブゴール系列リストの連結リストによる実現 ６０２ サブゴール系列リストのセル ６０３ サブゴール系列の連結リストによる実現 ６０４ サブゴール系列のセル ８０１、９０６、１１０６、１１０７、１３０６、１３
０７ 探索範囲を制限する楕円体領域 ８０２、９０２、１１０２、１３０２ サブゴールSg1 ８０３、９０３、１３０３ サブゴールSg2 ８０５ 隣接サブゴール間局所経路 ８０６ 楕円体領域とＣ空間障害物で囲まれた閉領域 ９０１、１１０１、１３０１，１８０２ ２次元離散化
C空間 ９０５、１１０５、１３０５ ロボットの可動範囲外を
示す領域 １００１ 展開候補ノードの連結リストOPENLIST １００２ 展開済みノードの連結リストCLOSEDLIST １００３ 探索範囲外ノードの連結リストNextOPENLIST １００４ 干渉ノードの連結リストCOLLISIONLIST １００５ 可動範囲外ノードの連結リストOUTRANGELIST １６０１ ロボット・環境モデル入力手段 １６０２ 幾何モデル記憶手段 １６０３ 干渉検査手段 １６０４ スタート・ゴール配置パラメータ入力手段 １６０５ パラメータ設定・変更手段 １６０６ 離散化配置空間設定手段 １６０７ サブゴール系列探索手段 １６０８ サブゴールグラフ記憶手段 １６０９ サブゴール系列リスト記憶手段 １６１０ サブゴール系列選択手段 １６１１ サブゴール系列一時記憶手段 １６１２ 隣接サブゴール選択手段 １６１３ 隣接サブゴール間経路探索手段 １６１４ 探索範囲制限手段 １６１５ 探索済み格子点情報保存手段 １６１６ 探索済み格子点情報取得・変換手段 １６１７ 経路記憶手段 １６１８ 経路出力手段 １７０１ ３自由度マニピュレータ １７０２ ３自由度移動ロボット １７０３ ２次元作業空間 １７０４ ２次元作業空間の障害物 １８０１ ２次元Ｃ空間 １８０６ 展開されたノード １８０７ 未展開ノード １８０８ ロボットと障害物が干渉した未展開ノード ３６０１、３６０２ 障害物 ３６０３ ３自由度マニピュレータ ３６０４ ３自由度マニピュレータのサブロボット０
（Link1） ３６０５ リンク１の手先側端点の描く軌跡 ３６０６ ３自由度マニピュレータのサブロボット１
（Link2） ３６０７ リンク２の手先側端点の描く軌跡 ３６０８ ３自由度マニピュレータのサブロボット２
（Link3）
───────────────────────────────────────────────────── フロントページの続き (72)発明者 高岡 佳市 福岡県北九州市八幡西区黒崎城石２番１号 株式会社安川電機内 (72)発明者 後藤 純 福岡県北九州市八幡西区黒崎城石２番１号 株式会社安川電機内 Ｆターム(参考） 3C007 AS00 BS12 MS05 MS08 MS29 5H269 AB33 BB05 BB14 CC09 DD05 EE03 EE25 NN16 NN17
Claims (30)
- 【請求項１】ロボットのスタート配置とゴール配置が与
えられたとき、ロボットとその作業環境にある障害物の
幾何学的形状とそれらの配置を記述する計算機上の幾何
学モデル手段と、モデル同士の干渉を検出する計算機上
の干渉検出手段とを用い、ロボットと障害物が干渉を起
こさないように２段階の経路探索を行うロボットの大域
動作経路計画方法において、 ステップ１として、配置空間（Ｃ空間）または作業
空間を大まかに離散化した格子点（サブゴール候補）を
持つ空間（離散化Ｃ空間）を形成し、グラフ（状態空
間）探索手法を用いて、スタート配置あるいはゴール配
置から前記格子点を経由してゴール配置あるいはスター
ト配置へ至るサブゴール系列を求め、 ステップ２として、前記ステップ１で求められたサ
ブゴール系列から隣接するサブゴールを２つづつ取り出
し、その隣接するサブゴールを包含する空間において前
記ステップ１より微少間隔で離散化した格子点を持つ空
間（離散化Ｃ空間）を形成し、グラフ（状態空間）探索
手法を用いて前記隣接サブゴール間の微少間隔格子点列
すなわち局所微少間隔経路を求める処理を繰り返し、 前記ステップ２で、サブゴール系列に含まれる隣接
サブゴールSg#m，Sg#n間の局所経路探索に失敗した場合
は、ステップ１に戻り（バックトラッキング）、サブゴ
ールSg#mからSg#nへのアークを経由しない別のサブゴー
ル系列を求め直し、再びステップ２に移る処理を繰り返
し、 スタート配置からゴール配置までの連続した微小間
隔経路が求まった場合に、繰り返し処理を終了すること
を特徴とするロボットの大域動作経路計画方法。 - 【請求項２】前記ステップ２において、隣接サブゴール
間の局所経路を求める際、探索範囲を２つの隣接サブゴ
ールを包含する所定の局所領域に限定することを特徴と
する請求項１記載のロボットの大域動作経路計画方法。 - 【請求項３】前記ステップ２において、隣接サブゴール
Sg#m，Sg#nを結ぶ局所経路を求める際、離散化Ｃ空間上
の２点p,q間の距離をd(p,q)、rPara を１以上のパラメ
ータとしたとき、探索領域Srngを Srng =｛ p｜d(Sg#m,p)＋d(p,Sg#n) <= d(Sg#m,Sg#n)×
rPara ｝ により制限することを特徴とする請求項２記載のロボッ
トの大域動作経路計画方法。 - 【請求項４】前記ステップ２において、隣接サブゴール
間の局所経路を求める際、隣接サブゴールを包含する前
記探索範囲を動的かつ多段階に変更することを特徴とす
る請求項２又は３記載のロボットの大域動作経路計画方
法。 - 【請求項５】前記ステップ１でサブゴール系列を求める
際に、生成された全てのサブゴールおよびその候補を、
重複を省いて計算機上にグラフ形式（サブゴールグラ
フ）で記憶し、前記ステップ２の実行結果によってサブ
ゴールグラフのデータを更新し、前記ステップ１、２と
もに最新のサブゴールグラフのデータを反映して探索を
行うことを特徴とする請求項１〜４のいずれか１項記載
のロボットの大域動作経路計画方法。 - 【請求項６】 前記ステップ１で求められたサブゴー
ル系列を全てリストに保存しておき、 ステップ１のサブゴール系列探索に成功した場合
は、そのサブゴール系列に対して前記ステップ２を実行
し、 ステップ１のサブゴール系列探索に失敗した場合
は、サブゴール系列のリストの中からサブゴール系列を
適当に１つ選び出し、 過去ステップ２において局所経路探索に失敗した隣
接サブゴール間に対しては、前回よりも前記探索範囲を
広くしてステップ２を実行し、 その局所経路探索に成功した場合は、それ以降の未
探索の隣接サブゴール間局所経路探索に対しては前記探
索範囲を元に戻すことを特徴とする請求項４又は５記載
のロボットの大域動作経路計画方法。 - 【請求項７】前記ステップ２での各隣接サブゴール間局
所経路探索において、探索した微少離散化間隔の格子点
情報を全て保存し、前記請求項６のの処理において再
利用することを特徴とする請求項６記載のロボットの大
域動作経路計画方法。 - 【請求項８】前記ステップ２の各隣接サブゴール間局所
経路探索において、前記請求項２または３において制限
された探索範囲外にある格子点に対してはロボットが可
動範囲内にあるかどうかのチェック（可動範囲チェッ
ク）および障害物と干渉するかしないかのチェック（干
渉チェック）を行わず、その探索範囲外格子点の情報と
探索範囲内の格子点情報を区別がつくように保存するこ
とを特徴とする請求項７記載のロボットの大域動作経路
計画方法。 - 【請求項９】前記請求項６のの処理において、前記探
索範囲を広げて隣接サブゴール間の微小間隔局所経路を
再探索する場合に、前回の探索時に保存しておいた（前
回の）探索範囲外格子点に対して可動範囲チェックおよ
び干渉チェックを実施し、その中の可動範囲内および非
干渉の格子点を今回の探索開始点の候補とすることを特
徴とする請求項８記載のロボットの大域動作経路計画方
法。 - 【請求項１０】前記ステップ２の各隣接サブゴール間局
所経路探索において、探索した微少離散化間隔の前記格
子点情報を、ツリー構造を有し１つまたは複数のリスト
形式で保存することを特徴とする請求項７〜９のいずれ
か１項記載のロボットの大域動作経路計画方法。 - 【請求項１１】前記ステップ２の各隣接サブゴール間局
所経路探索において、探索した微少離散化間隔の前記格
子点情報を、前記請求項５のサブゴールグラフの該当す
るアークに関連付けて保存することを特徴とする請求項
７〜１０のいずれか１項記載のロボットの大域動作経路
計画方法。 - 【請求項１２】前記ステップ１において、前記請求項５
のサブゴールグラフに既に登録済みのサブゴールSg#mを
親ノードとして、前記サブゴールグラフに新規サブゴー
ルSg#nを追加する際、前記サブゴールSg#mから前記サブ
ゴールSg#nへのアークだけでなく、Sg#nからSg#ｍへの
反対方向へのアークも登録することを特徴とする請求項
５〜１１のいずれか1項記載のロボットの大域動作経路
計画方法。 - 【請求項１３】前記請求項５のサブゴールグラフに登録
されたサブゴールSg#mからSg#nへのアークに、過去に反
対方向Sg#nからSg#mに向かって前記ステップ２の局所経
路探索を実行したかどうかを示すフラグを保存し、前記
ステップ２においてSg#mからSg#nへの局所経路探索を行
う際、常にそのフラグをチェックし、フラグが立ってい
なければ（過去に反対方向Sg#nからSg#mに局所経路探索
を行っていない）、サブゴールグラフのSg#nからSg#mへ
のアークの該当するフラグを立てSg#mからSg#nへの局所
経路探索を実行し、前記フラグが立っていれば（過去に
反対方向Sg#nからSg#mに局所経路探索を行った）、Sg#n
からSg#mへの局所経路探索の全格子点情報を利用して必
要であれば（Sg#m,Sg#n間の局所経路がまだ求まってい
なければ）Sg#nからSg#mへの局所経路探索を、前記探索
範囲を広げて行うことを特徴とする請求項１２記載のロ
ボットの大域動作経路計画方法。 - 【請求項１４】スタート配置またはゴール配置が変更さ
れた場合、前記幾何学モデルの障害物個数、形状、配置
が変更されなければ、前記請求項５のサブゴールグラフ
とそのアークに関連付けられた前記ステップ２の局所経
路探索実行結果に関する全ての情報を再利用して、ステ
ップ１およびステップ２を実行することを特徴とする請
求項５〜１３のいずれか１項記載のロボットの大域動作
経路計画方法。 - 【請求項１５】前記サブゴール系列リストからサブゴー
ル系列を１つ選択する際に、最もゴール配置近くまで前
記ステップ２の隣接サブゴール間局所経路探索に成功
し、かつ最もその経路長が短いサブゴール系列から優先
的に選択することを特徴とする請求項６記載のロボット
の大域動作経路計画方法。 - 【請求項１６】 前記ステップ２において、隣接サブゴ
ール間局所経路探索を行う際に、前記探索範囲を予め設
定した最大範囲にして探索に失敗した場合、そのサブゴ
ール系列を前記サブゴール系列のリストから削除あるい
はラベル付けし、以降選択されないようにすることを特
徴とする請求項６又は１５記載のロボットの大域動作経
路計画方法。 - 【請求項１７】前記サブゴール系列のリストが空になっ
た場合あるいはラベル付けによりサブゴール系列の選択
に失敗した場合、 サブゴール系列探索のために大まかな間隔で離散化
して形成した前記離散化Ｃ空間の各格子点間を整数個に
分割して細かく離散化した離散化Ｃ空間を形成し直し、 その新しい離散化Ｃ空間においてステップ1 を実行
し、 以下同様の処理を、スタート配置からゴール配置ま
での連続した微小間隔経路が求まるまであるいは予め定
めた一定時間内で、再帰的に繰り返すことを特徴とする
請求項1〜１６のいずれか１項記載のロボットの大域動
作経路計画方法。 - 【請求項１８】ロボットとその作業環境の幾何モデルを
入力するロボット・環境モデル入力手段と、前記ロボッ
ト・環境モデル入力手段が出力する前記幾何モデルを記
憶する幾何モデル記憶手段と、前記幾何モデル記憶手段
の情報を利用してロボットと環境との干渉を検査する干
渉検査手段と、ロボットのスタート配置とゴール配置お
よびその他パラメータを入力するスタート・ゴール配置
パラメータ入力手段と、前記パラメータ入力手段が出力
するパラメータを設定、あるいは探索の状況に応じて変
更するパラメータ設定・変更手段と、前記パラメータ設
定・変更手段によって設定されたパラメータを用いて、
サブゴール系列探索のための離散化配置空間を設定する
離散化配置空間設定手段と、前記離散化配置空間設定手
段によって定められた離散化配置空間において、 前記干渉検査手段の干渉検査結果を利用してサブゴール
の系列を探索するサブゴール系列探索手段と、 前記サブゴール系列探索手段によって生成されたサブゴ
ールおよびその候補をグラフの形式で記憶するサブゴー
ルグラフ記憶手段と、 前記サブゴール系列探索手段によって得られたサブゴー
ル系列をリストの形式で記憶するサブゴール系列リスト
記憶手段と、 前記サブゴール系列リスト記憶手段が記憶している情報
から１つのサブゴール系列を選択するサブゴール系列選
択手段と、 前記サブゴール系列選択手段によって選ばれたサブゴー
ル系列を一時的に記憶しておくサブゴール系列一時記憶
手段と、 前記サブゴール系列選択手段によって選ばれ、前記サブ
ゴール系列一時記憶手段に記憶されているサブゴール系
列から２つの隣接サブゴールを選択する隣接サブゴール
選択手段と、 前記隣接サブゴール選択手段から得られる２つの隣接サ
ブゴール間の局所経路を探索する隣接サブゴール間経路
探索手段と、 前記隣接サブゴール間経路探索手段が前記経路を探索す
るときに探索範囲を制限する探索範囲制限手段と、 前記隣接サブゴール間経路探索手段が探索した全格子点
データをサブゴールグラフの該当アークに関連付けて保
存する探索済み格子点情報保存手段と、 同一の隣接サブゴール間に対して前記隣接サブゴール間
経路探索を再実行する場合に前記探索済み格子点情報保
存手段が保存した格子点情報を取得・変換して再利用さ
せる探索済み格子点情報取得・変換手段と、 前記サブゴールグラフ記憶手段に記憶されている情報と
前記サブゴール系列一時記憶手段に記憶されている情報
を受けて、前記スタート配置から前記ゴール配置に至る
連続した経路を記憶する経路記憶手段と、 前記経路記憶手段から前記経路を出力する経路出力手段
と、を備えたことを特徴とするロボットの大域動作経路
計画制御装置。 - 【請求項１９】複数の関節軸自由度を有するロボットに
おいて、ロボットの大域動作経路計画を行なう場合に、
Ａ軸群探索値決定手段として請求項１〜18に記載された
いずれか、またはいくつかの組み合わせの探索法を用い
て探索する軸群（A軸群）とその他の軸群（B軸群）とを
区別する区別手段を有し、B軸群については、ロボット
のスタート配置からゴール配置までの途中の中間値を探
索処理毎に一定のルールによって逐次演算するB軸群チ
ェック値演算手段を有し、サブゴール探索時にＡ軸群探
索値決定手段で得たA軸群の各軸の探索値と、B軸群チェ
ック値演算手段によって得たB軸群の各軸のチェック値
とを用いてロボットの幾何学モデルを求め、障害物との
干渉を検出することを特徴とするロボットの大域動作経
路計画制御装置。 - 【請求項２０】前記B軸群チェック値演算手段として、
予めロボットのスタート配置からゴール配置までのA軸
群の各軸の移動量が最大の軸（M軸）の移動量（M#DLT）
を求め、探索処理時にはM軸の探索値からM軸のスタート
配置を差し引いた値（M#VAL）とM#DLTの比（M#VAL／M#D
LT）をロボットのスタート配置からゴール配置までのB
軸群の各軸の移動量に乗算し、B軸群の各軸のチェック
値とすることを特徴とする請求項１９記載のロボットの
大域動作経路計画制御装置。 - 【請求項２１】前記ステップ１およびステップ２におい
て、ロボットとその作業環境にある障害物の幾何学的形
状とそれらの配置を記述する計算機上の幾何学モデル手
段と、モデル同士の干渉を検出する計算機上の干渉検出
手段とを用いて、検査対象である離散化配置空間上の格
子点関節角座標値に対するロボットと障害物との干渉を
検査する際に、ロボットと障害物が干渉する場合は、ロ
ボットのどの部位が干渉するかという情報を前記干渉検
出手段から受取り、前記ロボット部位と関連付けて干渉
を起こすロボットの前記関節角座標値を保存することを
特徴とする請求項1〜１７のいずれか１項記載のロボッ
トの大域動作経路計画方法。 - 【請求項２２】ロボットとその作業環境にある障害物の
幾何学的形状とそれらの配置を記述する計算機上の幾何
学モデル手段と、 モデル同士の干渉を検出する計算機上の干渉検出手段
と、 検査対象である離散化配置空間上の格子点関節角座標値
に対するロボットと障害物との干渉を検査する際に、ロ
ボットと障害物が干渉する場合は、ロボットのどの部位
が干渉するかという情報を前記干渉検出手段から受取
り、前記ロボット部位と関連付けて干渉を起こすロボッ
トの前記関節角座標値を保存するロボットの関節角座標
値保存手段と、を備えたことを特徴とする請求項１８又
は１９記載のロボットの大域動作経路計画制御装置。 - 【請求項２３】前記ステップ１およびステップ２におい
て、前記幾何学モデル手段と、前記モデル同士の干渉検
出手段とを用いて、検査対象である離散化配置空間上の
格子点関節角座標値に対するロボットと障害物との干渉
を検査する前に、 ロボット部位に関連付けて保存された干渉を起こす複数
のロボットの関節角座標値と検査対象である関節角座標
値を比較し、 前記複数のロボットの関節角座標値のうち少なくとも１
つが検査対象である関節角座標値に一致する場合は、前
記幾何学モデル手段と前記モデル同士の干渉検出手段を
用いた干渉検査を行なわずに、ロボットと障害物が干渉
すると判断することを特徴とする請求項２１記載のロボ
ットの大域動作経路計画方法。 - 【請求項２４】前記幾何学モデル手段と、前記モデル同
士の干渉検出手段とを用いて、検査対象である離散化配
置空間上の格子点関節角座標値に対するロボットと障害
物との干渉を検査する前に、 ロボット部位に関連付けて保存された干渉を起こす複数
のロボットの関節角座標値と検査対象である関節角座標
値を比較し、 前記複数のロボットの関節角座標値のうち少なくとも１
つが検査対象である関節角座標値に一致する場合は、前
記幾何学モデル手段と前記モデル同士の干渉検出手段を
用いた干渉検査を行なわずに、ロボットと障害物が干渉
すると判断することを特徴とする請求項２２記載のロボ
ットの大域動作経路計画制御装置。 - 【請求項２５】ロボットを駆動軸とそれに付随して運動
する機構に基づいて複数サブロボットへ分割し、各サブ
ロボットの経路を、ベース部のサブロボットから先端の
サブロボットに向かって順番に、１つ手前までのサブロ
ボットの探索済み経路に拘束された空間上で探索し、あ
るサブロボットの経路探索に失敗した場合に、１つ手前
のサブロボットの経路を再探索してロボット全体経路を
求めるシーケンシャル探索において、 サブロボットの自由度を１あるいは２以上とし、各サブ
ロボットの経路を前記ステップ１および前記ステップ２
に基く２段階経路探索により求めることを特徴とする請
求項１〜１７、２１，２３のいずれか１項記載のロボッ
トの大域動作経路計画方法。 - 【請求項２６】ロボットを駆動軸とそれに付随して運動
する機構に基づいて複数サブロボットへ分割し、各サブ
ロボットの経路を、ベース部のサブロボットから先端の
サブロボットに向かって順番に、１つ手前までのサブロ
ボットの探索済み経路に拘束された空間上で探索し、あ
るサブロボットの経路探索に失敗した場合に、１つ手前
のサブロボットの経路を再探索してロボット全体経路を
求めるシーケンシャル探索において、 サブロボットの自由度を１あるいは２以上とし、各サブ
ロボットの経路を前記ステップ１および前記ステップ２
に基く２段階経路探索により求めることを特徴とする請
求項１８〜２０、２２記載のロボットの大域動作経路計
画制御装置。 - 【請求項２７】前記シーケンシャル探索における各サブ
ロボットの２段階経路探索において、サブロボットをロ
ボットのベース部に近い駆動軸に対応するサブロボット
から0，1，2と番号付けしたとき、サブロボットkでの２
段階経路探索に失敗した場合に、サブロボットk-1のサ
ブゴールグラフのどのサブゴールあるいはサブゴール間
アークが原因でサブロボットkの２段階経路探索に失敗
したかを推定し、サブロボットk-1の該当するサブゴー
ルあるいはサブゴール間アークに重み付けをし、サブロ
ボットk-1の２段階経路探索に後戻りした際に、その重
み付けの効果によって前回とは別のサブゴール系列およ
び隣接サブゴール間局所経路を探索することを特徴とす
る請求項２５記載のロボットの大域動作経路計画方法。 - 【請求項２８】前記シーケンシャル探索における各サブ
ロボットの２段階経路探索において、サブロボットをロ
ボットのベース部に近い駆動軸に対応するサブロボット
から0，1，2と番号付けしたとき、サブロボットkでの２
段階経路探索に失敗した場合に、サブロボットk-1のサ
ブゴールグラフのどのサブゴールあるいはサブゴール間
アークが原因でサブロボットkの２段階経路探索に失敗
したかを推定し、サブロボットk-1の該当するサブゴー
ルあるいはサブゴール間アークに重み付けをし、サブロ
ボットk-1の２段階経路探索に後戻りした際に、その重
み付けの効果によって前回とは別のサブゴール系列およ
び隣接サブゴール間局所経路を探索することを特徴とす
る請求項２６記載のロボットの大域動作経路計画制御装
置。 - 【請求項２９】前記重み付けが、同一のサブゴールある
いはサブゴール間アークに対して複数回行われる際に、
サブゴールあるいはサブゴール間アークへの重みを前記
回数分だけ加算あるいは積算することを特徴とする請求
項２７記載のロボットの大域動作経路計画方法。 - 【請求項３０】前記重み付けが、同一のサブゴールある
いはサブゴール間アークに対して複数回行われる際に、
サブゴールあるいはサブゴール間アークへの重みを前記
回数分だけ加算あるいは積算することを特徴とする請求
項２８記載のロボットの大域動作経路計画制御装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2001172590A JP2002073130A (ja) | 2000-06-13 | 2001-06-07 | ロボットの大域動作経路計画方法とその制御装置 |
Applications Claiming Priority (3)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2000177051 | 2000-06-13 | ||
| JP2000-177051 | 2000-06-13 | ||
| JP2001172590A JP2002073130A (ja) | 2000-06-13 | 2001-06-07 | ロボットの大域動作経路計画方法とその制御装置 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| JP2002073130A true JP2002073130A (ja) | 2002-03-12 |
Family
ID=26593842
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2001172590A Abandoned JP2002073130A (ja) | 2000-06-13 | 2001-06-07 | ロボットの大域動作経路計画方法とその制御装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP2002073130A (ja) |
Cited By (35)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2008204161A (ja) * | 2007-02-20 | 2008-09-04 | Toyota Motor Corp | 経路作成装置 |
| JP2008204188A (ja) * | 2007-02-20 | 2008-09-04 | Nagoya Institute Of Technology | モーションコントローラおよびモーションプランナおよび多軸サーボシステムおよびサーボアンプ |
| CN101881950A (zh) * | 2010-07-09 | 2010-11-10 | 哈尔滨工程大学 | 潜器全方位推进器旋转盘运动姿态监测装置 |
| JP2011131326A (ja) * | 2009-12-24 | 2011-07-07 | Ihi Aerospace Co Ltd | ロボットアームの干渉回避方法 |
| JP2011161624A (ja) * | 2010-01-12 | 2011-08-25 | Honda Motor Co Ltd | 軌道計画方法、軌道計画システム及びロボット |
| JP2012056063A (ja) * | 2010-09-13 | 2012-03-22 | Institute Of National Colleges Of Technology Japan | 円滑経路生成装置および円滑経路生成方法 |
| JP2012056064A (ja) * | 2010-09-13 | 2012-03-22 | Sugino Machine Ltd | 経路生成装置および経路生成方法 |
| JP2012164061A (ja) * | 2011-02-04 | 2012-08-30 | Honda Motor Co Ltd | 軌道計画方法、軌道制御方法、軌道計画システム及び軌道計画・制御システム |
| JP2013003693A (ja) * | 2011-06-14 | 2013-01-07 | Mazda Motor Corp | 部品製造ラインの設計支援方法及び設計支援システム |
| CN107015563A (zh) * | 2016-12-29 | 2017-08-04 | 北京航空航天大学 | 移动机器人路径规划方法及装置 |
| JP2018505788A (ja) * | 2015-01-26 | 2018-03-01 | デューク・ユニバーシティＤｕｋｅ Ｕｎｉｖｅｒｓｉｔｙ | 専用ロボットの動作を計画するハードウェアならびにその作製方法および使用方法 |
| WO2018143003A1 (ja) * | 2017-01-31 | 2018-08-09 | 株式会社安川電機 | ロボットパス生成装置及びロボットシステム |
| JP2018532608A (ja) * | 2015-11-04 | 2018-11-08 | フランカ エミカ ゲーエムベーハーＦＲＡＮＫＡ ＥＭＩＫＡ ＧｍｂＨ | 位置および／または姿勢の離散手動入力の制御システムを備えるロボット |
| CN109129470A (zh) * | 2018-08-02 | 2019-01-04 | 深圳市智能机器人研究院 | 机器人直线路径的规划方法及系统 |
| JP2020099977A (ja) * | 2018-12-25 | 2020-07-02 | 株式会社日立製作所 | 軌道生成装置、軌道生成方法、及びロボットシステム |
| KR20200109552A (ko) * | 2019-03-13 | 2020-09-23 | (주)아이로텍 | 장애물 회피 경로 생성 시뮬레이션 시스템 |
| CN113156970A (zh) * | 2021-05-08 | 2021-07-23 | 珠海市一微半导体有限公司 | 一种通行区域的路径融合规划方法、机器人及芯片 |
| CN113624239A (zh) * | 2021-08-11 | 2021-11-09 | 火种源码(中山)科技有限公司 | 基于层级可开关稀疏位姿图优化的激光建图方法及装置 |
| JP2022012887A (ja) * | 2020-07-02 | 2022-01-17 | 株式会社デンソー | 動作経路生成装置、動作経路生成方法および動作経路生成プログラム |
| US11235465B2 (en) | 2018-02-06 | 2022-02-01 | Realtime Robotics, Inc. | Motion planning of a robot storing a discretized environment on one or more processors and improved operation of same |
| CN114004174A (zh) * | 2021-10-29 | 2022-02-01 | 中船重工奥蓝托无锡软件技术有限公司 | 适用多套复杂网格耦合cfd计算的高效宿主单元搜索方法 |
| US11292456B2 (en) | 2018-01-12 | 2022-04-05 | Duke University | Apparatus, method and article to facilitate motion planning of an autonomous vehicle in an environment having dynamic objects |
| CN114371713A (zh) * | 2022-01-12 | 2022-04-19 | 深圳鹏行智能研究有限公司 | 足式机器人的路径规划方法、电子设备及存储介质 |
| US11429105B2 (en) | 2016-06-10 | 2022-08-30 | Duke University | Motion planning for autonomous vehicles and reconfigurable motion planning processors |
| CN115723125A (zh) * | 2022-10-31 | 2023-03-03 | 北京工业大学 | 一种机械臂重复操作运动规划方法 |
| CN115826586A (zh) * | 2023-02-14 | 2023-03-21 | 泉州装备制造研究所 | 一种融合全局算法和局部算法的路径规划方法及系统 |
| US11623346B2 (en) | 2020-01-22 | 2023-04-11 | Realtime Robotics, Inc. | Configuration of robots in multi-robot operational environment |
| US11634126B2 (en) | 2019-06-03 | 2023-04-25 | Realtime Robotics, Inc. | Apparatus, methods and articles to facilitate motion planning in environments having dynamic obstacles |
| US11673265B2 (en) | 2019-08-23 | 2023-06-13 | Realtime Robotics, Inc. | Motion planning for robots to optimize velocity while maintaining limits on acceleration and jerk |
| WO2023149298A1 (ja) * | 2022-02-04 | 2023-08-10 | 三菱電機株式会社 | 軌道生成装置 |
| CN116620802A (zh) * | 2023-07-19 | 2023-08-22 | 中建安装集团有限公司 | 一种利用室内施工智能物料运输系统的运输方法 |
| US11738457B2 (en) | 2018-03-21 | 2023-08-29 | Realtime Robotics, Inc. | Motion planning of a robot for various environments and tasks and improved operation of same |
| JP2023540169A (ja) * | 2020-09-23 | 2023-09-22 | アプライド マテリアルズ インコーポレイテッド | ロボット関節空間グラフ経路計画および移動実行 |
| CN118067131A (zh) * | 2024-04-17 | 2024-05-24 | 山东省科学院海洋仪器仪表研究所 | 一种用于水下机器人的路径规划控制方法 |
| US12017364B2 (en) | 2019-04-17 | 2024-06-25 | Realtime Robotics, Inc. | Motion planning graph generation user interface, systems, methods and articles |
-
2001
- 2001-06-07 JP JP2001172590A patent/JP2002073130A/ja not_active Abandoned
Cited By (58)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2008204161A (ja) * | 2007-02-20 | 2008-09-04 | Toyota Motor Corp | 経路作成装置 |
| JP2008204188A (ja) * | 2007-02-20 | 2008-09-04 | Nagoya Institute Of Technology | モーションコントローラおよびモーションプランナおよび多軸サーボシステムおよびサーボアンプ |
| JP2011131326A (ja) * | 2009-12-24 | 2011-07-07 | Ihi Aerospace Co Ltd | ロボットアームの干渉回避方法 |
| JP2011161624A (ja) * | 2010-01-12 | 2011-08-25 | Honda Motor Co Ltd | 軌道計画方法、軌道計画システム及びロボット |
| CN101881950A (zh) * | 2010-07-09 | 2010-11-10 | 哈尔滨工程大学 | 潜器全方位推进器旋转盘运动姿态监测装置 |
| JP2012056063A (ja) * | 2010-09-13 | 2012-03-22 | Institute Of National Colleges Of Technology Japan | 円滑経路生成装置および円滑経路生成方法 |
| JP2012056064A (ja) * | 2010-09-13 | 2012-03-22 | Sugino Machine Ltd | 経路生成装置および経路生成方法 |
| US8924013B2 (en) | 2011-02-04 | 2014-12-30 | Honda Motor Co., Ltd. | Method and system for path planning and controlling |
| JP2012164061A (ja) * | 2011-02-04 | 2012-08-30 | Honda Motor Co Ltd | 軌道計画方法、軌道制御方法、軌道計画システム及び軌道計画・制御システム |
| JP2013003693A (ja) * | 2011-06-14 | 2013-01-07 | Mazda Motor Corp | 部品製造ラインの設計支援方法及び設計支援システム |
| US10723024B2 (en) | 2015-01-26 | 2020-07-28 | Duke University | Specialized robot motion planning hardware and methods of making and using same |
| JP2018505788A (ja) * | 2015-01-26 | 2018-03-01 | デューク・ユニバーシティＤｕｋｅ Ｕｎｉｖｅｒｓｉｔｙ | 専用ロボットの動作を計画するハードウェアならびにその作製方法および使用方法 |
| US10994415B2 (en) | 2015-11-04 | 2021-05-04 | Franka Emika Gmbh | Robot with control system for discrete manual input of positions and/or poses |
| JP2018532608A (ja) * | 2015-11-04 | 2018-11-08 | フランカ エミカ ゲーエムベーハーＦＲＡＮＫＡ ＥＭＩＫＡ ＧｍｂＨ | 位置および／または姿勢の離散手動入力の制御システムを備えるロボット |
| US11429105B2 (en) | 2016-06-10 | 2022-08-30 | Duke University | Motion planning for autonomous vehicles and reconfigurable motion planning processors |
| CN107015563A (zh) * | 2016-12-29 | 2017-08-04 | 北京航空航天大学 | 移动机器人路径规划方法及装置 |
| JPWO2018143003A1 (ja) * | 2017-01-31 | 2019-06-27 | 株式会社安川電機 | ロボットパス生成装置及びロボットシステム |
| WO2018143003A1 (ja) * | 2017-01-31 | 2018-08-09 | 株式会社安川電機 | ロボットパス生成装置及びロボットシステム |
| US11446820B2 (en) | 2017-01-31 | 2022-09-20 | Kabushiki Kaisha Yaska Wa Denki | Robot path generating device and robot system |
| US11292456B2 (en) | 2018-01-12 | 2022-04-05 | Duke University | Apparatus, method and article to facilitate motion planning of an autonomous vehicle in an environment having dynamic objects |
| US11970161B2 (en) | 2018-01-12 | 2024-04-30 | Duke University | Apparatus, method and article to facilitate motion planning of an autonomous vehicle in an environment having dynamic objects |
| US12090668B2 (en) | 2018-02-06 | 2024-09-17 | Realtime Robotics, Inc. | Motion planning of a robot storing a discretized environment on one or more processors and improved operation of same |
| US11745346B2 (en) | 2018-02-06 | 2023-09-05 | Realtime Robotics, Inc. | Motion planning of a robot storing a discretized environment on one or more processors and improved operation of same |
| US11235465B2 (en) | 2018-02-06 | 2022-02-01 | Realtime Robotics, Inc. | Motion planning of a robot storing a discretized environment on one or more processors and improved operation of same |
| US11964393B2 (en) | 2018-03-21 | 2024-04-23 | Realtime Robotics, Inc. | Motion planning of a robot for various environments and tasks and improved operation of same |
| US11738457B2 (en) | 2018-03-21 | 2023-08-29 | Realtime Robotics, Inc. | Motion planning of a robot for various environments and tasks and improved operation of same |
| US12083682B2 (en) | 2018-03-21 | 2024-09-10 | Realtime Robotics, Inc. | Motion planning of a robot for various environments and tasks and improved operation of same |
| CN109129470A (zh) * | 2018-08-02 | 2019-01-04 | 深圳市智能机器人研究院 | 机器人直线路径的规划方法及系统 |
| CN109129470B (zh) * | 2018-08-02 | 2021-07-09 | 深圳市智能机器人研究院 | 机器人直线路径的规划方法及系统 |
| CN111443703A (zh) * | 2018-12-25 | 2020-07-24 | 株式会社日立制作所 | 轨道生成装置、轨道生成方法以及机器人系统 |
| JP2020099977A (ja) * | 2018-12-25 | 2020-07-02 | 株式会社日立製作所 | 軌道生成装置、軌道生成方法、及びロボットシステム |
| CN111443703B (zh) * | 2018-12-25 | 2023-05-09 | 株式会社日立制作所 | 轨道生成装置、轨道生成方法以及机器人系统 |
| US11577391B2 (en) | 2018-12-25 | 2023-02-14 | Hitachi, Ltd. | Trajectory generation device, trajectory generation method, and robot system |
| KR20200109552A (ko) * | 2019-03-13 | 2020-09-23 | (주)아이로텍 | 장애물 회피 경로 생성 시뮬레이션 시스템 |
| KR102170752B1 (ko) | 2019-03-13 | 2020-10-27 | (주)아이로텍 | 장애물 회피 경로 생성 시뮬레이션 시스템 |
| US12017364B2 (en) | 2019-04-17 | 2024-06-25 | Realtime Robotics, Inc. | Motion planning graph generation user interface, systems, methods and articles |
| US11634126B2 (en) | 2019-06-03 | 2023-04-25 | Realtime Robotics, Inc. | Apparatus, methods and articles to facilitate motion planning in environments having dynamic obstacles |
| US11673265B2 (en) | 2019-08-23 | 2023-06-13 | Realtime Robotics, Inc. | Motion planning for robots to optimize velocity while maintaining limits on acceleration and jerk |
| US11623346B2 (en) | 2020-01-22 | 2023-04-11 | Realtime Robotics, Inc. | Configuration of robots in multi-robot operational environment |
| JP7400644B2 (ja) | 2020-07-02 | 2023-12-19 | 株式会社デンソー | 動作経路生成装置、動作経路生成方法および動作経路生成プログラム |
| US12049012B2 (en) | 2020-07-02 | 2024-07-30 | Denso Corporation | Motion path generation device, motion path generation method, and non-transitory tangible computer readable storage medium |
| JP2022012887A (ja) * | 2020-07-02 | 2022-01-17 | 株式会社デンソー | 動作経路生成装置、動作経路生成方法および動作経路生成プログラム |
| JP7544954B2 (ja) | 2020-09-23 | 2024-09-03 | アプライド マテリアルズ インコーポレイテッド | ロボット関節空間グラフ経路計画および移動実行 |
| JP2023540169A (ja) * | 2020-09-23 | 2023-09-22 | アプライド マテリアルズ インコーポレイテッド | ロボット関節空間グラフ経路計画および移動実行 |
| CN113156970B (zh) * | 2021-05-08 | 2023-06-09 | 珠海一微半导体股份有限公司 | 一种通行区域的路径融合规划方法、机器人及芯片 |
| CN113156970A (zh) * | 2021-05-08 | 2021-07-23 | 珠海市一微半导体有限公司 | 一种通行区域的路径融合规划方法、机器人及芯片 |
| CN113624239B (zh) * | 2021-08-11 | 2024-05-31 | 火种源码(中山)科技有限公司 | 基于层级可开关稀疏位姿图优化的激光建图方法及装置 |
| CN113624239A (zh) * | 2021-08-11 | 2021-11-09 | 火种源码(中山)科技有限公司 | 基于层级可开关稀疏位姿图优化的激光建图方法及装置 |
| CN114004174A (zh) * | 2021-10-29 | 2022-02-01 | 中船重工奥蓝托无锡软件技术有限公司 | 适用多套复杂网格耦合cfd计算的高效宿主单元搜索方法 |
| CN114004174B (zh) * | 2021-10-29 | 2023-08-25 | 中船奥蓝托无锡软件技术有限公司 | 适用多套复杂网格耦合cfd计算的高效宿主单元搜索方法 |
| CN114371713A (zh) * | 2022-01-12 | 2022-04-19 | 深圳鹏行智能研究有限公司 | 足式机器人的路径规划方法、电子设备及存储介质 |
| JP7399357B1 (ja) * | 2022-02-04 | 2023-12-15 | 三菱電機株式会社 | 軌道生成装置 |
| WO2023149298A1 (ja) * | 2022-02-04 | 2023-08-10 | 三菱電機株式会社 | 軌道生成装置 |
| CN115723125A (zh) * | 2022-10-31 | 2023-03-03 | 北京工业大学 | 一种机械臂重复操作运动规划方法 |
| CN115826586A (zh) * | 2023-02-14 | 2023-03-21 | 泉州装备制造研究所 | 一种融合全局算法和局部算法的路径规划方法及系统 |
| CN116620802B (zh) * | 2023-07-19 | 2023-10-24 | 中建安装集团有限公司 | 一种利用室内施工智能物料运输系统的运输方法 |
| CN116620802A (zh) * | 2023-07-19 | 2023-08-22 | 中建安装集团有限公司 | 一种利用室内施工智能物料运输系统的运输方法 |
| CN118067131A (zh) * | 2024-04-17 | 2024-05-24 | 山东省科学院海洋仪器仪表研究所 | 一种用于水下机器人的路径规划控制方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP2002073130A (ja) | ロボットの大域動作経路計画方法とその制御装置 | |
| Chen et al. | SANDROS: a dynamic graph search algorithm for motion planning | |
| JP4103057B2 (ja) | ロボットの動作経路計画方法およびその装置 | |
| Foskey et al. | A Voronoi-based hybrid motion planner | |
| Kallmann et al. | Geometric and discrete path planning for interactive virtual worlds | |
| Saha et al. | Planning tours of robotic arms among partitioned goals | |
| Kallmann et al. | Navigation meshes and real-time dynamic planning for virtual worlds | |
| US5999881A (en) | Automated path planning | |
| Abu-Dakka et al. | A direct approach to solving trajectory planning problems using genetic algorithms with dynamics considerations in complex environments | |
| Raheem et al. | Robot arm free Cartesian space analysis for heuristic path planning enhancement | |
| Gupta | Practical global motion planning for many degrees of freedom: A novel approach within sequential framework | |
| Fu et al. | Computationally-efficient roadmap-based inspection planning via incremental lazy search | |
| Bohlin et al. | A randomized approach to robot path planning based on lazy evaluation | |
| Gayle et al. | Constraint-based motion planning of deformable robots | |
| Hwang et al. | A heuristic and complete planner for the classical mover's problem | |
| Kingston et al. | Scaling multimodal planning: Using experience and informing discrete search | |
| JP2000181539A (ja) | ロボットの大域動作経路計画方法とその制御装置 | |
| JP4244443B2 (ja) | ロボットの大域動作経路計画方法とその制御装置 | |
| Melo et al. | Parameterized space conditions for the definition of locomotion modes in modular snake robots | |
| 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 | |
| Baginski | Efficient dynamic collision detection using expanded geometry models | |
| Henning et al. | Motion planning for serpentine robots | |
| Khoury et al. | Efficient sampling of transition constraints for motion planning under sliding contacts | |
| 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 |
|
| RD04 | Notification of resignation of power of attorney |
Free format text: JAPANESE INTERMEDIATE CODE: A7424 Effective date: 20071127 |
|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20080515 |
|
| A762 | Written abandonment of application |
Free format text: JAPANESE INTERMEDIATE CODE: A762 Effective date: 20090714 |