JP6770393B2 - トラッキング装置及びプログラム - Google Patents

Description

本発明は、トラッキング装置及びプログラムに関する。

従来より、車両前方の道路風景を撮像するＣＣＤカメラと、ＣＣＤカメラからの映像信号に対して画面全体に一様な処理を施す前処理部と、入力画面上にレーンマーカを検出するための複数の小領域を設定するレーンマーカ検出用小領域設定部と、複数の小領域に対してレーンマーカの一部を検出する直線検出部と、直線検出部による直線検出結果がレーンマーカの一部であることを検証するレーンマーカ候補点検出部と、レーンマーカの検出結果に基づいて車両前方の道路形状を表すための道路モデルパラメータを算出する道路モデルパラメータ算出部と、を有する走行路認識装置が知られている（特許文献１）。この走行路認識装置の道路モデルパラメータ算出部は、時間的および空間的に連続性を有し、パラメータ決定に確率的な合理性のある拡張カルマンフィルタを含む。

また、車両に搭載され、車両に関する観測を行い観測量を出力する複数の観測手段と、複数の観測手段により出力される観測量の信頼度を、観測量毎に算出する信頼度算出手段と、観測手段により出力された観測量、及び信頼度算出手段により算出された観測量の信頼度を推定モデルに入力して車両の状態量を推定する推定手段と、を備える車両状態量推定装置が知られている（特許文献２）。この車両状態量推定装置では、複数の観測手段には、同一種類の観測量を複数出力可能な観測手段が含まれ、信頼度算出手段は、同一の観測手段により出力された同一種類の複数の観測量を含めた複数の観測量について信頼度を算出し、推定手段は、同一の観測手段により出力された同一種類の複数の観測量のそれぞれを前記推定モデルに入力して車両の状態量を推定する。

特開2002-109695号公報 特開2013-129289号公報

しかしながら、カルマンフィルタで使用する時間更新モデルの制御入力項（上記の従来技術では舵角である。）に誤差が加わると、従来のカルマンフィルタでは推定値に定常誤差が発生するという問題があった。

これは、従来のカルマンフィルタでは、システムノイズと観測ノイズしか考慮されていないため、時間更新モデルの制御入力項の誤差が考慮できていなかったためである。言い換えると、時間更新モデルの制御入力項には誤差がないという前提でカルマンフィルタが組まれていたので、制御入力項に誤差がのった場合、それが推定誤差に影響していた。

本発明は、上記の事情に鑑みてなされたもので、センサ出力に誤差がある場合でも、カルマンフィルタにより推定対象の状態の推定値を精度良く求めることができるトラッキング装置及びプログラムを提供することを目的とする。

上記の目的を達成するために本発明に係るトラッキング装置は、センサ観測値に基づいて推定対象の状態の推定値をカルマンフィルタで求めるトラッキング装置であって、前記センサ観測値とは異なるセンサの出力であって、状態の時間更新の入力となる前記センサの出力についての予め定められた変動に対応する補正分をシステムノイズに加算した前記カルマンフィルタを用いて、前記センサ観測値と前記センサの出力とから、前記状態の推定値を求める推定部を含んで構成されている。

本発明に係るプログラムは、センサ観測値に基づいて推定対象の状態の推定値をカルマンフィルタで求めるためのプログラムであって、コンピュータを、前記センサ観測値とは異なるセンサの出力であって、状態の時間更新の入力となる前記センサの出力についての予め定められた変動に対応する補正分をシステムノイズに加算した前記カルマンフィルタを用いて、前記センサ観測値と前記センサの出力とから、前記状態の推定値を求める推定部として機能させるためのプログラムである。

本発明によれば、推定部は、前記センサ観測値とは異なるセンサの出力であって、状態の時間更新の入力となる前記センサの出力についての予め定められた変動に対応する補正分をシステムノイズに加算した前記カルマンフィルタを用いて、前記センサ観測値と前記センサの出力とから、前記状態の推定値を求める。

このように、状態の時間更新の入力となる前記センサの出力についての変動に対応する補正分をシステムノイズに加算した前記カルマンフィルタを用いることにより、センサ出力に誤差がある場合でも、カルマンフィルタにより推定対象の状態の推定値を精度良く求めることができる。

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

以上説明したように、本発明のトラッキング装置及びプログラムによれば、状態の時間更新の入力となる前記センサの出力についての変動に対応する補正分をシステムノイズに加算した前記カルマンフィルタを用いることにより、センサ出力に誤差がある場合でも、カルマンフィルタにより推定対象の状態の推定値を精度良く求めることができる、という効果が得られる。

