JP6771846B2 - 車両の運動を制御するシステム及び方法 - Google Patents

Description

本発明は包括的には、車両の運動の経路計画に関し、より詳細には、異なる状態間で車両の運動を自動的に制御する経路計画システム及び方法に関する。

自律車両又は自律走行モードを実行する車両のいずれかの車両によって利用されるいくつかの制御システムは、他の車両又は歩行者等の障害物を回避するためのみではなく、車両の操作に関連付けられたいくつかの基準を最適化するためも含む双方で、未来を予測し、車両の安全運動又は経路を予測する。車両は、物体軌道の一部、より詳細には、２次元走行面上への軌道の投影である経路に沿って制御される。例えて言うと、経路は、走行中の車両の全ての幾何学的座標を接続する線である。

経路計画の基本的な目的は、自律運転モードにおいて動作している自律車両又は半自律車両等の走行中の車両の２つの状態、例えば、初期状態及び目標状態を接続する軌道を構成すること、並びに、軌道に沿って車両の運動を制御することである。「状態」は、本明細書において使用されるときに、平面内の車両の幾何学的座標と、これらの幾何学的座標における車両の方位とを含むベクトルであることを意図している。目標状態は、固定ロケーション、移動ロケーション、速度ベクトル、領域、又はそれらの組み合わせのいずれかとすることができる。道路縁、歩行者、及び他の車両等の周囲環境は、車両のセンサーによって検知される、及び／又は、事前に与えられた情報によって少なくとも部分的に既知である。

初期状態及び目標状態を接続する経路は一般に、いくつかの基本経路（elementary path）（又は略して、ＥＰＴ）から構成される。状況によっては、経路計画は、同じ種類又は異なる種類の基本経路でのみ機能する。そのために、走行中の物体の経路は一般に、同じ接合点（junction points）（ＪＰと略される）を共有する複数の基本経路から構成される。したがって、各接合点は２つの隣接する基本経路（ＥＰＴ）に属するので、接合点は経路の特殊な点である。

経路に関して要求される１つの特殊な特徴は、状態が進路全体に沿って、又は接合点において不規則に変化しないことである。すなわち、１つの基本経路から隣接する基本経路への移行が、その一次導関数（接線）及び二次導関数（曲率）だけでなく、基本経路の接合点においても連続しているのを確実にしなければならない。車両のステアリング及び速度を線形になるように、すなわち連続するように変更することによって、１つの基本経路から隣接する基本経路への移行を実行できるときに、その経路は接合点において連続している。

例えば、Dubins及びReeds及びSheppによる先駆的な仕事は、車両の２つの状態間の最短経路のパターンを調査した。Ｒｅｅｄｓ−Ｓｈｅｐｐ（ＲＳ）経路と呼ばれる最短経路は、線分及び／又は最小回転半径の接線円弧の逐次合成である。ＲＳ経路は、非常に効率的に計算することができるが、複数の線分を伴うＲＳ経路に沿った曲率は不連続である。車両が停止し、止まったままステアリングを実行しなければならず、結果として、無用な時間遅延及び余分なタイヤの摩耗につながるので、そのような曲率不連続性は、実用時には望ましくない。

いくつかの方法は、線分及び円弧に加えて、他の断片を導入することによってＲＳ経路の不連続性を克服する。例えば、経路計画は、スプライン、多項式、三角関数又はクロソイド等の種々のタイプの基本経路を合成することができる。上記の基本経路は、その数学的記述によって主に区別される。全ての基本経路が共通して有する特徴は、基本経路を直線及び／又は円弧（circular line）と合成して、連続曲率経路を形成できることである。

クロソイドは操作される自動車の走行規則に密接に関連付けられるので、いくつかの従来の方法は、その経路計画を、クロソイドを基にすることを選ぶ。クロソイドは特殊なタイプの平面曲線であり、その曲率はｃによって表され、経路に沿って線形に変化する。クロソイドは、初期状態が終了状態と接続されるときに、経路の長さが最小化されるという更なる利点を有する。

例えば、非特許文献１の教示は、円弧によって接続される２つのクロソイドによって形成されるクロソイドターン（ＣＣターンと略される）の概念を導入し、ＣＣターン及び直線の間の連続した曲率移行を確実にするμ−接線条件（μ−tangency conditions）を教示している。しかしながら、連続経路は通常、複数のＣＣターン及び線のシーケンスを含み、連続経路を形成する最適なシーケンスのパラメーターを見つけるのに時間を要する可能性がある。

例えば、特許文献１において開示される方法は、最初にＲＳ経路を特定し、そのＲＳ経路に可能な限り近くにある連続曲率経路を見つけようと試みる。この方法は、非線形制約下で最適化問題を解くことによって、連続曲率経路を計算する。しかしながら、この方法への解法は反復プロセスを必要とし、それはいくつかの実用的な適用例の場合に、計算量が膨大になる可能性がある。また、その方法の場合に結果として生じる最適化問題は非凸である。非凸問題への実現可能な解法は、反復プロセスによって解かれるという保証はなく、それゆえ、連続曲率経路の構成は失敗する場合がある。それゆえ、この方法は、車両のリアルタイム制御に適していない場合がある。

同様に、特許文献２において記述されている方法は計算効率を改善するが、依然として、連続曲率経路を構成するために、計算コストが高い反復プロセスを伴う。それゆえ、この方法も、車両のリアルタイム制御に適していない場合がある。

米国特許出願公開第２０１６／０３１３３７３５号 米国特許第８４２８８２０号

T. Fraichard及びA. Scheuer「From Reeds and Shepp’s to continuous curvature paths」

したがって、車両のリアルタイム制御に適するようにして、初期状態から目標状態まで車両の運動を制御するための連続曲率経路を構成することが必要とされている。

いくつかの実施形態の目的は、車両の初期状態を目標状態に接続する連続曲率経路を構成する経路計画システム及び方法を提供することである。いくつかの実施形態の別の目的は、車両のリアルタイム制御に適したそのようなシステム及び方法を提供することである。本明細書において使用されるときに、連続曲率経路は、１つの基本経路から隣接する基本経路への移行が、その一次導関数及び二次導関数だけでなく、基本経路の接合点においても連続しているように接続される、複数の基本経路の逐次合成である。基本経路の例は、直線、円弧、スプライン、多項式、三角関数、クロソイド及びクロソイドターンを含む。

車両の初期状態及び目標状態を接続する連続曲率経路は、基本経路の異なるシーケンスによって形成することができる。基本経路の逐次合成タイプは、本明細書において、基本経路のパターン、又は単にパターンと呼ばれる。例えば、１つの線、１つのスプライン及び１つの多項式が、特定の順序において配置される、３つの特定のタイプの基本経路を含むパターンを形成する。各基本経路の寸法、及びパターン内の基本経路の相互配置等の幾何学的パラメーターは、異なる可能性がある。

種々の実施形態によって使用される各パターンが、連続曲率経路を表す。本明細書において使用されるときに、いくつかの初期状態及び目標状態に関して、結果として初期状態及び目標状態を接続する連続曲率経路をもたらす、或るパターンを形成する基本経路のタイプに関する幾何学的パラメーターが存在する場合には、そのパターンは連続曲率経路を表す。例えば、直線と円弧とでは連続曲率経路を形成できず、それゆえ、これらのタイプはパターン内で隣接することはできない。一方、μ−接線条件が満たされる場合には、クロソイドターンと直線とで連続曲率経路を形成することができ、それゆえ、クロソイドターンと直線との組み合わせはパターンを形成することができる。

いくつかの実施形態は、基本経路の異なるパターンによって単一の連続曲率経路を形成できるという認識に基づく。さらに、単一のパターンに関して、要求される連続曲率経路を形成することができる、その基本経路の幾何学的パラメーターの異なる組み合わせが存在することができる。したがって、連続曲率経路を規定する系は、劣決定である。劣決定系の解法は一般に、計算コストがかかる反復プロセスを伴い、望ましくない可能性がある。

いくつかの実施形態は、パターン内の基本経路の相互配置に課せられる幾何学的制約が、劣決定系を決定系に変えることができるという理解に基づく。決定系は多くても１つの独立した解を有し、その解は解析的に見つけることができる。したがって、決定系の場合、計算コストがかかる反復解法を回避して、経路計画のプロセッサの性能及びメモリ使用を改善することができる。

いくつかの実施形態は、そのような付加的な幾何学的制約を、パターンごとにオフラインで決定できるという別の理解に基づく。この理解によれば、基本経路の最適なシーケンスのリアルタイム、すなわち、オンライン決定を、最適な幾何学的制約のオフライン決定に置き換えることができるようになる。異なるパターンでは最適な幾何学的制約が異なる可能性があり、それらの幾何学的制約を満たす連続曲率経路に関する解析解が反復最適化から生じることになる解に少なくとも近くなるように最適な幾何学的制約を決定することができる。

例えば、異なるパターンに関するいくつかの付加的な幾何学的制約は、パターンの対称性の利用、μ−接線条件の解析、及び特定のパターン内の実現可能な経路の長さの比較から決定される。例えば、１つの実施形態は、対称性を用いて、運転パターンＣＣ_ｕ｜Ｃ_ｕＣに関する付加的な幾何学的制約を導出する。それに加えて、又はその代わりに、幾何学的解析を利用して、運転パターンＣ｜Ｃ_ｕＣ_ｕ｜Ｃ、Ｃ｜Ｃ_π／２ＳＣ−１及びＣ｜Ｃ_π／２ＳＣ_π／２｜Ｃにおいて、連続曲率経路を解析的に決定する付加的な洞察を導出する。

そのために、いくつかの実施形態は、基本経路の一組のパターンに対応する一組の解析関数をあらかじめ決定する。オンライン経路計画中に初期状態及び目標状態の値を受信するのに応答して、それらの実施形態は、初期状態及び目標状態を接続する連続曲率経路の最小コストに対応する解析関数を選択し、選択された解析関数を用いて、その連続曲率経路のパラメーターを解析的に求める。そのようにして、望ましくない最適化手順が回避される。

例えば、１つの実施形態において、経路計画システムのメモリが、異なるパターンごとに決定された解析関数及びコスト関数を含む一対の関数を記憶する。初期状態及び目標状態の値を受信するのに応答して、その実施形態は、それらの対応するコスト関数を用いて各パターンのコストを求め、最小コストを有するパターンに対応する解析関数を選択し、選択された解析関数を呼び出して、連続曲率経路を生成する。例えば、連続曲率経路は、初期状態及び目標状態を接続する点の座標のシーケンスによって、又は結果として連続経路に従って車両を走行させることになる制御入力のシーケンスによって、又はその両方によって表すことができる。

いくつかの実施形態は、初期状態及び目標状態の全ての取り得る組み合わせの場合に、パターンが実現可能でない場合があるという認識に基づく。初期状態及び目標状態の特定の値の場合に、パターンが実現可能な連続曲率経路を表さない場合があることが起こり得る。それらの状況では、そのパターンに関して決定された解析関数は、初期状態及び目標状態の特定の値の場合に解を有しない。例えば、１つの実施形態において、パターンの解析関数が初期状態及び目標状態の特定の値の場合に解を有しないとき、そのパターンに関する連続経路のコストは無限に大きい。

いくつかの実施形態は、パターンが単純に選択されるときに、初期状態及び目標状態のいくつかの値の場合にいずれのパターンも実現可能な連続曲率経路を表さない状況が存在する可能性があり、経路計画手順が失敗する場合があるという認識に基づく。この状況を回避するために、いくつかの実施形態は、網羅的な一組のパターンに関する解析関数を決定する。本明細書において使用されるときに、初期状態及び目標状態の任意の値に関して、障害物を考慮することなく、初期状態及び目標状態を接続する実現可能な連続経路を表す少なくとも１つのパターンが存在する場合には、一組のパターンは合わせて、又は集合的に網羅的である。

