(57)【特許請求の範囲】
【請求項１】
ロボットと作業環境の幾何学的形状および前記ロボットと前記作業環境の配置を記述した計算機上の幾何モデル手段と、モデル同士の干渉を検査する計算機上の干渉検査手段と、を利用し、ロボットのスタートおよびゴール配置が与えられたときロボットと作業環境内の障害物とが干渉を起こさずにスタート配置からゴール配置へ至るロボットの動作経路を計画する動作経路計画方法において、
（１）スタート配置を始端サブゴール、ゴール配置を終端サブゴールとし、ロボットの始端サブゴールと終端サブゴールを結ぶ作業空間あるいは配置空間における直線経路を２分割する点を最初のサブゴール候補とする第１ステップを実行し、
（２）隣接するサブゴールをサブゴールａ、ｂと表し、該サブゴールａ、ｂ間を結ぶ作業空間あるいは配置空間における直線経路を２分割する点をサブゴール候補１と表して新たなサブゴールの候補とし、前記サブゴール候補１においてロボットと障害物との干渉を前記幾何モデル手段と干渉検査手段とを用いて検査する第２ステップを実行し、
（３）（ｉ)前記サブゴール候補１において干渉が検出されなかったときは、サブゴール候補１を新たなサブゴールｃとして、ａ→ｃ→ｂで表されるサブゴールの接続関係を記述する有向グラフを生成し、（ｉｉ)前記サブゴール候補１において干渉が検出されたときは、前記サブゴール候補１から、予め定めた複数のロボット回避動作方向それぞれに予め定めた距離間隔でロボットを移動させた複数の点において干渉検査を行い、それぞれの前記回避動作方向あるいはその内の幾つかに対して、干渉が検出されない点の中から新たなサブゴールｄ1、ｄ2、ｄ3、・・・を１つづつ選び、ａ→ｄ1→ｂ、ａ→ｄ2→ｂ、ａ→ｄ3→ｂ、・・・で表されるサブゴールの有向グラフを生成する第３ステップを実行し、
（４）前記第２および第３ステップを各サブゴール間の距離が予め定めた値以下になるまで、再帰的に繰り返す第４ステップを実行し、
（５）前記サブゴールの有向グラフを用いて始端サブゴールから終端サブゴールへ至るサブゴール経路であるサブゴール系列を複数組計画する第５ステップを実行し、
（６）前記計画された複数組のサブゴール系列の中から、各サブゴール間距離の和が最小となるサブゴール系列を選出する第６ステップを実行し、
（７）前記選出されたサブゴール系列に対して、前記サブゴール系列内の各サブゴールまたは各サブゴールを中心とした所定半径の円もしくは球の内側を通過する経路を、前記ロボットと前記作業環境の干渉を検査しながら、離散化された配置空間におけるグラフ探索手法を用いて、前記サブゴール系列の間隔よりもより微小な間隔で求める第７ステップを実行することを特徴とするロボットの動作経路計画方法。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、ロボットと作業環境の幾何学的形状およびそれらの配置を記述した計算機上の幾何モデル手段と、モデル同士の干渉を検査する計算機上の干渉検査手段を利用し、ロボットのスタートおよびゴール配置が与えられたとき、ロボットと作業環境内の障害物とが干渉しないロボットの動作経路を計画する方法およびその装置に関するものである。
【０００２】
【従来の技術】
従来より、ロボットの障害物回避の経路計画問題は、配置空間（Configuration space 、以後「Ｃ空間」と言う）における点の移動経路計画問題という形式に一般化することができる。Ｃ空間はロボットの配置を一意に決定するパラメータ空間であり、多関節マニピュレータの場合は関節空間に一致する。また、２次元作業空間内における移動ロボットの場合は、作業空間座標系を基準にしたロボット座標原点の位置（Ｘ, Ｙ）と、ロボットの向きθの３自由度空間（Ｘ, Ｙ, θ）となる。Ｃ空間では全てのロボットの任意の配置が点で表される。Ｃ空間において、障害物と干渉しない経路を求めるためには、通常、Ｃ空間の空間記述が必要になる。すなわち、作業空間におけるロボットと障害物の幾何学的形状およびその位置関係を用いてＣ空間における障害物（以後「Ｃ障害物」と言う）を記述し、Ｃ空間内の自由（すなわち非干渉）空間（以後「Ｃ自由空間」と言う）を求める必要がある。
Ｃ空間記述に基づくロボットの経路計画方式は、
▲１▼  ロードマップ法、
▲２▼  セル分解法、
▲３▼  Ｃ空間離散化法、
▲４▼  ポテンシャルフィールド法、に大別される。
▲１▼ロードマップ法、▲２▼セル分解法、▲３▼Ｃ空間離散化法の相違点は、Ｃ空間の記述法とその記述に基づく探索手法とにある。▲１▼ロードマップ法や▲２▼セル分解法では、通常、Ｃ空間の「陽な記述」が必要になる。２次元作業空間における２自由度移動ロボットの場合はＣ自由空間を陽に記述することができる。すなわち、Ｃ障害物の境界を陽に求めることができる。多関節マニピュレータの場合は、Ｃ障害物を解析的に求める一般的手法は存在せず、
▲１▼  Slice Projection（Needle）法、
▲２▼  Swept Volume法、
▲３▼  Point Evaluation法などのＣ空間の離散化に基づく近似手法によりＣ空間記述を求めるのが一般的である。その意味から、▲１▼ロードマップ法や▲２▼セル分解法は２次元作業空間における移動ロボットの経路計画問題に適しており、▲３▼Ｃ空間離散化法は多関節マニピュレータの経路計画問題に適している。
これら従来方式は、文献「Y.K.Hwang and N.Ahuja : Gross Motion Planning -- A Survey, ACM Computing Surveys, Vol.24, No.3, pp.219-291, 1992」に詳しくまとめられている。
経路計画問題は、計算の複雑性理論（Computational Complexity Thoery ）によれば、NP（Nondeterministic Polymonial ）--hardと呼ばれる解の導出が非常に困難な問題に属する。ロボットの自由度、障害物の数、障害物の面の数などが大きくなるに従い、現実的な時間内では障害物回避経路を求めることができなくなる。上記従来法を現状の100MIPS 程度の計算機を用いて数分以内に経路が求まるという基準で評価すると、▲１▼ロードマップ法や▲２▼セル分解法は２次元作業空間内における移動ロボット、▲３▼Ｃ空間離散化法（局所探索）や▲４▼ポテンシャルフィールド法は３次元作業空間内における６自由度マニピュレータという具合いに、適用可能範囲が限られて来る。
【０００３】
Ｃ空間の離散化記述は、微少間隔で離散化された各Ｃ空間領域（以後、「セル」と言う）をノードとし、それらセルの隣接関係をアークとする連結度グラフで表される。各ノードは自由空間か干渉空間かという情報を持つ。
例えば、▲３▼Point Evaluation法では、各離散化セルの代表点（通常、幾何学的重心）においてロボットと作業環境との干渉を幾何モデラと干渉チェッカを用いて検査し、代表点で干渉が検出されなければそのセルを自由空間とし、干渉が検出されればそのセルを障害物空間とする。この連結度グラフ上のスタート配置ノードからゴール配置ノードへ至る経路を、縦型・横型・山登り・Ａ＊探索などのグラフ探索手法を用いて求めるのが、▲３▼のＣ空間離散化法の基本的なアプローチである。
▲３▼のＣ空間離散化法には、(i) 予めＣ空間全域の離散化記述を求めておく方式と、(ii)予め離散化記述を求めず、探索の過程で逐次離散化セルにおける干渉を検査する方式とがある。
▲４▼のポテンシャルフィールド法は、ゴール配置からロボットに作用する引力、障害物からロボットに作用する斥力、ロボットを動作範囲内に留める斥力の和からなる人工ポテンシャル関数を定義し、ポテンシャル関数のＣ空間における最急勾配方向に経路を探索する方法である。
多関節マニピュレータの場合はＣ障害物を陽に記述するのは困難なため、リンクにいくつかの代表点を設け、各代表点から一定距離内にある障害物からそれぞれの代表点に作用する斥力を作業空間で定義する。
【０００４】
【発明が解決しようとする課題】
しかしながら、上記従来例においては、▲３▼のＣ空間離散化法の場合は、ロボットの自由度、障害物の面の総数などに対して、計算量、記憶領域が指数関数的に増加してしまう。そのため、６自由度以上のマニピュレータに対して、Ｃ空間離散化法の基本アプローチをそのまま適用するのは現実的でなく、探索空間を制限したり（局所探索）、探索効率を高めるための工夫が必要となる。様々なヒューリスティックス（発見的手法:Heuristics ）を用いて探索空間削減、探索効率化をする方式が種々提案されているが、未だ、経路が高速で求まる実用的な方式が少ないという問題がある。
依って、本発明は、上記技術的問題を解決し、３次元作業空間内における自由度の大きな多関節マニピュレータに対しても、経路が高速で求まる実用的なロボットの動作経路計画方法およびその装置を提供することを目的としている。
【０００５】
【課題を解決するための手段】
上記課題を解決するため、請求項１記載のロボットの動作経路計画方法の発明は、ロボットと作業環境の幾何学的形状および前記ロボットと前記作業環境の配置を記述した計算機上の幾何モデル手段と、モデル同士の干渉を検査する計算機上の干渉検査手段と、を利用し、ロボットのスタートおよびゴール配置が与えられたときロボットと作業環境内の障害物とが干渉を起こさずにスタート配置からゴール配置へ至るロボットの動作経路を計画する動作経路計画方法において、（１）スタート配置を始端サブゴール、ゴール配置を終端サブゴールとし、ロボットの始端サブゴールと終端サブゴールを結ぶ作業空間あるいは配置空間における直線経路を２分割する点を最初のサブゴール候補とする第１ステップを実行し、（２）隣接するサブゴールをサブゴールａ、ｂと表し、該サブゴールａ、ｂ間を結ぶ作業空間あるいは配置空間における直線経路を２分割する点をサブゴール候補１と表して新たなサブゴールの候補とし、前記サブゴール候補１においてロボットと障害物との干渉を前記幾何モデル手段と干渉検査手段とを用いて検査する第２ステップを実行し、（３）（ｉ)前記サブゴール候補１において干渉が検出されなかったときは、サブゴール候補１を新たなサブゴールｃとして、ａ→ｃ→ｂで表されるサブゴールの接続関係を記述する有向グラフを生成し、（ｉｉ)前記サブゴール候補１において干渉が検出されたときは、前記サブゴール候補１から、予め定めた複数のロボット回避動作方向それぞれに予め定めた距離間隔でロボットを移動させた複数の点において干渉検査を行い、それぞれの前記回避動作方向あるいはその内の幾つかに対して、干渉が検出されない点の中から新たなサブゴールｄ1、ｄ2、ｄ3、・・・を１つづつ選び、ａ→ｄ1→ｂ、ａ→ｄ2→ｂ、ａ→ｄ3→ｂ、・・・で表されるサブゴールの有向グラフを生成する第３ステップを実行し、（４）前記第２および第３ステップを各サブゴール間の距離が予め定めた値以下になるまで、再帰的に繰り返す第４ステップを実行し、（５）前記サブゴールの有向グラフを用いて始端サブゴールから終端サブゴールへ至るサブゴール経路であるサブゴール系列を複数組計画する第５ステップを実行し、（６）前記計画された複数組のサブゴール系列の中から、各サブゴール間距離の和が最小となるサブゴール系列を選択する第６ステップを実行し、（７）前記選出されたサブゴール系列に対して、前記サブゴール系列内の各サブゴールまたは各サブゴールを中心とした所定半径の円もしくは球の内側を通過する経路を、前記ロボットと前記作業環境の干渉を検査しながら、離散化された配置空間におけるグラフ探索手法を用いて、前記サブゴール系列の間隔よりもより微小な間隔で求める第７ステップを実行することを特徴としている。
【０００６】
上記構成によれば、隣接するサブゴールａ、ｂの直線経路を２分割するサブゴール候補１を、干渉が検出されない場合は新たなサブゴールｃとしてａ→ｃ→ｂのサブゴール有向グラフを作成し、干渉が検出された場合は予め定められた複数のロボット回避方向における複数点で干渉検査を行い、干渉が検出されない点の中から新たなサブゴールｄ1 、ｄ2 、ｄ3 ・・・を１つづつ選んで、ａ→ｄ1 →ｂ、ａ→ｄ2 →ｂ、ａ→ｄ3 →ｂ、・・・というサブゴール有向グラフを作成して、始端サブゴールから終端サブゴールへ至るサブゴール系列を複数組計画する。
これら複数のサブゴール系列の中から、最も小さい距離和等から選ばれたサブゴール系列におけるサブゴールあるいは各サブゴール近傍を通過する経路を、離散化された位置空間におけるグラフ探索手法により、離散化セル単位の、より微小間隔で詳細に求めることによって、終端サブゴールまでの経路を高速、実用的に求めることが可能になる。
あるいは、このように離散化された位置空間内の離散化セル単位の微小間隔経路の求め方についても、単に各サブゴールを経由する微小間隔経路を求めるのではなく、今、サブゴール系列内の連続するサブゴールを、ｓｂn-1 、ｓｂn 、ｓｂn+1 とした場合に、サブゴールｓｂn-1 近傍から、サブゴールｓｂn 近傍へ微小間隔経路を探索する際に、ｓｂn-1 、ｓｂn を含む離散化された局所配置空間とｓｂn を中心とした近傍領域を設定し、探索される離散化セルをｃｐ、点ｘ−ｙの距離ｄ、任意の実数をａ、ｂ、ｃとして、
ａ×ｄ（ｓｂn-1 、ｃｐ）＋ｂ×ｄ（ｃｐ、ｓｂn ）＋ｃ×ｄ（ｃｐ、ｓｂn+1 ）
を評価関数として、この評価関数が小さな離散化セルを優先的に探索することによって、終端サブゴールまでの距離和がより小さく、滑らかな経路を得ることができる。
あるいは、このようなサブゴール系列内の微小間隔経路を、これまでのＣ空間離散化法ではなく、局所ポテンシャル法により求めることも可能であり、この場合はＣ空間離散化法と同様に比較的距離の短い２つのサブゴールを包含する局所Ｃ空間に探索範囲を限定することによって、より精細な経路探索を行うことができる。
あるいは、経路が見つからない場合の判定と経路変更については、優先順位の高いサブゴール系列から順に１つサブゴール系列を選択し、選択されたサブゴール系列内の各サブゴール、若しくは各サブゴール近傍を通過する微小間隔経路を求めるような場合に、あるサブゴール近傍から次のサブゴール近傍へ至る経路が、その２つのサブゴールを包含する局所配置空間内では存在しないと判明した場合、又は一定時間内に経路が見つからない場合は、次に優先順位が高いサブゴール系列を選出して微小間隔経路を求める手続きを繰り返すので、無駄な探索を省いて効率的で高速な経路探索を実現できる。
あるいは、３次元作業空間における多関節マニピュレータの回避動作方向を設定するような場合にも、水平面内の４方向と鉛直方向の５つの方向等に、ハンド先端位置あるいは手首点位置が沿うように、Ｃ空間において経路探索法を適用することによって、有効に経路の探索空間を削減し、高速に回避動作経路を求めることが可能である。
【０００７】
【発明の実施の形態】
（第１の実施の形態）
以下、本発明の第１の実施の形態について図を参照して説明する。
図１は本発明の第１の実施の形態に係るロボットの動作経路計画装置のブロック図である。
図２は図１に示すサブゴール有向グラフ生成手続きのフローチャートである。
図３は図１に示すサブゴール有向グラフ生成過程の説明図である。
図４は図１に示すサブゴール有向グラフ生成手段により生成されたサブゴール有向グラフを示す図である。
図５は図１に示す局所配置空間内経路探索により求める微小間隔経路の説明図である。
図６は図１に示すロボットが２自由ロボットの場合のＣ空間を示す図である。
先ず、本実施の形態の基本構成について、図６を用いて説明する。
図６（ａ）は２次元作業空間内の２自由度マニピュレータ、図６（ｂ）は２次元作業空間内の２自由度移動ロボットを表している。
【０００８】
図６（ａ）および（ｂ）において、７０１は２自由度マニピュレータ、７０２はそのエンドエフェクタ、７０３は２自由度移動ロボットを示している。７０４は作業空間における障害物である。２自由度ロボットエンドエフェクタ７０２の位置、移動ロボット７０３の位置をｐ（ｘ, ｙ）と表す。また、マニピュレータ７０１の第１、２関節角度をそれぞれｑ1,ｑ2 と表す。図６（ｃ）の７０５は２自由度マニピュレータ７０１の関節空間、すなわちＣ空間を表している。また、移動ロボット７０３のＣ空間は（ｘ, ｙ）であるが、どちらも同じ２次元Ｃ空間であり、図６（ｃ）はマニピュレータ、移動ロボットの区別なく任意の２自由度ロボットのＣ空間を表すものとする。図６（ｃ）において、７０６、７０７はそれぞれロボットのスタート配置Ｓ、ゴール配置Ｇを表している。
以下、本実施の形態は、一般的に図６（ａ）および（ｂ）のようなロボットと作業環境の幾何学情報、スタート配置Ｓ、ゴール配置Ｇが与えられたとき、ＳからＧへ至る非干渉経路を計画する方法およびその装置を扱うものである。
【０００９】
図１において、１０１はスタート、ゴール配置入力手段であり、図６に示したようなスタート配置Ｓ、ゴール配置Ｇ等を入力する手段である。以下、ここに述べる各手段は計算機上に設定される機能である。
１０２はロボット、環境モデル入力手段で、図６に示すようにロボットと作業環境の幾何学情報等を入力する手段である。１０４はロボットと作業環境の幾何学形状を記述した幾何モデルを記憶する記憶手段、１０５は干渉検査手段であって、記憶手段１０４の幾何モデラを参照してモデル同士の干渉の有無を判定する計算機上の判定手段である。１０７はサブゴール間の距離を判定するサブゴール間距離評価手段であり、１０３は干渉検査手段１０５とサブゴール間距離評価手段１０７の評価を基に始端サブゴールＳから終端サブゴールＧまでのサブゴール有向グラフを生成するサブゴール有向グラフ生成手段である。
１０６はサブゴール系列生成・ソート手段であって、サブゴール有向グラフ生成手段１０３によって生成されたサブゴール有向グラフより、幾つかのサブゴール系列を計算機上に実現して、始端サブゴールＳ〜終端サブゴールＧまでの距離和の小さい順にソートを行う。１０８はサブゴール系列選択手段で、例えば、ソート順にサブゴール系列を選択し取り出す。１０９は隣接サブゴール選択手段であって、サブゴール系列選択手段１０８で選択されたサブゴール系列内の隣接する２つのサブゴールをスタート配置から順に選択する。１１０は配置空間局所化手段であって、Ｃ空間離散化法による微小間隔経路探索のための局所離散化Ｃ空間を設定する。１１１は局所配置空間内経路探索手段であり、局所離散化Ｃ空間内において微小間隔経路探索を行う。１１２は経路存在判定手段で、微小間隔経路探索の結果、微小間隔経路が存在するかなどを判定する。１１３は経路記憶手段で、得られた探索経路を記憶している。１１４は経路出力手段であり、記憶手段１１３に保持する経路を要求に応じて出力する。
【００１０】
つぎに動作について説明する。
図３は本発明による（障害物回避）動作経路計画方法におけるサブゴール有向グラフの生成過程を説明している図である。
図３において、３０１は２次元Ｃ空間、３０２、３０３はそれぞれロボットのスタート配置Ｓ、ゴール配置Ｇを示している。スタート配置Ｓ、ゴール配置Ｇをそれぞれ始端サブゴールＳ、終端サブゴールＧと表現し、以降、便宜上サブゴールとして扱う。
先ず、図３（ａ）において、始端サブゴールＳ、終端サブゴールＧを結ぶ直線の中点をサブゴール候補３０４とし、ロボットと作業環境の幾何学的形状を記述した計算機上の幾何モデル記憶手段１０４と、モデル同士の干渉を判定する計算機上の干渉検査手段１０５を用いて、サブゴール候補３０４におけるロボットと作業環境との干渉を検査する。
サブゴール有向グラフ生成手段１０３は、サブゴール候補３０４において干渉が検出されなかった場合は、図３（ｂ）に示すように、サブゴール候補３０４をサブゴール３０５（以降、ａ0 と表す）とし、サブゴールの有向グラフＳ→ａ0 →Ｇを生成する。
サブゴール候補３０４において、干渉が検出された場合、その点から複数のロボット回避動作方向に沿って新たなサブゴール候補を複数設定する。図３（ｃ）において３０６は干渉が検出されたサブゴール候補、３０７、３０８はロボットの回避動作方向を示している。３０７ａ、３０７ｂ、３０７ｃはロボット回避動作方向３０７に沿ったサブゴール候補を、３０８ａ、３０８ｂ、３０８ｃはロボット回避動作方向３０８に沿ったサブゴール候補を示している。そして、サブゴール候補３０７ａ、３０７ｂ、３０７ｃ、および３０８ａ、３０８ｂ、３０８ｃにおける干渉を検査し、干渉が検出されなかったサブゴール候補の中から、それぞれのロボット回避動作方向に対して、新たなサブゴールを１つ設定する。
図３（ｄ）において、３０９はロボット動作回避方向３０７に沿った新たなサブゴールｂ0 を、３１０はロボット動作回避方向３０８に沿った新たなサブゴールｃ0 を示している。新たなサブゴールｂ0,ｃ0 に対して、サブゴールの有向グラフＳ→ｂ0 →ＧおよびＳ→ｃ0 →Ｇを生成する。以上の手続を任意の隣接する２つのサブゴールに対して、各サブゴール間の距離が一定値以下になるまで、再帰的に繰り返す。
図４には、以上の手続の再帰的繰り返しによって得られたサブゴールの有向グラフを表している。図４において、４０１、４０２、４０３はそれぞれ２次元Ｃ空間、始端サブゴールＳ、終端サブゴールＧである。４０４は各サブゴールである。図４では、特に符合をつけていないが、４０４と同様の記号は全てサブゴールを表している。サブゴール有向グラフはサブゴール有向グラフ生成手段１０３により、連結リストや配列などのデータ構造により計算機上に実現することができる。
また、図３および図４では、２つのサブゴールを結ぶＣ空間での直線の中点をサブゴール候補としているが、作業空間における直線の中点をサブゴール候補としてもよい。
更には、２つのサブゴールを結ぶ任意曲線を適当に２分割する点をサブゴール候補としてもよい。
図３（ｃ）では、ロボット回避動作方向３０７、３０８に沿ったサブゴール候補を等間隔で設定しているが、適当な間隔でサブゴール候補を設定してもよい。
また、必ずしも複数のロボット回避動作方向全てに対して、サブゴールを設定しなくてもよい。例えば、それぞれのロボット動作回避方向に沿ったサブゴール候補について干渉検査を行う際、それぞれの回避方向について、干渉の検出されなかったサブゴール候補が連続していくつあるかを調べ、連続した非干渉サブゴール候補の個数が大きな回避方向から順に、適当な数、サブゴールを設定するようしてもよい。
また、図３および図４ではサブゴール間を結ぶＣ空間での直線に対して垂直な方向に２つロボット回避動作方向を設定しているが、他の適当な基準やアルゴリズムに基づいて、任意個数、任意方向に回避動作を決定してもよい。
【００１１】
図２は以上のサブゴール有向グラフ生成手続のフローチャートを示す。
図２において、以上のサブゴール有向グラフ生成手続きを反芻すると、スタート配置Ｓ・ゴール配置Ｇを入力して（Ｓ２００）、サブゴール有向グラフ生成手段１０３によるサブゴール有向グラフの作成を開始する（Ｓ２０１）。サブゴール間距離評価手段１０７の間隔評価に基づき各サブゴールの間隔を確認し（Ｓ２０２〜Ｓ２０４）。一定距離以上間隔があれば新たにサブゴール候補を作成して（Ｓ２０５）、干渉検査手段１０５による干渉チェックを行い（Ｓ２０６）、非干渉が確認されたら新サブゴールとして追加し（Ｓ２０９）、サブゴール有向グラフを作成して（Ｓ２０１）、サブゴール間の間隔を確認し（Ｓ２０２〜２０４）、一定距離以下になったら終了する（Ｓ２１０）。
一方、Ｓ２０６の干渉チェックで干渉が確認されたら、回避方向のサブゴール候補を作成して（Ｓ２０７）、改めて干渉チェックとサブゴール選択を行う（Ｓ２０８）。
次に、以上の手続によって得られたサブゴール有向グラフを用いて、始端サブゴールＳから終端サブゴールＧへ至るサブゴール経路（サブゴール系列）を求める。
図４の例では、サブゴール系列は以下の５つある。サブゴール系列は、各サブゴールの関節角を要素に持つ連結リストによって計算機上に実現できる。
（１）Ｓ→・・・→ｄ1 →・・・→ｂ0 →・・・→ｆ1 →ｈ1 →Ｇ
（２）Ｓ→・・・→ｄ1 →・・・→ｂ0 →・・・→ｆ1 →ｉ1 →Ｇ
（３）Ｓ→・・・→ｅ1 →・・・→ｂ0 →・・・→ｆ1 →ｈ1 →Ｇ
（４）Ｓ→・・・→ｅ1 →・・・→ｂ0 →・・・→ｆ1 →ｉ1 →Ｇ
（５）Ｓ→・・・→ｃ0 →ｊ1 →ｇ1 →・・・              →Ｇ
上の式（１）〜（５）に示したようなサブゴール系列連結リストを求める際、サブゴール系列生成・ソート手段１０６により、サブゴール間の距離和を計算する。各サブゴール系列の距離和とそれぞれのサブゴール系列連結リストの先頭要素へのポインタを要素として持つヘッダー連結リストを別に作成する。その距離和が小さい順にヘッダー連結リストをソートすることにより、距離和が小さいサブゴール系列から順にデータを取り出すことができる。
次に、サブゴール間距離和の最も小さなサブゴール系列に対して、配置空間局所化手段１１０と局所配置空間内経路探索手段１１１により、図５に示すような各サブゴール間を結ぶ微少間隔経路をＣ空間離散化法に基づいて求める。
図５において、６０２はスタート配置、すなわち始端サブゴールである。６０３ａ、６０３ｂ、６０３ｃはそれぞれ連続するサブゴールｓｂ1 、ｓｂ2 、ｓｂ3 である。６０１はｓｂ1 、ｓｂ2 を包含する離散化された局所Ｃ空間である。６０１における微少な正方形それぞれが離散化セルである。あるサブゴール（ｓｂ1 ）から次のサブゴール（ｓｂ2 ）への経路を、局所離散化Ｃ空間６０１内の各離散化セルにおいて、逐次、ロボットと作業環境の干渉を検査しながらＡ＊などのグラフ探索手法を用いて探索する。６０４は探索された経路を示している。
経路がサブゴールｓｂ2 に到達した時点で、サブゴールｓｂ2 とつぎのサブゴールｓｂ3 を包含する局所離散化Ｃ空間を新たに設定し直し、サブゴールｓｂ3 へ至る微少間隔経路を同様にして探索する。局所離散化Ｃ空間内６０１の全てのセルを探索しても、または、一定時間探索しても、その局所離散化Ｃ空間内のサブゴールｓｂ1 から次のサブゴールｓｂ2 への経路が見つからない場合、その２つのサブゴール系列を含まず、前述した距離和が、つぎに小さなサブゴール系列をサブゴール系列連結リストから選びだし、同様に微少間隔経路を求めていく。この手続をスタート配置Ｓからゴール配置Ｇへ至る微少間隔経路が求まるまで、あるいは一定時間繰り返す。
このようにして、一定時間バックトラッキングを繰り返しても、スタート配置Ｓからゴール配置Ｇへ至る微少間隔経路が見つからない場合、または、全てのサブゴール系列に対して、微少間隔経路探索に失敗した場合は、経路存在判定手段１１３は経路が存在しないものと判断する。
【００１２】
（第２の実施の形態）
次に、本発明の第２の実施の形態について図を参照して説明する。
図７は本発明の第２の実施の形態に係るＣ空間離散化法による微小間隔経路の探索過程を示す図である。
前実施の形態に示したようなサブゴール系列は、各サブゴール付近の方向を辿っていけば、ゴール配置に効率的に到達できる可能性が高いということを示しているだけであるから、各サブゴールを経由する微少間隔経路を求める必然性はあまりない。また、経路が滑らかにならない場合が多くなる。
そこで、第２の実施の形態では、図７に示すように、局所離散化Ｃ空間内のサブゴールｓｂ1 、ｓｂ2 それぞれを中心とする近傍領域６０５ａ、６０５ｂを設定し、各サブゴール近傍領域を通過する微少間隔経路を探索するように構成している。
今、サブゴールｓｂn-1 近傍から、サブゴールｓｂn 近傍への微少間隔経路を探索する際、オープンされる離散化セルをｃｐとすると、
0004103057.tif 000002
を評価関数として、上記評価関数値が小さな離散化セルを優先的に探索する。
ここで、ｄ（ｘ, ｙ）は点ｘから点ｙまでの距離を表す。離散化セルの探索経路がサブゴールｓｂn の近傍領域に侵入した時点で、ｓｂn およびｓｂn+1 を包含する局所離散化Ｃ空間を新たに設定し直し、ｓｂn+1 近傍への経路を式（６）の評価関数に基づいて同様に探索する。ただし、終端Ｇについては近傍領域を設定しない。このようにして、終端サブゴールまでの距離和が改善され、より滑らかな経路が得られる。
【００１３】
（第３の実施の形態）
次に、本発明の第３の実施の形態について説明する。
第３の実施の形態は、前実施の形態がＣ空間離散化法を用いて探索したのに対し、局所ポテンシャル法により求めるものである。
始端サブゴールから終端サブゴールへ至るサブゴール系列内の各サブゴール、または各サブゴール近傍領域を通過する微少間隔経路を局所ポテンシャル法により求めるには、Ｃ空間離散化法ベースの上記方法と同様に、２つのサブゴールを包含する局所Ｃ空間を設定し、そのなかでの経路をより精細に探索する。
ポテンシャルフィールド法は、目標配置以外のポテンシャル極小点に経路が落ち込む問題が指摘されているが、本発明による方式では、この問題をうまく回避できる場合がある。
つまり、比較的距離の短い２つのサブゴールを包含する局所Ｃ空間に探索範囲を限定することによって、局所ポテンシャル関数の極小値が発生する可能性が低くなる。
しかも、各サブゴール近傍を通過する経路を探索することにより、たとえ、ポテンシャル極小値が存在しても、その極小値が目標のサブゴール近傍領域内にあれば、その極小値は問題にならない。なぜなら、目標のサブゴール近傍領域に到達した時点で、そのつぎのサブゴールを新たな目標として、ポテンシャル関数を再定義して経路を探索するからである。このようにして局所ポテンシャル法を有効に導入できる。
【００１４】
（第４の実施の形態）
次に、本発明の第４の実施の形態について図を参照して説明する。
図８は本発明の第４の実施の形態に係る６自由度マニピュレータの作業環境を示す図である。
第１の実施の形態における図３、４では、簡便さのため、２自由度ロボットを例にとって説明したが、２自由度以上の多自由度ロボットについて、同様にしてサブゴールの有向グラフを求めることができる。
図８（ａ）は３次元作業空間における多関節マニピュレータを示している。
図８（ａ）  において、５０１ａ、５０１ｂ、５０１ｃ、５０１ｄ、５０１ｅ、５０１ｆはそれぞれ、多関節マニピュレータの肩、上腕、肘、前腕、手首、ハンドを示している。また、５０２は作業台、５０３は作業台５０２上の障害物である。図５（ｂ）において、５１０は多関節マニピュレータのベース座標系、５１１は手首点５０１ｅをｘｙ平面内に投射した点Ｗｐを示している。ベース座標５１０のＺ正方向は鉛直上方向に一致している。つまりｘｙ平面は水平面である。  ピックアンドプレース作業などハンドリング作業の多くは、図８（ ａ） に示すように作業台は水平に設置され、マニピュレータは鉛直に設置されている。そして、障害物も鉛直に設定されている場合が多い。このような場合におけるマニピュレータの回避動作は、矢印５０４に示す鉛直上方向回り、５０５に示すマニピュレータベースに対する内回り（水平面内）、５０６に示す同外回りの３つのパターン、および水平面内における時計回り、反時計回りに限定しても、実用上問題ない場合が多い。
そこで、上記パターン分類に基づいてロボット回避動作方向を設定する。
図８（ｂ）において、５０７はＺ軸正方向、５０８ａはベクトルＷｐＯ方向、５０８ｂはベクトルＯＷｐ方向である。５０９ａ、５０９ｂは水平面内における直線ＯＷｐに対して垂直な２方向である。この水平面内の４方向と鉛直上向の５つの方向に、ハンド先端位置あるいは手首点位置が沿うように、Ｃ空間においてロボット回避動作方向を設定する。
このように、作業内容や作業環境およびロボットの構造を利用したヒューリスティックス（すなわち、経験によって得られる、状況に応じた知識）に基づき、ロボットの回避動作方向を予め定めておくことにより、経路の探索空間を大幅に削減し、高速に経路を求めることが可能となる。
【００１５】
（第５の実施の形態）
次に、本発明の第５の実施の形態について図を参照して説明する。
図８に示したような６自由度マニピュレータの場合、以上説明したように、まず、サブゴール系列を求め、各サブゴール、あるいは各サブゴール近傍を経由する微少間隔経路を、手首点位置経路に対してはＣ空間離散化法で、マニピュレータの手首点座標系を基準にしたハンド姿勢経路に対しては局所ポテンシャル法で求めるようにして、２方式併用により相乗効果が期待できる。
【００１６】
【発明の効果】
以上説明したように、本発明によれば、スタート配置を始端サブゴール、ゴール配置を終端サブゴールとし、ロボットの始端サブゴールと終端サブゴールを結ぶ作業空間あるいは配置空間における直線経路を２分割する点を最初のサブゴール候補とし、隣接するサブゴールをサブゴールａ、ｂと表し、その間を結ぶ作業空間あるいは配置空間における直線経路を２分割する点をサブゴール候補１と表して新たなサブゴールの候補とし、サブゴール候補１においてロボットと障害物との干渉を幾何モデル手段と干渉検査手段を用いて検査し、サブゴール候補１ににおいて干渉が検出されなかった場合、サブゴール候補１を新たなサブゴールｃとして、ａ→ｃ→ｂで表されるサブゴールの接続関係を記述する有向グラフを生成し、サブゴール候補１において干渉が検出された場合はサブゴール候補１から、予め定めた複数のロボット回避動作方向それぞれに適当な距離間隔でロボットを移動させた複数の点において干渉検査を行い、それぞれの回避動作方向、あるいはその内の幾つかに対して、干渉が検出されない点の中から新たなサブゴールｄ1 、ｄ2 、ｄ3 、・・・を１つづつ選び、ａ→ｄ1 →ｂ、ａ→ｄ2 →ｂ、ａ→ｄ3 →ｂ、・・・で表されるサブゴールの有向グラフを生成する手続を、各サブゴール間の距離が予め定めた値以下になるまで、再帰的に繰り返し、サブゴールの有向グラフを用いて、始端サブゴールから終端サブゴールへ至るサブゴール経路、すなわちサブゴール系列を複数組計画するように構成し、更に各サブゴール間を結ぶ経路、あるいは各サブゴール近傍を通過する経路を、Ｃ空間離散化法を用いて、より微少間隔で求めるように構成しているため、探索空間が大幅に抑制され、探索効率が向上し、３次元作業空間における自由度の大きな多関節マニピュレータに対しても高速で実用的な経路計画が実現できるという効果がある。
【図面の簡単な説明】
【図１】本発明の第１の実施の形態に係るロボットの動作経路計画装置のブロック図である。
【図２】図１に示すサブゴール有向グラフ生成手続きのフローチャートである。
【図３】図１に示すサブゴール有向グラフ生成過程の説明図である。
【図４】図１に示すサブゴール有向グラフ生成手段により生成されたサブゴール有向グラフを示す図である。
【図５】図１に示す局所配置空間内経路探索により求める微小間隔経路の説明図である。
【図６】図１に示すロボットが２自由度ロボットの場合のＣ空間を示す図である。
【図７】本発明の第２の実施の形態に係るＣ空間離散化法による微小間隔経路の探索過程を示す図である。
【図８】本発明の第４の実施の形態に係る６自由度マニピュレータの作業環境を示す図である。
【符号の説明】
１０１  スタート・ゴール配置入力手段
１０２  ロボット・作業環境の幾何モデル入力手段
１０３  サブゴール有向グラフ生成手段
１０４  幾何モデル記憶手段
１０５  干渉検査手段
１０６  サブゴール系列生成・ソート手段
１０７  サブゴール間距離評価手段
１０８  サブゴール系列選択手段
１０９  隣接サブゴール選択手段
１１０  配置空間局所化手段
１１１  局所配置空間内経路探索手段
１１２  経路存在判定手段
１１３  経路記憶手段
１１４  経路出力手段
３０１  Ｃ空間
３０２  スタート配置
３０３  ゴール配置
３０４  ３０２と３０３を結ぶ直線経路を２分割するサブゴール候補
３０５  ３０２と３０３を結ぶ直線経路を２分割するサブゴール
３０６  干渉が検出されたサブゴール候補
３０７  ロボット回避動作方向
３０７ａ、ｂ、ｃ  ロボット回避動作方向３０７に沿ったサブゴール候補
３０８  ロボット回避動作方向
３０８ａ、ｂ、ｃ  ロボット回避動作方向３０８に沿ったサブゴール候補
３０９  ロボット回避動作方向３０７から選択されたサブゴール
３１０  ロボット回避動作方向３０８から選択されたサブゴール
４０１  Ｃ空間
４０２  スタート配置
４０３  ゴール配置
４０４  サブゴール
５０１ａ  マニピュレータの肩
５０１ｂ  マニピュレータの上腕
５０１ｃ  マニピュレータの肘
５０１ｄ  マニピュレータの前腕
５０１ｅ  マニピュレータの手首
５０１ｆ  マニピュレータのハンド
５０２  作業台
５０３  障害物
５０４  鉛直上回りの障害物回避経路
５０５  マニピュレータに対して内回りの水平面内障害物回避経路
５０６  マニピュレータに対して外回りの水平面内障害物回避経路
５０７  鉛直上方向のマニピュレータ回避動作方向
５０８ａ  ＷｐＯ方向のマニピュレータ回避動作方向
５０８ｂ  ＯＷｐ方向のマニピュレータ回避動作方向
５０９ａ、ｂ  ＷｐＯに対して垂直かつ水平平面内のマニピュレータ回避動作方向
５１０  マニピュレータのベース座標系
５１１  マニピュレータ手首点を水平面（ＸＹ平面）に投影した点Ｗｐ
６０１  局所離散化Ｃ空間
６０２  スタート配置
６０３ａ、ｂ、ｃ  サブゴールｓｂ1 、ｓｂ2 、ｓｂ3
６０４  探索された離散化経路
６０５ａ、ｂ  サブゴール６０３ａ、ｂの近傍領域
７０１  ２次元作業空間における２自由度マニピュレータ
７０２  ２次元作業空間における２自由度マニピュレータのエンドエフェクタ
７０３  ２次元作業空間における２自由度移動ロボット
７０４  障害物
７０５  Ｃ空間
７０６  スタート配置
７０７  ゴール配置
【図１】
0004103057.tif 000003
【図２】
0004103057.tif 000004
【図３】
0004103057.tif 000005
【図４】
0004103057.tif 000006
【図５】
0004103057.tif 000007
【図６】
0004103057.tif 000008
【図７】
0004103057.tif 000009
【図８】
0004103057.tif 000010