JP7450814B2 - 駐車スペースのハイブリッド探索を用いた自律駐車 - Google Patents

Description

本発明は、概して車両の経路計画に関し、より具体的には初期状態から目標空間への車両の移動を決定論的に制御する経路計画方法に関する。

自律車両または自律走行モードで動作する車両のいずれかである車両によって採用されるいくつかの制御システムは、他の車両または歩行者等の障害物を避けるためだけでなく、車両の動作に関連付けられたいくつかの判定基準を最適化するためにも、車両の将来の安全な動きまたは経路を予測する。目標状態は、固定位置、移動位置、速度ベクトル、領域、またはこれらの組み合わせであり得る。道路縁、歩行者、および他の車両等の周囲は、車両のセンサによって検知され、ならびに／または、事前に与えられた情報によって少なくとも一部が既知である。

経路計画は、自律車両、ロボット工学、製造、およびコンピュータビジョンアプリケーション等のさまざまな用途で生じる。数例を挙げると、グラフベースのＡ^＊およびＤ^＊法や、ナビゲーション機能およびポテンシャル場を伴う連続的アプローチや、さらに最近では、確率的ロードマップ（ＰＲＭ：probabilistic roadmap）、拡張空間ツリー、高速探索ランダムツリー（ＲＲＴ：rapidly-exploring random tree）、最適変形ＲＲＴ^＊および確率的ロードマップ（ＰＲＭ^＊）、随時ＲＲＴ、およびＡ検索ガイドツリー（ＡＧＴ：A-search guided tree）等のサンプリングベースの方法など、多くの計画アルゴリズムが提案されている。

自律走行モードで動作する自律または半自律車両を制御するためのタスクの１つは、本明細書では目標状態と呼ばれる駐車位置および向きに車両を自動的に駐車する。この駐車タスクは、以下のように定式化することができる。車両力学と、駐車スペースの地図と、車両の開始位置および向きを表す初期状態と、車両の目標駐車位置および向きを表す目標状態とが与えられると、初期状態から目標状態への車両の所望の経路または動きが決定され、次に、車両がこの所望の経路または動きに確実に従うように車両のアクチュエータ（たとえば車両のアクセルペダルおよびステアリングホイール）が制御される。しかしながら、車両の動きに対する非ホロノミック制約と、屋内駐車場または屋外駐車場等の駐車スペース内の典型的に狭い自由空間とが原因で、車両を自動的に駐車するための経路計画は困難である。

多種多様なグラフベースのアプローチは、ロボットの構成空間を近似する予め定められたグラフを検索する。このグラフは、各ノードおよびエッジが同じ構成および接続経路をそれぞれ表すという意味で、ある程度静的である。グラフのノードおよびエッジの双方は、検索アルゴリズムの探索中に、無衝突であるまたは占有されているとマークされる。Ａ^＊およびＤ^＊等の確立されたアルゴリズムは、ある状況下で最適性を保証した上で、解決完全性を達成する。予め定められたグラフの複雑さは、構成空間の次元とともに指数関数的に増大するので、これらのアプローチは高次元システムでは実際には実行不可能になる。

サンプリングベースの方法は、次元問題を克服し、高次元システムで普及する可能性がある。サンプリングベースの方法によれば、予め定められたグラフを使用するのではなく、グラフはオンザフライで構築され、そのノードは構成空間からランダムに引き出される。これらの方法の有効性は、グラフ構築（すなわち空間探索）の効率に掛かっている。

しかしながら、オリジナルのＲＲＴ法等のサンプリングベースの方法は、均一なサンプリング戦略のため、計算コストが高くなり得る。そのために、多くの研究では、バイアスされたサンプリング方式を利用して計算効率を高めている。たとえば、米国出願第１５／４０２，３５３号は、中間ではあるがマイルストーンである構成を表すウェイポイント（waypoint）のセットによってサンプリングをバイアスする。しかしながら、バイアスされたサンプリングでも、用途によっては計算上非実用的であり得る。加えて、サンプリングベースの方法が経路を見つけるのにかかる計算時間は確率的であり、これは実際には望ましくない。

最近の研究である米国出願第１６／１４０，５９１号は、自動駐車のための経路計画を解決するために、Ａ検索ガイドツリー（ＡＧＴ）法およびその変形である双方向ＡＧＴを教示している。そのツリー構築は、有名なＡ検索アルゴリズムと同様に、ヒューリスティックノードコストによって導かれる。一方、ヒューリスティックノードコストは、拡張すべきノードを選択すること（ノード選択ステップ）によってツリーの成長をバイアスするために利用される。多くの場合、ＢＩＡＧＴは、ノード選択を通してツリーの成長をバイアスすることに加えて、バイアスされたツリー拡張のおかげで、ノード拡張中にツリーの成長をバイアスすることによって従来のＡベースのアルゴリズムよりも計算的に優れている。ＢＩＡＧＴは、完全な地図情報を用いて短距離の自律車両駐車問題を解決するのには有効であるが、計画問題が長距離である場合、およびセンサから受信する更新された障害物情報によって当初計画された軌道が無効になる場合は、その性能は必然的に、長い計画時間および多数のノードの影響を受けてしまう。所与の長距離経路計画タスクについて得られるツリーの複雑さをさらに低減して、実用的なコンピューティングプラットフォームの限られたメモリおよび計算能力に自動駐車システムを適合させる必要がある。

加えて、自律駐車システムは、計画当初はその地図が完全には分かっていない環境に直面することが多い。これは、経路計画部によって生成される初期経路が実行不可能になる（当初は未知の障害物に衝突する）可能性があることを意味する。このため、初期経路／動きのリアルタイム調整が必要となる。リアルタイム軌道修復は、オンラインヒューリスティック更新、サンプリングベースの検索構造のプルーニングおよび再接続、ならびにスプラインベースのキノダイナミック検索を含む、さまざまな異なる側面からアプローチがなされてきた。ヒューリスティック更新方法は、グラフベースの構造のために設計されており、ツリーベースの検索構造には直接適用できず、ツリープルーニングおよび再接続方法は、メモリ制約のあるシナリオではあまり望ましくない。

いくつかの実施形態は、数例を挙げると、グラフベースのＡ^＊およびＤ^＊法や、ナビゲーション機能およびポテンシャル場を伴う連続的アプローチや、確率的ロードマップ（ＰＲＭ）、拡張空間ツリー、高速探索ランダムツリー（ＲＲＴ）、最適変形ＲＲＴ^＊および確率的ロードマップ（ＰＲＭ^＊）、随時ＲＲＴ、およびＡ検索ガイドツリー（ＡＧＴ）等のサンプリングベースの方法など、さまざまな計画アルゴリズムを自律駐車に使用することができるという認識に基づいている。これらの計画アルゴリズムはすべて、異なる計画アルゴリズムによって採用される異なる原理を用いて駐車スペースを探索するために使用することができる。たとえば、ＲＲＴ法は、サンプリングを使用して駐車スペースを探索するが、ＡＧＴは、予め定められた拡張プリミティブによって形成されたエッジでツリーを拡張する。

異なる計画アルゴリズムは、ある駐車シナリオでは有利であり、別の駐車シナリオでは準最適であり得る。しかしながら、これらの計画アルゴリズムはいずれも、次元問題に悩まされる。具体的には、計画アルゴリズムは、典型的には、大きな駐車スペースを検索するためのスケーリングが不十分である。実際に、個人のガレージの比較的小さい駐車スペースに対しては、異なる種類の計画アルゴリズムを効率的に使用することができる。しかしながら、駐車問題を多層駐車場またはスーパーマーケットの駐車場に拡張すると、ほとんどの動作計画部にとって探索問題が大きくなりすぎて、埋め込まれた先進運転支援システム（ＡＤＡＳ：advanced driver-assistance system）等のさまざまな制御システムのメモリ要件を満たすことができなくなる。このような問題は、本明細書では広範囲の動作計画問題と呼ばれる。

いくつかの実施形態は、計画アルゴリズムの単一の原理を、検索空間をまず粗い方法で探索した後により改良された方法で探索するハイブリッド原理に置き換えることによって、広範囲の動作計画問題に対処することができるという認識に基づいている。いくつかの実施形態は、粗い探索および改良された探索は目的が異なるため、粗い探索の原理は改良された探索の原理とは異なり得るという認識に基づいている。実際に、「広範囲の動作計画」は、（１）広範囲と（２）動作計画とを含む２つの問題に分割することができる。したがって、粗い探索の目的は、探索の結果がさらなる適合化なしでは動きに適さない場合でも、広範囲にわたって空間を検索することである。これに対して、改良された探索は、計画の広範囲の側面を気にする必要がなく、動作計画の部分に焦点を当てるべきである。

いくつかの実施形態は、粗い探索を、検索効率のよい方法で駐車スペース内のレイアウトおよび／または交通ネットワークを表す予め定められたデータ構造上で、より決定論的な方法で効率的に決定することができるという認識に基づいている。このような構造の一例は、エッジによって接続されたウェイポイントを含む有向グラフである。このようなデータ構造では、各ウェイポイントは、車両が有向グラフに沿って移動する位置および向きを規定し、各エッジは、有向グラフにおける１つのウェイポイントから次のウェイポイントまでの車両の無衝突経路を規定する。このような有向グラフは、運動学的に実行可能または実行不可能であり得る。実行不可能な有向グラフは、車両の幾何学的形状を使用するだけでより容易に規定することができるが、それでも有向グラフは計算効率のよい方法でかなりの深さまで検索できるため、広範囲にわたる検索空間の粗い探索における効率的なツールであり得る。

たとえば、いくつかの実施形態では、車両を駐車するために、または車両を初期状態から目標状態に移動させるために、粗い経路は、車両の初期状態に最も近い有向グラフ上の初期ウェイポイントを車両の目標状態に最も近い有向グラフ上の目標ウェイポイントに接続する、有向グラフ上の最短経路である。これに加えてまたはこれに代えて、粗い経路は、初期／目標ウェイポイント候補を絞り込むために導入される進行方向制約等の制約を受けて選択され得る。注目すべきことに、粗い探索の検索空間は有向グラフの空間に限定されるため、計算効率がよい。しかしながら、この効率には代償がある。たとえば、初期ウェイポイントの位置および向きは、車両の初期状態の位置および向きとは異なる場合がある。同様に、目標ウェイポイントの位置および向きは、車両の目標状態の位置および向きとは異なる場合がある。したがって、粗い経路は通常、車両の初期状態を目標状態に接続しない。加えて、有向グラフの構成によっては、粗い経路は運動学的に実行可能でないことがあり、さらに適合させなければ動作計画に使用することができない。しかしながら、有向グラフの予め定められた検索空間を通る粗い経路の構成は、たとえば５０メートル、１００メートル、またはさらには１０００メートルを超える広範囲にわたっても非常に効率的である。

粗い経路とは対照的に、改良された経路の目的は、動作計画問題の広範囲の部分の程度を無視して、実行可能な動作計画に焦点を当てることである。いくつかの実施形態は、改良された計画が、粗い経路のコストからの偏差が低減された（たとえば偏差が最小の）コストを有する実行可能な無衝突の動きのための動作計画として構築される場合、このような目的が達成され得るという認識に基づいている。この制約の下では、改良された経路は、３つの顕著な例外を除いて、広範囲探索を利用して粗い経路を追跡する傾向がある。第１の例外は、コスト効率の高い方法で車両の初期状態を粗い経路に接続する必要性であり、第２の例外は、コスト効率の高い方法で粗い経路を目標状態に接続する必要性であり、第３の例外は、粗い経路の動きが実行不可能な区間に対してコスト効率の高い修正を提供する必要性である。広範囲にわたって、粗い経路はほとんど実行可能であるので、改良された経路は、これらの比較的小さい例外に焦点を当てる。実際、ハイブリッド空間探索は、改良された動作計画のための新たな領域の探索を、車両の初期および目標状態の近傍と、いくつかの曲がり角および／または障害物回避の近傍とに低減する。この検索空間は通常、元の検索空間よりもはるかに小さく、計算およびメモリ能力が限られている埋め込みシステムによっても効率的に実行することができる。

したがって、一実施形態は、駐車スペース内に車両を駐車するための制御システムを開示し、制御システムは、少なくとも１つのプロセッサと、命令が格納されたメモリとを含み、命令は、少なくとも１つのプロセッサによって実行されると、制御システムに、車両の初期位置および初期向きを規定する駐車スペース内の車両の初期状態と、車両の目標位置および目標向きを規定する駐車スペース内の車両の目標状態と、駐車スペース内の交通ネットワークを表す有向グラフとを受信することを実行させ、有向グラフは、エッジによって接続されたウェイポイントを含み、各ウェイポイントは、車両が有向グラフに沿って移動する位置および向きを規定し、各エッジは、有向グラフにおける１つのウェイポイントから次のウェイポイントまでの車両の無衝突経路を規定し、制御システムにさらに、車両の初期状態に最も近い有向グラフ上の初期ウェイポイントを車両の目標状態に最も近い有向グラフ上の目標ウェイポイントに接続するウェイポイントの最短ルートによって規定される粗い経路を選択することと、車両の初期状態を規定する初期ノードと車両の目標状態を規定する目標ノードとを含む車両の異なる状態を規定する複数のノードを有するツリーを構築することとを実行させ、ツリー内のノードの各ペアは、衝突なしに接続されたノードの状態の間で車両を移動させる運動学的に実行可能な動きによって規定されるエッジで接続され、ツリーは、ウェイポイントのルートを規定する粗い経路のコストからの、ツリーの初期ノードを目標ノードに接続する改良された経路によって規定される動きのコストの偏差を低減するように構築され、制御システムにさらに、改良された経路に基づいて車両を制御することを実行させる。