本発明の第１の実施の形態に係る走路認識装置を示すブロック図である。 本発明の第１の実施の形態に係る走路認識装置における走路認識処理ルーチンの内容を示すフローチャートである。 本発明の第１の実施の形態に係る走路認識装置における走路パラメータ推定処理ルーチンの内容を示すフローチャートである。 本発明の第２の実施の形態に係る歩行者検出装置を示すブロック図である。 歩行者の位置を説明するための図である。 本発明の第２の実施の形態に係る歩行者検出装置における歩行者検出処理ルーチンの内容を示すフローチャートである。 本発明の第２の実施の形態に係る歩行者検出装置における歩行者パラメータ推定処理ルーチンの内容を示すフローチャートである。 従来手法と本実施の形態に係る手法との比較を説明するための図である。 従来手法と本実施の形態に係る手法との比較を説明するための図である。 従来手法と本実施の形態に係る手法との比較を説明するための図である。

以下、図面を参照して、本発明の実施の形態について説明する。本発明の第１の実施の形態では、走路パラメータを推定する走路認識装置に本発明を適用した場合を例に説明する。

［第１の実施の形態］
＜走路認識装置のシステム構成＞
図１に示すように、第１の実施の形態に係る走路認識装置１０は、自車両の前方を撮像する撮像装置１２と、車速センサ１４と、ヨーレイトセンサ１６と、撮像装置１２によって撮像された自車両の前方の道路画像、車速センサ１４によって検出された自車両の車速、及びヨーレイトセンサ１６によって検出された自車両のヨーレイトに基づいて、走路パラメータを推定するコンピュータ１８とを備えている。

撮像装置１２は、自車両の前方の道路を撮像する車載カメラである。撮像装置１２は、例えば車両のリアビューミラー付近に設置され、自車両が走行する走路の画像の一例として、自車両の前方の道路画像を撮像する。

コンピュータ１８は、ＣＰＵと、ＲＡＭと、後述する走路パラメータ推定処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。コンピュータ１８は、拡張カルマンフィルタの時間更新式の入力項であるヨーレイトの誤差の補正分を算出する入力項補正分算出部３０と、算出された補正分をシステムノイズに加算するように設定するシステムノイズ設定部３２と、撮像装置１２によって撮像された道路画像に基づいて、自車両が走行する走行車線の境界を検出する車線境界検出部３４と、自車両が走行する走路に関する走路パラメータを推定する走路パラメータ推定部３６とを備えている。

車線境界検出部３４は、撮像装置１２によって撮像された道路画像に基づいて、自車両が走行する走行車線の境界として、白線の位置を検出する。

具体的には、車線境界検出部３４は、道路画像から走行車線の白線の候補点の位置の座標値を抽出する。例えば、自車両が走行中の車線の左又は右の白線を抽出する。

一般には、走路には白線レーンマークが敷設されているため、本実施の形態では、車線境界検出部３４は、白線レーンマークを走行車線の境界の候補として抽出する。白線レーンマークは、道路画像上では走路面に対して輝度が高いため、車線境界検出部３４は、例えば、道路画像を既知のＳｏｂｅｌフィルタ等によって処理し、得られた処理結果が閾値よりも大きい点を走行車線の白線の候補点として抽出する。

さらに、白線レーンマークは、局所的には進行方向に直線状に連続しているため、Ｈｏｕｇｈ変換等の手法により、直線状に連続している箇所を、車線の白線の候補として絞り込んでもよい。また、白線レーンマークの幅は局所的には一定なので、白線レーンマークの左右の車線の境界の候補対を照合して、車線の境界の候補点として選別してもよい。

走路パラメータ推定部３６は、検出された走行車線の境界の位置（走行車線の白線の候補点の座標値）に基づいて、走路パラメータを、拡張カルマンフィルタを用いて推定する。

ここで、走路パラメータを、拡張カルマンフィルタを用いて推定する原理について説明する。

まず、計算モデルについて説明する。

走路認識のモデルの状態ベクトルを次式に示す。

このとき、状態ベクトルの要素のうち、ｘ₀は自車線の横位置［ｍ］であり、θ₀はヨー角（車線の傾き）［ｒａｄ］であり、ｃ₀は、道路の曲率［１／ｍ］であり、ｃ₁は、道路の曲率変化率［１／ｍ²］であり、φはピッチ角［ｒａｄ］であり、ｗは車線幅［ｍ］である。

また、時刻ｔ−１の走路パラメータｘ_t-1から時刻ｔの走路パラメータｘ_tの時間更新モデルは次式で表現される。

このとき、Ｖは車速［ｍ／ｓ］であり、ωはヨーレイト［ｒａｄ／ｓ］であり、Ｔはカルマンフィルタの更新時間であり、ｕはＱ＝Ｅ［ｕｕ^T］となるシステムノイズであり、Ｆは状態ベクトルｘの時間更新行列であり、Ｂはヨーレイトωから状態ベクトルｘへの寄与項である。

走路認識では一般に路面に連続的にペイントされた白線を画像上で観測して、その白線上の点を画像上の座標値を観測値とする。よって、白線点ｊの観測関数ｈ_j（ｘ）は次式で示される。