例えば、いくつかの実施形態は、線分及び最小回転半径の接線円弧の逐次合成である、Ｒｅｅｄｓ−Ｓｈｅｐｐ（ＲＳ）経路の取り得るパターンによって着想を得た。ＲＳ経路は初期状態及び目標状態を接続する最短経路であり、４８個までの運転パターンを有することができ、それは１２クラスに更に分類される。ＲＳ経路は不連続曲率経路である。しかしながら、いくつかの実施形態は、円弧を対応するクロソイドターンに置き換えることによって、不連続ＲＳ経路を連続曲率経路に変換できるという認識に基づく。そのようにして、変換された連続ＲＳ経路は、ＲＳ経路の構造に酷似しており、それゆえ、変換されたＲＳ経路の４８パターンが網羅的に一組のパターンを形成する。本明細書において使用されるときに、変換された各ＲＳ経路は、クロソイドターン及び直線を含むグループから選択された基本経路の逐次合成によって形成される。１つの実施形態において、変換された各ＲＳ経路はクロソイドターンを含む。１つの実施形態において、変換された各ＲＳ経路は、クロソイドターン及び直線からなるグループから選択される基本経路を含む。

いくつかの実施形態は、ＣＣターン及び直線の間の連続曲率移行を確実にするために、変換されたＲＳ経路のパターン内の隣接する基本経路がμ−接線条件を満たす必要があるという認識に基づく。μ−接線条件は、隣接する基本経路の相互配置に関する幾何学的制約である。しかしながら、そのような幾何学的制約は、変換されたＲＳ経路の全てのパターンに関して決定系を形成できない。そのために、いくつかの実施形態は、変換されたＲＳ経路に関する付加的な幾何学的制約を課し、μ−接線条件及び付加的な幾何学的制約の両方を満たす解を有する解析関数を決定する。

したがって、１つの実施形態は、車両の運動を制御するシステムを開示する。そのシステムは、基本経路の一組のパターンに対応する一組の解析関数を記憶するメモリであって、各パターンは連続経路を表し、対応するパターンに従う基本経路の逐次合成によって入力状態を接続する連続経路を規定する、車両の入力状態に関する解析解を与えるように、対応するパターンごとに各解析関数が決定される、メモリと、車両の初期状態及び目標状態を受信するのに応答して、メモリから、初期状態を目標状態と接続する連続曲率経路の最小コストに対応する解析関数を選択し、選択された解析関数を用いて連続曲率経路のパラメーターを解析的に求める経路計画器と、連続曲率経路のパラメーターに従って車両の運動を制御するコントローラーとを備える。

別の実施形態は、車両の運動を制御する方法であって、その方法は、基本経路の一組のパターンに対応する一組の解析関数及び一組のコスト関数を記憶するメモリに結合されるプロセッサを使用し、各パターンは連続経路を表し、対応するパターンに従う基本経路の逐次合成によって入力状態を接続する連続経路を規定する、車両の入力状態に関する解析解を与えるように、対応するパターンごとに各解析関数が決定され、各コスト関数は、入力状態を接続し、対応するパターンによって表される連続経路に従う車両の運動のコストを示す対応するパターンのコストを与えるように決定され、プロセッサはその方法を実施する記憶された命令に結合され、命令は、プロセッサによって実行されるときに、方法の少なくともいくつかのステップを実行する方法を開示する。その方法は、車両の初期状態及び目標状態を受信することと、対応するパターンによって表され、初期状態を目標状態と接続する連続経路に従う車両の運動のコストを示す各パターンのコストを求めることと、最小コストを有するパターンの解析関数を選択することと、選択された解析関数を用いて、連続経路のパラメーターを解析的に求めることと、連続経路のパラメーターに従って車両の運動を制御することとを含む。

更に別の実施形態は、方法を実行するためにプロセッサによって実行可能であるプログラムを具現する非一時的コンピューター可読記憶媒体であって、メモリが、基本経路の一組のパターンに対応する一組の解析関数及び一組のコスト関数を記憶し、各パターンは連続経路を表し、対応するパターンに従う基本経路の逐次合成によって入力状態を接続する連続経路を規定する、車両の入力状態に関する解析解を与えるように、対応するパターンごとに各解析関数が決定され、各コスト関数は、入力状態を接続し、対応するパターンによって表される連続経路に従う車両の運動のコストを示す対応するパターンのコストを与えるように決定される、非一時的コンピューター可読記憶媒体を開示する。その方法は、車両の初期状態及び目標状態を受信することと、対応するパターンによって表され、初期状態を目標状態と接続する連続経路に従う車両の運動のコストを示す各パターンのコストを求めることと、最小コストを有するパターンの解析関数を選択することと、選択された解析関数を用いて、連続経路のパラメーターを解析的に求めることと、連続経路のパラメーターに従って車両の運動を制御することとを含む。

前方固定（front-fixed）ハンドル及び固定された平行な後輪を備える車両の概略図である。 いくつかの実施形態によって検討される連続曲率経路計画（ＣＣＰＰ）問題を示す概略図である。 いくつかの実施形態によって使用される最短長経路のクラス及び運転パターンを要約する表である。 曲率がクロソイドターンに沿っていかに変化するかを示すプロット図である。 そのプロットに従って変化する曲率を有するクロソイドターンの概略図である。 いくつかの実施形態による、ＳＣμ−接線条件を示す運転パターンの概略図である。 いくつかの実施形態による、ＣＣμ−接線条件を示す運転パターンの概略図である。 いくつかの実施形態による、Ｃ｜Ｃμ−接線条件を示す運転パターンの概略図である。 いくつかの実施形態による、車両の運動を制御するシステムのブロック図である。 いくつかの実施形態による、或る特定の運転パターンの場合の連続曲率経路のコストを解析的に評価するコスト関数を設計するブロック図である。 いくつかの実施形態による、固定運転パターンを用いて連続曲率経路を決定する解析関数を決定する方法のブロック図である。 いくつかの実施形態による、無衝突連続曲率経路を決定する方法のブロック図である。 １つの実施形態による、無衝突移行のグラフを構成する方法のフローチャートである。 １つの実施形態による、ステアリングを決定する方法のフローチャートである。 別の実施形態による、ステアリングを決定する方法のフローチャートである。 １つの実施形態による、自動化駐車システムの機能図である。 １つの実施形態による、運動計画システムの一般構造を示す図である。 いくつかの実施形態による、或る特定の運転パターンに関する連続曲率経路及びそのコストを決定するために見つけられる、又は課せられる種々の付加的な幾何学的制約の例を記載する表である。 いくつかの実施形態による、クラスＣＳＣ−１のパターンに従う連続曲率経路のためのコストの解析的推定を与えるコスト関数を示す概略図である。 いくつかの実施形態による、クラスＣＳＣ−２のパターンに従う連続曲率経路のためのコストの解析的推定を与えるコスト関数を示す概略図である。 いくつかの実施形態による、クラスＣ｜Ｃ｜Ｃのパターンに従う連続曲率経路のためのコストの解析的推定を与えるコスト関数を示す概略図である。 いくつかの実施形態による、クラスＣ｜ＣＣのパターンに従う連続曲率経路のためのコストの解析的推定を与えるコスト関数を示す概略図である。 いくつかの実施形態による、クラスＣＣ｜Ｃのパターンに従う連続曲率経路のためのコストの解析的推定を与えるコスト関数を示す概略図である。 いくつかの実施形態による、クラスＣＣ_ｕ｜Ｃ_ｕＣのパターンに従う連続曲率経路のためのコストの解析的推定を与えるコスト関数を示す概略図である。 いくつかの実施形態による、クラスＣ｜Ｃ_ｕＣ_ｕ｜Ｃのパターンに従う連続曲率経路のためのコストの解析的推定を与えるコスト関数を示す概略図である。 いくつかの実施形態による、クラスＣ｜Ｃ_π／２ＳＣ−１のパターンに従う連続曲率経路のためのコストの解析的推定を与えるコスト関数を示す概略図である。 いくつかの実施形態による、クラスＣ｜Ｃ_π／２ＳＣ−２のパターンに従う連続曲率経路のためのコストの解析的推定を与えるコスト関数を示す概略図である。 いくつかの実施形態による、クラスＣ｜Ｃ_π／２ＳＣ_π／２｜Ｃのパターンに従う連続曲率経路のためのコストの解析的推定を与えるコスト関数を示す概略図である。 いくつかの実施形態による、クラスＣＳＣ−１のパターンに従う連続曲率経路を決定するための解析解を与える解析関数を示す概略図である。 いくつかの実施形態による、クラスＣＳＣ−２のパターンに従う連続曲率経路を決定するための解析解を与える解析関数を示す概略図である。 いくつかの実施形態による、クラスＣ｜Ｃ｜Ｃのパターンに従う連続曲率経路を決定するための解析解を与える解析関数を示す概略図である。 いくつかの実施形態による、クラスＣ｜ＣＣのパターンに従う連続曲率経路を決定するための解析解を与える解析関数を示す概略図である。 いくつかの実施形態による、クラスＣＣ｜Ｃのパターンに従う連続曲率経路を決定するための解析解を与える解析関数を示す概略図である。 いくつかの実施形態による、クラスＣＣ_ｕ｜Ｃ_ｕＣのパターンに従う連続曲率経路を決定するための解析解を与える解析関数を示す概略図である。 いくつかの実施形態による、クラスＣ｜Ｃ_ｕＣ_ｕ｜Ｃのパターンに従う連続曲率経路を決定するための解析解を与える解析関数を示す概略図である。 いくつかの実施形態による、クラスＣ｜Ｃ_π／２ＳＣ−１のパターンに従う連続曲率経路を決定するための解析解を与える解析関数を示す概略図である。 いくつかの実施形態による、クラスＣ｜Ｃ_π／２ＳＣ−２のパターンに従う連続曲率経路を決定するための解析解を与える解析関数を示す概略図である。 いくつかの実施形態による、クラスＣ｜Ｃ_π／２ＳＣ_π／２｜Ｃのパターンに従う連続曲率経路を決定するための解析解を与える解析関数を示す概略図である。 １つの実施形態による、車両のシステムの概略図である。 いくつかの実施形態による、自動化駐車システム１０００のブロック図である。

図１Ａは、前方固定ハンドル及び固定された平行な後輪を備える車両１００の概略図を示す。点Ｒは車両の後輪の中央に位置する。車両の姿勢は、３つ組値（ｘ，ｙ，θ）によって一意的に記述される。ただし、（ｘ，ｙ）は地球座標系内のＲの座標を表し、θは地球座標系の正のｘ軸に対する車両の方位角である。車両は、軸距ｂ及びステアリング角φを有する。車両の運動学的モデルは、

によって与えられる。ただし、曲率κは（ｘ，ｙ，θ）に加えた追加の構成パラメーターである。車両モデル（１）の制御入力はｕ＝（ｖ，σ）である。ただし、ｖは、後輪の運転速度であり、σはステアリングレートである。φ、κ及びσの間の関係は以下のように確立される。

その車両について前方運動及び後方運動が許され、運転速度は制限されるのに対して、ハンドルの角度は、機械的制約を課せられる、すなわち、

であると仮定する。

また、ステアリングレートσも制限される、すなわち、

であると仮定される。

いくつかの実施形態によれば、車両の構成又は状態は、ｑ＝（ｘ，ｙ，θ，κ）によって規定される。

図１Ｂは、いくつかの実施形態によって検討される連続曲率経路計画（ＣＣＰＰ）問題を示す概略図を示す。具体的には、いくつかの実施形態が、車両の連続曲率経路のパラメーターを求める。より形式的には、この問題は以下のように定式化することができる。すなわち、初期構成１０１ｑ_ｓ＝（ｘ_ｓ，ｙ_ｓ，θ_ｓ，κ_ｓ）、最終構成１０２ｑ_ｆ＝（ｘ_ｆ，ｙ_ｆ，θ_ｆ，κ_ｆ）及び制約（２）、（３）を有する車両モデル（１）を与えられたときに、有限時間Ｔ_ｆについて、
１．境界条件

