JP7548859B2 - 物体追跡装置 - Google Patents

Description

本開示は、物体追跡装置に関する。

従来、車載のレーダ装置によって自車の周辺の物体を認識する手法として、レーダ装置から得られた観測信号を周波数解析し、周波数解析した信号の電力の極大点を抽出（即ち、ピークサーチ）する手法が一般的である。

しかし、自車の周辺の環境によっては、所望の物体に対応する信号をピークとして抽出できない場合がある。
そのような場合に有効な手法として、過去の物体追跡の結果を用いて、現時刻での物体に関する予測値を算出し、その予測値に基づいて物体を示す信号を抽出する技術が提案されている（例えば、特許文献１参照）。

特開２００３－２７０３４１号公報

しかしながら、発明者の詳細な検討の結果、従来の技術について、下記のような課題が見出された。
具体的には、自車の周囲の物体（即ち、レーダ装置のターゲット）や自車の挙動が大きく変化したときには、上述の予測値の誤差が大きくなることがある。そのような場合には、抽出される信号（即ち、履歴信号）の誤差も大きくなる。

そのため、現時刻での通常の信号を正しく検出できている場合でも、予測値と誤差を持った履歴信号とを関連付けてしまった場合には、ターゲットや自車の挙動に好適に対応できず、ターゲットを見失う（即ち、ロストする）可能性がある。つまり、上述した技術では、物体の追跡を精度良く行うことができない可能性がある。

本開示の１つの局面は、物体の追跡を精度良く行うことができる技術を提供することにある。

本開示の一態様は、センサ（７）からの観測信号に基づいて、移動体の周囲の物標を追跡するように構成された物体追跡装置（３）に関するものである。
この物体追跡装置は、取得部（２１）と信号解析部（２３）と第１信号抽出部（２５）と予測部（２７）と第２信号抽出部（２９）とコスト設定部（３１）と関連付け部（３３）と推定部（３５）と信頼度判定部（３７）とコスト変更部（３９）とを備えている。

取得部は、前記センサから前記観測信号を取得するように構成されている。
信号解析部は、前記観測信号又は当該観測信号から得られる信号の強度分布を生成するように構成されている。

第１信号抽出部は、前記強度分布に基づいて、前記物標に対応する１又は複数の第１検知信号を抽出するように構成されている。
予測部は、過去の前記物標の状態（例えば、距離、速度、方位）に基づいて、現時刻での前記物標の状態の予測値を算出するように構成されている。

第２信号抽出部は、前記強度分布に基づいて、前記物標の前記予測値に対応する１又は複数の第２検知信号を抽出するように構成されている。
コスト設定部は、前記予測値と前記第１検知信号との関連付けコストと、前記予測値と前記第２検知信号との関連付けコストと、を設定するように構成されている。

関連付け部は、前記関連付けコストに基づいて、前記予測値に関連付ける条件（例えば、関連付けコストが最小の条件など）を満たす関連信号を求めて、前記予測値と前記関連信号との関連付けを行うように構成されている。

推定部は、前記予測値と前記関連信号（即ち、予測値に関連付けされた信号、例えば、第１検知信号又は第２検知信号、或いは、第１検知信号及び／又は第２検知信号から得られる信号）とから、現在の前記物標の状態を推定するように構成されている。

信頼度判定部は、前記第２検知信号の信頼度を判定するように構成されている。
コスト変更部は、前記信頼度判定部の判定結果に基づいて、前記予測値と前記第２検知信号との前記関連付けコストを変更するように構成されている。

このような構成により、本開示では、物標（即ち、物体）の追跡を精度良く行うことができる。
つまり、本開示では、第２検知信号の信頼度の判定結果に基づいて、予測値と第２検知信号との関連付けコストを変更するので、この変更された関連付けコストに基づいて、上述した関連付け部における関連付けを適切に行うことができる。例えば、最も関連付けコストの低い組み合わせの予測値と関連信号とを関連付けることができる。

従って、予測値と関連信号（即ち、予測値に関連付けられた信号）とから、現在の物標の状態を精度良く推定することができるので、物体の追跡を精度良く行うことができる。
ここで、前記関連付けコストとは、予測値と前記各検知信号とをそれぞれ組み合わせた場合において、組み合わせた予測値と前記各検知信号との関連性の程度を示す指標である。詳しくは、予測値と前記各検知信号に対応した観測値（即ち、観測信号から得られる物標の状態を示す値）とを関連付けする際の困難性を示す指標である。

この関連付けコストが低いほど、予測値と観測値（従って、各検知信号）との関連性が高いと考えられる。例えば、関連付けコストが低いほど、観測値が実際の物標の状態を示す可能性が高いと考えられる。

前記関連付けとは、予測値と関連信号（例えば、各検知信号や各検知信号から得られた値）とに基づいて現在の物標の状態を推定する場合に、その推定に用いる関連信号を所定の条件（例えば、関連付けコスト）に基づいて求めて、予測値と所定の条件で求めた関連信号とを結びつける処理を示している。

前記関連信号とは、予測値に関連付けられる信号、即ち、現在の物標の状態を推定する際に予測値と共に用いられる信号（即ち、前記所定の条件で求めた信号）である。この関連信号としては、例えば、第１検知信号や第２検知信号、或いは、第１検知信号及び／又は第２検知信号から得られる信号（例えば、重み付けによって得られる信号）が挙げられる。

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

第１実施形態の物体追跡装置を含む物体追跡システムの構成を示すブロック図である。 レーダ装置が自車の前方に搭載される場合における検知エリアの一例を示す図である。 レーダ装置が自車の前方以外にも搭載される場合における検知エリアの一例を示す図である。 物体追跡装置の構成を機能的に示すブロック図である。 路側壁がある場合のレーダ波の反射波の状態を示す説明図である。 通常の信号と履歴信号との検出方法を示す説明図である。 レーンチェンジの状態と予測誤差が発生する状態とを示す説明図である。 関連付コストに関する説明図である。 追跡処理を示すメインのフローチャートである。 前回推定値から今回推定値を算出するまでの処理を説明するための模式図である。 コスト設定処理を示すフローチャートである。 コスト加算判定処理を示すフローチャートである。 第２実施形態における処理を示すフローチャートである。

