JP2020040651A - 自律型システム内の組み込みリアルタイム制約付き最適制御問題に対する平滑化及び正則化されたｆｉｓｃｈｅｒ−ｂｕｒｍｅｉｓｔｅｒ解決法 - Google Patents

Abstract

【課題】自律車両を制御するためのシステムを提供する。【解決手段】自律車両を制御するためのシステムは有する。システムは制御装置を含む。各サンプリング時間において、制御装置は、センサ値と、センサ値及び動的モデルに基づいて計算された推定値とを入力して、センサ値及び推定値を制御装置のメモリに記録する。制御装置は、センサ値及び推定値を、制約付き最適制御問題に付随する最適性の条件に組み込み、最適性の条件を、Ｆｉｓｃｈｅｒ−Ｂｕｒｍｅｉｓｔｅｒ関数を用いて非平滑システムにマッピングし、非平滑システムを近似する平滑化されたシステムにニュートン法反復を行って解に収束し、車両動作中にアクチュエータを制御するコマンドを出す。【選択図】図７

Description

関連出願
本出願は、出願番号第１５／７０５，９３４号（２０１７年９月１５日に出願）及び出願番号第１５／８６６，７６７号（２０１８年１月１０日に出願）に関連する。なお、これらの文献の全体の内容は参照により本明細書に組み込まれている。

本開示は全般的に、自律システムに対する方法及び制御に関し、より詳細には、自律車両を制御する（車両安全性限界を表すアクチュエータ限界に関する制約を受ける経路追従誤差を最小限にすることを含む）ための方法に関する。本方法及び制御では、正則化及び平滑化されたFischer-Burmeister技術を適用して、目標経路からの車両の距離をモデリングしてリアルタイムで最小限にする。

車両のアクチュエータがセンサからの入力を用いて制御装置によって制御される自律車両が開発されている。現在、車両サブシステムの制御の改善が行われている。たとえば、典型的に１つまたは複数の制御装置をエンジン制御ユニット（ＥＣＵ）に組み込んで、センサが車両性能を検出することに応じて種々の自動車用アクチュエータを制御して、車両エンジンの制御を最適化する場合がある。車両性能を検出するためのセンサには、エンジン速度及び車両速度を検出するセンサが含まれている場合がある。他のセンサ（たとえば、カメラ及び種々のタイプのレーダー）がドライバーアシストシステムの一部として車両内にますます搭載されている。車両がバックしている間に車両の後方からの視野を得るためにビデオカメラが用いられている。レーダーアンテナは近くの物体の表示を得る働きをする。車両とともに用いるためにさらなるタイプのセンサ技術が開発されている。さらなるセンサ技術の例はＬＩＤＡＲである。これはレーザー光線を用いて目標物を照らすものである。ＬＩＤＡＲは物体までの距離を測定することができ、障害物検出用に使用される場合がある。ＬＩＤＡＲは、能動的なドライバーアシスト技術（たとえば、ステアリング及び加速度補償）の一部として用いられている。たとえば、ステアリング及び加速度補償は、車両が別の車両のすぐ後ろに続いている場合に、または車両が別のレーン内または道路の端近くに方向を変えているときに実施される場合がある。

車両制御は、単純なルールから本格的な最適化プロセスまで、多くの形態を取り得る。最適化問題を解くことによって制御を行うと、時間及び必要な計算能力が多大になる可能性がある。言い換えれば、最適化問題の解を得るには多大な計算能力が必要となる場合があり、リアルタイムで行うことはできない場合がある。それでも、最適化問題の解をリアルタイムで得る問題に対処する研究が行われている。

リアルタイムの組み込み最適化問題の重要な例はモデル予測制御（ＭＰＣ）である。モデル予測制御では、各サンプリング時間の間に、後退ホライズン（receding horizon）に対する最適制御問題が解かれる。以下を参照のこと。L.Grune及びJ.nnekの「Nonlinear model predictive control」（Nonlinear Model Predictive Control, pp.43-66, Springer,2011）、J. B. Rawlings及びD. Q. Mayneの「Model predictive control: Theory and design」（Nob Hill Pub., 2009）及びG. C. Goodwin, M. M. Seron及びJ. A. De Donaの「Constrained control and estimation: an optimisation approach」（Springer Science & Business Media, 2006）。それぞれその全体が参照により本明細書に組み込まれている。離散時間線形二次ＭＰＣ定式化に対する最適制御問題を、凸二次計画（ＱＰ）と表現することができる。さらに、凸ＱＰは、非線形モデル予測制御（ＮＭＰＣ）たとえば逐次二次計画法（ＳＱＰ）またはリアルタイム反復方式（時間ステップあたり１つのＱＰだけを解く）で用いる多くのアルゴリズムに対する基礎を形成する。以下を参照のこと。P. T. Boggs及びJ. W. Tolleの「Sequential quadratic programming」（Acta numerica, vol.4, pp.1-51, 1995、S. Gros、M. Zanon、R. Quirynen、A. Bemporad及びM. Diehlの「From linear to nonlinear mpc: bridging the gap via the real-time iteration」（International Journal of Control、pp. 1-19、2016）、及びM. Diehl、H. G. Bock、J. P. Schloder、R. Findeisen、Z. Nagy及びF. Allgowerの「Real-time optimization and nonlinear model predictive control of processes governed by differential-algebraic equations」（Journal of Process Control、vol. 12、No.4、pp.577-585、2002）。それぞれその全体が参照により本明細書に組み込まれている。