いくつかの実施形態は、ハイブリッド検索探索は任意の動作計画問題に使用可能であるが、比較的大きい駐車スペースにわたって検索することによってその利点の少なくとも一部が得られるという認識に基づいている。このような駐車スペースは、交差部および曲がり角がある複雑なレイアウトを有し得る。このようなレイアウトは、いくつかのウェイポイントの位置の重複によって有向グラフに反映される。具体的には、駐車スペースの交差部は、車両の位置は同じであるが向きが異なる複数のウェイポイントによって表すことができる。

いくつかの実施形態は、粗い経路が、初期ウェイポイントと目標ウェイポイントとを接続する最短経路であり得るという理解に基づいている。たとえば、複数のウェイポイントは、ユークリッドの観点から車両の初期および／または目標状態に最も近いと見なすことができる。粗い経路を最短にすることで、この曖昧さを明確にすることができ、経路計画のメモリ要件も低減することができる。これに加えてまたはこれに代えて、初期ウェイポイントおよび目標ウェイポイントは、進行方向制約を受ける最短の粗い経路を提供するように選択され、進行方向制約において、車両の向きと初期ウェイポイントまたは目標ウェイポイントの向きとの差は特定の閾値未満であってもよい。

いくつかの実施形態は、有向グラフにおけるウェイポイントの密度が、改良された経路と粗い経路との間のコスト偏差の最小化に影響を及ぼし得るという理解に基づいている。たとえば、隣接するウェイポイント同士の間の距離を規定する十分な密度があれば、車両の初期状態から粗い経路へのよりコスト効率の高い接続は、粗い経路の初期ウェイポイントへの接続ではなく、後続のウェイポイントへの接続であり得る。そのために、いくつかの実施形態は、駐車スペース内の交入口、出口、交差部、および曲がり角を規定する駐車スペースのマイルストーンウェイポイントを捕捉した後、マイルストーンウェイポイント同士を接続する中間ウェイポイントをさらに追加することによって有向グラフの分解能を高める。いくつかの実現例では、有向グラフにおける隣接するウェイポイント同士の間の距離は閾値未満である。この閾値は、平均的な車両の幾何学的形状、駐車スペースの駐車場所の幾何学的形状、駐車スペースのレイアウト、およびこれらの組み合わせに基づいて規定することができる。

異なる実施形態は、粗い経路および／または改良された経路のコストの異なる尺度を使用する。たとえば、いくつかの実施形態は、経路のコストの尺度として経路の長さを使用する。これらの実施形態は、より容易に実現することができ、経路のコストに関するユーザの直感と合致する。しかしながら、燃費および経路の滑らかさ等の、経路のコストの代替尺度を使用することができる。なお、改良された経路のコストは、車両力学の制約により、および／または、改良された経路が車両の初期および目標状態と粗い経路との間の隙間を埋める必要があるため、粗い経路のコストよりもほぼ常に大きい。しかしながら、いくつかの実施形態の目的は、コース経路のコストと同じコストを有する改良された経路を見つけることではない。たとえば、いくつかの実施形態の目的の１つは、改良された経路の異なる可能性のコストに関して、粗い経路のコストからのコストの偏差が最小となる改良された経路を見つけることである。

いくつかの実施形態は、異なる種類の動作計画アルゴリズムを、改良された経路を規定するツリーを構築するように適合させることができるという認識に基づいている。このような適合化は、最適化原理および構築の原理に基づき得る。たとえば、いくつかの実施形態は、Ａ検索ガイドツリー（ＡＧＴ）法および／または双方向ＡＧＴ（ＢＩＡＧＴ）を、粗い経路から逸脱しないようにバイアスされるように適合させる。これらの方法を、限られたメモリリソースにより適したツリー構築原理に基づく、バイアスされた検索探索に適合させることができる。

たとえば、ＡＧＴ法およびＢＩＡＧＴ法はともに、プリミティブ動作の予め定められたセットから選択されたエッジでツリーを消費する。このような選択を、追跡が実行不可能でない限り粗い経路をたどる（すなわち追跡する）ように、および／または、コスト偏差を低減するこのようなプリミティブ動作を選択するように、適合させることができる。

異なる実施形態は、動作計画に適した改良された経路を規定するツリーを構築するために異なる技術を使用する。たとえば、いくつかの実施形態は、プリミティブ動作の予め定められたセットから選択された運動学的に実行可能な動きを規定するエッジでツリーを拡大する。このように、コスト偏差を低減する経路の運動学的に実行可能な区間を探す問題は、コスト偏差を低減するためにセットから動作プリミティブを選択する問題になる。

異なる実施形態は、このような動作プリミティブを選択するために異なる技術を使用する。たとえば、一実施形態は、異なるツリー拡張モードを規定し、異なるモードに対して異なる種類の動作プリミティブを選択してコスト偏差を低減することができる。モードは、現在のノードによって規定される現在の状態と目標状態との間の距離と、現在の状態における車両を取り囲む無衝突空間のレイアウトとに基づいて選択することができる。これは、車両の自由度が高いほど、車両が粗い経路を追跡する選択肢がより良好になるからである。逆に、車両を取り囲む空間が狭い場合は、車両の自由度を高めて異なる動作プリミティブをテストすべきである。

いくつかの実施形態は、ナビゲーションモード、通常モード、および縦列駐車モード等のモードを使用する。たとえば、初期ツリーノード（ｔノード）が前方および後方の自由空間が狭い車両状態を表す場合、ツリーは縦列駐車モードで開始し、初期ｔノードが、開けた車道における、（その目標ｔノードによって指定される）駐車場所から特定の距離だけ離れた車両状態を表す場合、ツリーはナビゲーションモードで開始し、それ以外の場合、ツリーは通常モードで開始する。ｔノード拡張のために選択されたツリーのｔノードの前方および後方空間が比較的大きい場合、縦列駐車モードから通常モードへの移行が行われ、ｔノード拡張のために選択されたツリーのｔノードが同じツリーの目標ｔノードの特定の近傍に入る場合、ナビゲーションモードから通常モードへの移行が行われる。

これに加えてまたはこれに代えて、いくつかの実施形態は、車両の目標または初期状態に関してではなく、粗い経路の目標または初期ウェイポイントに関して求められたノードのコストに基づいて、ツリーのノードを選択して消費する。粗い経路は、初期ウェイポイントと目標ウェイポイントとを接続する最短経路を表すので、このような接続にかかるコストが最も小さくなる。したがって、粗い経路のコストが目標または初期ウェイポイントに関して表される場合、ツリーのノードは、可能であれば粗い経路上で選択され、またはより小さい偏差で粗い経路に戻るように選択される。

このように、粗い経路のウェイポイントは、ＲＰ－ＢＩＡＧＴと呼ばれるいくつかの実施形態の開示されている動作計画において以下の３つの役割を果たす。１）ウェイポイントから対応する目的地までの経路長は、ＢＩＡＧＴ検索を誘導するヒューリスティックとして使用され、２）駐車スペースに最も近いウェイポイントの進行方向は、正しい方向に出るようにＢＩＡＧＴを誘導し、３）車道の直線区間上のウェイポイントの進行方向は、不要なステアリング動作を避けるとともに、実行可能な軌道を見つけるのに必要なノードの数を減らすために、ＢＩＡＧＴが利用可能な動作プリミティブをトリミングするために利用される。

広範囲の動作計画問題におけるメモリ負担に対処することに加えて、いくつかの実施形態は、車両の駐車経路が十分に長く、公共空間で起こる場合、考慮していない新たな障害物が現れて改良された経路をたどることを妨げる可能性があるという理解に基づいている。このような状況では、改良された経路を修復または再計画すべきである。いくつかの実施形態は、改良された経路を修復することができるか、新たな改良された経路を最初から決定しなければならないか、を判断するためのテストが必要であるという別の認識に基づいている。

いくつかの実施形態は、無衝突空間を無視して障害物を避けるクリアランスがこのようなテストとして使用可能であるという認識に基づいている。クリアランスが小さすぎる場合は、動作計画をやり直すべきである。クリアランスが十分大きい場合は、動作計画を修復することができる。クリアランスは、たとえば車両の幾何学的なシフトによって求めることができる。たとえば、いくつかの実施形態は、クリアランスをクリアランス閾値と比較して修復判断を行う。これに加えてまたはこれに代えて、いくつかの実施形態は、クリアランスを評価して修復判断を出力するように関数を訓練した。

このように、いくつかの実施形態は、階層的なフレームワークを採用して修復および再計画手順を遂行し、このフレームワークでは、まず幾何学的なシフトが行われて無衝突ウェイポイントのセットが特定された後、シフト結果からのヒューリスティック誘導を用いて動的に実行可能な軌道が得られる。これは、計算効率を高めるだけでなく、軌道修復か再計画かを判断するための便利な判定基準も提供する。

修復困難さテストのための最大クリアランスを提供することに加えて、これらの幾何学的なシフトは、修復計画部のヒューリスティック誘導にも役立つ。修復判定基準が満たされると、元のルート計画部ウェイポイントの代わりに、各衝突ノードに対する無衝突シフトの中央値を、ヒューリスティック計算のために用いることができる。

いくつかの実施形態は、（すべての制御アクションを適用することによって）ノードを全方向に拡張することは不利であり、拡張すると、均一なサンプリングを使用する確率的アプローチと同様の欠点があることを認識している。本発明は、車両の周囲に応じて選択的に拡張を行うことによるツリー構築方法をさらに提示する。具体的には、車両の前端および後端の自由空間が狭い場合、ツリーは縦列駐車モードで成長し、車両が駐車場所から離れている場合、ツリーはナビゲーションモードで実行され、それ以外の場合、ツリーは通常駐車モードで実行される。ツリーは異なるモードで実行されることから、ノード拡張時に動作プリミティブの異なるセットが使用される。この方式では、通常モードと比較して、ナビゲーションモードでは動作プリミティブの数が少なくなり、縦列駐車モードでは動作プリミティブの数が多くなる。これにより、典型的には、ツリーの複雑さが低減され、すなわちノードおよびエッジの数が少なくなる。

いくつかの実施形態によって対処される駐車シナリオの一例を示す図である。 いくつかの実施形態に係る車両の幾何学的表現の例示的な概略図である。 図１Ａの駐車シナリオに対応する駐車スペースの地図を示す図である。 一実施形態に係る自動駐車システムの機能図である。 一実施形態に係る動作計画システムの概略構造を示す図である。 いくつかの実施形態に係る、駐車スペース内に車両を駐車する方法のブロック図である。 いくつかの実施形態に係る、ＲＰ－ＢＩＡＧＴの例示的な実現例の概略図である。 一実施形態に係る、ルート計画部によって実行される方法のブロック図である。 いくつかの実施形態に係る、例示的な駐車スペースのレイアウトと、有向グラフを用いて例示的な駐車スペースの交通ネットワークを表す概略図とを示す図である。 いくつかの実施形態に係る、図５Ａの有向グラフを改良して密な有向グラフを生成する概略図である。 いくつかの実施形態に係る、密な有向グラフに基づいて粗い経路を決定するルート計画部の概略図である。 いくつかの実施形態に係る、ヒューリスティック誘導および動作プリミティブトリミングを示す図である。 いくつかの実施形態に係る例示的な駐車シナリオの概略図である。 一実施形態に係るノードのコストを求める方法の概略図である。 いくつかの実施形態に係る、制御アクションＡおよび積分時間Ｔのセットに関連付けられたプリミティブ動作を示す概略図である。 いくつかの実施形態に係るモードＲＰ－ＢＩＡＧＴの移行の図である。 いくつかの実施形態に係る、修復を検討する経路計画方法のブロック図である。 一実施形態に係る、クリアランスチェックを行う方法のフローチャートを示す図である。 いくつかの実施形態に係る、クリアランスチェックの例示的なシナリオを示す概略図である。 いくつかの実施形態に係る、クリアランスチェックで求められた最大クリアランスの一例を示す図である。 いくつかの実施形態に係る、クリアランスチェック手順の結果に基づいて初期経路を修復する困難さを評価する方法のブロック図である。 いくつかの実施形態に係る、訓練データに基づいて評価関数の表現を得る手順の概略図である。 一実施形態に係るシステムの概略図である。 いくつかの実施形態に係る自動駐車システムのブロック図である。

いくつかの実施形態の目的は、自動車両制御のためのリアルタイム経路生成を可能にする経路および／または動作計画方法を開示することである。いくつかの実施形態の別の目的は、たとえば自動駐車システムおよび方法に適した、リアルタイム自動車両制御を可能にする動作計画方法を開示することである。明確にすることのみを目的として、いくつかの実施形態は自動駐車シナリオに関連して説明される。しかしながら、駐車シナリオに関連して説明される原理は、他の自動車両制御用途における代替の実施形態によって使用される。

図１Ａは、いくつかの実施形態によって対処される駐車シナリオの一例を示す。この例では、駐車スペース１５０の境界は、サイズがＬ×Ｈの矩形で表されている。車両１００は、初期状態１０１を有し、目標状態１０２によって規定される駐車場所に駐車する必要がある。各状態（たとえば初期および目標状態）は、車両の位置および向きを規定する。本発明では、開始状態が車道上で目標状態が駐車場所の中であるタスク（入庫タスクと呼ぶ）と、開始状態が駐車場所の中で目標状態が車道上であるタスク（出庫タスクと呼ぶ）の２つの異なる種類のタスクを想定することができる。駐車スペース１５０は障害物を含む。障害物は、駐車スペースのレイアウトの一部、すなわち駐車スペースの壁および／または柱等の恒久的な障害物１０３、ならびに道路上の他の車両等の一時的な障害物１０４であり得る。恒久的な障害物の寸法は、通常は既知であるか、または妥当な程度まで推定することができる。図１Ａは、このような寸法の非限定的な例を示す。

