JP6749480B2 - 車両を駐車するシステム、方法及び非一時的コンピューター可読記憶媒体 - Google Patents

Description

本発明は、包括的には、車両の経路計画に関し、より詳細には、車両を目標空間に自動的に駐車する経路計画方法に関する。

自律車両又は自律走行モードを実行する車両のいずれかの車両によって利用されるいくつかの制御システムは、他の車両又は歩行者等の障害物を回避するためのみではなく、車両の操作に関連付けられたいくつかの基準を最適化するためも含む双方で、未来を予測し、車両の安全運動又は経路を予測する。目標状態は、固定ロケーション、移動ロケーション、速度ベクトル、領域、又はそれらの組み合わせのいずれかとすることができる。道路縁、歩行者、及び他の車両等の周囲環境は、車両のセンサーによって検知される、及び／又は、事前に与えられた情報によって少なくとも部分的に既知である。

自律運転モードにおいて実行される自律車両又は半自律車両を制御するタスクのうちの１つは、車両を、本明細書において目標状態と称される駐車位置及び駐車方位に自動的に駐車することである。駐車タスクは、以下のように定式化することができる。車両動態、駐車空間のマップ、車両の開始位置及び開始方位を表す初期状態、並びに車両の目標駐車位置及び目標駐車方位を表す目標状態が与えられると、車両の初期状態から目標状態への所望の経路又は運動を求め、次いで、車両のアクチュエーター、例えば車両のガスペダル及びハンドルを制御して、車両が所望の経路又は運動を辿ることを確実にする。しかしながら、車両の運動についての非ホロノミック制約及び駐車施設又は駐車場等の駐車空間における一般的に狭小な自由空間に起因して、車両を自動的に駐車する経路計画は、困難を有する。

大半の既存の経路計画解決法は、特定の駐車シナリオに対処するか、又は駐車空間の特定のジオメトリを想定するのみである。例えば、特許文献１に記載の方法は、縦列駐車及び後方駐車の経路を計算する。特許文献２に記載の方法は、駐車経路の特別の構造を想定し、障害物を無視し、駐車空間の特定のジオメトリに基づいて経路を計算する。また、特許文献３に記載の方法は、縦列駐車に対処し、車両の初期状態が、事前にコード化された縦列駐車操作が開始されるいわゆる実現可能開始領域内にあることを必要とする。上述の方法は、リアルタイム経路生成を達成するが、多かれ少なかれ、障害物のタイプ、駐車空間のジオメトリ、及び駐車タスクを制限しており、全般的な駐車シナリオを考慮することができない。

米国特許第７７３７８６６号 米国特許第８４９７７８２号 米国特許第８８６２３２１号

したがって、実生活における多様な駐車シナリオに適した、車両を目標空間に自動的に駐車するシステム及び方法が必要とされている。

いくつかの実施形態の目的は、車両の自動化駐車システム及び方法のためのリアルタイム経路生成を可能にする経路計画方法を開示することである。いくつかの実施形態の別の目的は、実生活の狭小な駐車空間における多様な駐車シナリオについての自動駐車の具体例によって引き起こされる経路計画の計算コストを低減するような方法を提供することである。

いくつかの実施形態は、任意のレイアウトを有する駐車空間を通じて所望の経路を発見するのに、サンプリングに基づく経路計画方法を用いることができるという理解に基づいている。一方で、いくつかの実施形態は、高速探索ランダムツリー（ＲＲＴ：ｒａｐｉｄ−ｅｘｐｌｏｒｉｎｇ ｒａｎｄｏｍ ｔｒｅｅ）方法及びその変形等のサンプリングに基づく経路計画方法を、自動化駐車問題に適用する結果として、このタスクを計算上非実用的にする複数の無駄な計算がもたらされる可能性があるという認識に基づいている。一般的な駐車シナリオにおいて、駐車スポットは、車両の初期位置、例えば現在位置から比較的離れて位置する。加えて、駐車施設又は駐車場の空間等の駐車空間の狭小な自由空間及び複雑なマップに起因して、駐車スポットへの経路は、異なる形式及び形状を有する可能性がある。そのために、そのような経路を発見する目的で、経路の実現可能性について、駐車空間全体をサンプリングしてテストする必要があり得る。結果として、サンプリングに基づく経路計画方法の単純な適用により、全ての未探索状態空間が不必要にサンプリングされ、リアルタイム駐車応用の非実用的な解がもたらされる可能性がある。

いくつかの実施形態は、駐車問題を、少なくとも２つのステージを用いて解くことができるという認識に基づいている。第１のステージにおいて、車両の初期状態を目標状態と接続する運動学的経路（ｋｉｎｅｍａｔｉｃ ｐａｔｈ）が構築される。第２のステージにおいて、この運動学的経路を辿る実際の軌道が求められる。運動学的経路は、車両の動的モデルよりも単純である運動学的モデルを用いて構築することができるので、このような手法は有利である。

２ステージ問題分割により、当初の駐車問題を以下のものに再定式化することができる。車両のジオメトリ、駐車空間のマップ、及び車両の運動学的モデルが与えられると、本明細書において運動学的経路と称される、初期状態を目標状態に接続する、実現可能でありかつ衝突を伴わない（ｃｏｌｌｉｓｉｏｎ ｆｒｅｅ）経路を発見する。運動学的な実現可能性は、運動学的経路が初期状態をその初期値とする車両運動学の解であり、したがって、この運動学的経路を、車両が初期状態から開始して辿ることができることを確実にする。衝突を伴わないという特性は、車両が、運動学的経路に沿って運動する間、駐車空間において障害物と衝突しないことを保証する。

運動学的モデルが通常、車両の動的モデルよりも単純であるにもかかわらず、運動学的経路の運動学的実現可能性について駐車空間全体をサンプリングしてテストする必要がある場合、自動化駐車問題の解は、依然として計算上非実用的である可能性がある。高速道路を走行する車両についての経路計画と比較して、車両の自動化駐車システムの経路計画は、固有の特性を有する。すなわち、環境内の衝突を伴わない空間は、通常、非常に狭小であり、サンプリングに基づく方法が遥かに低速で駐車空間を探索することが必要とされる。結果として、衝突を伴わない空間を通じて２つの状態を接続する運動学的経路を構築することは、より困難である。

加えて、いくつかの実施形態は、駐車空間全体のサンプリング問題を、計画の部分問題のセットに分割することができるという全体的な理解に基づいている。例えば、初期状態及び目標状態の双方からの運動学的グラフの構築を開始すること、及び／又は、駐車空間を部分空間のセットに分割することが可能である。一方で、ランダム分割は、計算効率を高めない場合があり、断片的な及び／又は実現不可能な運動学的経路をもたらす場合がある。

いくつかの実施形態は、各部分問題の解を接続して、初期状態と目標状態とを接続する実現可能な運動学的経路を求めることができるように、駐車空間問題のサンプリングを部分問題のセットに分割する必要があるという認識に基づいている。加えて、部分問題のセットへの分割は、駐車空間の分割状態をサンプリングする方法についての指針を提供するべきである。これは、一般的なサンプリング問題において、サンプリングされた状態は、初期状態と目標状態との間の比較的大きな距離に起因して、駐車空間において構築される運動学的ツリーの近くにあり、このツリーに追加される確率が低いためである。この低い確率は、運動学的ツリーへの近接性を求めるために行われる計算が無駄になる可能性の方が高いことを意味する。近接性の検出における計算の無駄を回避するために、サンプリングされた状態は、双方のツリーから離れているものの、運動学的ツリーの近傍として扱うことができる。一方で、そのように扱うことで、衝突検出ステージにおいて無駄な計算が増加する。これは、距離のあるサンプリングされた状態を、運動学的ツリーへの近傍として扱う場合、サンプリングされた状態と運動学的ツリーとの間の衝突を伴わない接続を有する確率が低いためである。低確率イベントを検出するのに適用されるいずれの計算も、無駄になる可能性が最も高く、したがって、低計算効率を招く。

いくつかの実施形態は、車両の初期状態を、通過点のセットを通じて駐車車両の目標状態と接続する衝突を伴わない幾何学的経路（ｇｅｏｍｅｔｒｉｃ ｐａｔｈ）を用いて、運動学的経路を構築するためにサンプリング問題を部分問題のセットに分割することができ、それにより、サンプリングの計算効率を高めるとともに実現可能な運動学的経路を提供することができるという理解に基づいている。

幾何学的経路は、車両のジオメトリ及び駐車空間のマップのみを用いて、すなわち、車両の運動学的モデル又は動的モデルを考慮することなく構築することができる。このようにして、幾何学的経路の計算が単純化される。幾何学的経路の衝突を伴わないという特性は、衝突を伴わない運動学的経路を幾何学的経路から開始して構築することができる可能性を高める。加えて、各通過点が車両の位置及び方位を規定する通過点の対は、サンプリング問題の有利な分割を提供する。

具体的には、車両の運動学的モデルを用いて、近傍の通過点の各対を接続する運動学的サブグラフのセットを求めて運動学的グラフを形成することができる。例えば、１つの実施形態において、２つの隣接した通過点が、例えば、一本の線の直線線分又は異なる幾何学的形状を用いて接続される。そのような場合、各部分問題は、「２つの隣接した通過点を接続する運動学的経路を発見する」こととして定式化することができる。全ての部分問題を解くことができる場合、初期状態は、運動学的経路を通じて目標状態に接続することができる。これは、当初の経路計画問題が解かれることを更に暗示する。

とりわけ、近傍の通過点は、通常、互いの方が車両の初期状態及び目標状態よりも近くにあるので、駐車空間のバイアスサンプリングを用いて各運動学的サブグラフを構築することができる。そのために、状態の各新たなサンプルは、運動学的サブツリーのうちの１つに追加される可能性の方が高く、したがって、計算の無駄を低減する。

１つの実施形態において、部分問題は、逐次的に解かれる。例えば、部分問題１は、「初期ノードと第２の通過点に対応するノードとを接続する運動学的サブグラフＳＧ_１を発見する」こととして構築され、部分問題２は、「運動学的サブグラフＳＧ_１を第３の通過点と接続する運動学的サブグラフＳＧ_２を発見する」こととして構築され、最終的に、最後の部分問題は、「最後から１つ前の運動学的サブグラフを最後の通過点と接続する運動学的グラフを発見する」こととして構築される。別の実施形態において、部分問題は並列的に解かれ、したがって、計算速度を桁違いに高速化する。並列的に解かれる部分問題は、逐次的に解かれる部分問題と異なるものとすることができる。

１つの実施形態は、バイアスされたサンプリングを用いて、運動学的グラフを構築するために状態サンプリングを導く。付加的又は代替的に、１つの実施形態は、近似的な到達可能セットをサンプリングステップに用いて、サンプリングされた状態の品質を改善する。近似的な到達可能セットを考慮に入れることによって、接続するのが困難であるか又はコストがかかるサンプリングされた状態は、拒絶される。この処理は、非効率なサンプルに起因した衝突検出における計算時間の無駄を低減することができ、また、運動学的グラフの高速な構築を可能にする通過点のセットを提供することができる。