２．実現可能性∀ｔ∈［０，Ｔ_ｆ］，Ｐ_ＣＣ（ｔ）∈Ｒ^２×Ｓ^ｌ×［−κ_ｍａｘ，κ_ｍａｘ］
３．チャタリングなし
であるような、ｑ_ｓとｑ_ｆとの間の実現可能経路Ｐ_ＣＣを見つける。

任意の経路が（１）を満たし、境界条件は経路に沿った連続曲率κを有し、それゆえ、連続曲率経路であることに留意されたい。

図１Ｃは、ＲＳ経路とも呼ばれる、最短長経路のクラス及び運転パターンを要約する表１９９を示す。ＲＳ経路との関連において、図１ＣのＣは円弧を表し、Ｓは線分を表す。一方、Ｌ及びＲは左折及び右折を指定し、＋又は−はそれぞれ前方運動及び後方運動を表す。下付き文字は、或る特定の円弧の（絶対）角度値を表し、｜は走行方向の変化を表す。また、図１Ｂは、１０１及び１０２を接続するクラスＣＳＣ−１の運転パターンＲ＋Ｓ＋Ｒ＋における１つの例示的なＲＳ経路を示す。その例示的なＲＳ経路は、３つの部分、すなわち、円弧１０５、線１０６及び円弧１０７からなる。その曲率が点１０１、１０３、１０４及び１０２において不連続であり、それにより、静止ステアリング、時間遅延及び余分なタイヤ摩耗を被ることになるので、そのＲＳ経路は或る特定の基準によれば望ましくない。

いくつかの実施形態は、ＲＳ経路と同じようにしてクロソイドターン（ＣＴ又はＣＣターン）と線分とを組み合わせることによってＣＣＰＰを満たすことができるという認識に基づく。１つの実施形態において、クロソイドは、その曲率κが、その弧長ｓに対して線形に変化する曲線であり、すなわち、κ（ｓ）＝σ_ｍａｘｓである。ただし、σ_ｍａｘはクロソイドの尖鋭度（sharpness）である。

１つの実施形態において、０≦ｓ≦κ_ｍａｘ／σ_ｍａｘにわたって定義されるクロソイドの構成は、クロソイド弧に沿ってシステム（１）を積分することによって求めることができる。

ただし、

はそれぞれ、フレネル余弦積分及び正弦積分である。クロソイドは、ｑ_ｓ及びｑ_ｌを接続する弧として図１Ｅにプロットされる。

クロソイドの偏差（deflection）は、ｑ_ｓ及びｑ_ｌの方位の差を表す。一定のκ_ｍａｘ及びσ_ｍａｘの場合、クロソイドは以下の重要な特性を有する。
１．一定の偏差

２．一定の弧長ｓ＝κ_ｍａｘ／σ_ｍａｘ
３．一意に求められる解ｑ（ｓ）

これらの特性は、計算効率を高めるために、あらかじめ計算し、メモリに記憶することができる。

図１Ｄは、クロソイドターンに沿って曲率がいかに変化するかを示すプロット１００ｄを示す。図１Ｅは、プロット１００ｄに従って変化する曲率を有するクロソイドターンの概略図を示す。

そのようなクロソイドターンの一例が図１Ｅにおいてｑ_ｓからｑ_ｆまでの弧１１１によって与えられる。ただし、弧は、左前方クロソイドターンであり、左折しながら、前方に走行することに相当する。その偏差δ＝θ_ｓ−θ_ｆに応じて、クロソイドターンは、２つまでのクロソイド及び１つの円弧からなることができる。図１Ｅに示されるように、事例２δ_ｃ≦δ＜２δ_ｃ＋πを例示する、左前方クロソイドターン１１１は、以下を含む。
１．尖鋭度σ_ｍａｘを有する、ｑ_ｓからｑ_ｌまでのクロソイド。
２．ｑ_１から開始し、ｑ_２において終了する、半径

及び角度δ_ｆ−２δ_ｃの円弧。
３．尖鋭度−σ_ｍａｘでｑ_２から開始し、ｑ_ｆにおいて終了する第２のクロソイド。

δ＝０であるとき、クロソイドターンは長さ２Ｒ_Ωｓｉｎμの線分になる。ただし、Ｒ_Ω及びμは後に定義される。０≦δ＜２δ_ｃの場合、クロソイドターンは、尖鋭度σ≦σ_ｍａｘのクロソイド、及び尖鋭度−σの対称クロソイドを含む。ただし、尖鋭度σは、

によって与えられ、各クロソイドの長さは

である。２δ_ｃ＋π≦δ＜２πを有するクロソイドターンは、左後方運動に対応し、同様に取り扱うことができる。クロソイドターンはその偏差δによってパラメーター化でき、線分はその長さｌによってパラメーター化できることは明らかである。

幾何学的観点からＣＣＰＰを簡略化することに関する１つの概念はＣＣ円である。図１Ｅは、構成ｑ_ｓに関連付けられる４つのＣＣ円を示す。詳細には、

はそれぞれ、左前方クロソイドターン１１１、左後方クロソイドターン、右前方クロソイドターン及び右後方クロソイドターンに対応する。その対応するＣＣ円によって包囲されるクロソイドターンは常に、入口点及び出口点において、接線ベクトルとμ角をなす方向において、対応するＣＣ円に入り、ＣＣ円を出ることに留意されたい。

興味深いことに、各ＣＣ円は、それぞれのクロソイドターンの円弧と厳密に同じ中心を有する。例えば、Ω、すなわち、ＣＣ円１２１

の中心は、ｑ_１において車両方位に対して接線方向にある円弧の中心と一致する。その座標は

によって与えられる。

一定のκ_ｍａｘ及びσ_ｍａｘの場合、ＣＣ円半径及びμ−角は

によって与えられる。ただし、μはｑ_ｓにおける車両方位と、ＣＣ円１２１の接線ベクトルとの間の角度である。

いくつかの実施形態は、線分及びクロソイドターンを組み合わせることによって、ｑ_ｓとｑ_ｆとの間の連続曲率経路を決定するＣＣＰＰ法を開示する。１つの実施形態において、連続曲率経路は、図１ＣのＣが円弧の代わりにクロソイドターンを表すことを除いて、図１Ｃに記載されるＲＳ経路の同じクラス及び運転パターンを取ると仮定される。本実施形態は、図１Ｃに示されるそれらのクラスが、入力状態の任意の値に関して、障害物を考慮することなく、入力状態を接続する実現可能な連続曲率経路を表す少なくとも１つのパターンが存在することを保証する、網羅的な一組のパターンを形成するという理解に基づく。

具体的には、この実施形態は、円弧を対応するクロソイドターンに置き換えることによって、不連続ＲＳ経路を連続曲率経路に変換できるという認識に基づく。そのようにして、変換された連続ＲＳ経路は、ＲＳ経路の構造に酷似しており、それゆえ、変換されたＲＳ経路の４８パターンが網羅的な一組のパターンを形成する。本明細書において使用されるときに、変換された各ＲＳ経路は、クロソイドターン及び直線を含むグループから選択された基本経路の逐次合成によって形成される。１つの実施形態において、変換された各ＲＳ経路はクロソイドターンを含む。１つの実施形態において、変換された各ＲＳ経路は、クロソイドターン及び直線からなるグループから選択される基本経路を含む。

初期構成ｑ_ｓと最終構成ｑ_ｆとの間の、規定された運転パターンを伴う連続曲率経路は、クロソイドターンの偏差及び線分の長さによってパラメーター化することができる。例えば、運転パターンＬ＋Ｓ＋Ｌ＋を仮定すると、連続曲率経路は、偏差δ_１を有する左前方クロソイドターンで開始し、それに、長さｌを有する線分と、偏差δ_２を有する別の左前方ターンとが後続する。パラメーターδ_１、δ_２、ｌ及び初期構成ｑ_ｓの値を与えられると、φ（ｑ_ｓ，ｔ）によって表される、（１）の解を得るのは、当業者にとって容易であり、その最終構成はφ（ｑ_ｓ，δ_１、ｌ、δ_２）によって与えられる。ＣＣＰＰ問題の定式化によれば、ｑ_ｓ及びｑ_ｆの場合のＣＣＰＰは、一組の非線形方程式

を満たす

を求めることと同じである。

一組の方程式（７）はパラメーターに関して非凸であり、それゆえ、確実に解けるわけではない。また、それは反復アルゴリズムに頼り、計算コストがかかる。

１つの実施形態において、クロソイドターン及び線の組み合わせを決定するμ−接線条件を用いて、連続曲率経路を再パラメーター化し、ＣＣＰＰを実行することができる。図１Ｃの運転パターンに適合する連続曲率経路において３つの取り得る組み合わせ、すなわち、ＳＣ、ＣＣ、Ｃ｜Ｃが存在する。そのために、３つのタイプのμ−接線条件が存在する。

図１Ｆは、いくつかの実施形態に従ってＣＣ円１３２を一意に決定する、線１３３とクロソイドターンとの間のＳＣμ−接線条件を示す運転パターンの概略図を示す。クロソイドターンは１０１で開始し、ｑ_１１３１において線１３３に移行する。クロソイドターンと線１３３との間のＳＣμ−接線条件は、ｑ_１１３１におけるＣＣ円１３２の接線ベクトル１３４と線１３３との間の角度がμになることである。

図１Ｇは、いくつかの実施形態による、ＣＣ円１４１及び１４２にそれぞれ対応する、左前方クロソイドターンと右前方クロソイドターンとの間のＣＣμ−接線条件を示す運転パターンの概略図を示す。ＣＣμ−接線条件は、以下を規定する。
１．ＣＣ円１４１及び１４２は接触していなければならない。
２．ＣＣ円１４１及び１４２の中心１４３と１４４との間の距離Ｌ（Ω_１Ω_２）は２Ｒ_Ωに等しい。

図１Ｈは、いくつかの実施形態による、ＣＣ円１４５及び１４６にそれぞれ対応する、左前方クロソイドターンと右後方クロソイドターンとの間のＣ｜Ｃμ−接線条件を示す運転パターンの概略図を示す。Ｃ｜Ｃμ−接線条件は、以下を規定する。
１．ＣＣ円１４５及び１４６は互いに交差し、交差構成１４９において、ＣＣ円１４５及び１４６それぞれの接線ベクトル１４７及び１４８は、１４９において車両の方位１４９に対してμの角度をなす。
２．ＣＣ円１４５及び１４６の中心１５１と１５２との間の距離Ｌ（Ω_１Ω_２）は２Ｒ_Ωｃｏｓμになるべきである。

いくつかの実施形態は、基本経路の異なるパターンによって単一の連続曲率経路を形成できるという認識に基づく。さらに、単一のパターンに関して、要求される連続曲率経路を形成することができる、その基本経路の幾何学的パラメーターの異なる組み合わせが存在することができる。したがって、連続曲率経路を規定する系は、劣決定である。劣決定系の解法は一般に、計算コストがかかる反復プロセスを伴い、望ましくない可能性がある。

いくつかの実施形態は、パターン内の基本経路の相互配置に課せられる幾何学的制約が、劣決定系を決定系に変えることができるという理解に基づく。決定系は多くても１つの独立した解を有し、その解は解析的に見つけることができる。したがって、決定系の場合、計算コストがかかる反復解法を回避して、経路計画のプロセッサの性能及びメモリ使用を改善することができる。

いくつかの実施形態は、そのような付加的な幾何学的制約を、パターンごとにオフラインで決定できるという別の理解に基づく。この理解によれば、基本経路の最適なシーケンスのリアルタイム、すなわち、オンライン決定を、最適な幾何学的制約のオフライン決定に置き換えることができるようになる。異なるパターンでは最適な幾何学的制約が異なる可能性があり、それらの幾何学的制約を満たす連続曲率経路に関する解析解が反復最適化から生じることになる解に少なくとも近くなるように最適な幾何学的制約を決定することができる。

