JP7635116B2 - 外界認識装置 - Google Patents

Description

本発明は、本発明は外界認識装置に関する。

現在、様々な運転支援システムが開発され製品化されている。これらの運転支援システムの一つとして、衝突被害軽減ブレーキシステムがある。衝突被害軽減ブレーキシステムは、車両に搭載されたセンサによって、車両の周辺で動く物体（動物体）を精度よく検出し、その動物体を追跡する必要がある。

動物体を追跡する手法として、カルマンフィルタを用いた手法が知られている。カルマンフィルタを用いた手法は、所定の時刻までの動物体の位置の変化に基づいて、所定の時刻よりも後の時刻における動物体の位置を予測することができる。

しかし、この手法では、得られた観測（検出した動物体の位置）に対する観測誤差を正確に設定できない場合、予測の精度が低下する。画像に基づいて検出された動物体の情報を利用する場合、その画像において動物体が検出された位置に応じて、観測誤差の大きさは変化する。また、画像を撮影するカメラの画角や取付け角度によっては、追跡対象となる動物体が画像の外側に外れてしまって検出できない場合がある。これらの場合、動物体の追跡に失敗し、自車両が動物体と衝突するおそれがある。

移動体に取り付けられたカメラを用いて動物体、特に人を追跡する方法として、次の手法が提案されている（例えば、特許文献１参照）。

すなわち、移動体に取り付けられたカメラ装置により、カメラ装置の視野範囲内で、移動体の周辺の画像が一定の時間間隔で撮影される。そして、画像処理システムにより、撮影して得られた時系列で前後する複数の画像に基づいて、追跡対象である動物体に対応する画素が特定される。

動物体のそれぞれについて、動物体が道路の面に接する空間点に対応する、画像座標への投影を表す画素が特定される。道路面に接する空間点の動きは、道路面に接する空間点の位置と道路面の関係速度とを成分とする、少なくとも４次元の状態ベクトルを用いた状態推定器によって追跡される。

状態推定器による動きの追跡は、状態ベクトルごとに予測を生成する工程と、この予測に適した投影方程式を介して画像座標に変換する工程と、この予測に予想される誤差を共分散行列によって画像座標内で計算する工程と、この予測をより最近の画像内で抽出された画素と比較して更新する工程とにより行われ、この動きの追跡によって動物体を追跡する。

特開２００９－０８９３６５号公報

衝突被害軽減ブレーキシステムのように、周囲の動物体の位置から自車両までの距離を計算して衝突までの時間を推定し、推定した時間に基づいて、衝突の危険がある場合に自動的にブレーキをかけるシステムは、動物体までの距離を正確に計測し、追跡する必要がある。このため、衝突被害軽減ブレーキシステムを実現するためには、継続して追跡するだけでなく、誤った検出結果を追跡しないことも重要となる。

特許文献１により提案された技術を自動駐車システムに適用した場合、自車両の近傍では観測値に対して予想される誤差が大きくなり、自車両の直近の位置がずれた検出結果と対応付けられる。この結果、追跡中の動物体までの距離が正確に計測できないため、自車両は動物体と衝突するおそれがある。

本発明は上記事情に鑑みなされたものであり、カメラによって取得した画像に基づいて、精度の高い動物体の追跡を行うことができる外界認識装置を提供することを目的とする。

本発明は、カメラを備えた移動体の周囲の動物体を検出、追跡する外界認識装置であって、前記カメラにより撮影した画像に基づいて、前記動物体の位置を観測値として検出する物体位置検出部と、前記物体位置検出部により検出した前記観測値に基づいて、前記画像における前記動物体が属する領域を判定する領域判定部と、前記領域判定部による判定の結果に基づいて、観測値に対する誤差を計算する観測誤差設定部と、現在時刻より前の時刻である前時刻までの前記観測値と前記観測誤差設定部で計算された誤差とに基づいて、前記現在時刻における前記動物体の状態を予測する状態予測部と、前記状態予測部により予測した前記現在時刻における前記動物体の状態と前記物体位置検出部により検出した前記観測値とを対応付ける対応付け部と、前記対応付け部によって対応付けた結果に基づいて、前記動物体の状態を更新する状態更新部と、を備えた外界認識装置である。

本発明に係る外界認識装置によれば、カメラによって取得した画像を利用して、精度の高い動物体の追跡を行うことができる。

本発明の一実施形態に係る外界認識装置を示すブロック図である。 動物体の検出結果として人、車両を例とした模式図である。 検出結果の画像中位置の定義を示した図である。 実施形態１の処理の流れを示すフローチャートである。 世界座標系の定義を示した図である。 移動体座標系の定義を示した図である。 画像の分割された領域の一例を示す図である。 領域を分割する基準の一例として、画像を縦方向に走査する様子を示す模式図である。 図７Ａの走査により、画像中のｙ座標が１［ピクセル］変化するごとに、人までの距離がどの程度変化するかの対応関係を計算により求めて、曲線で近似したグラフである。 自車両に対して検出対象（例えば人）が相対的に遠方にいる場合の例を示す俯瞰画像である。 自車両に対して検出対象（例えば人）が相対的に近くにいる場合の例を示す俯瞰画像である。 図８Ａに示した検出対象及び図８Ｂに示した検出対象をそれぞれ投影した移動体座標系の図である。 検出結果の画像中位置から自車両までの距離計算方法を示した図である。 対応付けの例を示した図である。 見切れ結果の例を示した図である。 カルマンフィルタの更新処理のフローチャートである。 カルマンフィルタの状態管理テーブルの例を示す図である。