これに加えてまたはこれに代えて、障害物は、他の駐車車両または移動車両１０４を含み得る。車両１０４および駐車すべき車両１００の幾何学的寸法は、車両のタイプに基づいて求めることができる。明確にするために、本開示は前輪駆動車両を想定しているが、異なる実施形態は後輪駆動車両および全輪駆動車両を含む他の車両に適用される。

いくつかの実施形態では、駐車スペースのレイアウトおよび障害物１０３の位置は、駐車スペースの地図上で指定される。このような地図は、駐車タスクの前に予め求めておくこともできるし、駐車タスク中にリアルタイムで構築することもできる。さまざまな実施形態は、実行可能で無衝突の、初期状態と目標状態とを接続する運動学的経路１０５を決定する。

図１Ｂは、いくつかの実施形態に係る車両の幾何学的表現の例示的な概略図を示す。この例では、車両は矩形１１５として抽象化されている。車両状態は、その後輪軸の中点を表す位置（ｘ，ｙ）１１０と、車体軸と水平軸との間の角度を示す向きθ１２０とを含む。向きは、車体軸の矢印１２０が指し示す車両の進行方向を規定する。いくつかの実施形態では、車両の位置と向きの組み合わせは、車両の構成と呼ばれる。

いくつかの実施形態では、環境地図は、駐車スペースのレイアウトに関する情報、ならびに障害物の位置および幾何学的特性に関する情報を含む。自動車両制御システムは、駐車スペースのレイアウトまたは一部の障害物の位置を完全には把握していない場合がある。しかしながら、自動車両制御システムは、駐車スペースのレイアウトおよび障害物に関する更新情報を車載マッピングおよびローカライゼーションシステムから継続的に受信する。

このように、運動学的モデルおよび動的モデルの両方に異なる実施形態を適用することができる。一般性を失うことなく、以下の説明は運動学的モデルに基づいて展開される。

いくつかの実施形態は、駐車スペースの地図とも呼ばれる駐車スペースの幾何学的表現に基づいて、車両状態に衝突がないか否かを判定する。いくつかの実施形態では、駐車スペースの地図は、駐車スペースのすべての障害物および境界を単純な幾何学的形状として近似することによって導出される。一実施形態では、環境（すなわち駐車スペース）内の障害物１０３は、障害物ごとに最小の境界ボックスを構築することによって導出される矩形として近似することができる。駐車スペースの障害物および境界を幾何学的に近似することにより、駐車スペースまたは環境を幾何学的オブジェクトのリストによって完全に記述することができる。別の実施形態では、駐車スペースの地図は、占有または非占有としてマークされたグリッドのグループとして表される。どちらの地図の表現も、ある構成の車両が無衝突であるか否かを判定するために利用することができる。

図１Ｃは、図１Ａの駐車シナリオに対応する駐車スペースの地図１３０を示す。両方の種類の障害物が、矩形表現を用いて１種類の恒久的な障害物１０３として統合されている。障害物は、経路計画のための搭載プロセッサの計算能力に応じて、複数の幾何学的形状によって近似することができる。たとえば、別の実施形態では、計算能力が十分でない場合は、障害物ごとに境界円を構築することによって障害物を近似することができる。これに代えて、障害物をポリトープによって近似することもできるが、経路計画における計算負荷が増大する場合がある。また、駐車スペース内の障害物は、同じ幾何学的形状によって近似されない場合がある。いくつかの実施形態では、環境地図はセルのグループに分解され、セルの各々は、ある体積を有する構成およびその近傍に対応する。セルは異なる体積を有し得る。駐車スペース内の障害物は、セルのグループによって表すことができる。

図２Ａは、一実施形態に係る自動駐車システムの機能図を示す。環境マッピングおよびローカライゼーションブロック２０１は、駐車スペースの地図を構築または更新し、環境および車両動作状態を検知することによって車両の現在位置を求める。たとえば、３軸加速度計、３軸ジャイロスコープ、および／または磁力計を含み得る慣性測定ユニット（ＩＭＵ）を用いて、車両動作を検知することができる。全地球測位システムセンサを用いて、車両の位置および速度を提供することができる。環境２００を検知するセンサは、他の車両、歩行者、および建物を含む障害物を捕捉するビデオカメラ、車両と障害物との間の距離を検出する超音波／レーダおよびＬｉＤＡＲセンサ等であり得る。一実施形態では、環境地図がさらに処理されて、図１Ｃに示すような駐車スペースの幾何学的表現が生成される。

目標状態選択ブロック２０２は、駐車場候補を特定することによって車両を駐車する駐車場所の目標状態を選択し、目標状態を動作計画ブロック２０３に与える。一実施形態では、利用可能な駐車場所は、駐車場の管理に関連付けられた別個のシステムによって追跡される。これに加えてまたはこれに代えて、駐車場所は、自動駐車システムのセンサ２０３を用いて検出することができる。一実施形態では、動作計画ブロックは、目標状態が駐車可能であるか否か、すなわち駐車場所への実行可能な経路があるか否かを判定するためのチェックを行い、目標状態選択ブロック２０２にチェック結果を通知する。目標状態が駐車可能でない場合、目標選択ブロック２０２は、評価のための別の目標状態を選択する。別の実施形態では、目標状態選択ブロック２０２も目標状態が駐車可能であるか否かを評価することでき、駐車可能な目標状態を動作計画ブロックに送信するのみである。

目標状態が駐車可能である場合は、動作計画２０３は、車両モデル２１０と、車両の初期および目標状態と、駐車スペースの地図とに基づいて、基準軌道２４１を決定する完全な動作計画手順を開始する。一実施形態では、基準軌道は、車両速度およびステアリング角度のプロファイルを時間の経過とともに規定する。別の実施形態では、基準軌道は、車両状態（ｘ，ｙ，θ）ならびに車両の縦方向速度ｖおよびステアリング角度のプロファイルを時間の経過とともに規定する。ほとんどの場合、基準軌道のさまざまな形態は同等であり、車両モデルを用いて互いを決定することができる。

基準軌道２４１が与えられると、車両コントローラおよびアクチュエータ２０４は、車両状態にその基準軌道２４１を追跡させるための制御コマンドを決定して実行する。一実施形態では、制御コマンドは、アクセルペダル圧力および／またはステアリングトルクであり得る。車両コントローラ／アクチュエータは、信号２４３および２４４を用いて制御コマンドを決定してもよい。信号２４３は、測定されたステアリング角度、またはステアリングホイールもしくはアクセルペダルを動かすモータの測定された電流、ＩＭＵの出力、およびローカライゼーションブロック２０１によって推定された車両の現在の状態であり得る。

図２Ｂは、一実施形態に係る動作計画システム２０３の概略構造を示す。動作計画システム２０３は、動作計画システム２０３のモジュールを実行するための少なくとも１つのプロセッサ２７０を含む。プロセッサ２７０は、車両の幾何学的形状および駐車スペースの地図等の幾何学的情報２８１を格納するメモリ２８０に接続される（２７１）。メモリ２８０は、車両の運動学的モデルおよび車両の動的モデル等の車両のモデル２８２も格納することができる。メモリ２８０は、動作計画部の内部情報２８３も格納することができ、内部情報２８３は、車両の初期状態、駐車車両の目標状態、コスト関数、計算された各状態の値、各状態に至る動き、駐車スペースの接続性および許可された交通流情報を捕捉する交通ネットワークを表す有向グラフ、幾何学的グラフ、運動学的グラフ、ウェイポイント、基準軌道、予め選択された制御アクション、予め計算された動き軌道等を含むがこれらに限定されない。いくつかの実施形態では、メモリ２８０は、自動駐車のための方法を実現する格納された命令を含むことができ、この命令は、プロセッサ２７０によって実行されると、この方法の少なくともいくつかのステップを実施する。

いくつかの実施形態は、数例を挙げると、グラフベースのＡ^＊およびＤ^＊法や、ナビゲーション機能およびポテンシャル場を伴う連続的アプローチや、確率的ロードマップ（ＰＲＭ）、拡張空間ツリー、高速探索ランダムツリー（ＲＲＴ）、最適変形ＲＲＴ^＊および確率的ロードマップ（ＰＲＭ^＊）、随時ＲＲＴ、およびＡ検索ガイドツリー（ＡＧＴ）等のサンプリングベースの方法など、さまざまな計画アルゴリズムを自律駐車に使用することができるという認識に基づいている。これらの計画アルゴリズムはすべて、異なる計画アルゴリズムによって採用される異なる原理を用いて駐車スペースを探索するために使用することができる。たとえば、ＲＲＴ法は、サンプリングを使用して駐車スペースを探索するが、ＡＧＴは、予め定められた拡張プリミティブによって形成されたエッジでツリーを拡張する。

異なる計画アルゴリズムは、ある駐車シナリオでは有利であり、別の駐車シナリオでは準最適であり得る。しかしながら、これらの計画アルゴリズムはいずれも、次元問題に悩まされる。具体的には、計画アルゴリズムは、典型的には、大きな駐車スペースを検索するためのスケーリングが不十分である。実際に、個人のガレージの比較的小さい駐車スペースに対しては、異なる種類の計画アルゴリズムを効率的に使用することができる。しかしながら、駐車問題を多層駐車場またはスーパーマーケットの駐車場に拡張すると、ほとんどの動作計画部にとって探索問題が大きくなりすぎて、埋め込まれた先進運転支援システム（ＡＤＡＳ）等のさまざまな制御システムのメモリ要件を満たすことができなくなる。このような問題は、本明細書では広範囲の動作計画問題と呼ばれる。

いくつかの実施形態は、計画アルゴリズムの単一の原理を、検索空間をまず粗い方法で探索した後により改良された方法で探索するハイブリッド原理に置き換えることによって、広範囲の動作計画問題に対処することができるという認識に基づいている。いくつかの実施形態は、粗い探索および改良された探索は目的が異なるため、粗い探索の原理は改良された探索の原理とは異なり得るという認識に基づいている。実際に、「広範囲の動作計画問題」は、（１）広範囲と（２）動作計画とを含む２つの問題に分割することができる。したがって、粗い探索の目的は、探索の結果がさらなる適合化なしでは動きに適さない場合でも、広範囲にわたって空間を検索することである。これに対して、改良された探索は、計画の広範囲の側面を気にする必要がなく、動作計画の部分に焦点を当てるべきである。

いくつかの実施形態は、粗い探索を、検索効率のよい方法で駐車スペース内のレイアウトおよび／または交通ネットワークを表す予め定められたデータ構造上で、より決定論的な方法で効率的に決定することができるという認識に基づいている。このような構造の一例は、エッジによって接続されたウェイポイントを含む有向グラフである。このようなデータ構造では、各ウェイポイントは、車両が有向グラフに沿って移動する位置および向きを規定し、各エッジは、有向グラフにおける１つのウェイポイントから次のウェイポイントまでの車両の無衝突経路を規定する。このような有向グラフは、運動学的に実行可能または実行不可能であり得る。実行不可能な有向グラフは、車両の幾何学的形状を使用するだけでより容易に規定することができるが、それでも有向グラフは計算効率のよい方法でかなりの深さまで検索できるため、広範囲にわたる検索空間の粗い探索における効率的なツールであり得る。

たとえば、いくつかの実施形態では、車両を駐車するために、または車両を初期状態から目標状態に移動させるために、粗い経路は、車両の初期状態に最も近い有向グラフ上の初期ウェイポイントを車両の目標状態に最も近い有向グラフ上の目標ウェイポイントに接続する、有向グラフ上の最短経路である。これに加えてまたはこれに代えて、粗い経路は、初期／目標ウェイポイント候補を絞り込むために導入される進行方向制約等の制約を受けて選択され得る。注目すべきことに、粗い探索の検索空間は有向グラフの空間に限定されるため、計算効率がよい。しかしながら、この効率には代償がある。たとえば、初期ウェイポイントの位置および向きは、車両の初期状態の位置および向きとは異なる場合がある。同様に、目標ウェイポイントの位置および向きは、車両の目標状態の位置および向きとは異なる場合がある。したがって、粗い経路は通常、車両の初期状態を目標状態に接続しない。加えて、有向グラフの構成によっては、粗い経路は運動学的に実行可能でないことがあり、さらに適合させなければ動作計画に使用することができない。しかしながら、有向グラフの予め定められた検索空間を通る粗い経路の構成は、たとえば５０メートル、１００メートル、またはさらには１０００メートルを超える広範囲にわたっても非常に効率的である。

粗い経路とは対照的に、改良された経路の目的は、動作計画問題の広範囲の部分の程度を無視して、実行可能な動作計画に焦点を当てることである。いくつかの実施形態は、改良された計画が、粗い経路のコストからの偏差が低減された（たとえば偏差が最小の）コストを有する実行可能な無衝突の動きのための動作計画として構築される場合、このような目的が達成され得るという認識に基づいている。この制約の下では、改良された経路は、３つの顕著な例外を除いて、広範囲探索を利用して粗い経路を追跡する傾向がある。第１の例外は、コスト効率の高い方法で車両の初期状態を粗い経路に接続する必要性であり、第２の例外は、コスト効率の高い方法で粗い経路を目標状態に接続する必要性であり、第３の例外は、粗い経路の動きが実行不可能な区間に対してコスト効率の高い修正を提供する必要性である。広範囲にわたって、粗い経路はほとんど実行可能であるので、改良された経路は、これらの比較的小さい例外に焦点を当てる。実際、ハイブリッド空間探索は、改良された動作計画のための新たな領域の探索を、車両の初期および目標状態の近傍と、いくつかの曲がり角および／または障害物回避の近傍とに低減する。この検索空間は通常、元の検索空間よりもはるかに小さく、計算およびメモリ能力が限られている埋め込みシステムによっても効率的に探索することができる。

