(57)【特許請求の範囲】
【請求項１】
初期状態から目標状態までの車両の運動を制御するシステムであって、
運転パターンの逐次合成によって前記初期状態を前記目標状態と接続する不連続曲率経路を決定する経路計画器であって、前記不連続曲率経路を中心にして、前記車両が障害物と衝突しない最大距離を許容包絡線とする、経路計画器と、
前記許容包絡線内にとどまる変更済み経路を形成するために、前記不連続曲率経路内の少なくとも１つの処理可能プリミティブの位置を特定し、特定された位置の曲率が不連続な部分を曲率が連続する部分に置き換える経路変換器であって、各処理可能プリミティブは、曲率が連続するように置き換えることが可能な、基本経路の所定のパターンである、経路変換器と、
前記変更済み経路に従って前記車両の前記運動を制御するコントローラと、
を備える、システム。
【請求項２】
前記経路計画器は、不連続曲率接合部において接線方向に接続される直線部分及び円弧の組み合わせを用いて前記不連続曲率経路を形成するＲｅｅｄ−Ｓｈｅｐｐ（ＲＳ）経路計画器であり、前記システムは、
運転パターンの前記逐次合成を処理可能プリミティブの系列に変換するコンバータを更に備える、請求項１に記載のシステム。
【請求項３】
前記コンバータは、ＲＳ経路を形成する複数のＲＳ運転パターンのそれぞれを互いに独立して変換する、請求項２に記載のシステム。
【請求項４】
前記コンバータは、新たな基本経路をＲＳ運転パターンの前記逐次合成内に挿入するか、又はＲＳ運転パターンの前記逐次合成内の少なくとも１つの既存の基本経路を切り離して、前記逐次合成を変更し、ＲＳ運転パターンの前記変更済み逐次合成を処理可能プリミティブの系列に分割する、請求項２に記載のシステム。
【請求項５】
前記コンバータは、処理可能プリミティブとして、前記不連続曲率経路を形成する基本経路を構成する各タプルを、前記タプルが処理可能プリミティブの集合に属するか否かについて、運転系列全体が処理されるまで繰り返し試験し、前記処理可能プリミティブを表さないタプルに、前記不連続曲率経路の前記許容包絡線内にとどまる新たな直線部分を挿入する、請求項２に記載のシステム。
【請求項６】
前記コンバータは、前記タプルの前に、又は前記タプルの後に、前記新たな直線部分を挿入する、請求項５に記載のシステム。
【請求項７】
前記不連続曲率経路が前記処理可能プリミティブの系列であるように、前記経路計画器は、前記不連続曲率経路に関して、前記不連続曲率経路の初期線分及び最終線分のうちの少なくとも一方が直線部分であることを要求する制約を課す、請求項１に記載のシステム。
【請求項８】
前記処理可能プリミティブは第２の弧部分に接続される第１の弧部分に接続される直線部分からなる系列を含み、前記経路変換器は前記処理可能プリミティブを一対のクロソイドターンに置き換える、請求項１に記載のシステム。
【請求項９】
前記処理可能プリミティブは、直線部分に接続される第２の弧部分に接続される第１の弧部分からなる系列を含み、前記経路変換器は前記処理可能プリミティブを一対のクロソイドターンに置き換える、請求項１に記載のシステム。
【請求項１０】
前記経路変換器は、直線部分に接続される偶数個の円弧部分からなる系列を偶数個のクロソイドターンの系列に置き換える、請求項１に記載のシステム。
【請求項１１】
前記不連続曲率経路の円弧を分割し、分割した円弧の間に新たな直線部分を挿入することで、奇数個の円弧部分の系列を前記偶数個の円弧部分の系列に変換するコンバータを更に備える、請求項１０に記載のシステム。
【請求項１２】
前記経路変換器は、前記不連続曲率経路の接続部における組み合わせに応じたμ−接線条件を満たし、前記不連続曲率経路の前記許容包絡線内にとどまる前記連続曲率部分のパラメータを特定する、請求項１に記載のシステム。
【請求項１３】
初期状態から目標状態までの車両の運動を制御する方法であって、前記方法は、前記方法を実施する記憶された命令と結合されるプロセッサを使用し、前記命令は、前記プロセッサによって実行されるときに、前記方法の少なくともいくつかのステップを実行し、前記方法は、
運転パターンの逐次合成によって前記初期状態を前記目標状態と接続する不連続曲率経路を決定することであって、前記不連続曲率経路上を中心にして、前記車両が障害物と衝突しない最大距離を許容包絡線とすることと、
前記許容包絡線内にとどまる変更済み経路を形成するために、前記不連続曲率経路内の少なくとも１つの処理可能プリミティブの位置を特定し、特定された位置の曲率が不連続な部分を曲率が連続する部分に置き換えることであって、各処理可能プリミティブは、曲率が連続するように置き換えることが可能な、基本経路の所定のパターンであることと、
前記変更済み経路に従って前記車両の前記運動を制御することと、
を含む、方法。
【請求項１４】
前記不連続曲率経路を決定する経路計画器は、不連続曲率接合部において接線方向に接続される直線部分及び円弧の組み合わせを用いて前記不連続曲率経路を形成するＲｅｅｄｓ−Ｓｈｅｐｐ（ＲＳ）経路計画器であり、前記方法は、
運転パターンの前記逐次合成を処理可能プリミティブの系列に変換することを更に含む、請求項１３に記載の方法。
【請求項１５】
前記決定することは、前記不連続曲率経路が前記処理可能プリミティブの系列であるように、前記不連続曲率経路に関して、前記不連続曲率経路の初期線分及び最終線分のうちの少なくとも一方が直線部分であることを要求する制約を課す、請求項１３に記載の方法。
【請求項１６】
方法を実行するためのプロセッサによって実行可能であるプログラムを具現する非一時的コンピュータ可読記憶媒体であって、前記方法は、
運転パターンの逐次合成によって初期状態を目標状態と接続する不連続曲率経路を決定することであって、前記不連続曲率経路上を中心にして、車両が障害物と衝突しない最大距離を許容包絡線とすることと、
前記許容包絡線内にとどまる変更済み経路を形成するために、前記不連続曲率経路内の少なくとも１つの処理可能プリミティブの位置を特定し、特定された位置の曲率が不連続な部分を曲率が連続する部分に置き換えることであって、各処理可能プリミティブは、曲率が不連続な部分を曲率が連続する連続するように置き換えることが可能な、基本経路の所定のパターンであることと、
前記変更済み経路に従って車両の運動を制御することと、
を含む、非一時的コンピュータ可読記憶媒体。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は包括的には、車両の運動の経路計画に関し、より詳細には、異なる状態間で車両の運動を自動的に制御する経路計画システム及び方法に関する。
【背景技術】
【０００２】
自律車両又は自律走行モードを実行する車両のいずれかによって利用されるいくつかの制御システムは、他の車両又は歩行者等の障害物を回避するためのみではなく、車両の操作に関連付けられたいくつかの基準を最適化するためも含む双方で、未来を予測し、車両の安全運動又は経路を予測する。車両は、物体軌道の一部、より詳細には、２次元走行面上への軌道の投影である経路に沿って制御される。例えて言うと、経路は、走行中の車両の全ての幾何学的座標を接続する線である。
【０００３】
経路計画の基本的な目的は、自律運転モードにおいて動作している自律車両又は半自律車両等の走行中の車両の２つの状態、例えば、初期状態及び目標状態を接続する軌道を構成すること、並びに、軌道に沿って車両の運動を制御することである。「状態」は、本明細書において使用されるときに、平面内の車両の幾何学的座標と、これらの幾何学的座標における車両の方位とを含むベクトルであることを意図している。目標状態は、固定ロケーション、移動ロケーション、速度ベクトル、領域、又はそれらの組み合わせのいずれかとすることができる。道路縁、歩行者、及び他の車両等の周囲環境は、車両のセンサによって検知される、及び／又は、事前に与えられた情報によって少なくとも部分的に既知である。
【０００４】
初期状態及び目標状態を接続する経路は一般に、いくつかの基本経路（elementary path）（又は略して、ＥＰＴ）から構成される。状況によっては、経路計画は、同じ種類又は異なる種類の基本経路でのみ機能する。そのために、走行中の物体の経路は一般に、同じ接合点（junction points）（ＪＰと略される）を共有する複数の基本経路から構成される。したがって、各接合点は２つの隣接するＥＰＴに属するので、接合点は経路の特殊な点である。
【０００５】
経路に関して要求される１つの特殊な特徴は、状態が進路全体に沿って、又は接合点において不規則に変化しないことである。すなわち、１つの基本経路から隣接する基本経路への移行が、その一次導関数（接線）及び二次導関数（曲率）だけでなく、基本経路の接合点においても連続しているのを確実にしなければならない。車両のステアリング及び速度を線形になるように、すなわち連続するように変更することによって、１つの基本経路から隣接する基本経路への移行を実行できるときに、その経路は接合点において連続している。
【０００６】
例えば、Dubins及びReeds及びSheppによる先駆的な仕事は、車両の２つの状態間の最短経路のパターンを調査した。Ｒｅｅｄｓ−Ｓｈｅｐｐ（ＲＳ）経路と呼ばれる最短経路は、線分及び／又は最小／一定回転半径の接線円弧の逐次合成である。ＲＳ経路は、非常に効率的に計算することができるが、複数の線分を伴うＲＳ経路に沿った曲率は不連続である。車両が停止し、止まったままステアリングを実行しなければならず、結果として、無用な時間遅延及び余分なタイヤの摩耗につながるので、そのような曲率不連続性は、実用時には望ましくない。
【０００７】
クロソイドは操作される自動車の走行規則に密接に関連付けられるので、いくつかの従来の方法は、その経路計画を、クロソイドを基にすることを選ぶ。クロソイドは特殊なタイプの平面曲線であり、その曲率はｃによって表され、経路に沿って線形に変化する。クロソイドは、初期状態が終了状態と接続されるときに、経路の長さが最小化されるという更なる利点を有する。
【０００８】
例えば、非特許文献１の教示は、円弧によって接続される２つのクロソイドによって形成されるクロソイドターン（ＣＣターンと略される）の概念を導入し、ＣＣターン及び直線の間の連続した曲率移行を確実にするμ−接線条件（μ−tangency conditions）を教示している。しかしながら、連続曲率経路は通常、複数のＣＣターン及び線のシーケンスを含み、連続曲率経路を形成する最適なシーケンスのパラメータを見つけるのに時間を要する可能性がある。
【０００９】
連続曲率経路を直接計算する代わりに、別の種類の方法は、最初にＲＳ経路を計算することによって、連続曲率経路を構成する。例えば、特許文献１において開示される方法は、最初にＲＳ経路を特定し、そのＲＳ経路に可能な限り近くにある連続曲率経路を見つけようと試みる。この方法は、非線形制約下で最適化問題を解くことによって、連続曲率経路を計算する。しかしながら、この方法への解法は反復プロセスを必要とし、それはいくつかの実用的な適用例の場合に、計算量が膨大になる可能性がある。また、その方法の場合に結果として生じる最適化問題は非凸である。非凸問題への実現可能な解法は、反復プロセスによって解かれるという保証はなく、それゆえ、連続曲率経路の構成は失敗する場合がある。それゆえ、この方法は、車両のリアルタイム制御に適していない場合がある。同様に、特許文献２において記述されている方法は計算効率を改善するが、依然として、連続曲率経路を構成するために、計算コストが高い反復プロセスを伴う。それゆえ、この方法も、車両のリアルタイム制御に適していない場合がある。
【先行技術文献】
【特許文献】
【００１０】
【特許文献１】米国特許出願公開第２０１６／０３１３３７３５号
【特許文献２】米国特許第８４２８８２０号
【非特許文献】
【００１１】
【非特許文献１】T. Fraichard及びA. Scheuer「From Reeds and Shepp’s to continuous curvature paths」
【発明の概要】
【発明が解決しようとする課題】
【００１２】
したがって、車両のリアルタイム制御に適するようにして、初期状態から目標状態まで車両の運動を制御するための連続曲率経路を構成することが必要とされている。この作業は、ＲＳ経路計画と類似の計算効率及び信頼性を達成することを目指して、連続曲率経路計画を実行する。
【課題を解決するための手段】
【００１３】
いくつかの実施形態の目的は、車両の初期状態を目標状態に接続する連続曲率経路を構成する経路計画システム及び方法を提供することである。いくつかの実施形態の別の目的は、車両のリアルタイム制御に適したそのようなシステム及び方法を提供することである。本明細書において使用されるときに、連続曲率経路は、１つの基本経路から隣接する基本経路への移行が、その一次導関数及び二次導関数だけでなく、基本経路の接合点においても連続しているように接続される、複数の基本経路の逐次合成である。基本経路の例は、直線、円弧、スプライン、多項式、三角関数、クロソイド及びクロソイドターンを含む。対照的に、不連続曲率経路は、少なくとも２つの基本経路が不連続曲率接合部において接続される経路である。
【００１４】
いくつかの実施形態は、障害物回避を考慮する車両及び他のロボットのためのリアルタイム連続曲率（ＣＣ）経路計画を実行する。いくつかの実施形態は、障害物回避を考慮する種々のＣＣ経路計画結果と比較するときに、Ｒｅｅｄｓ−Ｓｈｅｐｐ（ＲＳ）ベースの経路計画が依然として計算効率及び信頼性に関して並ぶものがないという事実が動機となっている。しかしながら、ＲＳ経路は、その曲率が不連続である。そのため、いくつかの実施形態は、ＲＳ経路を後処理することによって、ＲＳ経路計画と概ね同等の計算効率及び信頼性を達成する方法を開示する。いくつかの実施態様において、後処理はμ−接線条件の幾何学的解釈の助けを借りるので、計画作業の大部分をオフラインで実行できるようになるだけでなく、収束保証アルゴリズムを開発できるようになる。
【００１５】
いくつかの実施形態は、不連続曲率経路を後処理して、ＣＣ経路を生成し、そのＣＣ経路は、ＣＣ特性を満たすだけでなく、ＣＣ経路のための衝突検出を回避できるように障害物クリアランスを保持するために、不連続曲率経路の近傍内にとどまる。ＣＣ特性を確実にするために、いくつかの実施形態は、μ−接線条件によって与えられる幾何学的洞察を利用して、不連続曲率経路を処理する。
【００１６】
具体的には、いくつかの実施形態は、ＲＳ経路のクラス及びパターンに応じて、不連続曲率経路が最大で４つのタイプの不連続曲率経路接合部（ＤＣＪ：discontinuous curvature junctions）、すなわち、ＳＣ、ＣＳ、ＣＣ、Ｃ｜Ｃを有するという認識に基づき、Ｓ、Ｃ及び｜はそれぞれ、直線部分、円弧及び速度方向の変化を表す。Ｌ及びＲは左折及び右折を指定し、＋又は−はそれぞれ、前方運動又は後方運動を表す。それゆえ、４つの全てのタイプのＤＣＪを後処理するために、オフラインで、４つの特有の方式が考案される。これらの４つのＤＣＪを解決できる限り、これらのＤＣＪの組み合わせを含む、２つの構成ｑｓとｑｆとの間の全ＲＳ経路を処理して、ｑｓとｑｆとの間のＣＣ経路を生成することができる。
【００１７】
１つの実施形態は更に、開示される方式のパラメータを特定するために、臨界条件を識別し、簡略化する。これらの臨界条件は、μ−接線条件に基づいて導出され、非線形方程式の形をとる。ＳＣタイプのＤＣＪの場合、結果として生じる非線形方程式は、簡略化し、解析的に解くことができ、それゆえ、オフラインで行われる。ＣＣ及びＣ｜Ｃタイプの場合、これらの臨界方程式は、幾何学に基づいて、オフラインで、１組の簡略化された非線形方程式に更に簡略化され、それらの方程式は、解の存在を示すことができ、解析的に解くことができる。したがって、提案される方式は、ＲＳ経路を処理するのに成功することを保証する。簡略化された非線形方程式はオンラインで解く必要があり、本発明は、それらの方程式を効率的に解くことができるアルゴリズムを教示する。
【００１８】
１つの実施形態において、ＲＳ経路計画は、初期構成ｑ０と最終構成ｑｆとの間のＲＳ経路を生成すべきであり、ＲＳ経路は、ε−クリアランスを有する（全ての方向において障害物から少なくともεだけ離れている）。別の実施形態では、ＲＳ経路計画は、
【数１】
0006896103.tif 000002
とｑｆとの間のＲＳ経路を生成し、ｃはｑ０におけるロボットの後方運動に沿ったオフセットベクトルである。結果として、ｑ０とｑｆとの間のＲＳ経路は、或る特定の長さを有する前方直線運動で開始し、それゆえ、後処理は常に、Ｓ個の線分で開始するＲＳ経路を取り扱う。それらの方式は、ＳＣ、ＣＣ、Ｃ｜ＣのＤＣＪを処理するために、後方運動を含まず、結果として生じるＣＣ経路は、より簡単な方向転換（maneuver）を有する。
【００１９】
したがって、１つの実施形態は、初期状態から目標状態までの車両の運動を制御するシステムを開示する。本システムは、運転パターンの逐次合成によって初期状態を目標状態と接続する不連続曲率経路を決定する経路計画器であって、不連続曲率経路は、不連続曲率経路を中心にした許容包絡線（tolerance envelope）内で無衝突である、経路計画器と、許容包絡線内にとどまる変更済み経路を形成するために、不連続曲率経路内の少なくとも１つの処理可能プリミティブ（treatable primitive）の位置を特定し、対応する連続曲率部分に置き換える経路変換器であって、各処理可能プリミティブは、基本経路の所定のパターンである、経路変換器と、変更済み経路に従って車両の運動を制御するコントローラとを備える。
【００２０】
別の実施形態は、初期状態から目標状態までの車両の運動を制御する方法を開示し、本方法は、本方法を実施する記憶された命令と結合されるプロセッサを使用し、命令は、プロセッサによって実行されるときに、本方法の少なくともいくつかのステップを実行する。本方法は、運転パターンの逐次合成によって初期状態を目標状態と接続する不連続曲率経路を決定することであって、不連続曲率経路は、不連続曲率経路上を中心にした許容包絡線内で無衝突であることと、許容包絡線内にとどまる変更済み経路を形成するために、不連続曲率経路内の少なくとも１つの処理可能プリミティブの位置を特定し、対応する連続曲率部分に置き換えることであって、各処理可能プリミティブは、基本経路の所定のパターンであることと、変更済み経路に従って車両の運動を制御することとを含む。
【００２１】
別の実施形態は、方法を実行するためのプロセッサによって実行可能であるプログラムを具現する非一時的コンピュータ可読記憶媒体を開示する。この方法は、運転パターンの逐次合成によって初期状態を目標状態と接続する不連続曲率経路を決定することであって、不連続曲率経路は、不連続曲率経路上を中心にした許容包絡線内で無衝突であることと、許容包絡線内にとどまる変更済み経路を形成するために、不連続曲率経路内の少なくとも１つの処理可能プリミティブの位置を特定し、対応する連続曲率部分に置き換えることであって、各処理可能プリミティブは、基本経路の所定のパターンであることと、変更済み経路に従って車両の運動を制御することとを含む。
【図面の簡単な説明】
【００２２】
【図１Ａ】いくつかの実施形態による、前方固定（front-fixed）ハンドル及び固定された平行な後輪の制御を備える車両の概略図である。
【図１Ｂ】いくつかの実施形態によって検討される連続曲率経路計画（ＣＣＰＰ）問題の一例を示す図である。
【図１Ｃ】Ｒｅｅｄ−Ｓｈｅｐｐ（ＲＳ）経路とも呼ばれる、最短経路長のクラス及び運転パターンを要約する図である。
【図１Ｄ】いくつかの実施形態によって使用されるクロソイドターンの一例を示す図である。
【図１Ｅ】曲率がクロソイドターンに沿っていかに変化するかを示すグラフである。
【図１Ｆ】いくつかの実施形態によって使用されるμ−接線条件の一例を示す図である。
【図１ＧＡ】左前方クロソイドターンと右前方クロソイドターンとの間のμ−接線条件を示す図である。
【図１ＧＢ】左前方クロソイドターンと右後方クロソイドターンとの間のμ−接線条件を示す図である。
【図２Ａ】いくつかの実施形態による、初期状態から目標状態までの車両の運動を制御するための運動計画器を含むシステムのブロック図である。
【図２Ｂ】いくつかの実施形態による、図２Ａのシステムのシステム図である。
【図３Ａ】いくつかの実施形態による、経路計画器によって決定される不連続曲率経路の構造を示す図である。
【図３Ｂ】ＲＳ経路計画器の異なる実施形態によって決定される異なるＲＳ運転パターンの例を掲載する表である。
【図４Ａ】処理可能プリミティブを形成する基本経路の或る所定のパターンの例を掲載する表である。
【図４Ｂ】異なる実施形態による、処理可能プリミティブの系列を形成するために不連続曲率経路を処理する一例を示す図である。
【図４Ｃ】異なる実施形態による、処理可能プリミティブの系列を形成するために不連続曲率経路を処理する別の例を示す図である。
【図５Ａ１】いくつかの実施形態による、異なるＲＳパターンを処理可能プリミティブの系列に変換する異なる例のうちの１つを示す図である。
【図５Ａ２】いくつかの実施形態による、異なるＲＳパターンを処理可能プリミティブの系列に変換する異なる例のうちの１つを示す図である。
【図５Ａ３】いくつかの実施形態による、異なるＲＳパターンを処理可能プリミティブの系列に変換する異なる例のうちの１つを示す図である。
【図５Ａ４】いくつかの実施形態による、異なるＲＳパターンを処理可能プリミティブの系列に変換する異なる例のうちの１つを示す図である。
【図５Ｂ】いくつかの実施形態による、異なるＲＳパターンを処理可能プリミティブの系列に変換する異なる例のうちの１つを示す図である。
【図５Ｃ】いくつかの実施形態による、異なるＲＳパターンを処理可能プリミティブの系列に変換する異なる例のうちの１つを示す図である。
【図５Ｄ】いくつかの実施形態による、異なるＲＳパターンを処理可能プリミティブの系列に変換する異なる例のうちの１つを示す図である。
【図５Ｅ】いくつかの実施形態による、異なるＲＳパターンを処理可能プリミティブの系列に変換する異なる例のうちの１つを示す図である。
【図５Ｆ】いくつかの実施形態による、異なるＲＳパターンを処理可能プリミティブの系列に変換する異なる例のうちの１つを示す図である。
【図５Ｇ】いくつかの実施形態による、異なるＲＳパターンを処理可能プリミティブの系列に変換する異なる例のうちの１つを示す図である。
【図６】１つの実施形態による、不連続曲率経路の運転パターンの系列を処理する方法のフローチャートである。
【図７Ａ】１つの実施形態によって使用される不連続曲率接合部（ＤＣＪ）処理方式の概略図である。
【図７Ｂ１】１つの実施形態による、連続曲率接合部を形成するために、不連続曲率接合部ＳＣを処理する概略図のうちの１つを示す図である。
【図７Ｂ２】１つの実施形態による、連続曲率接合部を形成するために、不連続曲率接合部ＳＣを処理する概略図のうちの１つを示す図である。
【図７Ｂ３】１つの実施形態による、連続曲率接合部を形成するために、不連続曲率接合部ＳＣを処理する概略図のうちの１つを示す図である。
【図７Ｂ４】１つの実施形態による、連続曲率接合部を形成するために、不連続曲率接合部ＳＣを処理する概略図のうちの１つを示す図である。
【図７Ｃ１】１つの実施形態による、連続曲率接合部を形成するために、不連続曲率接合部ＣＣを処理する概略図のうちの１つを示す図である。
【図７Ｃ２】１つの実施形態による、連続曲率接合部を形成するために、不連続曲率接合部ＣＣを処理する概略図のうちの１つを示す図である。
【図７Ｃ３】１つの実施形態による、連続曲率接合部を形成するために、不連続曲率接合部ＣＣを処理する概略図のうちの１つを示す図である。
【図７Ｃ４】１つの実施形態による、連続曲率接合部を形成するために、不連続曲率接合部ＣＣを処理する概略図のうちの１つを示す図である。
【図７Ｄ１】１つの実施形態による、連続曲率接合部を形成するために、不連続曲率接合部Ｃ｜Ｃを処理する概略図のうちの１つを示す図である。
【図７Ｄ２】１つの実施形態による、連続曲率接合部を形成するために、不連続曲率接合部Ｃ｜Ｃを処理する概略図のうちの１つを示す図である。
【図７Ｄ３】１つの実施形態による、連続曲率接合部を形成するために、不連続曲率接合部Ｃ｜Ｃを処理する概略図のうちの１つを示す図である。
【図７Ｄ４】１つの実施形態による、連続曲率接合部を形成するために、不連続曲率接合部Ｃ｜Ｃを処理する概略図のうちの１つを示す図である。
【図８Ａ】１つの実施形態による、自動化駐車システムの機能図である。
【図８Ｂ】１つの実施形態による、運動計画システムの全体構造を示す図である。
【図９】１つの実施形態による、システムの概略図である。
【図１０】いくつかの実施形態による、自動化駐車システムのブロック図である。
【発明を実施するための形態】
【００２３】
図１Ａは、いくつかの実施形態による、前方固定ハンドル及び固定された平行な後輪の制御を備える車両の概略図を示す。点Ｒは車両の後輪の中央に位置する。車両の姿勢は、３つ組値（ｘ，ｙ，θ）によって一意的に記述される。ただし、（ｘ，ｙ）は地球座標系内のＲの座標を表し、θは地球座標系の正のｘ軸に対する車両の方位角である。車両は、軸距ｂ及びステアリング角φを有する。
【００２４】
車両の運動学的モデルは、
【数２】
0006896103.tif 000003
によって与えられる。ただし、曲率κは（ｘ，ｙ，θ）に加えた追加の構成パラメータである。
【００２５】
車両モデル（１）の制御入力はｕ＝（ｖ，σ）である。ただし、ｖは、後輪の運転速度であり、σはステアリングレートである。φ、κ及びσの間の関係は以下のように確立される。
【数３】
0006896103.tif 000004
【００２６】
その車両について前方運動及び後方運動が許され、運転速度は制限されるのに対して、ハンドルの角度は、機械的制約を課せられる、すなわち、
【数４】
0006896103.tif 000005
であると仮定する。
【００２７】
また、ステアリングレートσも制限される、すなわち、
【数５】
0006896103.tif 000006
であると仮定される。
【００２８】
車両モデル（１）を満たす任意の経路が、その経路に沿って連続曲率κを有し、それゆえ、連続曲率（ＣＣ）経路である。いくつかの実施形態によれば、車両の構成又は状態は、ｑ＝（ｘ，ｙ，θ，κ）によって規定される。
【００２９】
図１Ｂは、いくつかの実施形態によって検討される連続曲率経路計画（ＣＣＰＰ）問題を定義する一例を示す。初期状態１０１  ｑｓ＝（ｘｓ，ｙｓ，θｓ，κｓ）、最終状態１０２  ｑｆ＝（ｘｆ，ｙｆ，θｆ，κｆ）、及び制約（２）、（３）を伴う車両モデル（１）を与えられるとき、有限時間Ｔｆに関して、
１．境界条件ＰＣＣ（０）＝ｑｓ及びＰＣＣ（Ｔｆ）＝ｑｆ
２．実現可能性∀ｔ∈［０，Ｔｆ］，ＰＣＣ（ｔ）∈Ｒ２×Ｓｌ×［−κｍａｘ，κｍａｘ］
であるような、ｑｓとｑｆとの間の実現可能経路ＰＣＣを見つける。
【００３０】
図１Ｃは、Ｒｅｅｄ−Ｓｈｅｐｐ（ＲＳ）経路とも呼ばれる、最短長経路のクラス及び運転パターンを要約する。ＲＳ経路との関連において、図１ＣのＣは円弧を表し、Ｓは線分を表す。一方、Ｌ及びＲは左折及び右折を指定し、＋又は−はそれぞれ前方運動及び後方運動を表す。下付き文字は、或る特定の円弧の（絶対）角度値を表し、｜は走行方向の変化を表す。
【００３１】
また、図１Ｂは、１０１及び１０２を接続するクラスＣＳＣ−１の運転パターンＲ＋Ｓ＋Ｒ＋における１つの例示的なＲＳ経路を含む。その例示的なＲＳ経路は、３つの部分、すなわち、円弧１０５、線１０６及び円弧１０７からなる。その曲率が点１０１、１０３、１０４及び１０２において不連続であり、それにより、静止ステアリング、時間遅延及び余分なタイヤ摩耗を被ることになるので、そのＲＳ経路は或る特定の基準によれば望ましくない。
【００３２】
厳密なＣＣＰＰを解くのは時間を要する。代わりに、いくつかの実施形態は、ＲＳ経路パターンに基づいて、準最適なＣＣ経路を求めることによってＣＣＰＰを実行する。さらに、いくつかの実施形態は、不連続曲率経路を中心とした許容包絡線と本明細書において呼ばれる、有界誤差（bounded error）内で対応するＲＳ経路パターンを「模倣する」ＣＣ経路を取得する。そのために、いくつかの実施形態は、以下のように、所与の経路Ｐのε近傍を定義する。
【数１】
0006896103.tif 000007
ただし、ε＞０は無衝突である最大距離を意味する誤差限界であり、｜Ｘ｜はベクトルＸの標準的な２−ノルムを表す。
【００３３】
したがって、準最適なＣＣＰＰは、以下の「連続曲率変換」問題と見なすことができる。
問題１  Ｃ’において初期構成ｑ０と最終構成ｑｆとを接続するＲＳ経路ＰＲＳを与えられるとき、以下であるような、連続曲率経路ＰＣＣを見つける。
（ｉ）ＰＣＣ（０）＝ｑ’０、ＰＣＣ（１）＝ｑ’ｆ、ただし、ｑ’０（それぞれｑ’ｆ）は、零曲率（null curvature）κ０＝０（それぞれκｆ＝０）を有する拡張構成空間Ｃ’内の初期構成（それぞれ最終構成）である。
（ｉｉ）ＰＣＣが或る特定の
【数７】
0006896103.tif 000008
に関するモデル（１）〜（３）を満たす。
（ｉｉｉ）ＰＣＣが、所与のε＞０に関して、ε−近傍Ｂ（ＰＲＳ，ε）内にとどまる。
【００３４】
いくつかの実施形態は、問題１が、ＲＳ経路を後処理してＣＣ経路にするために、設計自由度（design freedom）ｖを利用するという理解に基づく。その問題は良設定であり、すなわち、その解の存在が保証され、それは、以下の議論によって大まかに示すことができる。経路長ｓを独立変数として扱うことによって、システム（１）を書き換えることができる。
【数８】
0006896103.tif 000009
ＰＲＳに沿った速度プロファイルｖＣＣを定義する。ただし、ｖＣＣは不連続曲率接合部（ＤＣＪ）の周りで小さい値
【数９】
0006896103.tif 000010
をとる。これは、ＤＣＪの周りで、ｄκ／ｄｓが極めて大きく、それゆえ、Ｐｒｓの瞬時曲率変化を十分に近似できることを意味する。各ｖＣＣは、ＣＣ経路ＰＣＣを生じさせ、それはシステム（１）を積分することによって取得することができる。異なる
【数１０】
0006896103.tif 000011
を選択することによって、ＣＣ経路群
【数１１】
0006896103.tif 000012
を取得することができ、ＣＣ経路群は
【数１２】
0006896103.tif 000013
によってパラメータ化され、
【数１３】
0006896103.tif 000014
への連続的な依存性を有する。
【数１４】
0006896103.tif 000015
であるとき、
【数１５】
0006896103.tif 000016
である。それゆえ、近傍Ｂ（ＰＲＳ，ε）を与えられるとき、ＤＣＪの周りで
【数１６】
0006896103.tif 000017
である限り、ＣＣ経路
【数１７】
0006896103.tif 000018
が問題１の条件を満たすような、
【数１８】
0006896103.tif 000019
を常に見つけることができる。
【００３５】
いくつかの実施形態は、クロソイド及び／又はクロソイドターンを用いて、分離（disjunction）を解決できるという理解に基づく。クロソイドは（１）を満たす曲線とすることができ、その曲率κはその弧長ｓに対して線形に変化し、すなわち、κ（ｓ）＝σｍａｘｓである。ただし、σｍａｘはクロソイドの尖鋭度（sharpness）である。１つの実施形態において、０≦ｓ≦κｍａｘ／σｍａｘにわたって定義されるクロソイドの構成は、クロソイド弧に沿ってシステム（１）を積分することによって求めることができる。
【数１９】
0006896103.tif 000020
ただし、
【数２０】
0006896103.tif 000021
及び
【数２１】
0006896103.tif 000022
はそれぞれ、フレネル余弦積分及び正弦積分である。所与のσｍａｘの場合のクロソイドは、あらかじめ計算することができ、図１Ｄにおいてｑｓ及びｑｌを接続する弧としてプロットされる。
【００３６】
クロソイドの偏差（deflection）は、ｑｓ及びｑｌの方位の差を表す。一定のκｍａｘ及びσｍａｘの場合、クロソイドは以下の重要な特性を有する。
１．一定の偏差
【数２２】
0006896103.tif 000023
２．一定の弧長ｓ＝κｍａｘ／σｍａｘ
３．一意に求められる解ｑ（ｓ）
【００３７】
これらの特性は、計算効率を高めるために、あらかじめ計算し、メモリに記憶することができる。
【００３８】
図１Ｄは、いくつかの実施形態によって使用されるクロソイドターンの一例を示す。具体的には、図１Ｄは、事例２δｃ≦δ＜２δｃ＋πを例示する、左前方クロソイドターン１１１をプロットし、この事例は以下からなる。
１．尖鋭度σｍａｘを有する、ｑｓからｑｌまでのクロソイド。
２．ｑ１から開始し、ｑ２において終了する、半径
【数２３】
0006896103.tif 000024
及び角度δｆ−２δｃの円弧。
３．尖鋭度−σｍａｘでｑ２から開始し、ｑｆにおいて終了する第２のクロソイド。
δ＝０であるとき、クロソイドターンは長さ２ＲΩｓｉｎμの線分になる。ただし、ＲΩ及びμは後に定義される。０≦δ＜２δｃの場合、クロソイドターンは、尖鋭度σ≦σｍａｘのクロソイド、及び尖鋭度−σの対称クロソイドを含む。ただし、尖鋭度σは、
【数２４】
0006896103.tif 000025
によって与えられ、各クロソイドの長さは
【数２５】
0006896103.tif 000026
である。２δｃ＋π≦δ＜２πを有するクロソイドターンは、左後方運動に対応し、同様に取り扱うことができる。クロソイドターンはその偏差δによってパラメータ化でき、線分はその長さｌによってパラメータ化できることは明らかである。
【００３９】
図１Ｅは、図１Ｄにおいてｑｓからｑｆまでの弧１１１によって与えられるクロソイドターン等のクロソイドターンに沿って曲率がいかに変化するかを示すグラフを示し、ただし、弧は左前方クロソイドターン、すなわち、左折しながら前方に走行することを表す。その偏差δ＝θｓ−θｆに応じて、クロソイドターンは、２つまでのクロソイド及び１つの円弧を含むことができる。
【００４０】
幾何学的観点から準最適なＣＣＰＰを簡略化することに関する１つの概念はＣＣ円である。図１Ｄは、構成ｑｓに関連付けられる４つのＣＣ円を示す。詳細には、１２１
【数２６】
0006896103.tif 000027
、１２２
【数２７】
0006896103.tif 000028
、１２３
【数２８】
0006896103.tif 000029
、及び１２４
【数２９】
0006896103.tif 000030
はそれぞれ、左前方クロソイドターン１１１、左後方クロソイドターン、右前方クロソイドターン及び右後方クロソイドターンに対応する。その対応するＣＣ円によって包囲されるクロソイドターンは常に、入口点及び出口点において、接線ベクトルとμ角をなす方向において、対応するＣＣ円に入り、ＣＣ円を出ることに留意されたい。
【００４１】
特に、各ＣＣ円は、それぞれのクロソイドターンの円弧と厳密に同じ中心を有する。例えば、Ω、すなわち、ＣＣ円１２１
【数３０】
0006896103.tif 000031
の中心は、ｑ１において車両方位に対して接線方向にある円弧の中心と一致する。その座標は
【数３１】
0006896103.tif 000032
によって与えられる。
【００４２】
一定のκｍａｘ及びσｍａｘの場合、ＣＣ円半径及びμ−角は
【数３２】
0006896103.tif 000033
によって与えられる。ただし、μはｑｓにおける車両方位と、ＣＣ円１２１の接線ベクトルとの間の角度である。
【００４３】
１つの実施形態において、クロソイドターン及び線の組み合わせを決定するμ−接線条件を用いて、準最適なＣＣＰＰを実行することができる。図１Ｃの運転パターンに適合する連続曲率経路において３つの取り得る組み合わせ、すなわち、ＳＣ、ＣＣ、Ｃ｜Ｃが存在する。したがって、３つのタイプのμ−接線条件が存在する。
【００４４】
図１Ｆは、ＣＣ円１３２を一意に決定する、線１３３とクロソイドターンとの間のＳＣμ−接線条件を示す。クロソイドターンは１０１で開始し、ｑ１１３１において線１３３に移行する。クロソイドターンと線１３３との間のＳＣμ−接線条件は、ｑ１１３１におけるＣＣ円１３２の接線ベクトル１３４と線１３３との間の角度がμになることである。
【００４５】
図１ＧＡは、ＣＣ円１４１及び１４２にそれぞれ対応する、左前方クロソイドターンと右前方クロソイドターンとの間のＣＣμ−接線条件を示す。ＣＣμ−接線条件は、以下を規定する。
１．ＣＣ円１４１及び１４２は接触していなければならない。
２．ＣＣ円１４１及び１４２の中心１４３と１４４との間の距離Ｌ（Ω１Ω２）は２ＲΩに等しい。
【００４６】
図１ＧＢは、ＣＣ円１４５及び１４６にそれぞれ対応する、左前方クロソイドターンと右後方クロソイドターンとの間のＣ｜Ｃμ−接線条件を示す。Ｃ｜Ｃμ−接線条件は、以下を規定する。
１．ＣＣ円１４５及び１４６は互いに交差し、交差構成１４９において、ＣＣ円１４５及び１４６それぞれの接線ベクトル１４７及び１４８は、１４９において車両の方位１４９に対してμの角度をなす。
２．ＣＣ円１４５及び１４６の中心１５１と１５２との間の距離Ｌ（Ω１Ω２）は２ＲΩｃｏｓμになるべきである。
【００４７】
いくつかの実施形態は、障害物回避を考慮する車両及び他のロボットのためのリアルタイム連続曲率（ＣＣ）経路計画を実行する。いくつかの実施形態は、障害物回避を考慮する種々のＣＣ経路計画結果と比較するときに、Ｒｅｅｄｓ−Ｓｈｅｐｐ（ＲＳ）ベースの経路計画が依然として計算効率及び信頼性に関して並ぶものがないという事実が動機となっている。しかしながら、ＲＳ経路は、不連続曲率経路である。そのため、いくつかの実施形態は、ＲＳ経路を後処理することによって、ＲＳ経路計画と概ね同等の計算効率及び信頼性を達成する方法を開示する。いくつかの実施態様において、後処理はμ−接線条件の幾何学的解釈の助けを借りるので、計画作業の大部分をオフラインで実行できるようになるだけでなく、収束保証アルゴリズムを開発できるようになる。
【００４８】
図２Ａは、いくつかの実施形態による、初期状態から目標状態までの車両の運動を決定するための運動計画器２５０を含むシステム２００のブロック図を示す。そのシステムは、運転パターンの逐次合成によって初期状態を目標状態と接続する不連続曲率経路２１５を決定する経路計画器２１０を備える。経路計画器２１０は、ＲＳベースの経路計画によって利用される原理を用いて、不連続曲率経路を中心にした許容包絡線内で無衝突である、そのような不連続曲率経路を生成する。
【００４９】
いくつかの実施形態は、不連続曲率経路の全てのパターンではないが、いくつかのパターンを、許容包絡線内にとどまる連続曲率部分に置き換えることができるという認識に基づく。これらの実施形態は、４つのタイプの不連続曲率経路接合部（ＤＣＪ）、すなわちＳＣ、ＣＳ、ＣＣ、Ｃ｜Ｃの解析に基づき、Ｓ、Ｃ及び｜はそれぞれ、直線部分、円弧及び速度方向の変化を表す。Ｌ及びＲは左折及び右折を指定し、＋又は−はそれぞれ、前方運動又は後方運動を表す。
【００５０】
この解析に基づいて、いくつかの実施形態は、連続曲率部分を形成する対応するパターンに置き換えることができる基本経路の異なるパターンを識別し、すなわち、オフラインで決定する。すなわち、これらの所定のパターン内の経路の指定された近傍内にとどまる連続曲率経路を見つけることが保証される。連続曲率部分に置き換えることができる基本経路のそのような所定のパターンは、本明細書において、「処理可能プリミティブ」と呼ばれる。例えば、第２の弧部分に接続される第１の弧部分に接続される直線部分からなる系列は、連続曲率部分を形成する線、クロソイド及びクロソイドターンの組み合わせに置き換えることができる処理可能プリミティブを形成する。同様に、直線部分に接続される第２の弧部分に接続される第１の弧部分からなる系列も、必要に応じて、一対のクロソイドターン及び線に置き換えることができる処理可能プリミティブを形成する。
【００５１】
処理可能プリミティブのタイプは異なることができるが、ＣＣ経路のオンライン計算を迅速化するために、それらの実施形態は、処理可能プリミティブと、連続曲率部分の対応する構造とをあらかじめ決定する。例えば、直線部分に接続される偶数の円弧部分からなる系列を偶数のクロソイドターンの系列に置き換えることができる。連続曲率部分を形成するクロソイドターンのパラメータはオンラインで決定されるが、連続曲率部分の構造はあらかじめ決定されているので、いくつかの実施形態は、パラメータのオンライン計算を簡略化する。
【００５２】
そのため、システム２００は、許容包絡線内にとどまる変更済み経路を形成するために、不連続曲率経路内の少なくとも１つの処理可能プリミティブの位置を特定し、対応する連続曲率部分に置き換えるように構成される経路変換器２３０と、変更済み経路２３５に従って車両の運動を制御するコントローラ２４０とを備える。
【００５３】
１つの実施形態において、経路変換器は、不連続曲率経路２１５の一部のみを形成する処理可能プリミティブを変換する。したがって、その実施形態において、変更済み経路２３５も不連続であるが、経路２１５より、その程度が小さい不連続曲率接合部を有する。代替の実施形態では、システム２００の目的は、変更済み経路２３５が連続曲率経路であるように、処理可能プリミティブによってのみ形成される不連続曲率経路を生成することである。
【００５４】
例えば、１つの実施形態において、経路計画器２１０は、不連続曲率接合部において接線方向に接続される直線部分及び円弧の組み合わせを用いて不連続曲率経路を形成する基本Ｒｅｅｄ−Ｓｈｅｐｐ（ＲＳ）経路計画器である。この実施形態は、経路計画器２１０の実施態様を簡略化するが、不連続曲率経路２１５が処理可能プリミティブのみを含むことを保証しない。そのため、この実施形態は、不連続曲率経路２１５の運転パターンの逐次合成を処理可能プリミティブ２２５の系列によって形成される別の不連続曲率経路に変換するコンバータ２２０も含む。例えば、コンバータ２２０は、経路２１５内に新たな直線部分を挿入し、経路２２５を形成することができる。そのようにして、経路変換器２３０が経路２２５を変換した後に、変更済み経路２３５は連続曲率経路である。コンバータ２２０の動作の例が後に与えられる。
【００５５】
それに加えて、又はその代わりに、経路計画器２１０は、処理可能プリミティブの系列のみによって既に形成されているそのような不連続曲率経路２１５を形成する変更済みＲＳ経路計画器とすることができる。例えば、１つの実施形態において、経路計画器は、不連続曲率経路に関して、不連続曲率経路の初期線分及び最終線分のうちの少なくとも一方が直線部分であることを要求する制約を課す。
【００５６】
図２Ｂは、いくつかの実施形態による、図２Ａのシステム２００のシステム図を示す。システム２００は、有限数の処理可能プリミティブ２１１を記憶するメモリ２０１と、不連続曲率接合部（ＤＣＪ）処理方式２１２と、運転パターン処理方式２１３と、ＲＳ経路処理方式２１４とを含むことができる。これらの全ての方式が、メモリ内に符号の形で存在する。プロセッサ２０２が、車両の初期構成及び最終構成を示す入力状態２２１に基づいて、運動計画２５０を実施し、ＲＳ経路２２２を決定し、ＣＣ経路２２３を決定する。アクチュエータ２０３、例えば、モーターが、ＣＣ経路に従って車両の運動を制御する。
【００５７】
いくつかの実施形態は、不連続曲率経路を後処理して、ＣＣ経路を生成し、そのＣＣ経路は、ＣＣ特性を満たすだけでなく、ＣＣ経路のための衝突検出を回避できるように障害物クリアランスを保持するために、不連続曲率経路の近傍内にとどまる。ＣＣ特性を確実にするために。いくつかの実施形態は、μ−接線条件によって与えられる幾何学的洞察を利用して、不連続曲率経路を処理する。そのため、いくつかの実施形態において、経路変換器２３０は、μ−接線条件を満たす、連続曲率部分のパラメータを特定する。
【００５８】
図３Ａは、いくつかの実施形態による、経路計画器によって決定される不連続曲率経路２１５の構造を示す。それらの実施形態において、不連続曲率経路は、複数のＲＳ運転パターン３１１、３１２のうちの１つ又はその系列によって形成されるＲＳ経路３１０である。ＲＳ運転パターン３１１及び３１２の例は、図１Ｃに示されるパターン１９９を含む。図１Ｃにおいて見ることができるように、各ＲＳ運転パターンは、基本経路３２１及び３２２の系列を含む。経路計画器は、有限集合｛Ｃ，Ｓ｝から基本経路を選択し、Ｃ及びＳはそれぞれ円弧又は線分を表す。
【００５９】
図３Ｂは、異なる実施形態によって決定される異なるＲＳ運転パターンの例を掲載する表３３０を示す。例えば、１つの実施形態は、ＲＳ経路の基本パターン３３２を決定する。パターン３３２は、これらのパターンによって形成されるＲＳ経路が処理可能プリミティブのみを含むことを保証しない。代替的には、いくつかの実施形態は、直線部分Ｓによって形成される余分な基本経路を意図的に挿入することによって、パターン３３４、３３６及び３３８を生成する。例えば、１つの実施形態は、運転パターン３３２の前に直線部分Ｓを挿入して、パターン３３４を形成する。別の実施形態は、運転パターン３３２の最後に直線部分Ｓを挿入して、パターン３３６を形成する。更に別の実施形態は、運転パターン３３２の最後及び最初に直線部分Ｓを挿入して、パターン３３８を形成する。Ｓの挿入を伴う変更済みＲＳ経路計画器は、結果として生じるＲＳ経路の後処理を簡略化することができ、方向転換の少ない連続曲率経路を生成することができる。
【００６０】
図４Ａは、処理可能プリミティブ４１０を形成する基本経路の或る所定のパターンの例を掲載する表を示す。この例は処理可能プリミティブの集合４１０｛Ｓ，ＳＣＳ，ＳＣＣ，ＣＣＳ，ＳＣ｜Ｃ，Ｃ｜ＣＳ｝を開示し、それは、いくつかの実施形態によって使用される処理方式の１つのステップによって処理することができる。この図４Ａにおいて見ることができるように、処理可能プリミティブ４１０のパターンは、直線部分Ｓで開始し、及び／又は終了する。そのため、パターン３３４、３３６及び３３８は、経路計画器によって生成される不連続曲率経路において処理可能プリミティブの位置を特定するのを簡単にする。
【００６１】
図４Ｂ及び図４Ｃは、異なる実施形態による、不連続曲率経路内の処理可能プリミティブの位置を特定し、処理する例を示す。例えば、いくつかの実施形態は、不連続曲率経路を形成するパターンの系列４２０内の処理可能プリミティブの有限集合の位置を特定することによって、ＣＣ経路を生成し、それは、単一のステップによってＣＣ運転パターンに変換することができる最短ＲＳ運転パターンを表す。
【００６２】
例えば、図４Ｂの実施形態は、ＲＳ経路の各ＲＳ運転パターン内の処理可能プリミティブを互いに独立して識別する（４３０）。逆に、図４Ｃの実施形態は、処理可能プリミティブの位置を特定するために、ＲＳ経路全体をまとめて処理することができる。これにより、処理可能プリミティブの選択を簡単にするために、ＲＳ経路を変更できるようになる。例えば、この実施形態は、パターンの系列に基本経路を挿入するか、その基本経路を分割するか、又はその系列から基本経路を除去することによって、ＲＳ経路を変更することができる（４４０）。また、この実施形態は、ＲＳ運転パターンの変更済み経路をパターンの新たな系列に分割することができ（４５０）、各新たなパターンは処理可能プリミティブであり、また、この実施形態は、新たな系列を処理して、連続曲率経路を生成することができる（４６０）。
【００６３】
図５Ａ〜図５Ｇは、いくつかの実施形態による、異なるＲＳパターンを処理可能プリミティブの系列に変換する異なる例を示す。
【００６４】
図５Ａ１は、ＲＳ運転パターンＣＳＣ５０１ａのための処理方式を示し、それは最初に、Ｓを挿入し、及び／又は付加することによって、運転パターン５０２ａに拡張される。Ｓａというワードは、或る特定の長さａを有する線分を表す。５０２ａを与えられると、２つのＣ間のＳは２つのＳａ／２に分割される（５０３ａ）。処理後の運転パターンＳａ｜ＳａＣＳａ／２Ｓａ／２ＣＳａ｜Ｓａは、２つの処理可能プリミティブＳ、並びに別の２つの処理可能プリミティブＳａＣＳａ／２５０４ａ及びＳａ／２ＣＳａ５０５ａからなることが明らかである。
【００６５】
図５Ａ２は、ＲＳ運転パターンＳＣＳＣ５１１ａのための処理方式を提示し、それは、運転ワードＳを最後に付加することによって、運転パターン５１２ａに最初に拡張される。運転パターン５１２ａは、５１３ａを与えるために、第２のＳを２つのＳａ／２に分割することによって更に処理される。変更済み運転パターン５１３ａは、処理可能プリミティブＳＣＳ５１４ａ、ＳＣＳ５１５ａ及びＳからなることが明らかである。
【００６６】
図５Ａ３は、ＲＳ運転パターンＣＳＣＳ５２１ａのための処理方式を教示し、それは、運転ワードＳを最初に付加することによって、運転パターン５２２ａに最初に拡張される。運転パターン５２２ａは、５２３ａを与えるために、第３のＳを２つのＳａ／２に分割することによって更に処理される。変更済み運転パターン５２３ａは、処理可能プリミティブＳＣＳ５２４ａ、ＳＣＳ５２５ａ及びＳを含むことが明らかである。
【００６７】
図５Ａ４は、ＲＳ運転パターンＳＣＳＣＳ５３１ａのための処理方式を表し、それは、５３２ａを与えるために、第２のＳを２つのＳａ／２に分割することによって処理される。変更済み運転パターン５３２ａは、処理可能プリミティブＳＣＳ５３２ａ及びＳＣＳ５３４ａからなることが明らかである。
【００６８】
図５Ｂは、ＲＳ運転パターンＳＣ｜Ｃ｜ＣＳ５０１ｂのための処理方式を開示し、それは、５０２ｂを与えるために、第２のＣを２つのＣａ／２に分割することによって処理される。変更済み運転パターン５０２ｂは、処理可能プリミティブＳＣ｜Ｃ５０３ｂ及びＣ｜ＣＳ５０４ｂからなることが明らかである。
【００６９】
図５Ｃは、ＲＳ運転パターンＳＣ｜ＣＣ５０１ｃのための処理方式を開示し、それは、５０２ｃを与えるために、最初の運転ワードＣの後にＳを挿入することによって拡張される。変更済み運転パターン５０２ｃは、処理可能プリミティブＳＣＣ５０３ｃ及びＳＣＣＳ５０４ｃからなることが明らかである。
【００７０】
図５Ｄは、ＲＳ運転パターンＣＣ｜ＣＣ５０１ｄのための処理方式を開示し、それは、変更済み運転パターン５０２ｄを与えるために、第２のＣと第３のＣとの間のＳを挿入することによって処理される。変更済み運転パターン５０２ｄは、処理可能プリミティブＣＣＳ５０３ｄ及びＳＣＣ５０４ｄからなることが明らかである。
【００７１】
図５Ｅは、ＲＳ運転パターンＳＣ｜ＣＣ｜ＣＳ５０１ｅのための処理方式を開示し、それは、処理可能プリミティブＳＣ｜Ｃ５０２ｅ及びＣ｜ＣＳ５０３ｅからなる。
【００７２】
図５Ｆは、ＲＳ運転パターンＣ｜ＣＳＣＳ５０１ｆのための処理方式を開示し、それは、５０２ｆを与えるために、第１のＳを２つのＳａ／２に分割することによって処理される。変更済み運転パターン５０２ｆは、処理可能プリミティブＣ｜ＣＳ５０３ｆ及びＳＣＳ５０４ｆからなることが明らかである。
【００７３】
図５Ｇは、ＲＳ運転パターンＣ｜ＣＳＣ｜Ｃ５０１ｇのための処理方式を開示し、それは、５０２ｇを与えるために、第１のＳを２つのＳａ／２に分割することによって処理される。変更済み運転パターン５０２ｇは、処理可能プリミティブＳＣ｜Ｃ５０４ｇ及びＣ｜ＣＳ５０３ｇからなることが明らかである。
【００７４】
図６は、１つの実施形態による、不連続曲率経路を形成する運転パターンの系列を処理するための方法６０１のフローチャートを示す。この実施形態において、コンバータ２２０が、処理可能プリミティブとして、不連続曲率経路を形成する基本経路の各タプルを繰り返し試験し、処理可能プリミティブを表さないタプルに新たな直線部分を挿入する。例えば、運転系列は、最初に常に未処理であり、構文解析され（６０２）、未処理の運転系列内の運転ワードの第１のタプルが取り出され、それが処理可能プリミティブの集合に属するか否かを試験される（６０３）。属する場合には、運転系列の残りが新たな未処理の運転系列になり、そうでない場合には、取り出されたタプルが、タプルの前に、又はタプルの第２の運転ワードの後にＳを追加し、処理可能プリミティブを生成することによって処理され、新たな未処理の運転系列は、タプルの最後のワードと、後続の古い未処理の運転系列とを含む。運転系列全体が処理された場合には、処理プロセスは中止する。そうでない場合には、上記の取り出すステップ６０２、試験するステップ６０３、追加するステップ６０４及び検証するステップ６０５が繰り返される。
【００７５】
図７Ａは、１つの実施形態によって使用される不連続曲率接合部（ＤＣＪ）処理方式の概略図を示す。これらの方式は、処理可能プリミティブの識別につながる。図１ＣのＲＳ運転パターンのクラスは、ＤＣＪが直線部分及び円弧の間の接続部において生じることを明らかにする。具体的には、ＤＣＪは包括的には、ＳＣパターン、ＣＳパターン、ＣＣパターン及びＣ｜Ｃパターンに分類することができる。したがって、ＤＣＪ処理方式は、ＳＣ７０３ａ、Ｃ｜Ｃ７０４ａ及びＣＣ７０５ａの処理を含む。
【００７６】
いくつかの実施形態において、一対の一定のκｍａｘ及びσｍａｘに関してシステム動態（１）を満たすクロソイドが、あらかじめ計算され、メモリに記憶され（７０１ａ）、ＤＣＪを処理するためにオンラインで再利用することができる。同様に、クロソイド円の中心、及びｑｓにおいて開始するクロソイドに対応するクロソイドターン７０２ａの半径もオフラインで計算し、オンラインで使用するためにメモリに記憶することができる。
【００７７】
図７Ｂ１、図７Ｂ２、図７Ｂ３及び図７Ｂ４は、１つの実施形態による、連続曲率接合部を形成するために、不連続曲率接合部ＳＣを処理する概略図を示す。不連続曲率接合部ＳＣは、線と円弧との間の移行に対応する。適切な座標系（coordinate framework）を採用することによって、これは、以下の問題を解くことに対応する。
【００７８】
問題２  初期構成ｑ０＝（ｘ０，０，０，０）７０１ｂ及び最終構成
【数３３】
0006896103.tif 000034
７９９ｂを接続する簡単なＲＳ運転パターン
【数３４】
0006896103.tif 000035
が［（ｉ）］からなるとする。
１．−∞＜ｘ０≦ｘ≦０及びｙ＝０によって記述される線分Ｓ７１１ｂ
２．原点（０，０）７１３ｂにおいてＳに接する円弧Ｃ７１２ｂ
Ｃの半径及び角度はそれぞれ
【数３５】
0006896103.tif 000036
及びθによって与えられる。問題１の条件（ｉ）及び（ｉｉ）を満たしながら、ｑ０とｑｆとを接続し、
【数３６】
0006896103.tif 000037
内にとどまる連続曲率経路ＰＣＣを構成する。
【００７９】
簡単なＳＣ  ＲＳ運転パターンが図７Ｂ１に示される。θ＞２δｃであるようなσｍａｘの値を与えられるとき、以下のＳＣ移行方式は、ＳとＣとの間でＣＣ接続を行うことを保証する（詳細に関しては図７Ｂ−１及び図７Ｂ−２を参照）。
１．初期構成ｑ０＝（ｘ０，０，０，０）７０１ｂから開始し、構成ｑｓ＝（ｘ＊，０，０，０）７０２ｂにおいて終了する前方直線部分。ただし、ｘ０≦ｘ＊≦０。
２．構成ｑｓ＝（ｘ＊，０，０，０）７０２ｂから開始し、ｑ１＝（ｘ１，ｙ１，δ，０）７０３ｂにおいて終了する前方右クロソイドターン（ＣＴ）。ただし、偏差δ＜０。
３．偏差が−δであり、ｑ１７０３ｂから開始し、構成ｑ２＝（ｘ２，ｙ２，０，０）７０４ｂにおいて終了する前方左ＣＴ。
４．ｑ２７０４ｂと、点（ｘ３，ｙ３）∈Ｃであるような、ｑ３＝（ｘ３，ｙ３，δｃ，κｍａｘ）７０５ｂとを接続する前方左クロソイド。
【００８０】
ＳＣ処理方式におけるパラメータ、すなわち、ｑｓ、ｑ１、ｑ２を決定するために、以下の手順に従うことができる。
１．図７Ｂ−２によって表されるように、ＣＣ円
【数３７】
0006896103.tif 000038
７１７ｂ及び円弧Ｃ７１２ｂはいずれも同じ中心を共有する。それゆえ、
【数３８】
0006896103.tif 000039
である。その後、ｑ２の座標が
【数３９】
0006896103.tif 000040
によって計算される。
２．構成ｑ２が前方左ＣＴの終了構成であると考えるとき、ＣＣ円
【数４０】
0006896103.tif 000041
の中心Ω２を直ちに特定することができる。
３．或る特定のｘ＊に関して、ＣＣ円
【数４１】
0006896103.tif 000042
は中心Ω１（ｘ＊）を有し、それはｘ＊によって一意に規定される。一方、ｑｓから開始する第１の右ＣＴに対応するＣＣ円
【数４２】
0006896103.tif 000043
は、
【数４３】
0006896103.tif 000044
に接するべきである。それゆえ、ｘ＊は、以下の非線形方程式を解くことによって特定することができる。
【数４４】
0006896103.tif 000045
４．ｘ＊、
【数４５】
0006896103.tif 000046
及び
【数４６】
0006896103.tif 000047
を用いて、以下のように、補助角αを計算する。
【数４７】
0006896103.tif 000048
【００８１】
２つのＣＴの偏差が、それに応じて、δ及び−δによって与えられる。ただし、δ＝α−π／２＋μである。直線部分の長さは、ｘ＊−ｘ０によって与えられる。
【００８２】
ＣＣ円７１５ｂ、７１６ｂ、７１７ｂの間の関係が図７Ｂ２に示される。あらかじめ確立された計算手順を、図７Ｂ３に示されるＣＣＰ−ＳＣ法として要約することができる。
【００８３】
ＣＣＰ−ＳＣ法は、入力として、誤差限界εとともに、初期構成ｑ０及び最終構成ｑｆを取り込む。初期設計パラメータ
【数４８】
0006896103.tif 000049
は、θ＞２δｃが満たされるようにして選択される。最初に、ＣＣＰ−ＳＣアルゴリズムは、ＰＣＣ⊆Ｂ（ＰＲＳ，ε）を保証するために、｜ｙ２｜≦εであるような、適切なσｍａｘを計算する（ライン２〜９）。σｍａｘを取得した後に、ＣＣＰ−ＳＣアルゴリズムは、対応するｘ＊を計算する（ライン１０〜１４）。計算手順において説明されるように、ｘ＊は、式（８）及び経路制約ｘ０＜ｘ＊＜０の両方を満たすべきである。いくつかの実施態様は、簡単な幾何学的解析に基づいて、（８）をライン１３の式に変換する。その式を解くことによって、適切なｘ＊が取得されると、反復ループが終了し、ＣＣＰ−ＳＣアルゴリズムは、それに応じて、ＣＣ経路ＰＣＣを計算し、出力する（ライン１５〜１６）。そうでない場合には、その方法は、σｍａｘの値を増加させて、ｘ＊の値を更新する。
【００８４】
図７Ｂ４は、具体例に関して提案されるＳＣ処理方式の解をプロットする。
【００８５】
図７Ｃ１、図７Ｃ２、図７Ｃ３及び図７Ｃ４は、１つの実施形態による、連続曲率接合部を形成するために、不連続曲率接合部ＣＣを処理する概略図を示す。不連続曲率接合部ＣＣは、２つの円弧の間の接合によって形成される。適切な座標系を採用することによって、これは、以下の問題を解くことに対応する。
【００８６】
問題３
【数４９】
0006896103.tif 000050
を初期構成ｑ０＝（０，０，０）７０１ｃから開始し、ｑｆ７９９ｃにおいて終了するＣＣタイプのＲＳ経路とする。
【数５０】
0006896103.tif 000051
は以下のものからなる。
１．原点から開始し、Ａ７１３ｃにおいて終了する円弧Ｃ１７１１ｃ。Ｃ１の半径及び角度はそれぞれ
【数５１】
0006896103.tif 000052
及びθ１によって与えられる。
２．最終構成ｑｆ＝（ｘｆ，ｙｆ，θｆ）７９９ｃにおいて終了し、ＡにおいてＣ１と接し、Ｃ１と同じ方向に進み、
【数５２】
0006896103.tif 000053
及びθ２によってそれぞれ与えられる半径及び角度を有する、円弧Ｃ２７１２ｃ。
以下の要件、すなわち、（ｉ）ＰＣＣが零曲率拡張構成ｑ０’＝（０，０，０，０）及びｑｆ’＝（ｘｆ，ｙｆ，θｆ，０）を接続する、（ｉｉ）
【数５３】
0006896103.tif 000054
（ｉｉｉ）ＰＣＣが問題１の条件（ｉ）及び（ｉｉ）を満たす、を達成するように、連続曲率経路パターンＰＣＣを構成する。
【００８７】
例示のために、本発明は、θ１∈［０，π］（反時計回り）及びθ２∈［０，π］（時計回り）であると仮定する。このシナリオが図７Ｃ１に示される。Ｃ１及びＣ２の中心が
【数５４】
0006896103.tif 000055
として取得される。
【００８８】
Ｏ１及びＯ２の座標に基づいて、弧Ｃ２の最終構成ｑｆ（それゆえ、
【数５５】
0006896103.tif 000056
）が、その際、
【数５６】
0006896103.tif 000057
として取得される。
【００８９】
その実施形態は、ｑ’０及びｑ’ｆを接続する所望のＰＣＣの構成に進む。問題３を解くことは、構成ｑ０＝（０，０，０，０）とｑｆ（９）とを接続する連続曲率経路を計算することと同様である。そのため、問題３を解くために、連続曲率経路パターンＳ｜ＣＣを提案する。ただし、Ｓ及びＣはそれぞれ、線分及びＣＴを表す。図７Ｃ２に示されるように、提案されるＣＣ経路は以下のものから構成される。
１．Ｓ：ｑ０７０１ｃから開始し、構成ｑ１＝（ｘ＊，０，０，０）７０２ｃにおいて終了する後方線分。ただし、ε≦ｘ＊＜０。
２．ＣＴ１：ｑ１７０２ｃから開始し、構成ｑ２＝（ｘ１，ｙ１，δ１，０）７０３ｃにおいて終了する、偏差δ１を伴う前方左ＣＴ。
３．ＣＴ２：ｑ２７０３ｃにおいて開始し、ｑｆ７９９ｃにおいて終了する、偏差δ２＝θ１−θ２−δ１を伴う前方右ＣＴ。
適切なσｍａｘを与えられるとき、以下の方法を利用して、移行パラメータｘ＊、ｘ１、ｙ１、δ１及びδ２を計算することができる。
１．ＣＴ２が（９）によって与えられる、ｑｆにおいて終了する場合に、ＣＣ円
【数５７】
0006896103.tif 000058
７１６ｃの中心
【数５８】
0006896103.tif 000059
を有する。
２．ＣＴ１はｑ１７０２ｃから開始するので、ＣＣ円
【数５９】
0006896103.tif 000060
７１５ｃの中心Ω１（ｘ＊）がｘ＊のベクトル値関数である。
３．ＣＣ円
【数６０】
0006896103.tif 000061
７１５ｃ及び
【数６１】
0006896103.tif 000062
７１６ｃは互いに接しなければならない。そのために、ｘ＊は、
【数６２】
0006896103.tif 000063
を満たすべきである。ただし、ＲΩは（６）によって与えられる。
４．（１０）から解かれたｘ＊を与えられるとき、
【数６３】
0006896103.tif 000064
を特定することができる。補助角αを以下のように計算する。
【数６４】
0006896103.tif 000065
【００９０】
ＣＴ１及びＣＴ２の偏差はそれぞれ、δ１＝α＋π／２−μ及びδ２＝θ１−θ２−δ１によって与えられ、一方、線分の長さは｜ｘ＊｜として取得される。
【００９１】
同様に、１つの実施形態は、ＣＣ｜Ｓ経路パターンを用いて、２つのＣＴと、直線部分内の後方運動とを含むＣＣを処理することもできる。上記のＳ｜ＣＣ経路パターンに対するその対称性に起因して、ＣＣ｜Ｓも、同じようにして効率的に計算することができる。
【００９２】
ｑ’０とｑ’ｆとを接続するＳ｜ＣＣ経路の計算手順が、図７Ｃ３においてＣＣＰ−ＣＣ法として要約される。
【００９３】
ＣＣＰ−ＳＣ法と同様に、ＣＣＰ−ＣＣ法は、入力として、ｑ０、ｑｆ及びεを取り込む。直観的には、ＣＣＰ−ＣＣアルゴリズムは、（１０）を満たすことができるように、繰り返し適切なｘ＊を計算する。そのような目的のために、第ｋの反復ステップにおいて、ＣＣＰ−ＣＣは、
【数６５】
0006896103.tif 000066
に関して、ＲΩ及びμを計算する（ライン３及び４）。次に、
【数６６】
0006896103.tif 000067
の中心Ω２が取得される（ライン５）。一方、簡単な幾何学的推論から、
【数６７】
0006896103.tif 000068
の中心Ω１は、ｘ＊及び
【数６８】
0006896103.tif 000069
の中心Ω０から取得することができる（ライン６及び７）。Ω１の座標に基づいて、（１０）は、その後、ＣＣＰ−ＣＣアルゴリズムのライン８において提示される式に変換することができる。この式を解くことにより、ｘ＊の値がもたらされる。｜ｘ＊｜＜εであるときにはいつでも、ｘ＊を受け入れることができる（ライン９〜１２）。その後、それに応じて、ｘ＊に関して、所望のＰＣＣを計算することができる。
【００９４】
図７Ｃ４は、具体例に関して提案されるＣＣ処理方式の解をプロットする。
【００９５】
図７Ｄ１、図７Ｄ２、図７Ｄ３及び図７Ｄ４は、１つの実施形態による、連続曲率接合部を形成するために、不連続曲率接合部Ｃ｜Ｃを処理する概略図を示す。不連続曲率接合部Ｃ／Ｃは、逆の運動の場合に２つの円弧の間の接合によって形成される。適切な座標系を採用することによって、これは、以下の問題を解くことに対応する。
【００９６】
問題４
【数６９】
0006896103.tif 000070
を初期構成ｑ０＝（０，０，０）７０１ｄから開始するＲＳ経路とし、それは以下のものからなる。
１．ｑ０から開始する円弧Ｃ１７１１ｄ。Ｃ１の半径及び角度はそれぞれ
【数７０】
0006896103.tif 000071
及びθ１によって与えられる。
２．点Ａ７１３ｄにおいてＣ１と接する円弧Ｃ２７１２ｄ。
車両は、最終構成ｑｆ＝（ｘｆ，ｙｆ，θｆ）７９９ｄまで、Ｃ１内と反対方向に動く。Ｃ２の半径及び角度はそれぞれ
【数７１】
0006896103.tif 000072
及びθ２によって与えられる。
（ｉ）ＰＣＣが零曲率拡張構成ｑ０’＝（０，０，０，０）及びｑｆ’＝（ｘｆ，ｙｆ，θｆ，０）を接続する。（ｉｉ）
【数７２】
0006896103.tif 000073
（ｉｉｉ）ＰＣＣが問題１の条件（ｉ）及び（ｉｉ）を満たすように、連続曲率経路パターンＰＣＣを構成する。
【００９７】
明確にするために、この実施形態は、一般性を失うことなく、θ１∈［０，π］（反時計回り）及びθ２∈［０，π］（反時計回り）であると仮定する。それぞれＯ１及びＯ２として表される、円弧Ｃ１７１１ｄ及び円弧Ｃ２７１２ｄの中心は、ＣＣ  ＤＣＪの状況と全く同じように計算される。
【数７３】
0006896103.tif 000074
【００９８】
図７Ｄ１に示されるように、ＲＳ経路
【数７４】
0006896103.tif 000075
は構成
【数７５】
0006896103.tif 000076
において終了する。
【００９９】
１つの実施形態は、図７Ｄ２に示されるＣＣ処理方式を開示し、その処理方式は、（１１）においてｑ’０＝（ｑ０，０）及びｑ’ｆ＝（ｑｆ，０）を接続するために、パターンＳ｜Ｃ｜ＣのＣＣ経路を構成する。詳細には、提案されるＣＣ経路は以下のものを含む。
１．Ｓ：ｑ０７０１ｄにおいて開始し、構成ｑ１＝（ｘ＊，０，０，０）７０２ｄにおいて終了する後方線分。ただし、−ε≦ｘ＊＜０。
２．ＣＴ１：ｑ１７０２ｄから開始し、構成ｑ２＝（ｘ１，ｙ１，δ１，０）７０３ｄにおいて終了する、偏差δ１を伴う前方左ＣＴ。
３．ＣＴ２：ｑ２７０３ｄから開始し、ｑｆ７９９ｄにおいて終了する、偏差δ２＝θ１＋θ２−δ１を伴う後方右ＣＴ。
一定のσｍａｘを与えられるとき、以下の手順を利用して、移行パラメータｘ＊、ｘ１、ｙ１、δ１及びδ２を特定することができる。例示のために図７Ｄ２を参照されたい。
１．ＣＴ２が（１１）によって与えられるｑｆにおいて終了する場合に、
【数７６】
0006896103.tif 000077
として、
【数７７】
0006896103.tif 000078
７１６ｄの中心の座標を有する。
２．ＣＴ１がｑ１において開始する場合、ＣＣ円
【数７８】
0006896103.tif 000079
７１５ｄの中心Ω１（ｘ＊）はｘ＊のベクトル値関数によって与えられる。
３．μ−接線条件に基づいて、ＣＣ円
【数７９】
0006896103.tif 000080
７１５ｄ及び
【数８０】
0006896103.tif 000081
７１６ｄはμ−交差しなければならない。すなわち、ｘ＊は、
【数８１】
0006896103.tif 000082
を確実にすべきである。
４．（１２）から解かれたｘ＊を与えられるとき、
【数８２】
0006896103.tif 000083
を特定することができる。補助角αを以下のように計算する。
【数８３】
0006896103.tif 000084
【０１００】
ＣＴ１及びＣＴ２の偏差はそれぞれ、δ１＝α＋π／２及びδ２＝θ１＋θ２−δ１によって与えられ、一方、線分の長さは｜ｘ＊｜によって与えられる。
【０１０１】
ＣＣ  ＤＣＪを扱うＣＣ処理方式と同様に、Ｃ｜Ｃ  ＤＣＪを処理するために、あらかじめ確立されたＳ｜Ｃ｜Ｃ経路の代替経路として、連続曲率Ｃ｜Ｃ｜Ｓ経路パターンを利用することもできる。
【０１０２】
Ｓ｜Ｃ｜Ｃの上記の計算手順を、図７Ｄ３のＣＣＰ−Ｃ｜Ｃ法において要約することができる。
【０１０３】
ＣＣＰ−Ｃ｜Ｃアルゴリズムの入力は、ｑ０、ｑｆ及びεを含む。直観的には、ＣＣＰ−Ｃ｜Ｃアルゴリズムは、（１２）を解くように、ｘ＊を繰り返し求める。そのために、ＣＣＰ−Ｃ｜Ｃは最初に、第ｋの反復ステップにおいて、
【数８４】
0006896103.tif 000085
に関してＲΩ及びμを計算する（ライン３及び４）。次に、
【数８５】
0006896103.tif 000086
の中心Ω２が取得される（ライン５）。さらに、
【数８６】
0006896103.tif 000087
の中心Ω１は、
【数８７】
0006896103.tif 000088
の中心Ω０に基づいて計算することができる（ライン６及び７）。Ω１の座標を受けて、（１２）はＣＣＰ−Ｃ｜Ｃアルゴリズムのライン８において提示される式に変換することができる。この式を解くことにより、ｘ＊の値がもたらされる。反復が終了する前に、｜ｘ＊｜＜εが満たされるときにはいつでも、ｘ＊の値を受け入れることができる（ライン９〜１２）。その後、それに応じて、ｘ＊に関して、所望のＰＣＣを計算することができる。
【０１０４】
図７Ｄ４は、一例に関してＣ｜Ｃ処理方式の解をプロットする。
【０１０５】
図８Ａは、１つの実施形態による、自動化駐車システムの機能図を示す。環境マップ作成及び位置特定ブロック８０１が、環境及び車両動作条件を検知することによって、駐車空間のマップを構成又は更新し、車両の現在の場所を特定する。例えば、３軸加速度計（複数の場合もある）、３軸ジャイロスコープ（複数の場合もある）及び／又は磁気計（複数の場合もある）を含む場合がある慣性測定ユニットを用いて、車両動作を検知することができる。全地球測位システムセンサを用いて、車両の位置及び速度を与えることができる。環境８００を検知するセンサは、他の車両、歩行者及び建物を含む障害物を捕捉するビデオカメラ、車両と障害物との間の距離を検出する超音波／レーダーセンサ等とすることができる。最終状態選択ブロック８０１が、駐車場候補を識別することによって、車両を駐車する駐車位置に関する最終状態を選択し、最終状態を運動計画ブロック８０３に送信する。１つの実施形態において、利用可能な駐車位置は、駐車場ビルの管理に関連付けられる別のシステムによって追跡される。
【０１０６】
それに加えて、又はその代わりに、駐車位置は、自動化駐車システムのセンサ８３０を用いて検出することができる。一実施形態において、運動計画ブロックが、最終状態が駐車可能であるか否か、すなわち、駐車位置までの実現可能な経路があるか否かを判断するようにチェックし、チェック結果を最終状態選択ブロック８０２に通知する。最終状態が駐車可能でない場合には、目標選択ブロック８０２が、評価対象の別の最終状態を選択する。別の実施形態では、最終状態選択ブロック８０１が、最終状態が駐車可能であるか否かを評価することもでき、駐車可能な最終状態のみを運動計画ブロックに送信する。
【０１０７】
最終状態が駐車可能である場合には、運動計画８０３は、車両モデル８１０、車両の初期状態及び最終状態、並びに駐車空間のマップに基づいて、基準軌道８４１を決定する完全運動計画手順を開始する。１つの実施形態において、基準軌道は、経時的な車両速度及びステアリング角のプロファイルを規定する。別の実施形態において、基準軌道は、経時的な車両状態のプロファイルを規定する。
【０１０８】
基準軌道８４１を与えられると、車両コントローラ及びアクチュエータ８０４が、基準軌道が状態プロファイルである場合には、車両状態が基準軌道８４１を追跡するように強制し、又は、基準軌道が車両速度及びステアリング角プロファイルである場合には、車両速度及びステアリング角が基準軌道を追跡するように強制する制御コマンドを決定し、行使する。１つの実施形態において、制御コマンドは、アクセル圧又はステアリングトルクとすることができる。また、車両コントローラ／アクチュエータは、信号８４３を用いて、制御コマンドを決定することもできる。信号８４３は、測定ステアリング角、又はハンドル若しくはアクセルを動かすモーターの測定電流とすることができる。
【０１０９】
図８Ｂは、１つの実施形態による、運動計画システム８０３の全体構造を示す。運動計画システム８０３は、運動計画システム８０３のモジュールを実行するための少なくとも１つのプロセッサ８７０を備える。プロセッサ８７０は、メモリ８８０に接続され（８７１）、メモリは、車両のジオメトリ及び駐車空間のマップ等の幾何学的情報８８１を記憶する。また、メモリ８８０は、車両の運動学的モデル及び車両の動的モデル等の車両のモデル８８２を記憶することもできる。また、メモリ８８０は、限定はしないが、車両の初期状態、駐車後の車両の最終状態、コスト関数、計算された各状態の値、σの対の集合に関するあらかじめ計算されたクロソイド及びその偏角及び偏差、ＣＣ円の半径、各状態につながる運動、運動学的グラフ、基準軌道を含む、運動計画器の内部情報８８３を記憶することもできる。いくつかの実施形態において、メモリ８８０は、自動化駐車のための方法を実施する記憶された命令を含むことができ、それらの命令は、プロセッサ８７０によって実行されるときに、その方法の少なくともいくつかのステップを実行する。
【０１１０】
図９は、１つの実施形態によるシステムの概略図を示している。システムは、自動化駐車９５０を実行するように構成されたプロセッサ９０２を備える車両９０１を備える。車両は、ＬＩＤＡＲ９１０及び／又はカメラ９２０等の少なくとも１つのセンサも備える。ＬＩＤＡＲセンサ９１０は、低分解能の第１のセンサであり、カメラ９２０は、高分解能の第２のセンサである。センサ９１０及び／又はカメラ９２０は、プロセッサ９０２に動作的に接続されるとともに、駐車空間の少なくとも一部のジオメトリを示す情報を検知するように構成される。この情報を用いて、プロセッサ９０２は、駐車空間のマップ９００を求め、及び／又は更新する。そのために、プロセッサ９０２は、マップ９００を用いて自動化駐車９５０を実行する。
【０１１１】
図１０は、いくつかの実施形態による自動化駐車システム１０００のブロック図を示している。システム１０００は、車両９０１の内部に実装することができる。付加的又は代替的に、システム１０００は、車両９０１に通信可能に接続することができる。
【０１１２】
システム１０００は、センサ１０１０、慣性測定ユニット（ＩＭＵ）１０３０、プロセッサ１０５０、メモリ１０６０、送受信機１０７０、及びディスプレイ／スクリーン１０８０のうちの１つ又は組み合わせを備えることができる。これらは、接続１０２０を通じて他の構成要素に動作的に結合することができる。接続１０２０は、バス、ライン、ファイバー、リンク又はそれらの組み合わせを含むことができる。
【０１１３】
送受信機１０７０は、例えば、１つ以上のタイプの無線通信ネットワークを通じて１つ以上の信号を送信することを可能にする送信機と、１つ以上のタイプの無線通信ネットワークを通じて送信された１つ以上の信号を受信する受信機とを備えることができる。送受信機１０７０は、様々な技術に基づいて無線ネットワークとの通信を可能にすることができる。これらの技術は、標準規格のＩＥＥＥ８０２．１１ファミリーに基づくことができるフェムトセル、Ｗｉ−Ｆｉネットワーク又は無線ローカルエリアネットワーク（ＷＬＡＮ）、標準規格のＩＥＥＥ８０２．１５ｘファミリーに基づくＢｌｕｅｔｏｏｔｈ（東特商標）、近距離場通信（ＮＦＣ）、ネットワーク等の無線パーソナルエリアネットワーク（ＷＰＡＮ）、及び／又はＬＴＥ、ＷｉＭＡＸ等の無線ワイドエリアネットワーク（ＷＷＡＮ）等であるが、これらに限定されるものではない。システム１０００は、有線ネットワークを通じて通信するための１つ以上のポートを備えることもできる。
【０１１４】
いくつかの実施形態では、システム１０００は、ＣＣＤセンサ若しくはＣＭＯＳセンサ、レーザー及び／又はカメラ等の画像センサ１０１０を備えることができる。この画像センサは、以下では「センサ１０１０」と呼ばれる。例えば、センサ１０１０は、光画像を電子画像又はデジタル画像に変換することができ、取得された画像をプロセッサ１０５０に送信することができる。付加的又は代替的に、センサ１０１０は、シーン内のターゲット物体から反射された光を検知し、捕捉された光の強度をプロセッサ１０５０にサブミットすることができる。
【０１１５】
例えば、センサ１０１０は、「カラー情報」を提供するカラーカメラ又はグレースケールカメラを含むことができる。「カラー情報」という用語は、本明細書において用いられるとき、カラー情報及び／又はグレースケール情報を指す。一般に、カラー画像又はカラー情報は、本明細書において用いられるとき、１〜Ｎ個のチャネルを含むものとみなすことができる。ここで、Ｎは、画像を記憶するのに用いられている色空間に依存する或る整数である。例えば、ＲＧＢ画像は、３つのチャネルを含み、赤情報、青情報及び緑情報についてそれぞれ１つのチャネルを有する。
【０１１６】
例えば、センサ１０１０は、「深度情報」を提供する深度センサを含むことができる。深度情報は、深度センサを用いて様々な方法で取得することができる。「深度センサ」という用語は、深度情報を単独で及び／又は他のいくつかのカメラと併せて取得するのに用いることができる機能ユニットを指すのに用いられる。例えば、いくつかの実施形態では、深度センサ及び光カメラは、センサ１０１０の一部分とすることができる。例えば、いくつかの実施形態では、センサ１０１０はＲＧＢＤカメラを備える。このＲＧＢＤカメラは、カラー（ＲＧＢ）画像に加えて、深度センサが有効にされているときはピクセルごとの深度（Ｄ）情報を捕捉することができる。
【０１１７】
別の例として、いくつかの実施形態では、センサ１０１０は、３Ｄ飛行時間（３ＤＴＯＦ）カメラを含むことができる。３ＤＴＯＦカメラを用いた実施形態では、深度センサは、３ＤＴＯＦカメラに結合されたストロボライトの形態を取ることができる。このストロボライトは、シーン内の物体を照明することができ、反射された光は、センサ１０１０内のＣＣＤ／ＣＭＯＳセンサが捕捉することができる。深度情報は、光パルスが物体に進んでセンサに戻って来るまでに要する時間を測定することによって取得することができる。
【０１１８】
更なる例として、深度センサは、センサ１０１０に結合された光源の形態を取ることができる。１つの実施形態では、この光源は、１つ以上の狭い光の帯を含むことができる構造化された光パターン又はテクスチャー付けされた光パターンをシーン内の物体に投射する。深度情報は、物体の表面形状によって引き起こされる投射パターンの幾何学的歪みを利用することによって取得される。１つの実施形態は、赤外線構造化光プロジェクターと、ＲＧＢカメラに位置合わせされた赤外線カメラとの組み合わせ等のステレオセンサから深度情報を求める。
【０１１９】
いくつかの実施形態では、センサ１０１０は立体カメラを備える。例えば、深度センサは、２つ以上のカメラを用いてシーンの深度情報を取得することができる受動ステレオビジョンセンサの一部分を成すことができる。捕捉されたシーンにおける双方のカメラに共通の点のピクセル座標を、カメラ姿勢情報及び／又は三角測量技法とともに用いて、ピクセルごとの深度情報を取得することができる。
【０１２０】
いくつかの実施形態では、システム１０００は、デュアルフロントカメラ及び／又は前面カメラ及び背面カメラ等の複数のセンサ１０１０に動作的に接続することができ、これらの複数のセンサは、様々なセンサを組み込むこともできる。いくつかの実施形態では、センサ１０１０は、静止画像及びビデオ画像の双方を捕捉することができる。いくつかの実施形態では、センサ１０１０は、例えば、３０フレーム毎秒（ｆｐｓ）で画像を捕捉することが可能なＲＧＢＤ又は立体ビデオカメラを備えることができる。１つの実施形態では、センサ１０１０によって捕捉された画像は、生の未圧縮フォーマットとすることができ、処理及び／又はメモリ１０６０への記憶の前に圧縮することができる。いくつかの実施形態では、画像圧縮は、プロセッサ１０５０によって可逆圧縮技法又は非可逆圧縮技法を用いて実行することができる。
【０１２１】
いくつかの実施形態では、プロセッサ１０５０は、ＩＭＵ１０３０から入力を受信することもできる。他の実施形態では、ＩＭＵ１０３０は、３軸加速度計（複数の場合もある）、３軸ジャイロスコープ（複数の場合もある）、及び／又は磁気計（複数の場合もある）を備えることができる。ＩＭＵ１０３０は、速度、方位、及び／又は他の位置関連情報をプロセッサ１０５０に提供することができる。いくつかの実施形態では、ＩＭＵ１０３０は、測定情報を、センサ１０１０による各画像フレームの捕捉と同期して出力することができる。いくつかの実施形態では、ＩＭＵ１０３０の出力は、プロセッサ１０５０がセンサ測定値を融合し及び／又は融合された測定値を更に処理するのに部分的に用いられる。
【０１２２】
また、システム１０００は、カラー画像及び／又は深度画像等の画像をレンダリングするスクリーン又はディスプレイ１０８０を備えることができる。いくつかの実施形態では、ディスプレイ１０８０は、センサ１０１０によって捕捉されたライブ画像、融合画像、拡張現実（ＡＲ）画像、グラフィカルユーザインターフェース（ＧＵＩ）、及び他のプログラム出力を表示するのに用いることができる。いくつかの実施形態では、ディスプレイ１０８０は、ユーザが、仮想キーボード、アイコン、メニュー、又は他のＧＵＩ、ユーザジェスチャー及び／又はスタイラス及び他の筆記用具等の入力デバイスの或る組み合わせを介してデータを入力することを可能にするタッチスクリーンを備えることができ及び／又はこのようなタッチスクリーンとともに収容することができる。いくつかの実施形態では、ディスプレイ１０８０は、液晶ディスプレイ（ＬＣＤ）又は有機ＬＥＤ（ＯＬＥＤ）ディスプレイ等の発光ダイオード（ＬＥＤ）ディスプレイを用いて実施することができる。他の実施形態では、ディスプレイ１０８０は、ウェアラブルディスプレイとすることができる。いくつかの実施形態では、融合の結果をディスプレイ１０８０にレンダリングすることもできるし、システム１０００の内部又は外部に存在することができる異なるアプリケーションにサブミットすることもできる。
【０１２３】
例示的なシステム１０００は、図示した機能ブロックのうちの１つ以上の追加、組み合わせ、又は省略等によって、本開示と整合性を有するように様々な方法で変更することもできる。例えば、いくつかの構成では、システム１０００は、ＩＭＵ１０３０又は送受信機１０７０を備えていない。さらに、いくつかの特定の例示の実施態様では、システム１０００は、周辺光センサ、マイクロフォン、音響センサ、超音波センサ、レーザーレンジファインダー等の様々な他のセンサ（図示せず）を備える。いくつかの実施形態では、システム１０００のいくつかの部分は、１つ以上のチップセット等の形態を取る。
【０１２４】
プロセッサ１０５０は、ハードウェア、ファームウェア及びソフトウェアの組み合わせを用いて実現することができる。プロセッサ１０５０は、センサ融合及び／又は融合した測定値を更に処理する方法に関連付けられる計算手順又はプロセスの少なくとも一部を実行するように構成可能な１つ以上の回路を表すことができる。プロセッサ１０５０は、メモリ１０６０から命令及び／又はデータを引き出す。プロセッサ１０５０は、１つ以上の特定用途向け集積回路（ＡＳＩＣ）、中央及び／又はグラフィカル処理ユニット（ＣＰＵ及び／又はＧＰＵ）、デジタルシグナルプロセッサ（ＤＳＰ）、デジタル信号処理デバイス（ＤＳＰＤ）、プログラマブル論理デバイス（ＰＬＤ）、フィールドプログラマブルゲートアレイ（ＦＰＧＡ）、コントローラ、マイクロコントローラ、マイクロプロセッサ、埋め込みプロセッサコア、電子デバイス、本明細書において記述される機能を実行するように設計された他の電子ユニット、又はその組み合わせを用いて実現することができる。
【０１２５】
メモリ１０６０は、プロセッサ１０５０の内部に、及び／又はプロセッサ１０５０の外部に実装することができる。本明細書において使用されるときに、「メモリ」という用語は、任意のタイプの長期、短期、揮発性、不揮発性又は他のメモリを指しており、任意の特定のタイプのメモリ若しくはメモリの数、又はメモリが記憶される物理媒体のタイプに制限されるべきではない。いくつかの実施形態では、メモリ１０６０は、自動化駐車を容易にするプログラムコードを保持する。
【０１２６】
例えば、メモリ１０６０は、静止画像、深度情報、ビデオフレーム、プログラム結果、並びにＩＭＵ１０３０及び他のセンサによって提供されるデータ等のセンサの測定値を記憶することができる。メモリ１０６０は、車両のジオメトリ、駐車空間のマップ、車両の運動学的モデル、及び車両の動的モデルを記憶するメモリを格納することができる。一般に、メモリ１０６０は、任意のデータ記憶機構を表すことができる。メモリ１０６０は、例えば、一次メモリ及び／又は二次メモリを含むことができる。一次メモリは、例えば、ランダムアクセスメモリ、リードオンリーメモリ等を含むことができる。図１０においてプロセッサ１０５０とは別であるように示されるが、一次メモリの全て若しくは一部をプロセッサ１０５０内に設けることができるか、又はそうでなくても、プロセッサ１０５０と同一の場所に配置し、及び／又はプロセッサ１０５０に結合することができることは理解されたい。
【０１２７】
二次メモリは、例えば、一次メモリと同じ、又は類似のタイプのメモリ、及び／又は例えば、フラッシュ／ＵＳＢメモリドライブ、メモリカードドライブ、ディスクドライブ、光ディスクドライブ、テープドライブ、ソリッドステートドライブ、ハイブリッドドライブ等の１つ以上のデータ記憶デバイス又はシステムを含むことができる。或る特定の実施態様において、二次メモリは、取外し可能な媒体ドライブ（図示せず）内の非一時的コンピュータ可読媒体に動作的に収容可能であるか、又は別の方法で、動作的に構成可能とすることができる。いくつかの実施形態において、非一時的コンピュータ可読媒体は、メモリ１０６０及び／又はプロセッサ１０５０の一部を形成する。
【０１２８】
本発明の上記で説明した実施形態は、多数の方法のうちの任意のもので実施することができる。例えば、実施形態は、ハードウェア、ソフトウェア又はそれらの組み合わせを用いて実施することができる。ソフトウェアで実施される場合、ソフトウェアコードは、単一のコンピュータに設けられるのか又は複数のコンピュータ間に分散されるのかにかかわらず、任意の適したプロセッサ又はプロセッサの集合体において実行することができる。そのようなプロセッサは、１つ以上のプロセッサを集積回路部品に有する集積回路として実装することができる。ただし、プロセッサは、任意の適したフォーマットの回路類を用いて実装することができる。
【０１２９】
また、本発明の実施形態は、例が提供された方法として実施することができる。この方法の一部として実行される動作は、任意の適切な方法で順序付けすることができる。したがって、動作が示したものと異なる順序で実行される実施形態を構築することができ、これには、例示の実施形態では一連の動作として示されたにもかかわらず、いくつかの動作を同時に実行することを含めることもできる。
【０１３０】
請求項の要素を修飾する、特許請求の範囲における「第１」、「第２」等の序数の使用は、それ自体で、或る請求項の要素の別の請求項の要素に対する優先順位も、優位性も、順序も暗示するものでもなければ、方法の動作が実行される時間的な順序も暗示するものでもなく、請求項の要素を区別するために、単に、或る特定の名称を有する或る請求項の要素を、同じ（序数の用語の使用を除く）名称を有する別の要素と区別するラベルとして用いられているにすぎない。
【図１Ａ】
0006896103.tif 000089
【図１Ｂ】
0006896103.tif 000090
【図１Ｃ】
0006896103.tif 000091
【図１Ｄ】
0006896103.tif 000092
【図１Ｅ】
0006896103.tif 000093
【図１Ｆ】
0006896103.tif 000094
【図１ＧＡ】
0006896103.tif 000095
【図１ＧＢ】
0006896103.tif 000096
【図２Ａ】
0006896103.tif 000097
【図２Ｂ】
0006896103.tif 000098
【図３Ａ】
0006896103.tif 000099
【図３Ｂ】
0006896103.tif 000100
【図４Ａ】
0006896103.tif 000101
【図４Ｂ】
0006896103.tif 000102
【図４Ｃ】
0006896103.tif 000103
【図５Ａ１】
0006896103.tif 000104
【図５Ａ２】
0006896103.tif 000105
【図５Ａ３】
0006896103.tif 000106
【図５Ａ４】
0006896103.tif 000107
【図５Ｂ】
0006896103.tif 000108
【図５Ｃ】
0006896103.tif 000109
【図５Ｄ】
0006896103.tif 000110
【図５Ｅ】
0006896103.tif 000111
【図５Ｆ】
0006896103.tif 000112
【図５Ｇ】
0006896103.tif 000113
【図６】
0006896103.tif 000114
【図７Ａ】
0006896103.tif 000115
【図７Ｂ１】
0006896103.tif 000116
【図７Ｂ２】
0006896103.tif 000117
【図７Ｂ３】
0006896103.tif 000118
【図７Ｂ４】
0006896103.tif 000119
【図７Ｃ１】
0006896103.tif 000120
【図７Ｃ２】
0006896103.tif 000121
【図７Ｃ３】
0006896103.tif 000122
【図７Ｃ４】
0006896103.tif 000123
【図７Ｄ１】
0006896103.tif 000124
【図７Ｄ２】
0006896103.tif 000125
【図７Ｄ３】
0006896103.tif 000126
【図７Ｄ４】
0006896103.tif 000127
【図８Ａ】
0006896103.tif 000128
【図８Ｂ】
0006896103.tif 000129
【図９】
0006896103.tif 000130
【図１０】
0006896103.tif 000131