JP2016045767A - 運動量推定装置及びプログラム - Google Patents

Abstract

【課題】自車両の運動量の推定の不良地点を検出することができるようにする。
【解決手段】測距情報取得部２２が、車両周辺に存在する物体上の複数の点の各々の車両を基準とする位置であって、かつ車両に搭載された測距センサ１６によって計測された位置を各時刻ｔについて取得する。車両運動量変換部２４が、取得された時刻ｔの点に対応する位置の各々と、過去に取得された点に対応する位置の各々とに基づいて、時刻ｔにおける車両の運動量を推定する。不良推定地点判定部２８が、推定された車両の運動量に基づいて、車両の運動量の試行量を複数生成し、時刻ｔの点に対応する位置の各々に対して試行量を用いて座標変換して得られる点の位置の各々と、測距センサ１６によって過去に計測された点に対応する位置の各々とを比較して、試行量の確からしさを表すスコアを算出し、時刻ｔにおける車両の運動量の推定の不良地点らしさを算出する。
【選択図】図３

Description

本発明は、運動量推定装置及びプログラムに関する。

従来より、車速情報、ヨーレートセンサ、操舵角等の車両情報により自車運動量を推定する技術が知られている（例えば、特許文献１）。

また、レーザレーダデータの時系列照合により自車運動量を推定する技術が知られている（例えば、特許文献２，３）。

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

しかしながら、上記の特許文献１に記載の技術では、レーザレーダの計測結果を用いて移動物と静止物との判定を行う場合、レーザレーダデータと車両情報の間には、時間の遅延や応答遅れがあるため、特に加減速時や旋回時に、自車運動推定値の誤差が生じやすい。この場合、移動物と静止物の誤判定の原因となる。

また、上記の特許文献２，３に記載の技術では、トンネル等のように左右の路側端が凹凸のない平坦面で構成された箇所では、進行方向の時系列照合が不定となり、これにより、自車運動推定が劣化して、測距センサによる移動物と静止物の判別が劣化して誤検出が増大する。

本発明は、上記の事情に鑑みてなされたもので、車両の運動量の推定の不良地点を検出することができる運動量推定装置及びプログラムを提供することを目的とする。

上記の目的を達成するために第１の発明に係る運動量推定装置は、移動体の周辺に存在する物体上の複数の点の各々の前記移動体を基準とする位置であって、かつ前記移動体に搭載された計測手段によって計測された前記位置を各時刻ｔについて取得する取得手段と、前記取得手段によって取得された時刻ｔの前記点に対応する位置の各々と、過去に取得された前記点に対応する位置の各々とに基づいて、時刻ｔにおける前記移動体の運動量を推定する運動量推定手段と、前記運動量推定手段によって推定された前記移動体の運動量に基づいて、前記移動体の運動量の試行量を複数生成し、生成された前記複数の試行量の各々について、前記時刻ｔの前記点に対応する位置の各々に対して前記試行量を用いて座標変換して得られる点の位置の各々と、前記計測手段によって過去に計測された前記点に対応する位置の各々とを比較して、前記試行量の確からしさを表すスコアを算出し、前記複数の試行量の各々について算出された前記スコアと、前記複数の試行量とに基づいて、時刻ｔにおける前記移動体の運動量の推定の不良地点らしさを算出する不良地点らしさ算出手段とを含んで構成されている。

第２の発明に係るプログラムは、コンピュータに、移動体の周辺に存在する物体上の複数の点の各々の前記移動体を基準とする位置であって、かつ前記移動体に搭載された計測手段によって計測された前記位置を各時刻ｔについて取得する取得手段、前記取得手段によって取得された時刻ｔの前記点に対応する位置の各々と、過去に取得された前記点に対応する位置の各々とに基づいて、時刻ｔにおける前記移動体の運動量を推定する運動量推定手段、及び前記運動量推定手段によって推定された前記移動体の運動量に基づいて、前記移動体の運動量の試行量を複数生成し、生成された前記複数の試行量の各々について、前記時刻ｔの前記点に対応する位置の各々に対して前記試行量を用いて座標変換して得られる点の位置の各々と、前記計測手段によって過去に計測された前記点に対応する位置の各々とを比較して、前記試行量の確からしさを表すスコアを算出し、前記複数の試行量の各々について算出された前記スコアと、前記複数の試行量とに基づいて、時刻ｔにおける前記移動体の運動量の推定の不良地点らしさを算出する不良地点らしさ算出手段として機能させるためのプログラムである。

第１及び第２発明によれば、取得手段によって、移動体の周辺に存在する物体上の複数の点の各々の移動体を基準とする位置であって、かつ移動体に搭載された計測手段によって計測された位置を各時刻ｔについて取得する。

そして、運動量推定手段によって、取得手段によって取得された時刻ｔの点に対応する位置の各々と、過去に取得された点に対応する位置の各々とに基づいて、時刻ｔにおける移動体の運動量を推定する。

そして、不良地点らしさ算出手段によって、運動量推定手段によって推定された移動体の運動量に基づいて、移動体の運動量の試行量を複数生成し、生成された複数の試行量の各々について、時刻ｔの点に対応する位置の各々に対して試行量を用いて座標変換して得られる点の位置の各々と、計測手段によって過去に計測された点に対応する位置の各々とを比較して、試行量の確からしさを表すスコアを算出し、複数の試行量の各々について算出されたスコアと、複数の試行量とに基づいて、時刻ｔにおける移動体の運動量の推定の不良地点らしさを算出する。

このように、移動体の周辺に存在する物体上の複数の点の各々の車両を基準とする位置を各時刻ｔについて取得し、取得された時刻ｔの点に対応する位置の各々と、過去に取得された点に対応する位置の各々とに基づいて、時刻ｔにおける移動体の運動量を推定し、推定された移動体の運動量に基づいて、移動体の運動量の試行量を複数生成し、生成された複数の試行量の各々について、時刻ｔの点に対応する位置の各々に対して試行量を用いて座標変換して得られる点の位置の各々と、測距センサによって過去に計測された点に対応する位置の各々とを比較して、試行量の確からしさを表すスコアを算出し、複数の試行量の各々について算出されたスコアと、複数の試行量とに基づいて、時刻ｔにおける移動体の運動量の推定の不良地点らしさを算出することにより、移動体の運動量の推定の不良地点を検出することができる。

また、本発明は、運動量算出手段、及び出力手段を更に含み、前記取得手段は、前記移動体の運動状態を各時刻ｔについて更に取得し、前記運動量算出手段は、前記取得手段によって取得された時刻ｔの前記移動体の運動状態に基づいて、時刻ｔにおける前記移動体の運動量を算出し、前記出力手段は、推定された時刻ｔにおける前記移動体の運動量の推定の不良地点らしさが予め定められた閾値以上の場合に、前記運動量算出手段によって算出された前記移動体の運動量を出力し、推定された時刻ｔにおける前記移動体の運動量の推定の不良地点らしさが予め定められた閾値未満の場合に、前記運動量推定手段によって推定された前記移動体の運動量を出力するようにすることができる。

第３の発明に係る運動量推定装置は、移動体の周辺に存在する物体上の複数の点の各々の前記移動体を基準とする位置であって、かつ前記移動体に搭載された計測手段によって計測された前記位置と、前記移動体の運動状態をとを各時刻ｔについて取得する取得手段と、前記取得手段によって取得された時刻ｔの前記点に対応する位置の各々と、過去に取得された前記点に対応する位置の各々とに基づいて、時刻ｔにおける前記移動体の運動量を推定する運動量推定手段と、前記取得手段によって取得された時刻ｔの前記移動体の運動状態に基づいて、時刻ｔにおける前記移動体の運動量を算出する運動量算出手段と、前記運動量推定手段によって推定された前記移動体の運動量に基づいて、前記移動体の運動量の試行量を複数生成し、生成された前記複数の試行量の各々について、前記時刻ｔの前記点に対応する位置の各々に対して前記試行量を用いて座標変換して得られる点の位置の各々と、前記計測手段によって過去に計測された前記点に対応する位置の各々とを比較して、前記試行量の確からしさを表すスコアを算出し、前記複数の試行量の各々について算出された前記スコアと、前記複数の試行量とに基づいて、前記移動体の運動量の重み付き共分散行列を算出する分散行列算出手段と、前記分散行列算出手段によって算出された重み付き共分散行列に基づいて、カルマンフィルタで用いられる観測雑音の共分散を決定し、決定された前記観測雑音の共分散に基づいて、カルマンゲインを算出し、前記運動量推定手段によって推定された前記移動体の運動量、及び前記運動量算出手段によって算出された前記移動体の運動量を観測値として、算出された前記カルマンゲインを用いたカルマンフィルタに従って、時刻ｔにおける前記移動体の運動量を推定する状態推定手段と、を含んで構成される。

