JP2024517890A - 通行エリアの経路融合計画方法、ロボット及びチップ - Google Patents

Abstract

通行エリアの経路融合計画方法、ロボット及びチップを提供し、ロボットが、障害物が多く分布する狭い通路内で融合可能な候補ルートを予め探索し、その後、経路融合計画方法を用いて、ヒューリスティック探索アルゴリズムと探索条件に合致した候補ルートとを融合して全体的なナビゲーション経路を計画し、地図の格子のマーキング誤差を克服し、これにより、格子地図における通路が狭くてマーキング誤差が生じやすい通行可能エリア内でのロボットのナビゲーション経路計画問題を解決する。【選択図】図１

Description

本発明は、ロボットの経路計画の技術分野に関し、通行エリアの経路融合計画方法、ロボット及びチップに関する。

従来技術における移動ロボットは、センサを用いて周囲環境を自律的に検知し、コントローラを用いて機器本体の動きを決定し、ホイールなどのアクチュエータを用いて動きを実現するロボットである。従来技術の清掃ロボットは、家庭環境においてスツールの４本足やリビングテーブルの入り口など、様々な家具コンポーネン間の制限によって形成される狭い通路、部屋のドアが開いて形成される狭い通路に移動することが多い。ロボットがこれらの狭い通路へ進入すると、ロボットが滑ったり、測位用センサに累積誤差が存在したり、視覚地図の最適化にも誤差が発生したりすることがあるため、ロボットは、リアルタイムで構築した格子地図上でこれらの狭い通路を障害物が占めるエリアとしてマークしやすく、その結果、ロボットの構築した格子地図にマッピングされた狭い通路の入り口は塞がれているが、実際の運動シーンでは本当に塞がれていない。このため、従来の経路探索アルゴリズムに基づくと、地図の誤差でマークされた障害物格子を迂回することができず、ロボットがナビゲーションに頼って、通路が狭いためマーキング誤差が生じやすい通行可能エリアに出入りするのに不利である。

本発明は、通行エリアの経路融合計画方法、ロボット及びチップを提供する。格子地図における、通路が狭くてマーキング誤差が生じやすい通行可能エリア内でのロボットの出入りの問題を解決するために、ロボットが、障害物が多く分布する狭い通路内で融合可能な候補ルートを予め探索し、その後、前記経路融合計画方法を用いて、ヒューリスティック探索アルゴリズムと探索条件に合致した候補ルートとを融合して全体的なナビゲーション経路を計画し、地図の格子のマーキング誤差を克服する。

具体的には、以下の構成となる。
通行エリアの経路融合計画方法であって、格子地図においてナビゲーション始点及びナビゲーション終点を設定し、トラバース対象ノードキャッシュ空間を作成するステップ１と、ナビゲーション始点を現在の親ノードに設定し、トラバース対象ノードキャッシュ空間に追加するステップ２と、現在の親ノードを探索中心として、格子地図において近傍探索を行うステップ３であって、現在の親ノードに隣接する８つの格子点は、それぞれその子ノードとするステップ３と、ステップ３にて近傍で探索された子ノードが、予め探索された候補ルート座標集合内の対応する候補ルートの一方の端点である場合、対応する候補ルートの他方の端点をトラバース対象ノードキャッシュ空間に追加し、同時に、対応する候補ルートの両端点間のすべての中間ノードとこの端点とを、すべて、繰り返し探索不可能なノードに設定し、ステップ３にて探索された、前記対応する候補ルート上にないフリー格子点をトラバース対象ノードキャッシュ空間に追加し、その後、ステップ５に進むステップ４であって、前記候補ルート座標集合内の対応する候補ルートは、格子地図上にマークされた障害物格子点を通過することを許可され、トラバース対象ノードキャッシュ空間に追加されたすべてのノードは、後続の経路ノードのトレースバックのために、その親ノードの位置情報が記録されているステップ４と、ステップ４で述べたトラバース対象ノードキャッシュ空間内から、経路コストの合計値が最小となるノードを次の親ノードとして選択し、次に、前記次の親ノードが前記ナビゲーション終点であるか否かを判断し、ＹＥＳであれば、前記記録された親ノードの位置情報に基づいて、前記ナビゲーション終点から、ステップ４で述べた候補ルートのすべての中間ノード及びその対応する１つの端点を除いて、子ノード及びその親ノードを、前記ナビゲーション始点に接続されるまで順次接続することで、前記ナビゲーション始点から前記ナビゲーション終点までの経路を計画し、ＮＯであれば、前記次の親ノードをステップ３で述べた現在の親ノードに更新し、そしてステップ３に戻るステップ５と、を含む。

従来技術と比べて、この経路計画案によれば、ナビゲーション始点から始まり、トラバース対象ノードキャッシュ空間内から経路コストの合計値が最小となるノードを親ノードとして順次選択して近傍探索を行い、近傍探索された、候補ルート上にないフリー格子点と、候補ルート上にある識別的価値を持つ１つの端点（始点又は終点）を、すべて、トラバース対象ノードキャッシュ空間に追加し、対応する親ノードを経路ノード遡及情報として記録する。同時に、候補ルート上の中間ノード（いずれもこれらの狭い通路に適したナビゲーション目標点であるに関わらず）による繰り返し探索への干渉を排除し、親ノードとしてナビゲーション終点となると、さらに子ノードと親ノードの位置関係に従って、ナビゲーション終点から、探索した候補ルートを接続し、ナビゲーション始点に逆方向に接続することにより、通行条件に合致する候補ルートを成熟した経路探索アルゴリズムに融合してナビゲーション経路を計画し、格子地図にマッピングされた狭い通路が狭くてマーキング誤差が生じやすいことを克服し、ロボットが計画された全体的なナビゲーション経路に沿ってこの狭い通路に効果的に出入りできるようになり、経路のナビゲーションが失敗する確率を減らす。

さらに、ステップ４は、ステップ３にて近傍で探索された子ノードが前記候補ルート座標集合内のすべての候補ルートの端点でない場合、ステップ３にて近傍で探索されたフリー格子点をすべて前記トラバース対象ノードキャッシュ空間内に追加し、ステップ３にて近傍で探索されたフリー格子点の親ノードの位置情報を記録し、その後、ステップ５に進むことをさらに含む。これにより、子ノードの探索範囲を拡大するだけでなく、前記候補ルート座標集合内のルートの始点及び終点の探索をより広い範囲で拡張することにも寄与し、計画された経路をより高い実用性及び代表性を有するものにできる。

さらに、前記ステップ４では、ステップ３にて近傍で探索された子ノードが前記候補ルート座標集合内の対応する候補ルートの始点である場合、対応する候補ルートの終点を前記トラバース対象ノードキャッシュ空間に追加し、前記対応する候補ルートの終点の親ノードの位置情報を、前記候補ルート座標集合内で各候補ルートがその終点をルート識別情報として使用するように記録し、同時に、対応する候補ルートの両端点間のすべての中間ノードと同一の候補ルートの始点とを、すべて、繰り返し探索不可能なノードに設定し、ここで、前記対応する候補ルートの始点からその終点に向かう経路延在方向に沿って、前記対応する候補ルート上の各ノードの親ノードは、そのノードに隣接するノード位置に位置している。

さらに、前記ステップ４では、ステップ３にて近傍で探索された子ノードが前記候補ルート座標集合内の対応する候補ルートの終点である場合、対応する候補ルートの始点を前記トラバース対象ノードキャッシュ空間に追加し、対応する候補ルートの始点の親ノードの位置情報を、前記候補ルート座標集合内で各候補ルートがその始点をルート識別情報として使用するように記録し、同時に、対応する候補ルートの両端点間のすべての中間ノードと同一の候補ルートの終点とを、すべて、繰り返し探索不可能なノードに設定し、ここで、前記対応する候補ルートの終点からその始点に向かう経路延在方向に沿って、前記対応する候補ルート上の各ノードの親ノードは、そのノードに隣接するノード位置に位置している。

ステップ４に対応する２つの技術案によれば、前記候補ルートの始点又はその終点を、１つの候補ルートを一意に特定できるノードとして構成し、対応する候補ルートの識別子として用いて、対応する親ノード情報を記録する一方、同一の候補ルートの残りのノードを、繰り返し探索されたノードとして構成することにより、経路計画中の探索量を減らす。

さらに、前記ステップ５では、前記記録された親ノードの位置情報に基づいて、前記ナビゲーション終点から、ステップ４で述べた候補ルートのすべての中間ノード及びその対応する１つの端点を除いて、子ノード及びその親ノードを、前記ナビゲーション始点に接続されるまで順次接続することは、具体的には、前記記録された親ノードの位置情報に基づいて、前記ナビゲーション終点から、前記ナビゲーション終点及びその親ノードを接続し、その後、ステップ５２に進むステップ５１と、現在決定された親ノードを子ノードとし、さらに、前記記録された親ノードの位置情報に基づいて、この子ノードの親ノードを接続するステップ５２と、ステップ５２を、ステップ４で述べた対応する候補ルートの一方の端点に接続されるまで繰り返し、次いで、ステップ４で述べた対応する候補ルートの他方の端点から、ステップ４で述べた対応する候補ルートの他方の端点及びその親ノードを接続し、その後、ステップ５２に戻るステップ５３と、ステップ５２及びステップ５３を、前記ナビゲーション始点に接続されるまで繰り返すことで、逆方向接続により、ナビゲーション始点からナビゲーション終点までのナビゲーション経路を得るステップ５４と、を含む。

従来技術と比べて、上記ナビゲーション経路を計画した後、本技術案によれば、ロボットが狭い通路を通過する過程において、ヒューリスティック探索アルゴリズム又はインクリメンタルなヒューリスティック探索アルゴリズムを用いて経路を探索する際に、狭い通路が検出されると、狭い通路の近くで、ヒューリスティック探索アルゴリズム又はインクリメンタルなヒューリスティック探索アルゴリズムで探索されたナビゲーション経路への進入から、前記候補ルートへの進入に切り替わり、一方、狭い通路から出ていることが検出されると、前記候補ルートから、ヒューリスティック探索アルゴリズム又はインクリメンタルなヒューリスティック探索アルゴリズムにより探索されたナビゲーション経路への進入に切り替わり、これにより、ロボットが狭い通路を効果的に通過できるようになり、狭い通路を乗り越えるナビゲーションの成功率を高める。

さらに、前記ステップ５では、前記経路コストの合計値は、トラバース済み経路コストと予測経路コストとを加算又は重み付けして得られ、ここで、トラバース済み経路コストは、前記トラバース対象ノードキャッシュ空間内の１つの指定したノードから前記ナビゲーション始点までのコストであり、予測経路コストは、前記トラバース対象ノードキャッシュ空間内の同一の指定したノードから前記ナビゲーション終点までのコストであり、前記経路コストの合計値が小さいほど、指定したノードの、前記トラバース対象ノードキャッシュ空間内のトラバース優先度を高く設定し、ここで、２つの隣接ノード間の移動コストは、マンハッタン距離、対角距離、又はユークリッド距離で表される。ナビゲーションコストが最も低い最短経路を探すのに寄与する。