このとき、Ｉ_jは道路画像上のｊ番目の白線点の垂直方向の座標値［画素］であり、Ｊ_jは、道路画像上のｊ番目の白線点の水平方向の座標値［画素］であり、Ｈ_cが路面平面からのカメラ高さ［ｍ］であり、ｆは焦点距離［ｍ］である。また、Ｉ_cは画像中心の垂直方向の座標値［画素］であり、Ｊ_cは、画像中心の水平方向の座標値［画素］であり、Ｒ_vは画像の垂直方向の解像度［ｍ／画素］であり、Ｒ_hは画像の水平方向の解像度［ｍ／画素］であり、ｓは画像上のｊ番目の白線点が右白線の場合＋１、左白線の場合−１であり、ｔは、画像上のｊ番目の白線点を路面平面上へ投影した場合の進行方向距離［ｍ］である。

次に、上述した計算モデルに基づくカルマンフィルタの更新式を以下に示す。

時間更新：

観測更新：

このとき、Ｐは、誤差共分散行列であり、Ｈは観測行列であり、Ｒは、観測ノイズの分散行列であり、Ｑは、システムノイズの分散行列であり、Ｋはカルマンゲインである。

次に、ヨーレイト誤差の影響低減方法について説明する。

上記式（２−２）の時間更新モデルでヨーレイト誤差Δωを考慮すると次式となる。

ヨーレイト誤差分を考慮して誤差共分散行列の時間更新モデルを見直すと、以下のように展開できる。このとき、ｘ、ω_s、Δω、ｕは無相関で、ｘ、ω_sの平均は０と仮定する。

誤差共分散行列の定義より、以下の式が得られる。

平均を表す関数Ｅ{}で書き直すと、以下の式が得られる。

カルマンフィルタによる時間更新式中では、ヨーレイトの計測誤差が入るので、以下の式が得られる。

一方、元の系ではヨーレイト誤差を含まないので、以下の式が得られる。

以上より、式（４−３）に式（４−４）と式（４−５）を代入して、以下の式が得られる。

式（４−６）を整理すると、以下の式が得られる。

このとき、Ｅ（Δω²）はヨーレイト誤差の二乗平均である。

以上より、以下の式が得られる。

よって、カルマンフィルタ更新式（２−８）を式（４−１０）へ置き換えることにより、ヨーレイト誤差分Δωをカルマンフィルタへ取り込み、定常偏差を低減できる。

以上説明した原理により、本実施の形態では、入力項補正分算出部３０は、拡張カルマンフィルタの時間更新式の入力項であるヨーレイトの誤差Δωの補正分Ｅ｛Δω²｝ＢＢ^Tを算出する。ただし、ヨーレイトの誤差Δωは、予め定められたものであり、例えば、ヨーレイトセンサ１６の分解能である。

システムノイズ設定部３２は、上記式（４−１０）に従って、算出された補正分Ｅ｛Δω²｝ＢＢ^TをシステムノイズＱに加算するように設定する。

走路パラメータ推定部３６は、検出された走行車線の境界の位置（走行車線の白線の候補点の座標値）に基づいて、システムノイズ設定部３２により設定された拡張カルマンフィルタにより走路パラメータの推定処理を行う。

拡張カルマンフィルタによる推定処理は、予測ステップとフィルタリングステップとを含んで構成される。以下、走路パラメータ推定部３６によって実行される拡張カルマンフィルタによる予測ステップとフィルタリングステップとについて説明する。

（１）予測ステップ
まず、予測ステップにおける処理について説明する。走路パラメータ推定部３６は、予測ステップにおいて、前回のステップで算出された時刻ｔ−１の走路パラメータｘ_t-1と、上記式（２−３）に示す更新行列Ｆとに基づいて、上記式（２−７）に従って、時刻ｔの走路パラメータｘ_t,t-1を算出する。

そして、走路パラメータ推定部３６は、上記式（２−３）に示す更新行列Ｆと、前回のステップで推定された共分散行列Ｐ_t-1と、システムノイズ分散行列Ｑと、ヨーレイトの誤差Δωの補正分Ｅ｛Δω²｝ＢＢ^Tとに基づいて、上記式（４−１０）に従って、共分散行列Ｐ_t,t-1を算出する。

（２）フィルタリングステップ
次に、フィルタリングステップにおける処理について説明する。走路パラメータ推定部３６は、フィルタリングステップにおいて、予測ステップで算出された共分散行列Ｐ_t,t-1と、観測行列Ｈと、観測ノイズの分散行列Ｒとに基づいて、上記式（２−１１）に従って、カルマンゲインＫを算出する。

次に、走路パラメータ推定部３６は、算出されたカルマンゲインＫと、予測ステップで算出された時刻ｔにおける状態ベクトルｘ_t,t-1と、上記式（２−５）で示す観測関数を用いて算出される観測値ｙ（＝ｈ（ｘ））と、予測値ｈ（ｘ_t,t-1）と、上記式（２−１２）に示す観測行列Ｈとに基づいて、上記式（２−９）に従って、時刻ｔにおける走路パラメータｘ_tを推定する。

