特許 7260416 追跡装置

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2023-04-10

(45)【発行日】2023-04-18

(54)【発明の名称】追跡装置

(51)【国際特許分類】

   G01S 13/66 20060101AFI20230411BHJP

   G08G 1/16 20060101ALI20230411BHJP

【ＦＩ】

G01S13/66

G08G1/16 C

【請求項の数】 10

(21)【出願番号】P 2019117142

(22)【出願日】2019-06-25

(65)【公開番号】P2021004737

(43)【公開日】2021-01-14

【審査請求日】2021-12-02

【前置審査】

(73)【特許権者】

【識別番号】000004695

【氏名又は名称】株式会社ＳＯＫＥＮ

(73)【特許権者】

【識別番号】000004260

【氏名又は名称】株式会社デンソー

(74)【代理人】

【識別番号】110000578

【氏名又は名称】名古屋国際弁理士法人

(72)【発明者】

【氏名】赤峰 悠介

(72)【発明者】

【氏名】北村 尭之

【審査官】▲高▼場 正光

(56)【参考文献】

【文献】特開２００９－０４２１８１（ＪＰ，Ａ）

【文献】米国特許出願公開第２００８／００６５３２８（ＵＳ，Ａ１）

【文献】特開２００２－２６０１９２（ＪＰ，Ａ）

【文献】特公平０４－０５３３９４（ＪＰ，Ｂ２）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｇ０１Ｓ ７／００ － Ｇ０１Ｓ ７／６４

Ｇ０１Ｓ １３／００ － Ｇ０１Ｓ １７／９５

Ｇ０８Ｇ １／１６

(57)【特許請求の範囲】

【請求項1】

車両に搭載されて、前記車両の周辺に存在する一つまたは複数の物標を追跡する追跡装置（４）であって、
予め設定された処理サイクルの繰り返し周期が経過する毎に、前記一つまたは複数の物標をセンサで観測した観測情報と、前記一つまたは複数の物標における過去の状態量との少なくとも一方に基づいて、前記一つまたは複数の物標のそれぞれにおける現在の前記状態量を推定するように構成された状態量推定部（Ｓ４０，Ｓ２２０，Ｓ２３０）と、
前記一つまたは複数の物標のそれぞれに対して、前記一つまたは複数の物標の状態に基づいて、予め設定された少なくとも直線運動モデルからなる第１運動モデルと旋回運動モデルからなる第２運動モデルの中から一つの運動モデルを選択するように構成されたモデル選択部（Ｓ５０）と、
前記一つまたは複数の物標のそれぞれについて、前記モデル選択部で選択された前記一つの運動モデルで前記状態量推定部に前記状態量を推定させるように構成された推定選択部（Ｓ２１０）と
を備え、
前記モデル選択部は、
前記一つまたは複数の物標の状態を示すように予め設定されたパラメータを選択パラメータとし、前記選択パラメータは前記一つまたは複数の物標の対地速度または相対速度または縦方向位置または距離または横方向位置または方位または反射強度のうち少なくとも一つであって、前記選択パラメータが予め設定された選択判定値以上であるか否かを判断することにより前記一つの運動モデルを選択し、
前記一つまたは複数の物標の対地速度または相対速度が前記選択パラメータの場合には、前記選択パラメータが前記選択判定値以上である場合、
前記一つまたは複数の物標の縦方向位置または距離が前記選択パラメータの場合には、前記選択パラメータが前記選択判定値以上である場合、
前記一つまたは複数の物標の横方向位置または方位が前記選択パラメータの場合には、前記選択パラメータが前記選択判定値未満である場合、
前記一つまたは複数の物標の反射強度が前記選択パラメータの場合には、前記選択パラメータが前記選択判定値以上である場合のいずれか一つを含む場合には、前記第２運動モデルが選択される追跡装置。

【請求項2】

請求項１に記載の追跡装置であって、
前記モデル選択部は、予め設定された第２運動モデル選択条件が成立した場合に、前記第２運動モデルを選択し、前記第２運動モデル選択条件が成立していない場合に、前記第１運動モデルを選択する追跡装置。

【請求項3】

請求項１または請求項２に記載の追跡装置であって、
前記第２運動モデルは、状態変数に加速度を含む追跡装置。

【請求項4】

請求項１～請求項３の何れか１項に記載の追跡装置であって、
前記状態量推定部は、更に、前記一つまたは複数の物標における過去の前記状態量の推定値に基づいて、前記一つまたは複数の物標のそれぞれにおける現在の前記状態量の予測値を算出し、
前記モデル選択部は、更に、現在の前記一つの運動モデルを設定してから経過した前記処理サイクルの数である設定経過サイクルが予め設定された切替判定サイクル以上であり、且つ、前記予測値と、前記観測情報が示す観測値との差である予測残差が予め設定された切替判定値以上である場合に、前回に選択した前記一つの運動モデルと異なる前記一つの運動モデルを選択する追跡装置。

【請求項5】

請求項１に記載の追跡装置であって、
前記選択判定値は、選択されている前記一つの運動モデルに応じて変更される追跡装置。

【請求項6】

請求項１～請求項５の何れか１項に記載の追跡装置であって、
前記状態量推定部は、更に、前記状態量の誤差共分散を推定する追跡装置。

【請求項7】

請求項６に記載の追跡装置であって、
前記一つまたは複数の物標のそれぞれについて、前記一つの運動モデルが前回から変更された場合に、前記状態量および前記誤差共分散の少なくとも一方を、変更された後の前記一つの運動モデルに対応するように変換するように構成された変換部（Ｓ５２０，Ｓ６２０）を備える追跡装置。

【請求項8】

請求項１～請求項７の何れか１項に記載の追跡装置であって、
選択された前記一つの運動モデルに関わらず、前記一つまたは複数の物標の前記状態量が格納されるように構成された状態量格納部（１３ａ）を備え、
前記状態量格納部は、少なくとも前記第１運動モデルと前記第２運動モデルのうち状態変数の数が最も多い前記一つの運動モデルにおける前記状態量を格納可能な容量を有する追跡装置。

【請求項9】