したがって、１つの実施形態は、車両を駐車空間内に駐車する方法であって、方法は、車両のジオメトリ、駐車空間のマップ、車両の運動学的モデル、及び車両の動的モデルを記憶するメモリに結合されたプロセッサを使用し、プロセッサは、方法を実施する記憶命令に結合され、記憶命令は、プロセッサによって実行されると、方法の少なくともいくつかのステップを実行し、少なくともいくつかのステップは、
車両のジオメトリ及び駐車空間のマップを用いて、車両の初期状態を、通過点のセットを通じて駐車車両の目標状態と接続する衝突を伴わない幾何学的経路を求めることであって、各通過点は、車両の位置及び方位を規定することと、
車両の運動学的モデルを用いて、運動学的エッジと接続された複数のノードを有する運動学的グラフを形成する運動学的サブグラフのセットを求めることであって、各運動学的サブグラフは、幾何学的経路の近傍の通過点の対を接続し、各ノードは、車両の状態を規定し、２つのノードを接続する各運動学的エッジは、車両の運動学的特性に従って２つのノードを接続する衝突を伴わない運動学的経路を規定することと、
運動学的グラフから、車両の初期状態に対応する初期ノードを、中間ノードのセットを通じて、車両の目標状態に対応する目標ノードと接続する運動学的経路を選択することと、
車両の動的モデルを用いて、運動学的経路を時間の関数として追跡する基準軌道を求めることと、
基準軌道に従って車両の運動を制御することと、
を含み、
幾何学的経路を求めることは、
初期ノードを、衝突を伴わないノードのセットを通じて目標ノードと接続する幾何学的グラフを構築することであって、幾何学的グラフにおけるノードの各対は、車両及び駐車空間のジオメトリのみを用いて求められた衝突を伴わないエッジで接続されることと、
幾何学的経路の通過点のセットを形成する幾何学的グラフからノードのセットを選択することと、
を含み、
駐車空間の状態空間内の点をサンプリングして、サンプリングされた状態を生成することと、
幾何学的グラフのノードの全ての状態がサンプリングされた状態の到達不可能エリア内にある場合、サンプリングされた状態を拒絶することと、
そうではない場合、
サンプリングされた状態に最も近い状態を有する幾何学的グラフの最近傍のノードを求めることと、
サンプリングされた状態のノードを幾何学的グラフに追加すること、及び追加されたノードを、エッジが衝突を伴わない場合にエッジを介して最近傍のノードと接続することと、
サンプリングすること、拒絶すること、求めること、及び追加することを、初期ノードが目標ノードに接続されるまで繰り返すことと、
を更に含む、方法を開示する。

別の実施形態は、車両を駐車空間内に駐車するシステムであって、
車両のジオメトリ、駐車空間のマップ、車両の運動学的モデル、及び車両の動的モデルを記憶するメモリと、
メモリに結合されたプロセッサであって、
車両のジオメトリ及び駐車空間のマップを用いて、車両の初期状態を、通過点のセットを通じて駐車車両の目標状態と接続する衝突を伴わない幾何学的経路を求めることであって、各通過点は、車両の位置及び方位を規定することと、
車両の運動学的モデルを用いて、運動学的エッジと接続された複数のノードを有する運動学的グラフを形成する運動学的サブグラフのセットを求めることであって、各運動学的サブグラフは、幾何学的経路の近傍の通過点の対を接続し、各ノードは、車両の状態を規定し、２つのノードを接続する各運動学的エッジは、車両の運動学的特性に従って２つのノードを接続する衝突を伴わない運動学的経路を規定することと、
運動学的グラフから、車両の初期状態に対応する初期ノードを、中間ノードのセットを通じて、車両の目標状態に対応する目標ノードと接続する運動学的経路を選択することと、
車両の動的モデルを用いて、運動学的経路を時間の関数として追跡する基準軌道を求めることと、
を行うように構成された、プロセッサと、
基準軌道に従って車両の運動を制御するコントローラーと、
を備え、
プロセッサは、
駐車空間の状態空間内の点をサンプリングして、サンプリングされた状態を生成することであって、サンプリングすることは、通過点の状態の関数として平均及び共分散を有する駐車空間の状態の確率分布を用いてバイアスして実行されることと、
幾何学的グラフのノードの全ての状態がサンプリングされた状態の到達不可能エリア内にある場合、サンプリングされた状態を拒絶することと、
そうではない場合、
サンプリングされた状態に最も近い状態を有する幾何学的グラフの最近傍のノードを求めることと、
サンプリングされた状態のノードを幾何学的グラフに追加すること、及び追加されたノードを、エッジが衝突を伴わない場合にエッジを介して最近傍のノードと接続することと、
サンプリングすること、拒絶すること、求めること、及び追加することを、初期ノードが目標ノードに接続されるまで繰り返すことと、
を行うように構成される、システムを開示する。

更に別の実施形態は、方法をプロセッサによって実行させるためのプログラムが格納された非一時的コンピューター可読記憶媒体を開示する。

いくつかの実施形態によって対処される駐車シナリオの一例の図である。 いくつかの実施形態による、車両の幾何学的表現の一例示的な概略図である。 図１Ａの駐車シナリオに対応する駐車空間のマップの概略図である。 １つの実施形態による自動化駐車システムの機能図である。 １つの実施形態による運動計画システムの全体構造の図である。 いくつかの実施形態による運動計画のステージのブロック図である。 いくつかの実施形態による、運動的経路（ｋｉｎｅｔｉｃ ｐａｔｈ）の計画部分問題のセットへの分割決定の概略図である。 いくつかの実施形態による、運動学的経路計画問題を部分問題のセットに分割することのブロック図である。 いくつかの実施形態による、運動学的経路計画問題を部分問題のセットに分割する通過点のセットを有する、図１Ａの駐車シナリオの概略図である。 いくつかの実施形態による、車両を駐車空間内に駐車する方法のブロック図である。 １つの実施形態による、運動学的グラフを形成する運動学的サブグラフのセットの一例の図である。 いくつかの実施形態による、幾何学的経路を求める方法のフローチャートである。 １つの実施形態による、幾何学的グラフを構築することの概略図である。 いくつかの実施形態による、車両の到達不可能エリアの例の概略図である。 １つの実施形態による、幾何学的グラフを構築する方法のフローチャートである。 １つの実施形態による、運動学的経路を選択する方法のブロック図である。 １つの実施形態による、通過点のセットによって規定される運動学的経路を用いて、計画問題を部分問題のセットに分割することのブロック図である。 図６Ａの部分問題に対処するように構築された運動学的サブグラフの概略図である。 図６Ａの部分問題に対処するように構築された運動学的サブグラフの概略図である。 図６Ａの部分問題に対処するように構築された運動学的サブグラフの概略図である。 １つの実施形態による、部分問題を逐次的に解く方法のブロック図である。 通過点のセットを用いて部分問題を定式化する代替的な一実施形態の概略図である。 １つの実施形態による、運動学的グラフを求める方法のブロック図である。 １つの実施形態による、部分問題を解く一例示的な方法のブロック図である。 １つの実施形態による、バイアスされたサンプリングの一実施態様のブロック図である。 １つの実施形態による、バイアスされたサンプリングの方法のブロック図である。 １つの実施形態による、運動学的グラフから運動学的経路を選択する方法のブロック図である。 １つの実施形態による、初期運動学的経路を平滑化するフローチャートである。 １つの実施形態による、図１１Ａのフローチャートの一ブロックによって実行される方法のフローチャートである。 図１Ａに示す駐車シナリオの運動学的経路のプロットである。 運動学的経路に沿った車両１００の運動を示すプロットである。 １つの実施形態による、運動学的経路を更に処理して基準軌道を生成する方法のブロック図である。 本発明の１つの実施形態によるシステムの概略図である。 いくつかの実施形態による自動化駐車システムのブロック図である。

図１Ａは、いくつかの実施形態によって対処される駐車シナリオの一例を示している。この例において、駐車空間１５０の境界は、Ｌ×Ｈのサイズを有する矩形によって表されている。車両１００は、初期状態１０１、例えば、現在状態を有し、目標状態１０２によって規定された駐車スポットに駐車される必要がある。各状態、例えば、初期状態及び目標状態は、車両の位置及び方位を規定する。駐車空間１５０は、障害物を含む。障害物は、駐車空間のレイアウトの一部、すなわち、駐車空間の壁及び／又は柱等の常設の（ｐｅｒｍａｎｅｎｔ）障害物１０３とすることができる。常設の障害物の寸法は、通常既知である。図１Ａは、そのような寸法の非限定的な例を示している。

付加的又は代替的に、障害物は、他の駐車中であるか又は運動している車両１０４を含むことができる。車両１０４及び駐車することになる車両１００の幾何学的寸法は、車両のタイプに基づいて求めることができる。明確にするために、本開示は、前輪駆動車両を検討するが、異なる実施形態は、後輪駆動車両及び全輪駆動（ｆｕｌｌ ｗｈｅｅｌ ｄｒｉｖｅ：四輪駆動）車両を含む他の車両に適用される。

いくつかの実施形態において、駐車空間のレイアウト及び障害物１０３の位置は、駐車空間のマップ上で指定される。そのようなマップは、所定のものとすることもできるし、駐車中又は駐車前にリアルタイムで構築することもできる。種々の実施形態は、初期状態と目標状態とを接続する運動学的経路１０５が実現可能でありかつ衝突を伴わないものであるように、この運動学的経路を求める。

図１Ｂは、いくつかの実施形態による、車両の幾何学的表現の一例示的な概略図を示している。この例において、車両は、矩形１１５として抽象化されている。車両状態は、この車両の後輪軸の中点を表す位置（ｘ，ｙ）１１０、及び、車体軸と水平軸との間の角度を示す方位θ１２０を含む。

車両が経路に沿って運動する速度を求める運動計画は、車両１００の動的モデルを用いる。本明細書において用いられる場合、車両の動的モデルは、車両の状態における時間依存変化を説明する。動的モデルは、通常、微分方程式によって表される。１つの実施形態において、車両の動的モデルは、５次の微分方程式

であり、ここで、ｖは、前輪の速度であり、ζは、前輪と車両方位との間の角度であり、ａ_１は、並進加速度であり、ａ_２は、操舵角速度であり、ｌは、（ｘ，ｙ）と前輪同士の中点との間の距離である。

経路計画のために、いくつかの実施形態は、車両の質量又は運動を引き起こす力を考慮しない車両の運動を記述する車両の運動学的モデルを用いる。

１つの実施形態において、以下の運動学的モデルが考慮される。

ここで、ｕ_１＝ｃｏｓ（ζ）ｖは、後輪軸の中点の速度であり、ｕ_２＝ｔａｎ（ζ）ｕ_１／ｌである。

或る経路が運動学的モデル（２）の解である場合、その経路は運動学的に実現可能である。車両状態Ｘ＝（ｘ，ｙ，θ）は、位置Ｘに配置される車両がいかなる障害物とも衝突せず、かつ駐車空間の境界の内側に完全に収まる場合にのみ、衝突を伴わないものである。初期状態１０１は、Ｘ_０＝（ｘ_０，ｙ_０，θ_０）として略式表現され（ａｂｂｒｅｖｉａｔｅｄ）、目標状態１０２は、Ｘ_ｆ＝（ｘ_ｆ，ｙ_ｆ，θ_ｆ）によって示される。矩形Ｌ×Ｈによって表される駐車空間での特定の駐車タスクについて、車両状態は、状態空間

に常に属する。