そして、走路パラメータ推定部３６は、予測ステップで推定された共分散行列Ｐ_t,t-1と、算出されたカルマンゲインＫと、上記式（２−１２）に示す観測行列Ｈとに基づいて、上記式（２−１０）に従って、共分散行列Ｐ_tを算出する。

フィルタリングステップで算出された各値（カルマンゲインＫ、走路パラメータｘ_t、共分散行列Ｐ_t）は、次回の予測ステップにおける処理で用いられる。

走路パラメータ推定部３６は、推定された走路パラメータを、警報装置２０及び車両制御装置２２へ出力する。警報装置２０は、例えば、走路パラメータに含まれる自車両の横位置に基づいて、車線逸脱を示す警報を出力する。車両制御装置２２は、走路パラメータの各々に基づいて、運転支援や自動運転を行う。

＜走路認識装置１０の動作＞
次に、第１の実施の形態に係る走路認識装置１０の動作について説明する。まず、自車両が走行し、撮像装置１２によって自車両の前方が逐次撮像され、車速センサ１４によって自車両の車速が逐次検出され、ヨーレイトセンサ１６によって自車両のヨーレイトが逐次検出されているときに、コンピュータ１８において、図２に示す走路認識処理ルーチンが実行される。

＜走路認識処理ルーチン＞
ステップＳ１００において、入力項補正分算出部３０は、拡張カルマンフィルタの時間更新式の入力項であるヨーレイトの誤差Δωの補正分Ｅ｛Δω²｝ＢＢ^Tを算出する。

ステップＳ１０２において、システムノイズ設定部３２は、上記式（４−１０）に従って、算出された補正分Ｅ｛Δω²｝ＢＢ^TをシステムノイズＱに加算するように設定する。

ステップＳ１０４において、撮像装置１２によって撮像された道路画像、車速センサ１４によって検出された自車両の車速、及びヨーレイトセンサ１６によって検出された自車両のヨーレイトを取得する。

ステップＳ１０６において、車線境界検出部３４は、上記ステップＳ１０４で取得した道路画像から、走行車線の白線の候補点の位置の座標値を抽出する。

ステップＳ１０８において、走路パラメータ推定部３６は、上記ステップＳ１０６で抽出された走行車線の白線の候補点の座標値と、自車両の車速と、自車両のヨーレイトとに基づいて、拡張カルマンフィルタを用いて走路パラメータを推定する。ステップＳ１０８は、図３に示す走路パラメータ推定処理ルーチンによって実現される。

＜走路パラメータ推定処理ルーチン＞
ステップＳ１５０において、走路パラメータ推定部３６は、予測ステップにおいて、前回のステップＳ１５６で算出された走路パラメータｘ_t-1と、上記式（２−３）に示す更新行列Ｆとに基づいて、上記式（２−７）に従って、時刻ｔの走路パラメータｘ_t,t-1を算出する。

ステップＳ１５２において、走路パラメータ推定部３６は、上記式（２−３）に示す更新行列Ｆと、前回のステップＳ１５８で推定された共分散行列Ｐ_t-1と、システムノイズ分散行列Ｑと、ヨーレイトの誤差Δωの補正分Ｅ｛Δω²｝ＢＢ^Tとに基づいて、上記式（４−１０）に従って、共分散行列Ｐ_t,t-1を算出する。

次のステップＳ１５４では、走路パラメータ推定部３６は、フィルタリングステップにおいて、上記ステップＳ１５２で算出された共分散行列Ｐ_t,t-1と、観測行列Ｈと、観測ノイズの分散行列Ｒとに基づいて、上記式（２−１１）に従って、カルマンゲインＫを算出する。

ステップＳ１５６では、走路パラメータ推定部３６は、上記ステップＳ１５４で算出されたカルマンゲインＫと、上記ステップＳ１５０で算出された時刻ｔにおける状態ベクトルｘ_t,t-1と、上記式（２−５）で示す観測関数を用いて算出される観測値ｙ（＝ｈ（ｘ））と、予測値ｈ（ｘ_t,t-1）と、上記式（２−１２）に示す観測行列Ｈとに基づいて、上記式（２−９）に従って、時刻ｔにおける走路パラメータｘ_tを推定する。

そして、ステップＳ１５８では、走路パラメータ推定部３６は、上記ステップＳ１５２で推定された共分散行列Ｐ_t,t-1と、上記ステップＳ１５４で算出されたカルマンゲインＫと、上記式（２−１２）に示す観測行列Ｈとに基づいて、上記式（２−１０）に従って、共分散行列Ｐ_tを算出する。

フィルタリングステップで算出された各値（カルマンゲインＫ、走路パラメータｘ_t、共分散行列Ｐ_t）は、次回の予測ステップにおける処理で用いられる。

次に、走路認識処理ルーチンに戻り、ステップＳ１１０において、上記ステップＳ１０８で得られた走路パラメータを、警報装置２０及び車両制御装置２２に出力し、ステップＳ１１２において、時刻ｔを１インクリメントして、ステップＳ１０４へ戻る。