そのために、いくつかの実施形態は、図１Ｃの運転パターン等の種々の運転パターンに関する連続曲率経路及び／又はそのコストを解析的に計算する。例えば、１つの実施形態は、ＣＣ円の中心と線との間の角度α、θによる連続曲率経路の再パラメーター化を用いて、解析的に解くことを可能にする。これらのパラメーターは、μ−接線条件、存在条件、幾何学的制約及び運転パターンの対称性を利用することによってオフラインで一意に特定することができる。したがって、連続曲率経路のコストも解析的に取得することができる。

図２Ａは、いくつかの実施形態による、車両の運動を制御するシステムのブロック図を示す。そのシステムは、基本経路の一組のパターン２３１に対応する一組の解析関数２３５を記憶するメモリ２３０を備える。各パターンは連続経路を表し、対応するパターンに従う基本経路の逐次合成によって入力状態を接続する連続経路を規定する、車両の入力状態に関する解析解を与えるように、対応するパターンごとに各解析関数が決定される。

また、メモリ２３０は、任意選択で、一組のパターン及び一組の解析関数に対応する一組のコスト関数２３３を記憶することもできる。コスト関数２３３によって、対応するパターンに従う連続曲率経路のコストを解析的に計算できるようになる。コストの例は、パターンに従う経路に沿った長さ及び／又は進行時間を含む。また、メモリ２３０は、任意選択で、一組のパターン２３１自体を記憶することもできる。それに加えて、又はその代わりに、一組のパターンに関する一組の解析関数及びコスト関数が決定されるが、パターンの組自体はメモリに記憶されない。

経路計画器２４０は、本明細書において初期状態及び目標状態と呼ばれる一対の入力状態を接続する連続曲率経路を解析的に決定するように構成される（２４５）。例えば、経路計画器は、車両の初期状態及び目標状態を受信する（２４１）のに応答して、メモリ２３０から、初期状態を目標状態と接続する連続曲率経路の最小コストに対応する解析関数を選択するように（２４３）、そして、選択された解析関数を用いて連続曲率経路のパラメーターを解析的に求めるように（２４７）プログラムされるプロセッサを用いて実現することができる。

コントローラー２５０は、連続曲率経路のパラメーターに従って車両の運動を制御する。例えば、連続曲率経路のパラメーターは、連続曲率経路を形成する車両の運動の平面上の点の座標のシーケンスを含む。それに加えて、又はその代わりに、連続曲率経路のパラメーターは、制御入力のシーケンスを含み、制御入力のシーケンスに従う車両の運動の制御は、連続曲率経路に従う車両の状態に移行する。

メモリが一組のパターン及び一組の解析関数に対応する一組のコスト関数を記憶するとき、経路計画器２４０は、対応するコスト関数を用いて各パターンのコストを求めることができ、最小コストを有するパターンに対応する解析関数を選択することができる。それに加えて、又はその代わりに、経路計画器は、初期状態及び目標状態を接続するＲＳ経路を構成し、ＲＳ経路のパターンに対応する解析関数を選択することができる。そのような場合、コスト関数の構成及び／又は記憶は不要である。

１つの実施形態では、一組のパターンは、入力状態の任意の値に関して、障害物を考慮することなく、入力状態を接続する実現可能な連続曲率経路を表す少なくとも１つのパターンが存在することを保証する網羅的な組である。例えば、１つの実施形態では、一組のパターンは、変換されたＲｅｅｄｓ−Ｓｈｅｐｐ（ＲＳ）経路を表す４８個の異なるパターンを含み、組内の各パターンは、ＲＳ経路を表す対応するパターン内の円弧をクロソイドターンに置き換えることによって形成される。

例えば、１つの実施形態では、解析関数は、連続曲率経路を形成する隣接する基本経路の相互配置に関するμ−接線条件を満たす解を与えるように決定され、解析関数のうちの少なくともいくつかは、少なくとも１つの境界基本経路に対する少なくとも１つの中間基本経路の相互配置に関する付加的な幾何学的制約を満たす解を与えるように決定される。

ｑ_ｓ及びｑ_ｆを与えられると、この実施形態は、解析的なコスト式を用いて、図１Ｃの全てのパターンに関する連続曲率経路のコストを計算する。コスト式は、ｑ_ｓ及びｑ_ｆに関連付けられるＣＣ円に関する知識、例えば、半径Ｒ_Ω及び中心Ωに頼る。また、コスト式は、クロソイドの特性、例えば、μ角、偏差δ_Ｃ及び長さｌ_Ｃにも頼る。いくつかの実施形態において、この知識は全て、システム動態（１）を積分することを伴い、オンラインで非常に効率的に取得することができる。いくつかの実施形態において、これらの知識は、κ_ｍａｘ及びσ_ｍａｘのあらかじめ選択された対に関して、あらかじめ計算し、メモリに記憶することができる。

図２Ｂは、いくつかの実施形態による、入力状態２１１間、すなわち、初期構成ｑ_ｓと最終構成ｑ_ｆとの間の或る特定の運転パターンを伴う連続曲率経路のコストを解析的に評価するコスト関数を設計するブロック図を示す。（４）としてクロソイドに沿ってあらかじめ計算された構成、偏差及びμ角が２０１において記憶され、ＣＣ円についての他の値、例えば、ＣＣ円の中心及び半径があらかじめ計算され、記憶される（２０２）。あらかじめ計算された値と、ｑ_ｓ及びｑ_ｆとを用いて、ｑ_ｓ及びｑ_ｆに関連付けられるＣＣ円の中心、及びパラメーターα、θが求められる（２０３）。これら全ての情報を用いて、コスト関数が、存在条件２０５及び幾何学的制約２０７に従って、実現可能性、すなわち、ｑ_ｓとｑ_ｆとの間のそのような連続曲率経路を見つけることが実現可能であるか否かを判断する（２０４）。実現不可能である場合には、コスト関数は、実現不可能を示す値、例えば、無限値又は非常に大きい値を返す。そうでない場合には、クロソイドターンの偏差及び線分の長さ等のパラメーターが、コスト式２０９を表す、構成ｑ_ｓ及びｑ_ｆのα、θ及び方位の関数として求められる。１つの実施形態において、偏差パラメーターが２１０において検証条件２０６に対して更に検証され、それは、検証条件が違反される場合には実現不可能を示す。

いくつかの実施形態において、一定のκ_ｍａｘ及びσ_ｍａｘの対に対してシステム動態（１）を満たすクロソイドが、あらかじめ計算され、メモリ２０１に記憶され、連続曲率経路を解析的に決定するためにオンラインで再利用することができる。

図２Ｃは、いくつかの実施形態による、一定の運転パターンを伴う、初期構成ｑ_ｓと最終構成ｑ_ｆとを接続する連続曲率経路を決定する解析関数２２０を決定する方法のブロック図を示す。運転パターン及びそのパラメーター、クロソイドターンの偏差及び線の長さ２２１を与えられると、解析関数は、それらのパラメーターによって一意に決定される入力ｕ、σを用いて、システム動態（１）の解として連続曲率経路を生成する。

１つの実施態様において、解析関数は、４つの円弧生成ブロック、すなわち、その開始構成及び偏差に基づいて左前方クロソイドターンに対応する円弧を生成するためのＬＦＣＴ２２２、その開始構成及び偏差に基づいて左後方クロソイドターンに対応する円弧を生成するためのＬＢＣＴ２２３、その開始構成及び偏差に基づいて右前方クロソイドターンに対応する円弧を生成するためのＲＦＣＴ２２４、その開始構成及び偏差に基づいて右後方クロソイドターンに対応する円弧を生成するためのＲＢＣＴ２２５と、その開始構成及び長さに基づいて線分を生成するためのＬＩＮＥ２２６とを含むことができる。運転パターンによって指定される順序において、２２２、２２３、２２４、２２５及び２２６にそれぞれ、入力として開始構成、偏差又は長さを与えることによって、連続曲率経路を構成することができる。例えば、運転パターンＬ＋Ｓ＋Ｒ＋において実現可能な連続曲率経路を計算するために、解析関数２２０は、左前方クロソイドターンの偏差、前方線分の長さ、右前方クロソイドターンの偏差、及び初期構成をそれぞれ表す、入力δ_１、ｌ、δ_２及びｑ_ｓを必要とする。解析関数は、最初にＬＦＣＴ（ｑ_ｓ，δ_１）２２２を用いて、入力としてδ_１及びｑ_ｓを有する左前方クロソイドターン円弧２２２を取得し、それはｑ_１において終了し、その後、ＬＩＮＥ（ｑ_１，ｌ）２２６を用いて、入力ｌと、その開始構成としてｑ_１とを有する、ｑ_１において左前方クロソイドターンに対して接線方向にある線を求め、それは構成ｑ_２において終了し、最後に、ＲＦＣＴ（ｑ_２，δ_２）２２４を用いて、ｑ_２において線分と接線方向において接続され、最終構成ｑ_ｆに達する右前方クロソイドターン円弧を計算する。連続曲率経路は、左前方クロソイドターン、前方線分及び右前方クロソイドターンの和集合、すなわち、

である。解析関数は、異なる運転パターンごとに、図８Ａ〜図８Ｊによって更に示される。

図３は、いくつかの実施形態による、障害物を含む環境において、２つの構成ｑ_ｓとｑ_ｆとの間の無衝突連続曲率経路を決定する方法のブロック図を示す。車両のセンサーを用いて、１つの実施形態はセンサーから環境マップを構成し、それは車両の周囲についての車両の知識を表す。障害物は、他の車両、歩行者、建物、歩道等を表すことができる。１つの実施形態において、環境マップは、全ての障害物を検出し、車両上のセンサーによって検知することができる全ての障害物及び目に見える周囲物の境界を、障害物又は可視空間ごとの最小境界ボックスを表す単純な幾何学的形状、例えば、長方形として近似することによって導出される。障害物、及び可視空間の境界が幾何学的に近似されると、幾何学的物体のリストによって環境マップを完全に記述することができる。それに加えて、又はその代わりに、障害物を多面体によって近似することができるが、経路計画における計算負荷が大きくなる場合がある。

無衝突連続曲率経路計画器は最初に、環境マップ及び初期／最終構成３０１、並びに車両運動学的モデル３０３に従って、グラフＧを構成する。グラフＧはノード集合Ｎ及びエッジ集合Ｅを含み、Ｎ内の各ノードは無衝突構成ｑ_ｉに対応し、２つのノードｑ_ｉとｑ_ｊとの間のＥ内の各エッジは、ｑ_ｉとｑ_ｊとの間の無衝突連続曲率経路の存在を暗示する。構成３０２は、初期構成ｑ_ｓを最終構成ｑ_ｆに接続する一組のエッジを含む場合には、グラフＧを出力する。その後、Ａ^＊、Ｄ、Ｄ^＊等の現行技術水準の探索アルゴリズムを使用することによって、グラフＧに基づいて、３０４においてｑ_ｓとｑ_ｆとの間の無衝突連続曲率経路Ｐが決定される。車両の動的モデル３０６に基づいて、３０５において、車両が連続曲率経路に従うことを強制する基準軌道が決定される。基準軌道を更に利用して、車両上のアクチュエーターを制御し、車両の運動を制御する。

いくつかの実施形態において、基準軌道は、経時的な車両速度及びステアリング角のプロファイルを規定する。別の実施形態において、基準軌道は、経時的な車両状態（ｘ，ｙ，θ）のプロファイルを規定する。車両動的モデルは５次であり、

によって与えられる。ただし、ａ_１は並進加速度であり、ａ_２はステアリング角速度である。車両運動学的モデル３０１は（１）によって与えられる。

図４Ａは、１つの実施形態による、グラフＧを構成する（３２０）方法のフローチャートを示す。最初に、Ｇが、空集合のエッジ