いくつかの実施形態は、車両状態が衝突を伴わないものであるか否かを、駐車空間のマップとも称される、駐車空間の幾何学的表現に基づいて判断する。１つの実施形態において、駐車空間のマップは、全ての障害物及び駐車空間の境界を単純な幾何学的形状として近似することによって導出される。１つの実施形態において、環境（駐車空間と同等）内の障害物１０３は、障害物ごとに最小バウンディングボックスを構築することによって導出される矩形として近似することができる。障害物及び駐車空間の境界の幾何学的近似を用いて、駐車空間又は環境を、幾何学的オブジェクトのリストによって完全に記述することができる。

図１Ｃは、図１Ａの駐車シナリオに対応する駐車空間のマップ１３０を示している。双方のタイプの障害物が、矩形表現を用いて１つのタイプの常設障害物１０３としてともに統合される。障害物は、経路計画のために、搭載プロセッサの計算能力（ｃｏｍｐｕｔｉｎｇ ｐｏｗｅｒ）に依拠して、複数の幾何学的形状によって近似することができる。例えば、別の実施形態において、計算能力が十分ではない場合、障害物は、障害物ごとにバウンディングサークルを構築することによって近似することができる。代替的に、障害物は、ポリトープによって近似することができるが、これにより、経路計画における計算負荷を高める場合がある。また、駐車空間内の障害物は、同一の幾何学的形状によって近似されない場合もある。

図２Ａは、１つの実施形態による自動化駐車システムの機能図を示している。環境マッピング及びローカライゼーションブロック２０１は、駐車空間のマップを構築又は更新し、環境及び車両動作状態（ｖｅｈｉｃｌｅ ｏｐｅｒａｔｉｏｎ ｃｏｎｄｉｔｉｏｎ）を検知することによって車両の現在ロケーションを求める。例えば、車両動作を検知するのに、３軸加速度計（複数の場合もある）、３軸ジャイロスコープ（複数の場合もある）、及び／又は磁力計（複数の場合もある）を備えることができる慣性測定ユニットを用いることができる。車両の位置及び速度を提供するのに全地球測位システムセンサーを用いることができる。環境２００を検知するセンサーは、他の車両、歩行者、及び建物を含む障害物を捕捉するビデオカメラ、車両と障害物との間の距離を検出する超音波／レーダーセンサー、等とすることができる。１つの実施形態において、環境マップは、図１Ｃに示す駐車空間の幾何学的表現を生成するために更に処理される。

目標状態選択ブロック２０２は、車両を駐車する駐車スポットのための目標状態を、駐車場候補を識別することによって選択し、この目標状態を運動計画ブロック２０３に送信する。１つの実施形態において、利用可能な駐車スポットは、駐車施設の管理に関連付けられた別個のシステムによって追跡される。付加的又は代替的に、駐車スポットは、自動化駐車システムのセンサー２３０を用いて検出することができる。１つの実施形態において、運動計画ブロックは、チェックを行って、目標状態が、駐車可能（ｐａｒｋａｂｌｅ）、すなわち、駐車スポットまでの実現可能経路があるか否かを判断し、目標状態選択ブロック２０２にチェック結果を通知する。目標状態が駐車可能でない場合、目標状態選択ブロック２０２は、評価のために別の目標状態を選択する。別の実施形態において、目標状態選択ブロック２０２は、目標状態が駐車可能であるか否かを評価し、駐車可能な目標状態のみを運動計画ブロックに送信することもできる。

目標状態が駐車可能である場合、運動計画２０３は、全運動計画手順を開始し、車両モデル２１０、車両の初期状態及び目標状態、並びに駐車空間のマップに基づいて基準軌道２４１を求める。１つの実施形態において、基準軌道は、車両速度及び操舵角の経時的なプロファイルを規定する。別の実施形態において、基準軌道は、車両状態（ｘ，ｙ，θ）の経時的なプロファイルを規定する。

基準軌道２４１が与えられると、車両コントローラー及びアクチュエーター２０４は、制御コマンドを求めるとともに実行して、基準軌道が状態プロファイルである場合、車両状態が基準軌道２４１を追跡することを強制し、又は、基準軌道が車両速度プロファイル及び操舵角プロファイルである場合、車両速度及び操舵角が基準軌道を追跡することを強制する。１つの実施形態において、制御コマンドは、ガスペダル圧力又は操舵トルクとすることができる。車両コントローラー／アクチュエーターは、制御コマンドを求めるために信号２４３も用いることができる。信号２４３は、測定操舵角、又はハンドル若しくはガスペダルを動かすモーターの測定電流とすることができる。

図２Ｂは、１つの実施形態による運動計画システム２０３の全体図を示している。運動計画システム２０３は、この運動計画システム２０３のモジュールを実行する少なくとも１つのプロセッサ２７０を備える。プロセッサ２７０は、車両のジオメトリ及び駐車空間のマップ等の幾何学的情報２８１を記憶するメモリ２８０に接続される（２７１）。メモリ２８０は、車両の運動学的モデル及び車両の動的モデル等の車両のモデル２８２も記憶することができる。メモリ２８０は、限定されるものではないが、車両の初期状態、駐車車両の目標状態、コスト関数、それぞれの計算された状態の値、各状態に至る運動、幾何学的グラフ、運動学的グラフ、通過点、基準軌道を含む、運動計画器の内部情報２８３も記憶することができる。いくつかの実施形態において、メモリ２８０は、自動化駐車の方法を実施する記憶命令を含むことができ、ここで、命令は、プロセッサ２７０によって実行されると、方法の少なくともいくつかのステップを実行する。

いくつかの実施形態は、駐車問題を、少なくとも２つのステージを用いて解くことができるという認識に基づいている。第１のステージにおいて、車両の初期状態を目標状態と接続する運動学的経路が構築される。第２のステージにおいて、この運動学的経路を辿る実際の軌道が求められる。運動学的経路は、車両の動的モデルよりも単純である運動学的モデルを用いて構築することができるので、このような手法は有利である。

図３Ａは、いくつかの実施形態による運動計画２０３のステージのブロック図を示している。ブロック３０１は、車両のジオメトリ及び駐車空間のマップ１３０、車両の初期状態１０１及び目標状態１０２、並びに車両の運動学的モデル３０３に基づいて運動学的経路を求める。運動学的経路は、ブロック３０２において更に処理されて、車両の動的モデル３０４の解である基準軌道が生成される。

このような問題分割により、当初の駐車問題を以下の問題に再定式化することができる。車両のジオメトリ、駐車空間のマップ、及び車両の運動学的モデルが与えられると、本明細書において運動学的経路と称される、初期状態を目標状態に接続する、実現可能でありかつ衝突を伴わない経路を発見する。運動学的な実現可能性は、運動学的経路が初期状態をその初期値とする車両運動学の解であり、したがって、この運動学的経路を、車両が初期状態で開始して辿ることができることを確実にする。衝突を伴わないという特性は、車両が、運動学的経路に沿って運動する間、駐車空間において障害物と衝突しないことを保証する。

運動学的モデルが通常、車両の動的モデルよりも単純であるにもかかわらず、運動学的経路の運動学的実現可能性について駐車空間全体をサンプリングしてテストする必要がある場合、自動化駐車問題の解は、依然として計算上非実用的である可能性がある。高速道路を走行する車両についての経路計画と比較して、車両の自動化駐車システムの経路計画は、固有の特性を有する。すなわち、環境内の衝突を伴わない空間は、通常、非常に狭小であり、サンプリングに基づくアルゴリズムが遥かに低速で駐車空間を探索することが必要とされる。結果として、衝突を伴わない空間において２つの状態を接続する運動学的経路をリアルタイムで構築することは、より困難である。

図３Ｂは、駐車空間全体の状態のサンプリングを介した運動学的経路３０１の、計画部分問題３１２のセットへの分割（３１１）決定の概略図を示している。運動学的経路３１３は、部分問題３１２の解の和集合から選択される。

いくつかの実施形態は、各部分問題の解を接続して、初期状態と目標状態とを接続する実現可能な運動学的経路を求めることができるように、駐車空間問題のサンプリングを部分問題のセットに分割する必要があるという認識に基づいている。加えて、部分問題のセットへの分割は、分割された駐車空間をサンプリングする方法についての指針を提供するべきである。これは、一般的なサンプリング問題において、サンプリングされた状態は、初期状態と目標状態とが比較的離れている場合、駐車空間において構築される運動学的ツリーの近くにあり、このツリーに追加される確率が低いためである。この低い確率は、運動学的ツリーへの近接性を求めるために行われる計算が無駄になる可能性の方が高いことを意味する。近接性の検出における計算の無駄を回避するために、サンプリングされた状態は、双方のツリーから離れているものの、運動学的ツリーの近傍として扱うことができる。一方で、そのように扱うことで、必然的に、衝突検出においてより多くの計算の無駄がもたらされる。これは、双方のツリーから比較的離れていても運動学的ツリーに対する近傍として扱われるサンプリングされた状態について、このサンプリングされた状態と双方のツリーとの間の衝突を伴わない接続を有する確率は低いためである。低確率イベントを検出するのに適用されるいずれの計算も、無駄になる可能性が最も高く、したがって、低計算効率を招く。

いくつかの実施形態は、車両の初期状態を、通過点のセットを通じて駐車車両の目標状態と接続する衝突を伴わない幾何学的経路を用いて、運動学的経路を構築するために、サンプリング問題を部分問題のセットに分割することができ、それにより、サンプリングの計算効率を高めるとともに実現可能な運動学的経路を提供することができるという理解に基づいている。

図４Ａは、運動学的経路計画問題を部分問題のセットに分割することのブロック図を示している。この分割することは、車両の初期状態及び目標状態及びジオメトリ、並びに駐車空間に基づいて通過点のセットを生成するブロック４１１に依拠している。各通過点は、中間目標（ｍｉｌｅｓｔｏｎｅ）車両状態を規定する。通常、最初の通過点は初期状態Ｘ_０に対応し、最後の通過点は目標状態Ｘ_ｆに対応する。また、通過点のセットは、以下の接続特性、すなわち、全ての通過点が衝突を伴わないエッジと接続されることを満たすものとし、ここで、エッジは、２つのノード（又は状態）間の幾何学的接続又は運動学的接続を表すことができる。１つの実施形態において、２つの任意の状態Ｘ_１、Ｘ_２が与えられると、これらの状態の間の幾何学的エッジは、直線とすることができ、これらの状態の間の運動学的エッジは、車両運動学（２）と、０≦ｔ_１≦ｔ_２＜∞で範囲付けされた任意のものについて、境界条件Ｘ（ｔ_１）＝Ｘ_１、Ｘ（ｔ_２）＝Ｘ_２とを満たす任意の実現可能な解とすることができる。

例えば、通過点を介した幾何学的経路は、車両のジオメトリ及び駐車空間のマップのみを用いて、すなわち、車両の運動学的モデル又は動的モデルを考慮することなく構築することができる。このようにして、幾何学的経路の計算が単純化される。幾何学的経路の衝突を伴わないという特性は、衝突を伴わない運動学的経路を幾何学的経路の上に構築することができる可能性を高める。加えて、各通過点が車両の位置及び方位を規定する通過点の対は、サンプリング問題の有利な分割を提供する。

