(57)【特許請求の範囲】
【請求項１】
自車両の車両状態と自車両に対する制御入力値と他車両を含む運転環境の状況変化とをモデル化したコスト関数に適用する初期値を決定する演算装置であって、前記運転環境の状況変化を用いないで、自車両の位置と進行方向とに基づいて、自車両の車両状態を所定時間経過後に目標の車両状態に遷移させる、加速度および舵角変化率の制御入力値を初期値として求める制御入力値計算部を備える演算装置。
【請求項２】
自車両の現在の車両状態が継続したと仮定したときの所定時間経過後の車両状態に基づいて、前記目標の車両状態を設定する目標状態設定部を備え、
前記制御入力値計算部は、前記自車両の現在の車両状態を前記目標の車両状態に遷移させる制御入力値を求める請求項１に記載の演算装置。
【請求項３】
前記制御入力値計算部は、前記制御入力値の計算過程において、前記現在の車両状態から前記目標の車両状態への変化を前記制御入力値の一次関数で近似する請求項１または２に記載の演算装置。
【請求項４】
前記コスト関数は、離散時間モデルである請求項１乃至３のいずれかに記載の演算装置。
【請求項５】
前記制御入力値計算部は、現在の車両状態を０ステップとし、１〜ｎステップ先の車両状態を考慮するとき、ｋ番目のステップ（１≦ｋ≦ｎ）における目標の車両状態に遷移させる制御入力値を計算する請求項４に記載の演算装置。
【請求項６】
前記制御入力値計算部は、前記コスト関数で用いられる前記車両の状態変数がｎ個ある場合に、ｎ個未満の状態変数を用いて制御入力値を求める請求項１乃至５のいずれかに記載の演算装置。
【請求項７】
車両の状態変数を、現在の車両位置を原点、車両の進行方向をＸ軸とするローカル座標系に変換する座標系変換部を備え、
前記制御入力値計算部は、所定時間経過後における前記目標の車両状態を、車両のＹ座標位置及び進行方向で特定して、制御入力値を計算する請求項１乃至６のいずれかに記載の演算装置。
【請求項８】
前記制御入力値計算部は、前記目標の車両状態に遷移させる制御入力値が得られない場合には、目標の車両状態を表わす状態変数の数を減らす請求項１乃至７のいずれかに記載の演算装置。
【請求項９】
前記制御入力値計算部は、前記目標の車両状態に遷移させる制御入力値が得られない場合には、目標とする車両状態を他のステップの車両状態に変更する請求項５に記載の演算装置。
【請求項１０】
自車両の現在の車両状態に基づいて複数の前記目標の車両状態を設定する目標状態設定部を備え、
前記制御入力値計算部は、前記自車両の現在の車両状態を前記複数の目標の車両状態のそれぞれに遷移させる複数の制御入力値を求める請求項１に記載の演算装置。
【請求項１１】
前記複数の制御入力値のそれぞれを初期値として前記コスト関数の最適解を求める最適解計算部と、
前記複数の制御入力値に対応して求められた複数の最適解のうち、前記コスト関数を最小にする最適解を最良解として選択する最良解選択部と、
を備える請求項１０に記載の演算装置。
【請求項１２】
演算装置が、自車両の車両状態と自車両に対する制御入力値と他車両を含む運転環境の状況変化とをモデル化したコスト関数に適用する初期値を決定する方法であって、前記運転環境の状況変化を用いないで、自車両の位置と進行方向とに基づいて、自車両の車両状態を所定時間経過後に目標の車両状態に遷移させる、加速度および舵角変化率の制御入力値を初期値として求める演算方法。
【請求項１３】
自車両の車両状態と自車両に対する制御入力値と他車両を含む運転環境の状況変化とをモデル化したコスト関数に適用する初期値を決定するために、コンピュータに、前記運転環境の状況変化を用いないで、自車両の位置と進行方向とに基づいて、自車両の車両状態を所定時間経過後に目標の車両状態に遷移させる、加速度および舵角変化率の制御入力値を初期値として求めるステップを実行させるプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、環境状況に応じて適切な行動を計画する技術に関する。
【背景技術】
【０００２】
ＮＨＴＳＡ（National Highway Traffic Safety Administration）によって定義された自動運転レベル３から４において、自動運転車両は運転環境をモニタリングしながら適切な行動を実行する必要がある。環境の状況変化に応じて適切な意思決定を行う制御を実現するため、本発明者らは、モデル予測制御（ＭＰＣ）のアプローチを応用した自動運転車両のモーションプランニングのシステムを提案した（非特許文献１）。
【０００３】
モデル予測制御では、制御対象の力学モデルや制約条件を陽に設定し、有限の予測区間（予測ホライズン）にわたる制御対象の状態変化予測に基づき、コスト関数の最適解を与える制御入力値を算出する。モデル予測制御による行動計画において、コスト関数は「望ましい運転行動」を表現する関数となる。
【先行技術文献】
【特許文献】
【０００４】
【非特許文献１】大林真人，宇土敬祐，高野岳，「モデル予測制御を用いた舵角と加速度の生成による自動運転車両のモーションプランニング」，自動制御連合講演会2015
【発明の概要】
【発明が解決しようとする課題】
【０００５】
最適化の対象とするコスト関数が凸である場合には、コスト関数を最小化する大域最適解を効率的に得ることが可能である。しかし、車両状態、制御入力値及び環境状況を変数とする自動運転制御のコスト関数は本質的に非凸であると考えられる。短時間かつ狭い領域（数百ｍ秒、同一レーン内）の探索であれば、コスト関数を凸にすることも可能であるが、適切な追い抜き動作等を考慮するためには、長時間かつ大局的な空間を考慮する必要がある。
【０００６】
コスト関数が非凸の場合、大域最適解を得ることができる保証はなく、得られる解の良し悪しは最適化処理を開始する初期値に大きく依存する。そこで、本発明は、非凸のコスト関数の最適解を適切に求めることができる技術を提案する。
【課題を解決するための手段】
【０００７】
本発明の演算装置は、自車両の車両状態と自車両に対する制御入力値と他車両を含む運転環境の状況変化とをモデル化したコスト関数に適用する初期値を決定する演算装置であって、自車両の車両状態を所定時間経過後に目標の車両状態に遷移させる制御入力値を初期値として求める制御入力値計算部を備える。
【０００８】
本発明者らは、自車両の車両状態を目標の車両状態に遷移させるための制御入力値、つまり、他車両の状況を考慮しないで求めた制御入力値が、他車両を含む運転環境の状況変化をも含めてモデル化されたコスト関数を解くための初期値として好ましいものであることを見出した。本発明者らの実験によれば、このようにして求めた制御入力値を初期値とすることで、コスト関数の適切な解を得られた。
【０００９】
本発明の演算装置は、自車両の現在の車両状態が継続したと仮定したときの所定時間経過後の車両状態に基づいて、前記目標の車両状態を設定する目標状態設定部を備え、前記制御入力値計算部は、前記自車両の現在の車両状態を前記目標の車両状態に遷移させる制御入力値を求めてもよい。
【００１０】
このように現在の車両状態が継続した（例えば、速度一定、舵角一定）と仮定したときの車両状態に基づいて目標の車両状態を設定することにより、現在の車両状態との連続性が高い状態を目標の車両状態として設定することができる。
【００１１】
本発明の演算装置において、前記制御入力値計算部は、前記制御入力値の計算過程において、前記現在の車両状態から前記目標の車両状態への変化を前記制御入力値の一次関数で近似してもよい。これにより、計算コストを低減し、制御入力値を高速に求めることができる。
【００１２】
本発明の演算装置において、前記コスト関数は、離散時間モデルであってもよい。本発明の演算装置において、前記制御入力値計算部は、現在の車両状態を０ステップとし、１〜ｎステップ先の車両状態を考慮するとき、ｋ番目のステップ（１≦ｋ≦ｎ）における目標の車両状態に遷移させる制御入力値を計算してもよい。
【００１３】
この構成により、目標の車両状態へ遷移させるための制御入力値を適切に決定することができる。車両の制御においては、ｎステップのすべてにおいて目標の車両状態を実現する制御入力値を求めることは困難である。任意のｋ番目のステップに着目することで制御入力値を決定することができる。なお、ｋは複数であってもよい。
【００１４】
本発明の演算装置において、前記制御入力値計算部は、前記コスト関数で用いられる前記車両の状態変数がｎ個ある場合に、ｎ個未満の状態変数を用いて制御入力値を求めてもよい。
【００１５】
適合させるべき状態変数が多すぎると現在の車両状態から目標の車両状態へ遷移させる制御入力値を決定することができない。コスト関数に適用する初期値を求める際には、コスト関数で用いられるすべての状態変数を用いる必要はなく、用いる状態変数を少なくすることで、制御入力値を得ることができる。
【００１６】
本発明の演算装置は、車両の状態変数を、自車両の現在位置を原点、自車両の進行方向をＸ軸とするローカル座標系に変換する座標系変換部を備え、前記制御入力値計算部は、所定時間経過後における前記目標の車両状態を、車両のＹ座標位置及び進行方向で特定して、制御入力値を計算してもよい。
【００１７】
このようにローカル座標系に変換することによって、車両の位置をＹ座標位置で表わし、車両の進行方向を傾きで表現できる。これにより、状態変数の数を減らし、計算コストを減らすことができる。
【００１８】
本発明の演算装置において、前記制御入力値計算部は、前記目標の車両状態に遷移させる制御入力値が得られない場合には、目標の車両状態を表わす状態変数の数を減らしてもよい。
【００１９】
このように目標の車両状態に遷移させる制御入力値が得られない場合には、車両状態を表わす変数を減らすことにより、制御入力値を得ることができる可能性がある。なお、目標の車両状態に遷移させる制御入力値が得られない場合とは、必ずしも、目標の車両状態に完全に一致させるような制御入力値が得られないことを意味するのではなく、目標の車両状態に一定程度近似した車両状態が得られない場合としてもよい。
【００２０】
本発明の演算装置において、前記制御入力値計算部は、前記目標の車両状態に遷移させる制御入力値が得られない場合には、目標とする車両状態を他のステップの車両状態に変更してもよい。
【００２１】
このように目標の車両状態に遷移させる制御入力値が得られない場合には、目標とする車両状態を規定するステップを変えることにより、制御入力値を得ることができる可能性がある。なお、目標の車両状態に遷移させる制御入力値が得られない場合とは、必ずしも、目標の車両状態に完全に一致させるような制御入力値が得られないことを意味するのではなく、目標の車両状態に一定程度近似した車両状態が得られない場合としてもよい。
【００２２】
本発明の演算装置は、自車両の現在の車両状態に基づいて複数の前記目標の車両状態を設定する目標状態設定部を備え、前記制御入力値計算部は、前記自車両の現在の車両状態を前記複数の目標の車両状態のそれぞれに遷移させる複数の制御入力値を求めてもよい。また、本発明の演算装置は、前記複数の制御入力値のそれぞれを初期値として前記コスト関数の最適解を求める最適解計算部と、前記複数の制御入力値に対応して求められた複数の最適解のうち、前記コスト関数を最小にする最適解を最良解として選択する最良解選択部とを備えてもよい。
【００２３】
このように複数の目標状態を設定し、設定したそれぞれの目標状態に遷移させる複数の制御入力値を初期値として求める。そして、複数の初期値のそれぞれを用いてコスト関数の最適解を求め、その中から最良解を選択することにより、コスト関数の大域最適解を求めることができる可能性をいっそう高めることができる。
【００２４】
本発明の演算方法は、演算装置が、自車両の車両状態と自車両に対する制御入力値と他車両を含む運転環境の状況変化とをモデル化したコスト関数に適用する初期値を決定する方法であって、自車両の車両状態を所定時間経過後に目標の車両状態に遷移させる制御入力値を初期値として求める。
【００２５】
本発明のプログラムは、自車両の車両状態と自車両に対する制御入力値と他車両を含む運転環境の状況変化とをモデル化したコスト関数に適用する初期値を決定するために、コンピュータに、自車両の車両状態を所定時間経過後に目標の車両状態に遷移させる制御入力値を初期値として求めるステップを実行させる。
【発明の効果】
【００２６】
本発明は、自車両の車両状態を目標の車両状態に遷移させる制御入力値を初期値として用いることにより、自動運転制御のコスト関数の最適解を求めることができる。
【図面の簡単な説明】
【００２７】
【図１】第１の実施の形態の初期値決定装置を備えた自動運転制御装置の構成を示す図である。
【図２】自動運転制御装置が行うモデル予測制御（ＭＰＣ）の構成を示す図である。
【図３】本実施の形態において扱う状態変数を示す模式図である。
【図４】目標状態設定部による目標状態の設定例を示す図である。
【図５】ローカル座標系において目標の車両状態を設定する例を示す図である。
【図６】現在の車両状態を目標の車両状態に遷移させる制御入力値を示す図である。
【図７】本実施の形態の自動運転制御装置の動作を示すフローチャートである。
【図８】第２の実施の形態の初期値決定装置を備えた自動運転制御装置の構成を示す図である。
【図９】目標状態設定部にて設定された複数の目標状態の例を示す図である。
【発明を実施するための形態】
【００２８】
以下、本発明の実施の形態の演算装置について、図面を参照しながら説明する。以下では、自動運転制御装置においてコスト関数の最適解を求めるための初期値を決定する初期値決定装置を例として説明する。ただし、本発明の演算装置は、自動運転制御装置以外にも適用することができることは言うまでもなく、コスト関数の最適解を計算するために、汎用的に用いることができる。
【００２９】
（第１の実施の形態）
［自動運転制御装置の概要］
図１は、第１の実施の形態の初期値決定装置１０を備えた自動運転制御装置１の構成を示す図である。自動運転制御装置１は、自車両および運転環境の状況に基づいて、自車両がとるべき運転行動を決定し、車両機器２０に対して制御入力値を出力して、車両の運転制御を行う。車両機器２０に対する制御入力値には様々な値があるが、ここでは加速度と舵角を例とする。
【００３０】
最初に、本実施の形態の自動運転制御装置１が行うモデル予測制御（ＭＰＣ）の概要について説明する。図２は、自動運転制御装置１が行うモデル予測制御の構成を示す図である。図２において、枠Ｍで囲まれた領域がモデル予測制御で構築される箇所であり、自動運転制御装置１の制御内容である。
【００３１】
自動運転制御装置１は、自車両の状態、他車両を含む環境の状況変化、道路形状や交通法規等の制約、及びユーザからの巡航速度指示値を入力として、自車両の行動を決定する。具体的には、自動運転制御装置１は、有限の予測区間（予測ホライズン）にわたる自車両の状態変化予測に基づき、コスト関数の最適解を与える制御入力値を求める。なお、図２は一例であり、制約付き最適化の入力は適宜変えてもよい。
【００３２】
図２に示すように、自動運転車両が制御入力値に基づいて制御を行うと、その制御結果が環境に影響を与える。例えば、自車両が速度を落とすと後方の車両が車間距離を確保するために速度を落としたり、追い越しをかけたりする等の影響がある。自動運転制御装置１は、自車両の行動の結果、予測ホライズンにおいて起こるであろう環境の変化を予測して、最適の運転行動を求める。
【００３３】
なお、コスト関数は離散時間モデルであり、モデル予測制御は、制御周期ΔＴ毎に行動を決定する。本明細書では、現在時刻をステップ０として、制御周期ΔＴの各制御周期をステップ１、ステップ２・・・、ステップｎという。行動は、各ステップの制御入力値（制御入力値が複数ある場合には制御入力ベクトル）の系列で構成される。
【００３４】
制御周期ΔＴの時間間隔においては、各ステップで決定した制御入力値を与える続けることになるが（零次ホールド）、この制御周期ΔＴが大きい場合には、非特許文献１に見られるように、より高速な制御周期で車両を計画された行動に追従させるためのフィードバックコントローラを設けてもよい。
【００３５】
図１に戻って、自動運転制御装置１の構成について説明する。自動運転制御装置１には、運転制御を行うために必要となる情報を取得するために、種々の検出装置が接続されている。具体的には、自動運転制御装置１は、自車両の状態を検出する自車両状態検出装置３１と、現在位置を検出する現在位置検出装置３２と、他車両の状況を含む環境情報を検出する環境情報検出装置３３と、地図情報を記憶した地図情報データベース（以下、「地図情報ＤＢ」という）３４が接続されている。
【００３６】
自車両状態検出装置３１は、自車両に車両状態を検出するセンサであり、例えば、加速度、速度、舵角を検出するセンサ等がある。現在位置検出装置３２は、ＧＰＳや車輪速センサなどのデータに基づいて、自車両の現在位置を検出する機能を有する。環境情報検出装置３３は、車両の周辺にある他車両、歩行者または障害物等を検出するセンサであり、例えば、ミリ波レーダー、ＬＩＤＡＲ、カメラ等がある。
【００３７】
地図情報ＤＢ３４は、道路の構成と施設等の情報を含んでいる。道路の構成は、例えば、道路の車線数や車幅、歩道の有無、道路が湾曲している場合にはその曲率半径等の情報である。また、法定速度、追い越しの可否、一方通行かどうか等の交通法規の情報を含む。環境情報検出装置３３にて検出した情報と地図情報とを重ね合わせることにより、自車両を取り巻く運転環境の情報を取得できる。
【００３８】
自動運転制御装置１は、上述した自車両状態検出装置３１、現在位置検出装置３２、環境情報検出装置３３から取得した自車両の状態及び運転環境に関する情報に基づいて、コスト関数を最小にする最適解を計算する最適解計算部１５と、最適解を与える制御入力値を車両機器２０に対して出力する出力部１６とを有している。
【００３９】
最適解計算部１５には、コスト関数を記憶したコスト関数記憶部１７が接続されている。コスト関数は、「望ましい運転行動」を表現した関数である。時刻ｔにおける自車両の車両状態をｘ（ｔ）、制御入力値をｕ（ｔ）とすると、時刻ｔ０から予測ホライズンＨｐまでのコスト関数は、以下の形式で定義される。
【数１】
0006599817.tif 000002
【００４０】
なお、関数Ｌは、例えば、道路の幅方向において車両が占有する望ましい位置、横方向への移動制限、急な加減速の抑制、目標巡航速度からの逸脱の抑制、車間距離の維持などの様々な要素をコストとして、これらに適切な重み付けを行って作成する。
【００４１】
最適解計算部１５は、予測ホライズン（ｔｏ~Ｈｐ）における状態変化予測に基づいて、コスト関数を最小化する制御入力値ｕ（ｔ）を求める。最適解計算部１５は、所定の制御周期ΔＴごとの各ステップの制御入力値を計算する。
【００４２】
従来は、コスト関数の最適解を求めるための初期値として、自車両の現在の制御入力値を用いていたが、適切な制御入力値が得られない場合もあった。本実施の形態では、自動運転制御装置１は、最適解計算部１５で用いる適切な初期値を決定する初期値決定装置１０を有している。
【００４３】
［初期値決定装置］
初期値決定装置１０は、自車両の車両状態を、予測ホライズンの中の所定時刻において目標とする車両状態に遷移させる制御入力値を初期値として求める。初期値決定装置１０は、車両状態変数取得部１１と、座標系変換部１２と、目標状態設定部１３と、制御入力値計算部１４とを有している。
【００４４】
車両状態変数取得部１１は、自車両状態検出装置３１にて検出された車両状態、及び現在位置検出装置３２にて検出された現在位置の情報に基づいて、自車両の車両状態に関する状態変数を取得する機能を有する。
【００４５】
図３は、本実施の形態において扱う状態変数を示す模式図である。図３では、車両の後輪及び前輪を１つずつ示しているが、各車輪の中心は、後輪の軸中心および前輪の軸中心を模式的に示している。本実施の形態で扱う状態変数は、後輪軸中心のグローバル座標Ｘ，Ｙ、車両進行方向θ、速度Ｖ、角速度ω、舵角δである。
【００４６】
座標系変換部１２は、グローバル座標系をローカル座標系に変換する機能を有する。ローカル座標系は、車両の現在位置を原点、車両の進行方向をＸ軸とする座標系である。
【００４７】
目標状態設定部１３は、所定時刻において目標とする車両状態を設定する機能を有する。目標状態設定部１３は、自車両の現在の車両状態が継続したと仮定したときに所定時刻に実現する車両状態に基づいて、目標の車両状態を設定する。
【００４８】
図４は、目標状態設定部１３による目標状態の設定例を示す図である。図４は、自車両は３車線道路の左側を走行している。現在時刻t0における車両状態x(t0) =［X(t0),Y(t0),θ(t0),ω(t0),δ(t0)］Tが制御入力0の状態（［uα,uδ］=0）で継続する、すなわち車両が等速かつ一定舵角で移動すると仮定する。このとき、図４に示すxa(t1), xa(t2)のように、車両状態が遷移することが予測される。しかし、このようにして求めた状態遷移は、ときに制約を逸脱した状態となる。図４に示す例では、予測される状態遷移は道路から逸脱している。
【００４９】
目標状態設定部１３は、所定時刻における適切な車両状態のうち現在の車両状態が継続したときに予測される状態変数から最も近い状態を目標の状態変数として設定する。図４に示す例では、xa(t1), xa(t2)の状態を最近傍の左車線の中央上に垂直に交わるように射影した位置を理想的な初期位置xb(t1), xb(t2)とする。図４においては、道路は３車線なので、中央車線や右車線の中央を走行することも適切な車両状態であるが、予測される状態変数から最も近い状態を目標とすることにより、車両を適切に制御する制御入力値を求めることができる。図４では、グローバル座標系で説明したが、目標状態設定部１３は、上述した座標系変換部１２にて、車両状態の状態変数をローカル座標系に変換した上で、目標の車両状態の設定を行う。
【００５０】
図５は、ローカル座標系において目標の車両状態を設定する例を示す図である。図５は、図４に示す状態遷移を、車両の現在位置、より正確には後輪軸の中心を原点とし、車両の進行方向をＸ軸とするローカル座標系に変換した図である。このような座標変換を行うことにより、車両状態をY軸方向の位置Yb(t2)と、X軸に対する車両の進行方向θb(t2)の２変数によって表わすことができる。
【００５１】
図６は、車両状態の遷移と加速度の制御入力値uα(t0)，uα(t1)と、舵角変化率の制御入力値uδ(t0)，uδ(t1)との関係を示す図である。制御入力値は、制御周期ΔTごとに零次ホールドし、時刻ｔ０から時刻ｔ１までは制御入力値uα(t0)，uδ(t0)を与える。これにより、車両状態x (t0)が車両状態xb(t1)に遷移する。時刻ｔ１から時刻ｔ２までは制御入力値uα(t1)，uδ(t1)を与え、これにより、車両状態xb(t1)が車両状態xb(t2)に遷移する。
【００５２】
制御入力値計算部１４は、現在の車両状態から目標の車両状態へ遷移させる制御入力値を求める。制御入力値計算部１４は、図６に示すような遷移によって実現する目標の車両状態の状態変数を制御入力値の一次関数で近似することにより、制御入力値を計算する。
【００５３】
図６に示す例では、次式（２）のように目標の車両状態Yb(t2),θb(t2)を、行列Ａ，Ｂを用いて表す。詳しい計算例は後述するが、下記のような式の形式は、Δt1,Δt2の高次の項を無視することによって得られる。
［Yb(t2),θb(t2)］T = A［uα(t0), uδ(t0), uα(t1), uδ(t1)］T + B・・・（２）
式（２）が得られると、制御入力値は、以下の式（３）によって求められる。
［uα(t0), uδ(t0), uα(t1), uδ(t1)］T = A-1（［Yb(t2),θb(t2)］T -B)
・・・（３）
【００５４】
制御入力値計算部１４にて求めた制御入力値は、最適解計算部１５に入力される。これにより、最適解計算部１５は、初期値決定装置１０から入力された制御入力値を初期値として用いて、コスト関数を最小にする最適解を計算する。
【００５５】
以上、本実施の形態の初期値決定装置１０を含む自動運転制御装置１の構成について説明したが、自動運転制御装置１および初期値決定装置１０の各部の一部または全部は、ハードウェアで構成されてもよいし、コンピュータのプロセッサが所定のプログラムを実行することによって実現されてもよい。
【００５６】
図７は、自動運転制御装置１の動作を示すフローチャートである。自動運転制御装置１は、自車両状態検出装置３１、現在位置検出装置３２、環境情報検出装置３３から、自車両の車両状態、現在位置、環境情報を取得する（Ｓ１０）。初期値決定装置１０は、取得した情報から、自車両の車両状態を表わす状態変数を取得する（Ｓ１１）。ここで、取得する状態変数は、後輪軸中心のグローバル座標Ｘ，Ｙ、車両進行方向θ、速度Ｖ、角速度ω、舵角δである。
【００５７】
次に、初期値決定装置１０は、グローバル座標系で表わされた車両状態の状態変数をローカル座標系の状態変数に変換する（Ｓ１２）。続いて、初期値決定装置１０は、所定時間経過後の所定時刻において目標とする状態変数を設定する（Ｓ１３）。上述したとおり、現在の車両状態が所定時刻まで継続したと仮定したときの車両状態に基づいて、目標の車両状態を設定する。そして、初期値決定装置１０は、現在の車両状態を目標の車両状態に遷移させる制御入力値を算出する（Ｓ１４）。以上に説明したステップＳ１１からステップＳ１４までが、初期値を決定する処理である。
【００５８】
初期値決定装置１０は、求めた制御入力値を最適解計算部１５に渡す。最適解計算部１５は、初期値決定装置１０から渡された初期値を用いて、コスト関数を最小化する制御入力値を算出する（Ｓ１５）。自動運転制御装置１は、求めた制御入力値を車両機器２０へ送信し、運転制御の指示を行う（Ｓ１６）。
【００５９】
以上、第１の実施の形態の初期値決定装置１０を備えた自動運転制御装置１について説明した。本実施の形態の自動運転制御装置１は、初期値決定装置１０が、コスト関数の最適化計算に用いる適切な初期値を決定するので、この初期値を用いてコスト関数の最適解を求めることができる。
【００６０】
上記した実施の形態では、現在状態から２ステップ先の時刻ｔ２における目標の車両状態に適合させる制御入力値を求める例を挙げたが、目標の車両状態を何ステップ先に置くかは適宜決定することができる。自動運転制御装置１の予測ホライズンがｎステップであるとき、初期値決定装置１０は、ｎ以下の任意のｋ番目のステップを目標としてよい。
【００６１】
なお、このように設定したｋ番目のステップの目標状態に遷移させる制御入力値が求まらない場合には、別のステップにおいて目標状態を設定してもよい。ここで、目標状態に遷移させる制御入力値が求まらない場合とは、目標の車両変数との乖離が所定の閾値以下の車両状態に遷移させることができない場合としてよい。
【００６２】
また、目標の車両状態に遷移させる制御入力値が求まらない場合には、目標の車両状態と適合させる車両変数の数を減らしてもよい。上記した例は、Ｙ座標位置Yb(t2)と車両の進行方向θb(t2)を適合させる制御入力値を求めたが、もしこのような２つの状態変数に適合させる制御入力値が求まらなかった場合には、例えば、車両の進行方向θb(t2)だけを対象として、目標の車両状態に遷移させる制御入力値を求めて、初期値としてもよい。
【００６３】
なお、上記した実施の形態では、自動運転制御のコスト関数の初期値を求める例を挙げたが、本発明は、一般的に、非凸のコスト関数の初期値を求めるために用いることができる。この場合、コスト関数が有する複数の入力値の中から、いくつかの入力値に着目して目標の状態へと遷移する入力値を求め、続いて、求めた入力値を初期値としてコスト関数の最適解を求める。
【００６４】
（第２の実施の形態）
図８は、第２の実施の形態の初期値決定装置１０ａを備えた自動運転制御装置２の構成を示す図である。第２の実施の形態の初期値決定装置１０ａ及び自動運転制御装置２の基本的な構成は、第１の実施の形態と同じである。第２の実施の形態の初期値決定装置１０ａは、目標状態設定部１３ａが複数の目標状態を設定する。
【００６５】
図９（ａ）〜図９（ｃ）は、目標状態設定部１３ａによって設定される複数の目標状態の例を示す図である。図９（ａ）〜図９（ｃ）において、現在状態は、状態x(t0)で示すように、自車両が３車線道路の中央の車線を走行している。この例では、目標状態設定部１３ａは、図９（ａ）に示すように車両が直進した状態を第１の目標状態xb1(t1)，xb1(t2)，図９（ｂ）に示すように車両が左側にレーンチェンジした状態を第２の目標状態xb2(t1)，xb2(t2)，図９（ｃ）に示すように車両が右側にレーンチェンジした状態を第３の目標状態xb3(t1)，xb3(t2)として、３つの目標状態を設定している。いずれも自車両が各車線の中央を走行する状態を目標としている。
【００６６】
制御入力値計算部１４は、複数の目標状態のそれぞれに対して、上記した第１の実施の形態で説明した方法と同じ方法によって制御入力値を計算する。初期値決定装置１０ａは、制御入力値計算部１４にて求めた複数の制御入力値を初期値として最適解計算部１５に渡す。最適解計算部１５は、複数の初期値のそれぞれに対応するコスト関数の最適解を求め、求めた複数の最適解を最良解選択部１８に渡す。最良解選択部１８は、最適解計算部１５から受け取った複数の解の中から、コスト関数を最小にする最良解を選択し、選択された最良解を出力部１６に渡す。
【００６７】
第２の実施の形態の自動運転制御装置２は、初期値決定装置１０ａが、複数の初期値を決定し、それぞれの初期値を用いて求めたコスト関数の最適解の中から最良の解を選択する構成により、複数の適切な初期値から最適化計算を開始することで、コスト関数の大域最適解が得られる可能性をいっそう高めることができる。
【００６８】
（目標の車両状態の状態変数の線形化の計算例）
図３に示されるような自動車の２輪モデルに基づいて後輪軸中央をベースに車両運動を考えると，グローバル座標X,Y、車両進行方向θ、速度V、角速度ω、舵角δには、以下の微分方程式が成り立つ。ここで，uαとuδは車両進行方向への加速度［m/s2］と前輪舵角変化率［rad/s］である。
【数２】
0006599817.tif 000003
【００６９】
ここで、時刻t0から微小時間変位Δtが生じた時刻t1(Δt = t1 - t0)について考慮すると、速度V(t1)および前輪舵角δ(t1)は以下のように表現できる。
【数３】
0006599817.tif 000004
【００７０】
さらに車両進行方向θは以下のように近似できる。
【数４】
0006599817.tif 000005
【００７１】
ここで、上式の右辺第二項および第三項は、それぞれ以下のように表現できる。
【数５】
0006599817.tif 000006
【００７２】
上記の結果より、車両の広域座標XおよびYは以下のように表現できる。
【数６】
0006599817.tif 000007
【数７】
0006599817.tif 000008
【００７３】
今、時刻t0における車両の現在状態x(t0)=［X(t0),Y(t0),θ(t0),ω(t0),δ(t0)］Tにおいて制御入力u(t0) =［uα0, uδ0］Tを時刻t1になるまで与えたとする（零次ホールド）。さらに、時刻t1において制御入力u(t1) =［uα1, uδ1］Tを時刻t2になるまで同様に与える。このとき、t0＜Δt0≦t1、t1＜Δt1≦t2とするとt1≦t＜t2の任意の時刻tにおけるY(t)の値は以下のように表現できる。
【数８】
0006599817.tif 000009
【数９】
0006599817.tif 000010
【００７４】
ここで、図６のように現在状態x(t0)からx(t2)に状態遷移するための制御入力u(t0)，u(t1)を求めるためには、x(t0)からx(t2)への車両運動をu(t0)，u(t1)に関して線形化することが望ましい。そうすれば，x(t2) = A［u(t0), u(t1)］T +Bの形式を構成することができ、望ましい車両状態x(t2)を得るための制御入力を決定できる。
そこで、Δt0，Δt1について４乗以上の項は無視して、Y(t2)= Y(t0+Δt0+Δt1)として線形化すると、以下の式を得る。
【数１０】
0006599817.tif 000011
【産業上の利用可能性】
【００７５】
本発明は、自車両の車両状態を目標の車両状態に遷移させる制御入力値を初期値として用いることにより、自動運転制御のコスト関数の最適解を求めることができ、自動運転車両の制御装置等として有用である。
【符号の説明】
【００７６】
１，２  自動運転制御装置
１０，１０ａ  初期値決定装置
１１  車両状態変数取得部
１２  座標系変換部
１３，１３ａ  目標状態設定部
１４  制御入力値計算部
１５  最適解計算部
１６  出力部
１７  コスト関数記憶部
１８  最良解選択部
２０  車両機器
３１  自車両状態検出装置
３２  現在位置検出装置
３３  環境情報検出装置
３４  地図情報データベース
【図１】
0006599817.tif 000012
【図２】
0006599817.tif 000013
【図３】
0006599817.tif 000014
【図４】
0006599817.tif 000015
【図５】
0006599817.tif 000016
【図６】
0006599817.tif 000017
【図７】
0006599817.tif 000018
【図８】
0006599817.tif 000019
【図９】
0006599817.tif 000020