JP6110256B2 - 対象物推定装置および対象物推定方法 - Google Patents

Description

本発明は、対象物を撮影した画像から当該対象物の位置および速度を推定する対象物推定装置および対象物推定方法に関するものである。

例えば、自動車等の車両に搭載したカメラにより車両の前方を撮影して、撮影された画像から歩行者や他の車両等の対象物を検出する検出装置が各種提案されている（特許文献１，非特許文献１参照）。このような検出装置では、対象物を検出して、車両（カメラ）から対象物までの距離および対象物の動き（速度）を測定（推定）する。

このような対象物の位置および速度を把握することで、当該対象物が車両にとって危険であるか否か判定することが可能となる。そして、対象物が車両にとって危険となり得る場合には、ドライバーにその旨を警告したり、ブレーキを自動制御したりして、ドライバーの運転を支援することが可能となる。

非特許文献１には、２つのカメラで撮影した画像から対象物の３次元上の位置および速度を推定する方法が開示されている。具体的には、非特許文献１の方法では、両カメラで撮影した２つの画像から画素毎に視差が求められる。また、一方のカメラで取得された連続するフレーム画像から画素毎の推移（オプティカルフロー）が算出される。そして、これらの視差およびオプティカルフローを入力として、カルマンフィルタにより対象物の現在の位置および速度を推定する。

国際公開第２００６／０８１９０６号公報

クレメンス・ラーベ（Clemens Rabe）、ウェ・フランケ（Uwe Franke）、およびステファン・ギリグ（Stefan Gehrig）著、「複雑な背景における移動体の高速検出」（Fast detection of moving objects in complex scenarios）、インテリジェント自動車シンポジウム，２００７ＩＥＥＥ（Intelligent Vehicles Symposium, 2007 IEEE）、（イスタンブール）、２００７年６月１３日、p. 398−403

ところで、カメラで撮影した映像から取得される視差やオプティカルフローは、カメラの解像度に起因する誤差や、対象物を誤って追跡（トラッキングエラー）することによる誤差等が不可避的に生ずる。そして、このような誤差（異常値）を含んだ視差やオプティカルフローが入力されると、算出される対象物の推定値も誤差を含んだものとなる。

すなわち、視差やオプティカルフローの入力値が異常値である場合に、フィルタで算出された推定値を適切に補正して、対象物の状態（位置・速度）を高い精度で推定し得る装置はこれまで提供されていなかった。

そこで、本発明は、上記課題を解決するためになされたものであって、その目的は、視差情報および推移情報に異常値が検出された場合に推定値を適切に補正して、対象物の位置および速度を精度よく推定し得る対象物推定装置および対象物推定方法を提供することにある。

請求項１の発明では、
画像取得手段が異なる位置から撮影した複数の画像に基づいて、当該画像中の対象物の位置および速度を推定する対象物推定装置であって、
前記複数の画像のうち基準となる基準画像において、時間的に前後するフレームから対応する画素の位置の時間的な推移情報を取得する推移情報取得手段と、
前記複数の画像から対応する各画素の視差情報を前記基準画像を基準として取得する視差情報取得手段と、
前記推移情報取得手段が取得した推移情報と、前記視差情報取得手段が取得した視差情報とに基づいて、前記対象物の３次元空間上の位置および速度の推定値をフィルタを用いて推定する推定値取得手段と、
前記推移情報取得手段が取得した推移情報および前記視差情報取得手段が取得した視差情報が、それぞれ異常値であるか否か判定する判定手段と、
前記判定手段の判定結果に基づいて、前記推定値取得手段が取得した前記推定値を補正する補正手段とを備え、
前記補正手段は、前記推移情報取得手段が取得した前記推移情報が異常値であると前記判定手段が判定した場合と、前記視差情報取得手段が取得した前記視差情報が異常値であると前記判定手段が判定した場合とで、前記推定値取得手段が取得した推定値を異なる方法で補正することを特徴とする。

請求項１の発明によれば、判定手段は、推移情報および視差情報がそれぞれ異常値であるか否か判定する。そして、補正手段は、判定手段の判定結果に基づいて、推定値取得手段が取得した推定値を補正する。従って、推移情報や視差情報が異常値の場合に、推定値取得手段が取得した誤差を含む推定値が補正されて、対象物の位置および速度を精度よく推定することができる。

ここで、補正手段は、判定手段が推移情報を異常値であると判定した場合と、視差情報を異常値であると判定した場合とで、推定値を補正する方法を変更する。すなわち、異常値が生じた入力値（推移情報，視差情報）の種類に応じて推定値を適切に補正することで、対象物の状態をより正確に推定することができる。

請求項２の発明では、前記補正手段は、前記推移情報取得手段が取得した前記推移情報を前記判定手段が異常値であると判定した場合に、前記推定値取得手段が取得した前記推定値を無効にすると共に、前記フィルタを初期状態にリセットすることを特徴とする。

推移情報取得手段は、基準画像の時間的に前後するフレームから推移情報を取得する。すなわち、推移情報は、過去に取得した推移情報に依存して取得されることになる。従って、一旦、推移情報が異常値（トラッキングエラー）となると、以降のフレームで取得される推移情報は、当該異常値の推移情報の影響を受けて大きな誤差を含むことになる。

そこで、請求項２の発明では、補正手段は、推移情報が異常値である場合に、推定値取得手段が取得した推定値を無効値とすると共に、フィルタを初期状態にリセットする。すなわち、推移情報が異常値であると判定された画素については、当該フレームにおいて推定を中止する。これにより、トラッキングエラーが生じた画素を同一の対象物として推定するのを防止して、対象物推定装置の信頼性を高めることができる。しかも、フィルタが初期状態にリセットされることで、異常値が生じた画素についての追跡が新たに開始され、当該画素についての推定値を得ることができる。

請求項３の発明では、前記推定値取得手段は、前記対象物が予め設定したプロセスモデルに従うと仮定して、過去に取得した前記推定値から前記対象物の位置および速度の予測値を取得し、前記補正手段は、前記視差情報取得手段が取得した前記視差情報が異常値であると前記判定手段が判定した場合に、前記推定値取得手段が取得した前記推定値を当該推定値取得手段が取得した前記予測値の値に補正することを特徴とする。