以上説明したように、第１の実施の形態に係る走路認識装置によれば、状態ベクトルの時間更新の入力となるヨーレイトセンサの出力についての変動に対応する補正分をシステムノイズに加算した拡張カルマンフィルタを用いることにより、ヨーレイトセンサ出力に誤差がある場合でも、カルマンフィルタにより走路パラメータの推定値を精度良く求めることができる。

また、従来のカルマンフィルタでは無視されていた入力項の変動分を考慮してシステムノイズを設定することにより、入力項に起因する推定値の定常誤差が速やかに収束し、推定値の定常誤差が低減される。すなわち、カルマンフィルタによる走路のトラッキングにおいて、推定精度を改善することができる。

また、ヨーレイトを入力とした車載カメラ画像による走路認識において、ヨーレイトに誤差が加わった場合の曲率推定精度が改善される。

［第２の実施の形態］
＜歩行者検出装置のシステム構成＞
次に、第２の実施の形態について説明する。第２の実施の形態では、歩行者パラメータを推定する歩行者検出装置に本発明を適用した場合を例に説明する。なお、第１の実施の形態と同様の構成となる部分については、同一符号を付して説明を省略する。

第２の実施の形態では、歩行者パラメータを推定している点が第１の実施の形態と異なっている。

図４に示すように、第２の実施の形態に係る歩行者検出装置２１０は、撮像装置１２と、車速センサ１４と、ヨーレイトセンサ１６と、撮像装置１２によって撮像された自車両の前方の道路画像、車速センサ１４によって検出された自車両の車速、及びヨーレイトセンサ１６によって検出された自車両のヨーレイトに基づいて、歩行者パラメータを推定するコンピュータ２１８とを備えている。

コンピュータ２１８は、ＣＰＵと、ＲＡＭと、後述する歩行者パラメータ推定処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。コンピュータ２１８は、拡張カルマンフィルタの時間更新式の入力項であるヨーレイトの誤差の補正分及び車速の誤差の補正分を算出する入力項補正分算出部２３０と、算出された補正分をシステムノイズに加算するように設定するシステムノイズ設定部２３２と、撮像装置１２によって撮像された道路画像に基づいて、自車両の前方に存在する歩行者候補を検出する歩行者候補検出部２３４と、自車両の前方に存在する歩行者に関する歩行者パラメータを推定する歩行者パラメータ推定部２３６とを備えている。

歩行者候補検出部２３４は、撮像装置１２によって撮像された道路画像に基づいて、自車両の前方に存在する歩行者候補の位置を検出する。例えば、撮像装置１２は、ステレオカメラであり、歩行者候補検出部２３４は、自車両の前方に存在する歩行者候補の方位及び歩行者候補までの距離を検出する。

歩行者パラメータ推定部２３６は、検出された走行者候補の位置（歩行者候補の方位及び歩行者候補までの距離）に基づいて、歩行者パラメータを、拡張カルマンフィルタを用いて推定する。

ここで、歩行者パラメータを、拡張カルマンフィルタを用いて推定する原理について説明する。

まず、計算モデルについて説明する。歩行者をトラッキング対象とし、自車両がヨーレイトωで操舵回避して移動している場合において歩行者検出を行う場合を例として説明する。トラッキング対象の位置はその中心点位置座標(ｘ_p,ｚ_p)として表現する。このとき、カルマンフィルタの状態ベクトルを以下のように定義する。

このとき、状態ベクトルの要素のうち、ｘ_pはトラッキング対象のｘ座標値[m]であり、

は、トラッキング対象のx座標方向移動速度[m/s]であり、ｚ_pは、トラッキング対象のz座標値[m]であり、

は、トラッキング対象のz座標方向移動速度[m/s]である。

また、時間更新モデルは次式で表現される。

このとき、ωはヨーレイト[rad/s]であり、ｖは、車速(z方向の自車両の移動速度) [m/s]であり、Ｔはカルマンフィルタの更新時間であり、ｕは、Ｑ＝Ｅ［ｕｕ^T］となるシステムノイズであり、Ｆは状態ベクトルｘの時間更新行列であり、Ｂ_ωはヨーレイトωから状態ベクトルｘへの寄与項であり、Ｂ_vは車速ｖから状態ベクトルｘへの寄与項である。

レーダやステレオカメラ等で前方障害物や歩行者を検出すると、観測関数ｈ（ｘ）は次式で表される。

このとき、観測値は、θとγであり、θは、トラッキング対象（歩行者候補）の方位[rad]であり、γはトラッキング対象（歩行者候補）までの距離[m]である（図５参照）。

次に、カルマンフィルタ更新式について説明する。

計算モデルに基づく拡張カルマンフィルタの更新式を以下に示す。

時間更新：

観測更新：

このとき、Ｐは誤差共分散行列であり、Ｈは観測行列であり、Ｒは観測ノイズの分散行列であり、Ｑはシステムノイズ共分散行列であり、Ｋはカルマンゲインである。

次に、ヨーレイト誤差及び車速誤差の影響低減方法について説明する。

