JP2015161545A - 車両挙動予測装置及びプログラム - Google Patents

Abstract

【課題】車両の外部環境情報と、車両の位置情報とに基づいて、車両の位置情報及び運動状態を精度よく予測することができるようにする。
【解決手段】運転操作予測部２６が、車両情報取得部２２によって取得された時刻ｔ＋１の車両の外部環境情報と、地図情報と、運転操作車両状態推定部３２によって推定された時刻ｔ＋１の車両の運動状態及び車両のドライバの運転操作量とに基づいて、時刻ｔ＋２の車両のドライバの運転操作量を予測する。そして、車両挙動予測部２８は、運転操作予測部２６によって予測された時刻ｔ＋２のドライバの運転操作量と、運転操作車両状態推定部３２によって推定された時刻ｔ＋１の車両の運動状態とに基づいて、時刻ｔ＋２の車両の位置情報及び車両の運動状態を予測する。
【選択図】図１

Description

本発明は、車両挙動予測装置及びプログラムに係り、特に、車両の位置情報及び運動状態を予測する車両挙動予測装置及びプログラムに関する。

従来より、複数種類の操作状態毎に予め求めた道路状態と車両の加速度との関係と、各地点の道路状態と、予測された操作状態の時系列データとに基づいて、走行路における車両の加速度の時系列データを予測し、走行路における車両の加速度の時系列データに基づいて、走行路における車両の速度の時系列データを予測する速度予測装置が知られている（特許文献１）。

特開２０１２−１６８０３７号公報

しかしながら、上記の特許文献１に記載の技術では、車両の位置情報以外に、車速や加速度、操作量など、車両外から観測することが不可能な情報を使っているため、当該装置が搭載されている車両の予測に利用することができない。

また、複数の運転シーンを想定していないため、一般道運転の予測精度が低い。

本発明は、上記の事情に鑑みてなされたもので、車両の位置情報及び運動状態を精度よく予測することができる車両挙動予測装置及びプログラムを提供することを目的とする。

上記の目的を達成するために本発明に係る車両挙動予測装置は、車両の位置情報及び前記車両の外部環境情報を各時刻ｔについて取得する取得手段と、前記取得手段によって取得された時刻ｔ−１の前記車両の外部環境情報と、時刻ｔ−１の前記車両の運動状態とに基づいて、時刻ｔの前記車両のドライバの運転操作量を予測する運転操作予測手段と、前記運転操作予測手段によって予測された時刻ｔの前記ドライバの運転操作量と、時刻ｔ−１の前記車両の運動状態とに基づいて、時刻ｔの前記車両の位置情報及び前記車両の運動状態を予測する車両挙動予測手段と、前記取得手段によって取得された時刻ｔの車両の位置情報と、前記車両挙動予測手段によって予測された時刻ｔの前記車両の位置情報とに基づいて、時刻ｔにおける前記車両の運動状態を推定する運転操作車両状態推定手段と、を含み、前記運転操作予測手段は、前記取得手段によって取得された時刻ｔの前記車両の外部環境情報と、前記運転操作車両状態推定手段によって推定された時刻ｔの前記車両の運動状態に基づいて、時刻ｔ＋１の前記車両のドライバの運転操作量を予測し、前記車両挙動予測手段は、前記運転操作予測手段によって予測された時刻ｔ＋１の前記ドライバの運転操作量と、前記運転操作車両状態推定手段によって推定された時刻ｔの前記車両の運動状態とに基づいて、時刻ｔ＋１の前記車両の位置情報及び前記車両の運動状態を予測する
。

本発明に係るプログラムは、コンピュータを、車両の位置情報及び前記車両の外部環境情報を各時刻ｔについて取得する取得手段と、前記取得手段によって取得された時刻ｔ−１の前記車両の外部環境情報と、時刻ｔ−１の前記車両の運動状態とに基づいて、時刻ｔの前記車両のドライバの運転操作量を予測する運転操作予測手段、前記運転操作予測手段によって予測された時刻ｔの前記ドライバの運転操作量と、時刻ｔ−１の前記車両の運動状態とに基づいて、時刻ｔの前記車両の位置情報及び前記車両の運動状態を予測する車両挙動予測手段、及び前記取得手段によって取得された時刻ｔの車両の位置情報と、前記車両挙動予測手段によって予測された時刻ｔの前記車両の位置情報とに基づいて、時刻ｔにおける前記車両の運動状態を推定する運転操作車両状態推定手段として機能させるプログラムであって、前記運転操作予測手段は、前記取得手段によって取得された時刻ｔの前記車両の外部環境情報と、前記運転操作車両状態推定手段によって推定された時刻ｔの前記車両の運動状態に基づいて、時刻ｔ＋１の前記車両のドライバの運転操作量を予測し、前記車両挙動予測手段は、前記運転操作予測手段によって予測された時刻ｔ＋１の前記ドライバの運転操作量と、前記運転操作車両状態推定手段によって推定された時刻ｔの前記車両の運動状態とに基づいて、時刻ｔ＋１の前記車両の位置情報及び前記車両の運動状態を予測するプログラムである。

本発明によれば、取得手段によって、車両の位置情報及び車両の外部環境情報を各時刻ｔについて取得する。

そして、運転操作予測手段によって、取得手段によって取得された時刻ｔ−１の車両の外部環境情報と、時刻ｔ−１の車両の運動状態とに基づいて、時刻ｔの車両のドライバの運転操作量を予測する。

そして、車両挙動予測手段によって、運転操作予測手段によって予測された時刻ｔのドライバの運転操作量と、時刻ｔ−１の車両の運動状態とに基づいて、時刻ｔの車両の位置情報及び車両の運動状態を予測する。

そして、運転操作車両状態推定手段によって、取得手段によって取得された時刻ｔの車両の位置情報と、車両挙動予測手段によって予測された時刻ｔの車両の位置情報とに基づいて、時刻ｔにおける車両の運動状態を推定する。

また、運転操作予測手段は、取得手段によって取得された時刻ｔの車両の外部環境情報と、運転操作車両状態推定手段によって推定された時刻ｔの車両の運動状態に基づいて、時刻ｔ＋１の車両のドライバの運転操作量を予測する。

また、車両挙動予測手段は、運転操作予測手段によって予測された時刻ｔ＋１のドライバの運転操作量と、運転操作車両状態推定手段によって推定された時刻ｔの車両の運動状態とに基づいて、時刻ｔ＋１の車両の位置情報及び車両の運動状態を予測する。

このように、車両の外部環境情報と、車両の位置情報とに基づいて、車両の位置情報及び運動状態を精度よく予測することができる。

本発明に係る、前記運転操作予測手段は、前記取得手段によって取得された時刻ｔの前記車両の外部環境情報と、前記運転操作車両状態推定手段によって推定された時刻ｔの前記車両の運動状態の確率分布に基づいて、時刻ｔ＋１の前記車両のドライバの運転操作量の確率分布を予測し、前記車両挙動予測手段は、前記運転操作予測手段によって予測された時刻ｔ＋１の前記ドライバの運転操作量の確率分布と、前記運転操作車両状態推定手段によって推定された時刻ｔの前記車両の運動状態の確率分布とに基づいて、時刻ｔ＋１の前記車両の位置情報の確率分布及び前記車両の運動状態の確率分布を予測し、前記運転操作車両状態推定手段は、前記取得手段によって取得された時刻ｔ＋１の車両の位置情報と、前記車両挙動予測手段によって予測された時刻ｔ＋１の前記車両の位置情報の確率分布とに基づいて、時刻ｔ＋１における前記車両の運動状態の確率分布を推定するようにすることができる。

また、本発明は、前記外部環境情報に基づいて、予め定められた複数の速度制約条件の各々を満たす上限速度を算出し、前記算出された上限速度のうちの何れかを目標速度として選択する目標速度選択手段を更に含み、前記運転操作予測手段は、前記目標速度選択手段によって選択された前記目標速度と、前記取得手段によって取得された時刻ｔの前記車両の外部環境情報と、前記運転操作車両状態推定手段によって推定された時刻ｔの前記車両の運動状態に基づいて、前記時刻ｔ＋１の前記車両のドライバの運転操作量を予測するようにすることができる。

また、本発明は、各時刻ｔについて、前記車両の走路上の目標位置を決定する目標位置決定手段を更に含み、前記取得手段は、前記車両の外部環境情報として、前記車両が走行する走路における定常走行速度と、前記車両が走行する走路の曲率と、前記車両の前方を走行する車両との距離を示す前方車両情報とを、各時刻ｔについて取得し、前記目標速度選択手段は、前記定常走行速度を、定常走行に関する速度制約条件を満たす上限速度として算出し、前記走路の曲率に基づいて、カーブ走行に関する速度制約条件を満たす上限速度を算出し、前記前方車両情報に基づいて、前方車両追従に関する速度制約条件を満たす上限速度を算出し、前記目標位置決定手段によって決定された走路上の目標位置に基づいて、前後位置制御に関する速度制約条件を満たす上限速度を算出し、前記算出された上限速度のうちの何れかを目標速度として選択するようにすることができる。

