JP7345676B2 - 自動または半自動運転車両の適応制御 - Google Patents

Description

本発明は、概して車両の制御に関し、より具体的には自動または半自動運転車両の制御に関する。

自動運転車両は、高度な相互接続されたセンシングおよび制御要素の統合を必要とする複雑な意思決定システムである。最も高いレベルにおいて、ルートプランナーが一連の目的地を道路網を通して計算する。離散意思決定層が、車両のローカルな運転ゴールを決定する役割を担う。各決定は、右折、車線に留まる、左折、または交差点の特定の車線で完全停止する、のうちのいずれかの可能性がある。センシング・マッピングモジュールが、レーダー、ライダー（ＬＩＤＡＲ）、カメラ、および全地球測位システム（ＧＰＳ）情報等の各種センサ情報を、以前の地図情報とともに使用することにより、運転のシナリオに関連する周辺部分を推定する。

モーションプランニングの出力は、車両コントローラへの入力である。モーションプランナーは、センシング・マッピングモジュールからの出力に基づいて、車両が辿るべき安全で望ましくかつ動的に実現可能な軌道を決定する役割を担う。そこで、車両制御アルゴリズムは、この基準モーションを、たとえばステアリング角度、ホイールトルクおよびブレーキ力といったコマンドを出すことにより、比較的高いサンプリング周波数で追跡することを目指す。最後に、アクチュエータ制御層が、アクチュエータを調節することにより、要求されたこれらのコマンドを達成する。

自動運転車両におけるモーションプランニング問題は、多数の類似性を標準的なロボット工学設定と共有し、最適解はほとんどの場合、この問題の非凸性が原因で扱いにくい。直接的な動的最適化に依存する方策が、特殊なシナリオに対して開発されてきた。しかしながら、非凸性が原因で、局所的な最適解にしかならず、これは大域的な最適解からはほど遠いことがあり、場合によっては比較的大きな計算の負荷と時間という代償を払うことになる。モーションプランニングは、高速探索ランダムツリー（ＲＲＴ：rapidly-exploring random tree）等のサンプリングベースの方法、またはＡ^＊、Ｄ^＊およびその他の変形等のグラフサーチ方法を用いて行われることが多い。

いくつかの方法はサンプリングを決定論的に実行するが、米国特許第９，５６８，９１５号に記載されている方法のような他の方法は確率的サンプリングを使用する。サンプリング技術は高速機械計算には適しているが、サンプリング方法によって生成された経路は、自動または半自動運転車両の乗員には不自然に感じられることがある。このため、依然として自動または半自動運転車両の経路プランニングおよび制御を改善する必要がある。

いくつかの実施形態は、低速で長期のサンプリングベースのモーションプランニングアルゴリズムと高速で反応性が高い最適化ベースの予測車両コントローラとを含む、（半）自動運転システムのための統合アーキテクチャを開示する。この統合は、モーションプランニングおよび車両制御層により、競合する複数の目的を達成する責任を、体系的にかつ有効に共有することを可能にし、たとえば、安全性要件の充足を保証するだけでなく、比較的複雑なハイウェイおよび都市での運転のシナリオにおいて信頼できる快適な運転挙動を保証する。

モーションプランナーは、追跡すべき基準軌道の第１のモーメントを計算し、計画された軌道の対応する信頼範囲を規定する高次モーメントを計算する。我々のプランニングアルゴリズムは、たとえば共分散行列に関して、ターゲット軌道とその信頼度の双方を生成し、コントローラが追跡すべきターゲットとして前者のみを用いる訳ではないので、ここでは後者も、具体的には予測コントローラの最適制御問題定式化において時変追跡コスト関数のチューニングに使用する。その結果、競合する複数の目的間のトレードオフに対する自動チューニング機構が得られる。実際、軌道に対応付けられた信頼度は、経路プランナーが、計算した軌道が有効であるとどの程度信じているかを示す。予測コントローラのコスト関数の自動チューニングについての信頼度情報を使用することにより、このような信頼度が低い場合にはより多くの偏差を、このような信頼度が比較的高い場合にはより少ない偏差を許容する。

本発明のいくつかの実施形態は、関係する確率密度関数（ＰＤＦ：probability density function）を近似するための粒子フィルタリングを使用する、モーションプランニングのための確率的方法に基づいている。粒子フィルタに基づいたモーションプランナーは状態軌道のＰＤＦを計算するので、重み付けされた手段よりも高次のモーメントを求めることができる。例として、重み付けされた手段に沿って共分散行列（すなわち第２のモーメント）を計算することにより、予測コントローラにおいて追跡コスト関数を定式化し適応させるために直接使用される所望の基準軌道のガウス近似を求めることができる。

本発明のいくつかの実施形態は、ステージワイズ（stage-wise）最小二乗コスト関数の時変定式化による線形または非線形モデル予測制御（ＭＰＣ：model predictive control）の適応的実現に基づいた車両制御層の実現を使用する。特に、時変追跡重み行列を用いてＭＰＣコスト関数を規定することができる。経路プランナーは計画されたモーション軌道について一次モーメントと高次モーメントの双方を提供するので、この情報を、ステージワイズ最小二乗追跡コスト関数における基準値および重み付け行列の双方のチューニングのために直接使用することができる。

いくつかの実施形態は、モーションプランナーの不確実性とＭＰＣ問題における追跡コストとの間の反比例の関係に基づいており、これは結果として重み行列の時変シーケンスとなる。より具体的には、車両がこの車両の安全性に関する制約に比較的近くなると予測される場合、基準モーションプランの不確実性が増加し、基準軌道からの偏差のペナルティが減少すると予想され、その逆も同様である。そうすると、それぞれ、ＭＰＣ予測状態および／または制御軌道の、その基準、すなわちモーションプランニング軌道からのより大きなまたは小さな偏差を許容する。

自動運転車両の場合、分散等の単純なモーメントを使用することは、車両の安全な動作に十分ではない。例として、自動運転車両の前に車両があり、前の車両の後ろに留まることも車線変更によって追い越すことも可能であるとする。モーションプランナーが可能な軌道の分布を決定した場合、その分布についての分散を求めることは、過度の保守性を引き起こし、結果として性能が低下する。

したがって、一実施形態は、分布から異なるモードを求めることによってこの保守性を解決する。例として、あるモードは車線維持に対応する可能な軌道の分布であり、別のモードは車線変更に対応する可能な軌道の分布である。これらの異なるモードに基づいて、個々のモードごとに分散を計算することが可能であり、そのようにして保守性を減じる。

いくつかの実施形態は、モーションプランナーは車両制御アルゴリズムの特定の現在の状態に関する情報を利用できる、という認識に基づいている。例として、ＭＰＣは、障害物回避不等式制約を含む制約付き最適化方法に基づく。モーションプランナーからＭＰＣに伝搬される分散が比較的小さい場合、ＭＰＣコントローラが障害物回避制約を不必要に有効化してスムーズでない軌道をもたらす可能性がある。このため、本発明の一実施形態において、ＭＰＣは、予測された状態における制約有効化および／または制約違反の最新の量と、信頼度を調整するために、すなわちモーションプランナーにおける軌道の分布の分散を増加または減少させるために使用できる、ＭＰＣの制御軌道とを、モーションプランナーに知らせる。その結果、計画された基準軌道の安全性が増しスムーズさが改善される。

異なる実施形態は、モーションプランニング層および車両制御層のリアルタイム動作について異なる時間スケールを使用する。たとえば、一実施形態において、モーションプランナーは、長期の予測性が高い基準軌道を計算するが、典型的には比較的低いサンプリング周波数で実行する必要がある、すなわち、更新レートが比較的遅く（たとえば１秒ごと）、したがって反応性が比較的低い。むしろ、ＭＰＣは典型的にははるかに短い予測範囲を使用するが、はるかに高いサンプリング周波数（たとえば２５ミリ秒ごと）で動作し、ローカルな偏差に対するコントローラの反応性を高くすることができる。その理由はたとえば自車両および周辺障害物についての姿勢推定の不確実さにある。したがって、特に安全要件をリアルタイムでかつ不確実さの下で満たすために、安全かつ信頼できる運転挙動を保証するための責任をプランニングおよび制御層間で共有することが重要である。

したがって、一実施形態は、車両を制御するためのシステムを開示し、このシステムは、車両の現在の状態と、車両の現在の状態の近傍の環境の画像と、車両の目的地とを受け付けるように構成された入力インターフェイスと、確率的モーションプランナーと適応予測コントローラとを格納するように構成されたメモリとを備え、確率的モーションプランナーは、車両の現在の状態と、車両の目的地と、環境の画像とを受け付けて、車両のモーションプランを規定するターゲット状態のシーケンスにわたるパラメトリック確率分布のシーケンスを生成するように構成され、各パラメトリック確率分布のパラメータは、確率分布の一次モーメントと少なくとも１つの高次モーメントとを規定し、適応予測コントローラは、予測範囲にわたってコスト関数を最適化して、車両の１つまたは複数のアクチュエータに対する制御コマンドのシーケンスを生成するように構成され、コスト関数の最適化は、第１のモーメントによって規定されるターゲット状態のシーケンスにおける異なる状態変数を追跡するコストを均衡させ、異なる状態変数は、追跡するコストを均衡させる際の確率分布の高次モーメントのうちの１つまたは複数を用いて重み付けされる。システムはさらにプロセッサを備え、プロセッサは、車両の現在の状態と、車両の目的地と、環境の画像とを確率的モーションプランナーに送ることによって確率的モーションプランナーを実行するように構成され、かつ、確率的モーションプランナーによって生成されたパラメトリック確率分布のシーケンスを適応予測コントローラに送ることによって適応予測コントローラを実行して制御コマンドのシーケンスを生成するように構成される。システムはさらに、適応予測コントローラによって決定された少なくとも１つの制御コマンドを車両の少なくとも１つのアクチュエータに出力するように構成された出力インターフェイスを備える。