以下に、本開示の実施形態を、図面を参照しながら説明する。
［１．第１実施形態］
［１－１．全体構成］
まず、本第１実施形態における物体追跡装置を含む物体追跡システムの全体構成について説明する。

図１に示すように、物体追跡システム１において、物体追跡装置３は、移動体に搭載され、移動体の周囲の物体を追跡するための装置である。移動体とは、例えば、車両、航空機、船舶等である。

移動体には、物体追跡装置３の他に、センサ５として、レーダ装置７やレーダ装置７以外の各種の検知装置（即ち、他の検知装置）９が搭載されている。なお、検知装置９としては、周知の、車速センサ、前後加速度センサ、横加速度センサ、ヨーレートセンサ、ナビゲーション装置等が挙げられる。

本第１実施形態では、物体追跡装置３及びセンサ５は、自車ＪＶに搭載される。なお、自車ＪＶ以外の先行車等の車両を他車と称するが、車両一般を単に車両と称することがある。

図２に示すように、レーダ装置７は、自車ＪＶの前方中央（例えば、前方バンパの中央）に搭載され、自車ＪＶの前方中央の領域を検知エリアＲｄとしてもよい。また、図３に示すように、レーダ装置７は、自車ＪＶの前方中央、左前側方、右前側方、左後側方、及び右後側方の５箇所に搭載され、自車ＪＶの前方中央、左前方、右前方、左後方、及び右後方の領域を検知エリアＲｄとしてもよい。自車ＪＶに搭載するレーダ装置７の個数及び搭載位置は、適宜選択すればよい。

レーダ装置７は、高分解能ミリ波レーダであり、複数のアンテナ素子によって構成された送信アレーアンテナと、複数のアンテナ素子によって構成された受信アレーアンテナと、を含む。レーダ装置７は、予め定められた周期Ｔｃｙで到来する各処理サイクルにおいて、送信波を検知エリアＲｄに照射する。

そして、レーダ装置７は、送信波が物標の反射点で反射されて生じた反射波（即ち、受信波）を受信する。なお、物標としては、車両や路面や路側物等の物体が挙げられる。さらに、レーダ装置７は、送信波と反射波とを混合したビート信号を生成し、ビート信号をサンプリングすることで生成した信号を物体追跡装置３へ出力する。

このレーダ装置７から出力される信号を観測信号と称する。なお、ここでは、観測信号として、ビート信号をサンプリングすることで生成した信号が出力されるが、それに限定されるものではない。なお、レーダ装置７は、ＦＭＣＷ方式、多周波ＣＷ方式など、どのような変調方式でもよい。

図１に戻り、前記物体追跡装置３は、ＣＰＵ１１、ＲＯＭ１３、ＲＡＭ１５、フラッシュメモリ１７等を有する周知のマイクロコンピュータ（即ち、マイコン）を中心に構成される。ＣＰＵ１１は、ＲＯＭ１３に格納されたプログラムを実行する。当該プログラムが実行されることで、当該プログラムに対応する方法が実行される。なお、ＲＯＭ１３、ＲＡＭ１５、フラッシュメモリ１７等のメモリ１９は、非遷移的実体的記録媒体である。また、物体追跡装置３は、高速フーリエ変換処理（即ち、ＦＦＴ処理）等を実行するコプロセッサを備えてもよい。

物体追跡装置３を構成するマイコンの数は１つでも複数でもよい。また、物体追跡装置３が有する各種機能を実現する手法はソフトウェアに限るものではなく、その一部又は全部の要素について、一つあるいは複数のハードウェアを用いて実現してもよい。例えば、上記機能がハードウェアである電子回路によって実現される場合、その電子回路は多数の論理回路を含むデジタル回路、又はアナログ回路、あるいはこれらの組合せによって実現してもよい。

物体追跡装置３は、レーダ装置７にて生成された観測信号に基づき追跡処理を行い、物標の状態を示す状態量を推定する。なお、物標の状態量とは、例えば、自車ＪＶに対する物標の位置や、自車ＪＶに対する物標の相対速度であっても良い。より詳しくは、物標の位置とは、自車ＪＶから物標までの距離、及び、自車ＪＶに対する物標の方位であっても良い。なお、物標の相対速度の代わりに、相対速度と自車ＪＶの速度とから算出した物標の対地速度を物体の状態を示す状態量としても良い。また、物標の状態量には、物標の反射点からの反射強度が含まれていてもよい。

［１－２．物体追跡装置の構成］
次に、物体追跡装置３の構成を機能的に説明する。
図４に示すように、物体追跡装置３は、機能的な構成として、取得部２１と信号解析部２３と第１信号抽出部２５と予測部２７と第２信号抽出部２９とコスト設定部３１と関連付け部３３と推定部３５と信頼度判定部３７とコスト変更部３９とを備えている。

取得部２１は、レーダ装置７からの出力信号（即ち、通常の信号）を取得するように構成されている。つまり、各受信アレーアンテナ毎に前記観測信号を取得するように構成されている。

信号解析部２３は、観測信号をＦＦＴ処理して強度分布（即ち、パワースペクトル）を生成するように構成されている。このパワースペクトルにおけるピークは、周知のように、物標との距離に対応しているので、即ち、パワースペクトルの周波数が距離に対応しているので、以下では、このパワースペクトルを距離パワースペクトルと称する。

なお、各受信アレーアンテナ毎にそれぞれ同じ周波数の信号成分を抽出した信号成分についてＦＦＴ処理を行って、パワースペクトルを得ることができる。周知のように、このパワースペクトルの周波数は方位に対応しているので、以下では、このパワースペクトルを方位パワースペクトルと称する。

第１信号抽出部２５は、通常の信号から得られた前記強度分布（例えば、距離パワースペクトルや後述する距離速度パワースペクトル）に基づいて、物標に対応する１又は複数の第１検知信号を抽出するように構成されている。なお、この第１検知信号が、物標の位置等を示す観測値（即ち、第１観測値）に対応するものである（例えば、図８参照）。

