JP4661838B2 - 経路計画装置及び方法、コスト評価装置、並びに移動体 - Google Patents

Description

本発明は、グリッドで表された領域に含まれる始点から終点までの経路のうち、そのコストが最小のものを選択する経路計画装置及び経路計画方法、当該経路計画装置におけるコストを評価するためのコスト評価装置、並びに当該経路計画装置を搭載した移動体に関する。

従来の経路探索装置としては、特許文献１に移動ロボットが出発点から目的地に向かう経路を求める移動ロボット用経路計画システムが開示されている。この経路計画システムは、環境内の物体の幾何マップを用いて、位置姿勢を確率的に表した物体が占有する空間の分布を表したグリッドマップを生成するグリッドマップ生成手段と、生成したグリッドマップから、物体の位置姿勢の不確実性に応じた距離を用いてボロノイグラフを生成するボロノイグラフ生成手段と、生成したボロノイグラフの上で、物体との衝突可能性と経路長をコストとして、与えられた出発点から目的地へ向かう経路の探索を行う経路探索手段とを備える。

この構成により、物体の位置姿勢の不確実性に応じた距離を用いてボロノイグラフを生成して、生成した重み付きボロノイグラフの上で、コスト計算を行い経路の探索を行っているので、従来のボロノイグラフよりも安全な経路を生成する。

このように、従来の経路計画技術としては、上記特許文献１のように、移動空間をグリッド状に近似し、隣接するグリッド点を結ぶ経路のコストを評価する技術が知られている。このような経路探索システムでは、探索された経路は、隣接するグリッド点間を結ぶ線分の集合として表現される。このため、経路のコストは各線分の和として評価している。
特開２００５−３２１９６号公報

ここで、コスト評価を線分の長さのみで行なうと、最短経路探索を行なった場合に最短経路が複数存在する場合がある。図７（ｆ）は、スタートからゴールまでの最短距離を示す図、図７（ａ）乃至図７（ｅ）は、グリッド点間を結んだ場合の最短距離を示す図である。図７（ａ）乃至図７（ｅ）に示す５つの経路コストに差はない。図７（ｆ）に示す理想的な経路は、スタートとゴールを直線で結ぶ経路であるが、図７（ａ）乃至図７（ｅ）の５つの経路のうち、この理想的な経路に近いのは、図７（ｅ）に示す経路である。しかしながら、従来の経路探索システムでは、図７（ａ）乃至図７（ｄ）に示す経路が選ばれることがあり、例えばロボットに当該経路探索システムを搭載した場合には、ロボットが不自然な移動を発現したりするという問題点がある。

すなわち、グリッド格子状に離散化された探索空間では、経路長がグリッド点を結ぶ線分の単純な和として表現されるため、任意の２点を結ぶ直線が最適であることを認識することができなかった。よって、従来の経路探索システムでは、図７（ａ）乃至図７（ｅ）の経路長が同一として求まってしまうため、図７（ｅ）に示す経路ではなく、図７（ａ）乃至図７（ｄ）に示す経路が生成されることがあった。このように、従来の経路探索システムにおいては、経路の角度分解能が低く、理想的な最短経路に近い、グリッド経路を探索することができないという問題点がある。

本発明は、このような問題点を解決するためになされたものであり、グリッドで表現される探索空間において最適な経路を生成することができる経路探索装置、経路探索方法、経路探索装置で使用される最適な経路を生成するためのコスト評価装置、及び経路探索装置を搭載した移動体を提供することを目的とする。

本発明にかかる経路探索装置は、グリッドで表された領域に含まれる始点から終点までの経路のうち、そのコストが最小のものを選択する経路探索装置であって、各グリッドをノードとし、現在注目する注目ノードからこれに隣接する隣接ノードへ伸ばしたエッジのコストを、当該注目ノードから経路に沿って前記始点方向に所定数さかのぼったノードを基準ノードとし、
（該基準ノードから前記隣接ノードまでの最短距離を示す第１の距離）−（前記基準ノードから前記注目ノードまでの最短距離を示す第２の距離）
として求めた値とし、前記始点から前記終点までの経路コストを算出する経路コスト算出部と、当該経路コストに応じて経路を決定する経路決定部とを有するものである。

本発明においては、注目ノードから隣接ノードへ伸ばしたエッジのコストを、単純にその距離とするのではなく、基準ノードから隣接ノード（エッジの先端に位置する先端ノード）までの第１の距離と、基準ノードから注目ノード（エッジの根元に位置する根元ノード）までの第２の距離とから求める。基準ノードは、注目ノードから１以上さかのぼったノードであり、複数のノード間の最短距離に基づきエッジのコストを算出するため、エッジの距離として一律にコスト評価するのに比べ、そのエッジのパターンに基づいたより詳細なコスト評価を行なうことができ、経路の角度分解能が向上してより理想的な最短距離に近い経路を生成することができる。

また、前記経路コスト算出部は、前記始点から前記終点までの経路コストを当該経路を構成するエッジのコストの和として算出することができる。単なる距離ではなく、（第１の距離−第２の距離）として求めたエッジのコストの総和を経路コストとすることができる。

さらに、前記経路コスト算出部は、前記第１の距離及び前記第２の距離、又は（第１の距離−第２の距離）で求まるコストが格納されたデータベースを参照して前記始点から前記終点までのコストを算出することができる。データベースを参照することで経路コスト算出を高速化することができる。

さらにまた、（前記第１の距離−前記第２の距離）を算出して前記データベースを構築するデータベース生成部を更に有することができ、例えば経路探索を実行しつつデータベースを生成することができる。

また、前記始点から任意のノードまでのコストを当該任意のノードのコストとしたとき、エッジの先端に位置する先端ノードのうち、ノードのコストが最小のノードを注目ノードとし、注目ノードの少なくとも前方、斜め右前、及び斜め左前に位置する隣接ノードに伸ばすエッジを生成するエッジ生成部と、前記第１の距離及び前記第２の距離を算出又はデータベースから読み出すエッジコスト算出／読出部とを有し、前記経路コスト算出部は、前記始点から前記隣接ノードまでの経路コストを当該経路を構成するエッジのコストの和として算出し、前記経路決定部は、前記隣接ノードが前記終点の場合、前記始点から前記終点までのコストが最小の経路を選択することができる。注目ノードからエッジを生成し、エッジのコストを求め、経路を構成するエッジのコストの総和を経路コストとして求めることができる。

さらに、前記エッジ生成部は、前記注目ノードが前記始点の場合、当該始点の８近傍の隣接ノードにエッジを延ばすことができる。このことにより、始点に対する終点の位置が不明な場合であってもエッジを伸ばすことができる。なお、終点近傍の方向にのみ始点からエッジを伸ばすことも可能である。