別の実施形態は、車両を制御する方法を開示し、この方法は、確率的モーションプランナーと適応予測コントローラとを格納するメモリに結合されたプロセッサを使用し、確率的モーションプランナーは、車両の現在の状態と、車両の目的地と、環境の画像とを受け付けて、車両のモーションプランを規定するターゲット状態のシーケンスにわたるパラメトリック確率分布のシーケンスを生成するように構成され、各パラメトリック確率分布のパラメータは、確率分布の一次モーメントと少なくとも１つの高次モーメントとを規定し、適応予測コントローラは、予測範囲にわたってコスト関数を最適化して、車両の１つまたは複数のアクチュエータに対する制御コマンドのシーケンスを生成するように構成され、コスト関数の最適化は、第１のモーメントによって規定されるターゲット状態のシーケンスにおける異なる状態変数を追跡するコストを均衡させ、異なる状態変数は、追跡するコストを均衡させる際の確率分布の高次モーメントのうちの１つまたは複数を用いて重み付けされる。プロセッサは、方法を実現する、格納されている命令に結合され、命令はプロセッサによって実行されると方法のステップを実行し、方法は、車両の現在の状態と、車両の現在の状態の近傍の環境の画像と、車両の目的地とを受け付けるステップと、車両の現在の状態と、車両の目的地と、環境の画像とを確率的モーションプランナーに送ることによって確率的モーションプランナーを実行するステップと、確率的モーションプランナーによって生成されたパラメトリック確率分布のシーケンスを適応予測コントローラに送ることによって適応予測コントローラを実行して制御コマンドのシーケンスを生成するステップと、適応予測コントローラによって決定された少なくとも１つの制御コマンドを車両の少なくとも１つのアクチュエータに送るステップとを含む。

もう１つの実施形態は、方法を実行するためにプロセッサが実行可能なプログラムが実現される非一時的なコンピュータ読取可能記憶媒体を開示し、この媒体は、確率的モーションプランナーと適応予測コントローラとを格納し、確率的モーションプランナーは、車両の現在の状態と、車両の目的地と、環境の画像とを受け付けて、車両のモーションプランを規定するターゲット状態のシーケンスにわたるパラメトリック確率分布のシーケンスを生成するように構成され、各パラメトリック確率分布のパラメータは、確率分布の一次モーメントと少なくとも１つの高次モーメントとを規定し、適応予測コントローラは、予測範囲にわたってコスト関数を最適化して、車両の１つまたは複数のアクチュエータに対する制御コマンドのシーケンスを生成するように構成され、コスト関数の最適化は、第１のモーメントによって規定されるターゲット状態のシーケンスにおける異なる状態変数を追跡するコストを均衡させ、異なる状態変数は、追跡するコストを均衡させる際の確率分布の高次モーメントのうちの１つまたは複数を用いて重み付けされる。

この方法は、車両の現在の状態と、車両の現在の状態の近傍の環境の画像と、車両の目的地とを受け付けるステップと、車両の現在の状態と、車両の目的地と、環境の画像とを確率的モーションプランナーに送ることによって確率的モーションプランナーを実行するステップと、確率的モーションプランナーによって生成されたパラメトリック確率分布のシーケンスを適応予測コントローラに送ることによって適応予測コントローラを実行して制御コマンドのシーケンスを生成するステップと、適応予測コントローラによって決定された少なくとも１つの制御コマンドを車両の少なくとも１つのアクチュエータに送るステップとを含む。

いくつかの実施形態に係る予測コントローラおよびフィードバックシステムのブロック図である。 いくつかの実施形態の原理を採用したコントローラを含む車両の概略図である。 いくつかの実施形態の原理を採用したコントローラといくつかの実施形態に係る車両２０１のコントローラとの間のやり取りの概略図である。 意思決定部と、モーションプランナーと、車両コントローラと、アクチュエータコントローラとを含む多層制御構造およびこれらの層の間での情報の交換の概略図を示す。 意思決定部と、確率的モーションプランナーと、モデル予測コントローラと、アクチュエータコントローラとを含む多層制御構造およびこれらの層の間での情報の交換の概略図を示す。 確率的モーションプランナーからのコマンド情報に基づく多層制御構造におけるモデル予測コントローラのコスト関数における１つまたは複数の項の自動チューニングを示す図である。 いくつかの実施形態に係る、道路上での運転のための車両挙動の起こり得る制御目的と、確率的モーションプランナーにおけるこれらに対応する制御関数との、具体例としてのリストを示す図である。 いくつかの実施形態における、モーションプランナーによって採用される確率的制御関数の構造の概略図を示す。 いくつかの実施形態に係る、車両コントローラの性能に対するモーションプランナーの確率的出力の効果の一例を示す図である。 いくつかの実施形態に係る、車両コントローラの性能に対するモーションプランナーの確率的出力の効果の一例を示す図である。 いくつかの実施形態に係る、異なる制御関数の重要性を均衡させる方法の図を示す。 いくつかの実施形態に係る、各時間ステップにおいて不等式制約付き最適化問題を解くことにより車両コントローラを実現する適応モデル予測制御（ＭＰＣ）方法のブロック図を示す。 いくつかの実施形態に係る、各時間ステップにおいて不等式制約付き最適化問題を解くことにより車両コントローラを実現する適応モデル予測制御（ＭＰＣ）方法のブロック図を示す。 いくつかの実施形態に係る、最適制御構造化二次プログラム（ＱＰ）を解く適応ＭＰＣ方法のブロック図である。 いくつかの実施形態に係る、最適制御構造化非線形プログラム（ＮＬＰ）を解く適応ＭＰＣ方法のブロック図である。 適応線形または非線形ＭＰＣコントローラにおける時変基準追跡コストの特定の実施形態のための、基準および重み行列を適応させる方法のブロック図である。 適応線形または非線形ＭＰＣコントローラにおける時変基準追跡コストの特定の実施形態のための、基準および対角重み行列を適応させる方法のブロック図である。 確率的モーションプランナーからの基準値のシーケンスおよび共分散行列を用いる適応ＭＰＣコントローラの時変基準追跡コストの自動チューニングのためのアルゴリズムの記述の図である。 本発明のいくつかの実施形態に係る、車両のモーションを規定する状態遷移のツリーの概略図を示す。 本発明のいくつかの実施形態に係る、車両状態および確率分布のシーケンスを決定するための確率的モーションプランナーのフローチャートを示す図である。 各状態が確率的制御関数と整合する確率を求める方法のフローチャートを示す図である。 自動運転車両の位置の予測が障害物の不確実領域と交差し障害物がその位置にある確率が特定の衝突しきい値よりも高い例を示す図である。 確率的制御関数と整合する状態を生成するためのステップを３回繰り返した結果の簡略化された概略図を示す。 図９Ｄの最初の繰り返しにおける５つの状態の、割り当てられた起こり得る確率を示す図である。 車両を路上に留めるという制御目的に対応する制御関数の図を示す。 路上での車両走行時間を短縮するという制御目的に対応する制御関数の図を示す。 障害物を安全に追い越すという運転目的を表す制御関数の図を示す。 いくつかの実施形態に係る、現在の速度を維持するという目的の制御関数を説明することが可能な方法の図を示す。

本発明のいくつかの実施形態は、システムの動作を制御するためのシステムおよび方法、または、予測コントローラを使用するシステムを提供する。予測コントローラの一例は、制御されるシステムのモデルに基づいて制御入力を求めるモデル予測制御（ＭＰＣ）である。

図１は、いくつかの実施形態に係る、状態推定器１３０を介して予測コントローラ１１０に接続された一例としてのシステム１２０を示す。いくつかの実装形態において、予測コントローラは、システムの動的モデル１０２に従ってプログラムされたＭＰＣコントローラである。このモデルは方程式のセットであってもよく、これは、ある時間にわたるシステム１２０の状態および出力１０３の変化を、現在および以前の入力１１１ならびに以前の出力１０３の関数として表す。このモデルは、システムの物理的および動作上の制限を表す制約１０４を含み得る。動作中、コントローラは、システムの所望の挙動を示すコマンド１０１を受信する。コマンドは、たとえばモーションコマンドであってもよい。コントローラは、コマンド１０１を受けたことに応じて、システムに対する入力の役割をする制御信号１１１を生成する。システムは、この入力に応じて、システムの出力１０３を更新する。推定器は、システム１０３の出力の測定値に基づいて、システム１２１の推定された状態を更新する。この推定されたシステム１２１の状態が、状態フィードバックをコントローラ１１０に与える。

本明細書に記載のシステム１２０は、場合によっては電圧、圧力、力、トルク等の物理量に対応付けられた、決められた操作入力信号１１１（入力）によって制御され、場合によってはシステムの以前の状態から現在の状態への状態遷移を示す電流、流量、速度、位置等の物理量に対応付けられた、いくつかの制御された出力信号１０３（出力）を返す、任意の機械または装置であってもよい。出力値の一部はシステムの以前の出力値に関係し、一部は以前および現在の入力値に関係する。以前の入力および以前の出力に対する依存性をシステムの状態で符号化する。システムの動作、たとえばシステムの構成要素のモーションは、決められた入力値の適用に続いてシステムが生成した出力値のシーケンスを含み得る。

システムのモデル１０２は、システム出力がある時間にわたって如何に変化するかを、現在および以前の入力ならびに以前の出力の関数として記述する、数学的方程式のセットを含み得る。システムの状態は、一般的には時間とともに変化する、任意の情報のセットであり、例として、現在および以前の入力および出力の適切なサブセットであり、システムのモデルおよび将来の入力とともに、システムの将来のモーションを一意に規定することができる。

システムは、システムの出力、入力、および場合によっては状態が機能できる範囲を限定する、物理的制限および仕様制約１０４を受ける可能性がある。

コントローラ１１０は、ハードウェアで実現することができる、または、固定もしくは可変制御周期サンプリング間隔でシステムの推定状態１２１と所望のモーションコマンド１０１とを受信しこの情報を用いてシステムを動作させるための入力、たとえば制御信号１１１を求める、プロセッサ、たとえばマイクロプロセッサにおいて実行されるソフトウェアプログラムとして、実現することができる。

