JP7090751B2

JP7090751B2 - 車両の移動を制御するシステム及び方法

Info

Publication number: JP7090751B2
Application number: JP2020570585A
Authority: JP
Inventors: ワン、イェビン
Original assignee: Mitsubishi Electric Corp
Current assignee: Mitsubishi Electric Corp
Priority date: 2018-09-25
Filing date: 2019-04-19
Publication date: 2022-06-24
Anticipated expiration: 2039-04-19
Also published as: CN112703147A; EP3856615A1; JP2021528307A; CN112703147B; WO2020066104A1; US20200097014A1; US10809732B2

Description

本発明は、包括的には、車両の経路計画に関し、より詳細には、車両の移動を決定論的方法で初期状態から目標空間内に制御する経路計画方法に関する。

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

経路計画は、自律車両、ロボット工学、製造用途、及びコンピュータービジョンアプリケーション等の様々な用途に見られる。多くの計画アルゴリズムが提案されてきた。いくつか例を挙げると、グラフベースのＡ＊方法及びＤ＊方法、ナビゲーション機能及びポテンシャル場を伴う連続的手法があり、より最近では、確率的ロードマップ（ＰＲＭ：probabilistic roadmap）、開放的空間木（expansive-space trees）、高速探索ランダム木（ＲＲＴ：rapidly-exploring random trees）、最適な変形形態であるＲＲＴ＊及びＰＲＭ＊、並びにエニータイムＲＲＴ等のサンプリングベースの方法がある。

自律走行モードにおいて実行される自律車両又は半自律車両を制御するタスクのうちの１つは、本明細書において目標状態と呼ばれる駐車位置及び方位に車両を自動的に駐車する。この駐車タスクは、次のように定式化することができる。車両動態、駐車空間のマップ、車両の開始位置及び開始方位を表す初期状態、並びに車両の目標駐車位置及び目標駐車方位を表す目標状態が与えられると、初期状態から目標状態への車両の所望の経路又は運動を求め、次に、車両が所望の経路又は運動に追従することを保証するように車両のアクチュエーター、例えば、車両のアクセルペダル及びハンドルを制御する。しかしながら、車両の運動に対する非ホロノミック制約と、駐車場ビル又は屋外駐車場等の駐車空間内の通常は狭い自由空間とに起因して、車両を自動的に駐車する経路計画は困難なものとなる。

広範囲のグラフベースの手法が、ロボットの構成空間を近似する既定のグラフを検索する。このグラフは、各ノード及び各エッジがそれぞれ同じ構成及び接続経路を表すという意味で或る程度静的である。グラフのノード及びエッジの双方は、検索アルゴリズムの探索中に無衝突又は占有としてマーキングされる。Ａ＊及びＤ＊等の十分に確立したアルゴリズムは、いくつかの特定の状況下で最適性保証を有する解像度完全性（resolution-completeness）を達成する。既定のグラフの複雑さは、構成空間の次元とともに指数関数的に増大するので、これらの手法は、高次元システムでは事実上実現不可能となる。

サンプリングベースの方法は、次元の問題を克服し、高次元システム上で普及することができる。サンプリングベースの方法によれば、既定のグラフを使用するのではなく、グラフは実行時に構築され、そのノードは構成空間からランダムに取り出される。これらの方法の有効性は、グラフ構築（すなわち、空間探索）の効率によって決まる。

しかしながら、ＲＲＴ方法等のサンプリングベースの方法は計算コストが高い。そのために、多くの研究成果は、サンプリング方式の計算効率の改善に偏ったものであった。例えば、特許文献１は、中間ではあるが里程標構成を表す一組の中間地点（waypoint）によってサンプリングを偏倚させる。しかしながら、偏倚されたサンプリングであっても、いくつかの用途では計算的に実用的でない可能性がある。

米国特許出願第１５／４０２３５３号

いくつかの実施の形態の目的は、自動化車両制御のリアルタイム経路生成を可能にする運動計画方法を開示することである。いくつかの実施の形態の別の目的は、例えば、自動化駐車システム及び方法に適したリアルタイム自動化車両制御を可能にする運動計画方法を開示することである。いくつかの実施の形態の別の目的は、車両の状態空間を探索するサンプリング方法の確率的性質によって引き起こされる経路計画の計算コストを削減するそのような方法を提供することである。いくつかの実施の形態の別の目的は、計算複雑さ及び／又はメモリ要件を低減する、車両の状態空間の決定論的探索のためのシステム及び方法を提供することである。

いくつかの実施の形態は、車両の移動を車両の初期状態及び車両の目標状態から制御する自動化車両制御システムが、車両の状態を定義する複数のノードを有するグラフを構築するように構成される運動計画器を備えることができるという認識に基づいている。運動計画器は、そのようなグラフを使用して車両の状態空間を探索し、車両を移動させる制御動作を選択することができる。例えば、グラフのノードは、車両の初期状態を定義する初期ノードと、車両の目標状態を定義する目標ノードとを含む。グラフにおける各ノード対は、接続されるノードの状態の間で車両を移動させる無衝突プリミティブ運動のうちの１つによって定義されるエッジと接続される。プリミティブ運動は、当該プリミティブ運動のタイプ及び延長を定義する制御動作及び積分時間の組み合わせによって形成される。

このグラフがあると、運動計画器は、初期ノードを目標ノードと接続するグラフを通る経路であって、車両を初期状態から目標状態に移動させる一連のプリミティブ運動を定義するような経路を求めて、この一連のプリミティブ運動を使用して車両の移動を制御することができる。しかしながら、そのようなシステムの課題は、リアルタイムの経路生成及び制御に適した計算効率の良い方法でグラフを構築することにある。

いくつかの実施の形態は、サンプリングベースの経路計画方法を使用して、任意のレイアウトを有する状態空間を通る所望の経路を見つけることができるという理解に基づいている。しかしながら、高速探索ランダム木（ＲＲＴ）方法及びその変形形態等のサンプリングベースの経路計画方法を自動化制御問題に適用すると、多数の無駄な計算が生じる可能性があり、これによって、このタスクは、リアルタイムの用途には計算的に非実用的なものとなる可能性がある。例えば、車両の目標状態を初期状態、例えば、現在の状態から隔てることができる多数の障害物に起因して、目標状態への経路は、異なる形態及び形状を有する可能性がある。そのために、そのような経路を見つけるために、状態空間全体をサンプリングし、経路の実現の可否について検査することが必要となる場合がある。

その結果、確率的サンプリングベースの経路計画方法は、全ての未探索の状態空間を不必要にサンプリングする可能性があり、その結果、リアルタイム車両制御アプリケーションの解決策が実用的でない可能性がある。加えて、その不必要なサンプリングに起因して、確率的計画器は、人間の直観と整合しない経路を出力する可能性が高い。そのような経路は、不快感を与えるとともに人間の運転手を混乱させる可能性がある。そのために、計算効率の目的に加えて又はこれに代えて、いくつかの実施の形態の目的は、人間のオペレーターが選択し得る経路に類似している経路を生成することである。

いくつかの実施の形態は、障害物を有する空間を通って車両を走行させるための経路を選択する人間のオペレーターの挙動及び／又は根拠に関する観察結果から収集されたいくつかの認識に基づいている。これらの観察結果は、一括して又は個別に自動経路計画及び制御のいくつかの実施の形態をもたらす。

第１に、１つの実施の形態は、空間の探索が、人間のオペレーターによって行われる方法と同様に、決定論的であるべきである、すなわち、確率論的であるべきでないという認識に基づいている。いくつかの実施の形態は、決定論的方法で車両の初期状態と目標車両とを接続するグラフを構築することによって、この認識を計画制御システムに組み込む。例えば、１つの実施の形態は、反復的にノードを選択し、選択されたノードを、既定のプリミティブ運動の有限集合を用いて拡張することによってグラフ（又は木）を構築する。この方法の課題は、この方法の計算効率を確保するために、拡張するノードを決定論的に識別する方法及び／又はノードを拡張する運動プリミティブを決定論的に選択する方法を求めることにある。

第２に、１つの実施の形態は、人間のオペレーターが局所的な情報だけでなく大域的な情報も使用して空間探索を実行するという認識に基づいている。例えば、人間のオペレーターは、車両の現在の状態、すなわち、初期状態の周囲の空間と、車両の目標状態の周囲の空間とを評価して、車両の経路について意思決定する。いくつかの実施の形態は、１つの木の代わりに、グラフを形成する２つの木を構築することによって、すなわち、車両の初期状態に対応する初期ノードを起点するノードの初期木を構築することと、車両の目標状態に対応する目標ノードを起点とするノードの目標木を構築することとによって、大域的な情報を決定論的グラフベースの経路計画方法に組み込むことができるという認識に基づいている。初期木は、車両の周囲の局所的な情報を表し、目標木は、車両の目的地の周囲の局所的な情報を表し、初期木と目標木との間の関係は、大域的な情報に関する見識を提供する。

加えて、この実施の形態は、自由空間から閉塞空間に向かう「内向木（inward tree）」を構築するのではなく、閉塞空間から自由空間への「外向木（outward tree）」を構築することによって、空間を取り囲む障害物をより効率的に回避することができるという、複数のシミュレーション及び実験によって確認された直観に基づいている。自律駐車システム等の複数の用途において、駐車空間は、通常、車庫内又は駐車空間につながる駐車場における空間よりも閉塞している。そのために、いくつかの実施の形態は、二重木（doubletree）空間探索が単一木（singletree）空間探索よりも計算効率が良く、人間の直観により忠実である可能性があるという認識に基づいている。

いくつかの実施の形態は、拡張する初期木又は目標木上のノードを選択することによって二重木空間探索の計算効率性を利用する。具体的には、いくつかの実施の形態は、拡張可能ノード（expandable node：展開可能ノード）のコストに基づいて初期木又は目標木内の拡張可能ノードを選択し、子ノードのコストが拡張可能ノードのコストよりも少なくなるような、無衝突プリミティブ運動によって定義されるエッジを用いて拡張可能ノードに接続された子ノードを追加することによってグラフを拡張する。そのような方法で、木の決定論的な選択及び拡張が達成される。