第３の発明によれば、取得手段によって、移動体の周辺に存在する物体上の複数の点の各々の移動体を基準とする位置であって、かつ移動体に搭載された計測手段によって計測された位置と、移動体の運動状態をとを各時刻ｔについて取得する。

運動量推定手段によって、取得手段によって取得された時刻ｔの点に対応する位置の各々と、過去に取得された点に対応する位置の各々とに基づいて、時刻ｔにおける移動体の運動量を推定する。

そして、運動量算出手段によって、取得手段によって取得された時刻ｔの移動体の運動状態に基づいて、時刻ｔにおける移動体の運動量を算出する。

そして、分散行列算出手段によって、運動量推定手段によって推定された移動体の運動量に基づいて、移動体の運動量の試行量を複数生成し、生成された複数の試行量の各々について、時刻ｔの点に対応する位置の各々に対して試行量を用いて座標変換して得られる点の位置の各々と、計測手段によって過去に計測された点に対応する位置の各々とを比較して、試行量の確からしさを表すスコアを算出し、複数の試行量の各々について算出されたスコアと、複数の試行量とに基づいて、移動体の運動量の重み付き共分散行列を算出する。

そして、状態推定手段によって、分散行列算出手段によって算出された重み付き共分散行列に基づいて、カルマンフィルタで用いられる観測雑音の共分散を決定し、決定された観測雑音の共分散に基づいて、カルマンゲインを算出し、運動量推定手段によって推定された移動体の運動量、及び運動量算出手段によって算出された移動体の運動量を観測値として、算出されたカルマンゲインを用いたカルマンフィルタに従って、時刻ｔにおける移動体の運動量を推定する。

このように、運動量推定手段によって推定された移動体の運動量に基づいて、移動体の運動量の試行量を複数生成し、生成された複数の試行量の各々について、時刻ｔの点に対応する位置の各々に対して試行量を用いて座標変換して得られる点の位置の各々と、計測手段によって過去に計測された点に対応する位置の各々とを比較して、試行量の確からしさを表すスコアを算出し、複数の試行量の各々について算出されたスコアと、複数の試行量とに基づいて、複数の試行量の重み付き共分散行列を算出し、算出された重み付き共分散行列に基づいて、カルマンフィルタで用いられる観測雑音の共分散を決定し、決定された観測雑音の共分散に基づいて、カルマンゲインを算出し、運動量推定手段によって推定された移動体の運動量、及び運動量算出手段によって算出された移動体の運動量を観測値として、算出されたカルマンゲインを用いたカルマンフィルタに従って、時刻ｔにおける移動体の運動量を推定することにより、移動体の運動量を精度良く推定することができる。

また、本発明における前記不良地点らしさ算出手段は、前記運動量推定手段によって推定された前記移動体の運動量に基づいて、前記移動体の運動量を含む所定の範囲から、サンプリングによって、前記移動体の運動量の試行量を複数生成するようにすることができる。

また、本発明は、前記取得手段によって取得された時刻ｔの前記点に対応する位置の各々に基づいて、移動体の周辺に存在する複数の視線誘導標の各々の前記移動体を基準とする位置を各時刻ｔについて取得する視線誘導標候補抽出手段を更に含み、前記運動量推定手段は、前記視線誘導標候補抽出手段によって取得された時刻ｔの前記視線誘導標に対応する位置の各々と、過去に取得された、前記視線誘導標に対応する位置の各々とに基づいて、時刻ｔにおける前記移動体の運動量を推定するようにすることができる。

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

以上説明したように、第１の発明の運動量推定装置及び第２の発明のプログラムによれば、移動体の周辺に存在する物体上の複数の点の各々の車両を基準とする位置を各時刻ｔについて取得し、取得された時刻ｔの点に対応する位置の各々と、過去に取得された点に対応する位置の各々とに基づいて、時刻ｔにおける移動体の運動量を推定し、推定された移動体の運動量に基づいて、移動体の運動量の試行量を複数生成し、生成された複数の試行量の各々について、時刻ｔの点に対応する位置の各々に対して試行量を用いて座標変換して得られる点の位置の各々と、測距センサによって過去に計測された点に対応する位置の各々とを比較して、試行量の確からしさを表すスコアを算出し、複数の試行量の各々について算出されたスコアと、複数の試行量とに基づいて、時刻ｔにおける移動体の運動量の推定の不良地点らしさを算出することにより、移動体の運動量の推定の不良地点を検出することができる、という効果が得られる。

また、第３の発明の運動量推定装置及びプログラムによれば、運動量推定手段によって推定された移動体の運動量に基づいて、移動体の運動量の試行量を複数生成し、生成された複数の試行量の各々について、時刻ｔの点に対応する位置の各々に対して試行量を用いて座標変換して得られる点の位置の各々と、計測手段によって過去に計測された点に対応する位置の各々とを比較して、試行量の確からしさを表すスコアを算出し、複数の試行量の各々について算出されたスコアと、複数の試行量とに基づいて、複数の試行量の重み付き共分散行列を算出し、算出された重み付き共分散行列に基づいて、カルマンフィルタで用いられる観測雑音の共分散を決定し、決定された観測雑音の共分散に基づいて、カルマンゲインを算出し、運動量推定手段によって推定された移動体の運動量、及び運動量算出手段によって算出された移動体の運動量を観測値として、算出されたカルマンゲインを用いたカルマンフィルタに従って、時刻ｔにおける移動体の運動量を推定することにより、移動体の運動量を精度良く推定することができる、という効果が得られる。

車両が市街地等を走行する場合のレーザレーダによる車両周辺の検出例と、車両がトンネル等を走行する場合のレーザレーダによる車両周辺の検出例とを示す図である。 運動量推定の分散を説明するための図である。 本発明の第１の実施の形態に係る運動量推定装置を示すブロック図である。 レーザレーダによって検出されたデータの一例である。 レーザレーダによる検出によって生成された距離画像と反射輝度画像との一例を示す図である。 本発明の第１の実施の形態に係る運動量推定装置における運動量推定処理ルーチンの内容を示すフローチャートである。 本発明の第２の実施の形態に係る運動量推定装置を示すブロック図である。 レーザレーダによる検出によって生成された反射輝度画像から、視線誘導標候補を抽出する一例を示す図である。 反射輝度画像から抽出された視線誘導標候補の一例を示す図である。 視線誘導標候補の誤検出を防ぐ方法の一例を説明するための図である。 車両の運動量と車体座標系との関係例を示す図である。 本発明の第２の実施の形態に係る運動量推定装置における運動量推定処理ルーチンの内容を示すフローチャートである。 本発明の第３の実施の形態に係る運動量推定装置を示すブロック図である。 本発明の第３の実施の形態に係る運動量推定装置における運動量推定処理ルーチンの内容を示すフローチャートである。 本発明の第３の実施の形態に係る運動量推定装置における状態推定処理ルーチンの内容を示すフローチャートである。

＜本発明の実施の形態の概要＞
大型車（バス、トラック）の運転者の居眠りや異常による高速道路上での衝突事故や逸脱事故を低減するためには、運転者が運転不能な場合に、センサで走路端を検知して、その走路端へ車両を自動で横付けして停車する運転支援システムが有用である。

このような運転支援システム構築のためには、センサ情報で静止物と移動物とを判別する必要があり、その前提として自車両の運動量を推定する必要がある。

従来法としては、レーザレーダ等の測距情報の時系列照合により、自車両の運動量を算出する方法が提案されていた。

レーザレーダの測距情報の時系列照合により自車両の運動量を算出する場合、図１（Ａ）に示すように、走行シーンが市街地であるときには、レーザレーダのスキャンデータの時系列照合が容易であることがわかる。

しかし、図１（Ｂ）に示すように、高速道路に頻出するトンネルやガードレールなどの単調な走行シーンでは、測距センサによって計測された路側物形状の変化に乏しく、レーザレーダのスキャンデータの時系列照合が困難となるため、車両の運動量の推定精度が劣化するという問題点があった。