視差情報取得手段は、同一フレームにおける複数の画像から視差情報を取得する。すなわち、視差情報は、時間的に独立して算出されるから、過去の視差情報が以後に算出される視差情報に影響を与えることはない。従って、視差情報の異常値についても、フレーム毎に独立して発生することになる。

そこで、請求項３の発明では、視差情報が異常値である場合に、補正手段は、推定値取得手段が取得した推定値を予測値に補正する。すなわち、異常値である視差情報を用いて取得された誤差を含む推定値を予測値に変更することで、当該フレームでの推定値の誤差を小さくすることができる。

そして、視差情報が異常値である場合、補正手段は、フィルタを初期状態にリセットすることなく、以後のフレームで当該画素についての推定を継続させる。すなわち、視差情報に異常値が生じた場合は、当該フレームでの視差情報を予測値に設定すると共に、以後のフレームで当該画素についての推定を継続させることができる。

実施形態に係る対象物推定装置が搭載された自動車のシステム構成を示すブロック図。 実施形態に係る対象物推定装置が対象物を推定する状態を示す概念図。 （ａ）は対象物としての他車両が撮影された画像を示す図、（ｂ）は視差情報を示す図、（ｃ）は推移情報を示す図。 推移情報に異常値が生じて推定値を補正する場合を説明した図。 視差情報に異常値が生じて推定値を補正する場合を説明した図。 実施形態に係る対象物推定方法により対象物の状態推定をする際の制御フローを示すフローチャート。 実験例の実験条件を示す図。 実験例の実験結果を示すグラフ。

次に、実施形態に係る対象物推定装置および対象物推定方法について説明する。実施形態では、車両としての自動車（普通乗用車）に対象物推定装置を搭載した場合で説明する。

［実施形態］
図１は、本実施形態に係る対象物推定装置１０が搭載された自動車のシステム構成を示すブロック図であって、対象物推定装置１０は、画像取得部１２（画像取得手段）および車両情報取得部１４に電気的に接続されている。そして、対象物推定装置１０は、画像取得部１２および車両情報取得部１４からの入力に基づいて、画像取得部１２で撮影された対象物（例えば、他の車両や歩行者）の状態（位置および速度）を推定する。

なお、対象物推定装置１０は、図示しない運転支援装置に電気的に接続され、対象物推定装置１０で得られた対象物の推定値が運転支援装置に出力される。運転支援装置では、入力された推定値を用いて、例えばドライバーに対象物の存在を警告したり、ブレーキを自動制御したりして、ドライバーの運転を支援する。

（画像取得部について）
画像取得部１２は、例えば自動車のフロントガラス（ウインドシールドガラス）（図示せず）の車両室側に取り付けられ、自動車の進行方向の前方を撮影する。本実施形態の画像取得部１２は、左右（自動車の進行方向に対し垂直な水平方向）に離間する一対のカメラ１６，１８を備えたステレオカメラである。なお、以下の説明では、左側のカメラを第１カメラ１６（第１撮影手段）、右側のカメラを第２カメラ１８（第２撮影手段）とそれぞれ指称する。

第１カメラ１６および第２カメラ１８は、同一時刻にフレームが撮影されるよう撮影タイミングの同期が図られている。第１カメラ１６および第２カメラ１８は、互いに異なる位置から自動車の走行方向の前方を撮影して、それぞれ第１の画像（基準画像）および第２の画像を取得する。

（車両情報取得部について）
車両情報取得部１４は、自動車の走行状態を検出するものであって、自動車のヨーレートを検出するヨーレートセンサ２０および自動車の車速（走行速度）を検出する車速センサ２２を備えている。車両情報取得部１４で取得されたヨーレートおよび車速は、対象物推定装置１０の後述する推定値取得部２６に出力する。

（対象物推定装置について）
図１に示すように、対象物推定装置１０は、入力値取得部２４および推定値取得部２６を基本構成としている。

（入力値取得部について）
入力値取得部２４は、推移情報算出部２８（推移情報取得手段）および視差情報算出部３０（視差情報取得手段）を備えている。入力値取得部２４は、画像取得部１２が取得した第１の画像および第２の画像に基づいて、推定値取得部２６に入力する入力値（後述する視差情報および推移情報）を取得する。

推移情報算出部２８は、第１カメラ１６に電気的に接続され、第１カメラ１６で取得された第１の画像が入力される。また、推移情報算出部２８は、ＲＡＭ等の書き換え可能な記憶部（図示せず）を備え、１フレーム前の第１の画像を記憶部に随時記憶および更新する。

推移情報算出部２８は、現在のフレームの第１の画像と記憶部に記憶された１フレーム前の第１の画像（時間的に前後するフレーム）から対応する画素の位置の時間的な推移情報（オプティカルフロー）を取得する。

実施形態では、推移情報は、第１カメラ１６の光軸中心を原点として、当該画素の横方向（ｘ方向）の位置成分ｕと、当該画素の縦方向（ｙ方向）の位置成分ｖとして定義される。

なお、推移情報算出部２８が推移情報（ｕ，ｖ）を算出する際には、例えば濃度勾配法や、Ｈｏｒｎ−Ｓｃｈｕｎｃｋの方法（B.K. P. Horn and B.G. Schunck, Determining Optical Flow, AI(17), No. 1-3, 1981, pp. 185-203）等、公知の手法が用いられる。

視差情報算出部３０は、第１カメラ１６および第２カメラ１８の双方に電気的に接続され、両カメラ１６，１８で取得された第１の画像および第２の画像がそれぞれ入力される。視差情報算出部３０は、入力された第１の画像および第２の画像に基づいて、画素毎に視差（視差情報ｄ）を算出する。

具体的には、第１の画像の各画素と、第２の画像の対応する各画素について、例えばＳｅｍｉ−Ｇｌｏｂａｌ Ｍａｔｃｈｉｎｇによる画像処理によって視差情報ｄを算出する。本実施形態では、視差情報算出部３０は、第１の画像を基準として視差情報ｄを算出するよう設定されている。すなわち、視差情報ｄは、第１カメラ１６の光軸中心を原点とする当該画素の奥行き位置（第１カメラ１６から対象物までの距離）と一意に対応付けられる。なお、各画素のフレーム間のトラッキングは、推移情報算出部２８が算出する推移情報（ｕ，ｖ）（すなわち、オプティカルフロー）に基づいて行われる。