予測部２７は、過去の物標の状態の推定値に基づいて、現時刻での物標の状態の予測値を算出するように構成されている。
例えば、前サイクルの物標の状態量の推定値（例えば、物標との距離、物標との相対速度、自車ＪＶからの物標の方位等の物標の情報）から、現時刻（例えば、今サイクル）における、物標の状態量の予測値（例えば、現時刻における、物標との距離、物標との相対速度、自車ＪＶからの物標の方位等の予測値）を算出する。

なお、以下では、物標との距離、物標との相対速度、自車ＪＶからの物標の方位を、それぞれ、単に、距離、速度、方位と記すことがある。
第２信号抽出部２９は、前記強度分布（例えば、距離パワースペクトルや距離速度パワースペクトル）に基づいて、物標の予測値に対応する１又は複数の第２検知信号を抽出するように構成されている。

つまり、通常の信号から得られる強度分布に、過去の推定値から得られた予測値を考慮して、第２検知信号を抽出する。なお、この第２検知信号が、物標の位置等を示す観測値（即ち、第２観測値）に対応するものである（例えば、図８参照）。

例えば、予測値として、距離と速度との情報がある場合、距離速度パワースペクトルにおいて前記予測値に対して所定の距離範囲と速度範囲の全てのセルを抽出し、それらのセルにおいて最大のパワーのピークを第２検知信号としてもよい。

または、上述のようにして抽出した全てのセルに対して方位推定を行って、予測値に最も近いセルにおけるピークを第２検知信号としてもよい。
コスト設定部３１は、予測値と第１検知信号との関連付けコストと、予測値と第２検知信号との関連付けコストと、を設定するように構成されている。

ここで、関連付けコストとは、予測値と第１検知信号又は第２検知信号の各検知信号とを関連付ける際の困難性を示す指標であり、関連付けコストが低いほど予測値と各検知信号との関連性が高いことを示している。そのため、関連付けコストが低いほど予測値と各検知信号との関連付けがし易くなるという特徴がある。

例えば、関連付けコストとして、予測値と各検知信号に対応した観測値との距離（例えば、ユークリッド距離やマハラノビス距離）を用いる場合、予測値と観測値との距離が小さいほど、関連付けコストが小さいとすることができる。

なお、以下で関連付けコストを算出する場合に距離を用いる場合には、ユークリッド距離やマハラノビス距離等を採用することができる。
関連付け部３３は、関連付けコストに基づいて、予測値に関連付ける条件（例えば、関連付けコストが最小の条件）を満たす関連信号を求めて、予測値と関連信号との関連付けを行うように構成されている。

例えば、予測値と第１検知信号との関連付けコストと、予測値と第２検知信号との関連付けコストと、を比較し、最も関連付けコストが低い検知信号を関連信号として採用する。従って、その関連信号と予測値とが関連付けられることになる。なお、ここでは、予測値と関連付けられた検知信号が関連信号である。

なお、後述するように、コスト変更部３９によって、予測値と第２検知信号との関連付けコストが変更された場合には、変更された関連付けコストに基づいて関連付けが行われる。

この関連付けとは、例えば、今回予測値と今回の第１検知信号や第２検知信号の検知信号とから、今回推定値を算出する場合に、より精度の高い今回推定値を算出することができるような今回の検知信号（即ち、関連信号）を選択するための処理である。

例えば、関連付けコストとして、予測値と各検知信号に対応した観測値との前記距離を用いる場合、予測値に当該予測値との距離が小さい方の検知信号を関連付けることができる。

なお、後述するように、第１検知信号や第２検知信号が複数ある場合には、関連付けコストに基づいて、例えば、予測値と第１検知信号及び／又は検知信号から関連信号を求め、その関連信号と予測値との関連付けを行ってもよい。

推定部３５は、予測値と当該予測値に関連付けされた信号（例えば、第１検知信号又は第２検知信号などの関連信号）とから、現在の物標の状態（例えば、距離、速度、方位等の状態量）を推定するように構成されている。

信頼度判定部３７は、第２検知信号の信頼度を判定するように構成されている。例えば、センサ５からの自車ＪＶや先行車の状態等を示す出力信号などに基づいて、第２検知信号の信頼度を判定するように構成されている。

コスト変更部３９は、信頼度判定部３７の判定結果に基づいて、予測値と第２検知信号との関連付けコストを変更するように構成されている。
［１－３．処理の原理］
次に、物体追跡装置３において物標を追跡する手法の原理を説明する。

図５に示すように、例えば、自車ＪＶと物標である先行車とが路側壁に沿って走行している場合に、自車ＪＶから先行車に対してレーダ波を照射すると、レーダ波の反射波は、異なる経路をたどることがある。

具体的には、自車ＪＶは、先行車で反射したレーダ波を直接受信したり（図５の直接反射参照）、先行車の反射波が路側壁で反射したレーダ波を受信することがある（図５のマルチパス参照）。

このような状況において、受信したレーダ波をＦＦＴ処理して距離パワースペクトルを求めた場合、図６の上図で示すように、直接反射の信号とマルチパスの信号とが合成された信号（即ち、ＦＦＴ処理された後の観測信号）が得られる。そのため、このＦＦＴ処理された信号のピークに基づいてさらに方位パワースペクトルを求めると、方位推定に誤差が発生する恐れがある。

なお、図６において、通常の信号とは、現時刻での信号（例えば、今サイクルの処理で得られた信号）を示し、履歴信号とは、過去に得られた信号（例えば、前サイクルの処理で得られた信号）を示している。

この対策として、上述したマルチパスが発生するような状況では、図６の下図に示すように、過去の信号（即ち、履歴信号）から得られた物標の状態量の予測値に基づいて、物標が存在する可能性の高い情報（例えば、自車ＪＶと物標との予測距離）を求め、この予測距離に対応する信号を抽出する。なお、ここでは、予測距離は、直接反射の信号から得られるピークに対応したものと考える。そして、この抽出した信号に基づいて、方位パワースペクトルを求めると、方位推定に誤差が発生することを抑制できる可能性が高い。

つまり、履歴信号を用いることにより、方位を含む物標の位置を精度よく検出すること、即ち、物体の追跡を精度よく行うことができると考えられる。
しかし、図７の上図に示すように、例えば自車ＪＶがレーンチェンジを行う場合のように、運転状態を急変させたときなどには、別の問題が発生する恐れがある。