そこで、本発明の実施の形態では、レーザレーダ等の測距センサによる自車両の運動量推定の良好・不良状態を、推定された運動量推定値の分散から推定する。レーザレーダによる車両の運動量推定の分散（Ｘ軸方向とＺ軸方向）の例を図２に示す。なお、Ｘ軸方向は車両の横方向を表し、Ｚ軸方向は車両進行方向を表す。

上記図１（Ａ）に示すように、車両の走行シーンが市街地であるときには、レーザレーダのスキャンデータの時系列照合が容易であるため、車両の運動量推定の分散は例えば図２（Ａ）に示すような分布となる。一方、上記図１（Ｂ）に示すように、高速道路に頻出するトンネルやガードレールなどの単調な走行シーンでは、車両の運動量推定の分散は例えば図２（Ｂ）に示すような分布となる。

そこで、本発明の実施の形態では、レーザレーダ等の測距センサによる自車両の運動量推定の良好・不良状態を自車両の運動量推定値の分散から推定し、不良時は車両ＣＡＮ（Controller Area Network）情報にて代替する。

以下、図面を参照して、本発明の実施の形態について説明する。本発明の実施の形態では、自車両の運動量の推定の不良地点らしさを算出し、不良地点らしさに応じて自車両の運動量を推定する運動量推定装置に本発明を適用した場合を例に説明する。

［第１の実施の形態］
＜運動量推定装置のシステム構成＞
図３に示すように、第１の実施の形態に係る運動量推定装置１０は、自車両の速度Ｖ［ｍ／ｓ］を逐次検出する車速センサ１２と、自車両のヨーレートω［ｒａｄ／ｓ］を逐次検出するヨーレートセンサ１４と、車両周辺に存在する物体上の複数の点の各々の車両を基準とする位置を逐次計測する測距センサ１６と、車両の運動量を推定するコンピュータ１８と、推定された車両の運動量に基づいて、走行環境を認識する走行環境認識装置３２とを備えている。測距センサ１６は計測手段の一例である。

車速センサ１２は、１秒間の車輪回転数Ｎ［回／秒］を計数して、車輪回転数Ｎ［回／秒］と既知の車輪径Ｒ[m]とから、算出式Ｖ＝π×Ｎ×Ｒに従って、車両の速度Ｖ［ｍ／ｓ］を検出する。

測距センサ１６は、自車両に搭載され、自車両周辺に存在する物体上の複数の点の各々の、自車両を基準とする位置を各時刻ｔについて計測する。本発明の実施の形態では、測距センサとして、レーザレーダを用いる場合を例に説明する。レーザレーダは車両先端部に組み込まれ、自車両の周辺の走行環境のスキャンデータを取得する。以下では、説明の都合上、レーザレーダは路面から高さｈ、車両の進行方向の向きで水平に設置されているものとする。

レーザレーダは、投光部と受光部から構成されていて、投光部より発射されたレーザ光が対象物に反射されて受光部で検知するまでのTOF(Time of Flight)を計測することにより測距を行う。測距の結果、距離画像と反射輝度画像とが得られる。図４にレーザレーダによる測距情報の一例を示す。また、図５に、レーザレーダによる計測によって得られた距離画像と反射輝度画像との一例を示す。

コンピュータ１８は、ＣＰＵと、ＲＡＭと、後述する運動量推定処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。コンピュータ１８は、車速センサ１２によって検出された自車両の速度と、ヨーレートセンサ１４によって検出された車両のヨーレートとを、車両情報として逐次取得する車両情報取得部２０と、測距センサ１６によって計測された自車両周辺に存在する物体上の複数の点に対応する位置の各々を取得する測距情報取得部２２と、取得した車両情報に基づいて、自車両の運動量を算出する車両運動量変換部２４と、測距情報取得部２２によって取得された測距情報に基づいて、自車両の運動量を推定する時系列照合部２６と、時系列照合部２６によって推定された自車両の運動量に基づいて、時刻ｔにおける車両の運動量の推定の不良地点らしさを算出し、推定の不良地点であるか否かを判定する不良推定地点判定部２８と、を備えている。なお、車両運動量変換部２４は、車両運動量算出手段の一例であり、時系列照合部２６は、車両運動量推定手段の一例であり、不良推定地点判定部２８は、不良地点らしさ算出手段の一例である。

車両情報取得部２０は、車速センサ１２によって検出された自車両の速度と、ヨーレートセンサ１４によって検出された自車両のヨーレートとを、自車両の運動状態として各時刻ｔについて取得する。

測距情報取得部２２は、測距センサ１６によって計測された自車両周辺に存在する物体上の複数の点の各々に対応する位置を各時刻ｔについて取得する。

車両運動量変換部２４は、車両情報取得部２０によって取得された時刻ｔの自車両の運動状態に基づいて、時刻ｔにおける自車両の運動量を算出する。
具体的には、車両運動量変換部２４は、車両情報取得部２０によって自車両の運動状態として取得された自車両の速度Ｖ［ｍ／ｓ］とヨーレートω［ｒａｄ／ｓ］とから、以下の算出式に従って、計測周期時間Ｔ当たりの自車両の運動量Δｘ［ｍ］、Δｚ［ｍ］、Δθ［ｒａｄ］を計算する。

Δｘ＝０．５×ω×Ｖ×Ｔ^２
Δｚ＝Ｖ×Ｔ
Δθ＝ω×Ｔ

上記算出式によって算出された計算値は、幾何学的な関係に基づくΔｘ、Δｚ、Δθの予測値である。
本発明の実施の形態では、上記の算出式によって車両の運動量を算出するが、車両運動方程式を適用して、より高精度な予測値を算出しても良い。

時系列照合部２６は、測距情報取得部２２によって取得された時刻ｔの複数の点に対応する位置の各々と、過去に取得された複数の点に対応する位置の各々とに基づいて、時刻ｔにおける自車両の運動量を推定する。

具体的には、時系列照合部２６は、測距情報取得部２２によって取得された過去のスキャンデータと最新のスキャンデータを照合し、路側物は対地固定していると仮定して、自車両の運動量Δｘ［ｍ］、Δｚ［ｍ］、Δθ［ｒａｄ］を推定する。スキャンデータの時系列照合は、既知のiterative closest pointアルゴリズム等により計算することができる。また、過去の時刻のスキャンデータを走行平面に対応させた２次元マップに蓄積しておき、最新のスキャンデータと当該２次元マップとを照合して、自車両の運動量を推定してもよい。

不良推定地点判定部２８は、時系列照合部２６によって推定された自車両の運動量に基づいて、自車両の運動量の試行量を複数生成する。次に不良推定地点判定部２８は、生成された複数の試行量の各々について、時刻ｔの点に対応する位置の各々に対して試行量を用いて座標変換して得られる点の位置の各々と、測距センサ１６によって過去に計測された点に対応する位置の各々とを比較して、試行量の確からしさを表すスコアを算出する。

次に、不良推定地点判定部２８は、複数の試行量の各々について算出されたスコアと、複数の試行量とに基づいて、時刻ｔにおける自車両の運動量の推定の不良地点らしさを算出する。

そして、不良推定地点判定部２８は、推定された時刻ｔにおける自車両の運動量の推定の不良地点らしさが予め定められた閾値以上の場合に、推定の不良地点であると判定し、推定された時刻ｔにおける自車両の運動量の推定の不良地点らしさが予め定められた閾値未満の場合に、推定の不良地点でないと判定する。

具体的には、不良推定地点判定部２８では、時系列照合部２６によって推定された自車両の運動量の推定値Δｘ、Δｚ、Δθの分散を推定し、トンネルなどの分散が増大する、推定の不良地点に自車両が存在するかどうかを判定する。

不良推定地点判定部２８は、時系列照合部２６によって推定された自車両の運動量の推定値Δｘ、Δｚ、Δθの分散を以下の（１）〜（４）の手順で計算する。

（１）サンプリング
まず、不良推定地点判定部２８は、時系列照合部２６によって推定された自車両の運動量に基づいて、自車両の運動量を含む所定の範囲から、サンプリングによって、自車両の運動量の試行量を以下のように複数生成する。