、及びノード集合Ｎ＝｛ｑ_ｓ｝で初期化される。その方法は、或る特定のサンプリング方式に従って、（１）の状態空間をサンプリングし（４０１）、無衝突構成４１１ ｑ_ｎｅｗを取得する。新たな無衝突構成４１１を用いて、その方法は、Ｎに属し、ｑ_ｎｅｗに最も近い近隣ノードｑ_ｎｅａｒを特定する（４０２）。ｑ_ｎｅａｒを初期構成として、ｑ_ｎｅｗを最終構成として用いて、ＣＣステアリング４０３が、本発明において開示される実施形態によるＣＣＰＰを実行し、可能である場合には、連続曲率経路Ｐ（ｑ_ｎｅａｒ，ｑ_ｎｅｗ）を出力し、そうでない場合には、実現不可能とする。信号４１３は、有効な連続曲率経路、又は実現不可能を示すパラメーターとすることができる。その方法は更に、連続曲率経路Ｐ（ｑ_ｎｅａｒ，ｑ_ｎｅｗ）が環境マップ内で障害物と衝突するか否かを判断する（４０４）。その経路が無衝突である場合には、４０５において、ノード集合Ｎに新たな構成ｑ_ｎｅｗが追加され、ｑ_ｎｅａｒ及びｑ_ｎｅｗを接続するエッジがエッジ集合Ｅに追加される。その後、Ｇの構成から抜けるために、４０６において、或る特定の中止基準が試験される。１つの実施形態において、ｑ_ｓ及びｑ_ｆを接続する一組のエッジが存在するとき、構成が中止される。別の実施形態において、ｑ_ｓとｑ_ｆとの間の経路数が或る特定の閾値を超えると、構成が中止される。中止基準が満たされない場合には、構成手順が繰り返される。

図４Ｂは、１つの実施形態による、ＣＣステアリング４０３を決定する方法のフローチャートを示す。２つの構成ｑ_ｎｅａｒ及びｑ_ｎｅｗを与えられると、その方法は、図１Ｃ内の全ての運転パターンを列挙することによって、ｑ_ｎｅａｒとｑ_ｎｅｗとの間のＲＳ経路の存在を試験する、Ｒｅｅｄｓ−Ｓｈｅｐｐ（ＲＳ）ステアリング４２１を最初に実行し、実現可能なＲＳ経路に対応する一組の運転パターン４３１を返す。コスト関数２００が、４３１内の運転パターンごとに、ＣＣＰＰの実現可能性を判断し、コストを求める。信号４３２は、ｑ_ｎｅａｒとｑ_ｎｅｗとの間の実現可能な連続曲率経路を有する、４３１内の運転パターンと、その対応するコストとを含む。最小コストをもたらす最良の運転パターンが選択され、解析関数２２０によって、対応する連続曲率経路が決定される（４２３）。

図４Ｃは、別の実施形態による、ＣＣステアリング４０３を決定する方法のフローチャートを示す。２つの構成ｑ_ｎｅａｒ及びｑ_ｎｅｗを与えられると、その方法は、コスト関数２００を使用することによって、ＣＣＰＰ４４１を実行し、図１Ｃ内の全ての運転パターンを列挙することによって、ｑ_ｎｅａｒとｑ_ｎｅｗとの間の連続曲率経路の存在を試験し、これらの運転パターンの実現可能性及びコストを返す。最小コストをもたらす最良の運転パターンが選択され、解析関数２２０によって、対応する連続曲率経路が決定される（４４２）。

図５Ａは、１つの実施形態による、自動化駐車システムの機能図を示す。環境マップ作成及び位置特定ブロック５０１が、環境及び車両動作条件を検知することによって、駐車空間のマップを構成又は更新し、車両の現在の場所を特定する。例えば、３軸加速度計（複数の場合もある）、３軸ジャイロスコープ（複数の場合もある）及び／又は磁力計（複数の場合もある）を含む場合がある慣性測定ユニットを用いて、車両動作を検知することができる。全地球測位システムセンサーを用いて、車両の位置及び速度を与えることができる。環境５００を検知するセンサーは、他の車両、歩行者及び建物を含む障害物を捕捉するビデオカメラ、車両と障害物との間の距離を検出する超音波／レーダーセンサー等とすることができる。最終状態選択ブロック５０１が、駐車場候補を識別することによって、車両を駐車する駐車位置に関する最終状態を選択し、最終状態を運動計画ブロック５０３に送信する。１つの実施形態において、利用可能な駐車位置は、駐車場ビルの管理に関連付けられる別個のシステムによって追跡される。

それに加えて、又はその代わりに、駐車位置は、自動化駐車システムのセンサー５３０を用いて検出することができる。１つの実施形態において、運動計画ブロックが、最終状態が駐車可能であるか否か、すなわち、駐車位置までの実現可能な経路があるか否かを判断するようにチェックし、チェック結果を最終状態選択ブロック５０２に通知する。最終状態が駐車可能でない場合には、目標選択ブロック５０２が、評価対象の別の最終状態を選択する。別の実施形態では、最終状態選択ブロック５０１が、最終状態が駐車可能であるか否かを評価することもでき、駐車可能な最終状態のみを運動計画ブロックに送信する。

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

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

図５Ｂは、１つの実施形態による、運動計画システム５０３の全体構造を示す。運動計画システム５０３は、運動計画システム５０３のモジュールを実行するための少なくとも１つのプロセッサ５７０を備える。プロセッサ５７０は、メモリ５８０に接続され（５７１）、メモリは、車両のジオメトリ及び駐車空間のマップ等の幾何学的情報５８１を記憶する。また、メモリ５８０は、車両の運動学的モデル及び車両の動的モデル等の車両のモデル５８２を記憶することもできる。また、メモリ５８０は、限定はしないが、車両の初期状態、駐車後の車両の最終状態、コスト関数、計算された各状態の値、κ_ｍａｘ、σ_ｍａｘ、一組のκ_ｍａｘ及びσ_ｍａｘ対に関してあらかじめ計算されたクロソイド及びそのμ角及び偏差、一対の（κ_ｍａｘ，σ_ｍａｘ）に対応するＣＣ円の半径、各状態につながる運動、運動学的グラフ、基準軌道を含む、運動計画器の内部情報５８３を記憶することもできる。いくつかの実施形態において、メモリ５８０は、自動化駐車のための方法を実施する記憶された命令を含むことができ、それらの命令は、プロセッサ５７０によって実行されるときに、その方法の少なくともいくつかのステップを実行する。

いくつかの実施形態では、解析関数は、連続曲率経路を形成する隣接する基本経路の相互配置に関するμ−接線条件を満たす解を与えるように決定される。解析関数のうちの少なくともいくつかは、少なくとも１つの境界基本経路に対する少なくとも１つの中間基本経路の相互配置に関する付加的な幾何学的制約を満たす解を与えるように決定される。いくつかの実施形態は、コスト関数２００及び解析関数２２０に課せられる存在条件、幾何学的制約及び検証条件を用いて、ｑ_ｓ及びｑ_ｆを接続する連続曲率経路を決定する。

図６は、いくつかの実施形態による、或る特定の運転パターンに関する連続曲率経路及びそのコストを決定するために見つけられるか、又は課せられる種々の付加的な幾何学的制約の例を記載する表６１０を示す。明確にするために、この例は、左前方クロソイドターンＬ＋で開始する運転パターンのＣＣ経路のみを決定し、零曲率初期及び最終構成ｑ_ｓ＝（０，０，０，０）及びｑ_ｆ＝（ｘ_ｆ，ｙ_ｆ，θ_ｆ，０）のみを取り扱う（これ以降、標準経路と見なされる）。また、２つのＣＳＣ_π／２｜ＣクラスはＣ｜Ｃ_π／２ＳＣクラスと同じように構成することができるので、本発明はここで、１０クラス、すなわち、ＣＳＣ−１、ＣＳＣ−２、Ｃ｜Ｃ｜Ｃ、Ｃ｜ＣＣ、ＣＣ｜Ｃ、ＣＣ_ｕ｜Ｃ_ｕＣ、Ｃ｜Ｃ_ｕＣ_ｕ｜Ｃ、Ｃ｜Ｃ_π／２ＳＣ−１、Ｃ｜Ｃ_π／２ＳＣ−２及びＣ｜Ｃ_π／２ＳＣ_π／２｜Ｃに関する存在条件を明示する。

図７Ａは、いくつかの実施形態による、クラスＣＳＣ−１のパターンに従う連続曲率経路のためのコストの解析的推定を与えるコスト関数を示す概略図を示す。例えば、図７Ａは、コスト関数２００において使用される、７００Ａ ｑ_０及び７９９Ａ ｑ_ｆを接続するＬ＋Ｓ＋Ｌ＋経路の条件及び式を開示する表７１０Ａを示す。この目的で、７００Ａｑ_ｓに関するＣＣ円７０１Ａ

及び７９９Ａ ｑ_ｆに関するＣＣ円７０２Ａ

が求められ、第１の左クロソイドターンの構成７１５Ａ ｑ_１及び第２の左クロソイドターンの構成７１６Ａ ｑ_２をそれぞれ特徴付けるために使用される。７１１Ａ θが線７０５Ａ Ω_１Ω_２と正のｘ軸との間の角度を表すものとする。７０１Ａと線７０６Ａ ｑ_１ｑ_２との間、及び線７０６Ａと７０２Ａとの間のμ−接線条件を達成するために、幾何学的解析が、直線７０６Ａが線７０５Ａに平行であることを示す。

それゆえ、ｑ_０及びｑ_ｆを接続するＬ＋Ｓ＋Ｌ＋経路は以下を含む。
１．左クロソイドターンＣＴ_１：ＣＴ_１の７１５Ａの方位が７１１Ａと同じであるので、ＣＴ_１の偏差：δ_１＝θを有する。
２．長さ

の線分７０６Ａ。ただし、Ｌ（Ω_１Ω_２）は線７０５Ａの長さを表す。
３．第２の左クロソイドターンＣＴ_２：ＣＴ_２の７１６Ａの方位もθであるので、ＣＴ_２の偏差はδ_２＝θ_ｆ−θになるはずである。

存在条件：Ｌ（Ω_１Ω_２）≧２Ｒ_Ωｓｉｎμ及びθ_ｆ≧θがいずれも満たされたときに限って、そのようなＬ＋Ｓ＋Ｌ＋経路が存在する。検証条件は、全てのクロソイドターンの偏差の絶対値がπ＋２δ_Ｃ未満であることを要求する。関数ａｒｃｌｅｎは、クロソイドターンの長さを計算し、その入力としてクロソイドターンの偏差が用いられる。

図７Ｂは、いくつかの実施形態による、クラスＣＳＣ−２のパターンに従う連続曲率経路のためのコストの解析的推定を与えるコスト関数を示す概略図を示す。例えば、図７Ｂは、７００Ｂ ｑ_０と７９９Ｂ ｑ_ｆとの間のＬ＋Ｓ＋Ｒ＋標準経路の条件及び式を開示する表７１０Ｂを示し、ＣＣ円７０２Ｂ

を用いて、７１６Ｂ ｑ_２を特徴付け、直線７０６Ｂ ｑ_１ｑ_２が線７０５Ｂ Ω_１Ω_２を横切る。αが、線７０５Ｂと７０６Ｂとの間の角度７１２Ｂを表すものとする。

によってαを求めることができることが明確に確認される。

それゆえ、所望の標準経路Ｌ＋Ｓ＋Ｒ＋は、（９）を用いて生成することができる。
１．偏差δ_１＝θ＋αを伴う左クロソイドターンＣＴ_１。
２．長さ

の線分７０６Ｂ。
３．第２の左クロソイドターンＣＴ_２：ＣＴ_２の構成７１６Ｂ ｑ_２は７１５Ｂ ｑ_１と同じ方位を共有するので、ＣＴ_２の偏差はδ_２＝θ_ｆ−θ−αになるはずであると結論付けられる。

