JP2011128758A - 経路探索システム、方法、プログラム、並びに移動体 - Google Patents

Abstract

【課題】短時間で終点までの経路を確実に探索することができる経路探索システム、経路探索方法、経路探索プログラム、及び移動体を提供すること。
【解決手段】本発明にかかる経路探索システムは、グリッドで表された領域に含まれる始点から終点までの経路のうち、そのコストが最小のものを選択する経路探索システムであって、最小コストのノードを展開していくノード展開部１１２と、展開された前記最小コストのノードに対する移動方向を判定する判定部１１３と、を備え、移動方向に応じて、最小コストのノードからの展開方向の数を設定する経路探索システム。
【選択図】 図２

Description

本発明は、経路探索システム、経路探索方法、経路探索プログラム、及び移動体に関し、特に詳しくはグリッドで表された領域に含まれる始点から終点までの経路のうち、そのコストが最小のものを選択する経路探索システム、経路探索方法、及び経路探索プログラム、並びに該経路探索システムを有する移動体に関する。

移動する領域をグリッドで表現して経路探索を行う経路探索方法が広く知られている。このような経路探索方法では、Ａ＊法（エースター）やダイクストラ法等が用いられている（特許文献１、２）。これらの方法では、縦横に区切られた各グリッドをノードとする。そして、コストが最小となるようにノードを展開していき、経路探索を行う。そのため、最小コストのノードに隣接するノードのコストを計算する。そして、コスト計算の結果、最小コストとなるノードを展開する。すなわち、最短のパス（経路）が決定していないノードの中で、コストが最小のノードが最小コストのノードとなる。このように、次にコストが小さいノードが最小コストのノードとして順番に展開されていく。これを繰り返すことで、移動終点までの経路を算出することができる。

特開平１１−２６４７３４号公報 特開平９−２２９８０４号公報

さらに、このような経路探索では、枝刈り探索を行うことで計算時間を短縮することができる。この枝刈り探索について、図６を用いて説明する。図６では左側に最初のステップが示され、右側に次のステップが示されている。また、図６に示すように、移動領域が正方形のグリッドによって表現されている。障害物などがある位置は、進入禁止のグリッドとなっている。図６では、Ｌ字型に配置された３つのグリッドが進入禁止グリッドとなっている。なお、以下の説明ではノードがグリッドに対応するものとして説明する。

枝刈り手法では、最小コストのグリッドからの展開方向を３方向にしている。そして、そのグリッドから展開された３つのグリッドに対してコストを算出する。すなわち、展開方向に応じた３つの隣接グリッドに対してコストを算出する。そして、このコスト計算の結果によって、次の最小コストのグリッドを展開する。枝刈り手法を用いることで、８方向の探索から３方向の探索になるため、計算時間を短縮することができる。

最小コストのグリッドからの展開方向は、そのグリッドに対する移動方向によって、決定される。例えば、最小コストのグリッドが左上隅から右斜め下に展開されたとする（図６左側の実線矢印参照）。すなわち、最小コストのグリッドに対するパスが、図６左側の実線矢印のように、右斜め下方向に沿っているとする。

この場合、次の展開方向は、右斜め下方向とその両隣りの方向となる。すなわち、右斜め下方向、右方向、下方向のグリッドが次の展開グリッドとなって、新たにコスト計算が行われる。従って、図６左側に示す点線矢印のように、３つのグリッドのコストを算出することになる。なお、図６では、次にコストを計算するグリッド（展開グリッド）を網掛けで示している。このようにすることで、コスト計算を少なくすることができる。このため、計算時間を短縮することができる。

ここで、右方向のグリッドが次の最小コストのグリッドとして展開されたとして説明する。すなわち、次のステップで、右方向に最小コストのグリッドが展開されたとする。この場合、到達不可能なグリッドが存在してしまうことになってしまう。右方向に展開した次のステップでは、右方向、右斜め下方向、右斜め上方向が展開方向となる（図６右側の点線矢印参照）。この場合、上方向のグリッドには到達できなくなってしまう。

このように、Ａ＊法やダイクストラ法などの経路探索方法では、あるグリッドに到達するパスは、最少コストとなる１通りだけである。上記のように、左からあるグリッドに到達した場合、このグリッドは必ずこの方向から最小グリッドとして展開される。そのため、最小グリッドとして展開される展開方向も一通りに固定されるため、到達不可能なグリッドが存在してしまう。

特に、障害物の入り組んだ複雑な環境では、到達できないグリッドが存在してしまうという問題点がある。すなわち、枝刈り探索を行うことで、終点に到達するための解を求められなくなってしまうという問題点がある。

