(57)【要約】【課題】無駄な経路候補を除いて経路探索を行うこと。【解決手段】経路探索システムは、最小コストのノードからの複数の展開方向に応じた複数のグリッドに対してコストを計算して、最小コストのノードを選択して移動し、該移動後のノードを最小コストのノードとして設定して展開していく処理を繰り返すノード展開部と、最小コストのノードからの展開先の各ノードの展開方向の数を設定する設定部と、を備える。設定部は、展開先のノードが、展開方向の数が３に設定された最小コストのノードに対する、移動元の隣接ノードからの移動方向の延長線上にあると判定した場合に、該展開先のノードの展開方向の数を３に設定し、展開先のノードが、展開方向の数が３に設定された最小コストのノードに対する、移動元の隣接ノードからの移動方向の延長線上にないと判定した場合に、該展開先のノードの展開方向の数を２に設定する。【選択図】図３
2017037468.tif 000002
【特許請求の範囲】
【請求項１】
グリッドで表された領域に含まれる始点から終点までの経路のうち、そのコストが最小のものを選択する経路探索システムであって、
最小コストのノードからの複数の展開方向に応じた複数のグリッドに対してコストを計算して、最小コストのノードを選択して移動し、さらに該移動後のノードを最小コストのノードとして設定して展開していく処理を繰り返すノード展開部と、
前記最小コストのノードからの展開先の各ノードの展開方向の数を設定する設定部と、を備え、
前記設定部は、
前記展開先のノードが、前記展開方向の数が３に設定された前記最小コストのノードに対する、移動元の隣接ノードからの移動方向の延長線上にあると判定した場合に、該展開先のノードの展開方向の数を３に設定し、
前記展開先のノードが、前記展開方向の数が３に設定された前記最小コストのノードに対する、移動元の隣接ノードからの移動方向の延長線上にないと判定した場合に、該展開先のノードの展開方向の数を２に設定する、
ことを特徴とする経路探索システム。
【請求項２】
請求項１記載の経路探索システムであって、
障害物が存在するグリッドに隣接するノードで前記展開方向の数を増加させる、ことを特徴とする経路探索システム。
【請求項３】
請求項２記載の経路探索システムであって、
前記設定部は、
前記障害物のグリッドに隣接する前記最小コストのノードに対して、前記障害物のグリッドの縦又は横方向に隣接する移動元の隣接ノードから移動したと判定した場合に、該最小コストのノードの展開方向の数を５に設定し、
前記障害物のグリッドに隣接する前記最小コストのノードに対して、前記障害物のグリッドの斜め方向に隣接する移動元の隣接ノード、又は、障害物のグリッドに隣接しない移動元の隣接ノードから移動したと判定した場合に、前記最小コストのノードの展開方向の数を４に設定し、
前記展開方向の数が４あるいは５に設定された最小コストのノードからの展開先のノードの展開方向の数を３に設定する、
ことを特徴とする経路探索システム。
【請求項４】
請求項１乃至３のうちいずれか１項記載の経路探索システムであって、
前記ノード展開部は、前記設定部により展開方向の数を２と設定された最小コストのノードを、該最小コストのノードに対する隣接ノードからの移動方向、および、前記延長線と平行方向、の２方向に展開する、ことを特徴とする経路探索システム。
【請求項５】
グリッドで表された領域に含まれる始点から終点までの経路のうち、そのコストが最小のものを選択する経路探索方法であって、
最小コストのノードからの複数の展開方向に応じた複数のグリッドに対してコストを計算して、最小コストのノードを選択して移動し、さらに該移動後のノードを最小コストのノードとして設定して展開していく処理を繰り返すステップと、
前記最小コストのノードからの展開先の各ノードの展開方向の数を設定するステップと、を含み、
前記展開先のノードが、前記展開方向の数が３に設定された前記最小コストのノードに対する、移動元の隣接ノードからの移動方向の延長線上にあると判定した場合に、該展開先のノードの展開方向の数を３に設定し、
前記展開先のノードが、前記展開方向の数が３に設定された前記最小コストのノードに対する、移動元の隣接ノードからの移動方向の延長線上にないと判定した場合に、該展開先のノードの展開方向の数を２に設定する、
ことを特徴とする経路探索方法。
【請求項６】
グリッドで表された領域に含まれる始点から終点までの経路のうち、そのコストが最小のものを選択するプログラムであって、
最小コストのノードからの複数の展開方向に応じた複数のグリッドに対してコストを計算して、最小コストのノードを選択して移動し、さらに該移動後のノードを最小コストのノードとして設定して展開していく処理を繰り返す処理と、
前記展開先のノードが、前記展開方向の数が３に設定された前記最小コストのノードに対する、移動元の隣接ノードからの移動方向の延長線上にあると判定した場合に、該展開先のノードの展開方向の数を３に設定する処理と、
前記展開先のノードが、前記展開方向の数が３に設定された前記最小コストのノードに対する、移動元の隣接ノードからの移動方向の延長線上にないと判定した場合に、該展開先のノードの展開方向の数を２に設定する処理と、
をコンピュータに実行させる、ことを特徴とするプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、グリッドで表された領域に含まれる始点から終点までの経路のうち、そのコストが最小のものを選択する経路探索システム、経路探索方法及びプログラムに関するものである。
【背景技術】
【０００２】
例えば、最小コストのノードを展開し、展開された最小コストのノードに対する移動方向に応じて最小コストのノードからの展開方向の数を設定する経路探索システムが知られている（特許文献１参照）。この経路探索システムにおいては、移動方向が斜め方向の場合に、展開方向の数を５に設定し、移動方向が縦方向、又は横方向の場合に、展開方向の数を３に設定している。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１１−１２８７５８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
上記経路探索システムにおいては、探索枝を常に５分岐（展開方向の数を５）あるいは３分岐（展開方向の数を３）させて全進行方向を経路探索している。この場合、明らかに最短経路の候補となり得ない無駄な経路候補を含めて経路探索を行うこととなるため、経路探索時間の短時間化に改善の余地が見込まれる。
本発明は、上記の問題点に鑑みてなされたものであり、無駄な経路候補を除いて経路探索を行うことで経路探索時間の短時間化を行うことができる経路探索システム、経路探索方法及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【０００５】
上記目的を達成するための本発明の一態様は、グリッドで表された領域に含まれる始点から終点までの経路のうち、そのコストが最小のものを選択する経路探索システムであって、最小コストのノードからの複数の展開方向に応じた複数のグリッドに対してコストを計算して、最小コストのノードを選択して移動し、さらに該移動後のノードを最小コストのノードとして設定して展開していく処理を繰り返すノード展開部と、前記最小コストのノードからの展開先の各ノードの展開方向の数を設定する設定部と、を備え、前記設定部は、前記展開先のノードが、前記展開方向の数が３に設定された前記最小コストのノードに対する、移動元の隣接ノードからの移動方向の延長線上にあると判定した場合に、該展開先のノードの展開方向の数を３に設定し、前記展開先のノードが、前記展開方向の数が３に設定された前記最小コストのノードに対する、移動元の隣接ノードからの移動方向の延長線上にないと判定した場合に、該展開先のノードの展開方向の数を２に設定する、
ことを特徴とする経路探索システムである。
この一態様において、障害物が存在するグリッドに隣接するノードで前記展開方向の数を増加させてもよい。
この一態様において、前記設定部は、前記障害物のグリッドに隣接する前記最小コストのノードに対して、前記障害物のグリッドの縦又は横方向に隣接する移動元の隣接ノードから移動したと判定した場合に、該最小コストのノードの展開方向の数を５に設定し、  前記障害物のグリッドに隣接する前記最小コストのノードに対して、前記障害物のグリッドの斜め方向に隣接する移動元の隣接ノード、又は、障害物のグリッドに隣接しない移動元の隣接ノードから移動したと判定した場合に、前記最小コストのノードの展開方向の数を４に設定し、前記展開方向の数が４あるいは５に設定された最小コストのノードからの展開先のノードの展開方向の数を３に設定してもよい。
この一態様において、前記ノード展開部は、前記設定部により展開方向の数を２と設定された最小コストのノードを、該最小コストのノードに対する隣接ノードからの移動方向、および、前記延長線と平行方向、の２方向に展開してもよい。
上記目的を達成するための本発明の一態様は、グリッドで表された領域に含まれる始点から終点までの経路のうち、そのコストが最小のものを選択する経路探索方法であって、  最小コストのノードからの複数の展開方向に応じた複数のグリッドに対してコストを計算して、最小コストのノードを選択して移動し、さらに該移動後のノードを最小コストのノードとして設定して展開していく処理を繰り返すステップと、前記最小コストのノードからの展開先の各ノードの展開方向の数を設定するステップと、を含み、前記展開先のノードが、前記展開方向の数が３に設定された前記最小コストのノードに対する、移動元の隣接ノードからの移動方向の延長線上にあると判定した場合に、該展開先のノードの展開方向の数を３に設定し、前記展開先のノードが、前記展開方向の数が３に設定された前記最小コストのノードに対する、移動元の隣接ノードからの移動方向の延長線上にないと判定した場合に、該展開先のノードの展開方向の数を２に設定する、ことを特徴とする経路探索方法であってもよい。
上記目的を達成するための本発明の一態様は、グリッドで表された領域に含まれる始点から終点までの経路のうち、そのコストが最小のものを選択するプログラムであって、  最小コストのノードからの複数の展開方向に応じた複数のグリッドに対してコストを計算して、最小コストのノードを選択して移動し、さらに該移動後のノードを最小コストのノードとして設定して展開していく処理を繰り返す処理と、前記展開先のノードが、前記展開方向の数が３に設定された前記最小コストのノードに対する、移動元の隣接ノードからの移動方向の延長線上にあると判定した場合に、該展開先のノードの展開方向の数を３に設定する処理と、前記展開先のノードが、前記展開方向の数が３に設定された前記最小コストのノードに対する、移動元の隣接ノードからの移動方向の延長線上にないと判定した場合に、該展開先のノードの展開方向の数を２に設定する処理と、をコンピュータに実行させる、ことを特徴とするプログラムであってもよい。
【発明の効果】
【０００６】
本発明によれば、無駄な経路候補を除いて経路探索を行うことで経路探索時間の短時間化を行うことができる経路探索システム、経路探索方法及びプログラムを提供することができる。
【図面の簡単な説明】
【０００７】
【図１】ロボットの構成を模式的に示す外観図である。
【図２】ロボットの制御部を示すブロック図である。
【図３】（ａ）乃至（ｄ）ノード展開部により展開された最小コストノードの一例を示す図である。
【図４】実施形態１に係る経路探索方法のフローの一例を示すフローチャートである。
【図５】（ａ）乃至（ｋ）障害物が存在する進入禁止グリッドに隣接するノードで展開方向の数を増加させた状態を示す図である。
【図６】実施形態２に係る経路探索方法のフローの一例を示すフローチャートである。
【図７】３次元座標において探索枝をＸＹＺ軸方向に分岐させた図である。
【発明を実施するための形態】
【０００８】
実施形態１
以下、図面を参照して本発明の実施形態について説明する。但し、本発明が以下の実施形態に限定される訳ではない。また、説明を明確にするため、以下の記載及び図面は、適宜、簡略化されている。
【０００９】
まず、本発明に係る移動体の一例であるロボットの構成について、図１を用いて説明する。図１はロボット１００の構成を模式的に示す外観図である。本実施形態１では、ロボット１００が、自律移動する移動ロボットとして説明する。ロボット１００は、車輪２と、筐体３と、センサ５と、を備えている。そして、筐体３の内部には、車輪２と接続されたモータ、及びモータを駆動するためのバッテリなどが設けられている。このモータがロボット１００を駆動するための駆動機構となる。モータを駆動することによって、車輪２が回転して、ロボット１００が移動する。ロボット１００は、例えば、人間の歩行速度と同程度の速度で移動する。さらに、頭部１には、ＣＣＤカメラやレーザセンサなどを有するセンサ５が設けられている。センサ５はロボット１００に周囲に存在する障害物や人間などを検知する。
【００１０】
ロボット１００には、制御部１１０が設けられている。制御部１１０は、ＣＰＵ（Central Processing Unit）、ＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）、通信用のインタフェイスなどを有する演算処理装置である。また、制御部１１０は、着脱可能なＨＤＤ、光ディスク、光磁気ディスク等を有し、各種プログラムや制御パラメータなどを記憶し、そのプログラムやデータを必要に応じてメモリ（不図示）等に供給する。もちろん、制御部１１０は、物理的に一つの構成に限られるものではない。制御部１１０は、ロボット１００が移動する移動経路を生成する。そして、その移動経路に沿ってロボット１００が移動するよう、車輪２を駆動するためのモータ等を制御する。もちろん、ロボット１００は車輪型の移動ロボットに限らず、歩行型やその他の移動ロボットでもよい。ロボット１００は自己位置推定を行なって移動する移動体であってもよい。
【００１１】
制御部１１０は、移動環境中にある移動始点から移動終点までの移動経路を決定する。ロボット１００は、移動始点から移動を開始する。そして、移動経路に沿って移動していき、移動終点まで移動したら停止する。すなわち、制御部１１０は、移動始点から移動終点までの移動経路を探索する経路探索システムとして機能する。本実施形態１では、制御部１１０は、ダイクストラ法を用いて、経路探索を行っているが、これに限定されない。制御部１１０は、例えば、Ａ＊、Ｔｈｅｔａ＊などの任意の最短経路探索アルゴリズムを用いることができる。
【００１２】
次に、ロボット１００の制御系について図２を用いて説明する。図２は、ロボット１００の制御部１１０を示すブロック図である。制御部１１０は、ロボット１００が移動する経路を決定するための演算処理を実行する。図２に示すように、制御部１１０は、地図情報記憶部１１１と、ノード展開部１１２と、設定部１１３と、を備えている。
【００１３】
地図情報記憶部１１１は、ロボット１００が移動する移動領域中の地図情報を記憶している。例えば、移動する環境中に存在する壁、机などの障害物の座標を記憶している。そして、地図情報記憶部１１１は、移動領域を２次元のグリッドによって表現している。すなわち、地図情報記憶部１１１は、移動領域を縦横のグリッド線で分割して、格子状のグリッドによって表している。さらに、地図情報には、障害物などの情報に基づいて、進入禁止グリッドが設定されている。すなわち、障害物がある位置には、ロボット１００が移動することができない。このため、その障害物の位置に対応するグリッドは、進入禁止グリッドとなる。
【００１４】
従って、地図情報記憶部１１１には、進入禁止グリッドと進入可能グリッドとから構成されるグリッドマップが記憶される。グリッドサイズは、移動領域の大きさやコンピュータの処理速度に応じて決定される。経路探索では、ロボット１００の移動始点に対応するグリッドから移動終点に対応するグリッドまでの最適経路が探索される。すなわち、グリッドで表された領域に含まれる始点から終点までの経路のうち、そのコストが最小のものを選択する。
【００１５】
グリッドは、図３（ａ）乃至（ｄ）に示すように、縦横に区切られている。このため、ある任意の一つのグリッド（たとえば、９つのグリッドの中央のグリッド）は、上下（縦方向）左右（横方向）の４つのグリッドと、それらの間の斜め方向の４つのグリッドと隣接する。すなわち、移動領域の端以外のグリッドでは、上方向、下方向、左方向、右方向、右斜め上方向、右斜め下方向、左斜め下方向、左斜め上方向の８近傍のグリッドが隣接していることになる。
【００１６】
ノード展開部１１２は、コスト計算を行って、最小コストのノード（以下、最小コストノード）を展開していく。ここで、ノードとは、グリッドに対応するものである。従って、１つのグリッドが１つのノードとなる。ノード展開部１１２は、コスト計算の結果、最小コストノードを順番に展開していく。ノード展開部１１２は、最小コストのノードからの複数の展開方向に応じた複数のグリッドに対してコストを計算して、最小コストのノードを選択して移動し、さらに該移動後のノードを最小コストのノードとして設定して展開していく処理を繰り返す。
【００１７】
このように、ノード展開部１１２は、コストが最小となるパスが決定したノードを最小コストノードとして、順番に展開していく。コストが最小となるパスが決定していないノードの中で、コスト計算の結果によってコストが最小となったノードが、最小コストノードとしてデータベースなどに順次追加されていく。そして、最小コストノードを追加していくことで、最小コストノードが展開されていく。
ノード展開部１１２は、例えば、最小コストノードからの展開方向を３方向とする枝刈り探索を用いて、コスト計算を行っている。
【００１８】
図３（ａ）乃至（ｄ）は、ノード展開部により展開された最小コストノードの一例を示す図である。図３（ａ）乃至（ｄ）において、各ノードに記載された数値は、各ノードでノード展開部１１２により展開された展開方向の数（探索枝の分岐数）である。例えば、図３（ａ）に示す如く、Ｄ１からＤ２に移動し、Ｄ２で３分岐してＤ２→Ｃ３、Ｄ２→Ｄ３、及びＤ２→Ｅ３に移動する。
【００１９】
ところで、従来の経路探索システムにおいては、最小コストノードの探索枝を常に５分岐（展開方向の数を５）あるいは３分岐（展開方向の数を３）させて全進行方向を経路探索している。この場合、明らかに最短経路の候補となり得ない無駄な経路候補を含めて経路探索を行うこととなるため、経路探索時間の短時間化に改善の余地が見込まれる。
【００２０】
例えば、図３（ａ）において、Ｄ１からＢ３へ到達する経路では、Ｄ１→Ｃ２→Ｂ３の経路の方が、Ｄ１→Ｄ２→Ｃ３→Ｂ３の経路よりも短くなる。このため、Ｄ１→Ｄ２→Ｃ３→Ｂ３の経路、すなわちＣ３→Ｂ３の経路は、最短経路の候補となり得ない無駄な経路候補となる。
同様に、Ｄ１からＡ４へ到達する経路では、Ｄ１→Ｃ２→Ｂ３→Ａ４の経路の方が、Ｄ１→Ｄ２→Ｃ３→Ｂ４→Ａ４の経路よりも短くなる。このため、Ｄ１→Ｄ２→Ｃ３→Ｂ４→Ａ４の経路、すなわちＢ４→Ａ４の経路は、最短経路の候補となり得ない無駄な経路候補となる。
【００２１】
以上により、例えば、図３（ａ）乃至（ｄ）に示す如く、展開方向の数が３に設定された最小コストノードに対する、隣接ノードからの移動方向の延長線（中央の探索枝）上では、展開先のノードの展開方向の数を３に設定する。この場合、最小コストノードから、左斜め前方、前方（延長線の方向）、及び右斜め前方の３方向（３分岐）に展開する。
【００２２】
一方、展開方向の数が３に設定された最小コストノードに対する、隣接ノードからの移動方向の延長線（中央の探索枝）上以外（中央の探索枝の両側枝上）では、展開先のノードの展開方向の数を２に設定する。この場合、最小コストノードから、前方（最小コストノードに対する隣接ノードからの移動方向）及び中央の探索枝（延長線）と平行方向の２方向（２分岐）にのみ展開する。一旦中央の探索枝から分岐するとその先で、元の中央の探索枝に戻る方向に分岐することは最短経路としてあり得ない分岐であるため、この前方及び中央の探索枝と平行方向の２方向としている。すなわち、展開方向の数が２に設定されたノードから分岐したノードには、展開方向の数２が設定される。このように、探索枝を３分岐から２分岐に減少させることで、その経路探索の計算量を略２／３に減少させることができる。
【００２３】
本実施形態１に係る経路探索システムにおいては、展開先のノードが、展開方向の数が３に設定された前記最小コストのノードに対する、移動元の隣接ノードからの移動方向の延長線（中央の探索枝）上にあると判定した場合に、該展開先のノードの展開方向の数を３に設定する。また、展開先のノードが、展開方向の数が３に設定された最小コストのノードに対する、移動元の隣接ノードからの移動方向の延長線上にない（両側の探索枝上を移動している）と判定した場合に、該展開先のノードの展開方向の数を２に設定する。
これにより、無駄な経路候補を除いて経路探索を行うことで経路探索時間の短時間化を行うことができる。
【００２４】
設定部１１３は、展開先のノードが、展開方向の数が３に設定された最小コストのノードに対する、移動元の隣接ノードからの移動方向の延長線上にあるか否かを判定することで、該展開先のノードの展開方向の数を設定する。
【００２５】
設定部１１３は、展開先のノードが、展開方向の数が３に設定された最小コストのノードに対する、移動元の隣接ノードからの移動方向の延長線上にあると判定した場合に、該展開先のノードの展開方向の数を３に設定する。
【００２６】
例えば、図３（ｂ）に示す如く、設定部１１３は、展開先のノードＣ２、Ｃ３、Ｃ４が、展開方向の数が３に設定された最小コストのノードＣ２に対する、移動元の隣接ノードＣ１からの移動方向の延長線上にあると判定した場合に、該展開先のノードＣ２、Ｃ３、Ｃ４の展開方向の数を３に設定する。このように、中央の探索枝（展開方向の数が３に設定された最小コストノードに対する、隣接ノードからの移動方向の延長線）上のノードには、展開方向の数３が設定される。
【００２７】
一方、設定部１１３は、展開先のノードが、展開方向の数が３に設定された最小コストのノードに対する、移動元の隣接ノードからの移動方向の延長線上にないと判定した場合に、該展開先のノードの展開方向の数を２に設定する。
【００２８】
例えば、図３（ｂ）に示す如く、設定部１１３は、展開先のノードＡ４〜Ａ５、Ｂ３〜Ｂ５、Ｄ３〜Ｄ４が、展開方向の数が３に設定された最小コストのノードＣ２に対する、移動元の隣接ノードＣ１からの移動方向の延長線上にないと判定した場合に、該展開先のノードＡ４〜Ａ５、Ｂ３〜Ｂ５、Ｄ３〜Ｄ４の展開方向の数を２に設定する。このように、両側の探索枝（展開方向の数が３に設定された最小コストのノードに対する、隣接ノードからの移動方向の延長線）上にないノードには、展開方向の数２が設定される。
【００２９】
上述したように本実施形態１では、最小コストノードからの展開方向を８方向から３方向に制限した枝刈り探索を用いて、コスト計算を行っている。さらに、本実施形態１では、上記最短経路の特性を考慮して、最小コストノードからの展開方向を３方向から２方向に減少させている。展開方向によって、コストを新たに算出する展開ノードの数が決まる。これにより、コスト計算を行うノード数をより小さくすることができ、計算時間をより短縮することができる。
【００３０】
ノード展開部１１２は、上記設定された３つの展開方向に応じた３つのノード、あるいは２つの展開方向に応じた２つのノードに対して、コストを算出する。そして、算出したコストに応じて、地図情報のコストマップを更新していく。すなわち、３つ、又は２つのグリッドにおいて、コストが小さいパスが発見された場合、そのグリッドのコストを更新する。このようにしてコストマップを更新する。コストマップを参照して、コストが最小となるパスが決定していないノードの中から、コストが最小のノードを選択する。このノードが新たに、最小コストノードとして追加される。このようにして、最小コストノードを展開していく。
【００３１】
次に、本実施形態１に係る経路探索方法について詳細に説明する。図４は、本実施形態１に係る経路探索方法のフローの一例を示すフローチャートである。
経路探索が開始されると、移動始点のノードから隣接する８方向（上方向、下方向、左方向、右方向、右斜め上方向、右斜め下方向、左斜め下方向、左斜め上方向）のノードに探索枝を分岐させる（ステップＳ１０１）。
【００３２】
ノードから分岐された探索枝の先のノードが、移動終点であるか否かが判定される（ステップＳ１０２）。ノードから分岐された探索枝の先のノードが移動終点であると判定されると（ステップＳ１０２のＹＥＳ）、本経路探索処理は終了する。
一方、ノードから分岐された探索枝の先のノードが移動終点でないと判定されると（ステップＳ１０２のＮＯ）、探索枝の先のノードの中から、最小コストノードが選択される（ステップＳ１０３）。なお、初回に選択された最小コストノードには、展開方向の数３が設定されている。
【００３３】
選択された最小コストノードから展開する際に、展開先のノードが、展開方向の数が３に設定された最小コストのノードに対する、移動元の隣接ノードからの移動方向の延長線上にあるか否かが判定される（ステップＳ１０４）。
展開方向の数が３に設定された最小コストノードに対する、隣接ノードからの移動方向の延長線上にあると判定されたとき（ステップＳ１０４のＹＥＳ）、展開先のノードの展開方向の数は３に設定され（ステップＳ１０５）、上記（ステップＳ１０２）に戻る。
【００３４】
一方、展開方向の数が３に設定された最小コストノードに対する、隣接ノードからの移動方向の延長線上にないと判定されたとき（ステップＳ１０４のＮＯ）、展開先のノードの展開方向の数は２に設定され（ステップＳ１０６）、上記（ステップＳ１０２）に戻る。
【００３５】
以上、本実施形態において、展開先のノードが、展開方向の数が３に設定された前記最小コストのノードに対する、移動元の隣接ノードからの移動方向の延長線上にあると判定した場合に、該展開先のノードの展開方向の数を３に設定する。また、展開先のノードが、展開方向の数が３に設定された最小コストのノードに対する、移動元の隣接ノードからの移動方向の延長線上にないと判定した場合に、該展開先のノードの展開方向の数を２に設定する。
これにより、無駄な経路候補を除いて経路探索を行うことで経路探索時間の短時間化を行うことができる。
【００３６】
実施形態２
本発明の実施形態２において、障害物が存在する進入禁止グリッドに隣接するノードで展開方向の数を増加させる。上述の如く、障害物が存在しない領域のおける最短経路は、基本的に直線的に移動するものであるため、探索枝の先を２分岐に減少させればよい。しかし、障害物が近傍に存在する場合、その障害物を回避するように、例えば、光が散乱、回折するが如く、探索枝の分岐数（展開方向の数）も増加（探索枝の枝増し）させるのが好ましい。
【００３７】
これに対し、本実施形態２において、例えば、設定部１１３は、ノード展開部１１２により最小コストノードから展開される際、障害物の進入禁止グリッドに隣接する最小コストノードに対して、障害物の進入禁止グリッドの縦又は横方向に隣接する移動元の隣接ノードから移動したと判定した場合に、その最小コストノートの展開方向の数を５に設定する。一方、設定部１１３は、ノード展開部１１２により最小コストノードから展開される際、障害物の進入禁止グリッドに隣接する最小コストノードに対して、障害物の進入禁止グリッドの斜め方向に隣接する移動元の隣接ノード、又は、障害物の進入禁止グリッドに隣接しない隣接ノードから移動したと判定した場合に、その最小コストノードの展開方向の数を４に設定する。また、ノード展開部１１２は、上記展開方向の数が４あるいは５に設定された最小コストノードから、この最小コストノードに対する隣接ノードからの移動方向に対し前方に位置し、進入禁止グリッド以外の隣接するノードに展開する。なお、本実施形態２において、上記実施形態１と同一部分に同一符号を付して詳細な説明は省略する。
【００３８】
図５（ａ）乃至（ｋ）は、障害物が存在する進入禁止グリッドに隣接するノードで展開方向の数を増加させた状態を示す図である。なお、図５（ａ）乃至（ｄ）、（ｇ）乃至（ｊ）は、展開先のノードの展開方向の数を４に設定した一例である。図５（ｅ）、（ｆ）、及び（ｋ）は、展開先のノードの展開方向の数を５に設定した一例である。
【００３９】
図５（ｅ）に示す如く、例えば、設定部１１３は、ノード展開部１１２により最小コストノードから展開される際、障害物の進入禁止グリッドＣ３に隣接する最小コストノードＢ２に対して、障害物の進入禁止グリッドＣ３の縦又は横方向に隣接する移動元の隣接ノードＢ３から移動したと判定した場合に、最小コストノードＢ２の展開方向の数を、５に設定する。この場合、ノード展開部１１２は、上記展開方向の数が５に設定された最小コストノードＢ２から、この最小コストノードＢ２に対する隣接ノードＢ３からの移動方向に対し前方に位置し、進入禁止グリッド以外の隣接するノードＡ１、Ａ２、Ｂ１、Ｃ１、Ｃ２に展開する。
【００４０】
一方、図５（ａ）に示す如く、例えば、設定部１１３は、ノード展開部１１２により最小コストノードから展開される際、障害物の進入禁止グリッドＣ３に隣接する最小コストノードＢ２に対して、障害物の進入禁止グリッドに隣接しない隣接ノードＢ１から移動したと判定した場合に、最小コストノードＢ２の展開方向の数を、４に設定する。同様に、図５（ｊ）に示す如く、設定部１１３は、ノード展開部１１２により最小コストノードから展開される際、障害物の進入禁止グリッドＣ３に隣接する最小コストノードＣ２に対して、障害物の進入禁止グリッドＣ３の斜め方向に隣接する移動元の隣接ノードＢ２から移動したと判定した場合に、最小コストノードＣ２の展開方向の数を、４に設定する。この場合、ノード展開部１１２は、上記展開方向の数が４に設定された最小コストノードＢ２から、この最小コストノードＢ２に対する隣接ノードＢ１からの移動方向に対し前方に位置し、進入禁止グリッド以外の隣接するノードＡ２、Ａ３、Ｂ３、Ｃ２に展開する。
【００４１】
上記のように展開方向の数が４あるいは５に設定された最小コストノードから展開された先のノードの展開方向の数は、３に設定される。例えば、図５（ａ）では、展開先のノードＣ２、Ｂ３、Ａ３、Ａ２の展開方向の数は、３に設定される。
【００４２】
図６は、本実施形態２に係る経路探索方法のフローの一例を示すフローチャートである。
経路探索が開示されると、移動始点のグリッドから８方向に探索枝を分岐させる（ステップＳ２０１）。
【００４３】
ノードから分岐された探索枝の先のノードが移動終点であるか否かが判定される（ステップＳ２０２）。ノードから分岐された探索枝の先のノードが移動終点であると判定されると（ステップＳ２０２のＹＥＳ）、本経路探索処理は終了する。
一方、ノードから分岐された探索枝の先のノードが移動終点でないと判定されると（ステップＳ２０２のＮＯ）、探索枝の先のノードの中から、最小コストノードが選択される（ステップＳ２０３）。
【００４４】
選択された最小コストノードから展開する際に、障害物の進入禁止グリッドに隣接する最小コストノードに対して、障害物の進入禁止グリッドの縦又は横方向に隣接する隣接ノードから移動したか否かが判定される（ステップＳ２０４）。
【００４５】
障害物の進入禁止グリッドに隣接する最小コストノードに対して、障害物の進入禁止グリッドの縦又は横方向に隣接する隣接ノードから移動したと判定されると（ステップＳ２０４のＹＥＳ）、その最小コストノードの展開方向の数は５に設定され（ステップＳ２０５）、下記（ステップＳ２１１）に移行する。
【００４６】
障害物の進入禁止グリッドに隣接する最小コストノードに対して、障害物の進入禁止グリッドの縦又は横方向に隣接する隣接ノードから移動しないと判定されると（ステップＳ２０４のＮＯ）、選択された最小コストノードから展開する際に、障害物の進入禁止グリッドに隣接する最小コストノードに対して、障害物の進入禁止グリッドの斜め方向に隣接する隣接ノード、又は、障害物の進入禁止グリッドに隣接しない隣接ノードから移動したか否かが判定される（ステップＳ１０６）。
【００４７】
障害物の進入禁止グリッドに隣接する最小コストノードに対して、障害物の進入禁止グリッドの斜め方向に隣接する隣接ノード、又は、障害物の進入禁止グリッドに隣接しない隣接ノードから移動したと判定されると（ステップＳ２０６のＹＥＳ）、最小コストノードの展開方向の数は４に設定され（ステップＳ２０７）、下記（ステップＳ２１１）に移行する。
【００４８】
障害物の進入禁止グリッドに隣接する最小コストノードに対して、障害物の進入禁止グリッドの斜め方向に隣接する隣接ノード、又は、障害物の進入禁止グリッドに隣接しない隣接ノードから移動していないと判定されると（ステップＳ２０６のＮＯ）、選択された最小コストノードから展開する際に、展開方向の数が３に設定された最小コストノードに対する、移動元の隣接ノードからの移動方向の延長線上にあるか否かが判定される（ステップＳ２０８）。
【００４９】
展開方向の数が３に設定された最小コストノードに対する、移動元の隣接ノードからの移動方向の延長線上にあると判定されたとき（ステップＳ２０８のＹＥＳ）、展開先のノードの展開方向の数は３に設定され（ステップＳ２０９）、上記（ステップＳ２０２）に戻る。
【００５０】
一方、展開方向の数が３に設定された最小コストノードに対する、移動元の隣接ノードからの移動方向の延長線上にないと判定されたとき（ステップＳ２０８のＮＯ）、展開先のノードの展開方向の数は２に設定され（ステップ２１０）、上記（ステップＳ２０２）に戻る。
【００５１】
上記展開方向の数が４あるいは５に設定された最小コストノードから展開された先のノードの展開方向の数は３に設定され（ステップＳ２１１）、上記（ステップＳ２０２）に戻る。
以上、本実施形態２によれば、経路探索時間の短時間化を行いつつ、さらに、障害物が存在する場合でも、その障害物を回避するように経路探索を行うことができる。
【００５２】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
上記実施形態において、経路探索システムは、室内自律移動のように、障害物が多く、複雑な環境下における経路探索に好適である。もちろん、経路探索システムを搭載する移動体は、特に限定されるものではない。例えば、倒立２輪を有するロボット車椅子、歩行ロボット、自律移動型のアシスタントロボットに搭載することもできる。さらに、最小コストでの解の可解性が保護される。また、障害物や人をよけることなども可能になるため、安全な自律移動が可能となる。
【００５３】
上記実施形態において、３次元の経路探索についても適用可能である。３次元の経路探索を行う場合、例えば、移動領域をボクセルで表現できる。すなわち、移動領域を３次元のそれぞれの方向にグリッドで仕切る。これにより、移動領域が３次元グリッドで表現される。そして、ボクセルを平面に分解する。３×３×３のボクセルでは、各平面に９マスのボクセルが含まれる。
【００５４】
３次元座標において、Ｘ軸方向及び／又はＹ軸方向のみに（ＸＹ平面上を）移動した場合、分解したボクセルの各平面に対して、上記２次元座標の場合と同様に経路探索を行えばよい。
さらに、３次元座標において、例えば、図７に示す如くノード（３、０、２）からノード（３、１、２）への探索枝からさらに分岐する（ＸＹＺ軸方向に移動した）場合を想定する。この場合、ノード（３、１、２）から、ノード（２、２、４）、（３、２、４）、（４、２、４）、（２、２、３）、（３、２、３）、（４、２、３）、（２、２、２）、（３、２、２）、（４、２、２）に分岐する。ここで、上記２次元座標系のときと同様に、最短経路の候補となり得ない無駄な経路候補を除外できる。例えば、ノード（３、１、２）からノード（４、２、３）への探索枝からさらに分岐する場合、ノード（５、２、４）、（５、２、３）、（５、２、２）への３分岐は最短経路の候補となり得ない無駄な経路候補であり、除外できる。したがって、ノード（４、２、３）から、次の６つのノード（４、３、４）、（５、３、４）、（４、３、３）、（５、３、３）、（４、３、２）、（５、３、２）に分岐することになる。これは、３次元座標の分岐枝をＸＹ平面に投影して考えると、上記２次元座標と同様に適用できる。
【００５５】
また、本発明は、例えば、図４又は図６に示す処理を、ＣＰＵにコンピュータプログラムを実行させることにより実現することも可能である。
プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。
【００５６】
また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。
【符号の説明】
【００５７】
１  頭部、  ２  車輪、  ３  筐体、  ５  センサ、  １００  ロボット、  １１０  制御部、  １１１  地図情報記憶部、  １１２  ノード展開部、  １１３  設定部
【図１】
2017037468.tif 000003
【図２】
2017037468.tif 000004
【図３】
2017037468.tif 000005
【図４】
2017037468.tif 000006
【図５】
2017037468.tif 000007
【図６】
2017037468.tif 000008
【図７】
2017037468.tif 000009