説明

追尾装置

【課題】フィルタを簡易に切り替えて早期に目標に追従可能とし、計算量の削減を図る。
【解決手段】等速直線運動モデル、蛇行運動モデル、多重運動モデルに基づくカルマンフィルタを用い、観測値から予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出する各カルマンフィルタ処理手段3〜5と、観測値、カルマンフィルタ処理手段3による各値に基づいて、目標が等速直線運動であるかを判定する等速直線運動判定手段7と、観測値、各カルマンフィルタ処理手段による各値に基づいて、各運動モデルに対する尤度を算出する尤度計算手段6と、各運動モデルに対する尤度および等速直線運動判定手段7による判定結果に基づいて、各運動モデルに対する信頼度を算出する信頼度計算手段8と、各運動モデルに対する信頼度に基づいて、各平滑値の加重平均値を算出する加重平均計算手段9とを備えた。

【発明の詳細な説明】
【技術分野】
【0001】
この発明は、異なる運動モデルに基づく複数のカルマンフィルタを用いて目標の多様な運動に対処する追尾装置に関するものである。
【背景技術】
【0002】
追尾装置による目標追尾においては、等速直線運動、蛇行運動、蛇行以外(旋廻等)のマニューバ運動等、多様な運動に対処する必要がある。そこで、等速直線運動モデルに基づくカルマンフィルタ、蛇行運動モデルに基づくカルマンフィルタ、および多重運動モデルに基づくカルマンフィルタを用いて、フィルタバンクによる並列処理を行い、目標の運動状態を判別して、目標に対する適切な平滑値を選択(フィルタ選択)する方法がある(例えば特許文献1参照)。
【0003】
この特許文献1に開示された追尾装置では、取得した目標に対する観測値に基づいて、等速直線運動カルマンフィルタ処理器で、等速直線運動モデルに基づくカルマンフィルタを用いて、平滑値(第1の平滑値)および予測値(第1の予測値)を算出し、蛇行運動カルマンフィルタ処理器で、蛇行運動モデルに基づくカルマンフィルタを用いて、平滑値(第2の平滑値)および予測値(第2の予測値)を算出している。なお、蛇行運動モデルは、等速直線運動モデルおよび正弦関数運動モデルから構成される。
そして、運動判別処理器で、第1,2の予測値に基づいて目標の運動状態を判別し、収束判定器で、第2の平滑値の収束状況を判定している。なお、運動判別処理器では、第1,2の予測値から残差2次形式をそれぞれ算出するとともに、各残差2次形式から各運動モデルの判定評価関数値を求め、これらの判定評価関数値と所定のしきい値とを比較することによって、目標の運動状態を判別している。
【0004】
そして、フィルタ出力選択器で、運動判別処理器が判別した目標の運動状態、および、収束判定器が判定した収束状況に基づいて、第1の平滑値および第2の平滑値のうちの一方を適切な平滑値として選択する。また、第2の平滑値が収束したと判定した後に、蛇行運動モデルを構成する等速直線運動モデルの平滑速度ベクトルの方向が所定値を超えて変化した場合には、第2の平滑値が未収束であると判定し、多重運動モデルに基づくカルマンフィルタを用いて算出した平滑値を適切な平滑値として選択する。
【先行技術文献】
【特許文献】
【0005】
【特許文献1】特開2005−274300号公報
【発明の概要】
【発明が解決しようとする課題】
【0006】
上述したように、特許文献1で開示された方法では、第1,2の予測値から残差2次形式をそれぞれ算出し、各残差2次形式から各運動モデルの判定評価関数値を求め、判定評価関数値と所定のしきい値とを比較することで、目標の運動状態を判別している。また、蛇行運動モデルに基づくカルマンフィルタを用いて算出した第2の平滑値に対する収束判定を行っている。
【0007】
しかしながら、追尾開始直後に観測雑音が大きい場合、蛇行運動モデルでの平滑誤差が小さくても、この蛇行運動モデルを構成する等速直線運動モデルと正弦関数運動モデルでの平滑値は安定せず、目標運動を等速直線運動成分と正弦関数運動成分とに分離するのに時間を要する。このような場合、等速直線運動モデルの平滑速度ベクトルが安定しないため、この平滑値を用いて目標の未来位置予測を行った際に、未来予測位置が大きく変動して誤差が大きくなってしまう。そのため、特許文献1では、目標が蛇行運動をしている場合であっても、蛇行運動モデルを構成する等速直線運動モデルの平滑速度ベクトルが安定するまでは、等速直線運動モデルに基づくカルマンフィルタを選択している。そのため、誤差が大きくなってしまうという課題がある。
【0008】
また、判定評価関数値と所定のしきい値とを比較することによる目標の運動状態の判別は処理が複雑であるという課題がある。さらに、等速直線運動モデルに基づくカルマンフィルタ、蛇行運動モデルに基づくカルマンフィルタおよび多重運動モデルに基づくカルマンフィルタを用いたフィルタバンク方式は計算負荷が大きいという課題がある。
【0009】
この発明は、上記のような課題を解決するためになされたもので、フィルタを簡易に切り替えることで早期に目標に追従可能であり、また、計算量の削減を図ることができる追尾装置を提供することを目的としている。
【課題を解決するための手段】
【0010】
この発明に係る追尾装置は、等速直線運動モデルに基づくカルマンフィルタを用いて、取得した目標に対する観測値に基づき、予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出する等速直線運動カルマンフィルタ処理手段と、蛇行運動モデルに基づくカルマンフィルタを用いて、観測値に基づき、予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出する蛇行運動カルマンフィルタ処理手段と、多重運動モデルに基づくカルマンフィルタを用いて、観測値に基づき、予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出する多重運動カルマンフィルタ処理手段と、観測値、および、等速直線運動カルマンフィルタ処理手段により算出された予測値、予測誤差共分散行列および観測誤差共分散行列に基づいて、目標の運動状態が等速直線運動であるかを判定する等速直線運動判定手段と、観測値、および、各カルマンフィルタ処理手段によりそれぞれ算出された予測値、予測誤差共分散行列および観測誤差共分散行列に基づいて、各運動モデルに対する尤度を算出する尤度計算手段と、尤度計算手段により算出された各運動モデルに対する尤度および等速直線運動判定手段による判定結果に基づいて、各運動モデルに対する信頼度を算出する信頼度計算手段と、信頼度計算手段により算出された各運動モデルに対する信頼度に基づいて、各カルマンフィルタ処理手段によりそれぞれ算出された平滑値の加重平均値を算出する加重平均計算手段とを備えたものである。
【発明の効果】
【0011】
この発明によれば、上記のように構成したので、フィルタを簡易に切り替えることで早期に目標に追従可能であり、また、計算量の削減を図ることができる。
【図面の簡単な説明】
【0012】
【図1】この発明の実施の形態1に係る追尾装置の構成を示すブロック図である。
【図2】この発明の実施の形態1における等速直線運動判定器の構成を示すブロック図である。
【図3】この発明の実施の形態1に係る追尾装置による動作を示すフローチャートである。
【図4】この発明の実施の形態2に係る追尾装置の構成を示すブロック図である。
【図5】この発明の実施の形態2における蛇行運動カルマンフィルタ処理器の構成を示すブロック図である。
【図6】この発明の実施の形態2に係る追尾装置による動作を示すフローチャートである。
【図7】この発明の実施の形態3に係る追尾装置の構成を示すブロック図である。
【図8】この発明の実施の形態3における等速直線運動判定器の構成を示すブロック図である。
【図9】この発明の実施の形態3における等速直線運動判定を示す図である。
【図10】この発明の実施の形態4に係る追尾装置の構成を示すブロック図である。
【図11】この発明の実施の形態4における蛇行運動カルマンフィルタ処理器の構成を示すブロック図である。
【図12】この発明の実施の形態5に係る追尾装置の構成を示すブロック図である。
【図13】この発明の実施の形態5における重み係数設定器の構成を示すブロック図である。
【図14】この発明の実施の形態5に係る追尾装置による動作を示すフローチャートである。
【図15】この発明の実施の形態5に係る追尾装置において、式(17)の第1項目が示すサンプリング時刻ごとのトラックスコアの積みあがりを示す図である。
【発明を実施するための形態】
【0013】
以下、この発明の実施の形態について図面を参照しながら詳細に説明する。
実施の形態1.
図1はこの発明の実施の形態1に係る追尾装置の構成を示すブロック図である。
追尾装置は、図1に示すように、観測装置1、追尾維持装置2、等速直線運動カルマンフィルタ処理器(等速直線運動カルマンフィルタ処理手段)3、蛇行運動カルマンフィルタ処理器(蛇行運動カルマンフィルタ処理手段)4、多重運動カルマンフィルタ処理器(多重運動カルマンフィルタ処理手段)5、尤度計算器(尤度計算手段)6、等速直線運動判定器(等速直線運動判定手段)7、信頼度計算器(信頼度計算手段)8、加重平均計算器(加重平均計算手段)9および未来位置予測器10から構成されている。
【0014】
観測装置1は、アンテナ、赤外線センサや光学カメラ等を用いて、対象となる目標が放射または反射する信号を検出し、この信号に対して信号処理を行って探知データとして出力するものである。
【0015】
追尾維持装置2は、観測装置1により出力された探知データの中から目標に対する観測値として適したデータをゲート判定処理により抽出し、追尾フィルタ(例えばカルマンフィルタ)による運動諸元の推定および予測を行って、運動諸元予測値(以下、観測値と称す)として出力するものである。
【0016】
等速直線運動カルマンフィルタ処理器3(以下、カルマンフィルタ処理器3と称す)は、等速直線運動モデルに基づくカルマンフィルタを用いて、追尾維持装置2により出力された観測値に基づき、各回サンプリング時刻(観測時刻)での目標に対する予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出するものである。
【0017】
蛇行運動カルマンフィルタ処理器4(以下、カルマンフィルタ処理器4と称す)は、蛇行運動モデルに基づくカルマンフィルタを用いて、追尾維持装置2により出力された観測値に基づき、各回サンプリング時刻での目標に対する予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出するものである。ここで蛇行運動とは、所定の基準直線(主軸成分)の近傍を移動する運動であって、基準直線を所定の周期で横切るような運動をいう。したがって蛇行運動は基準直線を横切る周波数(蛇行周波数)と基準直線の傾きおよび切片とで表現される。特にカルマンフィルタが基づいている蛇行運動モデルは、等速直線運動モデルと正弦関数運動モデルとから構成されているものを仮定する。
【0018】
多重運動カルマンフィルタ処理器5(以下、カルマンフィルタ処理器5と称す)は、蛇行以外(旋廻等)のマニューバ運動に対処する多重運動モデルに基づくカルマンフィルタを用いて、追尾維持装置2により出力された観測値に基づき、各回サンプリング時刻での目標に対する予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出するものである。
【0019】
尤度計算器6は、追尾維持装置2により出力された観測値、および、各カルマンフィルタ処理器3〜5によりそれぞれ算出された目標に対する予測値、予測誤差共分散行列および観測誤差共分散行列に基づいて、各運動モデル(等速直線運動モデル、蛇行運動モデルおよび多重運動モデル)に対する尤度をそれぞれ算出するものである。
【0020】
等速直線運動判定器7は、追尾装置2により出力された観測値、および、カルマンフィルタ処理器3により算出された目標に対する予測値、予測誤差共分散行列および観測誤差共分散行列に基づいて、目標の運動状態が等速直線運動であるかを判定し、その判定結果を信頼度計算器8に出力するものである。この等速直線運動判定器7の構成については後述する。
【0021】
信頼度計算器8は、尤度計算器6により算出された各運動モデルに対する各尤度および等速直線運動判定器7による判定結果に基づいて、各運動モデルに対する信頼度を算出するものである。ここで、信頼度計算器8は、等速直線運動判定器7により目標の運動状態が等速直線運動であると判定された場合には、等速直線運動モデルに対する信頼度を最大値にし、蛇行運動モデルおよび多重運動モデルに対する信頼度を最小値にする。
【0022】
加重平均計算器9は、信頼度計算器8により算出された各運動モデルに対する信頼度に基づいて、各カルマンフィルタ処理器3〜5によりそれぞれ算出された平滑値の加重平均値(統合平滑値)を算出するものである。
未来位置予測器10は、加重平均計算器9により算出された加重平均値に基づいて、外挿法により、Nサンプリング時刻後の目標の未来位置を予測するものである。
【0023】
次に、等速直線運動判定器7の構成について説明する。図2はこの発明の実施の形態1における等速直線運動判定器7の構成を示すブロック図である。
等速直線運動判定器7は、図2に示すように、残差2次形式算出器71、時間平均計算器(時間平均計算手段)72、判定器(判定手段)73およびしきい値設定器74から構成されている。
【0024】
残差2次形式算出器71は、追尾維持装置2により出力された観測値、および、カルマンフィルタ処理器3により算出された目標に対する予測値、予測誤差共分散行列および観測誤差共分散行列に基づいて、残差2次形式を算出するものである。
時間平均計算器72は、残差2次形式算出器71により算出された残差2次形式の所定期間分の平均値を算出するものである。
【0025】
判定器73は、時間平均計算器72により算出された残差2次形式の平均値としきい値設定器74により設定されたしきい値とを比較し、目標の運動状態が等速直線運動であるかを判定するものである。ここで、判定器73は、残差2次形式の平均値がしきい値以下であれば目標の運動状態が等速直線運動であると判定し、そうでなければ等速直線運動ではないと判定して、信頼度計算器8に判定結果を出力する。
しきい値設定器74は、判定器73で残差2次形式の平均値と比較する際に用いるしきい値を設定するものである。なお、しきい値は任意に設定可能である。
【0026】
次に、上記のように構成された追尾装置の一連の動作について説明する。図3はこの発明の実施の形態1に係る追尾装置の動作を示すフローチャートである。なお、観測装置1、追尾維持装置2、各カルマンフィルタ処理器3〜5および未来位置予測器10の動作は従来技術(例えば特許文献1)における動作と同様であるため、その説明を簡略化する。
追尾装置の動作では、図3に示すように、まず、観測装置1は、アンテナ、赤外線センサや光学カメラ等を用いて、対象となる目標が放射又は反射する信号を検出し、この信号に対して信号処理を行って探知データとして出力する(ステップST31)。
【0027】
次いで、追尾維持装置2は、観測装置1により出力された探知データの中から目標に対する観測値として適したデータをゲート判定処理により抽出し、追尾フィルタによる運動諸元の推定および予測を行って観測値として出力する(ステップST32)。
【0028】
次いで、カルマンフィルタ処理器3は、等速直線運動モデルに基づくカルマンフィルタを用いて、追尾維持装置2により出力された観測値に基づき、各回サンプリング時刻での目標に対する予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出する(ステップST33)。
【0029】
また、カルマンフィルタ処理器4は、蛇行運動モデルに基づくカルマンフィルタを用いて、追尾維持装置2により出力された観測値に基づき、各回サンプリング時刻での目標に対する予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出する(ステップST34)。
【0030】
また、カルマンフィルタ処理器5は、多重運動モデルに基づくカルマンフィルタを用いて、追尾維持装置2により出力された観測値に基づき、各回サンプリング時刻での目標に対する予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出する(ステップST35)。
【0031】
次いで、尤度計算器6は、追尾維持装置2により出力された観測値、および、各カルマンフィルタ処理器3〜5によりそれぞれ算出された目標に対する予測値、予測誤差共分散行列および観測誤差共分散行列に基づいて、各運動モデルに対する尤度をそれぞれ算出する(ステップST36)。
【0032】
具体的には、サンプリング時刻をk、各運動モデルをa(=1〜3)、観測値(観測ベクトル)をz、予測値(予測ベクトル)をx(ハット)k,a(−)、予測誤差共分散行列をPk,a(−)、観測誤差共分散行列をRとした場合、尤度計算器6は式(1)を用いて尤度νk,aを算出する。