本発明は、上記の問題点に鑑みてなされたものであり、短時間で終点までの経路を確実に探索することができる経路探索システム、経路探索方法、経路探索プログラム、及び移動体を提供することを目的とする。

本発明の第１の態様にかかる経路探索システムは、グリッドで表された領域に含まれる始点から終点までの経路のうち、そのコストが最小のものを選択する経路探索システムであって、最小コストのノードを展開していくノード展開部と、展開された前記最小コストのノードに対する移動方向を判定する判定部と、を備え、前記移動方向に応じて、前記最小コストのノードからの展開方向の数を設定するものである。これにより、短時間で終点までの経路を確実に探索することができる。

本発明の第２の態様にかかる経路探索システムは、上記の経路探索システムであって、前記移動方向が斜め方向の場合に、前記展開方向の数を５とし、前記移動方向が縦方向、又は横方向の場合に、前記展開方向の数を３とすることを特徴とするものである。これにより、計算時間を短縮することができる。

本発明の第３の態様にかかる経路探索システムは、上記の経路探索システムであって、前記移動方向が縦方向、又は横方向の場合に、前記移動方向と、前記移動方向の隣の斜め方向と、が前記展開方向となり、前記移動方向が斜め方向の場合に、前記移動方向と、前記移動方向の隣の縦方向と、前記移動方向の隣の横方向と、前記縦方向の隣の斜め方向と、前記横方向の隣の斜め方向と、が前記展開方向となることを特徴とするものである。これにより、適切に展開方向を設定することができる。

本発明の第４の態様にかかる経路探索方法は、グリッドで表された領域に含まれる始点から終点までの経路のうち、そのコストが最小のものを選択する経路探索方法であって、最小コストのノードを展開するステップと、展開された前記最小コストのノードに対する移動方向を判定するステップと、前記移動方向に応じて、前記最小コストのノードからの展開方向の数を設定するステップと、を備えるものである。これにより、短時間で終点までの経路を確実に探索することができる。

本発明の第５の態様にかかる経路探索方法は、上記の経路探索方法であって、前記移動方向が斜め方向の場合に、前記展開方向の数を５とし、前記移動方向が縦方向、又は横方向の場合に、前記展開方向の数を３とすることを特徴とするものである。これにより、計算時間を短縮することができる。

本発明の第６の態様にかかる経路探索方法は、上記の経路探索方法であって、前記移動方向が縦方向、又は横方向の場合に、前記移動方向と、前記移動方向の両隣の斜め方向と、が前記展開方向となり、前記移動方向が斜め方向の場合に、前記移動方向と、前記移動方向の隣の縦方向と、前記移動方向の隣の横方向と、前記縦方向の隣の斜め方向と、前記横方向の隣の斜め方向と、が前記展開方向となることを特徴とするものである。これにより、適切に展開方向を設定することができる。

本発明の第７の態様にかかる経路探索プログラムは、グリッドで表された領域に含まれる始点から終点までの経路のうち、そのコストが最小のものを選択する経路探索プログラムであって、コンピュータに対して、最小コストのノードを展開させるステップと、展開された前記最小コストのノードに対する移動方向を判定させるステップと、前記移動方向に応じて、前記最小コストのノードからの展開方向の数を設定するステップと、を備えるものである。

本発明の第８の態様にかかる経路探索プログラムは、上記の経路探索プログラムであって、前記移動方向が斜め方向の場合に、前記展開方向の数を５とし、前記移動方向が縦方向、又は横方向の場合に、前記展開方向の数を３とすることを特徴とするものである。これにより、計算時間を短縮することができる。

本発明の第９の態様にかかる経路探索プログラムは、上記の経路探索プログラムであって、前記移動方向が縦方向、又は横方向の場合に、前記移動方向と、前記移動方向の両隣の斜め方向と、が展開方向となり、前記移動方向が斜め方向の場合に、前記移動方向と、前記移動方向の隣の縦方向と、前記移動方向の隣の横方向と、前記縦方向の隣の斜め方向と、前記横方向の隣の斜め方向とが展開方向となることを特徴とするものである。これにより、適切に展開方向を設定することができる。

本発明の第１０の態様にかかる移動体は、上記の経路探索システムを有するものである。これにより、短時間で確実に移動終点まで移動することができる。これにより、短時間で終点までの経路を確実に探索することができる。