はじめに、不良推定地点判定部２８は、探索範囲Δｘ_ａ、Δｚ_ａ、Δθ_ａを設定する。探索範囲Δｘ_ａ、Δｚ_ａ、Δθ_ａは、車両の速度や加速度等から予め設定される。そして、自車両の運動量の推定値Δｘについて、Δｘ−Δx_ａ〜Δｘ＋Δx_ａの範囲から、一様分布確率に基づき、Ｍ個サンプリングして、 Δｘ_ｉを試行量の要素とする（ｉは０〜Ｍ−１の番号）。

次に、不良推定地点判定部２８は、自車両の運動量の推定値Δｚについて、同様に、Δｚ−Δｚ_ａ〜Δｚ＋Δｚ_ａの範囲から、一様分布確率に基づき、Ｍ個サンプリングして、 Δｚ_ｉを試行量の要素とする。

そして、不良推定地点判定部２８は、自車両の運動量の推定値Δθについて、同様に、Δθ−Δθ_ａ〜Δθ＋Δθ_ａの範囲から、一様分布確率に基づき、Ｍ個サンプリングして、 Δθ_ｉを試行量の要素とする。

そして、自車両の運動量の試行量を表すベクトルｘ_ｉ＾が、

ｘ_ｉ＾＝[Δｘ＋Δｘ_ｉ，Δｚ＋Δｚ_ｉ，Δθ＋Δθ_ｉ]

のようにＭ個求められる。なお、記号に付された「＾」は、当該記号が行列または多次元配列またはベクトルであることを表わしている。

（２）コスト算出
次に、不良推定地点判定部２８は、上記（１）でサンプリングされた自車両の運動量の試行量ｘ_ｉ＾に基づいて、最新のレーザレーダの計測点群（測距情報取得部２２によって取得された時刻ｔの複数の点に対応する位置の各々）を座標変換して、１時刻前のレーザレーダの計測点群（測距情報取得部２２によって取得された時刻ｔ−１の複数の点に対応する位置の各々）と重ねあわせて照合して、照合度合をＭ個のコストｃ_ｉとして求める。なお、最新のレーザレーダの計測点群を座標変換して、１時刻前のレーザレーダの計測点群との距離を算出して、当該距離をＭ個のコストｃ_ｉとして求めてもよい。

また、別の方法としては、最新のレーザレーダの計測点群を試行量ｘ_ｉ＾に基づき座標変換して、過去のスキャンデータを走行平面に対応させて蓄積した２次元マップと照合して、その照合値の合計をコストｃ_ｉとして求めてもよい。例えば、最新のレーザレーダの計測点群の立体物領域を試行量ｘ_ｉ＾に基づき座標変換して、過去のスキャンデータを走行平面に対応させて蓄積した２次元マップの立体物領域と照合して、その照合値の合計をコストｃ_ｉとして求めてもよい。

（３）コストの正規化
次に、不良推定地点判定部２８は、上記（２）で求めたコストｃ_ｉの総和が１となるように、各コスト値ｃ_ｉをコストの総和で除算して正規化して、試行量の確からしさを表すスコアｐ_ｉを算出する。スコアｐ_ｉは、自車運動量Δｘ_ｉ、Δｚ_ｉ、Δθ_ｉの確からしさを示す。

ｐ_ｉ＝ｃ_ｉ／ｓｕｍ(ｃ_ｉ)・・・（１）

なお、ｓｕｍ(ｃ_ｉ)は、各ｉのコストの総和を表す。

（４）重み付き共分散行列Ｖ＾の算出
次に、不良推定地点判定部２８は、上記（３）で算出したスコアｐ_ｉに基づいて、以下の式（２）により、ｘ_ｉ＾の重み付き共分散行列Ｖ＾を算出する。

そして、不良推定地点判定部２８は、上記式（２）の重み付き共分散行列Ｖ＾のΣ_ｉ（ｐ_ｉΔｚ_ｉ ^２）の項に着目し、Σ_ｉ（ｐ_ｉΔｚ_ｉ ^２）の値を自車両の運動量の推定の不良地点らしさとし、自車両の運動量の推定の不良地点らしさが予め定められた閾値よりも大きい場合には、自車両の運動量の推定の不良地点であると判定する。なお、本発明の実施の形態では、予め定められた閾値として車両の速度の分散を用いる。車両の速度の分散の値は予め計測して設定しておく。

出力部３０は、不良推定地点判定部２８によって、自車両の運動量の推定の不良地点であると判定された場合に、車両運動量変換部２４によって算出された自車両の運動量を出力し、不良推定地点判定部２８によって、自車両の運動量の推定の不良地点でないと判定された場合に、時系列照合部２６によって推定された自車両の運動量を出力する。このように、出力部３０は、推定の不良地点に自車両が存在する場合は、時系列照合部２６によって推定された自車両の運動量の推定値の代わりに、車両運動量変換部２４によって算出された自車両の運動量の推定値へ置換する。

走行環境認識装置３２は、測距センサ１６によって計測された自車両周辺に存在する物体上の複数の点の各々の自車両を基準とする位置と、出力部３０によって出力された自車両の運動量と基づいて、当該自車両周辺に存在する物体が移動しているか又は静止しているかを認識する。具体的には、走行環境認識装置３２は、出力部３０によって出力された自車両の運動量をキャンセルすることによって、当該自車両周辺に存在する物体が移動しているか又は静止しているかを認識する。

＜運動量推定装置の１０の動作＞
次に、第１の実施の形態に係る運動量推定装置の１０の動作について説明する。まず、車両のドライバの運転操作により自車両が走行し、車速センサ１２によって自車両の速度が逐次検出され、ヨーレートセンサ１４によって自車両のヨーレートが逐次検出され、測距センサ１６によって自車両周辺に存在する物体上の複数の点の各々の自車両を基準とする位置が逐次計測されているときに、コンピュータ１８において、図６に示す運動量推定処理ルーチンが実行される。

ステップＳ１００において、車両情報取得部２０によって、車速センサ１２によって検出された自車両の速度と、ヨーレートセンサ１４によって検出された自車両のヨーレートとを、自車両の運動状態として受け付ける。

ステップＳ１０２において、測距情報取得部２２によって、測距センサ１６によって計測された自車両周辺に存在する物体上の複数の点の各々に対応する位置を受け付ける。

ステップＳ１０４において、車両運動量変換部２４によって、上記ステップＳ１００で受け付けた自車両の運動状態に基づいて、自車両の運動量を算出する。

ステップＳ１０６において、時系列照合部２６によって、上記ステップＳ１０２で受け付けた複数の点に対応する位置の各々と、過去に取得された複数の点に対応する位置の各々とに基づいて、現時刻ｔにおける自車両の運動量を推定する。

ステップＳ１０８において、不良推定地点判定部２８によって、上記ステップＳ１０６で推定された自車両の運動量に基づいて、自車両の運動量を含む所定の範囲から、サンプリングによって、自車両の運動量の試行量ｘ_ｉ＾を複数生成する。

ステップＳ１１０において、不良推定地点判定部２８によって、上記ステップＳ１０８で生成された複数の試行量ｘ_ｉ＾の各々について、現時刻ｔの点に対応する位置の各々に対して試行量を用いて座標変換して得られる点の位置の各々と、測距センサ１６によって過去に計測された点に対応する位置の各々とを比較して、上記式（１）に従って、試行量の確からしさを表すスコアｐ_ｉを算出する。

ステップＳ１１２において、不良推定地点判定部２８によって、複数の試行量ｘ_ｉ＾の各々について上記ステップＳ１１０で算出されたスコアｐ_ｉと、複数の試行量ｘ_ｉ＾とに基づいて、上記式（２）に示すように、自車両の運動量の推定の不良地点らしさを含む重み付き共分散行列Ｖ＾を算出する。

ステップＳ１１４において、不良推定地点判定部２８によって、上記ステップＳ１１２で算出された重み付き共分散行列Ｖ＾のうち、自車両の運動量の推定の不良地点らしさΣ_ｉ（ｐ_ｉΔｚ_ｉ ^２）が、予め定められた閾値以上であるか否かを判定する。自車両の運動量の推定の不良地点らしさΣ_ｉ（ｐ_ｉΔｚ_ｉ ^２）が、予め定められた閾値以上である場合には、現時刻ｔに推定された自車両の運動量の推定は、不良地点におけるものであるとして、ステップＳ１１６へ進む。一方、自車両の運動量の推定の不良地点らしさΣ_ｉ（ｐ_ｉΔｚ_ｉ ^２）が、予め定められた閾値未満である場合には、現時刻ｔに推定された自車両の運動量の推定は、不良地点におけるものでないとして、ステップＳ１１８へ進む。