カルマンフィルタ更新式(ａ−９)を式(ａ−１４)へ置き換えることにより、ヨーレイト誤差分Δω及び車速誤差分Δｖをカルマンフィルタへ取り込み、トラッキングの推定誤差を低減できる。

このとき、

ただし、Δωは、ヨーレイトの誤差分[rad]であり、Δｖは、車速の誤差分[m/s]である。

以上説明した原理により、本実施の形態では、入力項補正分算出部２３０は、拡張カルマンフィルタの時間更新式の入力項であるヨーレイトの誤差Δωの補正分Ｅ｛Δω²｝Ｂ_ωＢ_ω ^T、及び車速の誤差Δｖの補正分Ｅ｛Δｖ²｝Ｂ_vＢ_v ^Tを算出する。ただし、ヨーレイトの誤差Δωは、予め定められたものであり、例えば、ヨーレイトセンサ１６の分解能である。また、車速の誤差Δｖは、予め定められたものであり、例えば、車速センサ１４の分解能である。拡張カルマンフィルタの最新の

の推定値を得て、Ｂ_ωを算出する。

システムノイズ設定部２３２は、上記式（ａ−１４）に従って、算出された補正分Ｅ｛Δω²｝Ｂ_ωＢ_ω ^T、Ｅ｛Δｖ²｝Ｂ_vＢ_v ^TをシステムノイズＱに加算するように設定する。

歩行者パラメータ推定部２３６は、検出された歩行者行候補の位置（歩行者候補の方位及び距離）に基づいて、システムノイズ設定部２３２により設定された拡張カルマンフィルタにより歩行者パラメータの推定処理を行う。

拡張カルマンフィルタによる推定処理は、予測ステップとフィルタリングステップとを含んで構成される。以下、歩行者パラメータ推定部２３６によって実行される拡張カルマンフィルタによる予測ステップとフィルタリングステップとについて説明する。

（１）予測ステップ
まず、予測ステップにおける処理について説明する。歩行者パラメータ推定部２３６は、予測ステップにおいて、前回のステップで算出された時刻ｔ−１の歩行者パラメータｘ_t-1と、上記式（ａ−３）に示す更新行列Ｆとに基づいて、上記式（ａ−８）に従って、時刻ｔの歩行者パラメータｘ_t,t-1を算出する。

そして、歩行者パラメータ推定部２３６は、上記式（ａ−３）に示す更新行列Ｆと、前回のステップで推定された共分散行列Ｐ_t-1と、システムノイズ分散行列Ｑと、ヨーレイトの誤差Δωの補正分Ｅ｛Δω²｝Ｂ_ωＢ_ω ^Tと、車速の誤差Δｖの補正分Ｅ｛Δｖ²｝Ｂ_vＢ_v ^Tと、に基づいて、上記式（ａ−１４）に従って、共分散行列Ｐ_t,t-1を算出する。

（２）フィルタリングステップ
次に、フィルタリングステップにおける処理について説明する。歩行者パラメータ推定部２３６は、フィルタリングステップにおいて、予測ステップで算出された共分散行列Ｐ_t,t-1と、観測行列Ｈと、観測ノイズの分散行列Ｒとに基づいて、上記式（ａ−１２）に従って、カルマンゲインＫを算出する。

次に、歩行者パラメータ推定部２３６は、算出されたカルマンゲインＫと、予測ステップで算出された時刻ｔにおける状態ベクトルｘ_t,t-1と、上記式（ａ−５）で示す観測関数を用いて算出される観測値ｙ（＝ｈ（ｘ））と、予測値ｈ（ｘ_t,t-1）と、上記式（ａ−１３）に示す観測行列Ｈとに基づいて、上記式（ａ−１０）に従って、時刻ｔにおける歩行者パラメータｘ_tを推定する。

そして、歩行者パラメータ推定部２３６は、予測ステップで推定された共分散行列Ｐ_t,t-1と、算出されたカルマンゲインＫと、上記式（ａ−１３）に示す観測行列Ｈとに基づいて、上記式（ａ−１１）に従って、共分散行列Ｐ_tを算出する。

フィルタリングステップで算出された各値（カルマンゲインＫ、歩行者パラメータｘ_t、共分散行列Ｐ_t）は、次回の予測ステップにおける処理で用いられる。

歩行者パラメータ推定部２３６は、推定された歩行者パラメータを、警報装置２０及び車両制御装置２２へ出力する。警報装置２０は、例えば、歩行者パラメータに含まれる歩行者位置に基づいて、歩行者との衝突を示す警報を出力する。車両制御装置２２は、歩行者パラメータの各々に基づいて、運転支援や自動運転を行う。

＜歩行者検出装置２１０の動作＞
次に、第２の実施の形態に係る歩行者検出装置２１０の動作について説明する。まず、自車両が走行し、撮像装置１２によって自車両の前方が逐次撮像され、車速センサ１４によって自車両の車速が逐次検出され、ヨーレイトセンサ１６によって自車両のヨーレイトが逐次検出されているときに、コンピュータ２１８において、図６に示す歩行者検出処理ルーチンが実行される。なお、第１の実施の形態と同様の処理については、同一符号を付して詳細な説明を省略する。