本発明によれば、短時間で終点までの経路を確実に探索することができる経路探索システム、経路探索方法、経路探索プログラム、及び移動体を提供することができる。

本発明にかかる移動体の構成を示す図である。 本発明にかかる移動体の制御部の構成を示すブロック図である。 移動方向に応じた展開方向を示す図である。 本発明にかかる経路生成方法を示すフローチャートである。 本発明にかかる経路生成の例を示す図である。 枝刈り経路探索における問題点を説明するための図である。 ３次元領域において、移動領域を説明するための図である。 ３次元領域において、移動方向に応じた展開方向を示す図である

以下、本発明に係る移動体の実施形態を、図面に基づいて詳細に説明する。但し、本発明が以下の実施形態に限定される訳ではない。また、説明を明確にするため、以下の記載及び図面は、適宜、簡略化されている。

発明の実施の形態１．
まず、本発明に係る移動体の一例であるロボットの構成について、図１を用いて説明する。図１はロボット１００の構成を模式的に示す外観図である。本実施の形態では、ロボット１００が、自律移動する移動ロボットとして説明する。ロボット１００は、車輪２と、筐体３と、センサ５と、を備えている。そして、筐体３の内部には、車輪２と接続されたモータ、及びモータを駆動するためのバッテリなどが設けられている。このモータがロボット１００を駆動するための駆動機構となる。モータを駆動することによって、車輪２が回転して、ロボット１００が移動する。ロボット１００は、例えば、人間の歩行速度と同程度の速度で移動する。さらに、頭部１には、ＣＣＤカメラやレーザセンサなどを有するセンサ５が設けられている。センサ５はロボット１００に周囲に存在する障害物や人間などを検知する。

ロボット１００には、制御部１１０が設けられている。制御部１１０は、ＣＰＵ（Central Processing Unit）、ＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）、通信用のインタフェイスなどを有する演算処理装置である。また、制御部１１０は、着脱可能なＨＤＤ、光ディスク、光磁気ディスク等を有し、各種プログラムや制御パラメータなどを記憶し、そのプログラムやデータを必要に応じてメモリ（不図示）等に供給する。もちろん、制御部１１０は、物理的に一つの構成に限られるものではない。制御部１１０は、ロボット１００が移動する移動経路を生成する。そして、その移動経路に沿ってロボット１００が移動するよう、車輪２を駆動するためのモータ等を制御する。もちろん、ロボット１００は車輪型の移動ロボットに限らず、歩行型やその他の移動ロボットでもよい。ロボット１００は自己位置推定を行なって移動する移動体であってもよい。

制御部１１０は、移動環境中にある移動始点から移動終点までの移動経路を決定する。ロボット１００は、移動始点から移動を開始する。そして、移動経路に沿って移動していき、移動終点まで移動したら停止する。すなわち、制御部１１０は、移動始点から移動終点までの移動経路を探索する経路探索システムとして機能する。本実施の形態では、制御部１１０は、Ａ＊法やダイクストラ法を用いて、経路探索を行う。

次に、ロボット１００の制御系について図２を用いて説明する。図２は、ロボット１００の制御部１１０を示すブロック図である。制御部１１０は、ロボット１００が移動する経路を決定するための演算処理を実行する。図２に示すように、制御部１１０は、地図情報記憶部１１１と、ノード展開部１１２と、判定部１１３と、を備えている。

地図情報記憶部１１１は、ロボット１００が移動する移動領域中の地図情報を記憶している。例えば、移動する環境中に存在する壁、机などの障害物の座標を記憶している。そして、地図情報記憶部１１１は、移動領域を２次元のグリッドによって表現している。すなわち、地図情報記憶部１１１は、移動領域を縦横のグリッド線で分割して、格子状のグリッドによって表している。さらに、地図情報には、障害物などの情報に基づいて、進入禁止グリッドが設定されている。すなわち、障害物がある位置には、ロボット１００が移動することができない。このため、その障害物の位置に対応するグリッドは、進入禁止グリッドとなる。

従って、地図情報記憶部１１１には、進入禁止グリッドと進入可能グリッドとから構成されるグリッドマップが記憶される。グリッドサイズは、移動領域の大きさやコンピュータの処理速度に応じて決定される。経路探索では、ロボット１００の移動始点に対応するグリッドから移動終点に対応するグリッドまでの最適経路が探索される。すなわち、グリッドで表された領域に含まれる始点から終点までの経路のうち、そのコストが最小のものを選択する。