ここで、g(z;m,A)は、平均m、共分散行列Aの3変量正規分布の観測値zにおける確率密度関数を示している。また、Hは各カルマンフィルタ処理器3〜5の観測行列、Λはレーダの観測座標系からフィルタの座標系への変換行列を示している。
【0033】
次いで、等速直線運動判定器7は、追尾装置2により出力された観測値、および、カルマンフィルタ処理器3により算出された目標に対する予測値、予測誤差共分散行列および観測誤差共分散行列に基づいて、目標の運動状態が等速直線運動であるかを判定する(ステップST37)。
【0034】
具体的には、カルマンフィルタ処理器3により算出された予測値(予測ベクトル)をx(ハット)k,cv(−)、予測誤差共分散行列をPk,cv(−)とした場合、まず、等速直線運動判定器7内の残差2次形式算出器71は式(2)〜(4)を用いて残差2次形式δk,cvを算出する。

ここで、Hcvはカルマンフィルタ処理器3の観測行列を示している。
【0035】
そして、時間平均計算器72は、式(5)を用いて、サンプリング時刻kからT時刻前までの残差2次形式の平均値δ(バー)k,cvを算出することで、過去の残差2次形式の時間変化を観測する。

【0036】
ここで、目標が等速直線運動を行っている場合、等速直線運動モデルに基づく残差2次形式の平均値δ(バー)k,cvは小さくなる。
そこで、判定器73は、時間平均計算器72により算出された残差2次形式の平均値δ(バー)k,cvと、しきい値設定器74により設定された所定のしきい値とを比較する。そして、平均値δ(バー)k,cvがしきい値以下であれば目標の運動状態が等速直線運動であると判定し、平均値δ(バー)k,cvがしきい値よりも大きければ目標の運動状態が等速直線運動ではないと判定し、その判定結果を信頼度計算器8に出力する。
【0037】
次いで、信頼度計算器8は、尤度計算器6により算出された各運動モデルに対する尤度および等速直線運動判定器7による判定結果に基づいて、各運動モデルに対する信頼度を算出する(ステップST38)。
具体的には、信頼度計算器8は式(6)を用いて信頼度μk,aを算出する。