さらに、ナビゲーション始点から、トラバースされた各格子の中心点を、前記指定したノードに接続されるまで順次接続し、さらに、格子の辺の長さから、現在接続してなる線の長さを求め、その長さを前記トラバース済み経路コストとし、前記指定したノードを親ノードに設定し、この親ノードに対応する各子ノードからナビゲーション終点までのすべての線の結び方を取得し、さらに格子の辺の長さから、各子ノードに対応する各結び方の線の長さを求め、各子ノードに対応する最短の長さを選択してその対応する予測経路コストとし、ここで、格子点は、格子の中心点で表され、格子の位置特徴を表すために用いられる。経路コストの計算方法を簡素化する。ここで、本技術案で計算されたコスト値は、ロボットの運動軌跡のコストの尺度であり、始点から指定したノードまで走行し、さらに終点まで走行するためのコストを表し、経路長を含む。

さらに、前記経路融合計画方法では、前記ステップ４にて繰り返し探索不可能に設定されたノードを格納するためのトラバース済みノードキャッシュ空間をさらに作成し、ここで、前記トラバース済みノードキャッシュ空間内に存在するノードは、前記トラバース対象ノードキャッシュ空間に追加することが禁止される。計画過程において、探索されたノード、または探索されたノードと見なされるノードを識別することを実現する。

さらに、前記ステップ４は、現在の親ノードの近傍における、前記対応する候補ルート上に位置しないフリー格子点、又は、現在の親ノードの近傍におけるすべてのフリー格子点を、トラバース対象ノードキャッシュ空間に追加していれば、この現在の親ノードを前記トラバース対象ノードキャッシュ空間から削除してから、この現在の親ノードを前記トラバース済みノードキャッシュ空間に追加することをさらに含む。同一の探索中心が繰り返し拡張されることを防止する。

さらに、前記経路融合計画方法を実行する前に、前記候補ルート座標集合内の対応する候補ルートの探索方法は、具体的には、ロボットを制御して予め設定された経路に沿って移動する過程で、事前探索エリアが第１の予め設定された円領域通行条件を満たすと判断すると、ステップＳ２に進むステップＳ１と、ロボットの現在位置を第１の経路ノードとして記録するとともに、新たな予測通行座標集合を作成し、第１の経路ノードを前記予測通行座標集合に格納し、その後、ステップＳ３に進むステップＳ２と、ロボットを制御して前記予め設定された経路に沿って移動し続け、最新に記録された経路ノードまでの直線距離がロボットの機体直径以上となる位置にロボットが移動したと判断すると、ステップＳ４に進むステップＳ３と、ロボットの現在位置を第２の経路ノードとして記録するとともに、第２の経路ノードが第２の予め設定された円領域通行条件を満たすか否かを判断し、ＹＥＳであればステップＳ５に進み、ＮＯであればステップＳ６に進むステップＳ４と、ロボットが現在前記狭い通路にあると判定し、第２の経路ノードをステップＳ２で述べた予測通行座標集合に追加し、そしてステップＳ３に戻るステップＳ５と、ロボットが現在前記狭い経路に存在しないと判定し、前記予測通行座標集合の内部に存在する経路ノードの数に応じて、前記予測通行座標集合を同一の候補ルート座標集合の内部に保存し、前記予測通行座標集合内に格納された経路ノードを、追加された順序に従って接続して、対応する候補ルートを形成し、その後、ステップＳ１に戻るステップＳ６と、を含み、ここで、同一の候補ルート座標集合の内部では、いずれかの前記予測通行座標集合の内部の先頭要素及びその末尾要素が一意であり、かつ同一の前記予測通行座標集合の内部の先頭要素及びその末尾要素は同じではなく、ここで、前記狭い通路は、２つ以上の障害物のギャップ通路であり、ギャップ通路は、２つの障害物間の最も狭い部分に対応する隙間であり、この隙間の幅は、ロボットの機体直径以上であり、ここで、前記予め設定された経路は、ロボットが予め計画した経路であり、経路ノードは、格子点で表されることができる。

従来技術と比べて、本技術案は、ロボットが狭い通路へ進入した状態で経路探索を実施する場合に適しており、まず、第１の予め設定された円領域通行条件を狭い通路の事前判定条件として設定し、前記予測通行座標集合に対応する候補ルートの経路ノードのソースを提供し、さらに、ロボットを元の経路に沿って引き続き移動させ、その後、第２の予め設定された円領域通行条件を狭い通路の精判定条件として設定し、前記予測通行座標集合に対応する候補ルートの格子点のソースを提供し続ける。これにより、ロボットは、一定の距離だけ移動して初めて、狭い通路を判定する資格を持つようになり、前記予測通行座標集合に収集された、格子点を接続するための候補ルートはより完全であり、狭い通路における地図の格子の誤差環境により適したものになり、対応する格子のマーキング情報にリアルタイムで注目することなく、実際に通行可能なルートをロボットに提供することができる。また、これに基づいて、対応する予め設定された円領域通行条件が満たされない場合、前記予測通行座標集合ための探索を停止し、単一の前記予測通行座標集合内の経路ノードが接続して独立した候補ルートを形成できると決定する。前述の関連ステップを反復実行することにより、ロボットは、通常の作業移動状態で、地図ドリフト誤差に起因して格子を通行不可能な問題を解消できるルート点の集合を探索する。障害物の空間レイアウトが複雑なシーンにおいてロボットが効果的な走行経路を見つける成功率を高める。

さらに、ロボットが現在、前記狭い通路にある状態から前記狭い通路にない状態に変化したことを検出すると、前記ステップ１～前記ステップ５を実行して、前記経路融合計画方法の実行を開始する。前記ステップ１～前記ステップ５の反復処理を実現することにより、ロボットが前記狭い通路から出るとき、探索された前記狭い通路を通過するのに適した候補ルートの座標点を、現在位置でのヒューリスティック探索アルゴリズムの使用に融合して、ナビゲーション経路を計画することができ、近傍探索のみに頼ってナビゲーション経路計画を行うのではないため、狭い通路で探索されたフリー格子点が障害物格子点としてマークされやすく、計画された経路では狭い通路を通過できないという問題を解決する。

さらに、前記予測通行座標集合の内部に存在する経路ノードの数に応じて、前記予測通行座標集合を同一の候補ルート座標集合に保存する方法は、具体的には、前記予測通行座標集合の内部に存在する経路ノードの数が２未満である場合、前記予測通行座標集合を削除し、そしてステップＳ１に戻って新たな予測通行座標集合を作成することと、前記予測通行座標集合の内部に存在する経路ノードの数が２以上である場合、前記予測通行座標集合を前記候補ルート座標集合内に保存し、新たな候補ルートを対応して保存し、そしてステップＳ１に戻って新たな予測通行座標集合を作成することと、を含み、ここで、前記第１の経路ノード及び前記第２の経路ノードは、記録された順序に従って、前記予測通行座標集合内に追加されることで、前記予測通行座標集合内に格納された経路ノードは、一定の順番で接続して対応する候補ルートを形成し、ここで、各予測通行座標集合内の先頭要素に対応する経路ノードは、対応する候補ルートの始点であり、各予測通行座標集合内の末尾要素に対応する経路ノードは、対応する候補ルートの終点である。これにより、各予測通行座標集合内の経路ノードが合わせて接続して１つの候補ルートとなり、ロボットが対応するエリア内を支障なく通行できると予測される候補ルートを形成する。当該技術案では、１つの予測通行座標集合で最終的に取得された経路ノードの数が、線で結ぶことが困難な程度に少ない場合、無効な経路ノードを削除し、減らすことができる。１つの予測通行座標集合で最終的に取得された経路ノードは全体として、１つの候補ルートを保存するための前記候補ルート座標集合に保存されるため、候補ルートの保存及び取り出し構造が合理的かつ秩序立ったものになる。

さらに、前記第１の予め設定された円領域通行条件は、前記事前探索エリア内の第１の通行不可エリアが占める面積の割合は、第１の通行評価値よりも大きいことを含み、ここで、第１の通行不可エリアは、前記第１の円形エリアに対応する格子エリア内で、未知格子点と障害物格子点とからなる格子エリアであり、第１の通行評価値は、構築された格子地図に存在するフリー格子のマーキング誤差を克服するために設定された事前判定閾値であり、ここで、前記事前探索エリアは、ロボットの現在位置を円の中心とし、ロボットの機体直径を半径とする第１の円形エリアである。ロボットが狭い通路へ進入し始めたか否かを予備判断するための粗い判断条件であり、後続では、継続移動中にロボットがさらなる判断を行うことに依存する。しかし、単一の格子のマーキング情報がロボットの通行を許可するか否かを考慮する必要がなく、単一の格子のマーキング誤差の影響を低減する。

さらに、前記第２の予め設定された円領域通行条件は、ロボットの現在位置を円の中心とし、ロボットの機体直径を半径とする第２の円形エリア内で、第２の通行不可エリアが占める面積の割合は、第２の通行評価値よりも大きいことを含み、ここで、第２の通行不可エリアは、第２の円形エリアに対応する格子エリア内で、未知格子点と障害物格子点とからなる格子エリアであり、第２の通行評価値は、構築された格子地図に存在するフリー格子のマーキング誤差を克服するために設定された判断閾値であり、かつ第１の通行評価値よりも大きい。この技術案によれば、前記第１の円形エリア、又は前回探索された第２の円形エリアを基礎とし、ロボットが、最新に記録された経路ノードからロボットの機体直径分だけ離れた場所に移動した後、さらに第２の通行不可エリアが占める面積の割合の判断を行い、単一の格子点のマーカ情報の影響を考慮する必要がなく、狭い通路の判断精度を高める。

さらに、前記第２の予め設定された円領域通行条件は、最新に記録された第１の経路ノードを円の中心とし、予め設定された倍数の機体直径を半径とする第２の円形エリア内で、経路探索アルゴリズムを用いて、最新に記録された第２の経路ノードから最新に記録された第１の経路ノードに至る経路を探索したことを含み、ここで、予め設定された倍数の機体直径は、第２の円形エリアが他のマークされた格子エリアと交差しないように設定されている。従来技術と比べて、当該技術案によれば、第２の円形エリアという探索エリアのサイズが合理的に設定されることで、他の既知の地図エリアと交差して、関連エリア内の計画された経路が納められてしまった結果、ロボットが他のエリアにガイドされて、現在の狭い通路を通るようにガイドされなくなることを回避する。一方、さらに、現在探索されている第２の円形エリア内で始点から終点までの完全たるナビゲーション経路を計画できるか否かを判断することによって、この第２の円形エリアの通行可能エリアが障害物の影響を受けなくなったこと、又は障害物格子のマーキング位置の影響を受けなくなったことを証明することができ、これにより、前記狭い通路の判断精度を高める。