コマンド１０１は、基準コマンド１０５と、この基準コマンド１０５の信頼度１０６を表したものとを含み得る。たとえば、コマンド１０１は、基準モーション１０５と、この基準モーションの周りの信頼範囲１０６とを含み得る。本発明のいくつかの実施形態において、基準モーション１０５は、状態および／または出力値の基準軌道で表すことができ、信頼範囲は、状態および／または出力値の基準軌道の周りの不確実性を規定する共分散行列で表すことができる。本発明のいくつかの実施形態において、コマンド１０１は、確率的モーションプランナーによって計算され、基準モーション１０５はモーションプランの統計の第１のモーメントに対応し、信頼度１０６はモーションプランの統計の第２のまたは高次モーメントに対応する。

推定器１３０は、ハードウェアで実現することができる、または、固定もしくは可変制御周期サンプリング間隔でシステムの出力１０３を受信しこの新たな出力および以前の出力の測定値を用いてシステム１２０の推定状態１２１を求める、コントローラ１１０と同一または異なるプロセッサにおいて実行されるソフトウェアプログラムとして、実現することができる。

図２Ａは、いくつかの実施形態の原理を採用する予測コントローラ２０２を含む車両２０１の概略図を示す。本明細書で使用される車両２０１は、乗用車、バス、またはローバー等の任意の種類の車輪自動車であればよい。また、車両２０１は、自動運転または半自動運転車両であってもよい。たとえば、いくつかの実施形態は車両２０１のモーションを制御する。モーションの例は、車両２０１のステアリングシステム２０３によって制御される車両の横方向モーションを含む。一実施形態において、ステアリングシステム２０３はコントローラ２０２によって制御される。これに加えてまたはこれに代えて、ステアリングシステム２０３は車両２０１の運転者によって制御されてもよい。

車両はまた、コントローラ２０２によってまたは車両２０１の他の構成要素によって制御可能なエンジン２０６を含み得る。車両はまた、周囲環境を検知する１つ以上のセンサ２０４を含み得る。センサ２０４の例は、測距装置、レーダー、ライダー、およびカメラを含む。車両２０１はまた、その現在のモーション量および内部ステータスを検知する１つ以上のセンサ２０５を含み得る。センサ２０５の例は、全地球測位システム（ＧＰＳ）、加速度計、慣性計測装置、ジャイロスコープ、シャフト回転センサ、トルクセンサ、撓みセンサ、圧力センサ、および流量センサを含み得る。センサは情報をコントローラ２０２に提供する。車両は、有線または無線通信チャネルを介したコントローラ２０２の通信機能を可能にするトランシーバ２０７を備えていてもよい。

図２Ｂは、いくつかの実施形態に係る、予測コントローラ２０２と車両２０１のコントローラ２２０との間のやり取りの概略図を示す。たとえば、いくつかの実施形態において、車両２０１のコントローラ２２０は、車両２２０の回転および加速を制御するステアリングコントローラ２２５およびブレーキ／スロットルコントローラ２３０である。このような場合、予測コントローラ２０２は、コントローラ２２５、２３０に制御入力を出力して車両の状態を制御する。コントローラ２２０はまた、予測コントローラ２０２の制御入力をさらに処理する、ハイレベルコントローラ、たとえば車線維持支援コントローラ２３５を含み得る。いずれの場合も、コントローラ２２０は、車両のモーションを制御するために、予測コントローラ２０２の出力を用いて車両のステアリングホイールおよび／またはブレーキ等の車両の少なくとも１つのアクチュエータを制御する。

図３Ａは、自動または半自動運転車両のための多層制御・意思決定構造の概略図を示す。自動運転車両は、高度な相互接続されたセンシングおよび制御要素の統合を必要とする複雑なシステムである。本発明の実施形態は、モーションプランニング層３１０と車両コントローラ３２０とを含む。モーションプランナー３１０は、モーションコマンド１０１を計算し、車両コントローラ３２０に与える。車両コントローラ３２０は、モーションコマンド１０１の基準１０５および信頼度１０６に基づいて、システムがモーションコマンドを実行するための制御入力１１１を計算する。本発明のいくつかの実施形態は、意思決定層３００および／またはアクチュエータコントローラ３３０をさらに含む。

最も高いレベルにおいて、ルートプランナーが一連の目的地を道路網を通して計算することができる。離散意思決定層３００は、ルートが与えられると車両の１つまたは複数のローカルな運転ゴールおよび対応する離散的意思３０１を決定する責任を担ってもよい。各意思決定は、右折、車線に留まる、左折、または交差点の特定の車線で完全停止する、のうちのいずれかである可能性がある。意思決定層３００は、レーダー、ライダー、慣性測定装置、カメラ、および／または全地球測位システム（ＧＰＳ）情報等の１つまたは複数のセンサからの情報を、以前の地図情報とともに使用することにより、特定の運転シナリオについて、システムの状態およびシステムに関連する周辺部分を推定する。センシングおよびマッピングモジュールからの推定情報を、図３Ａの構造の制御層および意思決定層のうちの１つ、複数またはすべてが利用できるようにすることができる。

モーションプランナー３１０は、１つまたは複数のローカル目的地ゴール３０１に基づいて、車両コントローラ３２０に与えられるモーションコマンド１０１を決定する役割を担う。いくつかの実施形態において、モーションコマンドは、基準コマンド１０５と信頼範囲１０６を含む。いくつかの実施形態において、基準コマンドは、車両が意思決定層３００からの出力に基づいて辿るべき、安全で望ましく動的に実現可能な軌道である。本発明のいくつかの実施形態は、重要な要件が、モーションプランナー３１０によって計算される基準軌道１０５は衝突がなく動的に実現可能で車両コントローラ３２０による追跡が可能な軌道である、という認識に基づいている。このことは、基準軌道が、環境とのいかなる衝突も回避し、数学的方程式のセットで表すことができるシステム１０２の動的モデルを考慮つつ、１つまたは複数のローカルな運転ゴールを達成することを、意味する。

本発明のいくつかの実施形態は、モーションプランニングタスクにおける典型的な制限要因が、対応する制約付き動的最適化問題の非凸性である、という認識に基づいている。結果として、局所的に最適な解しか実現されず、これは大域的な最適解からはほど遠く、単に実現可能な解を見出すためであっても、場合によっては非常に大きな計算負荷および時間を要する。モーションプランニングは、たとえば高速探索ランダムツリー（ＲＲＴ）等のサンプリングベースの方法、またはＡ^＊、Ｄ^＊およびその他の変形等のグラフサーチ方法を用いて実行することができる。

図３Ｂに示されるように、本発明のいくつかの実施形態は、たとえば関係する確率密度関数（ＰＤＦ）を近似するための粒子フィルタリングを用いる、モーションプランニング３１１のための確率的方法を使用する。道路上に留まること、左側または右側走行、および障害物回避等の運転要件は、非線形フィルタリング問題の測定値として定式化することができる。結果として得られるツリー展開は、粒子フィルタリングベースの確率的モーションプランナー３１１が状態空間をサンプリングせずむしろ粒子フィルタリングベースの確率的モーションプランナーが代わりに入力空間をサンプリングし運転要件に基づいて他の訂正項を追加するという点で、標準的なＲＲＴアルゴリズムとは異なり得る。

本発明のいくつかの実施形態において、確率的モーションプランナー３１１は、車両の現在の状態、車両の目的地、および環境の画像を受け付けて、車両のモーションコマンドを規定するターゲット状態および／または出力値のシーケンスにわたるパラメトリック確率分布のシーケンスを生成し、各パラメトリック確率分布のパラメータは、確率分布の一次モーメント３１６および少なくとも１つの高次モーメント３１７を規定する。本発明のいくつかの実施形態において、確率的モーションプランナー３１１は、車両のモーションコマンドにおいて望ましい挙動が実現されるように、コスト関数を最小にしてもよい。

本発明のいくつかの実施形態は、車両の各ターゲット状態は複数の状態変数を含むことができ、確率的モーションプランナー３１１における各パラメトリック確率分布は各状態変数の分布についてのパラメータを規定する多変量分布である、という認識に基づいている。この場合、各パラメトリック確率分布の第１のモーメントは平均値であり、パラメトリック確率分布の高次モーメントは空間および時間において変化する値の共分散行列である。

本発明のいくつかの実施形態において、確率的モーションプランナー３１１は粒子フィルタリングベースのアルゴリズムを含み、これは、時間的に伝搬してある時点におけるターゲット状態の尤度のセットを表す粒子のセットを有し、各粒子はこの時点におけるターゲット状態のガウス分布を含む。この場合、この時点のパラメトリック確率分布の一次モーメントは粒子の加重平均であり、この時点のパラメトリック確率分布の高次モーメントは粒子の加重共分散である。

図３Ａおよび３Ｂに示されるように、車両コントローラ３２０および／または３４０は、システムを動作させるための制御信号３２１を計算することにより、対応する信頼範囲１０６を考慮に入れてこの基準モーション１０５を実現することを目的とする。制御信号は、たとえばステアリング角、車輪トルクおよびブレーキ力の値等の、１つまたは複数の作動コマンドを含み得る。本発明のいくつかの実施形態において、車両コントローラ３２０は、車両の要求された挙動を実現するためにアクチュエータを直接調整する１つまたは複数のコントローラ３３０からなる追加の層に、制御信号３２１を与える。

本発明の異なる実施形態は、異なる技術を車両コントローラ３２０に用いて、モーションプランナー３１０のための特定のアルゴリズムによって計算された基準モーション１０５を追跡することができる。本発明のいくつかの実施形態において、モデル予測コントローラ３４０（ＭＰＣ）を車両制御層に用い、長期モーションプランにおける将来の情報を予測コントローラにおいて有効に使用することにより、車両の所望の挙動を実現することができる。

本発明のいくつかの実施形態において、線形動的モデルを線形制約および二次目的関数と組み合わせて用いて、モーションプランナーが計算した基準モーションを追跡する線形モデル予測コントローラ（ＬＭＰＣ：linear model predictive controller）を得る。本発明の他の実施形態において、制約および／または目的関数のうちの１つまたは複数は非線形の可能性があり、および／または車両状態挙動を記述する動的モデル方程式は非線形の可能性があり、モーションプランナーが計算した基準モーションを追跡する非線形モデル予測コントローラ（ＮＭＰＣ：nonlinear model predictive controller）を得る。