請求項６に記載の追跡装置であって、
選択された前記一つの運動モデルに関わらず、前記一つまたは複数の物標の前記状態量および前記誤差共分散を共通の物理量に変換して出力するように構成された出力部（Ｓ７０）を備える追跡装置。

【請求項10】

請求項１～請求項９の何れか１項に記載の追跡装置であって、
前記モデル選択部は、
さらに前記車両の状態を示すように予め設定されたパラメータを前記選択パラメータとし、前記選択パラメータは前記車両の回転半径または加速度の絶対値のうち少なくとも一つであって、前記選択パラメータが前記選択判定値以上であるか否かを判断することにより前記一つの運動モデルを選択し、
前記車両の回転半径が前記選択パラメータの場合には、前記選択パラメータが前記選択判定値未満である場合、
前記車両の加速度の絶対値が前記選択パラメータの場合には、前記選択パラメータが前記選択判定値以上である場合、のいずれか一つを含む場合には前記第２運動モデルが選択される追跡装置。

【発明の詳細な説明】

【技術分野】

【0001】

本開示は、車両の周辺に存在する物標を追跡する追跡装置に関する。

【背景技術】

【0002】

車載レーダにより物標の位置および速度などの状態量を算出するためには、前回の処理サイクルでの物標の位置および速度などの推定値から求めた今回の処理サイクルでの予測値と、今回の観測値とのフィルタ処理を行うのが一般的である。しかし、予測値の算出に用いる運動モデルと物標の実際の運動との間で乖離がある場合には、物標の状態量を精度良く推定することができない。それに対し、特許文献１には、運動モデルが互いに異なる複数のフィルタ処理部がそれぞれ物標の状態量を推定し、推定結果の収束状況に応じて、何れか一つのフィルタ処理部の出力を選択することで、精度良い推定結果を得る目標追尾装置が記載されている。

【先行技術文献】

【特許文献】

【0003】

【文献】特許第４３４８５３５号公報

【発明の概要】

【発明が解決しようとする課題】

【0004】

しかし、特許文献１に記載の技術では、一つの物標に対して複数のフィルタ処理を並行して実行させる必要があるため、処理負荷が大きくなってしまうという問題があった。
本開示は、処理負荷の増加を抑制して精度良く物標を追跡することを目的とする。

【課題を解決するための手段】

【0005】

本開示の一態様は、車両に搭載されて、車両の周辺に存在する一つまたは複数の物標を追跡する追跡装置（４）であって、状態量推定部（Ｓ４０，Ｓ２２０，Ｓ２３０）と、モデル選択部（Ｓ５０）と、推定選択部（Ｓ２１０）とを備える。

【0006】

状態量推定部は、予め設定された処理サイクルの繰り返し周期が経過する毎に、一つまたは複数の物標をセンサ（２）で観測した観測情報と、一つまたは複数の物標における過去の状態量との少なくとも一方に基づいて、一つまたは複数の物標のそれぞれにおける現在の状態量を推定するように構成される。

【0007】

モデル選択部は、一つまたは複数の物標のそれぞれに対して、一つまたは複数の物標の状態と、車両の状態との少なくとも一方に基づいて、予め設定された複数の運動モデルの中から一つの運動モデルを選択するように構成される。

【0008】

推定選択部は、一つまたは複数の物標のそれぞれについて、モデル選択部で選択された一つの運動モデルで状態量推定部に状態量を推定させるように構成される。
このように構成された本開示の追跡装置は、一つの物標に対して一つの運動モデルを用いて状態量を推定する。このため、本開示の追跡装置は、一つの物標に対して複数の運動モデルを並行して実行するという事態の発生を抑制し、処理負荷の増加を抑制することができる。さらに、本開示の追跡装置は、複数の運動モデルの中から一つの運動モデルを選択して、一つまたは複数の物標のそれぞれにおける現在の状態量を推定する。このため、本開示の追跡装置は、一つまたは複数の物標のそれぞれに対して適切な一つの運動モデルを選択して状態量を推定することができ、状態量の推定精度を向上させることができる。これにより、本開示の追跡装置は、精度良く物標を追跡することができる。

【図面の簡単な説明】

【0009】

【図1】運転支援システムの構成を示すブロック図である。

【図2】第１実施形態におけるレーダ装置の設置位置と検出範囲とを示す図である。

【図3】追跡処理を示すフローチャートである。

【図4】予測処理を示すフローチャートである。

【図5】状態量予測処理を示すフローチャートである。

【図6】関連付け処理を示すフローチャートである。

【図7】第１実施形態のモデル設定処理を示すフローチャートである。

【図8】第１設定処理を示すフローチャートである。

【図9】第２設定処理を示すフローチャートである。

【図10】登録処理を示すフローチャートである。

【図11】自車両の前方で道路を横断するために直進する歩行者を示す図である。

【図12】道路のカーブの手前を走行する自車両および先行車両を示す図である。

【図13】状態量格納部を示す図である。

【図14】第２実施形態のモデル設定処理を示すフローチャートである。

【図15】別の実施形態におけるレーダ装置の設置位置と検出範囲とを示す図である。

【発明を実施するための形態】

【0010】

（第１実施形態）
以下に本開示の第１実施形態を図面とともに説明する。
本実施形態の運転支援システム１は、車両に搭載され、図１に示すように、レーダ装置２と、挙動センサ群３と、追跡装置４と、支援実行部５とを備える。以下、運転支援システム１を搭載する車両を自車両という。

【0011】

レーダ装置２は、ミリ波またはマイクロ波等のレーダ波を送信し、反射したレーダ波を受信する。図２に示すように、レーダ装置２は、自車両の前方に向けてレーダ波を送信することにより、検出範囲Ｒｄ内に存在してレーダ波を反射した反射点に関する情報（以下、観測情報）を生成する。観測情報には、反射点までの距離と、反射点の方位と、反射点の相対速度と、反射点で反射したレーダ波の強度（以下、反射強度）とが含まれる。

【0012】

図１に示すように、挙動センサ群３は、自車両の挙動に関する情報と、自車両の挙動に影響を与える運転操作に関する情報とを検出するための各種機器からなる。挙動センサ群３が検出する情報には、例えば、アクセルペダルの操作量、ブレーキペダルの操作量、操舵角、車両速度および車両加速度が含まれる。