以下、本発明に係る外界認識装置の具体的な実施形態について、図面を参照して説明する。

［実施形態１］
図１は外界認識装置１０を示すブロック図である。図示の外界認識装置１０は本発明に係る外界認識装置の一実施形態である。外界認識装置１０は、自動車などの移動体に搭載されている。以下、外界認識装置１０が搭載される移動体を自車両と称して説明する。

外界認識装置１０は、外部システム４０と組み合わされて使用されるものである。また、外界認識装置１０は、予め決められた周期、例えば時間１００［ｍｓ］ごとの周期で動作する。そして、外界認識装置１０は、自車両から、速度、ハンドル角、シフト及びブレーキの状態などの車両情報を取得する機能と、カメラが撮影した画像を取得する機能と、を有する。

図１に示すように、外界認識装置１０は、物体位置検出部１１と、領域判定部１２と、観測誤差設定部１３と、対応付け部１４と、状態予測部１５と、状態更新部１６と、を備えている。外界認識装置１０は、例えば、ＣＰＵ、ＧＰＵ、ＲＡＭ及びＲＯＭ等を有しており、ＲＯＭに格納された所定のプログラムをＲＡＭに展開してＣＰＵが実行することにより、上述した物体位置検出部１１等の機能を実現する。なお、外界認識装置１０は、その機能の一部又は全部を、ＦＰＧＡ、ＡＳＩＣなどのハードウェアを用いて実現してもよい。

外界認識装置１０には、自車両に取り付けられたカメラによって撮影された画像２０と、自車両からＣＡＮなどを経由して取得した移動情報３０が入力される。画像２０は、自車両に搭載されたカメラによって所定のフレームレートで順次取得される。移動情報３０は、自車両から得られる自車両の移動に関する情報及び移動に関する情報に準ずる情報であり、例えば、車両速度、ハンドル角度、車両旋回半径、アクセル及びブレーキの状態、並びにシフトレバーの状態などである。移動情報３０も、画像２０と同様に、所定の周期で、順次取得される。

物体位置検出部１１は、画像２０に基づいて、人や車両など所望の動く物体（動物体）を、パターンマッチングなどの公知の手法を用いて検出する。図２は動物体の検出結果として人、車両を例とした模式図である。

図２に示すように、動物体の検出結果は、画像における動物体（人や車両）の位置を特定する情報である。具体的には、動物体を囲う矩形枠Ｆを定義する、矩形枠Ｆの鉛直（縦）方向の寸法である高さＨと矩形枠Ｆの水平（横）方向の寸法である幅Ｗと矩形枠Ｆの特定の角部（本実施形態では、左上の角部）である矩形枠Ｆの原点Ｃの位置とにより規定された情報である。物体位置検出部１１は、検出結果を領域判定部１２及び対応付け部１４へ出力する。

図３は検出された動物体の、画像２０（左上角部を画像原点Ｏ（０，０）、水平右方をｘ軸の正方向、鉛直下方をｙ軸の正方向とする）における位置を説明する模式図である。領域判定部１２は、物体位置検出部１１から入力された検出結果により、検出された動物体（検出動物体）が、画像２０を複数の領域に分割したときの、画像２０におけるいずれの領域に属しているかを判定する。

ここで、図３に示すように、検出動物体の位置は、上述した物体位置検出部１１によって特定された矩形枠Ｆの、下縁における水平方向の中心点（矩形枠Ｆの原点から、矩形枠Ｆの高さＨの位置で、矩形枠Ｆの幅Ｗの１／２の位置）を代表点とする。そして、領域判定部１２は、画像２０におけるこの代表点が属する領域を、動物体の位置として、観測誤差設定部１３に出力する。

観測誤差設定部１３は、領域判定部１２の結果に基づいて、観測値（画像２０に基づいて物体位置検出部１１が検出した動物体の位置）に含まれると想定される誤差を決定する。また、観測誤差設定部１３は、領域判定部１２の結果に基づいて、検出動物体の足元（足やタイヤの下部など）の一部など、画像に写っていない足切れや見切れなどにより、パターンマッチングによるパターン認識では検出できない領域であると判定された場合には、検出が無効であることを表す値を設定する。そして、観測誤差設定部１３は、設定した誤差や無効であることを表す値を、対応付け部１４に出力する。

対応付け部１４は、物体位置検出部１１の検出結果及び観測誤差設定部１３で設定された観測誤差に基づいて、状態推定器の状態予測部１５により予測された、現在時刻での動物体の位置（動物体予測位置）を対応付ける。このとき、対応付けられる動物体予測位置が無い場合には、新規に検出された動物体（新規検出動物体）とする。対応付け部１４は、ここで対応付けた結果と新規検出動物体の情報とを、状態更新部１６に出力する。