本発明のいくつかの実施形態は、モーションプランナーは、比較的長期の予測性が高いモーションプランを計算できるが、これは典型的には比較的遅いサンプリング周波数で実行する必要がある、という認識に基づいている。たとえば、モーションプランナーは、将来の５～５０秒の期間の基準モーションを計算することができるが、これは１秒または複数秒のサンプリング時間ごとに１回のみまたは複数回実行することができ、その結果、予測性は高いが反応性が比較的低いモーションプランになる。本発明の実施形態はさらに、予測コントローラは、基準モーションプランを、比較的高いサンプリング周波数で実行しつつ、比較的短い予測範囲にわたって制御信号を計算することにより、追跡できる、という認識に基づいている。たとえば、車両コントローラは、１～１０秒の予測範囲を使用できるが、これは１０～１００回／秒で実行できる。車両コントローラは、車両状態推定における不確実性ならびにセンシング・マッピングモジュールにおける他の不確実性、たとえば車両の周囲環境内の障害物に関連する他の不確実性に起因する局所偏差に対して高い反応性を有することができる。

本発明のいくつかの実施形態において、適応モデル予測コントローラ３４０の実行レートは、確率的モーションプランナー３１１の実行レートよりも大きく、プロセッサはモーションプランナーの各実行ごとに少なくとも一度ＭＰＣを実行する。本発明のいくつかの実施形態において、モーションプランは、ターゲット状態のシーケンスにわたるパラメトリック確率分布のシーケンスを、車両コントローラの予測範囲の長さよりも大きい期間の時間の関数として規定する。

本発明のいくつかの実施形態において、車両の異なる動的モデルを、自動または半自動運転車両のための多層制御および意思決定構造における異なる構成要素において使用することができる。たとえば、比較的単純であるが計算コストが低いモーションモデルをモーションプランナーに使用することができ、比較的正確であるが計算コストがより高い動的シングルまたはダブルトラック車両モデルを予測コントローラに使用することができる。

図３Ａおよび図３Ｂに示されるように、情報は、自動および半自動運転車両のための多層制御および意思決定構造における異なる構成要素間で共有できる。たとえば、地図および車両の周辺に関する情報は、意思決定者とモーションプランナー３０５との間、モーションプランナーと車両コントローラ３１５との間、または車両コントローラとアクチュエータコントローラとの間で、共有できる。加えて、本発明のいくつかの実施形態は、信頼性および安全性は、自動または半自動運転車両について、診断情報を用いることによって改善することができ、この診断情報は、たとえばある構成要素におけるアルゴリズムの成功および／または失敗の性能メトリックであり、これは、多層制御および意思決定構造の別の構成要素におけるアルゴリズムと共有できる、という認識に基づいている。

図３Ｂを参照すると、確率的モーションプランナー３１１は、モデル予測コントローラ３４０からの情報３１５を使用することができる。たとえば、本発明の一実施形態において、ＭＰＣコントローラは、各サンプリング時間ステップにおいて制約付き動的最適化問題を解き、各サンプリング時間ステップにおいて確率モーションプランナーにフィードバックを提供するために、各制御解に制約のアクティブセットを使用する。アクティブセットにおけるすべての制約は、制御解において等しく成立するが、ＭＰＣコントローラの解における残りの制約のすべては非アクティブと呼ばれる。本発明のいくつかの実施形態において、確率的モーションプランナー３１１は、ＭＰＣコントローラ３４０内のアクティブ制約の種類および／または数に基づいて、確率的分布の高次モーメントを調整するように構成される。これは、例として、自動または半自動運転車両の全体的な挙動を改善するために、モーションプランナーの挙動をモーションプランナーが検出していないまたはまだ検出していない環境変化に合わせて調整する必要がある場合に、有益である可能性がある。

図３Ｃは、確率的モーションプランナー３１１からの基準コマンド１０５および対応する信頼度１０６に基づく、多層制御構造内のモデル予測コントローラ３４０のコスト関数３５０における１つまたは複数の項の自動チューニングを示す。適応ＭＰＣコントローラ３４０は、予測範囲にわたってコスト関数を最適化することにより、車両の１つまたは複数のアクチュエータに対する制御コマンドのシーケンスを生成するように構成される。本発明のいくつかの実施形態において、コスト関数３５０の最適化は、パラメトリック確率分布３１６の第１のモーメントによって規定されるターゲット状態３５５のシーケンスを追跡するコストを、車両３６０のモーションの少なくとも１つの他のメトリックのコストに対して均衡させる。

本発明の実施形態は、追跡コストの重要性に、パラメトリック確率分布の高次モーメント３１７の１つまたは複数の関数を使用して重み付けすることができる、という認識に基づいている。確率的モーションプランナー３１１におけるパラメトリック確率分布の高次モーメント３１７に基づいた適応予測コントローラ３４０におけるコスト関数３５０のこの自動チューニングは、モーションプランナーおよび車両コントローラが制御目的を達成する際に負担を共有することを可能にし、制御目的は、たとえば、安全な障害物回避を保証すること、特定の車線に留まること、望ましい基準速度で運転することおよび／または比較的複雑なシナリオにおいて信頼性があり快適な運転挙動を実現することである。

確率分布の高次モーメント３１７は、車両のモーションプランに関する確率的モーションプランナーの信頼度を示す。本発明のいくつかの実施形態において、適応予測コントローラは、ターゲット状態の基準シーケンスからの予測車両状態値のより低い偏差を許容する信頼度の増加に伴って、均衡最適化における追跡の重みを増加させる。本発明のいくつかの実施形態において、適応予測コントローラは、基準軌道からの予測車両状態値のより大きな偏差を許容する信頼度の減少に伴って、均衡最適化における追跡の重みを減少させる。

本発明のいくつかの実施形態において、共分散行列Ｐ_ｋ３０６は、確率的モーションプランナーにおける確率分布の二次モーメントを表すために使用される。この共分散行列Ｐ_ｋ３０６の時変シーケンスを用いて、時変基準追跡項３５５の重み付けを、適応ＭＰＣコントローラの目的関数における、他の性能メトリック３６０に対し、自動的にチューニングすることができる。本発明のいくつかの実施形態は、これが、複数の競合する制御目的間のトレードオフに対する自動チューニング機構をもたらす、という認識に基づいている。ターゲット状態および／または出力値の基準軌道１０５に対応付けられた共分散行列Ｐ_ｋ３０６に関する信頼度１０６は、モーションプランナーが、その計算された軌道が有効であるとどの程度信じているかを示す。ＭＰＣコスト関数３５０を自動チューニングするための信頼情報を使用することにより、モーションプランナーによるそのような信頼が低いときは、基準軌道からのより多くの偏差を許容し、そのような信頼が比較的高いときは、より少ない偏差を許容する。

図４Ａは、いくつかの実施形態に係る、道路上での運転のための車両挙動の起こり得る制御目的と、確率的モーションプランナーにおける、これらの制御目的に対応する制御関数との、具体例としてのリストを示す。制御関数は、車両の現在の状態を、対応する目的に従って車両のターゲット状態に遷移させる。制御関数は、分析的に設計および／またはデータから学習することができる。たとえば、運転の１つの目的は、車両に対して道路上に留まるよう要求すること４００であり、その対応する路上滞留関数は、車両の位置を道路の縁の内側で維持するように構成される。起こり得るその他の運転の目的として、車両を車線の中央において４１０公称速度で４２０運転させることを挙げることができる。これらに対応する制御関数は、車両の位置を車線の中央で維持するように構成された中央車線関数および／または車両の所望の速度を維持するように構成された速度維持関数を含み得る。

別の例において、運転目的は、車両と道路上の障害物との間の最短距離を維持するように構成された安全余裕関数を用いて、周辺の障害物に対する安全な余裕の維持４３０を車両に要求することであってもよい。別の起こり得る運転目的は、同一車線４４０の車両への安全距離を維持することである。これは、対応する、車両と前方車両との間の最短運転間隔を維持するように構成された最短運転間隔関数によって実現することができる。乗員の快適性、燃費、摩損という理由、またはその他の理由から、本発明のいくつかの実施形態は、車両のスムーズな運転挙動４５０を要求することを目的とする。いくつかの実施形態は、この目的を、モーションプランナーにおいて車両の動きのスムーズさを維持するように構成されたスムーズ運転関数を用いることによって実現する。

確率的モーションプランナーにおける運転目的の他の例は、車両の速度を制限速度で維持するように構成された制限速度関数を用いて制限速度まで加速すること４６０、車両の現在位置を現在の車線から隣の車線に変更するように構成された車線変更関数を用いて車線を変更すること４７０、および交差点での車両のアイドル時間を減じるように構成された交差点横断関数を用いて交差点でのアイドリングを最小にすること４８０により燃費を減じることを含み得る。

本発明のいくつかの実施形態は、モーションプランナーは相反する運転目的を有し得る、という認識に基づいている。たとえば、周辺の障害物に対する安全な余裕を維持しつつ４３０、一定速度を維持すること４２０が、不可能な場合がある。いくつかの実施形態は、これらの相反する運転目的を、制御関数のうちの少なくとも１つを確率的にすることによって調和させる。

具体的には、いくつかの実施形態は、運転目的のすべてを正確に達成できる訳ではない、という認識に基づいている。たとえば、制限速度まで加速するという目的４６０は、周辺の障害物に対する安全な余裕を維持するという運転目的４３０と相容れない場合がある。また、運転者は、場合によって、どの運転目的が最も重要であるかについての判断が異なることがある。さらに、自動運転車両の場合、運転目的を正確に達成することを不可能にする他の不確実性が存在する。そのため、本発明の実施形態は、運転目的の達成には不正確さがあり、このような運転目的の達成の程度は場合によって変わる可能性がある、という認識に基づいている。

