経路探索システム、経路探索方法、及び自律移動体
【課題】広範な移動空間であっても、高速かつ正確に最適な経路を高速で探索することができる経路探索システム、経路探索方法、及び自律移動体を提供すること。
【解決手段】本発明にかかる経路探索システム１は、移動空間を、領域に含まれる任意の２点間に経路が存在するように、複数の領域に分割し、各領域に対してノードを設定すると共に、各領域間の隣接関係に従ってノード間をリンクで接続することで、ノード及びリンクから構成されるトポロジーマップを生成するトポロジーマップ生成手段２１と、生成されたトポロジーマップにおいて、経路を探索するトポロジーマップ経路探索手段２２と、トポロジーマップ経路探索手段２１により探索されたトポロジーマップ上の経路に対応する移動空間の領域において、移動始点Ｓより移動終点Ｇに到達する経路を探索する移動空間経路探索手段２３とを備えるものである。
【解決手段】本発明にかかる経路探索システム１は、移動空間を、領域に含まれる任意の２点間に経路が存在するように、複数の領域に分割し、各領域に対してノードを設定すると共に、各領域間の隣接関係に従ってノード間をリンクで接続することで、ノード及びリンクから構成されるトポロジーマップを生成するトポロジーマップ生成手段２１と、生成されたトポロジーマップにおいて、経路を探索するトポロジーマップ経路探索手段２２と、トポロジーマップ経路探索手段２１により探索されたトポロジーマップ上の経路に対応する移動空間の領域において、移動始点Ｓより移動終点Ｇに到達する経路を探索する移動空間経路探索手段２３とを備えるものである。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は移動経路を探索する経路探索システム、経路探索方法、及び自律移動体に関する。
【背景技術】
【０００２】
環境を自律的に移動する移動体は、予め又はリアルタイムに移動経路を探索する必要がある。従来より、多くの経路探索方法が提案されている。一般的な経路探索方法として、ダイクストラ、A＊などの手法が良く知られている。このような手法により経路探索を行う場合には、例えば、等間隔のグリッドをマップとして保持し、障害物をそのグリッドにプロットしたグリッドマップを用いて、各グリッドセルをノードとして移動始点から移動終点まで経路探索を行う技術が良く知られている。
【０００３】
しかしながら、グリッドマップ上において経路探索を行う従来技術では、グリッドを用いて広い範囲において経路探索を行う場合には、移動空間に応じてグリッドマップが大きくなるため、計算負荷やメモリ容量の増大を招き、リアルタイムで経路探索を実行することができないという問題がある。
【０００４】
そこで、広範な移動空間において経路探索を行う場合に、移動空間の特徴をノード及びリンクにより表現して、それらノード及びリンクから構成される幾何学的なトポロジーマップを用いて経路探索を行う技術が開示されている（例えば特許文献１、２）。
【特許文献１】特開２０００−１８１５３９号公報
【特許文献２】特開２００７−１５５６９９号公報
【発明の開示】
【発明が解決しようとする課題】
【０００５】
しかしながら、従来では、トポロジーマップを生成するため、移動空間に存在する例えば出入り口や、棚や、机などの特徴を人間が判断して抽出を行っており、広範な移動空間においては、手間の増加や、人により抽出される特徴が相違するなどの問題がある。更には、広範で複雑な移動空間においては、特徴点間に経路が存在しないような特徴点を間違って抽出する場合も想定され、かかる場合には、経路探索が実行不可能となる虞がある。即ち、従来の人手によるトポロジーマップの生成方法では、特徴点を自動的に抽出して、経路探索を正確に実行することができないという問題がある。
【０００６】
このように、従来の移動経路探索方法では人手により生成したトポロジーマップを用いて経路探索処理を行うため、広範な移動空間において、高速かつ正確に最適な経路を探索することができないという問題があった。
【０００７】
本発明は、かかる課題を解決するためになされたものであり、広範な移動空間であっても、高速かつ正確に最適な経路を探索することができる経路探索システム、経路探索方法、及び自律移動体を提供することを目的とする。
【課題を解決するための手段】
【０００８】
本発明にかかる経路探索システムは、移動空間内に存在する移動始点より移動を開始し、前記移動空間内に存在する移動終点に到達する経路を探索する経路探索システムであって、前記移動空間を、領域に含まれる任意の２点間に経路が存在するように、複数の領域に分割し、前記各領域に対してノードを設定すると共に、前記各領域間の隣接関係に従って前記ノード間をリンクで接続することで、ノード及びリンクから構成されるトポロジーマップを生成するトポロジーマップ生成手段と、生成された前記トポロジーマップにおいて、経路を探索するトポロジーマップ経路探索手段と、前記トポロジーマップ経路探索手段により探索されたトポロジーマップ上の経路に対応する前記移動空間の領域において、経路を探索する移動空間経路探索手段とを備えるものである。
【０００９】
これにより、各領域に含まれる任意の２点間には経路が存在するため、各ノードに対応する移動空間の各領域においても任意の２点間には経路が存在する。そして、各領域間の隣接関係に従ってノード間をリンクで接続するため、リンクで接続された複数のノードについて、それら複数のノードに対応する移動空間の複数の領域にわたって任意の２点間に経路が存在する。このため、トポロジーマップにおいて探索されたトポロジーマップ上の経路の経由ノードについて、それら経由ノードに対応する移動空間の限定された領域にわたっても任意の２点間に経路が存在する。即ち、移動空間の限定された領域にわたって、移動始点から移動終点への経路を正確に探索することができる。従って、このようなトポロジーマップを自動で生成して、トポロジーマップにおける経路探索結果を用いることで、移動空間において限定された領域において経路探索を行うことができるため、広範な移動空間においても、高速かつ正確に経路を生成することができる。
【００１０】
また、前記トポロジーマップ生成手段は、前記移動空間に含まれる複数の基点について、前記各基点から探索枝を所定の本数となるまで延ばして、前記探索枝により接続される点からなる領域をひとつの領域とすることで、前記移動空間を複数の領域として分割する領域分割手段と、前記各領域に対してノードを設定すると共に、前記各領域間の隣接関係に従って前記ノード間をリンクで接続するノード・リンク設定手段とを備えるようにしてもよい。これにより、移動空間を、各領域において任意の２点間に経路が存在するような複数の領域に簡単に分割することができる。
【００１１】
さらにまた、前記各基点から延ばす前記探索枝の本数を設定可能とするようにしてもよい。これにより、分割された各領域に含まれる点の個数をそれぞれ略同じ個数とすることで、移動空間を略均一に分割することができる。従って、移動空間を一様に圧縮したノードによって表現されるトポロジーマップを容易に生成することができる。
【００１２】
また、前記ノード・リンク設定手段が、前記各領域の重心に位置する代表点前記ノードとして設定するようにしてもよい。これにより、分割された各領域から、簡単にトポロジーマップのノードを設定することができる。
【００１３】
さらにまた、前記ノード・リンク設定手段が、前記代表点間の幾何学的な距離を前記リンクのコストとし、前記各領域間の隣接関係に従って前記ノード間をリンクで接続するようにしてもよい。これにより、移動空間における各領域の幾何学的な位置関係を保持したトポロジーマップを容易に生成することができる。
【００１４】
また、トポロジーマップ経路探索手段及び／又は移動空間経路探索手段が、ダイクストラ法を用いて経路を探索するようにしてもよい。これにより、容易かつ高速に経路を探索することができる。
【００１５】
本発明にかかる経路探索方法は、探索空間内に存在する探索開始点より探索を開始し、前記探索空間内に存在する探索終点に到達する経路を探索する経路探索方法であって、前記探索空間を、領域に含まれる任意の２点間に経路が存在するように、複数の領域に分割するステップと、分割された前記各領域に対してノードを設定すると共に、前記各領域間の隣接関係に従って前記ノード間をリンクで接続することでノード及びリンクを設定するステップと、前記ノード及びリンクからなるトポロジーマップを用いて、経路を探索するステップと、前記トポロジーマップ経路探索手段により探索されたトポロジーマップ上の経路に対応する前記探索空間の領域において、経路を探索するステップとを備えるものである。
【００１６】
このようにすれば、トポロジーマップにおいて探索されたトポロジーマップ上の経路の経由ノードについて、それら経由ノードに対応する探索空間の限定された領域にわたって任意の２点間に経路が存在する。即ち、探索空間の限定された領域にわたって、探索始点から探索終点への経路を正確に探索することができる。従って、このようなトポロジーマップを自動で生成して、トポロジーマップにおける経路探索結果を用いることで、探索空間において限定された領域において経路探索を行うことができるため、広範な探索空間においても、高速かつ正確に経路を生成することができる。
【００１７】
また、前記探索空間を複数の領域に分割するステップでは、前記探索空間に含まれる複数の各基点から探索枝を所定の本数となるまで延ばして、前記探索空間を、前記各基点から前記探索枝により接続される点からなる複数の領域として分割するようにしてもよい。
【００１８】
さらにまた、前記各基点から延ばす前記探索枝の本数を設定可能とするようにしてもよい。また、前記ノード及びリンクを設定するステップでは、前記各領域の重心に位置する代表点を前記ノードとして設定すると共に、前記各領域間の隣接関係に従って前記ノード間をリンクで接続するようにしてもよい。
【００１９】
また、前記ノード及びリンクを設定するステップでは、前記代表点間の幾何学的な距離を前記リンクのコストとし、前記各領域間の隣接関係に従って前記ノード間をリンクで接続するようにしてもよい。さらにまた、前記経路を探索するステップでは、ダイクストラ法を用いて経路を探索するようにしてもよい。
【００２０】
本発明にかかる自律移動体は、移動空間内に存在する移動始点より移動を開始し、前記移動空間内に存在する移動終点に到達する経路を探索可能な自律移動体であって、前記移動空間を、領域に含まれる任意の２点間に経路が存在するように、複数の領域に分割する領域分割手段と、分割された前記各領域に対してノードを設定すると共に、前記各領域間の隣接関係に従って前記ノード間をリンクで接続することでノード及びリンクを設定するノード・リンク設定手段と、前記ノード及びリンクからなるトポロジーマップを用いて、経路を探索するトポロジーマップ経路探索手段と、前記トポロジーマップ経路探索手段により探索されたトポロジーマップ上の経路に対応する前記移動空間の領域において、経路を探索する移動空間経路探索手段とを備えるものである。
【００２１】
これにより、移動空間の限定された領域にわたって、移動始点から移動終点への経路を正確に探索することができる。従って、このようなトポロジーマップを自動で生成して、トポロジーマップにおける経路探索結果を用いることで、移動空間において限定された領域において経路探索を行うことができるため、広範な移動空間においても、高速かつ正確に経路を生成することができる。
【発明の効果】
【００２２】
本発明によれば、広範な移動空間であっても、高速かつ正確に最適な経路を探索することができる経路探索システム、経路探索方法、及び自律移動体を提供することができる。
【発明を実施するための最良の形態】
【００２３】
以下、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。尚、各図面において、同一要素には同一の符号を付しており、説明の明確化のため、必要に応じて重複説明を省略する。
【００２４】
発明の実施の形態１．
本発明の実施の形態１にかかる経路探索システムでは、自律移動体としてロボットを用いている。ロボットは、例えば、それぞれ独立して回転制御される二つの車輪を備えたものでもよく、２輪よりも多い、例えば４輪走行のロボットや、１輪走行のロボット、２足歩行・４足歩行のロボットであってもよい。また、ロボットに限らず、２輪走行や４輪走行の車両にも適用できる。本例におけるロボットは、障害物を検出するセンサやカメラを備え、検出された障害物に応じて、これを避けるように経路探索を行なう。
【００２５】
当該経路探索システムは、移動空間内に存在する移動始点より移動を開始し、移動空間内に存在する移動終点に到達する経路を探索する。経路探索システムは、ノード及びリンクから構成されるトポロジーマップを用いて経路を探索するため、移動空間に基づいてトポロジーマップを生成する。経路探索システムは、まず、移動空間を、領域に含まれる任意の２点間に経路が存在するように、複数の領域に分割する。次いで、各領域に対してノードを設定すると共に、各領域間の隣接関係に従ってノード間をリンクで接続することで、ノード及びリンクから構成されるトポロジーマップを生成する。そして、経路探索システムは、生成されたトポロジーマップにおいて経路を探索した上で、その探索されたトポロジーマップ上の経路に対応する移動空間の領域において経路を探索する。
【００２６】
後述するように、探索されたトポロジーマップ上の経路の経由ノードに対応する移動空間の限定された領域にわたって、任意の２点間に経路が存在する。このため、移動空間の限定された領域にわたって、移動始点から移動終点への経路を正確に探索することができる。従って、このようなトポロジーマップを自動で生成して、トポロジーマップにおける経路探索結果を用いることで、移動空間全体ではなく限定された領域において経路探索を行うことができるため、広範な移動空間においても、高速かつ正確に経路を生成することができる。
【００２７】
本実施の形態１にかかる経路探索システムは、例えば、ロボットに搭載されたコンピュータやロボットとは別に設けられたコンピュータにより実現される。このコンピュータは、例えば、中央処理装置（ＣＰＵ）、ＲＯＭ、ＲＡＭ、ハードディスク等の補助記憶装置、ＣＤ−ＲＯＭ等の可搬型記憶媒体が挿入される記憶媒体駆動装置、入力手段や出力手段を備えている。ＲＯＭ、補助記憶装置、可搬型記憶媒体等の記憶媒体には、オペレーティングシステムと協働してＣＰＵ等に命令を与え、アプリケーションプログラムを記録することができる。アプリケーションプログラムはＲＡＭにロードされることによって実行される。このアプリケーションプログラムは、本発明にかかる経路探索システムを実現する特有の経路探索プログラムを含む。経路探索システムによる経路探索は、中央処理装置がアプリケーションプログラムをＲＡＭ上に展開した上で当該アプリケーションプログラムに従った処理を補助記憶装置に格納されたデータを読み出し、また格納を行なうことにより、実行される。
【００２８】
図１は、本実施の形態１にかかる経路探索システムの概略構成を示すブロック図である。図１に示されるように、経路探索システム１は、経路探索処理部２と経路探索記憶部３により構成されている。経路探索処理部２は、トポロジーマップ生成手段２１、トポロジーマップ経路探索手段２２、及び移動空間経路探索手段２３を備えている。
【００２９】
経路探索記憶部３には、移動空間を表現するためのグリッドマップ及びトポロジーマップが記憶されている。グリッドマップは、ロボットが移動する移動領域全体の形状に、略一定間隔ｄ（例えば１０ｃｍ）に配置された格子点を結ぶグリッド線を仮想的に描写することで得られる。ロボットは、ＧＰＳ等から得られた位置情報を、このグリッドマップ上における自己の位置に置き換えて、グリッドマップ上における自己位置を認識する。グリッドマップ上において、ロボットの自己位置に相当する場所、および目的地である移動終点、および移動終点におけるロボットの移動方向が特定される。ロボットは、グリッドマップ上において特定された自己位置を移動始点から、目的地である移動終点までの移動経路を作成し、作成された移動経路に従って移動を行う。
【００３０】
トポロジーマップは、ロボットが移動する移動空間を抽象化して表現するための幾何学的マップであり、ノード及びリンクから構成される。本実施の形態１における経路探索システムでは、グリッドマップに基づいて、後述するトポロジーマップ生成手段２１によりトポロジーマップを生成する。
【００３１】
トポロジーマップ生成手段２１は、グリッドマップに基づいて、トポロジーマップを生成する。より具体的には、グリッドマップを、領域に含まれる任意の２点間に経路が存在するように、複数の領域に分割する領域分割する。そして、各領域に対してノードを設定すると共に、各領域間の隣接関係に従ってノード間をリンクで接続することで、ノード及びリンクから構成されるトポロジーマップを生成する。
【００３２】
トポロジーマップ生成手段２１は、領域分割手段２１１及びノード・リンク設定手段２１２を備えている。領域分割手段２１１は、グリッドマップに含まれる複数の各基点から探索枝を所定の本数となるまで延ばして、各基点から探索枝により接続される点からなる領域をひとつの領域とすることで、グリッドマップを複数の領域として分割する。ノード・リンク設定手段２１２は、各領域に対してノードを設定すると共に、各領域間の隣接関係に従ってノード間をリンクで接続する。尚、トポロジーマップ生成処理の詳細については後述する。
【００３３】
トポロジーマップ経路探索手段２２は、トポロジーマップ生成手段２１により生成されたトポロジーマップにおいて、経路を探索する。また、移動空間経路探索手段２３は、トポロジーマップ経路探索手段２２により探索されたトポロジーマップ上の経路に対応するグリッドマップ上の領域において、経路を探索する。
【００３４】
図２は、本実施の形態１にかかる経路探索システム１による、経路探索処理の概要を示すフローチャートである。まず、経路探索システム１は、トポロジーマップ生成手段２１により、グリッドマップに基づいて、ノード及びリンクからなるトポロジーマップを生成する（ステップＳ１０１）。次いで、生成されたトポロジーマップを用いて、トポロジーマップ経路探索手段２２により、トポロジーマップ上において、移動始点に対応するノードから移動終点へと対応するノードへと至る経路を探索する（ステップＳ１０２）。次いで、移動空間経路探索手段２３により、トポロジーマップ経路探索手段２２により探索されたトポロジーマップ上の経由ノードに対応するグリッドマップ上の領域において、移動始点から移動終点へと至る経路を探索する（ステップＳ１０３）。以下、各ステップにおける処理について詳細に説明する。
【００３５】
まず、トポロジーマップ生成手段２１によるトポロジーマップ生成処理について説明する。図３乃至図８は、トポロジーマップ生成手段２１によるトポロジーマップ生成処理を説明するための図である。図３は、トポロジーマップ生成処理における、領域分割手段２１１による領域分割処理を示すフローチャートである。
【００３６】
領域分割手段２１１は、トポロジーマップと比較して詳細な分解能を有するグリッドマップを、一定の面積を有する複数の領域に分割する。分割された各領域に対して固有の領域番号を割り当てるものとする。まず、領域分割手段２１１は、領域を分割するために用いる基点（種ともいう。）をグリッドマップに設定する（ステップＳ２０１）。基点の設定方法は、詳細なグリッドマップ内を走査して、領域番号が割り当てられていないグリッド点を含む未分類領域があるか否かを判定し、未分類領域がある場合には、そのグリッド点を新たな基点（種）として設定する。基点（種）に対して、新たな領域番号を設定する。
【００３７】
次いで、ステップＳ２０１において設定された基点（種）から、グリッド点間の隣接関係に基づいて、探索枝を延ばす（ステップＳ２０２）。探索枝を延ばす処理は最短経路探索処理と同様の処理を行うものとし、領域番号を持つ基点（種）を開始点として、最小経路となる探索枝を順次延長してゆく。より詳細には、基点に対応するグリッド点から、隣接するグリッド点間の評価値のうち最も小さい評価値を有する隣接グリッド点を探索枝に順次選択してゆくことで、選択されたグリッド点を通過する探索枝を最短経路として決定することができる。隣接するグリッド点間の評価値としては、例えば、前後左右方向に隣接するグリッド間の評価値を５とし、斜め方向に隣接するグリッド間の評価値を７などとする。また、基点から延びた探索枝に含まれるグリッド点は、基点と同じ領域番号を持つものとする。このように経路探索手法を利用して領域を分割するため、一つの領域内の任意の二点間に経路が存在することが保証される。
【００３８】
次いで、探索枝に含まれるグリッド点が、他の領域番号を持つ領域と隣接するか否かを判定する（ステップＳ２０３）。グリッド点が、他の領域と隣接する場合には、その隣接領域の情報を基点に対応付けして、後述する隣接領域情報として経路探索記憶部３に記憶する（ステップＳ２０４）。後述するように、記憶された隣接領域情報を用いて、ノード・リンク設定処理においてノード間にリンクが存在するか否かの判定を行う。
【００３９】
次いで、探索枝を延ばしている領域番号の領域について、探索枝が指定本数分まで延びているか、又は、探索枝をこれ以上は増やすことができないか否かを判定する（ステップＳ２０５）。探索枝が指定数分だけ延びている、又は、これ以上増やすことができない場合には、新しい領域番号を、探索枝により探索されたグリッド点に対して割り当てる（ステップＳ２０６）。
【００４０】
このように、各基点から延ばす探索枝の本数を予め設定し、分割された各領域に含まれるグリッド点の個数をそれぞれ略同じ個数とすることで、グリッドマップを略均一に分割することができる。このため、グリッドマップを一様に圧縮したノードによって表現されるトポロジーマップを容易に生成することができる。
【００４１】
次いで、全ての領域に対して領域番号が割り当てられ、未分類領域が無くなったか否かを判定する（ステップＳ２０７）。未分類領域が無くなった場合には処理を終了するものとし、このようにして、ステップＳ２０１乃至２０７の処理を繰り返し、グリッドマップを複数の領域へと分割する。
【００４２】
図４は、領域分割処理のようすを示す図である。図４（ａ）は、初期状態のグリッドマップを示す図であり、図において領域Ｖはロボットの移動が禁止される移動禁止領域を示す。図４（ｂ）は、図４（ａ）に示したグリッドマップにおいて、基点Ｂ１（種）から一定領域を作成した後の状態を示す図であり、領域番号が１の領域をグリッドマップに割り当てたものである。ここで、基点からの矢印は、探索枝が延びた方向を示している。
【００４３】
図４（ｃ）は、次の基点Ｂ２から一定領域を作成した後の状態を示す図であり、領域番号が２の領域をグリッドマップに新たに割り当てたものである。また、図４（ｄ）は、次の基点Ｂ３から一定領域を作成した後の状態を示す図であり、領域番号が３の領域をグリッドマップに新たに割り当てたものである。このようにして、グリッドマップ内の全ての領域に対して複数の領域を割り当ててゆく。図４（ｅ）は、グリッドマップに対して全ての領域を割り当てた後の状態を示す図であり、グリッドマップを１４個の異なる領域番号を持つ領域に分割したものである。
【００４４】
図５は、分割された領域について、各領域に関する隣接領域の情報を示す表である。領域の割り当て処理が行われる際には、探索枝に含まれるグリッド点が、他の領域番号を持つ領域と隣接するか否かの判定を行う。グリッド点が、他の領域と隣接する場合には、その隣接領域の情報が基点に対応付けられ、隣接領域情報として経路探索記憶部３に記憶される。図に示すように、領域の領域番号（ＩＤ）が１である領域１に隣接する領域は２及び６であり、領域番号（ＩＤ）が２の領域２と、領域番号（ＩＤ）が６の領域６とが領域１に隣接していることを示す。
【００４５】
図６は、ノード・リンク設定手段２１２による、ノード及びリンクの設定処理を示すフローチャートである。まず、分割された各領域について代表点を算出し、ノードとして設定する（ステップＳ３０１）。より詳細には、グリッドマップをｘ、ｙ軸によって規定される座標系を用いて表現し、各領域の要素座標の代表座標値を算出し、算出した座標値を領域の基点に対応付けて記憶する。ここで、代表点は、領域内の上下端の中央値及び左右端の中央値を持つ点とし、領域の重心に位置する。尚、両中央値を持つ点が領域に含まれない場合には、いずれかの中央値を持ち、他方の中央値にできるだけ近い値を持つ領域内の点を代表点とする。
【００４６】
次に、代表点間の距離を隣接領域間を接続するリンクのコストとし、リンクにより接続する（ステップＳ３０２）。代表点として設定したノード間を、領域の隣接情報に従ってリンクにより接続する。代表点間の幾何学的な距離を算出して隣接領域間のコストとし、ノード間を接続するリンクにコストとして対応付ける。
【００４７】
図７は、各領域に対して代表点を算出して、ノードとして設定した例を示す図である。グリッドマップを１４個の領域に分割し、それぞれの領域に対して代表点としての１４個のノードを設定したものである。図８は、ノード間をリンクで接続した例を示す図である。ノード間を結ぶ線はリンクを示しており、各リンクに対応付けられている数字は、代表点間の幾何学的な距離、即ちリンクのコストを示す。図８に示すように、ノード及びリンクから構成される図をトポロジーマップという。
【００４８】
続いて、トポロジーマップ経路探索手段２２による、トポロジーマップ上における経路探索処理について説明する。まず、トポロジーマップ経路探索手段２２は、グリッドマップ上の移動始点及び移動終点位置を、トポロジーマップ上のノードへと対応付ける。より詳細には、グリッドマップ上の移動始点に対して、移動始点が含まれる領域を代表するノードへと対応付ける。また、グリッドマップ上の移動終点に対して、移動終点が含まれる領域を代表するノードへと対応付ける。
【００４９】
図９は、移動始点及び移動終点位置を、トポロジーマップ上のノードへと対応付けた一例を示す図である。図に示すように、グリッドマップ上の移動始点Ｓを、移動始点Ｓが含まれる領域１を代表するノード１へと対応付ける。また、グリッドマップ上の移動終点Ｇを、移動終点Ｇが含まれる領域１１を代表するノード１１へと対応付ける。
【００５０】
次いで、トポロジーマップにおいて、対応付けられた移動始点ノードから移動終点ノードへと至る経路について、最短経路探索処理を実行する。経路探索手法としては、例えば、ダイクストラ法を用いることができる。また、ダイクストラ法に限定されず、Ａ＊（エースター）法など公知の経路探索手法を用いてもよい。
【００５１】
図１０は、トポロジーマップにおける最短経路探索処理の探索結果を示す図である。図に示す例では、リンク（実線）により接続されるノード１、２、３、７、１２、１３、９、１０、１１を経由ノードとする経路が、ノード１からノード１１へと至る最短経路として探索される。尚、破線により示されるリンクは、最短経路として採用されなかったノード間のリンクを示している。
【００５２】
続いて、移動空間経路探索手段２３による、グリッドマップ上の領域における、経路探索処理について説明する。まず、移動空間経路探索手段２３は、最短経路として探索されたトポロジーマップ上の経由ノードを、グリッドマップ上の経由点として、グリッドマップ上に射影することで登録する。これにより、最短経路として探索されたトポロジーマップ上の経由ノードに対応した、グリッドマップ上の限定された領域のみを経路探索の対象とする。
【００５３】
図１１は、グリッドマップにおいて、探索されたトポロジーマップ上の経由ノードに対応する領域のみを、経路探索の対象領域として限定した例を示す図である。図に示すように、トポロジーマップ上のノード１、２、３、７、１２、１３、９、１０、１１を経由ノードとして、これらノードに対応する領域１、２、３、７、１２、１３、９、１０、１１を、グリッドマップにおける経路探索領域として限定する。
【００５４】
次いで、経由ノードに対応するグリッドマップ内の領域において、移動始点Ｓから移動終点Ｇへと至る最短経路探索処理を実行する。即ち、グリッドマップの限定された領域において、移動始点Ｓから移動終点Ｇへの経路を探索する。経路探索手法としては、例えば、ダイクストラ法を用いることができる。また、ダイクストラ法に限定されず、Ａ＊（エースター）法など公知の経路探索手法を用いてもよい。
【００５５】
図１２は、グリッドマップにおける最短経路探索処理の探索結果を示す図である。図に示す例では、実線により示される経路が移動始点Ｓから移動終点Ｇへと至る最短経路として探索される。尚、一点鎖線により示される経路は、トポロジーマップにおける最短経路を示している。
【００５６】
以上に説明したように、本実施の形態１にかかる経路探索システム１によれば、自動的に生成したトポロジーマップを用いて、グリッドマップにおける経路探索領域を限定することで、広範な移動空間においても高速かつ正確に経路探索を実行することができる。
【００５７】
本実施の形態１にかかる経路探索システム１は、経路探索手法を用いて、グリッドマップを、領域に含まれる任意の２点間に経路が存在するように、複数の領域に分割する。より詳細には、グリッドマップに含まれる複数の各基点から探索枝を所定の本数となるまで延ばして、各基点から探索枝により接続される点からなる領域をひとつの領域とすることで、グリッドマップを複数の領域として分割する。そして、各領域に対してノードを設定すると共に、各領域間の隣接関係に従ってノード間をリンクで接続することで、ノード及びリンクから構成されるトポロジーマップを生成する。次いで、経路探索システム１は、生成したトポロジーマップを用いて、トポロジーマップ上において、移動始点に対応するノードから移動終点に対応するノードへと至る経路を探索する。次いで、経路探索システム１は、探索されたトポロジーマップ上の経路に対応するグリッドマップの領域において、移動始点から移動終点へと至る詳細な経路を探索する。
【００５８】
このように経路探索手法を用いて領域を分割することで、各領域に含まれる任意の２点間には経路が存在するため、各ノードに対応するグリッドマップの各領域においても任意の２点間に経路が存在する。そして、各領域間の隣接関係に従ってノード間をリンクで接続することで、リンクで接続された複数のノードについて、それら複数のノードに対応するグリッドマップの複数の領域にわたって任意の２点間に経路が存在する。このため、トポロジーマップにおいて探索されたトポロジーマップ上の経路の経由ノードについて、それら経由ノードに対応するグリッドマップの限定された領域にわたっても任意の２点間に経路が存在する。即ち、グリッドマップの限定された領域にわたって、移動始点から移動終点への経路を正確に探索することができる。従って、このようなトポロジーマップを自動で生成して、トポロジーマップにおける経路探索結果を用いることで、グリッドマップにおいて限定された領域において経路探索を行うことができるため、広範な移動空間においても、高速かつ正確に経路を生成することができる。
【００５９】
その他の実施の形態．
また、上述した実施の形態では、移動空間が２次元のグリッドマップである場合の例を説明したが、移動空間が例えばｘ、ｙ、ｚ軸によって規定される３次元空間の場合でもよく、さらに、ロボットの姿勢（１次元）を併せて４次元の探索空間とした場合であっても本発明を適用することができる。
【００６０】
さらにまた、上述した実施の形態では、移動手段として車輪などを有するロボットに本発明を適用した一例を示したが、これに限定されず、ロボットは足を有し、自律歩行可能な移動ロボットであってもよい。
【００６１】
また、本発明は上述した実施の形態のみに限定されるものではなく、既に述べた本発明の要旨を逸脱しない範囲において種々の変更が可能であることは勿論である。
【図面の簡単な説明】
【００６２】
【図１】本発明の実施の形態にかかる経路探索システムの構成を示す構成図である。
【図２】本発明の実施の形態にかかる経路探索処理の概要を示すフローチャートである。
【図３】本発明の実施の形態にかかる領域分割処理を示すフローチャートである。
【図４（ａ）】本発明の実施の形態にかかる領域分割処理のようすを説明するための一例を示す図である。
【図４（ｂ）】本発明の実施の形態にかかる領域分割処理のようすを説明するための一例を示す図である。
【図４（ｃ）】本発明の実施の形態にかかる領域分割処理のようすを説明するための一例を示す図である。
【図４（ｄ）】本発明の実施の形態にかかる領域分割処理のようすを説明するための一例を示す図である。
【図４（ｅ）】本発明の実施の形態にかかる領域分割処理のようすを説明するための一例を示す図である。
【図５】本発明の実施の形態にかかる隣接領域情報を示す表である。
【図６】本発明の実施の形態にかかるノード・リンク設定処理を示すフローチャートである。
【図７】本発明の実施の形態にかかるノード・リンク設定処理のようすを説明するための一例を示す図である。
【図８】本発明の実施の形態にかかるノード・リンク設定処理のようすを説明するための一例を示す図である。
【図９】本発明の実施の形態にかかるトポロジーマップ上での経路探索処理のようすを説明するための一例を示す図である。
【図１０】本発明の実施の形態にかかるトポロジーマップ上での経路探索処理のようすを説明するための一例を示す図である。
【図１１】本発明の実施の形態にかかるグリッドマップ上での経路探索処理のようすを説明するための一例を示す図である。
【図１２】本発明の実施の形態にかかるグリッドマップ上での経路探索処理のようすを説明するための一例を示す図である。
【符号の説明】
【００６３】
１ 経路探索システム、
２ 経路探索処理部、
２１ トポロジーマップ生成手段、
２１１ 領域分割部、２１２ ノード・リンク設定手段、
２２ トポロジーマップ経路探索手段、
２３ 移動空間経路探索手段、
３ 経路探索記憶部、
Ｖ 移動禁止領域、Ｓ 移動始点、Ｇ 移動終点
【技術分野】
【０００１】
本発明は移動経路を探索する経路探索システム、経路探索方法、及び自律移動体に関する。
【背景技術】
【０００２】
環境を自律的に移動する移動体は、予め又はリアルタイムに移動経路を探索する必要がある。従来より、多くの経路探索方法が提案されている。一般的な経路探索方法として、ダイクストラ、A＊などの手法が良く知られている。このような手法により経路探索を行う場合には、例えば、等間隔のグリッドをマップとして保持し、障害物をそのグリッドにプロットしたグリッドマップを用いて、各グリッドセルをノードとして移動始点から移動終点まで経路探索を行う技術が良く知られている。
【０００３】
しかしながら、グリッドマップ上において経路探索を行う従来技術では、グリッドを用いて広い範囲において経路探索を行う場合には、移動空間に応じてグリッドマップが大きくなるため、計算負荷やメモリ容量の増大を招き、リアルタイムで経路探索を実行することができないという問題がある。
【０００４】
そこで、広範な移動空間において経路探索を行う場合に、移動空間の特徴をノード及びリンクにより表現して、それらノード及びリンクから構成される幾何学的なトポロジーマップを用いて経路探索を行う技術が開示されている（例えば特許文献１、２）。
【特許文献１】特開２０００−１８１５３９号公報
【特許文献２】特開２００７−１５５６９９号公報
【発明の開示】
【発明が解決しようとする課題】
【０００５】
しかしながら、従来では、トポロジーマップを生成するため、移動空間に存在する例えば出入り口や、棚や、机などの特徴を人間が判断して抽出を行っており、広範な移動空間においては、手間の増加や、人により抽出される特徴が相違するなどの問題がある。更には、広範で複雑な移動空間においては、特徴点間に経路が存在しないような特徴点を間違って抽出する場合も想定され、かかる場合には、経路探索が実行不可能となる虞がある。即ち、従来の人手によるトポロジーマップの生成方法では、特徴点を自動的に抽出して、経路探索を正確に実行することができないという問題がある。
【０００６】
このように、従来の移動経路探索方法では人手により生成したトポロジーマップを用いて経路探索処理を行うため、広範な移動空間において、高速かつ正確に最適な経路を探索することができないという問題があった。
【０００７】
本発明は、かかる課題を解決するためになされたものであり、広範な移動空間であっても、高速かつ正確に最適な経路を探索することができる経路探索システム、経路探索方法、及び自律移動体を提供することを目的とする。
【課題を解決するための手段】
【０００８】
本発明にかかる経路探索システムは、移動空間内に存在する移動始点より移動を開始し、前記移動空間内に存在する移動終点に到達する経路を探索する経路探索システムであって、前記移動空間を、領域に含まれる任意の２点間に経路が存在するように、複数の領域に分割し、前記各領域に対してノードを設定すると共に、前記各領域間の隣接関係に従って前記ノード間をリンクで接続することで、ノード及びリンクから構成されるトポロジーマップを生成するトポロジーマップ生成手段と、生成された前記トポロジーマップにおいて、経路を探索するトポロジーマップ経路探索手段と、前記トポロジーマップ経路探索手段により探索されたトポロジーマップ上の経路に対応する前記移動空間の領域において、経路を探索する移動空間経路探索手段とを備えるものである。
【０００９】
これにより、各領域に含まれる任意の２点間には経路が存在するため、各ノードに対応する移動空間の各領域においても任意の２点間には経路が存在する。そして、各領域間の隣接関係に従ってノード間をリンクで接続するため、リンクで接続された複数のノードについて、それら複数のノードに対応する移動空間の複数の領域にわたって任意の２点間に経路が存在する。このため、トポロジーマップにおいて探索されたトポロジーマップ上の経路の経由ノードについて、それら経由ノードに対応する移動空間の限定された領域にわたっても任意の２点間に経路が存在する。即ち、移動空間の限定された領域にわたって、移動始点から移動終点への経路を正確に探索することができる。従って、このようなトポロジーマップを自動で生成して、トポロジーマップにおける経路探索結果を用いることで、移動空間において限定された領域において経路探索を行うことができるため、広範な移動空間においても、高速かつ正確に経路を生成することができる。
【００１０】
また、前記トポロジーマップ生成手段は、前記移動空間に含まれる複数の基点について、前記各基点から探索枝を所定の本数となるまで延ばして、前記探索枝により接続される点からなる領域をひとつの領域とすることで、前記移動空間を複数の領域として分割する領域分割手段と、前記各領域に対してノードを設定すると共に、前記各領域間の隣接関係に従って前記ノード間をリンクで接続するノード・リンク設定手段とを備えるようにしてもよい。これにより、移動空間を、各領域において任意の２点間に経路が存在するような複数の領域に簡単に分割することができる。
【００１１】
さらにまた、前記各基点から延ばす前記探索枝の本数を設定可能とするようにしてもよい。これにより、分割された各領域に含まれる点の個数をそれぞれ略同じ個数とすることで、移動空間を略均一に分割することができる。従って、移動空間を一様に圧縮したノードによって表現されるトポロジーマップを容易に生成することができる。
【００１２】
また、前記ノード・リンク設定手段が、前記各領域の重心に位置する代表点前記ノードとして設定するようにしてもよい。これにより、分割された各領域から、簡単にトポロジーマップのノードを設定することができる。
【００１３】
さらにまた、前記ノード・リンク設定手段が、前記代表点間の幾何学的な距離を前記リンクのコストとし、前記各領域間の隣接関係に従って前記ノード間をリンクで接続するようにしてもよい。これにより、移動空間における各領域の幾何学的な位置関係を保持したトポロジーマップを容易に生成することができる。
【００１４】
また、トポロジーマップ経路探索手段及び／又は移動空間経路探索手段が、ダイクストラ法を用いて経路を探索するようにしてもよい。これにより、容易かつ高速に経路を探索することができる。
【００１５】
本発明にかかる経路探索方法は、探索空間内に存在する探索開始点より探索を開始し、前記探索空間内に存在する探索終点に到達する経路を探索する経路探索方法であって、前記探索空間を、領域に含まれる任意の２点間に経路が存在するように、複数の領域に分割するステップと、分割された前記各領域に対してノードを設定すると共に、前記各領域間の隣接関係に従って前記ノード間をリンクで接続することでノード及びリンクを設定するステップと、前記ノード及びリンクからなるトポロジーマップを用いて、経路を探索するステップと、前記トポロジーマップ経路探索手段により探索されたトポロジーマップ上の経路に対応する前記探索空間の領域において、経路を探索するステップとを備えるものである。
【００１６】
このようにすれば、トポロジーマップにおいて探索されたトポロジーマップ上の経路の経由ノードについて、それら経由ノードに対応する探索空間の限定された領域にわたって任意の２点間に経路が存在する。即ち、探索空間の限定された領域にわたって、探索始点から探索終点への経路を正確に探索することができる。従って、このようなトポロジーマップを自動で生成して、トポロジーマップにおける経路探索結果を用いることで、探索空間において限定された領域において経路探索を行うことができるため、広範な探索空間においても、高速かつ正確に経路を生成することができる。
【００１７】
また、前記探索空間を複数の領域に分割するステップでは、前記探索空間に含まれる複数の各基点から探索枝を所定の本数となるまで延ばして、前記探索空間を、前記各基点から前記探索枝により接続される点からなる複数の領域として分割するようにしてもよい。
【００１８】
さらにまた、前記各基点から延ばす前記探索枝の本数を設定可能とするようにしてもよい。また、前記ノード及びリンクを設定するステップでは、前記各領域の重心に位置する代表点を前記ノードとして設定すると共に、前記各領域間の隣接関係に従って前記ノード間をリンクで接続するようにしてもよい。
【００１９】
また、前記ノード及びリンクを設定するステップでは、前記代表点間の幾何学的な距離を前記リンクのコストとし、前記各領域間の隣接関係に従って前記ノード間をリンクで接続するようにしてもよい。さらにまた、前記経路を探索するステップでは、ダイクストラ法を用いて経路を探索するようにしてもよい。
【００２０】
本発明にかかる自律移動体は、移動空間内に存在する移動始点より移動を開始し、前記移動空間内に存在する移動終点に到達する経路を探索可能な自律移動体であって、前記移動空間を、領域に含まれる任意の２点間に経路が存在するように、複数の領域に分割する領域分割手段と、分割された前記各領域に対してノードを設定すると共に、前記各領域間の隣接関係に従って前記ノード間をリンクで接続することでノード及びリンクを設定するノード・リンク設定手段と、前記ノード及びリンクからなるトポロジーマップを用いて、経路を探索するトポロジーマップ経路探索手段と、前記トポロジーマップ経路探索手段により探索されたトポロジーマップ上の経路に対応する前記移動空間の領域において、経路を探索する移動空間経路探索手段とを備えるものである。
【００２１】
これにより、移動空間の限定された領域にわたって、移動始点から移動終点への経路を正確に探索することができる。従って、このようなトポロジーマップを自動で生成して、トポロジーマップにおける経路探索結果を用いることで、移動空間において限定された領域において経路探索を行うことができるため、広範な移動空間においても、高速かつ正確に経路を生成することができる。
【発明の効果】
【００２２】
本発明によれば、広範な移動空間であっても、高速かつ正確に最適な経路を探索することができる経路探索システム、経路探索方法、及び自律移動体を提供することができる。
【発明を実施するための最良の形態】
【００２３】
以下、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。尚、各図面において、同一要素には同一の符号を付しており、説明の明確化のため、必要に応じて重複説明を省略する。
【００２４】
発明の実施の形態１．
本発明の実施の形態１にかかる経路探索システムでは、自律移動体としてロボットを用いている。ロボットは、例えば、それぞれ独立して回転制御される二つの車輪を備えたものでもよく、２輪よりも多い、例えば４輪走行のロボットや、１輪走行のロボット、２足歩行・４足歩行のロボットであってもよい。また、ロボットに限らず、２輪走行や４輪走行の車両にも適用できる。本例におけるロボットは、障害物を検出するセンサやカメラを備え、検出された障害物に応じて、これを避けるように経路探索を行なう。
【００２５】
当該経路探索システムは、移動空間内に存在する移動始点より移動を開始し、移動空間内に存在する移動終点に到達する経路を探索する。経路探索システムは、ノード及びリンクから構成されるトポロジーマップを用いて経路を探索するため、移動空間に基づいてトポロジーマップを生成する。経路探索システムは、まず、移動空間を、領域に含まれる任意の２点間に経路が存在するように、複数の領域に分割する。次いで、各領域に対してノードを設定すると共に、各領域間の隣接関係に従ってノード間をリンクで接続することで、ノード及びリンクから構成されるトポロジーマップを生成する。そして、経路探索システムは、生成されたトポロジーマップにおいて経路を探索した上で、その探索されたトポロジーマップ上の経路に対応する移動空間の領域において経路を探索する。
【００２６】
後述するように、探索されたトポロジーマップ上の経路の経由ノードに対応する移動空間の限定された領域にわたって、任意の２点間に経路が存在する。このため、移動空間の限定された領域にわたって、移動始点から移動終点への経路を正確に探索することができる。従って、このようなトポロジーマップを自動で生成して、トポロジーマップにおける経路探索結果を用いることで、移動空間全体ではなく限定された領域において経路探索を行うことができるため、広範な移動空間においても、高速かつ正確に経路を生成することができる。
【００２７】
本実施の形態１にかかる経路探索システムは、例えば、ロボットに搭載されたコンピュータやロボットとは別に設けられたコンピュータにより実現される。このコンピュータは、例えば、中央処理装置（ＣＰＵ）、ＲＯＭ、ＲＡＭ、ハードディスク等の補助記憶装置、ＣＤ−ＲＯＭ等の可搬型記憶媒体が挿入される記憶媒体駆動装置、入力手段や出力手段を備えている。ＲＯＭ、補助記憶装置、可搬型記憶媒体等の記憶媒体には、オペレーティングシステムと協働してＣＰＵ等に命令を与え、アプリケーションプログラムを記録することができる。アプリケーションプログラムはＲＡＭにロードされることによって実行される。このアプリケーションプログラムは、本発明にかかる経路探索システムを実現する特有の経路探索プログラムを含む。経路探索システムによる経路探索は、中央処理装置がアプリケーションプログラムをＲＡＭ上に展開した上で当該アプリケーションプログラムに従った処理を補助記憶装置に格納されたデータを読み出し、また格納を行なうことにより、実行される。
【００２８】
図１は、本実施の形態１にかかる経路探索システムの概略構成を示すブロック図である。図１に示されるように、経路探索システム１は、経路探索処理部２と経路探索記憶部３により構成されている。経路探索処理部２は、トポロジーマップ生成手段２１、トポロジーマップ経路探索手段２２、及び移動空間経路探索手段２３を備えている。
【００２９】
経路探索記憶部３には、移動空間を表現するためのグリッドマップ及びトポロジーマップが記憶されている。グリッドマップは、ロボットが移動する移動領域全体の形状に、略一定間隔ｄ（例えば１０ｃｍ）に配置された格子点を結ぶグリッド線を仮想的に描写することで得られる。ロボットは、ＧＰＳ等から得られた位置情報を、このグリッドマップ上における自己の位置に置き換えて、グリッドマップ上における自己位置を認識する。グリッドマップ上において、ロボットの自己位置に相当する場所、および目的地である移動終点、および移動終点におけるロボットの移動方向が特定される。ロボットは、グリッドマップ上において特定された自己位置を移動始点から、目的地である移動終点までの移動経路を作成し、作成された移動経路に従って移動を行う。
【００３０】
トポロジーマップは、ロボットが移動する移動空間を抽象化して表現するための幾何学的マップであり、ノード及びリンクから構成される。本実施の形態１における経路探索システムでは、グリッドマップに基づいて、後述するトポロジーマップ生成手段２１によりトポロジーマップを生成する。
【００３１】
トポロジーマップ生成手段２１は、グリッドマップに基づいて、トポロジーマップを生成する。より具体的には、グリッドマップを、領域に含まれる任意の２点間に経路が存在するように、複数の領域に分割する領域分割する。そして、各領域に対してノードを設定すると共に、各領域間の隣接関係に従ってノード間をリンクで接続することで、ノード及びリンクから構成されるトポロジーマップを生成する。
【００３２】
トポロジーマップ生成手段２１は、領域分割手段２１１及びノード・リンク設定手段２１２を備えている。領域分割手段２１１は、グリッドマップに含まれる複数の各基点から探索枝を所定の本数となるまで延ばして、各基点から探索枝により接続される点からなる領域をひとつの領域とすることで、グリッドマップを複数の領域として分割する。ノード・リンク設定手段２１２は、各領域に対してノードを設定すると共に、各領域間の隣接関係に従ってノード間をリンクで接続する。尚、トポロジーマップ生成処理の詳細については後述する。
【００３３】
トポロジーマップ経路探索手段２２は、トポロジーマップ生成手段２１により生成されたトポロジーマップにおいて、経路を探索する。また、移動空間経路探索手段２３は、トポロジーマップ経路探索手段２２により探索されたトポロジーマップ上の経路に対応するグリッドマップ上の領域において、経路を探索する。
【００３４】
図２は、本実施の形態１にかかる経路探索システム１による、経路探索処理の概要を示すフローチャートである。まず、経路探索システム１は、トポロジーマップ生成手段２１により、グリッドマップに基づいて、ノード及びリンクからなるトポロジーマップを生成する（ステップＳ１０１）。次いで、生成されたトポロジーマップを用いて、トポロジーマップ経路探索手段２２により、トポロジーマップ上において、移動始点に対応するノードから移動終点へと対応するノードへと至る経路を探索する（ステップＳ１０２）。次いで、移動空間経路探索手段２３により、トポロジーマップ経路探索手段２２により探索されたトポロジーマップ上の経由ノードに対応するグリッドマップ上の領域において、移動始点から移動終点へと至る経路を探索する（ステップＳ１０３）。以下、各ステップにおける処理について詳細に説明する。
【００３５】
まず、トポロジーマップ生成手段２１によるトポロジーマップ生成処理について説明する。図３乃至図８は、トポロジーマップ生成手段２１によるトポロジーマップ生成処理を説明するための図である。図３は、トポロジーマップ生成処理における、領域分割手段２１１による領域分割処理を示すフローチャートである。
【００３６】
領域分割手段２１１は、トポロジーマップと比較して詳細な分解能を有するグリッドマップを、一定の面積を有する複数の領域に分割する。分割された各領域に対して固有の領域番号を割り当てるものとする。まず、領域分割手段２１１は、領域を分割するために用いる基点（種ともいう。）をグリッドマップに設定する（ステップＳ２０１）。基点の設定方法は、詳細なグリッドマップ内を走査して、領域番号が割り当てられていないグリッド点を含む未分類領域があるか否かを判定し、未分類領域がある場合には、そのグリッド点を新たな基点（種）として設定する。基点（種）に対して、新たな領域番号を設定する。
【００３７】
次いで、ステップＳ２０１において設定された基点（種）から、グリッド点間の隣接関係に基づいて、探索枝を延ばす（ステップＳ２０２）。探索枝を延ばす処理は最短経路探索処理と同様の処理を行うものとし、領域番号を持つ基点（種）を開始点として、最小経路となる探索枝を順次延長してゆく。より詳細には、基点に対応するグリッド点から、隣接するグリッド点間の評価値のうち最も小さい評価値を有する隣接グリッド点を探索枝に順次選択してゆくことで、選択されたグリッド点を通過する探索枝を最短経路として決定することができる。隣接するグリッド点間の評価値としては、例えば、前後左右方向に隣接するグリッド間の評価値を５とし、斜め方向に隣接するグリッド間の評価値を７などとする。また、基点から延びた探索枝に含まれるグリッド点は、基点と同じ領域番号を持つものとする。このように経路探索手法を利用して領域を分割するため、一つの領域内の任意の二点間に経路が存在することが保証される。
【００３８】
次いで、探索枝に含まれるグリッド点が、他の領域番号を持つ領域と隣接するか否かを判定する（ステップＳ２０３）。グリッド点が、他の領域と隣接する場合には、その隣接領域の情報を基点に対応付けして、後述する隣接領域情報として経路探索記憶部３に記憶する（ステップＳ２０４）。後述するように、記憶された隣接領域情報を用いて、ノード・リンク設定処理においてノード間にリンクが存在するか否かの判定を行う。
【００３９】
次いで、探索枝を延ばしている領域番号の領域について、探索枝が指定本数分まで延びているか、又は、探索枝をこれ以上は増やすことができないか否かを判定する（ステップＳ２０５）。探索枝が指定数分だけ延びている、又は、これ以上増やすことができない場合には、新しい領域番号を、探索枝により探索されたグリッド点に対して割り当てる（ステップＳ２０６）。
【００４０】
このように、各基点から延ばす探索枝の本数を予め設定し、分割された各領域に含まれるグリッド点の個数をそれぞれ略同じ個数とすることで、グリッドマップを略均一に分割することができる。このため、グリッドマップを一様に圧縮したノードによって表現されるトポロジーマップを容易に生成することができる。
【００４１】
次いで、全ての領域に対して領域番号が割り当てられ、未分類領域が無くなったか否かを判定する（ステップＳ２０７）。未分類領域が無くなった場合には処理を終了するものとし、このようにして、ステップＳ２０１乃至２０７の処理を繰り返し、グリッドマップを複数の領域へと分割する。
【００４２】
図４は、領域分割処理のようすを示す図である。図４（ａ）は、初期状態のグリッドマップを示す図であり、図において領域Ｖはロボットの移動が禁止される移動禁止領域を示す。図４（ｂ）は、図４（ａ）に示したグリッドマップにおいて、基点Ｂ１（種）から一定領域を作成した後の状態を示す図であり、領域番号が１の領域をグリッドマップに割り当てたものである。ここで、基点からの矢印は、探索枝が延びた方向を示している。
【００４３】
図４（ｃ）は、次の基点Ｂ２から一定領域を作成した後の状態を示す図であり、領域番号が２の領域をグリッドマップに新たに割り当てたものである。また、図４（ｄ）は、次の基点Ｂ３から一定領域を作成した後の状態を示す図であり、領域番号が３の領域をグリッドマップに新たに割り当てたものである。このようにして、グリッドマップ内の全ての領域に対して複数の領域を割り当ててゆく。図４（ｅ）は、グリッドマップに対して全ての領域を割り当てた後の状態を示す図であり、グリッドマップを１４個の異なる領域番号を持つ領域に分割したものである。
【００４４】
図５は、分割された領域について、各領域に関する隣接領域の情報を示す表である。領域の割り当て処理が行われる際には、探索枝に含まれるグリッド点が、他の領域番号を持つ領域と隣接するか否かの判定を行う。グリッド点が、他の領域と隣接する場合には、その隣接領域の情報が基点に対応付けられ、隣接領域情報として経路探索記憶部３に記憶される。図に示すように、領域の領域番号（ＩＤ）が１である領域１に隣接する領域は２及び６であり、領域番号（ＩＤ）が２の領域２と、領域番号（ＩＤ）が６の領域６とが領域１に隣接していることを示す。
【００４５】
図６は、ノード・リンク設定手段２１２による、ノード及びリンクの設定処理を示すフローチャートである。まず、分割された各領域について代表点を算出し、ノードとして設定する（ステップＳ３０１）。より詳細には、グリッドマップをｘ、ｙ軸によって規定される座標系を用いて表現し、各領域の要素座標の代表座標値を算出し、算出した座標値を領域の基点に対応付けて記憶する。ここで、代表点は、領域内の上下端の中央値及び左右端の中央値を持つ点とし、領域の重心に位置する。尚、両中央値を持つ点が領域に含まれない場合には、いずれかの中央値を持ち、他方の中央値にできるだけ近い値を持つ領域内の点を代表点とする。
【００４６】
次に、代表点間の距離を隣接領域間を接続するリンクのコストとし、リンクにより接続する（ステップＳ３０２）。代表点として設定したノード間を、領域の隣接情報に従ってリンクにより接続する。代表点間の幾何学的な距離を算出して隣接領域間のコストとし、ノード間を接続するリンクにコストとして対応付ける。
【００４７】
図７は、各領域に対して代表点を算出して、ノードとして設定した例を示す図である。グリッドマップを１４個の領域に分割し、それぞれの領域に対して代表点としての１４個のノードを設定したものである。図８は、ノード間をリンクで接続した例を示す図である。ノード間を結ぶ線はリンクを示しており、各リンクに対応付けられている数字は、代表点間の幾何学的な距離、即ちリンクのコストを示す。図８に示すように、ノード及びリンクから構成される図をトポロジーマップという。
【００４８】
続いて、トポロジーマップ経路探索手段２２による、トポロジーマップ上における経路探索処理について説明する。まず、トポロジーマップ経路探索手段２２は、グリッドマップ上の移動始点及び移動終点位置を、トポロジーマップ上のノードへと対応付ける。より詳細には、グリッドマップ上の移動始点に対して、移動始点が含まれる領域を代表するノードへと対応付ける。また、グリッドマップ上の移動終点に対して、移動終点が含まれる領域を代表するノードへと対応付ける。
【００４９】
図９は、移動始点及び移動終点位置を、トポロジーマップ上のノードへと対応付けた一例を示す図である。図に示すように、グリッドマップ上の移動始点Ｓを、移動始点Ｓが含まれる領域１を代表するノード１へと対応付ける。また、グリッドマップ上の移動終点Ｇを、移動終点Ｇが含まれる領域１１を代表するノード１１へと対応付ける。
【００５０】
次いで、トポロジーマップにおいて、対応付けられた移動始点ノードから移動終点ノードへと至る経路について、最短経路探索処理を実行する。経路探索手法としては、例えば、ダイクストラ法を用いることができる。また、ダイクストラ法に限定されず、Ａ＊（エースター）法など公知の経路探索手法を用いてもよい。
【００５１】
図１０は、トポロジーマップにおける最短経路探索処理の探索結果を示す図である。図に示す例では、リンク（実線）により接続されるノード１、２、３、７、１２、１３、９、１０、１１を経由ノードとする経路が、ノード１からノード１１へと至る最短経路として探索される。尚、破線により示されるリンクは、最短経路として採用されなかったノード間のリンクを示している。
【００５２】
続いて、移動空間経路探索手段２３による、グリッドマップ上の領域における、経路探索処理について説明する。まず、移動空間経路探索手段２３は、最短経路として探索されたトポロジーマップ上の経由ノードを、グリッドマップ上の経由点として、グリッドマップ上に射影することで登録する。これにより、最短経路として探索されたトポロジーマップ上の経由ノードに対応した、グリッドマップ上の限定された領域のみを経路探索の対象とする。
【００５３】
図１１は、グリッドマップにおいて、探索されたトポロジーマップ上の経由ノードに対応する領域のみを、経路探索の対象領域として限定した例を示す図である。図に示すように、トポロジーマップ上のノード１、２、３、７、１２、１３、９、１０、１１を経由ノードとして、これらノードに対応する領域１、２、３、７、１２、１３、９、１０、１１を、グリッドマップにおける経路探索領域として限定する。
【００５４】
次いで、経由ノードに対応するグリッドマップ内の領域において、移動始点Ｓから移動終点Ｇへと至る最短経路探索処理を実行する。即ち、グリッドマップの限定された領域において、移動始点Ｓから移動終点Ｇへの経路を探索する。経路探索手法としては、例えば、ダイクストラ法を用いることができる。また、ダイクストラ法に限定されず、Ａ＊（エースター）法など公知の経路探索手法を用いてもよい。
【００５５】
図１２は、グリッドマップにおける最短経路探索処理の探索結果を示す図である。図に示す例では、実線により示される経路が移動始点Ｓから移動終点Ｇへと至る最短経路として探索される。尚、一点鎖線により示される経路は、トポロジーマップにおける最短経路を示している。
【００５６】
以上に説明したように、本実施の形態１にかかる経路探索システム１によれば、自動的に生成したトポロジーマップを用いて、グリッドマップにおける経路探索領域を限定することで、広範な移動空間においても高速かつ正確に経路探索を実行することができる。
【００５７】
本実施の形態１にかかる経路探索システム１は、経路探索手法を用いて、グリッドマップを、領域に含まれる任意の２点間に経路が存在するように、複数の領域に分割する。より詳細には、グリッドマップに含まれる複数の各基点から探索枝を所定の本数となるまで延ばして、各基点から探索枝により接続される点からなる領域をひとつの領域とすることで、グリッドマップを複数の領域として分割する。そして、各領域に対してノードを設定すると共に、各領域間の隣接関係に従ってノード間をリンクで接続することで、ノード及びリンクから構成されるトポロジーマップを生成する。次いで、経路探索システム１は、生成したトポロジーマップを用いて、トポロジーマップ上において、移動始点に対応するノードから移動終点に対応するノードへと至る経路を探索する。次いで、経路探索システム１は、探索されたトポロジーマップ上の経路に対応するグリッドマップの領域において、移動始点から移動終点へと至る詳細な経路を探索する。
【００５８】
このように経路探索手法を用いて領域を分割することで、各領域に含まれる任意の２点間には経路が存在するため、各ノードに対応するグリッドマップの各領域においても任意の２点間に経路が存在する。そして、各領域間の隣接関係に従ってノード間をリンクで接続することで、リンクで接続された複数のノードについて、それら複数のノードに対応するグリッドマップの複数の領域にわたって任意の２点間に経路が存在する。このため、トポロジーマップにおいて探索されたトポロジーマップ上の経路の経由ノードについて、それら経由ノードに対応するグリッドマップの限定された領域にわたっても任意の２点間に経路が存在する。即ち、グリッドマップの限定された領域にわたって、移動始点から移動終点への経路を正確に探索することができる。従って、このようなトポロジーマップを自動で生成して、トポロジーマップにおける経路探索結果を用いることで、グリッドマップにおいて限定された領域において経路探索を行うことができるため、広範な移動空間においても、高速かつ正確に経路を生成することができる。
【００５９】
その他の実施の形態．
また、上述した実施の形態では、移動空間が２次元のグリッドマップである場合の例を説明したが、移動空間が例えばｘ、ｙ、ｚ軸によって規定される３次元空間の場合でもよく、さらに、ロボットの姿勢（１次元）を併せて４次元の探索空間とした場合であっても本発明を適用することができる。
【００６０】
さらにまた、上述した実施の形態では、移動手段として車輪などを有するロボットに本発明を適用した一例を示したが、これに限定されず、ロボットは足を有し、自律歩行可能な移動ロボットであってもよい。
【００６１】
また、本発明は上述した実施の形態のみに限定されるものではなく、既に述べた本発明の要旨を逸脱しない範囲において種々の変更が可能であることは勿論である。
【図面の簡単な説明】
【００６２】
【図１】本発明の実施の形態にかかる経路探索システムの構成を示す構成図である。
【図２】本発明の実施の形態にかかる経路探索処理の概要を示すフローチャートである。
【図３】本発明の実施の形態にかかる領域分割処理を示すフローチャートである。
【図４（ａ）】本発明の実施の形態にかかる領域分割処理のようすを説明するための一例を示す図である。
【図４（ｂ）】本発明の実施の形態にかかる領域分割処理のようすを説明するための一例を示す図である。
【図４（ｃ）】本発明の実施の形態にかかる領域分割処理のようすを説明するための一例を示す図である。
【図４（ｄ）】本発明の実施の形態にかかる領域分割処理のようすを説明するための一例を示す図である。
【図４（ｅ）】本発明の実施の形態にかかる領域分割処理のようすを説明するための一例を示す図である。
【図５】本発明の実施の形態にかかる隣接領域情報を示す表である。
【図６】本発明の実施の形態にかかるノード・リンク設定処理を示すフローチャートである。
【図７】本発明の実施の形態にかかるノード・リンク設定処理のようすを説明するための一例を示す図である。
【図８】本発明の実施の形態にかかるノード・リンク設定処理のようすを説明するための一例を示す図である。
【図９】本発明の実施の形態にかかるトポロジーマップ上での経路探索処理のようすを説明するための一例を示す図である。
【図１０】本発明の実施の形態にかかるトポロジーマップ上での経路探索処理のようすを説明するための一例を示す図である。
【図１１】本発明の実施の形態にかかるグリッドマップ上での経路探索処理のようすを説明するための一例を示す図である。
【図１２】本発明の実施の形態にかかるグリッドマップ上での経路探索処理のようすを説明するための一例を示す図である。
【符号の説明】
【００６３】
１ 経路探索システム、
２ 経路探索処理部、
２１ トポロジーマップ生成手段、
２１１ 領域分割部、２１２ ノード・リンク設定手段、
２２ トポロジーマップ経路探索手段、
２３ 移動空間経路探索手段、
３ 経路探索記憶部、
Ｖ 移動禁止領域、Ｓ 移動始点、Ｇ 移動終点
【特許請求の範囲】
【請求項１】
移動空間内に存在する移動始点より移動を開始し、前記移動空間内に存在する移動終点に到達する経路を探索する経路探索システムであって、
前記移動空間を、領域に含まれる任意の２点間に経路が存在するように、複数の領域に分割し、前記各領域に対してノードを設定すると共に、前記各領域間の隣接関係に従って前記ノード間をリンクで接続することで、ノード及びリンクから構成されるトポロジーマップを生成するトポロジーマップ生成手段と、
生成された前記トポロジーマップにおいて、経路を探索するトポロジーマップ経路探索手段と、
前記トポロジーマップ経路探索手段により探索されたトポロジーマップ上の経路に対応する前記移動空間の領域において、経路を探索する移動空間経路探索手段と
を備える経路探索システム。
【請求項２】
前記トポロジーマップ生成手段は、
前記移動空間に含まれる複数の基点について、前記各基点から探索枝を所定の本数となるまで延ばして、前記探索枝により接続される点からなる領域をひとつの領域とすることで、前記移動空間を複数の領域として分割する領域分割手段と、
前記各領域に対してノードを設定すると共に、前記各領域間の隣接関係に従って前記ノード間をリンクで接続するノード・リンク設定手段と
を備えることを特徴とする請求項１記載の経路探索システム。
【請求項３】
前記各基点から延ばす前記探索枝の本数を設定可能とする
ことを特徴とする請求項２記載の経路探索システム。
【請求項４】
前記ノード・リンク設定手段が、
前記各領域の重心に位置する代表点を前記ノードとして設定する
ことを特徴とする請求項２又は３いずれか１項記載の経路探索システム。
【請求項５】
前記ノード・リンク設定手段が、
前記代表点間の幾何学的な距離を前記リンクのコストとし、前記各領域間の隣接関係に従って前記ノード間をリンクで接続する
ことを特徴とする請求項２乃至４いずれか１項記載の経路探索システム。
【請求項６】
トポロジーマップ経路探索手段及び／又は移動空間経路探索手段が、
ダイクストラ法を用いて経路を探索する
ことを特徴とする請求項１乃至５いずれか１項記載の経路探索システム。
【請求項７】
探索空間内に存在する探索開始点より探索を開始し、前記探索空間内に存在する探索終点に到達する経路を探索する経路探索方法であって、
前記探索空間を、領域に含まれる任意の２点間に経路が存在するように、複数の領域に分割するステップと、
分割された前記各領域に対してノードを設定すると共に、前記各領域間の隣接関係に従って前記ノード間をリンクで接続することでノード及びリンクを設定するステップと、
前記ノード及びリンクからなるトポロジーマップを用いて、経路を探索するステップと、
前記トポロジーマップ経路探索手段により探索されたトポロジーマップ上の経路に対応する前記探索空間の領域において、経路を探索するステップと
を備える経路探索方法。
【請求項８】
前記探索空間を複数の領域に分割するステップでは、
前記探索空間に含まれる複数の基点について、前記各基点から探索枝を所定の本数となるまで延ばして、前記探索空間を、前記探索枝により接続される点からなる複数の領域として分割する
ことを特徴とする請求項７記載の経路探索方法。
【請求項９】
前記各基点から延ばす前記探索枝の本数を設定可能とする
ことを特徴とする請求項８記載の経路探索方法。
【請求項１０】
前記ノード及びリンクを設定するステップでは、
前記各領域の重心に位置する代表点を前記ノードとして設定すると共に、前記各領域間の隣接関係に従って前記ノード間をリンクで接続する
ことを特徴とする請求項８又は９いずれか１項記載の経路探索方法。
【請求項１１】
前記ノード及びリンクを設定するステップでは、
前記代表点間の幾何学的な距離を前記リンクのコストとし、前記各領域間の隣接関係に従って前記ノード間をリンクで接続する
ことを特徴とする請求項８乃至１０いずれか１項記載の経路探索方法。
【請求項１２】
前記経路を探索するステップでは、
ダイクストラ法を用いて経路を探索する
ことを特徴とする請求項７乃至１１いずれか１項記載の経路探索方法。
【請求項１３】
移動空間内に存在する移動始点より移動を開始し、前記移動空間内に存在する移動終点に到達する経路を探索可能な自律移動体であって、
前記移動空間を、領域に含まれる任意の２点間に経路が存在するように、複数の領域に分割する領域分割手段と、
分割された前記各領域に対してノードを設定すると共に、前記各領域間の隣接関係に従って前記ノード間をリンクで接続することでノード及びリンクを設定するノード・リンク設定手段と、
前記ノード及びリンクからなるトポロジーマップを用いて、経路を探索するトポロジーマップ経路探索手段と、
前記トポロジーマップ経路探索手段により探索されたトポロジーマップ上の経路に対応する前記移動空間の領域において、経路を探索する移動空間経路探索手段と
を備える自律移動体。
【請求項１】
移動空間内に存在する移動始点より移動を開始し、前記移動空間内に存在する移動終点に到達する経路を探索する経路探索システムであって、
前記移動空間を、領域に含まれる任意の２点間に経路が存在するように、複数の領域に分割し、前記各領域に対してノードを設定すると共に、前記各領域間の隣接関係に従って前記ノード間をリンクで接続することで、ノード及びリンクから構成されるトポロジーマップを生成するトポロジーマップ生成手段と、
生成された前記トポロジーマップにおいて、経路を探索するトポロジーマップ経路探索手段と、
前記トポロジーマップ経路探索手段により探索されたトポロジーマップ上の経路に対応する前記移動空間の領域において、経路を探索する移動空間経路探索手段と
を備える経路探索システム。
【請求項２】
前記トポロジーマップ生成手段は、
前記移動空間に含まれる複数の基点について、前記各基点から探索枝を所定の本数となるまで延ばして、前記探索枝により接続される点からなる領域をひとつの領域とすることで、前記移動空間を複数の領域として分割する領域分割手段と、
前記各領域に対してノードを設定すると共に、前記各領域間の隣接関係に従って前記ノード間をリンクで接続するノード・リンク設定手段と
を備えることを特徴とする請求項１記載の経路探索システム。
【請求項３】
前記各基点から延ばす前記探索枝の本数を設定可能とする
ことを特徴とする請求項２記載の経路探索システム。
【請求項４】
前記ノード・リンク設定手段が、
前記各領域の重心に位置する代表点を前記ノードとして設定する
ことを特徴とする請求項２又は３いずれか１項記載の経路探索システム。
【請求項５】
前記ノード・リンク設定手段が、
前記代表点間の幾何学的な距離を前記リンクのコストとし、前記各領域間の隣接関係に従って前記ノード間をリンクで接続する
ことを特徴とする請求項２乃至４いずれか１項記載の経路探索システム。
【請求項６】
トポロジーマップ経路探索手段及び／又は移動空間経路探索手段が、
ダイクストラ法を用いて経路を探索する
ことを特徴とする請求項１乃至５いずれか１項記載の経路探索システム。
【請求項７】
探索空間内に存在する探索開始点より探索を開始し、前記探索空間内に存在する探索終点に到達する経路を探索する経路探索方法であって、
前記探索空間を、領域に含まれる任意の２点間に経路が存在するように、複数の領域に分割するステップと、
分割された前記各領域に対してノードを設定すると共に、前記各領域間の隣接関係に従って前記ノード間をリンクで接続することでノード及びリンクを設定するステップと、
前記ノード及びリンクからなるトポロジーマップを用いて、経路を探索するステップと、
前記トポロジーマップ経路探索手段により探索されたトポロジーマップ上の経路に対応する前記探索空間の領域において、経路を探索するステップと
を備える経路探索方法。
【請求項８】
前記探索空間を複数の領域に分割するステップでは、
前記探索空間に含まれる複数の基点について、前記各基点から探索枝を所定の本数となるまで延ばして、前記探索空間を、前記探索枝により接続される点からなる複数の領域として分割する
ことを特徴とする請求項７記載の経路探索方法。
【請求項９】
前記各基点から延ばす前記探索枝の本数を設定可能とする
ことを特徴とする請求項８記載の経路探索方法。
【請求項１０】
前記ノード及びリンクを設定するステップでは、
前記各領域の重心に位置する代表点を前記ノードとして設定すると共に、前記各領域間の隣接関係に従って前記ノード間をリンクで接続する
ことを特徴とする請求項８又は９いずれか１項記載の経路探索方法。
【請求項１１】
前記ノード及びリンクを設定するステップでは、
前記代表点間の幾何学的な距離を前記リンクのコストとし、前記各領域間の隣接関係に従って前記ノード間をリンクで接続する
ことを特徴とする請求項８乃至１０いずれか１項記載の経路探索方法。
【請求項１２】
前記経路を探索するステップでは、
ダイクストラ法を用いて経路を探索する
ことを特徴とする請求項７乃至１１いずれか１項記載の経路探索方法。
【請求項１３】
移動空間内に存在する移動始点より移動を開始し、前記移動空間内に存在する移動終点に到達する経路を探索可能な自律移動体であって、
前記移動空間を、領域に含まれる任意の２点間に経路が存在するように、複数の領域に分割する領域分割手段と、
分割された前記各領域に対してノードを設定すると共に、前記各領域間の隣接関係に従って前記ノード間をリンクで接続することでノード及びリンクを設定するノード・リンク設定手段と、
前記ノード及びリンクからなるトポロジーマップを用いて、経路を探索するトポロジーマップ経路探索手段と、
前記トポロジーマップ経路探索手段により探索されたトポロジーマップ上の経路に対応する前記移動空間の領域において、経路を探索する移動空間経路探索手段と
を備える自律移動体。
【図１】
【図２】
【図３】
【図４（ａ）】
【図４（ｂ）】
【図４（ｃ）】
【図４（ｄ）】
【図４（ｅ）】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図２】
【図３】
【図４（ａ）】
【図４（ｂ）】
【図４（ｃ）】
【図４（ｄ）】
【図４（ｅ）】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【公開番号】特開２００９−５３８４９（Ｐ２００９−５３８４９Ａ）
【公開日】平成２１年３月１２日（２００９．３．１２）
【国際特許分類】
【出願番号】特願２００７−２１８６３６（Ｐ２００７−２１８６３６）
【出願日】平成１９年８月２４日（２００７．８．２４）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
【公開日】平成２１年３月１２日（２００９．３．１２）
【国際特許分類】
【出願日】平成１９年８月２４日（２００７．８．２４）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
[ Back to top ]