状態予測部１５は、現在時刻よりも前の時刻（前時刻）までに物体位置検出部１１により検出された結果を利用して、状態更新部１６で更新することにより得られた検出動物体の状態に基づいて、動物体の現在時刻の状態を予測する。ここで、状態は少なくとも位置と速度を持つものとする。状態予測部１５による予測結果は対応付け部１４において利用される。

状態更新部１６は、状態予測部１５と物体位置検出部１１の結果とを、対応付け部１４において対応付けた結果を利用した状態の更新や、新規検出動物体とされた動物体の追跡を開始する。更新された状態は外部システム４０へ出力され、外部システム４０は、この結果により、例えば自車両と人（動物体の一例）までの距離に基づいて、衝突までに要する時間を計測し、衝突までに要する時間以内に停止できるように、自動でブレーキを作動させるシステムにおいて利用する。

次に、外界認識装置１０の動作についてフローチャートを参照して説明する。図４は外界認識装置１０による処理全体の流れを示すフローチャートである。また、本実施形態は、自車両の後方にカメラが搭載されている状態を想定して説明する。

ここで、フローチャートの説明に先立って、外界認識装置１０の処理で用いられる座標系（世界座標系、移動体座標系）について説明する。

図５Ａは外部システム４０が起動したときの自車両の位置を原点とし、外部システム４０の起動時における自車両の後方を＋ｘ_w、＋ｘ_wに対して側を＋ｙ_w、＋ｘ_wから＋ｙ_wへの回転を＋θ_wとする２次元のｘ－ｙ平面上の値とする世界座標系を示す図、図５Ｂは外界認識装置１０が自車両の位置及び姿勢を表すために使用する、自車両の後方を＋ｘ_v、＋ｘ_vに対して側を＋ｙ_v、＋ｘ_vから＋ｙ_vへの回転を＋θ_vとする２次元のｘ－ｙ平面上の値とする移動体座標系を示す図である。移動体座標系は、ｘ軸の＋方向とｙ軸の＋方向とθの＋回転方向については世界座標と同じであるが、自車両の動きに伴って原点は移動する。

次に、図４のフローチャートについて説明する。フローチャートの全体処理は、外部システム４０が立ち上ったことにより開始される。

図４のフローチャートに示す一例を、自車両の後方を向いて自車両に取り付けられた単眼カメラ（以下、単にカメラという。）を利用するものとして説明する。また、このカメラ、自車両から取得されるデータの周期は３３［ｍｓ］とする。また、カメラによって得られる画像のサイズは、横（幅）６４０［ピクセル］×縦（高さ）４００［ピクセル］とする。車両情報は、自車両がどのように移動したかを計算するために必要となる情報である。さらに、検出対象の動物体の位置を予測するモデルは、等速運動を仮定したモデルを利用する。

物体位置検出部１１は、ステップ１０１（Ｓ１０１）において、Ｓ１００において読み込んだ画像２０に基づいて追跡対象となりうる動物体を検出する。ここでは、追跡対象の動物体を、例えば人と車両とする。

本実施形態では、追跡対象となりうる動物体の検出を、公知の手法であるHistograms of Oriented Gradients（ＨｏＧ）と、Support Vector Machineを（ＳＶＭ）を利用する。

ここで、ＨｏＧは、局所領域の画素値の勾配方向ごとの勾配強度を計算してヒストグラム化したもので、このヒストグラムを特徴量としたものをＨｏＧ特徴量という。ＨｏＧ特徴量は勾配を特徴量としているため、画像２０のスケール変化に対して頑健であるという利点がある。一方、ＨｏＧ特徴量は回転変化に弱いという欠点がある。

ＳＶＭは、入力に対して正解か不正解かの別を分類する分類器である。ＳＶＭは、大量のデータとラベルとの組に基づいて、データとラベルとの関係を学習することにより、正解と不正解とを分類するための境界を学習し、未知のデータが入力されたときに、学習した境界を利用することにより、入力されたデータが正解か不正解かの別を判断する。

本実施形態では、ＨｏＧ特徴量と、人及び車両の各ラベルを学習して分類器を作成する。また、検出結果は図２に示す通り、検出結果の原点及び検出結果を囲む矩形枠Ｆを描画するための幅Ｗと高さＨ、人や車両といったラベルを想定する。

それぞれの技術は公知のものであるため、詳細の説明は省略する。また、人、車両を検出する手法としては、パターン認識などの他の公知の手法を用いてもよく、検出手法は上述した手法に限定しない。また、動物体としては人や車両以外のものを検出対象、追跡対象としてもよい。

フローチャートのＳ１０１において、図３のように動物体の検出結果により、画像２０中における動物体の位置を検出する。画像２０の座標は図３に示しているように、画像２０の下方向が＋ｙ、画像２０の右方向が＋ｘとなる。画像中の位置（ｘ，ｙ）は、画像原点Ｏ（０，０）に、幅Ｗ／２（ｘ軸方向）、高さＨ（ｙ軸方向）を加算したものとする。