チップであって、前述のいずれかの技術案による経路融合計画方法を実行するためのプログラムコードが格納されている。障害物が多く分布する狭い通路内で、ヒューリスティック探索アルゴリズムと探索条件に合致する候補ルートとを融合してナビゲーション経路を計画することにより、地図格子における通路が狭くてマーキング誤差が生じやすいことを解消し、ロボットが計画された全体的なナビゲーション経路に沿ってこの狭い通路に効果的に出入りできるようになり、障害物空間のレイアウトが複雑なシーンで経路ナビゲーションが失敗する確率を減らす。

ロボットであって、前記経路融合計画方法を実行するようにロボットを制御するための前記チップを内蔵している。ロボットが、前記経路融合計画方法で計画したナビゲーション経路に従って狭い通路を通過する過程において、ヒューリスティック探索アルゴリズム又はインクリメンタルなヒューリスティック探索アルゴリズムを用いて経路を探索する際に、狭い通路が検出されると、狭い通路の近くで、ヒューリスティック探索アルゴリズム又はインクリメンタルなヒューリスティック探索アルゴリズムで探索されたナビゲーション経路への進入から、前記候補ルートへの進入に切り替わり、一方、狭い通路から出ていることが検出されると、前記候補ルートから、ヒューリスティック探索アルゴリズム又はインクリメンタルなヒューリスティック探索アルゴリズムで探索されたナビゲーション経路への進入に切り替わり、これにより、ロボットが、狭い通路を効果的に通過できるようになり、狭い通路を乗り越えるナビゲーションの成功率を高める。

本発明の一実施例による通行エリアの経路融合計画方法のフローチャートである。 本発明の別の実施例による候補ルート座標集合内の対応する候補ルートの探索方法のフローチャートである。

以下、本発明の具体的な実施形態について、図面を参照しつつさらに説明する。

なお、当業者であれば、格子地図には、ロボットの現在位置周辺の環境情報がマークされており、ロボットが構築した地図エリア内の格子は、フリー（free）、占有（occupied）及び未知（unknown）の３つの状態でマークされることと、これらの格子は、本実施例では格子の中心点である格子点で表されることと、フリー状態の格子点とは、障害物によって占有されていない格子を指し、ロボットが到達可能な格子位置点であって、フリー格子点であり、占有されないエリアを構成することができることと、占有状態の格子点とは、障害物によって占有された格子を指し、障害物格子点であり、占有エリアを構成することができることと、未知格子点とは、ロボットの地図構築中に具体的な状況がはっきりしない格子エリアを指し、その位置点が障害物によって遮られることが多く、未知エリアを構成することができることと、が理解される。

なお、探索アルゴリズムを使って問題を解決する際に、自分の位置の状態特徴とそれ以外の位置の状態との関係を表すデータ構造を構築する必要があり、このデータ構造はノードと呼ばれる。問題が異なれば、異なるデータ構造で記述する必要がある。探索問題で与えられた条件に応じて、１つのノードから出発して、１つ又は複数の新しいノードを生成することができ、この過程は、通常、拡張と呼ばれる。ノード間の関係は一般に、隣接する親ノードと子ノードとして表現することができる。探索アルゴリズムの探索過程は、実際には、初期条件と拡張ルールに応じて経路を構築して目標状態に合致するノードを探す過程である。

なお、従来技術のインテリジェント掃除ロボットは、家庭環境においてスツールの４本の足やリビングテーブルの入り口など、様々な家具コンポーネント間の制限によって形成される狭い通路、部屋のドアが開いて形成される狭い通路に移動することが多い。ただし付言しておけば、前記狭い通路は、各障害物のギャップ通路であり、各障害物のギャップ通路は、２つの障害物間の最も狭い部分に対応する隙間であり、この隙間の幅は、ロボットの機体直径よりも大きく、ロボットが通行可能である。ロボットが滑ったり、測位用センサに累積誤差が存在したり、視覚地図の最適化にも誤差が発生したりすることがあるため、ロボットは、リアルタイムスキャンによって構築された格子地図上で前記狭い通路を、障害物が占めているエリアとしてマークしやすく、即ち、前記狭い通路がもともとマッピングされていたフリー格子点を、障害物が占めているエリアとして誤ってマークすることにより、ロボットの構築した格子地図にマッピングされた狭い通路の入り口は塞がれている（しかし、実際の運動シーンでは本当に塞がれていない）ため、ロボットは、成熟した経路探索アルゴリズム、例えば、Ａ＊アルゴリズム等のヒューリスティック探索アルゴリズム、又はＤ＊アルゴリズム等のインクリメンタルなヒューリスティック探索アルゴリズムを使用して、前記狭い通路を貫通するナビゲーション経路を完成することができない。

このため、本発明は、通行エリアの経路融合計画方法を開示する。本発明では、まず、ロボットを制御して、元の通常のナビゲーション作業状態において、障害物が多く分布する狭い通路を予めトラバースして、融合可能な候補ルートを探索する必要があり、その後、前記経路融合計画方法を用いて、ヒューリスティック探索アルゴリズム（主にこのタイプの探索アルゴリズムの経路ノードの探索アイディア）と探索条件に合致する候補ルートとを融合して全体的なナビゲーション経路を計画し、地図の格子のマーキング誤差を克服する。これにより、格子地図における通路が狭くてマーキング誤差が生じやすい通行可能エリア内でのロボットのナビゲーション経路計画問題を解決する。

１つの実施例として、本発明の実施例は、ロボットが前記狭い通路から完全に離れた後、又は現在の狭い通路から離れて新たな狭い通路に入る直前に実施できる通行エリアの経路融合計画方法を開示する。図１に示すように、この経路融合計画方法は、以下のステップＳ１０１～ステップＳ１１１を含む。ステップＳ１０１では、格子地図においてナビゲーション始点及びナビゲーション終点を設定し、トラバース対象ノードキャッシュ空間を作成し、その後、ステップＳ１０２に進む。ここで、格子地図は、ロボットが構築した始点及び終点を含む局所地図であり、この格子地図には、様々なレイアウトの障害物情報も含まれており、また、ロボットがナビゲーション始点及びナビゲーション終点に応じて経路軌跡を計画するのに十分な空間エリアが含まれている。ナビゲーション始点及びナビゲーション終点は、格子地図の格子座標、格子の中心点座標、又はその他のタイプのナビゲーションデータで表すことができ、限定されないものであるが、いずれも格子地図用に変換して経路計画に関与することができる。一方、ステップＳ１０１では、トラバース対象ノードキャッシュ空間を作成して空に初期化し、その内部要素の優先度は、経路コストに関係しており、要素の優先度は、トラバース優先度、計画優先度とすることができる。

ステップＳ１０２では、ナビゲーション始点を現在の親ノードに設定し、具体的には、ナビゲーション始点が存在する格子又は格子点を現在の親ノードに設定し、トラバース対象ノードキャッシュ空間に追加し、その後、ステップＳ１０３に進む。１つの任意選択の実施形態では、ナビゲーション始点をトラバース対象ノードキャッシュ空間に追加する前に、最初のトラバース対象ノードキャッシュ空間は空であってもよく、この場合、ナビゲーション始点は、トラバース対象ノードキャッシュ空間に追加される最初のノードとなる。好ましくは、ロボット座標系の原点（ローカル座標系の原点）に位置する親ノードは、４つの象限エリア内で拡張探索された子ノードのいずれとも隣接する位置関係にある。

ここで、前記トラバース対象ノードキャッシュ空間は、本実施例の経路計画に用いる候補経路ノードの集合とみなされ、ロボットが通過する可能性のある経路ノードをこれに追加して、後続のスクリーニング処理に供する。前記トラバース対象ノードキャッシュ空間は、その内部の各経路ノードの位置と、ロボットが対応する経路ノードに移動した状態とを記録することができる。この例示的な実施形態では、格子地図における経路ノードについて、状態パラメータによって、ロボットが移動し経路ノードに到達したときの状態を表す。経路ノードの状態パラメータは、少なくとも、時刻、すなわち、ロボットがこの経路ノードに到達した時刻を含む。また、状態パラメータは、ロボットが経路ノードに到達したときの方向、ステアリング角（すなわち、ロボット自体が直進状態かステアリング状態か、及びステアリングの角度）を含む姿勢と、速度、すなわち、ロボットが経路ノードに到達したときの運動速度と、をさらに含んでもよい。経路計画を開始する際に、ナビゲーション始点の状態パラメータを先に決定することができ、ここで、現在時刻又はゼロ時刻をナビゲーション始点の時刻とし、現在のロボットの運動状態を始点に対応する姿勢及び速度とすることができ、例えば、ロボットが最初に静止状態であれば、ナビゲーション始点に対応するステアリング角及び速度が共に０となる。１つの例示的な実施例では、前記トラバース対象ノードキャッシュ空間は、対応する候補経路ノードが先入れ先出しの格納順序で追加される優先キューのデータ構造であり、ロボットの記憶空間内に保存される。

ステップＳ１０３では、現在の親ノードを中心として、格子地図において近傍探索を行う。ここで、現在の親ノードに隣接する８つの格子点は、それぞれ子ノードとする。その後、ステップＳ１０４に進む。ここで、親ノード（ステップＳ１０３で述べた現在の親ノード）から近傍における子ノードを探索する過程を「拡張」と呼ぶことができる。１回の拡張過程では、現在の親ノードの近傍において時計回り方向に格子点を１つずつ探索（近傍探索）したり、現在の親ノードの近傍において反時計回り方向に格子点を１つずつ探索（近傍探索）したりして、関連条件に合致する子ノードを探索することができる。

ステップＳ１０４では、ステップＳ１０３にて近傍で探索された子ノードが、予め探索された候補ルート座標集合内の対応する候補ルートの一方の端点であるか否かを判断し、ＹＥＳであればステップＳ１０５に進み、ＮＯであればステップＳ１０６に進む。すなわち、ステップＳ１０３にて近傍で探索された子ノードが、予め探索された候補ルート座標集合内の代表的な候補ルートの始点又は終点であるか否かを判断することにより、リアルタイムに検出された格子点のマーキング情報の誤差が、従来の成熟した経路探索アルゴリズムで計画されたナビゲーション経路に影響を与えること、特に、フリー格子点を障害物格子点として誤ってマークすることに起因して、従来の成熟した経路アルゴリズムで計画されたナビゲーション経路を介してはこの誤ってマークされた障害物格子点を通過できず、ロボットが前記狭い通路を通過するのに不利となることを考慮することなく、計画されたナビゲーション経路を、探索された候補ルートに接続して、実際のナビゲーション走行に使用することができる。ここで、前記候補ルートの一方の端点が始点である場合、前記候補ルートの他方の端点が終点となり、前記候補ルートの一方の端点が終点である場合、前記候補ルートの他方の端点が始点となる。