図４Ｂは、運動学的経路計画問題を部分問題のセットに分割するのに用いられる通過点のセットを有する、図１Ａの駐車シナリオの概略図を示している。通過点のセットは、車両状態Ｘ_０，．．．，Ｘ_５を規定し、ここで、各通過点は、車両の位置及び方位を含む車両状態を規定する。通過点Ｘ_０及びＸ_５は、それぞれ初期状態及び目標状態に対応する。例えば、状態Ｘ_１に対応する通過点は、車両の位置４２０及び方位４２５を規定する。同様に、状態Ｘ_２に対応する通過点は、車両の位置４３０及び方位４３５を規定する。これらの通過点は、衝突を伴わないものであり、衝突を伴わない幾何学的エッジ、例えば、直線４２７及び４３７と接続される。

いくつかの実施形態は、車両の運動学的モデルを用いて、近傍の通過点の各対を接続する運動学的サブグラフのセットを求めて運動学的グラフを形成する。例えば、各部分問題は、「２つの隣接した通過点を接続する運動学的経路を発見する」こととして定式化することができる。全ての部分問題を解くことができる場合、初期状態は、運動学的経路を通じて目標状態に接続することができる。これは、当初の経路計画問題が解かれることを更に暗示する。

図４Ｃは、車両を駐車空間内に駐車する方法のブロック図を示している。この方法は、車両のジオメトリ及び駐車空間のマップ４４０を記憶するメモリに結合されたプロセッサを用いる。メモリは、車両の運動学的モデル３０３及び車両の動的モデル３０４も記憶することができる。プロセッサは、方法を実施する記憶命令に結合され、ここで、命令は、プロセッサによって実行されると、方法の少なくともいくつかのステップを実行する。

方法は、車両のジオメトリ及び駐車空間のマップ４４０を用いて、車両の初期状態を、通過点のセットを通じて駐車車両の目標状態と接続する、衝突を伴わない幾何学的経路４５５を求める（４５０）。各通過点は、例えば、図４Ｂに示すような車両の位置及び方位を規定する。方法は、車両の運動学的モデル３０３を用いて、運動学的エッジと接続された複数のノードを有する運動学的グラフを形成する運動学的サブグラフのセットを求める（４６０）。各運動学的サブグラフは、近傍の通過点の対を接続し、各ノードは、車両の状態を規定し、２つのノードを接続する各運動学的エッジは、車両の運動学的特性に従って２つのノードを接続する、衝突を伴わない運動学的経路を規定する。

図４Ｄは、１つの実施形態による、運動学的グラフを形成する運動学的サブグラフのセットの一例を示している。この例において、運動学的グラフ４４５は、４つの運動学的サブグラフ４４６、４４７、４４８、及び４４９によって形成される。例えば、サブグラフの各々は、反復的に、例えば、通過点のセットに対応するシードのセットから反復的に、成長させることができる。

とりわけ、近傍の通過点は、通常、互いの方が車両の初期状態及び目標状態よりも近くにあるので、駐車空間のバイアスサンプリングを用いて各運動学的サブグラフを構築することができる。そのために、状態の各新たなサンプルは、運動学的サブツリーのうちの１つに追加される可能性の方が高く、したがって、計算の無駄を低減する。

１つの実施形態において、部分問題は、逐次的に解かれる。例えば、部分問題１は、第１の通過点Ｘ_０に対応する初期ノードと第２の通過点Ｘ_１に対応するノードとを接続する運動学的サブグラフＳＧ_１を発見することとして構築され、部分問題２は、運動学的サブグラフＳＧ_１を第３の通過点Ｘ_２と接続する運動学的サブグラフＳＧ_２を発見することとして構築され、以下同様であり、最終的に、最後の部分問題は、最後から１つ前の運動学的サブグラフＳＧＭ−１を、最後の通過点Ｘ_Ｍと接続する運動学的グラフを発見することとして構築される。別の実施形態において、部分問題は並列的に解かれ、したがって、計算速度を桁違いに高速化する。並列的に解かれる部分問題は、逐次的に解かれる部分問題とは異なるものとすることができる。

図５Ａは、いくつかの実施形態による、幾何学的経路を求める方法のフローチャートを示している。方法は、初期ノードを、衝突を伴わないノードのセットを通じて目標ノードと接続する幾何学的グラフＧＧを構築する（５０１）。幾何学的グラフ内のノードの各対は、車両及び駐車空間のジオメトリのみを用いて求められる、衝突を伴わないエッジと接続される。幾何学的グラフは、幾何学的グラフ内のノード間のエッジが運動学的ではなく幾何学的であることを除いて運動学的グラフに類似している。幾何学的エッジを利用することにより、幾何学的グラフは、運動学的グラフよりも遥かに高速に構築することができる。次に、方法は、幾何学的経路の通過点のセットを形成する幾何学的グラフから、ノードのセットを選択する（５０２）。

例えば、１つの実施形態において、幾何学的グラフは、車両及び駐車空間のジオメトリに従った衝突を伴わない接続のために、例えば一様に又はランダムに駐車空間の状態空間をサンプリングして、サンプリングされた近傍の状態間の接続をテストすることによって構築される。単一の衝突を伴わない接続を有しないサンプリングされた状態は剪定され、サンプル状態の残りが幾何学的グラフのノードを形成する。

図５Ｂは、１つの実施形態による、幾何学的グラフを構築することの概略図を示している。この例において、幾何学的グラフは、初期状態１０１に対応する初期ノード５１１を有する初期幾何学的ツリーＧＴ_ｉ５２１と、目標状態１０２に対応する目標ノード５１２を有する目標幾何学的ツリーＧＴ_ｔ５２２とを用いて双方の端点から構築される。初期幾何学的ツリーは、ノードセットＶ_ＧＴｉ及びエッジセットＥ_ＧＴｉを有する。目標幾何学的ツリーは、ノードセットＶ_ＧＴｔ及びエッジセットＥ_ＧＴｔを有する。

現在の反復中、Ｘ＝（ｘ，ｙ，θ）で示す状態５１０は、或る特定のサンプリングスキームに従ってサンプリングされ、衝突を伴わないものであることが検証されている。次に行われるのは、最近傍のノード、例えば、初期幾何学的ツリー上に位置するノード５３０を求めることである。初期ツリー５２１からの最近傍のノード５３０とサンプリングされた状態５１０との間の幾何学的エッジ５３５が衝突を伴わないものである場合、サンプリングされた状態５１０に対応する新たなノードが作成されて初期幾何学的ツリーのノードセットに追加されるとともに、エッジ５３５が初期幾何学的ツリーのエッジセットに追加される。

サンプリングされた状態５１０について、目標幾何学的ツリーに同様の手順が適用される。すなわち、目標幾何学的ツリー５２２上に位置する最近傍のノード５４０が特定される。ノード５４０とサンプリングされた状態との間の幾何学的エッジ５４５が衝突を伴わないものである場合、サンプリングされた状態に対応する新たなノードが作成されて目標幾何学的ツリーのノードセットに追加されるとともに、エッジ５４５が目標幾何学的ツリーのエッジセットに追加される。エッジ５３５及び５４５の双方が衝突を伴わないものである場合、初期幾何学的ツリーと目標幾何学的ツリーとが接続され、その場合、初期ノードと目標ノードとが接続される。そのために、幾何学的グラフは、初期幾何学的ツリーと目標幾何学的ツリーとの和集合である。

付加的又は代替的に、いくつかの実施形態は、到達可能性基準を用いてサンプリングを実行する。到達可能性基準に従って、状態空間内のサンプルは、このサンプルが既に構築されているグラフから到達可能である場合にのみ保存される。１つの実施形態において、到達可能性をテストするのに車両の動態の使用を回避するために、到達可能性は、到達不可能性（ｎｏｎ−ｒｅａｃｈａｂｉｌｉｔｙ：到達不可能範囲）の欠如として定義され、到達不可能性は、車両の側部付近の所定のエリアである。

図５Ｃは、車両の位置及び方位を規定する状態５１０における、車両１００の到達不可能エリア５５３及び５５７の例の概略図を示している。この例において、到達不可能エリアは、車両の車輪を最大限左又は右に向けた状態で、状態５１０において開始して所定の速度で運動する車両運動の円によって表される。以前にサンプリングされた状態５３０が到達不可能エリア５５３及び５５７内にある場合、状態５１０は、状態５３０のノードに接続されない。幾何学的グラフの全てのノードがサンプリングされた状態の到達不可能エリア内にある場合、そのサンプリングされた状態は、拒絶される。

図５Ｄは、１つの実施形態による、幾何学的グラフを構築する方法のフローチャートを示している。この実施形態において、サンプリングされた状態Ｘは、衝突を伴わないことがテストされることに加えて、通過点の品質を改善するために或る特定の運転可能性基準によって評価される。運転可能性基準は、運動学的グラフの構築において計算効率を改善することができる。

方法は、駐車場の状態空間内の点をサンプリングして（５５０）、サンプリングされた状態を生成する。幾何学的グラフのノードに対応する全ての状態がサンプリングされた状態の到達不可能エリア内にある場合（５６０）、サンプリングされた状態は拒絶される（５５５）。そうではない場合、方法は、サンプリングされた状態に最も近い状態を有する幾何学的グラフの最近傍のノードを求め（５７０）、サンプリングされた状態のノードを幾何学的グラフに追加して、この追加されたノードを、エッジが衝突を伴わない場合にこのエッジを介して最近傍のノードに接続する（５８０）。

方法は、サンプリングすること、拒絶すること、求めること、及び追加することを、初期ノードが目標ノードに接続されるまで繰り返す。例えば、１つの実施形態において、幾何学的グラフの構築は、初期幾何学的ツリーと目標幾何学的ツリーとが接続されている限り、停止される。別の実施形態において、幾何学的グラフの構築は、或る特定の数のサンプリングされた状態が初期幾何学的ツリーと目標幾何学的ツリーとの双方に追加されるまで、停止される。

幾何学的グラフは、初期ノードが目標ノードと接続されるような、ノード間の幾何学的接続を表すノード及びエッジを含む。幾何学的グラフが与えられると、初期状態から目標状態への多くの幾何学的経路が存在し得る。或る特定のコスト関数を低減する幾何学的グラフからの幾何学的経路を選択することが有利である。例えば、１つの実施形態は、２つのステップ、すなわち、ａ）ノードごとにコストを求めるステップであって、ここで、ノードコストは、そのノードによって指定される状態から目標状態への最小コストを表す、ステップと、ｂ）初期ノードから開始し、ノードコストに従ってノードのセットを選択するステップであって、ここで、初期ノードのコストは、車両がノードのセットを通過する場合にのみ最小に達する、ステップとを実行することによって幾何学的経路を選択する。

ノードコストは、エッジのコストによって求められる。それぞれ車両状態Ｘ_ｉ、Ｘ_ｊに対応する２つのノードＮ_ｉ、Ｎ_ｊを所与とし、Ｎ_ｉ及びＮ_ｊがエッジＥ_ｉ，ｊによって接続されると仮定する。エッジＥ_ｉ，ｊのコストは、ベクトルノルム：

と定義され、ここで、Ｑは、正定値行列である。この場合、Ｑは１であり、エッジコストは、ユークリッドベクトルノルムに還元される。幾何学的グラフの構築中に、上記のように全てのエッジのコストが計算される。任意のノードＮ_ｉについて、そのコストは、Ｖ（Ｎ_ｉ）によって示され、以下の価値反復を実行することによって得ることができる。