【0013】

追跡装置４は、レーダ装置２により生成された観測情報に基づいて、自車両の周囲に存在する物標を追跡し、追跡した物標の状態量を示す物標情報を生成する。
追跡装置４は、ＣＰＵ１１、ＲＯＭ１２およびＲＡＭ１３等を備えたマイクロコンピュータを中心に構成された電子制御装置である。マイクロコンピュータの各種機能は、ＣＰＵ１１が非遷移的実体的記録媒体に格納されたプログラムを実行することにより実現される。この例では、ＲＯＭ１２が、プログラムを格納した非遷移的実体的記録媒体に該当する。また、このプログラムの実行により、プログラムに対応する方法が実行される。なお、ＣＰＵ１１が実行する機能の一部または全部を、一つあるいは複数のＩＣ等によりハードウェア的に構成してもよい。また、追跡装置４を構成するマイクロコンピュータの数は１つでも複数でもよい。

【0014】

支援実行部５は、例えば、アクチュエータ、オーディオ装置および表示装置などを備える。支援実行部５は、追跡装置４にて生成される物標情報に基づき、自車両の挙動を制御したり、自車両の運転者に対する報知を実行したりする。

【0015】

追跡装置４のＣＰＵ１１が実行する追跡処理の手順を説明する。追跡処理は、追跡装置４の動作中において、予め設定された処理サイクル毎に繰り返し実行される処理である。処理サイクルの繰り返し周期をΔＴとする。以下では、本処理により追跡の対象となっている物標を追跡物標と呼ぶ。

【0016】

追跡処理が実行されると、ＣＰＵ１１は、図３に示すように、まずＳ１０にて、予測処理を実行する。ここで、Ｓ１０で実行される予測処理の手順を説明する。
予測処理が実行されると、ＣＰＵ１１は、図４に示すように、まずＳ１１０にて、一つまたは複数の追跡物標の中から、今回の予測処理において選択されていない一つの追跡物標を選択する。以下、選択された追跡物標を対象物標という。

【0017】

そしてＳ１２０にて、ＣＰＵ１１は、状態量予測処理を実行する。ここで、Ｓ１２０で実行される状態量予測処理の手順を説明する。
状態量予測処理が実行されると、ＣＰＵ１１は、図５に示すように、まずＳ２１０にて、対象物標に対して、後述の第１運動モデルが設定されているか、後述の第２運動モデルが設定されているかを判断する。ここで、第１運動モデルが設定されている場合には、ＣＰＵ１１は、Ｓ２２０にて、後述する第１予測処理を実行し、状態量予測処理を終了する。一方、第２運動モデルが設定されている場合には、ＣＰＵ１１は、Ｓ２３０にて、後述する第２予測処理を実行し、状態量予測処理を終了する。

【0018】

次に、Ｓ２２０で実行される第１予測処理を説明する。
第１予測処理では、ＣＰＵ１１は、第１運動モデルを用いて、前回の処理サイクルで求められた対象物標の状態ベクトルの推定値に基づき、今回の処理サイクルでの状態ベクトルの予測値を算出する。下式（１）～（６）は、拡張カルマンフィルタを表す式である。

【0019】

Ｘ_{ｋ｜ｋ－１}は状態ベクトルの予測値（すなわち、予測ベクトル）である。Ｘ_ｋは状態ベクトルの推定値である。ｚ_ｋは観測値である。Ｐ_{ｋ｜ｋ－１}は状態ベクトルの予測値の誤差共分散行列である。Ｐ_ｋは誤差共分散行列の推定値である。Ｓ_ｋはイノベーション行列である。Ｋ_ｋはカルマンゲインである。ｆは前の状態量からの予測値を与える関数である。ｈは観測値を与える関数である。Ｑ_ｋはプロセスノイズの分散である。Ｆ_ｋは関数ｆのヤコビアンで定義される状態遷移行列である。Ｒ_ｋは観測ノイズの誤差共分散行列である。Ｈ_ｋは関数ｈのヤコビアンで定義される状態空間を観測空間に写像する変換行列である。また、Ｘ_０は状態ベクトルの初期値（すなわち、初期推定ベクトル）、Ｐ_０は誤差共分散行列の初期値を表す。

【0020】

【数1】

そして第１運動モデルは、直線運動モデルである。直線運動モデルは、図１１に示すように、自車両Ｖ１の前方で、道路を横断するために直進する歩行者ＰＤを検出するためのモデルである。

【0021】

第１運動モデルでは、物標の状態量は、横方向位置ｘ、縦方向位置ｙ、横方向速度ｖ_ｘおよび縦方向速度ｖ_ｙで表される。これらの状態量を要素としてベクトル表現した状態ベクトルは、Ｘ＝［ｘ，ｙ，ｖ_ｘ，ｖ_ｙ］^Ｔで表される。なお、横方向は自車両の車幅方向であり、縦方向は車幅方向に垂直な方向である。

【0022】

また第１運動モデルでは、今回の処理サイクルでの状態量の予測値を状態ベクトルＸ_{ｋ｜ｋ－１}＝［ｘ’，ｙ’，ｖ_ｘ’，ｖ_ｙ’］^Ｔとし、前回の処理サイクルでの状態量の推定値を状態ベクトルＸ_ｋ－１＝［ｘ，ｙ，ｖ_ｘ，ｖ_ｙ］^Ｔとする。そして関数ｆは、状態ベクトルＸ_{ｋ｜ｋ－１}と状態ベクトルＸ_ｋ－１との間で、式（７）に示す関係が成立するように設定される。

【0023】

【数2】

すなわち、Ｓ２２０の第１予測処理では、ＣＰＵ１１は、式（７）で設定される関数ｆを用いて、式（１）により、今回の処理サイクルにおける状態ベクトルの予測値を算出し、式（２）により、今回の処理サイクルにおける状態ベクトルの予測値の誤差共分散行列を算出する。

【0024】

次に、Ｓ２３０で実行される第２予測処理を説明する。
Ｓ２３０の第２予測処理では、ＣＰＵ１１は、第２運動モデルを用いて、前回の処理サイクルで求められた対象物標の状態ベクトルの推定値に基づき、今回の処理サイクルでの状態ベクトルの予測値を算出する。