ステップＳ１１６において、出力部３０によって、上記ステップＳ１０４で算出された自車両の運動量を結果として出力する。

ステップＳ１１８において、出力部３０によって、上記ステップＳ１０６で推定された自車両の運動量を結果として出力する。

そして、ステップＳ１２０において、時刻ｔを１インクリメントして、ステップＳ１００へ戻る。

走行環境認識装置３２は、測距センサ１６によって計測された自車両周辺に存在する物体上の複数の点の各々の自車両を基準とする位置と、上記ステップＳ１１６又は上記ステップＳ１１８で結果として出力された自車両の運動量と基づいて、当該自車両周辺に存在する物体が移動しているか又は静止しているかを認識する。

以上説明したように、第１の実施の形態に係る運動量推定装置によれば、車両周辺に存在する物体上の複数の点の各々の車両を基準とする位置を各時刻ｔについて取得し、取得された時刻ｔの点に対応する位置の各々と、過去に取得された点に対応する位置の各々とに基づいて、時刻ｔにおける車両の運動量を推定し、推定された車両の運動量に基づいて、車両の運動量の試行量を複数生成し、生成された複数の試行量の各々について、時刻ｔの点に対応する位置の各々に対して試行量を用いて座標変換して得られる点の位置の各々と、測距センサによって過去に計測された点に対応する位置の各々とを比較して、試行量の確からしさを表すスコアを算出し、複数の試行量の各々について算出されたスコアと、複数の試行量とに基づいて、時刻ｔにおける車両の運動量の推定の不良地点らしさを算出することにより、車両の運動量の推定の不良地点を検出することができる。

また、車両の運動状態を各時刻ｔについて取得し、取得された時刻ｔの車両の運動状態に基づいて、時刻ｔにおける車両の運動量を算出し、推定された時刻ｔにおける車両の運動量の推定の不良地点らしさが予め定められた閾値以上の場合に、時刻ｔの車両の運動状態に基づいて算出された車両の運動量を出力し、推定された時刻ｔにおける前記移動体の運動量の推定の不良地点らしさが予め定められた閾値未満の場合に、測距センサによって取得された測距情報に基づいて推定された車両の運動量を出力することにより、車両の運動量を精度良く推定することができる。

また、高速道路のトンネルやガードレール等の単調な走行シーンにて、車両の運動量推定の劣化を防ぐことができる。

また、測距センサによる車両の運動量推定の分散値を常時監視することにより、不良推定状態を判定することが可能となり、適切なタイミングで車両ＣＡＮ情報に切り替えることができる。これにより、高速道路でロバストな静止物と移動物の判別ができ、誤認識を低減することが可能となる。

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

第２の実施の形態では、視線誘導標の位置を検出し、検出された視線誘導標の位置に基づいて、自車両の運動量を推定する点が第１の実施の形態と異なっている。

図７に示すように、第２の実施の形態に係る運動量推定装置２１０は、自車両の速度Ｖ［ｍ／ｓ］を逐次検出する車速センサ１２と、自車両のヨーレートω［ｒａｄ／ｓ］を逐次検出するヨーレートセンサ１４と、自車両周辺に存在する物体上の複数の点の各々の自車両を基準とする位置を逐次計測する測距センサ１６と、自車両の運動量を推定するコンピュータ２１８と、推定された自車両の運動量に基づいて、走行環境を認識する走行環境認識装置３２とを備えている。

コンピュータ２１８は、車両情報取得部２０と、測距情報取得部２２と、車両運動量変換部２４と、測距情報取得部２２によって取得された測距情報に基づいて、視線誘導標候補を抽出する視線誘導標候補抽出部２２５と、視線誘導標候補抽出部２２５によって抽出された視線誘導標候補に基づいて、自車両の運動量を推定する時系列照合部２２６と、不良推定地点判定部２８と、出力部３０とを備えている。

測距情報取得部２２は、測距センサ１６によって計測された自車両周辺に存在する物体上の複数の点の各々に対応する位置を各時刻ｔについて取得すると共に、測距センサ１６によって出力された反射輝度画像を各時刻ｔについて取得する。
視線誘導標候補抽出部２２５は、測距情報取得部２２によって取得された時刻ｔの点に対応する位置の各々及び反射輝度画像に基づいて、自車両周辺に存在する複数の視線誘導標各々の自車両を基準とする位置を各時刻ｔについて取得する。

具体的には、視線誘導標候補抽出部２２５は、測距情報取得部２２によって取得された反射強度画像から、局所的に反射強度値が高くなる点を視線誘導標候補点として抽出する。例えば、視線誘導標候補抽出部２２５は、反射強度画像を１次元ラプラシアンオペレータにより水平方向にスキャンし、スキャンによって得られた値が設定閾値以上となる点を視線誘導標候補として抽出する。図８に、反射強度画像を１次元ラプラシアンオペレータによって水平方向にスキャンする場合の一例を示す。また、図９に、反射強度画像から抽出された視線誘導標候補の一例を示す。

また、視線誘導標候補抽出部２２５は、反射強度画像の同一スキャンライン上の視線誘導標を組み合わせて対を生成する。そして、視線誘導標候補抽出部２２５は、対となる視線誘導標の間隔が車幅程度（例えば、１．４８〜２．５ｍ）であって、かつ対となる視線誘導標が同一距離にあり、図１０に示すように、当該視線誘導標の間に視線誘導標候補と同一距離の立体物がある場合は、車両ランプ部を誤検出していると判定して、当該視線誘導標候補を除外する。
そして、視線誘導標候補抽出部２２５は、測距情報取得部２２によって取得された時刻ｔの点に対応する位置の各々から、時刻ｔの視線誘導標に対応する位置の各々を取得する。

時系列照合部２２６は、視線誘導標候補抽出部２２５によって取得された時刻ｔの視線誘導標に対応する位置の各々と、過去に取得された視線誘導標に対応する位置の各々とに基づいて、時刻ｔにおける自車両の運動量を推定する。

具体的には、時系列照合部２２６は、図１１に示すように、視線誘導標候補抽出部２２５によって抽出された視線誘導標候補の測距値から車両座標系の座標値を計算する。時系列照合部２２６は、現在の時刻ｔにおける視線誘導標候補と、１フレーム前の時刻ｔ−１における視線誘導標候補を既知のＲＡＮＳＡＣ法により対応づける。すなわち、時系列照合部２２６は、視線誘導標の位置は対地に固定と仮定し、自車両の運動量Δｘ，Δｚ，Δθを既知のＲＡＮＳＡＣ法で推定する。

＜運動量推定装置の２１０の動作＞
次に、第２の実施の形態に係る運動量推定装置２１０の動作について説明する。まず、自車両のドライバの運転操作により車両が走行し、車速センサ１２によって自車両の速度が逐次検出され、ヨーレートセンサ１４によって自車両のヨーレートが逐次検出され、測距センサ１６によって自車両周辺に存在する物体上の複数の点の各々の自車両を基準とする位置が逐次計測され、反射輝度画像が逐次出力されているときに、コンピュータ２１８において、図１２に示す運動量推定処理ルーチンが実行される。なお、第１の実施の形態と同様の処理については、同一符号を付して説明を省略する。

ステップＳ１０２において、測距情報取得部２２によって、測距センサ１６によって計測された自車両周辺に存在する物体上の複数の点の各々に対応する位置、及び測距センサ１６によって出力された反射輝度画像を受け付ける。
ステップＳ２０５において、視線誘導標候補抽出部２２５によって、上記ステップＳ１０２で取得された時刻ｔの点に対応する位置の各々及び反射輝度画像に基づいて、自車両周辺に存在する複数の視線誘導標各々の自車両を基準とする位置を各時刻ｔについて取得する。

ステップＳ２０６において、時系列照合部２２６によって、上記ステップＳ２０５で取得された時刻ｔの視線誘導標に対応する位置の各々と、過去に取得された視線誘導標に対応する位置の各々とに基づいて、時刻ｔにおける自車両の運動量を推定する。

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

以上説明したように、第２の実施の形態に係る車両挙動予測装置によれば、車両周辺に存在する複数の視線誘導標各々の車両を基準とする位置を各時刻ｔについて取得し、取得された時刻ｔの視線誘導標に対応する位置の各々と、過去に取得された視線誘導標に対応する位置の各々とに基づいて、時刻ｔにおける車両の運動量を推定することにより、車両の運動量を精度良く推定することができる。

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