加えて、ノードのコストとは、本明細書において使用されるときは、このノードを通って初期ノードから目標ノードに到達するための最小コストであり、初期木のノードを通る第１の経路の第１のコスト（初期状態の周囲の局所的な情報を考慮する）と、目標木のノードを通る第２の経路の第２のコスト（目標状態の周囲の局所的な情報を考慮する）と、初期木のノードと目標木のノードとの間の第３の経路の第３のコスト（初期状態と目標状態との間の関係を考慮する）とを含む。拡張可能ノードのそのような選択によって、双方の木の構築から収集された情報を考慮することが可能になり、グラフ延長がより計算効率の良いものになる。例えば、複数の実験が、二重木グラフ構築が単一木グラフ構築よりも少ないノードを有するグラフを特定することを実証している。

第３に、１つの実施の形態は、ノード選択に加えてノード拡張も決定論的方法で実行され、全ての可能なプリミティブ運動を用いてノードの周囲の空間を探索する必要性を低減することができるという認識に基づいている。そのために、一実施の形態は、拡張可能ノードのコストを削減する第１の無衝突プリミティブ運動を選択する。すなわち、後続のプリミティブ運動の検査は行われない。計算負担を更に削減するために、いくつかの実施の形態は、一組のプリミティブ運動を、これらのプリミティブ運動のそれぞれと、拡張可能ノードにつながるグラフのエッジを定義するプリミティブ運動との類似度に基づいて順序付け、その順序でこれらのプリミティブ運動を検査する。これらの実施の形態は、人間のオペレーターが所望するような車両移動の平滑性（smoothness）を促進し、実際に、構築されるグラフの複雑さを低減するのに役立つ。

ノード拡張の削減は、構築されるグラフのスパーシティを低減し、これは、一方で計算効率を改善し、他方で空間探索の選択肢を削減する。いくつかの実施の形態の決定論的ノード拡張によって生み出されるこの問題に対処するために、いくつかの実施態様は、グラフの異なる部分においてグラフのスパーシティを変更する。例えば、１つの実施態様は、潜在的に拡張可能なノードのリストを運動計画器のメモリに保持し、拡張可能ノードのリストの長さに基づいてスパーシティの値を選択する。スパーシティの値は、プリミティブ運動を形成する制御動作の積分時間、及び、拡張可能ノードの子ノードとグラフにおける最も近いノードとの間の許容される最小距離のうちの一方又は組み合わせによって規定することができる。

したがって、１つの実施の形態は、車両の移動を該車両の初期状態及び該車両の目標状態から制御するシステムを開示する。該システムは、前記車両の状態を定義する複数のノードを有するグラフを構築することであって、前記ノードは、前記車両の前記初期状態を定義する初期ノードと、前記車両の前記目標状態を定義する目標ノードとを含み、前記グラフ内の各ノード対はエッジと接続され、該エッジは、前記接続されたノードの前記状態の間で前記車両を移動させる無衝突プリミティブ運動のうちの１つによって定義されることと、前記初期ノードを前記目標ノードと接続する前記グラフを通る経路であって、前記車両を前記初期状態から前記目標状態に移動させる一連のプリミティブ運動を定義するような経路を特定することと、前記一連のプリミティブ運動を使用して前記車両の前記移動を制御するように構成される前記車両のコントローラーに前記一連のプリミティブ運動を送信することとを行うように構成されるプロセッサを備える運動計画器を備える。

前記運動計画器は、前記初期ノードを起点とするノードの初期木を構築することと、前記目標ノードを起点とするノードの目標木を構築することとによって前記グラフを特定するように構成され、前記初期木又は前記目標木を構築するために、前記運動計画器は、前記初期木又は前記目標木内の拡張可能ノードを該拡張可能ノードのコストに基づいて選択し、無衝突プリミティブ運動によって定義されるエッジを用いて前記拡張可能ノードに接続される子ノードのコストが前記拡張可能ノードの前記コストよりも小さくなるような前記子ノードを追加することによって前記グラフを拡張するように構成され、前記ノードのコストは、該ノードを通って前記初期ノードから前記目標ノードに到達するための最小コストであり、前記初期木のノードを通る第１の経路の第１のコストと、前記目標木のノードを通る第２の経路の第２のコストと、前記初期木のノードと前記目標木のノードとの間の第３の経路の第３のコストとを含む。

別の実施の形態は、車両の移動を該車両の初期状態及び該車両の目標状態から制御する方法を開示し、該方法は、該方法を実施する記憶された命令と結合されたプロセッサを使用し、前記命令は、前記プロセッサによって実行されると、前記車両の状態を定義する複数のノードを有するグラフを構築することであって、前記ノードは、前記車両の前記初期状態を定義する初期ノードと、前記車両の前記目標状態を定義する目標ノードとを含み、前記グラフ内の各ノード対は、前記接続されたノードの前記状態の間で前記車両を移動させる無衝突プリミティブ運動のうちの１つによって定義されるエッジと接続され、前記グラフは、前記初期ノードを起点とするノードの初期木と、前記目標ノードを起点とするノードの目標木とを含み、前記初期木又は前記目標木を構築するために、前記プロセッサは、前記初期木又は前記目標木における拡張可能ノードを該拡張可能ノードのコストに基づいて選択し、無衝突プリミティブ運動によって定義されるエッジを用いて該拡張可能ノードに接続される子ノードのコストが前記拡張可能ノードの前記コストよりも小さくなるような前記子ノードを追加することによって前記グラフを拡張するように構成され、前記ノードのコストは、該ノードを通って前記初期ノードから前記目標ノードに到達するための最小コストであり、前記初期木のノードを通る第１の経路の第１のコストと、前記目標木のノードを通る第２の経路の第２のコストと、前記初期木のノードと前記目標木のノードとの間の第３の経路の第３のコストとを含むことと、前記初期ノードを前記目標ノードと接続する前記グラフを通る経路であって、前記車両を前記初期状態から前記目標状態に移動させる一連のプリミティブ運動を定義するような経路を特定することと、前記一連のプリミティブ運動を使用して前記車両の前記移動を制御するように構成される前記車両のコントローラーに前記一連のプリミティブ運動を送信することとを含む、該方法のステップを実行する。

更に別の実施の形態は、方法を実行するプロセッサによって実行可能なプログラムが具現化される非一時的コンピューター可読記憶媒体を開示し、前記方法は、前記車両の状態を定義する複数のノードを有するグラフを構築することであって、前記ノードは、前記車両の前記初期状態を定義する初期ノードと、前記車両の前記目標状態を定義する目標ノードとを含み、前記グラフ内の各ノード対は、前記接続されたノードの前記状態の間で前記車両を移動させる無衝突プリミティブ運動のうちの１つによって定義されるエッジと接続され、前記グラフは、前記初期ノードを起点とするノードの初期木と、前記目標ノードを起点とするノードの目標木とを含み、前記初期木又は前記目標木を構築するために、前記プロセッサは、前記初期木又は前記目標木における拡張可能ノードを該拡張可能ノードのコストに基づいて選択し、無衝突プリミティブ運動によって定義されるエッジを用いて該拡張可能ノードに接続される子ノードのコストが前記拡張可能ノードの前記コストよりも小さくなるような前記子ノードを追加することによって前記グラフを拡張するように構成され、前記ノードのコストは、該ノードを通って前記初期ノードから前記目標ノードに到達するための最小コストであり、前記初期木のノードを通る第１の経路の第１のコストと、前記目標木のノードを通る第２の経路の第２のコストと、前記初期木のノードと前記目標木のノードとの間の第３の経路の第３のコストとを含むことと、前記初期ノードを前記目標ノードと接続する前記グラフを通る経路であって、前記車両を前記初期状態から前記目標状態に移動させる一連のプリミティブ運動を定義するような経路を特定することと、前記一連のプリミティブ運動を使用して前記車両の前記移動を制御するように構成される前記車両のコントローラーに前記一連のプリミティブ運動を送信することとを含む。

いくつかの実施形態によって扱われる駐車シナリオの一例を示す図である。いくつかの実施形態による車両の幾何学的表現の例示的な概略図である。図１Ａの駐車シナリオに対応する駐車空間のマップを示す図である。１つの実施形態による自動化車両制御システムのブロック図である。１つの実施形態による運動計画システムの全体構造のブロック図である。いくつかの実施形態による運動計画器によって実行される方法のブロック図である。１つの実施形態による二重木構築（doubletree construction）を使用してグラフを特定する概略図である。いくつかの実施形態による運動計画方法の方法のブロック図である。１つの実施形態によるグラフ構築方法のブロック図である。いくつかの実施形態によるグラフを表すデータ構造の見本例を示す図である。１つの実施形態によるノードのデータ構造を示す図である。いくつかの実施形態の原理によるグラフ成長パラメーターを調節する方法のフローチャートである。別の実施形態によるグラフ成長パラメーターを調節する方法のフローチャートである。いくつかの実施形態による拡張可能ノードのリストから拡張可能ノードを選択する方法のブロック図である。１つの実施形態によるノード拡張のブロック図である。反復グラフ構築を終了するためにいくつかの実施形態によって使用されるいくつかの停止基準のブロック図である。１つの実施形態による、目標木が目標ノードのみを含むときのノードＸのコストＦ（Ｘ）を求める方法の概略図である。１つの実施形態による、目標木が複数のノードを含むときのノードＸのコストＦ（Ｘ）を求める方法の概略図である。異なる実施形態の利点を示す例の概略図である。いくつかの実施形態による一組の制御動作

及び積分時間

に関連したプリミティブ運動を示す概略図である。
ノードを拡張するためにいくつかの実施形態によって使用されるプリミティブ運動の順序付きリストを示す図である。１つの実施形態による、選択されたノードを拡張する運動プリミティブ又は運動プリミティブのサブセットを特定する方法のブロック図である。別の実施形態による、拡張可能ノードの拡張中に運動プリミティブを選択する方法のブロック図である。１つの実施形態によるシステムの概略図である。いくつかの実施形態による自動化駐車システムのブロック図である。

