JP7084276B2 - 物体追跡装置 - Google Patents

Description

本開示は、センサ情報に基づいて物体を追跡する物体追跡の技術に関する。

センサ情報に基づいて物体を追跡する際に、センサにより観測される物体の相対速度が折り返す可能性がある場合、相対速度は曖昧さを持つ。例えば、同一物体について連続的に検出される周波数成分の位相回転から相対速度を求める場合、検出された位相θに対して、実際の位相はθ＋２π×ｎ（ｎは整数）である可能性があり、相対速度を特定することができない。非特許文献１では、この速度の曖昧さを仮定した複数の物標を追跡することにより、真の速度を特定する技術が提案されている。

K. LI et al, ‘Multitarget Tracking with Doppler Ambiguity’, IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS VOL.49, NO.4 OCTOBER 2013

速度の曖昧さを仮定した複数の物標を追跡する場合、物標数の増加に伴い演算負荷が増加するという問題がある。物標の追跡情報に基づいて車両の走行支援を行う場合、物標の追跡情報をドライバへの警告や車両の制御に用いるため、物標の追跡情報にはリアルタイム性が要求される。しかしながら、物標を追跡する際における演算負荷が増加すると、物標の追跡情報のリアルタイム性が損なわれ、警告や制御の遅れにつながる可能性がある。

本開示は、全体の演算負荷を抑制しつつ、物標を高精度に追跡可能な物体追跡装置を提供することを目的とする。

本開示の一態様は、物体追跡装置（２０）であって、物体検出部（３１）と、速度算出部（３３）と、距離算出部（３４）と、候補生成部（７２）と、フィルタ処理部（５０）と、マッチング処理部（６２）と、候補削除部（７１）と、フィルタ選択部（４１）と、を備える。物体検出部は、センサにより検出されたセンサ情報からセンサの周辺に存在する物体を検出する。速度算出部は、物体検出部により検出された物体ごとに、観測値の１つとして、センサに対する物体の相対速度を算出する。距離算出部は、物体検出部により検出された物体ごとに、観測値の１つとして、センサから物体までの距離を算出する。候補生成部は、物体検出部により初めて検出された物体に対して速度の曖昧さを仮定して、複数の物標候補を生成する。フィルタ処理部は、互いに状態変数の数が異なる複数のフィルタを有し、複数のフィルタのうちの１つのフィルタを用いて、現在保持する物標候補のそれぞれの状態量の過去の推定値から、物標候補のそれぞれの状態量の現在の予測値を算出し、且つ、算出した予測値のそれぞれと、当該予測値のそれぞれにマッチングした現在の観測値とから、物標候補の状態量の現在の推定値を算出する。マッチング処理部は、各物標候補の状態量の予測値のそれぞれと観測値とをマッチングさせる。候補削除部は、物体検出部により検出された一つの物体について生成された複数の物標候補のそれぞれの真の物標である可能性を判定し、複数の物標候補のうち判定した可能性が設定された閾値よりも低い物標候補、又は、複数の物標候補のうち判定した可能性が低い方から順に所定数の物標候補を削除する。フィルタ選択部は、候補削除部により物標候補が削除されて物標候補の数が減少することに応じて、フィルタ処理部が用いるフィルタを、複数のフィルタのうち状態変数の多いフィルタに切り替える。

本開示によれば、初めて検出された物体に対して速度の曖昧性を仮定した複数の物標候補が生成される。そして、複数のフィルタのうちの１つのフィルタを用いて、各物標候補の状態量の過去の推定値から状態量の現在の予測値が算出される。そして、現在の予測値とその予測値とマッチングした現在の観測値とから、各物標候補の状態量の現在の推定値が算出される。すなわち、物標候補のそれぞれが追跡される。そして、複数の物標候補のうち真の物標である可能性が比較的低い物標候補が削除される。複数のフィルタには、互いに状態変数の数が異なる複数のフィルタが含まれる。状態変数の多いフィルタを用いるほど、物標候補の追跡精度が高くなるが、演算負荷が多くなる。

ここで、物標候補の数が多いときには、各物標候補を追跡することによって、複数の物標候補の中から真の物標を選択することが目的である。そのため、物標の追跡精度は低くてもよいが、物標候補の数が多いため、物標候補ごとのフィルタ処理の演算負荷は抑制したい。一方、物標候補の数が一つになった後は、その物標候補を追跡することによって、追跡情報を警報や車両制御に用いることが目的である。そのため、物標の追跡精度は高い必要があるが、物標候補の数が一つであるため、その物標候補のフィルタ処理の演算負荷は高くなってもよい。したがって、物標候補の数が減少することに応じて、フィルタ処理に用いられるフィルタが状態変数の多いフィルタに切り替えられる。これにより、全体の演算負荷しつつ、物標を高精度に追跡することができる。