車両サブシステムにモデル予測制御（ＭＰＣ）が適用されている（たとえばエンジン制御）。たとえば、レートベースのＭＰＣアプローチは、予測制御モデルにさらなる一体状態を加えることによって、一体型動作を組み込んでゼロ定常偏差を保証する。ＭＰＣモデルでは、多くの異なるエンジン動作範囲（燃料比及びエンジン速度）を用いて、各範囲に対して制御装置を形成してエンジンアクチュエータを制御する。

ディーゼルエンジンエアフローに適用するモデル予測制御の具体例では、エンジン内のフローの制御を、可変ジオメトリータービン（ＶＧＴ）、ＥＧＲスロットル、及びＥＧＲバルブアクチュエータを用いて行う。これらのシステムは強く結合されており、高度に非線形である。

しかし、組み込まれた自動車応用における制約付き最適制御に対する既存の方法では、必要な時間内で必要な計算を実行することができていない。ディーゼルエンジンに対する制御システム例では、エンジン制御ユニット（ＥＣＵ）は入力信号をサンプリングして、センサからの測定値を記録し、計算を実行して、コマンドを出す。これらの動作はすべて、入力信号サンプリング時間内に行わなければならない。詳細には、ＥＣＵには、すべての必要な計算を終了するために、サンプリング時間の固定割合が割り当てられる（計算の予算と言う）。

近年、ＱＰ及びより一般的な最適化問題の両方をオンラインで解くための速くて信頼性の高いアルゴリズムの開発に関する数多くの研究により、従来技術が著しく進歩した。しかし、オンライン最適化アルゴリズムの計算負担を管理することは、依然として問題である。

自律車両動作を、すべての必要な計算を計算の予算内で遂行するのに十分に速い時間で制御する（特に、アクチュエータを車両安全性限界内に制御する）ためのモデル予測制御装置を提供することが望ましい。

前述の「背景技術」の説明は、本開示の文脈を全般的に示すためのものである。この背景技術セクションで説明した範囲までの発明者の業績、ならびに本来は出願時における従来技術として適格ではあり得ない説明の側面は、本発明に対する従来技術として明示的にも黙示的にも認められない。

本開示及びその付随する優位点の多くをより完全に理解することは、これらが以下の詳細な説明を添付図面と共に考慮して参照することでより良好に理解されるため、容易になされる。

自律車両の概略図である。 図１の自律車両に対する制御構造である。 本開示の典型的な態様による経路追従モデルを例示する図である。 ＭＰＣ最適化問題に対するメイン関数を示す図である。 ＭＰＣ最適化問題に関連する方程式を取得する方法に対するフローチャートである。 正則化平滑化Fischer-Burmeister法に対するフローチャートである。 自律車両のリアルタイム制御に対するフローチャートである。 本開示の典型的な態様によるステアリング制御の例に対するフローチャートである。 典型的な制御装置に対するブロック図である。

本明細書で用いる場合、「一実施形態」または「いくつかの実施形態」または「実施形態」に言及することは、実施形態と共に説明される特定の要素、特徴、構造、または特性が少なくとも１つの実施形態に含まれることを意味する。明細書のいろいろな場所に語句「一実施形態では」が現れるが、必ずしもすべてが同じ実施形態に言及しているわけではない。本明細書で用いる条件付き言語、たとえば、とりわけ「ｃａｎ」「ｃｏｕｌｄ」「ｍｉｇｈｔ」「ｍａｙ」「ｅ．ｇ．」などは、特に記載のない限り、または使用した文脈内で他の方法で理解される場合を除いて、全般的に、ある特徴、要素及び／またはステップがある実施形態には含まれ、他の実施形態には含まれないと伝えることが意図されている。加えて、冠詞「ａ」及び「ａｎ」は、本出願及び添付の請求項で用いる場合、他に特に規定がない限り、「１つ以上の」または「少なくとも１つの」を意味すると解釈すべきである。