ステップＳ１０５では、ステップＳ１０４にて探索された子ノードが属する候補ルートの他方の端点を、前記トラバース対象ノードキャッシュ空間に追加し、すなわち、属する候補ルートの他方の端点をも、ステップＳ１０１にて作成されたトラバース対象ノードキャッシュ空間内に保存するとともに、ステップＳ１０３にて探索された、ステップＳ１０４の対応する候補ルート上にないフリー格子点を、ステップＳ１０１にて作成された同一のトラバース対象ノードキャッシュ空間内に追加して、後続で経路計画に用いる候補ノードの集合とし、同時に、前記トラバース対象ノードキャッシュ空間に追加された経路ノードの親ノードの位置情報を記録し、経路ノードの位置情報を、対応する親ノードの位置情報とともに、前記トラバース対象ノードキャッシュ空間内に保存する。好ましくは、子ノード及びその親ノードの時刻、姿勢、速度などの他の情報をも前記トラバース対象ノードキャッシュ空間内に記録する。現在実行しているステップＳ１０５にて前記トラバース対象ノードキャッシュ空間に追加された経路ノードのうち、属する候補ルートの他方の端点を除いた残りの経路ノードの親ノードは、いずれもステップＳ１０３で述べた現在の親ノードである。好ましくは、属する候補ルートの他方の端点の親ノードは、属する候補ルート上の隣接するノード又は属する候補ルート以外の隣接するノードである。これにより、この端点を探索した際に、直接その親ノードを介してこの候補ルートの位置分布情報を遡ることができ、経路計画の速度を速める。

ここで、属する候補ルートのうち両端点及びこれら両端点間の中間ノードではなく、一方の端点を前記トラバース対象ノードキャッシュ空間内に追加する理由としては、属する候補ルートは、予め探索しておいたものであり、前記経路融合計画方法を実行する前に、関連する座標点の集合が探索され、順次接続して経路セグメントとなることが知られており、このとき、この候補ルートのうち一方の端点のみでも、対応するルートの識別として、又はこのルート上の対応する始点のインデックスとして機能できるため、このステップでは、近傍探索された１つの端点が属する候補ルート上の別の端点（探索されていない経路ノードと等価）のみを、前記トラバース対象ノードキャッシュ空間内に追加し、後続では、前記トラバース対象ノードキャッシュ空間に追加された端点を中心として、この候補ルートに関連する通行意義のある新しい経路ノードを探索し続けることができることが挙げられる。ステップＳ１０４の対応する候補ルート上にない、前記現在の親ノードの近傍に属するフリー格子点については、最短経路を有するフリー格子点を提供するために使用される。

このため、ステップＳ１０７の実行中に、ステップＳ１０４にて探索された子ノードが属する候補ルートの両端点間のすべての中間ノードと、ステップＳ１０４にて探索された端点とを、すべて、繰り返し探索不可能なノードに設定する。本実施例ではまた、この目的でトラバース済みノードキャッシュ空間を特別に作成して、繰り返し探索不可能に設定されたノードを格納するために用いる。なお、前記トラバース済みノードキャッシュ空間内に存在するノードは、前記トラバース対象ノードキャッシュ空間に追加することが禁止され、これにより、計画過程において、トラバースされたノード、または探索されたノードと見なされるノードを識別することを実現し、繰り返し探索処理されることを回避する。ここで、ステップＳ１０５とステップＳ１０７とは、同時に実行されてもよい。ステップＳ１０７の実行が終了すると、ステップＳ１０８の実行に進む。

好ましくは、前記現在の親ノードの近傍における、前記対応する候補ルート上に位置しないフリー格子点、又は、現在の親ノードの近傍におけるすべてのフリー格子点を、トラバース対象ノードキャッシュ空間に追加していれば、この現在の親ノードを前記トラバース対象ノードキャッシュ空間から削除してから、この現在の親ノードを前記トラバース済みノードキャッシュ空間に追加し、これにより、同一の探索中心が繰り返し拡張される現象を防止する。現在の親ノードはステップＳ１０３で一度拡張されたからである。前記トラバース済みノードキャッシュ空間は、リストというデータ記憶構造の形でロボットの内部に存在することができる。

前記候補ルート座標集合内の対応する候補ルートは、格子地図上にマークされた障害物格子点が地図の誤差によってマークされているか否かによらず、この障害物格子点を通過することを許可されることに注意すべきである。

本実施例では、格子のマーキング誤差は、センサ誤差又は地図のドリフト誤差又は障害物の移動などの誤差が格子地図に反映されることにより形成されたものである。

ステップＳ１０６では、ステップＳ１０３にて近傍で探索された子ノードが前記候補ルート座標集合内のすべての候補ルートの端点でない場合、ステップＳ１０３にて近傍で探索されたフリー格子点をすべて前記トラバース対象ノードキャッシュ空間内に追加し、ステップＳ１０３にて近傍で探索されたフリー格子点の親ノードの位置情報を記録し、その後、ステップＳ１０８に進む。これにより、近傍探索中の子ノードの探索範囲を拡大するだけでなく、前記候補ルート座標集合内のルートの始点及び終点の探索をより広い範囲で拡張することにも寄与し、計画された経路をより高い実用性及び代表性を有するものにできる。

ステップＳ１０５の１つの実施例として、ステップＳ１０３にて近傍で探索された子ノードが前記候補ルート座標集合内の対応する候補ルートの始点である場合、対応する候補ルートの終点を前記トラバース対象ノードキャッシュ空間に追加し、対応する候補ルートの終点の親ノードの位置情報を、前記候補ルート座標集合内で各候補ルートがその終点をルート識別情報として使用するように記録し、同時に、対応する候補ルートの両端点間のすべての中間ノードと同一の候補ルートの始点とを、すべて、繰り返し探索不可能なノードに設定する。前記対応する候補ルートの始点からその終点に向かう経路延在方向に沿って、前記対応する候補ルート上の各ノードの親ノードは、そのノードに隣接するノード位置に位置している。具体的には、前記候補ルート座標集合内で、前記対応する候補ルートの始点から始まり、この候補ルートの始点からその終点に向かう第１の経路延在方向に、この候補ルートの各ノードの親ノードの位置情報を、この候補ルートの終点の親ノードの位置情報が記録されるまで順次記録し、ここで、この候補ルートの各ノードの親ノードは、属する候補ルートの第１の経路延在方向の隣接するノードである。したがって、この候補ルートの終点の親ノードは、属する候補ルートの外側に位置し、かつこの候補ルートの終点に隣接しており、この候補ルートの終点の子ノードは、属する候補ルート上に位置し、かつこの候補ルートの終点に隣接している。この候補ルートの始点の親ノードは、属する候補ルート上に位置し、かつこの候補ルートの始点に隣接しており、この候補ルートの始点の子ノードは、属する候補ルート上に位置し、かつこの候補ルートの始点に隣接している。

ステップＳ１０５の他の実施例として、ステップＳ１０３にて近傍で探索された子ノードが前記候補ルート座標集合内の対応する候補ルートの終点である場合、対応する候補ルートの始点を前記トラバース対象ノードキャッシュ空間に追加し、対応する候補ルートの始点の親ノードの位置情報を、前記候補ルート座標集合内で各候補ルートがその始点をルート識別情報として使用するように記録し、同時に、対応する候補ルートの両端点間のすべての中間ノードと同一の候補ルートの終点とを、すべて、繰り返し探索不可能なノードに設定する。ここで、前記対応する候補ルートの終点からその始点に向かう経路延在方向に沿って、前記対応する候補ルート上の各ノードの親ノードは、そのノードに隣接するノード位置に位置している。具体的には、前記候補ルート座標集合内で、この候補ルートの終点から始まり、前記対応する候補ルートの終点からその始点に向かう第２の経路延在方向に沿って、この候補ルートの各ノードの親ノードの位置情報を、この候補ルートの始点の親ノードの位置情報が記録されるまで順次記録し、ここで、この候補ルートの各ノードの親ノードは、属する候補ルートの第２の経路延在方向の隣接するノードである。したがって、この候補ルートの始点の親ノードは、属する候補ルートの外側に位置し、かつこの候補ルートの始点に隣接しており、この候補ルートの始点の子ノードは、属する候補ルート上に位置し、かつこの候補ルートの始点に隣接している。

上述したステップＳ１０５に基づく２つの実施例では、前記候補ルートの始点又はその終点を、１つの候補ルートを一意に特定できるように構成し、対応する候補ルートの識別子として用いて、対応する親ノード情報を記録する一方、同一の候補ルートの残りのノードを、繰り返し探索されたノードとして構成することにより、経路計画中の探索量を減らし、マッチングする候補ルートの遡及を加速化する。

ステップＳ１０８では、前述のトラバース対象ノードキャッシュ空間内から、経路コストの合計値が最小となるノードを次の親ノードとして選択し、そしてステップＳ１０９に進む。本実施例では、前記トラバース対象ノードキャッシュ空間内のすべてのノードの経路コストを比較することで、より優れたノードを次の親ノードとして選択する。ここで、経路コストの合計値が小さいほど、ノードの優先度が高くなり、経路コストの合計値が最も小さい（優先度が最も高い）ノードを次の親ノードとして、新たな近傍探索を開始し、前記次の親ノードを探索中心として探索を継続し、新たな近傍格子点（前記トラバース済みノードキャッシュ空間内に既に存在するノードとは区別される）を追加する。

本実施例では、前記経路コストの合計値は、トラバース済み経路コストと予測経路コストとを加算又は重み付けして得られる。ここで、トラバース済み経路コストは、前記トラバース対象ノードキャッシュ空間内の１つの指定したノードから前記ナビゲーション始点までのコストであり、予測経路コストは、前記トラバース対象ノードキャッシュ空間内の同一の指定したノードから前記ナビゲーション終点までのコストであり、これらの経路コストの定義は、Ａ＊アルゴリズムのヒューリスティック関数からのものである。前記経路コストの合計値が小さいほど、指定したノードの、前記トラバース対象ノードキャッシュ空間内のトラバース優先度を高く設定する。本実施例では、Ａ＊ヒューリスティック関数に基づいて、２つの隣接ノード間の移動コストを計算する際は、マンハッタン距離、対角距離、又はユークリッド距離を使用して表す。ここで、ロボットが現在位置している地図エリア内で上下左右の４方向への移動しか許可されていなければ、マンハッタン距離を使用することができる。ロボットが現在位置している地図エリア内で８方向への移動が許可されていれば、対角距離を使用することができる。ロボットが現在位置している地図エリア内で任意の方向への移動が許可されていれば、ユークリッド距離を使用することができる。ナビゲーションコストが最も低い最短経路を探索するのに寄与する。

経路コストの計算時に、ナビゲーション始点から、トラバースされた各格子の中心点を、前記指定したノードに接続されるまで順次接続し、さらに、格子の辺の長さから、現在接続してなる線の長さを求め、その長さを前記トラバース済み経路コストとする。経路コストの計算時に、前記指定したノードを親ノードに設定し、この親ノードに対応する各子ノードからナビゲーション終点までのすべての線の結び方を取得し、さらに格子の辺の長さから、各子ノードに対応する各結び方の線の長さを求め、各子ノードに対応する最短の長さを選択してその対応する予測経路コストとする。ここで、前述の格子点は、格子の中心点で表され、格子の位置特徴を表すために用いられる。本実施例で計算されるコスト値は、ロボットの運動軌跡のコストの尺度であり、始点から指定したノードまで走行し、さらに終点まで走行するためのコストを表し、経路長を含む。