ここで、μk,1は等速直線運動モデルに対する信頼度、μk,2は蛇行運動モデルに対する信頼度、μk,3は多重運動モデルに対する信頼度を示している。
【0038】
なお、信頼度計算器8は、等速直線運動判定器7による判定結果が、目標の運動状態が等速直線運動であることを示している場合には、式(6)を用いて算出した信頼度ではなく式(7)に示す信頼度を用いる。

すなわち、等速直線運動モデルに対する信頼度μk,1を最大値(=1)にし、蛇行運動モデルおよび多重運動モデルに対する信頼度μk,2,μk,3を最小値(=0)にする。
【0039】
次いで、加重平均計算器9は、信頼度計算器8により計算された信頼度に基づいて、各カルマンフィルタ処理器3〜5によりそれぞれ算出された平滑値の加重平均値を算出する(ステップST39)。
具体的には、加重平均計算器9は式(8)を用いて加重平均値x(ハット)(+)を算出する。

【0040】
次いで、未来位置予測器10は、加重平均計算器9により算出された加重平均値に基づいて、外挿法により、Nサンプリング時刻後の目標の未来位置を算出する(ステップST40)。
【0041】
ここで、実施の形態1に係る追尾装置では、従来の追尾装置のような運動状態の判別によるフィルタ選択ではなく、各運動モデルに対する信頼度に基づいて、各運動モデルによる平滑値の加重平均値を算出している。そのため、簡易なフィルタ切り替えが可能となり、早期に目標の運動に対し追従できる。
すなわち、例えば等速直線運動を行っている目標が急に旋廻した場合、等速直線運動モデルに基づくカルマンフィルタから多重運動モデルに基づくカルマンフィルタにフィルタを切り替える必要がある。しかしながら、従来の追尾装置では、運動状態が確定してからフィルタが切り替わるため、旋回直後では等速直線運動に基づくカルマンフィルタを選択してしまい、追尾誤差が大きくなってしまう。それに対して、実施の形態1に係る追尾装置では、各運動モデルに対する信頼度に基づいた各平滑値の加重平均値を算出することで、ある程度追尾できているフィルタの出力結果を使用することができ、追尾誤差の増大を抑えることができる。
【0042】
一方、各運動モデルに対する信頼度を用いる場合、目標が等速直線運動を行っているときには各運動モデル(等速直線運動モデル、蛇行運動モデルおよび多重運動モデル)に対する信頼度に差がなくなり、カルマンフィルタ処理器4およびカルマンフィルタ処理器5により算出された平滑値も加重平均されて、等速直線運動を行っている目標に対して追尾誤差が大きくなってしまう。そこで、実施の形態1に係る追尾装置では、等速直線運動判定器7を設けて、目標の運動状態が等速直線運動であると判定した場合には等速直線運動モデルに対する信頼度を最大値にし、蛇行運動モデルおよび多重運動モデルに対する信頼度を最小値にしている。これにより、等速直線運動を行っている目標に対しても精度よく追尾可能となる。
【0043】
以上のように、この実施の形態1によれば、運動状態の判別によってフィルタ選択を行うのではなく、各運動モデルに対する信頼度に基づいて、各運動モデルによる平滑値の加重平均値を算出するように構成したので、簡易にフィルタを切り替えることができ、早期に目標に追従可能となる。また、目標の運動状態が等速直線運動の場合には、等速直線運動モデルに対する信頼度を最大値にし、蛇行運動モデルおよび多重運動モデルに対する信頼度を最小値にすることで早期に目標に追従可能となるため、多様な運動に対して早期に追従することができる。
【0044】
実施の形態2.
図4はこの発明の実施の形態2に係る追尾装置の構成を示すブロック図であり、図5は蛇行運動カルマンフィルタ処理器4b(以下、カルマンフィルタ処理器4bと称す)の構成を示すブロック図である。
図4に示す実施の形態2に係る追尾装置は、図1に示す実施の形態1に係る追尾装置からカルマンフィルタ処理器3を削除し、カルマンフィルタ処理器4をカルマンフィルタ処理器4bに変更したものである。その他の構成は同様であり、同一の符号を付してその説明を省略する。
【0045】
カルマンフィルタ処理器4bは、実施の形態1におけるカルマンフィルタ処理器4による蛇行運動モデル(等速直線運動モデルおよび正弦関数運動モデル)に基づく目標に対する予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値の算出・出力処理に加えて、これらの各値の等速直線運動成分(主軸成分)も出力する。
このカルマンフィルタ処理器4bは、図5に示すように、複数の周波数別カルマンフィルタ処理器(周波数別カルマンフィルタ処理手段)41−1〜41−N、信頼度計算器42、統合予測器43および統合平滑器44から構成されている。
【0046】
各周波数別カルマンフィルタ処理器41−1〜41−Nは、それぞれ異なる所定の蛇行周波数ω1〜ωNで蛇行する蛇行運動モデル1〜Nに基づくカルマンフィルタを用いて、追尾維持装置2により出力された観測値に基づき、各回サンプリング時刻での目標に対する予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出するものである。この各周波数別カルマンフィルタ処理器41−1〜41−Nはそれぞれ、遅延器45−1〜45−N、混合器46−1〜46−N、予測器47−1〜47−N、平滑器48−1〜48−Nおよび尤度計算器49−1〜49−Nから構成されている。
以下では、周波数別カルマンフィルタ処理器41−1内の各機能部についてのみ説明を行うが、それ以外の周波数別カルマンフィルタ処理器41−2〜41−N内の各機能部についても同様である。
【0047】
遅延器45−1は、平滑器48−1により算出された前回サンプリング時刻でのフィルタゲイン、平滑値および平滑誤差共分散行列を1サンプリング期間保持した後、次回サンプリング時刻にて混合器46−1に出力するものである。
【0048】
混合器46−1は、目標の蛇行運動の蛇行周波数が1次マルコフ過程に基づいて1サンプリング時刻の間に他の蛇行周波数に遷移するとの仮定に従って、各蛇行運動モデル1〜Nに対応した遅延器45−1〜45−Nにより出力された平滑値および平滑誤差共分散行列、および、信頼度計算器42により算出された各蛇行運動モデル1〜Nに対する事後信頼度に基づいて、混合平滑値および混合平滑誤差共分散行列を算出するものである。
【0049】
予測器47−1は、等速直線運動モデルおよび正弦関数運動モデルの統合により表される蛇行運動モデルに従って、混合器46−1により算出された混合平滑値および混合平滑誤差共分散行列に基づいて、予測値および予測誤差共分散行列を算出するものである。
【0050】
平滑器48−1は、追尾維持装置2により出力された観測値、および、予測器47−1により算出された予測値および予測誤差共分散行列に基づいて、フィルタゲイン、平滑値および平滑誤差共分散行列を算出するものである。
【0051】
尤度計算器49−1は、追尾維持装置2により出力された観測値、および、予測器47−1により算出された予測値および予測誤差共分散行列に基づいて、蛇行運動モデル1に対する尤度を算出するものである。
【0052】
また、信頼度計算器42は、各蛇行運動モデル1〜Nに対応した尤度計算器49−1〜49−Nにより算出された尤度および前回サンプリング時刻での事後信頼度に基づいて、各蛇行運動モデル1〜Nに対する事後信頼度を算出するものである。
【0053】
統合予測器43は、各蛇行運動モデル1〜Nに対応した予測器47−1〜47−Nにより算出された予測値および予測誤差共分散行列、および、信頼度計算器42により算出された各蛇行運動モデル1〜Nに対する事後信頼度に基づいて、統合予測値および統合予測誤差共分散行列を算出するものである。この演算結果は、カルマンフィルタ4bにより算出された予測値および予測誤差共分散行列として尤度計算器6に出力され、また、この演算結果の等速直線運動成分(主軸成分)は、尤度計算器6および等速直線運動判定器7に出力される。
【0054】
統合平滑器44は、各蛇行運動モデル1〜Nに対応した平滑器48−1〜48−Nにより算出された平滑値および平滑誤差共分散行列、および、信頼度計算器42により算出された各蛇行運動モデル1〜Nに対する事後信頼度に基づいて、統合平滑値および統合平滑誤差共分散行列を算出するものである。この演算結果は、カルマンフィルタ4bにより算出された平滑値および平滑誤差共分散行列として加重平均計算器9に出力され、また、この演算結果の等速直線運動成分(主軸成分)は、加重平均計算器9に出力される。
【0055】
次に、上記のように構成された追尾装置の一連の動作について説明する。なお、図6に示す実施の形態2に係る追尾装置の動作は、図3に示す実施の形態1に係る追尾装観測装置1のステップST33,34をステップST61,62に変更したものであり、それ以外の動作は同様であるため、このステップST61,62の動作についてのみ説明を行う。また以下では、蛇行運動モデルや観測方程式は従来技術(例えば特許文献1)と同様であり、関係する式のみを用いて説明を行う。
なお、追尾維持装置2により出力されたサンプリング時刻kでの観測値の状態ベクトルxは式(9)のように定義される。