グリッドは、図３に示すように、縦横に区切られている。このため、ある任意の一つのグリッド（たとえば、９つのグリッドの中央のグリッド）は、上下左右の４つのグリッドと、それらの間の斜め方向の４つのグリッドと隣接する。すなわち、移動領域の端以外のグリッドでは、上方向、下方向、左方向、右方向、右斜め上方向、右斜め下方向、左斜め下方向、左斜め上方向の８近傍のグリッドが隣接していることになる。

図２に示すノード展開部１１２は、コスト計算を行って、最小コストのノードを展開していく。ここで、ノードとは、グリッドに対応するものである。従って、１つのグリッドが１つのノードとなる。ノード展開部１１２は、コスト計算の結果、最小コストのノードを順番に展開していく。すなわち、コストが最小となるパスが決定したノードを最小コストのノードとして、順番に展開していく。コストが最小となるパスが決定していないノードの中で、コスト計算の結果によってコストが最小となったノードが、最小コストノードとしてデータベースなどに順次追加されていく。そして、最小コストノードを追加していくことで、最小コストのノードが展開されていく。

また、本実施形態では、枝刈り探索を用いて、コスト計算を行っている。すなわち、最小コストノードからの展開方向を制限している。展開方向の数によって、コストを新たに算出する展開ノードの数が決まる。これにより、コスト計算を行うノード数を小さくすることができ、計算時間を短縮することができる。

さらに、本実施の形態では、最小コストノードからの展開方向の数を、移動方向（進行方向）に応じて変更している。なお、移動方向とは、最短パスにしたがって隣接ノードから最小コストノードに移動する時の方向である。すなわち、ある最小コストノードまでの最短パスにおいて、その最小コストノードに到達する方向が移動方向となる。従って、移動方向は、上方向、下方向、右方向、左方向、右斜め上方向、右斜め下方向、左斜め下方向、及び左斜め上方向の８方向のうちのいずれかとなる。

判定部１１３は、展開方向の数を設定するために、移動方向を判定する。判定部１１３は、移動方向が、斜め方向か、縦横方向（垂直方向）かを判定する。斜め方向とは、グリッドを区切るグリッド線を斜めに横切る方向である。すなわち、右斜め上方向、右斜め下方向、左斜め下方向、及び左斜め上方向が斜め方向となる。縦横方向（垂直方向）とは、グリッド線を垂直に横切る方向である。すなわち、上方向、下方向、右方向、及び左方向が縦横方向となる。本実施の形態では、移動方向が縦方向、又は横方向の場合に、展開方向の数を３として３方向展開を行う。一方、移動方向が、斜め方向の場合に、展開方向の数を５として、５方向展開を行う。

そして、ノード展開部１１２は、３つの展開方向に応じた３つのノード、あるいは５つの展開方向に応じた５つのノードに対して、コストを算出する。そして、算出したコストに応じて、コストマップを更新していく。すなわち、３つ、又は５のグリッドにおいて、コストが小さいパスが発見された場合、そのグリッドのコストを更新する。このようにしてコストマップを更新する。コストマップを参照して、コストが最小となるパスが決定していないノードの中から、コストが最小のノードを選択する。このノードが新たに、最小コストノードとして追加される。このようにして、最小コストのノードを展開していく。

次に、ノード展開部１１２と判定部１１３におけるノード展開処理について図３を用いて詳細に説明する。図３は、移動方向と、ノードの展開方向との関係を説明するための図である。図３では、３×３のノード（＝グリッド）が示されている。ここで、説明の簡略化のため、各ノードを次のように座標で識別して、説明する。例えば、左上隅のノードを（１，１）のノードとして、右下隅のノードを（３，３）とする。また、（１，１）のノードの１つ下のノードを（２，１）のノードとし、（１，１）のノードの１つ下のノードを（１，２）とする。このように座標を用いて、各ノードを識別する。

まず、斜め方向の移動について説明する（図３左側）。斜め方向に移動した場合、次のステップでは、５方向展開となる。すなわち、５つのノードに対して、新たにコストが算出される。従って、この５つ以外のノードでは、コストの値が更新されない。なお、図３では、斜線で示されるノードが、次の展開ノード、すなわち、新たにコストを算出するノードとなる。

右斜め下方向に移動する場合に付いて説明する。（１，１）のノードから（２，２）のノードに移動する場合、移動方向は、右斜め下となる。この場合、次のステップにおける展開方向は、右斜め下方向、下方向、右方向、右斜め上方向、左斜め下方向となる。すなわち、左斜め下方向〜右斜め上方向の間の５方向において隣接するノードが展開ノードとなる。従って、展開ノードである（１，３）、（２，３）、（３，３）、（３，２）、（３、１）のノードに対してコストが算出される。