次に図面（いくつかの図の全体にわたって同様の参照数字は同一または対応部分を示す）を参照して、以下の説明は、自律車両内の組み込みモデル予測制御に適用される制約付き最適化に対する技術に関する。

自律車両は、カスタム車両または変更された従来車両という形を取ってもよい。自律車両には、センサ、アクチュエータ、ソフトウェア実施のアルゴリズム、及びソフトウェアを実行するための少なくとも１つのプロセッサの組み合わせが含まれていてもよい。ナビゲーション及びガイダンス、運転及び安全性、ならびに性能を得るために、サブシステム内でセンサ及びアクチュエータを用いてもよい。ナビゲーション及びガイダンスに対するサブシステムはグローバルポジショニングシステム（ＧＰＳ）である。ＧＰＳは人工衛星から受信した信号に基づいて現在位置を計算する。

運転及び安全性に対するサブシステムには典型的に、ビデオカメラのアレイが含まれる。ビデオカメラは、レーンの場所を検出し、物体を検出し、マーカーを検出するために使用し得る画像を取り込んでもよい。物体を検知するための別のタイプのデバイスはＬＩＤＡＲシステムである。ＬＩＤＡＲシステムによって周囲環境についての正確な３Ｄ情報が得られる。このデータを用いて、プロセッサは、物体識別、動きベクトル決定、衝突予測、及び回避方略を実施する。ＬＩＤＡＲユニットによって、自動車の頂部にある回転スキャニングミラーアセンブリを用いることによって３６０°視野が得られる。

車両性能を制御するためのサブシステムには、電力制御に関係するセンサ及びアクチュエータが含まれていてもよい。電力制御には、所要電力を測定及び管理することが含まれていてもよい。車両性能の制御には、内燃機関に対する制御が伴っていてもよい。

本開示では、車両ダイナミクス（車両の加速及びステアリングを考慮する）と、経路の運動学（時間に対する経路フレームの動きを考慮して、道路に沿っての車両の距離と目標経路とを最小限にする）とを用いる。この距離の原因は、車両の動きと経路フレームの動きとの間の不一致であり得る。本開示では、経路は曲率によって規定し、ある点での位置、速度、及び加速度を含む運動学的特徴によって表す場合がある。軌跡は、所与の速度における時間に対する動きの経路として規定する。車両ダイナミクスは、力の作用の下での車両の動きの普通の意味を取る。

本開示では、車両安全性を、アクチュエータ範囲及びレートと障害物衝突包絡線とによって規定する。アクチュエータ範囲は、アクチュエータの動きの範囲に対する限定によって規定する。アクチュエータレートは、アクチュエータが反応するレート（または遅延）に対する限定によって規定する。車両制御は、制御システムがアクチュエータに出すコマンドによる。障害物衝突包絡線は、潜在的障害物までの距離の範囲である。

図１を参照して、車両４０１（以後、自律車両の例として記載する）には、センサ４０３のアレイが含まれる。センサは車両の屋根に搭載してもよいし、車両の本体内に含めてもよいし、またはそれらの組み合わせであってもよい。自律車両内に搭載してもよいセンサのタイプとしては、ＬＩＤＡＲ、ビデオカメラ、レーダーアンテナ、ソーナアンテナを挙げてもよい。自律車両は、車両の種々の状態（たとえば、ステアリング角及び車両速度）をモニタするための内部センサが含まれていてもよい。

図２を参照して、自律車両制御システムに対する制御構造には、車両モデル予測制御装置（ＭＰＣ）２０１、車両アクチュエータ２０３、及び推定装置２０５が含まれていてもよい。ＭＰＣ２０１は、車両の速度、加速度、及びステアリング角（タイヤの角度）についての入力情報を車両センサ２０３から受け取る。推定装置２０５はパラメータ（目標経路の弧長、車両の中心と目標経路との間のずれ、方位誤差、及び目標経路の曲率を含む）を計算してもよい。また推定装置２０５は障害物包絡線を検出することができてもよい。種々の信号入力及び推定パラメータが与えられたら、ＭＰＣ２０１は、速度コマンド及びステアリングコマンド（タイヤ角度コマンド）を車両アクチュエータ２０３に出力してもよい。これらのアクチュエータに関連する車両アクチュエータには、ステアリングアクチュエータ及びエンジン速度アクチュエータが含まれていてもよい。