いくつかの実施形態の目的は、自動化車両制御のためのリアルタイム経路生成を可能にする経路及び／又は運動計画方法を開示することである。いくつかの実施形態の別の目的は、例えば、自動化駐車システム及び方法に適したリアルタイム自動化車両制御を可能にする運動計画方法を開示することである。いくつかの実施形態は自動化駐車シナリオに関して説明されるが、これは、明瞭化という目的だけのものにすぎず、駐車シナリオに関して説明される原理は、他の自動化車両制御アプリケーションにおける代替の実施形態によっても使用される。

図１Ａは、いくつかの実施形態によって取り扱われる駐車シナリオの一例を示している。この例において、駐車空間１５０の境界は、Ｌ×Ｈのサイズを有する方形によって表される。車両１００は、初期状態１０１を有し、目標状態１０２によって規定される駐車位置に駐車される必要がある。各状態、例えば、初期状態及び目標状態は、車両の位置及び方位を規定する。駐車空間１５０は障害物を含む。これらの障害物は、駐車空間のレイアウトの一部、すなわち、駐車空間の壁及び／又は柱等の常設障害物１０３とすることができる。常設障害物の寸法は、通常は既知であるか、又は、妥当な範囲で推定することができる。図１Ａは、そのような寸法の非限定的な例を表している。

それに加えて、又はその代わりに、障害物は、他の駐車車両又は移動車両１０４を含むことができる。車両１０４及び駐車される車両１００の幾何学的寸法は、車両のタイプに基づいて求めることができる。明瞭にするために、本開示は前輪駆動車両を考察する。ただし、異なる実施形態は、後輪駆動車両及び全輪駆動車両を含む他の車両に適用される。

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

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

経路計画は次の問題を解くことである。初期構成

、目標構成

、及びシステム（２）が与えられると、
Ｘ_０で開始しかつＸ_ｆで終了し、
無衝突構成空間

にあり、
システム運動学（２）を満たす、
無衝突で運動学的に実現可能な経路

を見つける。

各非自明な経路に非負のコストを割り当てるコスト関数をＪ（・）とする。最適な経路計画は、Ｊ（・）を最小にする無衝突で運動学的に実現可能な経路

を見つけることである。

異なる実施形態の運動計画は、車両が経路に沿ってどのように移動するのかを、車両１００の運動学的モデル及び／又は動的モデルを使用して求める。例えば、車両の動的モデルは、車両の状態の時間依存変化を考慮する。動的モデルは、通常、微分方程式によって表される。１つの実施形態において、車両の動的モデルは、以下の五階微分方程式である。

ただし、ｖは前輪の速度であり、ζは前輪と車両方位との間の角度であり、ａ_ｖは並進加速度であり、ａ_ｓはステアリング角速度であり、ｂは（ｘ，ｙ）と前輪の中点との間の距離である。

いくつかの実施形態において、経路は、動的モデル（１）に従って生成される。動的に実現可能な経路が、動的モデル（１）の解である。したがって、車両状態はＸ＝（ｘ，ｙ，θ，ｖ，ζ）である。車両は、通常、０の速度及びステアリング角で開始及び終了するので、したがって、動的モデル（１）の初期状態及び目標状態は、それぞれＸ_０＝（ｘ_０，ｙ_０，θ_０，０，０）及びＸ_ｆ＝（ｘ_ｆ，ｙ_ｆ，θ_ｆ，０，０）である。

それに加えて、又はその代わりに、いくつかの実施形態は、車両の質量も運動を引き起こす力も考慮に入れずに車両の運動を記述する車両の運動学的モデルを運動計画に使用する。１つの実施形態において、以下の運動学的モデルが検討される。

ただし、ｕ_１＝ｃｏｓ（ζ）であり、ｖは後輪軸の中点の速度（以下、車両の縦速度（longitudinal velocity：前進速度）と呼ぶ）であり、ｕ_２＝ｔａｎ（ζ）／ｂである。一般性を失うことなく、ｕ_２は、（正規化された）ステアリング角と呼ばれる。

この実施形態によれば、経路が運動学的モデル（２）の解である場合には、その経路は運動学的に実現可能である。Ｘに位置する車両がいずれの障害物とも衝突せず、境界１０５の内部に完全にいる場合にのみ、車両状態Ｘ＝（ｘ，ｙ，θ）は無衝突である。全ての無衝突車両状態が無衝突構成空間

を構成する。初期状態１０１はＸ_０＝（ｘ_０，ｙ_０，θ_０）と略記され、目標状態１０２はＸ_ｆ＝（ｘ_ｆ，ｙ_ｆ，θ_ｆ）によって表される。方形Ｌ×Ｈによって表される駐車空間を用いた特定の駐車タスクの場合には、車両状態は、状態空間

に常に属する。

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

木

は、ノード集合

とエッジ集合

との和集合、すなわち、

である。混乱を引き起こすことなく、ノード、構成、及び車両状態は、以下において区別なく使用される。エッジ

は、ノードＸ_ｉとノードＸ_ｊとの間の無衝突で運動学的に実現可能な経路を表す。有限集合

の場合に、

はその要素の数を表す。

いくつかの実施形態は、車両状態が衝突を伴うものでないか否かを、駐車空間のマップとも呼ばれる駐車空間の幾何学的表現に基づいて判断する。駐車空間のマップは、駐車空間の全ての障害物及び境界を単純な幾何学的形状として近似することによって得られる。１つの実施形態において、環境（すなわち、駐車空間）内の障害物１０３は、各障害物の最小の境界ボックスを構築することによって得られる方形として近似することができる。駐車空間の障害物及び境界の幾何学的近似を用いると、駐車空間、すなわち、環境を幾何学的物体のリストによって完全に記述することができる。

図１Ｃは、図１Ａの駐車シナリオに対応する駐車空間のマップ１３０を示している。双方のタイプの障害物は、方形表現を使用して１つのタイプの常設障害物１０３として互いに統合される。障害物は、経路計画用のオンボードプロセッサの計算能力に応じて、複数の幾何学的形状によって近似することができる。例えば、別の実施形態において、計算能力が十分でない場合には、障害物は、各障害物の境界円を構築することによって近似することができる。或いは、障害物は、ポリトープによって近似することができるが、これは、経路計画において計算負担を増加させる場合がある。また、駐車空間内の障害物は、同じ幾何学的形状によって近似されない場合がある。いくつかの実施形態において、環境マップは一群のセルに分解され、これらのセルのそれぞれは、構成と、或る特定の体積を有するその近傍とに対応する。セルは異なる体積を有する可能性がある。駐車空間内の障害物は、一群のセルによって表すことができる。

図２Ａは、１つの実施形態による自動化車両制御システム２９９のブロック図を示している。環境マップ作成及び位置特定ブロック２０１は、環境及び車両動作条件を検知することによって、空間、例えば、駐車空間のマップを構築又は更新し、車両の現在の場所を特定する。例えば、３軸加速度計（複数の場合もある）、３軸ジャイロスコープ（複数の場合もある）及び／又は磁気計（複数の場合もある）を含むことができる慣性測定ユニット（ＩＭＵ：inertial measurement unit）を用いて、車両動作を検知することができる。全地球測位システムセンサーを用いて、車両の位置及び速度を与えることができる。環境２００を検知するセンサーは、他の車両、歩行者及び建物を含む障害物を捕捉するビデオカメラ、車両と障害物との間の距離を検出する超音波／レーダーセンサー及びＬｉＤＡＲセンサー等とすることができる。１つの実施形態において、環境マップが更に処理され、図１Ｃに示されるような駐車空間の幾何学的表現が生成される。

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

目標状態が駐車可能である場合には、運動計画２０３は、車両モデル２１０、車両の初期状態及び目標状態、並びに駐車空間のマップのうちの１つ又は組み合わせに基づいて、基準軌道２４１を決定する完全運動計画手順を開始する。基準軌道は、車両の一連のプリミティブ運動である。例えば、１つの実施形態において、基準軌道は、経時的な車両速度及びステアリング角のプロファイルを規定する。別の実施形態において、基準軌道は、経時的な車両状態（ｘ，ｙ，θ）のプロファイルを規定する。ほとんどの場合に、種々の形態の基準軌道は同等であり、１つの軌道が車両モデルを介して別の軌道を規定する。

運動計画器は、一連のプリミティブ運動によって決定される基準軌道を車両のコントローラー２０４に送信する。基準軌道２４１を与えられると、車両コントローラー及びアクチュエーター２０４は、車両状態がその基準軌道２４１を追跡するように強制する制御コマンドを決定し、行使する。１つの実施形態において、制御コマンドは、アクセル圧及び／又はステアリングトルクとすることができる。車両コントローラー／アクチュエーターは、信号２４３及び２４４を用いて、制御コマンドを決定することができる。信号２４３は、測定ステアリング角、又はハンドル若しくはアクセルを動かすモーターの測定電流、ＩＭＵの出力、及び位置特定ブロック２０１によって推定される車両の現在の状態とすることができる。

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

図３Ａは、いくつかの実施形態による、車両の初期状態から車両の目標状態への車両の移動を制御するシステム２９９の運動計画器２０３によって実行される方法のブロック図を示している。運動計画器は、車両の状態を定義する複数のノードを有するグラフを構築する（３１０）ように構成される。ノードは、車両の初期状態を定義する初期ノードと、車両の目標状態を定義する目標ノードとを含む。グラフ内の各ノード対は、接続されたノードの状態の間で車両を移動させる無衝突プリミティブ運動のうちの１つによって定義されるエッジと接続される。

運動計画器は、経路が初期状態から目標状態に車両を移動させる一連のプリミティブ運動を規定するように、初期ノードを目標ノードと接続するグラフを通る経路を求める（３３０）ことと、この一連のプリミティブ運動を使用して車両の移動を制御するように構成された車両のコントローラーにこの一連のプリミティブ運動を送信する（３３５）こととを行うようにも構成される。グラフが得られると、経路のコスト及び平滑性のうちの一方又は組み合わせ等の経路のパラメーターを最適化する種々の最適化技法を使用して経路を求めることができる。