ここで、Ｉは、ノードＮ_ｉに接続されるノードのインデックスセットであり、Ｖ^ｋ（Ｎ_ｉ）は、第ｋの価値反復におけるノードコストＶ（Ｎ_ｉ）の値である。幾何学的グラフの１度の価値反復は、固定のｋについての全てのノードについて価値反復（３）を実行することを意味する。価値反復は、ノードコストの初期推測を要求し、これは、ｋ＝０におけるノードコストである。

１つの実施形態において、全てのノードについてＶ^０（Ｎ_ｉ）＝０である。ｋ→∞の場合、Ｖ^ｋ（Ｎ_ｉ）は、ノードＮ_ｉの最小コストに収束し、したがって、ノードの最小コストは、価値反復によって得ることができることが確立されている。価値反復は、反復的に行われ、この反復は、実用上、或る特定の基準に従って停止されなければならない。１つの実施形態において、反復は、ｋが事前設定された大きな正の数に達すると停止する。別の実施形態において、反復は、全てのノードについて｜Ｖ^ｋ（Ｎ_ｉ）−Ｖ^ｋ−１（Ｎ_ｉ）｜が或る閾値未満になると停止する。

価値反復からもたらされるノードＮ_ｉの最小コストをＶ（Ｎ_ｉ）と表すものとする。最良の幾何学的経路を表すノードのセットは、方策反復を実行することによって求められる。すなわち、Ｎ_ｉが最良の幾何学的経路のうちの１つのノードであると仮定すると、Ｎ_ｉの次のノードは、

を解くことによって求められる。

一例として、図５Ｅにおいて、初期状態に対応する初期ノードから開始するとともに目標ノードにおいて停止する方策反復を、幾何学的グラフに適用することにより、緑色の線によって表される最良の幾何学的経路、及び車両状態Ｘ_０，Ｘ_１，．．．，Ｘ_５に対応するノードのセットが与えられる。

別の実施形態において、最良の幾何学的経路は、３つのステップ、すなわち、ａ）初期ノード及び目標ノードを除く全ての葉ノードを除去することによって幾何学的グラフをトリミングするステップであって、ここで、葉ノードは１つのみの近傍のノードを有する、ステップと、ｂ）トリミング後に幾何学的グラフにわたって価値反復を実行することによって各ノードの最小コストを求めるステップと、ｃ）トリミング後に初期ノードから開始して、幾何学的グラフに従ってノードのセットを選択するステップとによって求められる。この実施形態は、最良の幾何学的経路がいずれの葉ノードも含まず、したがって、トリミング後に幾何学的グラフにわたって価値反復を実行することにより、全ての非葉ノードの同じ最小コストが与えられるという理解に基づいている。この実施形態は、ノードの最小コスト、及び最良の幾何学的経路を求める計算負荷を大幅に低減することができる。最良の幾何学的グラフを規定するノードのセットから通過点のセットを抽出することができる。

図５Ｅは、１つの実施形態による、幾何学的経路４５５を選択する方法のブロック図を示している。方法は、幾何学的グラフの初期ノード及び各非葉ノードについて、目標ノードに到達する最小コストを求め（５９１）、初期ノードを、初期ノードについて求められた最小コストを有する目標ノードと接続するノードのセットを選択して（５９３）幾何学的経路の通過点のセットを形成する。例えば、ノードのコストを求める前に、方法は、初期ノード及び目標ノードを除く葉ノードを幾何学的グラフから除去するとともに、除去された葉ノードを接続するエッジを幾何学的グラフから除去することができる。

図６Ａは、１つの実施形態による、通過点のセットによって規定される幾何学的経路４５５を用いて、計画問題を部分問題のセットに分割することのブロック図を示している。図６Ｂ、図６Ｃ、及び図６Ｄは、図６Ａの部分問題のセットに対処するように構築された運動学的サブグラフのセットを示している。例えば、第１の部分問題６０１は、Ｘ_０とＸ_１とを接続する、すなわち、初期状態を、第２の通過点と接続する第１の運動学的サブグラフＳＧ_１を構築することと定義される。図６Ｂは、運動学的サブグラフＳＧ_１がＸ_０とＸ_１とを運動学的に接続するように構築されることを示している。

第２の部分問題６０２は、第１の運動学的サブグラフＳＧ_１とＸ_２とを接続する、すなわち、第２の通過点を、第３の通過点と接続する第２の運動学的サブグラフＳＧ_２を構築することと定義される。図６Ｃは、運動学的サブグラフＳＧ_２がＳＧ_１とＸ_２とを運動学的に接続するように構築されることを示している。最後の部分問題Ｍ６０９は、ＳＧ_Ｍ−１とＸ_Ｍとを接続する運動学的グラフＫＧを構築することと定義される。図６Ｄは、運動学的サブグラフＳＧ_Ｍ−１と、運動学的グラフＫＧがＳＧ_Ｍ−１とＸ_Ｍとを運動学的に接続するように構築されることとを示している。上述の定義の部分問題は、プロセッサにおいて逐次的に実行することができる。

図７Ａは、１つの実施形態による、部分問題を逐次的に解く方法のブロック図を示している。方法は、セット４７５からの２つの隣接した通過点７１０ごとに運動学的グラフ７４０を逐次的に（７３０）構築する（７２０）。グラフ７４０が構築された後、方法は、成功７７０又は失敗７６０の終了条件が満たされるまで、次の対７１０に移る（７５０）。

図７Ｂは、通過点のセットを用いて部分問題を定式化する代替的な一実施形態の概略図を示している。部分問題７８０及び７９０は、それぞれ｛Ｘ_０，Ｘ_１｝，．．．，｛Ｘ_Ｍ−１，Ｘ_Ｍ｝を接続する運動学的サブグラフＳＧ_１，．．．，ＳＧ_Ｍを構築することと定義される。全てのサブグラフが接続されている限り、運動学的グラフはサブグラフの和集合と定義され、接続されている。この実施形態における部分問題は、並列的に解くことができる。

とりわけ、近傍の通過点は、通常、互いの方が車両の初期状態及び目標状態よりも近くにあるので、駐車空間のバイアスサンプリングを用いて各運動学的サブグラフを構築することができる。そのために、状態の各新たなサンプルは、運動学的サブツリーのうちの１つに追加される可能性の方が高く、したがって、計算の無駄を低減する。

図８は、１つの実施形態による、運動学的グラフを求める方法のブロック図を示している。方法は、通過点のセットに近接する駐車空間をサンプリングして（８１０）、サンプリングされた状態８１５を生成する。そのようにして、サンプリングされた状態は、通過点のうちの少なくとも１つの状態にバイアスされる。例えば、サンプリングは、通過点の状態の関数として平均及び共分散を有する駐車空間の状態の確率分布を用いて実行することができる。

方法は、サンプリングされた状態を、ツリーを更新するために、運動学的エッジが衝突を伴わないものである場合にこの運動学的エッジを用いて少なくとも１つの通過点から開始する少なくとも１つのツリーと接続し（８２０）、終了条件が満たされるまでサンプリングすること及び接続することを繰り返す（８３０）。例えば、終了条件は、接続された幾何学的グラフの形成、運動学的グラフ上で初期ノードと目標ノードとを接続する経路の数が或る閾値を超えること、近傍の通過点のツリーに接続するノードの数、のうちの１つ又は組み合わせを含むことができる。

図９Ａは、１つの実施形態による、部分問題ｉ＋１を解く一例示的な方法のブロック図を示している。部分問題ｉ＋１は、通過点Ｘ_ｉ及びＸ_ｉ＋１、駐車空間のジオメトリ、及び車両の運動学的モデルから運動学的サブグラフＳＧ_ｉ＋１を構築する（７２０）ことを強制する。サブグラフＳＧ_ｉ＋１は、サブグラフＳＧ_ｉとＸ_ｉ＋１とを接続するように構築される。例えば、サブグラフＳＧ_ｉは、根ノードが状態Ｘ_０に対応する初期運動学的ツリーとして見ることができる。目標運動学的ツリーＫＴ_ｔは、部分問題ｉ＋１の目標状態として扱われ、状態Ｘ_ｉ＋１を有する根ノードで初期化される。バイアスされたサンプリングブロック９０１は、確率密度関数Ｐ（Ｘ）に従って状態空間

をサンプリングし、衝突を伴わない状態

を返す。接続ブロック９０２は、サンプリングされた状態と、サブグラフＳＧ_ｉ及び運動学的目標ツリーＫＴ_ｔ上に位置する当該状態の最近傍のノードとの間の接続を行う。サンプリングされた状態は、衝突を伴わない運動学的エッジでサブグラフＳＧ_ｉの、当該状態の最近傍のノードに接続することができる場合にのみ、サブグラフＳＧ_ｉに追加される。同様に、サンプリングされた状態は、衝突を伴わない運動学的エッジで運動学的目標ツリーの、当該状態の最近傍のノードに接続することができる場合にのみ、このツリーに追加される。また、最近傍のノードとサンプリング状態を有するノードとを接続するエッジは、最近傍のノードが位置するツリーに追加される。

バイアスされたサンプリング９０１及び接続９０２を含む上述のステップは、或る特定の停止基準が満たされるまで繰り返される。１つの実施形態において、基準は、サンプリングされた状態ＸがサブグラフＳＧ_ｉ及び運動学的目標ツリーの双方に追加される場合である。別の実施形態において、基準は、サブグラフＳＧ_ｉ及び運動学的目標ツリーＫＴ_ｔの双方に追加されるサンプリングされた状態の数が或る閾値を超える場合である。

図９Ｂは、１つの実施形態による、バイアスされたサンプリング９０１の一実施態様のブロック図を与えている。確率密度関数Ｐ（Ｘ）は、まず、通過点Ｘ_ｉ及びＸ_ｉ＋１の関数として構築される（９１１）。ブロック９１２において、サンプリングされた状態が、その確率密度関数がＰ（Ｘ）に一致するように生成される。サンプリング９１２は、サンプリングされた状態が衝突を伴わないもの（９１３）になるまで（９１４）、繰り返される。

例えば、サンプリングは、通過点の状態の関数として平均及び共分散を有する駐車空間の状態の確率分布を用いて実行される。１つの実施形態において、確率分布は、ガウス分布である。

図９Ｃは、平均μ及び共分散σがＸ_ｉ及びＸ_ｉ＋１の関数であるガウス確率密度関数Ｐ（Ｘ）９２０を用いる１つの実施形態による、バイアスされたサンプリングの方法のブロック図を示している。スカラー調整パラメーター０≦λ_μ≦１を定義すると、ガウス分布の平均値は以下によって与えられる。

ガウス分布の対角共分散行列は、σ＝ｄｉａｇ（λ_ｘσ_ｘ，λ_ｙσ_ｙ，λ_θσ_θ）として計算され、ここでσ_ｘ、σ_ｙ、σ_θは、式

によって求められる。ここで、ｄｏｔ（ｘ_１，ｘ_２）は、ベクトルｘ_１及びｘ_２のドット積を表し、λ_ｘ＝λ_ｙ＞λ_θ＞０は、或る特定の基準を反映するように確率密度関数について調整されるスケーリングパラメーターである。１つの実施形態において、λ_ｘ＝λ_ｙ＞λ_θ＞０は、人間のヒューリスティクスを反映するために用いることができる。