また、本発明における前記取得手段は、前記車両の外部環境情報として、更に、前記車両の走路上の信号情報と、前記車両の目的地とを、各時刻ｔについて取得し、前記目標位置決定手段は、前記信号情報に基づいて、信号停止に関する予め定められた位置制約条件を満たす車両位置を算出し、前記車両の目的地に基づいて求められた経路情報に基づいて、経路に関する位置制約条件を満たす車両位置を算出し、前記算出された車両位置のうちの何れかを選択し、選択した位置を目標位置として決定するようにすることができる。

また、本発明における前記運転操作予測手段は、前記取得手段によって取得された時刻ｔの前記車両の外部環境情報と、前記運転操作車両状態推定手段によって推定された時刻ｔの前記車両の運動状態及び前記車両のドライバの運転操作量に基づいて、時刻ｔ＋１の前記車両のドライバの運転操作量を予測し、前記運転操作車両状態推定手段は、前記取得手段によって取得された時刻ｔ＋１の車両の位置情報と、前記車両挙動予測手段によって予測された時刻ｔ＋１の前記車両の位置情報とに基づいて、時刻ｔ＋１における前記車両の運動状態及び前記車両のドライバの運転操作量を推定するようにすることができる。

また、本発明のプログラムは、記憶媒体に格納して提供することも可能である。

以上説明したように、本発明の速度予測装置及びプログラムによれば、取得された時刻ｔの車両の位置情報と、予測された時刻ｔの車両の位置情報とに基づいて、時刻ｔにおける車両の運動状態を推定し、時刻ｔの車両の外部環境情報と、推定された時刻ｔの車両の運動状態に基づいて、時刻ｔ＋１の車両のドライバの運転操作量を予測し、予測された時刻ｔ＋１のドライバの運転操作量と、推定された時刻ｔの車両の運動状態とに基づいて、時刻ｔ＋１の車両の位置情報及び運動状態を予測することにより、車両の外部環境情報と、車両の位置情報とに基づいて、車両の位置情報及び運動状態を精度よく予測することができる、という効果が得られる。

本発明の第１の実施の形態に係る車両挙動予測装置を示すブロック図である。 本発明の第１の実施の形態に係る状態空間モデルを示す図である。 本発明の第１の実施の形態に係る車両挙動予測装置における予測処理の概念を示す概念図である。 本発明の第１の実施の形態に係る車両挙動予測装置におけるフィルタリング処理の概念を示す概念図である。 本発明の第１の実施の形態に係る車両挙動予測装置の運転操作予測部２６の詳細を示すブロック図である。 本発明の第１の実施の形態に係る車両挙動予測装置の車両挙動予測部２８の詳細を示すブロック図である。 本発明の第１の実施の形態に係る車両挙動予測装置における車両挙動予測処理ルーチンの内容を示すフローチャートである。 本発明の第１の実施の形態に係る車両挙動予測装置における予測処理ルーチンの内容を示すフローチャートである。 本発明の第１の実施の形態に係る車両挙動予測装置における運転操作予測処理ルーチンの内容を示すフローチャートである。 本発明の第１の実施の形態に係る車両挙動予測装置における車両予測処理ルーチンの内容を示すフローチャートである。 本発明の第１の実施の形態に係る車両挙動予測装置における運転操作車両状態推定処理ルーチンの内容を示すフローチャートである。 本発明の第２の実施の形態に係る車両挙動予測装置を示すブロック図である。 本発明の第２の実施の形態に係る車両挙動予測装置の運転操作予測部２２６の詳細を示すブロック図である。

以下、図面を参照して、本発明の実施の形態について説明する。車両の位置情報と、車両の外部環境情報とに基づいて、運転操作量及び運動状態を推定し、車両の挙動を予測する車両挙動予測装置に本発明を適用した場合を例に説明する。なお、本実施の形態では、予測対象車両を自車両とし、運転操作量及び運動状態を推定し、車両の挙動を予測するために、拡張カルマンフィルタを用いた場合を例に説明する。

図１に示すように、第１の実施の形態に係る車両挙動予測装置１０は、ドライバが目的地を入力するための入力操作部１２と、地図情報（道路ネットワークデータ）、及び道路情報を記憶した道路ネットワークデータベース１４と、予測対象車両の前方を走行する前方車両を検出する前方車情報取得部１６と、予測対象車両の位置を計測する位置計測部１８と、車両の挙動を予測するコンピュータ２０と、予測結果を出力する出力装置４０とを備えている。

道路ネットワークデータベース１４には、地図情報、及び道路情報が記憶されている。道路情報には、走路速度情報及び走路曲率情報が含まれている。走路速度情報には、当該走路における定常走行速度に関する情報が含まれている。定常走行速度は、当該走路における過去の走行データ及び制限速度等から算出される。また、走路曲率情報には、当該走路の曲率に関する情報が含まれている。

前方車情報取得部１６は、例えばレーザレーダ装置が該当し、予測対象車両に対する前方車両までの走行方向の相対距離を計測し、相対距離と前方車との相対速度とを前方車両情報として各時刻ｔについて取得する。なお、相対速度は、相対距離を時間微分することにより得られる。

位置計測部１８は、例えば、ＧＰＳセンサを用いて構成され、予測対象車両の位置を各時刻ｔについて計測する。

コンピュータ２０は、ＣＰＵと、ＲＡＭと、後述する車両挙動予測処理ルーチン、予測処理ルーチン、運転操作予測処理ルーチン、車両予測処理ルーチン、及び運転操作車両状態推定処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。コンピュータ２０は、入力操作部１２により入力された目的地情報を取得すると共に、道路ネットワークデータベース１４から得られる地図情報及び道路情報、並びに前方車情報取得部１６によって検出された前方車両情報を、車両の外部環境情報として取得する車両情報取得部２２と、取得した外部環境情報に基づいて、ドライバの運転操作量、車両の位置情報及び運動状態を予測する予測部２４と、位置計測部１８によって計測された車両の位置情報を取得する位置情報取得部３０と、位置情報取得部３０によって取得された位置情報と、予測部２４によって予測された予測結果とに基づいて、ドライバの運転操作量、及び車両の運動状態を推定する運転操作車両状態推定部３２とを備えている。なお、予測部２４で行う処理が拡張カルマンフィルタにおける予測ステップに対応し、運転操作車両状態推定部３２で行う処理が拡張カルマンフィルタにおけるフィルタリングステップに対応する。また、車両情報取得部２２、及び位置情報取得部３０は、取得手段の一例である。

車両情報取得部２２は、車両の外部環境情報を各時刻ｔについて取得する。
具体的には、車両情報取得部２２は、車両の挙動を予測するときに、前方車情報取得部１６によって取得された前方車両情報と、道路ネットワークデータベース１４から得られる当該位置における地図情報及び道路情報とを逐次取得して、車両の外部環境情報として蓄積する。また、車両情報取得部２２は、入力操作部１２により受け付けた目的地情報を各時刻ｔについて取得する。

本実施の形態では、拡張カルマンフィルタを用いて、ドライバの運転操作量、車両の位置情報及び運動状態を予測し、ドライバの運転操作量及び運動状態を推定する。以下、拡張カルマンフィルタの原理について説明する。

本実施の形態では、車両の位置［ｘ_ｔ，ｙ_ｔ］と、車両の向きθ_ｔと、車両の速度ｖ_ｔと、車両の加速度α_ｔと、車両の曲率δ_ｔとを、以下の式（１）に示すように状態ベクトルＸ_ｔ＝［ｘ_ｔｙ_ｔθ_ｔｖ_ｔα_ｔδ_ｔ］として構成する。また、以下の式（２）に示すように、位置計測部１８によって計測された車両の位置情報を観測ベクトルＹ_ｔ＝［ｘ_ｔ ^＊ｙ_ｔ ^＊］として構成する。

（１）予測ステップ
まず、予測ステップにおける処理の概要について説明する。予測ステップでは、以下の式（３）に示すように、時刻ｔ−１の状態ベクトルＸ_ｔ−１と、システム雑音ｕ_ｔ（誤差項）とに基づいて、時刻ｔの状態ベクトルＸ_ｔを算出する。以下の式（３）におけるｆ_ｔは非線形関数であり、本実施の形態では、図２に示すように、ドライバモデルと車両物理モデルとで表現される。図２のドライバモデルが運転操作予測部２６に対応し、車両物理モデルが車両挙動予測部２８に対応する。

また、観測ベクトルＹ_ｔは、観測行列Ｈ_ｔと観測雑音ｗ_ｔ（誤差項）とに基づいて、以下の式（４）に示すように算出される。なお、観測行列Ｈ_ｔは予め求められている。

図３に、予測ステップの概念図を示す。図３に示すように、予測ステップでは、フィルタリングステップで推定された時刻ｔ−１の状態ベクトルＸ_{ｔ−１｜ｔ−１}に基づいて、時刻ｔの状態ベクトルＸ_{ｔ｜ｔ−１}を予測する（中心値の予測）。なお、Ｘ_{ｔ｜ｔ−１}は、時刻ｔ−１までに利用可能なデータに基づき予測された時刻ｔの状態ベクトルを表す。また、Ｘ_{ｔ−１｜ｔ−１}は、時刻ｔ−１までに利用可能なデータに基づき推定された時刻ｔ−１の状態ベクトルを表す。