なお、この欄及び特許請求の範囲に記載した括弧内の符号は、一つの態様として後述する実施形態に記載の具体的手段との対応関係を示すものであって、本開示の技術的範囲を限定するものではない。

本実施形態に係るレーダの搭載位置及び検知範囲の一例を示す図である。 本実施形態に係るレーダの搭載位置及び検知範囲の他の一例を示す図である。 本実施形態に係る物体追跡装置の構成を示すブロック図である。 本実施形態に係る物標情報生成処理を示すフローチャートである。 本実施形態に係るフィルタ処理を示すフローチャートである。 ２次元のＦＦＴ処理の概要を示す説明図である。 レーダにより観測される速度の折り返しを示す図である。 物標の実際の動きとトラッキングの様子を示す図である。 互いに異なる折り返しを仮定した複数の物標候補から真の物標を確定する様子を示す図である。 フィルタの状態変数を距離、相対速度、及び方位とした場合における、状態変数と実際の速度ベクトルを示す図である。 フィルタの状態変数をＸ座標、Ｙ座標、Ｘ方向速度、及びＹ方向速度とした場合における、状態変数と実際の速度ベクトルを示す図である。 フィルタの状態変数を３変数とした場合における演算時間と４変数とした場合における演算時間との比較を示す図である。

以下、図面を参照しながら、発明を実施するための形態を説明する。
（第１実施形態）
＜１．構成＞
まず、本実施形態に係る物体追跡装置２０の構成について、図面１～３を参照して説明する。物体追跡装置２０は、車両８０に搭載され、レーダ１０により検出された観測信号に基づいて、車両８０の周囲の物体を追跡する。物体追跡装置２０により生成された物体の追跡情報は、車両８０の走行支援に用いられることを想定している。

レーダ１０は、車両８０に搭載される。図１及び図２に、レーダ１０の搭載位置の例を示す。図１に示すように、レーダ１０は、車両８０の前方中央（例えば、前方バンパの中央）に搭載されて、車両８０の前方中央の領域を検知エリアＲｄとしてもよい。また、図２に示すように、レーダ１０は、車両８０の前方中央、左前側方、右前側方、左後側方、及び右後側方の５箇所に搭載されて、車両８０の前方中央、左前方、右前方、左後方、及び右後方の領域を検知エリアＲｄとしてもよい。車両８０に搭載するレーダ１０の個数及び搭載位置は、適宜選択すればよい。本実施形態では、レーダ１０がセンサに相当する。

レーダ１０は、レーダ信号を送受信し、同一物体に対して連続的に検出される周波数成分の位相回転から相対速度を求める方式のミリ波レーダである。本実施形態では、レーダ１０は、チャープ信号を送受信するFast Chirp Modulation（ＦＣＭ）方式のミリ波レーダである。チャープ信号は、周波数がノコギリ波状に変化するように周波数変調されたレーダ信号である。すなわち、チャープ信号は、周波数が連続的に増加又は減少するレーダ信号である。

レーダ１０は、送信アレーアンテナと受信アレーアンテナとを備える。レーダ１０は、１回の観測において、設定された繰り返し周期で、Ｍ個のチャープ信号を送信アレーアンテナから送信する。Ｍは２以上の整数である。そして、レーダ１０は、Ｍ個のチャープ信号が物体によって反射されて生じた反射信号を、受信アレーアンテナによって受信する。さらに、レーダ１０は、送信したＭ個のチャープ信号とＭ個の反射信号とから、Ｍ個のビート信号を算出し、算出した都度、Ｍ個のビート信号を物体追跡装置２０へ送信する。ビート信号は、チャープ信号と反射信号との周波数差信号であり、観測信号に相当する。本実施形態では、レーダ１０がセンサに相当し、観測信号がセンサ情報に相当する。

物体追跡装置２０は、ＣＰＵ、ＲＯＭ、ＲＡＭ、メモリ及びＩ／Ｏ等を含むマイクロコンピュータを備える。物体追跡装置２０は、ＣＰＵが非遷移的実体的記録媒体であるメモリに記憶されているプログラムを実行することにより、各種機能を実現する。また、このプログラムの実行により、プログラムに対応する方法が実行される。なお、物体追跡装置２０は、１つのマイクロコンピュータを備えていてもよいし、複数のマイクロコンピュータを備えていてもよい。さらに、物体追跡装置２０の各種機能を実現する手法は、ソフトウェアに限るものではなく、その一部又は全部を、論理回路やアナログ回路等を組み合わせたハードウェアを用いて実現してもよい。