【0025】

そして第２運動モデルは、旋回運動モデルである。旋回運動モデルは、図１２に示すように、自車両Ｖ１の前方で、道路のカーブＣＶを走行する先行車両Ｖ２を検出するためのモデルである。

【0026】

第２運動モデルでは、物標の状態量は、横方向位置ｘ、縦方向位置ｙ、速度ｖ、進行方向θ、加速度ａおよびヨーレートωで表される。これらの状態量を要素としてベクトル表現した状態ベクトルは、Ｘ＝［ｘ，ｙ，ｖ，θ，ａ，ω］^Ｔで表される。

【0027】

また第２運動モデルでは、今回の処理サイクルでの状態量の予測値を状態ベクトルＸ_{ｋ｜ｋ－１}＝［ｘ’，ｙ’，ｖ’，θ’，ａ’，ω’］^Ｔとし、前回の処理サイクルでの状態量の推定値を状態ベクトルＸ_ｋ－１＝［ｘ，ｙ，ｖ，θ，ａ，ω］^Ｔとする。そして関数ｆは、状態ベクトルＸ_{ｋ｜ｋ－１}と状態ベクトルＸ_ｋ－１との間で、式（８）に示す関係が成立するように設定される。

【0028】

【数3】

すなわち、Ｓ２３０の第２予測処理では、ＣＰＵ１１は、式（８）で設定される関数ｆを用いて、式（１）により、今回の処理サイクルにおける状態ベクトルの予測値を算出し、式（２）により、今回の処理サイクルにおける状態ベクトルの予測値の誤差共分散行列を算出する。

【0029】

なお、ＣＰＵ１１は、算出した状態量を、ＲＡＭ１３に設けられた状態量格納部１３ａに格納する。
状態量格納部１３ａは、図１３に示すように、複数の物標のそれぞれについて、［ｘ，ｙ，ｖ_ｘ，ｖ_ｙ］または［ｘ，ｙ，ｖ，θ，ａ，ω］を格納するように形成されている。具体的には、複数の物標のそれぞれについて、格納領域Ｒ１，Ｒ２，Ｒ３，Ｒ４，Ｒ５，Ｒ６が設けられている。そしてＣＰＵ１１は、例えば、第１運動モデルで状態ベクトルの推定値を算出した場合には、対応する物標の格納領域Ｒ１，Ｒ２，Ｒ３，Ｒ４にそれぞれ、ｘ，ｙ，ｖ_ｘ，ｖ_ｙを格納する。またＣＰＵ１１は、第２運動モデルで状態ベクトルの推定値を算出した場合には、対応する物標の格納領域Ｒ１，Ｒ２，Ｒ３，Ｒ４，Ｒ５，Ｒ６にそれぞれ、ｘ，ｙ，ｖ，θ，ａ，ωを格納する。

【0030】

状態量予測処理が終了すると、ＣＰＵ１１は、図４に示すように、Ｓ１３０にて、一つまたは複数の追跡物標の中の全ての追跡物標をＳ１１０で選択したか否かを判断する。ここで、全ての追跡物標を選択していない場合には、ＣＰＵ１１は、Ｓ１１０に移行する。一方、全ての追跡物標を選択した場合には、ＣＰＵ１１は、予測処理を終了する。

【0031】

予測処理が終了すると、ＣＰＵ１１は、図３に示すように、Ｓ２０にて、レーダ装置２により検出された反射点と、追跡物標とを関連付ける関連付け処理を実行する。ここで、Ｓ２０で実行される関連付け処理の手順を説明する。

【0032】

関連付け処理が実行されると、ＣＰＵ１１は、図６に示すように、まずＳ３１０にて、今回の処理サイクルで検出された一つまたは複数の反射点の中から、今回の関連付け処理において選択されていない一つの反射点を選択する。以下、選択された反射点を選択反射点という。

【0033】

次にＳ３２０にて、ＣＰＵ１１は、一つまたは複数の追跡物標の中から、今回の関連付け処理において選択されていない一つの追跡物標を選択物標として選択する。さらにＳ３３０にて、ＣＰＵ１１は、選択反射点と選択物標との間の距離を算出する。

【0034】

そしてＳ３４０にて、ＣＰＵ１１は、一つまたは複数の追跡物標の中の全ての追跡物標をＳ３２０で選択したか否かを判断する。ここで、全ての追跡物標を選択していない場合には、ＣＰＵ１１は、Ｓ３２０に移行する。一方、全ての追跡物標を選択した場合には、ＣＰＵ１１は、Ｓ３５０にて、今回の処理サイクルで検出された一つまたは複数の反射点の中の全ての反射点をＳ３１０で選択したか否かを判断する。ここで、全ての反射点を選択していない場合には、ＣＰＵ１１は、Ｓ３１０に移行する。

【0035】

一方、全ての反射点を選択した場合には、ＣＰＵ１１は、Ｓ３６０にて、一つまたは複数の追跡物標の中でＳ３７０の処理が実行されていない一つの追跡物標を選択物標として選択する。次にＳ３７０にて、ＣＰＵ１１は、Ｓ３３０で算出された距離に基づいて、反射点を選択物標と関連付けるか否かを決定する。以下、選択物標に関連づけられた反射点を関連反射点という。関連付け方法としては、例えば、選択物標Ｍｊから最も距離が小さい反射点が反射点Ｐｉであり、かつ、反射点Ｐｉから最も距離が小さい物標が選択物標Ｍｊである場合に、反射点Ｐｉと選択物標Ｍｊとを関連付ける。

【0036】

またＳ３８０にて、ＣＰＵ１１は、Ｓ１２０で算出された状態ベクトルの予測値が示す選択物標の位置と、関連反射点の観測情報が示す関連反射点の位置との間の距離を、選択物標の予測残差として算出する。

【0037】

そしてＳ３９０にて、ＣＰＵ１１は、一つまたは複数の追跡物標の中の全ての追跡物標をＳ３６０で選択したか否かを判断する。ここで、全ての追跡物標を選択していない場合には、ＣＰＵ１１は、Ｓ３６０に移行する。一方、全ての追跡物標を選択した場合には、ＣＰＵ１１は、関連付け処理を終了する。