ＭＰＣ２０１は車両アクチュエータ２０３（たとえば、ステアリングアクチュエータ及びエンジン速度アクチュエータ）を制御する。その際に、アクチュエータの駆動範囲を安全を得るために限定してもよい。一実施形態では、アクチュエータ限界はハード制約（違反することができない制約）として実施される。アクチュエータ限界は、アクチュエータ範囲（制限された移動範囲）及びアクチュエータレート（アクチュエータが反応する制限されたレート／遅延）によって特徴付けられてもよい。一実施形態では、タイヤ角度コマンドを−３０〜＋３０度の範囲に限定してもよい。

加えて、自律車両の場合、衝突回避を目的としてアクチュエータ限界を規定してもよい。自律車両は、任意の所与時間において障害物から固定距離にあると考えることができる。その後に、アクチュエータ限界（たとえば、ステアリング角及び加速限界）を、所与の速度においてオペレータが人間の応答時間内で衝突を阻止できる解析最大値として規定してもよい。一実施形態では、車両速度の関数としてのアクチュエータ限界を、ルックアップテーブルとして事前に記憶してもよい。

障害物から必要な距離を障害物衝突包絡線として規定してもよい。一実施形態では、障害物（たとえば、別の車両）が衝突包絡線内に入り得ることを車両センサが検出することを、ＭＰＣ２０１によって回避されるハード制約として実施してもよい。

ハード不等式制約を、人間の応答時間内の十分短い時間で計算することは難しい。本開示で取り入れたアプローチは、不等式制約を等式制約として再定式化することである。以下でより十分に説明するように、再定式化した等式制約によって、ニュートン法を用いた高速解法が可能になる。

またＭＰＣ２０１は、ソフト制約に基づいて車両アクチュエータ２０３を制御してもよい。ソフト制約は、解析的に違反することができる制約である。ソフト制約の違反は、制御コスト関数におけるペナルティとして（外部ペナルティとしてかまたはさらなる最適化パラメータとして）扱われる。その後、制御コスト関数は、望ましくない状態から離れる傾向がある。一実施形態では、ソフト制約を快適さ関連駆動と言ってもよいものに適用する。ソフト制約の種類は、加加速快適さ（加加速度は加速度の微分）、加速快適さ（所定閾値よりも大きい加速度にペナルティを課す）、横加速度、ロール（長手軸の周りの回転量）、及び牽引／スリップ制約（たとえば濡れた道路状態に起因する牽引の欠如またはスリップを伴う）を含んでもよい。

自律車両のステアリング及び加速を、エンジン制御ユニット（ＥＣＵ）に組み込まれたシステム（典型的に小さいマイクロ制御装置）を用いて所定の時間内で制御してもよい。詳細には、効果的であるために、ＭＰＣ制御装置２０１、及び推定装置２０５が行う制御動作を、センサ読み取りが行われる各サンプリング時間の間に行うべきである。

一実施形態では、ＥＣＵは約１００ｘ／秒でサンプリングしてもよい。サンプリング時間の間、ＥＣＵはセンサからの測定値を記録し、最適化計算を実行し、コマンドを出す。こうして、ＥＣＵは好ましくは、その必要な動作を、サンプリング時間の固定割合（計算の予算と言う）内で実行する。

全般的に、制御動作は、制約を受ける最適化問題の解と考えてもよい。離散時間線形二次ＭＰＣ定式化に対する最適制御問題を表現する１つの方法は、凸二次計画（ＱＰ）としてである。

本開示の態様は、各サンプリング時間の間に最適制御問題を解くＭＰＣである。

本開示の態様は、凸ＱＰを解くことにFischer-Burnmeister（ＦＢ）関数及びニュートン法を適用するＭＰＣである。以下ではこれを、Fischer-Burmeister正則化平滑化（ＦＢＲＳ）アプローチと言う。ＦＢＲＳには、組み込み最適化に対して魅力的であるいくつかの特性がある。第１に、ＦＢＲＳは解に対するグローバル線形収束及び二次漸近収束（それがその性質から減速一般化ニュートン法として引き継ぐ特性）を表示する。第２に、簡単に実施できる。最後に、関連するＱＰのシーケンスを解くときに効果的にウォームスタートさせることができる。

図３に例示するのは、本開示の典型的な態様による経路追従モデル（path tracking model）の要素である。本開示では、ＭＰＣは、固定されたグローバル位置の代わりに動的な経路フレームに基づくモデルを用いる。このモデルを用いるのは、ＭＰＣが目標経路からの車両の距離を最小限にしようとするからである。そうするために、モデルは、ずれＬ３１１ならびにずれＬの微分をモニタする。ずれＬは、車両が動く場合（すなわち、経路はまっすぐ進んで、車両が方向を変える）または経路が動く場合（すなわち、経路が方向を変えて、車両はまっすぐ進む）の両方の場合に変化する。したがって、モデルは動的な経路フレームを用いる。