図３は、いくつかの実施形態に係る、駐車スペース内に車両を駐車する方法３００のブロック図を示す。この方法は、プロセッサ２７０等の少なくとも１つのプロセッサと、少なくとも１つのプロセッサによって実行されるとこの方法のステップを実施する命令が格納されたメモリ２８０等のメモリとを使用するシステム２９９によって実現することができる。

これに加えてまたはこれに代えて、方法３００は、車両から離れた駐車管理システムによって実現することができる。これらの実施形態では、駐車管理システムは動作計画を決定し、動作計画を実行するために車両に与える。

したがって、方法３００は、車両の初期位置および初期向きを規定する駐車スペース内の車両の初期状態１０１と、車両の目標位置および目標向きを規定する駐車スペース内の車両の目標状態１０２とを受信すること（３１０）を含む。駐車スペース内の交通ネットワークを表す有向グラフ３１５を受信する（３１０）。有向グラフは、エッジによって接続されたウェイポイントを含み、各ウェイポイントは、車両が有向グラフに沿って移動する位置および向きを規定し、各エッジは、有向グラフにおける１つのウェイポイントから次のウェイポイントまでの車両の無衝突経路を規定する。

受信された情報は、車両のモジュールによって生成することができ、および／または無線通信チャネルを介して受信することができる。たとえば、車両の位置および向きなどを含む車両の初期状態は、車両のナビゲーションシステムから受信することができる。これに加えてまたはこれに代えて、車両の初期状態は、無線通信チャネルを介して駐車管理システムから受信することができる。目標状態および有向グラフも駐車管理システムから受信することができる。

方法３００は、車両の初期状態に最も近い有向グラフ上の初期ウェイポイントを車両の目標状態に最も近い有向グラフ上の目標ウェイポイントに接続するウェイポイントの最短ルートによって規定される粗い経路３２５を選択すること（３２０）を含む。たとえば、複数のウェイポイントは、ユークリッドの観点から車両の初期および／または目標状態に最も近いと見なすことができる。粗い経路を最短にすることで、この曖昧さを明確にすることができ、経路計画のメモリ要件も低減することができる。これに加えてまたはこれに代えて、初期ウェイポイントおよび目標ウェイポイントは、進行方向制約を受ける最短の粗い経路を提供するように選択され、進行方向制約において、車両の向きと初期ウェイポイントまたは目標ウェイポイントの向きとの差は特定の閾値未満であってもよい。

方法３００は、車両の初期状態を規定する初期ノードと車両の目標状態を規定する目標ノードとを含む車両の異なる状態を規定する複数のノードを有するツリーを構築すること（３３０）を含む。ツリー内のノードの各ペアは、衝突なしに接続されたノードの状態の間で車両を移動させる運動学的に実行可能な動きによって規定されるエッジで接続される。ツリーは、ウェイポイントのルートを規定する粗い経路のコストからの、ツリーの初期ノードを目標ノードに接続する改良された経路３３５によって規定される動きのコストの偏差を低減するように構築される。

方法３００は、改良された経路３３５を有すると、改良された経路に基づいて車両を制御すること（３４０）を含む。たとえば、システム２９９は、車両の動的モデルに従って改良された経路を追跡するために、車両のアクチュエータへの一連の制御コマンドを生成することができる。

いくつかの実施形態は、異なる種類の動作計画アルゴリズムを、改良された経路を規定するツリーを構築するように適合させることができるという認識に基づいている。このような適合化は、最適化原理および構築の原理に基づき得る。たとえば、いくつかの実施形態は、Ａ検索ガイドツリー（ＡＧＴ）法および／または双方向ＡＧＴ（ＢＩＡＧＴ）を、粗い経路から逸脱しないようにバイアスされるように適合させる。これらの方法を、限られたメモリリソースにより適したツリー構築原理に基づく、バイアスされた検索探索に適合させることができる。

たとえば、ＡＧＴ法およびＢＩＡＧＴ法はともに、プリミティブ動作の予め定められたセットから選択されたエッジでツリーを消費する。このような選択を、追跡が実行不可能でない限り粗い経路をたどる（すなわち追跡する）ように、および／または、コスト偏差を低減するこのようなプリミティブ動作を選択するように、適合させることができる。

異なる実施形態は、動作計画に適した改良された経路を規定するツリーを構築するために異なる技術を使用する。たとえば、いくつかの実施形態は、プリミティブ動作の予め定められたセットから選択された運動学的に実行可能な動きを規定するエッジでツリーを拡大する。このように、コスト偏差を低減する経路の運動学的に実行可能な区間を探す問題は、コスト偏差を低減するためにセットから動作プリミティブを選択する問題になる。

たとえば、いくつかの実施形態は、ウェイポイントの粗い経路によって規定されるルートを計画するためのルート計画ＢＩＡＧＴ（ＲＰ－ＢＩＡＧＴ）を開示し、粗い経路に向かってバイアスされたＢＩＡＧＴを実行して、改良された経路に到達する。

図４Ａは、いくつかの実施形態に係る、ＲＰ－ＢＩＡＧＴ３０５の例示的な実現例の概略図を示す。ルート計画部４０１は、駐車スペースの接続性および許容可能な交通流を特徴付ける交通ネットワークに基づいて解決ルートを決定する。解決ルートは、一連のウェイポイント４０５として表される。ウェイポイントはＢＩＡＧＴ４１０に送り込まれ、ヒューリスティック誘導４０６、進行方向制約４０７、および動作プリミティブトリミング４０８という３つの局面において利用される。

いくつかの実施形態では、交通ネットワークは、マイルストーンの無衝突構成のためのノードのセットと、関連付けられた２つのノード間の無衝突経路の存在を各々が示すエッジのセットとを含む、有向グラフの表現を有する。マイルストーンノードは、駐車スペースの入口、出口、交差部を表すが、これらに限定されない。交通流は、一方通行、対面通行、Ｕターン等であり得る。

図４Ｂは、一実施形態に係る、ルート計画部４０１によって実行される方法のブロック図を示す。ルート計画部４０１は、構成３００において提供される交通ネットワーク４１１と車両の初期／目標状態とに基づいて、解決ルートを決定する。まず、交通ネットワークを密な有向グラフ４１６として改良する（４１５）。車両の初期および目標状態１０１／１０２と密な有向グラフ４１６とに基づいて、ルート計画のための初期および目標ウェイポイントを特定する（４２０）。

いくつかの実施形態は、有向グラフにおけるウェイポイントの密度が、改良された経路と粗い経路との間のコスト偏差の最小化に影響を及ぼし得るという理解に基づいている。たとえば、隣接するウェイポイント同士の間の距離を規定する十分な密度があれば、車両の初期状態から粗い経路へのよりコスト効率の高い接続は、粗い経路の初期ウェイポイントへの接続ではなく、後続のウェイポイントへの接続であり得る。そのために、いくつかの実施形態は、駐車スペース内の交入口、出口、交差部、および曲がり角を規定する駐車スペースのマイルストーンウェイポイントを捕捉した後、マイルストーンウェイポイント同士を接続する中間ウェイポイントをさらに追加することによって有向グラフの分解能を高める。いくつかの実現例では、有向グラフにおける隣接するウェイポイント同士の間の距離は閾値未満である。この閾値は、平均的な車両の幾何学的形状、駐車スペースの駐車場所の幾何学的形状、駐車スペースのレイアウト、およびこれらの組み合わせに基づいて規定することができる。

初期および目標ウェイポイント４２０と、密な有向グラフ４１６とが与えられると、ダイクストラ法、ＡおよびＤ法等の任意のグラフ検索アルゴリズムを適用して、許可された交通流を用いて初期ウェイポイントを目標ウェイポイントに接続する、密な有向グラフの一連のウェイポイントを見つけることができる。

いくつかの実施形態では、ウェイポイントは、車両の位置および進行方向に関する情報を含む。エッジを接続する２つのウェイポイントは、同様の進行方向またはまったく同じ進行方向を有するものとし、ウェイポイントの進行方向は、関連する許容可能な交通流の方向と一致する必要がある。

図５Ａは、いくつかの実施形態に係る、例示的な駐車スペースのレイアウトと、有向グラフを用いて例示的な駐車スペースの交通ネットワークを表す概略図とを示す。この例では、駐車スペースは、４列の直角駐車場所Ａ、Ｂ、Ｃ、Ｄと、１列の縦列駐車場所Ｅとを含む。駐車スペースは、境界１３０を有する矩形として抽象化されており、５つの空き駐車場所Ａ３、Ｂ９、Ｃ１、Ｄ９およびＥ４を有するが、他の駐車場所はすべて埋まっている。駐車スペースは、４つのノード５０１、５０２、５０３および５０４によって表される４つの入口と、４つのノード５０５、５０６、５０７および５０８によって表される４つの交差部と、４つの対面通行車道５０１－５０５－５０８－５０４、５０２－５０６－５０７－５０３、５０５－５０６および５０７－５０８とを含む。ノードが位置情報のみを有する２次元平面における有向グラフとは対照的に、さまざまな実施形態の有向グラフにおけるノードは進行方向情報を含む。進行方向は、連続した間隔［０，２π）で任意の値を取る。具体的には、さまざまな実施形態では、ノードは、その関連する交通流の方向に一致する進行方向情報を含む。たとえば、入口５０１は、進行方向が２７０度の５０１から５０５への下向きの交通流と、進行方向が９０度の５０５から５０１への上向きの交通流という、２つの交通流に関連している。つまり、５０１は２つの進行方向に対応し、同じ位置を共有するが２つの別個の進行方向を有する２つの別々のノード５０１Ａおよび５０１Ｂを誘導する。同様に、交差部５０５は、進行方向が２７０度の５０１から５０５への、および５０５から５０８への２つの下向きの交通流と、進行方向が９０度の５０５から５０１への、および５０８から５０１への２つの上向きの交通流と、進行方向が０度の５０５から５０６への１つの右向きの交通流と、進行方向が１８０度の５０６から５０５への１つの左向きの交通流という、６つの交通流に関連付けられている。したがって、交差部５０５は、有向グラフにおける４つのノード、たとえば、５０５Ａ（下向き）、５０５Ｂ（上向き）、５０５Ｃ（右向き）、および５０５Ｄ（左向き）に対応する。したがって、ノード５０１Ａ、５０１Ｂ、５０５Ａ、５０５Ｂ、…、５０５Ｄを含むエッジは、（５０１Ａ，５０５Ａ）、（５０５Ａ，５０８Ａ）、（５０５Ｂ，５０２Ｂ）、（５０８Ｂ，５０５Ｂ）、（５０５Ｃ，５０６Ｃ）、（５０６Ｄ，５０５Ｄ）である。同じ位置にあるすべてのノードについても、ノード間の移行が許可されている限り、たとえば５０５Ａから５０５Ｃへの移行は左折を伴うので、その移行を記述するエッジも有向グラフに追加される。

実用的な交通ネットワークから有向グラフを導出する上記に教示した方法は、さまざまな交差部、ロータリー、および曲線車道を含む駐車スペースを処理するように一般化することができる。いくつかの実施形態では、有向グラフのノードは、ある座標の下でパラメータ化された位置および進行方向を含む。進行方向情報を含めることは、初期および目標状態により近い初期および目標ウェイポイントを特定する際に有利であり得るため、基礎となる駐車経路計画部である双方向Ａ検索ガイドツリー（ＢＩＡＧＴ）をよりよく誘導することができる。

図５Ａに有向グラフとして表されている交通ネットワークは、空間分解能が低い（任意の２つのノード間の距離が大きすぎる）場合があるので、低レベルの駐車経路計画部を効果的に誘導することができない。ノードが互いに離れすぎており、初期および目標ウェイポイントが有向グラフのノードに限定されている場合、これらの初期および目標ウェイポイントを有する有向グラフ上で検索アルゴリズムを実行すると、初期および目標ウェイポイントが車両の初期／目標状態から離れすぎているルートが得られてしまう。その結果、ルートは、ルートの初期／目標ウェイポイントと車両の初期および目標状態との間の誘導を提供しない。

図５Ｂは、いくつかの実施形態に係る、図５Ａの有向グラフを改良して密な有向グラフを生成する概略図を示す。図５Ｂに示すように、密な有向グラフは、有向グラフの各エッジをさまざまな短いエッジに分割し、追加ノードを導入してこれらの短いエッジを規定することによって得られる。一例として、エッジ（５０１Ａ，５０５Ａ）の場合、エッジはさまざまな小さいエッジ（５０１Ａ，５１１）、（５１１，５１２）、（５１２，５０５Ａ）に分割され、これらはすべて直列接続される。この分割により、新たに得られたエッジに加えて、２つの新たなノード５１１および５１２が導入される。有向グラフは、これらの新たに得られたエッジおよび関連付けられた新たなノードを追加し、注目すべきことに、エッジ（５０１，５０５）を除去する。有向グラフのすべてのエッジに対して上記プロセスを繰り返すことにより、指定閾値よりも距離が短いエッジと、駐車スペースにさらに密に分布するノードとのみを示す、密な有向グラフが得られる。なお、密な有向グラフは有向グラフよりもノードおよびエッジの数が必然的に多くなるため、ルートを決定するためのメモリおよび計算に対する要件が高くなる。しかし、この計算量の増加は、改良によってより効果的な誘導が提供されることによって低レベル経路計画部ＢＩＡＧＴのメモリ消費量および計算時間が減少することに比べると、無視できるものである。