さらにまた、前記エッジコスト算出／読出部は、前記始点から前記隣接ノードまでのノード数が前記所定数になるまでは、前記注目ノードから前記隣接ノードまで伸ばしたエッジのコストを前記注目ノードと前記隣接ノードとの距離とすることができ、所定数に達しない場合は、ノードのコストを距離とすることができる。

同様に、前記経路コスト算出部は、前記始点から前記隣接ノードまでのノード数が前記所定数になるまでは、前記隣接ノードのコストを前記始点から前記隣接ノードまでの経路を構成するエッジの距離の和として算出することができる。

本発明にかかるコスト評価装置は、グリッドで表された領域に含まれる始点から終点までの経路のうち、そのコストが最小のものを選択する経路探索に使用するコスト評価装置であって、各グリッドをノードとし、現在注目する注目ノードからエッジを伸ばすノードを隣接ノードとし、前記注目ノードから経路に沿って前記始点方向に所定数さかのぼったノードを基準ノードとしたとき、当該基準ノードから前記隣接ノードまでの最短距離を示す第１の距離、及び前記基準ノードから前記注目ノードまでの最短距離を示す第２の距離を算出、又はデータベースから読み出す距離準備部と、前記注目ノードから前記隣接ノードへ伸ばすエッジのコストを（前記第１の距離−第２の距離）として算出するエッジコスト算出部とを有するものである。

本発明にかかる経路探索方法は、グリッドで表された領域に含まれる始点から終点までの経路のうち、そのコストが最小のものを選択する経路探索方法であって、各グリッドをノードとし、現在注目する注目ノードから、これに隣接する隣接ノードへ伸ばすエッジのコストを、当該注目ノードから経路に沿って前記始点方向に所定数さかのぼったノードを基準ノードとし、
（当該基準ノードから前記隣接ノードまでの最短距離を示す第１の距離）−（前記基準ノードから前記注目ノードまでの最短距離を示す第２の距離）
として求めた値として前記始点から前記終点までの経路コストを算出する経路コスト算出工程と、当該経路コストに応じて経路を決定する経路決定工程とを有するものである。

本発明にかかる経路探索方法は、グリッドで表された領域に含まれる始点から終点までの経路のうち、そのコストが最小のものを選択する経路探索方法であって、各グリッドをノードとし、前記始点の８近傍の隣接ノードにエッジを延ばし、当該始点から前記隣接ノードまでの距離を当該隣接ノードのコストとして算出する第１工程と、前記始点から一のノードまでのコストを当該一のノードのコストとしたとき、エッジの先端に位置する先端ノードのうち、ノードのコストが最小のノードを注目ノードとし、当該注目ノードの少なくとも前方、斜め右前、及び斜め左前に位置する隣接ノードまでエッジを延ばし、当該隣接ノードのコストを算出する第２工程と、当該隣接ノードが前記終点の場合、前記始点から前記終点までのコストが最小の経路を選択する第３工程とを有し、前記隣接ノードが前記終点となるまで前記第２工程を繰り返すものであって、前記第２工程は、前記注目ノードから経路に沿って始点方向に所定数さかのぼったノードを基準ノードとしたとき、当該基準ノードから前記隣接ノードまでの最短距離を示す第１の距離、及び前記基準ノードから前記注目ノードまでの最短距離を示す第２の距離を算出し、又はデータベースから読み出す距離準備工程と、前記注目ノードから前記隣接ノードへ伸びるエッジのコストを（前記第１の距離−第２の距離）として算出するエッジコスト算出工程と、前記始点から前記隣接ノードまでの経路コストを当該経路を構成するエッジのコストの和として算出する経路コスト算出工程とを有するものである。

本発明においては、（第１の距離−第２の距離）によりエッジコストを算出し、このエッジコストの和を経路コストとし、隣接ノードが終点となるまでエッジを増殖しながら処理を繰り返すことにより、経路探索におけるコストを正確に評価することができ、経路の角度分解能を向上させることができる。

本発明にかかる移動体は、移動手段と、前記移動手段を駆動する駆動手段と、周囲の環境をグリッドで表し、当該グリッド空間に含まれる始点から終点までの経路のうち、そのコストが最小のものを選択する経路探索手段と、前記経路探索手段の経路探索結果に基づき前記駆動手段を制御する制御手段とを備え、前記経路探索手段は、各グリッドをノードとし、現在注目する注目ノードからこれに隣接する隣接ノードへ伸ばしたエッジのコストを、当該注目ノードから経路に沿って前記始点方向に所定数さかのぼったノードを基準ノードとし、（当該基準ノードから前記隣接ノードまでの最短距離を示す第１の距離）−（前記基準ノードから前記注目ノードまでの最短距離を示す第２の距離）として求めた値とし、前記始点から前記終点までの経路コストを算出する経路コスト算出手段と、当該経路コストに応じて経路を決定する経路決定手段とを有するものである。

本発明においては、経路探索手段がエッジの距離として一律にコスト評価するのに比べ、そのエッジのパターンに基づいたより詳細なコスト評価により経路の角度分解能が向上してより理想的な最短距離に近い経路を生成することができるので、移動体により自然な移動動作を表出させることができる。

本発明によれば、グリッドで表現される探索空間において最適な経路を生成することができる経路探索装置、経路探索方法、経路探索装置で使用される最適な経路を生成するための経路評価装置、及び経路探索装置を搭載した移動体を提供することができる。

以下、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。この実施の形態は、本発明を、ロボット等に搭載する経路探索装置に適用したものである。

本実施の形態にかかる経路探索装置においては、グリッド状の探索空間における経路探索において、理想的な最短距離にできるだけ近いグリッド経路を生成する。そして、経路の線分の長さのみならず、その経路の通過地点を考慮することにより、経路中の各エッジ（線分）のコストを評価する。ここでは先ず、このような経路探索装置が搭載されるロボットについて説明しておく。

図１は、本発明の実施の形態にかかるロボットを示す斜視図である。図１に示すように、ロボット１は、体幹部ユニット５の所定の位置に頭部ユニット２、左右２つの腕部ユニット３ａ、３ｂ、及び左右２つの脚部ユニット４ａ、４ｂが連結されている。本実施の形態にかかるロボット１は、脚部ユニット４ａ、４ｂに内蔵された車輪により移動可能に構成される。なお、ロボットは、２足若しくは４足歩行タイプのロボットであってもよい。

頭部ユニット２には、撮像部６が備えられ、周囲所定範囲を撮影することが可能である。また、頭部ユニット２には、図示せぬマイク、スピーカ等が備えられ、ユーザからの呼びかけを認識したり、それに答えたりすることも可能である。頭部ユニット２は、体幹部ユニット５に対して床面に対して水平な面内で左右方向に回動可能に接続されており、頭部ユニット２を回動することで撮像する範囲を状況に応じて変更し、周囲の環境を撮像することができる。