図３に示すように、物体追跡装置２０は、物体信号検出部３１、距離算出部３２、速度算出部３３、方位算出部３４、フィルタ選択部４１、フィルタ処理部５０、物標情報削除部７１、及び新規物標情報生成部７２の機能を備える。フィルタ処理部５０は、第１～第Ｎフィルタ処理部５１～５ＮのＮ個のフィルタ処理部を備える。Ｎは２以上の整数である。

第１～第Ｎフィルタ処理部５１～５Ｎは、それぞれ、予測処理部６１、マッチング部６２、及び更新処理部６３を備える。第１～第Ｎフィルタ処理部５１～５Ｎは、互いに状態変数の異なるフィルタを用いる。フィルタ選択部４１は、追跡状況に応じて、物標を追跡するために用いるフィルタ処理部を第１～第Ｎフィルタ処理部５１～５Ｎの中から選択する。状態変数の多いフィルタを用いるほど、物体追跡装置２０の演算負荷は大きくなる。物体追跡装置２０の各種機能の詳細は後述する。

＜２．処理＞
＜２－１．物標情報生成処理＞
次に、物体追跡装置２０が実行する物標情報生成処理の処理手順について、図４のフローチャートを参照して説明する。物体追跡装置２０は、物標情報生成処理を繰り返し実行する。

まず、Ｓ１０では、物体信号検出部３１は、受信した観測信号を周波数解析して、物体信号を検出する。詳しくは、図６に示すように、物体信号検出部３１は、１回目のＦＦＴ処理として、Ｍ個のビート信号のそれぞれに対してＦＦＴ処理を実行して、Ｍ個の距離スペクトラムを算出する。距離スペクトラムは、距離に対するパワーを表す２次元のスペクトラムである。ビート信号は物体までの距離に応じた周波数成分を持つため、算出された距離スペクトラムの周波数ＢＩＮは距離ＢＩＮに相当する。

さらに、物体信号検出部３１は、２回目のＦＦＴ処理として、算出したＭ個の距離スペクトラムの各距離ＢＩＮに対してＦＦＴ処理を実行して、距離速度スペクトラムを算出する。距離速度スペクトラムは、距離及び速度に対するパワーを表す３次元のスペクトラムである。そして、物体信号検出部３１は、算出した距離速度スペクトラムからピークとなる速度ＢＩＮ及び距離ＢＩＮをサーチして、ピークを物体の存在を示す物体信号として抽出する。

続いて、Ｓ２０では、距離算出部３２が、Ｓ１０において抽出された物体信号の距離ＢＩＮから、距離観測値を算出する。距離は、レーダ１０から物体までの距離である。距離算出部３２は、Ｓ１０において複数の物体信号が抽出されている場合には、物体信号ごとに距離観測値を算出する。

続いて、Ｓ３０では、速度算出部３３が、Ｓ１０において抽出された物体信号の速度ＢＩＮから、物体信号ごとに、相対速度観測値を算出する。相対速度は、レーダ１０に対する物体の速度である。

続いて、Ｓ４０では、方位算出部３４が、物体信号ごとに、物体信号に到来方向推定アルゴリズムを適用して、物体の方位情報を含む方位スペクトラムを算出し、算出した方位スペクトラムから方位観測値を算出する。方位は、レーダ１０に対する物体の方位である。

Ｓ２０～Ｓ４０の処理の実行により、物体信号ごとに、距離観測値、相対速観測値及び方位観測値の組み合わせが算出される。
そして、物体信号が現サイクルにおいて初めて検出された物体を示す信号である場合には、後述するＳ１００の新規物標情報生成処理において、新規物標情報生成部７２が、物体信号ごとに、速度の曖昧性を仮定して、複数の物標候補を生成する。

図６に示すように、相対速度観測値は、連続したＭ個のビート信号の周波数成分の位相回転量θから算出される値である。ここで、検出された位相回転量θは、実際にはθ＋２π・ｋである可能性がある。ｋは整数である。そのため、図７に示すように、相対速度観測値は折返している可能性があり曖昧さを持つ。新規物標情報生成部７２は、初検出の物体信号ごとに、速度の曖昧さを仮定して、複数の相対速度観測値を算出する。例えば、速度算出部３３は、－１回、０回、１回の折り返しを仮定して、３つの相対速度観測値を算出する。そして、新規物標情報生成部７２は、互いに異なる速度観測値を持つ物標候補を生成する。このため、現サイクルにおいて、過去に検出されている物体信号に対して複数の物標候補が存在している可能性がある。