いくつかの実施形態において、運動計画器は、二重木構築を使用してグラフを構築する（３１０）。具体的には、運動計画器は、初期ノードを起点とするノードの初期木を構築することと、目標ノードを起点とするノードの目標木を構築することとによってグラフを求める。初期木又は目標木を構築するために、運動計画器は、初期木又は目標木における拡張可能ノードをこの拡張可能ノードのコストに基づいて選択する（３１５）ことと、子ノードのコストが拡張可能ノードのコストよりも少なくなるような、無衝突プリミティブ運動によって定義されるエッジを用いて拡張可能ノードに接続された子ノードを追加することによってグラフを拡張する（３２０）こととを行うように構成される。

ノードのコストとは、本明細書において使用されるときは、このノードを通って初期ノードから目標ノードに到達するための最小コストであり、初期木のノードを通る第１の経路の第１のコストと、目標木のノードを通る第２の経路の第２のコストと、初期木のノードと目標木のノードとの間の第３の経路の第３のコストとを含む。様々な実施形態において、ノードのコストは、初期状態からノードの状態を通って目標状態への車両の移動時間と、車両が初期状態からノードの状態を通って目標状態に移動する距離と、車両が初期状態からノードの状態を通って目標状態へ移動する移動モードの変化速度とのうちの１つ又は組み合わせを示す。

いくつかの実施形態は、自由空間から閉塞空間に向かう「内向木」を構築するのではなく、閉塞空間から自由空間への「外向木」を構築することによって、空間を取り囲む障害物をより効率的に回避することができるという、複数のシミュレーション及び実験によって確認された直観に基づいている。自律駐車システムの用途等の複数の用途において、駐車空間は、通常、車庫内又は駐車空間につながる駐車場内の空間よりも閉塞している。そのために、いくつかの実施形態は、二重木空間探索が単一木空間探索よりも計算効率が良く、人間の直観により忠実である可能性があるという認識に基づいている。

いくつかの実施形態は、拡張する初期木又は目標木上のノードを選択することによって二重木空間探索の計算効率性を利用する。具体的には、いくつかの実施形態は、拡張可能ノードのコストに基づいて初期木又は目標木内の拡張可能ノードを選択し、子ノードのコストが拡張可能ノードのコストよりも少なくなるような、無衝突プリミティブ運動によって定義されるエッジを用いて拡張可能ノードに接続された子ノードを追加することによってグラフを拡張する。

図３Ｂは、１つの実施形態による二重木構築を使用してグラフを特定する概略図を示している。この例において、グラフは、初期状態１０１に対応する初期ノード３１１を有する初期木３２１と、目標状態１０２に対応する目標ノード３１２を有する目標木３２２とを使用して双方の端部から構築される。初期木は、ノード集合及びエッジ集合を有する。目標木も、ノード集合及びエッジ集合を有する。

現在の反復の間に、初期木又は目標木からノード３１３がそのコストに基づいて選択される。この例では、ノード３１３は初期木から選択される。例えば、１つの実施態様において、初期木又は目標木からのノードは区別なく選択される。ノード３１３は、初期ノードから初期木の全てのノードの中のノード３１３を通って目標ノードに到達するための最小コストを有する。このノードは、子ノードを用いて拡張される対象であり、したがって、このノードは拡張可能ノードと呼ばれる。

ノード３１３は、無衝突プリミティブ運動によって定義されるエッジを用いて拡張可能ノードに接続される子ノードを追加することによって初期木を拡張することができる当該無衝突運動プリミティブが存在する場合にのみ拡張可能ノードとして選択される。そうでない場合には、ノード３１３は拡張可能でない。そのために、いくつかの実施形態において、運動計画器は、拡張可能ノードを選択するために、複数のノードをそれらのそれぞれのコストの順序で検査し、検査されたノードのうち、選択されたノードのコストを削減する無衝突プリミティブ運動を有する最初のものを拡張可能ノードとして選択するように構成される。

例えば、ノード３１３のコストを削減することができる全てのプリミティブ運動が障害物１０３に至る場合には、ノード３１３は拡張可能でなく、実施形態は、別のノード、例えば、拡張するノード３１３のコストよりも大きなコストを有するノード３３１を選択する。ノード３３１が拡張可能である場合には、すなわち、拡張可能ノード３３１のコストを削減する３３６等の無衝突運動プリミティブが存在する場合には、実施形態は、この無衝突運動プリミティブによって定義されるエッジ３３６を使用して、ノード３３１のコストよりも少ないコストを有する子ノード３３７を初期木３２１に追加する。

次に、いくつかの実施態様において、この実施形態は、目標木を、例えば、ノード３４１から拡張する。例えば、いくつかの実施形態は、初期木及び目標木を交互に、すなわち、代わる代わる構築する。グラフ構築は、終了条件に到達するまで継続する。例えば、１つの実施形態において、運動計画器は、初期木及び目標木が無衝突リードシェップ（Reeds-Shepp）の経路と接続されるときにグラフの構築を停止する。

そのような方法で、木の決定論的選択及び拡張が達成される。加えて、ノードのコストとは、本明細書において使用されるときは、このノードを通って初期ノードから目標ノードに到達するための最小コストであり、初期木のノードを通る第１の経路の第１のコスト（初期状態の周囲の局所的な情報を考慮する）と、目標木のノードを通る第２の経路の第２のコスト（目標状態の周囲の局所的な情報を考慮する）と、初期木のノードと目標木のノードとの間の第３の経路の第３のコスト（初期状態と目標状態との間の関係を考慮する）とを含む。拡張可能ノードのそのような選択によって、双方の木の構築から収集された情報を考慮することが可能になり、グラフ延長がより計算効率の良いものになる。例えば、複数の実験が、二重木グラフ構築が単一木グラフ構築よりも少ないノードを有するグラフを特定することを実証している。

いくつかの実施形態は、ノード選択に加えてノード拡張も決定論的方法で実行され、全ての可能なプリミティブ運動を用いてノードの周囲の空間を探索する必要性を低減することができるという更なる認識に基づいている。そのために、１つの実施形態は、拡張可能ノードのコストを削減する第１の無衝突プリミティブ運動を選択する。計算負担を更に削減するために、いくつかの実施形態は、一組のプリミティブ運動を、これらのプリミティブ運動のそれぞれと、拡張可能ノードにつながるグラフのエッジを定義するプリミティブ運動との類似度に基づいて順序付け、その順序でこれらのプリミティブ運動を検査する。これらの実施形態は、人間のオペレーターが所望するような車両移動の平滑性を促進し、実際面では、構築されるグラフの複雑さを低減するのに役立つ。なぜならば、ノードにつながる運動プリミティブが無衝突である場合には、そのノードを起点とする同じ運動プリミティブも無衝突である可能性があり、その可能性は更に高いからである。

例えば、エッジ３３６は、拡張可能ノード３３１につながるグラフのエッジを定義する同じプリミティブ運動によって作成される。ノード３３１のコストをより一層削減することができる他のプリミティブ運動が存在する場合であっても、最初に検査されたプリミティブ運動がノードを拡張するために選択される。

図３Ｃは、いくつかの実施形態による運動計画方法のための方法のブロック図を示している。車両のジオメトリ、駐車空間のマップ、並びに初期状態及び目標状態から開始して、方法は、最初に、グラフ

を初期化し（３４５）、初期グラフ３５０を出力する。この初期グラフ及びプリミティブ運動３５５に基づいて、優先される制御動作を使用したノード選択及びノード拡張の２つのステップを繰り返すことによってグラフが構築される（３６０）。その結果、初期状態と目標状態とを接続するか又は目標状態のアトラクション領域（attraction region）内に成長するグラフ３６５が取得され、運動学的又は動的な経路３７５を求める（３７０）ために使用される。グラフは、目標ノードからε距離内にあるノードを含む場合に、目標状態のアトラクション領域内に成長すると言われる。経路は、車両を初期状態から目標状態に走行させる一連のプリミティブ運動を含むことができる。いくつかの実施形態において、基準軌道が、運動学的経路及び車両の動的モデル３８０に基づいて求められる（３８５）。別の実施形態において、基準軌道は、動的モデル３８０に基づいて動的経路３７５を平滑化することによって求められる。基準軌道３９０は制御モジュールに供給され、車両の運動は、車両の現在の状態等の検知された信号３９１に基づいて、車両運動が基準軌道に追従するように制御される（３９５）。

図４Ａは、１つの実施形態によるグラフ構築方法のブロック図を示している。グラフは、初期状態及び／又は目標状態に基づいて初期化され（３４５）、事前に設定されたグラフ成長パラメーターは、いくつかのメトリックの組み合わせに従って最初に調節される（４０１）。例えば、成長パラメーターは、一組の運動プリミティブ、グラフのスパーシティ、ノード拡張のために運動プリミティブを選択する順序のうちの１つ又は組み合わせを含むことができる。これらの成長パラメーターによって、グラフの異なる部分にスパーシティの異なる値を有する不均一に分散したノードを有するグラフを構築することが可能になる。いくつかの実施形態において、運動計画器は、拡張可能ノードのリストの長さに基づいてスパーシティの値を選択する。スパーシティの値は、プリミティブ運動を形成する制御動作の積分時間、及び、子ノードとグラフにおける最も近いノードとの間の許容される最小距離のうちの一方又は組み合わせによって規定される。

いくつかの実施形態において、運動計画器は、拡張可能ノードの１つ以上のリストをメモリに記憶する。全ての拡張可能ノードリストが空である場合には、グラフ構築は停止し、異なる成長パラメーターが選択されるか、又は、方法は失敗を返す。そうでない場合には、拡張可能ノード４４１が拡張可能ノードリストから選択される（４０２）。優先される制御動作が、ノード拡張を遂行するために適用される場合には、拡張可能ノード４４１は更に拡張される（４０３）。駐車空間のマップに基づいて、ノード拡張４０３は、場合に応じて、拡張可能ノードの無衝突子ノードのいずれも特定しないか、これらの無衝突子ノードのうちの１つを特定するか、又は一組のこれらの無衝突子ノードを特定し、したがって、拡張可能ノードとその子ノードとの間の無衝突エッジのいずれも特定しないか、これらの無衝突エッジのうちの１つを特定するか、又は一組のこれらの無衝突エッジを特定する。これらの無衝突子ノード及びエッジはグラフに追加される。構築アルゴリズムは、新たに構築されるグラフが或る特定の停止基準を満たしている場合には終了し、そうでない場合には調節４０１、選択４０２、及び拡張４０３を繰り返す。