具体的には、図７の下図に示すように、運転状態等が急変した場合に、履歴信号に基づいて物標の位置等の予測値を算出すると、通常の信号のみを用いた場合に比べて、予測誤差が発生する恐れがある。これは、運転状態等が急変した場合には、レーダ波の反射波の状態等に影響を及ぼすと考えられるからである。

そこで、本第１実施形態では、運転状態の急変等のように、精度の良い予測値が得られない場合があることを考慮し、第２検知信号（即ち、通常の信号に履歴信号を考慮した信号）の信頼度を判定し、その判定結果に基づいて、予測値と第２検知信号との関連付けコストを変更するようにしている。

詳しくは、信頼度判定部３７によって、第２検知信号の信頼度が低い状態であることを示す所定の条件が満たされたと判定された場合には、予測値と第２検知信号との関連付けコストを、前記所定の条件が満たされない場合に比べて、関連付けしにくくなるように、大きくしている。

例えば、図８の上図に示すように、予測値（例えば、今回予測値）に対して、所定の予測の範囲（例えば、後述する予測ゲート）内に、第１検知信号と第２検知信号が得られた場合において、第２検知信号の信頼度を考慮しないときに、予測値と第１検知信号との関連付けコスト（ｃｏｓｔ）が２、予測値と第２検知信号との関連付けコスト（ｃｏｓｔ）が１であったとする。

なお、図８において、第１検知信号の×印が、通常の信号から得られた物標の状態量の観測値（例えば、第１観測値）に該当し、第２検知信号の×印が、通常の信号に履歴信号を考慮して得られた物標の状態量の観測値（例えば、第２観測値）に該当する。

本第１実施形態では、このような場合において、図８の下図に示すように、第２検知信号の信頼度を考慮することにより、予測値と第２検知信号との関連付けコストを、第２検知信号の信頼度を考慮しない場合に比べて大きく（例えば、１１に）変更することが可能である。

このような場合には、つまり、予測値と第２検知信号との関連付けコストが予測値と第１検知信号との関連付けコストより大きくなった場合には、関連付けコストの小さい組み合わせである予測値と第１検知信号とを関連付ける。つまり、第１検知信号を関連信号として採用して、予測値と第１検知信号とを関連付ける。

なお、第２検知信号の信頼度を考慮することにより、予測値と第２検知信号との関連付けコストが２より小さくなった場合には、関連付けコストの小さい予測値と第２検知信号とを関連付ける。

従って、関連付けされた予測値と各検知信号（即ち、関連信号）とに基づいて、物標の今回の状態を推定すること（即ち、後述する今回推定値を求めること）により、物体の追跡を精度良く行うことができる。

このように、第２検知信号の信頼度が高い場合には、第２検知信号に基づいて、物体の追跡を精度良く行うことができ、一方、第２検知信号の信頼度が低い場合には、第１検知信号に基づいて、物体の追跡を精度良く行うことができる。

この結果、運転状態の急変等がある場合でも、常に、物体の追跡を精度良く行うことができる。
［１－４．処理の概要］
物体追跡装置３のＣＰＵ１１が実行する追跡処理を、図９に示すフローチャートを用いて説明する。なお、本処理は、処理サイクルごとに繰り返し起動する。

まず、Ｓ１００では、ＣＰＵ１１は、レーダ装置７から出力された各物標に対応した信号（例えば、観測信号）を取得する。
続いて、Ｓ１１０では、ＣＰＵ１１は、距離パワースペクトル算出処理を実行する。つまり、上述のように、観測信号をＦＦＴ処理して距離パワースペクトルを算出する。

なお、ここで、周知のように、距離パワースペクトルを更にＦＦＴ処理することにより、速度パワースペクトルを求めてもよい。これにより、距離と速度との２次元のパワースペクトル（即ち、距離速度パワースペクトル）を求めることができる。

さらに、上述したようにして、方位パワースペクトルを求めて、距離と速度と方位の情報を得るようにしてもよい。
続いて、Ｓ１２０では、ＣＰＵ１１は、予測処理を実行する。本第１実施形態では、ＣＰＵ１１は、前回の処理サイクルにおいて算出された物標の状態量の推定値である前回推定値から、今回の処理サイクルにおける物標の状態量の予測値である今回予測値を算出する。

なお、図１０は、一例として、前回推定値が示す物標の位置と、今回予測値が示す物標の位置と、今回推定値が示す物標の位置とを示している。なお、図１０における内部が斜線の×印は、今サイクルにおいて通常の信号から得られた位置等の状態量（即ち、第１検知信号に対応した第１観測値）を示しており、黒色の×印は、今サイクルにおいて通常の信号に履歴信号を考慮して得られた位置等の状態量（即ち、第２検知信号に対応した第２観測値）を示している。

また、ＣＰＵ１１は、今回予測値に基づいて予測ゲートを設定する。今回予測値に対応する物標と同じ物標に対応する各観測値が示す位置は、今回予測値が示す位置の近くに存在すると考えられる。そこで、今回予測値が示す位置を中心とする領域を、該今回予測値と同じ物標に対応する各観測値が示す位置が存在すると推定される領域である予測ゲートとして設定する。

一例として、図１０は、今回予測値が示す位置に対して設定される円形の予測ゲートを示している。例えば、距離（例えば、ユークリッド距離）に応じた大きさを示す予測ゲートを示している。なお、例えば、状態量が示す相対速度等に応じて、予測ゲートの大きさが定められても良い。

続いて、Ｓ１３０では、ＣＰＵ１１は、第１信号抽出処理を実行する。例えば、距離パワースペクトルから、ピークが閾値を超えかつ極大点である第１検知信号を抽出する。なお、距離速度パワースペクトルから、ピークが閾値を超えかつ極大点である第１検知信号を抽出してもよい。

続いて、Ｓ１４０では、ＣＰＵ１１は、第２信号抽出処理を実行する。例えば、距離パワースペクトルを用いて、物標の予測値（例えば、距離）に対応する第２検知信号を抽出する。または、距離速度パワースペクトルを用いて、物標の予測値（例えば、距離、速度）に対応する第２検知信号を抽出してもよい。

詳しくは、例えば、距離パワースペクトルに基づいて、予測値の距離と最も近いピークを抽出し、そのピークが示す値（即ち、距離に対応する値）を、第２検知信号として抽出してもよい。