推移情報算出部２８で算出された推移情報（ｕ，ｖ）および視差情報算出部３０で算出された視差情報ｄは、推定値取得部２６に出力される。

図１に示すように、推定値取得部２６は、算出部３２（推定値取得手段）、判定部３４（判定手段）、および補正部３６（補正手段）を備えている。

算出部３２は、推移情報（ｕ，ｖ）および視差情報ｄを入力値とし、所定のプロセスモデルおよび観測モデルを持つカルマンフィルタ（Ｋａｌｍａｎ Ｆｉｌｔｅｒ）を実行することで、対象物の位置および速度の推定値を算出する。なお、本実施形態では、算出部３２は、第１の画像を基準として推定値を算出する。

ここで、算出部３２は、計算量を抑制するため、第１の画像の全ての画素について推定値を算出するのではなく、例えば第１の画像の周縁部を除く所定範囲について算出するように設定されている。

図２は、対象物推定装置１０が対象物の状態を推定する際の概念図を示す。座標系は、第１カメラ１６の光軸中心を原点として図２に示すように設定され、算出部３２が対象物の位置（Ｘ，Ｙ，Ｚ）および速度（Ｖ_Ｘ，Ｖ_Ｙ，Ｖ_ｚ）を推定する。算出部３２には、数１に示すように、対象物の位置および速度を等速直線運動でモデル化するプロセスモデルが設定されている。

ここで、ｖは、平均ゼロ、共分散行列Ｑの多変数正規分布に従う。

なお、車速は、車速センサ２２での検出値、ヨーレートは、ヨーレートセンサ２０での検出値である。

また、算出部３２には、数４に示すように、視差情報ｄおよび推移情報（ｕ，ｖ）と、対象物の位置（Ｘ，Ｙ，Ｚ）および速度（Ｖｘ，Ｖｙ，Ｖｚ）との関係を規定する観測モデルが設定されている。

ここで、Hは、３Ｄ→２Ｄ射影変換行列である。第１の画像の横方向の焦点距離をｆｘ［ｐｉｘ］、縦方向の焦点距離をｆｙ［ｐｉｘ］、第１カメラ１６および第２カメラ１８の基線長（光軸間距離）をＢとすると、Ｈは数５の式のように表される。

これを数４の式に代入する。

数６の式は、非線形な関数であるため、このままでは、カルマンフィルタを適用できない。そこで、数６の式を線形近似するため、数７の式に示すように、時刻ｔ（ｔフレーム）における対象物の位置の予測値（プロセスモデルから算出）の周りでテイラー展開して１次の項までを採用する。

数７の式を整理すると、

となる。ここで、ｗは平均ゼロ、共分散行列Ｒの多変数正規分布に従う。

算出部３２は、上記のプロセスモデルおよび観測モデルに基づいて、カルマンフィルタを実行する。すなわち、算出部３２は、以下に示すように、プロセスモデルに基づく予測（数１２〜数１４）および観測モデルに基づく更新（数１５〜数２０）を、第１の画像を基準としてフレーム毎に繰り返す。これにより、算出部３２は、対象物の状態（位置・速度）を推定する。カルマンフィルタのｔフレームでの推定値および当該推定値の誤差の共分散行列を、それぞれ数１０および数１１に示す。

数１１のｘ_ｔは、対象物の状態（位置・速度）の真値を表す。
［予測］

［更新］

算出部３２には、上記予測および更新を実行するため、推定値の初期値（数２１）および推定値の誤差の共分散行列についての初期値（数２２）が予め設定されている。

これらの初期値は、カルマンフィルタを実行するモデルや条件に合わせて、適当な定数を設定したり、シミュレーションや実験等で求めた値を採用したりすることができる。

判定部３４は、推移情報算出部２８で算出された推移情報（ｕ，ｖ）および視差情報算出部３０で算出された視差情報ｄが異常値であるか否か判定する。以下、推移情報（ｕ，ｖ）および視差情報ｄの異常値が発生する場合について、具体例を挙げて説明する。

図３（ａ）に示すように、対象物推定装置１０が搭載された自動車の走行方向の前方を走行する自動車（以下、他車両３８と称する）を対象物とし、対象物推定装置１０による状態推定を行う場合を想定する。図３（ｂ）は、視差情報算出部３０が算出したｔ−１フレーム、ｔフレーム、ｔ＋１フレームでの視差情報ｄを表している。

図３（ｂ）の各フレームに付した円形の囲み線は、正常にトラッキング（すなわち、適正に推移情報が算出）された画素（追跡点）を示している。図３（ｂ）から分かるように、囲み線内の画素は対象物の同一箇所であるのに拘らず、ｔ−１フレームからｔフレームにかけて、当該画素での視差情報ｄが変化している。また、ｔ＋１フレームでは、視差情報ｄが修正されているのが分かる。すなわち、ｔフレームにおいて視差情報算出部３０が算出した囲み線内の画素の視差情報ｄは、真値からズレた異常値（外れ値）となっている。

このような異常値は、第１の画像および第２の画像に含まれるノイズの影響や、各画像を有限の画素で区画していることに伴い発生する誤差等に起因する。一方、ｔ＋１フレームで視差情報ｄが修正されているのは、視差情報算出部３０がフレーム毎に視差情報ｄを独立して算出することに起因する。すなわち、視差情報算出部３０は、過去に算出した視差情報ｄに依存することなく視差情報ｄを算出するため、一旦、視差情報ｄに異常値が生じたとしても、ｔ＋１フレームで適正な視差情報ｄが算出されたからである。

図３（ｃ）は、推移情報算出部２８が算出したｔ’−１フレーム、ｔ’フレーム、ｔ’＋１フレームでの推移情報（ｕ，ｖ）を表している。ｔ’フレームでの囲み線を付した箇所では、追跡点が他車両３８から他車両３８以外の風景（側壁）や別の車両４０に移ってしまった箇所を示す。すなわち、ｔ’フレームの囲み線を付した画素について推移情報算出部２８が算出した推移情報（ｕ，ｖ）は、異常値（外れ値）となっている。このような、推移情報（ｕ，ｖ）の異常値は、主としてパターンマッチングのエラーによって生じる。