右斜め上方向に移動する場合に付いて説明する。（３，１）のノードから（２，２）のノードに移動する場合、移動方向は、右斜め上となる。この場合、次のステップにおける展開方向は、右斜め上方向、上方向、右方向、右斜め下方向、左斜め上方向となる。すなわち、左斜め上方向〜右斜め下方向の間の５方向において隣接するノードが展開ノードとなる。従って、展開ノードである（１，１）、（１，２）、（１，３）、（２，３）、（３，３）のノードに対してコストが算出される。

左斜め上方向に移動する場合に付いて説明する。（３，３）のノードから（２，２）のノードに移動する場合、移動方向は、左斜め上となる。この場合、次のステップにおける展開方向は、左斜め上方向、上方向、左方向、左斜め下方向、右斜め上方向となる。すなわち、左斜め下方向〜右斜め上方向の間の５方向において隣接するノードが展開ノードとなる。従って、展開ノードである（１，１）、（１，２）、（１，３）、（２，１）、（３，１）のノードに対してコストが算出される。

左斜め下方向に移動する場合に付いて説明する。（１，３）のノードから（２，２）のノードに移動する場合、移動方向は、左斜め下となる。この場合、次のステップにおける展開方向は、左斜め下方向、下方向、左方向、左斜め上方向、右斜め下方向となる。すなわち、左斜め上方向〜右斜め下方向の間の５方向において隣接するノードが展開ノードとなる。従って、展開ノードである（１，１）、（２，１）、（３，１）、（３，２）、（３，３）のノードに対してコストが算出される。

５方向展開では、次の展開方向が、移動方向からその隣の斜め方向までの間の方向となる。従って、５方向展開では、展開方向が１８０°に広がっている。このため、５方向展開では、展開ノードがＬ字型に配置されることになる。

次に、縦方向、又は横方向の移動に付いて説明する（図３右側）。縦横方向に移動し場合、次のステップでは、３方向展開となる。すなわち、最小コストノードに隣接する３つのノードに対してコストを算出する。

右方向に移動する場合に付いて説明する。（２，１）のノードから（２，２）のノードに移動する場合、移動方向は、右となる。この場合、次のステップにおける展開方向は、右方向、右斜め上方向、右斜め下方向となる。すなわち、右斜め下方向〜右斜め上方向の間の３方向において隣接するノードが展開ノードとなる。従って、展開ノードである（１，３）、（２，３）、（３，３）のノードに対してコストが算出される。

下方向に移動する場合に付いて説明する。（１，２）のノードから（２，２）のノードに移動する場合、移動方向は、下となる。この場合、次のステップにおける展開方向は、下方向、右斜め下方向、左斜め下方向となる。すなわち、右斜め下方向〜左斜め下方向の間の３方向において隣接するノードが展開ノードとなる。従って、展開ノードである（３，１）、（３，２）、（３，３）のノードに対してコストが算出される。

左方向に移動する場合に付いて説明する。（２，３）のノードから（２，２）のノードに移動する場合、移動方向は、左となる。この場合、次のステップにおける展開方向は、左方向、左斜め上方向、左斜め下方向となる。すなわち、左斜め下方向〜左斜め上方向の間の３方向において隣接するノードが展開ノードとなる。従って、展開ノードである（１，１）、（２，１）、（３，１）のノードに対してコストが算出される。

上方向に移動する場合に付いて説明する。（３，２）のノードから（２，２）のノードに移動する場合、移動方向は、上となる。この場合、次のステップにおける展開方向は、上方向、右斜め上方向、左斜め上方向となる。すなわち、右斜め上方向〜左斜め上方向の間の３方向において隣接するノードが展開ノードとなる。従って、展開ノードである（１，１）、（１，２）、（１，３）のノードに対してコストが算出される。

３方向展開では、次の展開方向が、移動方向からその隣の斜め方向までの間の方向となる。従って、５方向展開では、展開方向が９０°に広がる。このため、３方向展開では、展開ノードがＩ字型に配置されることになる。このように、移動方向に応じて、展開方向の広がり度を変更する。