腕部ユニット３ａ、３ｂ及び脚部ユニット４ａ、４ｂは、体幹部ユニット５に内蔵された制御部に含まれる演算処理部によって、所定の制御プログラムに従って各腕部ユニット３ａ、３ｂに含まれる関節部、脚部ユニット４ａ、４ｂに含まれる車輪の駆動する量が制御され、各関節の関節駆動角度及び車輪の回転角度が決定されることで、所望の位置および姿勢をとるものである。

図２は、本実施の形態にかかるロボットを示すブロック図である。ロボット１は、制御部１０１、入出力部１０２、駆動部１０３、電源部１０４、及び外部記憶部１０５などを有している。

入出力部１０２は、周囲の映像を取得するためのＣＣＤ（Charge Coupled Device）などからなるカメラ１２１、周囲の音を集音するための１又は複数の内蔵マイク１２２、音声を出力してユーザと対話等を行なうためのスピーカ１２３、ユーザへの応答や感情等を表現するためのＬＥＤ１２４、タッチセンサなどからなるセンサ部１２５などを備える。

また、駆動部１０３は、モータ１３１及びモータを駆動するドライバ１３２などを有し、ユーザの指示などに従って脚部ユニット４ａ，４ｂや腕部ユニット３ａ，３ｂを動作させる。電源部１０４は、バッテリ１４１及びその放充電を制御するバッテリ制御部１４２を有し、各部に電源を供給する。

外部記憶部１０５は、着脱可能なＨＤＤ、光ディスク、光磁気ディスク等からなり、各種プログラムや制御パラメータなどを記憶し、そのプログラムやデータを必要に応じて制御部１０１内のメモリ（不図示）等に供給する。

制御部は、ＣＰＵ（Central Processing Unit）、ＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）、無線通信用のインターフェースなどを有し、ロボット１の各種動作を制御する。そして、この制御部１０１は、例えばＲＯＭに格納された制御プログラムに従って、カメラ１２１により取得した映像を解析する画像認識モジュール１２、画像認識結果に基づき、経路探索を行なう経路探索モジュール１３、各種認識結果に基づきとるべき行動を選択する行動決定モジュール１４、音声認識を行なう音声認識モジュール１５などを有する。特に、本実施の形態においては、経路探索モジュール１３により、理想的に近い探索経路が生成され、これに基づき駆動部１０３を制御するため、ロボット１はより自然な移動動作を表出する。

ここで、本実施の形態にかかるロボット１は、経路探索モジュール１３において、移動空間をグリッド状に近似し、とるべき経路を探索する。次に、本実施の形態にかかる経路探索モジュールについて詳細に説明する。

図３は、本実施の形態にかかる経路探索モジュールの詳細を示す図である。経路探索モジュール１３は、経路出力部２０及び距離・コストデータベース（ＤＢ）３０を有する。距離・コストＤＢ３０には、予め算出された各グリッドから各グリッドまでのコスト及び各グリッド間の距離が格納されている。特に、本実施の形態においては、各グリッドをノードとし、現在注目する注目ノードからエッジを伸ばすノードを隣接ノードとし、注目ノードから経路に沿って始点方向に所定数さかのぼったノードを基準ノードとしたとき、基準ノードから隣接ノードまでの最短距離を示す第１の距離、及び基準ノードから注目ノードまでの最短距離を示す第２の距離が格納されている。このことにより、注目ノードから隣接ノードへ伸ばしたエッジのコストを、基準ノードから隣接ノード（エッジの先端に位置する先端ノード）までの第１の距離と、基準ノードから注目ノード（エッジの根元に位置する根元ノード）までの第２の距離とから求めることができる。基準ノードは、注目ノードから所定数さかのぼったノードであり、複数のノード間の最短距離に基づきエッジのコストを算出するため、エッジの距離として一律にコスト評価するのに比べ、そのエッジのパターンに基づいたより詳細なコスト評価を行なうことができ、経路の角度分解能が向上してより理想的な最短距離に近い経路を生成することができる。

経路出力部２０は、エッジ生成部２１、エッジコスト算出／読出部２２、経路コスト算出部２３、及び経路決定部２４を有する。なお、本実施の形態においては、コスト及び距離が格納された距離・コストＤＢ３０を有し、このデータを経路出力部２０が適宜参照して経路探索を行なうものとして説明するが、経路出力部２０において、コスト及び距離を随時算出してもよい。また、距離・コストＤＢ３０は、経路出力部２０のエッジコスト算出／読出部２２及び経路コスト算出部２３により構築することが可能である。

ここで、本実施の形態においては、新しい環境を認識する毎に、当該環境をグリッドで現された探索空間として認識するものとする。そして、ロボット装置は、各グリッドをノードとする探索の問題に帰着し、ダイキストラ法、Ａ＊探索、ＤＰマッチング等の公知のアルゴリズムにより、最適経路を求めることができる。その際、ノード間の距離及びコストを算出し、距離・コストＤＢ３０に登録するものとする。なお、ロボットにおいて新しい環境となる場合に当該環境におけるノード間の距離・コストを外部から入力するようにしてもよい。また、エッジ生成、エッジコスト読出／算出、経路コスト算出、経路決定の各処理は、数ｍ秒乃至は数秒に１回の割合で実施し、当該タイミング毎に経路を生成するものとする。なお、当該タイミング毎に障害物情報などは画像認識モジュールやセンサからの情報として入力されるものとする。以下、各ブロックについて詳細に説明する。

エッジ生成部２１は、始点から一のノードまでのコストを当該一のノードのコストとしたとき、エッジの先端に位置する先端ノードのうち、ノードのコストが最小のノードを注目ノードとし、注目ノードの少なくとも前方、斜め右前、及び斜め左前に位置する隣接ノードに伸ばすエッジを生成する。ここで、前方とは、注目ノードに入るエッジの向く方向である。図４及び図５は、エッジ及びそのコストを説明するための模式図である。

エッジ生成部２１は、注目するノードからその前方、斜め右前、斜め左前の３方向にエッジを生成する。図４、図５は、４×４のグリッド（ｎ１〜ｎ１６）を示している。この図４において、注目するノードがｎ７である場合、ノードｎ６からノードｎ７に向く方向が前方であり、よって前方ノードはノードｎ８、斜め右前ノードはノードｎ４、斜め左ノードはノードｎ１２となる。エッジ生成部２１は、ノードｎ７からこれらノードｎ４、ｎ８、ｎ１２に伸びるエッジを生成する。ここで、後述するように、注目ノードが始点である場合には、３方向ではなく、上下左右斜め方向８近傍の隣接するノード全てに対してエッジを生成するものとする。なお、本実施の形態においては、始点以外のノードにおいては、前方、斜め右前、斜め左前の３方向のみエッジを生成するものとして説明するが、全ノードにおいて８近傍の隣接ノードに対してエッジを生成してもよく、又は例えば前方、右斜め前、左斜め前に加え、右横及び左横ノードを含めた５つのエッジを生成するものとしてもよい。本実施の形態のように、３方向のみエッジを生成することとすると計算量が少なく、処理が高速化する。また、本実施の形態においては、始点からは８近傍の隣接ノード全てにエッジを生成するものとするが、始点から終点に向く方向を求め、この方向に近い方向となる一部の隣接するノードにのみエッジを生成するようにしてもよい。