図５Ｃは、いくつかの実施形態に係る、密な有向グラフに基づいてルート（一連のウェイポイント）を決定するルート計画部の概略図を示す。一例として、車両を右下の入口から駐車場所Ｃ１の中に移動させる駐車タスクの場合、密な有向グラフ上の任意のノードと車両の初期状態および目標状態との間の距離をそれぞれチェックすることにより、初期ウェイポイント５２１および目標ウェイポイント５２２が特定される。いくつかの実現例は、初期または目標車両状態と比較して進行方向差が１８０度未満のノードのみが初期または目標ウェイポイントとして選択されることを確実にするために、進行方向チェック手順を採用する。複数のノードが初期または目標状態までの同じユークリッド距離を有する場合、ルート計画部は、初期および目標ウェイポイント候補のすべてのペアの間のルート長を計算し、最短ルート長を有するペアを計画タスクにおける初期および目標ウェイポイントとして使用する。このように、密な有向グラフに含まれている異なる進行方向のノードが重なっていても、１つの一意の初期ウェイポイントおよび１つの一意の目標ウェイポイントが存在することが常に保証される。

いくつかの実施形態では、ノードと車両状態との間の距離は、それらのユークリッド距離として計算される。初期および目標ウェイポイントが与えられると、さまざまな検索アルゴリズムを適用して、一連のウェイポイントを含むルート５２３を生成することができる。各ウェイポイントは、少なくとも、位置および進行方向の情報、ならびに現在のウェイポイントから目標ウェイポイントまでの推定移動コストの情報を含む。

いくつかの実施形態は、ＢＩＡＧＴ経路計画方法が、ツリーおよびｔノード（ツリー上のノードの略称）というさまざまな重要な概念に依存するという理解に基づいている。ＢＩＡＧＴは、２つのツリーの一方または組み合わせを含み得る。これら２つのツリーは、その初期ｔノードが車両の初期状態を示し、その目標ｔノードが車両の目標状態を示す、１つの開始または初期ツリー、および、その初期ｔノードが車両の目標状態を表し、その目標ｔノードが車両の初期状態を表す、１つの目的または目標ツリーである。各ツリーは、ｔエッジ（ツリー上のノードの略称）のリストを介して接続されるｔノードのリストを含み、各ｔエッジは２つのｔノード間の無衝突の運動学的に実行可能な経路を表す。たとえば、初期ツリーはその初期ｔノードから始まり、ツリーのｔノードと別のツリーのｔノードとの間に接続を作ること（これは「２つのツリーが接続される」と呼ばれる）、またはツリーのｔノードがその目標ｔノードに近づくか到達することを目的として、新たな無衝突ｔノードおよびその目標ｔノードに向かうｔエッジを追加することによって成長する。両方のツリーが接続された場合、またはツリーが、その目標ｔノードに接続されるかその目標ｔノードに近づく少なくとも１つのｔノードを含む場合、ＢＩＡＧＴは成功を返す。

実現例に応じて、異なる実施形態に係るツリーは、初期ｔノード、目標ｔノード、実行モード、ｔノードのリスト、ノード拡張のために選択すべきｔノードのリスト、救助のためのｔノードのリスト、ノード拡張のために選択されるｔノード、成功フラグ等のうちの１つまたはこれらの組み合わせを格納する。

実現例に応じて、異なる実施形態に係るｔノードは、車両状態、ｔノードコスト、推定移動コスト、親ｔノード、ｔノードに到達するために親ｔノードに適用される動作プリミティブ、制御アクションの優先度等のうちの１つまたはこれらの組み合わせを格納する。ツリー上のｔノードのｔノードコストは、ツリーの初期ｔノードからｔノードによって規定される状態まで車両を運転するコストを表す到着コストと、ツリーのｔノードによって規定される状態から目標ｔノードまで車両を運転する推定コストを表す推定移動コストの、２つのコストの合計である。言い換えれば、ｔノードコストは、ツリーの初期ｔノードとｔノードを通る目標ｔノードとの間の可能な経路のコストの推定値である。ｔノードの推定移動コストは、必ずしもグラウンドトゥルースを反映しているとは限らないので、ヒューリスティックコストまたはヒューリスティクスと呼ばれることが多い。検索のための有意義な誘導を提供することができる正確なヒューリスティクスを得ることは、ほとんどの検索ベースの動作計画部にとって最も困難な部分である。

ＢＩＡＧＴは、ｔノード選択およびｔノード拡張という２つの重要なステップを繰り返し実行することでツリーを成長させる。Ｔノード選択は、最小のｔノードコストを有するｔノードを選択する。Ｔノード拡張は、この選択したｔノードに予め定められた動作プリミティブの選択されたセットを適用することによって、ツリーを成長させることを試みる。ｔノードのすべての動作プリミティブを試したが失敗した場合、このｔノードはｔノード拡張のために選択されない。ＢＩＡＧＴの計算効率は、ヒューリスティックコストが所与のｔノードについての真の移動コストにどれだけ正確に近似するかに大きく依存する。ツリーが与えられると、ＢＩＡＧＴは、ｔノードと目標ｔノードとの間のＲｅｅｄｓ－Ｓｈｅｐｐ（ＲＳ）経路長をｔノードのヒューリスティックコストとして使用することができる。

このように、いくつかの実施形態は、車両の目標または初期状態に関してではなく、粗い経路の目標または初期ウェイポイントに関して求められたノードのコストに基づいて、ツリーのノードを選択して消費する。粗い経路は、初期ウェイポイントと目標ウェイポイントとを接続する最短経路を表すので、このような接続にかかるコストが最も小さくなる。したがって、粗い経路のコストが目標または初期ウェイポイントに関して表される場合、ツリーのノードは、可能であれば粗い経路上で選択され、またはより小さい偏差で粗い経路に戻るように選択される。

図６Ａは、いくつかの実施形態に係る、４０６ヒューリスティック誘導および４０８動作プリミティブトリミングの図を示す。ルート計画部４０１が解決ルート５２３を見つけた後、ＢＩＡＧＴは、ルート解決の誘導の下で、特定の決定論的法則に従って開始および目的ツリーのｔノード選択およびｔノード拡張を実行する。ＢＩＡＧＴは、最小のｔノードコストを有するｔノードを選択することによってｔノード選択を行う。ｔノード拡張では、拡張すべきｔノードが与えられると、ＢＩＡＧＴ４１０は、ユークリッド距離に従って現在のｔノード６０２に最も近いｋ個のウェイポイント６０１を検索し、（３）に基づいてヒューリスティクスを計算し、ｔノードコストを更新する。ＢＩＡＧＴがツリー上の新たなｔノードを拡張するたびにこの手順を繰り返す。ＢＩＡＧＴはまた、ナビゲーションモード６４５にあるとき、ｋ個の最近接ウェイポイントを有する現在のｔノードの進行方向をチェックし、次いで、ｔノード６０２とすべてのｋ個のウェイポイント６０１との間の進行方向差が閾値Ψ内にある場合はすべてのステアリング動作プリミティブをトリミングする。これは、近くのすべてのウェイポイントが同じ方向に進んでいるときに不要なステアリングを避けるだけでなく、メモリ制約を満たすためにノードの総数を実用限界内に制限するのにも役立つ。

人間の知識が前もって与えられていない場合、動作計画部は、経路長という点では最適であるが人間の論理に反する軌道を導き出すことが多い。これに対して、ルート計画部の解決ルートは交通方向によって制約されるので、ＲＰ－ＢＩＡＧＴは自然な駐車挙動を実施するための便利なインターフェイスを提供する。

図６Ｂは、いくつかの実施形態に係る例示的な駐車シナリオの概略図を示す。図６Ｂに示すように、駐車場所の外側の第１のウェイポイントは、出庫タスクのための初期ウェイポイント５２１、および入庫タスクのための目標ウェイポイント５２２である。駐車場所から成長するツリーについて、ＲＰ－ＢＩＡＧＴは、現在のノードと駐車場所の外側の第１のウェイポイントとの間の進行方向差が閾値θ未満であるときにのみ真となる先行指標を規定する。これらのツリー上のノードは、進行方向指標が真になるまで、ｋ個の最近接ウェイポイントの代わりに、駐車場所の外側の第１のウェイポイントのみをヒューリスティック誘導のために使用するので、固定進行方向が優先されるようになる。

ＢＩＡＧＴの１つの欠点は、駐車タスクを解決するのに必要なノードの数が、実用的な自律車両のメモリ制約をしばしば超えてしまうことである。加えて、検索中に動作プリミティブの細かいグリッドを使用すると、駐車スペースの周りでの車両操作はしやすくなるが、長い車道では軌道が不必要に曲がりくねってしまうことがある。したがって、ＲＰ－ＢＩＡＧＴはモードという概念を導入することにより、異なるシナリオを区別して適切な動作プリミティブを提供する。いくつかの実施形態では、ＲＰ－ＢＩＡＧＴを、縦列、通常およびナビゲーションという３つのモードで実行して、縦列駐車、通常駐車および前進走行を区別することができる。縦列および通常の両モードでは後進走行が可能であるが、縦列の方が動作プリミティブのグリッドが細かい。ナビゲーションモードでは前進走行のみが可能であり、動作プリミティブのためにはるかに長い弧を使用する。このツリーモードの分類は、狭い空間ではｔノード拡張時にｔツリーを成長させることが難しくなるという自動駐車システムの現実を反映するためのものである。したがって、ツリーの成長の失敗を避けるために、より多くの動作プリミティブを試すことが適切である。車道または交差部の中央等の開けた場所では、失敗は起こりにくいが、代わりに、ｔノード拡張時に新たなｔノード／ｔエッジをいかに追加し過ぎないようにするかが大きな懸念事項である。したがって、このようなシナリオでは、より少ない動作プリミティブを適用すべきである。

いくつかの実施形態では、選択および拡張のためのノードは、それらのコストに基づいて選択される。コストの観念は、改良された経路の粗い経路からの偏差を低減するように選択される。たとえば、初期ノードからツリーを成長させる一実施形態では、ツリーのノードは、目標ウェイポイントに関して初期状態から成長するツリーのノードのコストを低減するように選択および拡張され、ツリーの現在のノードのコストは、初期ノードからツリーを通って現在のノードに到着する第１のコスト、現在のノードを粗い経路のウェイポイントに接続する運動学的に実行可能な経路の第２のコスト、および、接続されたウェイポイントから粗い経路を通って目標ウェイポイントに移動する第３のコストである。ノードを粗い経路のウェイポイントに接続する運動学的に実行可能な経路の一例は、Ｒｅｅｄｓ－Ｓｈｅｐｐ（ＲＳ）経路である。このように、第２のコストおよび第３のコストは、現在のノードから目標ウェイポイントまでの、目標ノードまでの、またはその両方までのコストを最小化する。

これに加えてまたはこれに代えて、目標ノードからツリーを成長させる一実施形態では、ツリーのノードは、初期ウェイポイントに関して目標ノードから成長するツリーのノードのコストを低減するように選択および拡張され、ツリーの現在のノードのコストは、目標ノードからツリーを通ってノードに到着する第１のコスト、現在のノードを粗い経路のウェイポイントに接続する運動学的に実行可能な経路の第２のコスト、および、接続されたウェイポイントから粗い経路を通って初期ウェイポイントに移動する第３のコストである。

異なる実施形態は、動作計画に適した改良された経路を規定するツリーを構築するために異なる技術を使用する。たとえば、いくつかの実施形態は、プリミティブ動作の予め定められたセットから選択された運動学的に実行可能な動きを規定するエッジでツリーを拡大する。このように、コスト偏差を低減する経路の運動学的に実行可能な区間を探す問題は、コスト偏差を低減するためにセットから動作プリミティブを選択する問題になる。

いくつかの実施形態は、ノード選択に加えて、決定論的にノード拡張を実行して、すべての可能な動作プリミティブを用いてノードの周りの空間を探索する必要性を低減する。そのために、ある実施形態は、拡張可能ノードのコストを低減する第１の無衝突動作プリミティブを選択し、すなわち、後続のプリミティブ動作をテストしない。計算負荷をさらに低減するために、いくつかの実施形態は、拡張可能ノードに至るグラフのエッジを規定するプリミティブ動作に対する各プリミティブ動作の類似性に基づいてプリミティブ動作のセットを順序付け、その順序でプリミティブ動作をテストする。これらの実施形態は、人間のオペレータが望むような車両の動きの滑らかさを促進し、実際には、構築されるグラフの複雑さを低減するのに役立つ。

異なる実施形態は、このような動作プリミティブを選択するために異なる技術を使用する。たとえば、一実施形態は、異なるツリー拡張モードを規定し、異なるモードに対して異なる種類の動作プリミティブを選択してコスト偏差を低減することができる。モードは、現在のノードによって規定される現在の状態と目標状態との間の距離と、現在の状態における車両を取り囲む無衝突空間のレイアウトとに基づいて選択することができる。これは、車両の自由度が高いほど、車両が粗い経路を追跡する選択肢がより良好になるからである。逆に、車両を取り囲む空間が狭い場合は、車両の自由度を高めて異なる動作プリミティブをテストすべきであり、たとえば、より網羅的な動作プリミティブを適用してツリーをうまく成長させるべきである。

いくつかの実施形態は、ナビゲーションモード、通常モード、および縦列駐車モード等のモードを使用する。たとえば、初期ツリーノード（ｔノード）が前方および後方の自由空間が狭い車両状態を表す場合、ツリーは縦列駐車モードで開始し、初期ｔノードが、開けた車道における、（その目標ｔノードによって指定される）駐車場所から特定の距離だけ離れた車両状態を表す場合、ツリーはナビゲーションモードで開始し、それ以外の場合、ツリーは通常モードで開始する。ｔノード拡張のために選択されたツリーのｔノードの前方および後方空間が比較的大きい場合、縦列駐車モードから通常モードへの移行が行われ、ｔノード拡張のために選択されたツリーのｔノードが同じツリーの目標ｔノードの特定の近傍に入る場合、ナビゲーションモードから通常モードへの移行が行われる。