移動方向に応じて、５方向展開と３方向展開のいずれかを選択する。移動方向が縦方向、又は横方向の場合に、移動方向と、移動方向の両隣の斜め方向と、が展開方向となる。一方、移動方向が斜め方向の場合に、移動方向と、移動方向の隣の縦方向と、移動方向の隣の横方向と、前記縦方向の隣の斜め方向と、前記横方向の隣の斜め方向とが展開方向となる。そして、展開方向に隣接する展開ノードに対してコストが算出され、コストマップが更新される。このようにすることで、複雑な環境であっても、移動終点に確実に到達することができる。また、枝刈りによって展開方向を制限しているため計算時間を短縮することができる。

次に、上記の経路探索方法について、図４を用いて説明する。図４は、経路探索方法を示すフローチャートである。まず、最少コストのノードを展開する（ステップＳ１０1）。例えば、移動始点から計算する場合、ノード展開部１１２が、移動始点を０として、移動始点と隣接するノードに対するコストを算出する。そして、隣接するノードの中から、コストが最少となるノードを選択する。このノードが最小コストのノードとして展開される。また、移動始点からの計算が終了している場合、最短パスが決定していないノードの中で、コストが最小のノードを最小コストノードとして展開する。すなわち、直前のコスト計算によって最小コストとなったノードを最小コストとして格納する。そして、最小コストノードに対する移動方向を算出する（ステップＳ１０２）。

次に、判定部１１３が、最小コストノードへの移動方向が斜め方向か否かを判定する（ステップＳ１０３）。すなわち、移動方向が、斜め方向か縦横方向（垂直方向）のどちらになっているかを判別する。移動方向が縦横方向の場合、すなわち、縦方向又は横方向の場合、３方向展開を行う（ステップＳ１０４）。一方、移動方向が斜め方向の場合、５方向展開を行う（ステップＳ１０５）。

展開された全てのノードのコストを計算する（ステップＳ１０６）。すなわち、３方向展開の場合、３つの展開ノードに対してコストを算出する。５方向展開の場合、５つの展開ノードに対してコストを算出する。そして、コストが小さいパスが探索された展開ノードでは、そのコストが小さい値に更新される。ここでは、ノード展開部１１２が、展開ノードに対するコストを算出する。

そして、解となっているか否かを判定する（ステップＳ１０７）。すなわち、最短パスが決定したノードの中に、移動終点に対応するノードが存在するか否かを判定する。最小コストノードの中に、移動終点に対応するノードが存在する場合、移動終点に到達することになる。このため、経路探索を終了する（ステップＳ１０８）。

最短パスが決まったノードの中に移動終点に対応するノードが存在しない場合、解となっていない。この場合、移動終点に到達していないことになる。このため、ステップＳ１０１に戻り、最小コストのノードを格納する。すなわち、最短パスが決まっていないノードの中で、コストが最小のノードを最小コストノードとして展開する。上記の処理を繰り返し行っていくこと、最小コストノードを展開していく。そして、移動終点に到達するまで、探索処理を行っていく。

このように、ｎ回目（ｎは自然数）のコスト計算で決まった最小コストノードに対する移動方向に応じて、ｎ＋１回目のコスト計算における展開方向を制限する。すなわち、移動方向に応じて、展開方向の数を変化させる。これにより、図６に示すような場合でも、確実に移動終点に到着することができる。すなわち、進入禁止グリッドが入り組んでいりような複雑な環境であっても、解を求めることができる。よって、到達不可能なノード（グリッド）をなくすことができる。

枝刈りなしの場合、すなわち、常時５方向展開する場合の計算時間を５とする。すると、常時枝刈りを行う場合は、常時３方向展開を行うため計算時間が３となる。一方、枝刈りなしの場合、解到達性が１００％であるが、常時枝刈りを行ってしまうと、解到達性が１００％以下になってしまう。すなわち、３方向展開のみの場合は、図６に示すように解への到達できない場合がある。本実施形態にかかる探索手法では、計算時間がほぼ４となる。また、解到達性が１００％となる。このように、解到達性を低下させずに計算時間を、約４／５に短縮することができる。よって、短時間で終点までの経路を確実に探索することができる。なお、この探索処理は、ロボット１００の移動のみではなく、あらゆる経路探索問題へ応用することが可能である。たとえば、自動運転へ応用することができる。

上記の探索方法を用いた経路探索例について、図５を用いて説明する。図５は、移動領域をグリッドマップで示す図である。図５のＡに示すように、（１，１）のノードが移動始点（Ｓ）となり、（２，４）のノードが移動終点（Ｇ）となっている。

