経路探索システム、経路探索方法、及び移動体
【課題】理想的な最短経路により近い経路を生成可能とする。
【解決手段】本発明に係る経路探索システム１１０は、進入禁止グリッドが設定されたグリッドマップを記憶する地図情報記憶部１１１と、移動始点及び障害物からの距離に応じたポテンシャル値に基づき、各グリッドの距離ポテンシャル値を生成する距離ポテンシャル生成部１１２と、前回のグリッド探索ベクトルから、次探索グリッドの決定と、局所ポテンシャル場計算用グリッドの選択をする探索グリッド決定部１１３と、局所ポテンシャル場を計算する局所ポテンシャル場計算部１１４と、前回のグリッド探索ベクトルが次探索グリッドに進入する際のエッジ上の点を探索枝の基点として、局所ポテンシャル場の最急勾配で降る方向に探索枝を延ばし、その方向をグリッド探索ベクトルとして決定するグリッド探索方向決定部１１５とを備える。
【解決手段】本発明に係る経路探索システム１１０は、進入禁止グリッドが設定されたグリッドマップを記憶する地図情報記憶部１１１と、移動始点及び障害物からの距離に応じたポテンシャル値に基づき、各グリッドの距離ポテンシャル値を生成する距離ポテンシャル生成部１１２と、前回のグリッド探索ベクトルから、次探索グリッドの決定と、局所ポテンシャル場計算用グリッドの選択をする探索グリッド決定部１１３と、局所ポテンシャル場を計算する局所ポテンシャル場計算部１１４と、前回のグリッド探索ベクトルが次探索グリッドに進入する際のエッジ上の点を探索枝の基点として、局所ポテンシャル場の最急勾配で降る方向に探索枝を延ばし、その方向をグリッド探索ベクトルとして決定するグリッド探索方向決定部１１５とを備える。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、経路探索システム、経路探索方法、及び移動体に関する。
【背景技術】
【０００２】
グリッド空間において、最短経路探索を行う技術が提案されている。このような経路探索技術が、特許文献１乃至５に開示されている。例えば特許文献１に開示された移動体の経路生成装置では、移動体や障害物の情報を用いて移動領域内のグリッドマップを生成し、それぞれの相対位置関係から引力ポテンシャル、斥力ポテンシャルを計算する。そして、これらポテンシャルの合成ポテンシャルに基づきマップ内での経路探索を行い、ローカルミニマムを判定して経路を生成する。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００６−３５０７７６号公報
【特許文献２】特開平５−２７４０３７号公報
【特許文献３】特開２００３−０２９８３３号公報
【特許文献４】特開２００５−０３２１９６号公報
【特許文献５】特開２００９−０２５９７４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、グリッド空間において経路探索を行う技術では、理想的な最短経路とは大きく異なる経路が最短経路として生成されることがあるという問題があった。
【０００５】
この問題点を説明するため、図１０（Ａ）を参照して、グリッド空間における一般的な経路探索手法について説明する。図において、移動始点のグリッドＳから、移動終点のグリッドＧへと向けて、経路を探索する。なお、図において左下方向への斜線により示すグリッドは、障害物などにより移動が禁止されるグリッドを示す。
【０００６】
ここでの経路探索では、前後左右のグリッド及び斜め方向に位置する周囲のグリッドを次に選択可能なグリッドとする。そして、現在のグリッドから周囲の各グリッドまでのグリッド間距離に基づいて、移動始点から移動終点へと至る経路の経路長が最短となるように、当該経路を構成するグリッドを順次決定していく。このため、図１０（Ａ）の例では、前後左右のグリッド間の距離と斜め方向に位置するグリッド間の距離をそれぞれ例えば５及び７とした場合、グリッドＳから探索を開始して、白抜き矢印により示す方向のグリッドが、経路を構成するグリッドとして順次決定されていく。この際、探索される経路の経路長が、グリッド間距離の総和として表現される。
【０００７】
これに対して、グリッドＳからグリッドＧへと至る最適な経路の例を図１０（Ｃ）に示す。すなわち、グリッドＳからグリッドＧへと至る理想的な経路は、図１０（Ｃ）に示すような直線になるが、このような最適な経路に対して図１０（Ａ）に示した経路は大きく異なったものになる。
【０００８】
これは、経路探索の計算過程で、前後左右及び斜めに位置するグリッドに関して、グリッド間の距離の総和として経路長を表現しているために、経路長の距離分解能がグリッド間距離まで粗くなってしまい、その結果、理想的な経路とは大きく異なる経路が生成されてしまうためである。換言すると、グリッド単位で離散化しているために、経路長の計算数値がグリッドごとに丸め込まれてしまい、グリッド空間の離散誤差が蓄積することで、局所的には最適であるが、全体最適とはならない経路が生成されてしまうためである。
【０００９】
従って、本発明は、上述した課題を解決して、理想的な最短経路により近い経路を生成可能な経路探索システム、経路探索方法、及び移動体を提供することを目的とする。
【課題を解決するための手段】
【００１０】
本発明に係る第一の態様の経路探索システムは、移動領域を、各グリッドを区切る周囲の枠をグリッドのエッジとする、複数の格子状のグリッドに分割し、移動始点と移動終点の間を結ぶグリッド系列を探索する経路探索システムであって、障害物が占有し、移動体の進入が禁止される進入禁止グリッドが設定されたグリッドマップを記憶する地図情報記憶部と、前記移動始点からの距離に応じたポテンシャル値と、前記障害物からの距離に応じたポテンシャル値と、を加算して、前記グリッドマップの各グリッドの距離ポテンシャル値として生成する距離ポテンシャル生成部と、前回探索対象グリッドにおいて決定されたグリッド探索ベクトルが次に進入する隣接グリッドを、次に探索対象とする次探索グリッドとして決定すると共に、前記グリッド探索ベクトルの向きに応じて、前記次探索グリッドに隣接する第１及び第２のグリッドを、当該次探索グリッドについての局所ポテンシャル場計算用グリッドとして選択する探索グリッド決定部と、前記次探索グリッドの距離ポテンシャル値に対する前記局所ポテンシャル場計算用グリッドの距離ポテンシャル値の相対変化に基づく頂点を持つ局所ポテンシャル場であって、前記次探索グリッドについての局所ポテンシャル場を計算する局所ポテンシャル場計算部と、前回探索対象グリッドにおいて決定されたグリッド探索ベクトルが前記次探索グリッドに進入する際のエッジ上の点を探索枝の基点として、前記局所ポテンシャル場のポテンシャル値が最急勾配となる方向であって、当該勾配を降る方向に当該探索枝を延ばし、当該延ばした探索枝の方向を、前記次探索グリッドにおけるグリッド探索ベクトルとして決定するグリッド探索方向決定部と、を備えるものである。
【００１１】
このように次探索グリッドについて、局所ポテンシャル場を計算し、計算した局所ポテンシャル場での勾配に応じて次探索グリッドのエッジ間の探索枝を求め、その探索枝の方向をグリッド探索ベクトルとして決定し、そのグリッド探索ベクトルから、さらに続く次の探索対象グリッドを探索していくことで、従来手法と比較して次探索グリッドの決定をより詳細に行うことができるため、この結果、理想的な最短経路により近い経路を生成することができる。例えば、図１０に示した例では、従来技術による探索結果が図１０（Ａ）に示されるのに対して、本発明にかかる経路探索システムによる探索結果は図１０（Ｂ）に示される。
【００１２】
また、前記局所ポテンシャル場計算部は、前記局所ポテンシャル場として、前記次探索グリッドの距離ポテンシャル値の二乗と、前記局所ポテンシャル場計算用グリッドの距離ポテンシャル値の二乗と、の差に応じて定まる頂点を持つ、円錐ポテンシャル場を計算するものとしてもよい。
【００１３】
さらにまた、前記グリッド探索方向決定部は、前記探索枝の前記基点から前記局所ポテンシャル場の前記頂点へと向かう直線上の点であって、前記基点が位置するエッジとは異なる前記次探索グリッドの他のエッジ上に位置する点を、前記探索枝の端点として求め、前記探索枝の前記基点から前記端点へと向かう方向を、前記次探索グリッドにおけるグリッド探索ベクトルとして決定するものとしてもよい。
【００１４】
また、前記距離ポテンシャル生成部は、隣接グリッド間の移動コストとして所定の値を定め、前記移動始点から目標グリッドへと至るグリッド系列を探索して、探索した当該グリッド系列についての前記移動コストの総和を求め、当該総和値と、前記障害物からの距離に応じたポテンシャル値と、を加算して、前記目標グリッドの距離ポテンシャル値を算出するものとしてもよい。
【００１５】
本発明に係る第二の態様の経路探索方法は、移動領域内の移動始点から移動終点へと移動する移動体の経路探索方法であって、前記移動領域を、各グリッドを区切る周囲の枠をグリッドのエッジとする、複数の格子状のグリッドに分割したグリッドマップを用いて表して、障害物が占有し、前記移動体の進入が禁止される進入禁止グリッドが設定されたグリッドマップを記憶するステップと、前記移動始点からの距離に応じたポテンシャル値と、前記障害物からの距離に応じたポテンシャル値と、を加算して、前記グリッドマップの各グリッドの距離ポテンシャル値として生成するステップと、前回探索対象グリッドにおいて決定されたグリッド探索ベクトルが次に進入する隣接グリッドを、次に探索対象とする次探索グリッドとして決定すると共に、前記グリッド探索ベクトルの向きに応じて、前記次探索グリッドに隣接する第１及び第２のグリッドを、当該次探索グリッドについての局所ポテンシャル場計算用グリッドとして選択するステップと、決定した前記次探索グリッドと、選択した前記局所ポテンシャル場計算用グリッドについて、前記次探索グリッドの距離ポテンシャル値に対する前記局所ポテンシャル場計算用グリッドの距離ポテンシャル値の相対変化に基づく頂点を持つ局所ポテンシャル場であって、前記次探索グリッドについての局所ポテンシャル場を計算するステップと、前回探索対象グリッドにおいて決定されたグリッド探索ベクトルが前記次探索グリッドに進入する際のエッジ上の点を探索枝の基点として、計算した前記局所ポテンシャル場のポテンシャル値が最急勾配となる方向であって、当該勾配を降る方向に当該探索枝を延ばし、当該延ばした探索枝の方向を、前記次探索グリッドにおけるグリッド探索ベクトルとして決定するステップと、前記グリッド探索ベクトルが決定されたグリッド系列を、前記移動始点と移動終点の間を結ぶ経路として出力するステップと、を有するものである。これにより、理想的な最短経路により近い経路を生成することができる。
【００１６】
本発明に係る第三の態様の移動体は、移動領域を、各グリッドを区切る周囲の枠をグリッドのエッジとする、複数の格子状のグリッドに分割し、移動始点と移動終点の間を結ぶグリッド系列を経路として探索し、当該経路に従って移動する移動体であって、障害物が占有し、移動体の進入が禁止される進入禁止グリッドが設定されたグリッドマップを記憶する地図情報記憶部と、前記移動始点からの距離に応じたポテンシャル値と、前記障害物からの距離に応じたポテンシャル値と、を加算して、前記グリッドマップの各グリッドの距離ポテンシャル値として生成する距離ポテンシャル生成部と、前回探索対象グリッドにおいて決定されたグリッド探索ベクトルが次に進入する隣接グリッドを、次に探索対象とする次探索グリッドとして決定すると共に、前記グリッド探索ベクトルの向きに応じて、前記次探索グリッドに隣接する第１及び第２のグリッドを、当該次探索グリッドについての局所ポテンシャル場計算用グリッドとして選択する探索グリッド決定部と、前記次探索グリッドの距離ポテンシャル値に対する前記局所ポテンシャル場計算用グリッドの距離ポテンシャル値の相対変化に基づく頂点を持つ局所ポテンシャル場であって、前記次探索グリッドについての局所ポテンシャル場を計算する局所ポテンシャル場計算部と、前回探索対象グリッドにおいて決定されたグリッド探索ベクトルが前記次探索グリッドに進入する際のエッジ上の点を探索枝の基点として、前記局所ポテンシャル場のポテンシャル値が最急勾配となる方向であって、当該勾配を降る方向に当該探索枝を延ばし、当該延ばした探索枝の方向を、前記次探索グリッドにおけるグリッド探索ベクトルとして決定するグリッド探索方向決定部と、を備えるものである。これにより、理想的な最短経路により近い経路を生成することができる。
【発明の効果】
【００１７】
本発明によれば、理想的な最短経路により近い経路を生成可能な経路探索システム、経路探索方法、及び移動体を提供することができる。
【図面の簡単な説明】
【００１８】
【図１】実施の形態１に係る移動体の構成を示す図である。
【図２】実施の形態１に係る移動体の制御部の構成図である。
【図３】実施の形態１に係るグリッドなどの用語を説明するための図である。
【図４】実施の形態１に係る探索グリッドの決定及び局所ポテンシャル場計算用グリッドの選択方法を説明するための図である。
【図５】実施の形態１に係る局所ポテンシャル場の計算方法を説明するための図である。
【図６】実施の形態１に係る円錐ポテンシャル場の導出を説明するための図である。
【図７】実施の形態１に係るグリッド探索ベクトルの決定方法を説明するための図である。
【図８】実施の形態１に係る経路探索処理のフロー図である。
【図９】実施の形態１に係る経路探索による効果を説明するための図である。
【図１０】本発明に関連する技術及び本発明による経路探索結果を説明するための図である。
【発明を実施するための形態】
【００１９】
実施の形態１．
以下、図面を参照して本発明の実施の形態について説明する。
図１を参照して、本実施の形態に係る移動体の一例であるロボットの構成を説明する。図１は、ロボット１００の構成を模式的に示す外観図である。本実施の形態では、ロボット１００が、自律移動する移動ロボットとして説明する。
【００２０】
ロボット１００は、車輪２と、筐体３と、センサ５と、を備えている。筐体３の内部には、車輪２と接続されたモータ、及びモータを駆動するためのバッテリなどが設けられている。このモータがロボット１００を駆動するための駆動機構となる。モータを駆動することによって、車輪２が回転して、ロボット１００が移動する。ロボット１００は、例えば、人間の歩行速度と同程度の速度で移動する。さらに、頭部１には、ＣＣＤカメラやレーザセンサなどを有するセンサ５が設けられている。センサ５はロボット１００に周囲に存在する障害物や人間などを検知する。
【００２１】
ロボット１００には、制御部１１０が設けられている。制御部１１０は、ＣＰＵ（Central Processing Unit）、ＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）、通信用のインタフェイスなどを有する演算処理装置である。また、制御部１１０は、着脱可能なＨＤＤ、光ディスク、光磁気ディスク等を有し、各種プログラムや制御パラメータなどを記憶し、そのプログラムやデータを必要に応じてメモリ（不図示）等に供給する。制御部１１０は、物理的に一つの構成に限られるものではない。
【００２２】
制御部１１０は、ロボット１００が移動する移動経路を生成する。そして、その移動経路に沿ってロボット１００が移動するよう、車輪２を駆動するためのモータ等を制御する。ロボット１００は車輪型の移動ロボットに限らず、歩行型やその他の移動ロボットでもよい。ロボット１００は自己位置推定を行なって移動する移動体であってもよい。
【００２３】
制御部１１０は、移動環境中にある移動始点から移動終点までの移動経路を探索する。ロボット１００は、移動始点から移動を開始する。そして、移動経路に沿って移動していき、移動終点まで移動したら停止する。すなわち、制御部１１０は、移動始点から移動終点までの移動経路を探索する経路探索システムとして機能する。
【００２４】
次に、図２を参照して、ロボット１００の制御系を説明する。図２は、ロボット１００の制御部１１０を示すブロック図である。制御部１１０は、ロボット１００が移動する経路を決定するための演算処理を実行する。
【００２５】
図２に示すように、制御部１１０は、地図情報記憶部１１１と、距離ポテンシャル生成部１１２と、探索グリッド決定部１１３と、局所ポテンシャル場計算部１１４と、グリッド探索ベクトル決定部１１５と、を備えている。
【００２６】
地図情報記憶部１１１は、ロボット１００が移動する移動領域中の地図情報を記憶している。例えば、移動する環境中に存在する壁、机などの障害物の座標を記憶している。そして、地図情報記憶部１１１は、移動領域を２次元のグリッドによって表現している。
【００２７】
また、地図情報記憶部１１１は、移動領域を縦横のグリッド線で分割して、格子状のグリッドによって表している。さらに、地図情報には、障害物などの情報に基づいて、進入禁止グリッドが設定されている。障害物が占有する位置には、ロボット１００が移動することができない。このため、その障害物の位置に対応するグリッドは、進入禁止グリッドとなる。
【００２８】
従って、地図情報記憶部１１１には、進入禁止グリッドと進入可能グリッドとから構成されるグリッドマップが記憶される。グリッドサイズは、移動領域の大きさやコンピュータの処理速度に応じて決定される。経路探索では、ロボット１００の移動始点に対応するグリッドから移動終点に対応するグリッドまでのグリッド系列（最適経路）が探索される。
【００２９】
図３（Ａ）に示すように、グリッドは、縦横に区切られた、各格子の領域を示す。図に示す例では、４つのグリッドＡ、Ｂ、Ｃ、Ｄが示されている。グリッドのエッジは、グリッドの周囲の枠を示す。図に示す例では、例えばグリッドＡのエッジは、４つのエッジａ１、ａ２、ａ３、ａ４となる。また、ある任意の１つのグリッドは、上下左右の４つのグリッドと隣接する。すなわち、移動領域の端以外のグリッドでは、上方向、下方向、左方向、右方向の４近傍のグリッドが隣接していることになる。
【００３０】
また、図３（Ｂ）に示すように、ある１つのグリッドに関して、そのエッジ上の１つの点と、他の異なるエッジ上の１つの点と、を結ぶ直線をエッジ間の探索枝とする。探索枝が延びている方向を示すベクトルを、グリッド探索ベクトルとする。すなわち、グリッド探索ベクトルは、探索枝の方向（傾き）を示す。図に示す例では、点Ｑｊと点Ｑｊ＋１とを結ぶ直線がエッジ間の探索枝となり、この探索枝の方向がグリッド探索ベクトルとなる。グリッド探索ベクトルが隣接グリッドのエッジ上に進入した場合に、この進入点（グリッド探索ベクトルとエッジとの交点）は、エッジを介して隣接するグリッドにおける、探索枝の始点となる。経路探索の過程では、エッジ上における探索枝の端点の座標と、その探索枝の角度（グリッド探索ベクトル）とが、情報として保持される。なお、探索枝の角度は、これら端点の座標値から求めることができる。
【００３１】
距離ポテンシャル生成部１１２は、グリッドマップ内のグリッドに対して、移動始点からの距離と、障害物からの距離と、に基づいて、距離ポテンシャルを生成する。まず、距離ポテンシャル生成部１１２は、グリッドマップ内の各グリッドに対して、移動始点からの距離に応じたポテンシャル値を設定する。ここでは、移動始点のグリッドには値を０として設定し、移動始点から離れるほど大きくなるポテンシャル値を算出し、これらポテンシャル値を各グリッドに設定する。例えば、隣接グリッドへ移動するために要するコストとして、グリッド間距離などの適当な値を定める。そして、ダイクストラ法やＡ＊（スター）アルゴリズムなどの公知の探索アルゴリズムを用いて、移動始点から目標グリッドまでの最短経路を探索する。そして、その探索した経路上のグリッドについてのコストの総和を求め（すなわち、移動始点から目標グリッドまでの経路長に応じた値）、その総和値を、目標グリッドの距離ポテンシャル値として設定する。
【００３２】
また、距離ポテンシャル生成部１１２は、障害物が存在する場合には、その障害物からの距離に応じたポテンシャル値を算出し、移動始点からの距離に応じて設定したポテンシャル値に加算する。例えば、障害物グリッドからの距離に応じて単調に減少するポテンシャル関数を考え、進入可能グリッドの各グリッドに対してポテンシャル関数で計算される値の総和をその位置のポテンシャル値とするポテンシャル場を生成する。ただし、障害物グリッド自体には、ある一定のポテンシャル値（障害物でないグリッドに設定されるポテンシャル値と比較して十分大きな正の値であって、無限大と見なして処理する。）を設定する。なお、距離ポテンシャル生成部１１２による障害物からの距離に応じて設定する距離ポテンシャル値の算出手法については、従来公知のポテンシャル法を用いて行えばよく、ここでは、その詳細な説明を省略する。
【００３３】
探索グリッド決定部１１３は、前回探索対象となったグリッドについて決定したグリッド探索ベクトルから、次に探索するグリッドを決定し、また、その決定したグリッドについての局所ポテンシャル場の計算に利用するグリッドを選択する。より具体的には、前回決定したグリッド探索ベクトルがエッジを交差して進入する隣接グリッドを、次に探索対象とするグリッドとして決定する（以下、次探索グリッドと称する場合がある。）。そして、その決定した次探索グリッドの隣接グリッドのうちで、前記グリッド探索ベクトルの延長上にある複数のグリッドを、局所ポテンシャル場の計算用グリッドとして選択する（以下、局所ポテンシャル場計算用グリッドと称する場合がある。）。なお、局所ポテンシャル場の計算用グリッドは、次探索グリッドの前後左右に隣接するグリッドから選択される。
【００３４】
図４は、探索グリッド決定部１１３による探索グリッドの決定及び局所ポテンシャル場計算用グリッドの選択方法を説明するための図である。図に示す例では、３×３のグリッドが示されている。ここで、説明の簡略化のため、各グリッドを次のように座標で識別して、説明する。例えば、左上隅のノードを（１，１）のグリッドとして、右下隅のノードを（３，３）とする。また、（１，１）のグリッドの１つ下のノードを（２，１）のグリッドとし、（１，１）のグリッドの１つ下のグリッドを（１，２）とする。このように座標を用いて、各グリッドを識別する。なお、図４に示す例では、紙面右方向をｘ軸正方向とし、上方向をｙ軸正方向とするグローバル座標系での処理を示す。このため、ここでは、グローバル座標系での、探索枝の端点の座標と、その探索枝の角度とが、ＲＡＭなどの一時記憶部に保持されている。
【００３５】
図４（Ａ）に示す例では、まず、グリッド（２，１）内の矢印により示す探索方向（グリッド探索ベクトル）から、次探索グリッドとしてグリッド（２，２）を決定する。そして、グリッド探索ベクトルのｘ、ｙ成分に注目し、ｘ＞０、ｙ＞０であることから、グリッド（１，２）、グリッド（２，３）を局所ポテンシャル場計算用グリッドとして選択する。また、図４（Ｂ）に示す例では、次探索グリッドとしてグリッド（２，２）を決定し、ｘ＞０、ｙ＜０であることから、グリッド（２，３）、グリッド（３，２）を局所ポテンシャル場計算用グリッドとして選択する。このように、グリッド探索ベクトルの成分（矢印の入射角度）に応じて、次探索グリッドと、局所ポテンシャル場計算用グリッドと、が選択される。
【００３６】
なお、グリッド探索ベクトルのｘ、ｙ成分について、ｘ＝０、ｙ＝０である場合には、図４（Ａ）に示した場合と同じようにして２つの局所ポテンシャル場計算用グリッドを選択するものとしてもよいし、或いは、図４（Ｂ）に示した場合と同じようにして２つの局所ポテンシャル場計算用グリッドを選択するものとしてもよい。図４に示す例では、例えば、グリッド（２，２）と、グリッド（２，３）又はグリッド（３，２）のいずれか１つのグリッドと、を選択すればよい。なお、グリッド（２，３）又はグリッド（３，２）のいずれか１つの障害物グリッドある場合には、障害物グリッドでない他のグリッドを選択するようにしてもよい。
【００３７】
詳細は後述するが、グリッド（２，１）について計算する局所ポテンシャル場を用いて、グリッド（２，１）内のグリッド探索ベクトルが決定される。決定したグリッド探索ベクトルから次探索グリッドが決定される。
【００３８】
局所ポテンシャル場計算部１１４は、次探索グリッドについての局所ポテンシャル場を、次探索グリッドの距離ポテンシャル値に対する局所ポテンシャル場計算用グリッドの距離ポテンシャル値の相対変化に基づいて計算する。ここでは、局所ポテンシャル場として、円錐ポテンシャル場を計算する。また、円錐ポテンシャル場の頂点を、次探索グリッドのポテンシャル値の二乗と、局所ポテンシャル場計算用グリッドのポテンシャル値の二乗との差に応じて定める。
【００３９】
図５は、局所ポテンシャル場の計算方法を説明するための図である。図に示す例では、局所ポテンシャル場としての円錐ポテンシャル場を計算するための、３つのグリッドＡ、Ｂ、Ｃが示されている。なお、図５では、次探索グリッドをグリッドＡ、局所ポテンシャル場計算用グリッドをグリッドＢ及びグリッドＣとして、これら３つのグリッドのエッジが互いに交差する点を、座標系の原点と定める。すなわち、局所ポテンシャル場の計算においては、決定される次探索グリッドと、選択される局所ポテンシャル場計算用グリッドと、から定まるローカル座標系での処理を行う。
【００４０】
図５に示す例では、グリッドＡのエッジａ１上の点Ｑ１（ｘｑ，ｙｑ）に対して、その傾きが負である探索枝（実線の矢印で示す。）が進入したものとする（すなわち、グリッド探索ベクトルの成分について、グローバル座標系でのｘ、ｙについてｄｙ／ｄｘ＜０。）。このとき、上述したようにして、グリッドＡが次探索グリッドとなり、局所ポテンシャル場計算用グリッドとしてグリッドＢ、Ｃが選択される。局所ポテンシャル場計算部１１４は、グリッドＡのポテンシャル値ＰＡと、グリッドＢ、Ｃのポテンシャル値ＰＢ、ＰＣを利用して、グリッドＡについての局所ポテンシャル場ＰＮを計算する。
【００４１】
局所ポテンシャル場ＰＮは、以下の数（１）を用いて表される。
【数１】
【００４２】
数（１）により示される局所ポテンシャル場は、その頂点Ｏｃが以下の数（２）で示される、円錐ポテンシャル場である。また、数（１）において、Δｄはグリッドサイズ（エッジの大きさ）を示している。
【数２】
【００４３】
なお、局所ポテンシャル場計算用グリッドとして選択されたグリッドＢ、Ｃについて、そのいずれか１のグリッドが障害物グリッドである場合には、円錐ポテンシャル場を算出するのに代えて、平面ポテンシャル場を計算するものとする。例えば、局所ポテンシャル場計算用グリッドとして選択されたグリッドＢが障害物グリッドである場合には、上記の数（１）で示される局所ポテンシャル場に代えて、グリッドＡのポテンシャル値ＰＡと、グリッドＣのポテンシャル値ＰＣとを直線補間し、その直線補間された値からなる平面ポテンシャル場を、局所ポテンシャル場とする。また、グリッドＢ、Ｃがともに障害物グリッドである場合には、経路探索による解は無いと判定して、探索を打ち切るようにしてもよい。
【００４４】
ここで、図６を参照して、円錐ポテンシャル場の導出について補足する。
図に示す例では、円錐ポテンシャル場を計算するための、３つのグリッドＡ、Ｂ、Ｃが示されている。なお、グリッド探索ベクトル（不図示）が、グリッドＡのエッジ上に進入し、その進入点がＱ１であるものとする。また、ローカル座標系の原点を、Ｏとする。
【００４５】
まず、Ｏを原点とするローカル座標系でのグリッドＡ、Ｂ、Ｃの各位置Ａ（ｘ，ｙ）、Ｂ（ｘ，ｙ）、Ｃ（ｘ，ｙ）を、それぞれ以下の数（３）、（４）、（５）により示す。
【数３】
【数４】
【数５】
【００４６】
次に、円錐ポテンシャルの頂点Ｏｃを中心として、各グリッドのポテンシャル値に関して以下の数（６）、（７）、（８）を定義する。なお、以下では、頂点Ｏｃのｘ、ｙ座標値を単にＯｃｘ、Ｏｃｙとして表記し、グリッドＡの位置のｘ、ｙ座標値を単にＡｘ、Ａｙとして表記して説明する。
【数６】
【数７】
【数８】
【００４７】
そして、数（６）−数（７）を計算して、以下の数（９）を得る。
【数９】
また、数（６）−数（８）を計算して、以下の数（１０）を得る。
【数１０】
従って、数（９）及び数（１０）から、以下の数（１１）に示すように、円錐ポテンシャルの頂点Ｏｃを求める。
【数１１】
従って、数（１１）の頂点Ｏｃから、以下の数（１２）で示す円錐ポテンシャルＰが定まる。
【数１２】
【００４８】
図２に戻って説明を続ける。
グリッド探索ベクトル決定部１１５は、前回決定したグリッド探索ベクトルが進入するエッジ上の点を探索枝の基点として、計算した局所ポテンシャル場のポテンシャル値が最急勾配となる方向（ポテンシャル値の変化が最も急となる方向）であって、その勾配を降る方向（ポテンシャル値が小さくなる方向）に、探索枝を延ばす。グリッド探索ベクトル決定部１１５は、延ばした探索枝の方向を、グリッド探索ベクトルとして決定する。この決定したグリッド探索ベクトルはＲＡＭなどの一時記憶部に保持され、次に探索対象とするグリッドを決定する際の情報として利用される。
【００４９】
図７は、グリッド探索ベクトルの決定方法を説明するための図である。図では、現在の探索対象グリッドをグリッドＡとし、局所ポテンシャル場としての円錐ポテンシャルが計算されている。なお、円錐ポテンシャル場は、グリッドＡ、Ｂ、Ｃの各ポテンシャル値に基づいて計算され、Ｏｃは円錐ポテンシャル場の頂点を示す。
【００５０】
図７において、前回決定したグリッド探索ベクトルがグリッドＡのエッジａ１に進入し、その進入点を点Ｑ１とする。グリッドＡにおいて、点Ｑ１を探索枝の基点として、局所ポテンシャル場における最急勾配方向で、降る方向へと探索枝を延ばす。詳細は後述するが、点Ｑ１から延ばした探索枝がグリッドＡのエッジａ３に到達した場合に、このエッジａ３上の交点を点Ｑ２として、点Ｑ１から点Ｑ２へと向かう探索枝の方向が、局所ポテンシャル場における最急勾配方向であって、かつ、降る方向となる。
【００５１】
円錐ポテンシャル場において、最急勾配方向に降る方向は、エッジ上の基点から円錐の頂点へと向かう直線の方向に相当する（この直線は、円錐の円周に対して直角に交わる。）。すなわち、探索枝は、エッジ上の基点から円錐ポテンシャル場の頂点へと向かう方向へと延びる。探索枝の端点Ｑ２は、基点Ｑ１から円錐ポテンシャル場の頂点へと向かう直線上であって、かつ、エッジａ３上に位置する点となる。点Ｑ２の座標は、例えば、以下の方法により求めることができる。
【００５２】
まず、円錐ポテンシャル場の頂点Ｏｃは、以下の数（１３）により与えられる。
【数１３】
【００５３】
以下の数（１４）は、円錐ポテンシャル場の頂点の座標と、点Ｑ１の座標と、点Ｑ２の座標と、の関係を示す直線方程式である。円錐ポテンシャル場の頂点と点Ｑ１を結ぶ直線と、点Ｑ１と点Ｑ２を結ぶ直線と、は互いに平行となる。このため、この直線方程式を利用して、点Ｑ２の座標（Ｑ２ｘ，Ｑ２ｙ）を求めることができる。
【数１４】
【００５４】
また、点Ｑ１と点Ｑ２は、それぞれエッジ上に位置する点であるため、例えば、Ｑ２ｘ−Ｑ１ｘ＝Δｄという関係を利用して、以下の数（１５）に示すようにして、点Ｑ２の座標を求めることができる。
【数１５】
【００５５】
従って、図７に示す例では、エッジａ１上の基点Ｑ１の座標と、円錐ポテンシャル場の頂点の座標Ｏｃとから、エッジａ２上の点Ｑ２の座標を求める。そして、エッジａ１上の基点Ｑ１とエッジａ２上の点Ｑ２とを結ぶ直線をエッジ間の探索枝として、この探索枝の方向を、グリッド探索ベクトルとして決定することができる。
【００５６】
なお、図７に示した例では、グリッドＡは、他のグリッドと比べてより移動終点に近いグリッドであり、グリッドＢは他のグリッドと比べてより壁に近いグリッドであり、グリッドＣは、他のグリッドと比べてより移動始点に近いグリッドであり、各グリッドのポテンシャル値について、ＰＣ＜ＰＢ＜ＰＡが成立するような状況を示している。このような状況を反映した円錐ポテンシャル場が、図７に例示されている。すなわち、円錐ポテンシャル場の頂点Ｏｃは、探索枝の基点Ｑ１から見て、より移動始点へと近づく方向であって、かつ、障害物から離れる方向に位置するものとして算出されている。従って、グリッドＡへと進入したグリッド探索ベクトルは、計算した局所ポテンシャル場によって、その方向が障害物から遠ざかる方向へと曲げられる。
【００５７】
次に、図８を参照して、経路探索処理のフローについて説明する。
まず、制御部１１０は、グリッド空間において、移動始点のグリッドと、移動終点のグリッドを設定する（Ｓ１０１）。
【００５８】
次に、距離ポテンシャル生成部１１２は、グリッドマップ内の各グリッドに対して、移動終点や障害物からの距離に応じたポテンシャル値を設定し、距離ポテンシャルを生成する（Ｓ１０２）。
【００５９】
次に、探索グリッド決定部１１３は、前回決定したグリッド探索ベクトルから、次に探索するグリッドを決定し、また、その決定したグリッドについての局所ポテンシャル場の計算に利用するグリッドを選択する（Ｓ１０３）。
【００６０】
次に、局所ポテンシャル場計算部１１４は、決定した次探索グリッドについて、次探索グリッドのポテンシャル値と、選択した局所ポテンシャル場計算用グリッドの各ポテンシャル値と、に基づいて、局所ポテンシャル場を計算する（Ｓ１０４）。
【００６１】
次に、グリッド探索ベクトル決定部１１５は、局所ポテンシャル場を計算した次探索グリッドにおいて、前回決定したグリッド探索ベクトルがエッジに進入する点を探索枝の基点として、ポテンシャル値が最急勾配方向で降る方向に探索枝を延ばし、その延ばした探索枝の方向を、次探索グリッドの次に探索対象とするグリッドを決定するための、新たなグリッド探索ベクトルとして決定する（Ｓ１０５）。
【００６２】
制御部１１０は、グリッド探索ベクトルが示す次探索グリッドが移動始点のグリッドであるか否かを判定し、移動始点のグリッドに到達したか否かを判定する（Ｓ１０６）。判定の結果、到達していない場合には、ステップＳ１０３〜Ｓ１０５の処理を繰り返して、順次、グリッドを探索していく。一方で、判定の結果、到達した場合には、制御部１１０は、移動始点へと到達するまでに探索したグリッドの系列を出力する。すなわち、このグリッド系列を、移動終点と移動始点との間の経路として生成する。
【００６３】
以上説明したように本実施の形態によれば、従来のポテンシャル法によるグリッド探索のように、隣接グリッドのうちで最小のポテンシャル値を有するグリッドを単純に選択するものではなく、次に探索対象となるグリッドにおいてエッジ間の探索枝を求め、その探索枝の方向をグリッド探索ベクトルとした上で、そのグリッド探索ベクトルから次の探索対象グリッドを順次決定していくものである。ここで、グリッド探索ベクトルの決定に際しては、隣接グリッドのポテンシャル値をも考慮した局所ポテンシャル場を計算し、距離ポテンシャル値からより詳細なポテンシャル場を局所的に計算している。そして、局所ポテンシャル場において、ポテンシャル値が最急勾配方向で降る方向に、探索枝を延ばすことを特徴とするものである。
【００６４】
図９は、本実施の形態による効果をより具体的に説明するための図である。
図９（Ａ）は、グリッド間の距離を利用して経路長が最短となる経路を求める、従来手法による経路探索結果を示している。求められたグリッドの系列は、直線により示す最適な経路とは大きく異なるものである。
これに対して図９（Ｂ）は、本実施の形態による経路探索結果を示している。求められたグリッドの系列は、直線により示す最適な経路に、より近い経路となる。
【００６５】
従来のポテンシャル法を用いた経路探索手法では、単に、隣接グリッドのうちで最小のポテンシャルを有するグリッドの選択を繰り返していき、移動終点のグリッドに到達した場合には、探索を終了するものである。
これに対して本実施の形態では、基本的に全探索を行う経路探索手法（ダイクストラ法など）を用いて距離ポテンシャルを生成しているためにローカルミニマムを回避することができ、かつ、理想的な最短経路により近い経路を生成することができる。
【００６６】
また、特許文献１に示されるような従来のポテンシャル法を用いた経路生成技術では、ローカルミニマムを回避することを目的として、予め設定したポテンシャル場自体を合成ポテンシャルで補正するものである。
これに対して本実施の形態では、予め生成したポテンシャル場自体を補正するものではなく、経路探索の過程において、各探索グリッドについて、その詳細な局所ポテンシャル場を求めるものである。すなわち、予め生成したポテンシャル値を利用して、局所的に詳細な解を求めながら経路探索を行うことを特徴とする。このように局所的に詳細なポテンシャル値を求めることで、より最適解に近い経路を探索可能とする。
【００６７】
上述した実施の形態では、移動始点から移動終点に至る移動領域内に障害物が存在する場合における経路探索の例を説明したが、本発明はこれに限定されず、障害物の有無に依存せず、より理想的な経路を生成することができる。
【００６８】
また、上述した実施の形態では、移動終点のグリッドから移動始点のグリッドへと向けて探索を行う場合を例に説明したが、移動始点のグリッドから移動終点のグリッドへと向けて探索を行うようにしてもよく、任意の２地点間を結ぶ経路探索技術に適用可能である。
【００６９】
また、移動始点や障害物からの距離に応じたポテンシャル値を負の値として設定した場合には、局所ポテンシャル場において、ポテンシャル値が最急勾配方向で登る方向に探索枝を延ばし、その延ばした探索の方向をグリッド探索ベクトルとして決定すればよい。
【００７０】
なお、上述した処理によって求めた経路に対して、さらに補間処理を行うものとしてもよい。本発明により生成される経路は、より最短経路に近いものであるため、このような経路をもとに補間処理を行うことで、より最適かつ滑らかな経路を生成することができる。
【００７１】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
【符号の説明】
【００７２】
２ 車輪、
３ 筐体、
５ センサ、
１００ ロボット、
１１０ 制御部、
１１１ 地図情報記憶部、
１１２ 距離ポテンシャル生成部、
１１３ 探索グリッド決定部、
１１４ 局所ポテンシャル場計算部、
１１５ グリッド探索ベクトル決定部、
【技術分野】
【０００１】
本発明は、経路探索システム、経路探索方法、及び移動体に関する。
【背景技術】
【０００２】
グリッド空間において、最短経路探索を行う技術が提案されている。このような経路探索技術が、特許文献１乃至５に開示されている。例えば特許文献１に開示された移動体の経路生成装置では、移動体や障害物の情報を用いて移動領域内のグリッドマップを生成し、それぞれの相対位置関係から引力ポテンシャル、斥力ポテンシャルを計算する。そして、これらポテンシャルの合成ポテンシャルに基づきマップ内での経路探索を行い、ローカルミニマムを判定して経路を生成する。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００６−３５０７７６号公報
【特許文献２】特開平５−２７４０３７号公報
【特許文献３】特開２００３−０２９８３３号公報
【特許文献４】特開２００５−０３２１９６号公報
【特許文献５】特開２００９−０２５９７４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、グリッド空間において経路探索を行う技術では、理想的な最短経路とは大きく異なる経路が最短経路として生成されることがあるという問題があった。
【０００５】
この問題点を説明するため、図１０（Ａ）を参照して、グリッド空間における一般的な経路探索手法について説明する。図において、移動始点のグリッドＳから、移動終点のグリッドＧへと向けて、経路を探索する。なお、図において左下方向への斜線により示すグリッドは、障害物などにより移動が禁止されるグリッドを示す。
【０００６】
ここでの経路探索では、前後左右のグリッド及び斜め方向に位置する周囲のグリッドを次に選択可能なグリッドとする。そして、現在のグリッドから周囲の各グリッドまでのグリッド間距離に基づいて、移動始点から移動終点へと至る経路の経路長が最短となるように、当該経路を構成するグリッドを順次決定していく。このため、図１０（Ａ）の例では、前後左右のグリッド間の距離と斜め方向に位置するグリッド間の距離をそれぞれ例えば５及び７とした場合、グリッドＳから探索を開始して、白抜き矢印により示す方向のグリッドが、経路を構成するグリッドとして順次決定されていく。この際、探索される経路の経路長が、グリッド間距離の総和として表現される。
【０００７】
これに対して、グリッドＳからグリッドＧへと至る最適な経路の例を図１０（Ｃ）に示す。すなわち、グリッドＳからグリッドＧへと至る理想的な経路は、図１０（Ｃ）に示すような直線になるが、このような最適な経路に対して図１０（Ａ）に示した経路は大きく異なったものになる。
【０００８】
これは、経路探索の計算過程で、前後左右及び斜めに位置するグリッドに関して、グリッド間の距離の総和として経路長を表現しているために、経路長の距離分解能がグリッド間距離まで粗くなってしまい、その結果、理想的な経路とは大きく異なる経路が生成されてしまうためである。換言すると、グリッド単位で離散化しているために、経路長の計算数値がグリッドごとに丸め込まれてしまい、グリッド空間の離散誤差が蓄積することで、局所的には最適であるが、全体最適とはならない経路が生成されてしまうためである。
【０００９】
従って、本発明は、上述した課題を解決して、理想的な最短経路により近い経路を生成可能な経路探索システム、経路探索方法、及び移動体を提供することを目的とする。
【課題を解決するための手段】
【００１０】
本発明に係る第一の態様の経路探索システムは、移動領域を、各グリッドを区切る周囲の枠をグリッドのエッジとする、複数の格子状のグリッドに分割し、移動始点と移動終点の間を結ぶグリッド系列を探索する経路探索システムであって、障害物が占有し、移動体の進入が禁止される進入禁止グリッドが設定されたグリッドマップを記憶する地図情報記憶部と、前記移動始点からの距離に応じたポテンシャル値と、前記障害物からの距離に応じたポテンシャル値と、を加算して、前記グリッドマップの各グリッドの距離ポテンシャル値として生成する距離ポテンシャル生成部と、前回探索対象グリッドにおいて決定されたグリッド探索ベクトルが次に進入する隣接グリッドを、次に探索対象とする次探索グリッドとして決定すると共に、前記グリッド探索ベクトルの向きに応じて、前記次探索グリッドに隣接する第１及び第２のグリッドを、当該次探索グリッドについての局所ポテンシャル場計算用グリッドとして選択する探索グリッド決定部と、前記次探索グリッドの距離ポテンシャル値に対する前記局所ポテンシャル場計算用グリッドの距離ポテンシャル値の相対変化に基づく頂点を持つ局所ポテンシャル場であって、前記次探索グリッドについての局所ポテンシャル場を計算する局所ポテンシャル場計算部と、前回探索対象グリッドにおいて決定されたグリッド探索ベクトルが前記次探索グリッドに進入する際のエッジ上の点を探索枝の基点として、前記局所ポテンシャル場のポテンシャル値が最急勾配となる方向であって、当該勾配を降る方向に当該探索枝を延ばし、当該延ばした探索枝の方向を、前記次探索グリッドにおけるグリッド探索ベクトルとして決定するグリッド探索方向決定部と、を備えるものである。
【００１１】
このように次探索グリッドについて、局所ポテンシャル場を計算し、計算した局所ポテンシャル場での勾配に応じて次探索グリッドのエッジ間の探索枝を求め、その探索枝の方向をグリッド探索ベクトルとして決定し、そのグリッド探索ベクトルから、さらに続く次の探索対象グリッドを探索していくことで、従来手法と比較して次探索グリッドの決定をより詳細に行うことができるため、この結果、理想的な最短経路により近い経路を生成することができる。例えば、図１０に示した例では、従来技術による探索結果が図１０（Ａ）に示されるのに対して、本発明にかかる経路探索システムによる探索結果は図１０（Ｂ）に示される。
【００１２】
また、前記局所ポテンシャル場計算部は、前記局所ポテンシャル場として、前記次探索グリッドの距離ポテンシャル値の二乗と、前記局所ポテンシャル場計算用グリッドの距離ポテンシャル値の二乗と、の差に応じて定まる頂点を持つ、円錐ポテンシャル場を計算するものとしてもよい。
【００１３】
さらにまた、前記グリッド探索方向決定部は、前記探索枝の前記基点から前記局所ポテンシャル場の前記頂点へと向かう直線上の点であって、前記基点が位置するエッジとは異なる前記次探索グリッドの他のエッジ上に位置する点を、前記探索枝の端点として求め、前記探索枝の前記基点から前記端点へと向かう方向を、前記次探索グリッドにおけるグリッド探索ベクトルとして決定するものとしてもよい。
【００１４】
また、前記距離ポテンシャル生成部は、隣接グリッド間の移動コストとして所定の値を定め、前記移動始点から目標グリッドへと至るグリッド系列を探索して、探索した当該グリッド系列についての前記移動コストの総和を求め、当該総和値と、前記障害物からの距離に応じたポテンシャル値と、を加算して、前記目標グリッドの距離ポテンシャル値を算出するものとしてもよい。
【００１５】
本発明に係る第二の態様の経路探索方法は、移動領域内の移動始点から移動終点へと移動する移動体の経路探索方法であって、前記移動領域を、各グリッドを区切る周囲の枠をグリッドのエッジとする、複数の格子状のグリッドに分割したグリッドマップを用いて表して、障害物が占有し、前記移動体の進入が禁止される進入禁止グリッドが設定されたグリッドマップを記憶するステップと、前記移動始点からの距離に応じたポテンシャル値と、前記障害物からの距離に応じたポテンシャル値と、を加算して、前記グリッドマップの各グリッドの距離ポテンシャル値として生成するステップと、前回探索対象グリッドにおいて決定されたグリッド探索ベクトルが次に進入する隣接グリッドを、次に探索対象とする次探索グリッドとして決定すると共に、前記グリッド探索ベクトルの向きに応じて、前記次探索グリッドに隣接する第１及び第２のグリッドを、当該次探索グリッドについての局所ポテンシャル場計算用グリッドとして選択するステップと、決定した前記次探索グリッドと、選択した前記局所ポテンシャル場計算用グリッドについて、前記次探索グリッドの距離ポテンシャル値に対する前記局所ポテンシャル場計算用グリッドの距離ポテンシャル値の相対変化に基づく頂点を持つ局所ポテンシャル場であって、前記次探索グリッドについての局所ポテンシャル場を計算するステップと、前回探索対象グリッドにおいて決定されたグリッド探索ベクトルが前記次探索グリッドに進入する際のエッジ上の点を探索枝の基点として、計算した前記局所ポテンシャル場のポテンシャル値が最急勾配となる方向であって、当該勾配を降る方向に当該探索枝を延ばし、当該延ばした探索枝の方向を、前記次探索グリッドにおけるグリッド探索ベクトルとして決定するステップと、前記グリッド探索ベクトルが決定されたグリッド系列を、前記移動始点と移動終点の間を結ぶ経路として出力するステップと、を有するものである。これにより、理想的な最短経路により近い経路を生成することができる。
【００１６】
本発明に係る第三の態様の移動体は、移動領域を、各グリッドを区切る周囲の枠をグリッドのエッジとする、複数の格子状のグリッドに分割し、移動始点と移動終点の間を結ぶグリッド系列を経路として探索し、当該経路に従って移動する移動体であって、障害物が占有し、移動体の進入が禁止される進入禁止グリッドが設定されたグリッドマップを記憶する地図情報記憶部と、前記移動始点からの距離に応じたポテンシャル値と、前記障害物からの距離に応じたポテンシャル値と、を加算して、前記グリッドマップの各グリッドの距離ポテンシャル値として生成する距離ポテンシャル生成部と、前回探索対象グリッドにおいて決定されたグリッド探索ベクトルが次に進入する隣接グリッドを、次に探索対象とする次探索グリッドとして決定すると共に、前記グリッド探索ベクトルの向きに応じて、前記次探索グリッドに隣接する第１及び第２のグリッドを、当該次探索グリッドについての局所ポテンシャル場計算用グリッドとして選択する探索グリッド決定部と、前記次探索グリッドの距離ポテンシャル値に対する前記局所ポテンシャル場計算用グリッドの距離ポテンシャル値の相対変化に基づく頂点を持つ局所ポテンシャル場であって、前記次探索グリッドについての局所ポテンシャル場を計算する局所ポテンシャル場計算部と、前回探索対象グリッドにおいて決定されたグリッド探索ベクトルが前記次探索グリッドに進入する際のエッジ上の点を探索枝の基点として、前記局所ポテンシャル場のポテンシャル値が最急勾配となる方向であって、当該勾配を降る方向に当該探索枝を延ばし、当該延ばした探索枝の方向を、前記次探索グリッドにおけるグリッド探索ベクトルとして決定するグリッド探索方向決定部と、を備えるものである。これにより、理想的な最短経路により近い経路を生成することができる。
【発明の効果】
【００１７】
本発明によれば、理想的な最短経路により近い経路を生成可能な経路探索システム、経路探索方法、及び移動体を提供することができる。
【図面の簡単な説明】
【００１８】
【図１】実施の形態１に係る移動体の構成を示す図である。
【図２】実施の形態１に係る移動体の制御部の構成図である。
【図３】実施の形態１に係るグリッドなどの用語を説明するための図である。
【図４】実施の形態１に係る探索グリッドの決定及び局所ポテンシャル場計算用グリッドの選択方法を説明するための図である。
【図５】実施の形態１に係る局所ポテンシャル場の計算方法を説明するための図である。
【図６】実施の形態１に係る円錐ポテンシャル場の導出を説明するための図である。
【図７】実施の形態１に係るグリッド探索ベクトルの決定方法を説明するための図である。
【図８】実施の形態１に係る経路探索処理のフロー図である。
【図９】実施の形態１に係る経路探索による効果を説明するための図である。
【図１０】本発明に関連する技術及び本発明による経路探索結果を説明するための図である。
【発明を実施するための形態】
【００１９】
実施の形態１．
以下、図面を参照して本発明の実施の形態について説明する。
図１を参照して、本実施の形態に係る移動体の一例であるロボットの構成を説明する。図１は、ロボット１００の構成を模式的に示す外観図である。本実施の形態では、ロボット１００が、自律移動する移動ロボットとして説明する。
【００２０】
ロボット１００は、車輪２と、筐体３と、センサ５と、を備えている。筐体３の内部には、車輪２と接続されたモータ、及びモータを駆動するためのバッテリなどが設けられている。このモータがロボット１００を駆動するための駆動機構となる。モータを駆動することによって、車輪２が回転して、ロボット１００が移動する。ロボット１００は、例えば、人間の歩行速度と同程度の速度で移動する。さらに、頭部１には、ＣＣＤカメラやレーザセンサなどを有するセンサ５が設けられている。センサ５はロボット１００に周囲に存在する障害物や人間などを検知する。
【００２１】
ロボット１００には、制御部１１０が設けられている。制御部１１０は、ＣＰＵ（Central Processing Unit）、ＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）、通信用のインタフェイスなどを有する演算処理装置である。また、制御部１１０は、着脱可能なＨＤＤ、光ディスク、光磁気ディスク等を有し、各種プログラムや制御パラメータなどを記憶し、そのプログラムやデータを必要に応じてメモリ（不図示）等に供給する。制御部１１０は、物理的に一つの構成に限られるものではない。
【００２２】
制御部１１０は、ロボット１００が移動する移動経路を生成する。そして、その移動経路に沿ってロボット１００が移動するよう、車輪２を駆動するためのモータ等を制御する。ロボット１００は車輪型の移動ロボットに限らず、歩行型やその他の移動ロボットでもよい。ロボット１００は自己位置推定を行なって移動する移動体であってもよい。
【００２３】
制御部１１０は、移動環境中にある移動始点から移動終点までの移動経路を探索する。ロボット１００は、移動始点から移動を開始する。そして、移動経路に沿って移動していき、移動終点まで移動したら停止する。すなわち、制御部１１０は、移動始点から移動終点までの移動経路を探索する経路探索システムとして機能する。
【００２４】
次に、図２を参照して、ロボット１００の制御系を説明する。図２は、ロボット１００の制御部１１０を示すブロック図である。制御部１１０は、ロボット１００が移動する経路を決定するための演算処理を実行する。
【００２５】
図２に示すように、制御部１１０は、地図情報記憶部１１１と、距離ポテンシャル生成部１１２と、探索グリッド決定部１１３と、局所ポテンシャル場計算部１１４と、グリッド探索ベクトル決定部１１５と、を備えている。
【００２６】
地図情報記憶部１１１は、ロボット１００が移動する移動領域中の地図情報を記憶している。例えば、移動する環境中に存在する壁、机などの障害物の座標を記憶している。そして、地図情報記憶部１１１は、移動領域を２次元のグリッドによって表現している。
【００２７】
また、地図情報記憶部１１１は、移動領域を縦横のグリッド線で分割して、格子状のグリッドによって表している。さらに、地図情報には、障害物などの情報に基づいて、進入禁止グリッドが設定されている。障害物が占有する位置には、ロボット１００が移動することができない。このため、その障害物の位置に対応するグリッドは、進入禁止グリッドとなる。
【００２８】
従って、地図情報記憶部１１１には、進入禁止グリッドと進入可能グリッドとから構成されるグリッドマップが記憶される。グリッドサイズは、移動領域の大きさやコンピュータの処理速度に応じて決定される。経路探索では、ロボット１００の移動始点に対応するグリッドから移動終点に対応するグリッドまでのグリッド系列（最適経路）が探索される。
【００２９】
図３（Ａ）に示すように、グリッドは、縦横に区切られた、各格子の領域を示す。図に示す例では、４つのグリッドＡ、Ｂ、Ｃ、Ｄが示されている。グリッドのエッジは、グリッドの周囲の枠を示す。図に示す例では、例えばグリッドＡのエッジは、４つのエッジａ１、ａ２、ａ３、ａ４となる。また、ある任意の１つのグリッドは、上下左右の４つのグリッドと隣接する。すなわち、移動領域の端以外のグリッドでは、上方向、下方向、左方向、右方向の４近傍のグリッドが隣接していることになる。
【００３０】
また、図３（Ｂ）に示すように、ある１つのグリッドに関して、そのエッジ上の１つの点と、他の異なるエッジ上の１つの点と、を結ぶ直線をエッジ間の探索枝とする。探索枝が延びている方向を示すベクトルを、グリッド探索ベクトルとする。すなわち、グリッド探索ベクトルは、探索枝の方向（傾き）を示す。図に示す例では、点Ｑｊと点Ｑｊ＋１とを結ぶ直線がエッジ間の探索枝となり、この探索枝の方向がグリッド探索ベクトルとなる。グリッド探索ベクトルが隣接グリッドのエッジ上に進入した場合に、この進入点（グリッド探索ベクトルとエッジとの交点）は、エッジを介して隣接するグリッドにおける、探索枝の始点となる。経路探索の過程では、エッジ上における探索枝の端点の座標と、その探索枝の角度（グリッド探索ベクトル）とが、情報として保持される。なお、探索枝の角度は、これら端点の座標値から求めることができる。
【００３１】
距離ポテンシャル生成部１１２は、グリッドマップ内のグリッドに対して、移動始点からの距離と、障害物からの距離と、に基づいて、距離ポテンシャルを生成する。まず、距離ポテンシャル生成部１１２は、グリッドマップ内の各グリッドに対して、移動始点からの距離に応じたポテンシャル値を設定する。ここでは、移動始点のグリッドには値を０として設定し、移動始点から離れるほど大きくなるポテンシャル値を算出し、これらポテンシャル値を各グリッドに設定する。例えば、隣接グリッドへ移動するために要するコストとして、グリッド間距離などの適当な値を定める。そして、ダイクストラ法やＡ＊（スター）アルゴリズムなどの公知の探索アルゴリズムを用いて、移動始点から目標グリッドまでの最短経路を探索する。そして、その探索した経路上のグリッドについてのコストの総和を求め（すなわち、移動始点から目標グリッドまでの経路長に応じた値）、その総和値を、目標グリッドの距離ポテンシャル値として設定する。
【００３２】
また、距離ポテンシャル生成部１１２は、障害物が存在する場合には、その障害物からの距離に応じたポテンシャル値を算出し、移動始点からの距離に応じて設定したポテンシャル値に加算する。例えば、障害物グリッドからの距離に応じて単調に減少するポテンシャル関数を考え、進入可能グリッドの各グリッドに対してポテンシャル関数で計算される値の総和をその位置のポテンシャル値とするポテンシャル場を生成する。ただし、障害物グリッド自体には、ある一定のポテンシャル値（障害物でないグリッドに設定されるポテンシャル値と比較して十分大きな正の値であって、無限大と見なして処理する。）を設定する。なお、距離ポテンシャル生成部１１２による障害物からの距離に応じて設定する距離ポテンシャル値の算出手法については、従来公知のポテンシャル法を用いて行えばよく、ここでは、その詳細な説明を省略する。
【００３３】
探索グリッド決定部１１３は、前回探索対象となったグリッドについて決定したグリッド探索ベクトルから、次に探索するグリッドを決定し、また、その決定したグリッドについての局所ポテンシャル場の計算に利用するグリッドを選択する。より具体的には、前回決定したグリッド探索ベクトルがエッジを交差して進入する隣接グリッドを、次に探索対象とするグリッドとして決定する（以下、次探索グリッドと称する場合がある。）。そして、その決定した次探索グリッドの隣接グリッドのうちで、前記グリッド探索ベクトルの延長上にある複数のグリッドを、局所ポテンシャル場の計算用グリッドとして選択する（以下、局所ポテンシャル場計算用グリッドと称する場合がある。）。なお、局所ポテンシャル場の計算用グリッドは、次探索グリッドの前後左右に隣接するグリッドから選択される。
【００３４】
図４は、探索グリッド決定部１１３による探索グリッドの決定及び局所ポテンシャル場計算用グリッドの選択方法を説明するための図である。図に示す例では、３×３のグリッドが示されている。ここで、説明の簡略化のため、各グリッドを次のように座標で識別して、説明する。例えば、左上隅のノードを（１，１）のグリッドとして、右下隅のノードを（３，３）とする。また、（１，１）のグリッドの１つ下のノードを（２，１）のグリッドとし、（１，１）のグリッドの１つ下のグリッドを（１，２）とする。このように座標を用いて、各グリッドを識別する。なお、図４に示す例では、紙面右方向をｘ軸正方向とし、上方向をｙ軸正方向とするグローバル座標系での処理を示す。このため、ここでは、グローバル座標系での、探索枝の端点の座標と、その探索枝の角度とが、ＲＡＭなどの一時記憶部に保持されている。
【００３５】
図４（Ａ）に示す例では、まず、グリッド（２，１）内の矢印により示す探索方向（グリッド探索ベクトル）から、次探索グリッドとしてグリッド（２，２）を決定する。そして、グリッド探索ベクトルのｘ、ｙ成分に注目し、ｘ＞０、ｙ＞０であることから、グリッド（１，２）、グリッド（２，３）を局所ポテンシャル場計算用グリッドとして選択する。また、図４（Ｂ）に示す例では、次探索グリッドとしてグリッド（２，２）を決定し、ｘ＞０、ｙ＜０であることから、グリッド（２，３）、グリッド（３，２）を局所ポテンシャル場計算用グリッドとして選択する。このように、グリッド探索ベクトルの成分（矢印の入射角度）に応じて、次探索グリッドと、局所ポテンシャル場計算用グリッドと、が選択される。
【００３６】
なお、グリッド探索ベクトルのｘ、ｙ成分について、ｘ＝０、ｙ＝０である場合には、図４（Ａ）に示した場合と同じようにして２つの局所ポテンシャル場計算用グリッドを選択するものとしてもよいし、或いは、図４（Ｂ）に示した場合と同じようにして２つの局所ポテンシャル場計算用グリッドを選択するものとしてもよい。図４に示す例では、例えば、グリッド（２，２）と、グリッド（２，３）又はグリッド（３，２）のいずれか１つのグリッドと、を選択すればよい。なお、グリッド（２，３）又はグリッド（３，２）のいずれか１つの障害物グリッドある場合には、障害物グリッドでない他のグリッドを選択するようにしてもよい。
【００３７】
詳細は後述するが、グリッド（２，１）について計算する局所ポテンシャル場を用いて、グリッド（２，１）内のグリッド探索ベクトルが決定される。決定したグリッド探索ベクトルから次探索グリッドが決定される。
【００３８】
局所ポテンシャル場計算部１１４は、次探索グリッドについての局所ポテンシャル場を、次探索グリッドの距離ポテンシャル値に対する局所ポテンシャル場計算用グリッドの距離ポテンシャル値の相対変化に基づいて計算する。ここでは、局所ポテンシャル場として、円錐ポテンシャル場を計算する。また、円錐ポテンシャル場の頂点を、次探索グリッドのポテンシャル値の二乗と、局所ポテンシャル場計算用グリッドのポテンシャル値の二乗との差に応じて定める。
【００３９】
図５は、局所ポテンシャル場の計算方法を説明するための図である。図に示す例では、局所ポテンシャル場としての円錐ポテンシャル場を計算するための、３つのグリッドＡ、Ｂ、Ｃが示されている。なお、図５では、次探索グリッドをグリッドＡ、局所ポテンシャル場計算用グリッドをグリッドＢ及びグリッドＣとして、これら３つのグリッドのエッジが互いに交差する点を、座標系の原点と定める。すなわち、局所ポテンシャル場の計算においては、決定される次探索グリッドと、選択される局所ポテンシャル場計算用グリッドと、から定まるローカル座標系での処理を行う。
【００４０】
図５に示す例では、グリッドＡのエッジａ１上の点Ｑ１（ｘｑ，ｙｑ）に対して、その傾きが負である探索枝（実線の矢印で示す。）が進入したものとする（すなわち、グリッド探索ベクトルの成分について、グローバル座標系でのｘ、ｙについてｄｙ／ｄｘ＜０。）。このとき、上述したようにして、グリッドＡが次探索グリッドとなり、局所ポテンシャル場計算用グリッドとしてグリッドＢ、Ｃが選択される。局所ポテンシャル場計算部１１４は、グリッドＡのポテンシャル値ＰＡと、グリッドＢ、Ｃのポテンシャル値ＰＢ、ＰＣを利用して、グリッドＡについての局所ポテンシャル場ＰＮを計算する。
【００４１】
局所ポテンシャル場ＰＮは、以下の数（１）を用いて表される。
【数１】
【００４２】
数（１）により示される局所ポテンシャル場は、その頂点Ｏｃが以下の数（２）で示される、円錐ポテンシャル場である。また、数（１）において、Δｄはグリッドサイズ（エッジの大きさ）を示している。
【数２】
【００４３】
なお、局所ポテンシャル場計算用グリッドとして選択されたグリッドＢ、Ｃについて、そのいずれか１のグリッドが障害物グリッドである場合には、円錐ポテンシャル場を算出するのに代えて、平面ポテンシャル場を計算するものとする。例えば、局所ポテンシャル場計算用グリッドとして選択されたグリッドＢが障害物グリッドである場合には、上記の数（１）で示される局所ポテンシャル場に代えて、グリッドＡのポテンシャル値ＰＡと、グリッドＣのポテンシャル値ＰＣとを直線補間し、その直線補間された値からなる平面ポテンシャル場を、局所ポテンシャル場とする。また、グリッドＢ、Ｃがともに障害物グリッドである場合には、経路探索による解は無いと判定して、探索を打ち切るようにしてもよい。
【００４４】
ここで、図６を参照して、円錐ポテンシャル場の導出について補足する。
図に示す例では、円錐ポテンシャル場を計算するための、３つのグリッドＡ、Ｂ、Ｃが示されている。なお、グリッド探索ベクトル（不図示）が、グリッドＡのエッジ上に進入し、その進入点がＱ１であるものとする。また、ローカル座標系の原点を、Ｏとする。
【００４５】
まず、Ｏを原点とするローカル座標系でのグリッドＡ、Ｂ、Ｃの各位置Ａ（ｘ，ｙ）、Ｂ（ｘ，ｙ）、Ｃ（ｘ，ｙ）を、それぞれ以下の数（３）、（４）、（５）により示す。
【数３】
【数４】
【数５】
【００４６】
次に、円錐ポテンシャルの頂点Ｏｃを中心として、各グリッドのポテンシャル値に関して以下の数（６）、（７）、（８）を定義する。なお、以下では、頂点Ｏｃのｘ、ｙ座標値を単にＯｃｘ、Ｏｃｙとして表記し、グリッドＡの位置のｘ、ｙ座標値を単にＡｘ、Ａｙとして表記して説明する。
【数６】
【数７】
【数８】
【００４７】
そして、数（６）−数（７）を計算して、以下の数（９）を得る。
【数９】
また、数（６）−数（８）を計算して、以下の数（１０）を得る。
【数１０】
従って、数（９）及び数（１０）から、以下の数（１１）に示すように、円錐ポテンシャルの頂点Ｏｃを求める。
【数１１】
従って、数（１１）の頂点Ｏｃから、以下の数（１２）で示す円錐ポテンシャルＰが定まる。
【数１２】
【００４８】
図２に戻って説明を続ける。
グリッド探索ベクトル決定部１１５は、前回決定したグリッド探索ベクトルが進入するエッジ上の点を探索枝の基点として、計算した局所ポテンシャル場のポテンシャル値が最急勾配となる方向（ポテンシャル値の変化が最も急となる方向）であって、その勾配を降る方向（ポテンシャル値が小さくなる方向）に、探索枝を延ばす。グリッド探索ベクトル決定部１１５は、延ばした探索枝の方向を、グリッド探索ベクトルとして決定する。この決定したグリッド探索ベクトルはＲＡＭなどの一時記憶部に保持され、次に探索対象とするグリッドを決定する際の情報として利用される。
【００４９】
図７は、グリッド探索ベクトルの決定方法を説明するための図である。図では、現在の探索対象グリッドをグリッドＡとし、局所ポテンシャル場としての円錐ポテンシャルが計算されている。なお、円錐ポテンシャル場は、グリッドＡ、Ｂ、Ｃの各ポテンシャル値に基づいて計算され、Ｏｃは円錐ポテンシャル場の頂点を示す。
【００５０】
図７において、前回決定したグリッド探索ベクトルがグリッドＡのエッジａ１に進入し、その進入点を点Ｑ１とする。グリッドＡにおいて、点Ｑ１を探索枝の基点として、局所ポテンシャル場における最急勾配方向で、降る方向へと探索枝を延ばす。詳細は後述するが、点Ｑ１から延ばした探索枝がグリッドＡのエッジａ３に到達した場合に、このエッジａ３上の交点を点Ｑ２として、点Ｑ１から点Ｑ２へと向かう探索枝の方向が、局所ポテンシャル場における最急勾配方向であって、かつ、降る方向となる。
【００５１】
円錐ポテンシャル場において、最急勾配方向に降る方向は、エッジ上の基点から円錐の頂点へと向かう直線の方向に相当する（この直線は、円錐の円周に対して直角に交わる。）。すなわち、探索枝は、エッジ上の基点から円錐ポテンシャル場の頂点へと向かう方向へと延びる。探索枝の端点Ｑ２は、基点Ｑ１から円錐ポテンシャル場の頂点へと向かう直線上であって、かつ、エッジａ３上に位置する点となる。点Ｑ２の座標は、例えば、以下の方法により求めることができる。
【００５２】
まず、円錐ポテンシャル場の頂点Ｏｃは、以下の数（１３）により与えられる。
【数１３】
【００５３】
以下の数（１４）は、円錐ポテンシャル場の頂点の座標と、点Ｑ１の座標と、点Ｑ２の座標と、の関係を示す直線方程式である。円錐ポテンシャル場の頂点と点Ｑ１を結ぶ直線と、点Ｑ１と点Ｑ２を結ぶ直線と、は互いに平行となる。このため、この直線方程式を利用して、点Ｑ２の座標（Ｑ２ｘ，Ｑ２ｙ）を求めることができる。
【数１４】
【００５４】
また、点Ｑ１と点Ｑ２は、それぞれエッジ上に位置する点であるため、例えば、Ｑ２ｘ−Ｑ１ｘ＝Δｄという関係を利用して、以下の数（１５）に示すようにして、点Ｑ２の座標を求めることができる。
【数１５】
【００５５】
従って、図７に示す例では、エッジａ１上の基点Ｑ１の座標と、円錐ポテンシャル場の頂点の座標Ｏｃとから、エッジａ２上の点Ｑ２の座標を求める。そして、エッジａ１上の基点Ｑ１とエッジａ２上の点Ｑ２とを結ぶ直線をエッジ間の探索枝として、この探索枝の方向を、グリッド探索ベクトルとして決定することができる。
【００５６】
なお、図７に示した例では、グリッドＡは、他のグリッドと比べてより移動終点に近いグリッドであり、グリッドＢは他のグリッドと比べてより壁に近いグリッドであり、グリッドＣは、他のグリッドと比べてより移動始点に近いグリッドであり、各グリッドのポテンシャル値について、ＰＣ＜ＰＢ＜ＰＡが成立するような状況を示している。このような状況を反映した円錐ポテンシャル場が、図７に例示されている。すなわち、円錐ポテンシャル場の頂点Ｏｃは、探索枝の基点Ｑ１から見て、より移動始点へと近づく方向であって、かつ、障害物から離れる方向に位置するものとして算出されている。従って、グリッドＡへと進入したグリッド探索ベクトルは、計算した局所ポテンシャル場によって、その方向が障害物から遠ざかる方向へと曲げられる。
【００５７】
次に、図８を参照して、経路探索処理のフローについて説明する。
まず、制御部１１０は、グリッド空間において、移動始点のグリッドと、移動終点のグリッドを設定する（Ｓ１０１）。
【００５８】
次に、距離ポテンシャル生成部１１２は、グリッドマップ内の各グリッドに対して、移動終点や障害物からの距離に応じたポテンシャル値を設定し、距離ポテンシャルを生成する（Ｓ１０２）。
【００５９】
次に、探索グリッド決定部１１３は、前回決定したグリッド探索ベクトルから、次に探索するグリッドを決定し、また、その決定したグリッドについての局所ポテンシャル場の計算に利用するグリッドを選択する（Ｓ１０３）。
【００６０】
次に、局所ポテンシャル場計算部１１４は、決定した次探索グリッドについて、次探索グリッドのポテンシャル値と、選択した局所ポテンシャル場計算用グリッドの各ポテンシャル値と、に基づいて、局所ポテンシャル場を計算する（Ｓ１０４）。
【００６１】
次に、グリッド探索ベクトル決定部１１５は、局所ポテンシャル場を計算した次探索グリッドにおいて、前回決定したグリッド探索ベクトルがエッジに進入する点を探索枝の基点として、ポテンシャル値が最急勾配方向で降る方向に探索枝を延ばし、その延ばした探索枝の方向を、次探索グリッドの次に探索対象とするグリッドを決定するための、新たなグリッド探索ベクトルとして決定する（Ｓ１０５）。
【００６２】
制御部１１０は、グリッド探索ベクトルが示す次探索グリッドが移動始点のグリッドであるか否かを判定し、移動始点のグリッドに到達したか否かを判定する（Ｓ１０６）。判定の結果、到達していない場合には、ステップＳ１０３〜Ｓ１０５の処理を繰り返して、順次、グリッドを探索していく。一方で、判定の結果、到達した場合には、制御部１１０は、移動始点へと到達するまでに探索したグリッドの系列を出力する。すなわち、このグリッド系列を、移動終点と移動始点との間の経路として生成する。
【００６３】
以上説明したように本実施の形態によれば、従来のポテンシャル法によるグリッド探索のように、隣接グリッドのうちで最小のポテンシャル値を有するグリッドを単純に選択するものではなく、次に探索対象となるグリッドにおいてエッジ間の探索枝を求め、その探索枝の方向をグリッド探索ベクトルとした上で、そのグリッド探索ベクトルから次の探索対象グリッドを順次決定していくものである。ここで、グリッド探索ベクトルの決定に際しては、隣接グリッドのポテンシャル値をも考慮した局所ポテンシャル場を計算し、距離ポテンシャル値からより詳細なポテンシャル場を局所的に計算している。そして、局所ポテンシャル場において、ポテンシャル値が最急勾配方向で降る方向に、探索枝を延ばすことを特徴とするものである。
【００６４】
図９は、本実施の形態による効果をより具体的に説明するための図である。
図９（Ａ）は、グリッド間の距離を利用して経路長が最短となる経路を求める、従来手法による経路探索結果を示している。求められたグリッドの系列は、直線により示す最適な経路とは大きく異なるものである。
これに対して図９（Ｂ）は、本実施の形態による経路探索結果を示している。求められたグリッドの系列は、直線により示す最適な経路に、より近い経路となる。
【００６５】
従来のポテンシャル法を用いた経路探索手法では、単に、隣接グリッドのうちで最小のポテンシャルを有するグリッドの選択を繰り返していき、移動終点のグリッドに到達した場合には、探索を終了するものである。
これに対して本実施の形態では、基本的に全探索を行う経路探索手法（ダイクストラ法など）を用いて距離ポテンシャルを生成しているためにローカルミニマムを回避することができ、かつ、理想的な最短経路により近い経路を生成することができる。
【００６６】
また、特許文献１に示されるような従来のポテンシャル法を用いた経路生成技術では、ローカルミニマムを回避することを目的として、予め設定したポテンシャル場自体を合成ポテンシャルで補正するものである。
これに対して本実施の形態では、予め生成したポテンシャル場自体を補正するものではなく、経路探索の過程において、各探索グリッドについて、その詳細な局所ポテンシャル場を求めるものである。すなわち、予め生成したポテンシャル値を利用して、局所的に詳細な解を求めながら経路探索を行うことを特徴とする。このように局所的に詳細なポテンシャル値を求めることで、より最適解に近い経路を探索可能とする。
【００６７】
上述した実施の形態では、移動始点から移動終点に至る移動領域内に障害物が存在する場合における経路探索の例を説明したが、本発明はこれに限定されず、障害物の有無に依存せず、より理想的な経路を生成することができる。
【００６８】
また、上述した実施の形態では、移動終点のグリッドから移動始点のグリッドへと向けて探索を行う場合を例に説明したが、移動始点のグリッドから移動終点のグリッドへと向けて探索を行うようにしてもよく、任意の２地点間を結ぶ経路探索技術に適用可能である。
【００６９】
また、移動始点や障害物からの距離に応じたポテンシャル値を負の値として設定した場合には、局所ポテンシャル場において、ポテンシャル値が最急勾配方向で登る方向に探索枝を延ばし、その延ばした探索の方向をグリッド探索ベクトルとして決定すればよい。
【００７０】
なお、上述した処理によって求めた経路に対して、さらに補間処理を行うものとしてもよい。本発明により生成される経路は、より最短経路に近いものであるため、このような経路をもとに補間処理を行うことで、より最適かつ滑らかな経路を生成することができる。
【００７１】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
【符号の説明】
【００７２】
２ 車輪、
３ 筐体、
５ センサ、
１００ ロボット、
１１０ 制御部、
１１１ 地図情報記憶部、
１１２ 距離ポテンシャル生成部、
１１３ 探索グリッド決定部、
１１４ 局所ポテンシャル場計算部、
１１５ グリッド探索ベクトル決定部、
【特許請求の範囲】
【請求項１】
移動領域を、各グリッドを区切る周囲の枠をグリッドのエッジとする、複数の格子状のグリッドに分割し、移動始点と移動終点の間を結ぶグリッド系列を探索する経路探索システムであって、
障害物が占有し、移動体の進入が禁止される進入禁止グリッドが設定されたグリッドマップを記憶する地図情報記憶部と、
前記移動始点からの距離に応じたポテンシャル値と、前記障害物からの距離に応じたポテンシャル値と、を加算して、前記グリッドマップの各グリッドの距離ポテンシャル値として生成する距離ポテンシャル生成部と、
前回探索対象グリッドにおいて決定されたグリッド探索ベクトルが次に進入する隣接グリッドを、次に探索対象とする次探索グリッドとして決定すると共に、前記グリッド探索ベクトルの向きに応じて、前記次探索グリッドに隣接する第１及び第２のグリッドを、当該次探索グリッドについての局所ポテンシャル場計算用グリッドとして選択する探索グリッド決定部と、
前記次探索グリッドの距離ポテンシャル値に対する前記局所ポテンシャル場計算用グリッドの距離ポテンシャル値の相対変化に基づく頂点を持つ局所ポテンシャル場であって、前記次探索グリッドについての局所ポテンシャル場を計算する局所ポテンシャル場計算部と、
前回探索対象グリッドにおいて決定されたグリッド探索ベクトルが前記次探索グリッドに進入する際のエッジ上の点を探索枝の基点として、前記局所ポテンシャル場のポテンシャル値が最急勾配となる方向であって、当該勾配を降る方向に当該探索枝を延ばし、当該延ばした探索枝の方向を、前記次探索グリッドにおけるグリッド探索ベクトルとして決定するグリッド探索方向決定部と、を備える
経路探索システム。
【請求項２】
前記局所ポテンシャル場計算部は、
前記局所ポテンシャル場として、前記次探索グリッドの距離ポテンシャル値の二乗と、前記局所ポテンシャル場計算用グリッドの距離ポテンシャル値の二乗と、の差に応じて定まる頂点を持つ、円錐ポテンシャル場を計算する
ことを特徴とする請求項１に記載の経路探索システム。
【請求項３】
前記グリッド探索方向決定部は、
前記探索枝の前記基点から前記局所ポテンシャル場の前記頂点へと向かう直線上の点であって、前記基点が位置するエッジとは異なる前記次探索グリッドの他のエッジ上に位置する点を、前記探索枝の端点として求め、前記探索枝の前記基点から前記端点へと向かう方向を、前記次探索グリッドにおけるグリッド探索ベクトルとして決定する
ことを特徴とする請求項１又は２に記載の経路探索システム。
【請求項４】
前記距離ポテンシャル生成部は、
隣接グリッド間の移動コストとして所定の値を定め、前記移動始点から目標グリッドへと至るグリッド系列を探索して、探索した当該グリッド系列についての前記移動コストの総和を求め、当該総和値と、前記障害物からの距離に応じたポテンシャル値と、を加算して、前記目標グリッドの距離ポテンシャル値を算出する
ことを特徴とする請求項１乃至３いずれか１項に記載の経路探索システム。
【請求項５】
移動領域内の移動始点から移動終点へと移動する移動体の経路探索方法であって、
前記移動領域を、各グリッドを区切る周囲の枠をグリッドのエッジとする、複数の格子状のグリッドに分割したグリッドマップを用いて表して、障害物が占有し、前記移動体の進入が禁止される進入禁止グリッドが設定されたグリッドマップを記憶するステップと、
前記移動始点からの距離に応じたポテンシャル値と、前記障害物からの距離に応じたポテンシャル値と、を加算して、前記グリッドマップの各グリッドの距離ポテンシャル値として生成するステップと、
前回探索対象グリッドにおいて決定されたグリッド探索ベクトルが次に進入する隣接グリッドを、次に探索対象とする次探索グリッドとして決定すると共に、前記グリッド探索ベクトルの向きに応じて、前記次探索グリッドに隣接する第１及び第２のグリッドを、当該次探索グリッドについての局所ポテンシャル場計算用グリッドとして選択するステップと、
決定した前記次探索グリッドと、選択した前記局所ポテンシャル場計算用グリッドについて、前記次探索グリッドの距離ポテンシャル値に対する前記局所ポテンシャル場計算用グリッドの距離ポテンシャル値の相対変化に基づく頂点を持つ局所ポテンシャル場であって、前記次探索グリッドについての局所ポテンシャル場を計算するステップと、
前回探索対象グリッドにおいて決定されたグリッド探索ベクトルが前記次探索グリッドに進入する際のエッジ上の点を探索枝の基点として、計算した前記局所ポテンシャル場のポテンシャル値が最急勾配となる方向であって、当該勾配を降る方向に当該探索枝を延ばし、当該延ばした探索枝の方向を、前記次探索グリッドにおけるグリッド探索ベクトルとして決定するステップと、
前記グリッド探索ベクトルが決定されたグリッド系列を、前記移動始点と移動終点の間を結ぶ経路として出力するステップと、を有する
経路探索方法。
【請求項６】
移動領域を、各グリッドを区切る周囲の枠をグリッドのエッジとする、複数の格子状のグリッドに分割し、移動始点と移動終点の間を結ぶグリッド系列を経路として探索し、当該経路に従って移動する移動体であって、
障害物が占有し、移動体の進入が禁止される進入禁止グリッドが設定されたグリッドマップを記憶する地図情報記憶部と、
前記移動始点からの距離に応じたポテンシャル値と、前記障害物からの距離に応じたポテンシャル値と、を加算して、前記グリッドマップの各グリッドの距離ポテンシャル値として生成する距離ポテンシャル生成部と、
前回探索対象グリッドにおいて決定されたグリッド探索ベクトルが次に進入する隣接グリッドを、次に探索対象とする次探索グリッドとして決定すると共に、前記グリッド探索ベクトルの向きに応じて、前記次探索グリッドに隣接する第１及び第２のグリッドを、当該次探索グリッドについての局所ポテンシャル場計算用グリッドとして選択する探索グリッド決定部と、
前記次探索グリッドの距離ポテンシャル値に対する前記局所ポテンシャル場計算用グリッドの距離ポテンシャル値の相対変化に基づく頂点を持つ局所ポテンシャル場であって、前記次探索グリッドについての局所ポテンシャル場を計算する局所ポテンシャル場計算部と、
前回探索対象グリッドにおいて決定されたグリッド探索ベクトルが前記次探索グリッドに進入する際のエッジ上の点を探索枝の基点として、前記局所ポテンシャル場のポテンシャル値が最急勾配となる方向であって、当該勾配を降る方向に当該探索枝を延ばし、当該延ばした探索枝の方向を、前記次探索グリッドにおけるグリッド探索ベクトルとして決定するグリッド探索方向決定部と、を備える
移動体。
【請求項１】
移動領域を、各グリッドを区切る周囲の枠をグリッドのエッジとする、複数の格子状のグリッドに分割し、移動始点と移動終点の間を結ぶグリッド系列を探索する経路探索システムであって、
障害物が占有し、移動体の進入が禁止される進入禁止グリッドが設定されたグリッドマップを記憶する地図情報記憶部と、
前記移動始点からの距離に応じたポテンシャル値と、前記障害物からの距離に応じたポテンシャル値と、を加算して、前記グリッドマップの各グリッドの距離ポテンシャル値として生成する距離ポテンシャル生成部と、
前回探索対象グリッドにおいて決定されたグリッド探索ベクトルが次に進入する隣接グリッドを、次に探索対象とする次探索グリッドとして決定すると共に、前記グリッド探索ベクトルの向きに応じて、前記次探索グリッドに隣接する第１及び第２のグリッドを、当該次探索グリッドについての局所ポテンシャル場計算用グリッドとして選択する探索グリッド決定部と、
前記次探索グリッドの距離ポテンシャル値に対する前記局所ポテンシャル場計算用グリッドの距離ポテンシャル値の相対変化に基づく頂点を持つ局所ポテンシャル場であって、前記次探索グリッドについての局所ポテンシャル場を計算する局所ポテンシャル場計算部と、
前回探索対象グリッドにおいて決定されたグリッド探索ベクトルが前記次探索グリッドに進入する際のエッジ上の点を探索枝の基点として、前記局所ポテンシャル場のポテンシャル値が最急勾配となる方向であって、当該勾配を降る方向に当該探索枝を延ばし、当該延ばした探索枝の方向を、前記次探索グリッドにおけるグリッド探索ベクトルとして決定するグリッド探索方向決定部と、を備える
経路探索システム。
【請求項２】
前記局所ポテンシャル場計算部は、
前記局所ポテンシャル場として、前記次探索グリッドの距離ポテンシャル値の二乗と、前記局所ポテンシャル場計算用グリッドの距離ポテンシャル値の二乗と、の差に応じて定まる頂点を持つ、円錐ポテンシャル場を計算する
ことを特徴とする請求項１に記載の経路探索システム。
【請求項３】
前記グリッド探索方向決定部は、
前記探索枝の前記基点から前記局所ポテンシャル場の前記頂点へと向かう直線上の点であって、前記基点が位置するエッジとは異なる前記次探索グリッドの他のエッジ上に位置する点を、前記探索枝の端点として求め、前記探索枝の前記基点から前記端点へと向かう方向を、前記次探索グリッドにおけるグリッド探索ベクトルとして決定する
ことを特徴とする請求項１又は２に記載の経路探索システム。
【請求項４】
前記距離ポテンシャル生成部は、
隣接グリッド間の移動コストとして所定の値を定め、前記移動始点から目標グリッドへと至るグリッド系列を探索して、探索した当該グリッド系列についての前記移動コストの総和を求め、当該総和値と、前記障害物からの距離に応じたポテンシャル値と、を加算して、前記目標グリッドの距離ポテンシャル値を算出する
ことを特徴とする請求項１乃至３いずれか１項に記載の経路探索システム。
【請求項５】
移動領域内の移動始点から移動終点へと移動する移動体の経路探索方法であって、
前記移動領域を、各グリッドを区切る周囲の枠をグリッドのエッジとする、複数の格子状のグリッドに分割したグリッドマップを用いて表して、障害物が占有し、前記移動体の進入が禁止される進入禁止グリッドが設定されたグリッドマップを記憶するステップと、
前記移動始点からの距離に応じたポテンシャル値と、前記障害物からの距離に応じたポテンシャル値と、を加算して、前記グリッドマップの各グリッドの距離ポテンシャル値として生成するステップと、
前回探索対象グリッドにおいて決定されたグリッド探索ベクトルが次に進入する隣接グリッドを、次に探索対象とする次探索グリッドとして決定すると共に、前記グリッド探索ベクトルの向きに応じて、前記次探索グリッドに隣接する第１及び第２のグリッドを、当該次探索グリッドについての局所ポテンシャル場計算用グリッドとして選択するステップと、
決定した前記次探索グリッドと、選択した前記局所ポテンシャル場計算用グリッドについて、前記次探索グリッドの距離ポテンシャル値に対する前記局所ポテンシャル場計算用グリッドの距離ポテンシャル値の相対変化に基づく頂点を持つ局所ポテンシャル場であって、前記次探索グリッドについての局所ポテンシャル場を計算するステップと、
前回探索対象グリッドにおいて決定されたグリッド探索ベクトルが前記次探索グリッドに進入する際のエッジ上の点を探索枝の基点として、計算した前記局所ポテンシャル場のポテンシャル値が最急勾配となる方向であって、当該勾配を降る方向に当該探索枝を延ばし、当該延ばした探索枝の方向を、前記次探索グリッドにおけるグリッド探索ベクトルとして決定するステップと、
前記グリッド探索ベクトルが決定されたグリッド系列を、前記移動始点と移動終点の間を結ぶ経路として出力するステップと、を有する
経路探索方法。
【請求項６】
移動領域を、各グリッドを区切る周囲の枠をグリッドのエッジとする、複数の格子状のグリッドに分割し、移動始点と移動終点の間を結ぶグリッド系列を経路として探索し、当該経路に従って移動する移動体であって、
障害物が占有し、移動体の進入が禁止される進入禁止グリッドが設定されたグリッドマップを記憶する地図情報記憶部と、
前記移動始点からの距離に応じたポテンシャル値と、前記障害物からの距離に応じたポテンシャル値と、を加算して、前記グリッドマップの各グリッドの距離ポテンシャル値として生成する距離ポテンシャル生成部と、
前回探索対象グリッドにおいて決定されたグリッド探索ベクトルが次に進入する隣接グリッドを、次に探索対象とする次探索グリッドとして決定すると共に、前記グリッド探索ベクトルの向きに応じて、前記次探索グリッドに隣接する第１及び第２のグリッドを、当該次探索グリッドについての局所ポテンシャル場計算用グリッドとして選択する探索グリッド決定部と、
前記次探索グリッドの距離ポテンシャル値に対する前記局所ポテンシャル場計算用グリッドの距離ポテンシャル値の相対変化に基づく頂点を持つ局所ポテンシャル場であって、前記次探索グリッドについての局所ポテンシャル場を計算する局所ポテンシャル場計算部と、
前回探索対象グリッドにおいて決定されたグリッド探索ベクトルが前記次探索グリッドに進入する際のエッジ上の点を探索枝の基点として、前記局所ポテンシャル場のポテンシャル値が最急勾配となる方向であって、当該勾配を降る方向に当該探索枝を延ばし、当該延ばした探索枝の方向を、前記次探索グリッドにおけるグリッド探索ベクトルとして決定するグリッド探索方向決定部と、を備える
移動体。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【公開番号】特開２０１１−２２７８０７（Ｐ２０１１−２２７８０７Ａ）
【公開日】平成２３年１１月１０日（２０１１．１１．１０）
【国際特許分類】
【出願番号】特願２０１０−９８６０６（Ｐ２０１０−９８６０６）
【出願日】平成２２年４月２２日（２０１０．４．２２）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
【公開日】平成２３年１１月１０日（２０１１．１１．１０）
【国際特許分類】
【出願日】平成２２年４月２２日（２０１０．４．２２）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
[ Back to top ]