ここで、エッジ生成部２１は、後述する各ノードのコストが最も小さいノードを注目ノードとし、そこから３近傍ノードにエッジを生成する。本明細書において各ノードのコストとは、始点から当該ノードまでのコストを示すものとする。エッジを生成したら、さらに現在存在するエッジの全ての先端ノードが有するノードのコストのうち、最小のノードのコストとなるノードを選択し、同様にエッジを生成する、という処理を繰り返す。

エッジコスト算出／読出部２２は、基準ノードから隣接ノードまでの最短距離を示す第１の距離、及び基準ノードから注目ノードまでの最短距離を示す第２の距離を算出、又はデータベースから読み出す距離準備部２２ａと、注目ノードから隣接ノードへ伸ばすエッジのコストを（前記第１の距離−第２の距離）として算出するエッジコスト算出部２２ｂとを有する。なお、エッジコストは、各パターンについて予め算出され、距離・コストＤＢ３０に格納されているものとしてもよい。その場合は、エッジコスト算出部２２ｂは不要である。

次に、距離・コストＤＢ３０に格納されているエッジコストの算出方法について説明する。従来、エッジのコストを評価する際には、単純にエッジの長さのみを使用していたのに対し、本実施の形態においては、注目しているノードに経路が到達するまでのエッジの形状からエッジのコストを評価（算出）する。エッジの形状のパターンは、エッジをどこまでさかのぼって評価するかによる。評価に含める経路長を長くする（エッジ数を多くする）ほど、後述する経路の角度分解能は向上する。図６は、２つ分のエッジをさかのぼる場合のエッジの形状を示す図である。この場合、エッジの総数は３であるので、２^３＝８通りのエッジのパターンを有することになる。なお、従来の方法で算出したコストと異なるのは、図６（ａ）〜（ｃ）及び図６（ｅ）〜（ｇ）であり、図６（ｄ）及び（ｈ）は従来法と同一コストになる。図６（ａ）乃至図６（ｈ）に示すように、注目するノードｇ２から伸ばしたエッジの先端ノード（隣接ノード）ｇ３を現す経路パターンが８種類存在する。ここで、当該注目するノードｇ２から経路に沿って始点方向に所定数さかのぼったノードを基準ノードとするが、本例では、２つノード（エッジ）をさかのぼったノードｇ１を基準ノードとする。注目ノードｇ２→先端ノードｇ３のコストは、基準ノードｇ１から先端ノードｇ３の距離及び基準ノードｇ１から注目ノードｇ２の距離から求めることができる。なお、本実施の形態においては、エッジを２つ分さかのぼることとして説明するが、３つ以上さかのぼってもよく、又は１つのみさかのぼることとしてもよい。

エッジコストの算出について具体的に説明する。図４に示すノードｎ７を注目ノード、ノードｎ１２、ｎ８、ｎ４を、ノードｎ７から延ばしたエッジの先端ノードとするとき、ノードｎ７からは、エッジ生成部２１により、エッジＥ１、Ｅ２、Ｅ２が生成される。ここでｎ７→ｎ１２のエッジＥ１のコストを求める方法について説明する。

上述したように、基準ノードはノードｎ７から２つ分のエッジをさかのぼったノードｎ５となる。この基準ノードｎ５からエッジの先端であるノードｎ１２までの距離Ｌ１を距離・コストＤＢ３０から読み出す。距離Ｌ１は、ノードｎ５→ｎ１２の最短距離で表される。これを第１の距離とする。なお、本明細書においては、各ノード間の距離を１として説明する。本例では、距離Ｌ１は、√（３^２＋１^２）＝√（１０）≒３．１６となる。

次に、基準ノードｎ５からエッジの根元である注目ノードｎ７までの距離Ｌ２を距離・コストＤＢ３０から読み出す。距離Ｌ２は、ノードｎ５→ｎ７の最短距離で表される。これを第２の距離とする。本例では、距離Ｌ２は、"２"となる。そして、エッジＥ１のコスト＝第１の距離Ｌ１−第２の距離Ｌ２＝３．１６−２＝１．１６として求めることができる。

他のエッジＥ２、Ｅ３についても同様に求めることができる。すなわち、エッジＥ２のコストは、{基準ノードｎ５からエッジＥ２の先端ノードｎ８までの距離（第１の距離）}−{基準ノードｎ５からエッジＥ２の根元ノードｎ７までの距離（第２の距離）}＝３−２＝１となる。また、エッジＥ３のコストは、{基準ノードｎ５からエッジＥ３の先端ノードｎ４までの距離（第１の距離）}−{基準ノードｎ５からエッジＥ３の根元ノードｎ７までの距離（第２の距離）}＝３．１６−２＝１．１６となる。

同様に、図５において、注目ノードｎ１１からエッジを２つさかのぼったノードｎ５が基準ノードとなり、注目ノードｎ１１からノードｎ１２に伸ばしたエッジＥ４のコストは、{基準ノードｎ５からエッジＥ４の先端ノードｎ１２までの距離（第１の距離）}−{基準ノードｎ５からエッジＥ２の根元ノードｎ１１までの距離（第２の距離）}＝３．１６−２．２４＝０．８９となる。

また、エッジＥ５のコストは、{基準ノードｎ５からエッジＥ５の先端ノードｎ１６までのコスト（第１の距離）}−{基準ノードｎ５からエッジＥ５の根元ノードｎ１１までの距離（第２の距離）}＝３．６１−２．２４＝１．３７となる。さらに、エッジＥ６のコストは、{基準ノードｎ５からエッジＥ５の先端ノードｎ８までの距離（第１の距離）}−{基準ノードｎ５からエッジＥ６の根元ノードｎ１１までの距離（第２の距離）}＝３−２．２４＝０．７６となる。

経路コスト算出部２３は、始点から、エッジ生成部２１にて生成されたエッジの先端ノードまでのコストを算出する。この場合、各エッジのコストは距離・コストＤＢ３０に格納されているため、各エッジのコストを読出し、加算すればよい。例えば、図４に示す例では、始点からノードｎ８のコストは、（始点から基準ノードｎ５までのコスト）＋（ｎ５→ｎ６のコスト）＋（ｎ６→ｎ７のコスト）＋（エッジＥ１のコスト＝１．１６）で現される。ここで、ノードｎ５→ｎ６のコストや、ノードｎ６→ｎ７のコストは、当該ノードｎ５、ｎ６に対する基準ノードによって異なってくる。すなわち、従来法のように、単純にこれらのコスト＝距離＝１とならない場合がある。同様に、図５に示す例では、始点からノードｎ１２までのコストは、（始点から基準ノードｎ５までのコスト）＋（ｎ５→ｎ６のコスト）＋（ｎ６→ｎ１１のコスト）＋（ｎ１１→ｎ１２のコスト＝０．８９）となる。