図３に見られるように、グローバルフレーム（Ｘ，Ｙ）３０１は、定常点の場所（たとえば、建物または何らかの他の固定構造物の場所）を表す。車両の中心３０３はグローバルフレーム３０１に対する点である。車両に垂直で車両の中心３０３を通っているライン３０５は、目標経路３０９上で点３０７と交差する。目標経路の交点における点３０７は投影位置である。その点３０７で、目標経路３０９は経路方位３１３を有する。投影位置及び経路方位３１３によって、経路フレーム（Ｘ，Ｙ）が規定される。車両が速度方向３２７に移動すると、車両方向方位と、目標経路接線３１７と平行な方向との間に方位誤差３１５が生じ得る。目標経路３０９に沿っての投影位置３０７までの距離を、弧長３２１によって規定する。投影位置３０７における曲線は半径３２３を有する。ＭＰＣは車両ステアリング（タイヤ角度３２５）を調整して目標経路からの車両の距離を最小限にする。

経路追従モデルでは、ＭＰＣ制御装置２０１は、車両加速度ａ、車両速度Ｖ、及び実際のタイヤ角度δ、ならびに推定装置２０５から得られる推定値に基づいて、力コマンドＦ^ｃｍｄ及びタイヤ角度コマンドδ^ｃｍｄを生成する。推定値には弧長ｓ、ずれｌ、及び方位誤差θが含まれる。動的な経路フレームを、点車両加速度ａ、車両速度Ｖ、弧長ｓ、目標経路からのずれｌ、方位誤差θ及びタイヤ角度δによって表してもよい。モデルは、タイヤ角度コマンドδ^ｃｍｄ及び加速度コマンドＦ^ｃｍｄに対するアクチュエータレート制約を考慮してもよい。また、モデルは、横速度がなく、車両に対する力及び車両加速度が車両の長手方向にあるようなものであってもよい。しかし、経路追従モデルでは横速度及び横加速度も考慮に入れてもよい。動的な経路フレームの代替表現は、曲率ｋ、曲率対弧長の変化∂ｋ／∂ｓ、基準速度及び加速度によるものであってもよい。

図４に関して、ＭＰＣ最適化問題に対するメイン関数を示す。ＭＰＣは、曲率ｋにおける速度追従誤差（Ｖｋ−Ｖｋ^ｔｇｔ（ｓ）、ずれ距離（ｌ_ｋ）、方位誤差（θ_ｋ）、ステアリングレート（Δδ_ｋ ^ｃｍｄ−Δδ_ｋ ^ｆｆ）、及び加速率（ΔＦ_ｋ ^ｃｍｄ−ΔＦ_ｋ ^ｆｆ）の関数としての、ステアリングレートΔδ^ｃｍｄ及び加速率ΔＦｃｍｄを最小限にしようとする４０３。曲率ｋは典型的に、半径の逆数として規定され、弧長ｓの関数である。メイン関数は、ステアリングレートΔδ^ｃｍｄ及び加速率ΔＦ^ｃｍｄに対するハード制約を受け得る。一実施形態では、スリップを、経路方位からのずれ−（方位＋スリップ角）に対してペナルティを課すことによって考慮してもよい。

図５に関して、ＭＰＣ最適化問題を解くために、Ｓ５０１において、図４のメイン関数を最適化すべきコスト関数として設定する。Ｓ５０３において、制約を適用すべき制約として設定する。Ｓ５０５において、コスト関数及び制約を線形近似を用いて変換して、二次計画（ＱＰ）を得る。これをＳ５０７においてＦＢＲＳを用いて解くことになる。Ｓ５０５におけるＱＰ例は次の形式である。
ｍｉｎ（ｚ）ｚ^ＴＨｚ＋ｆ^Ｔｚ（Ａｚ≦ｂという条件で）
ここで、Ｈ∈Ｓ^ｎはヘッセ行列であり、
である。簡単にするために、ＱＰは等式制約がない場合に対してであってもよい。当業者であれば分かるように、等式及び不等式制約問題に拡張することは簡単である。この例の場合、ＱＰに対するKarush-Kuhn-Tucker（ＫＫＴ）条件は以下の通りである。

Ｈｚ＋ｆ＋Ａ^Ｔｖ＝０

ｖ^Ｔｙ＝０

ｖ≧０，ｙ≧０

変数ｚは制御動作、特にコスト関数を最小限にする制御動作を表す。変数ｖは感度を表す。感度は、コスト関数が制約の影響をどれほど受けやすいかを定量化するシステムで計算された内部変数である。変数ｙ＝ｂ−Ａｚは制約マージンを表す。ｙが正ならば、制約を満足している。ｙが負ならば、制約を満足していない。