なお、各ノードについて、そのコスト値を計算することができ、コスト値は、ロボットの運動軌跡のコストの尺度であり、始点からこのノードまで運動し、さらに終点まで運動するためのコストを表し、経路長、所要時間、衝突が発生したか否か、速度や方向を頻繁に切り替えるか否かなどの要素を含む。

ステップＳ１０９では、前記次の親ノードが前記ナビゲーション終点であるか否かを判断し、ＹＥＳであればステップＳ１１０に進み、ＮＯであればステップＳ１１１に進む。

ステップＳ１１１では、前記次の親ノードを、ステップＳ１０３で述べた現在の親ノードに更新し、そしてステップＳ１０３に戻る。

格子地図の場合、前述のステップに対応する実施形態は、実質的に、ナビゲーション始点から始まり、絶えず拡張してナビゲーション終点に到達する過程であり、拡張探索された経路ノードと前記候補ルートとが接続して、最終的に計画されたナビゲーション経路を形成する。したがって、ステップＳ１１１は、繰り返し実行されるトリガステップであり、ステップＳ１０３に戻るたびに１回の拡張となり、これにより、ロボットが現在の親ノードから出発し、予め設定された間隔時間だけ走行して到達可能なノードは、子ノードとなる。簡単に言えば、各回の拡張は、ロボットが「１歩進む」ことを意味し、地図内で格子を１回またぐことに対応する。なお、予め設定された間隔時間は、各回の拡張の周期的な時間であり、５秒、１０秒等の短い時間単位を表し、予め設定された間隔時間が短いほど、計画されたナビゲーション経路が精密になるため、予め設定された間隔時間は、実際のニーズに応じて決定されてもよい。

本実施例では、後続で次の親ノードを探索中心として拡張する過程において、近傍で現在の親ノードを前記次の親ノードの子ノードとして探索していれば、現在の親ノードを前記トラバース対象ノードキャッシュ空間に追加する必要はない。

ステップＳ１１０では、前記トラバース対象ノードキャッシュ空間内にナビゲーション終点が存在し、すなわち、前述の近傍探索過程でナビゲーション終点を探索した（又はナビゲーション終点まで拡張した）と決定されると、前述のステップで記録された親ノードの位置情報に基づいて、前記ナビゲーション終点から、ステップＳ１０４で述べた候補ルートのすべての中間ノード及びその対応する１つの端点を除いて、子ノード及びその親ノードを、前記ナビゲーション始点に接続されるまで順次接続し、前記ナビゲーション始点から前記ナビゲーション終点までの経路を計画する。

従来技術と比べて、前述の実施例では、ロボットを制御して、同じ位置にて、ナビゲーション始点から始まり、トラバース対象ノードキャッシュ空間内から経路コストの合計値が最小となるノードを親ノードとして順次選択して近傍探索を行い、近傍探索された、候補ルート上にないフリー格子点と、候補ルート上にある識別的価値を持つ１つの端点（始点又は終点）を、すべて、トラバース対象ノードキャッシュ空間に追加し、対応する親ノードを経路ノード遡及情報として記録する。同時に、候補ルート上の中間ノード（いずれもこれらの狭い通路に適したナビゲーション目標点であるに関わらず）による繰り返し探索への干渉を排除し、親ノードとしてナビゲーション終点となると、さらに子ノードと親ノードの位置関係に従って、ナビゲーション終点から、探索された候補ルートを接続し、ナビゲーション始点に逆方向に接続することにより、通行条件に合致する候補ルートをヒューリスティック探索アルゴリズムに融合してナビゲーション経路を計画し、地図格子における通路が狭くてマーキング誤差が生じやすいことを克服し、ロボットが計画された全体的なナビゲーション経路に沿ってこの狭い通路に効果的に出入りできるようになり、経路のナビゲーションが失敗する確率を減らす。

具体的には、前記次の親ノードが前記ナビゲーション終点であると判断された場合、前記記録された親ノードの位置情報に基づいて、前記ナビゲーション終点から、ステップ４で述べた候補ルートのすべての中間ノード及びその対応する１つの端点を除いて、子ノード及びその親ノードを、前記ナビゲーション始点に接続されるまで順次接続することは、具体的には、以下のステップ５１～ステップ５４を含む。ステップ５１では、前記ナビゲーション終点から、既に記録された親ノードの位置情報、又は前記トラバース対象ノードキャッシュ空間内に格納された親ノード及び子ノードの位置情報に基づいて、前記ナビゲーション終点及びその親ノードを接続し、その後、ステップ５２に進む。これにより、ノードごとに逆方向の接続を開始する。

ステップ５２では、現在決定された親ノードを子ノードとし、すなわち、ステップ５１で決定された親ノードを、位置情報を含めて子ノードに更新し、さらに、前記記録された親ノードの位置情報に基づいて、この子ノードの親ノードを接続し、その後、ステップ５３に進む。

ステップ５３では、ステップ５２を、ステップＳ１０５で確かに探索された対応する候補ルートの一方の端点に接続されるまで繰り返し、次いで、この候補ルートの他方の端点から、前記対応する候補ルートの他方の端点及びその親ノードを接続し、その後、ステップ５２に戻る。ここで、前記対応する候補ルートの他方の端点の親ノードは、ステップＳ１０５で確かに探索された端点から属する候補ルートの他方の端点に向かう経路延在方向において、前記対応する候補ルートの他方の端点に隣接し、かつこの候補ルートの外部に位置するノードである。

１つの実施例として、ステップ５３では、ステップＳ１０５で確かに探索された対応する候補ルートの始点に接続された場合、前述の実施例から分かるように、前記対応する候補ルートの始点は、ステップ５２で更新された子ノードの親ノードであり、すなわち、前記対応する候補ルートの始点は、前記対応する候補ルートの始点からその終点に向かう経路延在方向とは逆方向において隣接するノードの親ノードとなる。このことから、前記対応する候補ルートの始点からその終点に向かう経路延在方向に沿って、前記対応する候補ルート上の各ノードの親ノードは、そのノードに隣接するノード位置に位置していることが判明され、同時に、この候補ルートの始点及び中間ノードは、いずれも繰り返し探索不可能なノード、すなわちトラバースされた経路ノードであることが判明される。したがって、ステップ５３では、この候補ルートの終点から、この候補ルートの終点及びその親ノードを接続するようにしており、これは、ステップ５２に戻ってこの候補ルートの外部エリアに遡ることができるように、この候補ルートの終点の親ノードがこの候補ルートの外部にあるからである。

別の実施例として、ステップ５３では、ステップＳ１０５で確かに探索された対応する候補ルートの終点に接続された場合、前述の実施例から分かるように、前記対応する候補ルートの終点は、ステップ５２で更新された子ノードの親ノードであり、すなわち、前記対応する候補ルートの終点は、前記対応する候補ルートの始点からその終点に向かう経路延在方向において隣接するノードの親ノードとなる。このことから、前記対応する候補ルートの終点からその始点に向かう経路延在方向に沿って、前記対応する候補ルート上の各ノードの親ノードは、そのノードに隣接するノード位置に位置していることが判明され、同時に、この候補ルートの始点及び中間ノードは、いずれも繰り返し探索不可能なノード、すなわちトラバースされた経路ノードであることが判明される。したがって、ステップ５３では、この候補ルートの始点から、この候補ルートの始点及びその親ノードを接続するようにしており、これは、ステップ５２に戻ってこの候補ルートの外部エリアに遡ることができるように、この候補ルートの始点の親ノードがこの候補ルートの外部にあるからである。

ステップ５４では、前述のステップ５２及び前述のステップ５３を、前記ナビゲーション始点に接続されるまで繰り返すことで、前述記録された親ノードの位置情報に基づいて、前述のステップＳ１０１～ステップＳ１１１で探索された親ノードを１つずつ逆方向に接続して、ナビゲーション始点からナビゲーション終点までのナビゲーション経路を得ることを実現する。ステップ５１～ステップ５４を実行する過程において、この子ノード（候補ルートの端点を含む）の親ノードを順次接続し、さらにこの親ノードを子ノードとする過程は、「逆方向拡張」と呼ぶことができ、実質的には、ナビゲーション終点から始まり、絶えず拡張して候補ルートの始点又は終点まで接続し、ナビゲーション始点に到達する経路ノードの遡及過程であり、逆方向拡張の軌跡は、ナビゲーション始点、障害物格子点を乗り越え可能な候補ルート、ナビゲーション終点を結ぶナビゲーション経路を得るようなものである。

従来技術と比べて、前述のステップを通じて上記ナビゲーション経路を計画した後、本実施例によれば、ロボットが狭い通路を通過する過程において、ヒューリスティック探索アルゴリズム又はインクリメンタルなヒューリスティック探索アルゴリズムを用いて経路を探索する際に、狭い通路が検出されると、狭い通路の近くで、ヒューリスティック探索アルゴリズム又はインクリメンタルなヒューリスティック探索アルゴリズムで探索されたナビゲーション経路への進入から、前記候補ルートへの進入に切り替わり、一方、狭い通路から出ていることが検出されると、前記候補ルートから、ヒューリスティック探索アルゴリズム又はインクリメンタルなヒューリスティック探索アルゴリズムで探索されたナビゲーション経路への進入に切り替わることを実現し、これにより、ロボットが狭い通路を効果的に通過できるようになり、狭い通路を乗り越えるナビゲーションの成功率を高める。

好ましくは、各ノードの状態パラメータをも、前記ナビゲーション経路にマークすることができ、各ノードの状態パラメータは、各ノードの時刻、すなわち、計画に従ってロボットが各ノードに移動した時刻を含む。これにより、状態パラメータを前記ナビゲーション経路に追加することができ、例えば、各ノードの時刻をナビゲーション経路にマークすることができ、地図中に前記狭い通路が存在する場合、ロボットが前記狭い通路に長時間閉じ込められるか否かを推定することができ、複雑な障害物レイアウトシーンにおいて効果的な経路計画を実現し、実際のニーズに応える。

前記トラバース対象ノードキャッシュ空間に追加する新たな子ノードがなく、近傍で拡張し続けることができない場合、これは、地図中に乗り越えにくい障害物が存在する可能性があり、例えば障害物間に形成された通路が機体の幅よりも小さいなどの事情が発生したかもしれないことを示すことに注意すべきである。

また、ステップ５２～５３を特定の回数まで繰り返し実行しても、前記ナビゲーション始点に接続できない場合、処理過程に異常が発生した可能性があることを示し、この特定の回数は、ナビゲーション始点とナビゲーション終点との間の距離に関連し得る。

