(57)【特許請求の範囲】
【請求項１】
半自律車両を制御する方法であって、
前記車両の運転者から受信した行為に応答して、前記車両のための現在経路を決定するステップであって、前記運転者の前記行為は、前記車両のハンドルの操作または前記車両のペダルの操作またはその両方を含み、前記現在経路は、前記車両の現在位置および現在方位において開始し、前記車両の目標位置および目標方位において終了し、前記現在経路は、点の系列を含み、各前記点は、前記車両の位置の座標と前記車両の方位とを示す、ステップと、
前記車両の前記現在位置及び前記目標位置を変更経路内で維持しながら、前記車両のための前記現在経路を変更するステップと、
前記変更経路に従って前記車両の動きを制御するために、前記運転者の前記行為を無効にするステップと
を含み、前記車両の各前記ステップは前記車両のプロセッサを用いて実行され、
前記変更するステップ及び前記無効にするステップは、前記車両の前記現在経路上で障害物を検出するのに応答して実行され、
前記変更するステップは、
前記現在位置および現在方位を示すノードを前記目標位置および目標方位を示すノードに接続する、粗く分離したノードの第１の集合によって規定される、粗い経路を決定することであって、各前記ノードは、前記車両の位置の値と方位の値とを示す、ことと、
細かく分離したノードの第２の集合によって形成され、前記決定された粗い経路に近い精緻化経路を生成するために、前記粗い経路を精緻化することと、
枝刈り経路を生成するために、障害物を回避するために役に立つことなくコスト関数の値を高める前記精緻化経路の冗長ノードを少なくとも１つ除去することであって、前記コスト関数は、前記現在経路からの前記変更経路への偏差と、前記変更経路の曲率と前記現在経路の曲率との間の差との組み合わせのバランスを取る、ことと、
前記変更経路を生成するために、前記枝刈り経路の前記ノードを接続する軌道を平滑化することと
を含む、
方法。
【請求項２】
前記変更するステップは、
前記車両の前記現在位置から前記目標位置までの１組の経路を決定することであって、各前記経路は、運転制約を満たしながら前記障害物を回避する、ことと、
前記１組の経路に含まれる各前記経路ごとに、前記現在経路と前記１組の経路に含まれる各前記経路との間の差の関数として、性能測定基準を決定することと、
各前記経路の前記性能測定基準に従った変更経路として、前記１組の経路の中から１つの経路を選択することと
を含む、請求項１に記載の方法。
【請求項３】
前記性能測定基準は、
前記現在経路と前記１組の経路に含まれる各前記経路との間の全位置差であり、前記変更経路として選択された前記経路は、前記性能測定基準を有し、最小の全位置差に対応している、
請求項２に記載の方法。
【請求項４】
前記経路の前記性能測定基準は、前記現在経路からの前記経路の偏差と、前記経路の曲率と前記現在経路の曲率との間の差との組み合わせのバランスを取るコスト関数であり、
前記変更経路として選択された前記経路は、前記コスト関数の最小値に対応する前記性能測定基準を有する、
請求項２に記載の方法。
【請求項５】
前記変更するステップは、
前記現在経路からの障害物を回避する前記変更経路の前記偏差を低減する前記コスト関数を最適化することによって前記変更経路を決定することを含む、請求項１に記載の方法。
【請求項６】
前記変更するステップは、
前記現在経路からの前記変更経路への前記偏差と、前記変更経路の前記曲率と前記現在経路の前記曲率との間の前記差との前記組み合わせのバランスを取る前記コスト関数を最適化することによって、前記障害物を回避する前記変更経路を決定することを含む、請求項１に記載の方法。
【請求項７】
前記粗い経路を決定することは、
ノードを接続するリンクが、前記現在位置を前記目標位置に接続する１組の粗い経路を形成するように、前記ノードのツリーを生成することと、
新たなノードを通り抜ける粗い経路が、前記コスト関数に基づいて、前記新たなノードを通り抜けない別の粗い経路より最適である場合には、前記ツリー内に前記新たなノードを配置することと
を含む、請求項１に記載の方法。
【請求項８】
前記ノードの前記ツリーを生成することは、
前記車両の運転可能空間においてランダムノードをサンプリングすることと、
前記ランダムノードと前記ノードの前記ツリー内の最も近いノードとを接続する経路上の新たなノードを決定することと、
前記新たなノードと前記最も近いノードとを接続する前記経路に沿った障害物との衝突を検出することと、
前記衝突が検出されなかった場合に、前記新たなノードを、前記ノードの前記ツリー内の１つの既存のノードに追加することであって、前記新たなノードは、前記ノードの前記ツリーのルートノードから前記新たなノードへ到達するコストを低減するものであって、前記コストは、前記コスト関数によって決定される、前記ルートノードから前記既存のノードに到達するためのコストと、前記既存のノードから前記新たなノードに到達するためのコストとの和を含む、ことと、
を含む、
請求項７に記載の方法。
【請求項９】
前記ノードの前記ツリーに前記新たなノードを追加することに応答して、新たなリンクを通って前記新たなノードから前記新たなノードの付近のノードに到達するコストが、古いリンクを通って前記付近のノードに到達するコストよりも少ない場合に、前記付近のノードに到達する前記古いリンクを、前記新たなノードから前記付近のノードに到達する前記新たなリンクに置き換えることをさらに含む、
請求項８に記載の方法。
【請求項１０】
前記精緻化することは、
前記粗い経路を中心にしたエリア内に、前記粗い経路内の密度より高い密度を有する新たなノードを配置することを含む、請求項１に記載の方法。
【請求項１１】
前記目標位置から前記現在位置までの後方に向かう前記粗い経路の１組の点を決定することと、
前記目標位置に接近する前記変更経路の一部分を維持しながら、前記車両の前記現在位置の変化に応答して、前記現在位置から前記目標位置までの前方に向かう前記変更経路に対応する前記粗い経路のノードの集合を更新することと
を更に含む、請求項１に記載の方法。
【請求項１２】
半自律車両を制御する方法であって、
前記車両の運転者から受信した行為に応答して、前記車両のための現在経路を決定するステップであって、前記運転者の前記行為は、前記車両のハンドルの操作または前記車両のペダルの操作またはその両方を含み、前記現在経路は、前記車両の現在位置および現在方位において開始し、前記車両の目標位置および目標方位において終了し、前記現在経路は、点の系列を含み、各前記点は、前記車両の位置の座標と前記車両の方位とを示す、ステップと、
前記車両の前記現在経路上の障害物を検出するのに応答して、前記障害物を回避する変更経路を生成するために、前記車両のための前記現在経路を変更するステップであって、前記変更することは、前記現在経路からの前記変更経路への偏差のコスト関数を最適化することを含む、ステップと、
前記変更経路に従って前記車両の動きを制御するために、前記運転者の前記行為を無効にするステップと
を含み、前記車両の各前記ステップは前記車両のプロセッサを用いて実行され、
前記変更するステップ及び前記無効にするステップは、前記車両の前記現在経路上で障害物を検出するのに応答して実行され、
前記変更するステップは、
前記現在位置および現在方位を示すノードを前記目標位置および目標方位を示すノードに接続する、粗く分離したノードの第１の集合によって規定される、粗い経路を決定することであって、各前記ノードは、前記車両の位置の値と方位の値とを示す、ことと、
細かく分離したノードの第２の集合によって形成され、前記決定された粗い経路に近い精緻化経路を生成するために、前記粗い経路を精緻化することと、
枝刈り経路を生成するために、障害物を回避するために役に立つことなくコスト関数の値を高める前記精緻化経路の冗長ノードを少なくとも１つ除去することであって、前記コスト関数は、前記現在経路からの前記変更経路への偏差と、前記変更経路の曲率と前記現在経路の曲率との間の差との組み合わせのバランスを取る、ことと、
前記変更経路を生成するために、前記枝刈り経路の前記ノードを接続する軌道を平滑化することと
を含む、方法。
【請求項１３】
前記コスト関数は前記車両の動きに関する制約を前提として最適化される、請求項１２に記載の方法。
【請求項１４】
前記制約は、前記変更経路を前記車両の前記現在位置において開始し、前記車両の前記目標位置において終了するように強制する制約を含む、請求項１３に記載の方法。
【請求項１５】
半自律車両であって、
前記車両の運転者から受信した行為に応答して、前記車両のための現在経路を決定するためのナビゲーションシステムであって、前記運転者の前記行為は、前記車両のハンドルの操作または前記車両のペダルの操作またはその両方を含み、前記現在経路は、前記車両の現在位置および現在方位において開始し、前記車両の目標位置および目標方位において終了し、前記現在経路は、点の系列を含み、各前記点は、前記車両の位置の座標と前記車両の方位とを示す、ナビゲーションシステムと、
前記車両の前記現在経路上の障害物を検出するためのセンサーと、
前記車両の前記現在経路上の障害物を検出するのに応答して、前記障害物を回避する変更経路を生成するために、前記車両のための前記現在経路を変更するための経路計画システムであって、
前記変更することは、前記現在経路からの前記変更経路への偏差のコスト関数を最適化することを含み、前記コスト関数は前記車両の動きに関する制約を前提として最適化され、前記制約は、前記変更経路を前記車両の前記現在位置において開始し、前記車両の前記目標位置において終了するように強制する制約を含み、前記変更することは、前記車両の前記現在経路上で障害物を検出するのに応答して実行され、
前記変更することは、
前記現在位置および現在方位を示すノードを前記目標位置および目標方位を示すノードに接続する、粗く分離したノードの第１の集合によって規定される、粗い経路を決定することであって、各前記ノードは、前記車両の位置の値と方位の値とを示す、ことと、
細かく分離したノードの第２の集合によって形成され、前記決定された粗い経路に近い精緻化経路を生成するために、前記粗い経路を精緻化することと、
枝刈り経路を生成するために、障害物を回避するために役に立つことなくコスト関数の値を高める前記精緻化経路の冗長ノードを少なくとも１つ除去することであって、前記コスト関数は、前記現在経路からの前記変更経路への偏差と、前記変更経路の曲率と前記現在経路の曲率との間の差との組み合わせのバランスを取る、ことと、
前記変更経路を生成するために、前記枝刈り経路の前記ノードを接続する軌道を平滑化することと
を含む、経路計画システムと、
前記変更経路に従って前記車両の動きを制御するために、前記運転者の前記行為を無効にするための１組のアクチュエーターと、
を備える、半自律車両。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は包括的には車両を制御することに関し、より詳細には、半自律車両を制御することに関する。
【背景技術】
【０００２】
自律車両又は半自律車両によって使用される幾つかの制御システムは、車両の種々のセンサーによって検知された周囲状況を通して車両のための安全経路を予測することによって車両の事故を回避することを目的とする。検知情報は、近隣車両、歩行者、道路縁、及び事故のおそれを評価するための他の顕著な特徴に関連するデータを含むことができる。そのような制御システムは、理想的には、大きなおそれがある場合にのみ動作し、すなわち、そのような制御システムによれば、車両の運転者は、事故のおそれが小さい状況では車両を完全に制御できるようになるが、事故のおそれが大きい状況では、制御システムが運転者の行為を無効にして、車両の動きを制御する。例えば、特許文献１に記述されている方法を参照されたい。そのような事故のおそれが大きい状況では、車両の制御は、自律運転車両の制御と同様の制御となる。
【０００３】
自律運転に適した経路計画が、幾つかの異なるシステム及び／又は方法によって検討されてきた。例えば、特許文献２の方法は、おそらく車線変更又は追越し操作を行うために道路上のあらかじめ割り当てられた格子から経路を生成することを記述している。特許文献３において、格子は固定されず、アルゴリズム実行中に拡張されるが、運動学的制約を考慮に入れるために、複雑な数値問題を解く必要がある。これらの問題を回避するために、特許文献４の方法は、運動学的制約を考慮に入れるために、平滑化アルゴリズムに追随される格子探索を使用する。経路計画アルゴリズムにおいて複数の点から格子を構成することは多くの場合に非常に複雑であり、それゆえ、特許文献５では、ランダム化アルゴリズムが提案されており、そのアルゴリズムは、車両配置空間内で動作し、車両が達成できる経路に近い経路を得るが、その経路は、平滑ではなく、すなわち、車両が厳密には達成することができない急カーブを有する。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】米国特許出願公開第２０１０／０２２８４２７号明細書
【特許文献２】米国特許出願公開第２０１４／０２０７３２５号明細書
【特許文献３】米国特許第８，０３８，０６２号明細書
【特許文献４】米国特許第７，７３４，３８７号明細書
【特許文献５】米国特許第８，６６６，５４８号明細書
【発明の概要】
【発明が解決しようとする課題】
【０００５】
それら全ての方法は、運転者の以前の行為を無視する。しかしながら、半自律車両では、事故のおそれが小さくなった後に、運転者は車両の制御を取り戻し、現在無視されている以前の行為によって示される目的に従って車両を更に制御する必要がある。
【課題を解決するための手段】
【０００６】
本発明の幾つかの実施形態は、半自律車両の制御システムによって変更された経路は、運転者によって望まれる現在経路にできる限り近いままにする必要があるという認識に基づく。そのような場合、制御システムの運転者の制御に対する無効化行為によって、運転者が方向を失うことが少なくなり、運転者が制御を取り戻したときに、運転の目的により迅速に戻ることができる。そのため、本発明の異なる実施形態は、車両の運転者から受信した行為に応答して決定された車両の現在経路を考慮して、車両が障害物を回避するための経路を決定する。
【０００７】
例えば、一実施形態は、現在経路による車両の目標位置が変化しないままであるように、変更経路を決定する。それに加えて、又はその代わりに、幾つかの実施形態は、現在経路からの偏差を最小限に抑えて障害物を回避する変更経路を決定する。例えば、一実施形態は、現在経路からの変更経路の偏差のコスト関数を最適化することによって、変更経路を決定する。別の実施形態は、現在経路からの偏差と、現在経路に対する障害物を回避する変更経路の曲率の差との組み合わせのバランスを取るコスト関数を最適化することによって変更経路を決定する。
【０００８】
本発明の幾つかの実施形態は、そのような計算が、例えば、車両の現在経路上の障害物を検出するのに応答して、リアルタイムに実行されなければならないという認識に基づく。そのため、変更経路を決定するために必要とされる計算は効率的でなければならない。しかしながら、車両の動きに関する制約を前提としてコスト関数を最適化するために必要とされる最適化問題を解くのは一般に複雑である。
【０００９】
それゆえ、幾つかの実施形態は、変更経路を計算するために種々の段階を与える。第１の段階は粗く、現在位置と目標位置とを接続する、粗く分離したランダムにサンプリングされた点の異なる系列によって形成された複数の粗い経路を最適化することを含む。それらの粗い経路の最適化は、計算の複雑度を緩和する。次の段階は、細かく分離したランダムにサンプリングされた点の異なる系列によって形成され、粗い経路に近い、複数の細かい経路を最適化することによって、精緻化することを伴う。後続の段階は、当該細かい経路内の冗長点を除去して、枝刈り経路を得ることを伴う。最終段階は、枝刈り経路を平滑化することを伴う。そのようにして、変更経路を決定する方法の全体的な計算複雑度が緩和される。
【００１０】
さらに、幾つかの実施形態は、後方に向かって、すなわち、目標位置から現在位置に向かって複数の粗い経路を決定する。しかしながら、車両の現在位置が変化するとき、その実施形態は、目標位置に接近する変更経路の部分を維持しながら、選択された粗い経路を前方に更新する。そのようにして、選択経路の一部を再利用することができ、それにより、計算複雑度を更に緩和する。
【００１１】
したがって、一実施形態は、半自律車両を制御する方法を開示する。本方法は、車両の運転者から受信した行為に応答して、車両の現在位置において開始され、車両の目標位置において終了される、車両のための現在経路を決定することと、車両の現在位置及び目標位置を変更経路内で維持しながら車両のための現在経路を変更することと、変更経路に従って車両の動きを制御するために、運転者の行為を無効にすることとを含み、ここで、それらの車両のステップは、車両のプロセッサを用いて実行される。
【００１２】
別の実施形態は、半自律車両を制御する方法を開示する。本方法は、車両の運転者から受信した行為に応答して、車両のための現在経路を決定することと、車両の現在経路上の障害物を検出するのに応答して、障害物を回避する変更経路を生成するために、車両のための現在経路を変更することであって、当該変更は、現在経路からの変更経路への偏差のコスト関数を最適化することを含む、変更することと、当該変更経路に従って車両の動きを制御するために、運転者の行為を無効にすることとを含み、それらの車両のステップは車両のプロセッサを用いて実行される。
【００１３】
更に別の実施形態は、半自律車両を開示する。この半自律車両は、車両の運転者から受信した行為に応答して、車両のための現在経路を決定するためのナビゲーションシステムであって、当該現在経路は、車両の現在位置において開始し、車両の目標位置において終了する、ナビゲーションシステムと、車両の現在経路上の障害物を検出するためのセンサーと、車両の現在経路上の障害物を検出するのに応答して、障害物を回避する変更経路を生成するために、車両のための現在経路を変更するための経路計画システムであって、当該変更することは、現在経路からの変更経路への偏差のコスト関数を最適化することを含み、当該コスト関数は、車両の動きに関する制約を前提として最適化され、当該制約は、変更経路に対して、車両の現在位置において開始して、車両の目標位置において終了するように強制する制約を含む、経路計画システムと、変更経路に従って車両の動きを制御するために、運転者の行為を無効にするための１組のアクチュエーターとを備える。
【図面の簡単な説明】
【００１４】
【図１Ａ】本発明の幾つかの実施形態による、半自律車両のブロック図である。
【図１Ｂ】本発明の一実施形態による、経路計画システムのブロック図である。
【図２】本発明の種々の実施形態によって利用される幾つかの原理に従って決定された種々の変更経路の概略図である。
【図３】本発明の幾つかの実施形態による、運転者によって望まれる経路を考慮することによって半自律車両を制御する方法のフローチャートである。
【図４Ａ】本発明の幾つかの実施形態による、変更経路を決定する多段階方法のフローチャートである。
【図４Ｂ】本発明の一実施形態による粗い経路の決定を示す概略図である。
【図５】本発明の一実施形態による、所与の粗い経路の周囲の細かい経路を得ることを示す概略図である。
【図６】本発明の一実施形態による、精緻化経路から冗長ノードを除去することを示す概略図である。
【図７】本発明の一実施形態による、枝刈り経路を平滑化することを示す概略図である。
【図８Ａ】本発明の幾つかの実施形態による、変更経路を更新する方法のフローチャートである。
【図８Ｂ】本発明の一実施形態による変更経路の更新を示す概略図である。
【発明を実施するための形態】
【００１５】
図１Ａは、本発明の幾つかの実施形態による、半自律車両１００のブロック図を示す。車両１００は、特定の条件が生じたときに、車両の運転者の行為を無効にし、運転者から車両の制御を引き継ぐことができる自律システムを含む任意のタイプの移動車両とすることができる。例えば、車両１００は四輪乗用車とすることができる。
【００１６】
車両１００は、ハンドル及びペダルの操作のような、車両の運転者から受信した行為に応答して、車両のための現在経路を決定するナビゲーションシステム１０３を含む。例えば、現在経路は、車両の現在位置において開始し、車両の目標位置において終了する。車両１００は、車両の現在経路上の障害物を検出するための少なくとも１つのセンサー１０６と、ナビゲーションシステムによって決定された現在経路とは異なる変更経路に従って障害物を回避するために、運転者の行為を無効にし、車両の動きを制御するための１組のアクチュエーターとを含む。障害物は、別の車両又は歩行者のような実在する障害物とするか、あるいは、許可された走行車線を画定するライン若しくは停止線のような、違法の運転挙動を表す仮想的な障害物とすることができる。
【００１７】
また、車両１００は、車両の現在経路上の障害物を検出するのに応答して、車両のための現在経路を変更し、障害物を回避する変更経路を生成する経路計画システム１０１を含む。一実施形態では、経路計画システムは、現在経路から変更経路への偏差のコスト関数を最適化する。そのようにして、経路計画システムは、障害物を回避する変更経路を決定するために、ハンドル及びペダルの操作のような運転者の以前の行為によって示される現在経路を考慮する。結果として、事故のおそれを有する障害物が回避された後に、運転者が車両の制御を取り戻すとき、運転者は、運転者の以前の行為によって示される目的に更に従って、より容易に車両を制御することができる。
【００１８】
例えば、一実施形態は、変更経路内の車両の現在位置及び目標位置を維持しながら、車両のための現在経路を変更する。コスト関数を用いて変更経路を決定する幾つかの実施態様では、コスト関数は、車両の動きに関する制約を前提として最適化される。当該制約には、変更経路を、車両の現在位置において開始し、車両の目標位置において終了するように強制する制約を含む。
【００１９】
経路計画システムは、車両のための現在位置と、目標位置と、運転可能エリア、並びに障害物及び一般道路以外のような非運転可能エリアを含む周囲環境地図とを含む現在経路を受信し、非運転可能エリアを回避して、目標に到達する車両のための変更経路を与える（１０４）。その経路は、車両の位置、方位、並進速度、及び、回転速度のうちの１つ又はその組み合わせを含むことができる。その経路は、操舵、制動、スロットルのような、車両運動に影響を及ぼし、結果として、与えられた経路に一致する車両のための車両運動１０５を生成する車両コマンドを発動させるために車両制御システムによって使用される。
【００２０】
経路計画システム１０１は、機械に直結されるか、又は遠隔接続されるセンサー、ハードウェア又はソフトウェアから、車両運動についての情報１０９を受信する。情報１０９は車両の状態を含む。また、経路計画システムは、センサー１０６から、地図の形で表される環境についての情報も受信する。経路計画システム１０１は、車両経路の動き１０５を決定する車両経路１０４を選択するために車両情報及び環境情報を使用する。
【００２１】
図１Ｂは、本発明の一実施形態による、経路計画システム１０１の全体構造を示す。経路計画システム１０１は、経路計画システムのモジュールを実行するための少なくとも１つのプロセッサ１３０を含む。プロセッサ１３０は、地図１１２及び車両情報１１５を記憶するためのメモリ１２０に動作可能に接続される。幾つかの実施態様では、メモリ１２０は、ナビゲーションシステム１０３と共有される。本発明の幾つかの実施形態の目的は、制約１１４を前提として、調整可能なコスト関数１１５と、不確定要素を伴う機械のモデルとを用いて、経路１０４を決定することである。幾つかの実施形態では、車両及び環境に関する情報は、車両から受信した情報１０９及び検知１０８に基づいて更新される（１１６）。
【００２２】
本発明の幾つかの実施形態は、半自律車両の制御システムによって変更された経路は、運転者によって望まれる現在経路にできる限り近いままにする必要があるという認識に基づく。そのような場合、運転者が制御を取り戻すとき、制御システムの無効化行為によって運転者が方向を失う可能性は小さく、運転の目的により迅速に戻ることができる。そのため、本発明の異なる実施形態は、車両の運転者から受信した行為に応答して決定された車両の現在経路を考慮して、車両が障害物を回避するための経路を決定する。
【００２３】
図２は、本発明の種々の実施形態によって利用される幾つかの原理に従って決定された種々の変更経路の概略図を示す。運転可能エリア２５０は、障害物が存在しているエリア２５２を除く道路２５１を含む。運転者所望経路、すなわち、現在経路２５３は、車両の現在位置２５５から目標位置２５４に到達する基準経路と解釈することができるが、障害物の非運転可能エリア２５２を通り抜ける。したがって、経路計画システムは、障害物２５２を回避するが、現在経路２５３と同じ位置において開始及び終了する経路２５６及び２５７のような変更経路を生成する。
【００２４】
異なる実施形態では、変更経路は、現在経路から変更経路への偏差と、障害物を回避する変更経路の曲率、及び現在経路と変更経路との間の曲率の差のような、他の性能測定基準との組み合わせのバランスを取るコスト関数を最適化することによって決定される。ステアバイワイヤシステム（steer-by-wire system）を備える車両では、現在経路と変更経路との間の曲率の小さな差を保つことによって、ハンドルを変更することなく車両車輪を操舵し、そして最終的には、運転者が方向を失わないように、ハンドルと車両車輪とを位置合わせし直すことによって、変更経路を達成できるようにすることができる。例えば、変更経路２５７は、現在経路からの全体的な位置の差を最小化するが、障害物２５２の周囲において大きな曲率差を生成する。逆に、変更経路２５６は、現在経路から、全体的に大きな位置の差を有するが、現在経路２５２からの曲率差は小さい。コスト関数式は、最終的な変更経路として、２つの経路のうちのいずれが選択されるかを判断する。
【００２５】
図３は、本発明の幾つかの実施形態による、運転者によって望まれる現在経路を考慮することによって半自律車両を制御する方法のフローチャートを示す。その方法は、車両の運転者から受信した行為３０１に応答して、車両のための現在経路を決定する（３１０）。現在経路上で障害物が検出されるとき（３２０）、その方法は、障害物を回避する変更経路を生成するために、車両のための現在経路を変更する（３３０）。この方法では、変更することは、現在経路のコスト関数３０５を最適化することを含む。そのようにして、運転者によって望まれる現在経路を考慮に入れて、変更経路が生成される。
【００２６】
例えば、一実施形態では、コスト関数は、現在経路からの変更経路の偏差の関数である。それに加えて、又はその代わりに、コスト関数は、変更経路と現在経路との曲率の差、又は任意の他の性能測定基準の関数とすることができる。例えば、一実施形態では、変更することは、現在経路からの変更経路の偏差を低減するコスト関数を最適化することを含む。代替の実施形態では、変更することは、現在経路からの変更経路の偏差と、障害物を回避するための変更経路と現在経路との曲率差とのバランスを取るコスト関数を最適化することを含む。
【００２７】
幾つかの実施形態は、各経路が運転制約を満たしながら障害物を回避するような、車両の現在位置から目標位置までの１組の経路を決定し、現在経路と変更経路との間の差と、性能測定基準とに基づいて、その１組の経路から変更経路を選択する。変更経路が決定された後に、その方法は、変更経路に従って車両の動きを制御するために、運転者の行為を無効にする（３４０）。
【００２８】
本発明の幾つかの実施形態は、例えば、車両の現在経路上で障害物を検出するのに応答して、そのような計算がリアルタイムに実行されなければならないという認識に基づく。そのため、変更経路を決定するために必要とされる計算は効率的でなければならない。しかしながら、車両の動きに関する制約を前提としてコスト関数を最適化するために必要とされる最適化問題を解くのは一般に複雑である。
【００２９】
それゆえ、幾つかの実施形態は、変更経路を計算するために種々の段階を与える。第１の段階は粗く、現在位置と目標位置とを接続する、粗く分離したランダムにサンプリングされた点の異なる系列によって形成された、複数の粗い経路を最適化することを含む。それらの粗い経路の最適化は、計算の複雑度を緩和する。次の段階は、細かく分離したランダムにサンプリングされた点の異なる系列によって形成され、粗い経路に近接した、複数の細かい経路を最適化することによって、精緻化することを伴う。後続の段階は、細かい経路内の冗長点を除去して、枝刈り経路を得ることを伴う。最終段階は、枝刈り経路を平滑化することを伴う。そのようにして、変更経路を決定する方法の全体的な計算複雑度が緩和される。
【００３０】
図４Ａは、本発明の幾つかの実施形態による、変更経路を決定する多段階方法のフローチャートを示す。その方法は、現在位置と目標位置とを接続する、粗く分離した１組の点によって規定された粗い経路を決定する（４０１）。例えば、一実施形態は、１組の粗い経路を決定し、変更経路に関する制約を前提として、現在経路からの各粗い経路への偏差のコスト関数を最適化することによって、１組の粗い経路から、１つの粗い経路を選択する。
【００３１】
次に、その方法は、細かく分離した、すなわち、粗い分離より細かく分離した、選択された粗い経路に近い１組の点によって規定される精緻化経路を生成するために、粗い経路を精緻化する（４０２）。例えば、粗い経路の周囲のエリアのみを、この時点で高い密度のサンプル点を用いて探索することによって精緻化経路を生成することができる（４０２）。次に、その方法は、障害物を回避するために役に立つことなく、及び／又は必要とされることなくコスト関数の値を高める、精緻化経路の冗長点を除去し（４０３）、枝刈りされた精緻化経路の点を接続する軌道を平滑化して（４０４）、車両運転可能経路を生成する。例えば、精緻化経路から冗長点を除去して、カーブが少ない経路を得ることができ、適切な曲率関数を適用することによって、その経路が平滑化される。
【００３２】
粗い経路の生成
図４Ｂは、一実施形態による粗い経路の決定４０１を示す概略図を示す。その実施形態は、１つのツリー上のノードとして粗い経路の点を生成し、そのツリー上の１組の経路を形成する道筋を更新する。運転可能空間４３０内の現在のツリーが、車両の現在位置を表すルートノード４２０とともに示されており、リンク４１１のようなリンクによって接続されるノード４１０のようなノードを含む。また、そのツリーは、そのツリーが最終的に到達しなければならない目標位置４２１も含むことができる。
【００３３】
一実施形態では、運転可能空間内の新たな点４２２がランダムに選択され、距離指標によるツリー内の最も近いノード４２６が選択される。点４２２及びノード４２６から、例えば、４２６から所与の距離内にある４２６と４２２との間のライン上の点として、ツリーの新たなノード４２３が得られる。そのような距離は、粗い経路の場合に長く、細かい経路の場合に短い。ノード４２０と４２３とを接続する経路が他の取り得る経路よりコスト関数に照らして最適であるように、新たなノード４２３から、新たなノード４２３の近隣にあるツリー上のノード４２５までリンク４２９を追加することによって、新たなノード４２３がツリーに接続される。次に、ツリー上の既存のリンクが更新される。例えば、新たなノード４２３の近隣にあるノードを評価して、ルート４２０から新たなノード４２３を通り抜けるノードまでの、より良好な経路を決定する。例えば、ノード４２６の場合、古いリンク４２７を破棄することができ、新たなリンク４２４を追加することができる。
【００３４】
幾つかの実施形態は、大域的制約及び局所的制約を前提として、半自律運転のための現在位置から目標位置までの経路を決定する。ここで、大域的制約は経路特性に関して表され、局所的制約はノード特性に関して表される。大域的制約は障害物を回避するために用いられ、障害物は静的及び動的とすることができる。通常条件下で、車両は現在経路、例えば、運転者指示経路に沿って移動し、この経路の推定値はあらかじめわかっている。
【００３５】
種々の実施形態は現在経路を基準経路として使用する。しかしながら、そのような基準経路は障害物、例えば、停止している車両、又は路上の歩行者、又は車線障害物と衝突する可能性がある。これらの状況において、幾つかの実施形態は、運転者の動揺を抑えるために基準経路から過度に外れることなく、そのような障害物を回避するように車両に操縦を行わせる運転者の行為を無効にする。したがって、幾つかの実施形態によって生成される経路は、基準経路から開始し、必要とされる操縦を実行し、基準経路に戻る。
【００３６】
幾つかの実施形態では、局所的制約は、車両の運動学的、かつ動的な挙動に関連する。例えば、１つの運動学的制約は、操縦経路の曲率κが基準経路の基準曲率κｒｅｆに近い必要があることである。曲率が不連続であると横方向加速度及び速度に非常に大きな変動を引き起こす可能性があるので、動的制約に関して、幾つかの実施形態は曲率が連続していることを確実にする。そのため、経路計画システム１０１によって決定される経路は少なくとも二次微分可能である。幾つかの実施形態はＧ２連続経路を探索し、それは、経路の２つの連続した区間が、その接合点において同じ接線及び同じ曲率中心を有することを意味する。
【００３７】
一実施形態は、半自律車両の配置空間内の点をサンプリングする。例えば、それらの実施形態は、車両の位置及びその方位
【数１】
0006525332.tif 000002
をサンプリングする。ただし、
【数２】
0006525332.tif 000003
は運転可能空間からサンプリングされ、ψｒａｎｄは（−π、π］間で均一にサンプリングされる。２つの連続したウェイポイント間の方位変化は制限することができるので、方位をサンプリングすることは、経路の主な平滑性を達成するのに役立つ。また、一実施形態は、方位を用いて、精緻化経路から冗長点を除去する。
【００３８】
幾つかの実施形態では、サンプリングは２段階において行われる。第１の段階では、環境全体がサンプリングされ、環境全体を迅速に探索することによって粗い経路が生成される。粗い経路は、初期配置と目標配置との間の許容経路であるが、その性能は改善することができる。
【００３９】
車両の配置をｘ：（ｐｘ，ｐｙ，ψ）によって２Ｄ環境内で表す。ただし、ｐｘ及びｐｙは車両位置のｘ座標及びｙ座標であり、ψは車両の方位であり、ψ∈（−π、π］である。ｘ＝（ｐ，ψ）を用いて、１つの配置を表す。ただし、ｐ＝（ｐｘ，ｐｙ）は２Ｄ環境内の車両の位置である。車両の初期配置及び目標配置をそれぞれｘｉｎｉｔ及びｘｇｏａｌによって表す。有界の、接続された、配置状態空間をＸ∈Ｒ３によって表す。配置空間内の障害物領域及び無障害物領域をそれぞれＸｏｂｓ及びＸｆｒｅｅ：＝Ｘ＼Ｘｏｂｓによって表す。配置空間内の実現可能経路は
【数３】
0006525332.tif 000004
である。ただし、Ｓは経路の全長である。Ｘｆｒｅｅ内の全ての実現可能経路を
【数４】
0006525332.tif 000005
によって表す。２つの状態ｘ１とｘ２との間の経路を
【数５】
0006525332.tif 000006
によって表す。Ｘ内の基準経路をσｒｅｆによって表す。ただし、σｒｅｆはＸ内にあるが、必ずしもＸｆｒｅｅ内にはない。
【数６】
0006525332.tif 000007
を、全ての非自明無衝突経路に非負コストを割り当てるコスト関数とする。このコスト関数が加法的であると仮定する。すなわち、ｘ２がｘ１とｘ３とを接続する経路上にある点である場合には、
【数７】
0006525332.tif 000008
である。ツリー内の任意のノードｖ∈Ｖの場合に、ｘｒｏｏｔをルートとするツリーＴ＝（Ｖ，Ｅ）を考えると、Ｃｏｓｔ（ｖ）は、ツリーのルートからｖまでの経路の最適なコストである。すなわち、
【数８】
0006525332.tif 000009
である。
【００４０】
幾つかの実施形態は、以下の問題を解くことによって現在経路を変更した。有界の接続配置空間Ｘ、障害物領域Ｘｏｂｓ、初期状態ｘｉｎｉｔ∈Ｘｆｒｅｅ、目標状態ｘｇｏａｌ∈Ｘｆｒｅｅ、及び２回連続微分可能基準経路σｒｅｆを仮定して、
（ｉ）σ＊（０）＝ｘｉｎｉｔ及びσ＊（Ｓ）＝ｘｇｏａｌであり、かつ、
（ｉｉ）
【数９】
0006525332.tif 000010
であるような、Ｇ２連続経路
【数１０】
0006525332.tif 000011
を見つける。ただし、ｃ（σ）は、基準経路σｒｅｆとσとの間の曲率の差にペナルティを科すコスト関数である。そのような経路が存在しない場合には、一実施形態は失敗を報告する。
【００４１】
幾つかの実施形態は、関数
【数１１】
0006525332.tif 000012
を定義する。ただし、Ｘｆｒｅｅは運転可能空間であり、その関数は、運転可能空間４３０からの独立同一分布（i.i.d.：independent identically distributed）サンプルを返す。図４Ｂにおいて、関数ＳａｍｐｌｅＦｒｅｅは点４２２を返す。Ｖが１組のノードであり、Ｅが、それぞれ２つのノードを接合する１組のエッジ又はリンクであるツリーＴ＝（Ｖ，Ｅ）と、点ｘ∈Ｘｆｒｅｅとを仮定すると、関数
【数１２】
0006525332.tif 000013
は、コスト関数に関してｘに最も近いノードｖ∈Ｖを返し、すなわち、
【数１３】
0006525332.tif 000014
である。図４Ｂにおいて、関数Ｎｅａｒｅｓｔは点４２６を返す。
【００４２】
２つの点ｘ１，ｘ２∈Ｘｆｒｅｅ、正の実数η∈Ｒ＞０を仮定すると、関数
【数１４】
0006525332.tif 000015
は、ｘ３がｘ１とｘ２とを接続する最適な経路上の点であるようなｘ３∈Ｘｆｒｅｅを返す。本明細書において、ｘ３を
【数１５】
0006525332.tif 000016
として選択する。ただし、||・||はユークリッドノルムであり、ηは許される距離である。図４Ｂにおいて、関数Ｓｔｅｅｒは点４２３を返す。
【００４３】
ツリーＴ＝（Ｖ，Ｅ）、状態ｘ＝（ｐ，ψ）∈Ｘｆｒｅｅ及び正の実数η∈Ｒ＞０を仮定すると、関数
【数１６】
0006525332.tif 000017
は、ｘの近隣にあるＶ内のノードを返す。すなわち、
【数１７】
0006525332.tif 000018
である。次式のように、ツリー内のノードの数の関数としてｒを選択する。
【数１８】
0006525332.tif 000019
ただし、
【数１９】
0006525332.tif 000020
であり、μ（Ｘｆｒｅｅ）は自由空間の体積であり、ζｄはＲｄ内の単位球体の体積であり、｜Ｖ｜はＶ内のノードの数を表す。図４Ｂにおいて、関数Ｎｅａｒは点４２５、４２６を返す。
【００４４】
２つの状態ｘ１，ｘ２∈Ｘｆｒｅｅを仮定すると、ブール関数ＣｏｌｌｉｓｉｏｎＦｒｅｅ（ｘ１，ｘ２）は、ｘ１とｘ２との間の最適経路σ＊ｘ１，ｘ２が運転可能空間Ｘｆｒｅｅ４３０内に存在する場合には、真（Ｔｒｕｅ）を返し、そうでない場合には、偽（Ｆａｌｓｅ）を返す。
【００４５】
一実施形態は以下のようにノードのツリーを決定する。その実施形態は、唯一のノードとしてツリーのルートを設定し（Ｖ＝ｘｒｏｏｔ）、エッジがない、すなわち、
【数２０】
0006525332.tif 000021
と設定することによって初期化される。その後、運転可能空間においてランダム点４２２がサンプリングされ、ツリー内の最も近い点４２６が見つけられ、ツリー内の最も近いノードを、最も近いノードとランダム点との間のライン内にある新たなノードに向けることによって、新たなランダム点４２３が返される。新たなノードと最も近いノードとを接続するラインに沿った衝突がチェックされ、衝突が見つからない場合には、新たなノードがツリーに追加される。新たなノードは、ルートノードから既存のツリーノードに到達するためのコストと、既存のツリーノードから新たなノードに到達するコストとの和の最小値を与える既存のツリーノードに接続される。新たなノードに到達するコストは、（ルートノードから既存のノードに到達するコスト）＋（既存のノードから新たなノードに到達するコスト）として設定される。
【００４６】
新たなノードがツリーに追加される場合には、新たなノード付近のノード４２５、４２６を評価して、それらのノードが、新たなノードを通り抜けることによって最も低いコストを有する経路を用いて到達できるか否かをチェックする。そのため、その実施形態は、新たなノードに到達するためのコストを求め、新たなノードから任意の近いノードに到達するコストを加える。そのようなコストが、そのノードに現在関連付けられているコストより小さい場合には、新たなノードからそのノードへのリンクが追加され、新たなノードに到達する古い方のリンクが除去される。
【００４７】
コスト関数
２つの状態ｘ１＝（ｐ１，ψ１）及びｘ２＝（ｐ２，ψ２）があり、ただし、
【数２１】
0006525332.tif 000022
及び
【数２２】
0006525332.tif 000023
であると仮定して、一実施形態はコスト関数を
【数２３】
0006525332.tif 000024
のように選択する。ただし、ｗ１はユークリッド距離に関する重みであり、ｗ２は経路の平滑性に関する重みであり、ｗ３は計画経路と基準経路との曲率の差に関する重みであり、θ１はψ１とベクトル
【数２４】
0006525332.tif 000025
との間の角度である。具体的には、
【数２５】
0006525332.tif 000026
である。同様に、θ２はψ２とベクトル
【数２６】
0006525332.tif 000027
との間の角度であり、θ１，θ２∈［０，π］であり、κ（ｘ１）はノードｘ１における曲率であり、κｒｅｆ（ｘ１，σｒｅｆ）はｘ１における対応する基準曲率である。次に、ｘ１、ｘ２及びσｒｅｆを仮定して、κ（ｘ１）及びκｒｅｆ（ｘ１，σｒｅｆ）を計算する方法を示す。
【００４８】
コスト関数（１）内の計画経路の曲率は、複数の方法において計算することができる。例えば、平滑化手順がＧ２連続立方ベジエ螺旋（Ｇ２ＣＢＳ：G2 Continuous Cubic Bezier Spiral）を用いて、２つのエッジを接続する場合には、２つのエッジの接合ノードにおける曲率としてＧ２ＣＢＳ曲線の最大曲率を用いることができる。ノードｘ１の親がｘｐａｒｅｎｔ＝（ｐｐａｒｅｎｔ，ψｐａｒｅｎｔ）であるとき、ｘ１における曲率の大きさは
【数２７】
0006525332.tif 000028
によって与えられる。ただし、ｑ４は、
【数２８】
0006525332.tif 000029
を満たすパラメーターであり、
【数２９】
0006525332.tif 000030
であり、γはベクトル
【数３０】
0006525332.tif 000031
とベクトル
【数３１】
0006525332.tif 000032
との間の角度であり、
【数３２】
0006525332.tif 000033
であり、ＬはＢ０とｐ１との間の距離である。κ（ｘ１）の符号は、２Ｄ平面内のｐ１における単位接線ベクトルの回転方向によって決定される。具体的には、Ｂ０が曲線の始点であり、Ｅ０が曲線の終点であるとして、単位接線ベクトルが反時計回りに回転する場合には、κ（ｘ１）＞０である。代わりに、単位接線ベクトルが時計回りに回転する場合には、κ（ｘ１）＜０である。κ（ｘ１）の符号は、クロス乗積
【数３３】
0006525332.tif 000034
の方向を考慮することによって決定することができる。すなわち、
【数３４】
0006525332.tif 000035
であり、ただし、
【数３５】
0006525332.tif 000036
は、右手系を完成させる、
【数３６】
0006525332.tif 000037
及び
【数３７】
0006525332.tif 000038
の両方に対して垂直な単位ベクトルである。ここで、
【数３８】
0006525332.tif 000039
は、
【数３９】
0006525332.tif 000040
及び
【数４０】
0006525332.tif 000041
が同一直線上にあり、曲率がκ（ｘ１）＝０であることを意味する。
【００４９】
ｘ１、κｒｅｆ（ｘ１，σｒｅｆ）の場合の基準曲率の値は、ｘ１を基準経路σｒｅｆ上の点に射影し、そのような点の曲率を計算することによって計算される。例えば、最近点射影（Nearest point projection）を用いることができる。
【数４１】
0006525332.tif 000042
ただし、ｘｒｅｆ＝（ｐｒｅｆ，ψｒｅｆ）はσｒｅｆ上の任意の点である。
【００５０】
この射影の１つの利点は、ツリー内の各ノードの基準曲率を一度しか計算する必要がないことである。その後、計算された曲率はメモリに記憶され、多数の反復の計算効率を高めるために後の計算に再利用される。式（１）のコスト関数は、経路計画アルゴリズム全体を通して最近隣点を計算するためにも用いることができ、最近隣点は、コスト関数（１）を用いて各ノードからランダムノードまでのコストを計算することによって、かつランダムノードまでの最近隣点として最も低いコストを有するノードを設定することによって選択される。この結果として、コスト関数（１）の項の重みに応じて、異なる経路が生成される。例えば、計画経路と基準経路との間の曲率差に関する重みが大きい場合には、経路４５６が結果として生成された変更経路であり、一方、ユークリッド距離に関する重みが大きい場合には、経路４５７が選択される。
【００５１】
精緻化経路の生成
図５は、一実施形態による、ステップ４０１において決定された粗い経路５０１を精緻化すること（４０２）を示す概略図を示す。粗い経路５０１を仮定すると、その周囲のエリア５０３が検討され、そのようなエリアにおいて、より高いノード密度を有する精緻化経路５０２が得られる。例えば、一実施形態は、粗い経路５０１を中心にしたエリア５０５内に新たな点５０４をランダムに配置する。
【００５２】
精緻化段階中に、精緻化経路の近隣がサンプリングされる。図５に示されるように、点｛ｘｉｎｉｔ，ｘ１，ｘ２，．．．，ｘｎ，ｘｇｏａｌ｝を有する粗い経路５０１を仮定して、幾つかの実施形態は最初にこれらの点ｘｉから点５０４をランダムに選択し、その後、その実施形態は、選択された点を中心にした円柱５０５
【数４２】
0006525332.tif 000043
内を均一にサンプリングする。ここで、円柱の寸法は、ｒ’＝ηと選択することによって決定される。ただし、ηは経路計画システムの延長区間長であり、δは一般に
【数４３】
0006525332.tif 000044
内で選択される。
【００５３】
精緻化段階中、サンプリングは、粗い経路の周囲のエリアにおいてより高密度であり、そのエリアは、任意の新たな点４２３を、粗い経路生成中に以前に許された距離よりも、最も近いノード４２６から強制的に短い距離に置くことによって得られる。
【００５４】
幾つかの実施形態では、第１の段階では、第２の段階より多くの点が配置される。その理由は、探索エリアが小さく、それゆえ、少ないサンプルしか必要としない精緻化を行うために、第２の段階に入る前に環境全体を探索して十分に良好な粗い経路を見つけることが重要であるためである。
【００５５】
ノード／点の除去
幾つかの実施形態は新たなノードをランダムに配置するので、経路内に幾つかの不要な点が存在する可能性があり、それらの点が不要な方向転換を導入し、車両の動きによって運転者の快適性が低下する可能性がある。本発明の幾つかの実施形態によって、これらの点が除去される。
【００５６】
図６は、一実施形態による、精緻化経路５０２から冗長ノードを除去すること（４０３）を示す概略図を示す。不要な方向転換を引き起こすノード６０２、６０３、６０４が除去され、対応する切断されたリンクが除去され、除去されたノードの先行ノードと後続ノードとの間のリンク６０５、６０６、６０７が追加される。
【００５７】
精緻化経路の点をσ＝｛ｘ０，ｘ１，ｘ２，．．．，ｘｎ，ｘｎ＋１｝とする。ただし、ｘｉ＝（ｐｉ，ψｉ）、ｘ０＝ｘｉｎｉｔ及びｘｎ＋１＝ｘｇｏａｌである。枝刈り経路をσｐとし、
【数４４】
0006525332.tif 000045
と設定し、ｊ＝ｎ＋１とする。枝刈りは以下のように機能する。ｘｊをσｐに加える。ｘ０から開始して、
【数４５】
0006525332.tif 000046
が無衝突であるような、｛ｘ０，ｘ１，．．．，ｘｊ−１｝内の全ての点を見つけ、これらのウェイポイントをＸｃａｎｄｉｄａｔｅによって表す。全てのｘｋ∈Ｘｃａｎｄｉｄａｔｅに対して、（θｋ＋θｊ）を最小化する点を見つける。ただし、
【数４６】
0006525332.tif 000047
であり、
【数４７】
0006525332.tif 000048
である。その後、ｊ＝ｋとして、ｋ＝０までこのプロセスを繰り返す。
【００５８】
平滑化
不要な点が除去された後でも、その経路は依然として角部によって接合される区間を含む可能性がある。したがって、その経路は滑らかではなく、少なくとも２つの問題を引き起こす可能性がある。第一に、自動車のような数多くの車両は、滑らかな経路に従うことしかできないので、その経路に厳密に従うことはできない。また、経路を受信し、車両コマンドを発動させる制御システムは通常、滑らかな操作経路を必要とする。
【００５９】
図７は、一実施形態による、ノード除去４０３後の経路７０１の平滑化４０４を例示する概略図を示す。そのように平滑化しても、結果として、経路が依然としてノードを通り抜けるが、その経路は、鋭いエッジを有する一連のリンクではなく、滑らかな曲線を含む。
【００６０】
幾つかの実施形態では、経路７０１は、点間に滑らかな曲線７０２を当てはめることによって平滑化される。例えば、Ｇ２連続立方ベジエ螺旋（Ｇ２ＣＢＳ）を用いて、２つの連続した線分の間に連続曲率経路を生成することができる。ベジエ曲線は以下のように定義される。
【数４８】
0006525332.tif 000049
ただし、ｎはベジエ曲線の曲度であり、ｓ∈［０，１］であり、Ｐｉは制御点である。式（５）によって記述される曲線のための８つの制御点は以下の式によって与えられる。
【数４９】
0006525332.tif 000050
ただし、ｕ１はライン
【数５０】
0006525332.tif 000051
に沿った単位ベクトルであり、ｕ２はライン
【数５１】
0006525332.tif 000052
に沿った単位ベクトルであり、ｕｄはライン
【数５２】
0006525332.tif 000053
に沿った単位ベクトルであり、
【数５３】
0006525332.tif 000054
であり、
【数５４】
0006525332.tif 000055
であり、γはベクトル
【数５５】
0006525332.tif 000056
と    
【数５６】
0006525332.tif 000057
との間の角度である。パラメーターｑ１、ｑ２、ｑ３、ｑ４は
【数５７】
0006525332.tif 000058
である。
【００６１】
パラメーターＬはｐ１（ｘ１＝（ｐ１，ψ１））とＢ０との間の距離であり、ｐ１とＥ０との間の距離にも等しい。これに基づいて、Ｌは、以下のように選択することができる。
【数５８】
0006525332.tif 000059
【００６２】
式（７）において記述されるようにＬを選択する理由は、平滑化のために１つのエッジが２回使用されるので、２Ｌが
【数５９】
0006525332.tif 000060
以下であるためである。この平滑化技法を用いて、初期点と最終点との間のＧ２連続経路が得られる。その経路が初期点及び最終点においても連続しているように要求される場合には、ヨー方向において２つの更なるウェイポイントψｉｎｉｔ及びψｇｏａｌを追加することができ、その後、経路計画システムを用いて、これら２つの新たなウェイポイント間にある更なるウェイポイントを見つけることができる。平滑化は、更なる点を含む全ての点に適用することができ、初期点及び最終点においても滑らかであるＧ２連続経路を与える。
【００６３】
経路更新
本発明の幾つかの実施形態は、路上の変化に応じて、現在経路及び／又は変更経路を更新することができるという認識に基づく。例えば、車両の現在位置の変化に応答して、及び／又は新たな障害物を検出するか、若しくは以前に検出された障害物の挙動の変化を検出するのに応答して、変更経路を更に更新することができる。そのような状況において、幾つかの実施形態は、効率的に実行するために、変更経路を更新して、変更経路の少なくとも幾つかの部分を再利用する。
【００６４】
図８Ａは、本発明の幾つかの実施形態による、変更経路を更新する方法のフローチャートを示す。その方法は、非運転可能エリア内の変化を検出し（８１１）、新たな非運転可能エリアを通る経路を除去し（８１２）、新たな運転可能エリアを横切る新たな経路を生成する（８１３）。次に、その方法は、車両の現在位置及び目標位置を更新し、既存の経路と現在位置及び目標位置とを接続し、変更経路を更新する（８１４）。
【００６５】
図８Ｂは、障害物８０１のような非運転可能エリアが以前の位置８０２から移動した後の、本発明の一実施形態による変更経路の更新を示す概略図を示す。その動きに起因して、８０３のような特定のリンク及びノードがもはや有効ではなく、ツリーから除去される。これにより、リンク８０４が除去されるので、目標位置８０５への経路内に中断が生じる。しかしながら、障害物によって以前に占有されていたエリア８０２が、現時点で運転可能である。したがって、そのエリア内に更なるノードが追加され、目標位置への更新された経路を形成する新たなリンク、例えば、リンク８０６が決定される。
【００６６】
あらかじめ計画された経路が利用可能であるとき、経路計画システムは、その経路を用いて、計算回数を削減することができる。しかしながら、その経路は、例えば、１つ又は複数の障害物が除去されたことに起因する非運転可能エリア内の変化、及び車両位置の変化を考慮に入れるために調整を必要とする場合がある。
【００６７】
幾つかの実施形態では、新たな障害物が現れるか、又は障害物が位置を変更するとき、最初に、新たな障害物と衝突するエッジが特定される。障害物と交差するエッジごとに、対応する子終点ノードが無効とマーキングされる。次に、無効ノードを見つけるために、ｘｉｎｉｔからｘｇｏａｌまでの元の経路がチェックされる。その経路がそのようなノードを含まない場合には、その経路は変更されない。そのようなノードを含む場合には、ツリーが刈り込まれ、再生される。
【００６８】
刈込み（trimming）は、ツリーの各ノードをトラバース（traverse）し、無効ノードの全ての子を無効とマーキングすることによって実行することができる。その後、全ての無効ノードと、それらのノードを接続するエッジがツリーから除去される。ツリーが刈り込まれた後に、新たな経路を見つけるためにツリーが再生される。幾つかの実施形態は、運転可能から非運転可能に、及び非運転可能から運転可能に変化した、運転可能エリアの部分を包囲するエリアの近隣においてサンプルを生成する。このようにして、変更エリアに広がるツリーの新たなブランチを、より迅速に決定することができる。
【００６９】
さらに、幾つかの実施形態は、後方に向かって、すなわち、目標位置から現在位置に向かって経路を決定する。しかしながら、車両の現在位置が変更されるとき、実施形態は、目標位置に接近する変更経路の部分を維持しながら、選択経路を更新する。そのようにして、選択経路の一部を再利用することができ、それにより、計算複雑度が更に緩和される。
【００７０】
車両現在位置としてツリールートが選択される場合には、車両が移動すると、ルートが変化しているので、直ちにツリーを再計算する必要がある。代わりに、幾つかの実施形態は、ルートを目標位置に設定し、目標を現在車両位置に設定する。このようにして、そのツリーは目標位置を運転可能エリア内の任意の現在車両位置に接続しており、それは、車両位置が変化するときに、新たな経路が既に利用可能であることを意味する。中止判定基準に関しては、それらの実施形態は、ｘｉｎｉｔ＝（ｐｉｎｉｔ，ψｉｎｉｔ）を中心にした円柱内にランダムノードが配置され、ランダムノードをｘｉｎｉｔに接続する経路が無衝突であるとき、ｘｉｎｉｔとｘｇｏａｌとの間の１つの経路を返す。エリア５０４は、
【数６０】
0006525332.tif 000061
と定義することができる。ただし、Ψは、ψｉｎｉｔとその親の配置との間の最大ヨー差である。
【００７１】
本発明の上記の実施形態は、数多くの方法のいずれにおいても実現することができる。例えば、それらの実施形態は、ハードウェア、ソフトウェア又はその組み合わせを用いて実現することができる。ソフトウェアにおいて実現されるとき、そのソフトウェアコードは、単一のコンピューター内に設けられるにしても、複数のコンピューター間に分散されるにしても、任意の適切なプロセッサ、又はプロセッサの集合体において実行することができる。そのようなプロセッサは集積回路として実現することができ、集積回路構成要素内に１つ又は複数のプロセッサが含まれる。一方、プロセッサは、任意の適切な構成の回路を用いて実現することができる。
【００７２】
また、本明細書において概説される種々の方法又はプロセスは、種々のオペレーティングシステム又はプラットフォームのいずれか１つを利用する１つ又は複数のプロセッサ上で実行可能であるソフトウェアとしてコード化することができる。さらに、そのようなソフトウェアは、幾つかの適切なプログラミング言語及び／又はプログラミングツール若しくはスクリプト記述ツールのいずれかを用いて書くことができ、フレームワーク又は仮想機械上で実行される実行可能機械語コード又は中間コードとしてコンパイルすることもできる。通常、プログラムモジュールの機能は、種々の実施形態において望ましいように、組み合わせることもできるし、分散させることもできる。
【００７３】
また、本発明の実施形態は方法として具現することができ、その一例が提供されてきた。その方法の一部として実行される動作は、任意の適切な方法において順序化することができる。したがって、例示的な実施形態において順次の動作として示される場合であっても、例示されるのとは異なる順序において動作が実行される実施形態を構成することもでき、異なる順序は、幾つかの動作を同時に実行することを含むことができる。
【図１Ａ】
0006525332.tif 000062
【図１Ｂ】
0006525332.tif 000063
【図２】
0006525332.tif 000064
【図３】
0006525332.tif 000065
【図４Ａ】
0006525332.tif 000066
【図４Ｂ】
0006525332.tif 000067
【図５】
0006525332.tif 000068
【図６】
0006525332.tif 000069
【図７】
0006525332.tif 000070
【図８Ａ】
0006525332.tif 000071
【図８Ｂ】
0006525332.tif 000072