なお、以下では、添え字ｔ｜ｔ−１が付与された値は、時刻ｔ−１までに利用可能なデータに基づき予測された時刻ｔの事前推定値を表す。また、添え字ｔ｜ｔ（又は、ｔ−１｜ｔ−１）が付与された値は、時刻ｔまでに利用可能なデータに基づき推定された時刻ｔの事後推定値を表す。

また、予測ステップでは、関数ｆ_ｔを偏微分して得られる行列＾Ｆ_ｔと行列＾Ｇ_ｔとを、以下の式（５）〜（６）に従って算出する。

ここで、「’」はベクトルの転置を表す。

そして、上記式（５）〜（６）に従って算出した行列＾Ｆ_ｔと行列＾Ｇ_ｔとに基づいて、以下の式（７）に従って、事前誤差共分散行列Ｐ_{ｔ｜ｔ−１}を算出する（分散の予測）。

なお、上記式（７）におけるＱ_ｔは、システム雑音ｕ_ｔ（誤差項）の分散を表す。

すなわち、予測ステップでは、時刻ｔの状態ベクトルＸ_{ｔ｜ｔ−１}を中心値とし、事前誤差共分散行列Ｐ_{ｔ｜ｔ−１}を分散とする確率分布を予測していることになる。

（２）フィルタリングステップ
次に、フィルタリングステップについての概要を説明する。図４に、フィルタリングステップの概念図を示す。図４に示すように、フィルタリングステップでは、上記式（１）で示した時刻ｔの状態ベクトルＸ_ｔの要素である車両の位置［ｘ_ｔ，ｙ_ｔ］と、上記式（２）で示した時刻ｔの車両の位置情報［ｘ^＊ _ｔ，ｙ^＊ _ｔ］（観測中心）とに基づいて、フィルタリングを行い、予測ステップで出力された時刻ｔの状態ベクトルＸ_{ｔ｜ｔ−１}を修正してＸ_ｔ｜ｔを算出すると共に、事前誤差共分散行列Ｐ_{ｔ｜ｔ−１}を修正して事後誤差共分散行列Ｐ_ｔ｜ｔを算出する。

具体的には、まず、フィルタリングステップでは、予測ステップで算出された事前誤差共分散行列Ｐ_{ｔ｜ｔ−１}と、観測行列Ｈ_ｔと、観測雑音ｗ_ｔ（誤差項）の分散Ｒ_ｔとに基づいて、以下の式（８）に従って、カルマンゲインＫ_ｔを算出する。

次に、算出されたカルマンゲインＫ_ｔと、予測ステップで算出された時刻ｔにおける状態ベクトルＸ_{ｔ｜ｔ−１}と、上記式（２）で示したＹ_ｔ＝［ｘ^＊ _ｔ，ｙ^＊ _ｔ］と、観測行列Ｈ_ｔとに基づいて、以下の式（９）に従って、時刻ｔにおける状態ベクトルＸ_ｔ｜ｔを推定する。

そして、予測ステップで算出された事前誤差共分散行列Ｐ_{ｔ｜ｔ−１}と、算出されたカルマンゲインＫ_ｔと、観測行列Ｈ_ｔとに基づいて、以下の式（１０）に従って、事後誤差共分散行列Ｐ_ｔ｜ｔを算出する。

フィルタリングステップで算出された各値（カルマンゲインＫ_ｔ、状態ベクトルＸ_ｔ｜ｔ、事後誤差共分散行列Ｐ_ｔ｜ｔ）は、次回の予測ステップにおける処理で用いられる。

予測部２４は、車両情報取得部２２によって取得された時刻ｔ−１の車両の外部環境情報と、運転操作車両状態推定部３２によって推定された時刻ｔ−１の車両の運動状態の確率分布及びドライバの運転操作量の確率分布に基づいて、時刻ｔのドライバの運転操作量の確率分布、車両の位置情報の確率分布、及び車両の運動状態の確率分布を予測する。本実施の形態では、ドライバの運転操作量として、ステアリング操作量に基づく車両の曲率δ_ｔと、アクセルペダルの操作量に基づく車両の加速度α_ｔとを予測する。また、車両の位置情報及び運動状態として、車両の位置［ｘ_ｔ，ｙ_ｔ］と、車両の向きθ_ｔと、車両の速度ｖ_ｔとを予測する。

なお、本実施の形態では、車両の位置情報は０．１秒毎に取得されるものとする。また、予測部２４は、車両情報取得部２２によって取得された０．１秒前（時刻ｔ−１）の車両の外部環境情報と、運転操作車両状態推定部３２によって推定された０．１秒前（時刻ｔ−１）の車両の運動状態及びドライバの運転操作量に基づいて、現時刻（時刻ｔ）のドライバの運転操作量、車両の位置情報、及び車両の運動状態を予測する。そして、予測部２４は、運転操作予測部２６におけるドライバの運転操作量の予測処理と、車両挙動予測部２８における車両の位置情報、及び車両の運動状態の予測処理とを３０回繰り返し、３秒後までの車両の位置情報及び運動状態を予測するものとする。

具体的には、予測部２４は、運転操作予測部２６及び車両挙動予測部２８を備えている。運転操作予測部２６は、車両情報取得部２２によって取得された時刻ｔ−１の車両の外部環境情報と、運転操作車両状態推定部３２によって推定された時刻ｔ−１の状態ベクトルＸ_ｔ−１（車両の位置情報、運動状態、及びドライバの運転操作量）とに基づいて、時刻ｔのドライバの運転操作量を予測する。運転操作予測部２６は、図５に示すように、運転操作受付部１００と、経路生成部１０２と、目標位置選択部１０４と、横位置制御部１０６と、目標速度制約部１０８と、目標速度選択部１１８と、速度制御部１２０と、運転操作出力部１２２とを備えている。

運転操作受付部１００は、運転操作車両状態推定部３２によって推定された時刻ｔ−１の状態ベクトルＸ_ｔ−１を受け付ける。

経路生成部１０２は、車両情報取得部２２によって取得された目的地情報、及び地図情報と、運転操作受付部１００によって受け付けた時刻ｔ−１の状態ベクトルＸ_ｔ−１の要素である［ｘ_ｔ−１，ｙ_ｔ−１］とに基づいて、最短経路を探索するアルゴリズムや、最短時間の経路を探索するアルゴリズムなど（例えば、Ａ＊アルゴリズム）を用いて、出発地から目的地までの最短経路を生成する。

目標位置選択部１０４は、経路生成部１０２によって生成された最短経路に基づいて、各時刻ｔについて、車両の走路上の目標位置を決定する。例えば、目標位置選択部１０４は、経路生成部１０２によって生成された車両の目的地から求められた経路情報と、運転操作受付部１００によって受け付けた時刻ｔ−１の状態ベクトルＸ_ｔ−１の要素である［ｘ_ｔ−１，ｙ_ｔ−１］とに基づいて、経路上における位置［ｘ_ｔ−１，ｙ_ｔ−１］から１００ｍ先の地点を目標位置として選択する。

横位置制御部１０６は、目標位置選択部１０４によって選択された目標位置［ｘ_ｒ，ｙ_ｒ］と、運転操作受付部１００によって受け付けた時刻ｔ−１の状態ベクトルＸ_ｔ−１の要素である［ｘ_ｔ−１，ｙ_ｔ−１，ｖ_ｔ−１，θ_ｔ−１］とに基づいて、ドライバのステアリング操作量を予測する。そして、横位置制御部１０６は、予測されたステアリング操作量に基づいて、車両の曲率δを算出する。ステアリング操作量と曲率δとの関係は予め求められている。例えば、横位置制御部１０６は、前方注視モデルを用いて、ステアリング操作量を予測し、予測されたステアリング操作量に基づいて曲率δを算出する。

目標速度制約部１０８は、車両情報取得部２２によって取得された時刻ｔ−１の車両の外部環境情報と、目標位置選択部１０４によって選択された目標位置［ｘ_ｒ，ｙ_ｒ］と、運転操作受付部１００によって受け付けた時刻ｔ−１の状態ベクトルＸ_ｔ−１とに基づいて、複数の速度制約条件の各々を満たす上限速度を算出する。複数の速度制約条件の各々は、運転操作における様々なシーンに対応する。目標速度制約部１０８は、前後位置制御制約部１１０と、定常走行制約部１１２と、追従制約部１１４と、カーブ制約部１１６とを備えている。

前後位置制御制約部１１０は、運転操作受付部１００によって受け付けた時刻ｔ−１の状態ベクトルＸ_ｔ−１の要素である［ｘ_ｔ−１，ｙ_ｔ−１］と、目標位置選択部１０４によって選択された走路上の目標位置［ｘ_ｒ，ｙ_ｒ］とに基づいて、前後位置制御に関する速度制約条件を満たす上限速度を算出する。例えば、前後位置制御制約部１１０は、比例制御モデルを用いて、時刻ｔ−１の車両の位置［ｘ_ｔ−１，ｙ_ｔ−１］と目標位置［ｘ_ｒ，ｙ_ｒ］との差に応じて、上限速度を算出する。