【0038】

関連付け処理が終了すると、ＣＰＵ１１は、図３に示すように、Ｓ３０にて、関連反射点が存在しない追跡物標を消滅させる消滅処理を実行する。具体的には、追跡物標毎に、消滅カウンタが設けられ、ＣＰＵ１１は、関連反射点が存在する場合に消滅カウンタをリセットする。一方、関連反射点が存在しない場合には、ＣＰＵ１１は、消滅カウンタをインクリメントする。そして、消滅カウンタの値が所定値に達した場合、すなわち、所定回の処理サイクルで連続して関連反射点が存在しなかった場合に、ＣＰＵ１１は、その追跡物標を消滅させる。

【0039】

そしてＳ４０にて、ＣＰＵ１１は、Ｓ２０の関連付け処理によって互いに関連づけられた追跡物標の状態ベクトルの予測値と、関連反射点の観測情報とに基づいて、今回の処理サイクルで追跡物標の状態ベクトルの推定値を算出する更新処理を実行する。具体的には、ＣＰＵ１１は、式（３）～（６）を用いて、今回の処理サイクルにおける状態ベクトルの推定値（すなわち、状態ベクトルＸ_ｋ）と、誤差共分散行列の推定値（すなわち、行列Ｐ_ｋ）とを算出する。なお、ＣＰＵ１１は、Ｓ３７０において関連反射点が存在していない追跡物標の状態ベクトルおよび誤差共分散行列の予測値を、推定値に設定する。

【0040】

更新処理が終了すると、ＣＰＵ１１は、Ｓ５０にて、モデル設定処理を実行する。ここで、Ｓ５０で実行されるモデル設定処理の手順を説明する。
モデル設定処理が実行されると、ＣＰＵ１１は、図７に示すように、まずＳ４１０にて、一つまたは複数の追跡物標の中から、今回のモデル設定処理において選択されていない一つの追跡物標を選択する。以下、選択された追跡物標を対象物標という。
そしてＳ４１１にて、ＣＰＵ１１は、対象物標の後述する経過サイクルが予め設定された第５判定値以下であるか否かを判断する。なお、追跡物標毎に経過サイクルカウンタが設けられている。そしてＣＰＵ１１は、追跡物標が検出されてから処理サイクルの繰り返し周期が経過する毎に、対応する経過サイクルカウンタをインクリメント（すなわち、１加算）する。経過サイクルは、経過サイクルカウンタの値であり、対象物標を検出してから経過した処理サイクルの数に相当する。
ここで、経過サイクルが第５判定値以下である場合には、ＣＰＵ１１は、Ｓ５１０に移行する。

【0041】

一方、経過サイクルが第５判定値を超えている場合には、ＣＰＵ１１は、Ｓ４２０にて、ＣＰＵ１１は、挙動センサ群３から取得した車両速度Ｖ_ｎと、Ｓ４０で算出した状態ベクトルの推定値を用いて算出した縦方向速度とに基づいて、対地速度Ｖ_ａｂｓを算出する。対地速度Ｖ_ａｂｓは、地面に対する物標の進行速度である。すなわち、対地速度Ｖ_ａｂｓは、自車両の走行速度に、自車両の進行方向に沿った物標の相対速度を加算することにより算出される。

【0042】

具体的には、ＣＰＵ１１は、Ｓ１２０において第１運動モデルで状態ベクトルの予測値を算出した場合には、式（９）により、対地速度Ｖ_ａｂｓを算出する。またＣＰＵ１１は、Ｓ１２０において第２運動モデルで状態ベクトルの予測値を算出した場合には、式（１０）により、対地速度Ｖ_ａｂｓを算出する。

【0043】

Ｖ_ａｂｓ＝Ｖ_ｎ＋ｖ_ｙ ・・・（９）
Ｖ_ａｂｓ＝Ｖ_ｎ＋ｖ／ｃｏｓθ ・・・（１０）
そしてＳ４３０にて、ＣＰＵ１１は、Ｓ４２０にて算出された対地速度Ｖ_ａｂｓが予め設定された第１判定値以上であるか否かを判断する。ここで、対地速度Ｖ_ａｂｓが第１判定値以上である場合には、ＣＰＵ１１は、Ｓ５２０に移行する。一方、対地速度Ｖ_ａｂｓが第１判定値未満である場合には、ＣＰＵ１１は、Ｓ４４０にて、対象物標の縦方向位置が予め設定された第２判定値以上であるか否かを判断する。

【0044】

ここで、縦方向位置が第２判定値以上である場合には、ＣＰＵ１１は、Ｓ５２０に移行する。一方、縦方向位置が第２判定値未満である場合には、ＣＰＵ１１は、Ｓ４５０にて、対象物標の横方向位置が予め設定された第３判定値未満であるか否かを判断する。ここで、横方向位置が第３判定値未満である場合には、ＣＰＵ１１は、Ｓ５２０に移行する。

【0045】

一方、横方向位置が第３判定値以上である場合には、ＣＰＵ１１は、Ｓ４６０にて、対象物標に対応する観測情報に基づいて、対象物標の反射強度が予め設定された第４判定値以上であるか否かを判断する。ここで、反射強度が第４判定値以上である場合には、ＣＰＵ１１は、Ｓ５２０に移行する。

【0046】

一方、反射強度が第４判定値未満である場合には、ＣＰＵ１１は、Ｓ４８０にて、挙動センサ群３から取得した操舵角に基づいて、自車両が走行している走行経路の回転半径を算出する。

【0047】

そしてＳ４９０にて、ＣＰＵ１１は、Ｓ４８０で算出した回転半径が予め設定された第６判定値未満であるか否かを判断する。ここで、回転半径が第６判定値未満である場合には、ＣＰＵ１１は、Ｓ５２０に移行する。一方、回転半径が第６判定値以上である場合には、ＣＰＵ１１は、Ｓ５００にて、挙動センサ群３から取得した車両加速度に基づいて、車両加速度の絶対値が予め設定された第７判定値以上であるか否かを判断する。

【0048】

