(57)【要約】【課題】センサ出力に誤差がある場合でも、カルマンフィルタにより推定対象の状態の推定値を精度良く求めることができるようにする。【解決手段】入力項補正分算出部３０は、センサ観測値とは異なるセンサの出力であって、状態の時間更新の入力となるセンサの出力についての予め定められた変動に対応する補正分を算出する。システムノイズ設定部３２は、算出された補正分をシステムノイズに加算するように設定する。走路パラメータ推定部３６は、カルマンフィルタを用いて、センサ観測値とセンサの出力とから、走路パラメータの推定値を求める。【選択図】図１
2018060326.tif 000002
【特許請求の範囲】
【請求項１】
センサ観測値に基づいて推定対象の状態の推定値をカルマンフィルタで求めるトラッキング装置であって、
前記センサ観測値とは異なるセンサの出力であって、状態の時間更新の入力となる前記センサの出力についての予め定められた変動に対応する補正分をシステムノイズに加算した前記カルマンフィルタを用いて、前記センサ観測値と前記センサの出力とから、前記状態の推定値を求める推定部
を含むトラッキング装置。
【請求項２】
前記センサ観測値を、移動体が走行する走路の画像から検出された、前記移動体が走行する走行車線の境界の位置とし、
前記センサの出力を、ヨーレイトセンサで検出されたヨーレイトとし、
前記状態の推定値を、前記移動体に対する車線の横位置、ヨー角、及び設定距離分だけ前方の曲率を含む走路パラメータとした請求項１記載のトラッキング装置。
【請求項３】
前記センサ観測値を、移動体が走行する走路の画像から検出された、前記移動体に対する歩行者候補の方位及び距離とし、
前記センサの出力を、ヨーレイトセンサで検出されたヨーレイトとし、
前記状態の推定値を、前記移動体に対する歩行者の位置、及び移動速度を含む歩行者パラメータとした請求項１記載のトラッキング装置。
【請求項４】
前記カルマンフィルタの時間更新式は、以下の式である請求項１〜請求項３の何れか１項記載のトラッキング装置。
【数１】
2018060326.tif 000027
2018060326.tif 000028
ただし、ｘは、前記状態を表す状態ベクトルであり、Ｆは状態ベクトルｘの時間更新行列であり、Ｂは前記センサの出力ωから状態ベクトルｘへの寄与項であり、Ｐは、誤差共分散行列であり、Ｑは、システムノイズの分散行列であり、Ｅ{}は、平均を表す関数である。
【請求項５】
センサ観測値に基づいて推定対象の状態の推定値をカルマンフィルタで求めるためのプログラムであって、
コンピュータを、
前記センサ観測値とは異なるセンサの出力であって、状態の時間更新の入力となる前記センサの出力についての予め定められた変動に対応する補正分をシステムノイズに加算した前記カルマンフィルタを用いて、前記センサ観測値と前記センサの出力とから、前記状態の推定値を求める推定部
として機能させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、トラッキング装置及びプログラムに関する。
【背景技術】
【０００２】
従来より、車両前方の道路風景を撮像するＣＣＤカメラと、ＣＣＤカメラからの映像信号に対して画面全体に一様な処理を施す前処理部と、入力画面上にレーンマーカを検出するための複数の小領域を設定するレーンマーカ検出用小領域設定部と、複数の小領域に対してレーンマーカの一部を検出する直線検出部と、直線検出部による直線検出結果がレーンマーカの一部であることを検証するレーンマーカ候補点検出部と、レーンマーカの検出結果に基づいて車両前方の道路形状を表すための道路モデルパラメータを算出する道路モデルパラメータ算出部と、を有する走行路認識装置が知られている（特許文献１）。この走行路認識装置の道路モデルパラメータ算出部は、時間的および空間的に連続性を有し、パラメータ決定に確率的な合理性のある拡張カルマンフィルタを含む。
【０００３】
また、車両に搭載され、車両に関する観測を行い観測量を出力する複数の観測手段と、複数の観測手段により出力される観測量の信頼度を、観測量毎に算出する信頼度算出手段と、観測手段により出力された観測量、及び信頼度算出手段により算出された観測量の信頼度を推定モデルに入力して車両の状態量を推定する推定手段と、を備える車両状態量推定装置が知られている（特許文献２）。この車両状態量推定装置では、複数の観測手段には、同一種類の観測量を複数出力可能な観測手段が含まれ、信頼度算出手段は、同一の観測手段により出力された同一種類の複数の観測量を含めた複数の観測量について信頼度を算出し、推定手段は、同一の観測手段により出力された同一種類の複数の観測量のそれぞれを前記推定モデルに入力して車両の状態量を推定する。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開2002-109695号公報
【特許文献２】特開2013-129289号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら、カルマンフィルタで使用する時間更新モデルの制御入力項（上記の従来技術では舵角である。）に誤差が加わると、従来のカルマンフィルタでは推定値に定常誤差が発生するという問題があった。
【０００６】
これは、従来のカルマンフィルタでは、システムノイズと観測ノイズしか考慮されていないため、時間更新モデルの制御入力項の誤差が考慮できていなかったためである。言い換えると、時間更新モデルの制御入力項には誤差がないという前提でカルマンフィルタが組まれていたので、制御入力項に誤差がのった場合、それが推定誤差に影響していた。
【０００７】
本発明は、上記の事情に鑑みてなされたもので、センサ出力に誤差がある場合でも、カルマンフィルタにより推定対象の状態の推定値を精度良く求めることができるトラッキング装置及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【０００８】
上記の目的を達成するために本発明に係るトラッキング装置は、センサ観測値に基づいて推定対象の状態の推定値をカルマンフィルタで求めるトラッキング装置であって、前記センサ観測値とは異なるセンサの出力であって、状態の時間更新の入力となる前記センサの出力についての予め定められた変動に対応する補正分をシステムノイズに加算した前記カルマンフィルタを用いて、前記センサ観測値と前記センサの出力とから、前記状態の推定値を求める推定部を含んで構成されている。
【０００９】
本発明に係るプログラムは、センサ観測値に基づいて推定対象の状態の推定値をカルマンフィルタで求めるためのプログラムであって、コンピュータを、前記センサ観測値とは異なるセンサの出力であって、状態の時間更新の入力となる前記センサの出力についての予め定められた変動に対応する補正分をシステムノイズに加算した前記カルマンフィルタを用いて、前記センサ観測値と前記センサの出力とから、前記状態の推定値を求める推定部として機能させるためのプログラムである。
【００１０】
本発明によれば、推定部は、前記センサ観測値とは異なるセンサの出力であって、状態の時間更新の入力となる前記センサの出力についての予め定められた変動に対応する補正分をシステムノイズに加算した前記カルマンフィルタを用いて、前記センサ観測値と前記センサの出力とから、前記状態の推定値を求める。
【００１１】
このように、状態の時間更新の入力となる前記センサの出力についての変動に対応する補正分をシステムノイズに加算した前記カルマンフィルタを用いることにより、センサ出力に誤差がある場合でも、カルマンフィルタにより推定対象の状態の推定値を精度良く求めることができる。
【００１２】
また、本発明のプログラムは、記憶媒体に格納して提供することも可能である。
【発明の効果】
【００１３】
以上説明したように、本発明のトラッキング装置及びプログラムによれば、状態の時間更新の入力となる前記センサの出力についての変動に対応する補正分をシステムノイズに加算した前記カルマンフィルタを用いることにより、センサ出力に誤差がある場合でも、カルマンフィルタにより推定対象の状態の推定値を精度良く求めることができる、という効果が得られる。
【図面の簡単な説明】
【００１４】
【図１】本発明の第１の実施の形態に係る走路認識装置を示すブロック図である。
【図２】本発明の第１の実施の形態に係る走路認識装置における走路認識処理ルーチンの内容を示すフローチャートである。
【図３】本発明の第１の実施の形態に係る走路認識装置における走路パラメータ推定処理ルーチンの内容を示すフローチャートである。
【図４】本発明の第２の実施の形態に係る歩行者検出装置を示すブロック図である。
【図５】歩行者の位置を説明するための図である。
【図６】本発明の第２の実施の形態に係る歩行者検出装置における歩行者検出処理ルーチンの内容を示すフローチャートである。
【図７】本発明の第２の実施の形態に係る歩行者検出装置における歩行者パラメータ推定処理ルーチンの内容を示すフローチャートである。
【図８】従来手法と本実施の形態に係る手法との比較を説明するための図である。
【図９】従来手法と本実施の形態に係る手法との比較を説明するための図である。
【図１０】従来手法と本実施の形態に係る手法との比較を説明するための図である。
【発明を実施するための形態】
【００１５】
以下、図面を参照して、本発明の実施の形態について説明する。本発明の第１の実施の形態では、走路パラメータを推定する走路認識装置に本発明を適用した場合を例に説明する。
【００１６】
［第１の実施の形態］
＜走路認識装置のシステム構成＞
図１に示すように、第１の実施の形態に係る走路認識装置１０は、自車両の前方を撮像する撮像装置１２と、車速センサ１４と、ヨーレイトセンサ１６と、撮像装置１２によって撮像された自車両の前方の道路画像、車速センサ１４によって検出された自車両の車速、及びヨーレイトセンサ１６によって検出された自車両のヨーレイトに基づいて、走路パラメータを推定するコンピュータ１８とを備えている。
【００１７】
撮像装置１２は、自車両の前方の道路を撮像する車載カメラである。撮像装置１２は、例えば車両のリアビューミラー付近に設置され、自車両が走行する走路の画像の一例として、自車両の前方の道路画像を撮像する。
【００１８】
コンピュータ１８は、ＣＰＵと、ＲＡＭと、後述する走路パラメータ推定処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。コンピュータ１８は、拡張カルマンフィルタの時間更新式の入力項であるヨーレイトの誤差の補正分を算出する入力項補正分算出部３０と、算出された補正分をシステムノイズに加算するように設定するシステムノイズ設定部３２と、撮像装置１２によって撮像された道路画像に基づいて、自車両が走行する走行車線の境界を検出する車線境界検出部３４と、自車両が走行する走路に関する走路パラメータを推定する走路パラメータ推定部３６とを備えている。
【００１９】
車線境界検出部３４は、撮像装置１２によって撮像された道路画像に基づいて、自車両が走行する走行車線の境界として、白線の位置を検出する。
【００２０】
具体的には、車線境界検出部３４は、道路画像から走行車線の白線の候補点の位置の座標値を抽出する。例えば、自車両が走行中の車線の左又は右の白線を抽出する。
【００２１】
一般には、走路には白線レーンマークが敷設されているため、本実施の形態では、車線境界検出部３４は、白線レーンマークを走行車線の境界の候補として抽出する。白線レーンマークは、道路画像上では走路面に対して輝度が高いため、車線境界検出部３４は、例えば、道路画像を既知のＳｏｂｅｌフィルタ等によって処理し、得られた処理結果が閾値よりも大きい点を走行車線の白線の候補点として抽出する。
【００２２】
さらに、白線レーンマークは、局所的には進行方向に直線状に連続しているため、Ｈｏｕｇｈ変換等の手法により、直線状に連続している箇所を、車線の白線の候補として絞り込んでもよい。また、白線レーンマークの幅は局所的には一定なので、白線レーンマークの左右の車線の境界の候補対を照合して、車線の境界の候補点として選別してもよい。
【００２３】
走路パラメータ推定部３６は、検出された走行車線の境界の位置（走行車線の白線の候補点の座標値）に基づいて、走路パラメータを、拡張カルマンフィルタを用いて推定する。
【００２４】
ここで、走路パラメータを、拡張カルマンフィルタを用いて推定する原理について説明する。
【００２５】
まず、計算モデルについて説明する。
【００２６】
走路認識のモデルの状態ベクトルを次式に示す。
【００２７】
【数１】
2018060326.tif 000003
【００２８】
このとき、状態ベクトルの要素のうち、ｘ0は自車線の横位置［ｍ］であり、θ0はヨー角（車線の傾き）［ｒａｄ］であり、ｃ0は、道路の曲率［１／ｍ］であり、ｃ1は、道路の曲率変化率［１／ｍ2］であり、φはピッチ角［ｒａｄ］であり、ｗは車線幅［ｍ］である。
【００２９】
また、時刻ｔ−１の走路パラメータｘt-1から時刻ｔの走路パラメータｘtの時間更新モデルは次式で表現される。
【００３０】
【数２】
2018060326.tif 000004
【００３１】
このとき、Ｖは車速［ｍ／ｓ］であり、ωはヨーレイト［ｒａｄ／ｓ］であり、Ｔはカルマンフィルタの更新時間であり、ｕはＱ＝Ｅ［ｕｕT］となるシステムノイズであり、Ｆは状態ベクトルｘの時間更新行列であり、Ｂはヨーレイトωから状態ベクトルｘへの寄与項である。
【００３２】
走路認識では一般に路面に連続的にペイントされた白線を画像上で観測して、その白線上の点を画像上の座標値を観測値とする。よって、白線点ｊの観測関数ｈj（ｘ）は次式で示される。
【００３３】
【数３】
2018060326.tif 000005
【００３４】
このとき、Ｉjは道路画像上のｊ番目の白線点の垂直方向の座標値［画素］であり、Ｊjは、道路画像上のｊ番目の白線点の水平方向の座標値［画素］であり、Ｈcが路面平面からのカメラ高さ［ｍ］であり、ｆは焦点距離［ｍ］である。また、Ｉcは画像中心の垂直方向の座標値［画素］であり、Ｊcは、画像中心の水平方向の座標値［画素］であり、Ｒvは画像の垂直方向の解像度［ｍ／画素］であり、Ｒhは画像の水平方向の解像度［ｍ／画素］であり、ｓは画像上のｊ番目の白線点が右白線の場合＋１、左白線の場合−１であり、ｔは、画像上のｊ番目の白線点を路面平面上へ投影した場合の進行方向距離［ｍ］である。
【００３５】
次に、上述した計算モデルに基づくカルマンフィルタの更新式を以下に示す。
【００３６】
時間更新：
【数４】
2018060326.tif 000006
観測更新：
【数５】
2018060326.tif 000007
2018060326.tif 000008
【００３７】
このとき、Ｐは、誤差共分散行列であり、Ｈは観測行列であり、Ｒは、観測ノイズの分散行列であり、Ｑは、システムノイズの分散行列であり、Ｋはカルマンゲインである。
【００３８】
次に、ヨーレイト誤差の影響低減方法について説明する。
【００３９】
上記式（２−２）の時間更新モデルでヨーレイト誤差Δωを考慮すると次式となる。
【００４０】
【数６】
2018060326.tif 000009
【００４１】
ヨーレイト誤差分を考慮して誤差共分散行列の時間更新モデルを見直すと、以下のように展開できる。このとき、ｘ、ωs、Δω、ｕは無相関で、ｘ、ωsの平均は０と仮定する。
【００４２】
誤差共分散行列の定義より、以下の式が得られる。
【００４３】
【数７】
2018060326.tif 000010
【００４４】
平均を表す関数Ｅ{}で書き直すと、以下の式が得られる。
【００４５】
【数８】
2018060326.tif 000011
【００４６】
カルマンフィルタによる時間更新式中では、ヨーレイトの計測誤差が入るので、以下の式が得られる。
【００４７】
【数９】
2018060326.tif 000012
【００４８】
一方、元の系ではヨーレイト誤差を含まないので、以下の式が得られる。
【００４９】
【数１０】
2018060326.tif 000013
【００５０】
以上より、式（４−３）に式（４−４）と式（４−５）を代入して、以下の式が得られる。
【００５１】
【数１１】
2018060326.tif 000014
【００５２】
式（４−６）を整理すると、以下の式が得られる。
【００５３】
【数１２】
2018060326.tif 000015
【００５４】
このとき、Ｅ（Δω2）はヨーレイト誤差の二乗平均である。
【００５５】
以上より、以下の式が得られる。
【００５６】
【数１３】
2018060326.tif 000016
【００５７】
よって、カルマンフィルタ更新式（２−８）を式（４−１０）へ置き換えることにより、ヨーレイト誤差分Δωをカルマンフィルタへ取り込み、定常偏差を低減できる。
【００５８】
以上説明した原理により、本実施の形態では、入力項補正分算出部３０は、拡張カルマンフィルタの時間更新式の入力項であるヨーレイトの誤差Δωの補正分Ｅ｛Δω2｝ＢＢTを算出する。ただし、ヨーレイトの誤差Δωは、予め定められたものであり、例えば、ヨーレイトセンサ１６の分解能である。
【００５９】
システムノイズ設定部３２は、上記式（４−１０）に従って、算出された補正分Ｅ｛Δω2｝ＢＢTをシステムノイズＱに加算するように設定する。
【００６０】
走路パラメータ推定部３６は、検出された走行車線の境界の位置（走行車線の白線の候補点の座標値）に基づいて、システムノイズ設定部３２により設定された拡張カルマンフィルタにより走路パラメータの推定処理を行う。
【００６１】
拡張カルマンフィルタによる推定処理は、予測ステップとフィルタリングステップとを含んで構成される。以下、走路パラメータ推定部３６によって実行される拡張カルマンフィルタによる予測ステップとフィルタリングステップとについて説明する。
【００６２】
（１）予測ステップ
まず、予測ステップにおける処理について説明する。走路パラメータ推定部３６は、予測ステップにおいて、前回のステップで算出された時刻ｔ−１の走路パラメータｘt-1と、上記式（２−３）に示す更新行列Ｆとに基づいて、上記式（２−７）に従って、時刻ｔの走路パラメータｘt,t-1を算出する。
【００６３】
そして、走路パラメータ推定部３６は、上記式（２−３）に示す更新行列Ｆと、前回のステップで推定された共分散行列Ｐt-1と、システムノイズ分散行列Ｑと、ヨーレイトの誤差Δωの補正分Ｅ｛Δω2｝ＢＢTとに基づいて、上記式（４−１０）に従って、共分散行列Ｐt,t-1を算出する。
【００６４】
（２）フィルタリングステップ
次に、フィルタリングステップにおける処理について説明する。走路パラメータ推定部３６は、フィルタリングステップにおいて、予測ステップで算出された共分散行列Ｐt,t-1と、観測行列Ｈと、観測ノイズの分散行列Ｒとに基づいて、上記式（２−１１）に従って、カルマンゲインＫを算出する。
【００６５】
次に、走路パラメータ推定部３６は、算出されたカルマンゲインＫと、予測ステップで算出された時刻ｔにおける状態ベクトルｘt,t-1と、上記式（２−５）で示す観測関数を用いて算出される観測値ｙ（＝ｈ（ｘ））と、予測値ｈ（ｘt,t-1）と、上記式（２−１２）に示す観測行列Ｈとに基づいて、上記式（２−９）に従って、時刻ｔにおける走路パラメータｘtを推定する。
【００６６】
そして、走路パラメータ推定部３６は、予測ステップで推定された共分散行列Ｐt,t-1と、算出されたカルマンゲインＫと、上記式（２−１２）に示す観測行列Ｈとに基づいて、上記式（２−１０）に従って、共分散行列Ｐtを算出する。
【００６７】
フィルタリングステップで算出された各値（カルマンゲインＫ、走路パラメータｘt、共分散行列Ｐt）は、次回の予測ステップにおける処理で用いられる。
【００６８】
走路パラメータ推定部３６は、推定された走路パラメータを、警報装置２０及び車両制御装置２２へ出力する。警報装置２０は、例えば、走路パラメータに含まれる自車両の横位置に基づいて、車線逸脱を示す警報を出力する。車両制御装置２２は、走路パラメータの各々に基づいて、運転支援や自動運転を行う。
【００６９】
＜走路認識装置１０の動作＞
次に、第１の実施の形態に係る走路認識装置１０の動作について説明する。まず、自車両が走行し、撮像装置１２によって自車両の前方が逐次撮像され、車速センサ１４によって自車両の車速が逐次検出され、ヨーレイトセンサ１６によって自車両のヨーレイトが逐次検出されているときに、コンピュータ１８において、図２に示す走路認識処理ルーチンが実行される。
【００７０】
＜走路認識処理ルーチン＞
ステップＳ１００において、入力項補正分算出部３０は、拡張カルマンフィルタの時間更新式の入力項であるヨーレイトの誤差Δωの補正分Ｅ｛Δω2｝ＢＢTを算出する。
【００７１】
ステップＳ１０２において、システムノイズ設定部３２は、上記式（４−１０）に従って、算出された補正分Ｅ｛Δω2｝ＢＢTをシステムノイズＱに加算するように設定する。
【００７２】
ステップＳ１０４において、撮像装置１２によって撮像された道路画像、車速センサ１４によって検出された自車両の車速、及びヨーレイトセンサ１６によって検出された自車両のヨーレイトを取得する。
【００７３】
ステップＳ１０６において、車線境界検出部３４は、上記ステップＳ１０４で取得した道路画像から、走行車線の白線の候補点の位置の座標値を抽出する。
【００７４】
ステップＳ１０８において、走路パラメータ推定部３６は、上記ステップＳ１０６で抽出された走行車線の白線の候補点の座標値と、自車両の車速と、自車両のヨーレイトとに基づいて、拡張カルマンフィルタを用いて走路パラメータを推定する。ステップＳ１０８は、図３に示す走路パラメータ推定処理ルーチンによって実現される。
【００７５】
＜走路パラメータ推定処理ルーチン＞
ステップＳ１５０において、走路パラメータ推定部３６は、予測ステップにおいて、前回のステップＳ１５６で算出された走路パラメータｘt-1と、上記式（２−３）に示す更新行列Ｆとに基づいて、上記式（２−７）に従って、時刻ｔの走路パラメータｘt,t-1を算出する。
【００７６】
ステップＳ１５２において、走路パラメータ推定部３６は、上記式（２−３）に示す更新行列Ｆと、前回のステップＳ１５８で推定された共分散行列Ｐt-1と、システムノイズ分散行列Ｑと、ヨーレイトの誤差Δωの補正分Ｅ｛Δω2｝ＢＢTとに基づいて、上記式（４−１０）に従って、共分散行列Ｐt,t-1を算出する。
【００７７】
次のステップＳ１５４では、走路パラメータ推定部３６は、フィルタリングステップにおいて、上記ステップＳ１５２で算出された共分散行列Ｐt,t-1と、観測行列Ｈと、観測ノイズの分散行列Ｒとに基づいて、上記式（２−１１）に従って、カルマンゲインＫを算出する。
【００７８】
ステップＳ１５６では、走路パラメータ推定部３６は、上記ステップＳ１５４で算出されたカルマンゲインＫと、上記ステップＳ１５０で算出された時刻ｔにおける状態ベクトルｘt,t-1と、上記式（２−５）で示す観測関数を用いて算出される観測値ｙ（＝ｈ（ｘ））と、予測値ｈ（ｘt,t-1）と、上記式（２−１２）に示す観測行列Ｈとに基づいて、上記式（２−９）に従って、時刻ｔにおける走路パラメータｘtを推定する。
【００７９】
そして、ステップＳ１５８では、走路パラメータ推定部３６は、上記ステップＳ１５２で推定された共分散行列Ｐt,t-1と、上記ステップＳ１５４で算出されたカルマンゲインＫと、上記式（２−１２）に示す観測行列Ｈとに基づいて、上記式（２−１０）に従って、共分散行列Ｐtを算出する。
【００８０】
フィルタリングステップで算出された各値（カルマンゲインＫ、走路パラメータｘt、共分散行列Ｐt）は、次回の予測ステップにおける処理で用いられる。
【００８１】
次に、走路認識処理ルーチンに戻り、ステップＳ１１０において、上記ステップＳ１０８で得られた走路パラメータを、警報装置２０及び車両制御装置２２に出力し、ステップＳ１１２において、時刻ｔを１インクリメントして、ステップＳ１０４へ戻る。
【００８２】
以上説明したように、第１の実施の形態に係る走路認識装置によれば、状態ベクトルの時間更新の入力となるヨーレイトセンサの出力についての変動に対応する補正分をシステムノイズに加算した拡張カルマンフィルタを用いることにより、ヨーレイトセンサ出力に誤差がある場合でも、カルマンフィルタにより走路パラメータの推定値を精度良く求めることができる。
【００８３】
また、従来のカルマンフィルタでは無視されていた入力項の変動分を考慮してシステムノイズを設定することにより、入力項に起因する推定値の定常誤差が速やかに収束し、推定値の定常誤差が低減される。すなわち、カルマンフィルタによる走路のトラッキングにおいて、推定精度を改善することができる。
【００８４】
また、ヨーレイトを入力とした車載カメラ画像による走路認識において、ヨーレイトに誤差が加わった場合の曲率推定精度が改善される。
【００８５】
［第２の実施の形態］
＜歩行者検出装置のシステム構成＞
次に、第２の実施の形態について説明する。第２の実施の形態では、歩行者パラメータを推定する歩行者検出装置に本発明を適用した場合を例に説明する。なお、第１の実施の形態と同様の構成となる部分については、同一符号を付して説明を省略する。
【００８６】
第２の実施の形態では、歩行者パラメータを推定している点が第１の実施の形態と異なっている。
【００８７】
図４に示すように、第２の実施の形態に係る歩行者検出装置２１０は、撮像装置１２と、車速センサ１４と、ヨーレイトセンサ１６と、撮像装置１２によって撮像された自車両の前方の道路画像、車速センサ１４によって検出された自車両の車速、及びヨーレイトセンサ１６によって検出された自車両のヨーレイトに基づいて、歩行者パラメータを推定するコンピュータ２１８とを備えている。
【００８８】
コンピュータ２１８は、ＣＰＵと、ＲＡＭと、後述する歩行者パラメータ推定処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。コンピュータ２１８は、拡張カルマンフィルタの時間更新式の入力項であるヨーレイトの誤差の補正分及び車速の誤差の補正分を算出する入力項補正分算出部２３０と、算出された補正分をシステムノイズに加算するように設定するシステムノイズ設定部２３２と、撮像装置１２によって撮像された道路画像に基づいて、自車両の前方に存在する歩行者候補を検出する歩行者候補検出部２３４と、自車両の前方に存在する歩行者に関する歩行者パラメータを推定する歩行者パラメータ推定部２３６とを備えている。
【００８９】
歩行者候補検出部２３４は、撮像装置１２によって撮像された道路画像に基づいて、自車両の前方に存在する歩行者候補の位置を検出する。例えば、撮像装置１２は、ステレオカメラであり、歩行者候補検出部２３４は、自車両の前方に存在する歩行者候補の方位及び歩行者候補までの距離を検出する。
【００９０】
歩行者パラメータ推定部２３６は、検出された走行者候補の位置（歩行者候補の方位及び歩行者候補までの距離）に基づいて、歩行者パラメータを、拡張カルマンフィルタを用いて推定する。
【００９１】
ここで、歩行者パラメータを、拡張カルマンフィルタを用いて推定する原理について説明する。
【００９２】
まず、計算モデルについて説明する。歩行者をトラッキング対象とし、自車両がヨーレイトωで操舵回避して移動している場合において歩行者検出を行う場合を例として説明する。トラッキング対象の位置はその中心点位置座標(ｘp,ｚp)として表現する。このとき、カルマンフィルタの状態ベクトルを以下のように定義する。
【００９３】
【数１４】
2018060326.tif 000017
【００９４】
このとき、状態ベクトルの要素のうち、ｘpはトラッキング対象のｘ座標値[m]であり、
2018060326.tif 000018
は、トラッキング対象のx座標方向移動速度[m/s]であり、ｚpは、トラッキング対象のz座標値[m]であり、
2018060326.tif 000019
は、トラッキング対象のz座標方向移動速度[m/s]である。
【００９５】
また、時間更新モデルは次式で表現される。
【００９６】
【数１５】
2018060326.tif 000020
【００９７】
このとき、ωはヨーレイト[rad/s]であり、ｖは、車速(z方向の自車両の移動速度) [m/s]であり、Ｔはカルマンフィルタの更新時間であり、ｕは、Ｑ＝Ｅ［ｕｕT］となるシステムノイズであり、Ｆは状態ベクトルｘの時間更新行列であり、Ｂωはヨーレイトωから状態ベクトルｘへの寄与項であり、Ｂvは車速ｖから状態ベクトルｘへの寄与項である。
【００９８】
レーダやステレオカメラ等で前方障害物や歩行者を検出すると、観測関数ｈ（ｘ）は次式で表される。
【００９９】
【数１６】
2018060326.tif 000021
【０１００】
このとき、観測値は、θとγであり、θは、トラッキング対象（歩行者候補）の方位[rad]であり、γはトラッキング対象（歩行者候補）までの距離[m]である（図５参照）。
【０１０１】
次に、カルマンフィルタ更新式について説明する。
【０１０２】
計算モデルに基づく拡張カルマンフィルタの更新式を以下に示す。
【０１０３】
時間更新：
【数１７】
2018060326.tif 000022
観測更新：
【数１８】
2018060326.tif 000023
【０１０４】
このとき、Ｐは誤差共分散行列であり、Ｈは観測行列であり、Ｒは観測ノイズの分散行列であり、Ｑはシステムノイズ共分散行列であり、Ｋはカルマンゲインである。
【０１０５】
次に、ヨーレイト誤差及び車速誤差の影響低減方法について説明する。
【０１０６】
カルマンフィルタ更新式(ａ−９)を式(ａ−１４)へ置き換えることにより、ヨーレイト誤差分Δω及び車速誤差分Δｖをカルマンフィルタへ取り込み、トラッキングの推定誤差を低減できる。
【０１０７】
【数１９】
2018060326.tif 000024
このとき、
2018060326.tif 000025
【０１０８】
ただし、Δωは、ヨーレイトの誤差分[rad]であり、Δｖは、車速の誤差分[m/s]である。
【０１０９】
以上説明した原理により、本実施の形態では、入力項補正分算出部２３０は、拡張カルマンフィルタの時間更新式の入力項であるヨーレイトの誤差Δωの補正分Ｅ｛Δω2｝ＢωＢωT、及び車速の誤差Δｖの補正分Ｅ｛Δｖ2｝ＢvＢvTを算出する。ただし、ヨーレイトの誤差Δωは、予め定められたものであり、例えば、ヨーレイトセンサ１６の分解能である。また、車速の誤差Δｖは、予め定められたものであり、例えば、車速センサ１４の分解能である。拡張カルマンフィルタの最新の
2018060326.tif 000026
の推定値を得て、Ｂωを算出する。
【０１１０】
システムノイズ設定部２３２は、上記式（ａ−１４）に従って、算出された補正分Ｅ｛Δω2｝ＢωＢωT、Ｅ｛Δｖ2｝ＢvＢvTをシステムノイズＱに加算するように設定する。
【０１１１】
歩行者パラメータ推定部２３６は、検出された歩行者行候補の位置（歩行者候補の方位及び距離）に基づいて、システムノイズ設定部２３２により設定された拡張カルマンフィルタにより歩行者パラメータの推定処理を行う。
【０１１２】
拡張カルマンフィルタによる推定処理は、予測ステップとフィルタリングステップとを含んで構成される。以下、歩行者パラメータ推定部２３６によって実行される拡張カルマンフィルタによる予測ステップとフィルタリングステップとについて説明する。
【０１１３】
（１）予測ステップ
まず、予測ステップにおける処理について説明する。歩行者パラメータ推定部２３６は、予測ステップにおいて、前回のステップで算出された時刻ｔ−１の歩行者パラメータｘt-1と、上記式（ａ−３）に示す更新行列Ｆとに基づいて、上記式（ａ−８）に従って、時刻ｔの歩行者パラメータｘt,t-1を算出する。
【０１１４】
そして、歩行者パラメータ推定部２３６は、上記式（ａ−３）に示す更新行列Ｆと、前回のステップで推定された共分散行列Ｐt-1と、システムノイズ分散行列Ｑと、ヨーレイトの誤差Δωの補正分Ｅ｛Δω2｝ＢωＢωTと、車速の誤差Δｖの補正分Ｅ｛Δｖ2｝ＢvＢvTと、に基づいて、上記式（ａ−１４）に従って、共分散行列Ｐt,t-1を算出する。
【０１１５】
（２）フィルタリングステップ
次に、フィルタリングステップにおける処理について説明する。歩行者パラメータ推定部２３６は、フィルタリングステップにおいて、予測ステップで算出された共分散行列Ｐt,t-1と、観測行列Ｈと、観測ノイズの分散行列Ｒとに基づいて、上記式（ａ−１２）に従って、カルマンゲインＫを算出する。
【０１１６】
次に、歩行者パラメータ推定部２３６は、算出されたカルマンゲインＫと、予測ステップで算出された時刻ｔにおける状態ベクトルｘt,t-1と、上記式（ａ−５）で示す観測関数を用いて算出される観測値ｙ（＝ｈ（ｘ））と、予測値ｈ（ｘt,t-1）と、上記式（ａ−１３）に示す観測行列Ｈとに基づいて、上記式（ａ−１０）に従って、時刻ｔにおける歩行者パラメータｘtを推定する。
【０１１７】
そして、歩行者パラメータ推定部２３６は、予測ステップで推定された共分散行列Ｐt,t-1と、算出されたカルマンゲインＫと、上記式（ａ−１３）に示す観測行列Ｈとに基づいて、上記式（ａ−１１）に従って、共分散行列Ｐtを算出する。
【０１１８】
フィルタリングステップで算出された各値（カルマンゲインＫ、歩行者パラメータｘt、共分散行列Ｐt）は、次回の予測ステップにおける処理で用いられる。
【０１１９】
歩行者パラメータ推定部２３６は、推定された歩行者パラメータを、警報装置２０及び車両制御装置２２へ出力する。警報装置２０は、例えば、歩行者パラメータに含まれる歩行者位置に基づいて、歩行者との衝突を示す警報を出力する。車両制御装置２２は、歩行者パラメータの各々に基づいて、運転支援や自動運転を行う。
【０１２０】
＜歩行者検出装置２１０の動作＞
次に、第２の実施の形態に係る歩行者検出装置２１０の動作について説明する。まず、自車両が走行し、撮像装置１２によって自車両の前方が逐次撮像され、車速センサ１４によって自車両の車速が逐次検出され、ヨーレイトセンサ１６によって自車両のヨーレイトが逐次検出されているときに、コンピュータ２１８において、図６に示す歩行者検出処理ルーチンが実行される。なお、第１の実施の形態と同様の処理については、同一符号を付して詳細な説明を省略する。
【０１２１】
＜歩行者検出処理ルーチン＞
ステップＳ２００において、入力項補正分算出部２３０は、拡張カルマンフィルタの時間更新式の入力項であるヨーレイトの誤差Δωの補正分Ｅ｛Δω2｝ＢωＢωT、及び車速の誤差Δｖの補正分Ｅ｛Δｖ2｝ＢvＢvTを算出する。
【０１２２】
ステップＳ１０２において、システムノイズ設定部３２は、上記式（ａ−１４）に従って、算出された補正分Ｅ｛Δω2｝ＢωＢωT、Ｅ｛Δｖ2｝ＢvＢvTをシステムノイズＱに加算するように設定する。
【０１２３】
ステップＳ１０４において、撮像装置１２によって撮像された道路画像、車速センサ１４によって検出された自車両の車速、ヨーレイトセンサ１６によって検出された自車両のヨーレイトを取得する。
【０１２４】
ステップＳ２０６において、歩行者候補検出部２３４は、上記ステップＳ１０４で取得した道路画像から、歩行者候補の方位及び距離を検出する。
【０１２５】
ステップＳ２０８において、歩行者パラメータ推定部２３６は、上記ステップＳ２０６で検出された歩行者候補の方位及び距離と、自車両の車速と、自車両のヨーレイトとに基づいて、拡張カルマンフィルタを用いて歩行者パラメータを推定する。ステップＳ２０８は、図７に示す歩行者パラメータ推定処理ルーチンによって実現される。
【０１２６】
＜歩行者パラメータ推定処理ルーチン＞
ステップＳ２５０において、歩行者パラメータ推定部２３６は、予測ステップにおいて、前回のステップＳ２５６で算出された歩行者パラメータｘt-1と、上記式（ａ−３）に示す更新行列Ｆとに基づいて、上記式（ａ−８）に従って、時刻ｔの歩行者パラメータｘt,t-1を算出する。
【０１２７】
ステップＳ２５２において、歩行者パラメータ推定部２３６は、上記式（ａ−３）に示す更新行列Ｆと、前回のステップＳ２５８で推定された共分散行列Ｐt-1と、システムノイズ分散行列Ｑと、ヨーレイトの誤差Δωの補正分Ｅ｛Δω2｝ＢωＢωTと、車速の誤差Δｖの補正分Ｅ｛Δｖ2｝ＢvＢvTとに基づいて、上記式（ａ−１４）に従って、共分散行列Ｐt,t-1を算出する。
【０１２８】
次のステップＳ２５４では、歩行者パラメータ推定部２３６は、フィルタリングステップにおいて、上記ステップＳ２５２で算出された共分散行列Ｐt,t-1と、観測行列Ｈと、観測ノイズの分散行列Ｒとに基づいて、上記式（ａ−１２）に従って、カルマンゲインＫを算出する。
【０１２９】
ステップＳ２５６では、歩行者パラメータ推定部２３６は、上記ステップＳ２５４で算出されたカルマンゲインＫと、上記ステップＳ２５０で算出された時刻ｔにおける状態ベクトルｘt,t-1と、上記式（ａ−５）で示す観測関数を用いて算出される観測値ｙ（＝ｈ（ｘ））と、予測値ｈ（ｘt,t-1）と、上記式（ａ−１３）に示す観測行列Ｈとに基づいて、上記式（ａ−１０）に従って、時刻ｔにおける歩行者パラメータｘtを推定する。
【０１３０】
そして、ステップＳ２５８では、歩行者パラメータ推定部２３６は、上記ステップＳ２５２で推定された共分散行列Ｐt,t-1と、上記ステップＳ２５４で算出されたカルマンゲインＫと、上記式（ａ−１３）に示す観測行列Ｈとに基づいて、上記式（ａ−１１）に従って、共分散行列Ｐtを算出する。
【０１３１】
フィルタリングステップで算出された各値（カルマンゲインＫ、歩行者パラメータｘt、共分散行列Ｐt）は、次回の予測ステップにおける処理で用いられる。
【０１３２】
次に、歩行者検出処理ルーチンに戻り、ステップＳ２１０において、上記ステップＳ２０８で得られた歩行者パラメータを、警報装置２０及び車両制御装置２２に出力し、ステップＳ１１２において、時刻ｔを１インクリメントして、ステップＳ１０４へ戻る。
【０１３３】
以上説明したように、第２の実施の形態に係る歩行者検出装置によれば、状態ベクトルの時間更新の入力となるヨーレイトセンサ及び車速センサの出力についての変動に対応する補正分をシステムノイズに加算した拡張カルマンフィルタを用いることにより、ヨーレイトセンサ及び車速センサの出力に誤差がある場合でも、カルマンフィルタにより歩行者パラメータの推定値を精度良く求めることができる。
【０１３４】
また、カルマンフィルタによる歩行者のトラッキングにおいて、歩行者パラメータの推定精度を改善することができる。
【０１３５】
また、ヨーレイトを入力とした車載カメラ画像による歩行者のトラッキングにおいて、ヨーレイトに誤差が加わった場合の追従精度が改善される。
【０１３６】
＜実施例＞
上記第１の実施の形態に係る走路認識装置１０による、走路パラメータ推定処理の効果の例を図８〜図１０に示す。曲率半径を１００ｍとし、入力項に誤差がある場合の横位置の定常偏差を比較したシミュレーション結果を図８に示した。図８では、カルマンフィルタのヨーレイト入力に誤差が重畳した場合について示している。図８より、入力項に誤差があることを考慮しない従来技術に比べて、横位置の定常偏差が低減していることから、本実施の形態に係る提案法の効果が確認された。
【０１３７】
また、曲率半径を１００ｍとし、入力項に誤差がある場合のヨー角の定常偏差を比較したシミュレーション結果を図９に示した。図９では、カルマンフィルタのヨーレイト入力に誤差が重畳した場合について示している。図９より、入力項に誤差があることを考慮しない従来技術に比べて、ヨー角の定常偏差が低減していることから、本実施の形態に係る提案法の効果が確認された。
【０１３８】
また、曲率半径を１００ｍとし、入力項に誤差がある場合の曲率の定常偏差を比較したシミュレーション結果を図１０に示した。図１０では、カルマンフィルタのヨーレイト入力に誤差が重畳した場合について示している。図１０より、入力項に誤差があることを考慮しない従来技術に比べて、曲率の定常偏差が低減していることから、本実施の形態に係る提案法の効果が確認された。
【０１３９】
なお、上記の実施の形態では、移動体として車両を対象とする場合を例に説明したが、これに限定されるものではなく、他の移動体を対象としてもよい。
【０１４０】
また、走路や歩行者をトラッキング対象とする場合を例に説明したが、これに限定されるものではない。例えば、障害物をトラッキング対象としてもよい。
【０１４１】
また、撮像装置１２に代えてレーザレーダを用いることもできる。レーザレーダは対象までの距離を計測するアクティブセンサであるが反射波の受光強度により道路画像を生成することも可能である。
【０１４２】
なお、本発明のプログラムは、記録媒体に格納して提供することができる。
【符号の説明】
【０１４３】
１０   走路認識装置
１２   撮像装置
１４   車速センサ
１６   ヨーレイトセンサ
１８、２１８  コンピュータ
２０   警報装置
２２   車両制御装置
３０、２３０  入力項補正分算出部
３２、２３２  システムノイズ設定部
３４   車線境界検出部
３６   走路パラメータ推定部
２１０ 歩行者検出装置
２３４ 歩行者候補検出部
２３６ 歩行者パラメータ推定部
【図１】
2018060326.tif 000029
【図２】
2018060326.tif 000030
【図３】
2018060326.tif 000031
【図４】
2018060326.tif 000032
【図５】
2018060326.tif 000033
【図６】
2018060326.tif 000034
【図７】
2018060326.tif 000035
【図８】
2018060326.tif 000036
【図９】
2018060326.tif 000037
【図１０】
2018060326.tif 000038