図４Ｂは、いくつかの実施形態によるグラフを表すデータ構造の見本例を示している。例えば、１つの実施形態において、グラフ

は、少なくともノードの集合４３１及びエッジの集合４３２を含み、ノードは、車両Ｘの或る特定の無衝突状態に基づいて作成される。エッジは、２つのノードの間の無衝突の運動学的／動的に実現可能な経路を表す。１つの実施形態において、グラフは、少なくとも拡張可能ノードのリスト

４２１及び木

４２２を含む。グラフ初期化３４５は、初期状態に基づいて初期ノードを作成し、初期ノードを拡張可能リスト

に追加する。混乱を引き起こすことなく表記を簡単にするために、本明細書では、ノード及び状態が区別なく使用される。

１つの実施形態において、状態のアトラクション領域は、この状態から或る特定の距離内の全ての可能な状態を含むその近傍である。プリミティブ運動は、既定の制御動作Ａ_ｉ及びその積分時間ｔ_ｆｉの対、又は対（Ａ_ｉ，ｔ_ｆｉ）を車両モデルに適用することから得られる一連の状態によって表される。いくつかの実施形態において、加速度等の制御動作が車両の動的モデルに適用され、状態が取得される。別の実施形態において、速度及びステアリング角等の制御動作が車両の運動学的モデルに適用され、対応する状態が生成される。

いくつかの実施形態において、車両の既定の制御動作は、ステアリング角及び縦速度を合成したものである。簡単にするために、ステアリング角及び縦速度を、双方が［－１，１］内の値を取るように正規化することにする。その結果、車両の制御動作は、領域［－１，１］×［－１，１］にわたって規定される。本発明において教示される方法を適用するために、領域は、既定の制御動作を生成するように近似する必要がある。１つの実施形態において、領域の近似は、ステアリング角の範囲［－１，１］をいくつか（例えば５つ）の可能な動作｛－１，－０．５，０，０．５，＋１｝に離散化することによって取得される一方、縦速度の範囲は、いくつか（例えば２つ）の可能な動作｛－１，＋１｝に離散化される。離散化によって、既定の制御動作を含む既定の動作集合が誘導される。例えば、１つの実施形態において、既定の動作集合Ａは、１０個の要素、すなわち、｛（－１，１），（－０．５，１），（０，１），（＋０．５，１），（１，１），（－１，－１），（－０．５，－１），（０，－１），（＋０．５，－１），（１，－１）｝を含む。既定の制御動作Ａ_ｉは、Ａの任意の要素とすることができる。

同様に、積分時間は、有限の正の実数である。１つの実施形態において、積分時間も、その領域が［０，１］となるように正規化される。例えば、１つの実施形態は、その領域をいくつかの値に離散化する。例えば、領域は、５つの要素の有限集合ＴＦ＝｛０．２，０．４，０．６，０．８，１｝に均一に離散化され、ｔ_ｆｉの値は、有限集合ＴＦから選択される。

動作集合Ａ及び積分時間集合ＴＦが与えられると、運動プリミティブは、（Ａ_ｉ，ｔ_ｆｉ）の対に関連付けられる。いくつかの実施形態において、制御動作の集合は、以下のように事前に定義され、

積分時間の集合は、以下のように表される。

したがって、第ｉのプリミティブ運動が対（Ａ_ｉ，ｔ_ｆｉ）に対応する、

のプリミティブ運動が定義される。

いくつかの実施形態において、各動作Ａ_ｉは同じ積分時間を有することができる。すなわち、ＴＦは１つの要素しか含まない。運動プリミティブは、それに応じて定義することができる。

別の実施形態において、グラフは、初期木

４２６の拡張可能ノードのリスト

４２５と、目標木

４２８の別の拡張可能ノードリスト

４２７とを含む。グラフ初期化３４５は、目標状態に基づいて目標ノードを作成し、初期ノード及び目標ノードをそれぞれリスト

及び

に追加する。

図４Ｃは、１つの実施形態によるノードのデータ構造を示している。ノード４４１は、状態４４１ａ、ノードコスト４４１ｂ、運動プリミティブの衝突確率４４１ｃ、運動プリミティブの優先度４４１ｄ、及び最も高い優先度を有する運動プリミティブ４４１ｅの特性を有する。例えば、状態４４１ａは車両の構成又はロケーションを記述し、コスト４４１ｂは、そのノードを通過し初期状態及び目標状態を接続する経路の推定コストを記述し、運動プリミティブの衝突確率は、この運動プリミティブが衝突をもたらす推定尤度を指定し、優先度４４１ｄは、どのプリミティブがコストの削減をもたらす可能性が最も高いのかを決定する。

いくつかの実施形態において、衝突確率は、０と１との間の連続領域上の実数値を取り、優先度確率は、境界のある連続領域上の実数値を取る。別の実施形態において、衝突確率は、２つの要素｛０，１｝を含む有限集合からの離散値を取り、優先度は、２つの要素｛０，１｝を含む有限集合からの離散値を取る。

いくつかの実施形態において、新たな子ノードが、プリミティブを親に適用することによって生成されると、そのノードは、運動プリミティブの衝突確率及び優先度をその親から最初に継承する。親ノードが、運動プリミティブを適用することによって拡張された後、適用されたプリミティブの衝突確率及び優先度は、そのプリミティブの結果に従って更新される。例えば、プリミティブが、新たなノード、及び、新たなノードと親ノードとの間の衝突した経路をもたらす場合には、親ノードのプリミティブの衝突確率は増加される。極端な場合には、衝突確率は１に設定される。プリミティブが、親ノードよりも低いコストを与える新たなノードをもたらす場合には、親ノードのプリミティブの優先度は上昇し、そうでない場合には低下する。いくつかの場合には、優先度は、新たなノードのコストに応じて１に上昇するか又は０に低下する。そのような方法で、各拡張可能ノードの運動プリミティブの確率的推定を利用する決定論的手法は、グラフ構築の計算複雑さを低減する。

１つの実施形態において、運動プリミティブの優先度は、親ノードのコストと新たなノードのコストとの間の差に直線的に比例する量だけ上昇又は低下する。この実施形態は、親の運動プリミティブに対する他の運動プリミティブの相対的な優先度を上昇させ、決定論的グラフ拡張における種々の選択肢をより良好に考慮する。

いくつかの実施形態は、決定論的グラフ構築３６０が拡張可能ノードの数の削減をもたらすという認識に基づいている。さらに、ノード拡張の削減は、構築されるグラフのスパーシティを増加させ、これは、一方で計算効率を改善し、他方で空間探索の選択肢を削減する。決定論的ノード拡張によって生み出されるこの問題に対処するために、いくつかの実施形態は、グラフの異なる部分においてグラフのスパーシティを変更する。例えば、１つの実施態様は、潜在的に拡張可能なノードのリストを運動計画器のメモリに保持し、拡張可能ノードのリストの長さに基づいてスパーシティの値を選択する。例えば、スパーシティの値は、プリミティブ運動を形成する制御動作の積分時間、及び、拡張可能ノードの子ノードとグラフにおける最も近いノードとの間の許容される最小距離のうちの一方又は組み合わせによって規定される。

図５Ａは、いくつかの実施形態の原理による、グラフ３６５の単一木

のグラフ成長パラメーター４０１を調節する方法のフローチャートを示している。車両状態の近傍を定義するスパーシティの値が、拡張可能ノードリストの長さに従って（５０２）調節される（５０３）。詳細には、拡張可能ノードリスト

の長さが或る特定の閾値（下限）を下回る場合には、車両状態の近傍までの距離を規定するスパーシティは、グラフ構築の早期終了（経路が実現可能でないことを意味する）を回避するために低減される。いくつかの実施形態において、スパーシティは比例して再帰的に低減される。ノードの長さがもう１つの閾値（上限）を上回る場合には、スパーシティは比例して再帰的に増加される。スパーシティは、ノードリストの長さが下限と上限との間にあるときは定数を維持する。

図５Ｂは、いくつかの実施形態による、二重木グラフ

、

のグラフ成長パラメーター４０２を調節する方法のフローチャートを示している。これらの実施形態のいくつかの原理は、図５Ａのスパーシティ調整と同様である。ブロック４０２は、双方の拡張可能ノードリスト

が空である場合にのみ失敗を返す。各木は、そのそれぞれの拡張可能ノードリストのステータスに応じて、同じ値又は異なる値の成長パラメーターを有することができる。

図５Ｃは、いくつかの実施形態による拡張可能ノードのリストから拡張可能ノードを選択する（４０２）方法のブロック図である。通常、いくつかの実施形態は、拡張可能ノードリストにおいて最も低い推定コストを有するノードを選択する。拡張可能ノードが完全に拡張されている（全ての制御動作が試行済みである）場合には、このノードは、リストから削除され（５２３）、アルゴリズムは、新たな拡張可能ノードリストから最も低いコストのノードを見つけるべきである。

図５Ｄは、１つの実施形態によるノード拡張４０３のブロック図を示している。選択された拡張可能ノードＸ_ｂｅｓｔを用いて、高い優先度を有する制御動作Ａ_ｉ又は制御動作のサブセット

が選択され（５４１）、適用されて、選択された制御動作に対応するプリミティブ運動を使用して子ノード候補Ｘが特定される（５４２）。いくつかの実施形態において、子ノード候補は、初期条件Ｘ_ｂｅｓｔ及び選択された制御動作Ａ_ｉを用いて既定の期間［０，ｔ_ｆｉ］にわたって車両モデルを積分することによって取得することができる。ｔ_ｆｉは、制御動作が異なれば異なる可能性があることに留意されたい。