別の実施形態において、確率密度関数Ｐ（Ｘ）は、平均μ及び共分散σがＸ_ｉ及びＸ_ｉ＋１を含む通過点のセットの関数であるガウス過程である。

図１０は、１つの実施形態による、運動学的グラフＫＧ４６５から運動学的経路を選択する方法のブロック図を示している。方法は、初期ノード及び目標ノードを除く全ての葉ノードを除去することによって運動学的グラフをトリミングし（１００１）、ここで、葉ノードは、運動学的エッジで１つの近傍のノードにのみ接続される。方法は、運動学的グラフにわたって、幾何学的グラフの場合と同じように、価値反復を実行することによって各ノードの最小コストを求める（１００２）。初期ノードから開始して、ブロック１００３は、ノードコスト及び運動学的グラフに従って初期運動学的経路を選択する。１つの実施形態において、ブロック１００３は、幾何学的グラフからの最良の幾何学的経路（通過点のセットと同等）の選択において採用される方策反復を用いて初期運動学的経路を選択する。例えば、初期運動学的経路は、初期ノード及び目標ノードを含むノードのセットを含む。ブロック１００４は、初期運動学的経路を平滑化して、初期状態から開始して目標状態において終端する運動学的経路を生成する。

図１１Ａは、１つの実施形態による、初期運動学的経路１１１０を平滑化する（１００４）フローチャートを示している。いくつかの実施形態において、運動学的経路を平滑化することは、運動学的経路において後続する通過点に到達するのに不必要である中間通過点を除去することを含む。例えば、初期運動学的経路Ｐ_０＝｛Ｘ_０，．．．，Ｘ_Ｍ｝が与えられると、ブロック１００４は、Ｍ≦１である場合（１１２０）、何も行わず、すなわち、経路を返す（１１２５）。Ｍ＝２である場合、中間経路は、初期運動学的経路Ｐ_０と定義され、これは１０１２でラベル付けされている。Ｍ＞２である場合（１１３０）、ブロック１００４は、部分経路ＰＳ＝｛Ｘ_１，．．．，Ｘ_Ｍ｝の平滑化を試みて（１１４０）、平滑化された部分経路ＳＰＳを生成し、中間経路｛Ｘ_０，ＳＰＳ｝を返し、これは１０１３でラベル付けされている。ブロック１０１１は、Ｘ_０と残りのノードとの間の接続を試みることによって、Ｐ_０又はＳＰＳのいずれかの中間経路を単純化して、より平滑な経路を生成する（１１５０）。

図１１Ｂは、１つの実施形態による、図１１Ａのフローチャートのブロック１０１１によって実行される方法のフローチャートを示している。Ｍとして初期化されるインデックス変数をｊとする。ブロック１１６５は、衝突を伴わない運動学的エッジでＸ_１とＸ_ｊとを接続することを試みる。接続に成功した場合、ブロック１１８０は、｛Ｘ_１，Ｘ_ｉ，Ｍ−ｊ≦ｉ≦Ｍ｝からなる新たな経路を出力し、接続に失敗した場合、ｊを１だけ増分する。ｊがＭ−１に達した場合、ブロック１０１１は停止して、中間経路｛Ｘ_ｉ，１≦ｉ≦Ｍ｝を出力し、ｊがＭ−１に達していない場合、新たな接続をテストするために新たなＸ_ｊがブロック１１６５に提供される。

ブロック１００４は、車両を初期状態Ｘ_０から目標状態Ｘ_ｆに運動させるために運動学的経路が通過する必要があるノードのセットを出力する。運動学的経路は、初期値が初期状態であり、速度及び操舵角を外部入力とする車両の運動学的モデルをシミュレートすることによって生成することができる。運動学的経路の２つの隣接したノードである、Ｘ_ｉからＸ_ｊまで車両を運動させるのに必要な速度及び操舵角は、運動学的グラフの構築中に得られる。具体的には、これらは、Ｘ_ｉを、エッジＥ（Ｘ_ｉ，Ｘ_ｊ）を通じてＸ_ｊと接続するときに解かれ、エッジＥ（Ｘ_ｉ，Ｘ_ｊ）においてアクションコードとして記憶される。

図１１Ｃは、図１Ａに示す駐車シナリオの、平滑化ブロック１００４において計算される、運動学的経路１１９０をプロットしている。図１１Ｄは、運動学的経路１１９０に沿った車両１００の運動を示す概略図１１９５をプロットしている。

いくつかの実施形態は、車両の動的モデルを用いて、運動学的経路を時間の関数として追跡する基準軌道を求め、この基準軌道に従って車両の運動を制御する。一方で、ブロック１００４は、モデル（１）の車両動態を満たさない運動学的経路Ｘ_ｋ（ｔ），ｔ∈［０，ｔ_ｎ］を生成することができる。

図１２は、１つの実施形態による、リアルタイム制御のために、運動学的経路を更に処理して基準軌道を生成する方法のブロック図を示している。ブロック１２０１において、運動学的経路及び加速度制約１２１０から初期軌道Ｘ^０（ｔ）が生成される。ブロック１２０２は、以下の最適制御問題を解くことによって基準軌道１２２０を生成する。

ただし

ここで、Ｗは、正定値行列であり、ａ_１ｍａｘは、車両の最大並進加速度であり、ａ_２ｍａｘは、ハンドルの最大角速度であり、εは、小さい正のスカラー値である。上述の最適制御問題は、通常、時間において離散化され、解くのに初期推測を必要とする非線形最適化問題に変換される。

１つの実施形態において、初期推測は、運動学的経路Ｘ_ｋ（ｔ）から生成し、車両動態（１）を変換することができる。別の実施形態において、境界条件Ｘ（ｔ_ｎ）＝Ｘ_ｆ及びＸ（０）＝Ｘ_０は、満たすのが困難である場合がある。以下の最適化問題を考慮することが有利である。

ただし

ここで

ただし、Ｓ_０及びＳ_ｎは、正定値行列である。

別の実施形態において、運動学的経路の計算中に得られるｔ_ｎ、すなわち、車両がＸ_０からＸ_ｆまで運動する時間は、満たすには過度に厳格であり、したがって、上述の最適化問題の実現可能性を危うくする場合がある。ｔ_ｎ〜（１＋α）ｔ_ｎを０＜α＜∞で緩和することによって、初期軌道と同等に運動学的経路を生成することができる。

図１３は、１つの実施形態によるシステムの概略図を示している。システムは、自動化駐車１３５０を実行するように構成されたプロセッサ１３０２を備える車両１３０１を備える。車両は、ＬＩＤＡＲ１３１０及び／又はカメラ１３２０等の少なくとも１つのセンサーも備える。ＬＩＤＡＲセンサー１３１０は、低分解能の第１のセンサーであり、カメラ１３２０は、高分解能の第２のセンサーである。センサー１３１０及び／又は１３２０は、プロセッサ１３０２に動作的に接続されるとともに、駐車空間の少なくとも一部のジオメトリを示す情報を検知するように構成される。この情報を用いて、プロセッサ１３０２は、駐車空間のマップ１３０を求め、及び／又は更新する。そのために、プロセッサ１３０２は、マップ１３０を用いて自動化駐車１３５０を実行する。

図１４は、いくつかの実施形態による自動化駐車システム１４００のブロック図を示している。システム１４００は、車両１３０１の内部に実装することができる。付加的又は代替的に、システム１４００は、車両１３０１に通信可能に接続することができる。

システム１４００は、カメラ１４１０、慣性測定ユニット（ＩＭＵ）１４３０、プロセッサ１４５０、メモリ１４６０、送受信機１４７０、及びディスプレイ／スクリーン１４８０のうちの１つ又は組み合わせを備えることができる。これらは、接続１４２０を通じて他の構成要素に動作的に結合することができる。接続１４２０は、バス、ライン、ファイバー、リンク又はそれらの組み合わせを含むことができる。

送受信機１４７０は、例えば、１つ以上のタイプの無線通信ネットワークを通じて１つ以上の信号を送信することを可能にする送信機と、１つ以上のタイプの無線通信ネットワークを通じて送信された１つ以上の信号を受信する受信機とを備えることができる。送受信機１４７０は、様々な技術に基づいて無線ネットワークとの通信を可能にすることができる。これらの技術は、標準規格のＩＥＥＥ８０２．１１ファミリーに基づくことができるフェムトセル、Ｗｉ−Ｆｉネットワーク又は無線ローカルエリアネットワーク（ＷＬＡＮ）、標準規格のＩＥＥＥ８０２．１５ｘファミリーに基づくＢｌｕｅｔｏｏｔｈ（登録商標）、近距離場通信（ＮＦＣ）、ネットワーク等の無線パーソナルエリアネットワーク（ＷＰＡＮ）、及び／又はＬＴＥ、ＷｉＭＡＸ等の無線ワイドエリアネットワーク（ＷＷＡＮ）等であるが、これらに限定されるものではない。システム１４００は、有線ネットワークを通じて通信する１つ以上のポートを備えることもできる。

いくつかの実施形態では、システム１４００は、ＣＣＤセンサー若しくはＣＭＯＳセンサー、レーザー及び／又はカメラ等の画像センサー１４１０を備えることができる。この画像センサーは、以下では「センサー１４１０」と呼ばれる。例えば、センサー１４１０は、光画像を電子画像又はデジタル画像に変換することができ、取得された画像をプロセッサ１４５０に送信することができる。付加的又は代替的に、センサー１４１０は、シーン内のターゲット物体から反射された光を検知し、捕捉された光の強度をプロセッサ１４５０にサブミットすることができる。

例えば、センサー１４１０は、「カラー情報」を提供するカラーカメラ又はグレースケールカメラを含むことができる。「カラー情報」という用語は、本明細書において用いられるとき、カラー情報及び／又はグレースケール情報を指す。一般に、カラー画像又はカラー情報は、本明細書において用いられるとき、１〜Ｎ個のチャネルを含むものとみなすことができる。ここで、Ｎは、画像を記憶するのに用いられている色空間に依存する或る整数である。例えば、ＲＧＢ画像は、３つのチャネルを含み、赤情報、青情報及び緑情報についてそれぞれ１つのチャネルを有する。

例えば、センサー１４１０は、「深度情報」を提供する深度センサーを含むことができる。深度情報は、深度センサーを用いて様々な方法で取得することができる。「深度センサー」という用語は、深度情報を単独で及び／又は他のいくつかのカメラと併せて取得するのに用いることができる機能ユニットを指すのに用いられる。例えば、いくつかの実施形態では、深度センサー及び光カメラは、センサー１４１０の一部分とすることができる。例えば、いくつかの実施形態では、センサー１４１０はＲＧＢＤカメラを備える。このＲＧＢＤカメラは、カラー（ＲＧＢ）画像に加えて、深度センサーが有効にされているときはピクセルごとの深度（Ｄ）情報を捕捉することができる。

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

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

いくつかの実施形態では、センサー１４１０は立体カメラを備える。例えば、深度センサーは、２つ以上のカメラを用いてシーンの深度情報を取得することができる受動ステレオビジョンセンサーの一部分を成すことができる。捕捉されたシーンにおける双方のカメラに共通の点のピクセル座標を、カメラ姿勢情報及び／又は三角測量技法とともに用いて、ピクセルごとの深度情報を取得することができる。