第３の実施の形態では、各センサによって検出された自車両の運動状態から算出された車両の運動量と、測距センサ１６によって検出された測距情報から推定された自車両の運動量とを組み合わせて自車両の運動量を推定する点が、第１の実施の形態と異なっている。

第３の実施の形態では、車速センサ１２によって検出された自車両の速度、及びヨーレートセンサ１４によって検出されたヨーレートから算出された自車両の運動量と、測距センサ１６によって検出された測距情報から推定された自車両の運動量とを、カルマンフィルタを用いて組み合わせて算出する場合を例に説明する。

具体的には、算出した重み付き共分散行列Ｖ＾に基づいて、各センサによって検出された自車両の速度及びヨーレートから算出された自車両の運動量と、測距センサ１６によって検出された測距情報から推定された自車両の運動量とを、既知のカルマンフィルタよって融合する。以下、図面を参照して、第３の実施の形態について説明する。

図１３に示すように、第３の実施の形態に係る運動量推定装置３１０は、自車両の速度Ｖ［ｍ／ｓ］を逐次検出する車速センサ１２と、自車両のヨーレートω［ｒａｄ／ｓ］を逐次検出するヨーレートセンサ１４と、自車両周辺に存在する物体上の複数の点の各々の自車両を基準とする位置を逐次計測する測距センサ１６と、自車両の運動量を推定するコンピュータ３１８と、推定された自車両の運動量に基づいて、走行環境を認識する走行環境認識装置３２とを備えている。

コンピュータ３１８は、車両情報取得部２０と、測距情報取得部２２と、車両運動量変換部２４と、時系列照合部２６と、時系列照合部２６によって推定された自車両の運動量に基づいて、重み付き共分散行列を算出する分散行列算出部３２７と、車両運動量変換部２４によって算出された自車両の運動量と、時系列照合部２６によって推定された自車両の運動量と、分散行列算出部３２７によって算出された重み付き共分散行列とに基づいて、自車両の運動量を推定する状態推定部３２８と、出力部３０とを備えている。

分散行列算出部３２７は、第１の実施の形態の不良推定地点判定部２８と同様に、時系列照合部２６によって推定された自車両の運動量に基づいて、自車両の運動量の試行量を複数生成する。

次に、分散行列算出部３２７は、不良推定地点判定部２８と同様に、生成された複数の試行量の各々について、現時刻ｔの点に対応する位置の各々に対して試行量を用いて座標変換して得られる点の位置の各々と、測距センサ１６によって過去に計測された点に対応する位置の各々とを比較して、当該試行量の確からしさを表すスコアを算出する。

そして、分散行列算出部３２７は、不良推定地点判定部２８と同様に、複数の試行量の各々について算出されたスコアと、複数の試行量とに基づいて、上記式（２）に示す重み付き共分散行列Ｖ＾を算出する。

状態推定部３２８は、分散行列算出部３２７によって算出された重み付き共分散行列Ｖ＾に基づいて、カルマンフィルタで用いられる観測雑音の共分散を決定し、決定された観測雑音の共分散に基づいて、カルマンゲインを算出する。

次に、状態推定部３２８は、車両運動量変換部２４によって推定された自車両の運動量、及び時系列照合部２６によって算出された自車両の運動量を観測値として、算出されたカルマンゲインを用いたカルマンフィルタに従って、時刻ｔにおける自車両の運動量を推定する。そして、状態推定部３２８は、推定された時刻ｔにおける自車両の運動量を出力する。

具体的には、状態推定部３２８は、車両運動量変換部２４によって算出された自車両の運動量と、時系列照合部２６によって推定された自車両の運動量とから、カルマンフィルタによって自車両の運動量を推定する。

カルマンフィルタで使用する運動モデルの一例を以下に示す。

このとき、ｘ＾は状態ベクトル、Ｆ＾は更新行列、ｗ＾は共分散行列がＱ＾で表されるシステムノイズ項である。
また、状態ベクトルｘ＾は次式で表される。

状態ベクトルｘ＾の各要素は、自車両の運動量Δｘ（横方向移動量）、Δｚ（進行方向移動量）、Δθ（方位角方向移動量）と、それらの時間に対する１次微分項Δｘ^・，Δｚ^・，Δθ^・である。ここでは、単純な質点モデルと見なして、更新行列Ｆ＾を次式のように設定した。もし、車両運動モデルが既知であれば、それを適用することにより、より推定精度を改善することができる。Ｔは更新時間［ｓｅｃ］である。

また、観測モデルは以下の式で表される。このとき、ｚ＾は観測値ベクトル、Ｈ＾は観測行列である。ｖ＾は観測ノイズである。

このとき、観測値ベクトルｚ＾は、車両運動量変換部２４によって算出された自車両の運動量Δｘ_Ｃ、Δｚ_Ｃ、Δθ_Ｃと、時系列照合部２６によって推定された自車両の運動量Δｘ_Ｌ、Δｚ_Ｌ、Δθ_Ｌとを要素とするベクトルである。

このとき、観測行列Ｈ＾は次式で表される。

また、観測ノイズｖ＾の分散行列Ｒ＾は次式で設定する。

このとき、Ｖ＾_ｃａｒ及びＶ＾_{ｌｉｄａｒ}は３行ｘ３列の行列となる。０＾は３行ｘ３列の零行列である。Ｖ＾_{ｌｉｄａｒ}には分散行列算出部３２７によって算出された重み付き共分散行列Ｖ＾を代入すれば良い。また、Ｖ＾_ｃａｒは、車両運動量変換部２４で取得された計測周期時間Ｔ当たりの自車両の運動量Δｘ［ｍ］、Δｚ［ｍ］、Δθ［ｒａｄ］の実測値から、実測値の分散を算出して設定する。

そして、下記のようなカルマンフィルタの既知の更新式により、車両運動量変換部２４によって算出された自車両の運動量Δｘ_Ｃ、Δｚ_Ｃ、Δθ_Ｃと、時系列照合部２６によって推定された自車両の運動量Δｘ_Ｌ、Δｚ_Ｌ、Δθ_Ｌとの２つの自車両の運動量を観測値ｚ＾として、Ｖ＾_ｃａｒとＶ＾_{ｌｉｄａｒ}との重みを考慮して融合し、逐次更新することにより、時々刻々の自車両の運動量の推定値ｘ＾が得られる。第３の実施の形態では、ｋが時刻に対応する。また、Ｐ＾は誤差共分散行列、Ｋ＾はカルマンゲインである。Ｐ＾の初期値としては、例えば、観測値ｚ＾の初期値のばらつきの範囲を考慮して、比較的大きめの値を設定しておく。

ここで、添え字「ｐ，ｋ」が付与された値は、時刻ｋ−１までに利用可能なデータに基づき予測された時刻ｋの事前推定値を表す。また、添え字「ｋ」が付与された値は、時刻ｋまでに利用可能なデータに基づき推定された時刻ｋの事後推定値を表す。

カルマンフィルタによる推定処理は、予測ステップとフィルタリングステップとを含んで構成される。

（１）予測ステップ
まず、予測ステップにおける処理について説明する。状態推定部３２８は、予測ステップにおいて、前回のフィルタリングステップで算出された時刻ｋ−１の状態ベクトルｘ＾_ｋ−１と、更新行列Ｆ＾とに基づいて、上記式（１０）に従って、時刻ｋの状態ベクトルｘ＾_ｐ，ｋを算出する。

そして、状態推定部３２８は、更新行列Ｆ＾と、前回のフィルタリングステップで推定された共分散行列Ｐ＾_ｋ−１と、システムノイズｗ＾の共分散行列Ｑ＾とに基づいて、上記式（１１）に従って、事前誤差共分散行列Ｐ＾_ｐ，ｋを算出する。

（２）フィルタリングステップ
次に、フィルタリングステップにおける処理について説明する。状態推定部３２８は、分散行列算出部３２７によって算出された重み付き共分散行列Ｖ＾を、観測ノイズｖ＾の分散行列Ｒ＾のＶ＾_{ｌｉｄａｒ}に代入し、観測ノイズｖ＾の分散行列Ｒ＾を決定する。

そして、状態推定部３２８は、フィルタリングステップにおいて、予測ステップで算出された事前誤差共分散行列Ｐ＾_ｐ，ｋと、観測行列Ｈ＾と、決定された観測ノイズｖ＾の分散行列Ｒ＾とに基づいて、上記式（１２）に従って、カルマンゲインＫ＾を算出する。