いくつかの実施形態において、プリミティブ運動は、車両モデルに適用される制御動作Ａ_ｉ及び期間ｔ_ｆｉを指定する。プリミティブ運動は、対（Ａ_ｉ，ｔ_ｆｉ）、又は対応する制御動作及び期間から得られる一連の状態のいずれかの形でメモリに記憶される。一連の状態は、０の初期条件及び対応するプリミティブ運動を用いて車両の運動学的モデル又は動的モデルを積分することによって取得される。

図５Ｄに示されるように、Ｘ_ｂｅｓｔとＸとの間のプリミティブ運動の結果としての経路が、マップ情報１３０を使用して無衝突である場合にのみ、子ノード候補Ｘ及びエッジ（Ｘ_ｂｅｓｔ，Ｘ）が、グラフのノード集合及びエッジ集合にそれぞれ追加される。子ノードＸのノードコストがＦ（Ｘ）として推定され、このノードコストがＸ_ｂｅｓｔのノードコストよりも低いか否かがチェックされる（５４４）。低い場合には、ノード拡張は終了し、そうでない場合、すなわち、Ｘ_ｂｅｓｔが最も低いノードコストをまだ有する場合には、別の制御動作又は制御動作のサブセットが、ノードＸ_ｂｅｓｔの拡張を継続するために選択される。

図５Ｅは、反復グラフ構築を終了するためにいくつかの実施形態によって使用されるいくつかの停止基準４０４のブロック図を示している。１つの実施形態において、追加された子ノードがＸ_ｆに十分近い（Ｘ_ｆの既定の近傍に到達する）か否かをチェックする検査が行われる（５５１）。別の実施形態において、追加された子ノードとＸ_ｆとの間の標準的なリードシェップの経路の構築を試み（５５２）、このリードシェップの経路が無衝突であるか否かを検証する（５５２）。無衝突である場合には、このリードシェップの経路がグラフのエッジ集合に追加され、グラフ構築は停止する。グラフ構築は、ノードの数が閾値を越えた場合に終了することができ、その場合に、方法はＸ_０とＸ_ｆとの間の実現可能な経路を見つけることができない。

いくつかの実施形態は、木の決定論的選択及び拡張を行うために、二重木グラフ構築を利用することによって拡張するノードのコストを求める。いくつかの実施形態において、ノードのコストは、このノードを通って初期ノードから目標ノードに到達するための最小コストであり、初期木のノードを通る第１の経路の第１のコスト（初期状態の周囲の局所的な情報を考慮する）と、目標木のノードを通る第２の経路の第２のコスト（目標状態の周囲の局所的な情報を考慮する）と、初期木のノードと目標木のノードとの間の第３の経路の第３のコスト（初期状態と目標状態との間の関係を考慮する）とを含む。拡張可能ノードのそのような選択によって、双方の木の構築から収集された情報を考慮することが可能になり、グラフ延長がより計算効率の良いものになる。例えば、複数の実験が、二重木グラフ構築が単一木グラフ構築よりも少ないノードを有するグラフを特定することを実証している。

いくつかの実施形態において、グラフは、２つの木

及び

を含み、前者は根ノードＸ_０を有し、後者は根ノードＸ_ｆを有する。木

及び

は、５１１において拡張のために交互に選ばれる。すなわち、ブロック５１１は、リスト

及びリスト

を交互に選ぶことによって拡張可能ノードリストを選択する。いくつかの実施形態において、現在のノードＸについて、その到着コスト（第１のコスト）及び推定残余コスト（cost-to-go）（第２のコスト）が別々に求められる。

１つの実施形態において、適切に定義された以下の式の関数Ｆ（・）を通じて各ノード（状態）Ｘにコストが割り当てられる。

ただし、ｇ（Ｘ_０，Ｘ）は、Ｘ_０からＸへの初期ノード（状態）からの到着コスト、すなわち、第１のコストを表し、ｈ（Ｘ，Ｘ_ｆ）は、ＸからＸ_ｆへの推定残余コスト、すなわち、第２のコストを表す。ノードＸのＦ値は、このノードを通過している間のＸ_０からＸ_ｆの可能性のある経路の推定コストである。

１つの実施形態において、到着コストｇ（Ｘ_０，Ｘ）は、Ｘ_０からＸへの全てのエッジのコストを合計したものであり、エッジのコストは、Ｘ_０とＸとの間の無衝突の運動学的／動的に実現可能な経路の長さを表す経路コストｌ（Ｘ_０，Ｘ）である。別の実施形態において、到着コストは、Ｘ_０からＸへの経路に沿った制御動作の変更にペナルティを科す制御コストｃ（ｕ（Ｘ_０），ｕ（Ｘ））を更に含み、ｕ（Ｘ）は、Ｘにつながる動作を表す。

図６Ａは、１つの実施形態による、目標木が目標ノードのみを含むときのノードＸのコストＦ（Ｘ）を求める方法の概略図を示している。図６Ａにおいて、Ｘ_０とＸ_ｆとの間の経路は、エッジ（Ｘ_０，Ｘ_１）、．．．、（Ｘ_ｂｅｓｔ，Ｘ）によって表され、したがって、経路コストは、これらのエッジの長さによって正確に求めることができる。すなわち、以下の式となる。

同様に、制御コストは以下の式となる。

いくつかの実施形態において、残余コストｈ（Ｘ，Ｘ_ｆ）は、ＸとＸ_ｆとの間の無衝突の運動学的／動的に実現可能な経路の長さを表す経路コストｌ（Ｘ，Ｘ_ｆ）を正確に捕捉する。構築されたグラフは目標Ｘ_ｆに到達していないので、ＸをＸ_ｆに接続する一連のエッジを見つけることはできず、これは、ｌ（Ｘ，Ｘ_ｆ）が未知であることを意味する。したがって、推定される残余コストｈ（Ｘ，Ｘ_ｆ）がｌ（Ｘ，Ｘ_ｆ）の代わりとなる。いくつかの実施態様において、ｈ（Ｘ，Ｘ_ｆ）は、障害物を考慮しない、ＸとＸ_ｆとの間の運動学的／動的に実現可能な経路を表す推定経路コストｌ_ｅ（Ｘ，Ｘ_ｆ）、及び、障害物が推定経路コストに与える影響を考慮する衝突コストｌ_ｃ（Ｘ，Ｘ_ｆ）である。

例えば、ＸとＸ_ｆとの間の運動学的／動的に実現可能な経路６０１及びそのコストｌ_ｅ（Ｘ，Ｘ_ｆ）が存在すると仮定する。実際、経路６０１は円形の障害物１０３と衝突しており、したがって、ｌ_ｅ（Ｘ，Ｘ_ｆ）は真の残余コストではない。代わりに、真の残余コストは、ｌ_ｅ（Ｘ，Ｘ_ｆ）よりも高い残余コストを有する経路６０２に対応する。ｌ_ｃ（Ｘ，Ｘ_ｆ）は、差ｌ（Ｘ，Ｘ_ｆ）－ｌ_ｅ（Ｘ，Ｘ_ｆ）を近似するために使用される。

いくつかの実施形態において、ｌ_ｅ（Ｘ，Ｘ_ｆ）は、｜Ｘ_ｆ－Ｘ｜_ｐによって表されるベクトルＸ－Ｘ_ｆのｐ－ノルムに従って計算される。別の実施形態において、ｌ_ｅ（Ｘ，Ｘ_ｆ）は、ＸとＸ_ｆとの間のリードシェップ経路の長さである。いくつかの実施形態において、ｌ（Ｘ，Ｘ_ｆ）－ｌ_ｅ（Ｘ，Ｘ_ｆ）は、衝突メトリックｆ_ｃ（・）の単調関数である。この衝突メトリックは、各障害物の全ての頂点がガウスモデルを構築するために使用されるグループガウス混合モデルとして構築することができる。ノードの場合には、その衝突メトリックはｆ_ｃ（Ｘ）として評価することができる。ｆ_ｃ（Ｘ）が大きいほど、ＸとＸ_ｆとの間の中間経路（neutral path）は衝突に終わる可能性が高くなる。

図６Ｂは、１つの実施形態による、目標木が複数のノードを含むときのノードＸのコストＦ（Ｘ）を求める方法の概略図を示している。図６Ｂに示されるように、Ｘ_０を拡張することによって、下付き文字ｓを有するノードの木

が得られるのに対して、Ｘ_ｆを拡張することによって、下付き文字ｇを有するノードの木

が得られる。

が成長のために選択され（５１１）、具体的には、ノードＸ_{ｂｅｓｔｇ}が、

の中で最も低いコストを有するので、拡張のために選択されると仮定する。制御動作Ａ_ｇを用いたその拡張の結果、子ノードＸ_ｇが得られる。このノードＸ_ｇのコストは、以下の式のように、その到着コスト及び推定残余コストの合計によって与えられる。

推定残余コストは、Ｘ_ｇとＸ_０との間の経路コストを近似したものであることに留意されたい。

いくつかの実施形態において、ｈ（Ｘ_ｇ，Ｘ_０）は、上記のようにｌ_ｅ（Ｘ_ｇ，Ｘ_０）及びｌ_ｃ（Ｘ_ｇ，Ｘ_０）と近似される。別の実施形態において、ｈ（Ｘ_ｇ，Ｘ_０）は、以下のように近似される。

ただし、Ｘ_{ｂｅｓｔｓ}は、最も低いコストを有するリスト

のノードであり、ｇ（Ｘ_０，Ｘ_{ｂｅｓｔｓ}）は、Ｘ_０からＸ_{ｂｅｓｔｓ}への到着コスト（第３のコスト）であり、正確に判明しており、ｈ（Ｘ_ｇ，Ｘ_{ｂｅｓｔｓ}）＝ｌ_ｅ（Ｘ_ｇ，Ｘ_{ｂｅｓｔｓ}）＋ｌ_ｃ（Ｘ_ｇ，Ｘ_{ｂｅｓｔｓ}）は、Ｘ_ｇとＸ_{ｂｅｓｔｓ}との間の残余コストの推定値である。推定される残余コストのこの定義は、以前に推定された残余コストｈ（Ｘ_ｇ，Ｘ_０）＝ｌ_ｅ（Ｘ_ｇ，Ｘ_０）＋ｌ_ｃ（Ｘ_ｇ，Ｘ_０）を上回る大きな利点を有する。なぜならば、前者は、Ｘ_ｇとＸ_{ｂｅｓｔｓ}との間の経路コストしか推定しないからである。