また、上述したように、距離速度パワースペクトルにおいて、予測値（例えば、距離、速度）に対応するセルを選択するとともに、その選択したセルの近傍のセルを探索し、電力最大のセルに対応する信号を第２検知信号として抽出してもよい。

続いて、Ｓ１５０では、ＣＰＵ１１は、コスト設定処理を実行する。このコスト設定処理とは、予測値と第１検知信号との関連付けコストと、予測値と第２検知信号との関連付けコストと、を設定する処理である。なお、このコスト設定処理については、後に詳述する。

続いて、Ｓ１６０では、ＣＰＵ１１は、関連付け処理を実行する。この関連付け処理とは、本第１実施形態では、上述した関連付けコスト（例えば、前記距離）に基づいて、予測値と第１検知信号又は第２検知信号との関連付けを行う処理である。つまり、第１検知信号又は第２検知信号のうち、関連付けコストの小さい方の検知信号を関連信号として採用し、その関連信号と予測値とを関連付ける。

例えば、関連付けコストが前記距離に応じて設定される場合には、予測値と第１検知信号又は第２検知信号との距離に基づいて、予測値と当該予測値との距離が小さい方の検知信号とを関連付ける。

なお、後述するように、予測値と第１検知信号及び／又は第２検知信号から得られる信号との関連付けを行ってもよい。
続いて、Ｓ１７０では、ＣＰＵ１１は、推定処理を実行し、一旦本処理を終了する。

この推定処理とは、カルマンフィルタ等を用いて、今回の処理サイクルにおける各物標の状態量の今回推定値を算出する。
一例として、Ｓ１６０では、今回の処理サイクルにおいて今回予測値と関連付けられた観測値（即ち、第１検知信号又は第２検知信号）が存在する場合、ＣＰＵ１１は、今回予測値と、今回予測値と関連付けられた観測値とに基づき、物標の状態量の今回推定値を算出する。これにより、例えば、前記図１０に示すように、今回予測値と、今回予測値と関連付けられた観測値（例えば、第２観測値を示す第２検出信号）とに基づき、今回推定値が算出される。

［１－５．コスト設定処理］
次に、前記コスト設定処理について、図１１に示すフローチャートを用いて説明する。
まず、Ｓ２００では、ＣＰＵ１１は、処理未完了の予測値（例えば、今回予測値）が存在するか否かを判定する。具体的には、ＣＰＵ１１は、今回予測値のうち、これ以降に続くＳ２１０～Ｓ２６０の処理をしていない今回予測値が存在するか否かを判定する。ここで肯定判断されるとＳ２１０に進み、一方否定判断されると一旦本処理を終了する。

なお、ここで肯定判断された場合には、未処理の予測値のうちの１つを選択してＳ２１０に移行する。
Ｓ２１０では、ＣＰＵ１１は、処理未完了の観測値が存在するか否かを判定する。つまり、選択中の今回予測値に対応する予測ゲート内の位置を示す観測値、即ち、予測ゲート内の第１観測値や第２観測値の中に、未処理の観測値が存在するか否かを判定する。この未処理の観測値とは、関連付けが行われておらず、且つ、続くＳ２２０以降の処理により、選択中の今回予測値との間のコストが算出されていない観測値を意味する。

なお、ここで肯定判断された場合には、未処理の観測値のうちの１つを選択してＳ２２０に移行する。
Ｓ２２０では、ＣＰＵ１１は、Ｓ２００で選択された今回予測値と、Ｓ２１０で選択された１つの観測値との間のコスト（即ち、関連付けコスト）を算出する。具体的には、ＣＰＵ１１は、今回予測値が示す位置と、観測値が示す位置との距離（例えば、ユークリッド距離）が短いほど、より小さいコストを算出する。

なお、今回予測値と観測値との関連付けコストを求める場合に、例えば、距離（即ち、ユークリッド距離）、速度、方位をパラメータとしたマハラノビス距離を用いてもよい。
続くＳ２３０では、ＣＰＵ１１は、観測値が第２検知信号から算出されたものであるか否かを判定する。ここで肯定判断されるとＳ２４０に進み、一方、否定判断されるとＳ２１０に戻る。

Ｓ２４０では、ＣＰＵ１１は、コスト加算判定処理を行う。このコスト加算判定処理とは、第２検知信号の信頼度に応じてコストを加算するか否かを判定するための処理である。なお、このコスト加算判定処理については、後に詳述する。

続くＳ２５０では、ＣＰＵ１１は、コスト加算対象であるか否かを判定する。つまり、Ｓ２４０のコスト加算判定処理の結果に応じて、従って、第２検知信号の信頼度に応じて、コストを加算するか否かを判定するための処理である。なお、ここで肯定判断されるとＳ２６０に進み、一方否定判断されると前記Ｓ２１０に戻る。

Ｓ２６０では、ＣＰＵ１１は、コスト加算を行い、前記Ｓ２１０に戻る。つまり、前記Ｓ２５０にて、今回の観測値がコスト加算対象であると判定されたので、前記Ｓ２１０で算出されたコストに、コスト加算判定処理の結果に応じた必要なコストの加算を行う。

［１－６．コスト加算判定処理］
次に、前記コスト加算判定処理について、図１２に示すフローチャートを用いて説明する。

まず、Ｓ３００では、ＣＰＵ１１は、自車ＪＶの横速度が閾値以上か否かを判定する。ここで肯定判断されるとＳ３８０に進み、一方否定判断されるとＳ３１０に進む。
なお、横速度とは、自車ＪＶを水平面に対して垂直の方向から見た場合に、自車ＪＶの前後方向に対して垂直の左右方向の速度のことであり、自車ＪＶの検知装置９からの出力信号（例えば、自車ＪＶの移動方向を示すベクトル）から求めることができる。

Ｓ３１０では、ＣＰＵ１１は、物標の横速度が閾値以上か否かを判定する。ここで肯定判断されるとＳ３８０に進み、一方否定判断されるとＳ３２０に進む。なお、物体の横速度は、過去の物標の状態量のデータから求めることができる。