別の実施例として、本発明は、前記候補ルート座標集合内の対応する候補ルートの探索方法をさらに開示し、この探索方法は、前記経路融合計画方法を実行する前に実行される。図２に示すように、具体的には、以下のステップＳ２０１～ステップＳ２０７を含む。ステップＳ２０１では、ロボットを制御して予め設定された経路に沿って移動する過程で、事前探索エリアが第１の予め設定された円領域通行条件を満たすか否かをリアルタイムで判断し、ＹＥＳであればステップＳ２０２に進み、ＮＯであれば、ロボットを制御して予め設定された経路に沿って移動し続ける。ここで、前記予め設定された経路は、ロボットが予め計画した経路である。経路ノードは、格子点で表されることができる。予め設定された経路は、ロボットの通常の作業経路又はナビゲーション経路であり、ロボットが掃除ロボットである場合、予め設定された経路は、弓字型の移動経路、エッジ沿い走行経路、「回」の字型の経路等の計画された清掃経路であってもよい。前記予め設定された経路を移動するロボットは、通常の作業中には、前記狭い通路を乗り越えるのに適したルート座標点又はルートの探索を行ったり、狭い通路へ入る過程で対応するルート探索を行ったりすることができる。なお、前記事前探索エリアは、ロボットの現在位置を円の中心とし、ロボットの機体直径を半径とする第１の円形エリアであり、ロボットの周囲の最小通行可能エリアをカバーする。

ステップＳ２０２では、ロボットの現在位置を第１の経路ノードとして記録するとともに、新たな予測通行座標集合を作成し、第１の経路ノードを前記予測通行座標集合に格納し、その後、ステップＳ２０３に進む。本実施例では、前記予測通行座標集合内の要素は、ノードを順番に格納して１つのルートを形成するように構成された経路ノードの集合であり、ここで、前記予測通行座標集合の内部の先頭要素又はその末尾要素は、いずれも一意であるため、前記予測通行座標集合の内部の先頭要素又はその末尾要素は、それが表すルートの一意の識別情報となり、経路計画中におけるインデックスノードとして、又はトレースバック経路のマークされたノードとして機能することができる。そして、同一の前記予測通行座標集合の内部の先頭要素及びその末尾要素は同じではなく、これは、同一の前記予測通行座標集合の内部の先頭要素及びその末尾要素が同じである場合、前記予測通行座標集合の内部の要素は、記録された順序に従って閉じた図形を形成し、ナビゲーションの価値を持たない経路を形成するからである。

本実施例では、ステップＳ２０１を実行する際に、ロボットが、その現在位置において、前記事前探索エリアが第１の予め設定された円領域通行条件を満たすと判断した場合、ロボットの現在位置を新たな第１の経路ノードとして記録し、予測通行座標集合に追加することにより、対応する候補ルートの始点として構成することができ、ナビゲーション出発点と等価である。

本実施例では、前記第１の予め設定された円領域通行条件は、前記事前探索エリア内の第１の通行不可エリアが占める面積の割合が第１の通行評価値よりも大きいことを含む。ここで、第１の通行不可エリアは、前記第１の円形エリアに対応する格子エリア内で、未知格子点と障害物格子点とからなる格子エリアである。第１の通行評価値は、構築された格子地図に存在するフリー格子のマーキング誤差を克服するために設定された事前判断閾値であり、前記狭い通路内で実験を繰り返した結果である。第１の通行評価値は５０％とすることが好ましい。前記第１の円形エリアに対応する格子エリア内の第１の通行不可エリアが占める面積の割合が５０％以下であるとロボットが判断した場合、ロボットを制御して、前記第１の円形エリアに対応する格子エリア内の第１の通行不可エリアが占める面積の割合が５０％よりも大きいとロボットが判断するまで、予め設定された経路に沿った移動を維持する。したがって、前記第１の予め設定された円領域通行条件は、ロボットが狭い通路へ進入し始めたか否かを予備判断するための粗い判断条件であり、後続では、継続移動中にロボットがさらなる判断を行うことに依存する。しかし、単一の格子のマーキング情報がロボットの通行を許可するか否かを考慮する必要がなく、単一の格子のマーキング誤差の影響を低減する。

ステップＳ２０３では、ロボットを制御して前記予め設定された経路に沿って移動し続け、最新に記録された経路ノードまでの直線距離がロボットの機体直径以上となる位置にロボットが移動したか否かをリアルタイムで判断し、ＹＥＳであればステップＳ２０４に進み、ＮＯであれば、ロボットを制御して前記予め設定された経路に沿って移動し続ける。このステップＳ２０３では、ロボットの現在位置から第１の経路ノードまでの直線距離を含む、ロボットから前回記録された経路ノードまでの直線距離を検出する。ここで、ロボットは、現在位置から前回記録された経路ノードまでの直線距離をリアルタイムで検出する。また、距離サンプリング検出を一定の検出周期毎に行い、一定の通行条件が満たされたときに、リアルタイムで検出された経路ノードを記録するようにしてもよい。

ステップＳ２０４では、ロボットの現在位置と最新に記録された経路ノードとの直線距離がロボットの機体直径以上であることが検出された場合、ロボットの現在位置を第２の経路ノードとして記録し、その後、ステップＳ２０５に進む。

ステップＳ２０５では、前記第２の経路ノードが第２の予め設定された円領域通行条件を満たすか否かを判断し、ＹＥＳであればステップＳ２０６に進み、ＮＯであればステップＳ２０７に進む。したがって、前述のステップＳ２０４で最新に記録された経路ノードには、前回記録された第１の経路ノード又は前回記録された第２の経路ノードが含まれる。ここで、ステップＳ２０４で記録された第２の経路ノードは、前回のステップＳ２０４の実行に対応して記録された第２の経路ノードに対して、新たな第２の経路ノードであり、又は、ステップＳ２０２に対応して記録された第１の経路ノードに対して、新たな経路ノードである。具体的には、ステップＳ２０２からステップＳ２０３までを実行した場合、ステップＳ２０３で述べた最新に記録された経路ノードは、ステップＳ２０２で記録された第１の経路ノードであり、このとき、現在記録された第１の経路ノード及び第２の経路ノードが追加された予測通行座標集合は、前回実行されたステップＳ２０２で作成された予測通行座標集合ではない。ステップＳ２０２を省略してステップＳ２０３を１回繰り返し実行する（後続のステップから戻って反復実行する）場合、ステップＳ２０３で述べた最新に記録された経路ノードは、前回のステップＳ２０４の実行時に記録された第２の経路ノードであり、これら反復処理して得られた第２の経路ノードは、いずれも同一の予測通行座標集合内に順次保存される。

第２の予め設定された円領域通行条件の１つの実施形態として、前記第２の予め設定された円領域通行条件は、ロボットの現在位置（すなわち、最新に記録された第２の経路ノード）を円の中心とし、ロボットの機体直径を半径とする第２の円形エリア内で、第２の通行不可エリアが占める面積の割合が第２の通行評価値よりも大きいことを含む。ここで、第２の通行不可エリアは、第２の円形エリアに対応する格子エリア内で、未知格子点と障害物格子点とからなる格子エリアである。第２の通行評価値は、構築された格子地図に存在するフリー格子のマーキング誤差を克服するために設定された判断閾値であり、かつ、判断精度を高めるために第１の通行評価値よりも大きくされている。第２の通行評価値は７５％とすることが好ましい。前記第２の円形エリアに対応する格子エリア内の第２の通行不可エリアが占める面積の割合が７５％以下であるとロボットが判断した場合、ステップＳ２０７に進む。ここで、第２の円形エリアのカバーしているエリアは、第１の円形エリアのカバーしているエリアとは異なり、これは、ロボットの位置が変化するからである。したがって、前記第１の円形エリア、又は前回探索された第２の円形エリアを基礎として、ロボットが、最新に記録された経路ノードからロボットの機体直径分だけ離れた場所に移動した後、さらに第２の通行不可エリアが占める面積の割合の判断を行い、単一の格子点のマーカ情報の影響を考慮する必要がなく、狭い通路の判断精度を高める。本実施例では、前記第２の円形エリアの探索半径を、より大きい数値ではなく、ロボットの機体直径に設定するだけであり、これにより、無関係な（検知範囲外の）格子エリアが計算に関与することを回避し、時間的なオーバヘッドを低減する。

第２の予め設定された円領域通行条件のもう１つの実施形態として、前記第２の予め設定された円領域通行条件は、最新に記録された第１の経路ノードを円の中心とし、予め設定された倍数の機体直径を半径とする第２の円形エリア内で、経路探索アルゴリズムを用いて、最新に記録された第２の経路ノードから最新に記録された第１の経路ノードに至る経路を探索し、現在の第２の円形エリア内の経路探索が障害物の影響を受けなくなったこと、又は障害物格子の誤マークの影響を受けなくなったことを証明することを含む。前記第２の円形エリアは狭い通路の近くにある可能性があるので、最新に記録された第２の経路ノードから最新に記録された第１の経路ノードへの間に障害物が存在する可能性があり、そして、本実施例では、衝突する可能性のあるノードを無効ノード又は不正ノードと呼び、経路の計画時はこれらのノードを回避する必要があるため、成熟し安定した経路探索アルゴリズムを使用してエリアの通行可能性を検証する必要がある。本実施例では、経路探索アルゴリズムとしてＡ＊アルゴリズムを使用することで、前記第２の円形エリア内で最新に記録された第２の経路ノードから最新に記録された第１の経路ノードに至るナビゲーション経路を効率的かつ迅速に探索する。なお、本実施例では、前記第１の円形エリア又は前記第２の円形エリアを清掃エリアと見なすことができる。ここで、予め設定された倍数の機体直径は、第２の円形エリアが他のマークされた格子エリアと交差しないように設定されており、このようにして、他のマークされた格子エリア内の既に計画された経路が判断に関与することを回避し、さらに誤判断を回避する。前記予め設定された倍数の機体直径は、形成される第２の円形エリアが前記第１の円形エリアよりも大きくなるように機体直径の３分の２に設定される。従来技術と比べて、本実施例では、第２の円形エリアという探索エリアのサイズが合理的に設定されることで、他の既知の地図エリアと交差して、関連エリア内の計画された経路が納められてしまった結果、ロボットが他のエリアにガイドされて、現在の狭い通路を通るようにガイドされなくなることを回避する。一方、さらに、現在探索されている第２の円形エリア内で始点から終点までの完全たるナビゲーション経路を計画できるか否かを判断することによって、この第２の円形エリアの通行可能エリアが障害物の影響を受けなくなったこと、又は障害物格子のマーキング位置の影響を受けなくなったことを証明することができ、これにより、前記狭い通路の判断精度を高める。

ステップＳ２０６では、ロボットが現在前記狭い通路にあると判定し、第２の経路ノードをステップＳ２０２で述べた予測通行座標集合内に追加し、そしてステップＳ２０３に戻る。これは、ロボットが、その現在位置が前記狭い通路にあることを認識した後、ステップＳ２０３に戻ることにより、前記狭い通路内で前記予め設定された経路に沿って移動し続けるからである。したがって、本実施例では、ロボットは、第１の経路ノードを探索した後、機体直径分の直線距離だけ移動するたびに、ロボットの現在位置又は現在の探索エリアが第２の予め設定された円領域通行条件を満たすか否かを判断する。その後、第２の予め設定された円領域通行条件により、ロボットが現在前記狭い通路にあると決定された場合、前記予め設定された経路に沿って前記狭い通路を移動し続け、新たな第２の経路ノードを探索し続け、前記狭い通路を通過するためのルートを接続して形成する。なお、前記狭い通路は、２つ以上の障害物のギャップ通路であり、ギャップ通路は、２つの障害物間の最も狭い部分に対応する隙間であり、この隙間の幅は、ロボットの機体直径以上である。