存在条件：Ｌ（Ω_１Ω_２）≧２Ｒ_Ω及びδ_１、δ_２がいずれも検証条件を満たすときに限って、そのようなＬ＋Ｓ＋Ｒ＋経路が存在する。

図７Ｃは、いくつかの実施形態による、クラスＣ｜Ｃ｜Ｃのパターンに従う連続曲率経路のためのコストの解析的推定を与えるコスト関数を示す概略図を示す。例えば、図７Ｃは、７００Ｃ ｑ_０及び７９９Ｃ ｑ_ｆを接続する標準Ｃ｜Ｃ｜Ｃ経路、すなわち、Ｌ＋Ｒ−Ｌ＋経路の条件及び式を開示する表７１０Ｃを示す。７００Ｃ ｑ_０に関するＣＣ円７０１Ｃ

及び７９９Ｃ ｑ_ｆに関するＣＣ円７０２Ｃ

が、第１の左クロソイドターンの構成７１５Ｃ ｑ_１及び第２の左クロソイドターンの構成７１６Ｃ ｑ_２を特徴付けるために使用される。

Ｌ＋Ｒ−Ｌ＋経路は、２つの左前方クロソイドターンが右後方クロソイドターンによって接続されることを示唆しており、それは、ＣＣ円７０３Ｃ

が

と一致しなければならないことを暗示する。Ω_３が、ＣＣ円７０３Ｃの中心を表すものとし、αが、線７０６Ｃ Ω_１Ω_２と線７０５Ｃ Ω_１Ω_３との間の角度を表すものとする。７０１Ｃ

と７０３Ｃ

との間、及び７０３Ｃ

と７０２Ｃ

との間のμ−接線条件は、

を示唆する。

三角形Ω_１Ω_２Ω_３に余弦則を適用すると、

がもたらされる。

それゆえ、Ω_３の座標は、

によって与えることができる。

θ_１が、線７０７Ｃ Ω_２Ω_３と正のｘ軸との間の角度７１３Ｃを表すものとする。

経路Ｌ＋Ｒ−Ｌ＋は以下を含む。
１．左クロソイドターンＣＴ_１：７０１Ｃ

と７０３Ｃ

との間のμ−接線条件は、７１５Ｃ ｑ_１の方位がθ＋α＋π／２であり、それが厳密にＣＴ_１の偏差δ_１であることを断定する。
２．右後方クロソイドターンＣＴ_２：７１６Ｃ ｑ_２の方位は、７０３Ｃ

と７０２Ｃ

との間のμ−接線条件に従って、θ_１−π／２であるので、ＣＴ_２の偏差はδ_２＝θ_１−θ−α−πである。
３．偏差δ_３＝θ_ｆ−θ_１＋π／２を伴う第２の左クロソイドターンＣＴ_３。

存在条件：三角形Ω_１Ω_２Ω_３は有効になるはずであり、すなわち、

である。

検証条件：δ_１、δ_２及びδ_３の絶対値がπ＋２δ_Ｃ未満である。

図７Ｄは、いくつかの実施形態による、クラスＣ｜ＣＣのパターンに従う連続曲率経路のためのコストの解析的推定を与えるコスト関数を示す概略図を示す。例えば、図７Ｄは、運転パターンＬ＋Ｒ−Ｌ−におけるＣ｜ＣＣ経路の条件及び式を開示する表７１０Ｄを示す。ＣＣ円７０１Ｄ

の中心Ω_１及びＣＣ円７０２Ｄ

の中心Ω_２を計算した後に、中間構成７１５Ｄ ｑ_１及び７１６Ｄ ｑ_２と、Ω_３として表される、ＣＣ円７０３Ｄ

の中心とを求めることを目標とする。

７０１Ｄ

と７０３Ｄ

との間、及び７０３Ｄ

と７０２Ｄ

との間のμ−接線条件に基づいて、

が得られる。

αが、線７０５Ｄ Ω_１Ω_２と線７０６Ｄ Ω_１Ω_３との間の角度７１２Ｄを表すものとする。余弦則は、三角形Ω_１Ω_２Ω_３に関して、

であることを示唆する。

αが（１４）によって計算されることを除いて、Ω_３の座標も（１２）によって表すことができる。θ_１が、線７０７Ｄ Ω_２Ω_３と正のｘ軸との間の角度７１３Ｄを表すものとする。したがって、経路Ｌ＋Ｒ−Ｌ−は以下を含む。
１．偏差δ_１＝θ＋α＋π／２を伴う左クロソイドターンＣＴ_１。
２．右後方クロソイドターンＣＴ_２：７１６Ｄ ｑ_２の方位はθ_１−π／２−μであるので、ＣＴ_２の偏差はδ_２＝θ_１−θ−α−μ−πである。
３．偏差δ_２＝θ_ｆ−θ_１＋π／２＋μを伴う左後方クロソイドターンＣＴ_３。

存在条件：三角形Ω_１Ω_２Ω_３は有効になるはずであり、すなわち、

である。

検証条件：δ_１、δ_２及びδ_３の絶対値がπ＋２δ_Ｃ未満である。

図７Ｅは、いくつかの実施形態による、クラスＣＣ｜Ｃのパターンに従う連続曲率経路のためのコストの解析的推定を与えるコスト関数を示す概略図を示す。例えば、図７Ｅは、７００Ｅと７９９Ｅとの間のＣＣ｜Ｃ経路の条件及び式を開示する表７１０Ｅを示し、ＣＣ円７０１Ｅ

及び７０２Ｅ

の中心はそれぞれΩ_１及びΩ_２である。

７１５Ｅ ｑ_１及び７１６Ｅ ｑ_２を、その経路に沿った２つの中間構成とする。ＣＣ円７０３Ｅ

の中心Ω_３を求めるために、三角形Ω_１Ω_３Ω_２において余弦則を適用し、角度７１２Ｅ αを得る。

それはΩ_３の座標をもたらす。

７１３Ｅ θ_１が、線７０７Ｅ Ω_２Ω_３と正のｘ軸との間の角度を表すものとする。

経路Ｌ＋Ｒ＋Ｌ−は、以下の逐次連結である。
１．左クロソイドターンＣＴ_１：７０１Ｅ

と７０３Ｅ

との間のμ−接線条件は、７１５Ｅ ｑ_１の方位がθ−α＋π／２−μであり、すなわち、ＣＴ_１が、偏差δ_１＝θ−α＋π／２−μを有することを暗示する。
２．右前方クロソイドターンＣＴ_２：７０３Ｅ

と７０２Ｅ

との間のμ−接線条件は、７１６Ｅ ｑ_２の方位がθ_１−π／２であり、それゆえ、ＣＴ_２が、偏差δ_２＝θ_１−θ＋α−μ−πを有することを暗示する。
３．偏差δ_３＝θ_ｆ−θ_１＋π／２を伴う左後方クロソイドターンＣＴ_３。

存在条件：三角形Ω_１Ω_３Ω_２は有効になるはずである。すなわち、

である。

検証条件：δ_１、δ_２及びδ_３の絶対値がπ＋２δ_Ｃ未満である。

図７Ｆは、いくつかの実施形態による、クラスＣＣ_ｕ｜Ｃ_ｕＣのパターンに従う連続曲率経路のためのコストの解析的推定を与えるコスト関数を示す概略図を示す。例えば、図７Ｆは、７００Ｆと７９９Ｆとの間のＣＣ_ｕ｜Ｃ_ｕＣ経路Ｌ＋Ｒ_ｕ＋Ｌ_ｕ−Ｒ−の条件及び式を開示する表７１０Ｆを示す。それは、中間構成７１５Ｆ ｑ_１、７１６Ｆ ｑ_２、７１７Ｆ ｑ_３及びその関連するＣＣ円の決定に頼る。

Ω_１及びΩ_２がそれぞれ、ＣＣ円７０１Ｆ

及び７０２Ｆ

の中心を表すものとし、θが線７０５Ｆ Ω_１Ω_２と正のｘ軸との間の角度７１１Ｆを表すものとする。その経路に沿った中間構成ｑ_１、ｑ_２及びｑ_３が図７Ｆに示される。ＣＣ円７０３Ｆ

及びＣＣ円７０４Ｆ

の中心をそれぞれΩ_３及びΩ_４とする。μ−接線条件は、

を暗示する。

経路の特定を促進し、結果として生じる経路の最適性を改善するために、線７０７Ｆ Ω_３Ω_４が線７０５Ｆに平行であるという幾何学的制約が課せられ、Ω_１Ω_３Ω_４Ω_２が二等辺台形になる。αが、線７０５Ｆ Ω_１Ω_２と線７０６Ｆ Ω_１Ω_３との間の角度７１２Ｆを表すものとし、幾何学的解析は、

を暗示する。

Ω_３及びΩ_４の座標はそれぞれ、

によって与えられる。

θ_１が線７０８Ｆ Ω_２Ω_４と正のｘ軸との間の角度７１３Ｆを表すものとし、その際、標準Ｌ＋Ｒ_ｕ＋Ｌ_ｕ−Ｒ−経路は以下を逐次的に結合することによって形成することができる。
１．左前方クロソイドターンＣＴ_１：７０１Ｆ

と７０３Ｆ

との間のμ−接線条件は、７１５Ｆ ｑ_１の方位がθ−α−μ＋π／２であり、それゆえ、ＣＴ_１が偏差δ_１＝θ−α−μ＋π／２を有することを暗示する。
２．右前方クロソイドターンＣＴ_２：７０３Ｆ

と７０４Ｆ

との間のμ−接線条件は、７１６Ｆ ｑ_２の方位がθ−π／２であり、それゆえ、ＣＴ_２の偏差がｕ＝α＋μ−πであることを暗示する。
３．偏差ｕ＝α＋μ−πを伴う左後方クロソイドターンＣＴ_３。
４．右後方クロソイドターンＣＴ_４：７０４Ｆ

と７０２Ｆ

との間のμ−接線条件は、７１７Ｆ ｑ_３の方位がθ_１−π／２＋μであり、それゆえ、ＣＴ_４の偏差がδ_２＝θ_ｆ−θ_１−μ＋π／２であることを暗示する。

存在条件：角度７１２Ｆ αは明確に定義され、それゆえ、

である。

検証条件：δ_１、δ_２、δ_３及びδ_４の絶対値がπ＋２δ_Ｃ未満である。

図７Ｇは、いくつかの実施形態による、クラスＣ｜Ｃ_ｕＣ_ｕ｜Ｃのパターンに従う連続曲率経路のためのコストの解析的推定を与えるコスト関数を示す概略図を示す。例えば、図７Ｇは、７００Ｇと７９９Ｇとの間のＣ｜Ｃ_ｕＣ_ｕ｜Ｃ経路、すなわち、Ｌ＋Ｒ_ｕ−Ｌ_ｕ−Ｒ＋の条件及び式を開示する表７１０Ｇを示す。その鍵は、３つの中間構成７１５Ｇ ｑ_１、７１６Ｇ ｑ_２、７１７Ｇ ｑ_３を決定することである。

Ω_１及びΩ_２がそれぞれ、７００Ｇ ｑ_０及び７９９Ｇ ｑ_ｆに関連付けられるＣＣ円７０１Ｇ

及び７０２Ｇ

の中心を表すものとする。θが、線７０５Ｇ Ω_１Ω_２と正のｘ軸との間の角度７１１Ｇを表すものとする。図７Ｇに示されるように、Ω_３及びΩ_４をそれぞれＣＣ円７０３Ｇ

及び７０４Ｇ

の中心とする。７０１Ｇ

と７０３Ｇ

との間、及び７０４Ｇ

と７０２Ｇ

との間のμ−接線条件は、

であることを暗示する。

さらに、７０３Ｇ

と７０４Ｇ

との間のμ−接線条件は、