ＦＢ関数を用いて最適性の条件を解くために（Ｓ５０７）、ＦＢ変換を用いて条件を関数Ｆ（ｘ）と書き換える。たとえば、相補性条件ｖ≧０、ｙ≧０、ｙ^Ｔｖ＝０をＦＢ方程式φ（ｙ，ｖ）と置換する。ここで、ｘ＝（ｚ，ｖ）及びｙ＝ｂ−Ａｚである。ＦＢ関数を相補性条件に適用して、以下の非線形マッピングを得る。

ＦＢ関数の特性のために、関数Ｆ（ｘ）は、ｘがＫＫＴ条件を満たすときに限りＦ（ｘ）＝０であるという特性を有する。

ＦＢＲＳアルゴリズムは、減少シーケンスε_ｊ→０に対するニュートン法を用いて一連の問題Ｆ_εｊ（ｘ_ｊ）＝０を近似的に解くことによって機能し、このようにして、他のホモトピー方法（たとえば、内点法（ＩＰＭ））と同様である。しかし、内点法とは異なり、部分問題はε_ｊ→０のときに不良条件にもならず解くのが困難にもならず、実際には、有限数のホモトピーステップの後にε_ｊを正確にゼロに設定することができる。

マッピングＦ（ｘ）は一価ではあるが連続微分可能ではない。しかしＦ（ｘ）は半平滑であるため、半平滑ニュートン法を求根問題Ｆ（ｘ）＝０に適用することができる。Ｆが平滑化されていると（ε＞０）、Ｆ_ε（ｘ）は連続微分可能であるため、（正規の）ニュートン法を適用することができる。

図６に関して、Ｆ（ｘ）＝０を解くために、半平滑ニュートン法を適用して、以下の反復が得られる。

ここで、Ｖ_ｋ∈∂_ＣＦ_εｋ（ｘ_ｋ）であり、ｔ_ｋ∈（０，１］は、Ｓ６０３において、解から遠い場合に収束を適用するために直線探索によって選択されたステップ長である。反復行列またはヤコビアンＶ_ｋは任意のε＞０に対して常に非特異であるが、εが小さい場合に不良条件になる場合がある。状況を改善するために、Ｓ６０１において、近接正則化項を加えて、数値調整の制御を助ける。正則化及び平滑化方程式は以下のように規定される。

ここでδ≧０は正則化を制御する。したがって、修正ニュートン反復（Ｓ６０５）は以下のようになる。

ここで、Ｋ_ｋ＝Ｖ_ｋ＋∇_ｘＲ（ｘ_ｋ，δ）をＶ_ｋの代わりに用いる。

ＦＢＲＳに対するアルゴリズムを以下のアルゴリズム１にまとめる。アルゴリズムでは、直線探索及びホモトピーを用いてグローバル化された半平滑ニュートン法を表している。アルゴリズムの各ステップをグローバル化するために用いるメリット関数を以下のように規定する。

パラメータσ∈（０，０．５）は、メリット関数においてどのくらいの減少が必要であるかをエンコードする。所望の許容範囲をτと示し、β∈（０，１）はバックトラッキング直線探索における減少を制御する。ｔ_ｋを計算するためのより高性能なアルゴリズム（たとえば、多項式補間）を、バックトラッキング直線探索の代わりに用いることができるが、実際には、この単純なアルゴリズムが効果的である。Ｃ．Ｔ．Ｋｅｌｌｅｙ，Ｉｔｅｒａｔｉｖｅ ｍｅｔｈｏｄｓ ｆｏｒ ｏｐｔｉｍｉｚａｔｉｏｎ．ＳＩＡＭ，１９９９を参照のこと。この文献はその全体において参照により本明細書に組み込まれている。固定パラメータに対する典型的な値はσ≒１０^−４及びβ≒０．７である。