ここで、xはサンプリング時刻kでの状態ベクトル、x1,kは正弦関数運動成分の位置、x1,kは正弦関数運動成分の速度、x2,kは等速直線運動成分の位置、x2,kは等速直線運動成分の速度を示している。
【0056】
ステップST61において、カルマンフィルタ処理器4bは、蛇行運動モデルに基づくカルマンフィルタ処理を行う。具体的には、まず、各周波数別カルマンフィルタ処理器41−1〜41−Nは、それぞれ異なる所定の蛇行周波数ω1〜ωNで蛇行する蛇行運動モデル1〜Nに基づくカルマンフィルタを用いて、追尾維持装置2により出力された観測値に基づいて、各回サンプリング時刻での目標に対する予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出する。そして、信頼度計算器42は、各蛇行運動モデル1〜Nに対応した各尤度計算器49−1〜49−Nにより算出された尤度および前回サンプリング時刻での各蛇行運動モデル1〜Nに対する事後信頼度に基づいて、各蛇行運動モデル1〜Nに対する事後信頼度を算出する。
【0057】
そして、統合予測器43は、各蛇行運動モデル1〜Nに対応した予測器47−1〜47−Nにより算出された予測値および予測誤差共分散行列、および、信頼度計算器42により算出された各蛇行運動モデル1〜Nに対する事後信頼度に基づいて、統合予測値および統合予測誤差共分散行列を算出する。
具体的には、予測値をx(ハット)k,a(−)、予測誤差共分散行列をP(ハット)k,a(−)、事前信頼度をβ(ハット)k,a(−)とした場合、統合予測器43は式(10)および(11)を用いて統合予測ベクトルx(ハット)(−)および統合予測誤差共分散行列P(−)を算出する。