定常走行制約部１１２は、運転操作受付部１００によって受け付けた時刻ｔ−１の状態ベクトルＸ_ｔ−１の要素である［ｘ_ｔ−１，ｙ_ｔ−１］と、車両情報取得部２２によって取得された道路情報に含まれる走路速度情報に基づいて、当該走路の定常走行速度を、以下の式（１１）に示す定常走行に関する速度制約条件を満たす上限速度として算出する。具体的には、定常走行制約部１１２は、時刻ｔ−１の車両の位置［ｘ_ｔ−１，ｙ_ｔ−１］に基づいて、当該位置の走路速度情報から得られる、車両が現在走行中の走路の一般的な最高速度を表すパラメータｐ_ｒｏａｄ（定常走行速度）を取得し、上限速度とする。ｐ_ｒｏａｄは、当該走路での過去の走行データや制限速度等から算出される。

追従制約部１１４は、運転操作受付部１００によって受け付けた時刻ｔ−１の状態ベクトルＸ_ｔ−１の要素である［ｖ_ｔ−１］と、車両情報取得部２２によって取得された前方車情報（前方車との相対速度）とに基づいて、以下の式（１２）に示す前方車両追従に関する速度制約条件を満たす上限速度を算出する。具体的には、追従制約部１１４は、時刻ｔ−１の車両の速度［ｖ_ｔ−１］と前方車との相対速度に基づいて、等速予測により、Ｔ秒先の前方車との車間距離の予測値＾ｘ_ｆ（ｔ＋Ｔ）を算出する。そして、追従制約部１１４は、式（１２）の制約条件を満たす上限速度を算出する。

ここで、ｐ_ｆは予め求められたパラメータであり、Ｌは車両の長さである。

カーブ制約部１１６は、運転操作受付部１００によって受け付けた時刻ｔ−１の状態ベクトルＸ_ｔ−１の要素である［ｘ_ｔ−１，ｙ_ｔ−１］と、車両情報取得部２２によって取得された地図情報に含まれる走路曲率情報に基づいて、以下の式（１３）に示すカーブ走行に関する速度制約条件を満たす上限速度を算出する。具体的には、カーブ制約部１１６は、時刻ｔ−１の車両の位置［ｘ_ｔ−１，ｙ_ｔ−１］に基づいて、当該位置におけるＴ秒先の走路の曲率の絶対値の予測値｜＾δ（ｔ＋Ｔ）｜を算出し、算出された予測値｜＾δ（ｔ＋Ｔ）｜に基づいて、式（１３）を満たす上限速度を算出する。

なお、ｐ_ｃは予め求められたパラメータである。

目標速度選択部１１８は、目標速度制約部１０８によって算出された上限速度のうち、最も低い速度を目標速度ｖ_ｒとして選択する。

速度制御部１２０は、目標速度選択部１１８によって選択された目標速度ｖ_ｒと、運転操作受付部１００によって受け付けた時刻ｔ−１の状態ベクトルＸ_ｔ−１の要素である［ｖ_ｔ−１］とに基づいて、アクセルペダルの操作量を予測する。そして、速度制御部１２０は、予測されたアクセルペダルの操作量に基づいて、車両の加速度αを算出する。例えば、速度制御部１２０は、比例制御モデルを用いて、目標速度ｖ_ｒと推定された時刻ｔ−１の車両の速度ｖ_ｔ−１との差に応じて、アクセルペダルの操作量を予測し、加速度αを算出する。

運転操作出力部１２２は、運転操作受付部１００によって受け付けた時刻ｔ−１の状態ベクトルＸ_ｔ−１の要素である［α_ｔ−１，δ_ｔ−１］と、横位置制御部１０６によって算出された曲率δと、速度制御部１２０によって算出された加速度αとに基づいて、時刻ｔにおける曲率δ_ｔと加速度α_ｔとを予測する。例えば、運転操作出力部１２２は、時刻ｔ−１における曲率δ_ｔ−１と算出された曲率δとの差を抑制するように、時刻ｔにおける曲率δ_ｔを予測する。同様に、運転操作出力部１２２は、時刻ｔ−１における加速度α_ｔ−１と算出された加速度αとの差を抑制するように、時刻ｔにおける加速度α_ｔを予測する。また、運転操作出力部１２２は、加速度α_ｔと曲率δ_ｔとを出力する。

車両挙動予測部２８は、運転操作出力部１２２によって出力された曲率δ_ｔ及び加速度α_ｔと、運転操作車両状態推定部３２によって推定された時刻ｔ−１の状態ベクトルＸ_ｔ−１とに基づいて、時刻ｔの車両の位置情報及び運動状態を予測する。車両挙動予測部２８は、図６に示すように、車両挙動受付部２８０と、速度算出部２８２と、向き算出部２８４と、位置算出部２８６と、車両挙動出力部２８８とを備えている。

車両挙動受付部２８０は、運転操作車両状態推定部３２によって推定された時刻ｔ−１の状態ベクトルＸ_ｔ−１を受け付ける。

速度算出部２８２は、運転操作出力部１２２によって出力された車両の加速度α_ｔと、車両挙動受付部２８０によって受け付けた時刻ｔ−１の状態ベクトルＸ_ｔ−１の要素である［ｖ_ｔ−１］とに基づいて、車両の速度ｖ_ｔを予測する。例えば、速度算出部２８２は、速度算出式ｖ_ｔ＝ｖ_ｔ−１＋α_ｔＴに従って、車両の速度ｖ_ｔを予測する。

向き算出部２８４は、運転操作出力部１２２によって出力された曲率δ_ｔと、車両挙動受付部２８０によって受け付けた時刻ｔ−１の状態ベクトルＸ_ｔ−１の要素である［θ_ｔ−１］と、速度算出部２８２によって予測された車両の速度ｖ_ｔとに基づいて、車両の向きθ_ｔを予測する。

位置算出部２８６は、速度算出部２８２によって予測された車両の速度ｖ_ｔと、向き算出部２８４によって予測された車両の向きθ_ｔと、車両挙動受付部２８０によって受け付けた時刻ｔ−１の状態ベクトルＸ_ｔ−１の要素である［ｘ_ｔ−１，ｙ_ｔ−１］とに基づいて、車両の位置［ｘ_ｔ，ｙ_ｔ］を予測する。

車両挙動出力部２８８は、速度算出部２８２よって予測された車両の速度ｖ_ｔと、向き算出部２８４によって予測された車両の向きθ_ｔと、位置算出部２８６によって予測された車両の位置［ｘ_ｔ，ｙ_ｔ］とを、車両の位置情報及び運動状態として出力する。

また、車両挙動予測部２８は、運転操作車両状態推定部３２によって推定された時刻ｔ−１の車両の位置情報、運動状態、及びドライバの運転操作量からなる状態ベクトルＸ_ｔ−１に基づいて、上記式（５）〜（７）に従って、事前誤差共分散行列Ｐ_{ｔ｜ｔ−１}を算出する。

位置情報取得部３０は、位置計測部１８によって計測された車両の位置情報を逐次取得する。

運転操作車両状態推定部３２は、位置情報取得部３０によって取得された時刻ｔの車両の位置情報と、車両挙動出力部２８８によって出力された時刻ｔの車両の位置情報の確率分布とに基づいて、時刻ｔにおける車両の運動状態の確率分布及び車両のドライバの運転操作量の確率分布を推定する。
具体的には、運転操作車両状態推定部３２は、位置情報取得部３０によって取得された時刻ｔの車両の位置情報［ｘ^＊ _ｔ，ｙ^＊ _ｔ］と、車両挙動出力部２８８によって出力された時刻ｔの車両の位置情報［ｘ_ｔ，ｙ_ｔ］と、車両挙動予測部２８によって算出された事前誤差共分散行列Ｐ_{ｔ｜ｔ−１}とに基づいて、上記式（１１）〜（１２）に従って、車両の運動状態及び車両のドライバの運転操作量（状態ベクトルＸ_ｔ｜ｔ）を推定する。また、運転操作車両状態推定部３２は、車両挙動予測部２８によって算出された事前誤差共分散行列Ｐ_{ｔ｜ｔ−１}に基づいて、上記式（１３）に従って、事後誤差共分散行列Ｐ_ｔ｜ｔを算出する。

出力装置４０は、予測部２４によって出力された、時刻ｔの状態ベクトルＸ_ｔ＝［ｘ_ｔ ｙ_ｔ θ_ｔ ｖ_ｔ α_ｔ δ_ｔ］を出力する。

＜車両挙動予測装置１０の動作＞
次に、第１の実施の形態に係る車両挙動予測装置１０の動作について説明する。なお、以下の説明では、次の時刻（時刻ｔ）のドライバの運転操作量、車両の位置情報、及び車両の運動状態を予測した後、運転操作予測部２６におけるドライバの運転操作量の予測処理と、車両挙動予測部２８における車両の位置情報、及び車両の運動状態の予測処理とを更にＳ−１回（Ｓは例えば３０）繰り返し、３秒後までの車両の位置情報及び車両の運動状態を予測するものとする。また、３秒後までの繰り返し処理内で予測された状態ベクトルは、Ｘ_ｔ＋ｓ＝［ｘ_ｔ＋ｓ，ｙ_ｔ＋ｓ，θ_ｔ＋ｓ，ｖ_ｔ＋ｓ，α_ｔ＋ｓ，δ_ｔ＋ｓ］と表記する。