経路決定部２４は、エッジ生成部２１が生成したエッジの先端ノードが終点になった場合、当該始点から終点までのコストが最小のものを経路として選択する。図７は、ノードｎ７が始点、ノードｎ２４が終点の経路を示す図である。図７（ａ）乃至図７（ｅ）には、従来のコスト算出方法では同一コストとなる。よって、経路決定部２４において、図７（ａ）乃至図７（ｄ）に示すような理想的な経路から外れた経路が選択される場合があった。これに対し、本実施の形態にかかる経路コスト算出部２３の経路算出結果としては、図７（ｅ）に示すものがコスト最小と算出される。ここで、ノードｎ７からノードｎ２４までの経路は、ノードを無視すると単純にこれらのノードを直線で結んだ図７（ｆ）に示すものが最短の理想的な経路となる。本実施の形態においては、経路を構成するエッジのコストを単なる距離とするのではなく、基準ノードにさかのぼり、その基準ノードからのエッジの先端及び根元ノードの距離の差により評価することで、図７（ｆ）に示す理想的な最短経路に最も近い図７（ｅ）に示す経路を選択することができる。

次に、本実施の形態にかかる経路探索結果、生成される経路の分解能について説明する。ノード間を結ぶエッジのみで経路を表現する場合、あるノードから隣接する８近傍のノードにエッジを延ばすことができる。このとき、エッジとエッジとの間の角度は最小で４５°となる。これに対し、本実施の形態のように、２つ分のエッジをさかのぼり、３つのエッジで表現するとした場合、図６（ｈ）、図６（ｂ）、図６（ｃ）、図６（ｄ）に示すように、４５°の範囲内に４つのパターンを含む。すなわち、約４５／４≒１１．３°の分解能となり、１本のエッジで表現する場合に比して４倍の分解能とすることができる。これにより、図７（ａ）乃至図７（ｄ）ではなく、図７（ｅ）のような経路を生成することを可能としている。このことにより、ロボットの旋回角度が４５°未満の経路を生成することができ、ロボットが急旋回することなく、なめらかな動作をすることができる。

図８、図９はそれぞれ従来及び本実施の形態にかかるロボットの移動経路を示す図である。図８に示すように、従来のロボット装置は、斜め右若しくは左に４５°、又は９０°に旋回する経路しかとることができない。これに対し、本実施の形態のように、２つ分エッジをさかのぼり、３つのエッジで経路を表現することができることとすると、図９に示すように、０乃至４５°の間に、２つ、すなわち、１１．３°付近と、２２．６付近にも旋回することができるようになり、なめらかな曲線の経路を描くことができ、ロボット装置として自然な移動動作を表出させることができる。

次に、本実施の形態にかかる経路探索方法について、図１０及び図１１に示すフローチャート、並びに図１２乃至図１８に示す経路図を参照して具体的に説明する。図１０は、実施の形態にかかる経路決定方法を示すフローチャート、図１１は、エッジのコスト算出方法を示すフローチャートである。図１２、図１３は、本実施の形態にかかる経路探索方法を示す図、図１４、図１５は、従来の経路探索方法を示す図、図１６は本実施の形態にかかる経路探索方法によって生成される経路を示す図、図１７、図１８は、障害物がある場合の本実施の形態にかかる経路探索方法である。

図１０に示すように、先ず、エッジ生成部２１は、注目するノードが始点であることを確認すると、左右上下斜めの隣接８近傍のノード全てにエッジを伸ばす（ステップＳ１）。そして、この段階では、始点からの距離＝ノードのコストとして従来と同様の方法にてコストを算出する（ステップＳ２）。

具体的には、図１２（ａ）に示すように、エッジ生成部２１は、始点ノードｎ７の隣接８近傍ノード全てに対してエッジを生成する。なお、本例においては、左側がグリッド領域の境界のため、左横、左上、左下へエッジを伸ばすことはできない。これにより、始点ノードｎ７からノードｎ１、ｎ２、ｎ８、ｎ１４、ｎ１３に伸びるエッジが生成される。そして、エッジコスト算出／読出部２２が注目ノードである始点ｎ７から次のノードｎ１、ｎ２、ｎ８、ｎ１４、ｎ１３へ伸ばしたエッジのコスト＝距離として、コストを求める。ここでは、始点ｎ７からのエッジのコストが各ノードｎ１、ｎ２、ｎ８、ｎ１４、ｎ１３のコストとなる。すなわち、エッジコスト算出／読出部２２は、始点ノードｎ７からの距離を読み出す。

次に、エッジ生成部２１は、各エッジの先端ノードにおいて、コストが最小のノードを選択する（ステップＳ３）。図１２（ｂ）において、始点からｎ１、ｎ２、ｎ８、ｎ１４、ｎ１３までのコストは、その距離であるから、それぞれ１、１．４１、１、１．４１、１となる。このうちエッジ生成部２１は、最小コストのノードｎ８、ｎ１３を選択する。なお、ｎ１はグリッドの最外周であり、これ以上コストは算出できないので、計算対象外となる。

次に、エッジ生成部２１は、ｎ８、ｎ１３から、ｎ８、ｎ１３に入るエッジの方向を前方とし、前方、右斜め前、左斜め前の３方向にエッジを伸ばす（ステップＳ４）。次に、エッジコスト算出／読出部２２は、エッジのコストを求める前に、始点から当該エッジの先端ノードまでのノード数Ｎ≧３であるか否かを判定する。本実施の形態においては、注目するノードから２つエッジをさかのぼってコストを評価することとしているため、始点から先端ノードまでの数Ｎ＝３以上になるまでは、従来法と同様のコスト算出方法とする。

図１２（ｂ）に示すように、ｎ８、ｎ１３から伸ばしたエッジの先端ノードまでのノード数は２であるので、ステップＳ２に戻り、上述と同様、当該エッジのコスト＝エッジの距離として求めることができる。そして、各エッジの先端ノードのコストは、それぞれ始点からの距離となる。例えばｎ８からは右斜め前ノードｎ３、前方ノードｎ９、左斜め前ノードｎ１５までエッジが伸び、各先端ノードのコストはそれぞれ２．４１、２、２．４１と求まる（ステップＳ３）。