図４Ｂは、モーションプランナーのいくつかの実施形態が採用する確率的制御関数４１５の構造の概略図を示す。各制御関数４１５は、車両の現在の状態を、その対応する制御目的に基づいて、確率的にターゲット状態に遷移させるように構成される。そのため、確率的制御関数４１５は、現在の状態をターゲット状態４５５に遷移させるための決定論的要素４２５と、決定論的要素によって求められたターゲット状態の周辺の値の確率論的分布４６５を求めるための確率論的要素４３５とを含む。決定論的要素は、ターゲット状態の１つの値４４５または複数の値のシーケンス４５５を生成することができる。これに加えてまたはこれに代えて、決定論的要素を複数回実行することにより、シーケンス４５５を生成することができる。しかしながら、いくつかの実装形態では、確率的制御関数の出力が、第１のモーメントおよび少なくとも１つの高次モーメントによって規定されるターゲット状態にわたるパラメトリック確率分布４７５となるように、ターゲット状態のシーケンスの各値４４５ごとに、確率的分布４６５がある。

図４Ｃは、いくつかの実施形態に係る、車両コントローラ３２０の性能に対するモーションプランナー３１０の確率的出力の効果の一例を示す。この例において、図４Ｃは、車両の現在の状態４１０ｃと、車両が到達しようとするターゲット状態４２０ｃとを示す。ターゲット状態４２０ｃは、デカルト座標位置、速度、または車両に対応付けられた別のエンティティであってもよい。ターゲット状態４２０ｃは、特定の状態または状態の領域であってもよい。たとえば、ターゲット状態４２０ｃは、許容速度の区間またはデカルト座標空間における可能な位置の領域であってもよい。適切な制御入力は、図４Ａのような運転目的を達成しつつ車両４１０ｃをその初期状態からターゲット状態４２０ｃにする入力であり、たとえば、運転目的は道路上の領域４３１ｃ内に留まることであってもよい。入力の結果として生じる状態４３０ｃは、１時間ステップの制御入力の適用をもたらす状態遷移に対応し得る、または、結果として生じる状態４３０ｃは、軌道４１１ｃすなわち状態遷移のシーケンスによって初期状態４１０ｃに接続し得る。

現在の状態からターゲット状態への遷移は、自動運転車両または半自動運転車両の場合、車両のモーションの動的モデルに対する制御入力をテストすることによって実行することができる。モーションのモデルは、モデルに与えられた制御入力に従って車両の状態を遷移させる。各種実施形態において、車両のモーションの数学モデルは不確実性を含む。このため、車両のモーションモデルは、モデルが車両の実際のモーションの単純化された記述であるという事実を説明するためだけでなく、車両の真の状態の検知における不確実性、障害物の状態の検知における不確実性、および環境の検知における不確実性も説明するための、確率的モーションモデルである。

図４Ｃは、制御入力の特定の選択から生じたターゲット状態領域４３１ｃを示し、４３１ｃは非ゼロの確率的要素であり、４３０ｃは領域４３１ｃに含まれる決定論的要素である。運転目的領域４２０ｃは、ターゲット状態領域４３１ｃと重ならない、すなわち、領域４３１ｃは運転目的領域４２０ｃを含まない。このため、図４Ｃを参照すると、運転目的４２０ｃは達成されない可能性が高く、コントローラは、１つまたは複数の運転目的をより適切に達成するためにその動作を変更することができる。

図４Ｄは、制御入力の特定の選択から生じたターゲット状態領域４３１ｄを示し、４３１ｄは非ゼロの確率的要素であり、４３０ｄは領域４３１ｄに含まれる決定論的要素である。運転目的領域４２０ｄは、ターゲット状態領域４３１ｄよりも小さく、全体が領域４３１ｄに含まれる、すなわち、領域４３１ｄは領域４２０ｄを含む。このため、図４Ｄを参照すると、運転目的４２０ｄは、制御入力の選択によって達成される可能性が高い。

図４Ｅは、いくつかの実施形態に係る、異なる制御関数の重要性をどのように均衡させるかについての方法の図を示す。図４Ｅは、公称速度の維持４２０および安全距離の維持４３０という制御関数として表される２つの駆動目的が存在する状況を示す。速度要件は、公称速度からの速度偏差の関数として表され、安全距離は、自動運転車両から道路上の障害物までのユークリッド距離として表される。速度制御関数の決定論的要素はゼロであり、確率的要素は決定論的要素を中心とする狭い形状４１０ｅである。他方、安全距離要件は、非ゼロである決定論的要素４２０ｅと、決定論的要素の周りの大きな変動を許容する確率的要素４３０ｅとを有する。また、この例示において、決定論的要素４２０ｅは、確率的要素４３０ｅの平均を中心としていない。速度は距離と同じではないので、速度および安全距離制御関数は、制御関数を比較することができる共通状態に転換される４４０ｅ。次に、制御関数は組み合わされて４５０ｅ同時分布（joint distribution）にされ、これは制御関数をともに重み付けする。本発明のいくつかの実施形態において、異なる制御目的の相対的スケーリングが、制御関数の相対的重要性を組み合わせて均衡させるために使用される。

たとえば、図４Ｅにおいて、速度制御関数の確率分布は安全距離制御関数の確率的要素４３０ｅよりもはるかに小さい変動４１０ｅを有するので、同時分布は、どちらも速度制御関数のそれぞれの要素により近い共通の決定論的要素４７０ｅおよび確率的要素４６０ｅに重要性を均衡させる４５０ｅ。

同時分布は複数のやり方で選択することができる。例として、シーケンス内の各ステップに対する各制御関数の確率的要素がガウス分布である場合、同時分布は多変量ガウス分布として選択することができ、各制御関数の重要性の重み付けは、各要素についての共分散の逆数による重み付けである。

決定論的要素は複数のやり方で選択することができる。例として、一実施形態は、決定論的要素を、それらがガウス分布の平均を構成するように、ベクトルに積み重ねることによって組み合わせる。

シーケンスにおける各ステップの確率的要素がガウス分布であるにもかかわらず、分布のシーケンスは、特に組み合わされて同時分布のシーケンスにされる場合、非ガウス分布となる。例として、決定論的要素は、現在の状態を制御関数出力にマッピングする非線形関数の場合があり、これは、シーケンスを非ガウス分布にする。そのような場合において組み合わされた状態および分布のシーケンスを決定するために、数値近似を、たとえばサンプリングによって使用することができる。

本発明のいくつかの実施形態は、確率的モーションプランナーによって計算される基準モーションを追跡することを目的とする適応予測コントローラにおいて、異なる制御目的の同様の均衡化を使用することができる、という認識に基づいている。たとえば、異なる状態変数の追跡を組み合わせてジョイント追跡コスト関数にすることができ、この場合、重み行列を使用して、状態変数のうちの１つを追跡することの重要性を、他の状態変数のうちの１つまたは複数を追跡することの重要性に対して均衡させる。本発明のいくつかの実施形態において、重み行列は、確率的モーションプランナー３１１によって計算される車両のモーションコマンドを規定するターゲット状態および／または出力値のシーケンスにわたるパラメトリック確率分布のシーケンスのスケーリング行列および１つまたは複数の高次モーメント２１７の関数として計算される。いくつかの実施形態は、ジョイントコスト関数における制御目的の各々の重み付けのための下限と上限との間の重み行列の各々を境界付ける飽和関数を含む。

異なる状態変数の追跡に加えて、本発明のいくつかの実施形態は、予測コントローラによって考慮されるべき１つまたは複数の他の目的項を含む。そのような他の項の例は、運転快適性、速度制限、消費エネルギー、汚染などに関連し得る。これらの実施形態は、異なる状態変数の基準値を追跡するコストと、これらの他の目的項とを均衡させる。

図５Ａは、いくつかの実施形態に係る、システムの現在の状態１２１および制御コマンド１０１から制御信号１１１を計算するコントローラ１１０を実現するモデル予測制御（ＭＰＣ）のためのシステムおよび方法のブロック図を示す。具体的には、ＭＰＣは、各制御時間ステップにおいて最適制御構造化プログラムの形態の不等式制約付き最適化問題を解くこと５５０により、システムの予測時間範囲にわたる将来の最適制御入力のシーケンス５６０を含む制御解、たとえば解ベクトル５５５を計算する。この最適化問題５５０における目的関数５４０の最適制御データ５４５、等式および不等式制約５３０は、動的モデル５２５、システム制約５２０、システムの現在の状態１２１、ならびに基準１０５および信頼度１０６からなる制御コマンド１０１に依存する。

いくつかの実施形態において、この不等式制約付き最適化問題の解５５０は、メモリから読み取ることができる、以前の制御時間ステップからの予測時間範囲にわたる状態および制御値５１０を使用する。この概念は、最適化アルゴリズムのウォームスタートまたはホットスタートと呼ばれ、いくつかの実施形態において、ＭＰＣコントローラの必要な計算労力を大幅に低減することができる。同様に、対応する解ベクトル５５５を使用して、次の制御時間ステップのための最適または準最適状態および制御値のシーケンスを更新し格納する５６０ことができる。

本発明のいくつかの実施形態において、ＭＰＣコントローラ３４０は、最適制御コスト関数５４０における１つまたは複数の項を、確率的モーションプランナー３１１によって計算される基準軌道１０５および対応する信頼範囲１０６に適応させる。図５Ｂに示すように、最小二乗型コスト関数は、特定の重み行列で基準軌道を追跡するために使用することができる。本発明のいくつかの実施形態において、基準軌道は第１のモーメント２１６によって規定され、重み行列は、共分散行列３０６の関数として、または確率的モーションプランナー３１１によって計算される車両のモーションコマンドを規定するターゲット状態および／または出力値のシーケンスにわたるパラメトリック確率分布のシーケンスの１つまたは複数の高次モーメント２１７の関数として計算される。

図６Ａは、システムの現在の状態１２１および制御コマンド１０１から最適制御構造化二次プログラム（ＱＰ）６５０を解いて制御信号１１１を計算することにより、いくつかの実施形態に係る車両コントローラ１１０を実現する適応ＭＰＣ３４０のためのシステムおよび方法のブロック図を示す。本発明のいくつかの実施形態において、適応ＭＰＣコントローラは、線形動的モデルと組み合わせて線形－二次目的関数を使用して、車両の挙動および線形不等式制約を予測し、結果として以下のように解することができる最適制御構造化ＱＰ問題定式化を得る。