まず、予測対象車両のドライバの運転操作により車両が走行し、位置計測部１８によって車両の位置情報が逐次計測されているときに、入力操作部１２により目的地情報が入力されると、コンピュータ２０において、図７に示す車両挙動予測処理ルーチンが実行される。

ステップＳ１００において、車両情報取得部２２によって、入力操作部１２により入力された目的地情報を受け付ける。

ステップＳ１０２において、位置情報取得部３０によって、位置計測部１８によって計測された車両の位置情報を受け付ける。

ステップＳ１０４において、上記ステップＳ１０２で受け付けた車両の位置情報に基づいて、状態ベクトルＸ_ｔ、事後誤差共分散行列Ｐ_ｔ｜ｔ、システム雑音ｕ_ｔ（誤差項）の分散Ｑ_ｔ、及び観測雑音ｗ_ｔ（誤差項）の分散Ｒ_ｔの初期状態（ｔ＝０）を設定すると共に、ｔに０を代入する。そして、ステップＳ１０５において、時刻ｔを１だけインクリメントする。

ステップＳ１０６において、運転操作受付部１００によって、上記ステップＳ１０４で設定され、又は前回のステップＳ１１４で推定された時刻ｔ−１の状態ベクトルＸ_ｔ−１を受け付ける。

ステップＳ１０８において、予測部２４によって、上記ステップＳ１００で取得された目的地情報と、上記ステップＳ１０６で受け付けた時刻ｔ−１の車両の位置情報、車両の運動状態及び車両のドライバの運転操作量（状態ベクトルＸ_ｔ−１）とに基づいて、時刻ｔのドライバの運転操作量、車両の位置情報及び車両の運動状態（状態ベクトルＸ_ｔ）を予測する。ステップ１０８は、図８に示す予測処理ルーチンによって実現される。

＜予測処理ルーチン＞
まず、ステップＳ２００において、繰り返し回数ｓに０を代入する。

次に、ステップＳ２０２において、運転操作予測部２６によって、上記ステップＳ１０６で受け付けた時刻ｔ−１の状態ベクトルＸ_ｔ−１、又は前回のステップＳ２０２及び前回のステップＳ２０４で予測されたドライバの運転操作量、車両の位置情報、及び車両の運動状態Ｘ_{ｔ＋ｓ−１}に基づいて、ドライバの運転操作を予測する。ステップ２０２は、図９に示す運転操作予測処理ルーチンによって実現される。

＜運転操作予測処理ルーチン＞
運転操作予測処理ルーチンでは、上記予測処理ルーチンにおける繰り返し回数ｓが０である場合には、上記ステップＳ１０４で受け付けた時刻ｔ−１の状態ベクトルＸ_ｔ−１に基づいて以下のステップＳ３０２〜ステップＳ３１６の各処理を行う。一方、上記予測処理ルーチンにおける繰り返し回数ｓが０でない場合には、予測処理ルーチンにおける前回のステップＳ２０２及び前回のステップＳ２０４で予測されたドライバの運転操作量、車両の位置情報、及び車両の運動状態Ｘ_{ｔ＋ｓ−１}に基づいて以下のステップＳ３０２〜ステップＳ３１６の各処理を行う。

まず、ステップＳ３００において、車両情報取得部２２によって、時刻ｔ−１の車両の外部環境情報を取得する。

ステップＳ３０２において、経路生成部１０２によって、上記ステップＳ１００で受け付けた目的地情報と、上記ステップＳ３００で受け付けた外部環境情報と、上記ステップＳ１０４で受け付けた時刻ｔ−１の状態ベクトルＸ_ｔ−１の要素である［ｘ_ｔ−１，ｙ_ｔ−１］、又は前回のステップＳ２０４で車両挙動予測部２８によって予測された［ｘ_{ｔ＋ｓ−１}，ｙ_{ｔ＋ｓ−１}］とに基づいて、出発地から目的地までの最短経路を生成する。

ステップＳ３０４において、目標位置選択部１０４によって、上記ステップＳ３０２で生成された経路情報と、上記ステップＳ１０４で受け付けた時刻ｔ−１の状態ベクトルＸ_ｔ−１の要素である［ｘ_ｔ−１，ｙ_ｔ−１］、又は前回のステップＳ２０４で車両挙動予測部２８によって予測された［ｘ_{ｔ＋ｓ−１}，ｙ_{ｔ＋ｓ−１}］とに基づいて、経路上における目標位置［ｘ_ｒ，ｙ_ｒ］を選択する。

ステップＳ３０６において、横位置制御部１０６によって、上記ステップＳ３０４で選択された目標位置［ｘ_ｒ，ｙ_ｒ］と、上記ステップＳ１０４で受け付けた時刻ｔ−１の状態ベクトルＸ_ｔ−１の要素である［ｘ_ｔ−１，ｙ_ｔ−１，ｖ_ｔ−１，θ_ｔ−１］、又は前回のステップＳ２０４で車両挙動予測部２８によって予測された［ｘ_{ｔ＋ｓ−１}，ｙ_{ｔ＋ｓ−１}，ｖ_{ｔ＋ｓ−１}，θ_{ｔ＋ｓ−１}］とに基づいて、ドライバのステアリング操作量を予測する。そして、予測されたステアリング操作量に基づいて、車両の曲率δを算出する。

ステップＳ３０８において、複数の速度制約条件の各々を満たす上限速度を算出する。
本ステップでは、まず、前後位置制御制約部１１０によって、上記ステップＳ１０４で受け付けた時刻ｔ−１の状態ベクトルＸ_ｔ−１の要素である［ｘ_ｔ−１，ｙ_ｔ−１］、又は前回のステップＳ２０４で車両挙動予測部２８によって予測された［ｘ_{ｔ＋ｓ−１}，ｙ_{ｔ＋ｓ−１}］と、上記ステップＳ３０４で選択された目標位置［ｘ_ｒ，ｙ_ｒ］とに基づいて、上限速度を算出する。
次に、定常走行制約部１１２によって、上記ステップＳ１０４で受け付けた時刻ｔ−１の状態ベクトルＸ_ｔ−１の要素である［ｘ_ｔ−１，ｙ_ｔ−１］、又は前回のステップＳ２０４で車両挙動予測部２８によって予測された［ｘ_{ｔ＋ｓ−１}，ｙ_{ｔ＋ｓ−１}］と、上記ステップＳ３００で受け付けた外部環境情報に含まれる走路速度情報に基づいて、当該走路の定常走行速度を、上記式（８）に示す速度制約条件の上限速度として算出する
また、追従制約部１１４によって、上記ステップＳ１０４で受け付けた時刻ｔ−１の状態ベクトルＸ_ｔ−１の要素である［ｖ_ｔ−１］、又は前回のステップＳ２０４で車両挙動予測部２８によって予測された［ｖ_{ｔ＋ｓ−１}］と、上記ステップＳ３００で受け付けた外部環境情報に含まれる前方車情報（前方車との相対速度）とに基づいて、上記式（９）に示す速度制約条件の上限速度を算出する。
そして、カーブ制約部１１６によって、上記ステップＳ１０４で受け付けた時刻ｔ−１の状態ベクトルＸ_ｔ−１の要素である［ｘ_ｔ−１，ｙ_ｔ−１］、又は前回のステップＳ２０４で車両挙動予測部２８によって予測された［ｘ_{ｔ＋ｓ−１}，ｙ_{ｔ＋ｓ−１}］と、上記ステップＳ３００で受け付けた外部環境情報の地図情報に含まれる走路曲率情報に基づいて、上記式（１０）に示す速度制約条件の上限速度を算出する。

ステップＳ３１０において、目標速度選択部１１８によって、上記ステップＳ３０８で算出された上限速度のうち、最も低い速度を目標速度ｖ_ｒとして選択する。

ステップＳ３１２において、速度制御部１２０によって、上記ステップＳ３１０で選択された目標速度ｖ_ｒと、上記ステップＳ１０４で受け付けた時刻ｔ−１の状態ベクトルＸ_ｔ−１の要素である［ｖ_ｔ−１］、又は前回のステップＳ２０４で車両挙動予測部２８によって予測された［ｖ_{ｔ＋ｓ−１}］とに基づいて、アクセルペダルの操作量を予測する。そして、予測されたアクセルペダルの操作量に基づいて、車両の加速度αを算出する。

ステップＳ３１４において、運転操作出力部１２２によって、上記ステップＳ１０４で受け付けた時刻ｔ−１の状態ベクトルＸ_ｔ−１の要素である［α_ｔ−１，δ_ｔ−１］、又は前回のステップＳ２０２で運転操作予測部２６によって予測された［α_{ｔ＋ｓ−１}，δ_{ｔ＋ｓ−１}］と、上記ステップＳ３０６で算出された曲率δと、上記ステップＳ３１２で算出された加速度αとに基づいて、時刻ｔ＋ｓにおける曲率δ_ｔ＋ｓと加速度α_ｔ＋ｓと（ｓ＝０の場合は、曲率δ_ｔ、及び加速度α_ｔ）を予測する。

ステップＳ３１６において、上記ステップＳ３１４で予測された時刻ｔ＋ｓにおける曲率δ_ｔ＋ｓと加速度α_ｔ＋ｓと（ｓ＝０の場合は、曲率δ_ｔ、及び加速度α_ｔ）を、運転操作の中心値として出力して運転操作予測処理ルーチンを終了する。