ここで、ｔ’＋１フレームでは、他車両３８から外れた追跡点がそのままトラッキングされている。すなわち、一旦、推移情報（ｕ，ｖ）に異常値が発生すると、追跡点が修正されることはない（他車両３８上の追跡点に復帰することはない）。これは、推移情報（ｕ，ｖ）（オプティカルフロー）が時間的に前後するフレーム間に亘って算出されて、過去に算出した推移情報（ｕ，ｖ）に依存することに起因する。

上述のように、推移情報算出部２８が算出した推移情報（ｕ，ｖ）や、視差情報算出部３０が算出した視差情報ｄは、エラーによる異常値である可能性がある。そこで、判定部３４は、推移情報（ｕ，ｄ）および視差情報ｄと算出部３２が算出した予測値（数１３）との差に基づいて、推移情報（ｕ，ｖ）および視差情報ｄが異常値であるか否か判定する。

具体的には、判定部３４は、推移情報算出部２８が算出した推移情報（ｕ，ｖ）と、算出部３２が算出した予測値から得られ、推移情報（ｕ，ｖ）に対応する予測推移情報との差に基づいて、当該推移情報（ｕ，ｖ）が異常値であるか否か判定する。また、判定部３４は、視差情報算出部３０が算出した視差情報ｄと、算出部３２が算出した予測値から得られ、視差情報ｄに対応する予測視差情報との差に基づいて、当該視差情報ｄが異常値であるか否か判定する。

すなわち、判定部３４は、算出部３２がカルマンフィルタを更新する際に算出する観測値および予測値の残差（数１６）を用いる。

そして、推移情報と予測情報との差および視差情報と予測視差情報との差が、それぞれ所定の閾値Ｔｈ_ｕ，Ｔｈ_ｖ，Ｔｈ_ｄよりも大きい場合に、判定部３４は観測値（視差情報ｄ、推移情報（ｕ，ｄ））がそれぞれ異常値であると判定する。

すなわち、数２４または数２５の式の少なくとも１つを満たす場合、判定部３４は、推移情報（ｕ，ｖ）が異常値であると判定する。また、数２６を満たす場合、判定部３４は、視差情報ｄが異常値であると判定する。この閾値Ｔｈ_ｕ，Ｔｈ_ｖ，Ｔｈ_ｄとしては、シミュレーションや実験等で得られた固定値が採用される。

但し、閾値としては、必ずしも固定値とする必要はない。例えば、遠方の対象物の視差情報ｄは、撮影された画像のノイズ等が大きく影響し、算出される視差情報ｄの誤差も大きくなり易くなる。その結果、固定の閾値を採用した場合には、判定部３４が視差情報ｄを常に異常値であると判定してしまう虞がある。

そこで、観測値および予測値のばらつきを考慮した判定を実施するため、固定値としての閾値に代えて、変数としての閾値を用いてもよい。

すなわち、数１７に示す残差の共分散行列であるＳ_ｔの要素を用いて、閾値を調整（変動）してもよい。

そして、以下の場合に、判定部３４は観測値が異常値であると判定する。

すなわち、数２８または数２９の式の少なくとも１つを満たす場合、判定部３４は、推移情報（ｕ，ｖ）が異常値であると判定する。また、数３０を満たす場合、判定部３４は、視差情報ｄが異常値であると判定する。

補正部３６は、判定部３４が観測値の何れかが異常値である場合、算出部３２が算出した推定値を補正する。一方、観測値が異常値ではない場合、補正部３６は、算出部３２で算出された推定値を補正することはない。

ここで、補正部３６は、推移情報（ｕ，ｖ）が異常値であると判定された場合と、視差情報ｄが異常値であると判定された場合とで、推定値を異なる方法で補正する。

（推移情報が異常値であると判定された場合について）
先ず始めに、推移情報（ｕ，ｖ）が異常値であると判定された場合に、補正部３６が推定値を補正する方法について、図４を用いて説明する。なお、図４に示す画像は、推移情報算出部２８が算出した推移情報（ｕ，ｖ）を示すものであって、図３（ｃ）で説明したものと同一である。

図４に示すように、ｔ’−１フレームまでは、図中の囲み線内で示す箇所（追跡点Ａ）のトラッキング（推移情報）は、正常に行われている。従って、算出部３２で算出される推定値は、他車両３８上の追跡点Ａを観測して推定された値となっている。

ｔ’フレームにおいて、トラッキングエラーが発生すると、追跡点が他車両３８上のＡから背景（側壁）上のＢに推移している。すなわち、ｔ’フレームで推移情報算出部２８が算出した推移情報（ｕ，ｖ）は、数２４または数２５（もしくは数２８または数２９）を満たす異常値となる。この場合に、ｔ’−１フレームでの追跡点Ａについての推定値を用いて算出したｔ’フレームでの追跡点Ｂについての推定値は、異なる対象物を追跡して得られたエラー値となる。

そこで、補正部３６は、推移情報（ｕ，ｖ）が異常値である場合、算出部３２が算出したｔ’フレームでの推定値をキャンセル（無効）すると共に、カルマンフィルタを初期状態にリセットする。このように、トラッキングエラーが発生した場合には、その後、推移情報（ｕ，ｖ）が正常値に修正されることはないため、補正部３６は、推定値を数２１の初期値にリセットする。

推移情報（ｕ，ｖ）が異常値である場合、当該画素についてのトラッキングは無効となるため、補正部３６は、対象物推定装置１０の出力として無効値を設定する。すなわち、推移情報（ｕ，ｖ）が異常値と判断された当該画素についての推定値は、無効値が出力され、当該画素についての推定が終了する。

なお、カルマンフィルタが初期状態にリセットされた後も、追跡点Ｂについてのトラッキングは継続される。従って、ｔ’＋１フレームでは、算出部３２が追跡点Ｂについての推定値を前述した初期値を用いて算出する。そして、以降のフレームでは、追跡点Ｂについての推定値が算出されることとなる。