図６は画像２０の分割された領域Ａ１，Ａ２，Ａ３，Ａ４の一例を示す図、図７Ａは領域を分割する基準の一例として、画像２０を縦方向に走査する様子を示す模式図、図７Ｂは図７Ａの走査により、画像２０中のｙ座標が１［ピクセル］変化するごとに、人までの距離がどの程度変化するかの対応関係を計算により求めて、曲線で近似したグラフである。

Ｓ１０２の領域の判定は、図６に示すように、画像２０を例えば縦方向の４つの領域Ａ１，Ａ２，Ａ３，Ａ４に区切った際に、検出結果（動物体）の位置がいずれの領域に属しているかを判定する処理である。領域Ａ１～Ａ４の分割は、例えばカメラのレンズの歪みと自車両へのカメラの取付姿勢に基づいて、画像２０中におけるｙ軸方向の位置ごとに、１［ピクセル］のずれに対応した距離の誤差を、図７Ｂの曲線に基づいて決定する。

図６の例では、４つの領域Ａ１，Ａ２，Ａ３，Ａ４に区切った。領域Ａ１は、検出動物体の位置が１［ピクセル］ずれることで数メートルの距離の誤差が生じる又は空が映る領域、領域Ａ２は、検出動物体の位置が１［ピクセル］ずれることで領域Ａ１よりも小さいが相対的に大きな誤差が生じる領域、領域Ａ３は、検出動物体の位置が１［ピクセル］ずれることで領域Ａ２よりも小さい誤差が生じる領域、領域Ａ４は自車両と検出対象までの距離が物理的に近く、検出対象を正しく検出できないと考えられる領域である。

また、４つの領域Ａ１，Ａ２，Ａ３，Ａ４に区切った分割の基準（境界）は、画像２０における消失点（図６参照）よりも上方を領域Ａ１、１［ピクセル］の誤差で距離約０．８［ｍ］の移動体座標系上での検出位置が変化する地点から領域Ａ１までを領域Ａ２、領域Ａ２から１［ピクセル］の誤差で距離約０．２［ｍ］の移動体座標系上での検出位置が変化する地点から領域Ａ２までを領域Ａ３、領域Ａ３より下方を領域Ａ４として設定した。

領域判定部１２は、検出結果の画像２０における位置が領域Ａ１～Ａ４のうちいずれの領域にあるかを判定して（Ｓ１０２）、観測誤差設定部１３に出力する。さらに、領域Ａ１のような検出誤差が非常に大きい位置にある検出動物体又は位置が地面上にないと考えられる検出動物体については、追跡対象としないで削除する。

ここで、領域Ａ１～Ａ４の分割方法は、カメラのレンズの歪みやカメラの取付位置といった物理的なパラメータを利用したものに限定しない。また、分割する領域Ａ１～Ａ４も縦方向だけでなく横方向に分割したものと混在させてもよく、分割の数も４つに限定しない。

Ｓ１０３において、状態推定器により追跡中の動物体の、現在時刻における世界座標系上での位置を予測する。状態推定器として用いられるものとしては、カルマンフィルタ、拡張カルマンフィルタ、アンセンテッドカルマンフィルタなどがあるが、本実施形態では、カルマンフィルタを利用する例で説明する。

まず、本実施形態におけるカルマンフィルタは、等速運動モデルを利用するため、推定する状態は４次元ベクトルｘ＝（ｘ，ｙ，ｖｘ，ｖｙ）となる。また、ｘ，ｙは世界座標系上での位置、ｖｘ，ｖｙは世界座標系上での速度を示している。現在時刻の世界座標系上での位置及びこの位置に対する誤差共分散行列の値は以下の式（１），（２）により予測する。

ここで、添字のｔは時刻を、－は予測値であること、＋は観測を基に更新された値であること、をそれぞれ表している。また、式（１），（２）において、Ｆは状態遷移行列を、Ｑはシステムノイズ、をそれぞれ表しており、本実施形態では式（３），（４）で表される。

式（３）中のΔｔは処理周期、式（４）中のσ² _sysytemnoiseは本実施例のモデルを適用した際に想定される誤差である。Ｓ１０３の処理は、無効状態ではない全てのカルマンフィルタについて行う。

Ｓ１０４では、車両からＣＡＮ（Control Area Network）など任意の方法で移動情報３０を取得する。ここで、移動情報３０は、Ｓ１０５で移動距離を計算するために必要となる情報である。例えば、自車両の速度、タイヤの回転数、ハンドルの転舵量などである。

Ｓ１０５では、Ｓ１０４で取得した移動情報３０を利用して自車両の位置を推定する。本実施形態では、タイヤ回転数とタイヤ径から計算した移動距離と、ハンドルの転舵量から推定した旋回半径を利用して、式（５）～（８）により車両位置を推定する。ここで、自車両は２次元平面上の移動を仮定しており、状態としてpose_car＝（ｘ_carpose，ｙ_carpose，θ_yaw）の３状態を持つものとする。