続いて、Ｓ５０では、フィルタ選択部４１が、現サイクルにおいて保持されている物標候補の中に、現サイクルにおいて未処理の物標候補が存在するか否か判定する。詳しくは、フィルタ選択部４１は、現サイクルの物標候補の中に、後述するフィルタ処理が実行されていない物標候補があるか否か判定する。フィルタ選択部４１は、未処理の物標候補が存在する場合にはＳ６０の処理へ進み、未処理の物標候補が存在しない場合にはＳ９０の処理へ進む。

Ｓ６０では、フィルタ選択部４１及びフィルタ処理部５０がフィルタ処理を実行する。フィルタ選択部４１は、現サイクルにおいて保持されている物標候補の数に応じて、第１フィルタ処理部５１～第Ｎフィルタ処理部５Ｎの中からフィルタ処理部を選択する。

選択されたフィルタ処理部は、フィルタを用いて、前サイクルにおける物標候補の状態量の推定値から、現サイクルにおける物標候補の状態量の予測値を算出する。そして、選択されたフィルタ処理部は、フィルタを用いて、算出した予測値と、その予測値にマッチングした現在の観測値とから、対応する物標候補の状態量の現在の推定値を算出する。状態量及びフィルタの詳細は後述する。

図８に示すように、サイクル１において、他車両９０が初めて検出された場合、他車両９０の観測信号から観測値が算出される。そして、算出された観測値に基づいて、－１回、０回、１回の折り返しを仮定した３つの推定値が算出され、互いに異なる推定値を有する３つの物標候補が生成される。続いて、サイクル２において、サイクル２における他車両９０の観測信号から観測値が算出されるとともに、物標候補ごとに、サイクル１における状態量の推定値からサイクル２における状態量の予測値が算出される。そして、サイクル２において、－１回の折り返しを仮定した物標候補の予測値と観測値とがマッチングするため、－１回の折り返しを仮定した物標候補の予測値と観測値とから、サイクル２における状態量の推定値が算出される。

続いて、Ｓ７０では、物標情報削除部７１が、物標候補が真の物標である可能性（以下、真物標可能性）を判定し、判定した真物標可能性に基づいて物標候補が削除条件を満たすか否か判定する。物標情報削除部７１は、物標候補の予測値と観測値とのマッチング成立回数が高いほど、及び／又は、物標候補の予測値と観測値との差分が小さいほど、真物標可能性を高く判定する。削除条件は、真物標可能性が設定された閾値よりも低いことである。具体的には、物標候補の予測値と観測値とのマッチング成立回数が回数閾値よりも低いこと、及び／又は、物標候補の予測値と観測値との差分が差分閾値よりも大きいこと、である。あるいは、削除条件は、複数の物標候補のうち真物標可能性が低い方から所定数番以内であること、である。この場合、削除対象の物標候補は、複数の物標候補のうち真物標可能性が低い方から順に所定数の物標候補である。所定数は、複数の物標候補の数に応じて設定してもよい。例えば、物標候補の数が４個以上の場合は、所定数を２に設定し、物標候補の数が３個以下の場合は、所定数を１に設定してもよい。

物標情報削除部７１は、削除条件を満たすと判定した場合にはＳ８０の処理へ進み、削除条件を満たさないと判定した場合にはＳ５０の処理へ戻る。
Ｓ８０では、物標情報削除部７１は、削除条件を満たす物標候補を削除しＳ５０の処理へ戻る。その後、次の物標候補について、Ｓ５０～Ｓ８０の処理が実行される。そして、物標情報削除部７１は、最終的に物標候補が一つになった場合に、その一つの物標候補を真の物標と確定する。

図９に示すように、サイクル１において物体が初検出された場合、－１回、０回、１回の折り返しを仮定した３つの状態量の推定値が算出され、互いに異なる推定値を有する３つの物標候補が生成される。そして、サイクル２では２つの観測値が算出される。このうちの一方はノイズ、又はサイクル１において検出された物体と異なる物体の観測値である。

サイクル２では、３つの物標候補のうち－１回の折り返しを仮定した物標候補の予測値は、観測値とマッチングしない。そのため、サイクル３において、－１回の折り返しを仮定した物標候補が削除される。一方、サイクル２では、３つの物標候補のうち０回及び１回の折り返しを仮定した物標候補の予測値は観測値とマッチングする。よって、サイクル２では、この２つの物標候補の予測値と、その予測値とマッチングする観測値とから推定値が算出される。