なお、推移情報（ｕ，ｖ）が異常値でないと判定された場合、補正部３６は、算出部３２で算出された推定値を補正することなく、当該推定値を対象物推定装置１０の出力に設定する。

（視差情報が異常値であると判定された場合について）
次に、視差情報ｄが異常値であると判定された場合に、補正部３６が推定値を補正する方法について、図５を用いて説明する。なお、図５の左側の画像は、視差情報算出部３０が算出した視差情報ｄを示すものであって、図３（ｂ）で説明したものと同一である。また、図５の右側の画像は、左側の画像の対応するフレームでのトラッキング（推移情報）を示したものである。なお、図中の円形の囲み線は、対象物の同一箇所（追跡点Ｃ）を示しており、ｔフレームにおいて視差情報ｄが異常値となっている。なお、推移情報（ｕ，ｄ）については、全てのフレームにおいて適正に算出されたものとする。すなわち、追跡点Ｃについてのトラッキングは、正確に行われている。

ｔ−１フレームでは、視差情報ｄが正しく算出されており、算出部３２が算出する推定値も適正なものとなる。従って、補正部３６は、算出部３２が算出した推定値を出力として設定する。

ｔフレームでは、トラッキングは正常に実行されているものの、視差情報ｄに誤差が生じている。すなわち、ｔフレームで視差情報算出部３０が算出する視差情報ｄは、数２６（または数３０）を満たす異常値となる。

この場合、異常値である視差情報ｄを用いて算出された推定値（数１９）よりも、数１３で得られる予測値の方が対象物の状態をより正確に表している（真値により近い）と考えられる。そこで、補正部３６は、視差情報ｄが異常値であると判定されると、数１９から算出された推定値をキャンセルすると共に、数１３で得られる予測値を推定値として採用する。そして、補正部３６は、対象物推定装置１０の出力として、予測値の値に補正された推定値を設定する。

ｔ+１フレームでは、視差情報ｄが修正されて、適正な値が算出されている。この場合、補正部３６は、算出部３２が算出した推定値を補正することなく採用する。すなわち、ｔ＋１フレームでは、算出部３２がｔフレームで得られた推定値（予測値）を用いて算出した推定値がそのまま採用される。そして、補正部３６は、当該推定値を出力として設定する。

次に、本実施形態の対象物推定装置１０による推定方法について、図６のフローチャートを参照して説明する。なお、図６のフローチャートは、任意のフレーム（ｔフレーム）において、対象物推定装置１０が対象物の状態を推定する際の制御フローを示すものである。

第１カメラ１６および第２カメラ１８（画像取得部１２）は、自動車の走行方向前方の風景を撮影し、それぞれ第１の画像および第２の画像を取得する。画像取得部１２で取得された第１の画像および第２の画像は、対象物推定装置１０（入力値取得部２４）に入力される（ステップＳ１０）。

推移情報算出部２８は、入力された第１の画像を記憶部に記憶する（ステップＳ１２）。この記憶部に記憶されたｔフレームの第１の画像は、次のフレーム（ｔ＋１フレーム）において推移情報算出部２８が推移情報（ｕ，ｖ）を算出する際に用いられる。

次に、車両情報取得部１４が取得した自動車のヨーレートおよび車速が対象物推定装置１０（推定値取得部２６）に入力される（ステップＳ１４）。すなわち、ヨーレートセンサ２０で検出されたヨーレートおよび車速センサ２２で検出された自動車の車速が対象物推定装置１０に入力される。

次いで、推移情報算出部２８は、記憶部から１フレーム前（ｔ−１フレーム）の第１の画像を読み出す（ステップＳ１６）。そして、推移情報算出部２８は、画像取得部１２から入力された当該フレーム（ｔフレーム）での第１の画像と、１フレーム前の第１の画像とに基づいて、推移情報（ｕ，ｖ）を算出する（ステップＳ１８）。

視差情報算出部３０は、画像取得部１２から入力された第１の画像および第２の画像に基づいて、視差情報ｄを算出する（ステップＳ２０）。このとき、視差情報算出部３０は、第１の画像を基準として、当該第１の画像の画素毎に視差情報ｄを算出する。入力値取得部２４で取得された視差情報ｄおよび推移情報（ｕ，ｖ）は、推定値取得部２６に出力される。

推定値取得部２６では、第１の画像の所定範囲の各画素について対象物の状態の推定値を取得する（ステップＳ２２〜Ｓ３８）。すなわち、入力値取得部２４から入力された視差情報ｄおよび推移情報（ｕ，ｖ）に基づいて、算出部３２は、予測（数１２〜数１４）および更新（数１５〜数２０）によるカルマンフィルタを実行する（ステップＳ２４）。

次に、判定部３４が、推移情報算出部２８で算出された推移情報（ｕ，ｖ）が異常値であるか否か判定する（ステップＳ２６）。すなわち、判定部３４は、推移情報（ｕ，ｖ）の各成分が、数２４および数２５（または数２８および数２９）の式をそれぞれ満たすか否か判定する。

ここで、当該画素についてのトラッキングが正しく行われている場合、推移情報（ｕ，ｖ）の各成分は、何れも数２４，数２５（または数２８，数２９）を満たすことはない。この場合、判定部３４は、推移情報算出部２８で算出された推移情報（ｕ，ｖ）が異常値ではないと判定する（ステップＳ２６のＮｏ）。

ステップＳ２６が否定されると、次に、判定部３４は、ステップＳ１６で視差情報算出部３０が算出した視差情報ｄが異常値であるか否か判定する（ステップＳ２８）。すなわち、判定部３４は、視差情報ｄが数２６（または数３０）を満たすか否か判定する。

ここで、当該画素についての視差情報ｄが適切に算出されている場合、視差情報ｄが数２６（または数３０）を満たすことはない。この場合、判定部３４は、視差情報算出部３０で算出された視差情報ｄが異常値でないと判定する（ステップＳ２８のＮｏ）。