上下左右方向の展開にかかるコストを５とし、斜め方向の展開にかかるコストを７とする。このコストはノード間の距離に対応する値である。ダイクストラ法では、ノードに書かれたトータルコストで展開される（図５のＢ参照）。この場合、従来の枝刈り探索では、図６で説明したのと同様に、移動終点（Ｇ）まで到達することができない。すなわち、常時３方向展開を行うと、図５Ｃ〜図５Ｅのようにコストが計算されていく。なお、図５Ｃ〜図５Ｈでは、（１，３）〜（３，６）のノードを拡大して示している。そして、移動領域の右上隅まで展開しても、移動終点のコストを算出することができない。よって、移動終点に到達しなくなってしまう（図５Ｅ参照）。

例えば、（３，３）のグリッドでは、コストが１４となる。（３，３）のグリッドには右斜め下方向から移動している。従って、次の展開では、右斜め上方向が展開方向に含まれない。そして、（３，４）のグリッドではコストが１９となる。次の移動方向が右方向となる。この場合、移動終点である（２，４）のノードに到達できない。

本実施の形態にかかる探索方法では、図５のＦに示すように、移動終点である（２，４）のノードでは、コストが２１となる。（３，３）のノードに対する移動方向が右斜め下方向の場合でも、右斜め上方向が次の展開方向に含まれる。従って、移動終点（Ｇ）のコストを算出することができる。これにより、短い計算時間で確実に解を求めることができる。さらに、図５Ｆの後は、図５Ｇ，図５Ｈのように移動領域の隅までのコストを算出していくことになる。

上記の経路探索方法をコンピュータプログラムによって実行してもよい。上記の実施の形態のように移動経路を探索することで、移動体であるロボット１００が人間や障害物等を避けるように移動する。よって、ロボット１００を安全に移動させることができる。

なお、上記の説明では移動方向が１つであるとして説明したが、２つ以上ある場合は、それぞれの展開方向に対してコストを算出してもよい。上記の経路探索システムａは、室内自律移動のように、障害物が多く、複雑な環境下における経路探索に好適である。もちろん、経路探索システムを搭載する移動体は、特に限定されるものではない。例えば、倒立２輪を有するロボット車椅子、歩行ロボット、自律移動型のアシスタントロボットに搭載することもできる。さらに、最小コストでの解の可解性が保護される。また、障害物や人をよけることなども可能になるため、安全な自律移動が可能となる。

さらに、本実施の形態にかかる経路探索手法は、３次元の経路探索についても適用可能である。３次元の経路探索を行う場合、図７に示すように、移動領域をボクセルで表現する。すなわち、移動領域を３次元のそれぞれの方向にグリッドで仕切る。これにより、移動領域が３次元グリッドで表現される。そして、図７に示すようにボクセルを平面に分解する。図７では、３×３×３のボクセルを示している。従って、各平面に９マスのボクセルが含まれる。

３次元移動領域での経路探索の場合、図８に示す３パターンで、展開方向が決定する。すなわち、ＸＹＺ方向のいずれの方向に移動したかに応じて、展開方向が変化する。図８では、３×３×３のボクセルの中央に移動する場合が３パターン示されている。また、図８に示すようにＸＹＺ直交座標系を用いて説明する。

ボクセルを示す３次元座標において、１軸方向のみに移動した場合、９ボクセルに展開する。例えば、Ｙ軸方向で座標が変化し、Ｘ軸方向、及びＺ軸方向では座標が変化しない場合、図８の上に示すように９ボクセルに展開する。なお、図８では、展開方向のボクセルがグレーで示されている。移動方向を（ｘ,ｙ,ｚ）で表すと（±１,０,０）、（０,±１,０）、（０,０,±１）の６種類となる。この場合、各Ｚ平面に付いて考えると、２次元の垂直方向への移動と同様に、３ボクセルに展開される。従って、それぞれのＺ平面において、３つの展開方向が存在するため、次の展開方向は９つとなる。なお、Ｘ軸方向にのみ移動する場合、及びＺ軸方向にのみ移動する場合について、同様に展開方向を決定することができる。

次に、２軸方向に移動する場合に付いて考える。ここでは、例えば、Ｘ方向とＹ方向に移動する場合について説明する。従って、Ｘ座標とＹ座標は変更するが、Ｚ座標は変わらない。この場合、図８の中央に示すように、１５ボクセルに展開することができる。移動方向を（ｘ,ｙ,ｚ）で表すと（±１, ±１,０）、（０,±１, ±１）、（±１,０,±１）の６種類となる。この場合、各Ｚ平面に付いて考えると、２次元の斜め方向への移動と同様に、５ボクセルに展開される。従って、それぞれのＺ平面において、５つの展開方向が存在するため、次の展開方向は１５となる。なお、Ｘ軸方向に移動しない場合、及びＹ軸方向に移動しない場合について、同様に展開方向を決定することができる。