であることを暗示する。

いくつかの実施形態は、線７０６Ｇ Ω_１Ω_３が線７０８Ｇ Ω_２Ω_４に平行であるという幾何学的制約を立証し、その制約は常に有効である。この理解は、Ω_３及びΩ_４の座標の効率的な計算を促進し、Ω_１Ω_４Ω_２Ω_３が平行四辺形になる。αが、線７０５Ｇ Ω_１Ω_２と線７０６Ｇ Ω_１Ω_３との間の角度７１２Ｇを表すものとする。三角形Ω_１ｑ_２Ω_３において余弦則を適用すると、

がもたらされる。

Ω_３及びΩ_４の座標は、

として得られる。

図７Ｇに示されるように、θ_１が、線７０８Ｇ Ω_３Ω_４と正のｘ軸との間の角度７１３Ｇを表すものとする。標準経路Ｌ＋Ｒ_ｕ−Ｌ_ｕ−Ｒ＋は、以下の逐次的連結である。
１．偏差δ_１＝θ＋α＋π／２を伴う左クロソイドターンＣＴ_１。
２．偏差ｕ＝θ_１−θ−α−μを伴う右後方クロソイドターンＣＴ_２。
３．偏差−ｕを伴う左後方クロソイドターンＣＴ_３。
４．偏差δ_２＝θ_ｆ−θ−α−π／２を伴う右前方クロソイドターンＣＴ_４。

存在条件：三角形Ω_１ｑ_２Ω_３は有効になるはずであり、すなわち、

である。

検証条件：δ_１、δ_２、δ_３及びδ_４の絶対値がπ＋２δ_Ｃ未満である。

図７Ｈは、いくつかの実施形態による、クラスＣ｜Ｃ_π／２ＳＣ−１のパターンに従う連続曲率経路のためのコストの解析的推定を与えるコスト関数を示す概略図を示す。例えば、図７Ｈは、７００Ｈ ｑ_０及び７９９Ｈ ｑ_ｆを接続するＬ＋Ｒ−Ｓ−Ｒ−経路を構成する条件及び式を開示する表７１０Ｈを示し、それは、中間構成７１５Ｈ ｑ_１、７１６Ｈ ｑ_２及び７１７Ｈ ｑ_３を決定することと同じである。この目的で、中心Ω_１を有するＣＣ円７０１Ｈ

及び中心Ω_２を有する７０２Ｈ

を使用する。

θが線７０５Ｈ Ω_１Ω_２と正のｘ軸との間の角度７１１Ｈを表すものとする。ＣＣ円７０３Ｈ

の中心をΩ_３と表す。本発明では、以下の事実、すなわち、Ω_１、Ω_２及びΩ_３が同じ直線７０５Ｈ内にあるという事実が成り立つことがわかった。この事実により、７００Ｈ ｑ_０及び７９９Ｈ ｑ_ｆを接続するＬ＋Ｒ−Ｓ−Ｒ−経路を計算する解析式を導出できるようになる。

それゆえ、Ｌ＋Ｒ−Ｓ−Ｒ−経路は直ちに計算することができる。
１．偏差δ_１＝θ＋π／２を伴う左前方クロソイドターンＣＴ_１。
２．偏差δ_２＝π／２を伴う右後方クロソイドターンＣＴ_２。
３．

によって与えられる長さを伴う線分７０４Ｈ ｑ_２ｑ_３。
４．偏差δ_３＝θ_ｆ−θ−πを伴う第２の右後方クロソイドターンＣＴ_３。

存在条件：Ｌ（ｑ_２ｑ_３）≧０、すなわち、

である。検証条件：δ_１及びδ_２の絶対値がπ＋２δ_Ｃ未満である。

図７Ｉは、いくつかの実施形態による、クラスＣ｜Ｃ_π／２ＳＣ−２のパターンに従う連続曲率経路のためのコストの解析的推定を与えるコスト関数を示す概略図を示す。例えば、図７Ｉは、一般性を失うことなく、ＣＣ経路のＣ｜ＣＳＣ−２クラスを構成する条件及び式を開示する表７１０Ｉを示し、そのクラスは、運転パターンＬ＋Ｒ−Ｓ−Ｌ−を共有する。このタスクは、ＣＣ円７０１Ｉ

及び７０２Ｉ

に基づき、その中心はそれぞれΩ_１及びΩ_２と表される。

θが線Ω_１Ω_２と正のｘ軸との間の角度を表すものとする。７１５Ｉ ｑ_１、７１６Ｉ ｑ_２及び７１７Ｉ ｑ_３がそれぞれ、第１の右後方クロソイドターン、線分７０４Ｉ及び第２の右後方クロソイドターンの開始構成を表すものとする。Ω_３をＣＣ円７０３Ｉ

の中心とし、αが線７０６Ｉ Ω_１Ω_３と線７０５Ｉ Ω_１Ω_２との間の角度７１２Ｉを表すものとする。線７０７Ｉ Ω_２Ω_０がΩ_１Ω_０に垂直になるように線７０６Ｉ Ω_１Ω_３をΩ_０まで延長する。線Ω_３Ａが線７０４Ｉ ｑ_２ｑ_３に垂直であるとし、Ｂを線７０７Ｉ Ω_２Ω_０と線７０４Ｉ ｑ_２ｑ_３との交点とする。７０１Ｉ

と７０３Ｉ

との間のμ−接線条件に基づいて、７１６Ｉ ｑ_２及び７１７Ｉ ｑ_３の両方の方位がθ＋α＋πに等しく、それは、線７０４Ｉ ｑ_２ｑ_３が線７０６Ｉ Ω_１Ω_０に平行であり、かつ線７０７Ｉ Ω_０Ω_２に垂直であることを暗示する。これは、Ω_３ＡＢΩ_０が長方形であることを示す。線７０４Ｉ ｑ_２ｑ_３と７０３Ｉ

との間、及び線７０４Ｉ ｑ_２ｑ_３と線７０２Ｉ

との間のμ−接線条件は、

であることを示唆し、それは、直角三角形Ω_０Ω_１Ω_２において、

であることを暗示する。

（２４）から、Ｌ＋Ｒ−Ｓ−Ｌ−標準ＣＣ経路は以下を含むという結果になる。
１．偏差δ_１＝θ＋α＋π／２を伴う左前方クロソイドターンＣＴ_１。
２．偏差δ_２＝π／２を伴う右後方クロソイドターンＣＴ_２。
３．長さ

の線分７０４Ｉ ｑ_２ｑ_３。
４．偏差δ_３＝θ_ｆ−θ−πを伴う第２の右後方クロソイドターンＣＴ_３。

存在条件：Ｌ（ｑ_２ｑ_３）≧０、すなわち、

検証条件：δ_１、δ_２及びδ_３の絶対値がπ＋２δ_Ｃ未満である。

図７Ｊは、いくつかの実施形態による、クラスＣ｜Ｃ_π／２ＳＣ_π／２｜Ｃのパターンに従う連続曲率経路のためのコストの解析的推定を与えるコスト関数を示す概略図を示す。例えば、図７Ｊは、特定の運転パターンＬ＋Ｒ−Ｓ−Ｌ−Ｒ＋を伴う、標準Ｃ｜ＣＳＣ｜Ｃ経路を構成する条件及び式を開示する表７１０Ｊを示す。

７０１Ｊ

及び７２０Ｊ

を７００Ｊ ｑ_０及び７９９Ｊ ｑ_ｆに関連付けられるＣＣ円とし、その中心はそれぞれΩ_１及びΩ_２である。θが線７０５Ｊ Ω_１Ω_２と正のｘ軸との間の角度７１１Ｊを表すものとする。図７Ｊに示されるように、中間構成７１５Ｊ ｑ_１、７１６Ｊ ｑ_２、７１７Ｊ ｑ_３及び７１８Ｊ ｑ_４を適切に決定するために、ＣＣ円７０３Ｊ

及び７０４Ｊ

を利用し、その中心はそれぞれΩ_３及びΩ_４である。この実施形態では、以下の必要な幾何学的特性、すなわち、線７０６Ｊ Ω_１Ω_３が線７０７Ｊ Ω_４Ω_２に平行であることが認識される。

Ω_４Ω_０が線７０６Ｊ Ω_１Ω_３に垂直であるとする。ｑ_０ｑ_１ｑ_２ｑ_３ｑ_４は７００Ｊ ｑ_０から７１８Ｊ ｑ_４までの標準Ｌ＋Ｒ−Ｓ−Ｌ−経路を形成するので、先行する推論から、Ｌ（Ω_４Ω_０）＝２Ｒ_Ωｃｏｓμという結果になる。さらに、Ω_５が、線７０８Ｊ Ω_２Ω_５がΩ_１Ω_５に垂直であるような、直線７０６Ｊ Ω_１Ω_３上の点であるとする。線７０７Ｊ Ω_４Ω_２＝２Ｒ_Ωｃｏｓμであり、それはΩ_１Ω_５に平行であり、Ω_０Ω_４Ω_２Ω_５は正方形になる。それゆえ、直角三角形Ω_１Ω_２Ω_５において補助角α７１２Ｊを求めることができる。

（２５）から直ちに、Ｌ＋Ｒ−Ｓ−Ｌ−Ｒ＋ＣＣ経路が以下によって形成されるという結果になる。
１．偏差δ_１＝θ＋α＋π／２を伴う左前方クロソイドターンＣＴ_１。
２．偏差δ_２＝π／２を伴う右後方クロソイドターンＣＴ_２。
３．長さ

の後方線分７５５Ｊ ｑ_２ｑ_３。
４．偏差δ_３＝−π／２を伴う左後方クロソイドターンＣＴ_３。
５．偏差δ_３＝θ_ｆ−θ−α−π／２を伴う右前方クロソイドターンＣＴ_４。

存在条件：Ｌ（ｑ_２ｑ_３）≧０、すなわち、

検証条件：δ_１、δ_２、δ_３及びδ_４の絶対値がπ＋２δ_Ｃ未満である。

図７Ａ〜図７Ｊと同様に、図８Ａ〜図８Ｊは、いくつかの実施形態による、所与の運転パターンの場合の経路を生成するために使用される解析関数の例を示す。

図８Ａは、いくつかの実施形態による、クラスＣＳＣ−１のパターンに従う連続曲率経路を決定するための解析解を与える解析関数を示す概略図を示す。例えば、図８Ａは、１つの実施形態による、パターン８２０Ａに従う連続曲率経路を解析的に見つけるためのパラメーターを開示する表８１０Ａを示す。

図８Ｂは、いくつかの実施形態による、クラスＣＳＣ−２のパターンに従う連続曲率経路を決定するための解析解を与える解析関数を示す概略図を示す。例えば、図８Ｂは、１つの実施形態による、パターン８２０Ｂに従う連続曲率経路を解析的に見つけるためのパラメーターを開示する表８１０Ｂを示す。

図８Ｃは、いくつかの実施形態による、クラスＣ｜Ｃ｜Ｃのパターンに従う連続曲率経路を決定するための解析解を与える解析関数を示す概略図を示す。例えば、図８Ｃは、１つの実施形態による、パターン８２０Ｃに従う連続曲率経路を解析的に見つけるためのパラメーターを開示する表８１０Ｃを示す。

図８Ｄは、いくつかの実施形態による、クラスＣ｜ＣＣのパターンに従う連続曲率経路を決定するための解析解を与える解析関数を示す概略図を示す。例えば、図８Ｄは、１つの実施形態による、パターン８２０Ｄに従う連続曲率経路を解析的に見つけるためのパラメーターを開示する表８１０Ｄを示す。

図８Ｅは、いくつかの実施形態による、クラスＣＣ｜Ｃのパターンに従う連続曲率経路を決定するための解析解を与える解析関数を示す概略図を示す。例えば、図８Ｅは、１つの実施形態による、パターン８２０Ｅに従う連続曲率経路を解析的に見つけるためのパラメーターを開示する表８１０Ｅを示す。