Ｓ３２０では、ＣＰＵ１１は、自車ＪＶの加速度（例えば、前後加速度）が閾値以上か否かを判定する。ここで肯定判断されるとＳ３８０に進み、一方否定判断されるとＳ３３０に進む。なお、自車ＪＶの加速度は、前後加速度センサによって求めることができる。

Ｓ３３０では、ＣＰＵ１１は、物標の加速度（例えば、前後加速度）が閾値以上か否かを判定する。ここで肯定判断されるとＳ３８０に進み、一方否定判断されるとＳ３４０に進む。なお、物体の加速度は、過去の物標の状態量のデータから求めることができる。

Ｓ３４０では、ＣＰＵ１１は、道路形状の曲率半径が閾値以上か否かを判定する。ここで肯定判断されるとＳ３８０に進み、一方否定判断されるとＳ３５０に進む。なお、道路形状の曲率半径は、先行車の過去の状態量のデータ（例えば、後述の第２実施形態の物標の軌跡のデータ）や、自車ＪＶの過去の状態量のデータ（例えば、自車ＪＶの軌跡のデータ）や、ガードレール等の路側物の位置のデータなどから求めることができる。

Ｓ３５０では、ＣＰＵ１１は、信号（例えば、第２検知信号）の電力が閾値以上か否かを判定する。ここで肯定判断されるとＳ３８０に進み、一方否定判断されるとＳ３６０に進む。なお、この信号としては、第２検知信号以外に、レーダ波の反射波から得られる各種の信号を採用してもよい。

Ｓ３６０では、ＣＰＵ１１は、前サイクルまでの予測誤差の平均値が閾値以上か否かを判定する。ここで肯定判断されるとＳ３８０に進み、一方否定判断されるとＳ３７０に進む。

なお、前サイクルまでの予測誤差の平均値としては、複数回のサイクルにおける予測誤差の平均値が挙げられる。また、予測誤差としては、例えば、今回予測値と観測値（例えば、関連付けされた信号に対応した観測値）との差や、今回予測値と今回推定値との差などが挙げられる。

Ｓ３７０では、ＣＰＵ１１は、今回の判定対象の観測値が、コスト加算対象でないとして、例えば、所定のフラグを設定して、一旦本処理を終了する。
一方、Ｓ３８０では、ＣＰＵ１１は、今回の判定対象の観測値が、コスト加算対象であるとして、例えば、所定のフラグを設定して、一旦本処理を終了する。

また、Ｓ３００～Ｓ３６０で、それぞれ判定を行って、肯定判断された場合に、コスト加算対象と判断しているが、その場合に、どの程度のコストを設定するが予め決められている。

例えば、各ステップで肯定判断された場合に、同じコスト（例えば１０）加算するようにしてもよい。或いは、誤差が発生し易い条件を区別して、各ステップで肯定判断された場合に異なる加算コストを設定してもよい。

［１－７．効果］
上記第１実施形態では、以下の作用効果を得ることができる。
（１ａ）本第１実施形態では、予測値と予測値に関連付けられた関連信号とから、現在の物標（即ち、先行車等の物体）の状態量を精度良く推定できるので、物体の追跡を精度良く行うことができる。

つまり、本第１実施形態では、第２検知信号の信頼度の判定結果に基づいて、予測値と第２検知信号との関連付けコストを変更することができる。
例えば、信頼度判定部３７によって、第２検知信号の信頼度が低い状態であることを示す所定の条件が満たされたと判定された場合には、予測値と第２検知信号との関連付けコストを、前記所定の条件が満たされない場合に比べて、関連付けしにくくなるように、大きく設定することができる。

よって、この変更された関連付けコストに基づいて、関連付け部３３における関連付けを適切に行うことができる。従って、予測値と予測値に関連付けられた検知信号（即ち、関連信号）とから、現在の物標の状態量を精度良く推定できるので、物体の追跡を精度良く行うことができる。

（１ｂ）本第１実施形態では、第１信号抽出部２５は、観測信号の強度分布（例えば、距離パワースペクトル）から強度が閾値を超えかつ極大点である第１検知信号を抽出することができる。また、第２信号抽出部２９は、観測信号の強度分布から物標の予測値に対応する第２検知信号を抽出することができる。

（１ｃ）本第１実施形態では、自車ＪＶまたは物標の横速度の絶対値が所定値より大きい第１条件が満たされた場合に、第２検知信号の信頼度が低いと判断し、関連付けコストを、第１条件が満たされない場合に比べて、関連付けしにくくなるように、大きく設定することができる。

（１ｄ）本第１実施形態では、自車ＪＶまたは物標の加速度の絶対値が所定値より大きい第２条件が満たされた場合に、第２検知信号の信頼度が低いと判断し、関連付けコストを、第２条件が満たされない場合に比べて、関連付けしにくくなるように、大きく設定することができる。

（１ｅ）本第１実施形態では、道路形状の曲率半径が所定値より小さい第３条件が満たされた場合に、第２検知信号の信頼度が低いと判断し、関連付けコストを、第３条件が満たされない場合に比べて、関連付けしにくくなるように、大きく設定することができる。

（１ｆ）本第１実施形態では、レーダ装置７から得られる信号の電力（例えば、第２検知信号の電力）が所定値より小さい第４条件が満たされた場合に、第２検知信号の信頼度が低いと判断し、関連付けコストを、第４条件が満たされない場合に比べて、関連付けしにくくなるように、大きく設定することができる。

（１ｇ）本第１実施形態では、過去の予測誤差が所定値より大きい第５条件が満たされた場合に、第２検知信号の信頼度が低いと判断し、関連付けコストを、第５条件が満たされない場合に比べて、関連付けしにくくなるように、大きく設定することができる。

［１－８．文言の対応関係］
本第１実施形態と本開示との関係において、レーダ装置７がセンサに対応し、物体追跡装置３が物体追跡装置に対応し、取得部２１が取得部に対応し、信号解析部２３が信号解析部に対応し、第１信号抽出部２５が第１信号抽出部に対応し、予測部２７が予測部に対応し、第２信号抽出部２９が第２信号抽出部に対応し、コスト設定部３１がコスト設定部に対応し、関連付け部３３が関連付け部に対応し、推定部３５が推定部に対応し、信頼度判定部３７が信頼度判定部に対応し、コスト変更部３９がコスト変更部に対応する。