この統合予測器43により算出された統合予測値および統合予測誤差共分散行列は、カルマンフィルタ4bによる予測値および予測誤差共分散行列として尤度計算器6に出力される。
【0058】
そして、統合平滑器44は、各蛇行運動モデル1〜Nに対応した平滑器48−1〜48−Nにより算出された平滑値および平滑誤差共分散行列、および、信頼度計算器42により算出された各蛇行運動モデル1〜Nに対する事後信頼度に基づいて、統合平滑値および統合平滑誤差共分散行列を算出する。
具体的には、平滑値をx(ハット)k,a(+)、平滑誤差共分散行列をP(ハット)k,a(+)とした場合、統合平滑器44は式(12)および(13)を用いて統合平滑ベクトルx(ハット)(+)および統合平滑誤差共分散行列P(+)を算出する。



【0059】
なお、カルマンフィルタ処理器4bの平滑値としては、正弦関数運動モデルおよび等速直線運動モデルの統合平滑値として構成されるため、式(14)を用いて両ベクトルを加算したx(ハット)int,a(+)が実際の目標に対する平滑位置および平滑速度ベクトルとなる。

この統合平滑器44により算出された統合平滑値および統合平滑誤差共分散行列は、カルマンフィルタ4bによる平滑値および平滑誤差共分散行列として加重平均計算器9に出力される。
【0060】
次いで、ステップST62において、カルマンフィルタ処理器4b(統合予測器43および統合平滑器44)は、蛇行運動モデルを構成する等速直線運動モデルに基づくカルマンフィルタ処理の結果を出力する。
ここで、主軸成分である等速直線運動成分の予測ベクトルx(ハット)2_cv,k(−)および平滑ベクトルx(ハット)2_cv,k(+)は式(15)および式(16)のように示される。