ここで、ｔは時刻、Ｒは旋回半径、ρは曲率、ΔＳは移動距離、をそれぞれ表している。自車両の位置を推定する方法として、他のモデルを利用する方法や、画像２０を利用して自車両の位置や姿勢を推定するVisual Odometryなどを用いてもよい。

Ｓ１０６において、領域判定の結果に基づいて観測結果の誤差を設定する。本実施形態では、領域Ａ１及び領域Ａ４に関しては、無効な検出結果として観測結果の誤差として無効な値を設定する。領域Ａ２に関しては図８のように誤差を動的に設定する。誤差の設定の手順は、まず画像上で想定される検出位置の誤差ｘ，ｙを検出対象の画像２０中の座標へ付加する。その後、誤差を加えた画像２０中の座標をピンホールカメラモデルに従って、カメラの姿勢と地面からのカメラの高さを利用して移動体座標系上に投影する。

図８Ａは自車両に対して検出対象（例えば人）が相対的に遠方にいる場合の例を示す俯瞰画像、図８Ｂは自車両に対して検出対象（例えば人）が相対的に近くにいる場合の例を示す俯瞰画像、図８Ｃは図８Ａに示した検出対象及び図８Ｂに示した検出対象をそれぞれ投影した移動体座標系の図である。

誤差を加えた画像２０中の座標を移動体座標系上へ投影すると、図８Ａの俯瞰画像において、自車両に対して検出対象（例えば人）が相対的に遠方にいる場合には、検出対象は図８Ｃに示すように自車両から遠い位置へ投影される。一方、図８Ｂの俯瞰画像において、自車両に対して検出対象が相対的に近くにいる場合には、検出対象は図８Ｃに示すように自車両に近い位置へ投影される。

この移動体座標系（図８Ｃ）に投影された、想定誤差を付加した画像中心位置を、観測誤差の値として、自車両に対して前後方向に投影されたｘ方向の差の平均をｘ方向の観測誤差、左右方向に投影されたｙ方向の差の平均をｙ方向の観測誤差の値とする。

検出対象の移動を等速運動によりモデル化するため、観測誤差を以下のように表現する。本実施形態では、移動体座標系に従いσ_xを自車両から見て前後方向の観測誤差、σ_yを自車両から見て左右方向の観測誤差とする。

また、画像中のｘ座標の変化により移動体座標系上でのｙ位置が変化する場合、また反対に、ｙ座標の変化により移動体座標系上でのｘ位置が変化する場合には、以下のように共分散値を設定してもよい。

領域Ａ３では、領域Ａ２と同じ方法で観測誤差を設定した場合、値が小さくなり過ぎることが考えられる。このため、領域Ａ２と同じ方法で観測誤差を計算した後、予め設定した最小値と比較し、最小値よりも小さかった場合には最小値を利用することとする。

上記のように観測誤差を動的に推定して設定することで、遠方の追跡対象に対する追従性の向上、及び車両近傍で誤検知した検出対象への追従性を低下させる効果がある。

Ｓ１０６による観測誤差の設定はＳ１０２で処理対象となった全てのＳ１０１の検出結果に対して行う。

Ｓ１０７において、状態推定器により予測した追跡中の動物体の現在時刻での位置と、画像２０中から検出した検出対象の画像２０中の位置とを、世界座標系に投影した結果に基づいて対応付ける。

図９は検出結果の画像中の位置から自車両までの距離の計算方法を示した図である。世界座標系の計算は、図９のように、地面から自車両に搭載されたカメラの原点までの高さと、自車両に対するカメラ取付位置、姿勢及びカメラの内部パラメータとを利用して、ピンホールカメラモデルと、検出した動物体は自車両と同じ平面上に存在するという仮定に基づき、カメラ原点と画像２０中の位置を結んだ直線と地面の交点を移動体座標系上での足元位置（ｘ_car，ｙ_car）として計算する。

次に、計算して得られた移動体座標系上の足元位置を、世界座標系上に変換する。世界座標系は、外部システム４０が起動された時点での位置を原点とした座標系であるため、現在時刻での自車両の位置と姿勢に基づいて変換すればよい。このため、式（７）で求めた（ｘ_carpose，ｙ_carpose）を、図９に示した方法により求めた移動体座標系上の足元位置に対して足すことで、移動体座標系から世界座標系に変換する。

加えて、観測誤差の値も自車両の向きに合わせて回転させる。ここで、固定カメラを利用して本発明を実施する場合は、図５において定義した世界座標系と移動体座標系とは常に一致するため、式（５）～（８）及び式（１１），（１２）を実施する必要はない。

図１０は対応付けの例を示した図である。画像から検出した結果のすべてに対して座標変換後、図１０に示すように、状態推定器により予測した追跡中の動物体の現在時刻での位置と、画像２０中から検出した検出対象の世界座標系上での位置に基づいて対応付ける。