［２．第２実施形態］
第２実施形態は、基本的な構成は第１実施形態と同様であるため、以下では主として第１実施形態との相違点について説明する。なお、第１実施形態と同じ符号は、同一構成を示すものであって、先行する説明を参照する。

本第２実施形態は、前記第１実施形態とは追跡処理の内容が異なるので、追跡処理について説明する。
図１３に示すように、本第２実施形態は、前記第１実施形態のＳ１００～Ｓ１７０と同様な処理を行う。

具体的には、Ｓ４００ではＳ１００と同様の観測信号を取得する処理を行い、Ｓ４１０ではＳ１１０と同様の距離パワースペクトル算出処理を行い、Ｓ４２０ではＳ１２０と同様の予測処理を行い、Ｓ４３０ではＳ１３０と同様の第１信号抽出処理を行い、Ｓ４４０ではＳ１４０と同様の第２信号抽出処理を行い、Ｓ４５０ではＳ１５０と同様のコスト設定処理行い、Ｓ４６０ではＳ１６０と同様の関連付け処理を行い、Ｓ４７０ではＳ１７０と同様の推定処理を行う。

そして、続くＳ４８０では、前記推定処理で得られた各物標の状態量の推定値を用いて、道路形状の推定を算出する道路形状推定処理を行う。
つまり、物標の状態量の推定値（例えば、距離、速度、方位）等から、物標がどのような軌跡を描くかが分かるので、この物標の軌跡から道路形状を推定することができる。

なお、このように推定した道路形状は、例えば前記図１２のＳ３４０の道路形状の曲率半径の判定に用いることができる。
本第２実施形態は、第１実施形態と同様な効果を奏する。また、本第２実施形態は、物情の状態量の推定値から、道路形状を推定することができる。

［３．第３実施形態］
第３実施形態は、基本的な構成は第１実施形態と同様であるため、以下では主として第１実施形態との相違点について説明する。なお、第１実施形態と同じ符号は、同一構成を示すものであって、先行する説明を参照する。

本第３実施形態は、第１検知信号と第２検知信号との処理内容が異なるので、異なる点を中心に説明する。
本第３実施形態では、第１検知信号及び／又は第２検知信号の複数の検知信号から関連信号を求め、その関連信号と予測値との関連付けを行う。

例えば、予測ゲート内に、１又は複数の第１検知信号や１又は複数の第２検知信号が抽出された場合には、各検知信号に重み付けして、予測値と関連付けられる総合的な検知信号（即ち、関連信号）を算出する。

例えば、複数の検知信号について、それぞれ予測値との関連付けコストを算出するとともに、この関連付けコストを用いて重み付けして、例えば重み付け平均を算出することにより、総合的な検知信号を算出する。ここでは、関連付けコストが大きいほど重みが小さくなるように重み付けすることができる。なお、この重み付けの方法として、関連付けコストの逆数を重みとして用いてもよい。

そして、この総合的な検知信号を関連信号として採用する。つまり、この関連信号と予測値とを関連付ける。そして、予測値と関連信号とから、第１実施形態と同様に、現在の物標の状態を推定する。

なお、予測ゲート内の各検知信号のうち、所定の条件を満たす複数の検知信号（例えば、予測値との前記距離が所定値より小さい検知信号）を選択して重み付けして、関連信号を求めるようにしてもよい。

本第３実施形態は、第１実施形態と同様な効果を奏する。また、本第３実施形態は、第１検知信号や第２検知信号が複数ある場合にも、好適に予測値との関連付けを行うことができる。

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

（４ａ）第１実施形態において、予測ゲート内に、例えば、第１検知信号が複数抽出された場合には、複数の第１検知信号に重み付けして、例えば第３実施形態のように関連付けコストを用いて重み付けして、総合的な第１検知信号を求めてもよい。

例えば、複数の第１検知信号の重み付け平均によって総合的な第１検知信号を求め、この総合的な第１検知信号を関連信号として採用してもよい。
なお、第２検知信号が複数ある場合も同様である。

（４ｂ）関連付けコストとしては、予測値と第１検知信号又は第２検知信号との距離（例えば、ユークリッド距離）に基づいた値を採用できる。また、距離、速度、方向の少なくとも２種以上をパラメータとしたマハラノビス距離などに基づいた値を採用できる。

（４ｃ）上記実施形態では、関連付けコストを、予測値と各検知信号とを関連付ける際の困難性を示す指標として説明したが、つまり、関連付けコストが大きいほど関連性が低いとしたが、その逆の指標を採用することもできる。

例えば、前記関連付けコストに代えて、予測値と各検知信号とを関連付ける際の容易さを示す指標を採用することができる。この場合には、その指標が大きいほど関連性が高いので、その指標が大きい方の検出信号を関連信号として採用することができる。

（４ｄ）上記実施形態では、レーダ装置がＦＭＣＷ方式を採用している例を示したが、レーダ装置のレーダ方式は、ＦＭＣＷに限定されるものではなく、例えば、２周波ＣＷ、ＦＣＭ又はパルスを採用するように構成されてもよい。ＦＣＭは、Fast-Chirp Modulationの略である。

（４ｅ）本開示に記載の物体追跡装置およびその手法は、コンピュータプログラムにより具体化された一つ乃至は複数の機能を実行するようにプログラムされたプロセッサおよびメモリを構成することによって提供された専用コンピュータにより、実現されてもよい。

あるいは、本開示に記載の物体追跡装置およびその手法は、一つ以上の専用ハードウェア論理回路によってプロセッサを構成することによって提供された専用コンピュータにより、実現されてもよい。

もしくは、本開示に記載の物体追跡装置およびその手法は、一つ乃至は複数の機能を実行するようにプログラムされたプロセッサおよびメモリと一つ以上のハードウェア論理回路によって構成されたプロセッサとの組み合わせにより構成された一つ以上の専用コンピュータにより、実現されてもよい。

また、コンピュータプログラムは、コンピュータにより実行されるインストラクションとして、コンピュータ読み取り可能な非遷移有形記録媒体（即ち、非遷移的実体的記録媒体）に記憶されてもよい。制御部に含まれる各部の機能を実現する手法には、必ずしもソフトウェアが含まれている必要はなく、その全部の機能が、一つあるいは複数のハードウェアを用いて実現されてもよい。