【0061】
以上のように、この実施の形態2によれば、カルマンフィルタ処理器4bにて、蛇行運動モデルを構成する等速直線運動モデルに基づくカルマンフィルタ処理の結果をカルマンフィルタ処理器3の代わりに用いるように構成したので、カルマンフィルタ処理器3は不要となり、このカルマンフィルタ処理器3による計算負荷を削減できる。
【0062】
実施の形態3.
図7はこの発明の実施の形態3に係る追尾装置の構成を示すブロック図であり、図8は等速直線運動判定器7bの構成を示すブロック図であり、図9は等速直線運動判定を示す図である。
図7に示す実施の形態3に係る追尾装置は、図4に示す実施の形態2における等速直線運動判定器7を等速直線運動判定器7bに変更し、カルマンフィルタ処理器4bからの蛇行運動モデルの出力と、その等速直線運動成分(主軸成分)の出力とが等速直線運動判定器7bへ入力するように変更したものである。その他の構成は同様であり、同一の符号を付してその説明を省略する。
【0063】
ここで、図9(a)に示すように、目標の運動状態が蛇行運動の場合では、蛇行運動モデルの平滑値(等速直線運動成分(主軸成分)と正弦関数運動成分とを統合した統合平滑値)と、蛇行運動モデルの等速直線運動成分の平滑値との差は大きい。一方、図9(b)に示すように、目標の運動状態が等速直線運動の場合では、その差は小さい。
そこで、等速直線運動判定器7bによる等速直線運動判定では、上記特性を利用して、上記差分値が所定のしきい値以下であれば目標の運動状態が等速直線運動であると判定し、そうでなければ等速直線運動ではないと判定して、信頼度計算器8に判定結果を出力する。
【0064】
図8に示す等速直線運動判定器7bは、図2に示す実施の形態2における等速直線運動判定器7の残差2次形式算出器71を差分計算器(差分計算手段)71bに変更し、時間平均計算器72を削除したものである。
【0065】
差分計算器71bは、カルマンフィルタ処理器4bにより算出された目標に対する平滑値と、この平滑値の等速直線運動成分(主軸成分)との差分値を算出するものである。
【0066】
また、判定器73は、差分計算器71bにより算出された差分値としきい値設定器74により設定されたしきい値とを比較し、目標の運動状態が等速直線運動であるかを判定する。ここで、判定器73は、差分値がしきい値以下であれば目標の運動状態が等速直線運動であると判定し、そうでなければ等速直線運動ではないと判定して、信頼度計算器8に判定結果を出力する。
また、しきい値設定器74は、判定器73で差分値と比較する際に用いるしきい値を設定する。なお、しきい値は任意に設定可能である。
【0067】
以上のように、この実施の形態3によれば、等速直線運動判定器7bにて、カルマンフィルタ処理器4bからの蛇行運動モデルの平滑値とその等速直線成分(主軸成分)との差分値がしきい値以下の場合に、目標の運動状態が等速直線運動であると見なすように構成したので、運動状態が等速直線運動である目標に対して、より精度のよい追尾が可能となる。
【0068】
実施の形態4.
図10はこの発明の実施の形態4に係る追尾装置の構成を示すブロック図であり、図11は蛇行運動カルマンフィルタ処理器4c(以下、カルマンフィルタ処理器4cと称す)の構成を示すブロック図である。
図10に示す実施の形態4に係る追尾装置は、図4に示す実施の形態2に係る追尾装置のカルマンフィルタ処理器4bをカルマンフィルタ処理器4cに変更したものである。その他の構成は同様であり、同一の符号を付してその説明を省略する。
【0069】
実施の形態2におけるカルマンフィルタ処理器4bでは、等速直線運動成分(主軸成分)を統合予測器43および統合平滑器44から出力している。この場合、等速直線運動モデルおよび正弦関数運動モデルから構成される蛇行運動モデルに基づいて処理を行うことになる。ここで、追尾開始直後に観測雑音が大きい場合、両モデルを合わせた統合平滑値に係る平滑誤差は小さくても、モデルごとの平滑値は安定しない。そのため、追尾初期において、等速直線運動成分を取り出した際に追尾誤差が大きくなってしまう。
【0070】
そこで、図10,11に示すように、実施の形態4におけるカルマンフィルタ処理器4cでは、蛇行運動モデルとして用意されている複数の蛇行周波数ω1〜ωNの中で最も高い周波数(図11では、周波数別カルマンフィルタ処理器41−Nの蛇行周波数)の等速直線運動成分(主軸成分)のみを取り出す。すなわち、高周波の蛇行周波数の振幅は低周波の蛇行周波数の振幅よりも小さいため、観測雑音に埋もれると、統合平滑としてはほとんど主軸成分しか残らないことを利用する。
【0071】
このカルマンフィルタ処理器4cでは、蛇行周波数ω1〜ωNで蛇行する蛇行運動モデル1〜Nに基づいて、蛇行運動モデル1〜Nごとの予測値および予測誤差共分散行列を算出し、各蛇行運動モデル1〜Nに対する事後信頼度に基づいて、統合予測値および統合予測誤差共分散行列を算出する。この統合予測値および統合予測誤差共分散行列は、カルマンフィルタ4cによる予測値および予測誤差共分散行列として尤度計算器6に出力される。同様に、蛇行運動モデル1〜Nごとの平滑値、平滑誤差共分散行列および事後信頼度に基づいて、統合平滑値および統合平滑誤差共分散行列を算出する。この統合平滑値および統合平滑誤差共分散行列は、カルマンフィルタ4cによる平滑値および平滑誤差共分散行列として加重平均計算器9に出力される。
さらに、カルマンフィルタ処理器4cは、蛇行運動モデル1〜Nが蛇行周波数ごとのモデルであるとした場合、最も高い蛇行周波数である蛇行運動モデルの等速直線運動成分(主軸成分)を取り出す。この取り出された予測値および予測誤差共分散行列の等速直線運動成分は、尤度計算器6および等速直線運動判定器7に出力され、平滑値および統合平滑誤差共分散行列の等速直線運動成分は、加重平均計算器9に出力される。
【0072】
以上のように、この実施の形態4によれば、カルマンフィルタ処理器4cにて、最も高い蛇行周波数である蛇行運動モデルの等速直線運動成分をカルマンフィルタ処理器3の代わりに用いるように構成したので、追尾初期において等速直線運動成分が十分に安定しない場合であっても、高周波である蛇行運動モデルの等速直線運動成分を取り出すことで、運動状態が等速直線運動である目標に対して、より精度のよい追尾が可能となる。
【0073】
実施の形態5.
実施の形態1〜4では、各運動モデルに対する信頼度に基づいて各平滑値の加重平均を算出するものについて示したが、実施の形態5では、非特許文献1で用いられているトラックスコア(track score)なる指標値を用いて、各サンプリング時刻での各運動状態(等速直線運動状態、蛇行運動状態または多重運動状態)に対する評価(重み付け)を行い、これに基づいて各平滑値の加重平均を算出するものについて示す。
[非特許文献1]
J.Arnold、S.Shaw,H. Pasternack “Efficient target tracking using dynamic programming”,IEEE Trans.Aerospace and Electronic System,Vol.29,No.1,pp.4566(1993)
【0074】
図12はこの発明の実施の形態5に係る追尾装置の構成を示すブロック図であり、図13は重み係数設定器11の構成を示すブロック図である。
図12に示す実施の形態5に係る追尾装置は、図4に示す実施の形態2に係る追尾装置から等速直線運動判定器7を削除し、尤度計算器6および信頼度計算器8を重み係数設定器(重み係数設定手段)11に変更したものである。その他の構成は同様であり、同一の符号を付してその説明を省略する。なお、実施の形態5に係る追尾装置は、図1に示す実施の形態1の追尾装置に対しても同様に適応可能である。
【0075】
重み係数設定器11は、追尾維持装置2により出力された観測値、カルマンフィルタ処理器4b,5により算出された予測値、予測誤差共分散行列および観測誤差共分散行列、および、カルマンフィルタ処理器4bにより算出された各値の等速直線運動成分(図1に示す追尾装置に実施の形態5の追尾装置の構成を適用した場合にはカルマンフィルタ処理器3により算出された各値)に基づいて、各運動モデルに対する重み係数を算出するものである。この重み係数設定器11は、図13に示すように、尤度計算器111、推移確率設定器(推移確率設定手段)112、トラックスコア算出器(トラックスコア算出手段)113、重み係数算出器(重み係数算出手段)114、残差2次形式算出器(残差2次形式算出手段)115、推移確率補正器(推移確率補正手段)116および遅延器117から構成されている。
【0076】
尤度計算器111は、追尾維持装置2により出力された観測値、カルマンフィルタ処理器4b,5により算出された予測値、予測誤差共分散行列および観測誤差共分散行列、および、カルマンフィルタ処理器4bにより算出された各値の等速直線運動成分に基づいて、今回サンプリング時刻での各運動モデルに対する尤度をそれぞれ計算するものである。
【0077】
推移確率設定器112は、目標が前回サンプリング時刻において所定の運動状態(各推移元運動状態)であった場合に、この推移元運動状態から今回サンプリング時刻において各運動状態へ推移する推移確率を設定するものである。なおこの際、推移確率設定器112は、遅延器117により出力された補正値に基づいて、推移確率を補正する。
【0078】
トラックスコア算出器113は、尤度計算器111により算出された各運動モデルに対する尤度および推移確率設定器112により設定された推移確率に基づいて、今回サンプリング時刻での各運動状態に対する評価値(トラックスコア)を算出するものである。このトラックスコア算出器113では、まず、今回サンプリング時刻での各運動状態に対して、前回サンプリング時刻での運動状態に対するトラックスコアと、推移確率設定器112により設定された前回サンプリング時刻での運動状態から今回サンプリング時刻での運動状態への推移確率との加算値が最大となる前回サンプリング時刻での運動状態(推移元運動状態)を選択する。そして、この推移元運動状態に対するトラックスコア、推移元運動状態からの推移確率および尤度計算器111により算出された各運動モデルに対する尤度に基づいて、今回サンプリング時刻での各運動状態に対するトラックスコアを算出する。
【0079】
重み係数算出器114は、トラックスコア算出器113により算出された各運動状態に対するトラックスコアに基づいて、各運動モデルに対する重み係数を算出するものである。この重み係数算出器114により算出された重み係数は信頼度として加重平均計算器9に出力される。
【0080】
残差2次形式算出器115は、追尾維持装置2により出力された観測値、カルマンフィルタ処理器4b,5により算出された予測値、予測誤差共分散行列および観測誤差共分散行列、および、カルマンフィルタ処理器4bにより算出された各値の等速直線運動成分に基づいて、各運動モデルに対する残差2次形式をそれぞれ算出するものである。
【0081】
推移確率補正器116は、残差2次形式算出器115により算出された各運動モデルに対する残差2次形式に基づいて、推移確率に対する補正値を算出するものである。
遅延器117は、推移確率補正器116により算出された前回サンプリング時刻での補正値を1サンプリング期間保持して、今回サンプリング時刻にて推移確率設定器112に出力するものである。
【0082】
次に、上記のように構成された追尾装置の一連の動作について説明する。なお、図14に示す実施の形態5に係る追尾装置の動作は、図6に示す実施の形態2に係る追尾装観測装置1のステップST36〜38をステップST111に変更したものであり、それ以外の動作は同様であるため、このステップST111についてのみ説明を行う。
【0083】
ステップST111において、重み係数設定器11は、追尾維持装置2により出力された観測値、カルマンフィルタ処理器4b,5により算出された予測値、予測誤差共分散行列および観測誤差共分散行列、および、カルマンフィルタ処理器4bにより算出された各値の等速直線運動成分に基づいて、各運動モデルに対する重み係数を算出する。
【0084】
具体的には、まず、観測値をz、予測値をx(ハット)(−)、予測誤差共分散行列をP(−)、観測誤差共分散行列をRとした場合、重み係数設定器11内の尤度計算器111は、式(1)を用いて各運動モデルに対する尤度をそれぞれ算出する。
【0085】
そして、トラックスコア算出器113は、各運動モデルに基づくカルマンフィルタごとに、式(17)を用いて、今回サンプリング時刻kでの各運動状態に対するトラックスコアを算出する。