次に、予測処理ルーチンに戻り、ステップＳ２０４において、車両挙動予測部２８によって、上記ステップＳ１０６で受け付けた時刻ｔ−１の状態ベクトルＸ_ｔ−１、又は前回のステップＳ２０２及び前回のステップＳ２０４で予測されたドライバの運転操作量、車両の位置情報、及び車両の運動状態Ｘ_{ｔ＋ｓ−１}に基づいて、時刻ｔ（又は時刻ｔ＋ｓ）の車両の位置情報及び車両の運動状態を予測する。ステップ２０４は、図１０に示す車両予測処理ルーチンによって実現される。

＜車両予測処理ルーチン＞
車両予測処理ルーチンでは、上記予測処理ルーチンにおける繰り返し回数ｓが０である場合には、上記ステップＳ１０４で受け付けた時刻ｔ−１の状態ベクトルＸ_ｔ−１に基づいて以下のステップＳ４００〜ステップＳ４０４の各処理を行い、上記予測処理ルーチンにおける繰り返し回数ｓが０でない場合には、予測処理ルーチンにおける前回のステップＳ２０２及び前回のステップＳ２０４で予測されたドライバの運転操作量、車両の位置情報、及び車両の運動状態Ｘ_{ｔ＋ｓ−１}に基づいて以下のステップＳ４００〜ステップＳ４０４の各処理を行う。

まず、ステップＳ４００において、速度算出部２８２によって、上記ステップＳ２０２で出力された車両の加速度α_ｔと、上記ステップＳ１０６で受け付けた時刻ｔ−１の状態ベクトルＸ_ｔ−１の要素である［ｖ_ｔ−１］、又は前回のステップＳ２０４で車両挙動予測部２８によって予測された［ｖ_{ｔ＋ｓ−１}］とに基づいて、車両の速度ｖ_ｔ＋ｓ（ｓ＝０の場合は、ｖ_ｔ）を予測する。

次に、ステップＳ４０２において、向き算出部２８４によって、上記ステップＳ２０２で出力された曲率δ_ｔと、上記ステップＳ１０６で受け付けた時刻ｔ−１の状態ベクトルＸ_ｔ−１の要素である［θ_ｔ−１］、又は前回のステップＳ２０４で車両挙動予測部２８によって予測された［θ_{ｔ＋ｓ−１}］と、上記ステップＳ４００で予測された車両の速度ｖ_ｔとに基づいて、車両の向きθ_ｔ＋ｓを予測する。

そして、ステップＳ４０４において、位置算出部２８６によって、上記ステップＳ４００で予測された車両の速度ｖ_ｔ＋ｓと、上記ステップＳ４０２で予測された車両の向きθ_ｔ＋ｓと、上記ステップＳ１０６で受け付けた時刻ｔ−１の状態ベクトルＸ_ｔ−１の要素である［ｘ_ｔ−１，ｙ_ｔ−１］、又は前回のステップＳ２０４で車両挙動予測部２８によって予測された［ｘ_{ｔ＋ｓ−１}，ｙ_{ｔ＋ｓ−１}］とに基づいて、車両の位置［ｘ_ｔ＋ｓ，ｙ_ｔ＋ｓ］を予測する。

ステップＳ４０６において、上記ステップＳ４００で予測された時刻ｔにおける車両の車速ｖ_ｔ＋ｓと、上記ステップＳ４００で予測された時刻ｔにおける車両の向きθ_ｔ＋ｓと、上記ステップＳ４０４で予測された時刻ｔにおける車両の位置［ｘ_ｔ＋ｓ、ｙ_ｔ＋ｓ］とを、車両挙動の中心値として出力して車両予測処理ルーチンを終了する。

次に、予測処理ルーチンに戻り、ステップＳ２０６において、繰り返し回数ｓがＳ−１以上であるか否かを判定する。繰り返し回数ｓがＳ−１以上である場合には、ステップＳ２０８へ進む。一方、繰り返し回数ｓがＳ−１未満の場合には、ステップＳ２１０でｓを１インクリメントし、ステップＳ２０２へ戻る。

ステップＳ２０８において、車両挙動予測部２８によって、運転操作車両状態推定部３２によって推定された時刻ｔ−１の車両の位置情報、運動状態、及びドライバの運転操作量からなる状態ベクトルＸ_ｔ−１に基づいて、上記式（５）〜（７）に従って、事前誤差共分散行列Ｐ_{ｔ｜ｔ−１}を算出する。

ステップＳ２１０において、上記ステップＳ２０２で出力された時刻ｔ〜ｔ＋Ｓ−１における曲率δ_ｔ＋ｓ及び加速度α_ｔ＋ｓ（ｓ＝０，・・・，Ｓ−１）と、上記ステップＳ２０４で出力された時刻ｔ〜ｔ＋Ｓ−１における車両の位置［ｘ_ｔ＋ｓ，ｙ_ｔ＋ｓ］、車両の向きθ_ｔ＋ｓ、及び車両の速度ｖ_ｔ＋ｓ（ｓ＝０，・・・，Ｓ−１）と、上記ステップＳ２０８で算出された事前誤差共分散行列Ｐ_{ｔ｜ｔ−１}を結果として出力して予測処理ルーチンを終了する。

次に、車両挙動予測処理ルーチンに戻り、ステップＳ１１０において、出力装置４０によって、上記ステップＳ１０８で出力された結果を、時刻ｔから３秒後までの予測結果として出力する。

ステップＳ１１２において、位置情報取得部３０によって、時刻ｔの車両の位置情報を受け付ける。

ステップＳ１１４において、運転操作車両状態推定部３２によって、上記ステップＳ１１２で受け付けた時刻ｔの車両の位置情報［ｘ^＊ _ｔ，ｙ^＊ _ｔ］と、上記ステップＳ１１０で出力された時刻ｔの車両の位置情報［ｘ_ｔ，ｙ_ｔ］とに基づいて、車両の運動状態及び車両のドライバの運転操作量を推定する。ステップ１１４は、図１１に示す運転状態車両状態推定処理ルーチンによって実現される。

ステップＳ５００において、上記ステップＳ１１０で出力された、時刻ｔにおける、曲率δ_ｔ、加速度α_ｔ、車両の速度ｖ_ｔ、車両の向きθ_ｔ、及び車両の位置［ｘ_ｔ，ｙ_ｔ］を受け付け、時刻ｔにおける状態ベクトルＸ_ｔ＝［ｘ_ｔ ｙ_ｔ θ_ｔ ｖ_ｔ α_ｔ δ_ｔ］として構成する。

ステップＳ５０２において、運転操作車両状態推定部３２によって、上記ステップＳ１１０で出力された事前誤差共分散行列Ｐ_{ｔ｜ｔ−１}と、観測行列Ｈ_ｔと、観測雑音ｗ_ｔ（誤差項）の分散Ｒ_ｔとに基づいて、上記式（１１）に従って、カルマンゲインＫ_ｔを算出する。

ステップＳ５０４において、運転操作車両状態推定部３２によって、上記ステップＳ５０２で算出されたカルマンゲインＫ_ｔと、上記ステップＳ５００で構成した時刻ｔにおける状態ベクトルＸ_ｔと、上記ステップＳ１１２で受け付けた車両の位置Ｙ_ｔ＝［ｘ^＊ _ｔ，ｙ^＊ _ｔ］と、観測行列Ｈ_ｔとに基づいて、上記式（１２）に従って、時刻ｔにおける状態ベクトル＾Ｘ_ｔ＝［＾ｘ_ｔ，＾ｙ_ｔ，＾θ_ｔ，＾ｖ_ｔ，＾α_ｔ，＾δ_ｔ］を推定する。

ステップＳ５０６において、運転操作車両状態推定部３２によって、上記ステップＳ２０６で算出された事前誤差共分散行列Ｐ_{ｔ｜ｔ−１}と、上記ステップＳ５０２で算出されたカルマンゲインＫ_ｔと、観測行列Ｈ_ｔとに基づいて、事後誤差共分散行列Ｐ_ｔ｜ｔを算出する。

ステップＳ５０８において、上記ステップＳ５０４で推定された時刻ｔにおける状態ベクトル＾Ｘ_ｔ＝［＾ｘ_ｔ，＾ｙ_ｔ，＾θ_ｔ，＾ｖ_ｔ，＾α_ｔ，＾δ_ｔ］と、上記ステップＳ５０６で算出された事後誤差共分散行列Ｐ_ｔ｜ｔとを結果として出力して、運転操作車両状態推定処理ルーチンを終了する。

次に、車両挙動予測処理ルーチンに戻り、ステップＳ１０５へ戻って、時刻ｔを１インクリメントする。

以上説明したように、第１の実施の形態に係る車両挙動予測装置によれば、取得された時刻ｔの車両の位置情報と、予測された時刻ｔの車両の位置情報とに基づいて、時刻ｔにおける車両の運動状態及び車両のドライバの運転操作量を推定し、時刻ｔの車両の外部環境情報と、推定された時刻ｔの車両の運動状態及び車両のドライバの運転操作量とに基づいて、時刻ｔ＋１の車両のドライバの運転操作量を予測し、予測された時刻ｔ＋１のドライバの運転操作量と、推定された時刻ｔの車両の運動状態とに基づいて、時刻ｔ＋１の車両の位置情報及び運動状態を予測することにより、車両の外部環境情報と、車両の位置情報とに基づいて、車両の位置情報及び運動状態を精度よく予測することができる。