ここで、車両加速度の絶対値が第７判定値以上である場合には、ＣＰＵ１１は、Ｓ５２０に移行する。一方、車両加速度の絶対値が第７判定値未満である場合には、ＣＰＵ１１は、Ｓ５１０にて、後述する第１設定処理を実行し、Ｓ５３０に移行する。また、Ｓ５２０に移行すると、ＣＰＵ１１は、後述する第２設定処理を実行し、Ｓ５３０に移行する。

【0049】

そしてＳ５３０に移行すると、一つまたは複数の追跡物標の中の全ての追跡物標をＳ４１０で選択したか否かを判断する。ここで、全ての追跡物標を選択していない場合には、ＣＰＵ１１は、Ｓ４１０に移行する。一方、全ての追跡物標を選択した場合には、ＣＰＵ１１は、モデル設定処理を終了する。

【0050】

ここで、Ｓ５１０で実行される第１設定処理の手順を説明する。
第１設定処理が実行されると、ＣＰＵ１１は、図８に示すように、まず、Ｓ６１０にて、対象物標に対して第２運動モデルが設定されているか否かを判断する。ここで、第２運動モデルが設定されていない場合には、ＣＰＵ１１は、第１設定処理を終了する。一方、第２運動モデルが設定されている場合には、ＣＰＵ１１は、Ｓ６２０にて、状態変数ｘ，ｙ，ｖ，θ，ａ，ωで表されている状態ベクトルおよび誤差共分散行列を、状態変数ｘ，ｙ，ｖ_ｘ，ｖ_ｙで表される状態ベクトルおよび誤差共分散行列へ変換する。そしてＳ６３０にて、ＣＰＵ１１は、対象物標に対して第１運動モデルを設定し、第１設定処理を終了する。

【0051】

次に、Ｓ５２０で実行される第２設定処理の手順を説明する。
第２設定処理が実行されると、ＣＰＵ１１は、図９に示すように、まず、Ｓ７１０にて、対象物標に対して第１運動モデルが設定されているか否かを判断する。ここで、第１運動モデルが設定されていない場合には、ＣＰＵ１１は、第２設定処理を終了する。一方、第１運動モデルが設定されている場合には、ＣＰＵ１１は、Ｓ７２０にて、状態変数ｘ，ｙ，ｖ_ｘ，ｖ_ｙで表されている状態ベクトルおよび誤差共分散行列を、状態変数ｘ，ｙ，ｖ，θ，ａ，ωで表される状態ベクトルおよび誤差共分散行列へ変換する。そしてＳ７３０にて、ＣＰＵ１１は、対象物標に対して第２運動モデルを設定し、第２設定処理を終了する。

【0052】

モデル設定処理が終了すると、ＣＰＵ１１は、図３に示すように、Ｓ６０にて、追跡物標と関連付けられなかった反射点を新規物標として新たに登録する登録処理を実行する。ここで、Ｓ６０で実行される登録処理の手順を説明する。

【0053】

登録処理が実行されると、ＣＰＵ１１は、図１０に示すように、まずＳ８１０にて、今回の処理サイクルで検出された反射点のうち、追跡物標と関連付けられなかった反射点を非関連反射点とし、一つまたは複数の非関連反射点の中から、今回の登録処理において選択されていない一つの非関連反射点を選択する。

【0054】

次にＳ８２０にて、ＣＰＵ１１は、選択された非関連反射点の観測情報（すなわち、反射点までの距離と、反射点の方位と、反射点の相対速度）と、挙動センサ群３から取得した操舵角および車両加速度とに基づいて、Ｘ＝［ｘ，ｙ，ｖ，θ，ａ，ω］^Ｔで表されている状態ベクトルの初期値（すなわち、初期推定ベクトル）を算出する。

【0055】

そしてＳ８３０にて、ＣＰＵ１１は、選択された非関連反射点を、Ｓ８２０で算出した初期推定ベクトルとともに、新規物標として登録する。
さらにＳ８４０にて、ＣＰＵ１１は、Ｓ８１０において全ての非関連反射点を選択したか否かを判断する。ここで、全ての非関連反射点を選択していない場合には、ＣＰＵ１１は、Ｓ８１０に移行する。一方、全ての非関連反射点を選択した場合には、ＣＰＵ１１は、登録処理を終了する。

【0056】

登録処理が終了すると、ＣＰＵ１１は、図３に示すように、Ｓ７０にて、物標情報を生成して、生成した物標情報を支援実行部５へ出力し、追跡処理を一旦終了する。具体的には、Ｓ７０にて、ＣＰＵ１１は、まず、一つまたは複数の追跡物標の状態ベクトルの推定値と、一つまたは複数の新規物標の状態ベクトルの初期値とを、第１運動モデルの状態変数で表される状態ベクトル（すなわち、Ｘ＝［ｘ，ｙ，ｖ_ｘ，ｖ_ｙ］^Ｔで表される状態ベクトル）に変換する。なお、追跡物標の状態ベクトルの推定値はＳ４０で算出され、新規物標の状態ベクトルの初期値はＳ５０で算出される。すなわち、ＣＰＵ１１は、追跡物標の状態ベクトルの推定値と、新規物標の状態ベクトルの初期値とについて、第２運動モデルの状態変数で表されている場合に、変換処理を実行する。さらにＣＰＵ１１は、誤差共分散行列を、状態変数ｘ，ｙ，ｖ_ｘ，ｖ_ｙで表される誤差共分散行列へ変換する。そしてＣＰＵ１１は、第１運動モデルの状態変数に変換された追跡物標の状態ベクトルの推定値と、第１運動モデルの状態変数に変換された新規物標の状態ベクトルの初期値と、第１運動モデルの状態変数に変換された誤差共分散行列とを、物標情報として生成する。

【0057】

このように構成された追跡装置４は、車両に搭載されて、車両の周辺に存在する一つまたは複数の物標を追跡する。
追跡装置４は、予め設定された処理サイクルの繰り返し周期ΔＴが経過する毎に、一つまたは複数の物標をレーダ装置２で観測した観測情報と、一つまたは複数の物標における過去の状態ベクトルの推定値Ｘ_ｋ－１とに基づいて、一つまたは複数の物標のそれぞれにおける現在の状態ベクトルの推定値Ｘ_ｋを推定する。