判定部３４が推移情報（ｕ，ｖ）および視差情報ｄが何れも異常値でないと判定すると、補正部３６は、ステップＳ２４で算出部３２が算出した推定値を補正することなく採用する。すなわち、補正部３６は、ステップＳ２４で算出された推定値をそのまま当該画素における推定値の出力として設定する（ステップＳ３０）。

一方、ステップＳ２６において、当該画素に対するトラッキングが適正に行われていなかった場合、推移情報（ｕ，ｖ）の少なくとも１つの成分が数２４，数２５（数２８，数２９）を満たすことになる。この場合、判定部３４は、推移情報（ｕ，ｖ）が異常値であると判定する（ステップＳ２６のＹｅｓ）。

推移情報（ｕ，ｖ）が異常値であると判定されると、補正部３６は、ステップＳ２４で算出された推定値を採用することなく、カルマンフィルタを初期状態にリセットする（ステップＳ３２）。すなわち、補正部３６は、予め設定された初期値（数２１）をカルマンフィルタにセットする。そして、補正部３６は、当該画素における推定値の出力として無効値を設定する（ステップＳ３４）。

このように、推移情報算出部２８が算出した推移情報（ｕ，ｖ）が異常値である場合、補正部３６は、当該推移情報（ｕ，ｖ）に基づいて算出された推定値をキャンセルして、当該画素における推定値を無効とする。これにより、適切にトラッキングされなかった画素についての推定結果を用いることで、大きな誤差を含んだ推定値が出力されるのを防止し得る。

一方、ステップＳ２８において、当該画素における視差情報ｄに誤差が生じていた場合、視差情報ｄは数２６（または数３０）を満たすことになる。この場合、判定部３４は、視差情報ｄが異常値であると判定する（ステップＳ２８のＹｅｓ）。視差情報ｄが異常値であると判定されると、補正部３６は、ステップＳ２４で算出部３２が算出した推定値をキャンセルする。そして、補正部３６は、算出部３２がカルマンフィルタを実行する際に算出した予測値（数１３参照）の値を推定値に設定する（ステップＳ３６）。

すなわち、補正部３６は、視差情報ｄが異常値である場合には、算出部３２が算出した推定値も誤差を含んでいることから、当該推定値をキャンセルする。そして、補正部３６は、算出部３２が算出した推定値よりも正確と考えられる予測値を当該画素における推定値に設定する。これにより、視差情報ｄに誤差が含まれている場合であっても、対象物の推定値として、より適切な値が採用されることになり、対象物推定装置１０の信頼性を高めることができる。

しかも、視差情報ｄは、フレーム毎に独立して算出されるから、現在のフレームの視差情報ｄが異常値であったとしても、後のフレームでの視差情報ｄの算出に影響を与えることがない。従って、補正部３６は、推移情報（ｕ，ｖ）が異常値の場合のようにカルマンフィルタを初期状態にリセットすることはなく、当該画素に対する以後の推定は継続される。

補正部３６は、推定値として予測値の値を設定すると、当該推定値の値を当該画素における出力値として設定する（ステップＳ３０）。

推定値取得部２６は、当該画素についての推定値が決定すると、ステップＳ２６〜Ｓ３６を次の画素について実行する。そして、第１の画像の所定範囲にある全ての画素について推定値が決定すると（ステップＳ３８）、推定値取得部２６は、出力に設定された推定値を運転支援装置に出力し（ステップＳ４０）、現在のフレームについての処理を終了する。

［実験例］
次に、前述した対象物推定装置１０の効果を確認するべく、視差情報ｄに異常値が生じる場合での実証実験（実験例）を行った。図７に示すように、対象物推定装置１０が搭載された自動車４２は、車速２５ｋｍ／ｈで道路を直進するものとする。そして、自動車４２の走行方向の前方を走行する車両（以下、他車両４４という）を対象物として、当該他車両４４の位置および速度を推定した。なお、座標系は、図７に示す通りであり、第１カメラ１６のレンズの中心を原点位置とした。

ここで、車速センサ２２の車速は７．３２ｍ／ｓ、ヨーレートセンサ２０のヨーレートは、０．１５ｄｅｇ／ｓであるとする（何れも誤差を含む）。この場合、数１に示すカルマンフィルタのプロセスモデルは、以下のように表される。

ここで、数３１の共分散行列Ｑは、以下に示すように、事前に行ったシミュレーションにおいて得られた適合値を採用した。

また、観測モデルの数９に示す共分散行列Ｒは、以下に示すように、事前に行ったシミュレーションにおいて得られた適合値を採用した。

また、本実験例においては、カルマンフィルタの初期値として、以下のような値を設定した。推定値の初期値（数２１参照）において、位置については、入力値の初期値（ｕ₀，ｖ_０，ｄ_０）から算出し、また、速度については、事前に行ったシミュレーションで得られた適合値（０．００）を用いた。

また、数２２に示す推定値の誤差の共分散行列の初期値については、共分散行列の一部の要素については、事前に行ったシミュレーションで求めた（ｕ，ｖ，ｄ）の分散（σ_ｕ ^２，σ_ｖ ^２，σ_ｄ ^２）に基づいて算出した。また、共分散行列のその他の要素については、当該シミュレーションで求めた適合値を直接採用した。

また、実験例では、判定部３４による入力値の判定として、数２８〜数３０の式を用いた。

以上の条件下において、０フレームから２０フレーム間に亘って、対象物推定装置１０による対象物の状態推定を行った。そして、他車両４４までの距離（Ｚ）の推定結果を図８のグラフに示す。なお、実験例では、９フレーム目において、視差情報ｄに異常値が生じたものとする。また、比較のため、カルマンフィルタを用いないで推定した結果（比較例１）と、視差情報ｄが異常値であっても補正部３６による補正を行わない条件で推定した結果（比較例２）とを併せて示す。なお、グラフ中の真値は、他車両４４までの実際の距離を示している。

図８のグラフから分かるように、フィルタを用いない比較例１では、視差情報ｄが異常値となった９フレームにおいて、真値に対して大きくズレていることが分かる。

また、推定値の補正を行わない比較例２については、８フレームまでは実験例と同様な軌跡を描いている。しかしながら、視差情報ｄが異常値となる９フレームでは、当該視差情報ｄから算出された推定値がそのまま採用されることで、推定結果が真値から外れていることが分かる。また、１０フレーム以降では、９フレームでの誤差を含む推定値の影響で、真値からズレた推定結果が続いている。