前述の説明の特徴を含むシステムによって多くの優位点が得られる。詳細には、本明細書で説明するＭＰＣは、目標経路からの車両の距離を、各センササンプリング時間においてリアルタイムで最適化して最小限にすることができる。図７に示すのは、車両最適制御の反復解法をリアルタイムで行うためのフローチャートである。Ｓ７０１及びＳ７０３に関して、適用すべきコスト関数及び制約（たとえば、図４における）を、車両が出荷される前にオフラインで規定してもよい。Ｓ７０５において、センサ読み取り値をサンプリングして推定値を計算して、現在の車両アクチュエータ状態を測定する。Ｓ７０７において、測定したシステム状態を制約付き最適制御問題に付随する最適性の条件に組み込む。Ｓ７０９において、Fischer-Burmeister関数変換を通して最適性条件を方程式に変換する。Ｓ７１１において、修正ニュートン法を用いて反復を行って、Ｓ７１３において制御値を取得する。一実施形態では、サンプリング時間の間に修正ニュートン法を１回の反復に対して行う。コスト関数によってサンプリング時間の必要な時間の十分範囲内である十分な解が形成されると決定された。Ｓ７１５において、制御値を送信して車両を制御する。残りの時間を、Ｓ７１７において、次のサイクルが始まるまで待つことに使う。車両最適制御を、各センササンプリング時間において、組み込みリアルタイム制御問題解決法を用いてオンラインで行ってもよい。

図８に示すのは、本開示の典型的な態様によるステアリング制御の例に対するフローチャートである。ステアリング制御は、車両センサ（タイヤ角度）から現在のステアリング状態を決定することによって行われ、将来における多くの点（たとえば、１２０点）として表される所定の予測水平線に対して、ステアリング角をこれらの将来の点に対して、将来のある時間に対して予測する。

詳細には、Ｓ８０１において、車両の現在の状態を検出する（測定された位置、速度、タイヤ角度、ずれ、方位）。Ｓ８０３において、５秒以下の時間の間に目標経路に基づいて経路フレームを予測する。Ｓ８０５において、候補制御動作の組（たとえば、ステアリングを１秒、２秒、３秒…５秒で行う）（ハード制約を満たす良好な解）を決定する。Ｓ８０７において、各候補に対する予測を行う。Ｓ８０９において、予測と所望（ターゲット）とを比べて誤差を取得し、最適化を行って、予測解をターゲットに近づける制御動作を得る。Ｓ８１１において、ステアリングアクチュエータに制御コマンドを出す。

コンピュータ−ベースの制御システム２０１は、マイクロ制御装置に基づいていてもよい。マイクロ制御装置には、１つ以上のプロセッサコア（ＣＰＵ）とともにメモリ（揮発性及び不揮発性）及びプログラム可能な入出力周辺機器が含まれていてもよい。フラッシュ、ＲＯＭ、ＥＰＲＯＭ、またはＥＥＰＲＯＭの形態のプログラムメモリがチップ上に、ならびにデータ記憶装置用のセカンダリＲＡＭ上に含まれていることが多い。一実施形態では、図９に関して、コンピュータ−ベースのシステム２０１は、マイクロ制御装置９１０を伴う集積回路基板９０１である。基板には、デジタルＩ／Ｏ９１５、アナログ入力９１７、及びハードウェアシリアルポート９１３が含まれていてもよい。

一実施形態では、マイクロ制御装置は、８ビットのＲＩＳＣベースのマイクロ制御装置であって、フラッシュメモリ９０３、ＳＲＡＭ９０７、ＥＥＰＲＯＭ９０５、汎用Ｉ／Ｏライン、３２個の汎用レジスタ、リアルタイムカウンタ、６個のフレキシブルタイマ／カウンタ、及びＡ／Ｄコンバータ９０９を有している。マイクロ制御装置は単一のＳＯＣであってもよい。当然のことながら、種々のタイプのマイクロ制御装置を用いてもよい。マイクロ制御装置は、処理コアの数、不揮発性メモリのサイズ、データメモリのサイズに基づいて、ならびにそれらにＡ／ＤコンバータまたはＤ／Ａコンバータが含まれているか否かに基づいて変わる。

前述した教示を考慮すれば、多くの変更及び変形が可能である。したがって、添付の請求項の範囲内で、本発明を本明細書で具体的に述べたこと以外で実施してもよいことを理解されたい。

したがって、前述の説明では単に本発明の典型的な実施形態を開示して説明している。当業者であれば分かるように、本発明を、その趣旨または本質的特徴から逸脱することなく他の具体的な形式で具体化してもよい。したがって、本発明の本開示は例示的であることが意図されており、本発明ならびに他の請求項の範囲を限定してはいない。本開示（本明細書で教示した内容の容易に認められる任意の変形を含む）では、本発明の主題が何ら公衆に提供されることがないように前述のクレーム用語の範囲を部分的に規定している。

Claims (20)

1. 自律車両を制御するシステムであって、
   制御装置を含み、前記制御装置は、
   各サンプリング時間において、
   センサ値と、前記センサ値及び動的モデルに基づいて計算された推定値とを入力して、前記センサ値及び前記推定値を前記制御装置のメモリに記録することと、
   前記センサ値及び前記推定値を、制約付き最適制御問題に付随する最適性の条件に組み込むことと、
   前記最適性の条件を、Fischer-Burmeister関数を用いて非平滑システムにマッピングすることと、
   前記非平滑システムを平滑化し、前記平滑化されたシステムにニュートン法反復を適用して解に収束することと、
   前記自律車両のアクチュエータを制御するコマンドを出すことと、を行うように構成されている、前記システム。