図８Ｆは、いくつかの実施形態による、クラスＣＣ_ｕ｜Ｃ_ｕＣのパターンに従う連続曲率経路を決定するための解析解を与える解析関数を示す概略図を示す。例えば、図８Ｆは、１つの実施形態による、パターン８２０Ｆに従う連続曲率経路を解析的に見つけるためのパラメーターを開示する表８１０Ｆを示す。

図８Ｇは、いくつかの実施形態による、クラスＣ｜Ｃ_ｕＣ_ｕ｜Ｃのパターンに従う連続曲率経路を決定するための解析解を与える解析関数を示す概略図を示す。例えば、図８Ｇは、１つの実施形態による、パターン８２０Ｇに従う連続曲率経路を解析的に見つけるためのパラメーターを開示する表８１０Ｇを示す。

図８Ｈは、いくつかの実施形態による、クラスＣ｜Ｃ_π／２ＳＣ−１のパターンに従う連続曲率経路を決定するための解析解を与える解析関数を示す概略図を示す。例えば、図８Ｈは、１つの実施形態による、パターン８２０Ｈに従う連続曲率経路を解析的に見つけるためのパラメーターを開示する表８１０Ｈを示す。

図８Ｉは、いくつかの実施形態による、クラスＣ｜Ｃ_π／２ＳＣ−２のパターンに従う連続曲率経路を決定するための解析解を与える解析関数を示す概略図を示す。例えば、図８Ｉは、１つの実施形態による、パターン８２０Ｉに従う連続曲率経路を解析的に見つけるためのパラメーターを開示する表８１０Ｉを示す。

図８Ｊは、いくつかの実施形態による、クラスＣ｜Ｃ_π／２ＳＣ_π／２｜Ｃのパターンに従う連続曲率経路を決定するための解析解を与える解析関数を示す概略図を示す。例えば、図８Ｊは、１つの実施形態による、パターン８２０Ｊに従う連続曲率経路を解析的に見つけるためのパラメーターを開示する表８１０Ｊを示す。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Claims (14)

1. 車両の運動を制御するシステムであって、
   基本経路の一組のパターンに対応する一組の解析関数と、前記一組のパターン及び前記一組の解析関数に対応する一組のコスト関数とを記憶するメモリであって、各パターンは連続経路を表し、対応するパターンに従う前記基本経路の逐次合成によって入力状態を接続する連続経路を規定する、前記車両の該入力状態に関する解析解を与えるように、該対応するパターンごとに各解析関数が決定される、メモリと、
   前記車両の初期状態及び目標状態を受信するのに応答して、前記メモリから、該初期状態を該目標状態と接続する連続曲率経路の最小コストに対応する解析関数を選択し、該選択された解析関数を用いて該連続曲率経路のパラメーターを解析的に求め、各パターンによって表される連続経路の長さを示す各パターンのコストを、前記対応するコスト関数を用いて求め、最小コストを有する前記パターンに対応する前記解析関数を選択する経路計画器と、
   前記連続曲率経路の前記パラメーターに従って前記車両の前記運動を制御するコントローラーと、
   を備え、
   前記一組のパターンは、前記入力状態の任意の値に関して、障害物を考慮することなく、前記入力状態を接続する実現可能な連続曲率経路を表す少なくとも１つのパターンが存在することを保証する網羅的な組である、
   システム。
2. 前記連続曲率経路の前記パラメーターは、前記連続曲率経路を形成する前記車両の前記運動の平面上の点の座標及び方位角のシーケンスを含む、請求項１に記載のシステム。
3. 前記連続曲率経路の前記パラメーターは制御入力のシーケンスを含み、前記制御入力のシーケンスに従う前記車両の前記運動の前記制御は、前記連続曲率経路に従う前記車両の状態に移行する、請求項１に記載のシステム。
4. 前記経路計画器は、前記初期状態及び前記目標状態を接続する一組のＲｅｅｄｓ−Ｓｈｅｐｐ（ＲＳ）経路を特定し、前記ＲＳ経路のパターンに類似する前記パターンに対応する前記コスト関数を用いて各ＲＳ経路のコストを評価し、最小コストを生成する前記コスト関数に対応する前記解析関数を選択する、請求項１に記載のシステム。
5. 前記経路計画器は、前記初期状態及び前記目標状態を接続するＲｅｅｄｓ−Ｓｈｅｐｐ（ＲＳ）経路を決定し、前記ＲＳ経路のパターンに対応する前記解析関数を選択する、請求項１に記載のシステム。
6. 前記一組のパターンは、変換されたＲｅｅｄｓ−Ｓｈｅｐｐ（ＲＳ）経路を表す４８個の異なるパターンを含み、前記組内の各パターンは、ＲＳ経路を表す対応するパターン内の円弧をクロソイドターンに置き換えることによって形成される、請求項１に記載のシステム。
7. 前記解析関数は、前記連続曲率経路を形成する隣接する基本経路の相互配置に関するμ−接線条件を満たす解を与えるように決定され、前記解析関数のうちの少なくともいくつかは、少なくとも１つの境界基本経路に対する少なくとも１つの中間基本経路の相互配置に関する付加的な幾何学的制約を満たす解を与えるように決定される、請求項６に記載のシステム。
8. パターンＣ_１Ｃ_２｜Ｃ_３Ｃ_４に関する前記付加的な幾何学的制約は、前記クロソイドターンＣ_２及びＣ_３の中心が前記クロソイドターンＣ_１及びＣ_４の中心を接続する線に平行である線上にあることを規定し、Ｃはクロソイドターンを表し、Ｃ_１、Ｃ_２、Ｃ_３、Ｃ_４は異なるクロソイドターンであり、記号「｜」は前記運動の方向を逆にすることを表し、
   パターンＣ_１｜Ｃ_２Ｃ_３｜Ｃ_４に関する前記付加的な幾何学的制約は、前記クロソイドターンＣ_１及びＣ_２の中心によって形成される線が、前記クロソイドターンＣ_３及びＣ_４の中心を接続する線に平行であることを規定し、
   パターンＣ_１｜Ｃ_２ＳＣ_３−１に関する前記付加的な幾何学的制約は、３つ全てのクロソイドターンＣ_１、Ｃ_２及びＣ_３の中心が直線を形成することを規定し、
   パターンＣ_１｜Ｃ_２ＳＣ_３｜Ｃ_４に関する前記付加的な幾何学的制約は、前記クロソイドターンＣ_１及びＣ_２の中心間の線が前記クロソイドターンＣ_３及びＣ_４の中心を接続する線に平行であることを規定する、請求項７に記載のシステム。
9. 前記車両の運転可能状態空間をサンプリングし、前記サンプリングされた運転可能状態を切り取り、前記車両の前記運動のための状態のシーケンスを生成し、前記状態のシーケンス内の隣接する状態の対ごとに前記経路計画器を呼び出し、連続経路のシーケンスを生成する運動計画器を更に備える、請求項１に記載のシステム。
10. 車両の運動を制御する方法であって、該方法は、基本経路の一組のパターンに対応する一組の解析関数及び一組のコスト関数を記憶するメモリに結合されるプロセッサを使用し、各パターンは連続経路を表し、対応するパターンに従う前記基本経路の逐次合成によって入力状態を接続する連続経路を規定する、前記車両の該入力状態に関する解析解を与えるように、該対応するパターンごとに各解析関数が決定され、各コスト関数は、前記入力状態を接続し、前記対応するパターンによって表される前記連続経路の長さを示す前記対応するパターンのコストを与えるように決定され、前記プロセッサは該方法を実施する記憶された命令に結合され、該命令は、該プロセッサによって実行されるときに、該方法の少なくともいくつかのステップを実行し、該方法は、
    前記車両の初期状態及び目標状態を受信することと、
    前記対応するパターンによって表され、前記初期状態を前記目標状態と接続する連続経路に従う前記車両の前記運動の前記コストを示す各パターンの前記コストを求めることと、
    最小コストを有する前記パターンの解析関数を選択することと、
    前記選択された解析関数を用いて、前記連続経路のパラメーターを解析的に求めることと、
    前記連続経路の前記パラメーターに従って前記車両の前記運動を制御することと、
    を含み、
    前記一組のパターンは、前記入力状態の任意の値に関して、障害物を考慮することなく、前記入力状態を接続する実現可能な連続曲率経路を表す少なくとも１つのパターンが存在することを保証する網羅的な組である、
    方法。
11. 前記一組のパターンは、変換されたＲｅｅｄｓ−Ｓｈｅｐｐ（ＲＳ）経路を表す４８個の異なるパターンを含み、前記一組内の各パターンは、ＲＳ経路を表す対応するパターン内の円弧をクロソイドターンに置き換えることによって形成される、請求項１０に記載の方法。
12. 前記解析関数は、前記連続経路を形成する隣接する基本経路の相互配置に関するμ−接線条件を満たす解を与えるように決定され、前記解析関数のうちの少なくともいくつかは、少なくとも１つの境界基本経路に対する少なくとも１つの中間基本経路の相互配置に関する付加的な幾何学的制約を満たす解を与えるように決定される、請求項１１に記載の方法。
13. パターンＣ_１Ｃ_２｜Ｃ_３Ｃ_４に関する前記付加的な幾何学的制約は、前記クロソイドターンＣ_２及びＣ_３の中心が前記クロソイドターンＣ_１及びＣ_４の中心を接続する線に平行である線上にあることを規定し、Ｃはクロソイドターンを表し、Ｃ_１、Ｃ_２、Ｃ_３、Ｃ_４は異なるクロソイドターンであり、記号「｜」は前記運動の方向を逆にすることを表し、
    パターンＣ_１｜Ｃ_２Ｃ_３｜Ｃ_４に関する前記付加的な幾何学的制約は、前記クロソイドターンＣ_１及びＣ_２の中心によって形成される線が、前記クロソイドターンＣ_３及びＣ_４の中心を接続する線に平行であることを規定し、
    パターンＣ_１｜Ｃ_２ＳＣ_３−１に関する前記付加的な幾何学的制約は、３つ全てのクロソイドターンＣ_１、Ｃ_２及びＣ_３の中心が直線を形成することを規定し、
    パターンＣ_１｜Ｃ_２ＳＣ_３｜Ｃ_４に関する前記付加的な幾何学的制約は、前記クロソイドターンＣ_１及びＣ_２の中心間の線が前記クロソイドターンＣ_３及びＣ_４の中心を接続する線に平行であることを規定する、請求項１２に記載の方法。
14. 方法を実行するためにプロセッサによって実行可能であるプログラムを具現する非一時的コンピューター可読記憶媒体であって、メモリが、基本経路の一組のパターンに対応する一組の解析関数及び一組のコスト関数を記憶し、各パターンは連続経路を表し、対応するパターンに従う前記基本経路の逐次合成によって入力状態を接続する連続経路を規定する、車両の該入力状態に関する解析解を与えるように、該対応するパターンごとに各解析関数が決定され、各コスト関数は、前記入力状態を接続し、前記対応するパターンによって表される前記連続経路の長さを示す前記対応するパターンのコストを与えるように決定され、前記一組のパターンは、前記入力状態の任意の値に関して、障害物を考慮することなく、前記入力状態を接続する実現可能な連続曲率経路を表す少なくとも１つのパターンが存在することを保証する網羅的な組であり、前記方法は、
    前記車両の初期状態及び目標状態を受信することと、
    前記対応するパターンによって表され、前記初期状態を前記目標状態と接続する連続経路に従う前記車両の運動の前記コストを示す各パターンの前記コストを求めることと、
    最小コストを有する前記パターンの解析関数を選択することと、
    前記選択された解析関数を用いて、前記連続経路のパラメーターを解析的に求めることと、
    前記連続経路の前記パラメーターに従って前記車両の前記運動を制御することと、
    を含む、非一時的コンピューター可読記憶媒体。

Images