＜歩行者検出処理ルーチン＞
ステップＳ２００において、入力項補正分算出部２３０は、拡張カルマンフィルタの時間更新式の入力項であるヨーレイトの誤差Δωの補正分Ｅ｛Δω²｝Ｂ_ωＢ_ω ^T、及び車速の誤差Δｖの補正分Ｅ｛Δｖ²｝Ｂ_vＢ_v ^Tを算出する。

ステップＳ１０２において、システムノイズ設定部３２は、上記式（ａ−１４）に従って、算出された補正分Ｅ｛Δω²｝Ｂ_ωＢ_ω ^T、Ｅ｛Δｖ²｝Ｂ_vＢ_v ^TをシステムノイズＱに加算するように設定する。

ステップＳ１０４において、撮像装置１２によって撮像された道路画像、車速センサ１４によって検出された自車両の車速、ヨーレイトセンサ１６によって検出された自車両のヨーレイトを取得する。

ステップＳ２０６において、歩行者候補検出部２３４は、上記ステップＳ１０４で取得した道路画像から、歩行者候補の方位及び距離を検出する。

ステップＳ２０８において、歩行者パラメータ推定部２３６は、上記ステップＳ２０６で検出された歩行者候補の方位及び距離と、自車両の車速と、自車両のヨーレイトとに基づいて、拡張カルマンフィルタを用いて歩行者パラメータを推定する。ステップＳ２０８は、図７に示す歩行者パラメータ推定処理ルーチンによって実現される。

＜歩行者パラメータ推定処理ルーチン＞
ステップＳ２５０において、歩行者パラメータ推定部２３６は、予測ステップにおいて、前回のステップＳ２５６で算出された歩行者パラメータｘ_t-1と、上記式（ａ−３）に示す更新行列Ｆとに基づいて、上記式（ａ−８）に従って、時刻ｔの歩行者パラメータｘ_t,t-1を算出する。

ステップＳ２５２において、歩行者パラメータ推定部２３６は、上記式（ａ−３）に示す更新行列Ｆと、前回のステップＳ２５８で推定された共分散行列Ｐ_t-1と、システムノイズ分散行列Ｑと、ヨーレイトの誤差Δωの補正分Ｅ｛Δω²｝Ｂ_ωＢ_ω ^Tと、車速の誤差Δｖの補正分Ｅ｛Δｖ²｝Ｂ_vＢ_v ^Tとに基づいて、上記式（ａ−１４）に従って、共分散行列Ｐ_t,t-1を算出する。

次のステップＳ２５４では、歩行者パラメータ推定部２３６は、フィルタリングステップにおいて、上記ステップＳ２５２で算出された共分散行列Ｐ_t,t-1と、観測行列Ｈと、観測ノイズの分散行列Ｒとに基づいて、上記式（ａ−１２）に従って、カルマンゲインＫを算出する。

ステップＳ２５６では、歩行者パラメータ推定部２３６は、上記ステップＳ２５４で算出されたカルマンゲインＫと、上記ステップＳ２５０で算出された時刻ｔにおける状態ベクトルｘ_t,t-1と、上記式（ａ−５）で示す観測関数を用いて算出される観測値ｙ（＝ｈ（ｘ））と、予測値ｈ（ｘ_t,t-1）と、上記式（ａ−１３）に示す観測行列Ｈとに基づいて、上記式（ａ−１０）に従って、時刻ｔにおける歩行者パラメータｘ_tを推定する。

そして、ステップＳ２５８では、歩行者パラメータ推定部２３６は、上記ステップＳ２５２で推定された共分散行列Ｐ_t,t-1と、上記ステップＳ２５４で算出されたカルマンゲインＫと、上記式（ａ−１３）に示す観測行列Ｈとに基づいて、上記式（ａ−１１）に従って、共分散行列Ｐ_tを算出する。

フィルタリングステップで算出された各値（カルマンゲインＫ、歩行者パラメータｘ_t、共分散行列Ｐ_t）は、次回の予測ステップにおける処理で用いられる。

次に、歩行者検出処理ルーチンに戻り、ステップＳ２１０において、上記ステップＳ２０８で得られた歩行者パラメータを、警報装置２０及び車両制御装置２２に出力し、ステップＳ１１２において、時刻ｔを１インクリメントして、ステップＳ１０４へ戻る。

以上説明したように、第２の実施の形態に係る歩行者検出装置によれば、状態ベクトルの時間更新の入力となるヨーレイトセンサ及び車速センサの出力についての変動に対応する補正分をシステムノイズに加算した拡張カルマンフィルタを用いることにより、ヨーレイトセンサ及び車速センサの出力に誤差がある場合でも、カルマンフィルタにより歩行者パラメータの推定値を精度良く求めることができる。