いくつかの実施形態では、システム１４００は、デュアルフロントカメラ及び／又は前面カメラ及び背面カメラ等の複数のセンサー１４１０に動作的に接続することができ、これらの複数のセンサーは、様々なセンサーを組み込むこともできる。いくつかの実施形態では、センサー１４１０は、静止画像及びビデオ画像の双方を捕捉することができる。いくつかの実施形態では、センサー１４１０は、例えば、３０フレーム毎秒（ｆｐｓ）で画像を捕捉することが可能なＲＧＢＤ又は立体ビデオカメラを備えることができる。１つの実施形態では、センサー１４１０によって捕捉された画像は、生の未圧縮フォーマットとすることができ、処理及び／又はメモリ１４６０への記憶の前に圧縮することができる。いくつかの実施形態では、画像圧縮は、プロセッサ１４５０によって可逆圧縮技法又は非可逆圧縮技法を用いて実行することができる。

いくつかの実施形態では、プロセッサ１４５０は、ＩＭＵ１４３０から入力を受信することもできる。他の実施形態では、ＩＭＵ１４３０は、３軸加速度計（複数の場合もある）、３軸ジャイロスコープ（複数の場合もある）、及び／又は磁気計（複数の場合もある）を備えることができる。ＩＭＵ１４３０は、速度、方位、及び／又は他の位置関連情報をプロセッサ１４５０に提供することができる。いくつかの実施形態では、ＩＭＵ１４３０は、測定された情報を、センサー１４１０による各画像フレームの捕捉と同期して出力することができる。いくつかの実施形態では、ＩＭＵ１４３０の出力は、プロセッサ１４５０がセンサー測定値を融合し及び／又は融合された測定値を更に処理するのに部分的に用いられる。

また、システム１４００は、カラー画像及び／又は深度画像等の画像をレンダリングするスクリーン又はディスプレイ１４８０を備えることができる。いくつかの実施形態では、ディスプレイ１４８０は、センサー１４１０によって捕捉されたライブ画像、融合画像、拡張現実（ＡＲ）画像、グラフィカルユーザーインターフェース（ＧＵＩ）、及び他の番組（ｐｒｏｇｒａｍ：プログラム）出力を表示するのに用いることができる。いくつかの実施形態では、ディスプレイ１４８０は、ユーザーが、仮想キーボード、アイコン、メニュー、又は他のＧＵＩ、ユーザージェスチャー及び／又はスタイラス及び他の筆記用具等の入力デバイスの或る組み合わせを介してデータを入力することを可能にするタッチスクリーンを備えることができ及び／又はこのようなタッチスクリーンとともに収容することができる。いくつかの実施形態では、ディスプレイ１４８０は、液晶ディスプレイ（ＬＣＤ）又は有機ＬＥＤ（ＯＬＥＤ）ディスプレイ等の発光ダイオード（ＬＥＤ）ディスプレイを用いて実施することができる。他の実施形態では、ディスプレイ１４８０は、ウェアラブルディスプレイとすることができる。いくつかの実施形態では、融合の結果をディスプレイ１４８０にレンダリングすることもできるし、システム１４００の内部又は外部に存在することができる異なるアプリケーションにサブミットすることもできる。

例示的なシステム１４００は、図示した機能ブロックのうちの１つ以上の追加、組み合わせ、又は省略等によって、本開示と整合性を有するように様々な方法で変更することもできる。例えば、いくつかの構成では、システム１４００は、ＩＭＵ１４３０又は送受信機１４７０を備えていない。さらに、いくつかの特定の例示の実施態様では、システム１４００は、周辺光センサー、マイクロフォン、音響センサー、超音波センサー、レーザーレンジファインダー等の様々な他のセンサー（図示せず）を備える。いくつかの実施形態では、システム１４００のいくつかの部分は、１つ以上のチップセット等の形態を取る。

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

メモリ１４６０は、プロセッサ１４５０の内部に、及び／又はプロセッサ１４５０の外部に実装することができる。本明細書において使用されるときに、「メモリ」という用語は、任意のタイプの長期、短期、揮発性、不揮発性又は他のメモリを指しており、任意の特定のタイプのメモリ若しくはメモリの数、又はメモリが記憶される物理媒体のタイプに制限されるべきではない。いくつかの実施形態では、メモリ１４６０は、自動化駐車を容易にするプログラムコードを保持する。

例えば、メモリ１４６０は、静止画像、深度情報、ビデオフレーム、プログラム結果、並びにＩＭＵ１４３０及び他のセンサーによって提供されるデータ等のセンサーの測定値を記憶することができる。メモリ１４６０は、車両のジオメトリ、駐車空間のマップ、車両の運動学的モデル、及び車両の動的モデルを記憶するメモリを格納することができる。一般に、メモリ１４６０は、任意のデータ記憶機構を表すことができる。メモリ１４６０は、例えば、一次メモリ及び／又は二次メモリを含むことができる。一次メモリは、例えば、ランダムアクセスメモリ、リードオンリーメモリ等を含むことができる。図１４においてプロセッサ１４５０とは別であるように示されるが、一次メモリの全て若しくは一部をプロセッサ１４５０内に設けることができるか、又はそうでなくても、プロセッサ１４５０と同一の場所に配置し、及び／又はプロセッサ１４５０に結合することができることは理解されたい。

二次メモリは、例えば、一次メモリと同じ、又は類似のタイプのメモリ、及び／又は例えば、フラッシュ／ＵＳＢメモリドライブ、メモリカードドライブ、ディスクドライブ、光ディスクドライブ、テープドライブ、ソリッドステートドライブ、ハイブリッドドライブ等の１つ以上のデータ記憶デバイス又はシステムを含むことができる。或る特定の実施態様において、二次メモリは、取外し可能な媒体ドライブ（図示せず）内の非一時的コンピューター可読媒体に動作的に収容可能であるか、又は別の方法で、動作的に構成可能とすることができる。いくつかの実施形態において、非一時的コンピューター可読媒体は、メモリ１４６０及び／又はプロセッサ１４５０の一部を形成する。

本発明の上記で説明した実施形態は、多数の方法のうちの任意のもので実施することができる。例えば、実施形態は、ハードウェア、ソフトウェア又はそれらの組み合わせを用いて実施することができる。ソフトウェアで実施される場合、ソフトウェアコードは、単一のコンピューターに設けられるのか又は複数のコンピューター間に分散されるのかにかかわらず、任意の適したプロセッサ又はプロセッサの集合体において実行することができる。そのようなプロセッサは、１つ以上のプロセッサを集積回路部品に有する集積回路として実装することができる。ただし、プロセッサは、任意の適したフォーマットの回路類を用いて実装することができる。

また、本発明の実施形態は、例が提供された方法として実施することができる。この方法の一部として実行される動作は、任意の適切な方法で順序付けすることができる。したがって、動作が示したものと異なる順序で実行される実施形態を構築することができ、これには、例示の実施形態では一連の動作として示されたにもかかわらず、いくつかの動作を同時に実行することを含めることもできる。

請求項の要素を修飾する、特許請求の範囲における「第１」、「第２」等の序数の使用は、それ自体で、１つの請求項の要素の別の請求項の要素に対する優先順位も、優位性も、順序も暗示するものでもなければ、方法の動作が実行される時間的な順序も暗示するものでもなく、請求項の要素を区別するために、単に、或る特定の名称を有する１つの請求項の要素を、同じ（序数の用語の使用を除く）名称を有する別の要素と区別するラベルとして用いられているにすぎない。

Claims (17)

1. 車両を駐車空間内に駐車する方法であって、前記方法は、前記車両のジオメトリ、前記駐車空間のマップ、前記車両の運動学的モデル、及び前記車両の動的モデルを記憶するメモリに結合されたプロセッサを使用し、前記プロセッサは、前記方法を実施する記憶命令に結合され、前記記憶命令は、前記プロセッサによって実行されると、前記方法の少なくともいくつかのステップを実行し、前記少なくともいくつかのステップは、
   前記車両のジオメトリ及び前記駐車空間のマップを用いて、前記車両の初期状態を、通過点のセットを通じて駐車車両の目標状態と接続する衝突を伴わない幾何学的経路を求めることであって、各通過点は、前記車両の位置及び方位を規定することと、
   前記車両の運動学的モデルを用いて、運動学的エッジと接続された複数のノードを有する運動学的グラフを形成する運動学的サブグラフのセットを求めることであって、各運動学的サブグラフは、前記幾何学的経路の近傍の通過点の対を接続し、各ノードは、前記車両の状態を規定し、２つのノードを接続する各運動学的エッジは、前記車両の運動学的特性に従って前記２つのノードを接続する衝突を伴わない運動学的経路を規定することと、
   前記運動学的グラフから、前記車両の初期状態に対応する初期ノードを、中間ノードのセットを通じて、前記車両の目標状態に対応する目標ノードと接続する運動学的経路を選択することと、
   前記車両の動的モデルを用いて、前記運動学的経路を時間の関数として追跡する基準軌道を求めることと、
   前記基準軌道に従って前記車両の運動を制御することと、
   を含み、
   前記幾何学的経路を前記求めることは、
   前記初期ノードを、衝突を伴わないノードのセットを通じて前記目標ノードと接続する幾何学的グラフを構築することであって、前記幾何学的グラフにおけるノードの各対は、前記車両及び前記駐車空間のジオメトリのみを用いて求められた衝突を伴わないエッジで接続されることと、
   前記幾何学的経路の前記通過点のセットを形成する前記幾何学的グラフからノードのセットを選択することと、
   を含み、
   前記駐車空間の状態空間内の点をサンプリングして、サンプリングされた状態を生成することと、
   前記幾何学的グラフのノードの全ての状態が前記サンプリングされた状態の到達不可能エリア内にある場合、前記サンプリングされた状態を拒絶することと、
   そうではない場合、
   前記サンプリングされた状態に最も近い状態を有する前記幾何学的グラフの最近傍のノードを求めることと、
   前記サンプリングされた状態のノードを前記幾何学的グラフに追加すること、及び前記追加されたノードを、エッジが衝突を伴わない場合に前記エッジを介して前記最近傍のノードと接続することと、
   前記サンプリングすること、前記拒絶すること、前記求めること、及び前記追加することを、前記初期ノードが前記目標ノードに接続されるまで繰り返すことと、
   を更に含む、方法。
2. 前記運動学的グラフを、前記通過点のセットに対応するシードのセットから反復的に成長させること、
   を更に含む、請求項１に記載の方法。
3. 前記車両の少なくとも１つのセンサーを用いて、前記駐車空間の少なくとも一部のジオメトリを求めることと、
   メモリから前記車両のジオメトリを選択することであって、前記センサー及び前記メモリは、前記プロセッサに動作的に接続されることと、
   を更に含む、請求項１に記載の方法。
4. 前記幾何学的グラフは、前記初期ノードから開始して前記目標ノードへ向かう初期ツリーと、前記目標ノードから開始して前記初期ノードへ向かう目標ツリーとを含み、前記最近傍のノードは、前記初期ツリーからの第１の最近傍のノードと、前記目標ツリーからの第２の最近傍のノードとのうちの一方又は組み合わせを含む、請求項１に記載の方法。
5. 前記幾何学的グラフの前記初期ノードと各非葉ノードとについて、前記目標ノードに到達する最小コストを求めることと、
   前記初期ノードについて求められた前記最小コストを有する、前記初期ノードを前記目標ノードと接続するノードのセットを選択して、前記幾何学的経路の前記通過点のセットを形成することと、
   を更に含む、請求項１に記載の方法。