本実施形態では、対応付けるための距離の指標として、統計学で用いられる距離の一種であるマハラノビス距離を利用する。マハラノビス距離は、あるデータ標本の分散共分散値に基づいて、新たなデータの類似性を明らかにするのに適した指標となる。マハラノビス距離の計算を以下に示す。

ここで、distはマハラノビス距離、ｚ_predはカルマンフィルタによる予測位置（ｘ_pred，ｙ_pred）、ｚ_detectは画像２０中から検出した動物体の位置（ｘ_world，ｙ_world）である。この処理を（カルマンフィルタの予測数）と（画像から検出した動物体の数）との積の数だけ繰り返して、画像２０から検出した動物体に対してマハラノビス距離が最小となるカルマンフィルタ予測値を対応付ける。

このとき、画像２０から検出した動物体の１つに対してカルマンフィルタを複数対応付いてもよい。画像２０から検出した動物体の１つに対してカルマンフィルタを複数対応付けることにより、カルマンフィルタを不要に削除することを防ぐ。

また、これ以外の対応付け方法として、ハンガリアンアルゴリズムなど公知の手法を用いてもよい。また、対応付けの指標として、ユークリッド距離等を用いてもよい。

図１１は見切れ結果の例を示した図である。Ｓ１０７では、Ｓ１０２での領域判定の結果、図１１のように見切れが発生して対象全体が映っていないと考えられる領域（実施形態では領域Ａ４）と対応付いたカルマンフィルタは、カルマンフィルタの予測値を利用してオプティカルフロー、メディアンフローなどの代替手段を利用して検出を試みる。

ここで、代替手段により検出できた場合には、代替手段で検出した結果に基づいて、Ｓ１０７の処理により世界座標系上での位置及びマハラノビス距離を計算して観測値として利用する。

オプティカルフローによる検出は、カルマンフィルタの予測値の内部状態ｘ^-＝（ｘ，ｙ，ｖｘ，ｖｙ）のうちｘ、ｙを画像２０上に投影して矩形枠Ｆを描き、矩形枠Ｆ内の速度ベクトルを計算する。矩形枠Ｆを描くために必要となる幅Ｗと高さＨは、カルマンフィルタに最後に対応付いた観測値が持つ高さＨ及び幅Ｗを利用すればよい。

ここで、代替手段で検出できなかった場合には観測値無しとする。オプティカルフロー等の手法は公知の手法であるため説明は省略する。

Ｓ１０８では、カルマンフィルタの予測値に対して対応付いた検出結果を用いて追跡中の動物体の状態を更新する。本実施形態では、カルマンフィルタの状態を追跡開始、追跡候補、追跡中、補間中及び無効という５つの状態で管理する。

追跡開始は、新規に動物体を検出した際の状態で検出した動物体の追跡を開始するためにカルマンフィルタの状態を初期化する。追跡候補は、検出開始から時間が経過しておらず、カルマンフィルタの内部状態が安定していない可能性が高い状態を指す。

追跡中は、検出開始から時間が経過しており、カルマンフィルタの内部状態が安定している状態を指す。補間中は、検出動物体の追跡を開始した後に、何らかの理由により追跡対象が未検知状態になっていることを指す。無効は、対象のカルマンフィルタが何も追跡していない状態を指す。また、カルマンフィルタは内部状態として、内部状態ｘ、誤差共分散行列Ｐ、補間回数Intpを持つこととする。

図１２はカルマンフィルタの更新処理のフローチャートである。図１２に示すように、まず、Ｓ２０１でカルマンフィルタに対応付く検出結果(観測値)があるか否かを確認する。観測値がある場合にはＳ２０２へ、無い場合にはＳ２０７へ移行する。

Ｓ２０２では、カルマンフィルタの予測値と観測値に基づいて、式（１４）で計算したマハラノビス距離を利用して棄却検定を行う。棄却検定には、ｘ二乗検定を用いた。ｘ二乗検定は、検定統計量が帰無仮説の下でｘ²分布に従うことを仮定して行う統計的検定である。

実施形態では、（ｘ，ｙ）からマハラノビス距離を求めているため、自由度２のｘ二乗分布となる。本実施形態では有意水準を５［％］としたため、ｘ二乗値は５．９９１４６となる。マハラノビス距離が５．９９１４６以上（又は５．９９１４６超え）の値を採っている場合は予測値から考えにくい対応付けであると判断してＳ２０３へ、５．９９１４６未満（又は５．９９１４６以下）の値を採っている場合はＳ２１０へ移行する。

また、ｘ二乗値が５．９９１４６以上の対応付けに関しては無効な対応付けとして、対応付いていた観測値は新規検出動物体とする。

図１３はカルマンフィルタの状態管理テーブルの例を示す図である。Ｓ２０３は、新規検出動物体である場合の処理であり、図１３に示すように、プログラム内部で保持しているテーブル（ｔ－１）から無効状態のものを探索する。