また、カルマンフィルタによる歩行者のトラッキングにおいて、歩行者パラメータの推定精度を改善することができる。

また、ヨーレイトを入力とした車載カメラ画像による歩行者のトラッキングにおいて、ヨーレイトに誤差が加わった場合の追従精度が改善される。

＜実施例＞
上記第１の実施の形態に係る走路認識装置１０による、走路パラメータ推定処理の効果の例を図８〜図１０に示す。曲率半径を１００ｍとし、入力項に誤差がある場合の横位置の定常偏差を比較したシミュレーション結果を図８に示した。図８では、カルマンフィルタのヨーレイト入力に誤差が重畳した場合について示している。図８より、入力項に誤差があることを考慮しない従来技術に比べて、横位置の定常偏差が低減していることから、本実施の形態に係る提案法の効果が確認された。

また、曲率半径を１００ｍとし、入力項に誤差がある場合のヨー角の定常偏差を比較したシミュレーション結果を図９に示した。図９では、カルマンフィルタのヨーレイト入力に誤差が重畳した場合について示している。図９より、入力項に誤差があることを考慮しない従来技術に比べて、ヨー角の定常偏差が低減していることから、本実施の形態に係る提案法の効果が確認された。

また、曲率半径を１００ｍとし、入力項に誤差がある場合の曲率の定常偏差を比較したシミュレーション結果を図１０に示した。図１０では、カルマンフィルタのヨーレイト入力に誤差が重畳した場合について示している。図１０より、入力項に誤差があることを考慮しない従来技術に比べて、曲率の定常偏差が低減していることから、本実施の形態に係る提案法の効果が確認された。

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

また、走路や歩行者をトラッキング対象とする場合を例に説明したが、これに限定されるものではない。例えば、障害物をトラッキング対象としてもよい。

また、撮像装置１２に代えてレーザレーダを用いることもできる。レーザレーダは対象までの距離を計測するアクティブセンサであるが反射波の受光強度により道路画像を生成することも可能である。

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

１０ 走路認識装置
１２ 撮像装置
１４ 車速センサ
１６ ヨーレイトセンサ
１８、２１８ コンピュータ
２０ 警報装置
２２ 車両制御装置
３０、２３０ 入力項補正分算出部
３２、２３２ システムノイズ設定部
３４ 車線境界検出部
３６ 走路パラメータ推定部
２１０ 歩行者検出装置
２３４ 歩行者候補検出部
２３６ 歩行者パラメータ推定部

Claims (2)

1. センサ観測値に基づいて推定対象の状態の推定値をカルマンフィルタで求めるトラッキング装置であって、
   前記センサ観測値とは異なるセンサの出力であって、状態の時間更新の入力となる前記センサの出力についての予め定められた変動に対応する補正分をシステムノイズに加算した前記カルマンフィルタを用いて、前記センサ観測値と前記センサの出力とから、前記状態の推定値を求める推定部
   を含み、
   前記センサ観測値を、移動体が走行する走路の画像から検出された、前記移動体が走行する走行車線の境界の位置とし、
   前記センサの出力を、ヨーレイトセンサで検出されたヨーレイトとし、
   前記状態の推定値を、前記移動体に対する車線の横位置、ヨー角、及び設定距離分だけ前方の曲率を含む走路パラメータとし、
   前記カルマンフィルタの時間更新式は、以下の式であるトラッキング装置。
   
   
   
   
   ただし、ｘは、前記状態を表す状態ベクトルであり、Ｆは状態ベクトルｘの時間更新行列であり、Ｂは前記センサの出力ωから状態ベクトルｘへの寄与項であり、Ｐは、誤差共分散行列であり、Ｑは、システムノイズの分散行列であり、Ｅ{}は、平均を表す関数である。
2. センサ観測値に基づいて推定対象の状態の推定値をカルマンフィルタで求めるためのプログラムであって、
   コンピュータを、
   前記センサ観測値とは異なるセンサの出力であって、状態の時間更新の入力となる前記センサの出力についての予め定められた変動に対応する補正分をシステムノイズに加算した前記カルマンフィルタを用いて、前記センサ観測値と前記センサの出力とから、前記状態の推定値を求める推定部
   として機能させるためのプログラムであり、
   前記センサ観測値を、移動体が走行する走路の画像から検出された、前記移動体が走行する走行車線の境界の位置とし、
   前記センサの出力を、ヨーレイトセンサで検出されたヨーレイトとし、
   前記状態の推定値を、前記移動体に対する車線の横位置、ヨー角、及び設定距離分だけ前方の曲率を含む走路パラメータとし、
   前記カルマンフィルタの時間更新式は、以下の式であるプログラム。
   
   
   
   
   ただし、ｘは、前記状態を表す状態ベクトルであり、Ｆは状態ベクトルｘの時間更新行列であり、Ｂは前記センサの出力ωから状態ベクトルｘへの寄与項であり、Ｐは、誤差共分散行列であり、Ｑは、システムノイズの分散行列であり、Ｅ{}は、平均を表す関数である。