次にノードのコストが最小のノードは、ｎ２、ｎ１４の１．４１である（ステップＳ３）。よって、このｎ２、ｎ１４から、右斜め前、前方ノード、左斜め前のノードにエッジを伸ばす（ステップＳ４）。ここで、ｎ２から右斜め前方ノードは存在せず、左斜め前ノードはｎ３となる。この場合、ｎ３はｎ８からのエッジが伸びている。このような場合は、ノードのコストが低い方の経路を採用するものとする。ノードのコストが同一の経路は、先に伸びた経路を採用するものとする。本例においては、ノードｎ２からの経路におけるノードｎ３のコストは２．４１であり、ノードｎ８からの経路におけるノードｎ３のコストは２であるから、ノードｎ２からの経路は採用されない。同様にノードｎ１４からノードｎ１５、ｎ２１、ｎ２０にエッジが伸びるが、ノードｎ１５、ｎ２０へ伸びるエッジはそれぞれノードｎ８からの経路、ノード１３からの経路が採用されるため、ノードｎ１４からの経路はいずれも採用されない。エッジの伸ばした先端ノードまでの数が３になるまでステップＳ２からの処理を繰り返す。

次に、エッジコスト算出／読出部２２は、各エッジの先端ノードのコストが最小のものを選択すると、図１２（ｂ）の状態では、ノードｎ９、ｎ１９が選択される。そこで、図１２（ｃ）に示すように、ノードｎ９、ノードｎ１９から前方及び左右斜め前ノードにエッジを伸ばす。次にエッジのコストを算出するが、先ず、エッジの伸びた先の先端ノードｎ４、ｎ１０、ｎ１６、ｎ２５、ｎ２６において、始点ノードｎ７からのノード数が３以上であるか判定する。ここでは、ノード数が３となり、ステップＳ６に進む。

ステップＳ６では、現在の注目ノードｎ９、ｎ１９からエッジを２つさかのぼって、そのエッジパターンを調べる。ノードｎ９からエッジを２つさかのぼるとノードｎ７（始点ノード）になり、これが基準ノードとなる。エッジパターンは、基準ノードｎ７から、先端ノードｎ４、ｎ１０、ｎ１６までのパターンである。このエッジパターンに基づき、先端ノードｎ４、ｎ１０、ｎ１６のコストを算出する（ステップＳ７）。

ここで、先端ノードｎ４、ｎ１０、ｎ１６のコストは、経路コスト算出部２３により、始点から基準ノードまでのコスト＋基準ノードから先端ノードまでのエッジパターンに基づくコストによって求めることができる。始点から基準ノードまでのコストは、始点から基準ノードまでの経路を構成するエッジの各コストをエッジコスト算出／読出部２２により距離・コストＤＢ３０から読出し、加算すればよい。本例においては、基準ノード＝始点であるため、基準ノードから先端ノードまでのエッジパターンにより、当該先端ノードのコストが求まり、先端ノードｎ４、ｎ１０、ｎ１６のコストは、それぞれ３．１６、３、３．１６と求まる。同様に、先端ノードｎ２５、ｎ２６のコストは３、３．１６と求まる。これらの処理を先端ノードが終点ノードｎ２４となるまで繰り替えし（ステップＳ８）、図１２（ｄ）、図１２（ｅ）、図１２（ｆ）、図１３（ａ）を経て、図１３（ｂ）の状態となる。このとき、ノードｎ１７から伸ばしたエッジが終点ノードｎ２４に達するため、先端ノード＝終点ノードｎ２４となる。この時点で最小コストの経路を選択して出力する（ステップＳ９）。本例においては、図１３（ｃ）に示すような経路が生成される。この経路は、図９（ｆ）に示す経路に最も近い経路となっている。

次に、始点からエッジの先端ノードまでのコスト（先端ノードのコスト）を算出する場合の方法について図１１を参照して説明する。上述したように注目ノードｎ９の基準ノードは始点であるノードｎ７である。先ず、ノードｎ９→ｎ４のエッジのコストを求める。この場合、先ず、図１１に示すように、基準ノードｎ７からエッジの先端ノートｎ４までの距離Ｌ１（＝３．１６）を算出する（ステップＳ１１）。次に、基準ノードｎ７からエッジの根元ノードｎ９までの距離Ｌ２（＝２）を算出する（ステップＳ１２）。エッジのコストは、Ｌ１−Ｌ２＝１．１６と求めることができる（ステップＳ１３）。このとき、距離Ｌ１、Ｌ２、エッジのコストを距離・コストＤＢ３０に登録することで、データベースを構築することができる。なお、エッジコスト算出／読出部２２は、このようにエッジのコストを算出してもよいし、距離Ｌ１、Ｌ２を距離・コストＤＢ３０から読出し、Ｌ１−Ｌ２を算出するようにしてもよい。データベースを構築する場合は、全ノードについて、距離及びコストを計算する（ステップＳ１５、Ｓ１６）。

同様に、ｎ９→ｎ１０のエッジコストは、{基準ノードｎ７からエッジの先端ノートｎ１０までの距離Ｌ１（＝３）}−{基準ノードｎ７からエッジの根元ノードｎ９までの距離Ｌ２（＝２）}となり、１と求めることができる。また、ｎ９→ｎ１６のエッジコストは、{基準ノードｎ７からエッジの先端ノートｎ１６までの距離Ｌ１（＝３．１６）}−{基準ノードｎ７からエッジの根元ノードｎ９までの距離Ｌ２（＝２）}となり、１．１６と求めることができる。以上により、各ノードｎ４、ｎ１０、ｎ１６のコストは、それぞれ３．１６、１、３．１６となる。同様に、ノードｎ２６のコストは３．１６、ｎ２５のコストは３となる。

次に、図１２（ｅ）に示すノードｎ２７のコストについて説明する。ノードｎ２７には、ノードｎ２０からと、ノードｎ２１からのエッジが入る経路が生成されるが、先に生成されるノードｎ２０からの経路の方が、後で生成されるノードｎ２１からの経路よりコストが大きく、後で生成されるノードｎ２１からの経路が採用される例である。

先ず、ｎ２０からｎ２７へエッジを伸ばしてｎ２７のコストを求める場合について説明する。ノードｎ２０→ｎ２７のエッジのコストは、（基準ノードｎ７からノードｎ２７までの距離）−（基準ノードｎ７からノードｎ２０までの距離）で求まり、３．６１−２．２４＝１．３７となる。この場合、ｎ２７のコストは、（ｎ７→ｎ１３のコスト）＋（ｎ１３→ｎ２０のコスト）＋（ｎ２０→ｎ２７のコスト）となり、１＋１．４１＋１．３７＝３．７８となる。