ステップＳ２０７では、ロボットが現在前記狭い通路に位置しないと判定し、前記予測通行座標集合の内部に存在する経路ノードの数に応じて、前記予測通行座標集合を同一の候補ルート座標集合の内部に保存し、前記予測通行座標集合内に格納された経路ノードを、追加された順序に従って接続して、対応する候補ルートを形成することにより、１つの予測通行座標集合が、前記候補ルート座標集合内で１つの候補ルートを表す点の集合となる。その後、ステップＳ２０１に戻って、前記候補ルート座標集合内で、新たな候補ルートを記述するための新たな予測通行座標集合を作成する。ここで、同一の候補ルート座標集合の内部には、それぞれ他の互いに異なる候補ルートを表す複数の前記予測通行座標集合が存在する。なぜなら、同一の候補ルート座標集合の内部では、いずれかの前記予測通行座標集合の内部の先頭要素及びその末尾要素が一意であるからである。

具体的には、前記予測通行座標集合の内部に存在する経路ノードの数に応じて、前記予測通行座標集合を同一の候補ルート座標集合に保存する方法は、具体的には、前記予測通行座標集合の内部に存在する経路ノードの数が２未満である場合、ロボットが、乗り越えにくい障害物があるか、又は閉じ込められたりするなどの異常事態が発生したかを検出したため、前記予測通行座標集合の内部に存在する経路ノードが無効ノードとなる可能性を示し、前記予測通行座標集合を削除し、そしてステップＳ２０１に戻って、新たな予測通行座標集合を作成することを含む。したがって、本実施例では、１つの予測通行座標集合によって最終的に取得された経路ノードの数が、線で結ぶことが困難な程度に少ない場合、関連する集合を削除して、無効な経路ノードを減らしても良い。

前記予測通行座標集合の内部に存在する経路ノードの数が２以上である場合、前記予測通行座標集合を、単一の候補ルートを表すために使用し、経路探索アルゴリズムが呼び出され、例えば、ヒューリスティック探索アルゴリズムが使用されるように、前記候補ルート座標集合内に保存し、そしてステップＳ２０１に戻って、新たな予測通行座標集合を作成する。本実施例では、１つの予測通行座標集合によって最終的に取得された経路ノードは接続することで全体的なルートとして、１つの候補ルートを保存するための前記候補ルート座標集合に保存できるため、候補ルートの保存及び取り出し構造が合理的かつ秩序立ったものになる。ここで、前記第１の経路ノード及び前記第２の経路ノードは、記録された順序に従って前記予測通行座標集合内に追加されることで、前記予測通行座標集合内に格納された経路ノードは、記録された順序で接続して対応する候補ルートを形成する。なお、各予測通行座標集合内の先頭要素に対応する経路ノードは、対応する候補ルートの始点であり、各予測通行座標集合内の末尾要素に対応する経路ノードは、対応する候補ルートの終点である。これにより、各予測通行座標集合内の経路ノードが対応して接続して１つの候補ルートとなり、ロボットが対応するエリア内を支障なく通行できると予測される候補ルートを形成する。従来技術と比べて、前述のステップＳ２０１～Ｓ２０７に記載した候補ルートの探索方法は、ロボットが狭い通路へ進入した状態で経路探索を実施する場合に適しており、まず、第１の予め設定された円領域通行条件を狭い通路の事前判定条件として設定し、前記予測通行座標集合に対応する候補ルートの経路ノードのソースを提供し、さらに、ロボットを元の経路に沿って引き続き移動させ、その後、第２の予め設定された円領域通行条件を狭い通路の精判定条件として設定し、前記予測通行座標集合に対応する候補ルートの格子点のソースを提供し続ける。これにより、ロボットは、一定の距離だけ移動して初めて、狭い通路を判定する資格を持つようになり、前記予測通行座標集合に収集された、格子点を接続した候補ルートはより完全であり、狭い通路における地図の格子の誤差環境により適したものになり、対応する格子のマーキング情報にリアルタイムで注目することなく、実際に通行可能なルートをロボットに提供することができる。また、これに基づいて、対応する予め設定された円領域通行条件が満たされない場合、前記予測通行座標集合ための探索を停止し、単一の前記予測通行座標集合内の経路ノードが接続して独立した候補ルートを形成できると決定する。前述の関連ステップを反復実行することにより、ロボットは、通常の作業移動状態で、地図ドリフト誤差に起因して格子を通行不可能な問題を解消できるルート点の集合を探索する。障害物の空間レイアウトが複雑なシーンにおいてロボットが効果的な走行経路を見つける成功率を高める。

特に、予測通行座標集合内に障害物格子点が含まれてもよく、対応する候補ルートに障害物格子点が存在する可能性があることを示すことに注意する必要がある。ステップＳ２０１～ステップＳ２０７から分かるように、ロボット自体が特定の経路ノードに移動し、例えば、１回の反復実行中にステップＳ２０２まで実行したとき（初期状態条件を満たす）、又は１回の反復実行中にステップＳ２０６まで実行したときの対応するロボット現在位置に移動し、かつ、この経路ノード上で前記第１の予め設定された円領域通行条件又は前記第２の予め設定された円領域通行条件が満たされると判断した場合、この経路ノードを、対応する候補ルートに接続される経路ノードとして、前記予測通行座標集合に追加する。しかし、センサ検知誤差、地図ドリフト誤差等により、この経路ノードについて格子地図上でマークされた格子情報がフリー状態ではなく、障害物占有状態、すなわち前記障害物格子点として誤ってマークされる可能性があり、この経路ノードは、格子地図上では障害物格子点としてマークされているが、前記予測通行座標集合に追加されて、対応する候補ルート上の経路ノードとされ、ロボットは、実際には、この経路ノードに移動することができ、この経路ノードは通過可能であるか、又は連通可能であることが証明される。

前述の実施例に基づいて、ロボットは、現在、前記狭い通路にある状態から前記狭い通路にない状態に変化したことを検出すると、前記経路融合計画方法の実行を開始し、すなわち、前述のステップＳ１０１～ステップＳ１１１の反復実行を開始することにより、ロボットが前記狭い通路に自由に出入りできるナビゲーション経路を計画して、地図ドリフト誤差を克服する。これにより、ロボットが前記狭い通路から出ているとき、又は現在の狭い通路から出て新たな狭い通路に入る直前に、探索された前記狭い通路を通過するのに適した候補ルートの座標点を、現在位置でのヒューリスティック探索アルゴリズムの使用に融合して、ナビゲーション経路を計画することができ、近傍探索のみに頼ってナビゲーション経路計画を行うのではないため、狭い通路で探索されたフリー格子点が障害物格子点としてマークされやすく、計画された経路では狭い通路を通過できないという問題を解決する。

チップであって、前述のいずれかの技術案に記載の経路融合計画方法を実行するためのプログラムコードが格納されている。上記チップによれば、障害物が多く分布する狭い通路内で、ヒューリスティック探索アルゴリズムと探索条件に合致する候補ルートとを融合してナビゲーション経路を計画することにより、地図格子における通路が狭くてマーキング誤差が生じやすいことを克服し、ロボットが計画された全体的なナビゲーション経路に沿ってこの狭い通路に効果的に出入りできるようになり、障害物空間のレイアウトが複雑なシーンで経路のナビゲーションが失敗する確率を減らす。

ロボットであって、前記経路融合計画方法を実行するようにロボットを制御するための前記チップを内蔵している。ロボットが、前記経路融合計画方法で計画されたナビゲーション経路に従って狭い通路を通過する過程において、ヒューリスティック探索アルゴリズム又はインクリメンタルなヒューリスティック探索アルゴリズムを用いて経路を探索する際に、狭い通路が検出されると、狭い通路の近くで、ヒューリスティック探索アルゴリズム又はインクリメンタルなヒューリスティック探索アルゴリズムで探索されたナビゲーション経路への進入から、前記候補ルートへの進入に切り替わり、一方、狭い通路から出ていることが検出されると、前記候補ルートから、ヒューリスティック探索アルゴリズム又はインクリメンタルなヒューリスティック探索アルゴリズムで探索されたナビゲーション経路への進入に切り替わり、これにより、ロボットが、対応する候補ルートが接続してなるナビゲーション経路に沿って、狭い通路を効果的に通過できるようになり、狭い通路を乗り越えるナビゲーションの成功率を高める。

Claims (17)

1. 通行エリアの経路融合計画方法であって、
   格子地図においてナビゲーション始点及びナビゲーション終点を設定し、トラバース対象ノードキャッシュ空間を作成するステップ１と、
   ナビゲーション始点を現在の親ノードに設定し、トラバース対象ノードキャッシュ空間に追加するステップ２と、
   現在の親ノードを探索中心として、格子地図において近傍探索を行うステップ３であって、現在の親ノードに隣接する８つの格子点は、それぞれその子ノードとするステップ３と、
   ステップ３にて近傍で探索された子ノードが、予め探索された候補ルート座標集合内の対応する候補ルートの一方の端点である場合、対応する候補ルートの他方の端点をトラバース対象ノードキャッシュ空間に追加し、同時に、対応する候補ルートの両端点間のすべての中間ノードとこの端点とを、すべて、繰り返し探索不可能なノードに設定し、ステップ３にて探索された、前記対応する候補ルート上にないフリー格子点をトラバース対象ノードキャッシュ空間に追加し、その後、ステップ５に進むステップ４であって、前記候補ルート座標集合内の対応する候補ルートは、格子地図上にマークされた障害物格子点を通過することを許可され、トラバース対象ノードキャッシュ空間に追加されたすべてのノードは、後続の経路ノードのトレースバックのために、その親ノードの位置情報が記録されているステップ４と、
   ステップ４で述べたトラバース対象ノードキャッシュ空間内から、経路コストの合計値が最小となるノードを次の親ノードとして選択し、次に、前記次の親ノードが前記ナビゲーション終点であるか否かを判断し、ＹＥＳであれば、前記記録された親ノードの位置情報に基づいて、前記ナビゲーション終点から、ステップ４で述べた候補ルートのすべての中間ノード及びその対応する１つの端点を除いて、子ノード及びその親ノードを、前記ナビゲーション始点に接続されるまで順次接続することで、前記ナビゲーション始点から前記ナビゲーション終点までの経路を計画し、ＮＯであれば、前記次の親ノードをステップ３で述べた現在の親ノードに更新し、そしてステップ３に戻るステップ５と、を含む、ことを特徴とする経路融合計画方法。
2. ステップ４は、
   ステップ３にて近傍で探索された子ノードが前記候補ルート座標集合内のすべての候補ルートの端点でない場合、ステップ３にて近傍で探索されたフリー格子点を、すべて、前記トラバース対象ノードキャッシュ空間内に追加し、ステップ３にて近傍で探索されたフリー格子点の親ノードの位置情報を記録し、その後、ステップ５に進むことをさらに含む、ことを特徴とする請求項１に記載の経路融合計画方法。