次に、状態推定部３２８は、算出されたカルマンゲインＫ＾と、予測ステップで算出された時刻ｋにおける状態ベクトルｘ＾_ｐ，ｋと、上記式（７）で示したｚ＾＝［Δｘ_Ｃ，Δｚ_Ｃ，Δθ_Ｃ，Δｘ_Ｌ，Δｚ_Ｌ，Δθ_Ｌ］と、観測行列Ｈ＾とに基づいて、上記式（１３）に従って、時刻ｋにおける状態ベクトルｘ＾_ｋを推定する。

そして、状態推定部３２８は、前回のフィルタリングステップで推定された共分散行列Ｐ＾_ｋ−１と、算出されたカルマンゲインＫ＾と、観測行列Ｈ＾とに基づいて、上記式（１４）に従って、事後誤差共分散行列Ｐ＾_ｋを算出する。

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

また、状態推定部３２８は、推定された時刻ｋにおける状態ベクトルｘ＾_ｋのうち、自車両の運動量［Δｘ，Δｚ，Δθ］を出力する。

出力部３０は、状態推定部３２８によって出力された自車両の運動量を出力する。

走行環境認識装置３３２は、測距センサ１６によって計測された自車両周辺に存在する物体上の複数の点の各々の自車両を基準とする位置と、出力部３０によって出力された自車両の運動量と基づいて、当該自車両周辺に存在する物体が移動しているか又は静止しているかを認識する。具体的には、走行環境認識装置３２は、出力部３０によって出力された車両の運動量をキャンセルすることによって、当該自車両周辺に存在する物体が移動しているか又は静止しているかを認識する。

＜運動量推定装置の３１０の動作＞
次に、第３の実施の形態に係る運動量推定装置３１０の動作について説明する。まず、自車両のドライバの運転操作により車両が走行し、車速センサ１２によって自車両の速度が逐次検出され、ヨーレートセンサ１４によって自車両のヨーレートが逐次検出され、測距センサ１６によって自車両周辺に存在する物体上の複数の点の各々の自車両を基準とする位置が逐次計測されているときに、コンピュータ３１８において、図１４に示す運動量推定処理ルーチンが実行される。

ステップＳ３０８において、分散行列算出部３２７によって、上記ステップＳ１０６で推定された自車両の運動量に基づいて、自車両の運動量を含む所定の範囲から、サンプリングによって、自車両の運動量の試行量ｘ_ｉ＾を複数生成する。

ステップＳ３１０において、分散行列算出部３２７によって、上記ステップＳ３０８で生成された複数の試行量ｘ_ｉ＾の各々について、現時刻ｔの点に対応する位置の各々に対して試行量を用いて座標変換して得られる点の位置の各々と、測距センサ１６によって過去に計測された点に対応する位置の各々とを比較して、上記式（１）に従って、試行量の確からしさを表すスコアｐ_ｉを算出する。

ステップＳ３１２において、分散行列算出部３２７によって、複数の試行量ｘ_ｉ＾の各々について上記ステップＳ３１０で算出されたスコアｐ_ｉと、複数の試行量ｘ_ｉ＾とに基づいて、上記式（２）に示すように、重み付き共分散行列Ｖ＾を算出する。

ステップＳ３１４において、状態推定部３２８によって、上記ステップＳ３１２で算出された重み付き共分散行列に基づいて、カルマンフィルタで用いられる観測雑音の共分散を決定し、決定された観測雑音の共分散に基づいて、カルマンゲインを算出する。そして、状態推定部３２８によって、上記ステップＳ１０４で算出された自車両の運動量、及び上記ステップＳ１０６で推定された自車両の運動量を観測値として、算出されたカルマンゲインを用いたカルマンフィルタに従って、時刻ｋにおける自車両の運動量を推定する。ステップＳ３１４は、図１５に示す状態推定処理ルーチンによって実現される。

＜状態推定処理ルーチン＞
まず、ステップＳ４００において、状態推定部３２８によって、上記ステップＳ３１２で算出された重み付き共分散行列Ｖ＾を取得する。

ステップＳ４０２において、状態推定部３２８によって、前回のステップＳ４１０で算出された時刻ｋ−１の状態ベクトルｘ＾_ｋ−１と、更新行列Ｆ＾とに基づいて、上記式（１０）に従って、時刻ｋの状態ベクトルｘ＾_ｐ，ｋを算出する。

ステップＳ４０４において、状態推定部３２８によって、更新行列Ｆ＾と、前回のステップＳ４１２で推定された共分散行列Ｐ＾_ｋ−１と、システムノイズｗ＾の共分散行列Ｑ＾とに基づいて、上記式（１１）に従って、事前誤差共分散行列Ｐ＾_ｐ，ｋを算出する。

ステップＳ４０６において、状態推定部３２８によって、分散行列算出部３２７によって算出された重み付き共分散行列Ｖ＾を、観測ノイズｖ＾の分散行列Ｒ＾のＶ＾_{ｌｉｄａｒ}に代入し、観測ノイズｖ＾の分散行列Ｒ＾を決定する。

ステップＳ４０８において、状態推定部３２８によって、上記ステップＳ４０４で算出された事前誤差共分散行列Ｐ＾_ｐ，ｋと、観測行列Ｈ＾と、上記ステップＳ４０６で決定された観測ノイズｖ＾の分散行列Ｒ＾とに基づいて、上記式（１２）に従って、カルマンゲインＫ＾を算出する。

ステップＳ４１０において、状態推定部３２８によって、上記ステップＳ４０８で算出されたカルマンゲインＫ＾と、上記ステップＳ４０２で算出された時刻ｋにおける状態ベクトルｘ＾_ｐ，ｋと、上記式（７）で示したｚ＾＝［Δｘ_Ｃ，Δｚ_Ｃ，Δθ_Ｃ，Δｘ_Ｌ，Δｚ_Ｌ，Δθ_Ｌ］と、観測行列Ｈ＾とに基づいて、上記式（１３）に従って、時刻ｋにおける状態ベクトルｘ＾_ｋを推定する。

ステップＳ４１２において、状態推定部３２８によって、前回のステップＳ４１２で算出されたフィルタリングステップで推定された共分散行列Ｐ＾_ｋ−１と、上記ステップＳ４０８で算出されたカルマンゲインＫ＾と、観測行列Ｈ＾とに基づいて、上記式（１４）に従って、事後誤差共分散行列Ｐ＾_ｋを算出する。

ステップＳ４１４において、上記ステップＳ４１０で算出された時刻ｋにおける状態ベクトルｘ＾_ｋのうち、自車両の運動量［Δｘ，Δｚ，Δθ］を結果として出力して、状態推定処理ルーチンを終了する。

次に、運動量推定処理ルーチンに戻り、ステップＳ３１６において、出力部３０によって、上記ステップＳ３１４で出力された自車両の運動量を結果として出力して、ステップＳ３１８において、時刻ｋを１インクリメントして、ステップＳ１００へ戻る。

走行環境認識装置３３２は、測距センサ１６によって計測された自車両周辺に存在する物体上の複数の点の各々の自車両を基準とする位置と、上記ステップＳ３１６で結果として出力された自車両の運動量と基づいて、当該自車両周辺に存在する物体が移動しているか又は静止しているかを認識する。

なお、第３の実施の形態に係る運動量推定装置の他の構成及び作用については、第１の実施の形態と同様であるため、説明を省略する。

以上説明したように、第３の実施の形態に係る運動量推定装置３１０によれば、時系列照合部２６によって推定された車両の運動量に基づいて、車両の運動量の試行量を複数生成し、生成された複数の試行量の各々について、時刻ｋの点に対応する位置の各々に対して試行量を用いて座標変換して得られる点の位置の各々と、測距センサ１６によって過去に計測された点に対応する位置の各々とを比較して、試行量の確からしさを表すスコアを算出し、複数の試行量の各々について算出されたスコアと、複数の試行量とに基づいて、複数の試行量の重み付き共分散行列を算出し、算出された重み付き共分散行列に基づいて、カルマンフィルタで用いられる観測雑音の共分散を決定し、決定された観測雑音の共分散に基づいて、カルマンゲインを算出し、時系列照合部２６によって推定された車両の運動量、及び車両運動量変換部２４によって算出された車両の運動量を観測値として、算出されたカルマンゲインを用いたカルマンフィルタに従って、時刻ｋにおける車両の運動量を推定することにより、車両の運動量を精度良く推定することができる。

