(57)【特許請求の範囲】
【請求項１】
移動体が基準点から目標地点まで移動する経路を探索する経路探索システムであって、かつ、過去に採用された経路が障害物により遮られ、遮られた経路部分を回避する複数の候補経路を作成して評価することにより、１つの経路を選択する経路探索システムであり、
基準点から目標地点までの複数の候補経路を抽出する候補経路抽出手段と、
前記移動体の目標軌道となる目標経路を、グリッド状に離散化された座標の集合として表現するグリッドマップ上で、前記候補経路抽出手段により抽出された複数の候補経路のそれぞれの評価値を算出する評価値算出手段と、
前記評価値算出手段によって算出された評価値に基づいて採用する経路を決定する経路決定手段とを備え、
前記評価値算出手段は、前記グリッドマップ上で、略一定間隔に配置された格子点について、過去に採用された経路上の格子点にマイナスの値を有するポイント値を付与し、前記候補経路上の格子点に付与されたマイナスの値を有するポイント値の総和と、前記経路長に応じて算出された値とを加算することにより当該候補経路の評価値を算出し、
前記経路決定手段は、算出された評価値のうち最も小さい評価値を有する経路を採用する経路探索システム。
【請求項２】
移動体が基準点から目標地点まで移動する経路を探索する経路探索システムであって、かつ、過去に採用された経路が障害物により遮られ、遮られた経路部分を回避する複数の候補経路を作成して評価することにより、１つの経路を選択する経路探索システムであり、
前記移動体の目標軌道となる目標経路を、グリッド状に離散化された座標の集合として表現するグリッドマップ上で、基準点から目標地点までの複数の候補経路を抽出する候補経路抽出手段と、
前記候補経路抽出手段により抽出された複数の候補経路のそれぞれの評価値を算出する評価値算出手段と、
前記評価値算出手段によって算出された評価値に基づいて採用する経路を決定する経路決定手段とを備え、
前記評価値算出手段は、
前記グリッドマップ上で、略一定間隔に配置された格子点について、
過去に採用された経路上の格子点にポイント値を付与し、
各格子点はリンクにより互いにつながれており、前記採用経路上の各格子点を中心として所定のリンク分離れた格子点まで含む範囲の格子点に、前記採用経路上の格子点に付与したポイント値と同一または当該ポイント値よりも絶対値が小さなポイント値を付与し、
前記候補経路上の格子点に付与されたポイント値の総和と、前記経路長に応じて算出された値とに基づいて当該候補経路の評価値を算出する経路探索システム。
【請求項３】
移動体が基準点から目標地点まで移動する経路を探索する経路探索システムであって、かつ、過去に採用された経路が障害物により遮られ、遮られた経路部分を回避する複数の候補経路を作成して評価することにより、１つの経路を選択する経路探索システムであり、
前記移動体の目標軌道となる目標経路を、グリッド状に離散化された座標の集合として表現するグリッドマップ上で、基準点から目標地点までの複数の候補経路を抽出する候補経路抽出手段と、
前記候補経路抽出手段により抽出された複数の候補経路のそれぞれの評価値を算出する評価値算出手段と、
前記評価値算出手段によって算出された評価値に基づいて採用する経路を決定する経路決定手段とを備え、
前記評価値算出手段は、
前記グリッドマップ上で、略一定間隔に配置された格子点について、
過去に採用された経路上の格子点にマイナスの値を有するポイント値を付与し、
各格子点はリンクにより互いにつながれており、前記採用経路上の各格子点を中心として所定のリンク分離れた格子点まで含む範囲の格子点に、前記採用経路上の格子点に付与したポイント値と同一または当該ポイント値よりも絶対値が小さなポイント値であって、マイナスの値を有するポイント値を付与し、
前記候補経路上の格子点に付与されたマイナスの値を有するポイント値の総和と、前記経路長に応じて算出された値とを加算することにより当該候補経路の評価値を算出し、
前記経路決定手段は、算出された評価値のうち最も小さい評価値を有する経路を採用する経路探索システム。
【請求項４】
前記評価値算出手段は、前記グリッドマップ上で、略一定間隔に配置された格子点について、過去に採用された経路上の格子点にマイナスの値を有するポイント値を付与することに代えて、過去に採用された経路上の格子点にプラスの値を有するポイント値を付与し、前記候補経路上の格子点に付与されたプラスの値を有するポイント値の総和と、前記経路長が短い程高い値となるように算出された値とを加算することにより当該候補経路の評価値を算出し、
前記経路決定手段は、算出された評価値のうち最も大きい評価値を有する経路を採用する
ことを特徴とする請求項１記載の経路探索システム。
【請求項５】
前記評価値算出手段は、
前記グリッドマップ上で、略一定間隔に配置された格子点について、過去に採用された経路上の格子点と前記範囲の格子点にマイナスの値を有するポイント値を付与することに代えて、
過去に採用された経路上の格子点にプラスの値を有するポイント値を付与し、
前記範囲の格子点に、前記採用経路上の格子点に付与したポイント値と同一または当該ポイント値よりも絶対値が小さなポイント値であって、プラスの値を有するポイント値を付与し、
前記候補経路上の格子点に付与されたプラスの値を有するポイント値の総和と、前記経路長が短い程高い値となるように算出された値とを加算することにより当該候補経路の評価値を算出し、
前記経路決定手段は、算出された評価値のうち最も大きい評価値を有する経路を採用する
ことを特徴とする請求項３記載の経路探索システム。
【請求項６】
前記評価値算出手段は、前記グリッドマップ上で、略一定間隔に配置された格子点について、過去に採用された経路上の格子点にマイナスの値を有するポイント値を付与することに代えて、過去に採用された経路上の格子点以外の格子点にプラスの値を有するポイント値を付与し、前記候補経路上の格子点に付与されたプラスの値を有するポイント値の総和と、前記経路長に応じて算出された値とを加算することにより当該候補経路の評価値を算出する
ことを特徴とする請求項１記載の経路探索システム。
【請求項７】
前記評価値算出手段は、
前記グリッドマップ上で、略一定間隔に配置された格子点について、過去に採用された経路上の格子点と前記範囲の格子点にマイナスの値を有するポイント値を付与することに代えて、過去に採用された経路上の格子点と前記範囲の格子点以外の格子点にプラスの値を有するポイント値を付与し、
前記候補経路上の格子点に付与されたプラスの値を有するポイント値の総和と、前記経路長に応じて算出された値とを加算することにより当該候補経路の評価値を算出する
ことを特徴とする請求項３記載の経路探索システム。
【請求項８】
前記移動体はロボットであることを特徴とする請求項１乃至７いずれかに記載の経路探索システム。
【請求項９】
移動体が基準点から目標地点まで移動する経路を探索する経路探索方法であって、かつ、過去に採用された経路が障害物により遮られ、遮られた経路部分を回避する複数の候補経路を作成して評価することにより、１つの経路を選択する経路探索方法であり、
前記移動体の目標軌道となる目標経路を、グリッド状に離散化された座標の集合として表現するグリッドマップ上で、基準点から目標地点までの複数の候補経路を抽出するステップと、
複数の候補経路のそれぞれの評価値を経路長と、過去に採用された経路履歴情報に基づいて算出するステップと、
前記評価値に基づいて採用する経路を決定するステップとを備え、
前記評価値を算出するステップでは、前記グリッドマップ上で、略一定間隔に配置された格子点について、過去に採用された経路上の格子点にマイナスの値を有するポイント値を付与し、前記候補経路上の格子点に付与されたマイナスの値を有するポイント値の総和と、前記経路長に応じて算出された値とを加算することにより当該候補経路の評価値を算出し、
前記経路を決定するステップでは、算出された評価値のうち最も小さい評価値を有する経路を採用する経路探索方法。
【請求項１０】
移動体が基準点から目標地点まで移動する経路を探索する経路探索方法であって、かつ、過去に採用された経路が障害物により遮られ、遮られた経路部分を回避する複数の候補経路を作成して評価することにより、１つの経路を選択する経路探索方法であり、
前記移動体の目標軌道となる目標経路を、グリッド状に離散化された座標の集合として表現するグリッドマップ上で、基準点から目標地点までの複数の候補経路を抽出するステップと、
複数の候補経路のそれぞれの評価値を経路長と、過去に採用された経路履歴情報に基づいて算出するステップと、
前記評価値に基づいて採用する経路を決定するステップとを備え、
前記評価値を算出するステップでは、
前記グリッドマップ上で、略一定間隔に配置された格子点について、
過去に採用された経路上の格子点にポイント値を付与し、
各格子点はリンクにより互いにつながれており、前記採用経路上の各格子点を中心として所定のリンク分離れた格子点まで含む範囲の格子点に、前記採用経路上の格子点に付与したポイント値と同一または当該ポイント値よりも絶対値が小さなポイント値を付与し、
前記候補経路上の格子点に付与されたポイント値の総和と、前記経路長に応じて算出された値とに基づいて当該候補経路の評価値を算出する経路探索方法。
【請求項１１】
移動体が基準点から目標地点まで移動する経路を探索する経路探索方法であって、かつ、過去に採用された経路が障害物により遮られ、遮られた経路部分を回避する複数の候補経路を作成して評価することにより、１つの経路を選択する経路探索方法であり、
前記移動体の目標軌道となる目標経路を、グリッド状に離散化された座標の集合として表現するグリッドマップ上で、基準点から目標地点までの複数の候補経路を抽出するステップと、
複数の候補経路のそれぞれの評価値を経路長と、過去に採用された経路履歴情報に基づいて算出するステップと、
前記評価値に基づいて採用する経路を決定するステップとを備え、
前記評価値を算出するステップでは、
前記グリッドマップ上で、略一定間隔に配置された格子点について、
過去に採用された経路上の格子点にマイナスの値を有するポイント値を付与し、
各格子点はリンクにより互いにつながれており、前記採用経路上の各格子点を中心として所定のリンク分離れた格子点まで含む範囲の格子点に、前記採用経路上の格子点に付与したポイント値と同一または当該ポイント値よりも絶対値が小さなポイント値であって、マイナスの値を有するポイント値を付与し、
前記候補経路上の格子点に付与されたマイナスの値を有するポイント値の総和と、前記経路長に応じて算出された値とを加算することにより当該候補経路の評価値を算出し、
前記経路を決定するステップでは、算出された評価値のうち最も小さい評価値を有する経路を採用する経路探索方法。
【請求項１２】
前記評価値を算出するステップでは、前記グリッドマップ上で、略一定間隔に配置された格子点について、過去に採用された経路上の格子点にマイナスの値を有するポイント値を付与することに代えて、過去に採用された経路上の格子点にプラスの値を有するポイント値を付与し、前記候補経路上の格子点に付与されたプラスの値を有するポイント値の総和と、前記経路長が短い程高い値となるように算出された値とを加算することにより当該候補経路の評価値を算出し、
前記経路を決定するステップでは、算出された評価値のうち最も大きい評価値を有する経路を採用する
ことを特徴とする請求項９記載の経路探索方法。
【請求項１３】
前記評価値を算出するステップでは、
前記グリッドマップ上で、略一定間隔に配置された格子点について、過去に採用された経路上の格子点と前記範囲の格子点にマイナスの値を有するポイント値を付与することに代えて、
過去に採用された経路上の格子点にプラスの値を有するポイント値を付与し、
前記範囲の格子点に、前記採用経路上の格子点に付与したポイント値と同一または当該ポイント値よりも絶対値が小さなポイント値であって、プラスの値を有するポイント値を付与し、
前記候補経路上の格子点に付与されたプラスの値を有するポイント値の総和と、前記経路長が短い程高い値となるように算出された値とを加算することにより当該候補経路の評価値を算出し、
前記経路を決定するステップでは、算出された評価値のうち最も大きい評価値を有する経路を採用する
ことを特徴とする請求項１１記載の経路探索方法。
【請求項１４】
前記評価値を算出するステップでは、前記グリッドマップ上で、略一定間隔に配置された格子点について、過去に採用された経路上の格子点にマイナスの値を有するポイント値を付与することに代えて、過去に採用された経路上の格子点以外の格子点にプラスの値を有するポイント値を付与し、前記候補経路上の格子点に付与されたプラスの値を有するポイント値の総和と、前記経路長に応じて算出された値とを加算することにより当該候補経路の評価値を算出する
ことを特徴とする請求項９記載の経路探索方法。
【請求項１５】
前記評価値を算出するステップでは、
前記グリッドマップ上で、略一定間隔に配置された格子点について、過去に採用された経路上の格子点と前記範囲の格子点にマイナスの値を有するポイント値を付与することに代えて、
過去に採用された経路上の格子点と前記範囲の格子点以外の格子点にプラスの値を有するポイント値を付与し、
前記候補経路上の格子点に付与されたプラスの値を有するポイント値の総和と、前記経路長に応じて算出された値とを加算することにより当該候補経路の評価値を算出する
ことを特徴とする請求項１１記載の経路探索方法。
【請求項１６】
前記移動体はロボットであることを特徴とする請求項９乃至１５いずれかに記載の経路探索方法。
【請求項１７】
移動体が基準点から目標地点まで移動する経路を探索する経路探索プログラムであって、かつ、過去に採用された経路が障害物により遮られ、遮られた経路部分を回避する複数の候補経路を作成して評価することにより、１つの経路を選択する経路探索プログラムであり、コンピュータに対して、
前記移動体の目標軌道となる目標経路を、グリッド状に離散化された座標の集合として表現するグリッドマップ上で、基準点から目標地点までの複数の候補経路を抽出するステップと、
複数の候補経路のそれぞれの評価値を経路長と、過去に採用された経路履歴情報に基づいて算出するステップと、
前記評価値に基づいて採用する経路を決定するステップを実行させ、
前記評価値を算出するステップでは、前記グリッドマップ上で、略一定間隔に配置された格子点について、過去に採用された経路上の格子点にマイナスの値を有するポイント値を付与し、前記候補経路上の格子点に付与されたマイナスの値を有するポイント値の総和と、前記経路長に応じて算出された値とを加算することにより当該候補経路の評価値を算出し、
前記経路を決定するステップでは、算出された評価値のうち最も小さい評価値を有する経路を採用する経路探索プログラム。
【請求項１８】
基準点から目標地点まで移動する経路を探索可能な移動体であって、かつ、過去に採用された経路が障害物により遮られ、遮られた経路部分を回避する複数の候補経路を作成して評価することにより、１つの経路を選択する移動体であり、
前記移動体の目標軌道となる目標経路を、グリッド状に離散化された座標の集合として表現するグリッドマップ上で、基準点から目標地点までの複数の候補経路を抽出し、
複数の候補経路のそれぞれの評価値を経路長と、過去に採用された経路履歴情報に基づいて算出し、
前記評価値に基づいて採用する経路を決定し、
前記評価値の算出では、前記グリッドマップ上で、略一定間隔に配置された格子点について、過去に採用された経路上の格子点にマイナスの値を有するポイント値を付与し、前記候補経路上の格子点に付与されたマイナスの値を有するポイント値の総和と、前記経路長に応じて算出された値とを加算することにより当該候補経路の評価値を算出し、
前記経路の決定では、算出された評価値のうち最も小さい評価値を有する経路を採用する移動体。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体の経路探索システム及び経路探索方法に関し、特にロボットの移動経路を探索するシステム及びその方法に関する。
【背景技術】
【０００２】
従来より、ロボットや車両等の移動体が目的地まで移動するために、経路を探索し、探索された経路に対して自動追従して移動する技術が提案されている。経路を探索する方法は、例えば、特許文献１や特許文献２に開示されている。
【０００３】
特許文献１には、生成したグリッドマップから所定の条件に基づきボロノイグラフを生成し、そして物体との衝突可能性と経路長をコスト（ポイント値）として経路の探索を行なう移動ロボット用経路計画システムが開示されている。
【０００４】
また、特許文献２には、移動体に搭載され、経路案内を行なうナビゲーション装置に関する技術が開示されている。この特許文献２に開示されたナビゲーション装置は、移動体の過去の移動履歴を考慮して経路探索場所を決定している。このようにすることで、ユーザの道路に対する知識の有無を予測することで情報センタとの通信を行なう通信時間・コストを軽減することができるという効果がある。
【特許文献１】特開２００５−３２１９６号公報
【特許文献２】特開２００１−１４１４９２号公報
【発明の開示】
【発明が解決しようとする課題】
【０００５】
障害物の位置が逐次変化する動的な環境において移動体が移動する場合には、環境の変化に応じて移動体の移動経路を逐次再計画する必要がある。このとき、経路計画には、距離や時間、消費エネルギー等の評価値を最小にするような経路を見つける手法が一般的である。しかしながら、図７で示されるようにロボット１がゴール地点２０に向かう経路上で人間のような動的な障害物２がロボット１の正面に存在する場合には、ロボット１の微小な位置変化によって最適経路が頻繁に変わる。特に、障害物が微小に位置変化することによって、左から避ける経路と右から避ける経路が頻繁に切り替わる状態が発生し、障害物２の前でロボット１が右往左往し、動作が不安定となる問題が発生した。
【０００６】
また、障害物の位置が変化しない環境においても、ロボットによる障害物の検出精度が高くない場合には、障害物の位置変化がない場合であっても位置変化があると検出してしまうことがあり、これに起因して最適経路が頻繁に変わることもあった。
【０００７】
さらには、グリッドマップ等を用いた経路探索においては、同じ経路長となる複数の経路が候補として選択されうるが、同じ経路長であればいずれの経路を選択すべきかが決まっておらずランダムに選択されるため、経路探索を実行する度に最適経路が異なる場合もあった。
【０００８】
本発明は、かかる課題を解決するためになされたものであり、移動体を安定して移動動作させることができる経路探索システム及び経路探索方法を提供することを目的とする。
【課題を解決するための手段】
【０００９】
本発明にかかる経路探索システムは、移動体が基準点から目標地点まで移動する経路を探索する経路探索システムであって、基準点から目標地点までの複数の候補経路を抽出する候補経路抽出手段と、前記候補経路抽出手段により抽出された複数の候補経路のそれぞれの評価値を算出する評価値算出手段と、評価値算出手段によって算出された評価値に基づいて採用する経路を決定する経路決定手段とを備え、前記評価値算出手段は、基準点から目標地点までの経路長と、過去に採用された経路履歴情報に基づいて評価値を算出するものである。本発明は、かかる構成を有するので、過去に採用された経路と類似した経路を採用することができ、移動体の移動動作を安定化することができる。
【００１０】
ここで、前記評価値算出手段は、過去に採用された経路上のノードにポイント値を付与し、前記候補経路上のノードに付与されたポイント値の総和と、前記経路長に応じて算出された値とに基づいて当該候補経路の評価値を算出することによって、容易に過去に採用された経路と類似した経路を採用する処理を実現できる。ここで、「過去に採用された経路」とは、前回採用された経路のみであってもよく、前回採用された経路に加えてさらにそれ以前に採用されことがある経路のうちの幾つかを含むものであってもよい。また、「ノード」とは、グリッドマップ上で、略一定間隔に配置された格子点をいう。
【００１１】
また、前記評価値算出手段は、過去に採用された経路上のノードにマイナスの値を有するポイント値を付与し、前記候補経路上のノードに付与されたマイナスの値を有するポイント値の総和と、前記経路長に応じて算出された値とを加算することにより当該候補経路の評価値を算出し、前記経路決定手段は、算出された評価値のうち最も小さい評価値を有する経路を採用することとすれば、簡単なアルゴリズムにより処理が実現できる。
【００１２】
さらに、前記評価値算出手段は、過去に採用された経路上のノードと当該採用経路の近傍に位置するノードにポイント値を付与し、前記候補経路上のノードに付与されたポイント値の総和と、前記経路長に応じて算出された値とに基づいて当該候補経路の評価値を算出することが望ましい。これにより、経路が最短経路へ安定的に漸近する経路探索を行なうことができる。また、前記評価値算出手段は、過去に採用された経路上のノードと当該採用経路の近傍に位置するノードにマイナスの値を有するポイント値を付与し、前記候補経路上のノードに付与されたマイナスの値を有するポイント値の総和と、前記経路長に応じて算出された値とを加算することにより当該候補経路の評価値を算出し、前記経路決定手段は、算出された評価値のうち最も小さい評価値を有する経路を採用することとすれば、簡単なアルゴリズムにより処理が実現できる。
【００１３】
好適な実施の形態における移動体はロボットである。
【００１４】
本発明にかかる経路探索方法は、移動体が基準点から目標地点まで移動する経路を探索する経路探索方法であって、基準点から目標地点までの複数の候補経路を抽出するステップと、複数の候補経路のそれぞれの評価値を経路長と、過去に採用された経路履歴情報に基づいて算出するステップと、評価値に基づいて採用する経路を決定するステップとを備えたものである。本発明は、かかるステップを実行するので、過去に採用された経路と類似した経路を採用することができ、移動体の移動動作を安定化することができる。
【００１５】
ここで、過去に採用された経路上のノードにポイント値を付与し、前記候補経路上のノードに付与されたポイント値の総和と、前記経路長に応じて算出された値とに基づいて当該候補経路の評価値を算出することによって、容易に過去に採用された経路と類似した経路を採用する処理を実現できる。
【００１６】
また、過去に採用された経路上のノードにマイナスの値を有するポイント値を付与し、前記候補経路上のノードに付与されたマイナスの値を有するポイント値の総和と、前記経路長に応じて算出された値とを加算することにより当該候補経路の評価値を算出し、算出された評価値のうち最も小さい評価値を有する経路を採用することとすれば、簡単なアルゴリズムにより処理が実現できる。
【００１７】
さらに、過去に採用された経路上のノードと当該採用経路の近傍に位置するノードにポイント値を付与し、前記候補経路上のノードに付与されたポイント値の総和と、前記経路長に応じて算出された値とに基づいて当該候補経路の評価値を算出することが望ましい。これにより、経路が最短経路へ安定的に漸近する経路探索を行なうことができる。また、過去に採用された経路上のノードと当該採用経路の近傍に位置するノードにマイナスの値を有するポイント値を付与し、前記候補経路上のノードに付与されたマイナスの値を有するポイント値の総和と、前記経路長に応じて算出された値とを加算することにより当該候補経路の評価値を算出し、算出された評価値のうち最も小さい評価値を有する経路を採用することとすれば、簡単なアルゴリズムにより処理が実現できる。
【００１８】
好適な実施の形態における移動体はロボットである。
【００１９】
他方、本発明にかかる経路探索プログラムは、移動体が基準点から目標地点まで移動する経路を探索する経路探索プログラムであって、コンピュータに対して、基準点から目標地点までの複数の候補経路を抽出するステップと、複数の候補経路のそれぞれの評価値を経路長と、過去に採用された経路履歴情報に基づいて算出するステップと、評価値に基づいて採用する経路を決定するステップを実行させるものである。本発明は、かかるプログラムをコンピュータに実行させることにより、過去に採用された経路と類似した経路を採用することができ、移動体の移動動作を安定化することができる。
また、本発明にかかる移動体は、基準点から目標地点まで移動する経路を探索可能な移動体であって、基準点から目標地点までの複数の候補経路を抽出し、複数の候補経路のそれぞれの評価値を経路長と、過去に採用された経路履歴情報に基づいて算出し、評価値に基づいて採用する経路を決定するものである。これにより、過去に採用された経路と類似した経路を採用することができ、移動体は安定して移動することができる。
【発明の効果】
【００２０】
本発明によれば、移動体を安定して移動動作させることができる経路探索システム及び経路探索方法を提供することができる。
【発明を実施するための最良の形態】
【００２１】
発明の実施の形態１．
本発明の実施の形態１にかかる経路探索システムでは、移動体としてロボットを用いている。ロボットは、例えば、それぞれ独立して回転制御される二つの車輪を備えたものでもよく、２輪よりも多い、例えば４輪走行のロボットや、１輪走行のロボット、２足歩行・４足歩行のロボットであってもよい。また、ロボットに限らず、２輪走行や４輪走行の車両にも適用できる。本例におけるロボットは、障害物を検出するセンサやカメラを備え、検出された障害物に応じて、これを避けるように経路探索を行なう。
【００２２】
当該経路探索システムは、例えば、ロボットに搭載されたコンピュータやロボットとは別に設けられたコンピュータにより実現される。このコンピュータは、例えば、中央処理装置（ＣＰＵ）、ＲＯＭ、ＲＡＭ、ハードディスク等の補助記憶装置、ＣＤ−ＲＯＭ等の可搬型記憶媒体が挿入される記憶媒体駆動装置、入力手段や出力手段を備えている。ＲＯＭ、補助記憶装置、可搬型記憶媒体等の記憶媒体には、オペレーティングシステムと協働してＣＰＵ等に命令を与え、アプリケーションプログラムを記録することができる。アプリケーションプログラムはＲＡＭにロードされることによって実行される。このアプリケーションプログラムは、本発明にかかる経路探索システムを実現する特有の経路探索プログラムを含む。経路探索システムによる経路探索は、中央処理装置がアプリケーションプログラムをＲＡＭ上に展開した上で当該アプリケーションプログラムに従った処理を補助記憶装置に格納されたデータを読み出し、また格納を行なうことにより、実行される。
【００２３】
最初に、本実施の形態１にかかる経路探索システムの基本的な処理について、図１の概念図を用いて説明する。
【００２４】
図１に示されるように、ロボット１がスタート地点（始点）１０からゴール地点（終点，目標地点）２０まで移動する経路１００を探索する。このとき、スタート・ゴール間にある障害物２が徐々に白抜き矢印の方向に移動したとする。すると、この障害物２の移動に伴って、経路１００は、経路１００ａ，１００ｂ，１００ｃ，１００ｄへと切り替わることになり、ロボット１が安定して移動することができる。
【００２５】
本発明では、ロボット１の経路計画を行なう場合に、経路の評価値として過去に採用された経路の履歴情報を利用する。これにより、障害物２を避けながら、かつ過去に採用された経路と類似した経路を生成することが可能となり探索経路にヒステリシスを持たせることができ、経路が頻繁に切り替わるのを抑制できるため、ロボット１の移動動作を安定させることができる。特に、経路長のみを評価値として経路探索を行なった場合に見られるような、経路探索を行なう度に全く異なる経路へ切り替わる現象がなくなり、安定した経路生成が実現できる。
【００２６】
図２は、本実施の形態１にかかる経路探索システムの概略構成を示すブロック図である。図に示されるように、経路探索システム３は、処理部３１と記憶部３２により構成されている。処理部３１は、候補経路抽出部３１１、評価値算出部３１２及び経路決定部３１３を備えている。候補経路抽出部３１１は、基準点から目標地点までの複数の候補経路を抽出する。評価値算出部３１２は、候補経路抽出部３１１により抽出された複数の候補経路のそれぞれの評価値を算出する。特にこの評価値算出部３１２は、基準点から目標地点までの経路長と、過去に採用された経路履歴情報に基づいて評価値を算出する。経路決定部３１３は、評価値算出部３１２によって算出された評価値に基づいて採用する経路を決定する。
【００２７】
続いて、本実施の形態１にかかる経路探索システムの具体的な処理について、図３に示すフローチャート及び図４に示すグリッドマップを用いて説明する。ここで、図４に示すグリッドマップは、ロボット１の目標軌道となる目標経路を、グリッド状に離散化された座標の集合として表現するものである。グリッドマップは、格子状に並んだノードと、各ノードをつなぐリンクを備えている。グリッド座標を利用するメリットは、最短経路探索を容易に実行できる点や、確実に目的地に到達できる経路を得ることができる点にある。グリッドマップに関する情報は、当該経路探索システムを構成するコンピュータの記憶手段に格納される。ここで、グリッドマップに関する情報には、各ノードの座標情報や各ノードに関連付けられたコスト情報であるポイント値情報が含まれる。なお、初期状態では、各ノードにはポイント値情報は含まれていないが、経路探索を実行した後に選択された採用経路上の各ノードにマイナスのポイント値情報が関連付けられる。
【００２８】
まず、スタート地点１０を基準点として基準点からゴール地点２０までの経路を候補経路として抽出する（Ｓ１０１）。この時点では、途中でスタート地点１０に戻るような経路を含まない経路を全て抽出する。抽出された候補経路の情報はコンピュータの記憶手段に格納される。
【００２９】
次に、複数の候補経路のそれぞれについて評価値を算出する（Ｓ１０２）。本例では、スタート地点からゴール地点までの経路長（リンク長）と、前回の経路（即ち、過去に採用した経路）と同じノードを通過することによるマイナスのポイント値とに基づいて評価値を算出し決定する。ロボット１の移動経路としては、短いことが望まれるため、経路長は短い方が、本例における評価値としては小さくなる。また、前回と同じノードを通過した場合には、マイナスの値を有するポイント値が与えられるため、前回と同じノードを通過すればするほど、評価値は小さくなる。具体的に、コンピュータは、記憶手段より候補経路の情報を読み出し、経路上の各ノードの座標より経路長（経路長に応じて算出された値に相当）を求めるとともに、同じく経路上の各ノードのポイント値に基づきマイナスの値を有するポイント値の総和とを求める。そして、経路長とポイント値の総和とを加算することによって評価値を算出する。算出された評価値もコンピュータの記憶手段に格納される。なお、経路長に応じて算出された値は、経路上の各ノードに対してプラスのポイント値を与え、これを総和したり、単にノード数に定数を掛けることで求めることも可能である。
【００３０】
このようにして複数の候補経路のそれぞれについて算出された評価値に基づいて採用する経路を決定する（Ｓ１０３）。本例では、最も小さい評価値と算出された候補経路が採用経路として選択される。具体的に、コンピュータは、記憶手段より各候補経路の評価値を読み出し、比較処理を行うことにより最も小さい評価値をもつ候補経路を選択し、その採用経路に関する情報を記憶手段に格納する。
【００３１】
次に、決定した採用経路上のノードに対してマイナスのポイント値を記憶する（Ｓ１０４）。このとき、前回決定された採用経路上のノードにはマイナスのポイント値が記憶されているが、このポイント値は削除され、新たに今回決定した採用経路上のノードに対してマイナスのポイント値が記憶される。即ち、前回の経路探索によりノードに与えられたポイント値が今回の経路探索の結果に応じて更新される。具体的に、コンピュータは、ノードに与えられたポイント値を削除し、そして記憶手段に格納された採用経路に関する情報を読み出し、この採用経路に含まれる各ノードに関連付けてマイナスのポイント値を記憶する。
【００３２】
図４に示す例では、採用された経路１００上において白丸で示される各ノード１０１に対してマイナスのポイント値が関連付けられる。この例では、採用された経路１００と同じ経路長の経路２００がある。しかしながら、同じ経路長をもつ経路１００と経路２００であっても、経路１００上の各ノード１０１にはマイナスのポイント値が与えられているため、次回の採用経路の決定処理（Ｓ１０３）では、経路１００の方が経路２００よりも当該マイナスのポイント値分だけ評価値が小さくなるので、経路１００の方が採用される。
【００３３】
図３で示されるステップＳ１０１〜Ｓ１０４で採用経路が決定されるが、かかる処理は、１秒間に１回以上（例えば、５回）の頻度で実行される。ここで、グリッド座標において表現される目標経路は直線と直線が接続された不連続な経路であるため、ロボット１がこのような経路を追従することは難しい。そのため、本例では、離散的な経路座標から、ロボット１の追従制御に利用可能な滑らかな目標経路を、重み付き移動平均を利用して補間曲線を求めることにより作成している。ロボット１は、このようにして作成した曲線からなる目標経路に対して追従制御を実行する。
【００３４】
本実施の形態１では、ロボット１が目標経路に対して追従制御を実行し、スタート地点からある程度移動した後であっても経路探索は、スタート地点からゴール地点まで行なう。ロボット１の目標経路は、上述のように補間曲線を求めているが、この補間曲線に対して円弧近似を行い、いわゆるフィードフォワード制御を実行する場合には、ロボット１の現在位置の最近傍に位置する、目標経路上の点のみならず、それよりも、スタート地点に近い点を通過するように円弧近似することにより滑らかな移動を実現できる。このような処理を可能にするためには、現在位置を基準点として基準点からゴール地点までを経路探索するのではなく、現在位置よりも前のスタート地点を基準点として基準点からゴール地点までを経路探索することが好ましい。
【００３５】
他方で、このように常にスタート地点を基準点として基準点からゴール地点まで経路探索すると、ロボット１が現在位置する場所から大幅に離れた位置に、再計算の結果、採用経路が位置する可能性がある。本実施の形態１では、上述のように、過去に探索して得られた経路と類似した経路が採用されるように処理しているため、このようにロボット１が大幅に採用経路から離間する可能性が少ない。
【００３６】
このとき、予め定められた距離以上にロボット１の現在位置と採用経路との間の距離が離れたかどうかを判定し、離れてない場合には、スタート地点からゴール地点までを経路探索し、離れたと判断した場合に、ロボット１の現在位置からゴール地点までを経路探索するようにしてもよい。
【００３７】
なお、上述したように、経路長は短い方が好ましいため、経路長の情報が小さい候補経路ほど優先的に評価される。そのため、上述の例ではこの経路長の情報に対してそのまま単純に加算処理することができるように、前回採用された経路上のノードに対してマイナスのポイント値を与えたが、経路長の情報を変換式により経路長が短い程高い数値となるようにした場合には、前回採用された経路上のノードに対してプラスのポイント値を与えるようにしてもよい。さらには、前回採用された経路上のノード以外のノードに対してプラスのポイント値を与えることにより相対的に前回採用された経路上のノードがマイナスとなるように処理することも可能である。また、ポイント値は、各ノードに対して同じである必要はない。例えば、経路上のノードに対してマイナスのポイント値を与える場合に、スタート地点に近いノードをゴール地点に近いノードよりも大きい値とすることも可能である。
【００３８】
なお、上述の例では、前回採用された経路上のノードに対してマイナスのポイント値を与えたが、これに限らず、過去に採用された経路上であれば、前回採用された経路でなくてもよい。例えば、前回採用された経路と、前々回採用された経路の双方にマイナスのポイント値を与えてもよい。即ち、前回採用された経路に加えてさらにそれ以前に採用されことがある経路のうちの幾つかを含むものであってもよい。
【００３９】
以上説明したように、本発明の実施の形態１にかかる経路探索システムでは、特に、基準点から目標地点までの経路長と、過去に採用された経路履歴情報に基づいて評価値を算出して、採用する経路を決定しているため、過去に採用された経路に対して類似度が高い経路を採用することができ、移動体を安定して移動動作させることができる。
【００４０】
発明の実施の形態２．
本実施の形態２にかかる経路探索システムは、前回採用した経路上のノードにマイナスのポイント値を与えることに加えて、前回採用した経路の近傍のノードに対してもマイナスのポイント値を与えている。これにより、探索経路が最短な経路へ漸近し、ロボット１が安定して移動することができる。
【００４１】
続いて、本実施の形態２にかかる経路探索システムの具体的な処理について、図５に示すフローチャート及び図６に示すグリッドマップを用いて、発明の実施の形態１との相違点を中心に説明する。従って、以下で説明していない構成及び動作に関しては、基本的に発明の実施の形態１と同じである。
【００４２】
まず、スタート地点１０からゴール地点２０までの経路を候補経路として抽出する（Ｓ２０１）。次に、複数の候補経路のそれぞれについて評価値を算出する（Ｓ２０２）。複数の候補経路のそれぞれについて算出された評価値に基づいて採用する経路を決定する（Ｓ２０３）。
【００４３】
次に、決定した採用経路上のノード及びその近傍のノードに対してマイナスのポイント値を記憶する（Ｓ２０４）。
【００４４】
図６に示す例では、採用された経路１００上において白丸で示されるノード１０１ａと、ノード１０１ａに対して一つのリンク分だけ離れたノード１０１ｂに対してマイナスのポイント値が関連付けられる。本例では、ノード１０１ａとノード１０１ｂに対して同じポイント値が与えられるが、ノード１０１ａの方がノード１０１ｂよりもポイント値の絶対値が大きくなるよう、即ち、値が小さくなるようなマイナスのポイント値を与えるようにしてもよい。
【００４５】
また、本実施の形態２では、ノード１０１ｂは、ノード１０１ａに対して一つのリンク分だけ離れたものであったが、二つ以上離れたノードまで含むようにしてもよい。
【００４６】
以上説明したように本実施の形態２にかかる経路探索システムによれば、採用された経路上のノードに加えてその近傍のノードにまでマイナスのポイント値を記憶させて、経路の再計画を繰り返し行なっているので、経路が最短経路へ安定的に漸近する経路探索を行なうことができる。
【図面の簡単な説明】
【００４７】
【図１】本発明にかかる経路探索システムの処理を説明するための説明図である。
【図２】本発明にかかる経路探索システムの構成を示すブロック図である。
【図３】本発明にかかる経路探索システムにおける処理の流れを示すフローチャートである。
【図４】本発明にかかる経路探索システムにおいて用いられるグリッドマップである。
【図５】本発明にかかる経路探索システムにおける処理の流れを示すフローチャートである。
【図６】本発明にかかる経路探索システムにおいて用いられるグリッドマップである。
【図７】従来の経路探索における問題点を説明するための説明図である。
【符号の説明】
【００４８】
１  ロボット
２  障害物
３  経路探索システム
１０  スタート地点
２０  ゴール地点
３１  処理部
３２  記憶部
１００  目標経路
１０１  ノード
３１１  候補経路抽出部
３１２  評価値算出部
３１３  経路決定部
【図１】
0004577248.tif 000002
【図２】
0004577248.tif 000003
【図３】
0004577248.tif 000004
【図４】
0004577248.tif 000005
【図５】
0004577248.tif 000006
【図６】
0004577248.tif 000007
【図７】
0004577248.tif 000008