以下、図面を参照して、本発明の実施の形態について説明する。本実施の形態では、運転行動モデルのモデルパラメータを学習すると共に、運転行動モデル用いて車両のドライバの運転支援を行う運転支援装置に、本発明を適用した場合を例に説明する。なお、本実施の形態では、複数の運転行動を切り替えるためのパラメータを学習するアルゴリズムとして、拡張カルマンフィルタを用いた場合を例に説明する。
<第1の実施の形態>
図1に示すように、第1の実施の形態に係る運転支援装置10は、自車両の車速を検出する車速センサ12と、自車両の位置を計測する位置計測部14と、自車両の前方を走行する前方車両を検出する前方車情報取得部16と、地図情報(道路ネットワークデータ)、道路情報、及び信号情報を記憶した道路ネットワークデータベース18と、道路ネットワークデータベース18から取得した信号情報に基づいて、次の信号の状態Sを取得する信号状態取得部20と、次の信号機の停止線の位置を取得し、当該停止線の位置に基づいて停止減速度Dを算出する停止減速度取得部22と、運転支援アクチュエータ40を制御するコンピュータ24と、運転支援を行うための運転支援アクチュエータ40とを備えている。
位置計測部14は、例えば、GPSセンサを用いて構成され、自車両の現時刻tの前後位置情報x* tを計測する。
前方車情報取得部16は、例えばレーザレーダ装置が該当し、自車両に対する前方車両までの走行方向の相対距離xfを計測し、相対距離xfと前方車両との相対速度とを前方車両情報として各時刻tについて取得する。なお、相対速度は、相対距離を時間微分することにより得られる。
道路ネットワークデータベース18には、地図情報、道路情報、及び信号情報が記憶されている。地図情報及び道路情報には、信号機の位置と当該信号機に対する停止線の位置を示すデータが含まれている。信号情報には、信号機の位置と当該信号機の信号変化の周期を示すデータが含まれている。
信号状態取得部20は、道路ネットワークデータベース18から取得した信号情報に基づいて、時刻tにおける次の信号の状態Sを取得する。
停止減速度取得部22は、道路ネットワークデータベース18から取得した地図情報及び道路情報と、信号状態取得部20によって取得された時刻tにおける次の信号の状態Sとに基づいて、次の信号の状態Sが青でない場合には、次の信号機の停止線の位置を取得し、当該停止線の位置に基づいて、停止減速度Dを算出する。
コンピュータ24は、CPUと、RAMと、後述する運転支援処理ルーチン、目標速度計算処理ルーチン、パラメータ学習処理ルーチン、予測処理ルーチン、及びフィルタリング処理ルーチンを実行するためのプログラムを記憶したROMとを備え、機能的には次に示すように構成されている。コンピュータ24は、車両の走行状態を示すセンサ情報を各時刻tについて取得する情報取得部26と、運転特性パラメータが格納されている運転特性パラメータデータベース28と、情報取得部26によって取得されたセンサ情報と、運転特性パラメータデータベース28に格納された運転特性パラメータに基づいて、運転支援のための目標速度を計算する目標速度計算部30と、計算された目標速度及び情報取得部26によって取得されたセンサ情報に基づいて、運転支援アクチュエータを制御する運転支援制御部32と、近似モデルデータベース34と、パラメータ学習部36とを備えている。
情報取得部26は、車両の走行状態を示すセンサ情報を各時刻tについて取得する。
具体的には、情報取得部26は、車両のドライバの運転支援を行うときに、車速センサ12によって計測された車速と、位置計測部14によって計測された車両の前後位置x* tと、前方車情報取得部16によって取得された前方車両情報と、信号状態取得部20によって取得された次の信号の状態Sと、停止減速度取得部22によって算出された停止減速度Dとを逐次取得して、センサ情報として蓄積する。情報取得部26は取得手段の一例である。
情報取得部26は、取得したセンサ情報を、パラメータ学習部36と、目標速度計算部30とへ出力する。
運転特性パラメータデータベース28には、後述するパラメータ学習部36によって学習された運転特性パラメータが格納されている。
目標速度計算部30は、情報取得部26によって取得されたセンサ情報と、運転特性パラメータデータベース28に格納された運転特性パラメータと、運転行動モデルとに基づいて、車両の目標速度を計算する。運転行動モデルは、センサ情報を用いて車両のドライバの複数の運転行動を離散的にモデル化し、かつ、条件分岐により切り替えるようにモデル化したものである。
例えば、運転行動モデルでは、条件分岐により、第1の運転行動、第2の運転行動、及び第3の運転行動を切り替え、切り替えられた運転行動に従って目標速度を計算する。第1〜第3の運転行動は、以下の条件分岐によって切り替えられる。
ここで、Sは車両からみて次の信号の色(信号の状態)を表し、Sblueは青信号をあらわす。また、Dは次の信号の停止線で停止するのに必要な減速度(停止減速度)を表し、xfは前方車両との距離(前方車距離)を表し、vrは目標速度を表し、vは自車速度を表す。また、運転特性パラメータのうち、aとbとは条件分岐で用いられる条件パラメータであり、cとdとは目標速度を算出するためのモデルパラメータである。
運転支援制御部32は、目標速度計算部30によって計算された目標速度と、情報取得部26によって取得されたセンサ情報に含まれる時刻tの車速とに基づいて、目標速度vrと時刻tの車速との差に応じて、運転支援アクチュエータ40を制御する。
運転支援アクチュエータ40は、例えば、アクセル操作量やブレーキ操作量を調整するためのアクチュエータである。
本実施の形態では、拡張カルマンフィルタを用いて、運転特性パラメータを学習する。以下、拡張カルマンフィルタの原理について説明する。
本実施の形態では、車両の前後位置xtと、車両の速度x・ tと、車両の加速度x・・ tと、運転特性パラメータ[at,bt,ct,dt]とを、以下の式(2)に示すように状態ベクトルXt=[xt,x・ t,x・・ t,at,bt,ct,dt]として構成する。なお、運転特性パラメータのうち[at,bt]は条件パラメータである。
また、以下の式(3)に示すように、位置計測部14によって計測された車両の位置情報を観測値Yt=[xt *]として構成する。
(1)予測ステップ
まず、予測ステップにおける処理の概要について説明する。予測ステップでは、以下の式(4)に示すように、時刻tの状態ベクトルXt−1=[xt−1,x・ t−1,x・・ t−1,at−1,bt−1,ct−1,dt−1]と、システムノイズut(誤差項)とに基づいて、時刻tの状態ベクトルXtを算出する。以下の式(4)におけるftはシステムモデルである。本実施の形態では、システムモデルは、運転支援装置10の目標速度計算部30の運転行動モデルを近似した後述する近似モデルと、車両物理モデルとを含むモデルである。
また、観測値Ytは、観測行列Htと観測ノイズwt(誤差項)とに基づいて、以下の式(5)に示すように算出される。なお、観測行列Htは予め求められている。
図2に、予測ステップの概念図を示す。図2に示すように、予測ステップでは、フィルタリングステップで推定された時刻t−1の状態ベクトルXt−1|t−1に基づいて、時刻tの状態ベクトルXt|t−1を予測する(中心値の予測)。なお、Xt|t−1は、時刻t−1までに利用可能なデータに基づき予測された時刻tの状態ベクトルを表す。また、Xt−1|t−1は、時刻t−1までに利用可能なデータに基づき推定された時刻t−1の状態ベクトルを表す。
なお、以下では、添え字t|t−1が付与された値は、時刻t−1までに利用可能なデータに基づき予測された時刻tの事前推定値を表す。また、添え字t|t(又は、t−1|t−1)が付与された値は、時刻tまでに利用可能なデータに基づき推定された時刻tの事後推定値を表す。
また、予測ステップでは、システムモデルftを偏微分して得られる行列^Ftと行列^Gtとを、以下の式(6)〜(7)に従って算出する。
ここで、「’」はベクトルの転置を表す。
そして、上記式(6)〜(7)に従って算出した行列^Ftと行列^Gtと、前回のフィルタリングステップで推定された事前誤差共分散行列Pt|t−1とに基づいて、以下の式(8)に従って、事前誤差共分散行列Pt|t−1を算出する(分散の予測)。
なお、上記式(8)におけるQtは、システム雑音ut(誤差項)の分散を表す。
すなわち、予測ステップでは、時刻tの状態ベクトルXt|t−1を中心値とし、事前誤差共分散行列Pt|t−1を分散とする確率分布を予測していることになる。
(2)フィルタリングステップ
次に、フィルタリングステップについての概要を説明する。図3に、フィルタリングステップの概念図を示す。図3に示すように、フィルタリングステップでは、上記式(2)で示した時刻tの状態ベクトルXtの要素である車両の前後位置[xt]と、上記式(3)で示した時刻tの車両の前後位置情報[x* t](観測中心)とに基づいて、フィルタリングを行い、予測ステップで出力された時刻tの状態ベクトルXt|t−1を修正してXt|tを算出すると共に、事前誤差共分散行列Pt|t−1を修正して事後誤差共分散行列Pt|tを算出する。
具体的には、まず、フィルタリングステップでは、予測ステップで算出された事前誤差共分散行列Pt|t−1と、観測行列Htと、観測ノイズwt(誤差項)の分散Rtとに基づいて、以下の式(9)に従って、カルマンゲインKtを算出する。
次に、算出されたカルマンゲインKtと、予測ステップで算出された時刻tにおける状態ベクトルXt|t−1と、上記式(3)で示したYt=[x* t]と、観測行列Htとに基づいて、以下の式(10)に従って、時刻tにおける状態ベクトルXt|tを推定する。
そして、予測ステップで算出された事前誤差共分散行列Pt|t−1と、算出されたカルマンゲインKtと、観測行列Htとに基づいて、以下の式(11)に従って、事後誤差共分散行列Pt|tを算出する。
フィルタリングステップで算出された各値(カルマンゲインKt、状態ベクトルXt|t、事後誤差共分散行列Pt|t)は、次回の予測ステップにおける処理で用いられる。
近似モデルデータベース34には、目標速度計算部30において用いられる運転行動モデルを、連続かつ微分可能な関数で近似した近似モデルが格納されている。本実施の形態では、条件パラメータである[at,bt]を、シグモイド関数によって連続かつ微分可能な関数で近似した近似モデルを用いる。
ここで、運転行動モデルを近似する方法について説明する。まず、上記式(1)のうち、以下の[]で示した、条件パラメータbを用いた条件分岐の部分を、近似モデルによって近似する。
上記式(12)における[]の部分は、以下の式(13)に示すように近似することができる。
ここで、p1は、条件分岐の曖昧さを表す予め求められたパラメータである。また、上記式(12)及び(13)を、V1を用いて書き直すと、以下の式(14)、(15)のようになる。
上記式(14)の[]で示した、条件パラメータaを用いた条件分岐の部分は、以下の式(16)に示すように近似することができる。
ここで、p2は、条件分岐の曖昧さを表す予め求められたパラメータである。そして、上記式(1)を、上記式(15)と(16)とを用いて書き直すと、以下の式(17)のように表すことができる。
以上のように、条件パラメータa、bを用いた運転行動モデルについて、上記式(17)に示すようにシグモイド関数によって連続かつ微分可能な関数で近似した近似モデルを導出することができる。
パラメータ学習部36は、近似モデルデータベース34に格納された近似モデルと、情報取得部26によって取得された車両の前後位置情報とに基づいて、運転行動モデルの条件分岐で用いられる条件パラメータa,bを含む運動特性パラメータを学習する。図4に示すように、パラメータ学習部36は、予測部360と、フィルタリング部362とを備えている。
予測部360は、近似モデルデータベース34に格納された近似モデルと、フィルタリング部362によって前回推定された時刻t−1の状態ベクトルXt−1とに基づいて、上記式(4)に従って、時刻tの状態ベクトルXtを予測する(予測ステップ)。予測部360における処理は、拡張カルマンフィルタの予測ステップに対応する。
具体的には、まず、予測部360は、情報取得部26によって取得されたセンサ情報に含まれる次の信号の状態S、前方車両との相対距離xf、及び停止減速度Dと、近似モデルデータベース34に格納された近似モデルとに基づいて、上記式(17)に従って、目標速度vrを算出する。
次に、予測部360は、算出された目標速度vrと、時刻t−1の状態ベクトルXt−1の要素である車両の速度x・ t−1とに基づいて、時刻tの加速度x・・ tを予測する。例えば、予測部360は、車両物理モデルである比例制御モデルを用いて、車速x・ t−1と目標速度vrとの差に応じて、時刻tの加速度x・・ tを予測する。
また、予測部360は、時刻t−1の状態ベクトルXt−1の要素である[x・ t−1,x・・ t−1]とに基づいて、車両物理モデルに従って、時刻tの車速x・ tを予測する。例えば、速度算出式x・ t=x・ t−1+x・・ t−1Tに従って、時刻tの車速x・ tを予測する。Tは、時刻tから時刻t−1までの経過時間を表す。
そして、予測部360は、予測された車両の速度x・ tと、時刻t−1の状態ベクトルXt−1の要素であるxt−1とに基づいて、車両物理モデルに従って、時刻tの車両の位置[xt]を予測する。
上記式(2)における[xt,x・ t,x・・ t]については、以上のように予測するが、運動特性パラメータ[at,bt,ct,dt]については、時刻t−1の運動特性パラメータ[at−1,bt−1,ct−1,dt−1]をそのまま代入する。
また、予測部360は、上記式(6)〜(7)に従って、上記式(17)で示した近似モデルの偏微分を行い、行列^Ftと行列^Gtとを算出する。上記式(17)における目標速度は、シグモイド関数によって連続かつ微分可能な関数で近似されているため、偏微分をすることができる。
そして、予測部360は、算出された行列^Ftと行列^Gtと、前回フィルタリング部362によって推定された事前誤差共分散行列Pt|t−1とに基づいて、上記式(8)に従って、事前誤差共分散行列Pt|t−1を算出する。
フィルタリング部362は、情報取得部26によって取得されたセンサ情報に含まれる車両の前後位置情報x* tと、予測部360によって予測された時刻tの状態ベクトルXtと、予測部360によって算出された事前誤差共分散行列Pt|t−1とに基づいて、時刻tにおける状態ベクトルXtを推定する(フィルタリングステップ)。フィルタリング部362における処理は、拡張カルマンフィルタにおけるフィルタリングステップに対応する。
具体的には、フィルタリング部362は、情報取得部26によって取得されたセンサ情報に含まれる時刻tの車両の前後位置情報x* tと、予測部360によって予測された時刻tの車両の前後位置情報xtと、予測部360によって算出された事前誤差共分散行列Pt|t−1とに基づいて、上記式(9)に従って、カルマンゲインKtを算出する。そして、フィルタリング部362は、算出されたカルマンゲインKtと、予測部360によって算出された時刻tにおける状態ベクトルXt|t−1と、上記式(3)で示したYt=[x* t]とに基づいて、上記式(10)に従って、時刻tにおける状態ベクトルXt|tを推定する。
また、フィルタリング部362は、算出されたカルマンゲインKtと、予測部360によって算出された事前誤差共分散行列Pt|t−1に基づいて、上記式(11)に従って、事後誤差共分散行列Pt|tを算出する。
そして、フィルタリング部362は、推定された状態ベクトルXt|tの要素である[at,bt,ct,dt]を運転特性パラメータとして更新し、運転特性パラメータデータベース28へ格納する。
<運転支援装置10の動作>
次に、第1の実施の形態に係る運転支援装置10の動作について説明する。まず、ドライバの運転操作により車両が走行し、運転支援装置10の情報取得部26によってセンサ情報が各時刻tについて取得されているときに、運転支援装置10において、図5に示す運転支援処理ルーチンが実行される。
まず、ステップS100において、目標速度計算部30によって、運転特性パラメータデータベース28に格納された運動特性パラメータが読み出される。
次に、ステップS102において、情報取得部26によって、時刻t−1のセンサ情報を受け付ける。
ステップS104において、目標速度計算部30によって、上記ステップS102で受け付けたセンサ情報と、上記ステップS100で読み出された運転特性パラメータとに基づいて、目標速度vrを計算する。ステップS306は、図6に示す目標速度計算処理ルーチンによって実現される。
<目標速度計算処理ルーチン>
まず、ステップS200において、上記ステップS102で取得されたセンサ情報に含まれる時刻t−1の信号の状態Sに基づいて、車両からみて次の信号が青であるか否かを判定する。次の信号が青である場合には、ステップS202へ進む。一方、次の信号が青でない場合には、ステップS204へ進む。
ステップS202において、上記ステップS102で取得されたセンサ情報に含まれる時刻t−1の前方車距離xfと、上記ステップS100で読み出された運転特性パラメータbとに基づいて、前方車距離xfが運転特性パラメータbより大きいか否かを判定する。前方車距離xfが運転特性パラメータbより大きい場合には、ステップS206へ進む。一方、前方車距離xfが運転特性パラメータb以下の場合には、ステップS208へ進む。
ステップS204において、上記ステップS102で取得されたセンサ情報に含まれる時刻t−1の停止減速度Dと、上記ステップS100で読み出された運転特性パラメータaとに基づいて、停止減速度Dが運転特性パラメータaより大きいか否かを判定する。停止減速度Dが運転特性パラメータaより大きい場合には、ステップS202へ移行する。一方、停止減速度Dが運転特性パラメータa以下の場合には、ステップS210へ進む。
ステップS206において、目標速度vrに上記ステップS100で読み出された運転特性パラメータcを代入する。
ステップS208において、上記ステップS100で読み出された運転特性パラメータdと、上記ステップS102で取得されたセンサ情報に含まれる時刻t−1の前方車距離xfと、センサ情報に含まれる時刻t−1の車速とに基づいて、算出式d×(前方車距離xf/車速)に従って、目標速度vrを算出する。
ステップS210において、目標速度vrに0を代入する。
ステップS212において、上記ステップS206で算出された目標速度vr、上記ステップS208で算出された目標速度vr、及び上記ステップS210で算出された目標速度vrの何れか1つを、目標速度として出力して、目標速度計算処理ルーチンを終了する。
次に、運転支援処理ルーチンに戻り、ステップS106において、運転支援制御部32によって、上記ステップS306で出力された目標速度vrと、センサ情報に含まれる時刻t−1の車速とに基づいて、運転支援アクチュエータ40の制御量を算出する。
ステップS108において、上記ステップS106で算出された制御量に基づいて、運転支援アクチュエータ40を制御し、ドライバの運転支援を行う。
ステップS110において、パラメータ学習部36によって、運動特性パラメータを学習する。ステップS110は、図7に示すパラメータ学習処理ルーチンによって実現される。
<パラメータ学習処理ルーチン>
まず、ステップS300において、予測部360によって、予測処理を実行する。ステップS300は、図8に示す予測処理ルーチンによって実現される。
<予測処理ルーチン>
まず、ステップS400において、予測部360によって、上記ステップS102で受け付けたセンサ情報に含まれる次の信号の状態S、前方車両との相対距離xf、及び停止減速度Dと、近似モデルデータベース34に格納された近似モデルとに基づいて、上記式(17)に従って、目標速度vrを予測する。
ステップS402において、予測部360によって、上記ステップS400で予測された目標速度と、前回のステップS302で推定された時刻t−1の状態ベクトルXt−1の要素[xt−1,x・ t−1,x・・ t−1]とに基づいて、時刻tの[xt,x・ t,x・・ t]を予測する。また、運動特性パラメータ[at,bt,ct,dt]については、時刻t−1の運動特性パラメータ[at−1,bt−1,ct−1,dt−1](上記ステップS100で読み出された運動特性パラメータ)をそのまま代入する。そして、時刻tの状態ベクトルXt[xt,x・ t,x・・ t,at,bt,ct,dt]を構成する。
ステップS404において、予測部360によって、上記ステップS402で予測された時刻tの状態ベクトルXtに基づいて、上記式(6)〜(7)に従って、近似モデルの偏微分を行い、行列^Ftと行列^Gtとを算出する。そして、予測部360によって、行列^Ftと行列^Gtと、前回のステップS302で推定された事後誤差共分散行列Pt−1|t−1とに基づいて、上記式(8)に従って、事前誤差共分散行列Pt|t−1を算出する。
ステップS406において、上記ステップS402で予測された時刻tの状態ベクトルXtと、上記ステップS404で算出された事前誤差共分散行列Pt|t−1とを結果として出力する。
次に、パラメータ学習処理ルーチンに戻り、ステップS302において、フィルタリング処理を実行する。ステップS302は、図9に示すフィルタリング処理ルーチンによって実現される。
<フィルタリング処理ルーチン>
まず、ステップS500において、上記ステップS300で出力された時刻tの状態ベクトルXtと、事前誤差共分散行列Pt|t−1とを受け付ける。
次に、ステップS502において、フィルタリング部362によって、上記ステップS500で受け付けた事前誤差共分散行列Pt|t−1に基づいて、上記式(9)に従って、カルマンゲインKtを算出する。
ステップS504において、フィルタリング部362によって、上記ステップS502で算出されたカルマンゲインKtと、上記ステップS500で受け付けた時刻tにおける状態ベクトルXt|t−1と、上記ステップS100で受け付けたセンサ情報に含まれる車両の前後位置情報x* tとに基づいて、上記式(10)に従って、時刻tにおける状態ベクトルXt|tを推定する。
ステップS506において、上記ステップS500で受け付けた事前誤差共分散行列Pt|t−1と、上記ステップS502で算出されたカルマンゲインKtに基づいて、上記式(11)に従って、事後誤差共分散行列Pt|tを算出する。
ステップS508において、上記ステップS306で推定された時刻tにおける状態ベクトルXt|tと、上記ステップS506で算出された事後誤差共分散行列Pt|tとを、結果として出力する。
次に、パラメータ学習処理ルーチンに戻り、ステップS304において、上記ステップS302で出力された時刻tにおける状態ベクトルXt|tのうち、運転特性パラメータ[at,bt,ct,dt]を、新たな運転特性パラメータとして更新し、運転特性パラメータデータベース28に格納する。
以上説明したように、第1の実施の形態に係る運転支援装置によれば、センサ情報を用いて車両のドライバの複数の運転行動を離散的にモデル化し、かつ、条件分岐により切り替えるようにモデル化した運転行動モデルを、連続かつ微分可能な関数で近似した近似モデルと、取得された車両のセンサ情報とに基づいて、運転行動モデルの条件分岐で用いられる条件パラメータを含む運転特性パラメータを学習することにより、ドライバに合ったモデルパラメータを学習することができる。
また、以上説明したように、第1の実施の形態に係る運転支援装置によれば、学習された運転特性パラメータと、運転行動モデルと、取得されたセンサ情報とに基づいて、車両のドライバの運転支援を行うことにより、ドライバに合った運転支援を行うことができる。
また、市街地走行時の運転支援システムを考慮すると、運転行動に応じた離散的な運転行動モデルが不可欠であり、その離散的な運転行動モデルのパラメータを、実測データから逐次に学習することで、より精度の高い運転支援が可能となる。
また、従来では、離散状態の切り替えとしてモデル化された運転行動モデルの学習は困難であった。また、クラスタリングなどに代表される判断モデルの同定手法を用いることで学習することも可能だが、学習に時間がかかるうえ、逐次的な学習が困難であった。本実施の形態の形態によれば、非連続な関数である運転行動モデルを、シグモイド関数を用いて、連続かつ微分可能な関数で近似し、拡張カルマンフィルタを用いることにより、より早く、逐次的に、離散的な運転行動モデルのパラメータを学習することができ、より精度のよい運転支援が可能となる。
また、更新したパラメータを次回実行時の運転行動モデルのパラメータとして使用することで、パラメータを逐次更新して精度を上げながら運転支援を行うことができる。
<第2の実施の形態>
次に、第2の実施の形態について説明する。なお、第1の実施の形態と同様の構成となる部分については、同一符号を付して説明を省略する。
第2の実施の形態では、目標位置、走路速度情報、及び走路曲率情報を更に考慮して加速度及び操舵角を含む目標の運転操作量を計算する点が、第1の実施の形態と異なっている。第2の実施の形態における状態空間モデルは、図10に示すように、ドライバモデルと車両物理モデルとで表現される。第2のシステムモデルftは、図10に示すドライバモデルと車両物理モデルとで表現される。
図11に示すように、第2の実施の形態に係る運転支援装置210は、ドライバが目的地を入力するための入力操作部212と、自車両の位置を計測する位置計測部14と、自車両の前方を走行する前方車両を検出する前方車情報取得部16と、地図情報(道路ネットワークデータ)、道路情報、及び信号情報を記憶した道路ネットワークデータベース218と、道路ネットワークデータベース218から取得した信号情報に基づいて、次の信号の状態Sを取得する信号状態取得部20と、運転支援アクチュエータ40を制御するコンピュータ24と、運転支援を行うための運転支援アクチュエータ40とを備えている。
道路ネットワークデータベース218に記憶されている地図情報及び道路情報には、走路速度情報及び走路曲率情報が含まれている。走路速度情報には、当該走路における定常走行速度に関する情報が含まれている。定常走行速度は、当該走路における過去の走行データ及び制限速度等から算出される。また、走路曲率情報には、当該走路の曲率に関する情報が含まれている。
情報取得部226は、更に、入力操作部212により受け付けた目的地情報と、道路ネットワークデータベース218より得られた走路速度情報及び走路曲率情報とを各時刻tについて取得する。
運転特性パラメータデータベース228には、後述するパラメータ学習部236によって学習された運転特性パラメータが格納されている。
なお、第2の実施の形態では、車両の位置[xt,yt]と、車両の向きθtと、車両の速度vtと、車両の加速度atと、車両の曲率δtと、後述する条件パラメータpsignalと、目標速度を算出するためのモデルパラメータproad、pf、及びpcとを、以下の式(18)に示すように状態ベクトルXt=[xtyt θtvtatδtproadpf pc psignal]として構成する。proad、pf、pc、及びpsignalは、運転特性パラメータである。また、以下の式(19)に示すように、位置計測部14によって計測された車両の位置情報を観測ベクトルYt=[xt *yt *]として構成する。
目標運転操作量計算部230は、情報取得部226によって取得されたセンサ情報と、運転特性パラメータデータベース228に格納された運転特性パラメータと、運転行動モデルとに基づいて、加速度及び操舵角を含む目標の運転操作量を計算する。目標運転操作量計算部230は、図12に示すように、運転行動モデル計算部250と、車両物理モデル計算部350とを備えている。
運転行動モデル計算部250は、情報取得部226によって取得されたセンサ情報と、運転特性パラメータデータベース228に格納された運転特性パラメータと、運転行動モデルと、後述する車両物理モデル計算部350によって前回計算された車両の速度vt−1、及び車両の向きθt−1とに基づいて、時刻tの加速度及び操舵角を含む目標の運転操作量を計算する。運転行動モデル計算部250は、図13に示すように、運転操作受付部252と、経路生成部254と、目標位置制約部256と、目標位置選択部262と、横位置制御部264と、目標速度制約部266と、目標速度選択部276と、速度制御部278と、運転操作出力部280とを備えている。
運転操作受付部252は、車両物理モデル計算部350によって前回計算された時刻t−1の車両の速度vt−1、及び車両の向きθt−1と、運転特性パラメータデータベース228に格納されている運転特性パラメータproad、pf、pc、及びpsignalを受け付ける。
経路生成部254は、情報取得部226によって取得されたセンサ情報に含まれる目的地情報、地図情報、及び車両の位置[x* t−1,y* t−1]に基づいて、最短経路を探索するアルゴリズムや、最短時間の経路を探索するアルゴリズムなど(例えば、A*アルゴリズム)を用いて、出発地から目的地までの最短経路を生成する。
目標位置制約部256は、情報取得部226によって取得されたセンサ情報に含まれる車両の位置[x* t−1,y* t−1]に基づいて、複数の位置制約条件を満たす車両位置を算出する。目標位置制約部256は、信号制約部258と、経路制約部260とを備えている。
信号制約部258は、情報取得部226によって取得されたセンサ情報に含まれる信号情報、車両の位置[x* t−1,y* t−1]、及び運転操作受付部252によって受け付けた時刻t−1の車両の速度vt−1と、運転特性パラメータデータベース228に格納されている運転特性パラメータpsignalとに基づいて、信号停止に関する予め定められた位置制約条件を満たす車両位置を算出する。具体的には、信号制約部258は、以下の式(20)に示す位置制約条件を満たす車両位置[xr,yr]を算出する。
ここで、[xr,yr]Tは車両位置、[x* t−1,y* t−1]Tは時刻t−1の車両の位置、[xstopline,ystopline]Tは信号に対応した停止線の位置、St−1は前回の信号の状態(赤、黄、青)、Sredは赤信号、vt−1は、時刻t−1の車両の速度である。psignalは、停止するか否かを決定するためのパラメータであり、後述するパラメータ学習部236によって学習される。本実施の形態の形態では、psignalが条件パラメータに対応する。
経路制約部260は、経路生成部254によって車両の目的地に基づいて求められた経路情報と、情報取得部226によって取得されたセンサ情報に含まれる車両の位置[x* t−1,y* t−1]とに基づいて、経路に関する位置制約条件を満たす車両位置を算出する。例えば、経路制約部260は、目的地までの経路上における、車両の位置[x* t−1,y* t−1]から100m先の地点を車両位置として算出する。
目標位置選択部262は、信号制約部258によって算出された車両位置、及び経路制約部260によって算出された車両位置の何れかを選択し、選択された車両位置を目標位置[xr,yr]として決定する。例えば、目標位置選択部262は、センサ情報に含まれる車両の位置[x* t−1,y* t−1]から最も近い車両位置を目標位置[xr,yr]として選択する。
横位置制御部264は、目標位置選択部262によって選択された目標位置[xr,yr]と、センサ情報に含まれる車両の位置[x* t−1,y* t−1]と、運転操作受付部252によって受け付けた時刻t−1の[vt−1,θt−1]とに基づいて、例えば前方注視モデルを用いて、ドライバの目標のステアリング操作量を計算する。そして、横位置制御部264は、計算された目標のステアリング操作量に基づいて、目標の車両の曲率δを算出する。ステアリング操作量と曲率δとの関係は予め求められている。
目標速度制約部266は、情報取得部226によって取得されたセンサ情報と、目標位置選択部262によって選択された目標位置[xr,yr]と、運転操作受付部252によって受け付けた時刻t−1の車両の速度vt−1及び車両の向きθt−1とに基づいて、複数の速度制約条件の各々を満たす上限速度を算出する。複数の速度制約条件の各々は、運転操作における様々なシーンに対応する。目標速度制約部266は、前後位置制御制約部268と、定常走行制約部270と、追従制約部272と、カーブ制約部274とを備えている。
前後位置制御制約部268は、センサ情報に含まれる車両の位置[x* t−1,y* t−1]と、目標位置選択部262によって選択された走路上の目標位置[xr,yr]とに基づいて、前後位置制御に関する速度制約条件を満たす上限速度を算出する。例えば、前後位置制御制約部268は、比例制御モデルを用いて、センサ情報に含まれる車両の位置[x* t−1,y* t−1]と目標位置[xr,yr]との差に応じて、上限速度を算出する。
定常走行制約部270は、センサ情報に含まれる車両の位置[x* t−1,y* t−1]と、情報取得部226によって取得されたセンサ情報の道路情報に含まれる走路速度情報に基づいて、当該走路の定常走行速度を、以下の式(21)に示す定常走行に関する速度制約条件を満たす上限速度として算出する。具体的には、定常走行制約部270は、センサ情報に含まれる車両の位置[x* t−1,y* t−1]に基づいて、当該位置の走路速度情報から得られる、車両が現在走行中の走路の一般的な最高速度を表すパラメータproad(定常走行速度)を取得し、上限速度とする。proadは、当該走路での過去の走行データや制限速度等から算出される。proadは、後述するパラメータ学習部236によって学習される。
追従制約部272は、運転操作受付部252によって受け付けた時刻t−1の車両の速度vt−1と、センサ情報に含まれる前方車情報(前方車との相対速度)とに基づいて、以下の式(22)に示す前方車両追従に関する速度制約条件を満たす上限速度を算出する。具体的には、追従制約部272は、運転操作受付部252によって受け付けた時刻t−1の車両の速度[vt−1]と前方車との相対速度に基づいて、等速予測により、T秒先の前方車との車間距離の予測値^xf(t+T)を算出する。そして、追従制約部272は、式(22)の制約条件を満たす上限速度を算出する。
ここで、pfは後述されるパラメータ学習部236によって学習される。Lは車両の長さである。
カーブ制約部274は、センサ情報に含まれる車両の位置[x* t−1,y* t−1]と、情報取得部226によって取得されたセンサ情報の地図情報に含まれる走路曲率情報に基づいて、以下の式(23)に示すカーブ走行に関する速度制約条件を満たす上限速度を算出する。具体的には、カーブ制約部274は、センサ情報に含まれる車両の位置[x* t−1,y* t−1]に基づいて、当該位置におけるT秒先の走路の曲率の絶対値の予測値|^δ(t+T)|を算出し、算出された予測値|^δ(t+T)|に基づいて、式(23)を満たす上限速度を算出する。
なお、pcは後述されるパラメータ学習部236によって学習される。
目標速度選択部276は、目標速度制約部266によって算出された上限速度のうち、最も低い速度を目標速度vrとして選択する。
速度制御部278は、目標速度選択部276によって選択された目標速度vrと、時刻t−1の車両の速度vt−1とに基づいて、目標のアクセルペダル操作量を計算する。そして、速度制御部278は、計算された目標のアクセルペダル操作量に基づいて、目標の車両の加速度atを算出する。例えば、速度制御部278は、比例制御モデルを用いて、目標速度vrと推定された時刻t−1の車両の速度vt−1との差に応じて、加速度atを算出する。
運転操作出力部280は、横位置制御部264によって算出された目標の曲率δtと、速度制御部278によって算出された目標の加速度atとを、目標の運転操作量として運転支援制御部232へ出力する。また、運転操作出力部280は、曲率δtと加速度atとを車両物理モデル計算部350へ出力する。
車両物理モデル計算部350は、運転操作出力部280によって出力された目標の曲率δtと目標の加速度atとに基づいて、車両の物理量を計算する。車両物理モデル計算部350は、図14に示すように、速度算出部352と、向き算出部354と、位置算出部356と、車両物理モデル出力部358とを備えている。
速度算出部352は、運転操作出力部280によって出力された目標の車両の加速度atと、速度算出部352で前回算出された車両の速度vt−1とに基づいて、車両の速度vtを予測する。例えば、速度算出部352は、速度算出式vt=vt−1+atTに従って、車両の速度vtを予測する。
向き算出部354は、運転操作出力部280によって出力された目標の曲率δtと、向き算出部354で前回算出されたθt−1と、速度算出部352によって予測された車両の速度vtとに基づいて、車両の向きθtを予測する。
位置算出部356は、速度算出部352によって予測された車両の速度vtと、向き算出部354によって予測された車両の向きθtと、センサ情報に含まれる車両の位置[x* t−1,y* t−1]とに基づいて、車両の位置[xt,yt]を予測する。
車両物理モデル出力部358は、速度算出部352よって予測された車両の速度vtと、向き算出部354によって予測された車両の向きθtと、位置算出部356によって予測された車両の位置[xt,yt]とを出力する。出力された車両の速度vtと車両の向きθtとは、運転行動モデル計算部250によって次回用いられる。
運転支援制御部232は、目標運転操作量計算部230によって計算された目標の加速度atと目標の曲率δtとを実現するように、運転支援アクチュエータ40を制御する。
近似モデルデータベース234には、目標運転操作量計算部230において用いられる運転行動モデルを、連続かつ微分可能な関数で近似した近似モデルが格納されている。本実施の形態では、条件パラメータpsignalを用いた運動行動モデルについて、以下の式(24)に示すように、シグモイド関数によって連続かつ微分可能な関数で近似した近似モデルを導出することができる。具体的には、上記式(20)のうち、以下のif文内の()で示した条件パラメータpsignalについての条件分岐の部分を、近似モデルによって近似する。上記式(20)を近似する場合、信号停止における条件分岐(if節)のパラメータpsignalを学習するため、この信号停止における条件分岐を以下の式(24)のようにシグモイド関数で連続化する。
ここで、psigaは切り替え条件の不確かさを表すパラメータである。
パラメータ学習部236は、近似モデルデータベース234に格納された近似モデルと、情報取得部226によって取得されたセンサ情報に含まれる車両の位置情報[xt *,yt *]とに基づいて、運転行動モデルの条件分岐で用いられる条件パラメータpsignalを含む運動特性パラメータを学習する。図15に示すように、パラメータ学習部236は、予測部2360と、フィルタリング部2362とを備えている。
予測部2360は、図16に示すように、近似運転行動モデル計算部2400と、車両物理モデル計算部2500とを備えている。
近似運転行動モデル計算部2400は、上記式(24)で近似された運転行動を含む運転行動モデルを計算する。近似運転行動モデル計算部2400は、図17に示すように、運転操作受付部2402と、経路生成部2404と、目標位置制約部2406と、目標位置選択部2412と、横位置制御部2414と、目標速度制約部2416と、目標速度選択部2426と、速度制御部2428と、運転操作出力部2430とを備えている。また、目標位置制約部2406は、信号制約部2408と、経路生成部2404とを備えている。また、目標位置制約部2406は、信号制約部2408と、経路生成部2404とを備えている。目標速度制約部2416は、前後位置制御制約部2418と、定常走行制約部2420と、追従制約部2422と、カーブ制約部2424とを備えている。
信号制約部2408で計算する運転行動モデルは、上記式(24)に示すように、信号制約部258において用いられる運転行動モデルを近似したものである。従って、信号制約部2408における運転行動以外は、運転行動モデル計算部250の各部と同様に計算が行われる。
運転操作受付部2402は、フィルタリング部2362によって推定された時刻t−1の状態ベクトルXt−1の要素である[xt−1,yt−1,θt−1,vt−1,at−1,δt−1]と、運転特性パラメータデータベース228に格納されている運転特性パラメータproad、pf、pc、及びpsignalとを受け付ける。
経路生成部2404は、情報取得部226によって取得されたセンサ情報に含まれる目的地情報、及び地図情報と、運転操作受付部2402によって受け付けた時刻t−1の状態ベクトルXt−1の要素である[xt−1,yt−1]とに基づいて、経路生成部254と同様に、出発地から目的地までの最短経路を生成する。
信号制約部2408は、情報取得部226によって取得されたセンサ情報に含まれる信号情報と、運転操作受付部2402によって受け付けた状態ベクトルXt−1の要素である[xt−1,yt−1,vt−1]と、運転特性パラメータデータベース228に格納されている運転特性パラメータpsignalとに基づいて、信号停止に関する予め定められた位置制約条件を満たす車両位置を算出する。具体的には、信号制約部2408は、上記式(22)に示す位置制約条件を満たす車両位置[xr,yr]を算出する。
経路制約部2410は、経路生成部2404によって車両の目的地に基づいて求められた経路情報と、運転操作受付部2402によって受け付けた時刻t−1の状態ベクトルXt−1の要素である[xt−1,yt−1]とに基づいて、経路制約部260と同様に、経路に関する位置制約条件を満たす車両位置を算出する。
目標位置選択部2412は、目標位置選択部262と同様に、信号制約部2408によって算出された車両位置、及び経路制約部2410によって算出された車両位置の何れかを選択し、選択された車両位置を目標位置[xr,yr]として決定する。
横位置制御部2414は、目標位置選択部2412によって選択された目標位置[xr,yr]と、運転操作受付部2402によって受け付けた時刻t−1の状態ベクトルXt−1の要素である[xt−1,yt−1,vt−1,θt−1]とに基づいて、横位置制御部264と同様に、ドライバのステアリング操作量を予測する。
前後位置制御制約部2418は、運転操作受付部2402によって受け付けた時刻t−1の状態ベクトルXt−1の要素である[xt−1,yt−1]と、目標位置選択部2412によって選択された走路上の目標位置[xr,yr]とに基づいて、前後位置制御制約部268と同様に、前後位置制御に関する速度制約条件を満たす上限速度を算出する。
定常走行制約部2420は、運転操作受付部2402によって受け付けた時刻t−1の状態ベクトルXt−1の要素である[xt−1,yt−1]と、情報取得部226によって取得されたセンサ情報の道路情報に含まれる走路速度情報に基づいて、定常走行制約部270と同様に、定常走行に関する速度制約条件を満たす上限速度を算出する。
追従制約部2422は、運転操作受付部2402によって受け付けた時刻t−1の状態ベクトルXt−1の要素であるvt−1と、センサ情報に含まれる前方車情報(前方車との相対速度)とに基づいて、追従制約部272と同様に、前方車両追従に関する速度制約条件を満たす上限速度を算出する。
カーブ制約部2424は、運転操作受付部2402によって受け付けた時刻t−1の状態ベクトルXt−1の要素である[xt−1,yt−1]と、情報取得部226によって取得されたセンサ情報の地図情報に含まれる走路曲率情報に基づいて、カーブ制約部274と同様に、カーブ走行に関する速度制約条件を満たす上限速度を算出する。
目標速度選択部2426は、目標速度選択部276と同様に、目標速度制約部2416によって算出された上限速度のうち、最も低い速度を目標速度vrとして選択する。
速度制御部2428は、目標速度選択部2426によって選択された目標速度vrと、運転操作受付部2402によって受け付けた時刻t−1の車両の速度vt−1とに基づいて、速度制御部278と同様に、アクセルペダルの操作量を予測し、車両の加速度atを算出する。
運転操作出力部2430は、横位置制御部2414によって算出された曲率δtと、速度制御部2428によって算出された加速度atとを車両物理モデル計算部2500へ出力する。
車両物理モデル計算部2500は、運転操作出力部2430によって出力された曲率δtと加速度atとに基づいて、車両の物理量を計算する。車両物理モデル計算部2500は、図18に示すように、車両物理モデル受付部2502と、速度算出部2504と、向き算出部2506と、位置算出部2508と、車両物理モデル出力部2510とを備えている。
車両物理モデル受付部2502は、フィルタリング部2362によって推定された時刻t−1の[xt−1,yt−1,θt−1,vt−1,at−1,δt−1]を受け付ける。
速度算出部2504は、運転操作出力部2430によって出力された車両の加速度vtと、車両物理モデル受付部2502によって受け付けた時刻t−1の車両の速度vt−1とに基づいて、速度算出部352と同様に、車両の速度vtを予測する。
向き算出部2506は、運転操作出力部2430によって出力された曲率δtと、車両物理モデル受付部2502によって受け付けた時刻t−1の車両の向きθt−1と、速度算出部2504によって予測された車両の速度vtとに基づいて、向き算出部354と同様に、車両の向きθtを予測する。
位置算出部2508は、速度算出部2504によって予測された車両の速度vtと、向き算出部2506によって予測された車両の向きθtと、車両物理モデル受付部2502によって受け付けた時刻t−1の車両の位置[xt−1,yt−1]とに基づいて、位置算出部356と同様に、車両の位置[xt,yt]を予測する。
上記式(18)における[xt,yt,θt,vt,at,δt]については、以上のように予測するが、運動特性パラメータproad、pf、pc、及びpsignalについては、時刻t−1の運動特性パラメータproad、pf、pc、及びpsignalをそのまま代入する。
また、予測部2360は、上記式(6)〜(7)に従って、上記式(24)で示した近似モデルを含む運転行動モデルの偏微分を行い、行列^Ftと行列^Gtとを算出する。上記式(24)は、シグモイド関数によって連続かつ微分可能な関数で近似されているため、偏微分をすることができる。
そして、予測部2360は、上記式(8)に従って、事前誤差共分散行列Pt|t−1を算出する。
フィルタリング部2362は、情報取得部226によって取得されたセンサ情報に含まれる車両の位置情報[x* t,y* t]と、予測部2360によって予測された時刻tの状態ベクトルXtと、予測部2360によって算出された事前誤差共分散行列Pt|t−1とに基づいて、第1の実施の形態と同様に、時刻tにおける状態ベクトルXtを推定する(フィルタリングステップ)。そして、フィルタリング部2362は、推定された状態ベクトルXt|tの要素である[proad,pf,pc,psignal]を運転特性パラメータとして更新し、運転特性パラメータデータベース228へ格納する。
<運転支援装置210の動作>
次に、第2の実施の形態に係る運転支援装置210の動作について説明する。まず、ドライバの運転操作により車両が走行し、運転支援装置210の情報取得部226によってセンサ情報が各時刻tについて取得されているときに、運転支援装置210において、図19に示す運転支援処理ルーチンが実行される。
ステップS2104において、目標運転操作量計算部230によって、上記ステップS102で受け付けたセンサ情報と、上記ステップS100で読み出された運転特性パラメータpsignalとに基づいて、現時刻tにおける加速度及び操舵量を含む目標の運転操作量を計算する。ステップS2104は、図20に示す目標運転操作量計算処理ルーチンによって実現される。
<目標運転操作量計算処理ルーチン>
まず、ステップS600において、経路生成部254によって、上記ステップS102で受け付けたセンサ情報に含まれる目的地情報、及び車両の位置[x* t−1,y* t−1]とに基づいて、出発地から目的地までの最短経路を生成する。
ステップS602において、信号制約部258によって、上記ステップS102で受け付けた時刻t−1の車両のセンサ情報に含まれる信号情報、及び車両の位置[x* t−1,y* t−1]と、前回のステップS618で予測された時刻t−1の車両の速度vt−1とに基づいて、上記式(20)に従って、信号停止に関する予め定められた位置制約条件を満たす車両位置[xr,yr]を設定する。
ステップS604において、経路制約部260によって、上記ステップS600で生成された経路情報と、上記ステップS102で受け付けたセンサ情報に含まれる車両の位置[x* t−1,y* t−1]とに基づいて、経路上における位置[xr,yr]を設定する。
ステップS606において、目標位置選択部262によって、上記ステップS602で設定された車両位置と、上記ステップS604で設定された車両位置と、上記ステップS102で受け付けたセンサ情報に含まれる車両の位置[x* t−1,y* t−1]とに基づいて、[x* t−1,y* t−1]から最も近い経路上における位置[xr,yr]を目標位置として選択する。
ステップS608において、横位置制御部264によって、上記ステップS606で選択された目標位置[xr,yr]と、上記ステップS102で受け付けたセンサ情報に含まれる車両の位置[x* t−1,y* t−1]と、前回のステップS618で計算された時刻t−1の車両の目標速度vt−1,及び目標の車両の向きθt−1とに基づいて、ドライバの目標のステアリング操作量を計算する。そして、計算された目標のステアリング操作量に基づいて、目標の車両の曲率δを算出する。
ステップS610において、複数の速度制約条件の各々を満たす上限速度を算出する。
本ステップでは、まず、前後位置制御制約部268によって、上記ステップS102で受け付けたセンサ情報に含まれる車両の位置[x* t−1,y* t−1]と、上記ステップS606で選択された目標位置[xr,yr]とに基づいて、上限速度を算出する。
次に、定常走行制約部270によって、上記ステップS102で受け付けたセンサ情報に含まれる車両の位置[x* t−1,y* t−1]と、上記ステップS102で受け付けたセンサ情報に含まれる走路速度情報に基づいて、当該走路の定常走行速度を、上記式(19)に示す速度制約条件の上限速度として算出する。
また、追従制約部272によって、前回のステップS618で推定された時刻t−1の車両の速度vt−1と、上記ステップS102で受け付けたセンサ情報に含まれる前方車情報(前方車との相対速度)とに基づいて、上記式(20)に示す速度制約条件の上限速度を算出する。
そして、カーブ制約部274によって、上記ステップS102で受け付けたセンサ情報に含まれる車両の位置[x* t−1,y* t−1]と、上記ステップS102で受け付けたセンサ情報の地図情報に含まれる走路曲率情報に基づいて、上記式(21)に示す速度制約条件の上限速度を算出する。
ステップS612において、目標速度選択部276によって、上記ステップS610で算出された上限速度のうち、最も低い速度を目標速度vrとして選択する。
ステップS614において、速度制御部278によって、上記ステップS612で選択された目標速度vrと、前回のステップS110で推定された時刻t−1の状態ベクトルXt−1の要素である[vt−1]とに基づいて、目標のアクセルペダル操作量を計算する。そして、計算された目標のアクセルペダル操作量に基づいて、目標の車両の加速度aを算出する。
ステップS616において、運転操作出力部280によって、上記ステップS608で算出された目標の曲率δと、上記ステップS614で算出された目標の加速度aとを、時刻tにおける目標の曲率δtと加速度atとして出力する。
ステップS618において、速度算出部352によって、上記ステップS616で算出された車両の加速度atと、前回本ステップS618で算出された車両の速度vt−1とに基づいて、車両の速度vtを予測する。
ステップS620において、向き算出部354によって、上記ステップS616で算出された曲率δtと、前回本ステップS620で算出されたθt−1と、上記ステップS618で予測された車両の速度vtとに基づいて、車両の向きθtを予測する。
ステップS622において、位置算出部356によって、上記ステップS618で予測された車両の速度vtと、上記ステップS620で予測された車両の向きθtと、センサ情報に含まれる車両の位置[x* t−1,y* t−1]とに基づいて、車両の位置[xt,yt]を予測する。
ステップS624において、上記ステップS616で予測された時刻tにおける曲率δtと加速度atとを結果として出力して目標運転操作量計算処理ルーチンを終了する。
次に、運転支援処理ルーチンに戻り、ステップS2110において、パラメータ学習処理を実行する。ステップS2110は、図21に示す、パラメータ学習処理ルーチンによって実現される。
<パラメータ学習処理ルーチン>
まず、ステップS2300において、予測部2360によって、予測処理を実行する。ステップS2300は、図22に示す予測処理ルーチンによって実現される。
<予測処理ルーチン>
まず、ステップS700において、経路生成部2404によって、上記ステップS102で受け付けたセンサ情報に含まれる目的地情報と、前回のステップS302で推定された時刻t−1の状態ベクトルXt−1の要素[xt−1,yt−1]とに基づいて、出発地から目的地までの最短経路を生成する。
ステップS702において、信号制約部2408によって、上記ステップS102で受け付けた時刻t−1の車両のセンサ情報に含まれる信号情報と、前回のステップS302で推定された時刻t−1の状態ベクトルXt−1の要素[xt−1,yt−1,vt−1]とに基づいて、上記式(24)に従って、信号停止に関する予め定められた位置制約条件を満たす車両位置[xr,yr]を設定する。
ステップS704において、経路制約部2410によって、上記ステップS702で生成された経路情報と、前回のステップS302で推定された時刻t−1の状態ベクトルXt−1の要素[xt−1,yt−1]とに基づいて、経路上における位置[xr,yr]を設定する。
ステップS706において、目標位置選択部2412によって、上記ステップS702で設定された車両位置と、上記ステップS704で設定された車両位置と、前回のステップS302で推定された時刻t−1の状態ベクトルXt−1の要素[xt−1,yt−1]とに基づいて、[xt−1,yt−1]から最も近い経路上における位置[xr,yr]を目標位置として選択する。
ステップS708において、横位置制御部2414によって、上記ステップS706で選択された目標位置[xr,yr]と、前回のステップS302で推定された時刻t−1の状態ベクトルXt−1の要素[xt−1,yt−1,vt−1,θt−1]とに基づいて、ドライバのステアリング操作量を予測する。そして、予測されたステアリング操作量に基づいて、車両の曲率δtを算出する。
ステップS710において、複数の速度制約条件の各々を満たす上限速度を算出する。
本ステップでは、まず、前後位置制御制約部2418によって、前回のステップS302で推定された時刻t−1の状態ベクトルXt−1の要素[xt−1,yt−1]と、上記ステップS706で選択された目標位置[xr,yr]とに基づいて、上限速度を算出する。
次に、定常走行制約部2420によって、前回のステップS302で推定された時刻t−1の状態ベクトルXt−1の要素[xt−1,yt−1]と、上記ステップS102で受け付けたセンサ情報に含まれる走路速度情報に基づいて、当該走路の定常走行速度を、上記式(19)に示す速度制約条件の上限速度として算出する。
また、追従制約部2422によって、前回のステップS302で推定された時刻t−1の状態ベクトルXt−1の要素vt−1と、上記ステップS102で受け付けたセンサ情報に含まれる前方車情報(前方車との相対速度)とに基づいて、上記式(20)に示す速度制約条件の上限速度を算出する。
そして、カーブ制約部2424によって、前回のステップS302で推定された時刻t−1の状態ベクトルXt−1の要素[xt−1,yt−1]と、上記ステップS102で受け付けたセンサ情報の地図情報に含まれる走路曲率情報に基づいて、上記式(21)に示す速度制約条件の上限速度を算出する。
ステップS712において、目標速度選択部2426によって、上記ステップS710で算出された上限速度のうち、最も低い速度を目標速度vrとして選択する。
ステップS714において、速度制御部2428によって、上記ステップS712で選択された目標速度vrと、前回のステップS302で推定された時刻t−1の状態ベクトルXt−1の要素[vt−1]とに基づいて、アクセルペダルの操作量を予測する。そして、予測されたアクセルペダルの操作量に基づいて、車両の加速度aを算出する。
ステップS716において、運転操作出力部2430によって、前回のステップS302で推定された時刻t−1の状態ベクトルXt−1の要素[at−1,δt−1]と、上記ステップS708で算出された曲率δと、上記ステップS714で算出された加速度aとに基づいて、時刻tにおける曲率δtと加速度atとを予測する。
ステップS718において、速度算出部2504によって、上記ステップS716で算出された車両の加速度atと、前回のステップS302で推定された時刻t−1の状態ベクトルXt−1の要素vt−1とに基づいて、車両の速度vtを予測する。
ステップS720において、向き算出部2506によって、上記ステップS716で算出された曲率δtと、前回のステップS302で推定された時刻t−1の状態ベクトルXt−1の要素θt−1と、上記ステップS718で予測された車両の速度vtとに基づいて、車両の向きθtを予測する。
ステップS722において、位置算出部2508によって、上記ステップS718で予測された車両の速度vtと、上記ステップS720で予測された車両の向きθtと、前回のステップS302で推定された時刻t−1の状態ベクトルXt−1の要素[xt−1,yt−1]とに基づいて、車両の位置[xt,yt]を予測する。
ステップS724において、上記ステップS716で予測された時刻tにおける曲率δtと加速度atと、上記ステップS718で予測された車両の速度vtと、上記ステップS720で予測された車両の向きθtと、上記ステップS722で予測された車両の位置[xt,yt]とを状態ベクトルXtとして構成する。なお、運動特性パラメータproad、pf、pc、及びpsignalについては、時刻t−1の運動特性パラメータproad、pf、pc、及びpsignalをそのまま代入し、状態ベクトルXtを[xt,yt,θt,vt,at,δt,proad,pf,pc,psignal]として構成する。
ステップS726において、予測部2360によって、上記ステップS724で構成された時刻tの状態ベクトルXtに基づいて、上記式(6)〜(7)に従って、近似モデルの偏微分を行い、行列^Ftと行列^Gtとを算出する。そして、予測部2360によって、行列^Ftと行列^Gtと、前回のステップS302で推定された事後誤差共分散行列Pt−1|t−1とに基づいて、上記式(8)に従って、事前誤差共分散行列Pt|t−1を算出する。
ステップS728において、予測部2360によって、上記ステップS724で構成された時刻tの状態ベクトルXtと、上記ステップS726で算出された事前誤差共分散行列Pt|t−1とを結果として出力する。
次に、パラメータ学習処理ルーチンに戻り、ステップS302において、フィルタリング部2362によって、第1の実施の形態と同様に、時刻tにおける状態ベクトルXtを推定する(フィルタリングステップ)。
そして、ステップS304において、上記ステップS302で推定された状態ベクトルXt|tの要素である[proad,pf,pc,psignal]を運転特性パラメータとして更新し、運転特性パラメータデータベース228へ格納する。
なお、第2の実施の形態に係る運転支援装置の他の構成及び作用については、第1の実施の形態と同様であるため、説明を省略する。
以上説明したように、第2の実施の形態に係る運転支援装置によれば、運転行動モデルとして、定常走行に関する速度制約条件を満たす上限速度として算出し、カーブ走行に関する速度制約条件を満たす上限速度を算出し、前方車両追従に関する速度制約条件を満たす上限速度を算出し、前後位置制御に関する速度制約条件を満たす上限速度を算出し、算出された上限速度のうちの何れかを目標速度として選択し、かつ信号停止に関する位置制約条件を満たす車両位置を算出し、経路に関する位置制約条件を満たす車両位置を算出し、算出された車両位置のうちの何れかを選択し、選択した位置を目標位置として決定し、運転行動モデルの条件分岐で用いられる条件パラメータを含むモデルパラメータを学習することにより、ドライバに合ったモデルパラメータを学習することができる。
また、以上説明したように、第2の実施の形態に係る運転支援装置によれば、学習された運転特性パラメータと、運転行動モデルと、取得されたセンサ情報とに基づいて、車両のドライバの運転支援を行うことにより、ドライバに合った運転支援を行うことができる。
なお、上記の実施の形態では、パラメータ学習部36(236)と、運転支援制御部32(232)とが1つの装置で構成される場合を例に説明したが、これに限定されるものではなく、別々の装置として構成してもよい。
また、上記の実施の形態における前方車情報取得部16は、レーザレーダ装置を用いる場合を例に説明したが、これに限定されるものではない。例えば、前方車情報取得部16として、車車間通信装置や、路車間通信装置等を用いて、前方車両情報を取得してもよい。
また、上記の実施の形態では、上述の運転支援装置は、道路ネットワークデータベース18(218)を備えている場合を例に説明したが、これに限定されるものではなく、例えば、道路ネットワークデータベース18(218)が車両挙動予測装置の外部装置に設けられ車両挙動予測装置は、外部装置と通信手段を用いて通信することにより、道路ネットワークデータベース18(218)を参照するようにしてもよい。
また、第2の実施の形態では、目標速度制約部266において、複数の速度制約条件の各々を満たす上限速度を算出し、目標速度選択部276において、上限速度のうち最も低いものを目標速度として選択する場合を例に説明したが、これに限定されるものではなく、複数の速度制約条件の各々の組み合わせに基づいて、目標速度を選択するようにしてもよい。
また、本実施の形態では、運転支援の方法として車両の制御を行う場合を例に説明したが、これに限定されるものではなく、例えば、ドライバへの情報提示であってもよい。
なお、本発明のプログラムは、記録媒体に格納して提供することができる。