一方、実験例では、推定の開始から徐々に真値に近づき、視差情報ｄが異常値となる９フレームにおいても、真値に近い推定結果となっている。これは、９フレームでは、算出部３２が算出した推定値ではなく、予測値の値が推定値として採用されたためである。また、１０フレーム以降においても、真値に近い推定結果となっている。このように、実験例が、対象物の距離を最も正確に推定していることが分かる。

以上に説明した実施形態に係る対象物推定装置１０および対象物推定方法によれば、以下の効果を奏する。

（１）補正部３６は、判定部３４の判定結果に基づいて、算出部３２が算出した推定値を補正する。従って、推定値の精度が向上して、対象物の状態を正確に推定することができる。

（２）推移情報（ｕ，ｖ）が異常値であると判定部３４が判定した場合、補正部３６は、カルマンフィルタを初期状態にリセット（推定値を初期値に設定）すると共に、算出部３２が算出した推定値を無効値とする。これにより、異常値である推移情報（ｕ，ｖ）を用いて算出した推定値をそのまま出力するのを防止することができる。すなわち、トラッキングエラーが生じた画素から得られた推定値を無効として、対象物推定装置の信頼性を高めることができる。

なお、カルマンフィルタを初期状態にリセットすることで、トラッキングエラーにより追跡点が推移した後は、当該追跡点を初期状態から追跡することができる。すなわち、異常値が生じた追跡点とは異なる新たな追跡点として、推定を継続させることができる。

（３）、視差情報ｄは、フレーム毎に算出されて、視差情報ｄの異常値もフレーム単位で独立して発生する。このような視差情報ｄの特性に鑑み、視差情報ｄが異常値である場合、補正部３６は、算出部３２が算出した推定値よりも精度の高いと考えられる予測値を推定値として設定する。これにより、視差情報ｄが異常値である場合に、対象物の推定値の誤差を小さくすることができる。

しかも、補正部３６は、推移情報（ｕ，ｖ）に異常値が生じた場合のようにカルマンフィルタを初期状態にリセットすることはない。従って、次のフレームでは、予測値に設定された推定値を用いて当該画素についての推定を継続することができる。

（４）判定部３４は、推移情報算出部２８が算出した推移情報（ｕ，ｖ）と、予測値から算出される予測推移情報との差（数２４，数２５または数２８，数２９参照）に基づいて、当該推移情報（ｕ，ｖ）が異常値であるか否か判定する。従って、推移情報算出部２８が算出した推移情報（ｕ，ｖ）が異常値であるか否かを判定部３４が適切に判定することができる。

（５）判定部３４は、視差情報算出部３０が算出した視差情報ｄと、予測値から算出される予測視差情報との差（数２６または数３０参照）に基づいて、当該視差情報ｄが異常値であると判定する。従って、視差情報算出部３０が算出した視差情報ｄが異常値であるか否かを判定部３４が適切に判定することができる。

（６）ここで、判定部３４が判定する際の閾値として、数２４〜数２６に示す固定値を採用した場合には、推移情報（ｕ，ｄ）および視差情報ｄの判定を効率的に行うことができ、判定部３４の処理負担を軽減することができる。

（７）一方、閾値として数２８〜数３０に示す変数値を採用した場合には、観測値や推測値のばらつきを考慮して適切な判定をすることができる。これにより、例えば、対象物が遠方にあって視差情報ｄがノイズの影響を受け易い場合には、閾値の値が大きくなって、異常値であると判定され難くするといったことが可能となる。一方、対象物が近くにあって視差情報ｄに対するノイズの影響が小さい場合には、閾値が小さくなって、視差情報ｄの異常値を正確に捉えることが可能となる。

（変更例）
前述した実施形態に係る対象物推定装置は、以下のように変更することが可能である。

・実施形態では、フィルタとして、予測（数１２〜数１４）および更新（数１５〜数２０）を繰り返すことで対象物の状態を推定するカルマンフィルタを採用した。しかしながら、カルマンフィルタとしては、実施形態に限定されるものではなく、例えば、アンセンテッドカルマンフィルタやパーティクルフィルタ等を採用してもよい。

・また、フィルタとしては、実施形態のようにカルマンフィルタに限定されるものではなく、例えばＨ^∞フィルタ等の他のフィルタを採用することができる。

・実施形態では、画像取得手段として、第１カメラおよび第２カメラを備えた画像取得部を例示した。しかしながら、画像取得手段としては、異なる位置から複数の画像を取得するものであればよく、３つ以上のカメラを備えていてもよい。また、実施形態では、第１カメラおよび第２カメラが取得した第１画像および第２画像から視差情報を算出したが、３つ以上のカメラで撮影した３つ以上の画像から視差情報を算出するようにしてもよい。

・実施形態では、第１カメラが取得した画像を基準画像として、対象物の状態を推定したが、第２カメラが取得した画像を基準画像として推定値を算出してもよい。この場合、第２カメラが第１撮影手段、第１カメラが第２撮影手段となり、第２カメラが撮影した画像が第１の画像、第１カメラが撮影した画像が第２の画像となる。

・実施形態では、第１の画像の連続するフレームから推移情報を算出したが、時間的に前後するフレームであれば、必ずしも連続するフレームから推移情報を算出する必要はない。例えば、１フレームおいて前後するフレーム間（すなわち、ｔフレームおよびｔ+２フレーム）から推移情報を算出するようにしてもよい。

・実施形態では、第１カメラおよび第２カメラが自動車の走行方向の前方を撮影して、自動車の前方の対象物を推定する場合を示した。しかしながら、例えば、第１カメラおよび第２カメラが自動車の側方（左右方向）や後方を撮影するようにしてもよい。これにより、自動車の前方以外の対象物（例えば、後方車両）について、状態を推定することが可能となる。

・実施形態では、対象物推定装置を普通乗用車の自動車に搭載した場合を例示したが、対象物推定装置は、バスやトラック等の大型車や自動二輪車等、あらゆる車両に搭載することができる。また、対象物推定装置をシミュレーション装置等の実験施設に搭載してもよい。