サイクル３では１つの観測値が算出される。サイクル３において存在する２つの物標候補のうち１回の折り返しを仮定した物標候補の予測値は、観測値とマッチングしない。そのため、サイクル４において、１回の折り返しを仮定した物標候補が削除される。一方、サイクル３では、０回の折返しを仮定した物標候補の予測値は観測値とマッチングするため、この物標候補の予測値と観測値とから推定値が算出される。そして、サイクル４では、０回の折り返しを仮定した物標候補のみが存在する。そのため、サイクル４において、０回の折り返しを仮定した物標候補が真の物標と確定される。

次に、Ｓ９０では、新規物標情報生成部７２が、未処理の物体信号が存在するか否か判定する。すなわち、新規物標情報生成部７２は、現サイクルにおいて初めて検出された物体信号が存在するか否か判定する。新規物標情報生成部７２は、未処理の物体信号が存在しない場合には本処理を終了し、未処理の物体信号が存在する場合には、Ｓ１００の処理へ進む。

Ｓ１００では、新規物標情報生成部７２は、未処理の物体信号の観測値から、速度の曖昧さを仮定した複数の物標候補を生成し、本処理を終了する。
＜２－２．フィルタ処理＞
次に、物体追跡装置２０が実行するフィルタ処理の処理手順について、図５のフローチャートを参照して説明する。

まず、Ｓ２００では、フィルタ選択部４１が、第１～第Ｎフィルタ処理部５１～５Ｎの中から、第Ｋフィルタ処理部５Ｋを選択する。Ｋは１～Ｎまでの整数。第１～第Ｎフィルタ処理部５１～５Ｎは、互いに状態変数の数が異なるフィルタを有する。具体的には、第１フィルタ処理部５１が状態変数の最も少ないフィルタを有し、第Ｎフィルタ処理部５Ｎが状態変数の最も多いフィルタを有する。そして、第１フィルタ処理部５１から第Ｎフィルタ処理部５Ｎまで、フィルタ処理部の次数が大きくなるほど状態変数の多いフィルタを有する。本実施形態では、フィルタは、カルマンフィルタである。

第１フィルタ処理部５１のフィルタは、観測値の数以下の数の状態変数を有する。すなわち、本実施形態では、観測値は３つであるから、第１フィルタ処理部５１のフィルタは、３つ以下の状態変数を有する。

また、第Ｎフィルタ処理部５Ｎのフィルタは、観測値の数に１加えた数以上の状態変数を有する。すなわち、本実施形態では、第Ｎフィルタ処理部５Ｎのフィルタは、４つ以上の状態変数を有する。

本実施形態では、Ｎ＝２とし、フィルタ処理部５０は、第１フィルタ処理部５１と第２フィルタ処理部５２とを備える。そして、第１フィルタ処理部５１は、状態変数が３つのフィルタを有し、第２フィルタ処理部５２は、状態変数が４つのフィルタを有する。

図１０に示すように、３つの状態変数を有するフィルタの状態量は、物体の距離Ｒ、相対速度Ｖ、及び方位θである。状態量の各要素が状態変数である。状態変数の空間と観測値の空間は一致するため、フィルタ処理は線形処理になる。よって、物体追跡装置２０の演算負荷が抑制される。

一方、図１１に示すように、４つの状態変数を有するフィルタの状態量は、Ｘ座標値、Ｙ座標値、Ｘ方向速度Ｖｘ、及びＹ方向速度Ｖｙである。この場合、観測値よりも状態変数が多いため、３つの状態変数を有するフィルタを用いる場合と比べて、物標の追跡精度が向上する。

図１０に示すように、３つの状態変数のフィルタを用いる場合、相対速度Ｖは、方向を持たず、実際の速度ベクトルを方位θの方向に射影した値になる。そのため、車両８０と他車両９０との位置関係によって、相対速度Ｖが実際の速度ベクトルからずれることがある。これに対して、図１１に示すように、４つの状態変数のフィルタを用いる場合、相対速度が方向を持つため、車両８０と他車両９０との位置関係にかかわらず、Ｘ方向速度及びＹ方向速度を合成すると実際の速度ベクトルになる。

しかしながら、図１１の４つの状態変数を有するフィルタを用いる場合、フィルタ処理は非線形処理になる。そのため、４つの状態変数を有するフィルタを用いる場合、３つの状態変数を有するフィルタを用いる場合と比べて、物体追跡装置２０の演算負荷が増加する。