適応ＭＰＣコントローラ３４０によって解かれる制約付きＱＰ６５０における目的関数は、１つまたは複数の最小二乗基準追跡項６５２を含み、これは、予測状態および／または出力値のシーケンス６５６と、モーションプランナーによって計算される基準状態および／または出力値のシーケンス１０５との間の差にペナルティを課す。基準状態および／または出力値１０５は、確率分布の第１のモーメント３１６を規定する。

各種実施形態において、モーションプランナーによって決定された基準値と予測コントローラによって決定された値との間のペナルティは、ターゲット状態の異なる状態変数に異なる重みを割り当てる重み行列によって重み付けされる。これに加えてまたはこれに代えて、いくつかの実施形態は、予測コントローラによって考慮されるべき他の目的項を追加する。そのような他の項の例は、運転快適性、速度制限、消費エネルギー、汚染などに関連し得る。これらの実施形態は、基準追跡のコストと、これらの他の目的項とを均衡させる。

不等式制約は、車両の縦方向または横方向の速度および／または加速度、その周辺に対する車両の位置および／または方位、スリップ比またはスリップ角、方位の角度または角速度、車輪速度、力および／またはトルクのうちの１つまたは複数の、１つまたは複数の組み合わせに対する制約を含み得る。たとえば、障害物回避制約は、適応ＭＰＣコントローラにおいて、車両の予測位置、予測位置に対する速度および方位、車両の周辺環境における１つまたは複数の障害物の予測位置、速度、および方位の、線形関数に対する１つまたは複数の不等式制約のセットを規定することにより、実現されてもよい。

ＱＰ６５０を解くための繰り返し最適化アルゴリズムの例は、主勾配もしくは双勾配ベースの方法、射影勾配もしくは近接勾配法、前方－後方分離法、交互方向乗数法、主、双もしくは主－双アクティブセット法、主もしくは主－双内点法、または、そのような最適化アルゴリズムの変形を含む。本発明のいくつかの実施形態において、ＱＰ行列６３０内のブロック－疎最適制御構造は、計算の複雑さを低減しそれによってＱＰ最適化アルゴリズムの実行時間およびメモリフットプリントを低減するために、最適化アルゴリズムの線形代数演算のうちの１つまたは複数において利用することができる。

本発明の他の実施形態は、非凸性最適制御構造化ＱＰ６５０を、非線形プログラミングのための最適化アルゴリズムを使用して、たとえば、適応ＭＰＣコントローラ３４０の各サンプリング時間における不等式制約付き最適化問題に対する、準最適な、局所的に最適な、または大域的に最適な制御解を見出すことができる逐次二次計画法（ＳＱＰ：sequential quadratic programming）または内点法（ＩＰＭ：interior point method）を使用して、解くことができる。

本発明の実施形態は、ＭＰＣコスト関数の追加の目的項を、ステージコストおよび／または終端コスト項６６３の形態で規定することができ、これらはいずれも、線形関数、線形－二次関数、または非線形関数の任意の組み合わせで構成することができる。これらの追加の目的項は、状態および／または制御入力変数の１つまたは複数の線形または非線形関数の１つまたは複数の組み合わせのペナルティを含み得る。たとえば、制約付きＮＬＰ６６０における目的関数６４５は、車両の縦方向または横方向の速度および／または加速度、スリップ比またはスリップ角、方位の角度または角速度、車輪速度、力、トルク、またはそのような量の任意の組み合わせの、線形、二次、または非線形ペナルティを含み得る。

本発明のいくつかの実施形態は、車両の挙動を予測するための離散時間動的モデル６６５は連続時間微分方程式または微分代数方程式のセットの時間離散化を実行することによって得ることができる、という認識に基づいている。そのような時間離散化は、分析的に実行されることがあるが、一般的には状態軌道の離散時間発展の数値近似を計算するために数値シミュレーションルーチンの使用を必要とする。連続時間微分方程式または微分代数方程式のセットを近似的にシミュレートする数値ルーチンの例は、明示的または暗黙的ルンゲ・クッタ（Runge-Kutta）法、明示的または暗黙的オイラー（Euler）、後方微分式および他のシングルステップまたはマルチステップ法を含む。

不等式制約は、車両の縦方向または横方向の速度および／または加速度、その周辺に対する車両の位置および／または方位、スリップ比またはスリップ角、方位の角度または角速度、車輪速度、力および／またはトルクのうちの１つまたは複数の、１つまたは複数の組み合わせに対する制約を含み得る。たとえば、障害物回避制約は、適応非線形ＭＰＣコントローラにおいて、車両の予測位置、予測位置に対する速度および方位、車両の周辺環境における１つまたは複数の障害物の予測位置、速度、および方位の、線形または非線形関数に対する１つまたは複数の不等式制約のセットを規定することにより、実現されてもよい。

本発明のいくつかの実施形態は、非線形適応ＭＰＣコントローラの各サンプリング時点において制約付き最適制御構造化ＮＬＰ６６０を効率的に解くための調整された最適化アルゴリズムに基づく。そのような最適化アルゴリズムは、解ベクトル５５５を発見することができ、これは、制約に関して適切であり大域的に最適である、または適切であるが局所的に最適である、または適切であるが準最適である、のいずれかであり、または、繰り返し最適化アルゴリズムは適切でもなく局所的に最適でもない低精度近似制御解を発見することができる。ＮＬＰ最適化アルゴリズムの例は、内点法の変形および逐次二次計画（ＳＱＰ）法の変形を含む。

特に、本発明のいくつかの実施形態は、少なくとも１つの凸性ブロック疎ＱＰ近似が非線形ＭＰＣコントローラの各サンプリング時点において解かれる必要があるように、準ニュートンまたは一般化ガウス－ニュートン型の定値ヘッセ近似と組み合わせた逐次二次計画法のオンライン変形である、リアルタイム繰り返し（ＲＴＩ：real-time iteration）アルゴリズムを使用する。各ＲＴＩ繰り返しは以下の２つのステップからなる。
（１）準備段階：システムダイナミクスを離散化および線形化し、残りの制約関数を線形化し、二次目的近似を評価して、最適制御構造化ＱＰサブ問題を構築する。
（２）フィードバック段階：ＱＰを解いてすべての最適化変数の現在値を更新し、次の制御入力を取得してシステムにフィードバックを適用する。

本発明のいくつかの実施形態において、ヘッセ行列および制約ヤコビ行列におけるブロック－疎最適制御構造は、計算の複雑さを低減しそれによってＮＬＰ最適化アルゴリズムの実行時間およびメモリフットプリントを低減するために、最適化アルゴリズムの線形代数演算のうちの１つまたは複数において利用することができる。

図７Ａは、適応線形または非線形ＭＰＣコントローラ３４０における時変基準追跡コスト７２０の特定の実施形態における基準および重み行列６４０を適応させる方法のブロック図を示す。ＭＰＣ追跡コスト７２０は、時変正定値または半正定値重み行列７１０で重み付けされ、重み行列の各々は、パラメトリック確率分布の高次モーメント３１７のうちの１つまたは複数の組み合わせによって表される、確率的モーションプランナーによって計算される基準の周りの時変不確実性に対する反比例関係に基づいて、計算される。

図７Ｃは、確率的モーションプランナーからの高次モーメントを使用する、適応ＭＰＣコントローラ５４０の時変基準追跡コストの自動チューニングのためのアルゴリズムの記述を示す。基準状態および／または出力値のシーケンスならびに対応する共分散行列７４０に基づいて、基準モーションのスムーズな近似７４３をＭＰＣコントローラの最小二乗型目的項において使用することができ、重み行列の各々は、確率的モーションプランナーからの対応する共分散行列７３０の各々との逆比例関係に基づいて個々に計算することができ、これは、ＭＰＣコントローラ７４４の各サンプリング時点において繰り返される。新たな基準モーションプランが、パラメトリック確率分布の第１のモーメント３１６および高次モーメント３１７からなる確率的モーションプランナーによって計算される場合７４１、基準状態および／または出力値のシーケンスならびに対応する共分散行列をリセットすることができる７４２。新たな基準モーションプランがまだ利用できない場合、基準状態および／または出力値の最新のシーケンスならびに対応する共分散行列を、ある制御時間ステップから次の制御時間ステップにシフトすることができる。

本発明のいくつかの実施形態は、モーションプランナーはＭＰＣよりも長い時間スケールで動作するのに対しＭＰＣコントローラはモーションプランナーよりも高いサンプリングレートで実行することができ、よって、モーションプランナーの比較的低い反応性と比較して、ＭＰＣは、環境変化に対しても、車両状態および車両の周辺の検知および推定における不確実性に対しても、より迅速に調整可能である、という認識に基づいている。

本発明のいくつかの実施形態において、確率的モーションプランナーにおける組み合わされた状態および確率分布のシーケンスの計算は、運転の決定に到達する状態のシーケンスが発見されるまで拡大するツリーとして実現される。

図８は、本発明のいくつかの実施形態に係る、車両のモーションを規定する状態遷移のツリーの概略図を示す。運転可能空間８３０内の現在のツリーが、車両の現在の状態を示すルートノード８００とともに示され、ツリーは、本発明の他の実施形態に従って選択された制御入力から生じる、状態空間内のノードとしての状態およびエッジとしての状態遷移を含む。たとえば、エッジ８２１は、制御入力を適用することにより生成された予め定められた時間にわたるルートノード８００から状態８２０までのモーションである。ツリーは、車両のターゲット状態８１０およびターゲット領域８４０を含み得る。本発明のいくつかの実施形態において、いくつかのターゲット状態８１０およびターゲット領域８４０が存在し得る。確率は、制御入力生成エッジ８２１に対応付けることができ、したがって状態８２０にも対応付けることができ、これは、車両の動的モデルにおける不確実性、ならびに障害物および車両の周辺環境の検知および推定における不確実性を説明することができる。

いくつかの実施形態において、エッジ８２１は、いくつかの時点にわたって制御入力を評価することによって作成されるが、他の実施形態は、各時点ごとに新たな制御入力を決定し、この場合の制御入力の決定について、本発明の他の実施形態に従って説明する。他の実施形態において、エッジ８２１は、１つまたは複数の時点にわたっていくつかの制御入力を集めることによって作成される。対象領域８４０に向かってツリーを拡大する際に、初期状態が選択され、制御入力が決定され、対応する状態シーケンスおよび最終状態が決定される。たとえば、８８０は、選択された状態であってもよく、８８１は、ツリーにエッジとして追加される軌道であってもよく、８６０は、モーションプランニングツリーにノードとして追加される最終状態である。