カルマンフィルタに従って、時刻ｋにおける車両の運動量を推定することにより、車両運動量変換部２４によって算出された車両の運動量と、時系列照合部２６によって推定された車両の運動量とについて、陽に切換判定をする必要がなくなる。

なお、上記の実施の形態では、移動体として車両を対象とする場合を例に説明したが、これに限定されるものではなく、他の移動体を対象としてもよい。

また、上記の実施の形態では、測距センサとして、レーザレーダを用いる場合を例に説明したが、これに限定されるものではなく、例えば車載したステレオカメラを用いても良い。この場合には、ステレオカメラによって撮影されたステレオ画像に基づいて、自車両周辺に存在する物体上の複数の点の各々に対応する位置を算出すればよい。

また、上記の実施の形態では、車両の運動状態として、車速センサ１２によって検出された自車両の速度と、ヨーレートセンサ１４によって検出された自車両のヨーレートとを、取得する場合を例に説明したが、これに限定されるものではなく、他のセンサによって得られた車両の運動状態を取得してもよい。例えば、ハンドル角センサによって、ハンドルの操舵角を車両の運動状態として取得してもよい。

また、上記の実施の形態では、車両の運動量を推定する運動量推定装置に本発明を適用させた場合を例に説明したが、これに限定されるものではなく、時刻ｔにおける車両の運動量の推定の不良地点らしさのみを算出する装置に本発明を適用させても良い。

また、上記の第１及び第２の実施の形態における不良推定地点判定部２８は、上記式（２）の重み付き共分散行列Ｖ＾のΣ_ｉ（ｐ_ｉΔｚ_ｉ ^２）の項に着目し、Σ_ｉ（ｐ_ｉΔｚ_ｉ ^２）の値を自車両の運動量の推定の不良地点らしさとする場合を例に説明したが、これに限定されるものではなく、上記式（２）の重み付き共分散行列Ｖ＾の他の要素の値を自車両の運動量の推定の不良地点らしさとしてもよい。

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

１０，２１０，３１０ 運動量推定装置
１２ 車速センサ
１４ ヨーレートセンサ
１６ 測距センサ
１８，２１８，３１８ コンピュータ
２０ 車両情報取得部
２２ 測距情報取得部
２４ 車両運動量変換部
２６ 時系列照合部
２８ 不良推定地点判定部
３０ 出力部
３２，３３２ 走行環境認識装置
２２５ 視線誘導標候補抽出部
２２６ 時系列照合部
３２７ 分散行列算出部
３２８ 状態推定部

Claims (6)

1. 移動体の周辺に存在する物体上の複数の点の各々の前記移動体を基準とする位置であって、かつ前記移動体に搭載された計測手段によって計測された前記位置を各時刻ｔについて取得する取得手段と、
   前記取得手段によって取得された時刻ｔの前記点に対応する位置の各々と、過去に取得された前記点に対応する位置の各々とに基づいて、時刻ｔにおける前記移動体の運動量を推定する運動量推定手段と、
   前記運動量推定手段によって推定された前記移動体の運動量に基づいて、前記移動体の運動量の試行量を複数生成し、生成された前記複数の試行量の各々について、前記時刻ｔの前記点に対応する位置の各々に対して前記試行量を用いて座標変換して得られる点の位置の各々と、前記計測手段によって過去に計測された前記点に対応する位置の各々とを比較して、前記試行量の確からしさを表すスコアを算出し、前記複数の試行量の各々について算出された前記スコアと、前記複数の試行量とに基づいて、時刻ｔにおける前記移動体の運動量の推定の不良地点らしさを算出する不良地点らしさ算出手段と、
   を含む運動量推定装置。
2. 運動量算出手段、及び出力手段を更に含み、
   前記取得手段は、前記移動体の運動状態を各時刻ｔについて更に取得し、
   前記運動量算出手段は、前記取得手段によって取得された時刻ｔの前記移動体の運動状態に基づいて、時刻ｔにおける前記移動体の運動量を算出し、
   前記出力手段は、推定された時刻ｔにおける前記移動体の運動量の推定の不良地点らしさが予め定められた閾値以上の場合に、前記運動量算出手段によって算出された前記移動体の運動量を出力し、推定された時刻ｔにおける前記移動体の運動量の推定の不良地点らしさが予め定められた閾値未満の場合に、前記運動量推定手段によって推定された前記移動体の運動量を出力する
   請求項１記載の運動量推定装置。
3. 移動体の周辺に存在する物体上の複数の点の各々の前記移動体を基準とする位置であって、かつ前記移動体に搭載された計測手段によって計測された前記位置と、前記移動体の運動状態をとを各時刻ｔについて取得する取得手段と、
   前記取得手段によって取得された時刻ｔの前記点に対応する位置の各々と、過去に取得された前記点に対応する位置の各々とに基づいて、時刻ｔにおける前記移動体の運動量を推定する運動量推定手段と、
   前記取得手段によって取得された時刻ｔの前記移動体の運動状態に基づいて、時刻ｔにおける前記移動体の運動量を算出する運動量算出手段と、
   前記運動量推定手段によって推定された前記移動体の運動量に基づいて、前記移動体の運動量の試行量を複数生成し、生成された前記複数の試行量の各々について、前記時刻ｔの前記点に対応する位置の各々に対して前記試行量を用いて座標変換して得られる点の位置の各々と、前記計測手段によって過去に計測された前記点に対応する位置の各々とを比較して、前記試行量の確からしさを表すスコアを算出し、前記複数の試行量の各々について算出された前記スコアと、前記複数の試行量とに基づいて、前記移動体の運動量の重み付き共分散行列を算出する分散行列算出手段と、
   前記分散行列算出手段によって算出された重み付き共分散行列に基づいて、カルマンフィルタで用いられる観測雑音の共分散を決定し、決定された前記観測雑音の共分散に基づいて、カルマンゲインを算出し、前記運動量推定手段によって推定された前記移動体の運動量、及び前記運動量算出手段によって算出された前記移動体の運動量を観測値として、算出された前記カルマンゲインを用いたカルマンフィルタに従って、時刻ｔにおける前記移動体の運動量を推定する状態推定手段と、
   を含む運動量推定装置。
4. 前記不良地点らしさ算出手段は、前記運動量推定手段によって推定された前記移動体の運動量に基づいて、前記移動体の運動量を含む所定の範囲から、サンプリングによって、前記移動体の運動量の試行量を複数生成する
   請求項１又は請求項２記載の運動量推定装置。
5. 前記取得手段によって取得された時刻ｔの前記点に対応する位置の各々に基づいて、移動体の周辺に存在する複数の視線誘導標の各々の前記移動体を基準とする位置を各時刻ｔについて取得する視線誘導標候補抽出手段を更に含み、
   前記運動量推定手段は、前記視線誘導標候補抽出手段によって取得された時刻ｔの前記視線誘導標に対応する位置の各々と、過去に取得された、前記視線誘導標に対応する位置の各々とに基づいて、時刻ｔにおける前記移動体の運動量を推定する
   請求項１〜請求項４の何れか１項記載の運動量推定装置。
6. コンピュータに、
   移動体の周辺に存在する物体上の複数の点の各々の前記移動体を基準とする位置であって、かつ前記移動体に搭載された計測手段によって計測された前記位置を各時刻ｔについて取得する取得手段、
   前記取得手段によって取得された時刻ｔの前記点に対応する位置の各々と、過去に取得された前記点に対応する位置の各々とに基づいて、時刻ｔにおける前記移動体の運動量を推定する運動量推定手段、及び
   前記運動量推定手段によって推定された前記移動体の運動量に基づいて、前記移動体の運動量の試行量を複数生成し、生成された前記複数の試行量の各々について、前記時刻ｔの前記点に対応する位置の各々に対して前記試行量を用いて座標変換して得られる点の位置の各々と、前記計測手段によって過去に計測された前記点に対応する位置の各々とを比較して、前記試行量の確からしさを表すスコアを算出し、前記複数の試行量の各々について算出された前記スコアと、前記複数の試行量とに基づいて、時刻ｔにおける前記移動体の運動量の推定の不良地点らしさを算出する不良地点らしさ算出手段
   として機能させるためのプログラム。