１つの実施形態において、Ｘ_{ｂｅｓｔｓ}は、Ｘ_ｇに最も近い木

のノードである。別の実施形態において、Ｘ_{ｂｅｓｔｓ}は、以下の最適化問題を解くことによって取得されるノードリスト

である。

ただし、Ｘ_{ｎｅａｒｓ}は、以下のように定義される集合である。

γは正の有限定数であり、ｄ（Ｘ_ｉ，Ｘ_ｊ）は任意の２つのノードＸ_ｉ及びＸ_ｊの間の距離関数であることに留意されたい。Ｘ_{ｎｅａｒｓ}は、Ｘ_ｇからのγ距離内にある

上の全てのノードを含む。Ｘ_{ｂｅｓｔｓ}は、最小推定残余コストｈ（Ｘ_０，Ｘ_ｇ）を与える木

上のノードを表す。

図６Ｂに示されるように、Ｘ_ｇとＸ_{ｂｅｓｔｓ}との間の運動学的／動的に実現可能な経路６１２は、より高い確率で、Ｘ_０とＸ_ｇとの間の運動学的／動的に実現可能な経路６１１よりも短く（したがって、ｌ_ｅ（Ｘ_ｇ，Ｘ_{ｂｅｓｔｓ}）によってより良好に近似することができる）、したがって、Ｘ_{ｂｅｓｔｓ}とＸ_ｇとの間の運動学的／動的に実現可能な経路は、障害物と衝突する可能性がはるかに低く、残余コストのはるかに良好な推定値を与える。同様に、木

上のノードＸ_ｓのコストは、以下のように推定することができる。

ただし、Ｘ_{ｂｅｓｔｇ}は、最も低いコストを有するリスト

のノードであり、ｇ（Ｘ_{ｂｅｓｔｓ}，Ｘ_ｆ）は、Ｘ_ｆからＸ_{ｂｅｓｔｇ}への到着コスト（第３のコスト）であり、正確に判明しており、ｈ（Ｘ_ｓ，Ｘ_{ｂｅｓｔｇ}）は、Ｘ_ｓからＸ_{ｂｅｓｔｇ}への推定残余コストである。

１つの実施形態において、Ｘ_{ｂｅｓｔｇ}は、Ｘ_ｓに最も近い木

のノードである。別の実施形態において、Ｘ_{ｂｅｓｔｇ}は、以下の最適化問題を解くことによって取得されるノードリスト

である。

ただし、Ｘ_{ｎｅａｒｇ}は、

であるような

上のノードの集合である。Ｘ_{ｎｅａｒｇ}は、Ｘ_ｓからγ距離内にある

上の全てのノードを含む。Ｘ_{ｂｅｓｔｇ}は、最小推定残余コストｈ（Ｘ_ｓ，Ｘ_ｆ）を与える木

上のノードを表す。

図６Ｃは、異なる実施形態の利点を示す例の概略図を示している。これらの例において、車両は、種々の障害物１０３を含む環境内で初期状態１０１から目標状態１０２に移動する必要がある。制御動作の優先を行わない方法は、シミュレーションによれば、６３３９個のノードを含む密で複雑な木６２１をもたらす。優先される運動プリミティブに基づく決定論的ノード拡張を有する実施形態は、２４６５個のノードのみを有するより単純な木６２２を用いて所望の運動を達成する。二重木構築を使用する実施形態は、１３２５個のノードのみからなるより一層単純な木６２３を用いて所望の運動を達成する。

図７Ａは、いくつかの実施形態による一組の制御動作

及び積分時間

に関連したプリミティブ運動を示す概略図を示している。車両モデルの初期条件Ｘ_ｏ７０１が与えられると、或る曲率を有する左折移動に対応するプリミティブ運動７５１を適用することによって、子ノード７１１が得られる。同様に、異なる移動、例えば、直進移動又は異なる曲率を有する旋回移動に対応するプリミティブ７５ｉを適用することによって、子ノード７１ｉが得られる。一組の全ての制御動作

及び一組の積分時間

は、所与の拡張可能ノードの全てのノード候補の間の距離がグラフのスパーシティを定義する或る特定の閾値δよりも少なくとも大きくなるように設計される。いくつかの実施形態において、状態Ｘ_ｉ及びＸ_ｊに対応する２つのノードの間の距離関数は、ベクトルＸ_ｉ-Ｘ_ｊの重み付きｐ－ノルム｜Ｘ_ｉ-Ｘ_ｊ｜_ｐと定義される。

いくつかの実施形態において、運動学的車両モデルは、プリミティブ運動に基づいてノード候補を生成するために使用され、プリミティブ運動は、制御動作Ａ_ｉ：（ｓ_ｉ，ｖ_ｉ）及び積分時間ｔ_ｆｉを含む。具体的には、ｓ_ｉ及びｖ_ｉは、それぞれ車両ハンドルのステアリング角及び車両の縦速度である。いくつかの実施形態において、ｓ_ｉ，

は、［－１，１］にわたって一様に分布し、ｖ_ｉは、｛－１，＋１｝上の値しか取らない。

一方、動的車両モデルは、プリミティブ運動に基づいてノード候補を生成するために使用することができ、プリミティブ運動は、制御動作Ａ_ｉ：（ａ_ｓｉ，ａ_ｖｉ）及び積分時間ｔ_ｆｉを含む。具体的には、ａ_ｓｉ及びａ_ｖｉは、それぞれ車両ハンドルのステアリング速度及び車両の縦加速度である。

いくつかの実施形態は、ノード選択に加えて、決定論的方法でノード拡張を実行して、全ての可能なプリミティブ運動を有するノードの周囲の空間を探索する必要性を低減する。そのために、一実施形態は、拡張可能ノードのコストを削減する第１の無衝突プリミティブ運動を選択する。すなわち、後続のプリミティブ運動の検査は行われない。計算負担を更に削減するために、いくつかの実施形態は、一組のプリミティブ運動を、これらのプリミティブ運動のそれぞれと、拡張可能ノードにつながるグラフのエッジを定義するプリミティブ運動との類似度に基づいて順序付け、その順序でこれらのプリミティブ運動を検査する。これらの実施形態は、人間のオペレーターが所望するような車両移動の平滑性を促進し、実際面では、構築されるグラフの複雑さを低減するのに役立つ。

図７Ｂは、ノードを拡張するためにいくつかの実施形態によって使用されるプリミティブ運動の順序付きリスト７２０を示している。いくつかの実施形態は、全ての可能なプリミティブ運動を検査する必要はなく、拡張したいのは、無衝突でありかつノードのコストを削減するプリミティブ運動のみであるという理解に基づいている。加えて、１つの制御動作が、親ノードのコストを削減する場合には、同様の制御動作も、ほぼ確実に子ノードのコストを削減する。そのために、親ノードの前方右プリミティブ運動が無衝突である場合には、この親ノードにつながる制御動作に対する類似度によって統制される順序で運動プリミティブの検査を開始することは理にかなっている。

そのために、１つの実施形態は、拡張可能ノードにつながるエッジのプリミティブ運動から開始して、プリミティブ運動を検査する。それに加えて、又はその代わりに、１つの実施形態は、グラフの空間認識を高めるために、拡張可能ノードにつながるエッジのプリミティブ運動を含む走行モードのプリミティブ運動から開始して、プリミティブ運動を検査する。例えば、走行モードは、前方走行モード又は後方走行モードとすることができ、前方走行モードは、前方方向における直進移動、右折移動、及び左折移動を定義する前方プリミティブ運動を含み、後方走行モードは、後方方向における直進移動、右折移動、及び左折移動を定義する後方プリミティブ運動を含む。それに加えて、又はその代わりに、走行モードは、直進走行モード又は旋回走行モードとすることができ、旋回走行モードは、車両の移動を異なる曲率を用いて旋回させる異なるプリミティブ運動を含む。

図８Ａは、１つの実施形態による、選択されたノードＸ_ｂｅｓｔを拡張する運動プリミティブ又は運動プリミティブのサブセットを特定する（５４１）方法のブロック図を示している。この実施形態は、未試行の運動プリミティブと、未試行の運動プリミティブの優先度８０３と、未試行の運動プリミティブの衝突確率８０４とに従って運動プリミティブ又は運動プリミティブのサブセットを選択する（８０２）。情報８０３及び８０４は、図４Ｃに示されるようなノードの特性４４１ｃ及び４４１ｄから容易に推論することができる。

図８Ｂは、別の実施形態による、拡張可能ノードＸ_ｂｅｓｔの拡張中に運動プリミティブを選択する方法のブロック図を示している。この実施形態は、未試行の運動プリミティブ８０１の中から運動プリミティブを選択する（８１２）。選択される運動プリミティブは、最も高い優先度を有する運動プリミティブ４４１ｅとの間の距離が最小であるものである。この規則は、最も高い優先度を有する運動プリミティブ４４１ｅが、最も低いコストを有する子ノードを生成する可能性がより高いという認識に基づいている。すなわち、最も高い優先度は、全ての試行済みの運動プリミティブの中で最も低いノードコストを与える運動プリミティブに割り当てられる。試行済みの運動プリミティブがない場合には、Ｘ_ｂｅｓｔの最も高い優先度を有する運動プリミティブ４４１ｅがその親ノードから継承される。

ノードが木の根ノードである場合には、全ての運動プリミティブの衝突確率は０であり（全ての運動プリミティブが無衝突であると仮定されることを意味する）、運動プリミティブの優先度４４１ｄは１に初期化される（全ての運動プリミティブが同じ優先度を有することを意味する）。子ノードの場合に、その優先度及び衝突確率は、最初は、その親ノードから継承される。