図１２に、状態変数が４つの場合と３つの場合における演算時間を示す。図１２では、状態変数が４つの場合における演算時間を１として、状態変数が３つの場合における演算時間を相対的に示している。状態変数が４つの場合は、状態変数が３つの場合と比べて、演算時間が３０％弱増加している。

そこで、フィルタ選択部４１は、一つの物体について、物標候補が削除されて物標候補の数が減少することに応じて、選択するフィルタ処理部を、より状態変数の多いフィルタを有するフィルタ処理部に切り換える。具体的には、フィルタ選択部４１は、一つの物体について、物標候補が１つになって真の物標が確定する前は、第１フィルタ処理部を選択する。そして、フィルタ選択部４１は、真の物標が確定した後は、状態変数が最も多いフィルタを有するフィルタ処理部、すなわち本実施形態では第２フィルタ処理部５２を選択する。

あるいは、フィルタ選択部４１は、検出されているすべての物体について生成された物標候補の全体数に応じて、フィルタ処理部を選択してもよい。すなわち、フィルタ選択部４１は、検出されている物体の数が少ない場合には、各物体の物標候補の数が一つになる前に、状態変数の多いフィルタを用いるフィルタ処理部を選択してもよい。

続いて、Ｓ２１０では、Ｓ２００において選択された第Ｋフィルタ処理部５Ｋの予測処理部６１が、フィルタを用いて、前サイクルにおける物標候補の状態量の推定値から、現サイクルにおける状態量の予測値を算出する。次の式（１）～（６）は、拡張カルマンフィルタを表す式である。

ここで、Ｘk|k-1は状態量の予測値である。Ｘｋは状態量の推定値である。ｚｋは観測値である。Ｐk|k-1は状態量の予測値の誤差分散行列である。Ｐｋは誤差分散行列の推定値である。Ｓｋはイノベーション行列である。Ｋｋはカルマンゲインである。ｆは前の状態量からの予測値を与える関数である。ｈは観測値を与える関数である。Ｑｋはプロセスノイズの分散である。Ｆｋは関数ｆのヤコビアンで定義される状態遷移行列である。Ｒｋは観測ノイズの誤差分散行列である。Ｈｋは関数ｈのヤコビアンで定義される状態空間を観測空間に写像する変換行列である。また、Ｘ０は状態量の初期値（即ち、初期推定値）、Ｐ０は誤差分散行列の初期値を表す。観測値と状態変数が線形である場合には、関数ｈは線形関数になり、観測値と状態変数が非線形の場合には、関数ｈは非線形関数になる。

Ｓ２１０では、予測処理部６１は、式（１）を用いて現サイクルにおける状態量の予測値を算出するとともに、式（２）を用いて現サイクルにおける予測値の誤差分散行列を算出する。

続いて、Ｓ２２０では、Ｓ２００において選択されたフィルタ処理部のマッチング部６２は、Ｓ２１０において算出された物標候補の状態量の予測値と観測値とをマッチングさせる。例えば、マッチング部６２は、状態量の予測値と観測値との差分が所定値以内である場合に、マッチングが成立したと判定する。

続いて、Ｓ２３０では、Ｓ２００において選択されたフィルタ処理部の更新処理部６３が、フィルタを用いて、Ｓ２００においてマッチングが成立した状態量の予測値と観測値とから、現サイクルにおける状態量の推定値を算出する。具体的には、更新処理部６３は、式（３）～（６）を用いて、現サイクルにおける状態量の推定値を算出する。なお、Ｓ２００においてマッチングが成立していない場合には、予測値を推定値とする。以上で本処理を終了し、Ｓ７０の処理へ進む。

＜３．効果＞
以上説明した本実施形態によれば、以下の効果が得られる。
（１）初めて検出された物体に対して速度の曖昧性を仮定した複数の物標候補が生成される。そして、第１～第Ｎフィルタ処理部５１～５Ｎのうちの１つのフィルタ処理部により、各物標候補の状態量の過去の推定値から現在の状態量の予測値が算出される。そして、現在の状態量の予測値とその予測値にマッチングした現在の観測値とから、各物標候補の状態量の現在の推定値が算出される。すなわち、物標候補のそれぞれが追跡される。さらに、複数の物標候補のうち真の物標である可能性が低い物標候補が削除される。