その後、ノードｎ２１からｎ２７へエッジを伸ばす経路が生成される。この場合、ｎ２１→ｎ２７のエッジのコストは、（基準ノードｎ７からｎ２７までの距離）−（基準ノードｎ７からｎ２１までの距離）で求まり、３．６１−２．８２＝０．７９となる。この場合、ｎ２７のコストは、（ｎ７→ｎ１４のコスト）＋（ｎ１４→ｎ１０のコスト）＋（ｎ２１→ｎ２７のコスト）となり、１．４１＋１．４１＋０．７９＝３．６１となる。このように、ノードｎ２７についてノードｎ２０から伸びるエッジによる経路の方が早く生成されるが、ノードｎ２７のコストとしてｎ２１を経由する経路の方が安いため、ここではノードｎ２７の経路としてノードｎ２６からの経路は不採用となり、ノードｎ２１からのコストが採用となる。

これに対し、図１４、図１５に示すように、従来法により経路探索すると、図１５（ｅ）に示すようなコストが抽出されてしまう。その理由は、この経路が従来法においては、破線で示す理想の経路と同様のコストになってしまうからである。本実施の形態においては、基準ノードからエッジの先端及び根元ノードまでの距離に基づき、当該エッジのノードを算出するため、理想的な経路に最も近い経路が生成される。

図１６は、始点ＳＴから本実施の形態にかかる経路探索方法にて経路を生成した図である。特に終点を設定しない場合は、図１６に示すように経路が生成されていく。

図１７、図１８は、障害物がある場合の経路探索結果を示す。図１７、図１８に示すように、ノードｎ８、ｎ９、ｎ１４、ｎ１５、ｎ２０、ｎ２１を含む領域に障害物４３がある場合、障害物４３の上から終点方向に生成される経路（ｎ７→ｎ１３→ｎ１９→ｎ２６→ｎ２７→ｎ２２）と、障害物４３の下から終点方向に生成される経路（ｎ７→ｎ２→ｎ３→ｎ１０→ｎ１１→ｎ１８）とが生成されるが、下から終点方向に生成される経路が先に終点ｎ１８に到達する。

本実施の形態においては、注目ノードから隣接ノードへ伸ばしたエッジのコストを、単純にその距離とするのではなく、基準ノードから隣接ノード（先端ノード）までの第１の距離と、基準ノードから注目ノード（根元ノード）までの第２の距離とから求める。基準ノードは、注目ノードから１以上さかのぼったノードであり、複数のノード間の最短距離に基づきエッジのコストを算出するため、エッジの距離として一律にコスト評価するのに比べ、そのエッジのパターンに基づいたより詳細なコスト評価を行なうことができ、経路の角度分解能が向上してより理想的な最短距離に近い経路を生成することができる。そして、そのエッジのパターンに基づいたより詳細なコスト評価により経路の角度分解能が向上してより理想的な最短距離に近い経路を生成することができるので、ロボットにより自然な移動動作を表出させることができる。

なお、本発明は上述した実施の形態のみに限定されるものではなく、本発明の要旨を逸脱しない範囲において種々の変更が可能であることは勿論である。例えば、上述の実施の形態では、ハードウェアの構成として説明したが、これに限定されるものではなく、任意の処理を、ＣＰＵ（Central Processing Unit）にコンピュータプログラムを実行させることにより実現することも可能である。この場合、コンピュータプログラムは、記録媒体に記録して提供することも可能であり、また、インターネットその他の伝送媒体を介して伝送することにより提供することも可能である。

本発明の実施の形態にかかる２足歩行タイプのロボットを示す斜視図である。 本発明の実施の形態にかかるロボットを示すブロック図である。 本発明の実施の形態にかかる経路探索モジュールの詳細を示す図である。 本発明の実施の形態にかかる経路決定装置の経路決定処理にて使用するエッジ及びそのコストを説明するための模式図である。 同じく、本発明の実施の形態にかかる経路決定装置の経路決定処理にて使用するエッジ及びそのコストを説明するための模式図である。 ２つ分のエッジをさかのぼる場合のエッジの形状を示す図である。 ノードｎ７が始点、ノードｎ２４が終点の経路を示す図である。 従来のロボットの移動経路を示す図である。 本発明の実施の形態にかかるロボットの移動経路を示す図である。 本発明の実施の形態にかかる経路決定方法を示すフローチャートである。 本発明の実施の形態にかかる経路決定装置におけるエッジのコスト算出方法を示すフローチャートである。 本発明の実施の形態にかかる経路探索工程を工程順に示す模式図である。 同じく、本発明の実施の形態にかかる経路探索工程を工程順に示す模式図であって、図１２（ｆ）に示す工程の次工程を示す図である。 従来の経路探索工程を工程順に示す模式図である。 同じく、従来の経路探索工程を工程順に示す模式図であって、図１４（ｆ）に示す工程の次工程を示す図である。 本発明の実施の形態にかかる経路探索方法によって生成される経路を示す図である。 本発明の実施の形態にかかる経路探索工程を工程順に示す模式図であって、障害物が存在する場合の経路探索工程を示す図である。 同じく、本発明の実施の形態にかかる経路探索工程を工程順に示す模式図であって、図１７（ｆ）に示す工程の次工程を示す図である。

符号の説明

１ ロボット
２ 頭部ユニット
３ａ，３ｂ 腕部ユニット
４ａ，４ｂ 脚部ユニット
５ 体幹部ユニット
６ 撮像部
１２ 画像認識モジュール
１３ 経路探索モジュール
１４ 行動決定モジュール
１５ 音声認識モジュール
２０ 経路出力部
２１ エッジ生成部
２２ エッジコスト算出／読出部
２２ａ 距離準備部
２２ｂ エッジコスト算出部
２３ 経路コスト算出部
２４ 経路決定部
４３ 障害物
１０１ 制御部
１０２ 入出力部
１０３ 駆動部
１０４ 電源部
１０５ 外部記憶部
１２１ カメラ
１２２ 内蔵マイク
１２３ スピーカ
１２５ センサ部
１３１ モータ
１３２ ドライバ
１４１ バッテリ
１４２ バッテリ制御部

Claims (16)

1. グリッドで表された領域に含まれる始点から終点までの経路のうち、そのコストが最小のものを選択する経路探索装置であって、
   各グリッドをノードとし、現在注目する注目ノードからこれに隣接する隣接ノードへ伸ばしたエッジのコストを、当該注目ノードから経路に沿って前記始点方向に所定数さかのぼったノードを基準ノードとし、
   （当該基準ノードから前記隣接ノードまでの最短距離を示す第１の距離）−（前記基準ノードから前記注目ノードまでの最短距離を示す第２の距離）
   として求めた値とし、前記始点から前記終点までの経路コストを算出する経路コスト算出部と、
   当該経路コストに応じて経路を決定する経路決定部とを有する経路探索装置。
2. 前記経路コスト算出部は、前記始点から前記終点までの経路コストを当該経路を構成するエッジのコストの和として算出する
   ことを特徴とする請求項１記載の経路探索装置。
3. 前記経路コスト算出部は、前記第１の距離及び前記第２の距離、又は（第１の距離−第２の距離）で求まるコストが格納されたデータベースを参照して前記始点から前記終点までのコストを算出する
   ことを特徴とする請求項１記載の経路探索装置。