図６Ｅは、いくつかの実施形態に係るモードＲＰ－ＢＩＡＧＴの移行の図を示す。ＲＰ－ＢＩＡＧＴは、６４０から開始して、ナビゲーションモード６４５、通常モード６５０、および縦列駐車モード６５５への移行を可能にする。ツリーの初期ｔノードが前方および後方の自由空間が狭い車両状態を表す場合、ツリーは縦列駐車モードで開始し、ツリーの初期ｔノードが、開けた車道における、（その目標ｔノードによって指定される）駐車場所から特定の距離だけ離れた車両状態を表す場合、ツリーはナビゲーションモードで開始し、それ以外の場合、ツリーは通常モードで開始する。ｔノード拡張のために選択されたツリーのｔノードの前方および後方空間が比較的大きい場合、縦列駐車モードから通常モードへの移行が行われ、ｔノード拡張のために選択されたツリーのｔノードが同じツリーの目標ｔノードの特定の近傍に入る場合、ナビゲーションモードから通常モードへの移行が行われる。いくつかの実現例では、６５０から６４５への点線矢印で示されるように、ＲＰ－ＢＩＡＧＴが再計画のために呼び出されるときを除いて、通常モードから縦列モードまたはナビゲーションモードへの移行は許可されない。動作プリミティブの所与の特定のグループについて、ツリーのすべてのｔノードがｔノード拡張のために選択された場合、ツリーは通常モードまたは縦列駐車モードから救助モード６６０に入れられ得る。救助モードでは、ｔノード拡張のために最近選択されたある数のｔノードが、動作プリミティブの新たなセットで再びｔノード選択できるようになる。新たなｔノードがツリーに追加されると、ツリーは救助モードの前のモードに戻される。

このように、粗い経路のウェイポイントは、ＲＰ－ＢＩＡＧＴと呼ばれるいくつかの実施形態の開示されている動作計画において以下の３つの役割を果たす。１）ウェイポイントから対応する目的地までの経路長は、ＢＩＡＧＴ検索を誘導するヒューリスティックとして使用され、２）駐車スペースに最も近いウェイポイントの進行方向は、正しい方向に出るようにＢＩＡＧＴを誘導し、３）車道の直線区間上のウェイポイントの進行方向は、不要なステアリング動作を避けるとともに、実行可能な軌道を見つけるのに必要なノードの数を減らすために、ＢＩＡＧＴが利用可能な動作プリミティブをトリミングするために利用される。

静的な駐車場地図情報は、オフラインで取得できることが多いが、トラフィックコーンまたは誤った場所に置かれたショッピングカート等の小さい一時的な道路遮断物についてのリアルタイム情報は含んでいない可能性が高い。したがって、リアルタイム障害物検出および経路修復コンポーネントが、任意のロバストな自律駐車解決策に有用である。新たに検出された障害物によって、最初に計画された軌道（すなわち初期経路／動き）が無効になると、解決策は典型的に、障害物を押し分けて、同じルートを異なる経路でたどる（経路修復）か、引き返して同じ目的地に到達する異なるルートを見つける（再計画）か、の２種類に分類される。新たな駐車スペースを目的地として検索するなど、より高レベルの意思決定が、別の可能性である。いくつかの実施形態は、元の計画目的（車両の目標状態）が変化しないと仮定して、経路／動作計画レベルでの意思決定を活用する。経路修復は、新たな障害物の影響が最小である場合に実行時間およびエネルギー消費の観点から望まれることが多い。しかしながら、新たな障害物が大きい場合、または利用可能な車道が非常に狭くなるような不都合な位置にある場合は、押し分けることは依然として実行可能であるかもしれないが、動作計画部が計画を修復して交通を遮断するのにかなりの時間がかかる可能性があり、検知ノイズおよびその他の不確実性がある場合は衝突のリスクが高くなる可能性もある。

したがって、いくつかの実施形態は「修復判定基準」を使用し、元の経路を迅速に修復できる場合にのみ修復することを試みる。たとえば、いくつかの実現例は、修復可能性をモデル化し、２つのイベントを最小化するように修復判定基準を規定する。これら２つのイベントは、実際には修復に費用がかかりすぎるが経路を修復し、最終的に再計画に頼ること、および、初期経路の修復は安く済むが再計画を起動すること、である。

図７Ａは、いくつかの実施形態に係る、修復を検討する経路計画方法のブロック図を示す。この方法は、車両の幾何学的形状、駐車スペースの地図、ならびに初期および目標状態７００を用いて開始して、まずルート計画部誘導型の双方向Ａ検索ガイドツリー（ＲＰ－ＢＩＡＧＴ）方法７０５を起動して、初期経路／動き７１０を決定する。初期経路／動き７１０は、検知された信号２４３とともにモジュールに供給され、このモジュールは、車両の位置および向きを初期経路／動き７１０に従うように時間の経過とともに制御する（７１５）。初期経路／動きを実行している間、車両は、新たな障害物ならびに新たな障害物の幾何学的形状および位置を検出することによって、環境地図２０１を継続的に更新する。新たな障害物が初期経路／動きの実行予定部分を干渉していないかをチェックする（７２０）。新たな障害物が初期経路の実行予定部分に衝突する場合、車両はクリアランスチェック７２５を行い、初期経路の修復が困難すぎるか否かを評価する（７３０）。困難すぎる場合、新たな経路計画タスクを規定し、ＲＰ－ＢＩＡＧＴ７０５を起動して初期経路／動きを再計画して更新する。そうでなければ、修復タスクを生成し、修復計画部が修復軌道で初期経路／動きを修復して（７３５）更新する。修復経路／軌道は７１５に供給されて実行される。

図７Ｂは、一実施形態に係るクリアランスチェック７２５を行う方法のフローチャートを示す。図７Ｂに示すように、車両の初期経路／動き上のｔノードのリスト７０１が、新たに検出された障害物と衝突していると判定された場合（７２１）、クリアランスチェック手順が実行される。この手順では、まず衝突ｔノードを両方向に横にシフトさせ（７０１）、無衝突シフトを探す。衝突しているすべてのｔノードについて無衝突シフトが存在する場合（７２１）、本実施形態は、クリアランス値、たとえば横方向シフトの値を出力する（７１６）。いずれのｔノードについても無衝突シフトが存在しない場合（７１１）は、実施形態は次に、無衝突シフトのないノードについて軸を回転させて（７２１）、この手順を繰り返す（７０６）。

いくつかの実現例では、本実施形態は、車両進行方向についての回転角度のセットを予め定め（７２６）、すべてのｔノードが無衝突シフトを見つけるまですべての回転角度をテストする。予め定められた回転角度をすべて使い果たした後に無衝突シフトのないｔノードがまだある場合、本実施形態は、クリアランスと、無衝突シフトのないノードの数とを出力することもできる。

図７Ｃは、いくつかの実施形態に係るクリアランスチェック７２５の例示的なシナリオを示す概略図である。車両が構成Ａに向かって走行すると、マッピングおよびローカライゼーションシステムは新たな障害物を検出して出力する（７５１）。車両は、新たな障害物７５１との衝突をチェックすることによって初期経路／動き７１２の安全性を検証し、Ｂを含むその計画軌道に沿った複数の構成と新たな障害物との衝突の可能性を確認する。新たな障害物と衝突するｔノードごとに、車両の進行方向に対して垂直な、対応する横軸を計算し、ｔノードを横軸に沿って両方向にシフトさせて無衝突シフトを探す。

図７Ｄは、いくつかの実施形態に係る、クリアランスチェック７２５で求められた７１３と７１４との間の横方向距離としての最大クリアランスの一例を示す。特に、７１３は、車両を一方向に横移動させたときの車両の第１の無衝突状態であり、７１４は、車両を同じ方向に横移動させたときの車両の最後の無衝突状態を示す。構成Ｂの右側には無衝突シフトは存在せず、構成Ｃ ７１３および構成Ｄ ７１４は無衝突シフトの境界である。ＣとＤとの間の横方向距離は最大クリアランスと定義される。新たな障害物と衝突しているすべてのｔノードについてこの手順を繰り返し、無衝突シフトが存在するか否かをチェックする。

いくつかの実施形態では、最大クリアランスは、新たな障害物と衝突するｔノードに関連付けられた特性である。いくつかの実施形態は、地図情報が更新され得るように、新たな障害物の幾何学的形状に関する比較的正確な推定値がセンサによって提供され得ることを前提として、単純な幾何学的形状に基づく、迅速に計算可能な修復判定基準を開示する。

図８Ａは、いくつかの実施形態に係る、クリアランスチェック手順の結果に基づいて初期経路を修復する困難さを評価する方法のブロック図を示す。クリアランスレベル７１５は評価関数ブロック８００に投入され、評価関数ブロック８００は、クリアランスレベルから修復の困難さを定量化する変数へのマッピングを規定する。８００の出力は、予め設定された特定の閾値と照合されて、「修復可能」８０６または「修復不可能」８０７の２つの決定に分類される（８０５）。

いくつかの実施形態では、修復の困難さは、修復に必要なｔノードの予想数、修復に必要な予想計算時間、および新たに検出された障害物の周りの無衝突空間の狭さのうちの１つまたはこれらの組み合わせによって測定される。

いくつかの実施形態では、評価関数は、クリアランスレベルと修復の困難さとの関係をモデル化する。クリアランスレベル（新たに検出された障害物の周りの無衝突空間の狭さを測定する）を意味する単純な形態の評価関数が、修復の困難さの指標として直接使用される。任意のｔノードについて、無衝突シフトが存在しない場合、または最大クリアランスが閾値η未満である場合は、修復判定基準は満たされてないと見なされ、ＲＰ－ＢＩＡＧＴは再計画に直接進む。そうでなければ、ＲＰ－ＢＩＡＧＴは、新たな障害物と衝突する元の軌道の区間を修復した後、その区間を初期経路の無衝突区間に接続する。

いくつかの実施形態では、評価関数は、クリアランスレベルと初期経路を修復するのに必要なｔノード数との関係を特徴付ける。したがって、修復判定基準は、埋め込みコンピューティングプラットフォームによって課される限られたメモリ制約を反映するノードの数であり得る。

修復に必要なｔノード数をクリアランスレベルの関数としてモデル化する評価関数は、分析形式では得られない場合があることが認識されている。クリアランスレベルと修復に必要なｔノード数との間のマッピングは、オフラインまたはオンラインで得られるグラウンドトゥルースデータのセットに一致するように関数近似器を訓練することによって近似可能であり得ることも認識されている。

図８Ｂは、いくつかの実施形態に係る、訓練データに基づいて評価関数８００の表現を得る手順の概略図を示す。クリアランス値（７１５の出力）および経路計画部出力８２０が、パラメータ化評価関数８２５を訓練するためのデータとして使用される。８２５への入力データは７１５であり、７１５はクリアランスチェック７２５によって求められる。経路計画部８１５の出力８２０は、計算時間とメモリのどちらが重要であるかに応じて、初期経路を修復するための時間、または初期経路を修復するためのｔノード数、またはそれらの組合せであり得る。経路計画部８１５は、最新の地図、新たな障害物、および初期経路等の情報を入力とし、初期経路を修復することを試み、修復のための計算時間またはｔノード数のいずれかを出力する。ブロック８２５の出力８３０およびデータ８２０はクリティックブロック８３５に与えられ、クリティックブロック８３５において８２０と８２５との間の誤差が計算され処理されて、パラメータ化評価関数８２５が８１５における修復に必要なｔノード数をどの程度良好に予測するかが示される。８４０において、誤差の何らかの関数を最適化することによってパラメータ値の新たな集合が得られる。

いくつかの実施形態では、評価関数は、未知の係数を有する冪級数と、訓練データにおいて評価され得る一連の基底関数との形式を許容する。評価関数を訓練することは、ある判定基準に従って８２０と８３０の最良の一致をもたらす係数を解くということになる。

これに加えてまたはこれに代えて、いくつかの実施形態では、衝突ｔノードのすぐ近傍を直接接続することは車両力学により通常禁止されているため、ＲＰ－ＢＩＡＧＴは衝突ｔノードから離れたｍ個のノードの修復を開始する。ここで、ｍは、車両サイズおよび動作プリミティブの弧長に依存する、カスタマイズ可能なパラメータである。

ＲＰ－ＢＩＡＧＴは、元の軌道の修復が困難であると判定すると、新たな障害物と衝突するエッジを密な有向グラフから除去し、ルート計画部を用いて、新たな初期状態および元の目標状態を有する新たなルートを見つける。再計画のための新たな初期状態は、典型的には車両の現在の状態である。次いで、当初のＲＰ－ＢＩＡＧＴ計画の手順と同様の手順に従い、このルートに沿った動的に実行可能な軌道を見つける。再計画手順と元の計画手順との間の主な違いは、すべてのツリーが駐車スペースから成長していない場合でも、すべてのツリーに対して進行方向制約が活用されることである。これは、再ルーティングは典型的に、元の走行方向と反対に方向転換することを伴い、これは経路長を最小化する動作計画部にとって特に困難であるからである。進行方向制約がなければ、これらの計画部が見つける軌道は、目的地までずっとバック運転するように車両に指示することが多くなるが、これは、経路長の観点からは最適な計画であるが、人間の運転者の観点からは非常に望ましくない。したがって、駐車場所から成長する目的ツリーに加えて、すべてのツリーに進行方向制約を課すことにより、自然な駐車挙動を保証する。