（４ｆ）上記実施形態における１つの構成要素が有する複数の機能を、複数の構成要素によって実現したり、１つの構成要素が有する１つの機能を、複数の構成要素によって実現したりしてもよい。また、複数の構成要素が有する複数の機能を、１つの構成要素によって実現したり、複数の構成要素によって実現される１つの機能を、１つの構成要素によって実現したりしてもよい。また、上記実施形態の構成の一部を省略してもよい。また、上記実施形態の構成の少なくとも一部を、他の上記実施形態の構成に対して付加または置換してもよい。

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

３：物体追跡装置、７：レーダ装置、２１：取得部、２３：信号解析部、２５：第１信号抽出部、２７：予測部、２９：第２信号抽出部、３１：コスト設定部、３３：関連付け部、３５：推定部、３７：信頼度判定部、３７：コスト変更部

Claims (11)

1. センサ（７）からの観測信号に基づいて、移動体の周囲の物標を追跡するように構成された物体追跡装置（３）であって、
   前記センサから前記観測信号を取得するように構成された取得部（２１）と、
   前記観測信号又は当該観測信号から得られる信号の強度分布を生成するように構成された信号解析部（２３）と、
   前記強度分布に基づいて、前記物標に対応する１又は複数の第１検知信号を抽出するように構成された第１信号抽出部（２５）と、
   過去の前記物標の状態に基づいて、現時刻での前記物標の状態の予測値を算出するように構成された予測部（２７）と、
   前記強度分布に基づいて、前記物標の前記予測値に対応する１又は複数の第２検知信号を抽出するように構成された第２信号抽出部（２９）と、
   前記予測値と前記第１検知信号との関連付けコストと、前記予測値と前記第２検知信号との関連付けコストと、を設定するように構成されたコスト設定部（３１）と、
   前記予測値と前記第１検知信号との前記関連付けコストと、前記予測値と前記第２検知信号との前記関連付けコストと、に基づいて、前記予測値に関連付ける条件を満たす関連信号を求めて、前記予測値と前記関連信号との関連付けを行うように構成された関連付け部（３３）と、
   前記予測値と前記関連信号とから、現在の前記物標の状態を推定するように構成された推定部（３５）と、
   前記第２検知信号の信頼度を判定するように構成された信頼度判定部（３７）と、
   前記信頼度判定部の判定結果に基づいて、前記予測値と前記第２検知信号との前記関連付けコストを変更するように構成されたコスト変更部（３９）と、
   を備えた、物体追跡装置。
2. 請求項１に記載の物体追跡装置であって、
   前記信頼度判定部によって、前記第２検知信号の信頼度が低い状態であることを示す所定の条件が満たされたと判定された場合には、前記予測値と前記第２検知信号との前記関連付けコストを、前記所定の条件が満たされない場合に比べて、大きく設定するように構成された、
   物体追跡装置。
3. 請求項１または請求項２に記載の物体追跡装置であって、
   前記関連付け部は、前記関連付けコストに基づいて、前記予測値と前記第１検知信号又は前記第２検知信号との関連付けを行うように構成された、
   物体追跡装置。
4. 請求項１または請求項２に記載の物体追跡装置であって、
   前記関連付け部は、前記関連付けコストに基づいて、前記第１検知信号及び／又は前記第２検知信号から前記関連信号を求めて、前記予測値と前記関連信号との関連付けを行うように構成された、
   物体追跡装置。
5. 請求項４に記載の物体追跡装置であって、
   前記第１検知信号及び／又は前記第２検知信号が複数ある場合において、
   前記複数の検知信号について、前記関連付けコストに基づいて重み付けを行い、当該重み付けされた前記複数の検知信号に基づいて、前記関連信号を求めるように構成された、
   物体追跡装置。
6. 請求項１から請求項５までのいずれか１項に記載の物体追跡装置であって、
   前記第１信号抽出部は、前記強度分布から当該強度分布における信号の強度が閾値を超えかつ極大点である前記第１検知信号を抽出するように構成され、
   前記第２信号抽出部は、前記強度分布から前記物標の前記予測値に対応する前記第２検知信号を抽出するように構成された、
   物体追跡装置。
7. 請求項１から請求項６までのいずれか１項に記載の物体追跡装置であって、
   自車または前記物標の横速度の絶対値が所定値より大きい第１条件が満たされた場合に、前記第２検知信号の信頼度が低いと判断し、前記予測値と前記第２検知信号との前記関連付けコストを、前記第１条件が満たされない場合に比べて、大きく設定するように構成された、
   物体追跡装置。
8. 請求項１から請求項７までのいずれか１項に記載の物体追跡装置であって、
   自車または前記物標の加速度の絶対値が所定値より大きい第２条件が満たされた場合に、前記第２検知信号の信頼度が低いと判断し、前記予測値と前記第２検知信号との前記関連付けコストを、前記第２条件が満たされない場合に比べて、大きく設定するように構成された、
   物体追跡装置。
9. 請求項１から請求項８までのいずれか１項に記載の物体追跡装置であって、
   道路形状の曲率半径が所定値より小さい第３条件が満たされた場合に、前記第２検知信号の信頼度が低いと判断し、前記予測値と前記第２検知信号との前記関連付けコストを、前記第３条件が満たされない場合に比べて、大きく設定するように構成された、
   物体追跡装置。
10. 請求項１から請求項９までのいずれか１項に記載の物体追跡装置であって、
    前記第２検知信号の電力が所定値より小さい第４条件が満たされた場合に、前記第２検知信号の信頼度が低いと判断し、前記予測値と前記第２検知信号との前記関連付けコストを、前記第４条件が満たされない場合に比べて、大きく設定するように構成された、
    物体追跡装置。
11. 請求項１から請求項１０までのいずれか１項に記載の物体追跡装置であって、
    過去の予測誤差が所定値より大きい第５条件が満たされた場合に、前記第２検知信号の信頼度が低いと判断し、前記予測値と前記第２検知信号との前記関連付けコストを、前記第５条件が満たされない場合に比べて、大きく設定するように構成された、
    物体追跡装置。