4. （前記第１の距離−前記第２の距離）を算出して前記データベースを構築するデータベース生成部を更に有する
   ことを特徴とする請求項３記載の経路探索装置。
5. 前記始点から任意のノードまでのコストを当該任意のノードのコストとしたとき、エッジの先端に位置する先端ノードのうち、ノードのコストが最小のノードを注目ノードとし、注目ノードの少なくとも前方、斜め右前、及び斜め左前に位置する隣接ノードに伸ばすエッジを生成するエッジ生成部と、
   前記第１の距離及び前記第２の距離を算出又はデータベースから読み出すエッジコスト算出／読出部とを有し、
   前記経路コスト算出部は、前記始点から前記隣接ノードまでの経路コストを当該経路を構成するエッジのコストの和として算出し、
   前記経路決定部は、前記隣接ノードが前記終点の場合、前記始点から前記終点までのコストが最小の経路を選択する
   ことを特徴とする請求項１又は２項記載の経路探索装置。
6. 前記エッジ生成部は、前記注目ノードが前記始点の場合、当該始点の８近傍の隣接ノードにエッジを延ばす
   ことを特徴とする請求項５記載の経路探索装置。
7. 前記エッジコスト算出／読出部は、前記始点から前記隣接ノードまでのノード数が前記所定数になるまでは、前記注目ノードから前記隣接ノードまで伸ばしたエッジのコストを前記注目ノードと前記隣接ノードとの距離とする
   ことを特徴とする請求項５又は６記載の経路探索装置。
8. 前記経路コスト算出部は、前記始点から前記隣接ノードまでのノード数が前記所定数になるまでは、前記隣接ノードのコストを前記始点から前記隣接ノードまでの経路を構成するエッジの距離の和として算出する
   ことを特徴とする請求項５乃至７のいずれか１項記載の経路探索装置。
9. グリッドで表された領域に含まれる始点から終点までの経路のうち、そのコストが最小のものを選択する経路探索に使用するコスト評価装置であって、
   各グリッドをノードとし、現在注目する注目ノードからエッジを伸ばすノードを隣接ノードとし、前記注目ノードから経路に沿って前記始点方向に所定数さかのぼったノードを基準ノードとしたとき、当該基準ノードから前記隣接ノードまでの最短距離を示す第１の距離、及び前記基準ノードから前記注目ノードまでの最短距離を示す第２の距離を算出、又はデータベースから読み出す距離準備部と、
   前記注目ノードから前記隣接ノードへ伸ばすエッジのコストを（前記第１の距離−第２の距離）として算出するエッジコスト算出部とを有するコスト評価装置。
10. グリッドで表された領域に含まれる始点から終点までの経路のうち、そのコストが最小のものを選択する経路探索方法であって、
    各グリッドをノードとし、現在注目する注目ノードから、これに隣接する隣接ノードへ伸ばすエッジのコストを、当該注目ノードから経路に沿って前記始点方向に所定数さかのぼったノードを基準ノードとし、
    （当該基準ノードから前記隣接ノードまでの最短距離を示す第１の距離）−（前記基準ノードから前記注目ノードまでの最短距離を示す第２の距離）
    として求めた値として前記始点から前記終点までの経路コストを算出する経路コスト算出工程と、
    当該経路コストに応じて経路を決定する経路決定工程とを有する経路探索方法。
11. 前記経路コスト算出工程では、前記始点から前記終点までの経路コストを当該経路を構成するエッジのコストの和として算出する
    ことを特徴とする請求項１０記載の経路探索方法。
12. 前記経路コスト算出工程では、前記第１の距離及び前記第２の距離、又は（第１の距離−第２の距離）で求まるコストが格納されたデータベースを参照して前記始点から前記終点までのコストを算出する
    ことを特徴とする請求項１０記載の経路探索方法。
13. 前記第１の距離及び前記第２の距離、又は（前記第１の距離−前記第２の距離）を算出して前記データベースを構築するデータベース生成工程を更に有する
    ことを特徴とする請求項１２記載の経路探索方法。
14. 前記経路コスト算出工程では、前記始点からのノード数が前記所定数になるまでは、前記始点からのコストを当該始点からの距離とする
    ことを特徴とする請求項１０乃至１３記載の経路探索方法。
15. グリッドで表された領域に含まれる始点から終点までの経路のうち、そのコストが最小のものを選択する経路探索方法であって、
    各グリッドをノードとし、前記始点の８近傍の隣接ノードにエッジを延ばし、当該始点から前記隣接ノードまでの距離を当該隣接ノードのコストとして算出する第１工程と、
    前記始点から一のノードまでのコストを当該一のノードのコストとしたとき、エッジの先端に位置する先端ノードのうち、ノードのコストが最小のノードを注目ノードとし、当該注目ノードの少なくとも前方、斜め右前、及び斜め左前に位置する隣接ノードまでエッジを延ばし、当該隣接ノードのコストを算出する第２工程と、
    当該隣接ノードが前記終点の場合、前記始点から前記終点までのコストが最小の経路を選択する第３工程とを有し、
    前記隣接ノードが前記終点となるまで前記第２工程を繰り返すものであって、
    前記第２工程は、
    前記注目ノードから経路に沿って始点方向に所定数さかのぼったノードを基準ノードとしたとき、当該基準ノードから前記隣接ノードまでの最短距離を示す第１の距離、及び前記基準ノードから前記注目ノードまでの最短距離を示す第２の距離を算出し、又はデータベースから読み出す距離準備工程と、
    前記注目ノードから前記隣接ノードへ伸びるエッジのコストを（前記第１の距離−第２の距離）として算出するエッジコスト算出工程と、
    前記始点から前記隣接ノードまでの経路コストを当該経路を構成するエッジのコストの和として算出する経路コスト算出工程とを有する経路探索方法。
16. 移動手段と、
    前記移動手段を駆動する駆動手段と、
    周囲の環境をグリッドで表し、当該グリッド空間に含まれる始点から終点までの経路のうち、そのコストが最小のものを選択する経路探索手段と、
    前記経路探索手段の経路探索結果に基づき前記駆動手段を制御する制御手段とを備え、
    前記経路探索手段は、
    各グリッドをノードとし、現在注目する注目ノードからこれに隣接する隣接ノードへ伸ばしたエッジのコストを、当該注目ノードから経路に沿って前記始点方向に所定数さかのぼったノードを基準ノードとし、
    （当該基準ノードから前記隣接ノードまでの最短距離を示す第１の距離）−（前記基準ノードから前記注目ノードまでの最短距離を示す第２の距離）
    として求めた値とし、前記始点から前記終点までの経路コストを算出する経路コスト算出手段と、
    当該経路コストに応じて経路を決定する経路決定手段とを有する移動体。

Images