6. 前記初期ノード及び前記目標ノードを除く葉ノードを、前記幾何学的グラフから除去することと、
   前記幾何学的グラフからの前記除去された前記葉ノードを接続する前記エッジを除去することと、
   を更に含む、請求項５に記載の方法。
7. 車両を駐車空間内に駐車する方法であって、前記方法は、前記車両のジオメトリ、前記駐車空間のマップ、前記車両の運動学的モデル、及び前記車両の動的モデルを記憶するメモリに結合されたプロセッサを使用し、前記プロセッサは、前記方法を実施する記憶命令に結合され、前記記憶命令は、前記プロセッサによって実行されると、前記方法の少なくともいくつかのステップを実行し、前記少なくともいくつかのステップは、
   前記車両のジオメトリ及び前記駐車空間のマップを用いて、前記車両の初期状態を、通過点のセットを通じて駐車車両の目標状態と接続する衝突を伴わない幾何学的経路を求めることであって、各通過点は、前記車両の位置及び方位を規定することと、
   前記車両の運動学的モデルを用いて、運動学的エッジと接続された複数のノードを有する運動学的グラフを形成する運動学的サブグラフのセットを求めることであって、各運動学的サブグラフは、前記幾何学的経路の近傍の通過点の対を接続し、各ノードは、前記車両の状態を規定し、２つのノードを接続する各運動学的エッジは、前記車両の運動学的特性に従って前記２つのノードを接続する衝突を伴わない運動学的経路を規定することと、
   前記運動学的グラフから、前記車両の初期状態に対応する初期ノードを、中間ノードのセットを通じて、前記車両の目標状態に対応する目標ノードと接続する運動学的経路を選択することと、
   前記車両の動的モデルを用いて、前記運動学的経路を時間の関数として追跡する基準軌道を求めることと、
   前記基準軌道に従って前記車両の運動を制御することと、
   を含み、
   前記運動学的グラフを前記求めることは、
   前記通過点のセットに近接する前記駐車空間をサンプリングして、サンプリングされた状態を生成することと、
   前記サンプリングされた状態を、ツリーを更新するために、前記運動学的エッジが衝突を伴わないものである場合に前記運動学的エッジを用いて少なくとも１つの通過点から開始する少なくとも１つのツリーと接続することと、
   終了条件が満たされるまで前記サンプリングすること及び前記接続することを繰り返すことと、
   を含み、前記終了条件は、前記接続された幾何学的グラフの前記形成、前記運動学的グラフ上で前記初期ノードと前記目標ノードとを接続する経路の数が閾値を超えること、近傍の通過点のツリーに接続するノードの数、のうちの１つ又は組み合わせを含む、方法。
8. 前記サンプリングすることは、前記通過点の状態の関数として平均及び共分散を有する前記駐車空間の状態の確率分布を用いてバイアスして実行される、請求項７に記載の方法。
9. 前記通過点のセットＸ０，．．．，ＸＭから２つの隣接した通過点Ｘｉ、Ｘｉ＋１を選択することと、
   サブグラフＳＧｉを、衝突を伴わないノード及び運動学的エッジのセットを通じて前記通過点Ｘｉ＋１に接続するサブグラフＳＧｉ＋１を形成することであって、ＳＧｉは、２つの隣接した通過点Ｘｉ−１、Ｘｉについての先行する反復中に求められた前記サブグラフであることと、
   ０≦ｉ≦Ｍ−１について前記選択すること及び前記形成することを繰り返すことと、
   を更に含む、請求項７に記載の方法。
10. 初期ツリー及び目標ツリーを用いて前記サブグラフを初期化することであって、前記初期ツリーは、幾何学的通過点｛Ｘ０，．．．，Ｘｉ｝に対応するノードを含む前記サブグラフＳＧｉを含み、前記目標ツリーは、前記通過点Ｘｉ＋１に対応する前記ノードを含むことと、
    通過点Ｘｉ、Ｘｉ＋１に近接する前記車両の運動学的モデルの状態空間から新たな状態Ｘをバイアスしてサンプリングすることと、
    前記車両が前記新たな状態から前記初期ツリー及び前記目標ツリーにおける状態への衝突を伴わない運動学的経路を辿ることができる場合、前記初期ツリー及び前記目標ツリーのうちの少なくとも一方に前記新たな状態Ｘを追加することと、
    前記新たな状態Ｘが前記初期ツリー及び前記目標ツリーの双方に追加されるまで、前記サンプリングすること及び前記追加することを繰り返すことと、
    を更に含む、請求項９に記載の方法。
11. 前記通過点Ｘｉ、Ｘｉ＋１と、前記車両及び前記駐車空間のジオメトリとに従って、確率分布関数Ｐ（Ｘ｜Ｘｉ，Ｘｉ＋１）を構築することと、
    前記確率分布関数Ｐ（Ｘ｜Ｘｉ，Ｘｉ＋１）に従って前記状態空間から前記新たな状態Ｘをサンプリングすることと、
    前記サンプリングされた状態が衝突を伴わないものになるまで前記サンプリングすることを繰り返すことと、
    を更に含む、請求項１０に記載の方法。
12. 車両を駐車空間内に駐車する方法であって、前記方法は、前記車両のジオメトリ、前記駐車空間のマップ、前記車両の運動学的モデル、及び前記車両の動的モデルを記憶するメモリに結合されたプロセッサを使用し、前記プロセッサは、前記方法を実施する記憶命令に結合され、前記記憶命令は、前記プロセッサによって実行されると、前記方法の少なくともいくつかのステップを実行し、前記少なくともいくつかのステップは、
    前記車両のジオメトリ及び前記駐車空間のマップを用いて、前記車両の初期状態を、通過点のセットを通じて駐車車両の目標状態と接続する衝突を伴わない幾何学的経路を求めることであって、各通過点は、前記車両の位置及び方位を規定することと、
    前記車両の運動学的モデルを用いて、運動学的エッジと接続された複数のノードを有する運動学的グラフを形成する運動学的サブグラフのセットを求めることであって、各運動学的サブグラフは、前記幾何学的経路の近傍の通過点の対を接続し、各ノードは、前記車両の状態を規定し、２つのノードを接続する各運動学的エッジは、前記車両の運動学的特性に従って前記２つのノードを接続する衝突を伴わない運動学的経路を規定することと、
    前記運動学的グラフから、前記車両の初期状態に対応する初期ノードを、中間ノードのセットを通じて、前記車両の目標状態に対応する目標ノードと接続する運動学的経路を選択することと、
    前記車両の動的モデルを用いて、前記運動学的経路を時間の関数として追跡する基準軌道を求めることと、
    前記基準軌道に従って前記車両の運動を制御することと、
    を含み、
    前記運動学的グラフの各ノードの最適コストを求めることであって、ノードの前記最適コストは、前記車両を、前記ノードから前記運動学的グラフのエッジを通じて前記目標ノードに運動させる最良推定コストを表すことと、
    方策反復によって、運動学的通過点と前記初期状態を前記目標状態と接続する関連付けられたエッジとのセットによって表される初期運動学的経路を求めることと、
    を更に含む、方法。
13. 現在状態Ｘｃを第１の運動学的通過点の初期状態Ｘ０として含むように前記初期運動学的経路Ｐ０を初期化することと、
    次の状態と前記現在状態との間のエッジＥ（Ｘｃ，Ｘｎ）が前記現在状態から前記目標状態への最小コストを生成するように、前記現在状態の前記次の状態Ｘｎを求めることと、
    前記次の状態Ｘｎ及び前記エッジＥ（Ｘｃ，Ｘｎ）を、前記初期運動学的経路Ｐ０＝｛Ｐ０，Ｘｎ，Ｅ（Ｘｃ，Ｘｎ）｝に追加することと、
    前記次の状態が前記目標状態になるまで、前記求めること及び前記追加することを繰り返すことと、
    を更に含む、請求項１２に記載の方法。
14. 前記運動学的経路において後続する通過点に到達するのに不必要な中間通過点を除去することによって前記運動学的経路を平滑化すること、
    を更に含む、請求項１に記載の方法。
15. 車両を駐車空間内に駐車するシステムであって、
    前記車両のジオメトリ、前記駐車空間のマップ、前記車両の運動学的モデル、及び前記車両の動的モデルを記憶するメモリと、
    前記駐車空間の少なくとも一部のジオメトリを示す情報を検知するように動作するセンサーと、
    前記メモリに結合されたプロセッサであって、
    前記車両のジオメトリ及び前記駐車空間のマップを用いて、前記車両の初期状態を、通過点のセットを通じて駐車車両の目標状態と接続する衝突を伴わない幾何学的経路を求めることであって、各通過点は、前記車両の位置及び方位を規定することと、
    前記車両の運動学的モデルを用いて、運動学的エッジと接続された複数のノードを有する運動学的グラフを形成する、前記通過点のセットに対応するシードのセットから成長した運動学的サブグラフのセットを求めることであって、各運動学的サブグラフは、前記幾何学的経路の近傍の通過点の対を接続し、各ノードは、前記車両の状態を規定し、２つのノードを接続する各運動学的エッジは、前記車両の運動学的特性に従って前記２つのノードを接続する衝突を伴わない運動学的経路を規定することと、
    前記運動学的グラフから、前記車両の初期状態に対応する初期ノードを、中間ノードのセットを通じて、前記車両の前記目標状態に対応する目標ノードと接続する運動学的経路を選択することと、
    前記車両の動的モデルを用いて、前記運動学的経路を時間の関数として追跡する基準軌道を求めることと、
    を行うように構成された、プロセッサと、
    前記基準軌道に従って前記車両の運動を制御するコントローラーと、
    を備え、
    前記プロセッサは、
    前記駐車空間の状態空間内の点をサンプリングして、サンプリングされた状態を生成することであって、前記サンプリングすることは、前記通過点の前記状態の関数として平均及び共分散を有する前記駐車空間の状態の確率分布を用いてバイアスして実行されることと、
    幾何学的グラフのノードの全ての状態が前記サンプリングされた状態の到達不可能エリア内にある場合、前記サンプリングされた状態を拒絶することと、
    そうではない場合、
    前記サンプリングされた状態に最も近い状態を有する前記幾何学的グラフの最近傍のノードを求めることと、
    前記サンプリングされた状態のノードを前記幾何学的グラフに追加すること、及び前記追加されたノードを、エッジが衝突を伴わない場合に前記エッジを介して前記最近傍のノードと接続することと、
    前記サンプリングすること、前記拒絶すること、前記求めること、及び前記追加することを、前記初期ノードが前記目標ノードに接続されるまで繰り返すことと、
    を行うように構成される、システム。
16. 前記プロセッサは、前記初期ノードを、衝突を伴わないノードのセットを通じて前記目標ノードと接続する幾何学的グラフを構築することであって、前記幾何学的グラフにおけるノードの各対は、前記車両及び前記駐車空間のジオメトリのみを用いて求められた衝突を伴わないエッジで接続されることと、前記幾何学的経路の前記通過点のセットを形成する前記幾何学的グラフからノードのセットを選択することとによって前記幾何学的経路を求める、請求項１５に記載のシステム。
17. 請求項１から１４のいずれか１項に記載の方法を前記プロセッサによって実行させるためのプログラムが格納された非一時的コンピューター可読記憶媒体。

Images