【0058】

追跡装置４は、一つまたは複数の物標のそれぞれに対して、一つまたは複数の物標の状態と、車両の状態との少なくとも一方に基づいて、予め設定された第１運動モデルおよび第２運動モデルの中から一つの運動モデルを選択する。

【0059】

追跡装置４は、一つまたは複数の物標のそれぞれについて、選択された一つの運動モデルで状態ベクトルの推定値Ｘ_ｋを推定する。
このように追跡装置４は、一つの物標に対して一つの運動モデルを用いて状態ベクトルの推定値を推定する。このため、追跡装置４は、一つの物標に対して複数の運動モデルを並行して実行するという事態の発生を抑制し、処理負荷の増加を抑制することができる。さらに追跡装置４は、複数の運動モデルの中から一つの運動モデルを選択して、一つまたは複数の物標のそれぞれにおける現在の状態ベクトルの推定値Ｘ_ｋを推定する。このため、追跡装置４は、一つまたは複数の物標のそれぞれに対して適切な一つの運動モデルを選択して状態ベクトルの推定値Ｘ_ｋを推定することができ、状態ベクトルの推定値Ｘ_ｋの推定精度を向上させることができる。これにより、追跡装置４は、精度良く物標を追跡することができる。

【0060】

また、第１運動モデルは、直線運動モデルであり、第２運動モデルは、旋回運動モデルである。車両に搭載されたレーダ装置２における主な物標は、車両と歩行者である。車両は操舵角に応じた旋回動作をするため、運動モデルとしては旋回運動モデルが適している。一方、例えば自車両の前方で横断する歩行者は、直線的な動きである可能性が高く、また、車両と異なり一定の回転半径での旋回挙動をしないため、旋回運動モデルよりも直線運動モデルが適している。

【0061】

追跡装置４は、物標の対地速度Ｖ_ａｂｓが第１判定値以上である場合に、第２運動モデルを選択する。
追跡装置４は、物標の縦方向位置が第２判定値以上である場合に、第２運動モデルを選択する。

【0062】

追跡装置４は、物標の横方向位置が第３判定値未満である場合に、第２運動モデルを選択する。
追跡装置４は、物標の反射強度が第４判定値以上である場合に、第２運動モデルを選択する。

【0063】

追跡装置４は、物標の経過サイクルが第５判定値以下である場合に、第１運動モデルを選択する。
追跡装置４は、自車両の回転半径が第６判定値未満である場合に、第２運動モデルを選択する。

【0064】

追跡装置４は、車両加速度の絶対値が第７判定値以上である場合に、第２運動モデルを選択する。
第２運動モデルは、状態変数に加速度ａを含む。これにより、追跡装置４は、物標の追跡精度を向上させることができる。

【0065】

追跡装置４は、Ｓ４３０，Ｓ４４０，Ｓ４５０，Ｓ４６０，Ｓ４７０，Ｓ４９０，Ｓ５００で否定判断された場合に、第１運動モデルを選択する。Ｓ４３０，Ｓ４４０，Ｓ４５０，Ｓ４６０，Ｓ４７０，Ｓ４９０，Ｓ５００の判断条件は、第２運動モデルを選択するために設定された第２運動モデル選択条件である。

【0066】

追跡装置４は、更に、誤差共分散行列の推定値Ｐ_ｋを推定する。
追跡装置４は、一つまたは複数の物標のそれぞれについて、一つの運動モデルが前回から変更された場合に、状態ベクトルおよび誤差共分散行列を、変更された後の一つの運動モデルに対応するように変換する。

【0067】

追跡装置４は、選択された一つの運動モデルに関わらず、一つまたは複数の物標の状態ベクトルが格納される状態量格納部１３ａを備える。状態量格納部１３ａは、第１運動モデルおよび第２運動モデルのうち状態変数の数が最も多い第２運動モデルにおける状態ベクトルを格納可能な容量を有する。これにより、追跡装置４は、第１運動モデルおよび第２運動モデルにおける状態ベクトルを、共通の状態量格納部１３ａに格納することができる。このため、追跡装置４は、第１運動モデルおよび第２運動モデルにおける状態ベクトルを格納するために、第１運動モデルと第２運動モデルとで別々のデータ格納部を設ける必要がなく、データ記憶容量の増加を抑制することができる。

【0068】

追跡装置４は、選択された一つの運動モデルに関わらず、一つまたは複数の物標の状態ベクトルおよび誤差共分散行列を、状態変数ｘ，ｙ，ｖ_ｘ，ｖ_ｙに変換して出力する。これにより、追跡装置４は、支援実行部５において、状態ベクトルおよび誤差共分散行列を、第１運動モデルの状態変数と第２運動モデルの状態変数との間で変換する処理を不要とすることができる。このため、追跡装置４は、支援実行部５における物標情報の利用を容易にすることができる。

【0069】

以上説明した実施形態において、Ｓ４０，Ｓ２２０，Ｓ２３０は状態量推定部としての処理に相当し、Ｓ５０はモデル選択部としての処理に相当し、Ｓ２１０は推定選択部としての処理に相当し、状態ベクトルは状態量に相当する。

【0070】

また、物標の対地速度Ｖ_ａｂｓ、物標の縦方向位置、物標の横方向位置、物標の反射強度、物標の経過サイクルおよび自車両の回転半径は選択パラメータに相当する。
また、第１判定値、第２判定値、第３判定値、第４判定値、第５判定値、第６判定値および第７判定値は選択判定値に相当する。

【0071】

また、誤差共分散行列は誤差共分散に相当し、Ｓ６２０，Ｓ７２０は変換部としての処理に相当し、Ｓ７０は出力部としての処理に相当し、状態変数ｘ，ｙ，ｖ_ｘ，ｖ_ｙは共通の物理量に相当する。

【0072】

（第２実施形態）
以下に本開示の第２実施形態を図面とともに説明する。なお第２実施形態では、第１実施形態と異なる部分を説明する。共通する構成については同一の符号を付す。

【0073】

