(57)【要約】【課題】移動体が障害物を回避して移動可能な移動経路を容易に探索すると共に、その経路長、及び経路上の任意地の点までの到着予想時刻を容易に計算可能な移動経路を探索すること。【解決手段】本発明に係る経路探索装置は、移動領域内に存在する障害物を中心として移動禁止領域を設定し、当該設定した移動禁止領域の外周の形状を、自律移動体１０の移動速度に基づいて設定した所定の曲率を有する図形を用いて生成する探索空間生成部２０と、移動禁止領域の接線と移動禁止領域の外周とを接続してなる経路候補から、自律移動体１０の移動速度に基づいて経路探索を行い、当該経路探索の結果、自律移動体１０が移動禁止領域の外周上を通過する場合には、移動経路のうち、移動禁止領域の外周を除く移動経路部分を接線とし、移動禁止領域の外周上の移動経路部分を図形に従う移動経路とする経路探索部２１とを有する。【選択図】図３
2010061293.tif 000002
【特許請求の範囲】
【請求項１】
移動領域内に存在する移動始点より移動を開始し、前記移動領域内に存在する移動終点に到達する自律移動体の移動経路を探索する経路探索装置であって、
前記移動領域内に存在する障害物を中心として移動禁止領域を設定し、当該設定した移動禁止領域の外周の形状を、前記自律移動体の移動速度に基づいて設定した所定の曲率を有する図形を用いて生成する探索空間生成部と、
前記移動禁止領域の接線と前記移動禁止領域の外周とを接続してなる経路候補から、前記自律移動体の移動速度に基づいて経路探索を行い、当該経路探索の結果、前記自律移動体が前記移動禁止領域の外周上を通過する場合には、前記移動経路のうち、前記移動禁止領域の外周を除く移動経路部分を前記接線とし、前記移動禁止領域の外周上の移動経路部分を前記図形に従う移動経路とする経路探索部と
を有する経路探索装置。
【請求項２】
前記図形は、
前記自律移動体の移動速度に基づいて設定した所定の曲率を有する円であるか、又は、前記自律移動体の移動速度に基づいて設定した所定の曲率を有する複数の円と当該複数の円間の共通接線とからなる図形である
ことを特徴とする請求項１記載の経路探索装置。
【請求項３】
前記経路探索部は、
前記移動始点と前記移動禁止領域との共通接線の長さと、前記移動禁止領域の外周における前記接線の接点間の外周の長さと、前記移動禁止領域間の共通接線の長さと、前記移動禁止領域と前記移動終点との共通接線の長さと、前記自律移動体の移動速度とから、前記自律移動体による前記経路候補の移動時間を算出し、当該算出した移動時間が最小となる経路候補を前記移動経路として経路探索を行う
ことを特徴とする請求項１又は２記載の経路探索装置。
【請求項４】
前記探索空間生成部は、
前記生成した移動禁止領域の外周部分が他の前記移動禁止領域の外周部分と互いに重複する場合に、これら移動禁止領域間の配置関係に基づいて、前記生成した移動禁止領域の外周部分が前記経路候補を構成するか否かを判定し、前記生成した移動禁止領域の外周部分が前記経路候補を構成しないものと判定した場合には、前記生成した移動禁止領域の外周部分から、当該経路候補を構成しないものと判定した移動禁止領域の外周部分を除外する
ことを特徴とする請求項１乃至３いずれか１項記載の経路探索装置。
【請求項５】
前記経路探索部は、
前記移動禁止領域の接線のうち、他の前記移動禁止領域と重複する接線を前記経路候補から除外する
ことを特徴とする請求項１乃至４いずれか１項記載の経路探索装置。
【請求項６】
前記経路探索部は、
移動障害物の将来位置を算出し、当該算出した移動障害物の将来位置に基づいて移動禁止領域を設定し、当該設定した移動禁止領域が前記経路候補と重なる場合には、前記移動障害物の移動禁止領域の接線と前記移動障害物の移動禁止領域の外周とを接続してなる経路候補をさらに用いて、前記移動障害物の移動禁止領域の外周上を通過するように経路探索を行う
ことを特徴とする請求項１記載の経路探索装置。
【請求項７】
前記経路探索部は、
前記移動障害物の移動禁止領域の外周上を通過する場合に、前記移動障害物の移動方向に対して後方の外周上を通過するように経路探索を行う
ことを特徴とする請求項６記載の経路探索装置。
【請求項８】
前記経路探索部は、
移動障害物の将来位置を算出し、当該算出した移動障害物の将来位置に基づいて移動禁止領域を設定し、当該設定した移動禁止領域が前記経路候補と重なる場合には、前記経路候補上において前記移動障害物の移動禁止領域と前記自律移動体との間に、前記自律移動体の移動速度を所定の速度へと減速指示する減速ノードを設定し、当該設定した減速ノードを用いて経路探索を行う
ことを特徴とする請求項１記載の経路探索装置。
【請求項９】
前記移動禁止領域の接線と当該移動禁止領域との接続を、前記自律移動体の移動速度に基づいて設定したクロソイド曲線により接続する
ことを特徴とする請求項１記載の経路探索装置。
【請求項１０】
前記探索空間生成部は、
前記自律移動体の移動速度に基づいて設定した大きさの図形と、前記自律移動体自体の大きさに対して所定の安全距離を加算して設定した大きさの図形とから、より大きな値の図形を選択し、当該選択した図形を前記所定の曲率を有する図形として用いる
ことを特徴とする請求項１記載の経路探索装置。
【請求項１１】
前記探索空間生成部は、
前記移動領域内に占める前記障害物の範囲が所定の範囲以上である場合に、当該移動領域を四分割し、当該四分割処理を繰り返すことにより、四分割処理後の移動領域内に占める前記障害物の範囲が前記所定の範囲よりも小さな移動領域を算出し、当該算出した移動領域を探索空間として用いる
ことを特徴とする請求項１記載の経路探索装置。
【請求項１２】
前記移動領域は３次元の領域であり、
前記探索空間生成部は、
３次元形状の障害物を中心とする移動禁止領域の外周の形状を、所定の曲率を有する３次元形状の図形を用いて生成する
ことを特徴とする請求項１記載の経路探索装置。
【請求項１３】
前記移動領域における前記障害物の配置位置に応じて、前記図形の大きさを設定する
ことを特徴とする請求項１記載の経路探索装置。
【請求項１４】
移動領域内に存在する移動始点より移動を開始し、前記移動領域内に存在する移動終点に到達する自律移動体の移動経路を探索する経路探索方法であって、
前記移動領域内に存在する障害物を中心として移動禁止領域を設定し、当該設定した移動禁止領域の外周の形状を、前記自律移動体の移動速度に基づいて設定した所定の曲率を有する図形を用いて生成し、
前記移動禁止領域の接線と前記移動禁止領域の外周とを接続してなる経路候補から、前記自律移動体の移動速度に基づいて経路探索を行い、当該経路探索の結果、前記自律移動体が前記移動禁止領域の外周上を通過する場合には、前記移動経路のうち、前記移動禁止領域の外周を除く移動経路部分を前記接線とし、前記移動禁止領域の外周上の移動経路部分を前記図形に従う移動経路とする経路探索方法。
【請求項１５】
前記図形は、
前記自律移動体の移動速度に基づいて設定した所定の曲率を有する円であるか、又は、前記自律移動体の移動速度に基づいて設定した所定の曲率を有する複数の円と当該複数の円間の共通接線とからなる図形である
ことを特徴とする請求項１４記載の経路探索方法。
【請求項１６】
前記経路探索を行うときに、前記移動始点と前記移動禁止領域との共通接線の長さと、前記移動禁止領域の外周における前記接線の接点間の外周の長さと、前記移動禁止領域間の共通接線の長さと、前記移動禁止領域と前記移動終点との共通接線の長さと、前記自律移動体の移動速度とから、前記自律移動体による前記経路候補の移動時間を算出し、当該算出した移動時間が最小となる経路候補を前記移動経路として経路探索を行う
ことを特徴とする請求項１４又は１５記載の経路探索方法。
【請求項１７】
前記移動禁止領域の外周を生成したときに、当該生成した移動禁止領域の外周部分が他の前記移動禁止領域の外周部分と互いに重複する場合に、これら移動禁止領域間の配置関係に基づいて、前記生成した移動禁止領域の外周部分が前記経路候補を構成するか否かを判定し、前記生成した移動禁止領域の外周部分が前記経路候補を構成しないものと判定した場合には、前記生成した移動禁止領域の外周部分から、当該経路候補を構成しないものと判定した移動禁止領域の外周部分を除外する
ことを特徴とする請求項１４乃至１６いずれか１項記載の経路探索方法。
【請求項１８】
前記経路探索を行うときに、
前記移動禁止領域の接線のうち、他の前記移動禁止領域と重複する接線を前記経路候補から除外する
ことを特徴とする請求項１４乃至１７いずれか１項記載の経路探索方法。
【請求項１９】
前記経路探索を行うときに、
移動障害物の将来位置を算出し、当該算出した移動障害物の将来位置に基づいて移動禁止領域を設定し、当該設定した移動禁止領域が前記経路候補と重なる場合には、前記移動障害物の移動禁止領域の接線と前記移動障害物の移動禁止領域の外周とを接続してなる経路候補をさらに用いて、前記移動障害物の移動禁止領域の外周上を通過するように経路探索を行う
ことを特徴とする請求項１４記載の経路探索方法。
【請求項２０】
前記経路探索を行うときに、
前記移動障害物の移動禁止領域の外周上を通過する場合に、前記移動障害物の移動方向に対して後方の外周上を通過するように経路探索を行う
ことを特徴とする請求項１９記載の経路探索方法。
【請求項２１】
前記経路探索を行うときに、
移動障害物の将来位置を算出し、当該算出した移動障害物の将来位置に基づいて移動禁止領域を設定し、当該設定した移動禁止領域が前記経路候補と重なる場合には、前記経路候補上において前記移動障害物の移動禁止領域と前記自律移動体との間に、前記自律移動体の移動速度を所定の速度へと減速指示する減速ノードを設定し、当該設定した減速ノードを用いて経路探索を行う
ことを特徴とする請求項１４記載の経路探索方法。
【請求項２２】
前記移動禁止領域の接線と当該移動禁止領域との接続を、前記自律移動体の移動速度に基づいて設定したクロソイド曲線により接続する
ことを特徴とする請求項１４記載の経路探索方法。
【請求項２３】
前記移動禁止領域の外周を生成するときに、
前記自律移動体の移動速度に基づいて設定した大きさの図形と、前記自律移動体自体の大きさに対して所定の安全距離を加算して設定した大きさの図形とから、より大きな値の図形を選択し、当該選択した図形を前記所定の曲率を有する図形として用いる
ことを特徴とする請求項１４記載の経路探索方法。
【請求項２４】
前記移動禁止領域の外周を生成するときに、
前記移動領域内に占める前記障害物の範囲が所定の範囲以上である場合に、当該移動領域を四分割し、当該四分割処理を繰り返すことにより、四分割処理後の移動領域内に占める前記障害物の範囲が前記所定の範囲よりも小さな移動領域を算出し、当該算出した移動領域を探索空間として用いる
ことを特徴とする請求項１４記載の経路探索方法。
【請求項２５】
前記移動領域は３次元の領域であり、
前記移動禁止領域の外周を生成するときに、
３次元形状の障害物を中心とする移動禁止領域の外周の形状を、所定の曲率を有する３次元形状の図形を用いて生成する
ことを特徴とする請求項１４記載の経路探索方法。
【請求項２６】
前記移動領域における前記障害物の配置位置に応じて、前記図形の大きさを設定する
ことを特徴とする請求項１４記載の経路探索方法。
【請求項２７】
移動領域内に存在する移動始点より移動を開始し、前記移動領域内に存在する移動終点に到達する自律移動体の移動経路を探索する経路探索プログラムであって、
コンピュータに対して、
前記移動領域内に存在する障害物を中心として移動禁止領域を設定し、当該設定した移動禁止領域の外周の形状を、前記自律移動体の移動速度に基づいて設定した所定の曲率を有する図形を用いて生成するステップと、
前記移動禁止領域の接線と前記移動禁止領域の外周とを接続してなる経路候補から、前記自律移動体の移動速度に基づいて経路探索を行い、当該経路探索の結果、前記自律移動体が前記移動禁止領域の外周上を通過する場合には、前記移動経路のうち、前記移動禁止領域の外周を除く移動経路部分を前記接線とし、前記移動禁止領域の外周上の移動経路部分を前記図形に従う移動経路とするステップと
を実行させる経路探索プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は経路探索装置、経路探索方法、及び経路探索プログラムに関する。
【背景技術】
【０００２】
近年、屋内や屋外の限られたエリアなどの所定の移動領域を自律的に移動する、いわゆる自律移動体としての車両や歩行ロボットなどが開発されている。このような自律移動体を移動させるためには、移動領域内での自律移動体の自己位置を認識させると共に、自律移動体が移動しようとする移動経路を予め又はリアルタイムに作成する必要がある。
【０００３】
自律移動体の移動経路作成方法としては、等間隔のグリッドを保持したグリッドマップを用いて経路探索を行う方法が良く知られている。即ち、このようなグリッドマップ上に、移動始点及び移動終点を配置し、経路探索アルゴリズム（Ａ＊探索アルゴリズムなど）により経路の全探索を行う。ここで、探索した経路は線分の組合せであるため、そのままでは自律移動体は経路に追従することができない。このため、その探索した経路を滑らかな曲線経路へと修正する処理が行われる（例えば特許文献１など）。
【特許文献１】特開２００７−２５７０９４号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
上述したように、従来の技術では、グリッドマップ上において探索した経路はそのままでは自律移動体が追従可能な経路とはならない。このため、自律移動体が追従可能な経路となるように円滑化処理を加える必要がある。しかしながら、この処理は以下の問題点を発生させる。
【０００５】
まず、円滑化処理後の経路は、処理前の経路と同等の安全性を保証しているとは限らないという点が挙げられる。これは、線分の繰り返しで表現されるグリッド経路に対して円滑化処理を施すことにより、グリッド間を通過するような経路が生成される。このように新たに経路が生成された場合には、障害物との衝突判定を行ってはいないために、障害物と衝突（接触）する危険性が残るためである。
【０００６】
また、円滑化処理前の経路が最短であることは探索アルゴリズムの性質から保証することができるものの、処理後の経路が最短であるとは限らないという点が挙げられる。これは、経路の円滑化処理には、経路の最短性を保証する処理が含まれていないためである。
【０００７】
さらにまた、円滑化処理後の経路について、その経路長を正確に計算することができないため、各地点における自律移動体の到達時間を計算することが困難であるという点が挙げられる。これは、滑らかにした後の経路は非線形に表現されるものが多く、経路長の計算が難しいためである。
【０００８】
このように、従来の技術は、グリッドマップ上において経路探索を行い、さらに、経路に円滑化処理を加えるという二段階の処理を経ることで、上述した様々な問題点を有するものであった。従って、移動体が障害物を回避して移動可能な移動経路を容易に探索すると共に、その経路長、及び経路上の任意地の点までの到着予想時刻を容易に計算可能な移動経路を探索することができる経路探索技術が強く求められている。
【０００９】
従って、本発明は、移動体が障害物を回避して移動可能な移動経路を容易に探索すると共に、その経路長、及び経路上の任意地の点までの到着予想時刻を容易に計算可能な移動経路を探索することができる経路探索装置、経路探索方法、及び経路探索プログラムを提供することを目的とする。
【課題を解決するための手段】
【００１０】
本発明に係る経路探索装置は、移動領域内に存在する移動始点より移動を開始し、前記移動領域内に存在する移動終点に到達する自律移動体の移動経路を探索する経路探索装置であって、前記移動領域内に存在する障害物を中心として移動禁止領域を設定し、当該設定した移動禁止領域の外周の形状を、前記自律移動体の移動速度に基づいて設定した所定の曲率を有する図形を用いて生成する探索空間生成部と、前記移動禁止領域の接線と前記移動禁止領域の外周とを接続してなる経路候補から、前記自律移動体の移動速度に基づいて経路探索を行い、当該経路探索の結果、前記自律移動体が前記移動禁止領域の外周上を通過する場合には、前記移動経路のうち、前記移動禁止領域の外周を除く移動経路部分を前記接線とし、前記移動禁止領域の外周上の移動経路部分を前記図形に従う移動経路とする経路探索部とを有するものである。
【００１１】
これにより、障害物との衝突を回避する移動経路を探索することができる。また、探索された移動経路は、移動禁止領域の接線と所定の曲率に従った移動禁止領域の外周とから構成されるため、移動経路に対してさらなる円滑化処理を必要としない。即ち、探索された移動経路を滑らかな経路へと修正する処理を別途必要としないため、移動体は探索された移動経路をそのまま移動することができる。また、このように構成された移動経路は、経路長を容易かつ正確に算出することができるため、経路上の各地点における自律移動体の到達時間を算出することが可能となる。従って、移動体が障害物を回避して移動可能な移動経路を容易に探索すると共に、その経路長、及び経路上の任意地の点までの到着予想時刻を容易に計算可能な移動経路を探索することができる。
【００１２】
また、前記図形は、前記自律移動体の移動速度に基づいて設定した所定の曲率を有する円であるか、又は、前記自律移動体の移動速度に基づいて設定した所定の曲率を有する複数の円と当該複数の円間の共通接線とからなる図形とすると好適である。図形を例えばこのような構成とすることで、移動禁止領域の外周の長さを容易かつ正確に計算することができる。
【００１３】
さらにまた、前記経路探索部は、前記移動始点と前記移動禁止領域との共通接線の長さと、前記移動禁止領域の外周における前記接線の接点間の外周の長さと、前記移動禁止領域間の共通接線の長さと、前記移動禁止領域と前記移動終点との共通接線の長さと、前記自律移動体の移動速度とから、前記自律移動体による前記経路候補の移動時間を算出し、当該算出した移動時間が最小となる経路候補を前記移動経路として経路探索を行うと好適である。
【００１４】
また、前記探索空間生成部は、前記生成した移動禁止領域の外周部分が他の前記移動禁止領域の外周部分と互いに重複する場合に、これら移動禁止領域間の配置関係に基づいて、前記生成した移動禁止領域の外周部分が前記経路候補を構成するか否かを判定し、前記生成した移動禁止領域の外周部分が前記経路候補を構成しないものと判定した場合には、前記生成した移動禁止領域の外周部分から、当該経路候補を構成しないものと判定した移動禁止領域の外周部分を除外するようにしてもよい。これにより、経路候補となりえない移動禁止領域の外周部分を除外することで、より効率的に経路探索を行うことができる。
【００１５】
さらにまた、前記経路探索部は、前記移動禁止領域の接線のうち、他の前記移動禁止領域と重複する接線を前記経路候補から除外するようにしてもよい。これにより、経路候補となりえない移動禁止領域の接線を除外することで、より効率的に経路探索を行うことができる。
【００１６】
また、前記経路探索部は、移動障害物の将来位置を算出し、当該算出した移動障害物の将来位置に基づいて移動禁止領域を設定し、当該設定した移動禁止領域が前記経路候補と重なる場合には、前記移動障害物の移動禁止領域の接線と前記移動障害物の移動禁止領域の外周とを接続してなる経路候補をさらに用いて、前記移動障害物の移動禁止領域の外周上を通過するように経路探索を行うと好適である。これにより、移動領域内に移動障害物が存在する場合においても、移動障害物を回避した移動経路を探索することができる。
【００１７】
さらにまた、前記経路探索部は、前記移動障害物の移動禁止領域の外周を通過する場合に、前記移動障害物の移動方向に対して後方の外周上を通過するように経路探索を行うようにしてもよい。これにより、移動障害物の移動方向の前方を移動せず回避するため、より安全な移動経路を探索することができる。
【００１８】
また、前記経路探索部は、移動障害物の将来位置を算出し、当該算出した移動障害物の将来位置に基づいて移動禁止領域を設定し、当該設定した移動禁止領域が前記経路候補と重なる場合には、前記経路候補上において前記移動障害物の移動禁止領域と前記自律移動体との間に、前記自律移動体の移動速度を所定の速度へと減速指示する減速ノードを設定し、当該設定した減速ノードを用いて経路探索を行うと好適である。これにより、移動障害物の移動に応じた新たな移動経路を探索せずに済み、自律移動体を減速させるのみで移動障害物との回避が可能な移動経路を探索することができる。
【００１９】
さらにまた、前記移動禁止領域の接線と当該移動禁止領域との接続を、前記自律移動体の移動速度に基づいて設定したクロソイド曲線により接続すると好適である。これにより、移動禁止領域の接線と移動禁止領域との接点においても、自律移動体は移動経路の追従性を維持しながら移動することができる。
【００２０】
また、前記探索空間生成部は、前記自律移動体の移動速度に基づいて設定した大きさの図形と、前記自律移動体自体の大きさに対して所定の安全距離を加算して設定した大きさの図形とから、より大きな値の図形を選択し、当該選択した図形を前記所定の曲率を有する図形として用いると好適である。これにより、自律移動体の移動速度に基づく移動経路の追従性と、衝突回避を重視する移動経路の安全性とを考慮した移動経路を探索することができる。
【００２１】
さらにまた、前記探索空間生成部は、前記移動領域内に占める前記障害物の範囲が所定の範囲以上である場合に、当該移動領域を四分割し、当該四分割処理を繰り返すことにより、四分割処理後の移動領域内に占める前記障害物の範囲が前記所定の範囲よりも小さな移動領域を算出し、当該算出した移動領域を探索空間として用いると好適である。これにより、自律移動体の移動に応じて経路探索の対象とする移動領域を限定することで、計算時間を削減することができる。従って、広大な探索空間においても経路探索を実現することができる。
【００２２】
また、前記移動領域は３次元の領域であり、前記探索空間生成部は、３次元形状の障害物を中心とする移動禁止領域の外周の形状を、所定の曲率を有する３次元形状の図形を用いて生成すると好適である。このように、２次元空間に限定されず、３次元空間においても経路探索を行うことで、例えばマニピュレータなどの軌道計算にも適用することができる。
【００２３】
さらにまた、前記移動領域における前記障害物の配置位置に応じて、前記図形の大きさを設定すると好適である。これにより、ユーザーの意図を反映させた移動経路を探索することができる。
【００２４】
また、本発明は方法としても具現化され、本発明に係る経路探索方法は、移動領域内に存在する移動始点より移動を開始し、前記移動領域内に存在する移動終点に到達する自律移動体の移動経路を探索する経路探索方法であって、前記移動領域内に存在する障害物を中心として移動禁止領域を設定し、当該設定した移動禁止領域の外周の形状を、前記自律移動体の移動速度に基づいて設定した所定の曲率を有する図形を用いて生成し、前記移動禁止領域の接線と前記移動禁止領域の外周とを接続してなる経路候補から、前記自律移動体の移動速度に基づいて経路探索を行い、当該経路探索の結果、前記自律移動体が前記移動禁止領域の外周上を通過する場合には、前記移動経路のうち、前記移動禁止領域の外周を除く移動経路部分を前記接線とし、前記移動禁止領域の外周上の移動経路部分を前記図形に従う移動経路とするものである。
【００２５】
これにより、障害物との衝突を回避する移動経路を探索することができる。また、探索された移動経路は、移動禁止領域の接線と所定の曲率に従った移動禁止領域の外周とから構成されるため、移動経路に対してさらなる円滑化処理を必要としない。即ち、探索された移動経路を滑らかな経路へと修正する処理を別途必要としないため、移動体は探索された移動経路をそのまま移動することができる。また、このように構成された移動経路は、経路長を容易かつ正確に算出することができるため、経路上の各地点における自律移動体の到達時間を算出することが可能となる。従って、移動体が障害物を回避して移動可能な移動経路を容易に探索すると共に、その経路長、及び経路上の任意地の点までの到着予想時刻を容易に計算可能な移動経路を探索することができる。
【００２６】
また、本発明はプログラムとしても具現化され、本発明に係る経路探索プログラムは、移動領域内に存在する移動始点より移動を開始し、前記移動領域内に存在する移動終点に到達する自律移動体の移動経路を探索する経路探索プログラムであって、コンピュータに対して、前記移動領域内に存在する障害物を中心として移動禁止領域を設定し、当該設定した移動禁止領域の外周の形状を、前記自律移動体の移動速度に基づいて設定した所定の曲率を有する図形を用いて生成するステップと、前記移動禁止領域の接線と前記移動禁止領域の外周とを接続してなる経路候補から、前記自律移動体の移動速度に基づいて経路探索を行い、当該経路探索の結果、前記自律移動体が前記移動禁止領域の外周上を通過する場合には、前記移動経路のうち、前記移動禁止領域の外周を除く移動経路部分を前記接線とし、前記移動禁止領域の外周上の移動経路部分を前記図形に従う移動経路とするステップとを実行させるものである。
【００２７】
これにより、移動体が障害物を回避して移動可能な移動経路を容易に探索すると共に、その経路長、及び経路上の任意地の点までの到着予想時刻を容易に計算可能な移動経路を探索することができる。
【発明の効果】
【００２８】
本発明によれば、移動体が障害物を回避して移動可能な移動経路を容易に探索することができると共に、その経路長、及び経路上の任意地の点までの到着予想時刻を容易に計算可能な移動経路を探索することができる経路探索装置、経路探索方法、及び経路探索プログラムを提供することができる。
【発明を実施するための最良の形態】
【００２９】
発明の実施の形態１．
本実施の形態１に係る経路探索装置は、まず、移動領域内に存在する障害物を中心として移動禁止領域を設定する。ここで、設定した移動禁止領域の外周の形状を、自律移動体の移動速度に基づいて設定した所定の曲率を有する図形を用いて生成する。そして、経路探索装置は、移動禁止領域の接線と移動禁止領域の外周とを接続してなる経路候補から、自律移動体の移動速度に基づいて経路探索を行う。経路探索の結果、自律移動体が移動禁止領域の外周上を通過する場合には、経路探索装置は、移動経路のうち、移動禁止領域の外周を除く移動経路部分を接線とし、移動禁止領域の外周上の移動経路部分を図形に従う移動経路とする。自律移動体は、探索した移動経路を参照して、移動領域内に存在する移動始点より移動を開始し、移動領域内に存在する移動終点に到達する。
【００３０】
このようにして探索された移動経路は、移動禁止領域の接線と所定の曲率に従った移動禁止領域の外周とから構成されるため、移動経路に対してさらなる円滑化処理を必要としない。即ち、探索された移動経路を滑らかな経路へと修正する処理を別途必要としないため、移動体は探索された移動経路をそのまま移動することができる。また、このように構成された移動経路は、経路長を容易かつ正確に算出することができるため、経路上の各地点における自律移動体の到達時間を算出することが可能となる。従って、移動体が障害物を回避して移動可能な移動経路を容易に探索すると共に、その経路長、及び経路上の任意地の点までの到着予想時刻を容易に計算可能な移動経路を探索することができる。
【００３１】
以下、図面を参照しながら本実施の形態１に係る経路探索装置について説明する。図１は、移動領域としての床部１上の限られたエリアＰ（破線に囲まれた領域）を、自律移動体としての車両１０が制御部１５からの信号により移動する自律移動体制御システム１００の一実施形態を概略的に示すものである。図１では、床部１上のエリアＰ上に物体は表記されていないが、既知の固定障害物及び外界センサにより検知された固定障害物や移動障害物が存在し、車両１０はこれらの障害物を回避する必要がある。
【００３２】
図２に示すように、車両１０は、箱型の車両本体１０ａと、１対の対向する車輪１１と、キャスタ１２を備える対向２輪型の車両であり、これらの車輪１１、キャスタ１２とで車両本体１０ａを水平に支持するものである。さらに、車両本体１０ａの内部には、車輪１１をそれぞれ駆動する駆動部（モータ）１３と、車輪の回転数を検出するためのカウンタ１４と、車輪を駆動するための制御信号を作成し、駆動部１３にその制御信号を送信する制御部１５が備えられている。そして、制御部１５内部に備えられた記憶部としてのメモリなどの記憶領域１５ａには、制御信号に基づいて車両１０の移動速度や移動方向、移動距離などを制御するための制御プログラムが記録されている。前述の移動速度や移動距離などは、カウンタ１４で検知された車輪１１の回転数に基づいて求められている。
【００３３】
また、車両本体１０ａの前面には、移動する方向に現れた障害物等を認識するための外界センサ１６が固定されており、この外界センサ１６で認識した画像や映像等の情報が制御部１５に入力された結果、制御プログラムに従って車両の移動する方向や速度等が決定される。外界センサ１６は、障害物等において反射されたレーザを検知するセンサや、ＣＣＤカメラにより構成することができる。
【００３４】
さらに、車両本体１０ａの上面には、自己位置を認識するためのアンテナ１７が備えられており、例えば図示しないＧＰＳ等からの位置情報を受け取り、制御部１５においてその位置情報を解析することにより、自己の位置を正確に認識することができる。
【００３５】
このように構成された車両１０は、１対の車輪１１の駆動量をそれぞれ独立に制御することで、直進や曲線移動（旋回）、後退、その場回転（両車輪の中点を中心とした旋回）などの移動動作を行うことができる。そして、車両１０は、外部からの移動場所を指定する制御部１５からの指令にしたがって、エリアＰ内の指定された目的地までの移動経路を作成し、その移動経路に追従するように移動することで、目的地に到達する。
【００３６】
制御部１５内部に備えられた記憶領域１５ａには、床部１上のエリアＰ全体の形状に、略一定間隔ｄ（例えば１０ｃｍ）に配置された格子点を結ぶグリッド線を仮想的に描写することで得られるグリッドマップが記憶されている。障害物の存在の有無を示す障害物情報が、予め又はリアルタイムに各グリッドに対して設定される。制御部１５は、グリッドマップ上において特定される自己位置を移動始点として、目的地である移動終点までの移動経路を作成し、作成された移動経路に従って移動を行う。
【００３７】
図３は、制御部１５が有する経路探索機能の構成を示すブロック図である。制御部１５は、探索空間生成部２０と、経路探索部２１と、経路データ２２と、自己位置計算部２３と、障害物位置データ２４とを有する。
【００３８】
障害物位置データ２４は、エリア内に存在する障害物の位置情報である。尚、障害物の位置情報は、ユーザーが予め登録してもよいし、前述したように外界センサ１６により取得してもよい。例えば、車両１０の移動する領域の図面などからユーザーが障害物位置情報を予め登録してもよいし、車両１０に備えたレーザレンジセンサなどの測距センサにより障害物を検出した後、グリッドマップに障害物位置情報を登録するようにしてもよい。
【００３９】
自己位置計算部２３は、前述のように、ＧＰＳ等から得られた車両１０の位置情報を、このグリッドマップ上における自己の位置に置き換えて、グリッドマップ上における自己位置として認識する。グリッドマップ上において、車両１０の自己位置に相当する場所、目的地である移動終了点、及び移動終了点における車両１０の移動方向が特定される。
【００４０】
探索空間生成部２０は、グリッドマップ内に存在する障害物を中心として移動禁止領域を設定する。ここで、探索空間生成部２０は、設定した移動禁止領域の外周の形状を、車両１０の移動速度に基づいて設定した所定の曲率を有する図形を用いて生成する。
【００４１】
経路探索部２１は、移動禁止領域の接線と移動禁止領域の外周とを接続してなる経路候補から、車両１０の移動速度に基づいて経路探索を行う。ここで、経路探索部２１は、経路探索の結果、車両１０が移動禁止領域の外周上を通過する場合には、移動経路のうち、移動禁止領域の外周を除く移動経路部分を接線とし、移動禁止領域の外周上の移動経路部分を図形に従う移動経路とする。そして、経路探索部２１は、探索した移動経路を経路データ２２として記憶する。制御部１５は、探索された経路データ２２に従って、車両１０の移動を制御する。
【００４２】
続いて、制御部１５が有する経路探索機能について、図４に示すフローチャートと図５乃至１４を用いて説明する。図４は、制御部１５による経路探索処理を示すフローチャートである。
【００４３】
まず制御部１５は、車両１０が経路探索を行う探索空間としてのグリッドマップを作成する（Ｓ１０１）。図５に、車両１０が経路探索を行うグリッドマップの一例を示す。図５に示すように、グリッドマップ内には、移動始点としての現在位置から移動終点としてのゴールＧへと移動する途中に障害物グリッドＢが配置される。車両１０は、グリッドマップ内に存在する障害物グリッドＢを回避するように、現在位置からゴールＧへと到達する移動経路を探索する。尚、探索空間としてはグリッドマップに限定されず、後述する移動禁止領域の外周上の接点（以下、ノードと称する。）と、それら接点間を接続する曲線及び直線（以下、アークと称する。）とを用いて表現可能な探索空間であればよい。
【００４４】
次いで、制御部１５は、車両１０の移動を禁止する領域である移動禁止領域を設定する。ここでは、車両１０の移動速度に基づいて設定した所定の曲率を有する図形として円を採用し、制御部１５は、移動禁止領域の外周の形状を、円を用いて生成する。即ち、制御部１５は、各障害物グリッドＢを中心とする経路円を作成する（Ｓ１０２）。図６に、各障害物グリッドＢを中心として作成した経路円の例を示す。
【００４５】
尚、移動禁止領域の外周を定める図形としては円に限定されず、車両１０が障害物グリッドＢに衝突しないような安全な距離を保ち、かつ、車両１０が追従可能な曲率を有する図形であればよい。採用する図形としては、例えば、図７に示すように、車両１０の移動速度に基づいて設定した所定の曲率を有する円（図７Ａ）、又は、車両１０の移動速度に基づいて設定した所定の曲率を有する複数の円とそれら複数の円間の共通接線とからなる図形（図７Ｂ、図７Ｃ）に示すような図形としてもよい。このような図形によれば、移動禁止領域の外周の長さを容易かつ正確に計算することができる。また、図７に示す図形以外の図形であっても、最終的に生成される移動経路の経路長が計算可能な図形を採用すると好適である。尚、以下の説明においては、説明の簡略化のため、移動禁止領域の外周を定める図形は円であるものとして説明する。
【００４６】
また、制御部１５は、作成した経路円のうち、移動経路となりえない円を除外し、残った円については、他の障害物グリッドＢと衝突しない円弧の範囲を計算する。即ち、経路円の円弧の一部が他の経路円と互いに重複する場合には、これら経路円の配置関係に基づいて、移動経路となりえない円と、移動経路となりうる円のうちで不要な円弧部分とを除外する。経路候補となりえない経路円と、経路円のうち不要な円弧部分とを除外することで、より効率的に経路探索を行うことができる。尚、円を辿る移動経路は右回りと左回りとがある。以下の説明では、中心が同一の場合であっても、右回りで回る場合と、左回りで回る場合とを、それぞれ別の円を回る場合として扱うものとする。
【００４７】
図８を参照して、移動経路となりえない円の判定方法について説明する。ここでの判定は、周囲に位置する円によって、その円が埋もれているか否かを判定するものである。図８に示す例では、経路円Ｃ０が移動経路となりうる円であるか否かを判定する。尚、円Ｃｘの半径をｒｘとし、円Ｃｘの中心をｃｘとする。円Ｃ０の中心ｃ０と円Ｃｘの中心ｃｘとを結ぶ線分をｃ０ｃｘとし、円Ｃ０の中心ｃ０と円Ｃｙの中心ｃｙとを結ぶ線分をｃ０ｃｙとする。線分ｃ０ｃｘと線分ｃ０ｃｙとがなす角をθｘｙとする。まず、不等式｜ｃ０ｃｘ｜<ｒ０＋ｒｘの関係を満足する円Ｃｘを全て列挙し、その集合をＣＵとする。そして、Ｃｘ∈ＣＵである全ての円Ｃｘについて、線分ｃ０ｃｘとｘ軸とのなす角の順にソートし、隣接する線分のなす角θｘｙを計算する。このとき、以下の数１を満足するθｘｙが一つでも存在する場合には、円Ｃ０は経路となりうる。全ての円の半径が等しい場合には、数１は単純にθｘｙ>πと置き換えられる。尚、数１が等式となった場合には、円Ｃ０と、円Ｃｘと、円Ｃｙとが共通接線を有する状態となる（例えば、図８に示す接線Ｌ０とＬ１２が重なる状態となる。）。
【数１】
2010061293.tif 000003
図８Ａに示す例では、θ１２は数１に示す条件を満足しているため、円Ｃ０は経路となりうるものと判定される。一方で、図８Ｂに示す例では、どのθｘｙも数１に示す条件を満足しておらず、円Ｃ０は経路となりえないものと判定される。従って、制御部１５は、図８Ｂに示す円Ｃ０を除外する。
【００４８】
次いで、制御部１５は、車両１０の現在位置と速度及び角速度とから、車両１０が最初に辿る仮想的な円を作成し、上述した場合と同様にして、作成した仮想円について、障害物グリッドＢと衝突しない円弧の範囲を計算する。仮想円は、進路の左側と右側の２つを作成する。図９に、以上のようにして作成した経路円と仮想円ＩＣＲ、ＩＣＬを示す。尚、図９に示す例では、経路円について、障害物グリッドＢと衝突しない円弧の範囲を太線により示している。
【００４９】
尚、仮想円の位置及び大きさは、車両１０の位置情報及び速度情報から決定する。車両１０が停止している場合には、例えば図１０Ａに示すように、車両１０に接するように仮想円ＩＣＲ、ＩＣＬを配置する。車両１０が移動している場合には、例えば図１０Ｂに示すように、車両１０の移動速度に応じて車両１０が追従しやすいように仮想円ＩＣＲ、ＩＣＬの半径を大きくし、車両１０の前方に配置する。また、仮想円は車両１０の進行方向ベクトルに沿って微分可能な曲線であればよく、円以外にも、クロソイド曲線やスプライン曲線などを用いて作成してもよい。
【００５０】
次いで、制御部１５は、車両１０の移動速度から、作成した２つの仮想円に車両１０が到達するまでの時間を計算し、その到達点をノードとして設定する。制御部１５は、仮想円のその２つのノードを最初のＯｐｅｎノードとし、Ｏｐｅｎノードリストに追加する（Ｓ１０３）。Ｓ１０３における計算の結果からＯｐｅｎノードが存在するか否かを判定する（Ｓ１０４）。判定の結果、Ｏｐｅｎノードが一つもなかった場合には、探索は失敗であるものとして処理を終了する。一方で、Ｏｐｅｎノードが一つ以上存在する場合には、Ｓ１０５へと進む。
【００５１】
以下、制御部１５は、Ｏｐｅｎノードのうち、全探索の評価値が最小となるノードＮを探索する。探索対象はゴールＧと移動禁止領域を示す円である。ここで、全探索に用いる評価値としては、ノードまでの到達時間が短いものほどその値が小さいものとする。全探索手法としては、Ａ＊探索アルゴリズムやダイクストラ法などの公知の手段を用いることができる。尚、以下の説明では、ノードＮが属する円を円Ｃと表記する。
【００５２】
制御部１５は、Ｏｐｅｎノードリストから、最も評価値が低いノードＮを選択する（Ｓ１０５）。制御部１５は、例えば図１０Ａに示すように、ノードＮを選択する。そして制御部１５は、選択したノードＮが、ゴールであるか否かを判定する（Ｓ１０６）。判定の結果、ノードＮがゴールであった場合には、探索は成功として経路探索処理を終了する。一方で、判定の結果、ノードＮがゴールでなかった場合には、Ｓ１０７へと進む。
【００５３】
次いで、制御部１５は、ノードＮが属する円Ｃが初めて訪れる円であるか否かを判定する（Ｓ１０７）。判定の結果、円Ｃを訪れるのが２回目以降であった場合には、１回目に訪れた際に作成した接線の情報をそのまま利用するものとし、Ｓ１０９へと進む。
【００５４】
一方で、判定の結果、ノードＮが属する円Ｃを初めて訪れた場合には、制御部１５は、円Ｃ以外の各円に対して、移動禁止領域と交わらない接線を引く（Ｓ１０８）。より具体的には、制御部１５は、円Ｃから、円Ｃと他の円との共通接線と、円ＣとゴールＧとの接線を引く。そして、制御部１５は、これら接線のうち、他の移動禁止領域に交わる接線を除外する。即ち、制御部１５は、他の障害物グリッドＢと接触しない接線を全て求める。経路候補となりえない不要な接線を処理対象から除くことで、より効率的に経路探索を行うことができる。
【００５５】
尚、ここでは、共通接線として、図１１に示す内接線Ｌｉｎと外接線Ｌｏｕｔの２種類の共通接線を扱う。車両１０の到達時間を計算する場合には、外接線Ｌｏｕｔで接続された円の左右の回り方は図１１Ａに示すように円Ｃと同じであるものとし、内接線Ｌｉｎで接続された円の回り方は図１１Ｂに示すように円Ｃと逆になるものとして計算する。中心が同一であっても、右回りで回る場合と左回りで回る場合の２通りを、それぞれ別の円として扱うものとする。
【００５６】
次いで、制御部１５は、Ｓ１０８で引かれた各接線の両端を出発ノード・到達ノードとして設定する（Ｓ１０９）。ここでは、円Ｃ以外の円から円Ｃへと引かれた接線と円Ｃの円弧との接点を到達ノードとし、円Ｃから他の円へと接線を引いた場合の円Ｃの円弧上の接点を出発ノードとして定義する。
【００５７】
次いで、制御部１５は、ノードＮと円Ｃの各出発ノード間、各出発ノードと各到達ノード間にアークを張り、車両１０がアークを移動するために必要な移動コストを計算する（Ｓ１１０）。例えば図１２に示すように、制御部１５は、円Ｃの到達ノードから円Ｃの出発ノードまでにアーク１を張り、さらに、円Ｃの出発ノードから他の円の到達ノードまでアーク２を張る。尚、アークのコストは、アークの長さ（距離）と車両１０の移動速度とから算出した移動時間とする。
【００５８】
次いで、制御部１５は、他の円の円弧上の到達ノードを、Ｏｐｅｎノードリストに追加する（Ｓ１１１）。制御部１５は、例えば図１３に示す到達ノードをＯｐｅｎノードリストに新たに追加する。制御部１５は、Ｏｐｅｎノードを追加した後、Ｓ１０４へと戻り、最も評価値の低いノードＮがゴールとなるか、又は、探索に失敗したかのいずれかの状態となるまで上述した処理を繰り返す。
【００５９】
経路探索に成功した場合には、制御部１５は、例えば図１４に示すような移動経路Ｒを経路データとして出力する。移動経路Ｒの各部分は、移動禁止領域の外周上の移動経路部分は円弧として構成されると共に、移動禁止領域の外周を除く移動経路部分は移動禁止領域間の接線から構成される。
【００６０】
これにより、障害物との衝突を回避する移動経路を探索することができる。また、探索された移動経路は、移動禁止領域の接線と所定の曲率に従った移動禁止領域の外周とから構成されるため、移動経路に対してさらなる円滑化処理を必要としない。即ち、探索された移動経路を滑らかな経路へと修正する処理を別途必要としないため、車両１０は探索された移動経路をそのまま移動することができる。また、このように構成された移動経路は、経路長を容易かつ正確に算出することができるため、経路上の各地点における自律移動体の到達時間を算出することが可能となる。従って、車両１０が障害物を回避して移動可能な移動経路を容易に探索すると共に、その経路長、及び経路上の任意地の点までの到着予想時刻を容易に計算可能な移動経路を探索することができる。
【００６１】
実施の形態２．
本実施の形態２に係る経路探索装置は、移動領域内に移動障害物が存在する場合においても、移動障害物の進行方向後方を移動する移動経路を探索することができる。上述した実施の形態１に係る経路探索装置では、移動障害物の移動予測について考慮していない。このため、移動領域内に移動障害物が存在する場合には、移動障害物の進行方向前方をふさぐような経路を探索することがある。移動障害物が例えば人間である場合に、大型の自律移動体（ロボット）が高速で人間の前方を横切るときには、横切られた人間は恐怖感を覚える。このため、移動障害物を回避する移動経路は、移動障害物の進行方向後方に回りこむ移動経路とすることが好ましい。
【００６２】
以下、図面を参照しながら本実施の形態２に係る経路探索装置について説明する。尚、本実施の形態２に係る車両１０の構成及び機能は、以下で特に説明する点を除いて実施の形態１で示した車両１０と同一であり、同一の構成及び機能については、その詳細な説明を省略する。
【００６３】
図１５は、本実施の形態２に係る車両１０の制御部１５が有する経路探索機能の構成を示すブロック図である。制御部１５は、探索空間生成部３０と、経路探索部３１と、経路データ３２と、自己位置計算部３３と、距離データ取得部３５とを有する。尚、自己位置計算部３３は、上述した実施の形態１における自己位置計算部２３と同一の構成及び機能を有するため、ここでは説明を省略する。
【００６４】
距離データ取得部３５は、車両１０に備えたレーザレンジセンサなどの測距センサから障害物の距離データを検出し、移動領域内の障害物を検出する。距離データ取得部３５は、検出した障害物の位置情報をグリッドマップに登録する。尚、移動領域内を移動する移動障害物については例えば円筒形により近似し、グリッド空間とは別個に情報を保持するものとする。また、移動障害物の運動は等速直線運動であるものと仮定する。さらに、移動障害物の移動方向の変化に関しては、短い周期で経路探索を繰り返すことにより対応するものとする。
【００６５】
探索空間生成部３０は、グリッドマップ内に存在する障害物を中心として移動禁止領域を設定する。ここで、探索空間生成部３０は、設定した移動禁止領域の外周の形状を、車両１０の移動速度に基づいて設定した所定の曲率を有する図形を用いて生成する。また、探索空間生成部３０は、移動領域内を移動する移動障害物に対しても移動禁止領域を設定する。尚、移動障害物に対して設定する移動禁止領域の詳細については後述する。
【００６６】
経路探索部３１は、移動禁止領域の接線と移動禁止領域の外周とを接続してなる経路候補から、車両１０の移動速度に基づいて経路探索を行う。ここで、経路探索部３１は、移動障害物の将来位置を算出し、算出した移動障害物の将来位置に基づいて、移動障害物を回避する移動経路を探索する。そして、経路探索部３１は、探索した移動経路を経路データ３２として記憶する。制御部１５は、探索された経路データ３２に従って、車両１０の移動を制御する。
【００６７】
続いて、本実施の形態２に係る制御部１５が有する経路探索機能について、図１６に示すフローチャートと図１７乃至１９を用いて説明する。図６は、本実施の形態２に係る制御部１５による経路探索処理を示すフローチャートである。
【００６８】
まず制御部１５は、車両１０が経路探索を行う探索空間としてのグリッドマップを作成する（Ｓ２０１）。
【００６９】
次いで、制御部１５は、移動障害物の現在位置と移動速度を推定する（Ｓ２０２）。移動障害物の現在位置は測距センサ３４の検出値に基づいて推定するものとしてもよいし、車両１０に搭載されているセンサに限定されず、車両１０が移動する環境内に設定されているカメラなどを用いて推定してもよい。また、移動障害物の移動方向及び移動速度は、パーティクルフィルタなどの統計的手法を用いて推定することができる。
【００７０】
次いで、制御部１５は、各障害物グリッドＢを中心とする経路円を作成する（Ｓ２０３）。また、制御部１５は、作成した経路円のうち、移動経路となりえない円を除外し、残った円については、他の障害物グリッドＢと衝突しない円弧の範囲を計算する。
【００７１】
次いで、制御部１５は、車両１０の現在位置と速度及び角速度とから、車両１０が最初に辿る仮想的な円を作成し、上述した場合と同様にして、作成した仮想円について、障害物グリッドＢと衝突しない円弧の範囲を計算する。また、制御部１５は、車両１０の移動速度から、作成した２つの仮想円に車両１０が到達するまでの時間を計算し、その到達点をノードとして設定する。そして、制御部１５は、仮想円のその２つのノードを最初のＯｐｅｎノードとし、Ｏｐｅｎノードリストに追加する（Ｓ２０４）。制御部１５は、Ｏｐｅｎノードが存在するか否かを判定する（Ｓ２０５）。判定の結果、Ｏｐｅｎノードが一つもなかった場合には、探索は失敗であるものとして処理を終了する。一方で、Ｏｐｅｎノードが一つ以上存在する場合には、Ｓ２０６へと進む。
【００７２】
以下、制御部１５は、Ｏｐｅｎノードのうち、全探索の評価値が最小となるノードＮを探索する。探索対象はゴールＧと移動禁止領域を示す円である。ここで、全探索の評価値としては、ノードまでの到達時間が短いものほどその値が小さいものとする。全探索手法としては、Ａ＊探索アルゴリズムやダイクストラ法などの公知の手段を用いることができる。尚、以下の説明では、ノードＮが属する円を円Ｃと表記する。
【００７３】
制御部１５は、Ｏｐｅｎノードリストから、最も評価値が低いノードＮを選択する（Ｓ２０６）。そして制御部１５は、選択したノードＮが、ゴールであるか否かを判定する（Ｓ２０７）。判定の結果、ノードＮがゴールであった場合には、探索は成功として経路探索処理を終了する。一方で、判定の結果、ノードＮがゴールでなかった場合には、Ｓ２０８へと進む。
【００７４】
次いで、制御部１５は、ノードＮが属する円Ｃが初めて訪れる円であるか否かを判定する（Ｓ２０８）。判定の結果、円Ｃを訪れるのが２回目以降であった場合には、１回目に訪れた際に作成した接線の情報をそのまま利用するものとし、Ｓ２１０へと進む。
【００７５】
一方で、判定の結果、ノードＮが属する円Ｃを初めて訪れた場合には、制御部１５は、円Ｃから静止障害物に衝突しない接線を引き、これらの接線集合Ｌを作成する（Ｓ２０９）。
【００７６】
次いで、制御部１５は、接線集合Ｌに含まれる接線ｓを選択する（Ｓ２１０）。以下、接線集合Ｌに含まれる全ての接線ｓについて、Ｓ２１０〜Ｓ２１８の処理を繰り返す。
【００７７】
次いで、制御部１５は、選択した接線ｓの出発ノードまでの到達予想時刻ｔｓを算出する（Ｓ２１１）。即ち、車両１０が、接線ｓにおける出発ノードに到達するまでの到達予想時刻ｔｓを算出する。言い換えると、現時点における接線ｓの出発地点（ｘｒ，ｙｒ）に対して、車両１０が到達する到達予想時刻ｔｓを算出する。
【００７８】
次いで、制御部１５は、接線ｓについて、時刻ｔｓにおける車両の予想位置を中心とした各移動障害物との相対空間を作成する（Ｓ２１２）。より具体的には、制御部１５は、時刻ｔｓにおける移動障害物の位置を計算し、時刻ｔｓにおける車両と移動障害物との相対的な位置関係を示す相対空間を作成する。例えば図１７Ａに示すように、車両１０の移動経路の一部である接線Ｌ１に対して、移動障害物が白抜き矢印の方向に移動している場合を想定する。このとき制御部１５は、図１７Ｂに示すように、車両１０が接線Ｌ１の出発地点に到達する時刻ｔ１を算出すると共に、時刻ｔ１における移動障害物の位置を計算する。尚、図１７Ｂでは、時刻ｔ１における車両１０と移動障害物とを破線により示している。
【００７９】
次いで、制御部１５は、作成した相対空間を参照して、接線ｓについて、車両１０と各移動障害物とが衝突するか否かの衝突判定処理を行う（Ｓ２１３）。
【００８０】
図１８は、制御部１５が作成した相対空間の一例を示す図である。制御部１５は、移動障害物を中心として移動禁止領域を設定する。ここでは、移動禁止領域として円を設定するものとし、移動障害物に対して、その中心から半径ｒｍの円を設定する。移動障害物の移動速度ベクトルを（ｖｘｍ，ｖｙｍ）とする。また、車両１０の半径をｒｒとし、車両１０の移動速度ベクトルを（ｖｘｒ，ｖｙｒ）とする。制御部１５は、車両１０と移動障害物との相対速度ベクトル（ｖｘｍ−ｖｘｒ，ｖｙｍ−ｖｙｒ）の延長線と、移動障害物の相対中心位置（ｘｍ−ｘｒ，ｙｍ−ｙｒ）との距離がｒｍ＋ｒｒ以下であるか否かを判定する。制御部１５は、延長線と移動障害物の相対中心位置との距離がｒｍ＋ｒｒ以下である場合には、車両１０と移動障害物とが衝突する可能性があるものと判定する。図１８に示す例では、制御部１５は、車両１０と移動障害物とが衝突する可能性があるものと判定する。
【００８１】
Ｓ２１３における衝突判定処理結果から、車両１０と各移動障害物とが接線ｓについて衝突する可能性があるか否かを判定する（Ｓ２１３）。判定の結果、移動障害物と衝突する可能性がない場合には、Ｓ２１６へと進む。
【００８２】
一方で、移動障害物と衝突する可能性がある場合には、制御部１５は、衝突する可能性のある移動障害物への回避円までの接線が引けるか否かを判定する（Ｓ２１５）。
【００８３】
判定の結果、回避円までの接線を引くことができる場合には、制御部１５は、移動障害物への回避円までの接線を引き、接線の両端を出発・到達ノードに登録すると共に、接線ｓを接線集合Ｌから取り除く（Ｓ２１６）。即ち、制御部１５は、時刻ｔｓにおける移動障害物を含む回避円へと接線を引く。このとき、移動障害物の進行方向ベクトルと引かれる接線のベクトルとの内積が負になるように、接線を引く（即ち、移動経路が移動障害物の後方を通過するようにする）。例えば図１７Ｂに示すように、制御部１５は、移動障害物の回避円に対して接線Ｌ２を引く。そして、制御部１５は、引かれた各接線の両端を出発ノード・到達ノードとして設定する。
【００８４】
一方で、判定の結果、回避円までの接線を引くことができない場合には、制御部１５は、接線ｓを接線集合Ｌから取り除く（Ｓ２１７）。そして、制御部１５は、集合Ｌは空であるか否かを判定する（Ｓ２１８）。接線集合Ｌが空でない場合には、Ｓ２１０へと戻り、Ｓ２１８までの処理を繰り返す。一方で、接線集合が空である場合には、Ｓ２１９へと進む。
【００８５】
次いで、制御部１５は、ノードＮと円Ｃの各出発ノード間、各出発ノードと各到達ノード間にアークを張り、車両１０がアークを移動するために必要な移動コストを計算する（Ｓ２１９）。さらに、制御部１５は、各到着他の円の円弧上の到達ノードを、Ｏｐｅｎノードリストに追加する（Ｓ２２０）。制御部１５は、Ｏｐｅｎノードを追加した後、Ｓ２０５へと戻り、最も評価値の低いノードＮがゴールとなるか、又は、探索に失敗したかのいずれかの状態となるまで上述した処理を繰り返す。
【００８６】
以下、上述したＳ２０９〜Ｓ２１８の処理について、図１９乃至図２４を参照して具体的に説明する。まず、図１９に示す例において、Ｓ２０８において、円Ｃは初めて訪れる円であると判定されたものとする。次いで、制御部１５は、Ｓ２０９において、円Ｃから静止障害物（図１９において、円Ｃ以外の円であり、移動障害物Ｍ１、Ｍ２を除いた他の円により静止障害物を示す。）に衝突しない接線集合Ｌを作成する。例えば図２０に示すように、Ｓ２０９における処理の結果、制御部１５により接線ｓ１〜ｓ５が作成され、これらの接線を含む接線集合Ｌが作成される。
【００８７】
次いで、制御部１５は、各接線についてＳ２１０〜Ｓ２１８に示した処理をそれぞれ行う。即ち、制御部１５は、選択した接線ｓについて、上述したＳ２１１〜Ｓ２１３に示す処理を行う。接線集合Ｌが、例えば図２１に示す接線ｓ１〜ｓ５により構成される場合には、制御部１５は、接線ｓ１〜ｓ５それぞれに対して移動障害物との衝突判定を行う。各接線についての衝突判定の結果、図２１に示す例では、制御部１５は、接線ｓ１は移動障害物Ｍ１と衝突し、接線ｓ４は移動障害物Ｍ２と衝突するものと判定する（即ち、接線ｓ１及び接線ｓ４については、Ｓ２１４での判定の結果がＹｅｓとなる）。一方で、図２１に示す例では、制御部１５は、接線ｓ１、ｓ２、ｓ５はいずれの移動障害物とも衝突しないものと判定する（即ち、接線ｓ１、ｓ２、ｓ５については、Ｓ２１４での判定の結果がＮｏとなる）。そして、制御部１５は、接線ｓ１、ｓ２、ｓ５については、接線の両端を出発・到達ノードに登録した後、これらの接線ｓ１、ｓ２、ｓ５を接線集合Ｌから取り除く。
【００８８】
一方で、制御部１５は、接線ｓ３及びｓ４については、移動障害物と衝突する可能性があるため、移動障害物との衝突を回避するための回避軌道の生成を試みる。まず、制御部１５は、接線ｓ３については、移動障害物Ｍ１と衝突する可能性があるため、移動障害物Ｍ１の回避軌道を生成する。即ち、衝突する移動障害物Ｍ１への回避円Ｃ＿Ｍ１までの接線が引けるか否かを判定する。図２２に示す例では、回避円Ｃ＿Ｍ１までの接線ｓは他の静止障害物と衝突するため、制御部１５は、この接線ｓを用いた回避軌道の生成は断念する（即ち、Ｓ２１５での判定の結果がＮｏとなる）。そして、制御部１５は、接線ｓ３を接線集合Ｌから取り除く。
【００８９】
また、制御部１５は、接線ｓ４については、移動障害物Ｍ２と衝突する可能性があるため、移動障害物Ｍ２の回避軌道を生成する。即ち、衝突する移動障害物Ｍ２への回避円Ｃ＿Ｍ２までの接線が引けるか否かを判定する。図２３に示す例では、回避円Ｃ＿Ｍ１までの接線ｓ6は他の静止障害物と衝突しないため、制御部１５は、この接線ｓ6を用いて回避軌道を生成する（即ち、Ｓ２１５での判定の結果がＹｅｓとなる）。従って、この接線ｓ6の両端を出発・到達ノードに登録した後、これらの接線ｓ6を接線集合Ｌから取り除く。
【００９０】
このようにして接線集合Ｌに含まれる各接線について処理を繰り返した結果、最終的に出発・到達ノードが登録された接線は、例えば、図２４に示すように、４本の接線ｓ１、ｓ２、ｓ５、ｓ６となる。
【００９１】
以上説明したようにして経路探索を行い、経路探索に成功した場合には、制御部１５は、例えば図１７Ｂにおいて実線で示すような回避経路Ｒを経路データとして出力する。回避経路Ｒの各部分は、移動障害物の円上の移動経路部分は円弧として構成されると共に、移動障害物の円を除いた移動経路部分は移動障害物円の接線から構成される。さらに、回避経路Ｒは、移動障害物の移動方向に対して後方を通過するものである。このため、回避経路Ｒに追従する車両１０は、移動障害物の移動方向の前方を移動せず回避するため、より安全な移動を行うものである。
【００９２】
尚、制御部１５は、移動障害物の後方を回りこんで回避するように移動経路を設定するが、移動障害物の後方を通過する回避経路としては、図２５に示すような４種類の回避経路が想定される。図２５Ａには、車両１０が前方の円Ｃを左回りした後、移動障害物の回避を右方向とした場合の回避経路（接線Ｌ３を含む。）を示す。図２５Ｂには、車両１０が前方の円Ｃを左回りした後、移動障害物の回避を左方向とした場合の回避経路（接線Ｌ４を含む。）を示す。図２５Ｃには、車両１０が前方の円Ｃを右回りした後、移動障害物の回避を右方向とした場合の回避経路（接線Ｌ５を含む。）を示す。図２５Ｄには、車両１０が前方の円Ｃを右回りした後、移動障害物の回避を左方向とした場合の回避経路（接線Ｌ６を含む。）を示す。
【００９３】
実施の形態３．
本実施の形態３に係る経路探索装置は、移動領域内に移動障害物が存在する場合においても、移動経路自体を変更せずに、移動障害物との衝突を回避する移動経路を探索することができる。上述した実施の形態２に係る経路探索装置では、車両１０が移動障害物の進行方向後方を移動する移動経路を新たに探索するものである。尚、本実施の形態３に係る車両１０の構成及び機能は、以下で特に説明する点を除いて実施の形態２で示した車両１０と同一であり、同一の構成及び機能については、その詳細な説明を省略する。
【００９４】
本実施の形態３に係る制御部１５は、上述した実施の形態２で説明したのと同様にして、現時点における経路円Ｃの出発地点に対して、車両１０が到達する到達予想時刻ｔ１を算出する。次いで、制御部１５は、時刻ｔ１における移動障害物の位置を計算する。例えば図２６Ａに示すように、車両１０の移動経路の一部である接線Ｌ１に対して、移動障害物が白抜き矢印の方向に移動している場合を想定する。このとき制御部１５は、図２６Ｂに示すように、車両１０が円Ｃの出発地点に到達する時刻ｔ１を算出すると共に、時刻ｔ１における移動障害物の位置を計算する。尚、図２６Ｂでは、時刻ｔ１における車両１０と移動障害物を破線により示している。
【００９５】
次いで、制御部１５は、車両１０と移動障害物とが時刻ｔ１において衝突する可能性があるか否かを判定する。判定の結果、衝突する可能性がある場合には、制御部１５は、経路候補である接線上において、時刻ｔ１の移動障害物の円と車両１０の位置との間に、減速ノードを設定する。減速ノードは、車両１０の移動速度を所定の速度へと減速指示するノードである。減速ノードの所定の速度は、移動障害物の円が現在の経路候補の接線を通過した以降に、車両１０がその経路候補を移動することができる速度として求める。制御部１５は、設定した減速ノードを用いて、以降の経路探索を行う。例えば図２６Ｂに示すように、制御部１５は、接線Ｌ８上に減速ノードＮ８を設定する。車両１０は設定した減速ノードＮ８を通過する際に減速するため、移動障害物との衝突を回避することができる。これにより、移動障害物の移動に応じた新たな移動経路を探索せずに済み、車両１０を減速させるのみで移動障害物との回避が可能となる。
【００９６】
尚、上述した実施の形態２と本実施の形態３とを組み合わせることにより、移動障害物との衝突を回避するものとしてもよい。即ち、ゴールＧへと到達するまでの時間を短くする行動として、移動障害物の後方を回避する回避経路を新たに探索する行動か、又は、移動障害物の前方において減速させる行動かのいずれかの行動を状況に応じて選択させるようにしてもよい。
【００９７】
実施の形態４．
本実施の形態４に係る経路探索装置は、経路円の接線と他の経路円との接点においても、車両１０が追従可能な移動経路を探索することができる。上述した各実施の形態に係る経路探索装置では、車両１０の角速度の追従性が低い場合には、車両１０は探索された移動経路を追従することが困難な場合がある。これは、移動経路を円と接線のみから構成しており、接線の接点において曲率半径に不連続が発生するためである。尚、本実施の形態４に係る車両１０の構成及び機能は、以下で特に説明する点を除いて上述した各実施の形態で示した車両１０と同一であり、同一の構成及び機能については、その詳細な説明を省略する。
【００９８】
本実施の形態４に係る制御部１５は、経路円の接線と他の経路円の接点との接続を、車両１０の移動速度に基づいて設定したクロソイド曲線により接続する。これにより、経路円の接線と他の経路円の接点においても、車両１０は移動経路の追従性を維持しながら移動することができる。
【００９９】
図２７は、本実施の形態４に係る制御部１５による経路探索処理を示すフローチャートである。図２７に示す例では、Ｓ１０８、Ｓ１０９における処理の対象をクロソイド曲線とする点において、図４に示した実施の形態１に係る制御部１５による経路探索処理と異なる。その他の処理については上述した各実施の形態と同一であり、その詳細な説明は省略する。
【０１００】
例えば図２８Ａや図２８Ｂに示すように、制御部ノード１５は、経路円の接線と他の経路円とをクロソイド曲線により接続する。制御部１５は、クロソイド曲線において、曲線と直線とが切り替わる点をノードとして全探索を行う。クロソイド曲線を接線の代わりに用いることで、自動車のように角速度が小さな移動体や、追従性が低い移動体であっても、追従可能な移動経路を探索することができる。また、クロソイド曲線を表す式からその長さを計算することができるため、制御部１５は、探索した移動経路の長さを計算することができる。さらに、クロソイド曲線の曲率の設定方法としては、例えば、車両１０の移動速度が速い場合にはクロソイド曲線の曲率を大きくし、車両１０の移動速度が遅い場合にはクロソイド曲線の曲率を小さくするように設定してもよい。
【０１０１】
実施の形態５．
本実施の形態５に係る経路探索装置は、車両１０の追従性を重視する移動経路の探索と、障害物との安全な回避を重視する移動経路の探索とを両立することができる。上述した各実施の形態に係る経路探索装置では、１種類の円を用いて、車両１０の追従性と安全性という２つの性質を表現している。このため、車両１０の経路追従性（車両１０の最小回転半径）を重視して障害物の円を作成した場合には、車両１０の回転半径に応じて作成した円の半径が小さなものとなり、障害物と衝突する可能性がある。一方で、障害物と衝突しないことのみを目標として障害物の円を作成した場合には、障害物の大きさに応じて作成した円の半径が小さなものとなり、車両１０が追従できない可能性がある。尚、本実施の形態５に係る車両１０の構成及び機能は、以下で特に説明する点を除いて上述した各実施の形態で示した車両１０と同一であり、同一の構成及び機能については、その詳細な説明を省略する。
【０１０２】
本実施の形態５に係る制御部１５は、車両１０の移動速度に基づいて設定した大きさの円と、車両１０の大きさに対して所定の安全距離を加算して設定した大きさの円とから、より大きな値の円選択し、その選択した円を経路円として作成する。これにより、車両１０の移動速度に基づく移動経路の追従性と、衝突回避を重視する移動経路の安全性とを両立して、移動経路を探索することができる。
【０１０３】
例えば図２９に示すように、制御部１５は、２つの円から、より大きな円を選択する。図では、Ｒ1を車両１０の追従限界半径とし（即ち、追従性を重視する円を示す。）、Ｒ２を（車両１０の半径＋所定の安全距離）とする（即ち、安全性を重視する円を示す。）。制御部１５は、これらＲ１及びＲ２の値を算出し、より大きな値の円を選択し、経路探索を行う。例えば、図４に示した実施の形態１に係る制御部１５による経路探索処理において、Ｓ１０２の経路円作成処理に先立って、これら２つの円から選択を行う。図２９Ａでは、半径がＲ１の円（追従性重視の円）では、追従は可能であっても障害物と接触する可能性がある。そこで、制御部１５は、半径がＲ２の円（安全性重視の円）を選択する。また、図２９Ｂでは、半径がＲ２の円（安全性重視の円）では、障害物と衝突しないものの、車両１０が追従できない可能性がある。そこで、制御部１５は、半径がＲ１の円（追従性重視の円）を選択する。
【０１０４】
実施の形態６．
本実施の形態６に係る経路探索装置は、広大な探索空間においても、移動経路の探索を実現することができる。上述した各実施の形態に係る経路探索装置では、探索アルゴリズムの性質上、障害物グリッドの個数の二乗に比例して、その計算量が増加する。このため、広い探索空間の場合には、移動経路の探索処理を実現することが困難となる可能性がある。尚、本実施の形態６に係る車両１０の構成及び機能は、以下で特に説明する点を除いて上述した各実施の形態で示した車両１０と同一であり、同一の構成及び機能については、その詳細な説明を省略する。
【０１０５】
本実施の形態６に係る制御部１５は、四分木（ｑｕａｄ−ｔｒｅｅ）に代表されるようなデータ構造表現を用いて、計算量が増大しない程度にまで探索空間を分解する。そして、制御部１５は、分解された各区分の領域を対象として、経路探索を行う。これにより、車両１０の移動に応じて経路探索の対象とする移動領域を限定することで、計算時間を削減することができる。従って、広大な探索空間においても経路探索を実現することができる。尚、四分木データ構造とは、平面全体を縦横半分に四分割し、分割後の各領域が全て図形によって占められている場合、又は、全く図形が含まれていない場合には分割を中止し、それら以外の場合には、各領域をさらに四分割してデータを格納する。これを繰り返し、例えば図３０Ｂに示すように２次元図形を区分する。これにより、四分木のツリー構造を用いて探索空間を表現することができる。
【０１０６】
より具体的には、まず、制御部１５は、車両１０が現在位置する領域内の障害物グリッドの数を計算し、計算した障害物グリッドの数が所定の個数以上であるか否かを判定する。判定の結果、障害物グリッドの数が所定の個数以上である場合には、制御部１５は、現在の車両１０が位置する移動領域を四分割する。制御部１５は、障害物グリッドの数が所定の数より少なくなるまで、この分割処理を繰り返す。そして、制御部１５は、分割が終了した領域（障害物の数が所定の数より少ない領域）内において、サブゴールを設定し、上述したようにして現在の位置からサブゴールまでの経路探索を行う。分割された領域内でサブゴールに到達した場合、制御部１５は、隣接する領域内の障害物グリッド数を計算し、計算した障害物グリッドの数が所定の個数以上であるか否かを判定する。以降、上述した処理を繰り返すことにより、ゴールまでの移動経路を探索する。
【０１０７】
例えば図３０Ａに示すように、車両１０が領域Ｐ内を移動する場合を想定する。図２４Ｂに示すように、制御部１５は、領域Ｐに対応するグリッドマップＭＧを四分木構造により表現する。ここで、車両１０が現在位置する区分を拡大したものをグリッドマップＭＬとして示している。グリッドマップＭＬに含まれる障害物グリッドの数は所定の数より少ない。制御部１５は、グリッドマップＭＬにおいて、現在位置からサブゴールまでの局所移動経路Ｒを探索する。
【０１０８】
その他の実施の形態．
上述した各実施の形態では、２次元平面内における経路探索処理について説明したが本発明はこれに限定されない。即ち、移動体が移動する移動領域は３次元の領域としてもよい。この場合には、経路探索装置は、３次元形状の障害物を中心とする移動禁止領域の外周の形状を、所定の曲率を有する３次元形状の図形を用いて生成する。例えば図３１に示すように、経路探索装置は、障害物を中心として球Ｓ１、Ｓ２、Ｓ３を生成する。そして、経路探索装置は、障害物を中心とする球Ｓ１、Ｓ３に沿って、移動経路を探索する。このように、２次元空間に限定されず、３次元空間においても経路探索を行うことで、例えばマニピュレータなどの軌道計算にも適用することができる。
【０１０９】
また、経路探索装置は、移動領域における障害物の配置位置に応じて、図形の大きさを設定するようにしてもよい。これにより、ユーザーの意図を反映させた移動経路を探索することができる。例えば、図３２に示すように、部屋の中心に配置される障害物については経路円の大きさを他の障害物に対する経路円の大きなものとすることで、車両１０に部屋の中心を移動させないような移動経路を意図的に経路探索装置に探索させることができる。また、図３２に示すように、壁を示す障害物に対する経路円の大きさを他の障害物の経路円よりも小さなものとすることで、車両１０に壁沿いに移動させるような移動経路を意図的に探索させることができる。
【０１１０】
また、上述した実施の形態では、自律移動体の制御部が経路探索機能を有するものとして説明したが本発明はこれに限定されない。経路探索機能は、例えば、自律移動体に搭載されたコンピュータや自律移動体とは別に設けられたコンピュータにより実現される。このコンピュータは、例えば、中央処理装置（ＣＰＵ）、ＲＯＭ、ＲＡＭ、ハードディスク等の補助記憶装置、ＣＤ−ＲＯＭ等の可搬型記憶媒体が挿入される記憶媒体駆動装置、入力手段や出力手段を備えている。ＲＯＭ、補助記憶装置、可搬型記憶媒体等の記憶媒体には、オペレーティングシステムと協働してＣＰＵ等に命令を与え、アプリケーションプログラムを記録することができ、ＲＡＭにロードされることによって実行される。このアプリケーションプログラムは、本発明にかかる経路探索機能を実現する特有の経路探索プログラムを含む。経路探索プログラムによる経路探索は、中央処理装置がアプリケーションプログラムをＲＡＭ上に展開した上で当該アプリケーションプログラムに従った処理を補助記憶装置に格納されたデータを読み出し、また格納を行なうことにより、実行される。
【０１１１】
尚、本発明は上述した実施の形態のみに限定されるものではなく、既に述べた本発明の要旨を逸脱しない範囲において種々の変更が可能であることは勿論である。
【図面の簡単な説明】
【０１１２】
【図１】本発明の実施の形態１に係る自律移動体制御システムを概略的に示す全体図である。
【図２】本発明の実施の形態１に係る自律移動体としての車両を示す概略図である。
【図３】本発明の実施の形態１に係る制御部の構成を示す図である。
【図４】本発明の実施の形態１に係る経路探索処理を示すフローチャートである。
【図５】本発明の実施の形態１に係る経路探索処理の様子を説明する図である。
【図６】本発明の実施の形態１に係る経路探索処理の様子を説明する図である。
【図７】本発明の実施の形態１に係る移動禁止領域の図形を例示する図である。
【図８】本発明の実施の形態１に係る不要な経路円を判定する様子を説明する図である。
【図９】本発明の実施の形態１に係る経路探索処理の様子を説明する図である。
【図１０】本発明の実施の形態１に係る経路探索処理の様子を説明する図である。
【図１１】本発明の実施の形態１に係る経路円の接線の種類を例示する図である。
【図１２】本発明の実施の形態１に係る経路探索処理の様子を説明する図である。
【図１３】本発明の実施の形態１に係る経路探索処理の様子を説明する図である。
【図１４】本発明の実施の形態１に係る経路探索処理の様子を説明する図である。
【図１５】本発明の実施の形態２に係る制御部の構成を示す図である。
【図１６】本発明の実施の形態２に係る経路探索処理を示すフローチャートである。
【図１７】本発明の実施の形態２に係る経路探索処理の様子を説明する図である。
【図１８】本発明の実施の形態２に係る相対空間を例示する概念図である。
【図１９】本発明の実施の形態２に係る経路探索処理の様子を説明する図である。
【図２０】本発明の実施の形態２に係る経路探索処理の様子を説明する図である。
【図２１】本発明の実施の形態２に係る経路探索処理の様子を説明する図である。
【図２２】本発明の実施の形態２に係る経路探索処理の様子を説明する図である。
【図２３】本発明の実施の形態２に係る経路探索処理の様子を説明する図である。
【図２４】本発明の実施の形態２に係る経路探索処理の様子を説明する図である。
【図２５】本発明の実施の形態２に係る移動障害物の回避方法の種類を例示する図である。
【図２６】本発明の実施の形態３に係る経路探索処理を示すフローチャートである。
【図２７】本発明の実施の形態４に係る経路探索処理を示すフローチャートである。
【図２８】本発明の実施の形態４に係るクロソイド曲線を例示する図である。
【図２９】本発明の実施の形態５に係る経路円の選択処理を説明する図である。
【図３０】本発明の実施の形態６に係る四分木データ構造を説明する概念図である。
【図３１】本発明のその他の実施の形態に係る３次元空間における経路探索の様子を説明する図である。
【図３２】本発明のその他の実施の形態に係る経路円の大きさの設定の様子を説明する図である。
【符号の説明】
【０１１３】
１  床部、１０  車両、１０ａ  車両本体、１１  車輪、１２  キャスタ、
１３  駆動部（モータ）、１４  カウンタ、１５  制御部、１５ａ  記憶領域、
１６  外界センサ、１７  アンテナ、１００  自律移動体制御システム、
２１、３１  経路探索部、２２、３２  経路データ、２３、３３  自己位置計算部、
２４  障害物位置データ、３４  測距センサ、３５  距離データ取得部、
Ｐ  エリア、Ｇ  ゴール、Ｂ  障害物グリッド、
Ｃ  経路円、ＩＣＲ、ＩＣＬ  仮想円、Ｃ＿Ｍ１、Ｃ＿Ｍ２  回避円、Ｎ  ノード、
Ｌ、ｓ  接線、Ｒ  経路、
ＭＧ、ＭＬ  グリッドマップ
【図１】
2010061293.tif 000004
【図２】
2010061293.tif 000005
【図３】
2010061293.tif 000006
【図４】
2010061293.tif 000007
【図５】
2010061293.tif 000008
【図６】
2010061293.tif 000009
【図７】
2010061293.tif 000010
【図８】
2010061293.tif 000011
【図９】
2010061293.tif 000012
【図１０】
2010061293.tif 000013
【図１１】
2010061293.tif 000014
【図１２】
2010061293.tif 000015
【図１３】
2010061293.tif 000016
【図１４】
2010061293.tif 000017
【図１５】
2010061293.tif 000018
【図１６】
2010061293.tif 000019
【図１７】
2010061293.tif 000020
【図１８】
2010061293.tif 000021
【図１９】
2010061293.tif 000022
【図２０】
2010061293.tif 000023
【図２１】
2010061293.tif 000024
【図２２】
2010061293.tif 000025
【図２３】
2010061293.tif 000026
【図２４】
2010061293.tif 000027
【図２５】
2010061293.tif 000028
【図２６】
2010061293.tif 000029
【図２７】
2010061293.tif 000030
【図２８】
2010061293.tif 000031
【図２９】
2010061293.tif 000032
【図３０】
2010061293.tif 000033
【図３１】
2010061293.tif 000034
【図３２】
2010061293.tif 000035