(57)【特許請求の範囲】
【請求項１】
車両の位置情報及び前記車両の外部環境情報を各時刻ｔについて取得する取得手段と、  前記取得手段によって取得された時刻ｔ−１の前記車両の外部環境情報と、時刻ｔ−１の前記車両の運動状態とに基づいて、時刻ｔの前記車両のドライバの運転操作量を予測する運転操作予測手段と、
前記運転操作予測手段によって予測された時刻ｔの前記ドライバの運転操作量と、時刻ｔ−１の前記車両の運動状態とに基づいて、時刻ｔの前記車両の位置情報及び前記車両の運動状態を予測する車両挙動予測手段と、
前記取得手段によって取得された時刻ｔの車両の位置情報と、前記車両挙動予測手段によって予測された時刻ｔの前記車両の位置情報とに基づいて、時刻ｔにおける前記車両の運動状態を推定する運転操作車両状態推定手段と、
を含み、
前記運転操作予測手段は、前記取得手段によって取得された時刻ｔの前記車両の外部環境情報と、前記運転操作車両状態推定手段によって推定された時刻ｔの前記車両の運動状態に基づいて、時刻ｔ＋１の前記車両のドライバの運転操作量を予測し、
前記車両挙動予測手段は、前記運転操作予測手段によって予測された時刻ｔ＋１の前記ドライバの運転操作量と、前記運転操作車両状態推定手段によって推定された時刻ｔの前記車両の運動状態とに基づいて、時刻ｔ＋１の前記車両の位置情報及び前記車両の運動状態を予測する
車両挙動予測装置。
【請求項２】
前記運転操作予測手段は、前記取得手段によって取得された時刻ｔの前記車両の外部環境情報と、前記運転操作車両状態推定手段によって推定された時刻ｔの前記車両の運動状態の確率分布に基づいて、時刻ｔ＋１の前記車両のドライバの運転操作量の確率分布を予測し、
前記車両挙動予測手段は、前記運転操作予測手段によって予測された時刻ｔ＋１の前記ドライバの運転操作量の確率分布と、前記運転操作車両状態推定手段によって推定された時刻ｔの前記車両の運動状態の確率分布とに基づいて、時刻ｔ＋１の前記車両の位置情報の確率分布及び前記車両の運動状態の確率分布を予測し、
前記運転操作車両状態推定手段は、前記取得手段によって取得された時刻ｔ＋１の車両の位置情報と、前記車両挙動予測手段によって予測された時刻ｔ＋１の前記車両の位置情報の確率分布とに基づいて、時刻ｔ＋１における前記車両の運動状態の確率分布を推定する請求項１に記載の車両挙動予測装置。
【請求項３】
前記外部環境情報に基づいて、予め定められた複数の速度制約条件の各々を満たす上限速度を算出し、前記算出された上限速度のうちの何れかを目標速度として選択する目標速度選択手段を更に含み、
前記運転操作予測手段は、前記目標速度選択手段によって選択された前記目標速度と、前記取得手段によって取得された時刻ｔの前記車両の外部環境情報と、前記運転操作車両状態推定手段によって推定された時刻ｔの前記車両の運動状態に基づいて、前記時刻ｔ＋１の前記車両のドライバの運転操作量を予測する
請求項１又は請求項２記載の車両挙動予測装置。
【請求項４】
各時刻ｔについて、前記車両の走路上の目標位置を決定する目標位置決定手段を更に含み、
前記取得手段は、前記車両の外部環境情報として、前記車両が走行する走路における定常走行速度と、前記車両が走行する走路の曲率と、前記車両の前方を走行する車両との距離を示す前方車両情報とを、各時刻ｔについて取得し、
前記目標速度選択手段は、前記定常走行速度を、定常走行に関する速度制約条件を満たす上限速度として算出し、前記走路の曲率に基づいて、カーブ走行に関する速度制約条件を満たす上限速度を算出し、前記前方車両情報に基づいて、前方車両追従に関する速度制約条件を満たす上限速度を算出し、前記目標位置決定手段によって決定された走路上の目標位置に基づいて、前後位置制御に関する速度制約条件を満たす上限速度を算出し、前記算出された上限速度のうちの何れかを目標速度として選択する請求項３記載の車両挙動予測装置。
【請求項５】
前記取得手段は、前記車両の外部環境情報として、更に、前記車両の走路上の信号情報と、前記車両の目的地とを、各時刻ｔについて取得し、
前記目標位置決定手段は、前記信号情報に基づいて、信号停止に関する予め定められた位置制約条件を満たす車両位置を算出し、前記車両の目的地に基づいて求められた経路情報に基づいて、経路に関する位置制約条件を満たす車両位置を算出し、前記算出された車両位置のうちの何れかを選択し、選択した位置を目標位置として決定する請求項４記載の車両挙動予測装置。
【請求項６】
前記運転操作予測手段は、前記取得手段によって取得された時刻ｔの前記車両の外部環境情報と、前記運転操作車両状態推定手段によって推定された時刻ｔの前記車両の運動状態及び前記車両のドライバの運転操作量に基づいて、時刻ｔ＋１の前記車両のドライバの運転操作量を予測し、
前記運転操作車両状態推定手段は、前記取得手段によって取得された時刻ｔ＋１の車両の位置情報と、前記車両挙動予測手段によって予測された時刻ｔ＋１の前記車両の位置情報とに基づいて、時刻ｔ＋１における前記車両の運動状態及び前記車両のドライバの運転操作量を推定する
請求項１〜請求項５の何れか１項記載の車両挙動予測装置。
【請求項７】
コンピュータを、
車両の位置情報及び前記車両の外部環境情報を各時刻ｔについて取得する取得手段と、  前記取得手段によって取得された時刻ｔ−１の前記車両の外部環境情報と、時刻ｔ−１の前記車両の運動状態とに基づいて、時刻ｔの前記車両のドライバの運転操作量を予測する運転操作予測手段、
前記運転操作予測手段によって予測された時刻ｔの前記ドライバの運転操作量と、時刻ｔ−１の前記車両の運動状態とに基づいて、時刻ｔの前記車両の位置情報及び前記車両の運動状態を予測する車両挙動予測手段、及び
前記取得手段によって取得された時刻ｔの車両の位置情報と、前記車両挙動予測手段によって予測された時刻ｔの前記車両の位置情報とに基づいて、時刻ｔにおける前記車両の運動状態を推定する運転操作車両状態推定手段
として機能させるプログラムであって、
前記運転操作予測手段は、前記取得手段によって取得された時刻ｔの前記車両の外部環境情報と、前記運転操作車両状態推定手段によって推定された時刻ｔの前記車両の運動状態に基づいて、時刻ｔ＋１の前記車両のドライバの運転操作量を予測し、
前記車両挙動予測手段は、前記運転操作予測手段によって予測された時刻ｔ＋１の前記ドライバの運転操作量と、前記運転操作車両状態推定手段によって推定された時刻ｔの前記車両の運動状態とに基づいて、時刻ｔ＋１の前記車両の位置情報及び前記車両の運動状態を予測するプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、車両挙動予測装置及びプログラムに係り、特に、車両の位置情報及び運動状態を予測する車両挙動予測装置及びプログラムに関する。
【背景技術】
【０００２】
従来より、複数種類の操作状態毎に予め求めた道路状態と車両の加速度との関係と、各地点の道路状態と、予測された操作状態の時系列データとに基づいて、走行路における車両の加速度の時系列データを予測し、走行路における車両の加速度の時系列データに基づいて、走行路における車両の速度の時系列データを予測する速度予測装置が知られている（特許文献１）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１２−１６８０３７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、上記の特許文献１に記載の技術では、車両の位置情報以外に、車速や加速度、操作量など、車両外から観測することが不可能な情報を使っているため、当該装置が搭載されている車両の予測に利用することができない。
【０００５】
また、複数の運転シーンを想定していないため、一般道運転の予測精度が低い。
【０００６】
本発明は、上記の事情に鑑みてなされたもので、車両の位置情報及び運動状態を精度よく予測することができる車両挙動予測装置及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【０００７】
上記の目的を達成するために本発明に係る車両挙動予測装置は、車両の位置情報及び前記車両の外部環境情報を各時刻ｔについて取得する取得手段と、前記取得手段によって取得された時刻ｔ−１の前記車両の外部環境情報と、時刻ｔ−１の前記車両の運動状態とに基づいて、時刻ｔの前記車両のドライバの運転操作量を予測する運転操作予測手段と、前記運転操作予測手段によって予測された時刻ｔの前記ドライバの運転操作量と、時刻ｔ−１の前記車両の運動状態とに基づいて、時刻ｔの前記車両の位置情報及び前記車両の運動状態を予測する車両挙動予測手段と、前記取得手段によって取得された時刻ｔの車両の位置情報と、前記車両挙動予測手段によって予測された時刻ｔの前記車両の位置情報とに基づいて、時刻ｔにおける前記車両の運動状態を推定する運転操作車両状態推定手段と、を含み、前記運転操作予測手段は、前記取得手段によって取得された時刻ｔの前記車両の外部環境情報と、前記運転操作車両状態推定手段によって推定された時刻ｔの前記車両の運動状態に基づいて、時刻ｔ＋１の前記車両のドライバの運転操作量を予測し、前記車両挙動予測手段は、前記運転操作予測手段によって予測された時刻ｔ＋１の前記ドライバの運転操作量と、前記運転操作車両状態推定手段によって推定された時刻ｔの前記車両の運動状態とに基づいて、時刻ｔ＋１の前記車両の位置情報及び前記車両の運動状態を予測する
。
【０００８】
本発明に係るプログラムは、コンピュータを、車両の位置情報及び前記車両の外部環境情報を各時刻ｔについて取得する取得手段と、前記取得手段によって取得された時刻ｔ−１の前記車両の外部環境情報と、時刻ｔ−１の前記車両の運動状態とに基づいて、時刻ｔの前記車両のドライバの運転操作量を予測する運転操作予測手段、前記運転操作予測手段によって予測された時刻ｔの前記ドライバの運転操作量と、時刻ｔ−１の前記車両の運動状態とに基づいて、時刻ｔの前記車両の位置情報及び前記車両の運動状態を予測する車両挙動予測手段、及び前記取得手段によって取得された時刻ｔの車両の位置情報と、前記車両挙動予測手段によって予測された時刻ｔの前記車両の位置情報とに基づいて、時刻ｔにおける前記車両の運動状態を推定する運転操作車両状態推定手段として機能させるプログラムであって、前記運転操作予測手段は、前記取得手段によって取得された時刻ｔの前記車両の外部環境情報と、前記運転操作車両状態推定手段によって推定された時刻ｔの前記車両の運動状態に基づいて、時刻ｔ＋１の前記車両のドライバの運転操作量を予測し、前記車両挙動予測手段は、前記運転操作予測手段によって予測された時刻ｔ＋１の前記ドライバの運転操作量と、前記運転操作車両状態推定手段によって推定された時刻ｔの前記車両の運動状態とに基づいて、時刻ｔ＋１の前記車両の位置情報及び前記車両の運動状態を予測するプログラムである。
【０００９】
本発明によれば、取得手段によって、車両の位置情報及び車両の外部環境情報を各時刻ｔについて取得する。
【００１０】
そして、運転操作予測手段によって、取得手段によって取得された時刻ｔ−１の車両の外部環境情報と、時刻ｔ−１の車両の運動状態とに基づいて、時刻ｔの車両のドライバの運転操作量を予測する。
【００１１】
そして、車両挙動予測手段によって、運転操作予測手段によって予測された時刻ｔのドライバの運転操作量と、時刻ｔ−１の車両の運動状態とに基づいて、時刻ｔの車両の位置情報及び車両の運動状態を予測する。
【００１２】
そして、運転操作車両状態推定手段によって、取得手段によって取得された時刻ｔの車両の位置情報と、車両挙動予測手段によって予測された時刻ｔの車両の位置情報とに基づいて、時刻ｔにおける車両の運動状態を推定する。
【００１３】
また、運転操作予測手段は、取得手段によって取得された時刻ｔの車両の外部環境情報と、運転操作車両状態推定手段によって推定された時刻ｔの車両の運動状態に基づいて、時刻ｔ＋１の車両のドライバの運転操作量を予測する。
【００１４】
また、車両挙動予測手段は、運転操作予測手段によって予測された時刻ｔ＋１のドライバの運転操作量と、運転操作車両状態推定手段によって推定された時刻ｔの車両の運動状態とに基づいて、時刻ｔ＋１の車両の位置情報及び車両の運動状態を予測する。
【００１５】
このように、車両の外部環境情報と、車両の位置情報とに基づいて、車両の位置情報及び運動状態を精度よく予測することができる。
【００１６】
本発明に係る、前記運転操作予測手段は、前記取得手段によって取得された時刻ｔの前記車両の外部環境情報と、前記運転操作車両状態推定手段によって推定された時刻ｔの前記車両の運動状態の確率分布に基づいて、時刻ｔ＋１の前記車両のドライバの運転操作量の確率分布を予測し、前記車両挙動予測手段は、前記運転操作予測手段によって予測された時刻ｔ＋１の前記ドライバの運転操作量の確率分布と、前記運転操作車両状態推定手段によって推定された時刻ｔの前記車両の運動状態の確率分布とに基づいて、時刻ｔ＋１の前記車両の位置情報の確率分布及び前記車両の運動状態の確率分布を予測し、前記運転操作車両状態推定手段は、前記取得手段によって取得された時刻ｔ＋１の車両の位置情報と、前記車両挙動予測手段によって予測された時刻ｔ＋１の前記車両の位置情報の確率分布とに基づいて、時刻ｔ＋１における前記車両の運動状態の確率分布を推定するようにすることができる。
【００１７】
また、本発明は、前記外部環境情報に基づいて、予め定められた複数の速度制約条件の各々を満たす上限速度を算出し、前記算出された上限速度のうちの何れかを目標速度として選択する目標速度選択手段を更に含み、前記運転操作予測手段は、前記目標速度選択手段によって選択された前記目標速度と、前記取得手段によって取得された時刻ｔの前記車両の外部環境情報と、前記運転操作車両状態推定手段によって推定された時刻ｔの前記車両の運動状態に基づいて、前記時刻ｔ＋１の前記車両のドライバの運転操作量を予測するようにすることができる。
【００１８】
また、本発明は、各時刻ｔについて、前記車両の走路上の目標位置を決定する目標位置決定手段を更に含み、前記取得手段は、前記車両の外部環境情報として、前記車両が走行する走路における定常走行速度と、前記車両が走行する走路の曲率と、前記車両の前方を走行する車両との距離を示す前方車両情報とを、各時刻ｔについて取得し、前記目標速度選択手段は、前記定常走行速度を、定常走行に関する速度制約条件を満たす上限速度として算出し、前記走路の曲率に基づいて、カーブ走行に関する速度制約条件を満たす上限速度を算出し、前記前方車両情報に基づいて、前方車両追従に関する速度制約条件を満たす上限速度を算出し、前記目標位置決定手段によって決定された走路上の目標位置に基づいて、前後位置制御に関する速度制約条件を満たす上限速度を算出し、前記算出された上限速度のうちの何れかを目標速度として選択するようにすることができる。
【００１９】
また、本発明における前記取得手段は、前記車両の外部環境情報として、更に、前記車両の走路上の信号情報と、前記車両の目的地とを、各時刻ｔについて取得し、前記目標位置決定手段は、前記信号情報に基づいて、信号停止に関する予め定められた位置制約条件を満たす車両位置を算出し、前記車両の目的地に基づいて求められた経路情報に基づいて、経路に関する位置制約条件を満たす車両位置を算出し、前記算出された車両位置のうちの何れかを選択し、選択した位置を目標位置として決定するようにすることができる。
【００２０】
また、本発明における前記運転操作予測手段は、前記取得手段によって取得された時刻ｔの前記車両の外部環境情報と、前記運転操作車両状態推定手段によって推定された時刻ｔの前記車両の運動状態及び前記車両のドライバの運転操作量に基づいて、時刻ｔ＋１の前記車両のドライバの運転操作量を予測し、前記運転操作車両状態推定手段は、前記取得手段によって取得された時刻ｔ＋１の車両の位置情報と、前記車両挙動予測手段によって予測された時刻ｔ＋１の前記車両の位置情報とに基づいて、時刻ｔ＋１における前記車両の運動状態及び前記車両のドライバの運転操作量を推定するようにすることができる。
【００２１】
また、本発明のプログラムは、記憶媒体に格納して提供することも可能である。
【発明の効果】
【００２２】
以上説明したように、本発明の速度予測装置及びプログラムによれば、取得された時刻ｔの車両の位置情報と、予測された時刻ｔの車両の位置情報とに基づいて、時刻ｔにおける車両の運動状態を推定し、時刻ｔの車両の外部環境情報と、推定された時刻ｔの車両の運動状態に基づいて、時刻ｔ＋１の車両のドライバの運転操作量を予測し、予測された時刻ｔ＋１のドライバの運転操作量と、推定された時刻ｔの車両の運動状態とに基づいて、時刻ｔ＋１の車両の位置情報及び運動状態を予測することにより、車両の外部環境情報と、車両の位置情報とに基づいて、車両の位置情報及び運動状態を精度よく予測することができる、という効果が得られる。
【図面の簡単な説明】
【００２３】
【図１】本発明の第１の実施の形態に係る車両挙動予測装置を示すブロック図である。
【図２】本発明の第１の実施の形態に係る状態空間モデルを示す図である。
【図３】本発明の第１の実施の形態に係る車両挙動予測装置における予測処理の概念を示す概念図である。
【図４】本発明の第１の実施の形態に係る車両挙動予測装置におけるフィルタリング処理の概念を示す概念図である。
【図５】本発明の第１の実施の形態に係る車両挙動予測装置の運転操作予測部２６の詳細を示すブロック図である。
【図６】本発明の第１の実施の形態に係る車両挙動予測装置の車両挙動予測部２８の詳細を示すブロック図である。
【図７】本発明の第１の実施の形態に係る車両挙動予測装置における車両挙動予測処理ルーチンの内容を示すフローチャートである。
【図８】本発明の第１の実施の形態に係る車両挙動予測装置における予測処理ルーチンの内容を示すフローチャートである。
【図９】本発明の第１の実施の形態に係る車両挙動予測装置における運転操作予測処理ルーチンの内容を示すフローチャートである。
【図１０】本発明の第１の実施の形態に係る車両挙動予測装置における車両予測処理ルーチンの内容を示すフローチャートである。
【図１１】本発明の第１の実施の形態に係る車両挙動予測装置における運転操作車両状態推定処理ルーチンの内容を示すフローチャートである。
【図１２】本発明の第２の実施の形態に係る車両挙動予測装置を示すブロック図である。
【図１３】本発明の第２の実施の形態に係る車両挙動予測装置の運転操作予測部２２６の詳細を示すブロック図である。
【発明を実施するための形態】
【００２４】
以下、図面を参照して、本発明の実施の形態について説明する。車両の位置情報と、車両の外部環境情報とに基づいて、運転操作量及び運動状態を推定し、車両の挙動を予測する車両挙動予測装置に本発明を適用した場合を例に説明する。なお、本実施の形態では、予測対象車両を自車両とし、運転操作量及び運動状態を推定し、車両の挙動を予測するために、拡張カルマンフィルタを用いた場合を例に説明する。
【００２５】
図１に示すように、第１の実施の形態に係る車両挙動予測装置１０は、ドライバが目的地を入力するための入力操作部１２と、地図情報（道路ネットワークデータ）、及び道路情報を記憶した道路ネットワークデータベース１４と、予測対象車両の前方を走行する前方車両を検出する前方車情報取得部１６と、予測対象車両の位置を計測する位置計測部１８と、車両の挙動を予測するコンピュータ２０と、予測結果を出力する出力装置４０とを備えている。
【００２６】
道路ネットワークデータベース１４には、地図情報、及び道路情報が記憶されている。道路情報には、走路速度情報及び走路曲率情報が含まれている。走路速度情報には、当該走路における定常走行速度に関する情報が含まれている。定常走行速度は、当該走路における過去の走行データ及び制限速度等から算出される。また、走路曲率情報には、当該走路の曲率に関する情報が含まれている。
【００２７】
前方車情報取得部１６は、例えばレーザレーダ装置が該当し、予測対象車両に対する前方車両までの走行方向の相対距離を計測し、相対距離と前方車との相対速度とを前方車両情報として各時刻ｔについて取得する。なお、相対速度は、相対距離を時間微分することにより得られる。
【００２８】
位置計測部１８は、例えば、ＧＰＳセンサを用いて構成され、予測対象車両の位置を各時刻ｔについて計測する。
【００２９】
コンピュータ２０は、ＣＰＵと、ＲＡＭと、後述する車両挙動予測処理ルーチン、予測処理ルーチン、運転操作予測処理ルーチン、車両予測処理ルーチン、及び運転操作車両状態推定処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。コンピュータ２０は、入力操作部１２により入力された目的地情報を取得すると共に、道路ネットワークデータベース１４から得られる地図情報及び道路情報、並びに前方車情報取得部１６によって検出された前方車両情報を、車両の外部環境情報として取得する車両情報取得部２２と、取得した外部環境情報に基づいて、ドライバの運転操作量、車両の位置情報及び運動状態を予測する予測部２４と、位置計測部１８によって計測された車両の位置情報を取得する位置情報取得部３０と、位置情報取得部３０によって取得された位置情報と、予測部２４によって予測された予測結果とに基づいて、ドライバの運転操作量、及び車両の運動状態を推定する運転操作車両状態推定部３２とを備えている。なお、予測部２４で行う処理が拡張カルマンフィルタにおける予測ステップに対応し、運転操作車両状態推定部３２で行う処理が拡張カルマンフィルタにおけるフィルタリングステップに対応する。また、車両情報取得部２２、及び位置情報取得部３０は、取得手段の一例である。
【００３０】
車両情報取得部２２は、車両の外部環境情報を各時刻ｔについて取得する。
具体的には、車両情報取得部２２は、車両の挙動を予測するときに、前方車情報取得部１６によって取得された前方車両情報と、道路ネットワークデータベース１４から得られる当該位置における地図情報及び道路情報とを逐次取得して、車両の外部環境情報として蓄積する。また、車両情報取得部２２は、入力操作部１２により受け付けた目的地情報を各時刻ｔについて取得する。
【００３１】
本実施の形態では、拡張カルマンフィルタを用いて、ドライバの運転操作量、車両の位置情報及び運動状態を予測し、ドライバの運転操作量及び運動状態を推定する。以下、拡張カルマンフィルタの原理について説明する。
【００３２】
本実施の形態では、車両の位置［ｘｔ，ｙｔ］と、車両の向きθｔと、車両の速度ｖｔと、車両の加速度αｔと、車両の曲率δｔとを、以下の式（１）に示すように状態ベクトルＸｔ＝［ｘｔｙｔθｔｖｔαｔδｔ］として構成する。また、以下の式（２）に示すように、位置計測部１８によって計測された車両の位置情報を観測ベクトルＹｔ＝［ｘｔ＊ｙｔ＊］として構成する。
【００３３】
【数１】
0005904226.tif 000002
【００３４】
（１）予測ステップ
まず、予測ステップにおける処理の概要について説明する。予測ステップでは、以下の式（３）に示すように、時刻ｔ−１の状態ベクトルＸｔ−１と、システム雑音ｕｔ（誤差項）とに基づいて、時刻ｔの状態ベクトルＸｔを算出する。以下の式（３）におけるｆｔは非線形関数であり、本実施の形態では、図２に示すように、ドライバモデルと車両物理モデルとで表現される。図２のドライバモデルが運転操作予測部２６に対応し、車両物理モデルが車両挙動予測部２８に対応する。
【００３５】
また、観測ベクトルＹｔは、観測行列Ｈｔと観測雑音ｗｔ（誤差項）とに基づいて、以下の式（４）に示すように算出される。なお、観測行列Ｈｔは予め求められている。
【００３６】
【数２】
0005904226.tif 000003
【００３７】
図３に、予測ステップの概念図を示す。図３に示すように、予測ステップでは、フィルタリングステップで推定された時刻ｔ−１の状態ベクトルＸｔ−１｜ｔ−１に基づいて、時刻ｔの状態ベクトルＸｔ｜ｔ−１を予測する（中心値の予測）。なお、Ｘｔ｜ｔ−１は、時刻ｔ−１までに利用可能なデータに基づき予測された時刻ｔの状態ベクトルを表す。また、Ｘｔ−１｜ｔ−１は、時刻ｔ−１までに利用可能なデータに基づき推定された時刻ｔ−１の状態ベクトルを表す。
【００３８】
なお、以下では、添え字ｔ｜ｔ−１が付与された値は、時刻ｔ−１までに利用可能なデータに基づき予測された時刻ｔの事前推定値を表す。また、添え字ｔ｜ｔ（又は、ｔ−１｜ｔ−１）が付与された値は、時刻ｔまでに利用可能なデータに基づき推定された時刻ｔの事後推定値を表す。
【００３９】
また、予測ステップでは、関数ｆｔを偏微分して得られる行列＾Ｆｔと行列＾Ｇｔとを、以下の式（５）〜（６）に従って算出する。
【００４０】
【数３】
0005904226.tif 000004
【００４１】
ここで、「’」はベクトルの転置を表す。
【００４２】
そして、上記式（５）〜（６）に従って算出した行列＾Ｆｔと行列＾Ｇｔとに基づいて、以下の式（７）に従って、事前誤差共分散行列Ｐｔ｜ｔ−１を算出する（分散の予測）。
【００４３】
【数４】
0005904226.tif 000005
【００４４】
なお、上記式（７）におけるＱｔは、システム雑音ｕｔ（誤差項）の分散を表す。
【００４５】
すなわち、予測ステップでは、時刻ｔの状態ベクトルＸｔ｜ｔ−１を中心値とし、事前誤差共分散行列Ｐｔ｜ｔ−１を分散とする確率分布を予測していることになる。
【００４６】
（２）フィルタリングステップ
次に、フィルタリングステップについての概要を説明する。図４に、フィルタリングステップの概念図を示す。図４に示すように、フィルタリングステップでは、上記式（１）で示した時刻ｔの状態ベクトルＸｔの要素である車両の位置［ｘｔ，ｙｔ］と、上記式（２）で示した時刻ｔの車両の位置情報［ｘ＊ｔ，ｙ＊ｔ］（観測中心）とに基づいて、フィルタリングを行い、予測ステップで出力された時刻ｔの状態ベクトルＸｔ｜ｔ−１を修正してＸｔ｜ｔを算出すると共に、事前誤差共分散行列Ｐｔ｜ｔ−１を修正して事後誤差共分散行列Ｐｔ｜ｔを算出する。
【００４７】
具体的には、まず、フィルタリングステップでは、予測ステップで算出された事前誤差共分散行列Ｐｔ｜ｔ−１と、観測行列Ｈｔと、観測雑音ｗｔ（誤差項）の分散Ｒｔとに基づいて、以下の式（８）に従って、カルマンゲインＫｔを算出する。
【００４８】
次に、算出されたカルマンゲインＫｔと、予測ステップで算出された時刻ｔにおける状態ベクトルＸｔ｜ｔ−１と、上記式（２）で示したＹｔ＝［ｘ＊ｔ，ｙ＊ｔ］と、観測行列Ｈｔとに基づいて、以下の式（９）に従って、時刻ｔにおける状態ベクトルＸｔ｜ｔを推定する。
【００４９】
そして、予測ステップで算出された事前誤差共分散行列Ｐｔ｜ｔ−１と、算出されたカルマンゲインＫｔと、観測行列Ｈｔとに基づいて、以下の式（１０）に従って、事後誤差共分散行列Ｐｔ｜ｔを算出する。
【００５０】
【数５】
0005904226.tif 000006
【００５１】
フィルタリングステップで算出された各値（カルマンゲインＫｔ、状態ベクトルＸｔ｜ｔ、事後誤差共分散行列Ｐｔ｜ｔ）は、次回の予測ステップにおける処理で用いられる。
【００５２】
予測部２４は、車両情報取得部２２によって取得された時刻ｔ−１の車両の外部環境情報と、運転操作車両状態推定部３２によって推定された時刻ｔ−１の車両の運動状態の確率分布及びドライバの運転操作量の確率分布に基づいて、時刻ｔのドライバの運転操作量の確率分布、車両の位置情報の確率分布、及び車両の運動状態の確率分布を予測する。本実施の形態では、ドライバの運転操作量として、ステアリング操作量に基づく車両の曲率δｔと、アクセルペダルの操作量に基づく車両の加速度αｔとを予測する。また、車両の位置情報及び運動状態として、車両の位置［ｘｔ，ｙｔ］と、車両の向きθｔと、車両の速度ｖｔとを予測する。
【００５３】
なお、本実施の形態では、車両の位置情報は０．１秒毎に取得されるものとする。また、予測部２４は、車両情報取得部２２によって取得された０．１秒前（時刻ｔ−１）の車両の外部環境情報と、運転操作車両状態推定部３２によって推定された０．１秒前（時刻ｔ−１）の車両の運動状態及びドライバの運転操作量に基づいて、現時刻（時刻ｔ）のドライバの運転操作量、車両の位置情報、及び車両の運動状態を予測する。そして、予測部２４は、運転操作予測部２６におけるドライバの運転操作量の予測処理と、車両挙動予測部２８における車両の位置情報、及び車両の運動状態の予測処理とを３０回繰り返し、３秒後までの車両の位置情報及び運動状態を予測するものとする。
【００５４】
具体的には、予測部２４は、運転操作予測部２６及び車両挙動予測部２８を備えている。運転操作予測部２６は、車両情報取得部２２によって取得された時刻ｔ−１の車両の外部環境情報と、運転操作車両状態推定部３２によって推定された時刻ｔ−１の状態ベクトルＸｔ−１（車両の位置情報、運動状態、及びドライバの運転操作量）とに基づいて、時刻ｔのドライバの運転操作量を予測する。運転操作予測部２６は、図５に示すように、運転操作受付部１００と、経路生成部１０２と、目標位置選択部１０４と、横位置制御部１０６と、目標速度制約部１０８と、目標速度選択部１１８と、速度制御部１２０と、運転操作出力部１２２とを備えている。
【００５５】
運転操作受付部１００は、運転操作車両状態推定部３２によって推定された時刻ｔ−１の状態ベクトルＸｔ−１を受け付ける。
【００５６】
経路生成部１０２は、車両情報取得部２２によって取得された目的地情報、及び地図情報と、運転操作受付部１００によって受け付けた時刻ｔ−１の状態ベクトルＸｔ−１の要素である［ｘｔ−１，ｙｔ−１］とに基づいて、最短経路を探索するアルゴリズムや、最短時間の経路を探索するアルゴリズムなど（例えば、Ａ＊アルゴリズム）を用いて、出発地から目的地までの最短経路を生成する。
【００５７】
目標位置選択部１０４は、経路生成部１０２によって生成された最短経路に基づいて、各時刻ｔについて、車両の走路上の目標位置を決定する。例えば、目標位置選択部１０４は、経路生成部１０２によって生成された車両の目的地から求められた経路情報と、運転操作受付部１００によって受け付けた時刻ｔ−１の状態ベクトルＸｔ−１の要素である［ｘｔ−１，ｙｔ−１］とに基づいて、経路上における位置［ｘｔ−１，ｙｔ−１］から１００ｍ先の地点を目標位置として選択する。
【００５８】
横位置制御部１０６は、目標位置選択部１０４によって選択された目標位置［ｘｒ，ｙｒ］と、運転操作受付部１００によって受け付けた時刻ｔ−１の状態ベクトルＸｔ−１の要素である［ｘｔ−１，ｙｔ−１，ｖｔ−１，θｔ−１］とに基づいて、ドライバのステアリング操作量を予測する。そして、横位置制御部１０６は、予測されたステアリング操作量に基づいて、車両の曲率δを算出する。ステアリング操作量と曲率δとの関係は予め求められている。例えば、横位置制御部１０６は、前方注視モデルを用いて、ステアリング操作量を予測し、予測されたステアリング操作量に基づいて曲率δを算出する。
【００５９】
目標速度制約部１０８は、車両情報取得部２２によって取得された時刻ｔ−１の車両の外部環境情報と、目標位置選択部１０４によって選択された目標位置［ｘｒ，ｙｒ］と、運転操作受付部１００によって受け付けた時刻ｔ−１の状態ベクトルＸｔ−１とに基づいて、複数の速度制約条件の各々を満たす上限速度を算出する。複数の速度制約条件の各々は、運転操作における様々なシーンに対応する。目標速度制約部１０８は、前後位置制御制約部１１０と、定常走行制約部１１２と、追従制約部１１４と、カーブ制約部１１６とを備えている。
【００６０】
前後位置制御制約部１１０は、運転操作受付部１００によって受け付けた時刻ｔ−１の状態ベクトルＸｔ−１の要素である［ｘｔ−１，ｙｔ−１］と、目標位置選択部１０４によって選択された走路上の目標位置［ｘｒ，ｙｒ］とに基づいて、前後位置制御に関する速度制約条件を満たす上限速度を算出する。例えば、前後位置制御制約部１１０は、比例制御モデルを用いて、時刻ｔ−１の車両の位置［ｘｔ−１，ｙｔ−１］と目標位置［ｘｒ，ｙｒ］との差に応じて、上限速度を算出する。
【００６１】
定常走行制約部１１２は、運転操作受付部１００によって受け付けた時刻ｔ−１の状態ベクトルＸｔ−１の要素である［ｘｔ−１，ｙｔ−１］と、車両情報取得部２２によって取得された道路情報に含まれる走路速度情報に基づいて、当該走路の定常走行速度を、以下の式（１１）に示す定常走行に関する速度制約条件を満たす上限速度として算出する。具体的には、定常走行制約部１１２は、時刻ｔ−１の車両の位置［ｘｔ−１，ｙｔ−１］に基づいて、当該位置の走路速度情報から得られる、車両が現在走行中の走路の一般的な最高速度を表すパラメータｐｒｏａｄ（定常走行速度）を取得し、上限速度とする。ｐｒｏａｄは、当該走路での過去の走行データや制限速度等から算出される。
【００６２】
【数６】
0005904226.tif 000007
【００６３】
追従制約部１１４は、運転操作受付部１００によって受け付けた時刻ｔ−１の状態ベクトルＸｔ−１の要素である［ｖｔ−１］と、車両情報取得部２２によって取得された前方車情報（前方車との相対速度）とに基づいて、以下の式（１２）に示す前方車両追従に関する速度制約条件を満たす上限速度を算出する。具体的には、追従制約部１１４は、時刻ｔ−１の車両の速度［ｖｔ−１］と前方車との相対速度に基づいて、等速予測により、Ｔ秒先の前方車との車間距離の予測値＾ｘｆ（ｔ＋Ｔ）を算出する。そして、追従制約部１１４は、式（１２）の制約条件を満たす上限速度を算出する。
【００６４】
【数７】
0005904226.tif 000008
【００６５】
ここで、ｐｆは予め求められたパラメータであり、Ｌは車両の長さである。
【００６６】
カーブ制約部１１６は、運転操作受付部１００によって受け付けた時刻ｔ−１の状態ベクトルＸｔ−１の要素である［ｘｔ−１，ｙｔ−１］と、車両情報取得部２２によって取得された地図情報に含まれる走路曲率情報に基づいて、以下の式（１３）に示すカーブ走行に関する速度制約条件を満たす上限速度を算出する。具体的には、カーブ制約部１１６は、時刻ｔ−１の車両の位置［ｘｔ−１，ｙｔ−１］に基づいて、当該位置におけるＴ秒先の走路の曲率の絶対値の予測値｜＾δ（ｔ＋Ｔ）｜を算出し、算出された予測値｜＾δ（ｔ＋Ｔ）｜に基づいて、式（１３）を満たす上限速度を算出する。
【００６７】
【数８】
0005904226.tif 000009
【００６８】
なお、ｐｃは予め求められたパラメータである。
【００６９】
目標速度選択部１１８は、目標速度制約部１０８によって算出された上限速度のうち、最も低い速度を目標速度ｖｒとして選択する。
【００７０】
速度制御部１２０は、目標速度選択部１１８によって選択された目標速度ｖｒと、運転操作受付部１００によって受け付けた時刻ｔ−１の状態ベクトルＸｔ−１の要素である［ｖｔ−１］とに基づいて、アクセルペダルの操作量を予測する。そして、速度制御部１２０は、予測されたアクセルペダルの操作量に基づいて、車両の加速度αを算出する。例えば、速度制御部１２０は、比例制御モデルを用いて、目標速度ｖｒと推定された時刻ｔ−１の車両の速度ｖｔ−１との差に応じて、アクセルペダルの操作量を予測し、加速度αを算出する。
【００７１】
運転操作出力部１２２は、運転操作受付部１００によって受け付けた時刻ｔ−１の状態ベクトルＸｔ−１の要素である［αｔ−１，δｔ−１］と、横位置制御部１０６によって算出された曲率δと、速度制御部１２０によって算出された加速度αとに基づいて、時刻ｔにおける曲率δｔと加速度αｔとを予測する。例えば、運転操作出力部１２２は、時刻ｔ−１における曲率δｔ−１と算出された曲率δとの差を抑制するように、時刻ｔにおける曲率δｔを予測する。同様に、運転操作出力部１２２は、時刻ｔ−１における加速度αｔ−１と算出された加速度αとの差を抑制するように、時刻ｔにおける加速度αｔを予測する。また、運転操作出力部１２２は、加速度αｔと曲率δｔとを出力する。
【００７２】
車両挙動予測部２８は、運転操作出力部１２２によって出力された曲率δｔ及び加速度αｔと、運転操作車両状態推定部３２によって推定された時刻ｔ−１の状態ベクトルＸｔ−１とに基づいて、時刻ｔの車両の位置情報及び運動状態を予測する。車両挙動予測部２８は、図６に示すように、車両挙動受付部２８０と、速度算出部２８２と、向き算出部２８４と、位置算出部２８６と、車両挙動出力部２８８とを備えている。
【００７３】
車両挙動受付部２８０は、運転操作車両状態推定部３２によって推定された時刻ｔ−１の状態ベクトルＸｔ−１を受け付ける。
【００７４】
速度算出部２８２は、運転操作出力部１２２によって出力された車両の加速度αｔと、車両挙動受付部２８０によって受け付けた時刻ｔ−１の状態ベクトルＸｔ−１の要素である［ｖｔ−１］とに基づいて、車両の速度ｖｔを予測する。例えば、速度算出部２８２は、速度算出式ｖｔ＝ｖｔ−１＋αｔＴに従って、車両の速度ｖｔを予測する。
【００７５】
向き算出部２８４は、運転操作出力部１２２によって出力された曲率δｔと、車両挙動受付部２８０によって受け付けた時刻ｔ−１の状態ベクトルＸｔ−１の要素である［θｔ−１］と、速度算出部２８２によって予測された車両の速度ｖｔとに基づいて、車両の向きθｔを予測する。
【００７６】
位置算出部２８６は、速度算出部２８２によって予測された車両の速度ｖｔと、向き算出部２８４によって予測された車両の向きθｔと、車両挙動受付部２８０によって受け付けた時刻ｔ−１の状態ベクトルＸｔ−１の要素である［ｘｔ−１，ｙｔ−１］とに基づいて、車両の位置［ｘｔ，ｙｔ］を予測する。
【００７７】
車両挙動出力部２８８は、速度算出部２８２よって予測された車両の速度ｖｔと、向き算出部２８４によって予測された車両の向きθｔと、位置算出部２８６によって予測された車両の位置［ｘｔ，ｙｔ］とを、車両の位置情報及び運動状態として出力する。
【００７８】
また、車両挙動予測部２８は、運転操作車両状態推定部３２によって推定された時刻ｔ−１の車両の位置情報、運動状態、及びドライバの運転操作量からなる状態ベクトルＸｔ−１に基づいて、上記式（５）〜（７）に従って、事前誤差共分散行列Ｐｔ｜ｔ−１を算出する。
【００７９】
位置情報取得部３０は、位置計測部１８によって計測された車両の位置情報を逐次取得する。
【００８０】
運転操作車両状態推定部３２は、位置情報取得部３０によって取得された時刻ｔの車両の位置情報と、車両挙動出力部２８８によって出力された時刻ｔの車両の位置情報の確率分布とに基づいて、時刻ｔにおける車両の運動状態の確率分布及び車両のドライバの運転操作量の確率分布を推定する。
具体的には、運転操作車両状態推定部３２は、位置情報取得部３０によって取得された時刻ｔの車両の位置情報［ｘ＊ｔ，ｙ＊ｔ］と、車両挙動出力部２８８によって出力された時刻ｔの車両の位置情報［ｘｔ，ｙｔ］と、車両挙動予測部２８によって算出された事前誤差共分散行列Ｐｔ｜ｔ−１とに基づいて、上記式（１１）〜（１２）に従って、車両の運動状態及び車両のドライバの運転操作量（状態ベクトルＸｔ｜ｔ）を推定する。また、運転操作車両状態推定部３２は、車両挙動予測部２８によって算出された事前誤差共分散行列Ｐｔ｜ｔ−１に基づいて、上記式（１３）に従って、事後誤差共分散行列Ｐｔ｜ｔを算出する。
【００８１】
出力装置４０は、予測部２４によって出力された、時刻ｔの状態ベクトルＸｔ＝［ｘｔ  ｙｔ  θｔ  ｖｔ  αｔ  δｔ］を出力する。
【００８２】
＜車両挙動予測装置１０の動作＞
次に、第１の実施の形態に係る車両挙動予測装置１０の動作について説明する。なお、以下の説明では、次の時刻（時刻ｔ）のドライバの運転操作量、車両の位置情報、及び車両の運動状態を予測した後、運転操作予測部２６におけるドライバの運転操作量の予測処理と、車両挙動予測部２８における車両の位置情報、及び車両の運動状態の予測処理とを更にＳ−１回（Ｓは例えば３０）繰り返し、３秒後までの車両の位置情報及び車両の運動状態を予測するものとする。また、３秒後までの繰り返し処理内で予測された状態ベクトルは、Ｘｔ＋ｓ＝［ｘｔ＋ｓ，ｙｔ＋ｓ，θｔ＋ｓ，ｖｔ＋ｓ，αｔ＋ｓ，δｔ＋ｓ］と表記する。
【００８３】
まず、予測対象車両のドライバの運転操作により車両が走行し、位置計測部１８によって車両の位置情報が逐次計測されているときに、入力操作部１２により目的地情報が入力されると、コンピュータ２０において、図７に示す車両挙動予測処理ルーチンが実行される。
【００８４】
ステップＳ１００において、車両情報取得部２２によって、入力操作部１２により入力された目的地情報を受け付ける。
【００８５】
ステップＳ１０２において、位置情報取得部３０によって、位置計測部１８によって計測された車両の位置情報を受け付ける。
【００８６】
ステップＳ１０４において、上記ステップＳ１０２で受け付けた車両の位置情報に基づいて、状態ベクトルＸｔ、事後誤差共分散行列Ｐｔ｜ｔ、システム雑音ｕｔ（誤差項）の分散Ｑｔ、及び観測雑音ｗｔ（誤差項）の分散Ｒｔの初期状態（ｔ＝０）を設定すると共に、ｔに０を代入する。そして、ステップＳ１０５において、時刻ｔを１だけインクリメントする。
【００８７】
ステップＳ１０６において、運転操作受付部１００によって、上記ステップＳ１０４で設定され、又は前回のステップＳ１１４で推定された時刻ｔ−１の状態ベクトルＸｔ−１を受け付ける。
【００８８】
ステップＳ１０８において、予測部２４によって、上記ステップＳ１００で取得された目的地情報と、上記ステップＳ１０６で受け付けた時刻ｔ−１の車両の位置情報、車両の運動状態及び車両のドライバの運転操作量（状態ベクトルＸｔ−１）とに基づいて、時刻ｔのドライバの運転操作量、車両の位置情報及び車両の運動状態（状態ベクトルＸｔ）を予測する。ステップ１０８は、図８に示す予測処理ルーチンによって実現される。
【００８９】
＜予測処理ルーチン＞
まず、ステップＳ２００において、繰り返し回数ｓに０を代入する。
【００９０】
次に、ステップＳ２０２において、運転操作予測部２６によって、上記ステップＳ１０６で受け付けた時刻ｔ−１の状態ベクトルＸｔ−１、又は前回のステップＳ２０２及び前回のステップＳ２０４で予測されたドライバの運転操作量、車両の位置情報、及び車両の運動状態Ｘｔ＋ｓ−１に基づいて、ドライバの運転操作を予測する。ステップ２０２は、図９に示す運転操作予測処理ルーチンによって実現される。
【００９１】
＜運転操作予測処理ルーチン＞
運転操作予測処理ルーチンでは、上記予測処理ルーチンにおける繰り返し回数ｓが０である場合には、上記ステップＳ１０４で受け付けた時刻ｔ−１の状態ベクトルＸｔ−１に基づいて以下のステップＳ３０２〜ステップＳ３１６の各処理を行う。一方、上記予測処理ルーチンにおける繰り返し回数ｓが０でない場合には、予測処理ルーチンにおける前回のステップＳ２０２及び前回のステップＳ２０４で予測されたドライバの運転操作量、車両の位置情報、及び車両の運動状態Ｘｔ＋ｓ−１に基づいて以下のステップＳ３０２〜ステップＳ３１６の各処理を行う。
【００９２】
まず、ステップＳ３００において、車両情報取得部２２によって、時刻ｔ−１の車両の外部環境情報を取得する。
【００９３】
ステップＳ３０２において、経路生成部１０２によって、上記ステップＳ１００で受け付けた目的地情報と、上記ステップＳ３００で受け付けた外部環境情報と、上記ステップＳ１０４で受け付けた時刻ｔ−１の状態ベクトルＸｔ−１の要素である［ｘｔ−１，ｙｔ−１］、又は前回のステップＳ２０４で車両挙動予測部２８によって予測された［ｘｔ＋ｓ−１，ｙｔ＋ｓ−１］とに基づいて、出発地から目的地までの最短経路を生成する。
【００９４】
ステップＳ３０４において、目標位置選択部１０４によって、上記ステップＳ３０２で生成された経路情報と、上記ステップＳ１０４で受け付けた時刻ｔ−１の状態ベクトルＸｔ−１の要素である［ｘｔ−１，ｙｔ−１］、又は前回のステップＳ２０４で車両挙動予測部２８によって予測された［ｘｔ＋ｓ−１，ｙｔ＋ｓ−１］とに基づいて、経路上における目標位置［ｘｒ，ｙｒ］を選択する。
【００９５】
ステップＳ３０６において、横位置制御部１０６によって、上記ステップＳ３０４で選択された目標位置［ｘｒ，ｙｒ］と、上記ステップＳ１０４で受け付けた時刻ｔ−１の状態ベクトルＸｔ−１の要素である［ｘｔ−１，ｙｔ−１，ｖｔ−１，θｔ−１］、又は前回のステップＳ２０４で車両挙動予測部２８によって予測された［ｘｔ＋ｓ−１，ｙｔ＋ｓ−１，ｖｔ＋ｓ−１，θｔ＋ｓ−１］とに基づいて、ドライバのステアリング操作量を予測する。そして、予測されたステアリング操作量に基づいて、車両の曲率δを算出する。
【００９６】
ステップＳ３０８において、複数の速度制約条件の各々を満たす上限速度を算出する。
本ステップでは、まず、前後位置制御制約部１１０によって、上記ステップＳ１０４で受け付けた時刻ｔ−１の状態ベクトルＸｔ−１の要素である［ｘｔ−１，ｙｔ−１］、又は前回のステップＳ２０４で車両挙動予測部２８によって予測された［ｘｔ＋ｓ−１，ｙｔ＋ｓ−１］と、上記ステップＳ３０４で選択された目標位置［ｘｒ，ｙｒ］とに基づいて、上限速度を算出する。
次に、定常走行制約部１１２によって、上記ステップＳ１０４で受け付けた時刻ｔ−１の状態ベクトルＸｔ−１の要素である［ｘｔ−１，ｙｔ−１］、又は前回のステップＳ２０４で車両挙動予測部２８によって予測された［ｘｔ＋ｓ−１，ｙｔ＋ｓ−１］と、上記ステップＳ３００で受け付けた外部環境情報に含まれる走路速度情報に基づいて、当該走路の定常走行速度を、上記式（８）に示す速度制約条件の上限速度として算出する
また、追従制約部１１４によって、上記ステップＳ１０４で受け付けた時刻ｔ−１の状態ベクトルＸｔ−１の要素である［ｖｔ−１］、又は前回のステップＳ２０４で車両挙動予測部２８によって予測された［ｖｔ＋ｓ−１］と、上記ステップＳ３００で受け付けた外部環境情報に含まれる前方車情報（前方車との相対速度）とに基づいて、上記式（９）に示す速度制約条件の上限速度を算出する。
そして、カーブ制約部１１６によって、上記ステップＳ１０４で受け付けた時刻ｔ−１の状態ベクトルＸｔ−１の要素である［ｘｔ−１，ｙｔ−１］、又は前回のステップＳ２０４で車両挙動予測部２８によって予測された［ｘｔ＋ｓ−１，ｙｔ＋ｓ−１］と、上記ステップＳ３００で受け付けた外部環境情報の地図情報に含まれる走路曲率情報に基づいて、上記式（１０）に示す速度制約条件の上限速度を算出する。
【００９７】
ステップＳ３１０において、目標速度選択部１１８によって、上記ステップＳ３０８で算出された上限速度のうち、最も低い速度を目標速度ｖｒとして選択する。
【００９８】
ステップＳ３１２において、速度制御部１２０によって、上記ステップＳ３１０で選択された目標速度ｖｒと、上記ステップＳ１０４で受け付けた時刻ｔ−１の状態ベクトルＸｔ−１の要素である［ｖｔ−１］、又は前回のステップＳ２０４で車両挙動予測部２８によって予測された［ｖｔ＋ｓ−１］とに基づいて、アクセルペダルの操作量を予測する。そして、予測されたアクセルペダルの操作量に基づいて、車両の加速度αを算出する。
【００９９】
ステップＳ３１４において、運転操作出力部１２２によって、上記ステップＳ１０４で受け付けた時刻ｔ−１の状態ベクトルＸｔ−１の要素である［αｔ−１，δｔ−１］、又は前回のステップＳ２０２で運転操作予測部２６によって予測された［αｔ＋ｓ−１，δｔ＋ｓ−１］と、上記ステップＳ３０６で算出された曲率δと、上記ステップＳ３１２で算出された加速度αとに基づいて、時刻ｔ＋ｓにおける曲率δｔ＋ｓと加速度αｔ＋ｓと（ｓ＝０の場合は、曲率δｔ、及び加速度αｔ）を予測する。
【０１００】
ステップＳ３１６において、上記ステップＳ３１４で予測された時刻ｔ＋ｓにおける曲率δｔ＋ｓと加速度αｔ＋ｓと（ｓ＝０の場合は、曲率δｔ、及び加速度αｔ）を、運転操作の中心値として出力して運転操作予測処理ルーチンを終了する。
【０１０１】
次に、予測処理ルーチンに戻り、ステップＳ２０４において、車両挙動予測部２８によって、上記ステップＳ１０６で受け付けた時刻ｔ−１の状態ベクトルＸｔ−１、又は前回のステップＳ２０２及び前回のステップＳ２０４で予測されたドライバの運転操作量、車両の位置情報、及び車両の運動状態Ｘｔ＋ｓ−１に基づいて、時刻ｔ（又は時刻ｔ＋ｓ）の車両の位置情報及び車両の運動状態を予測する。ステップ２０４は、図１０に示す車両予測処理ルーチンによって実現される。
【０１０２】
＜車両予測処理ルーチン＞
車両予測処理ルーチンでは、上記予測処理ルーチンにおける繰り返し回数ｓが０である場合には、上記ステップＳ１０４で受け付けた時刻ｔ−１の状態ベクトルＸｔ−１に基づいて以下のステップＳ４００〜ステップＳ４０４の各処理を行い、上記予測処理ルーチンにおける繰り返し回数ｓが０でない場合には、予測処理ルーチンにおける前回のステップＳ２０２及び前回のステップＳ２０４で予測されたドライバの運転操作量、車両の位置情報、及び車両の運動状態Ｘｔ＋ｓ−１に基づいて以下のステップＳ４００〜ステップＳ４０４の各処理を行う。
【０１０３】
まず、ステップＳ４００において、速度算出部２８２によって、上記ステップＳ２０２で出力された車両の加速度αｔと、上記ステップＳ１０６で受け付けた時刻ｔ−１の状態ベクトルＸｔ−１の要素である［ｖｔ−１］、又は前回のステップＳ２０４で車両挙動予測部２８によって予測された［ｖｔ＋ｓ−１］とに基づいて、車両の速度ｖｔ＋ｓ（ｓ＝０の場合は、ｖｔ）を予測する。
【０１０４】
次に、ステップＳ４０２において、向き算出部２８４によって、上記ステップＳ２０２で出力された曲率δｔと、上記ステップＳ１０６で受け付けた時刻ｔ−１の状態ベクトルＸｔ−１の要素である［θｔ−１］、又は前回のステップＳ２０４で車両挙動予測部２８によって予測された［θｔ＋ｓ−１］と、上記ステップＳ４００で予測された車両の速度ｖｔとに基づいて、車両の向きθｔ＋ｓを予測する。
【０１０５】
そして、ステップＳ４０４において、位置算出部２８６によって、上記ステップＳ４００で予測された車両の速度ｖｔ＋ｓと、上記ステップＳ４０２で予測された車両の向きθｔ＋ｓと、上記ステップＳ１０６で受け付けた時刻ｔ−１の状態ベクトルＸｔ−１の要素である［ｘｔ−１，ｙｔ−１］、又は前回のステップＳ２０４で車両挙動予測部２８によって予測された［ｘｔ＋ｓ−１，ｙｔ＋ｓ−１］とに基づいて、車両の位置［ｘｔ＋ｓ，ｙｔ＋ｓ］を予測する。
【０１０６】
ステップＳ４０６において、上記ステップＳ４００で予測された時刻ｔにおける車両の車速ｖｔ＋ｓと、上記ステップＳ４００で予測された時刻ｔにおける車両の向きθｔ＋ｓと、上記ステップＳ４０４で予測された時刻ｔにおける車両の位置［ｘｔ＋ｓ、ｙｔ＋ｓ］とを、車両挙動の中心値として出力して車両予測処理ルーチンを終了する。
【０１０７】
次に、予測処理ルーチンに戻り、ステップＳ２０６において、繰り返し回数ｓがＳ−１以上であるか否かを判定する。繰り返し回数ｓがＳ−１以上である場合には、ステップＳ２０８へ進む。一方、繰り返し回数ｓがＳ−１未満の場合には、ステップＳ２１０でｓを１インクリメントし、ステップＳ２０２へ戻る。
【０１０８】
ステップＳ２０８において、車両挙動予測部２８によって、運転操作車両状態推定部３２によって推定された時刻ｔ−１の車両の位置情報、運動状態、及びドライバの運転操作量からなる状態ベクトルＸｔ−１に基づいて、上記式（５）〜（７）に従って、事前誤差共分散行列Ｐｔ｜ｔ−１を算出する。
【０１０９】
ステップＳ２１０において、上記ステップＳ２０２で出力された時刻ｔ〜ｔ＋Ｓ−１における曲率δｔ＋ｓ及び加速度αｔ＋ｓ（ｓ＝０，・・・，Ｓ−１）と、上記ステップＳ２０４で出力された時刻ｔ〜ｔ＋Ｓ−１における車両の位置［ｘｔ＋ｓ，ｙｔ＋ｓ］、車両の向きθｔ＋ｓ、及び車両の速度ｖｔ＋ｓ（ｓ＝０，・・・，Ｓ−１）と、上記ステップＳ２０８で算出された事前誤差共分散行列Ｐｔ｜ｔ−１を結果として出力して予測処理ルーチンを終了する。
【０１１０】
次に、車両挙動予測処理ルーチンに戻り、ステップＳ１１０において、出力装置４０によって、上記ステップＳ１０８で出力された結果を、時刻ｔから３秒後までの予測結果として出力する。
【０１１１】
ステップＳ１１２において、位置情報取得部３０によって、時刻ｔの車両の位置情報を受け付ける。
【０１１２】
ステップＳ１１４において、運転操作車両状態推定部３２によって、上記ステップＳ１１２で受け付けた時刻ｔの車両の位置情報［ｘ＊ｔ，ｙ＊ｔ］と、上記ステップＳ１１０で出力された時刻ｔの車両の位置情報［ｘｔ，ｙｔ］とに基づいて、車両の運動状態及び車両のドライバの運転操作量を推定する。ステップ１１４は、図１１に示す運転状態車両状態推定処理ルーチンによって実現される。
【０１１３】
ステップＳ５００において、上記ステップＳ１１０で出力された、時刻ｔにおける、曲率δｔ、加速度αｔ、車両の速度ｖｔ、車両の向きθｔ、及び車両の位置［ｘｔ，ｙｔ］を受け付け、時刻ｔにおける状態ベクトルＸｔ＝［ｘｔ  ｙｔ  θｔ  ｖｔ  αｔ  δｔ］として構成する。
【０１１４】
ステップＳ５０２において、運転操作車両状態推定部３２によって、上記ステップＳ１１０で出力された事前誤差共分散行列Ｐｔ｜ｔ−１と、観測行列Ｈｔと、観測雑音ｗｔ（誤差項）の分散Ｒｔとに基づいて、上記式（１１）に従って、カルマンゲインＫｔを算出する。
【０１１５】
ステップＳ５０４において、運転操作車両状態推定部３２によって、上記ステップＳ５０２で算出されたカルマンゲインＫｔと、上記ステップＳ５００で構成した時刻ｔにおける状態ベクトルＸｔと、上記ステップＳ１１２で受け付けた車両の位置Ｙｔ＝［ｘ＊ｔ，ｙ＊ｔ］と、観測行列Ｈｔとに基づいて、上記式（１２）に従って、時刻ｔにおける状態ベクトル＾Ｘｔ＝［＾ｘｔ，＾ｙｔ，＾θｔ，＾ｖｔ，＾αｔ，＾δｔ］を推定する。
【０１１６】
ステップＳ５０６において、運転操作車両状態推定部３２によって、上記ステップＳ２０６で算出された事前誤差共分散行列Ｐｔ｜ｔ−１と、上記ステップＳ５０２で算出されたカルマンゲインＫｔと、観測行列Ｈｔとに基づいて、事後誤差共分散行列Ｐｔ｜ｔを算出する。
【０１１７】
ステップＳ５０８において、上記ステップＳ５０４で推定された時刻ｔにおける状態ベクトル＾Ｘｔ＝［＾ｘｔ，＾ｙｔ，＾θｔ，＾ｖｔ，＾αｔ，＾δｔ］と、上記ステップＳ５０６で算出された事後誤差共分散行列Ｐｔ｜ｔとを結果として出力して、運転操作車両状態推定処理ルーチンを終了する。
【０１１８】
次に、車両挙動予測処理ルーチンに戻り、ステップＳ１０５へ戻って、時刻ｔを１インクリメントする。
【０１１９】
以上説明したように、第１の実施の形態に係る車両挙動予測装置によれば、取得された時刻ｔの車両の位置情報と、予測された時刻ｔの車両の位置情報とに基づいて、時刻ｔにおける車両の運動状態及び車両のドライバの運転操作量を推定し、時刻ｔの車両の外部環境情報と、推定された時刻ｔの車両の運動状態及び車両のドライバの運転操作量とに基づいて、時刻ｔ＋１の車両のドライバの運転操作量を予測し、予測された時刻ｔ＋１のドライバの運転操作量と、推定された時刻ｔの車両の運動状態とに基づいて、時刻ｔ＋１の車両の位置情報及び運動状態を予測することにより、車両の外部環境情報と、車両の位置情報とに基づいて、車両の位置情報及び運動状態を精度よく予測することができる。
【０１２０】
また、外部環境情報と位置情報のみから、車両の位置情報及び運動状態の予測が可能となるため、従来に比べ、高い精度の予測性能が得られる。
【０１２１】
また、位置情報と外部環境情報とからドライバの運転操作量を予測することができるため、装置が搭載されていない車両（例えば、本実施の形態に係る車両挙動予測装置が搭載されている車両の前方車両など）の挙動を予測することができる。
【０１２２】
また、外部環境情報と位置情報のみから、車両の位置情報及び運動状態の予測が可能となるため、従来に比べ、高速で予測処理を行うことができる。
【０１２３】
また、外部環境情報と位置情報のみから、車両の位置情報及び運動状態の予測が可能となるため、車両の速度、加速度といったセンサ情報を用いずに、車両の位置情報及び運動状態の予測をすることができる。
【０１２４】
また、予め定められた複数の速度制約条件の各々を満たす上限速度を算出し、算出された上限速度のうちの何れかを目標速度として選択し、選択された目標速度と、時刻ｔの車両の外部環境情報と、地図情報と、推定された時刻ｔの車両の運動状態とに基づいて、時刻ｔ＋１の車両のドライバの運転操作量を予測することにより、車両の位置情報及び運動状態を精度よく予測することができる。
【０１２５】
また、複数の運転シーンを想定しているため、一般道運転の予測精度が高い。
【０１２６】
また、従来技術と比較し、高い速度予測精度を持ち、外部環境情報とＧＰＳ等の位置センサのみで挙動の予測が可能であり、位置センサの観測誤差に対してロバスト性が高い。
【０１２７】
また、一般道の運転の大半を占める、定常走行、前方車両追従、信号停止、及びカーブに対するドライバモデルを速度制約条件として導入し、それらの速度制約条件を満たす目標速度に対する速度制御を行うモデルで運転操作予測し、車両挙動を予測することで、観測した位置情報から拡張カルマンフィルタにより運転操作・車両状態の確率分布の推定が可能となり、外部環境情報とＧＰＳ等の位置センサのみで挙動の予測が可能になると共に、精度・ロバスト性ともに向上する。
【０１２８】
＜第２の実施の形態＞
次に、第２の実施の形態について説明する。なお、第１の実施の形態と同様の構成となる部分については、同一符号を付して説明を省略する。
【０１２９】
第２の実施の形態では、信号情報を更に考慮してドライバの運転操作量を予測し、車両の位置情報及び運動状態を予測する点が、第１の実施の形態と異なっている。
【０１３０】
図１２に示すように、第２の実施の形態に係る車両挙動予測装置２１０は、ドライバが目的地を入力するための入力操作部１２と、地図情報（道路ネットワークデータ）、道路情報、及び信号情報を記憶した道路ネットワークデータベース２１４と、予測対象車両の前方を走行する前方車を検出する前方車情報取得部１６と、予測対象車両の位置を計測する位置計測部１８と、車両の挙動を予測するコンピュータ２０と、予測結果を出力する出力装置４０とを備えている。
【０１３１】
道路ネットワークデータベース２１４には、地図情報、道路情報、及び信号情報が記憶されている。ここで、信号情報には、信号機の位置と当該信号機の信号変化の周期を示すデータが含まれている。
【０１３２】
車両情報取得部２２２は、道路ネットワークデータベース２１４から、車両の外部環境情報として更に、車両の走路上の信号情報を取得すると共に、取得した信号情報に基づいて、各時刻ｔの信号の状態を取得する。
【０１３３】
運転操作予測部２２６は、図１３に示すように、運転操作受付部１００と、経路生成部１０２と、目標位置制約部２１０１と、目標位置選択部２１０４と、横位置制御部１０６と、目標速度制約部１０８と、目標速度選択部１１８と、速度制御部１２０と、運転操作出力部１２２とを備えている。
【０１３４】
目標位置制約部２１０１は、車両情報取得部２２によって取得された時刻ｔ−１の車両の外部環境情報と、運転操作受付部１００によって受け付けた時刻ｔ−１の状態ベクトルＸｔ−１とに基づいて、複数の位置制約条件を満たす車両位置を算出する。目標位置制約部２１０１は、信号制約部２１０２と、経路制約部２１０３とを備えている。
【０１３５】
信号制約部２１０２は、車両情報取得部２２によって取得された時刻ｔ−１の車両の外部環境情報に含まれる信号情報と、運転操作受付部１００によって受け付けた時刻ｔ−１の状態ベクトルＸｔ−１の要素である［ｘｔ−１，ｙｔ−１，ｖｔ−１］とに基づいて、信号停止に関する予め定められた位置制約条件を満たす車両位置を算出する。具体的には、信号制約部２１０２は、以下の式（１４）に示す位置制約条件を満たす車両位置［ｘｒ，ｙｒ］を算出する。
【０１３６】
【数９】
0005904226.tif 000010
【０１３７】
ここで、［ｘｒ，ｙｒ］Ｔは車両位置、［ｘｔ−１，ｙｔ−１］Ｔは運転操作受付部１００によって受け付けた時刻ｔ−１の位置、［ｘｓｔｏｐｌｉｎｅ，ｙｓｔｏｐｌｉｎｅ］Ｔは信号に対応した停止線の位置、Ｓｔ−１は前回の信号の状態（赤、黄、青）、Ｓｒｅｄは赤信号、ｖｔ−１は、運転操作受付部１００によって受け付けた時刻ｔ−１の車両の速度、ｐｓｉｇｎａｌは、停止するか否かを決定するための予め求められたパラメータである。
【０１３８】
経路制約部２１０３は、経路生成部１０２によって車両の目的地に基づいて求められた経路情報と、運転操作受付部１００によって受け付けた時刻ｔ−１の状態ベクトルＸｔ−１の要素である［ｘｔ−１，ｙｔ−１］とに基づいて、経路に関する位置制約条件を満たす車両位置を算出する。例えば、経路制約部２１０３は、目的地までの経路上における、時刻ｔ−１の位置［ｘｔ−１，ｙｔ−１］から１００先の地点を車両位置として算出する。
【０１３９】
目標位置選択部２１０４は、信号制約部２１０２によって算出された車両位置、及び経路制約部２１０３によって算出された車両位置の何れかを選択し、選択された位置を目標位置として決定する。例えば、目標位置選択部２１０４は、時刻ｔ−１の位置［ｘｔ−１，ｙｔ−１］から最も近い車両位置を目標位置として選択する。
【０１４０】
なお、第２の実施の形態に係る車両挙動予測装置の他の構成及び作用については、第１の実施の形態と同様であるため、説明を省略する。
【０１４１】
以上説明したように、第２の実施の形態に係る車両挙動予測装置によれば、信号情報に基づいて、信号停止に関する予め定められた位置制約条件を満たす車両位置を算出し、車両の目的地に基づいて求められた経路情報に基づいて、経路に関する位置制約条件を満たす車両位置を算出し、算出された車両位置のうちの何れかを選択し、選択した位置を目標位置として決定することにより、車両の位置情報及び運動状態を精度良く予測することができる。
【０１４２】
なお、上記の実施の形態では、車両挙動予測装置が搭載されている自車両を、予測対象車両とした場合を例に説明したが、これに限定されるものではなく、車両挙動予測装置が搭載されていない、自車両以外の車両を、予測対象車両としてもよい。
車両挙動予測装置が搭載されていない車両を予測対象車両とする場合には、当該予測対象車両の車載器は入力操作部１２、前方車情報取得部１６、及び位置計測部１８を備え、車両挙動予測装置は、通信手段によって当該予測対象車両の外部環境情報と位置情報とを取得し、当該車両の位置情報及び運動状態を予測する。
【０１４３】
また、車両挙動予測装置が車両に搭載されている場合を例に説明したが、これに限定されるものではなく、交通センターなどに設定されているサーバによって、車両挙動予測装置が実現されてもよい。
【０１４４】
また、上記の実施の形態では、車両の外部環境情報として、地図情報、道路情報（走路速度情報及び走路曲率情報）、前方車両情報、信号情報を用いる場合を例に説明したが、これに限定されるものではなく、より多様なシーンに対応するために、対向車両や、周囲車両、歩行者等の情報を用いてもよい。
【０１４５】
また、上記の実施の形態における前方車情報取得部１６は、レーザレーダ装置を用いる場合を例に説明したが、これに限定されるものではない。例えば、前方車情報取得部１６として、車車間通信装置や、路車間通信装置等を用いて、前方車両情報を取得してもよい。
【０１４６】
また、上記の実施の形態では、上述の車両挙動予測装置は、道路ネットワークデータベース１４を備えている場合を例に説明したが、これに限定されるものではなく、例えば、道路ネットワークデータベース１４が車両挙動予測装置の外部装置に設けられ車両挙動予測装置は、外部装置と通信手段を用いて通信することにより、道路ネットワークデータベース１４を参照するようにしてもよい。
【０１４７】
また、上記の実施の形態における予測処理ルーチンでは、運転操作予測処理と車両挙動予測処理とを３０回繰り返す場合を例に説明したが、これに限定されるものではなく、運転操作予測処理と車両挙動予測処理とを繰り返さなくてもよい。
【０１４８】
また、上記の実施の形態では、目標速度制約部１０８において、複数の速度制約条件の各々を満たす上限速度を算出し、目標速度選択部１１８において、上限速度のうち最も低いものを目標速度として選択する場合を例に説明したが、これに限定されるものではなく、複数の速度制約条件の各々の組み合わせに基づいて、目標速度を選択するようにしてもよい。
【０１４９】
また、上記の実施の形態における運転操作出力部１２２は、時刻ｔ−１の状態ベクトルＸｔ−１の要素である［αｔ−１，δｔ−１］と、横位置制御部１０６によって算出された曲率δと、速度制御部１２０によって算出された加速度αとに基づいて、時刻ｔにおける曲率δｔと加速度αｔとを予測する場合を例に説明したが、これに限定されるものではなく、、時刻ｔ−１の状態ベクトルＸｔ−１の要素である［αｔ−１，δｔ−１］を用いずに、時刻ｔにおける曲率δｔと加速度αｔとを予測してもよい。
【０１５０】
また、上記の実施の形態における運転操作車両状態推定部３２は、時刻ｔにおける車両の運動状態及び車両のドライバの運転操作量を推定する場合を例に説明したが、これに限定されるものではなく、時刻ｔにおける車両の運動状態のみを推定してもよい。
【０１５１】
なお、本発明のプログラムは、記録媒体に格納して提供することができる。
【符号の説明】
【０１５２】
１０，２１０  車両挙動予測装置
１２   入力操作部
１４，２１４  道路ネットワークデータベース
１６   前方車情報取得部
１８   位置計測部
２０   コンピュータ
２２，２２２  車両情報取得部
２４   予測部
２６，２２６  運転操作予測部
２８   車両挙動予測部
３０   位置情報取得部
３２   運転操作車両状態推定部
４０   出力装置
１００ 運転操作受付部
１０２ 経路生成部
１０４，２１０４     目標位置選択部
１０６ 横位置制御部
１０８ 目標速度制約部
１１０ 前後位置制御制約部
１１２ 定常走行制約部
１１４ 追従制約部
１１６ カーブ制約部
１１８ 目標速度選択部
１２０ 速度制御部
１２２ 運転操作出力部
２８０ 車両挙動受付部
２８２ 速度算出部
２８４ 向き算出部
２８６ 位置算出部
２８８ 車両挙動出力部
２１０１      目標位置制約部
２１０２      信号制約部
２１０３      経路制約部
【図１】
0005904226.tif 000011
【図３】
0005904226.tif 000012
【図５】
0005904226.tif 000013
【図６】
0005904226.tif 000014
【図７】
0005904226.tif 000015
【図８】
0005904226.tif 000016
【図９】
0005904226.tif 000017
【図１０】
0005904226.tif 000018
【図１１】
0005904226.tif 000019
【図１２】
0005904226.tif 000020
【図１３】
0005904226.tif 000021
【図２】
0005904226.tif 000022
【図４】
0005904226.tif 000023