また、外部環境情報と位置情報のみから、車両の位置情報及び運動状態の予測が可能となるため、従来に比べ、高い精度の予測性能が得られる。

また、位置情報と外部環境情報とからドライバの運転操作量を予測することができるため、装置が搭載されていない車両（例えば、本実施の形態に係る車両挙動予測装置が搭載されている車両の前方車両など）の挙動を予測することができる。

また、外部環境情報と位置情報のみから、車両の位置情報及び運動状態の予測が可能となるため、従来に比べ、高速で予測処理を行うことができる。

また、外部環境情報と位置情報のみから、車両の位置情報及び運動状態の予測が可能となるため、車両の速度、加速度といったセンサ情報を用いずに、車両の位置情報及び運動状態の予測をすることができる。

また、予め定められた複数の速度制約条件の各々を満たす上限速度を算出し、算出された上限速度のうちの何れかを目標速度として選択し、選択された目標速度と、時刻ｔの車両の外部環境情報と、地図情報と、推定された時刻ｔの車両の運動状態とに基づいて、時刻ｔ＋１の車両のドライバの運転操作量を予測することにより、車両の位置情報及び運動状態を精度よく予測することができる。

また、複数の運転シーンを想定しているため、一般道運転の予測精度が高い。

また、従来技術と比較し、高い速度予測精度を持ち、外部環境情報とＧＰＳ等の位置センサのみで挙動の予測が可能であり、位置センサの観測誤差に対してロバスト性が高い。

また、一般道の運転の大半を占める、定常走行、前方車両追従、信号停止、及びカーブに対するドライバモデルを速度制約条件として導入し、それらの速度制約条件を満たす目標速度に対する速度制御を行うモデルで運転操作予測し、車両挙動を予測することで、観測した位置情報から拡張カルマンフィルタにより運転操作・車両状態の確率分布の推定が可能となり、外部環境情報とＧＰＳ等の位置センサのみで挙動の予測が可能になると共に、精度・ロバスト性ともに向上する。

＜第２の実施の形態＞
次に、第２の実施の形態について説明する。なお、第１の実施の形態と同様の構成となる部分については、同一符号を付して説明を省略する。

第２の実施の形態では、信号情報を更に考慮してドライバの運転操作量を予測し、車両の位置情報及び運動状態を予測する点が、第１の実施の形態と異なっている。

図１２に示すように、第２の実施の形態に係る車両挙動予測装置２１０は、ドライバが目的地を入力するための入力操作部１２と、地図情報（道路ネットワークデータ）、道路情報、及び信号情報を記憶した道路ネットワークデータベース２１４と、予測対象車両の前方を走行する前方車を検出する前方車情報取得部１６と、予測対象車両の位置を計測する位置計測部１８と、車両の挙動を予測するコンピュータ２０と、予測結果を出力する出力装置４０とを備えている。

道路ネットワークデータベース２１４には、地図情報、道路情報、及び信号情報が記憶されている。ここで、信号情報には、信号機の位置と当該信号機の信号変化の周期を示すデータが含まれている。

車両情報取得部２２２は、道路ネットワークデータベース２１４から、車両の外部環境情報として更に、車両の走路上の信号情報を取得すると共に、取得した信号情報に基づいて、各時刻ｔの信号の状態を取得する。

運転操作予測部２２６は、図１３に示すように、運転操作受付部１００と、経路生成部１０２と、目標位置制約部２１０１と、目標位置選択部２１０４と、横位置制御部１０６と、目標速度制約部１０８と、目標速度選択部１１８と、速度制御部１２０と、運転操作出力部１２２とを備えている。

目標位置制約部２１０１は、車両情報取得部２２によって取得された時刻ｔ−１の車両の外部環境情報と、運転操作受付部１００によって受け付けた時刻ｔ−１の状態ベクトルＸ_ｔ−１とに基づいて、複数の位置制約条件を満たす車両位置を算出する。目標位置制約部２１０１は、信号制約部２１０２と、経路制約部２１０３とを備えている。

信号制約部２１０２は、車両情報取得部２２によって取得された時刻ｔ−１の車両の外部環境情報に含まれる信号情報と、運転操作受付部１００によって受け付けた時刻ｔ−１の状態ベクトルＸ_ｔ−１の要素である［ｘ_ｔ−１，ｙ_ｔ−１，ｖ_ｔ−１］とに基づいて、信号停止に関する予め定められた位置制約条件を満たす車両位置を算出する。具体的には、信号制約部２１０２は、以下の式（１４）に示す位置制約条件を満たす車両位置［ｘ_ｒ，ｙ_ｒ］を算出する。

ここで、［ｘ_ｒ，ｙ_ｒ］^Ｔは車両位置、［ｘ_ｔ−１，ｙ_ｔ−１］^Ｔは運転操作受付部１００によって受け付けた時刻ｔ−１の位置、［ｘ_{ｓｔｏｐｌｉｎｅ}，ｙ_{ｓｔｏｐｌｉｎｅ}］^Ｔは信号に対応した停止線の位置、Ｓ_ｔ−１は前回の信号の状態（赤、黄、青）、Ｓ_ｒｅｄは赤信号、ｖ_ｔ−１は、運転操作受付部１００によって受け付けた時刻ｔ−１の車両の速度、ｐ_{ｓｉｇｎａｌ}は、停止するか否かを決定するための予め求められたパラメータである。

経路制約部２１０３は、経路生成部１０２によって車両の目的地に基づいて求められた経路情報と、運転操作受付部１００によって受け付けた時刻ｔ−１の状態ベクトルＸ_ｔ−１の要素である［ｘ_ｔ−１，ｙ_ｔ−１］とに基づいて、経路に関する位置制約条件を満たす車両位置を算出する。例えば、経路制約部２１０３は、目的地までの経路上における、時刻ｔ−１の位置［ｘ_ｔ−１，ｙ_ｔ−１］から１００先の地点を車両位置として算出する。

目標位置選択部２１０４は、信号制約部２１０２によって算出された車両位置、及び経路制約部２１０３によって算出された車両位置の何れかを選択し、選択された位置を目標位置として決定する。例えば、目標位置選択部２１０４は、時刻ｔ−１の位置［ｘ_ｔ−１，ｙ_ｔ−１］から最も近い車両位置を目標位置として選択する。

なお、第２の実施の形態に係る車両挙動予測装置の他の構成及び作用については、第１の実施の形態と同様であるため、説明を省略する。

以上説明したように、第２の実施の形態に係る車両挙動予測装置によれば、信号情報に基づいて、信号停止に関する予め定められた位置制約条件を満たす車両位置を算出し、車両の目的地に基づいて求められた経路情報に基づいて、経路に関する位置制約条件を満たす車両位置を算出し、算出された車両位置のうちの何れかを選択し、選択した位置を目標位置として決定することにより、車両の位置情報及び運動状態を精度良く予測することができる。

なお、上記の実施の形態では、車両挙動予測装置が搭載されている自車両を、予測対象車両とした場合を例に説明したが、これに限定されるものではなく、車両挙動予測装置が搭載されていない、自車両以外の車両を、予測対象車両としてもよい。
車両挙動予測装置が搭載されていない車両を予測対象車両とする場合には、当該予測対象車両の車載器は入力操作部１２、前方車情報取得部１６、及び位置計測部１８を備え、車両挙動予測装置は、通信手段によって当該予測対象車両の外部環境情報と位置情報とを取得し、当該車両の位置情報及び運動状態を予測する。

また、車両挙動予測装置が車両に搭載されている場合を例に説明したが、これに限定されるものではなく、交通センターなどに設定されているサーバによって、車両挙動予測装置が実現されてもよい。

また、上記の実施の形態では、車両の外部環境情報として、地図情報、道路情報（走路速度情報及び走路曲率情報）、前方車両情報、信号情報を用いる場合を例に説明したが、これに限定されるものではなく、より多様なシーンに対応するために、対向車両や、周囲車両、歩行者等の情報を用いてもよい。

また、上記の実施の形態における前方車情報取得部１６は、レーザレーダ装置を用いる場合を例に説明したが、これに限定されるものではない。例えば、前方車情報取得部１６として、車車間通信装置や、路車間通信装置等を用いて、前方車両情報を取得してもよい。

また、上記の実施の形態では、上述の車両挙動予測装置は、道路ネットワークデータベース１４を備えている場合を例に説明したが、これに限定されるものではなく、例えば、道路ネットワークデータベース１４が車両挙動予測装置の外部装置に設けられ車両挙動予測装置は、外部装置と通信手段を用いて通信することにより、道路ネットワークデータベース１４を参照するようにしてもよい。

また、上記の実施の形態における予測処理ルーチンでは、運転操作予測処理と車両挙動予測処理とを３０回繰り返す場合を例に説明したが、これに限定されるものではなく、運転操作予測処理と車両挙動予測処理とを繰り返さなくてもよい。