ここで、無効値(＝テーブルに空き)がある場合（Ｓ２０４でＹｅｓ）はＳ２０５へ、無効値が無い場合（Ｓ２０４でＮｏ）はＳ２０６へ移行する。Ｓ２０５では、テーブルの中から無効値となっている場所へ、新規検出動物体の追跡用にカルマンフィルタを新規に登録して、内部状態ｘ及び誤差共分散行列Ｐを初期化した後に、テーブルの状態を追跡開始とする。

内部状態の初期化は以下のように観測値の世界座標系を代入する。速度成分は０とする。誤差共分散行列Ｐは予め設定した値、例えば、０．１を代入する。補間回数Intpには、例えば０を代入する。

Ｓ２０６はテーブルに空きが無い場合の処理である。テーブルの空きが無い場合は、予め決めた基準により、最も不要と考えられる追跡中の動物体を削除し、代わりに新規に検出した動物体を登録（新規検出動物体の登録）する。

本実施形態では、まず、新規検出動物体と自車両の間のユークリッド距離と、テーブルに登録されている追跡中の動物体と自車両の間のユークリッド距離とを比較する。そして、最もユークリッド距離が大きいものが、テーブルに登録されている追跡中の動物体の場合は、自車両から最も遠い追跡中の動物体を衝突の可能性が最も低いと判定し、これを最も不要と考えられる追跡中の動物体として削除する。その後、新規検出動物体を、Ｓ２０５と同じ処理により登録する。

最も遠い物体が新規検出動物体の場合には、その動物体を登録しないで終了する。ここで、テーブルから削除する条件は、ユークリッド距離の大小に限定されるものではなく、ユークリッド距離以外の、例えば、追跡中の動物体の状態（補間中など）を適用してもよい。

Ｓ２０７では、観測値と対応付いてないカルマンフィルタに対して補間処理を行うか否かを判定する。判定は補間回数により行う。本実施形態では、補間回数が例えば３回以上の場合はＳ２０８へ、３回未満の場合はＳ２０９へ移行する。

Ｓ２０８では、補間回数がしきい値を超えたことにより、追跡対象が撮影範囲外へ移動するなどの理由から見失ったと判定して、追跡対象外とするため、該当するカルマンフィルタのテーブル中の状態を補間中から無効値へ変更する。

Ｓ２０９では、Ｓ１０３で予測した追跡中物体の予測値を更新後の値とする処理を行う。このとき補間回数を、１だけ加算する。

Ｓ２１０では、カルマンフィルタの状態により処理を判定する。状態が追跡中の場合、Ｓ２１１－ａへ、追跡開始、追跡候補、補間中のものに関してはＳ２１１－ｂへ移行する。Ｓ２１１－ａ及びＳ２１１－ｂは、カルマンフィルタへ対応付いた観測値に基づいて、内部状態を更新する。
まず、観測誤差Ｒ′と事前誤差共分散行列Ｐ^-、観測行列Ｈ、観測値の世界座標ｚを利用して、カルマンゲインＫを計算する。ここでＩは単位行列を表す。

Ｓ２１１－ａでは、処理を終了する。Ｓ２１１－ｂではＳ２１２へ移行する。ここまでの処理を実施した場合、追跡開始状態は追跡候補状態へ、補間中状態で初期速度が計算済みの場合は追跡中状態へ、初期速度が計算されていない場合は追跡候補状態に、テーブルの値を更新する。

Ｓ２１２では、カルマンフィルタの追跡開始時に０を代入した速度を計算するか否かを判定する。本実施形態では、初期速度の計算条件を３回以上カルマンフィルタが更新されていること、及び追跡開始位置から一定以上移動していることとする。

ここで、一定以上移動というしきい値は、Ｓ１０２で実施した観測値の領域判定結果に基づき決定する。本実施形態では、領域Ａ２で検出された場合は距離１．５[ｍ]、領域Ａ３で検出された場合は距離１．０[ｍ]とした。初期速度を推定する式を以下に示す。

式中のｎは追跡した回数、ｘ_world，ｙ_worldは追跡中に対応付いた観測値の世界座標値、framerateは撮影周期から計算したフレームレートとなる。本実施形態では３０［ｆｐｓ］となる。

ここで計算した値を、ｘ⁺のｖｘ、ｖｙに代入して初期速度とする。初期速度の計算後、追跡候補状態のカルマンフィルタを追跡状態に変更する。Ｓ１０８の一連の処理は、全てのカルマンフィルタ、新規検出動物体に対して行う。

Ｓ１０９では、追跡中の動物体の位置を外部システム４０へ出力する。本実施形態では、追跡中の動物体を最大３個出力する。出力対象となるのは追跡中、追跡候補、補間中の動物体のうち、自車両と追跡対象のユークリッド距離の近いものから順に、衝突の可能性が高いとして出力する。追跡開始状態の動物体に関しては、瞬間的な誤検知の可能性があるので出力しない。

以上、本発明の実施形態について説明したが、本発明は上述した実施形態に限定されるものではない。例えば、前述した実施形態は本発明を詳細に説明したものであり、必ずしも説明した全ての構成を備える必要はない。また、構成に他の実施形態の構成を加えることも可能ある。加えて、構成の一部について、追加、削除、置き換えが可能である。