ここで、Xk−1,bは前回サンプリング時刻k−1での運動状態に対するトラックスコアであり、P(θk,a|θk,b)は前回サンプリング時刻k−1での運動状態から今回サンプリング時刻kでの運動状態への推移確率、νk,aは各運動モデルに対する尤度である。
【0086】
式(17)に示すトラックスコアの算出式は、従来技術(非特許文献1)と基本的な動作は同じであり、前回サンプリング時刻k−1での推移元となる各運動状態の中から、今回サンプリング時刻kでの運動状態に対するトラックスコアを最大にする運動状態を選択し、これを推移元運動状態とする。そして、推移元運動状態に対するトラックスコアと、推移元運動状態から今回サンプリング時刻kでの運動状態への推移確率と、今回サンプリング時刻kでの運動モデルに対する尤度とから今回サンプリング時刻kでの運動状態に対するトラックスコアを求める。
【0087】
図15は式(17)の第1項目が示すサンプリングごとのトラックスコアの積みあがりを示したものである。
図15に示すように、サンプリング時刻0の場合には、各運動状態に対するトラックスコアはすべて0である。また、サンプリング時刻0からサンプリング時刻1となるとき、等速直線運動状態から等速直線運動状態への推移確率は3、等速直線運動状態から蛇行運動状態への推移確率は1、等速直線運動状態から多重運動状態への推移確率は0である。なおこの推移確率は推移確率設定器112により設定される。
そして、例えば、サンプリング時刻1での等速直線運動状態に対するトラックスコアを最大にするサンプリング時刻0での運動状態は、推移確率が最も高い等速直線運動状態となる。そして、サンプリング時刻1での等速直線運動状態に対するトラックスコアは、サンプリング時刻0での等速直線運動状態に対するトラックスコアである0と、サンプリング時刻0での等速直線運動状態からサンプリング時刻1での等速直線運動状態への推移確率である3との加算値である3となる。
同様にして、サンプリング時刻1での蛇行運動状態および多重運動状態に対するトラックスコアも算出される。
【0088】
また、例えば、サンプリング時刻2での蛇行運動状態に対してでは、推移元運動状態としてサンプリング時刻1の等速直線運動状態を選択する。そして、サンプリング時刻2での蛇行運動状態に対するトラックスコアは、サンプリング時刻1での等速直線運動状態に対するトラックスコアである3と、サンプリング時刻1での等速直線運動状態からサンプリング時刻2での蛇行運動状態への推移確率である2との加算値である5となる。
同様にして、サンプリング時刻2での等速直線運動状態および多重運動状態に対するトラックスコアも算出される。
なお図15は、式(17)の第1項目の推移確率を考慮したトラックスコアの積みあがりを示したものであるので、実際には第1項目の値と第2項目の尤度との加算値が各運動状態に対するトラックスコアとなる。
【0089】
そして、重み係数算出器114は、トラックスコア算出器113により算出された各運動状態に対するトラックスコアに基づいて、式(18)を用いて各運動モデルに対する重み係数を算出する。

この重み係数算出器114により算出された重み係数は信頼度として加重平均計算器9に出力される。
【0090】
一方、残差2次形式算出器115は、追尾維持装置2により出力された観測値、カルマンフィルタ処理器4b,5により算出された予測値、予測誤差共分散行列および観測誤差共分散行列、および、カルマンフィルタ処理器4bにより算出された各値の等速直線運動成分に基づいて、各運動モデルに対する残差2次形式をそれぞれ算出する。そして、推移確率補正器116は、残差2次形式算出器115により算出された各運動モデルに対する残差2次形式に基づいて、推移確率に対する補正値を算出し、遅延器117で1サンプリング期間保持された後、推移確率設定器112に出力される。
【0091】
例えば、目標が等速直線運動を継続して行っている場合には、等速直線運動状態に対するトラックスコアはサンプリング時刻ごとに積みあがっていき、等速直線運動モデルに対する重み係数が大きくなっていく。その後、目標が急に旋回した場合、等速直線運動状態に対するトラックスコアは大きく積みあがっているため、多重運動モデルに対する重み係数と比べて等速直線運動モデルに対する重み係数が大きくなってしまう。
一方、多重運動状態に対するトラックスコアが大きくならない場合、等速直線運動モデルに基づくカルマンフィルタの残差2次形式は多重運動モデルに基づくカルマンフィルタの残差2次形式と比べると値が非常に大きくなる。そこで、それぞれのカルマンフィルタの残差2次形式の値の差を利用し、多重運動状態のトラックスコアが大きく積み上がるように推移確率に対する補正を行う。これによって、多重運動モデルに対する重み係数を大きくすることができ、目標が急旋回する場合であっても追従できることが可能となる。
【0092】
以上のように、この実施の形態5によれば、運動状態の判別ではなく、トラックスコアを用いて、前回サンプリング時刻での推移元の運動状態の中から今回サンプリング時刻での運動状態に対するトラックスコアを最大にする運動状態を選択し、この推移元運動状態に対するトラックスコア、推移元運動状態からの推移確率および今回サンプリング時刻での運動モデルに対する尤度に基づいて、今回サンプリング時刻での運動状態に対するトラックスコアを算出し、各運動モデルに対する重み係数を算出するように構成したので、簡易にフィルタを切り替えることができ、早期に目標に追従可能となる。また、目標の運動状態が等速直線運動であり、各運動モデルに対する信頼度に差がなくなる場合であっても、等速直線運動状態に対するトラックスコアの積みあがりによって等速直線モデルに対する重み係数を大きくすることができる。そのため、早期に目標に追従することができる。さらに、目標が等速直線運動から急に旋廻した場合であっても、推移確率を補正することで多重運動状態に対するトラックスコアを積み上げることができ、多重運動モデルに対する重み係数を大きくすることができ、多様な運動に対しても早期に追従することができる。
【0093】
なお、本願発明はその発明の範囲内において、各実施の形態の自由な組み合わせ、あるいは各実施の形態の任意の構成要素の変形、もしくは各実施の形態において任意の構成要素の省略が可能である。
【符号の説明】
【0094】
1 観測装置、2 追尾維持装置、3 等速直線運動カルマンフィルタ処理器(等速直線運動カルマンフィルタ処理手段)、4,4b,4c 蛇行運動カルマンフィルタ処理器(蛇行運動カルマンフィルタ処理手段)、5 多重運動カルマンフィルタ処理器(多重運動カルマンフィルタ処理手段)、6 尤度計算器(尤度計算手段)、7,7b 等速直線運動判定器(等速直線運動判定手段)、8 信頼度計算器(信頼度計算手段)、9 加重平均計算器(加重平均計算手段)、10 未来位置予測器、11 重み係数設定器、41−1〜41−N 周波数別カルマンフィルタ処理器(周波数別カルマンフィルタ処理手段)、42 信頼度計算器、43 統合予測器、44 統合平滑器、45−1〜45−N 遅延器、46−1〜46−N 混合器、47−1〜47−N 予測器、48−1〜48−N 平滑器、49−1〜49−N 尤度計算器、71 残差2次形式算出器(残差2次形式算出手段)、71b 差分計算器(差分計算手段)、72 時間平均計算器(時間平均計算手段)、73 判定器(判定手段)、74 しきい値設定器、111 尤度計算器、112 推移確率設定器(推移確率設定手段)、113 トラックスコア算出器(トラックスコア算出手段)、114 重み係数算出器(重み係数算出手段)、115 残差2次形式算出器(残差2次形式算出手段)、116 推移確率補正器(推移確率補正手段)、117 遅延器。