図９Ａは、本発明のいくつかの実施形態に係る、状態および分布のシーケンスを決定するための確率的モーションプランナー８９９のフローチャートを示す。いくつかの実施形態において、確率的モーションプランナー８９９は、ある時点におけるターゲット状態の尤度のセットを表すために粒子のセットを時間的に伝搬する粒子フィルタである。各粒子は、その時点におけるターゲット状態の値のガウス分布を含み、その時点のパラメトリック確率分布の一次モーメントは粒子の加重平均であり、その時点のパラメトリック確率分布の高次モーメントは粒子の加重共分散である。この方法は、車両の初期状態から車両のターゲット状態までの車両のモーションを指定する制御入力のシーケンスを繰り返し決定する。異なる実施形態において、初期状態は、車両の現在の状態であり、および／または初期状態は、方法の以前の繰り返し中に決定された制御入力に対応する状態である。

このモーションは、たとえば図８に示すように、車両の状態をつなぐ状態遷移によって規定される。各状態は、車両の位置、速度、および進行方向を含む。モーションは、終了条件が満たされるまで、たとえば、ある期間または予め定められた繰り返し回数にわたって繰り返し決定される。図９Ａの方法の繰り返しは以下のステップを含む。

この方法は、対応する高い確率を有する特定の状態遷移が制御目的関数のサブセットと比較的整合するように、初期状態、サンプリングされた状態のセット、および対応する状態遷移のセットを決定する９００。たとえば、この方法は、図８の状態８８０、状態遷移８８１、および状態８６０を決定する。

図９Ｂは、各状態が確率的制御関数と整合する確率を求める方法９１０のフローチャートを示す。各状態の確率を求める場合、先ず状態の衝突について確認される９１１。次の状態およびその状態に至る状態遷移に衝突がない場合、状態は制御関数のサブセットと整合すると判断され９１２、各状態の確率が計算される９１３。

図９Ｃは、自動運転車両の位置の予測９１０ｃが障害物９２０ｃの不確実領域９２１ｃと交差し障害物９２０ｃが位置９１０ｃにある確率が衝突しきい値９３０ｃよりも高い例を示す。たとえば、車両のセンサは、障害物の位置を時間の関数として求めることができる。モーションプランニングシステムは、次の状態が障害物の不確実領域と交差する確率を求め、次の状態が障害物の不確実領域と交差する確率が衝突しきい値を上回る場合、サンプリングされた状態にゼロ確率を割り当てる。

方法９１０の別の実施形態において、集計された確率が、予め定められたしきい値９１４を下回る場合、状態が制御関数と整合する確率は低いため、方法は終了し９１５、モーションプランニングアルゴリズム８９９を再開する。

図９Ｄは、各繰り返しごとに５つのサンプリングされた状態が生成される場合にステップ９００、９１０および９２０を３回繰り返した結果の簡略化された概略図を示す。初期状態９１０ｄは、時間９１１ｄにおいて、車両モーションの動的モデルおよび確率的制御関数を使用して前方に予測され、次の５つの状態は、９２１ｄ、９２２ｄ、９２３ｄ、９２４ｄ、および９２５ｄである。確率は、確率制御関数９２６ｄおよび制御関数９２６ｄの確率的に許容される偏差９２７ｄの関数として求められる。各時間ステップにおいて、すなわち各繰り返しにおいて、確率の集計を使用して、集計された制御入力および対応する状態９２０ｄを生成する。いくつかの実装形態において、確率関数は、ターゲット状態の尤度を表す粒子を時間的に伝搬するように作用する。これらの実装形態において、各状態、たとえば、９２１ｄ、９２２ｄ、９２３ｄ、９２４ｄ、および９２５ｄは粒子である。

図９Ｅは、図９Ｄの最初の繰り返しにおける５つの状態の、割り当てられた起こり得る確率を示す。確率の値９２１ｅ、９２２ｅ、９２３ｅ、９２４ｅ、および９２５ｅは、状態９２１ｄ、９２２ｄ、９２３ｄ、９２４ｄ、および９２５ｄを示すドットの相対的なサイズを選択する際に反映される。

確率分布のシーケンスを求めることは、結果として、シーケンスにおける各時間ステップごとに図９Ｅの確率の分布のような確率の分布を求めることになる。例として、分布は図９Ｅのような離散分布として表すことができる、または確率に対応付けられる離散状態は、たとえばカーネル密度平滑化部を使用して連続させることができる。

再び図９Ｄを参照して、状態９２０ｄは次の繰り返しの初期状態になり、これが再び５つのサンプリングされた状態９３１ｄ、９３２ｄ、９３３ｄ、９３４ｄ、および９３５ｄを生成する。状態９３０ｄが、この繰り返しのサンプリングされた状態の確率に従って選択される。状態９３０ｄは、次の繰り返しの初期状態である。

いくつかの実施形態は、ノードおよびエッジのツリーＧ＝（Ｖ，Ｅ）を以下のように更新する９３０。方法９００の第１の繰り返しの場合、ツリーは現在の状態で初期化され、エッジは空である。そうでない場合、９００～９２０において求められた集計された状態のシーケンスおよび制御入力のシーケンスがノードとして追加され、状態を結ぶ軌道がエッジとして追加される。たとえば、図８の８６０は追加されたノードであり、対応するエッジは８８１である。これに代えて、一実施形態において、生成されたすべての状態がツリーに追加され、その場合、決定９２０を回避することができる。

図１０Ａ～図１０Ｄは、本発明のいくつかの実施形態に係る、可能な制御関数のサブセットならびにそれぞれの決定論的および確率的要素の図を示す。

図１０Ａは、車両１０１０ａを道路上に留める４００という制御目的に対応する制御関数の図を示し、道路の境界は１０２０ａによって画定される。制御関数は、決定論的要素１０３０ａおよび確率的要素１０４０ａによって規定される。決定論的要素は、たとえば人間の運転者のデータを記録しフィット（fit）を最適化することにより、または、たとえばデータへの平均ユークリッド距離を最小化することにより、または、良好なフィットである確率を最大化することにより、決定することができる。確率的要素１０４０ａは、決定論的要素１０３０ａの周囲の記録されたデータの変動を求めることにより、決定することができる。たとえば、確率的要素１０４０ａは、確率的要素１０４０ａの変動内に記録データのすべてを含む確率を最大化することにより、決定することができる、または、有限量の記録されたデータを考慮して、データの無限量の実際の変動を推定することにより、決定することができる。

いくつかの実施形態は、制御関数は道路境界１０２０ａによって限定されるものとしてモデル化することが可能であるが、これは人間が運転する方法ではない、という認識に基づいている。むしろ人間は乗車を短くするために次々に近道をするという判断をすることがある。図１０Ｂは、道路上の車両１０１０ｂの走行時間を短縮するという制御目的に対応する制御関数の図を示し、道路境界は１０２０ｂによって画定される。制御関数は、決定論的要素１０３０ｂおよび確率的要素１０４０ｂによって規定される。本発明のいくつかの実施形態に従うと、線形または非線形ＭＰＣコントローラにおける基準追跡コストは、制御目的の各々について決定論的要素１０３０ｂおよび確率的要素１０４０ｂを使用して確率的モーションプランナーが計算する確率分布の時変一次モーメントおよび高次モーメントに基づいて、自動的に適応させることができる。

図１０Ｃは、障害物１０６０ｃを安全に追い越すという運転目的を表す制御関数の図を示し、車両１０１０ｃは、車線境界１０７０ｃを有する２車線道路を運転する。図１０Ｃは、自動運転車両または半自動運転車両が障害物回避のための操作を実行する必要があるシナリオを示す。後者が必要になり得るのは、その車線の側に静止障害物または動的障害物（たとえば歩行者、自転車、および停車または駐車車両）のいずれかがあるために現在の車線の中央に安全に留まることができない可能性があり、しかも、たとえば交通規則または他方の車線が現在遮断されているかもしれないことが原因で他方の車線に変更することが不可能であるまたは望ましくないときである。決定論的要素１０２０ｃは、たとえば、同様の状況の人間の運転者から収集されたデータから決定することができる。確率的要素１０４０ｃは、すべての状況が同じであるとは限らず、典型的には障害物１０６０ｃがどのように動くかに応じて運転者の挙動がわずかに異なることを示す。

したがって、確率的要素１０４０ｃは、追い越しが実行される領域における、より大きな変動を示す。本発明のいくつかの実施形態において、追い越しが実行される領域における、この大きな変動は、適応ＭＰＣコントローラの基準追跡コスト項における対応する重みの低減をもたらし得る。これは、ＭＰＣ基準追跡アルゴリズムが、たとえば安全に障害物を回避するという要件を満たしつつ高追跡性能を実現することのような競合する制御目的間に存在するトレードオフを、自動的に適応させることを可能にする。より具体的には、車両が周辺障害物に比較的近くなると予測される場合に基準モーションプランの不確実性が増し、基準軌道からの偏差のペナルティが減少すると予想することができ、逆もまた同様である。これは、その基準からの、すなわちモーションプランニング軌道からの、ＮＭＰＣ軌道のより大きいまたはより小さい偏差を可能にする。

図１０Ｄは、いくつかの実施形態に係る、現在の速度の維持４２０という目的の制御関数を説明することが可能な方法の図を示す。図１０Ｄは、道路の曲率の関数として、すなわち道路の旋回半径の尺度として、速度がモデル化される場合を示す。曲率が小さい、すなわちほぼ直線の道路の場合、制御関数は、現在の速度１０１０ｄに等しい決定論的要素１０２０ｄを有する。しかしながら、曲率の増加に伴い、現在の速度を維持するのが難しくなり、決定論的要素はゼロまで減少する。さらに、図１０Ｄに示すように、曲率が小さい場合、確率的要素１０３０ｄは、公称速度を中心とし小さな変動を伴う。しかしながら、曲率の増加に伴い、運転者が出す速度のより大きな変動を反映する、より大きな非対称の変動がある。本発明のいくつかの実施形態において、道路の曲率が大きい場合、所望の速度のこの大きな変動は、適応ＭＰＣコントローラの基準追跡コスト項における対応する重みの低減をもたらし得る。道路の曲率が小さい場合、所望の速度のより小さな変動は、ＭＰＣ基準追跡コスト項における対応する重みの増加をもたらし得る。

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