2. 平滑化された前記ニュートン法には正則化及び直線探索が含まれる請求項１に記載のシステム。
3. 前記制約付き最適制御問題に付随する前記最適性の条件には、最適化すべきコスト関数と適用すべきハード制約とが含まれる請求項１に記載のシステム。
4. タイヤ角度センサ及び車両速度センサをさらに含み、
   前記センサ値には、前記タイヤ角度センサから取得されたタイヤ角度と前記車両速度センサから取得された車両速度とのうちの１つ以上が含まれる請求項１に記載のシステム。
5. 前記ハード制約は前記アクチュエータに対する制約であり、最大ステアリングと最大車両加速度とのうちの１つ以上が含まれる請求項３に記載のシステム。
6. 障害物センサをさらに含み、
   前記センサ値には障害物の情報が含まれ、
   前記ハード制約は、前記障害物の情報に基づいて決定された障害物共謀包絡線である請求項３に記載のシステム。
7. 前記コスト関数にはソフト制約の形態のペナルティが含まれる請求項３に記載のシステム。
8. 前記ソフト制約には、加加速快適さ、加速快適さ、横加速度、牽引制約、及びロールのうちの１つ以上が含まれる請求項７に記載のシステム。
9. 制御装置を有する自律車両を制御する方法であって、
   各サンプリング時間において、前記制御装置は、
   センサ値と、前記センサ値及び動的モデルに基づいて計算された推定値とを入力して、前記センサ値及び前記推定値を前記制御装置のメモリに記録することと、
   前記センサ値及び前記推定値を、制約付き最適制御問題に付随する最適性の条件に組み込むことと、
   前記最適性の条件を、Fischer-Burmeister関数を用いて非平滑システムにマッピングすることと、
   前記非平滑システムを近似する一連の平滑化されたシステムにニュートン法反復を行って、解に収束することと、
   前記自律車両のアクチュエータを制御するコマンドを出すことと、を行う、前記方法。
10. 平滑化された前記ニュートン法には正則化及び直線探索が含まれる請求項９に記載の方法。
11. 前記制約付き最適制御問題に付随する前記最適性の条件には、最適化すべきコスト関数と適用すべきハード制約とが含まれる請求項９に記載の方法。
12. 前記センサ値を入力することには、タイヤ角度センサからタイヤ角度を取得することと、車両速度センサから車両速度を取得することとが含まれる請求項９に記載の方法。
13. 前記ハード制約は前記アクチュエータに対する制約であり、最大ステアリングと最大加速度とのうちの１つ以上が含まれる請求項１１に記載の方法。
14. 前記センサ値を入力することには、障害物センサから障害物の情報を取得することが含まれ、前記方法にはさらに、
    前記ハード制約として前記障害物の情報に基づいて障害物共謀包絡線を決定することが含まれる請求項１１に記載の方法。
15. 前記コスト関数に付随する前記最適性の条件には、ソフト制約の形態のペナルティが含まれる請求項１１に記載の方法。
16. 前記ソフト制約には、加加速快適さ、加速快適さ、横加速度、牽引制約、及びロールのうちの１つ以上が含まれる請求項１５に記載の方法。
17. 自律車両を制御する制御装置であって、
    各サンプリング時間において、
    センサ値と、前記センサ値及び動的モデルに基づいて計算された推定値とを入力して、前記センサ値及び前記推定値を前記制御装置のメモリに記録することと、
    前記センサ値及び前記推定値を、制約付き最適制御問題に付随する最適性の条件に組み込むことと、
    前記最適性の条件をFischer-Burmeister関数を用いて非平滑システムにマッピングすることと、
    前記非平滑システムを平滑化し、前記平滑化されたシステムにニュートン法反復を適用して解に収束することと、
    前記自律車両内のアクチュエータの動作を制御するコマンドを出すことと、を行うように構成されている、前記制御装置。
18. 平滑化された前記ニュートン法には正則化及び直線探索が含まれる請求項１７に記載の制御装置。
19. 前記制約付き最適制御問題には、最適化すべきコスト関数と適用すべきハード制約とが含まれる請求項１７に記載の制御装置。
20. 前記ハード制約は前記アクチュエータに対する制約であり、最大ステアリングと最大加速度とのうちの１つ以上が含まれる請求項１９に記載の制御装置。