１０ 外界認識装置
１１ 物体位置検出部
１２ 領域判定部
１３ 観測誤差設定部
１４ 対応付け部
１５ 状態予測部
１６ 状態更新部
２０ 画像
３０ 移動情報

Claims (10)

1. カメラを備えた移動体の周囲の動物体を検出、追跡する外界認識装置であって、
   前記カメラにより撮影した画像に基づいて、前記動物体の位置を観測値として検出する物体位置検出部と、
   前記物体位置検出部により検出した前記観測値に基づいて、前記画像における前記動物体が属する領域を判定する領域判定部と、
   前記領域判定部による判定の結果に基づいて、観測値に対する誤差を計算する観測誤差設定部と、
   現在時刻より前の時刻である前時刻までの前記観測値と前記観測誤差設定部で計算された誤差とに基づいて、前記現在時刻における前記動物体の状態を予測する状態予測部と、
   前記状態予測部により予測した前記現在時刻における前記動物体の状態と前記物体位置検出部により検出した前記観測値とを対応付ける対応付け部と、
   前記対応付け部によって対応付けた結果に基づいて、前記動物体の状態を更新する状態更新部と、を備え、
   前記領域判定部は、前記移動体に対する前記カメラの取付位置、取付姿勢及びレンズ歪みという物理的なパラメータに基づいて、前記画像において検出された前記動物体の位置に対応した誤差の大きさに応じて複数の領域に分割した前記画像を用いて、前記物体位置検出部により検出した前記画像における前記観測値が、前記複数の領域のうちいずれの領域に属するかを判定する、外界認識装置。
2. 前記複数の領域は、前記画像の消失点よりも上方の第１領域と、前記第１領域に隣接する下方に設定された第２領域と、前記第２領域に隣接する下方に設定された第３領域と、前記第３領域に隣接する下方に設定された第４領域と、であり、
   前記観測誤差設定部は、前記第１領域に対しては前記誤差として無効な値を設定し、前記第２領域に対しては前記誤差を動的に設定し、前記第３領域に対しては前記誤差を動的に設定し、設定された前記誤差が予め設定した最小値よりも小さいときは前記誤差を前記最小値に設定し、前記第４領域に対しては観測値に対する誤差として無効な値を設定する、請求項１に記載の外界認識装置。
3. 前記物体位置検出部により検出した前記画像における前記観測値が、前記複数の領域のうち前記動物体の全体を含まない可能性が高い領域と対応づけられているときは、前記動物体を代替手段により検出する、請求項１又は２に記載の外界認識装置。
4. 前記物体位置検出部により検出した前記画像における前記観測値が、前記複数の領域のうち前記動物体の全体を含まない可能性が高い領域と対応づけられていて、前記代替手段による前記動物体の検出の際に、前記状態予測部の予測値により算出した前記画像の領域内で検出を行う請求項３に記載の外界認識装置。
5. 前記物体位置検出部により検出した前記画像における前記観測値が、前記複数の領域のうち前記画像の消失点よりも上方の領域と対応づけられているときは、前記動物体の位置を削除する、請求項１から４のうちいずれか１項に記載の外界認識装置
6. 前記観測誤差設定部は、
   前記物体位置検出部により検出した前記画像における前記観測値に想定される誤差を、前記画像における前記観測値に、前記画像の上方向、下方向、右方向及び左方向のそれぞれに付加し、
   前記誤差を付加した後の前画像における前記観測値を、ピンホールカメラモデルを用いて移動体座標系上に投影し、
   その後、前記画像における前記観測値を移動体座標系に投影した位置との各方向への差の絶対値の平均を観測誤差として設定する、請求項１から５のうちいずれか１項に記載の外界認識装置。
7. 前記対応付け部は、
   前記状態予測部により予測した前記現在時刻における前記動物体の状態と前記物体位置検出部により検出した前記観測値との対応付けを、マハラノビス距離を用いて行い、
   前記対応付け部による対応付けの結果を、ｘ二乗検定により検証する、請求項１から６のうちいずれか１項に記載の外界認識装置。
8. 前記状態予測部は、前記移動体の移動情報を用いることにより、前記画像の座標上ではなく、システム起動時における地点を原点とした座標系上において、前記観測値を予測し、
   前記状態更新部は、前記状態予測部により予測した前記座標系上における前記観測値を更新して、対象の動物体を追跡する、請求項１から７のうちいずれか１項に記載の外界認識装置。
9. 前記状態予測部と前記状態更新部とを有する状態推定器において、追跡する対象が速度を含む内部状態を有する場合、前記状態更新部は、各状態推定器に対応付いた観測値に基づいて、前記追跡する対象の移動速度を計算する、請求項１から８のうちいずれか１項に記載の外界認識装置。
10. 前記状態予測部と前記状態更新部とを有する状態推定器により前記観測値を得られない場合は、前記状態予測部の予測結果を用いて、前記状態更新部で内部状態及び誤差共分散行列を更新する、請求項１から９のうちいずれか１項に記載の外界認識装置。