・実施形態では、車速センサで並進運動を算出したが、第１撮影手段で取得された第１の画像または第２撮影手段で取得された第２の画像から自動車の並進運動を算出してもよい。同様に、回転運動についても、ヨーレートセンサを採用することなく、第１の画像または第２の画像から自動車の回転運動を算出してもよい。

・実施形態では、第１の画像の所定範囲の画素についてカルマンフィルタを実行し、推定値を算出したが、第１の画像の全ての画素について推定値を算出してもよい。

１２…画像取得部、２８…推移情報算出部、３０…視差情報算出部、３２…算出部、３４…判定部、３６…補正部、ｄ…視差情報。

Claims (10)

1. 画像取得手段（12）が異なる位置から撮影した複数の画像に基づいて、当該画像中の対象物の位置および速度を推定する対象物推定装置であって、
   前記複数の画像のうち基準となる基準画像において、時間的に前後するフレームから対応する画素の位置の時間的な推移情報を取得する推移情報取得手段（28）と、
   前記複数の画像から対応する各画素の視差情報を前記基準画像を基準として取得する視差情報取得手段（30）と、
   前記推移情報取得手段が取得した推移情報と、前記視差情報取得手段が取得した視差情報とに基づいて、前記対象物の３次元空間上の位置および速度の推定値をフィルタを用いて推定する推定値取得手段（32）と、
   前記推移情報取得手段が取得した推移情報および前記視差情報取得手段が取得した視差情報が、それぞれ異常値であるか否か判定する判定手段（34）と、
   前記判定手段の判定結果に基づいて、前記推定値取得手段が取得した前記推定値を補正する補正手段（36）とを備え、
   前記補正手段は、前記推移情報取得手段が取得した前記推移情報が異常値であると前記判定手段が判定した場合と、前記視差情報取得手段が取得した前記視差情報が異常値であると前記判定手段が判定した場合とで、前記推定値取得手段が取得した推定値を異なる方法で補正する
   ことを特徴とする対象物推定装置。
2. 前記補正手段は、前記推移情報取得手段が取得した前記推移情報を前記判定手段が異常値であると判定した場合に、前記推定値取得手段が取得した前記推定値を無効にすると共に、前記フィルタを初期状態にリセットする請求項１記載の対象物推定装置。
3. 前記推定値取得手段は、前記対象物が予め設定したプロセスモデルに従うと仮定して、過去に取得した前記推定値から前記対象物の位置および速度の予測値を取得し、
   前記補正手段は、前記視差情報取得手段が取得した前記視差情報が異常値であると前記判定手段が判定した場合に、前記推定値取得手段が取得した前記推定値を当該推定値取得手段が取得した前記予測値の値に補正する請求項１または２記載の対象物推定装置。
4. 前記判定手段は、前記推移情報取得手段が取得した前記推移情報と、前記推定値取得手段が取得した前記予測値から算出され、前記推移情報に対応する予測推移情報との差に基づいて、当該推移情報が異常値であるか否か判定する請求項３記載の対象物推定装置。
5. 前記判定手段は、前記視差情報取得手段が取得した前記視差情報と、前記推定値取得手段が取得した前記予測値から算出され、前記視差情報に対応する予測視差情報との差に基づいて、当該視差情報が異常値であるか否か判定する請求項３または４記載の対象物推定装置。
6. 画像取得手段（12）が異なる位置から撮影した複数の画像に基づいて、当該画像中の対象物の位置および速度を推定する対象物推定方法であって、
   前記複数の画像のうち基準となる基準画像において、時間的に前後するフレームから対応する画素の位置の時間的な推移情報を推移情報取得手段（28）が取得するステップ（S18）と、
   前記複数の画像から対応する各画素の視差情報を前記基準画像を基準として視差情報取得手段（30）が取得するステップ（S20）と、
   前記推移情報取得手段が取得した推移情報と、前記視差情報取得手段が取得した視差情報とに基づいて、前記対象物の３次元空間上の位置および速度の推定値をフィルタを用いて推定値取得手段（32）が推定するステップ（S24）と、
   前記推移情報取得手段が取得した推移情報および前記視差情報取得手段が取得した視差情報が、それぞれ異常値であるか否か判定手段（34）が判定するステップ（S26，S28）と、
   前記判定手段の判定結果に基づいて、前記推定値取得手段が取得した前記推定値を補正手段（36）が補正するステップ（S32，S34，S36）とを備え、
   前記補正手段は、前記推移情報取得手段が取得した前記推移情報が異常値であると前記判定手段が判定した場合と、前記視差情報取得手段が取得した前記視差情報が異常値であると前記判定手段が判定した場合とで、前記推定値取得手段が取得した推定値を異なる方法で補正する
   ことを特徴とする対象物推定方法。
7. 前記補正手段は、前記推移情報取得手段が取得した前記推移情報を前記判定手段が異常値であると判定した場合に、前記推定値取得手段が取得した前記推定値を無効にすると共に（S34）、前記フィルタを初期状態にリセットする（S32）請求項６記載の対象物推定方法。
8. 前記推定値取得手段は、前記対象物が予め設定したプロセスモデルに従うと仮定して、過去に取得した前記推定値から前記対象物の位置および速度の予測値を取得し、
   前記補正手段は、前記視差情報取得手段が取得した前記視差情報が異常値であると前記判定手段が判定した場合に、前記推定値取得手段が取得した前記推定値を前記推定値取得手段が取得した前記予測値の値に補正する（S36）請求項６または７記載の対象物推定方法。
9. 前記判定手段は、前記推移情報取得手段が取得した前記推移情報と、前記推定値取得手段が取得した前記予測値から算出され、前記推移情報に対応する予測推移情報との差に基づいて、当該推移情報が異常値であるか否か判定する請求項８記載の対象物推定方法。
10. 前記判定手段は、前記視差情報取得手段が取得した前記視差情報と、前記推定値取得手段が取得した前記予測値から算出され、前記視差情報に対応する予測視差情報との差に基づいて、当該視差情報が異常値であるか否か判定する請求項８または９記載の対象物推定方法。

Images