また、上記の実施の形態では、目標速度制約部１０８において、複数の速度制約条件の各々を満たす上限速度を算出し、目標速度選択部１１８において、上限速度のうち最も低いものを目標速度として選択する場合を例に説明したが、これに限定されるものではなく、複数の速度制約条件の各々の組み合わせに基づいて、目標速度を選択するようにしてもよい。

また、上記の実施の形態における運転操作出力部１２２は、時刻ｔ−１の状態ベクトルＸ_ｔ−１の要素である［α_ｔ−１，δ_ｔ−１］と、横位置制御部１０６によって算出された曲率δと、速度制御部１２０によって算出された加速度αとに基づいて、時刻ｔにおける曲率δ_ｔと加速度α_ｔとを予測する場合を例に説明したが、これに限定されるものではなく、、時刻ｔ−１の状態ベクトルＸ_ｔ−１の要素である［α_ｔ−１，δ_ｔ−１］を用いずに、時刻ｔにおける曲率δ_ｔと加速度α_ｔとを予測してもよい。

また、上記の実施の形態における運転操作車両状態推定部３２は、時刻ｔにおける車両の運動状態及び車両のドライバの運転操作量を推定する場合を例に説明したが、これに限定されるものではなく、時刻ｔにおける車両の運動状態のみを推定してもよい。

なお、本発明のプログラムは、記録媒体に格納して提供することができる。

１０，２１０ 車両挙動予測装置
１２ 入力操作部
１４，２１４ 道路ネットワークデータベース
１６ 前方車情報取得部
１８ 位置計測部
２０ コンピュータ
２２，２２２ 車両情報取得部
２４ 予測部
２６，２２６ 運転操作予測部
２８ 車両挙動予測部
３０ 位置情報取得部
３２ 運転操作車両状態推定部
４０ 出力装置
１００ 運転操作受付部
１０２ 経路生成部
１０４，２１０４ 目標位置選択部
１０６ 横位置制御部
１０８ 目標速度制約部
１１０ 前後位置制御制約部
１１２ 定常走行制約部
１１４ 追従制約部
１１６ カーブ制約部
１１８ 目標速度選択部
１２０ 速度制御部
１２２ 運転操作出力部
２８０ 車両挙動受付部
２８２ 速度算出部
２８４ 向き算出部
２８６ 位置算出部
２８８ 車両挙動出力部
２１０１ 目標位置制約部
２１０２ 信号制約部
２１０３ 経路制約部

Claims (7)

1. 車両の位置情報及び前記車両の外部環境情報を各時刻ｔについて取得する取得手段と、 前記取得手段によって取得された時刻ｔ−１の前記車両の外部環境情報と、時刻ｔ−１の前記車両の運動状態とに基づいて、時刻ｔの前記車両のドライバの運転操作量を予測する運転操作予測手段と、
   前記運転操作予測手段によって予測された時刻ｔの前記ドライバの運転操作量と、時刻ｔ−１の前記車両の運動状態とに基づいて、時刻ｔの前記車両の位置情報及び前記車両の運動状態を予測する車両挙動予測手段と、
   前記取得手段によって取得された時刻ｔの車両の位置情報と、前記車両挙動予測手段によって予測された時刻ｔの前記車両の位置情報とに基づいて、時刻ｔにおける前記車両の運動状態を推定する運転操作車両状態推定手段と、
   を含み、
   前記運転操作予測手段は、前記取得手段によって取得された時刻ｔの前記車両の外部環境情報と、前記運転操作車両状態推定手段によって推定された時刻ｔの前記車両の運動状態に基づいて、時刻ｔ＋１の前記車両のドライバの運転操作量を予測し、
   前記車両挙動予測手段は、前記運転操作予測手段によって予測された時刻ｔ＋１の前記ドライバの運転操作量と、前記運転操作車両状態推定手段によって推定された時刻ｔの前記車両の運動状態とに基づいて、時刻ｔ＋１の前記車両の位置情報及び前記車両の運動状態を予測する
   車両挙動予測装置。
2. 前記運転操作予測手段は、前記取得手段によって取得された時刻ｔの前記車両の外部環境情報と、前記運転操作車両状態推定手段によって推定された時刻ｔの前記車両の運動状態の確率分布に基づいて、時刻ｔ＋１の前記車両のドライバの運転操作量の確率分布を予測し、
   前記車両挙動予測手段は、前記運転操作予測手段によって予測された時刻ｔ＋１の前記ドライバの運転操作量の確率分布と、前記運転操作車両状態推定手段によって推定された時刻ｔの前記車両の運動状態の確率分布とに基づいて、時刻ｔ＋１の前記車両の位置情報の確率分布及び前記車両の運動状態の確率分布を予測し、
   前記運転操作車両状態推定手段は、前記取得手段によって取得された時刻ｔ＋１の車両の位置情報と、前記車両挙動予測手段によって予測された時刻ｔ＋１の前記車両の位置情報の確率分布とに基づいて、時刻ｔ＋１における前記車両の運動状態の確率分布を推定する請求項１に記載の車両挙動予測装置。
3. 前記外部環境情報に基づいて、予め定められた複数の速度制約条件の各々を満たす上限速度を算出し、前記算出された上限速度のうちの何れかを目標速度として選択する目標速度選択手段を更に含み、
   前記運転操作予測手段は、前記目標速度選択手段によって選択された前記目標速度と、前記取得手段によって取得された時刻ｔの前記車両の外部環境情報と、前記運転操作車両状態推定手段によって推定された時刻ｔの前記車両の運動状態に基づいて、前記時刻ｔ＋１の前記車両のドライバの運転操作量を予測する
   請求項１又は請求項２記載の車両挙動予測装置。
4. 各時刻ｔについて、前記車両の走路上の目標位置を決定する目標位置決定手段を更に含み、
   前記取得手段は、前記車両の外部環境情報として、前記車両が走行する走路における定常走行速度と、前記車両が走行する走路の曲率と、前記車両の前方を走行する車両との距離を示す前方車両情報とを、各時刻ｔについて取得し、
   前記目標速度選択手段は、前記定常走行速度を、定常走行に関する速度制約条件を満たす上限速度として算出し、前記走路の曲率に基づいて、カーブ走行に関する速度制約条件を満たす上限速度を算出し、前記前方車両情報に基づいて、前方車両追従に関する速度制約条件を満たす上限速度を算出し、前記目標位置決定手段によって決定された走路上の目標位置に基づいて、前後位置制御に関する速度制約条件を満たす上限速度を算出し、前記算出された上限速度のうちの何れかを目標速度として選択する請求項３記載の車両挙動予測装置。
5. 前記取得手段は、前記車両の外部環境情報として、更に、前記車両の走路上の信号情報と、前記車両の目的地とを、各時刻ｔについて取得し、
   前記目標位置決定手段は、前記信号情報に基づいて、信号停止に関する予め定められた位置制約条件を満たす車両位置を算出し、前記車両の目的地に基づいて求められた経路情報に基づいて、経路に関する位置制約条件を満たす車両位置を算出し、前記算出された車両位置のうちの何れかを選択し、選択した位置を目標位置として決定する請求項４記載の車両挙動予測装置。
6. 前記運転操作予測手段は、前記取得手段によって取得された時刻ｔの前記車両の外部環境情報と、前記運転操作車両状態推定手段によって推定された時刻ｔの前記車両の運動状態及び前記車両のドライバの運転操作量に基づいて、時刻ｔ＋１の前記車両のドライバの運転操作量を予測し、
   前記運転操作車両状態推定手段は、前記取得手段によって取得された時刻ｔ＋１の車両の位置情報と、前記車両挙動予測手段によって予測された時刻ｔ＋１の前記車両の位置情報とに基づいて、時刻ｔ＋１における前記車両の運動状態及び前記車両のドライバの運転操作量を推定する
   請求項１〜請求項５の何れか１項記載の車両挙動予測装置。
7. コンピュータを、
   車両の位置情報及び前記車両の外部環境情報を各時刻ｔについて取得する取得手段と、 前記取得手段によって取得された時刻ｔ−１の前記車両の外部環境情報と、時刻ｔ−１の前記車両の運動状態とに基づいて、時刻ｔの前記車両のドライバの運転操作量を予測する運転操作予測手段、
   前記運転操作予測手段によって予測された時刻ｔの前記ドライバの運転操作量と、時刻ｔ−１の前記車両の運動状態とに基づいて、時刻ｔの前記車両の位置情報及び前記車両の運動状態を予測する車両挙動予測手段、及び
   前記取得手段によって取得された時刻ｔの車両の位置情報と、前記車両挙動予測手段によって予測された時刻ｔの前記車両の位置情報とに基づいて、時刻ｔにおける前記車両の運動状態を推定する運転操作車両状態推定手段
   として機能させるプログラムであって、
   前記運転操作予測手段は、前記取得手段によって取得された時刻ｔの前記車両の外部環境情報と、前記運転操作車両状態推定手段によって推定された時刻ｔの前記車両の運動状態に基づいて、時刻ｔ＋１の前記車両のドライバの運転操作量を予測し、
   前記車両挙動予測手段は、前記運転操作予測手段によって予測された時刻ｔ＋１の前記ドライバの運転操作量と、前記運転操作車両状態推定手段によって推定された時刻ｔの前記車両の運動状態とに基づいて、時刻ｔ＋１の前記車両の位置情報及び前記車両の運動状態を予測するプログラム。