3. 前記ステップ４では、ステップ３にて近傍で探索された子ノードが前記候補ルート座標集合内の対応する候補ルートの始点である場合、対応する候補ルートの終点を前記トラバース対象ノードキャッシュ空間に追加し、前記対応する候補ルートの終点の親ノードの位置情報を、前記候補ルート座標集合内で各候補ルートがその終点をルート識別情報として使用するように記録し、同時に、対応する候補ルートの両端点間のすべての中間ノードと同一の候補ルートの始点とを、すべて、繰り返し探索不可能なノードに設定し、
   ここで、前記対応する候補ルートの始点からその終点に向かう経路延在方向に沿って、前記対応する候補ルート上の各ノードの親ノードは、そのノードに隣接するノード位置に位置している、ことを特徴とする請求項１に記載の経路融合計画方法。
4. 前記ステップ４では、ステップ３にて近傍で探索された子ノードが前記候補ルート座標集合内の対応する候補ルートの終点である場合、対応する候補ルートの始点を前記トラバース対象ノードキャッシュ空間に追加し、対応する候補ルートの始点の親ノードの位置情報を、前記候補ルート座標集合内で各候補ルートがその始点をルート識別情報として使用するように記録し、同時に、対応する候補ルートの両端点間のすべての中間ノードと同一の候補ルートの終点とを、すべて、繰り返し探索不可能なノードに設定し、
   ここで、前記対応する候補ルートの終点からその始点に向かう経路延在方向に沿って、前記対応する候補ルート上の各ノードの親ノードは、そのノードに隣接するノード位置に位置している、ことを特徴とする請求項１に記載の経路融合計画方法。
5. 前記ステップ５では、前記記録された親ノードの位置情報に基づいて、前記ナビゲーション終点から、ステップ４で述べた候補ルートのすべての中間ノード及びその対応する１つの端点を除いて、子ノード及びその親ノードを、前記ナビゲーション始点に接続されるまで順次接続することは、具体的には、
   前記記録された親ノードの位置情報に基づいて、前記ナビゲーション終点から、前記ナビゲーション終点及びその親ノードを接続し、その後、ステップ５２に進むステップ５１と、
   現在決定された親ノードを子ノードとし、さらに、前記記録された親ノードの位置情報に基づいて、この子ノードの親ノードを接続するステップ５２と、
   ステップ５２を、ステップ４で述べた対応する候補ルートの一方の端点に接続されるまで繰り返し、次いで、ステップ４で述べた対応する候補ルートの他方の端点から、ステップ４で述べた対応する候補ルートの他方の端点及びその親ノードを接続し、その後、ステップ５２に戻るステップ５３と、
   ステップ５２及びステップ５３を、前記ナビゲーション始点に接続されるまで繰り返すことで、逆方向接続により、ナビゲーション始点からナビゲーション終点までのナビゲーション経路を得るステップ５４と、を含む、ことを特徴とする請求項３又は４に記載の経路融合計画方法。
6. 前記ステップ５では、前記経路コストの合計値は、トラバース済み経路コストと予測経路コストとを加算又は重み付けして得られ、ここで、トラバース済み経路コストは、前記トラバース対象ノードキャッシュ空間内の１つの指定したノードから前記ナビゲーション始点までのコストであり、予測経路コストは、前記トラバース対象ノードキャッシュ空間内の同一の指定したノードから前記ナビゲーション終点までのコストであり、前記経路コストの合計値が小さいほど、指定したノードの、前記トラバース対象ノードキャッシュ空間内のトラバース優先度を高く設定し、
   ここで、２つの隣接ノード間の移動コストは、マンハッタン距離、対角距離、又はユークリッド距離で表される、ことを特徴とする請求項５に記載の経路融合計画方法。
7. ナビゲーション始点から、トラバースされた各格子の中心点を、前記指定したノードに接続されるまで順次接続し、さらに、格子の辺の長さから、現在接続してなる線の長さを求め、その長さを前記トラバース済み経路コストとし、
   前記指定したノードを親ノードに設定し、この親ノードに対応する各子ノードからナビゲーション終点までのすべての線の結び方を取得し、さらに格子の辺の長さから、各子ノードに対応する各結び方の線の長さを求め、各子ノードに対応する最短の長さを選択してその対応する予測経路コストとし、
   ここで、格子点は、格子の中心点で表され、格子の位置特徴を表すために用いられる、ことを特徴とする請求項６に記載の経路融合計画方法。
8. 前記経路融合計画方法では、前記ステップ４にて繰り返し探索不可能に設定されたノードを格納するためのトラバース済みノードキャッシュ空間をさらに作成し、
   ここで、前記トラバース済みノードキャッシュ空間内に存在するノードは、前記トラバース対象ノードキャッシュ空間に追加することが禁止される、ことを特徴とする請求項１に記載の経路融合計画方法。
9. 前記ステップ４は、
   現在の親ノードの近傍における、前記対応する候補ルート上に位置しないフリー格子点、又は、現在の親ノードの近傍におけるすべてのフリー格子点を、トラバース対象ノードキャッシュ空間に追加していれば、この現在の親ノードを前記トラバース対象ノードキャッシュ空間から削除してから、この現在の親ノードを前記トラバース済みノードキャッシュ空間に追加することをさらに含む、ことを特徴とする請求項８に記載の経路融合計画方法。
10. 前記経路融合計画方法を実行する前に、前記候補ルート座標集合内の対応する候補ルートの探索方法は、具体的には、
    ロボットを制御して予め設定された経路に沿って移動する過程で、事前探索エリアが第１の予め設定された円領域通行条件を満たすと判断すると、ステップＳ２に進むステップＳ１と、
    ロボットの現在位置を第１の経路ノードとして記録するとともに、新たな予測通行座標集合を作成し、第１の経路ノードを前記予測通行座標集合に格納し、その後、ステップＳ３に進むステップＳ２と、
    ロボットを制御して前記予め設定された経路に沿って移動し続け、最新に記録された経路ノードまでの直線距離がロボットの機体直径以上となる位置にロボットが移動したと判断すると、ステップＳ４に進むステップＳ３と、
    ロボットの現在位置を第２の経路ノードとして記録するとともに、第２の経路ノードが第２の予め設定された円領域通行条件を満たすか否かを判断し、ＹＥＳであればステップＳ５に進み、ＮＯであればステップＳ６に進むステップＳ４と、
    ロボットが現在前記狭い通路にあると判定し、第２の経路ノードをステップＳ２で述べた予測通行座標集合に追加し、そしてステップＳ３に戻るステップＳ５と、
    ロボットが現在前記狭い通路に位置しないと判定し、前記予測通行座標集合の内部に存在する経路ノードの数に応じて、前記予測通行座標集合を同一の候補ルート座標集合の内部に保存し、前記予測通行座標集合内に格納された経路ノードを、追加された順序に従って接続して、対応する候補ルートを形成し、その後、ステップＳ１に戻るステップＳ６と、を含み、
    ここで、同一の候補ルート座標集合の内部では、いずれかの前記予測通行座標集合の内部の先頭要素及びその末尾要素が一意であり、かつ同一の前記予測通行座標集合の内部の先頭要素及びその末尾要素は同じではなく、
    ここで、前記狭い通路は、２つ以上の障害物のギャップ通路であり、ギャップ通路は、２つの障害物間の最も狭い部分に対応する隙間であり、この隙間の幅は、ロボットの機体直径以上であり、
    ここで、前記予め設定された経路は、ロボットが予め計画した経路である、ことを特徴とする請求項７に記載の経路融合計画方法。
11. ロボットが現在、前記狭い通路にある状態から前記狭い通路にない状態に変化したことを検出すると、前記ステップ１～前記ステップ５を実行して、前記経路融合計画方法の実行を開始する、ことを特徴とする請求項１０に記載の経路融合計画方法。
12. 前記予測通行座標集合の内部に存在する経路ノードの数に応じて、前記予測通行座標集合を同一の候補ルート座標集合に保存する方法は、具体的には、
    前記予測通行座標集合の内部に存在する経路ノードの数が２未満である場合、前記予測通行座標集合を削除し、そしてステップＳ１に戻って新たな予測通行座標集合を作成することと、
    前記予測通行座標集合の内部に存在する経路ノードの数が２以上である場合、前記予測通行座標集合を前記候補ルート座標集合内に保存し、新たな候補ルートを対応して保存し、そしてステップＳ１に戻って新たな予測通行座標集合を作成することと、を含み、
    ここで、前記第１の経路ノード及び前記第２の経路ノードは、記録された順序に従って、前記予測通行座標集合内に追加されることで、前記予測通行座標集合内に格納された経路ノードは、一定の順番で接続して対応する候補ルートを形成し、
    ここで、各予測通行座標集合内の先頭要素に対応する経路ノードは、対応する候補ルートの始点であり、各予測通行座標集合内の末尾要素に対応する経路ノードは、対応する候補ルートの終点である、ことを特徴とする請求項１０に記載の経路融合計画方法。
13. 前記第１の予め設定された円領域通行条件は、
    前記事前探索エリア内の第１の通行不可エリアが占める面積の割合は、第１の通行評価値よりも大きいことを含み、
    ここで、第１の通行不可エリアは、前記第１の円形エリアに対応する格子エリア内で、未知格子点と障害物格子点とからなる格子エリアであり、第１の通行評価値は、構築された格子地図に存在するフリー格子のマーキング誤差を克服するために設定された事前判定閾値であり、
    ここで、前記事前探索エリアは、ロボットの現在位置を円の中心とし、ロボットの機体直径を半径とする第１の円形エリアである、ことを特徴とする請求項１０に記載の経路融合計画方法。
14. 前記第２の予め設定された円領域通行条件は、
    ロボットの現在位置を円の中心とし、ロボットの機体直径を半径とする第２の円形エリア内で、第２の通行不可エリアが占める面積の割合は、第２の通行評価値よりも大きいことを含み、
    ここで、第２の通行不可エリアは、第２の円形エリアに対応する格子エリア内で、未知格子点と障害物格子点とからなる格子エリアであり、第２の通行評価値は、構築された格子地図に存在するフリー格子のマーキング誤差を克服するために設定された判断閾値であり、かつ第１の通行評価値よりも大きい、ことを特徴とする請求項１３に記載の経路融合計画方法。
15. 前記第２の予め設定された円領域通行条件は、
    最新に記録された第１の経路ノードを円の中心とし、予め設定された倍数の機体直径を半径とする第２の円形エリア内で、経路探索アルゴリズムを用いて、最新に記録された第２の経路ノードから最新に記録された第１の経路ノードに至る経路を探索したことを含み、
    ここで、予め設定された倍数の機体直径は、第２の円形エリアが他のマークされた格子エリアと交差しないように設定されている、ことを特徴とする請求項１３に記載の経路融合計画方法。
16. 請求項１～１５のいずれか一項に記載の経路融合計画方法を実行するためのプログラムコードが格納されている、ことを特徴とするチップ。
17. ロボットであって、
    請求項１～１５のいずれか一項に記載の経路融合計画方法を実行するように前記ロボットを制御するための請求項１６に記載のチップを内蔵している、ことを特徴とするロボット。