図９は、一実施形態に係るシステムの概略図を示す。このシステムは、自動駐車９５０を実行するように構成されたプロセッサ９０２を含む車両９０１を含む。車両は、ＬＩＤＡＲ９１０および／またはカメラ９２０等の少なくとも１つのセンサも含む。ＬＩＤＡＲセンサ９１０は低解像度の第１のセンサであり、カメラ９２０は高解像度の第２のセンサである。センサ９１０および／または９２０は、プロセッサ９０２に動作可能に接続され、駐車スペースの少なくとも一部の幾何学的形状を示す情報を検知するように構成されている。プロセッサ９０２はこの情報を用いて、駐車スペースの地図１３０を決定および／または更新する。そのために、プロセッサ９０２は地図１３０を用いて自動駐車９５０を実行する。

システム１０００は、接続１０２０を介して他のコンポーネントに動作可能に結合することができる、カメラ１０１０、慣性測定ユニット（ＩＭＵ）１０３０、プロセッサ４５０、メモリ１０６０、トランシーバ１０７０、およびディスプレイ／画面１０８０のうちの１つまたはこれらの組み合わせを含み得る。接続１０２０は、バス、ライン、ファイバ、リンク、またはこれらの組み合わせを含み得る。

トランシーバ１０７０は、たとえば、１種類以上の無線通信ネットワークを介して１以上の信号を送信できるようにされる送信機と、１種類以上の無線通信ネットワークを介して送信された１以上の信号を受信する受信機とを含み得る。トランシーバ１０７０は、フェムトセル、ＩＥＥＥ８０２．１１規格ファミリに基づき得るＷｉ－Ｆｉ（登録商標）ネットワークもしくは無線ローカルエリアネットワーク（ＷＬＡＮ）、Ｂｌｕｅｔｏｏｔｈ（登録商標）等の無線パーソナルエリアネットワーク（ＷＰＡＮＳ）、近距離無線通信（ＮＦＣ）、ＩＥＥＥ８０２．１５ｘ規格ファミリに基づくネットワーク、および／またはＬＴＥ、ＷｉＭＡＸ等の無線ワイドエリアネットワーク（ＷＷＡＮ）その他を含むがこれらに限定されないさまざまな技術に基づく無線ネットワークとの通信を可能にすることができる。システム１０００はまた、有線ネットワークを介した通信用の１以上のポートを含み得る。

いくつかの実施形態では、システム１０００は、ＣＣＤもしくはＣＭＯＳセンサ等の画像センサ、レーザおよび／またはカメラ１０１０を含むことができ、これらは以下で「センサ１０１０」と呼ばれる。たとえば、センサ１０１０は、光学画像を電子またはデジタル画像に変換することができ、取得した画像をプロセッサ４５０に送信することができる。これに加えてまたはこれに代えて、センサ１０１０は、シーン内の対象物体から反射した光を検知し、捕捉した光の強度をプロセッサ４５０に与えることができる。

たとえば、センサ１０１０は、「カラー情報」を提供するカラーまたはグレースケールカメラを含み得る。本明細書で使用する「カラー情報」という用語は、カラーおよび／またはグレースケール情報を意味する。一般に、本明細書では、カラー画像またはカラー情報は、１～Ｎ個のチャネルを含むと見なすことができ、Ｎは、画像を記憶するために使用されるカラー空間に依存する何らかの整数である。たとえば、ＲＧＢ画像は３つのチャネルを含み、各チャネルはそれぞれ赤、青および緑の情報のためのものである。

たとえば、センサ１０１０は、「深度情報」を提供するための深度センサを含み得る。深度情報は、深度センサを用いてさまざまな方法で取得することができる。「深度センサ」という用語は、独立しておよび／またはいくつかの他のカメラと併せて深度情報を得るために使用され得る機能ユニットを意味するために使用される。たとえば、いくつかの実施形態では、深度センサおよび光学カメラはセンサ１０１０の一部であり得る。たとえば、いくつかの実施形態では、センサ１０１０は、カラー（ＲＧＢ）画像に加えて、深度センサが有効にされると画素ごとの深度（Ｄ）情報を捕捉し得るＲＧＢＤカメラを含む。

別の例として、いくつかの実施形態では、センサ１０１０は３Ｄ飛行時間（３Ｄ ＴＯＦ：3D Time Of Flight）カメラを含み得る。３Ｄ ＴＯＦカメラを用いる実施形態では、深度センサは、３Ｄ ＴＯＦカメラに結合されたストロボ光の形態を取ることができ、ストロボ光はシーン内の物体を照らすことができ、反射光はセンサ１０１０内のＣＣＤ／ＣＭＯＳセンサによって捕捉することができる。深度情報は、光パルスが物体に移動してからセンサに戻るまでにかかる時間を測定することによって得ることができる。

さらなる例として、深度センサは、センサ１０１０に結合された光源の形態を取ることができる。一実施形態では、光源は、１つ以上の狭帯域の光を含み得る構造化またはテクスチャ化された光パターンをシーン内の物体に投影する。物体の表面形状によって生じる投影パターンの幾何学的歪みを利用することによって、深度情報が得られる。一実施形態は、赤外線構造化光プロジェクタとＲＧＢカメラに登録された赤外線カメラとの組み合わせ等のステレオセンサから深度情報を求める。

いくつかの実施形態では、センサ１０１０は立体カメラを含む。たとえば、深度センサは、２つ以上のカメラを用いてシーンの深度情報を取得し得るパッシブステレオビジョンセンサの一部を形成してもよい。捕捉されたシーン内の両方のカメラに共通する点の画素座標をカメラ姿勢情報および／または三角測量技術とともに用いて、画素ごとの深度情報を取得してもよい。

いくつかの実施形態では、システム１０００を、デュアルフロントカメラならびに／または前面および背面カメラ等の複数のセンサ１０１０に動作可能に接続することができ、これらもさまざまなセンサを組み込み得る。いくつかの実施形態では、センサ１０１０は静止画像およびビデオ画像の両方を捕捉することができる。いくつかの実施形態では、センサ１０１０は、たとえば３０フレーム／秒（ｆｐｓ）で画像を捕捉することができるＲＧＢＤまたは立体ビデオカメラを含み得る。一実施形態では、センサ１０１０によって捕捉された画像は、未加工の非圧縮フォーマットであってもよく、処理される前および／またはメモリ１０６０に格納される前に圧縮することができる。いくつかの実施形態では、画像圧縮は、可逆または不可逆圧縮技術を使用するプロセッサ４５０によって実行することができる。

いくつかの実施形態では、プロセッサ４５０は、ＩＭＵ１０３０からの入力を受けることもできる。その他の実施形態では、ＩＭＵ１０３０は、３軸加速度計、３軸ジャイロスコープ、および／または磁力計を含み得る。ＩＭＵ１０３０は、速度、向き、および／または他の位置関連情報をプロセッサ４５０に与えることができる。いくつかの実施形態では、ＩＭＵ１０３０は、測定された情報を、センサ１０１０による各画像フレームの捕捉と同期して出力することができる。いくつかの実施形態では、ＩＭＵ１０３０の出力は、一部、プロセッサ４５０が、センサ測定値をフュージョンするために、および／またはフュージョンされた測定値をさらに処理するために、使用される。

システム１０００はまた、カラーおよび／または深度画像等の画像をレンダリングする画面またはディスプレイ１０８０を含み得る。いくつかの実施形態では、ディスプレイ１０８０を用いて、センサ１０１０が捕捉したライブ画像、フュージョンされた画像、拡張現実（ＡＲ）画像、グラフィカルユーザインターフェイス（ＧＵＩ）、およびその他のプログラム出力を表示することができる。いくつかの実施形態では、ディスプレイ１０８０は、仮想キーボード、アイコン、メニュー、またはその他のＧＵＩ、ユーザジェスチャーならびに／またはスタイラスおよびその他の書込手段等の入力デバイスの何らかの組み合わせを介して、ユーザがデータを入力できるようにする、タッチスクリーンを含み得る、および／またはタッチスクリーンが収容され得る。いくつかの実施形態では、ディスプレイ１０８０を、液晶ディスプレイ（ＬＣＤ）表示装置または有機ＬＥＤ（ＯＬＥＤ）表示装置等の発光ダイオード（ＬＥＤ）表示装置を用いて実現することができる。その他の実施形態では、ディスプレイ１０８０はウェアラブルディスプレイであってもよい。いくつかの実施形態では、フュージョンの結果をディスプレイ１０８０上でレンダリングすることができ、または、システム１０００の内部もしくは外部であり得る異なるアプリケーションに与えることができる。

例示的なシステム１０００は、本開示に従うさまざまなやり方で、たとえば図示されている機能ブロックのうちの１つ以上を追加する、組み合わせる、または削除することにより、修正することもできる。たとえば、いくつかの構成において、システム１０００は、ＩＭＵ１０３０またはトランシーバ１０７０を含まない。さらに、ある例示的な実現例では、システム１０００は、周囲光センサ、マイクロフォン、音響センサ、超音波センサ、レーザレンジファインダ等の、さまざまな他のセンサ（図示せず）を含む。いくつかの実施形態では、システム１０００の一部は、１つ以上のチップセットおよび／または同様のものの形態を取る。

プロセッサ４５０は、ハードウェア、ファームウェア、およびソフトウェアの組み合わせを用いて実現できる。プロセッサ４５０は、センサフュージョンに関連する計算手順もしくはプロセスの少なくとも一部、および／またはフュージョンされた測定値をさらに処理する方法を実行するように構成可能な１つ以上の回路を表し得る。プロセッサ４５０は、メモリ１０６０から命令および／またはデータを取り出す。プロセッサ４５０は、１つ以上の特定用途向け集積回路（ＡＳＩＣ）、中央処理装置および／またはグラフィック処理ユニット（ＣＰＵおよび／またはＧＰＵ）、デジタル信号プロセッサ（ＤＳＰ）、デジタル信号処理デバイス（ＤＳＰＤ）、プログラマブルロジックデバイス（ＰＬＤ）、フィールドプログラマブルゲートアレイ（ＦＰＧＡ）、コントローラ、マイクロコントローラ、マイクロプロセッサ、埋め込みプロセッサコア、電子デバイス、本明細書に記載の機能を実行するように設計されたその他の電子ユニット、またはその組み合わせを用いて実現できる。

メモリ１０６０は、プロセッサ４５０の内部および／またはプロセッサ４５０の外部で実現することができる。本明細書で使用する「メモリ」という用語は、長期、短期、揮発性、不揮発性、またはその他のメモリのうちのいずれかの種類を意味し、特定の種類のメモリもしくは特定の数のメモリに、または、メモリが格納される物理媒体の種類に限定されない。いくつかの実施形態では、メモリ１０６０は、自動駐車を容易にするプログラムコードを保持する。

たとえば、メモリ１０６０は、静止画像、深度情報、ビデオフレーム、プログラム結果等のセンサの測定値と、ＩＭＵ１０３０および他のセンサによって提供されるデータとを格納することができる。メモリ１０６０は、車両の幾何学的形状、駐車スペースの地図、車両の運動学的モデル、および車両の動的モデルを記憶するメモリを格納することができる。一般に、メモリ１０６０は任意のデータ記憶機構を表し得る。メモリ１０６０は、たとえば、一次メモリおよび／または二次メモリを含み得る。一次メモリは、たとえば、ランダムアクセスメモリ、読み出し専用メモリ等を含み得る。図４ではプロセッサ４５０とは別個のものとして示されているが、一次メモリのすべてまたは一部は、プロセッサ４５０の内部に提供することができる、そうでなければプロセッサ４５０と同一場所に配置することができるおよび／またはプロセッサ４５０に結合することができることが理解されねばならない。

二次メモリは、たとえば、一次メモリと同一もしくは同様の種類のメモリ、および／または、たとえば、フラッシュ／ＵＳＢメモリドライブ、メモリカードドライブ、ディスクドライブ、光ディスクドライブ、テープドライブ、ソリッドステートドライブ、ハイブリッドドライブ等の１つ以上のデータ記憶デバイスもしくはシステムを含み得る。ある実現例では、二次メモリは、リムーバブルメディアドライブ（図示せず）内の非一時的なコンピュータ読取可能媒体を動作可能に受けることができ、そうでなければ非一時的なコンピュータ読取可能媒体に構成可能であり得る。いくつかの実施形態では、非一時的なコンピュータ読取可能媒体は、メモリ１０６０および／またはプロセッサ４５０の一部を形成する。

上記本発明の実施形態は、数多くのやり方のうちのいずれかで実現することができる。たとえば、これらの実施形態は、ハードウェア、ソフトウェア、またはその組み合わせを用いて実現してもよい。ソフトウェアで実現する場合、ソフトウェアコードは、任意の適切なプロセッサまたはプロセッサの集まりにおいて、これが１つのコンピュータに設けられていても複数のコンピュータに分散されていても、実行することができる。このようなプロセッサは、１以上のプロセッサが集積回路コンポーネント内にある集積回路として実現してもよい。とはいえ、プロセッサは、任意の適切なフォーマットの回路を用いて実現してもよい。

また、本発明の実施形態は、方法として実施してもよく、その一例は既に示されている。この方法の一部として実行される動作は、いずれかの適切なやり方で順序付ければよい。したがって、例示された順序と異なる順序で動作が実行される実施形態を構成してもよく、これは、例示された実施形態では逐次的動作として示されているいくつかの動作を同時に実行することを含み得る。