次に、３軸方向全てに移動する場合に付いて考える。すなわち、Ｘ方向とＹ方向とＺ方向に移動する場合について説明する。従って、Ｘ座標、Ｙ座標、及びＺ座標全てが変更する。図８の下に示すように、１７ボクセルに展開することができる。移動方向を（ｘ,ｙ,ｚ）で表すと（±１, ±１, ±１）の８種類となる。それぞれのＺ平面に考えると、移動先と移動したＺ平面では、５つの展開方向が存在し、さらに上のＺ平面では、７つの展開方向が存在する。このため、次の展開方向は１７となる。このように通常であれば、２５マスの探索が必要であるところ、９、１５、又は１７マスに減らすことができる。さらに、到達可能性を１００％にすることができる。よって、短時間で確実に終点までの経路を探索することができる。

１ 頭部
２ 車輪
３ 筐体
５ センサ
１００ ロボット
１１０ 制御部
１１１ 地図情報記憶部
１１２ ノード展開部
１１３ 判定部

Claims (10)

1. グリッドで表された領域に含まれる始点から終点までの経路のうち、そのコストが最小のものを選択する経路探索システムであって、
   最小コストのノードを展開していくノード展開部と、
   展開された前記最小コストのノードに対する移動方向を判定する判定部と、を備え、
   前記移動方向に応じて、前記最小コストのノードからの展開方向の数を設定する経路探索システム。
2. 前記移動方向が斜め方向の場合に、前記展開方向の数を５とし、
   前記移動方向が縦方向、又は横方向の場合に、前記展開方向の数を３とすることを特徴とする請求項１に記載の経路探索システム。
3. 前記移動方向が縦方向、又は横方向の場合に、前記移動方向と、前記移動方向の隣の斜め方向と、が前記展開方向となり、
   前記移動方向が斜め方向の場合に、前記移動方向と、前記移動方向の隣の縦方向と、前記移動方向の隣の横方向と、前記縦方向の隣の斜め方向と、前記横方向の隣の斜め方向と、が前記展開方向となることを特徴とする請求項１、又は２に記載の経路探索システム。
4. グリッドで表された領域に含まれる始点から終点までの経路のうち、そのコストが最小のものを選択する経路探索方法であって、
   最小コストのノードを展開するステップと、
   展開された前記最小コストのノードに対する移動方向を判定するステップと、
   前記移動方向に応じて、前記最小コストのノードからの展開方向の数を設定するステップと、を備える経路探索方法。
5. 前記移動方向が斜め方向の場合に、前記展開方向の数を５とし、
   前記移動方向が縦方向、又は横方向の場合に、前記展開方向の数を３とすることを特徴とする請求項４に記載の経路探索方法。
6. 前記移動方向が縦方向、又は横方向の場合に、前記移動方向と、前記移動方向の両隣の斜め方向と、が前記展開方向となり、
   前記移動方向が斜め方向の場合に、前記移動方向と、前記移動方向の隣の縦方向と、前記移動方向の隣の横方向と、前記縦方向の隣の斜め方向と、前記横方向の隣の斜め方向と、が前記展開方向となることを特徴とする請求項４、又は５に記載の経路探索方法。
7. グリッドで表された領域に含まれる始点から終点までの経路のうち、そのコストが最小のものを選択する経路探索プログラムであって、
   コンピュータに対して、
   最小コストのノードを展開させるステップと、
   展開された前記最小コストのノードに対する移動方向を判定するステップと、
   前記移動方向に応じて、前記最小コストのノードからの展開方向の数を設定するステップと、を備える経路探索方法。
8. 前記移動方向が斜め方向の場合に、前記展開方向の数を５とし、
   前記移動方向が縦方向、又は横方向の場合に、前記展開方向の数を３とすることを特徴とする請求項７に記載の経路探索プログラム。
9. 前記移動方向が縦方向、又は横方向の場合に、前記移動方向と、前記移動方向の両隣の斜め方向と、が展開方向となり、
   前記移動方向が斜め方向の場合に、前記移動方向と、前記移動方向の隣の縦方向と、前記移動方向の隣の横方向と、前記縦方向の隣の斜め方向と、前記横方向の隣の斜め方向とが展開方向となることを特徴とする請求項７、又は８に記載の経路探索プログラム。
10. 請求項１乃至３のいずれか１項に記載の経路探索システムを有する移動体。

Images