第２実施形態の運転支援システム１は、モデル設定処理が変更された点が第１実施形態と異なる。
第２実施形態のモデル設定処理は、Ｓ４１２，Ｓ４１４，Ｓ４１６の処理が追加された点が第１実施形態と異なる。

【0074】

すなわち、図１４に示すように、Ｓ４１１にて、経過サイクルが第５判定値を超えている場合には、ＣＰＵ１１は、まずＳ４１２にて、対象物標の設定経過サイクルが予め設定された切替サイクル以上であるか否かを判断する。なお、追跡物標毎に設定経過サイクルカウンタが設けられている。そしてＣＰＵ１１は、追跡物標が検出されてから処理サイクルの繰り返し周期が経過する毎に、対応する設定経過サイクルカウンタをインクリメントする。但し、ＣＰＵ１１は、運動モデルが切り替わる毎に、対応する設定経過サイクルカウンタをリセット（すなわち、０に設定）する。設定経過サイクルは、設定経過サイクルカウンタの値であり、現在の運動モデルを設定してから経過した処理サイクルの数に相当する。
ここで、設定経過サイクルが切替サイクル未満である場合には、ＣＰＵ１１は、Ｓ４２０に移行する。

【0075】

一方、設定経過サイクルが切替サイクル以上である場合には、ＣＰＵ１１は、Ｓ４１４にて、対象物標の予測残差が予め設定された切替判定値以上であるか否かを判断する。ここで、予測残差が切替判定値未満である場合には、ＣＰＵ１１は、Ｓ４２０に移行する。一方、予測残差が切替判定値以上である場合には、ＣＰＵ１１は、Ｓ４１６にて、対象物標に対して第１運動モデルが設定されているか否かを判断する。ここで、第１運動モデルが設定されていない場合には、ＣＰＵ１１は、Ｓ５１０に移行する。一方、第１運動モデルが設定されている場合には、ＣＰＵ１１は、Ｓ５２０に移行する。

【0076】

このように構成された追跡装置４は、一つまたは複数の物標における過去の状態ベクトルの推定値Ｘ_ｋ－１に基づいて、一つまたは複数の物標のそれぞれにおける現在の状態ベクトルの予測値Ｘ_{ｋ｜ｋ－１}を算出する。そして追跡装置４は、経過サイクルが切替判定サイクル以上であり、且つ、状態ベクトルの予測値（本実施形態では、物標の位置）と、観測情報が示す観測値（本実施形態では、反射点の位置）との差である予測残差が切替判定値以上である場合に、前回に選択した一つの運動モデルと異なる一つの運動モデルを選択する。これにより、追跡装置４は、物標の追跡精度を向上させることができる。

【0077】

以上、本開示の一実施形態について説明したが、本開示は上記実施形態に限定されるものではなく、種々変形して実施することができる。
［変形例１］
例えば上記実施形態では、第１，２，３，４，５，６，７判定値が固定値である形態を示した。しかし、第１～７判定値は、選択されている一つの運動モデルに応じて変更されるようにしてもよい。すなわち、第１運動モデルが選択されているときの第１～７判定値と、第２運動モデルが選択されているときの第１～７判定値とが互いに異なるようにしてもよい。これにより、追跡装置４は、第１運動モデルと第２運動モデルとを切り替えるための閾値にヒステリシスを設けることができ、運動モデルが第１運動モデルと第２運動モデルとの間で頻繁に切り替わる所謂チャタリングが発生するのを抑制することができる。

【0078】

［変形例２］
上記実施形態では、自車両の前方に向けてレーダ波を送信する形態を示した。しかし、レーダ波を送信する方向は自車両の前方に限定されるものではなく、例えば図１５に示すように、自車両の前方に加えて、自車両の前方における左側および右側と、自車両の後方における左側および右側とに向けてレーダ波を送信するようにしてもよい。

【0079】

［変形例３］
上記実施形態では、物標の対地速度Ｖ_ａｂｓが第１判定値以上である場合に第２運動モデルを選択する形態を示した。しかし、対地速度の代わりに相対速度を用いるようにしてもよい。

【0080】

［変形例４］
上記実施形態では、物標の縦方向位置が第２判定値以上である場合に第２運動モデルを選択する形態を示した。しかし、縦方向位置の代わりに、自車両と物標との間の距離を用いるようにしてもよい。

【0081】

［変形例５］
上記実施形態では、物標の横方向位置が第３判定値未満である場合に第２運動モデルを選択する形態を示した。しかし、横方向位置の代わりに方位を用いるようにしてもよい。

【0082】

［変形例６］
上記実施形態では、複数の運動モデルが第１運動モデルと第２運動モデルとを含む形態を示したが、複数の運動モデルが３つ以上の運動モデルを含むようにしてもよい。また、上記実施形態では、第１運動モデルにおける物標の状態量の横方向速度ｖｘおよび縦方向速度ｖｙが相対速度である形態を示した。しかし、横方向速度ｖｘおよび縦方向速度ｖｙが対地速度であるようにしてもよい。

【0083】

本開示に記載の追跡装置４及びその手法は、コンピュータプログラムにより具体化された一つ乃至は複数の機能を実行するようにプログラムされたプロセッサ及びメモリを構成することによって提供された専用コンピュータにより、実現されてもよい。あるいは、本開示に記載の追跡装置４及びその手法は、一つ以上の専用ハードウェア論理回路によってプロセッサを構成することによって提供された専用コンピュータにより、実現されてもよい。もしくは、本開示に記載の追跡装置４及びその手法は、一つ乃至は複数の機能を実行するようにプログラムされたプロセッサ及びメモリと一つ以上のハードウェア論理回路によって構成されたプロセッサとの組み合わせにより構成された一つ以上の専用コンピュータにより、実現されてもよい。また、コンピュータプログラムは、コンピュータにより実行されるインストラクションとして、コンピュータ読み取り可能な非遷移有形記録媒体に記憶されてもよい。追跡装置４に含まれる各部の機能を実現する手法には、必ずしもソフトウェアが含まれている必要はなく、その全部の機能が、一つあるいは複数のハードウェアを用いて実現されてもよい。

【0084】

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

【0085】

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

【符号の説明】

【0086】

２…レーダ装置、４…追跡装置

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】