【特許請求の範囲】
【請求項1】
等速直線運動モデルに基づくカルマンフィルタを用いて、取得した目標に対する観測値に基づき、予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出する等速直線運動カルマンフィルタ処理手段と、
蛇行運動モデルに基づくカルマンフィルタを用いて、前記観測値に基づき、予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出する蛇行運動カルマンフィルタ処理手段と、
多重運動モデルに基づくカルマンフィルタを用いて、前記観測値に基づき、予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出する多重運動カルマンフィルタ処理手段と、
前記観測値、および、前記等速直線運動カルマンフィルタ処理手段により算出された予測値、予測誤差共分散行列および観測誤差共分散行列に基づいて、前記目標の運動状態が等速直線運動であるかを判定する等速直線運動判定手段と、
前記観測値、および、前記各カルマンフィルタ処理手段によりそれぞれ算出された予測値、予測誤差共分散行列および観測誤差共分散行列に基づいて、各運動モデルに対する尤度を算出する尤度計算手段と、
前記尤度計算手段により算出された各運動モデルに対する尤度および前記等速直線運動判定手段による判定結果に基づいて、各運動モデルに対する信頼度を算出する信頼度計算手段と、
前記信頼度計算手段により算出された各運動モデルに対する信頼度に基づいて、前記各カルマンフィルタ処理手段によりそれぞれ算出された平滑値の加重平均値を算出する加重平均計算手段と
を備えた追尾装置。
【請求項2】
前記等速直線運動判定手段は、
前記観測値、および、前記等速直線運動カルマンフィルタ処理手段により算出された予測値、予測誤差共分散行列および観測誤差共分散行列に基づいて、残差2次形式を算出する残差2次形式算出手段と、
前記残差2次形式算出手段により算出された残差2次形式の所定観測分の平均値を算出する時間平均計算手段と、
前記時間平均計算手段により算出された平均値が所定のしきい値以下の場合に、前記目標の運動状態が等速直線運動であると判定する判定手段とを有し、
前記信頼度計算手段は、前記等速直線運動判定手段により前記目標の運動状態が等速直線運動であると判定された場合に、前記等速直線運動モデルに対する信頼度を最大値にし、前記蛇行運動モデルおよび前記多重運動モデルに対する信頼度を最小値にする
ことを特徴とする請求項1記載の追尾装置。
【請求項3】
前記等速直線運動カルマンフィルタ処理手段に代えて、
前記蛇行運動カルマンフィルタ処理手段により算出された予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値のそれぞれの等速直線運動成分を用いる
ことを特徴とする請求項1または請求項2記載の追尾装置。
【請求項4】
前記等速直線運動判定手段は、
前記蛇行運動カルマンフィルタ手段により算出された平滑値および当該平滑値の等速直線成分の差分値を算出する差分計算手段と、
前記差分計算手段により算出された差分値が所定のしきい値以下の場合に、前記目標の運動状態が等速直線運動であると判定する判定手段とを有し、
前記信頼度計算手段は、前記等速直線運動判定手段により前記目標の運動状態が等速直線運動であると判定された場合に、前記等速直線運動モデルに対する信頼度を最大値にし、前記蛇行運動モデルおよび前記多重運動モデルに対する信頼度を最小値にする
ことを特徴とする請求項1または請求項3記載の追尾装置。
【請求項5】
前記蛇行運動カルマンフィルタ処理手段は、それぞれ異なる所定の蛇行周波数で蛇行する運動モデルに基づくカルマンフィルタを用いて、前記観測値に基づき、予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値を算出する周波数別カルマンフィルタ処理手段を複数備え、
前記等速直線運動カルマンフィルタ処理手段に代えて、
前記各周波数別カルマンフィルタ処理手段のうち、最も高い蛇行周波数に対応した周波数別カルマンフィルタ処理手段により算出された予測値、予測誤差共分散行列、観測誤差共分散行列および平滑値のそれぞれの等速直線運動成分を用いる
ことを特徴とする請求項1から請求項4のうちのいずれか1項記載の追尾装置。
【請求項6】
等速直線運動判定手段および信頼度計算手段に代えて、
前記目標が前回観測時刻において各運動状態であった場合に、当該各運動状態から今回観測時刻において各運動状態へ推移する推移確率を設定する推移確率設定手段と、
今回観測時刻での各運動状態に対して、前回観測時刻での運動状態に対する評価値であるトラックスコアと、前記推移確率設定手段により設定された当該前回観測時刻での運動状態から今回観測時刻での運動状態への推移確率との加算値が最大となる前回観測時刻での運動状態を選択し、当該選択した運動状態に対するトラックスコア、当該選択した運動状態からの推移確率および前記各運動モデルに対する尤度に基づいて、当該今回観測時刻での各運動状態に対するトラックスコアを算出するトラックスコア算出手段と、
前記トラックスコア算出手段により算出された各運動状態に対するトラックスコアに基づいて各運動モデルに対する重み係数を算出し、各運動モデルに対する信頼度として前記加重平均計算手段に出力する重み係数算出手段と
を有する重み係数設定手段を備えた
ことを特徴とする請求項1から請求項5のうちのいずれか1項記載の追尾装置。
【請求項7】
前記重み係数設定手段は、
前記観測値、および、前記各カルマンフィルタ処理手段によりそれぞれ算出された予測値、予測誤差共分散行列および観測誤差共分散行列に基づいて、残差2次形式を算出する残差2次形式算出手段と、
前記残差2次形式算出手段により算出された残差2次形式に基づいて、推移確率に対する補正値を算出する推移確率補正手段とを有し、
前記推移確率設定手段は、前記推移確率補正手段により算出された補正値に基づいて、前記推移確率を補正する
ことを特徴とする請求項6記載の追尾装置。

【図1】
image rotate

【図2】
image rotate

【図3】
image rotate

【図4】
image rotate

【図5】
image rotate

【図6】
image rotate

【図7】
image rotate

【図8】
image rotate

【図9】
image rotate

【図10】
image rotate

【図11】
image rotate

【図12】
image rotate

【図13】
image rotate

【図14】
image rotate

【図15】
image rotate