いくつかの実施形態において、拡張可能ノードの運動プリミティブの衝突確率は、その子ノード候補の衝突チェックの結果に従って更新される（５４３）。同様に、拡張可能ノードの運動プリミティブの優先度は、試行済みの運動プリミティブから得られるノードコストに従って更新される（５４４）。例えば、拡張可能ノードＸ_ｂｅｓｔの全ての子ノードの中で、運動プリミティブの優先度は、結果として得られた子ノードのコストの単調関数と定義される。すなわち、子ノードのコストが低いほど、運動プリミティブの優先度は高くなる。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Claims

車両の移動を該車両の初期状態及び該車両の目標状態から制御するシステムであって、
前記車両の状態を定義する複数のノードを有するグラフを構築することであって、前記ノードは、前記車両の前記初期状態を定義する初期ノードと、前記車両の前記目標状態を定義する目標ノードとを含み、前記グラフ内の各ノード対はエッジと接続され、該エッジは、前記接続されたノードの前記状態の間で前記車両を移動させる無衝突プリミティブ運動のうちの１つによって定義されることと、
前記初期ノードを前記目標ノードと接続する前記グラフを通る経路であって、前記車両を前記初期状態から前記目標状態に移動させる一連のプリミティブ運動を定義するような経路を特定することと、
前記一連のプリミティブ運動を使用して前記車両の前記移動を制御するように構成される前記車両のコントローラーに前記一連のプリミティブ運動を送信することと、
を行うように構成されるプロセッサを備える運動計画器を備え、
前記運動計画器は、前記初期ノードを起点とするノードの初期木を構築することと、前記目標ノードを起点とするノードの目標木を構築することとによって前記グラフを特定するように構成され、前記初期木又は前記目標木を構築するために、前記運動計画器は、前記初期木又は前記目標木内の拡張可能ノードを該拡張可能ノードのコストに基づいて選択し、無衝突プリミティブ運動によって定義されるエッジを用いて前記拡張可能ノードに接続される子ノードのコストが前記拡張可能ノードの前記コストよりも小さくなるような前記子ノードを追加することによって前記グラフを拡張するように構成され、前記ノードのコストは、該ノードを通って前記初期ノードから前記目標ノードに到達するための最小コストであり、前記初期木のノードを通る第１の経路の第１のコストと、前記目標木のノードを通る第２の経路の第２のコストと、前記初期木のノードと前記目標木のノードとの間の第３の経路の第３のコストとを含み、
前記プリミティブ運動は、前記車両の制御動作および前記制御動作の積分時間を含んで形成される、システム。
前記ノードの前記コストは、前記初期状態から前記ノードの前記状態を通って前記目標状態への前記車両の移動時間と、前記車両が前記初期状態から前記ノードの前記状態を通って前記目標状態に移動する距離とのうちの１つ又は組み合わせを示す、請求項１に記載のシステム。
前記運動計画器は、前記拡張可能ノードを選択するために、複数のノードをそれらのそれぞれのコストの順序で検査し、前記検査されたノードのうち、選択されたノードの前記コストを削減する前記無衝突プリミティブ運動を有する最初のものを前記拡張可能ノードとして選択するように構成される、請求項１に記載のシステム。
前記運動計画器は、前記拡張可能ノードを拡張するために、
一組のプリミティブ運動を、該一組内の該プリミティブ運動のそれぞれと、前記拡張可能ノードにつながる前記グラフのエッジを定義するプリミティブ運動との類似度に基づいて順序付けて、順序付きの一組のプリミティブ運動を生成することと、
前記順序付きの一組のプリミティブ運動から、前記拡張可能ノードの前記コストを削減する第１の無衝突プリミティブ運動を選択することと
前記第１の無衝突プリミティブ運動を用いて前記グラフを拡張することと、
を行うように構成される、請求項１に記載のシステム。
前記運動計画器は、前記拡張可能ノードにつながるエッジのプリミティブ運動から開始して、前記プリミティブ運動を検査する、請求項４に記載のシステム。
前記運動計画器は、最も高い優先度を有する前記拡張可能ノードのプリミティブ運動から開始して、前記プリミティブ運動を検査する、請求項４に記載のシステム。
前記拡張可能ノードの前記プリミティブ運動の前記優先度は、前記プリミティブ運動が、前記拡張可能ノードよりも低いコストを有する新たな子ノードにつながる場合には上昇し、そうでない場合には低下する、請求項６に記載のシステム。
前記優先度の前記上昇又は前記低下は、前記拡張可能ノード及び前記新たな子ノードのコストの差に直線的に比例する、請求項７に記載のシステム。
前記拡張可能ノードの前記プリミティブ運動の前記優先度は、前記プリミティブ運動が前記拡張可能ノードよりも低いコストを有する新たな子ノードをもたらす場合には１に設定され、そうでない場合には、前記拡張可能ノードの前記プリミティブ運動の前記優先度は、前記１より前記優先度の低い０に設定される、請求項６に記載のシステム。
前記グラフの前記ノードは、前記グラフの異なる部分においてスパーシティの異なる値を用いて不均一に分散される、請求項１に記載のシステム。
前記運動計画器は、拡張可能ノードのリストの長さに基づいて前記スパーシティの値を選択し、前記スパーシティの前記値は、前記プリミティブ運動を形成する前記制御動作の積分時間、及び、前記子ノードと前記グラフにおける最も近いノードとの間の許容される最小距離のうちの一方又は組み合わせによって規定される、請求項１０に記載のシステム。
前記第２の経路は、その終了ノードを前記目標木の拡張可能ノードリストから選択することによって特定され、前記終了ノードは最も低いコストを有する、請求項１に記載のシステム。
前記第２の経路は、その終了ノードを前記目標木から選択することによって特定され、前記終了ノードは、前記第１の経路の終了ノードに最も近い、請求項１に記載のシステム。
前記第２の経路は、前記第２のコスト及び前記第３のコストの合計が最小になるようにその終了ノードを前記目標木から選択することによって特定される、請求項１に記載のシステム。
前記運動計画器は、前記初期木及び前記目標木が無衝突リードシェップの経路によって互いに接続されるときに前記グラフの構築を停止する、請求項１に記載のシステム。
前記目標状態は、前記車両の駐車空間を規定する、請求項１に記載のシステム。
前記車両の前記初期状態及び前記車両の前記目標状態を推定する少なくとも１つのセンサーを備えるナビゲーションシステム、
を更に備える、請求項１に記載のシステム。
車両の移動を該車両の初期状態及び該車両の目標状態から制御する方法であって、該方法は、該方法を実施する記憶された命令と結合されたプロセッサを使用し、前記命令は、前記プロセッサによって実行されると、
前記車両の状態を定義する複数のノードを有するグラフを構築することであって、前記ノードは、前記車両の前記初期状態を定義する初期ノードと、前記車両の前記目標状態を定義する目標ノードとを含み、前記グラフ内の各ノード対はエッジと接続され、前記エッジは、前記接続されたノードの前記状態の間で前記車両を移動させる無衝突プリミティブ運動のうちの１つによって定義され、前記グラフは、前記初期ノードを起点とするノードの初期木と、前記目標ノードを起点とするノードの目標木とを含み、前記初期木又は前記目標木を構築するために、前記プロセッサは、前記初期木又は前記目標木における拡張可能ノードを該拡張可能ノードのコストに基づいて選択し、無衝突プリミティブ運動によって定義されるエッジを用いて該拡張可能ノードに接続される子ノードのコストが前記拡張可能ノードの前記コストよりも小さくなるような前記子ノードを追加することによって前記グラフを拡張するように構成され、前記ノードのコストは、該ノードを通って前記初期ノードから前記目標ノードに到達するための最小コストであり、前記初期木のノードを通る第１の経路の第１のコストと、前記目標木のノードを通る第２の経路の第２のコストと、前記初期木のノードと前記目標木のノードとの間の第３の経路の第３のコストとを含むことと、
前記初期ノードを前記目標ノードと接続する前記グラフを通る経路であって、前記車両を前記初期状態から前記目標状態に移動させる一連のプリミティブ運動を定義するような経路を特定することと、
前記一連のプリミティブ運動を使用して前記車両の前記移動を制御するように構成される前記車両のコントローラーに前記一連のプリミティブ運動を送信することと、
を含み、
前記プリミティブ運動は、前記車両の制御動作および前記制御動作の積分時間を含んで形成される、該方法のステップを実行する、方法。
方法を実行するプロセッサによって実行可能なプログラムが具現化される非一時的コンピューター可読記憶媒体であって、前記方法は、
車両の状態を定義する複数のノードを有するグラフを構築することであって、前記ノードは、前記車両の初期状態を定義する初期ノードと、前記車両の目標状態を定義する目標ノードとを含み、前記グラフ内の各ノード対はエッジと接続され、前記エッジは、前記接続されたノードの前記状態の間で前記車両を移動させる無衝突プリミティブ運動のうちの１つによって定義され、前記グラフは、前記初期ノードを起点とするノードの初期木と、前記目標ノードを起点とするノードの目標木とを含み、前記初期木又は前記目標木を構築するために、前記プロセッサは、前記初期木又は前記目標木における拡張可能ノードを該拡張可能ノードのコストに基づいて選択し、無衝突プリミティブ運動によって定義されるエッジを用いて該拡張可能ノードに接続される子ノードのコストが前記拡張可能ノードの前記コストよりも小さくなるような前記子ノードを追加することによって前記グラフを拡張するように構成され、前記ノードのコストは、該ノードを通って前記初期ノードから前記目標ノードに到達するための最小コストであり、前記初期木のノードを通る第１の経路の第１のコストと、前記目標木のノードを通る第２の経路の第２のコストと、前記初期木のノードと前記目標木のノードとの間の第３の経路の第３のコストとを含むことと、
前記初期ノードを前記目標ノードと接続する前記グラフを通る経路であって、前記車両を前記初期状態から前記目標状態に移動させる一連のプリミティブ運動を定義するような経路を特定することと、
前記一連のプリミティブ運動を使用して前記車両の前記移動を制御するように構成される前記車両のコントローラーに前記一連のプリミティブ運動を送信することと、
を含み、
前記プリミティブ運動は、前記車両の制御動作および前記制御動作の積分時間を含んで形成される、非一時的コンピューター可読記憶媒体。