物標候補の数が多いときには、各物標候補を追跡することによって、複数の物標候補の中から真の物標を選択することが目的である。そのため、物標の追跡精度は低くてもよいが、物標候補の数が多いため、物標候補ごとのフィルタ処理の演算負荷は抑制したい。一方、物標候補の数が一つになった後は、その物標候補を追跡することによって、追跡情報を警報や車両制御に用いることが目的である。そのため、物標の追跡精度は高い必要があるが、物標候補の数が一つであるため、その物標候補のフィルタ処理の演算負荷は高くなってもよい。したがって、物標候補の数が減少することに応じて、フィルタ処理に用いられるフィルタが状態変数の多いフィルタに切り替えられる。これにより、全体の演算負荷しつつ、物標を高精度に追跡することができる。

（２）観測値の数以下の数の状態変数のフィルタを用いることにより、フィルタ処理は線形処理になる。よって、状態変数の数が観測値の数と一致する第１フィルタ処理部５１が選択されることにより、フィルタ処理の演算負荷を抑制することができる。

（３）観測値の数よりも状態変数の数が多いフィルタを用いることにより、精度良く物標を追跡することができる。よって、状態変数の数が観測値の数よりも多い第２フィルタ処理部５２が選択されることにより、精度良く物標を追跡することができる。

（４）１つの物体信号に対する物標候補が一つになった場合には、状態変数が最も多いフィルタを用いても、演算負荷を抑制することができる。よって、物標候補が一つになった後では、状態変数の多いフィルタを用いることにより、精度良く物標を追跡することができる。

（５）式（４）に示すように、カルマンフィルタは、ゲインの計算量にも状態変数の数と線形性が影響する。そのため、本開示にカルマンフィルタを適用した場合、特に大きな効果が得られる。

（６）マッチング成立回数、及び／又は予測値と観測値との差分を用いて削除する物標候補を判定することにより、真の物標とは異なる物標候補を精度良く判定することができる。ひいては、複数の物標候補の中から精度良く真の物標を確定することができる。

（７）検出されている物体の数が少ない場合には、物体ごとに複数の物標候補が存在しても、物標候補の全体数が比較的少なく、計算量に余裕がある場合がある。このような場合には、各物体の物標候補の数が一つになる前に、状態変数の多いフィルタを用いることができる。

（他の実施形態）
以上、本開示を実施するための形態について説明したが、本開示は上述の実施形態に限定されることなく、種々変形して実施することができる。

（ａ）上記実施形態では、観測信号から速度観測値、距離観測値及び方位観測値を算出していたが、方位観測値は算出しなくてもよい。３つの観測値を算出した方が追跡精度は高くなるが、速度観測値と距離観測値を算出するだけでも物体追跡は可能である。

（ｂ）上記実施形態では、４つの状態変数を有するフィルタは、状態変数として、Ｘ座標値、Ｙ座標値、Ｘ方向速度Ｖｘ、及びＹ方向速度Ｖｙを有していたが、Ｘ座標値、Ｙ座標値、相対速度Ｖ、及び方位θ（すなわち速度方向θ）を有していてもよい。
（ｃ）上記実施形態では、フィルタ処理部５０が、２つのフィルタ処理部を備えていたが、互いに状態変数の数が異なる３つ以上のフィルタ処理部を備えていてもよい。例えば、状態量をＸ座標値、Ｙ座標値、相対速度、進行方向、加速度、及び角速度とするフィルタを用いてもよい。この場合、物体の旋回運動まで考慮した追跡を行うことができる。

（ｄ）上記実施形態では、フィルタとしてカルマンフィルタを用いているが、カルマンフィルタ以外のフィルタを用いてもよい。例えば、αβフィルタや移動平均フィルタを用いてもよい。

（ｅ）上記実施形態では、センサとして、ＦＣＭ方式のミリ波レーダを用いているが、ＦＣＭ方式に限らず、位相回転から相対速度を求める方式のミリ波レーダであればよい。例えば、２周波ＣＷ方式のミリ波レーダでもよい。また、ミリ波レーダ以外のセンサを用いてもよい。例えば、レーザレーダ、超音波センサなどを用いてもよい。すなわち、速度の観測値に曖昧さが生じる方式のセンサを用いる場合には、本開示を適用することができる。

（ｆ）上記実施形態における１つの構成要素が有する複数の機能を、複数の構成要素によって実現したり、１つの構成要素が有する１つの機能を、複数の構成要素によって実現したりしてもよい。また、複数の構成要素が有する複数の機能を、１つの構成要素によって実現したり、複数の構成要素によって実現される１つの機能を、１つの構成要素によって実現したりしてもよい。また、上記実施形態の構成の一部を省略してもよい。また、上記実施形態の構成の少なくとも一部を、他の上記実施形態の構成に対して付加又は置換してもよい。なお、特許請求の範囲に記載した文言のみによって特定される技術思想に含まれるあらゆる態様が本開示の実施形態である。