請求項において、請求項要素を修飾するために「第１」、「第２」等の順序を表す用語を使用する場合、それ自体が、ある請求項要素の、別の請求項要素に対する優先、先行、または順序を暗示しているのではなく、実行する方法の動作の時間的順序を暗示しているのでもなく、請求項要素を区別するために、特定の名称を有するある請求項要素を、同一の名称を有する（が順序を表す用語を使用する）別の要素から区別するためのラベルとして用いているに過ぎない。

本発明を、好ましい実施形態の例を用いて説明してきたが、本発明の精神および範囲の中でさまざまなその他の適合化および修正を実施できることが理解されるはずである。したがって、以下の請求項の目的は、本発明の真の精神および範囲に含まれるこのような変形および修正形すべてをカバーすることである。

Claims (19)

1. 駐車スペース内に車両を駐車するための制御システムであって、少なくとも１つのプロセッサと、命令が格納されたメモリとを備え、前記命令は、前記少なくとも１つのプロセッサによって実行されると、前記制御システムに、
   前記車両の初期位置および初期向きを規定する前記駐車スペース内の前記車両の初期状態と、前記車両の目標位置および目標向きを規定する前記駐車スペース内の前記車両の目標状態と、前記駐車スペース内の交通ネットワークを表す有向グラフとを受信することを実行させ、前記有向グラフは、エッジによって接続されたウェイポイントを含み、各ウェイポイントは、前記車両が前記有向グラフに沿って移動する位置および向きを規定し、各エッジは、前記有向グラフにおける１つのウェイポイントから次のウェイポイントまでの前記車両の無衝突経路を規定し、前記有向グラフは、前記駐車スペースにおいて、異なる前記車両の異なる目標状態での駐車のために予め定められており、前記制御システムにさらに、
   前記車両の前記初期向きに基づいて、前記車両の前記初期状態に最も近い前記有向グラフ上の初期ウェイポイントを特定することと、
   前記車両の前記目標向きに基づいて、前記車両の前記目標状態に最も近い前記有向グラフ上の目標ウェイポイントを特定することと、
   前記車両の前記初期状態に最も近い前記有向グラフ上の前記初期ウェイポイントを前記車両の前記目標状態に最も近い前記有向グラフ上の前記目標ウェイポイントに接続するウェイポイントの最短ルートによって規定される粗い経路を選択するために前記有向グラフの一部を取得することとを実行させ、前記粗い経路は、前記車両の前記初期状態を前記目標状態に接続しておらず、前記命令は、前記制御システムにさらに、
   前記車両の前記初期状態を規定する初期ノードと前記車両の前記目標状態を規定する目標ノードとを含む前記車両の異なる状態を規定する複数のノードを有するツリーを構築するために誘導されるヒューリスティック誘導を用いて前記駐車スペースを検索することを実行させ、前記有向グラフにおけるノードは、進行方向情報を含み、前記ウェイポイントに関して求められ、前記ツリー内のノードの各ペアは、衝突なしに接続された前記ノードの前記状態の間で前記車両を移動させる運動学的に実行可能な動きによって規定されるエッジで接続され、前記ツリーは、前記粗い経路を前記ヒューリスティック誘導として用いて、前記ウェイポイントのルートを規定する前記粗い経路に沿った動きの計算コストからの、前記ツリーの前記初期ノードを前記目標ノードに接続する改良された経路によって規定される動きの計算コストの偏差を低減するように構築され、前記命令は、前記制御システムにさらに、
   前記改良された経路に基づいて前記車両を制御することを実行させる、制御システム。
2. 前記有向グラフは、前記車両の同じ位置および異なる向きを規定する少なくともいくつかの前記ウェイポイントを含む、請求項１に記載の制御システム。
3. 前記初期ウェイポイントまたは前記目標ウェイポイントは、進行方向制約を受ける最短の粗い経路を提供するように選択され、前記進行方向制約において、前記初期状態によって規定される前記車両の向きと前記初期ウェイポイントまたは前記目標ウェイポイントの向きとの差は閾値未満である、請求項１に記載の制御システム。
4. 前記有向グラフは、ウェイポイント同士を接続する中間ウェイポイントを含み、隣接するウェイポイント同士の間の距離は閾値未満である、請求項１に記載の制御システム。
5. 前記ツリーは、前記粗い経路から逸脱しないようにバイアスされたＡ検索ガイドツリー（ＡＧＴ）法および双方向ＡＧＴ（ＢＩＡＧＴ）法の一方または組み合わせによって構築される、請求項１に記載の制御システム。
6. 前記ツリーの前記エッジを規定する前記運動学的に実行可能な動きの各々は、追跡が実行不可能でない限り粗い経路を追跡するプリミティブ動作の予め定められたセットから選択される、請求項１に記載の制御システム。
7. 前記メモリは、プリミティブ動作の異なるセットを格納し、前記プロセッサは、前記ツリーの現在のノードによって規定される現在の状態と前記目標状態との間の距離と、前記現在の状態における前記車両を取り囲む無衝突空間のレイアウトとに基づいて、前記現在のノードを拡張するためのモードを選択するように構成されている、請求項６に記載の制御システム。
8. 前記プロセッサは、モードのセットから前記モードを選択し、
   前記車両の現在の状態の位置と目標状態の位置との間の距離がナビゲーション閾値未満である場合、ナビゲーションモードが選択され、
   前記初期状態または前記目標状態における前記車両の向きに沿った無衝突の前方空間および後方空間が駐車閾値未満である場合、縦列駐車モードが選択され、
   それ以外の場合、通常モードが選択される、請求項７に記載の制御システム。
9. 前記プリミティブ動作のセットは、ナビゲーションモード、縦列駐車モード、および通常モードに従って規定され、
   前記ナビゲーションモードでは、前記プリミティブ動作のセットは、ステアリング角度の範囲が制限された前進動作のみを含み、
   前記縦列駐車モードでは、前記プリミティブ動作のセットは、前記ノード間の距離が短い前進動作および後進動作の両方を含み、
   前記通常モードでは、前記プリミティブ動作のセットは、前記ステアリング角度に対する制限はない前進動作および後進動作の両方を含む、請求項７に記載の制御システム。
10. 前記ツリーの前記ノードは、前記目標ウェイポイントに関して前記初期状態から成長する前記ツリーの前記ノードの計算コストを低減するように選択および拡張され、前記ツリーの現在のノードの計算コストは、前記初期ノードから前記ツリーを通って前記現在のノードに到着する第１の計算コスト、前記現在のノードを前記粗い経路のウェイポイントに接続する運動学的に実行可能な経路の第２の計算コスト、および、接続された前記ウェイポイントから前記粗い経路を通って前記目標ウェイポイントに移動する第３の計算コストである、請求項６に記載の制御システム。
11. 前記ノードを前記粗い経路の前記ウェイポイントに接続する前記運動学的に実行可能な経路は、Ｒｅｅｄｓ－Ｓｈｅｐｐ（ＲＳ）の経路である、請求項１０に記載の制御システム。
12. 前記第２の計算コストおよび前記第３の計算コストは、前記現在のノードから前記目標ウェイポイントまでの、前記目標ノードまでの、またはその両方までの計算コストを最小化する、請求項１０に記載の制御システム。
13. 前記ツリーの前記ノードは、前記初期ウェイポイントに関して前記目標ノードから成長する前記ツリーの前記ノードの計算コストを低減するように選択および拡張され、前記ツリーの現在のノードの計算コストは、前記目標ノードから前記ツリーを通って前記ノードに到着する第１の計算コスト、前記現在のノードを前記粗い経路のウェイポイントに接続する運動学的に実行可能な経路の第２の計算コスト、および、接続された前記ウェイポイントから前記粗い経路を通って前記初期ウェイポイントに移動する第３の計算コストである、請求項６に記載の制御システム。
14. 前記プロセッサは、前記改良された経路を遮る障害物を検出すると、前記障害物の周りのクリアランスを求め、前記クリアランスの値に基づいて、前記改良された経路を修復するか前記改良された経路を再計算する修復計画を立てる、請求項１に記載の制御システム。
15. 前記プロセッサは、前記クリアランスをクリアランス閾値と比較することによって前記修復計画を立てるように構成されている、請求項１４に記載の制御システム。
16. 前記プロセッサは、訓練された関数に前記クリアランスの値を与えることによって前記修復計画を立てるように構成されている、請求項１４に記載の制御システム。
17. 前記訓練された関数は、前記改良された経路を修復するのに必要なノードの数を出力する、請求項１６に記載の制御システム。
18. 駐車スペース内に車両を駐車するための制御方法であって、前記方法は、前記方法を実現する格納された命令に結合されたプロセッサを使用し、前記命令は、前記プロセッサによって実行されると前記方法のステップを実施し、前記方法は、
    前記車両の初期位置および初期向きを規定する前記駐車スペース内の前記車両の初期状態と、前記車両の目標位置および目標向きを規定する前記駐車スペース内の前記車両の目標状態と、前記駐車スペース内の交通ネットワークを表す有向グラフとを受信するステップを備え、前記有向グラフは、エッジによって接続されたウェイポイントを含み、各ウェイポイントは、前記車両が前記有向グラフに沿って移動する位置および向きを規定し、各エッジは、前記有向グラフにおける１つのウェイポイントから次のウェイポイントまでの前記車両の無衝突経路を規定し、前記有向グラフは、前記駐車スペースにおいて、異なる前記車両の異なる目標状態での駐車のために予め定められており、前記方法はさらに、
    前記車両の前記初期向きに基づいて、前記車両の前記初期状態に最も近い前記有向グラフ上の初期ウェイポイントを特定するステップと、
    前記車両の前記目標向きに基づいて、前記車両の前記目標状態に最も近い前記有向グラフ上の目標ウェイポイントを特定するステップと、
    前記車両の前記初期状態に最も近い前記有向グラフ上の前記初期ウェイポイントを前記車両の前記目標状態に最も近い前記有向グラフ上の前記目標ウェイポイントに接続するウェイポイントの最短ルートによって規定される粗い経路を選択するために前記有向グラフの一部を取得するステップと、を備え、前記粗い経路は、前記車両の前記初期状態を前記目標状態に接続しておらず、前記方法はさらに、
    前記車両の前記初期状態を規定する初期ノードと前記車両の前記目標状態を規定する目標ノードとを含む前記車両の異なる状態を規定する複数のノードを有するツリーを構築するために誘導されるヒューリスティック誘導を用いて前記駐車スペースを検索するステップとを備え、前記有向グラフにおけるノードは、進行方向情報を含み、前記ウェイポイントに関して求められ、前記ツリー内のノードの各ペアは、衝突なしに接続された前記ノードの前記状態の間で前記車両を移動させる運動学的に実行可能な動きによって規定されるエッジで接続され、前記ツリーは、前記粗い経路を前記ヒューリスティック誘導として用いて、前記ウェイポイントのルートを規定する前記粗い経路に沿った動きの計算コストからの、前記ツリーの前記初期ノードを前記目標ノードに接続する改良された経路によって規定される動きの計算コストの偏差を低減するように構築され、前記方法はさらに、
    前記改良された経路に基づいて前記車両を制御するステップを備える、制御方法。
19. 方法を実行するためにプロセッサが実行可能なプログラムが実装された非一時的なコンピュータ読取可能記憶媒体であって、前記方法は、
    車両の初期位置および初期向きを規定する駐車スペース内の前記車両の初期状態と、前記車両の目標位置および目標向きを規定する前記駐車スペース内の前記車両の目標状態と、前記駐車スペース内の交通ネットワークを表す有向グラフとを受信することを備え、前記有向グラフは、エッジによって接続されたウェイポイントを含み、各ウェイポイントは、前記車両が前記有向グラフに沿って移動する位置および向きを規定し、各エッジは、前記有向グラフにおける１つのウェイポイントから次のウェイポイントまでの前記車両の無衝突経路を規定し、前記有向グラフは、前記駐車スペースにおいて、異なる前記車両の異なる目標状態での駐車のために予め定められており、前記方法はさらに、
    前記車両の前記初期向きに基づいて、前記車両の前記初期状態に最も近い前記有向グラフ上の初期ウェイポイントを特定することと、
    前記車両の前記目標向きに基づいて、前記車両の前記目標状態に最も近い前記有向グラフ上の目標ウェイポイントを特定することと、 前記車両の前記初期状態に最も近い前記有向グラフ上の前記初期ウェイポイントを前記車両の前記目標状態に最も近い前記有向グラフ上の前記目標ウェイポイントに接続するウェイポイントの最短ルートによって規定される粗い経路を選択するために前記有向グラフの一部を取得することとを実行させ、前記粗い経路は、前記車両の前記初期状態を前記目標状態に接続しておらず、前記方法はさらに、
    前記車両の前記初期状態を規定する初期ノードと前記車両の前記目標状態を規定する目標ノードとを含む前記車両の異なる状態を規定する複数のノードを有するツリーを構築するために誘導されるヒューリスティック誘導を用いて前記駐車スペースを検索することとを備え、前記有向グラフにおけるノードは、進行方向情報を含み、前記ウェイポイントに関して求められ、前記ツリー内のノードの各ペアは、衝突なしに接続された前記ノードの前記状態の間で前記車両を移動させる運動学的に実行可能な動きによって規定されるエッジで接続され、前記ツリーは、前記粗い経路を前記ヒューリスティック誘導として用いて、前記ウェイポイントのルートを規定する前記粗い経路に沿った動きの計算コストからの、前記ツリーの前記初期ノードを前記目標ノードに接続する改良された経路によって規定される動きの計算コストの偏差を低減するように構築され、前記方法はさらに、
    前記改良された経路に基づいて前記車両を制御することを備える、非一時的なコンピュータ読取可能記憶媒体。

Images