また、本明細書で概要を述べた各種方法またはプロセスは、さまざまなオペレーティングシステムまたはプラットフォームのうちのいずれか１つを採用した１つ以上のプロセッサ上で実行可能なソフトウェアとして符号化されてもよい。加えて、このようなソフトウェアは、複数の好適なプログラミング言語および／またはプログラミングもしくはスクリプトツールのうちのいずれかを用いて記述されてもよく、フレームワークもしくは仮想マシン上で実行される、実行可能なマシン言語コードまたは中間符号としてコンパイルされてもよい。典型的に、プログラムモジュールの機能は、各種実施形態において所望される通りに組み合わせても分散させてもよい。

また、本開示の実施形態は、方法として実施されてもよく、その一例が提供されている。この方法の一部として実行される動作の順序は任意の適切なやり方で決められてもよい。したがって、実施形態は、例示されている順序と異なる順序で動作が実行されるように構成されてもよく、これは、いくつかの動作を、例示されている実施形態では一連の動作として示されるが、同時に実行することを含み得る
本発明を好ましい実施形態の例を用いて説明してきたが、その他の各種応用および修正を本発明の精神および範囲の中で行い得ることが理解されるはずである。したがって、以下の請求項の目的は、本発明の真の精神および範囲に含まれるこのような変形および修正のすべてをカバーすることである。

Claims (8)

1. 車両を制御するためのシステムであって、前記システムは、
   前記車両の現在の状態と、前記車両の前記現在の状態の近傍の環境の画像と、前記車両の目的地とを受け付けるように構成された入力インターフェイスと、
   確率的モーションプランナーと適応予測コントローラとを格納するように構成されたメモリとを備え、前記確率的モーションプランナーは、前記車両の前記現在の状態と、前記車両の前記目的地と、前記環境の前記画像とを受け付けて、前記車両のモーションプランを規定するターゲット状態のシーケンスにわたるパラメトリック確率分布のシーケンスを生成するように構成され、前記パラメトリック確率分布のパラメータは、前記確率分布の一次モーメントと少なくとも１つの高次モーメントとを規定し、前記高次モーメントは、確率的モーションプランナーの信頼度を示し、前記適応予測コントローラは、予測範囲にわたってコスト関数を最適化して、前記車両の１つまたは複数のアクチュエータに対する制御コマンドのシーケンスを生成するように構成され、前記コスト関数の前記最適化は、前記パラメトリック確率分布の前記一次モーメントによって規定される前記ターゲット状態のシーケンスを追跡するコストを、前記車両のモーションの、性能メトリックのコストに対して均衡させ、前記ターゲット状態のシーケンスにおける異なる状態変数は、前記追跡するコストを均衡させる際の前記確率分布の前記高次モーメントのうちの１つまたは複数を用いて重み付けされ、前記システムはさらに、
   プロセッサを備え、前記プロセッサは、前記車両の前記現在の状態と、前記車両の前記目的地と、前記環境の画像とを前記確率的モーションプランナーに送ることによって前記確率的モーションプランナーを実行するように構成され、かつ、前記確率的モーションプランナーによって生成された前記パラメトリック確率分布のシーケンスを前記適応予測コントローラに送ることによって前記適応予測コントローラを実行して前記制御コマンドのシーケンスを生成するように構成され、前記システムはさらに、
   前記適応予測コントローラによって決定された少なくとも１つの制御コマンドを前記車両の少なくとも１つのアクチュエータに出力するように構成された出力インターフェイスを備える、システム。
2. 前記適応予測コントローラの実行レートは前記確率的モーションプランナーの実行レートよりも大きく、前記プロセッサは、前記確率的モーションプランナーの各実行ごとに少
   なくとも１度前記適応予測コントローラを実行する、請求項１に記載のシステム。
3. 前記モーションプランは、前記ターゲット状態のシーケンスにわたる前記パラメトリック確率分布のシーケンスを、前記予測範囲よりも大きい期間の時間の関数として規定する、請求項２に記載のシステム。
4. 前記車両の各ターゲット状態は複数の状態変数を含み、各パラメトリック確率分布は、各状態変数の分布についてのパラメータを規定する多変数分布であり、前記パラメトリック確率分布の前記一次モーメントは平均値であり、前記パラメトリック確率分布の前記高次モーメントは値が空間および時間において変化する共分散行列である、請求項１に記載のシステム。
5. 前記確率的モーションプランナーは、粒子のセットを時間的に伝搬させてある瞬間における前記ターゲット状態の尤度のセットを表す粒子フィルタを含み、各粒子は、前記瞬間における前記ターゲット状態の値のガウス分布を含み、前記瞬間の前記パラメトリック確率分布の前記一次モーメントは、前記粒子の加重平均であり、前記瞬間の前記パラメトリック確率分布の前記高次モーメントは前記粒子の加重共分散である、請求項１に記載のシステム。
6. 前記確率的モーションプランナーは制御関数のセットを使用し、各制御関数は、前記車両の前記現在の状態を、対応する制御目的に基づいてターゲット状態に遷移させるように構成され、前記制御関数のうちの少なくともいくつかは、確率的であり、前記現在の状態を前記ターゲット状態に遷移させるための決定論的要素と、前記決定論的要素によって決定される前記ターゲット状態の周りの値の確率的分布を求めるための確率的要素とを含み、確率的制御関数の出力は、前記確率分布の一次モーメントと少なくとも１つの高次モーメントとを規定する前記ターゲット状態にわたるパラメトリック確率分布である、請求項１に記載のシステム。
7. 車両を制御する方法であって、前記方法は、確率的モーションプランナーと適応予測コントローラとを格納するメモリに結合されたプロセッサによって実行され、前記確率的モーションプランナーは、前記車両の現在の状態と、前記車両の目的地と、環境の画像とを受け付けて、前記車両のモーションプランを規定するターゲット状態のシーケンスにわたるパラメトリック確率分布のシーケンスを生成するように構成され、前記パラメトリック確率分布のパラメータは、前記確率分布の一次モーメントと少なくとも１つの高次モーメントとを規定し、前記高次モーメントは、確率的モーションプランナーの信頼度を示し、前記適応予測コントローラは、予測範囲にわたってコスト関数を最適化して、前記車両の１つまたは複数のアクチュエータに対する制御コマンドのシーケンスを生成するように構成され、前記コスト関数の前記最適化は、前記パラメトリック確率分布の前記一次モーメントによって規定される前記ターゲット状態のシーケンスを追跡するコストを、前記車両のモーションの、性能メトリックのコストに対して均衡させ、前記ターゲット状態のシーケンスにおける異なる状態変数は、前記追跡するコストを均衡させる際の前記確率分布の前記高次モーメントのうちの１つまたは複数を用いて重み付けされ、前記プロセッサは、前記方法を実現する、格納されている命令に結合され、前記命令は前記プロセッサによって実行されると前記方法のステップを実行し、前記方法は、
   前記車両の現在の状態と、前記車両の前記現在の状態の近傍の環境の画像と、前記車両の目的地とを受け付けるステップと、
   前記車両の前記現在の状態と、前記車両の前記目的地と、前記環境の画像とを前記確率的モーションプランナーに送ることによって前記確率的モーションプランナーを実行するステップと、
   前記確率的モーションプランナーによって生成された前記パラメトリック確率分布のシ
   ーケンスを前記適応予測コントローラに送ることによって前記適応予測コントローラを実行して前記制御コマンドのシーケンスを生成するステップと、
   前記適応予測コントローラによって決定された少なくとも１つの制御コマンドを前記車両の少なくとも１つのアクチュエータに送るステップとを含む、方法。
8. 方法を実行するためのプロセッサが実行可能なプログラムが実現される非一時的なコンピュータ読取可能記憶媒体であって、前記媒体は、確率的モーションプランナーと適応予測コントローラとを格納し、前記確率的モーションプランナーは、車両の現在の状態と、前記車両の目的地と、環境の画像とを受け付けて、前記車両のモーションプランを規定するターゲット状態のシーケンスにわたるパラメトリック確率分布のシーケンスを生成
   するように構成され、前記パラメトリック確率分布のパラメータは、前記確率分布の一次モーメントと少なくとも１つの高次モーメントとを規定し、前記高次モーメントは、確率的モーションプランナーの信頼度を示し、前記適応予測コントローラは、予測範囲にわたってコスト関数を最適化して、前記車両の１つまたは複数のアクチュエータに対する制御コマンドのシーケンスを生成するように構成され、前記コスト関数の前記最適化は、前記パラメトリック確率分布の前記一次モーメントによって規定される前記ターゲット状態のシーケンスを追跡するコストを、前記車両のモーションの、性能メトリックのコストに対して均衡させ、前記ターゲット状態のシーケンスにおける異なる状態変数は、前記追跡するコストを均衡させる際の前記確率分布の前記高次モーメントのうちの１つまたは複数を用いて重み付けされ、前記方法は、
   前記車両の現在の状態と、前記車両の前記現在の状態の近傍の環境の画像と、前記車両の目的地とを受け付けるステップと、
   前記車両の前記現在の状態と、前記車両の前記目的地と、前記環境の画像とを前記確率的モーションプランナーに送ることによって前記確率的モーションプランナーを実行するステップと、
   前記確率的モーションプランナーによって生成された前記パラメトリック確率分布のシーケンスを前記適応予測コントローラに送ることによって前記適応予測コントローラを実行して前記制御コマンドのシーケンスを生成するステップと、
   前記適応予測コントローラによって決定された少なくとも１つの制御コマンドを前記車両の少なくとも１つのアクチュエータに送るステップとを含む、非一時的なコンピュータ読取可能記憶媒体。

Images