（ｇ）上述した物体追跡装置の他、当該物体追跡装置を構成要素とするシステム、当該物体追跡装置としてコンピュータを機能させるためのプログラム、このプログラムを記録した半導体メモリ等の非遷移的実態的記録媒体、物体追跡方法など、種々の形態で本開示を実現することもできる。

１１０…レーダ、２０…物体追跡装置、３１…物体信号検出部、３２…距離算出部、３３…速度算出部、４１…フィルタ選択部、５０…フィルタ処理部、６２…マッチング部、７１…物標情報削除部、７２…新規物標情報生成部。

Claims (9)

1. センサ（１０）により検出されたセンサ情報から前記センサ（１０）の周辺に存在する物体を検出するように構成された物体検出部（３１）と、
   前記物体検出部により検出された物体ごとに、観測値の１つとして、前記センサに対する前記物体の相対速度を算出するように構成された速度算出部（３３）と、
   前記物体検出部により検出された物体ごとに、前記観測値の１つとして、前記センサから前記物体までの距離を算出するように構成された距離算出部（３２）と、
   前記物体検出部により初めて検出された物体に対して速度の曖昧さを仮定して、複数の物標候補を生成するように構成された候補生成部（７２）と、
   互いに状態変数の数が異なる複数のフィルタを有し、前記複数のフィルタのうちの１つのフィルタを用いて、現在保持する前記物標候補のそれぞれの状態量の過去の推定値から、前記物標候補のそれぞれの状態量の現在の予測値を算出し、且つ、算出した予測値のそれぞれと、当該予測値のそれぞれにマッチングした現在の前記観測値とから、前記物標候補の状態量の現在の推定値を算出するように構成されたフィルタ処理部（５０）と、
   各物標候補の状態量の予測値のそれぞれと前記観測値とをマッチングさせるように構成されたマッチング処理部（６２）と、
   前記物体検出部により検出された一つの前記物体について生成された前記複数の物標候補のそれぞれの真の物標である可能性を判定し、前記複数の物標候補のうち判定した前記可能性が設定された閾値よりも低いと判断した物標候補、又は、前記複数の物標候補のうち判定した前記可能性が低い方から順に所定数の物標候補を削除するように構成された候補削除部（７１）と、
   前記候補削除部により前記物標候補が削除されて前記物標候補の数が減少することに応じて、前記フィルタ処理部が用いるフィルタを、前記複数のフィルタのうち前記状態変数の多いフィルタに切り替えるように構成されたフィルタ選択部（４１）と、を備える
   物体追跡装置。
2. 前記複数のフィルタのうち前記状態変数が最も少ないフィルタは、前記観測値の次数以下の状態変数を有する、
   請求項１に記載の物体追跡装置。
3. 前記複数のフィルタのうちの前記状態変数が最も多いフィルタは、前記観測値の次数に１加えた数以上の状態変数を有する、
   請求項１又は２に記載の物体追跡装置。
4. 前記フィルタ処理部は、前記物体検出部により検出された一つの前記物体について生成された前記物標候補が一つになった時に、前記複数のフィルタのうちの前記状態変数が最も多いフィルタを用いる、
   請求項１～３のいずれか１項に記載の物体追跡装置。
5. 前記物体検出部により検出された前記物体ごとに、前記観測値の１つとして、前記センサに対する前記物体の方位を算出するように構成された方位算出部（３４）を備え、
   前記複数のフィルタのうちの前記状態変数が最も少ないフィルタは、前記状態変数として、物体の距離、相対速度、及び方位の少なくとも一つを有する、
   請求項１～４のいずれか１項に記載の物体追跡装置。
6. 前記複数のフィルタのうちの前記状態変数が最も多いフィルタは、前記状態変数として、Ｘ座標値とＹ座標値を有し、さらにＸ方向速度及びＹ方向速度、または速度及び速度方向を有する、
   請求項１～５のいずれか１項に記載の物体追跡装置。
7. 前記複数のフィルタはカルマンフィルタである、
   請求項１～６のいずれか１項に記載の物体追跡装置。
8. 前記候補削除部は、前記物標候補の前記予測値と前記観測値とのマッチング成立回数、及び／又は、前記予測値と前記観測値との差分に基づいて前記可能性を判定する、
   請求項１～７のいずれか１項に記載の物体追跡装置。
9. 前記フィルタ部は、現在保持している前記物標候補の全体数に応じて、前記複数のフィルタから用いるフィルタを選択する、
   請求項１～８のいずれか１項に記載の物体追跡装置。

Images