JP2008298738A - 目標追尾装置 - Google Patents

Abstract

【課題】直感的に設計可能な目標の想定加速度に基づく追尾フィルタを、カルマンフィルタに代表される既存の追尾フィルタに対しても実装可能とし、追尾性能の最適化を実現する目標追尾装置を得る。
【解決手段】目標の観測値、目標の観測精度、および目標の運動モデルにおけるシステム雑音に基づいて、目標の推定値、推定値と観測値との差である残差、および残差推定値を出力する追尾フィルタ部２０を備えた目標追尾装置において、目標の想定加速度、目標の観測精度、およびサンプリング周期を入力とし、目標の運動モデルにおける最適なシステム雑音を算出する最適システム雑音設定部１０をさらに備え、追尾フィルタ部２０は、最適システム雑音設定部１０により算出された最適なシステム雑音に基づいて目標の推定値を求める。
【選択図】図１

Description

本発明は、主としてセンサによる探知データを入力とした目標追尾装置に関するものであり、特に、目標（航空機、船舶、車両など）の運動に応じて追尾フィルタの最適設計を行う目標追尾装置に関する。

目標観測値を用いて目標を追尾する目標追尾装置では、追尾フィルタとしてカルマンフィルタを用いるものが一般的である。所望の追尾精度を確保するためには、システム条件（特に、目標の運動）に応じた追尾フィルタ設計が不可欠となる。しかし、カルマンフィルタでは、運動モデルにおけるシステム雑音の設計手法が確立されていない。

カルマンフィルタを使用した追尾処理では、目標の運動モデルを下式（１）のように仮定する。ここでは、サンプリング時刻（以下、時刻）をｔ_ｋで表す。

上式（１）におけるｘ_ｋ、φ、ｗ_ｋ、Γは、それぞれ以下のものを示している。
ｘ_ｋ：時刻ｔ_ｋにおける目標運動諸元の真値を表す状態変数ベクトル
φ：時刻ｔ_ｋ−１から時刻ｔ_ｋへの状態推移行列
ｗ_ｋ：時刻ｔ_ｋにおけるシステム雑音ベクトル
Γ：システム雑音の変換行列

従来の一般的な追尾装置では、目標の基本的な運動を等速直線運動と仮定して、上式（１）のような運動モデルを記述する。たとえば、ｘ、ｙ、ｚの３次元直交座標系における目標の位置と速度を推定する場合、状態ベクトルｘ_ｋ、状態推移行列φ、システム雑音の変換行列Γを、それぞれ下式（２）〜（４）のように定義する。

ここで、Ｔはベクトル、行列の転置を表し、ドットは時間微分を表す。また、Ｉ_ｎ×ｎは、ｎ行ｎ列の単位行列であり、Ｏ_ｎ×ｎは、ｎ行ｎ列の零行列であり、τは、一定のサンプリング周期を表す。

さらに、ｗ_ｋは、現実の目標運動を等速直線で近似したことによるｘ_ｋの誤差を表現するために設けられたｘ、ｙ、ｚの各方向の加速度項の雑音成分で構成される。ここで、ｗ_ｋは、平均零ベクトルであり、共分散行列Ｑ_ｋの３変量正規分布に従う白色雑音であると仮定する。次に、観測装置の観測モデルを、下式（５）のように仮定する。

上式（５）におけるｚ_ｋ、Ｈ、ｖ_ｋは、それぞれ以下のものを示している。
ｚ_ｋ：時刻ｔ_ｋにおける観測値ベクトル
Ｈ：観測行列
ｖ_ｋ：時刻ｔ_ｋにおける観測雑音ベクトル

たとえば、観測装置によりｘ、ｙ、ｚの３次元直交座標系における目標位置の観測結果が得られるとする。このとき、観測値ベクトルｚ_ｋ、観測行列Ｈを、下式（６）、（７）のように定義する。

また、ｖ_ｋは、観測装置の観測誤差を表すための雑音成分であり、平均零ベクトルであり、共分散行列Ｒ_ｋの３変量正規分布に従う白色雑音であると仮定する。

上記のような運動モデル、観測モデルの仮定のもと、カルマンフィルタは、下式（８）〜（１３）に示すアルゴリズムで、状態ベクトルの推定を行う。なお、以下では、時刻ｔ_ｋ−１までの観測結果に基づいて時刻ｔ_ｋの状態ベクトルを推定した予測値ベクトルをｘ（＾）_{ｋ｜ｋ−１}と表し、時刻ｔ_ｋまでの観測結果に基づいて時刻ｔ_ｋの状態ベクトルを推定した平滑値ベクトルをｘ（＾）_ｋ｜ｋと表す（ここで、ｘ（＾）という記述は、ｘの上に＾が付いたことを意味するものとする）。

また、ｘ（＾）_{ｋ｜ｋ−１}の誤差共分散行列を表す予測誤差共分散行列をＰ_{ｋ|ｋ−１}と表し、ｘ（＾）_ｋ｜ｋの誤差共分散行列を表す平滑誤差共分散行列をＰ_ｋ|ｋと表す。さらに、Ｋ_ｋは、フィルタのゲイン行列であり、上式（２）〜（４）、（６）、（７）の設定を行った場合、６行３列の行列である。さらに、Ｉは、６行６列の単位行列である。

なお、ｘ（＾）_ｋ｜ｋの初期値ｘ（＾）_０｜０と、Ｐ_ｋ|ｋの初期値Ｐ_０|０が別途算出され、与えられるものとする。また、システム雑音共分散行列Ｑ_ｋ、および観測雑音共分散行列Ｒ_ｋは、パラメータとして与える。

カルマンフィルタは、上式（８）、（１０）、（１２）により、平均２乗誤差を最小化する最適なフィルタとして知られている。また、一方で、最適ＲＳＥ（Ｒｅｄｕｃｅｄ Ｓｔａｔｅ Ｅｓｔｉｍａｔｏｒ）という追尾フィルタがある（例えば、非特許文献１参照）。最適ＲＳＥを使用した追尾処理では、目標の運動モデルを、下式（１４）のように仮定する。

状態ベクトルｘ_ｋ、状態推移行列φ、変換行列Γは、上式（２）〜（４）と同様である。ここで、λは、平均０（零ベクトル）の目標運動の曖昧さを示す時不変加速度ベクトルで、その共分散行列をΛとする。また、観測モデルは、カルマンフィルタと同様である（上式（５）〜（７）参照）。

上記のような運動モデル、観測モデルの仮定のもと、最適ＲＳＥは、下式（１５）〜（２４）に示すアルゴリズムで、状態ベクトルの推定を行う。

なお、ｘ（＾）_ｋ｜ｋの初期値ｘ（＾）_０｜０と、Ｍ_ｋ|ｋの初期値Ｍ_０|０と、Ｄ_ｋ|ｋの初期値Ｄ_０|０が別途算出され、与えられるものとする。また、加速度ベクトルの共分散行列Λ、および観測雑音共分散行列Ｒ_ｋは、パラメータとして与える。

Mookerjee, P., and Reifler, F. Reduced state estimator for systems with parametric inputs.IEEE Transactions on Aerospace and Electronic Systems, 40, 2 （Apr. 2004）, 446-461.

上述したように、カルマンフィルタは、目標の運動モデルおよび観測装置の観測モデルが正しく表現されていれば、最適な推定性能を与えることができる。ここで、カルマンフィルタを追尾処理に適用する場合、上式（１）が示すように、目標の等速直線運動からのずれを表現するための加速度項ｗ_ｋを、白色雑音と仮定する必要がある。

しかしながら、航空機、船舶、車両などの現実の目標が加速度運動を行う場合、直進しながら加速または減速したり、あるいは半径がほぼ一定の円弧を描きながら旋回したりするのが通常である。

このような場合、目標の加速度ベクトルは、一定値あるいは緩やかに時間変化する値であり、白色雑音としてモデル化することには無理がある。この結果、従来の追尾装置では、所望の追尾精度を得るために必要なシステム雑音共分散行列Ｑ_ｋの値をパラメータとして設定することが困難であった。

つまり、従来の追尾装置では、追尾すべき目標の運動特性（目標の加速度など）に関する知識を基に、パラメータＱ_ｋの値を的確に設定することができないため、結果的に最適性能を実現できないという問題があった。

一方、最適ＲＳＥでは、目標の等速直線運動からのずれを時不変加速度ベクトルλで表現している。これにより、目標の加速度を加速度ベクトル共分散行列Λの標準偏差として設計すれば、最適性能を得ることができる。つまり、追尾すべき目標の運動特性の知識を基に、直感的に設計することができるため、結果的に最適性能を実現できる。

しかし、カルマンフィルタが実装されているような従来の追尾装置において、カルマンフィルタを最適ＲＳＥに置き換えるためには、新規開発の必要がある、また、最適ＲＳＥを適用する場合には、カルマンフィルタよりも、上式（１６）、（１７）、（２２）、（２３）の行列演算が増えるため、演算負荷も重くなるといった問題がある。

本発明は、このような課題を解決するためになされたものであり、直感的に設計可能な目標の想定加速度に基づく追尾フィルタを、カルマンフィルタに代表される既存の追尾フィルタに対しても実装可能とし、追尾性能の最適化を実現する目標追尾装置を得ることを目的とする。

本発明に係る目標追尾装置は、目標の観測値、目標の観測精度、および目標の運動モデルにおけるシステム雑音に基づいて、目標の推定値、推定値と観測値との差である残差、および残差推定値を出力する追尾フィルタ部を備えた目標追尾装置において、目標の想定加速度、目標の観測精度、およびサンプリング周期を入力とし、目標の運動モデルにおける最適なシステム雑音を算出する最適システム雑音設定部をさらに備え、追尾フィルタ部は、最適システム雑音設定部により算出された最適なシステム雑音に基づいて目標の推定値を求めるものである。

本発明によれば、カルマンフィルタなどに代表される既存の追尾フィルタの設計パラメータとして、設計の難しいシステム雑音を直接与える代わりに、直感的にわかりやすい目標の想定加速度を与えることによりシステム雑音を算出する手段を付加することにより、直感的に設計可能な目標の想定加速度に基づく追尾フィルタを、カルマンフィルタに代表される既存の追尾フィルタに対しても実装可能とし、追尾性能の最適化を実現する目標追尾装置を得ることができる。

以下、本発明の目標追尾装置の好適な実施の形態につき図面を用いて説明する。

実施の形態１．
図１は、本発明の実施の形態１における目標追尾装置の構成図である。この目標追尾装置は、最適システム雑音設定部１０および追尾フィルタ部２０を備えて構成される。そして、本発明の目標追尾装置は、これらの構成要件のうち、最適システム雑音設定部１０の処理に特徴を有している。

図１における最適システム雑音設定部１０は、ゲイン設定部１１、ゲイン選択部１２、およびシステム雑音算出部１３で構成され、目標の想定加速度、観測精度、およびサンプリング周期を入力情報として取り込み、目標の運動モデルにおける最適なシステム雑音パラメータを出力する。観測精度としては、観測雑音の標準偏差が考えられ、以下の説明においては、観測精度として観測雑音の標準偏差を用いる。

本実施の形態１における目標追尾装置は、設計パラメータとして目標の想定加速度を入力することで、既存の追尾フィルタ部２０で用いられているフィルタの最適性能を実現する最適システム雑音設定部１０を備えている点を技術的特徴としている。そこで、図１に示した構成を有する、この最適システム雑音設定部１０による一連の処理を、数式を交えて、以下に詳細に説明する。

まず始めに、ゲイン設定部１１は、入力情報である目標の想定加速度および観測雑音の標準偏差と、サンプリング周期と、観測モデルに基づく最適ＲＳＥの定常解とから、定常ゲインを算出する。より具体的には、例えば、ｘ軸に関して、ゲイン設定部１１は、目標の想定加速度に相当する加速度パラメータλ_ｘ、観測雑音の標準偏差σ_ｖｘ、およびサンプリング周期τより、下式（２５）、（２６）を用いて定常ゲインα、βを算出する。

このようにして、各軸について、定常ゲインが算出されるものの、定常ゲインには複数の解が存在し、一意に定めることができない。そこで、次に、ゲイン選択部１２は、安定性などの制約条件に基づいて、ゲイン設定部１１で算出された定常ゲインの絞込みを行う。例えば、安定性に関する条件としては、下式（２７）を用いることができ、ゲイン選択部１２は、この式を満足する定常ゲインを選択する。

さらに、システム雑音算出部１３は、ゲイン選択部１２で選択された定常ゲイン、および追尾フィルタ部２０で用いられるフィルタの定常解から、システム雑音を逆算して出力する。例えば、代表的なカルマンフィルタが追尾フィルタ部２０に用いられている場合には、システム雑音算出部１３は、ゲイン選択部１２で選択されたゲイン、観測雑音の標準偏差（例えば、ｘ軸であればσ_ｖｘ）、およびカルマンフィルタの定常解から、システム雑音（例えば、ｘ軸であればσ_ｗｘ）を、下式（２８）を用いて逆算する。

そして、追尾フィルタ部２０は、システム雑音算出部１３により逆算されたシステム雑音、入力情報である観測雑音の標準偏差、およびサンプリング周期を設計パラメータとして、追尾フィルタ（例えば、カルマンフィルタ）を構築できる。さらに、追尾フィルタ部２０は、目標の観測値を入力することにより、目標の推定値、推定値と観測値の差（残差）と残差推定値を出力する。このような出力を得るにあたって、追尾フィルタ部２０は、従来の追尾フィルタと同様の処理を適用することができる。

以上のように、実施の形態１によれば、追尾フィルタの設計パラメータとして、設計の難しいシステム雑音を直接与える代わりに、直感的にわかりやすい目標の想定加速度を与えることによりシステム雑音を算出する最適システム雑音設定部を備えることにより、結果的に追尾フィルタの最適性能を実現できる。

実施の形態２．
図２は、本発明の実施の形態２における目標追尾装置の構成図である。この目標追尾装置は、最適システム雑音選択部１４およびシステム雑音分散設定表記憶部１５で構成される最適システム雑音設定部１０ａを備えており、先の図１における最適システム雑音設定部１０と比較すると、内部構成が異なっている。

先の図１における最適システム雑音設定部１０は、入力情報に応じて、その都度、上式（２５）〜（２８）を用いてシステム雑音を算出していた。これに対して、本実施の形態２の図２における最適システム雑音設定部１０ａは、あらかじめオフラインで上式（２５）〜（２８）を用いて様々な入力情報（目標の想定加速度、観測精度、サンプリング周期）の組み合わせに応じたシステム雑音を計算しておき、それらをシステム雑音分散設定表としてシステム雑音分散設定表記憶部１５に保持しておく。

そして、最適システム雑音選択部１４は、実際の入力情報（目標の想定加速度、観測精度、サンプリング周期）に応じて、このリアルタイムの入力情報の条件に対応したシステム雑音を、システム雑音分散設定表記憶部１５に記憶されたシステム雑音分散設定表から読み出すことにより特定する。

以上のように、実施の形態２によれば、実際の入力情報に応じて、あらかじめ用意したシステム雑音分散設定表からシステム雑音を読み出すことで、システム雑音を算出するための演算負荷の軽減を図ることができる。

実施の形態３．
図３は、本発明の実施の形態３における目標追尾装置の構成図である。先の実施の形態１における図１の構成と比較すると、本実施の形態３における図３の構成は、加速度パラメータ修正量算出部３０をさらに備えている点が異なっている。そこで、この加速度パラメータ修正量算出部３０の機能を中心に、以下に説明する。

図３における加速度パラメータ修正量算出部３０は、残差推定誤差算出部３１および加速度パラメータ修正判定部３２を備えている。先の実施の形態１、２と同様に、追尾フィルタ部２０は、観測値を入力とし、目標の推定値とともに、残差と残差推定値を出力する。そこで、残差推定誤差算出部３１は、追尾フィルタ部２０から出力された残差と残差推定値を入力として、残差推定誤差を出力する。

より具体的には、追尾フィルタ部２０は、例えば、現在の時刻ｔ_ｋにおける観測値ｚ_ｋ（上式（６）に相当）と前時刻ｔ_ｋ−１にて算出済みの予測値ベクトルｘ（＾）_{ｋ｜ｋ−１}との残差共分散行列Ｓ_ｏｋを、下式（２９）より算出する。

次に、加速度パラメータ修正量算出部３０内の残差推定誤差算出部３１は、追尾フィルタ部２０により求められた残差共分散行列Ｓ_ｏｋと残差共分散行列推定値Ｓ_ｋ（上式（１１）に相当）から、残差推定誤差行列ΔＳ_ｋを下式（３０）より求めて、そのｘ、ｙ、ｚ各軸毎の成分をΔＳ_ｘ、ΔＳ_ｙ、ΔＳ_ｚとする。

次に、加速度パラメータ修正判定部３２は、残差推定誤差算出部３１により求められた残差推定誤差行列ΔＳ_ｋを入力とし、残差推定誤差の絶対値が閾値以上である場合には、所与の加速度パラメータ修正量±Δλを出力する。

ここで、加速度パラメータ修正量の符号は、残差推定誤差行列ΔＳ_ｋの各軸ごとの成分であるΔＳ_ｘ、ΔＳ_ｙ、ΔＳ_ｚそれぞれの値が正であれば正の加速度パラメータ修正量となり、負であれば負の加速度パラメータ修正量となる。

一方、閾値未満である場合には、加速度パラメータ修正判定部３２は、加速度パラメータの修正は行わず、修正量をゼロとして出力する。

そして、最適システム雑音設定部１０は、加速度パラメータ修正判定部３２から、所与の加速度パラメータ修正量±Δλが出力された場合には、前時刻ｔ_ｋ−１にて入力した加速度パラメータにこの修正量を付加することにより、新たに目標の想定加速度を修正して、先の実施の形態１、２と同様の処理を実施する。

以上のように、実施の形態３によれば、残差と残差推定値の差の絶対値が所与の閾値以上である場合には、加速度パラメータを修正することにより、加速度が変化した場合においても追尾フィルタの最適性能を実現することができる。

実施の形態４．
図４は、本発明の実施の形態４における目標追尾装置の構成図である。先の実施の形態３における図３の構成と比較すると、本実施の形態４における図４の構成は、加速度パラメータ修正量算出部３０内で、残差推定誤差算出部３１の代わりに残差正規化部３３を備えている点が異なっている。そこで、この残差正規化部３３の機能を中心に、以下に説明する。

図４における加速度パラメータ修正量算出部３０は、残差正規化部３３および加速度パラメータ修正判定部３２を備えている。先の実施の形態１〜３と同様に、追尾フィルタ部２０は、観測値を入力とし、目標の推定値とともに、残差と残差推定値を出力する。そこで、残差正規化部３３は、追尾フィルタ部２０から出力された残差と残差推定値を入力として、残差推定誤差を出力する。

より具体的には、加速度パラメータ修正量算出部３０内の残差正規化部３３は、追尾フィルタ部２０により先の実施の形態３と同じようにして求められた残差共分散行列Ｓ_ｏｋと残差共分散行列推定値Ｓ_ｋ（上式（１１）に相当）とに基づいて、残差共分散行列Ｓ_ｏｋを残差共分散行列推定値Ｓ_ｋで正規化し、下式（３１）に相当する正規化した値ΔＳ’を求める。

次に、加速度パラメータ修正判定部３２は、残差正規化部３３により求められた正規化した値ΔＳ’を入力とし、正規化された誤差の絶対値が閾値ｄ_１以上である場合、すなわち、下式（３２）の関係が成立する場合には、所与の加速度パラメータ修正量±Δλを出力する。

ここで、加速度パラメータ修正量の符号は、正規化した値ΔＳ’の各軸ごとの成分であるΔＳ_ｘ’、ΔＳ_ｙ’、ΔＳ_ｚ’それぞれの値が正であれば正の加速度パラメータ修正量となり、負であれば負の加速度パラメータ修正量となる。

一方、閾値ｄ_１未満である場合には、加速度パラメータ修正判定部３２は、加速度パラメータの修正は行わず、修正量をゼロとして出力する。

そして、最適システム雑音設定部１０は、加速度パラメータ修正判定部３２から、所与の加速度パラメータ修正量±Δλが出力された場合には、前時刻ｔ_ｋ−１にて入力した加速度パラメータにこの修正量を付加することにより、新たに目標の想定加速度を修正して、先の実施の形態１〜３と同様の処理を実施する。

以上のように、実施の形態４によれば、残差を残差推定値で正規化した値の絶対値が所与の閾値以上である場合には、加速度パラメータを修正することにより、加速度が変化した場合においても、先の実施の形態３と同様に、追尾フィルタの最適性能を実現することができる。

なお、上述の実施の形態３、４では、最適システム雑音設定部として、図１に示した構成のものを用いる場合を説明したが、図２に示した構成のものを用いても、同様の効果を得ることができる。

本発明の実施の形態１における目標追尾装置の構成図である。 本発明の実施の形態２における目標追尾装置の構成図である。 本発明の実施の形態３における目標追尾装置の構成図である。 本発明の実施の形態４における目標追尾装置の構成図である。

符号の説明

１０、１０ａ 最適システム雑音設定部、１１ ゲイン設定部、１２ ゲイン選択部、１３ システム雑音算出部、１４ 最適システム雑音選択部、１５ 最適システム雑音分散設定表記憶部（記憶部）、２０ 追尾フィルタ部、３０ 加速度パラメータ修正量算出部（修正量設定部）、３１ 残差推定誤差算出部、３２ 加速度パラメータ修正判定部（修正判定部）、３３ 残差正規化部。

Claims (6)

1. 目標の観測値、前記目標の観測精度、および前記目標の運動モデルにおけるシステム雑音に基づいて、前記目標の推定値、前記推定値と前記観測値との差である残差、および残差推定値を出力する追尾フィルタ部を備えた目標追尾装置において、
   前記目標の想定加速度、前記目標の観測精度、およびサンプリング周期を入力とし、前記目標の運動モデルにおける最適なシステム雑音を算出する最適システム雑音設定部をさらに備え、
   前記追尾フィルタ部は、前記最適システム雑音設定部により算出された前記最適なシステム雑音に基づいて前記目標の推定値を求める
   ことを特徴とする目標追尾装置。
2. 請求項１に記載の目標追尾装置において、
   前記最適システム雑音設定部は、
   前記目標の想定加速度、前記目標の観測精度、前記サンプリング周期、および前記目標の観測モデルに基づく最適ＲＳＥの定常解に基づいて定常ゲインを算出するゲイン設定部と、
   所定の制約条件に基づいて、前記ゲイン設定部で算出された前記定常ゲインの絞込みを行うゲイン選択部と、
   前記ゲイン選択部で選択された定常ゲイン、および前記追尾フィルタ部で用いられるフィルタの定常解から、システム雑音を逆算することにより前記最適なシステム雑音を算出するシステム雑音算出部と
   を備えることを特徴とする目標追尾装置。
3. 請求項１に記載の目標追尾装置において、
   前記最適システム雑音設定部は、
   目標の想定加速度、目標の観測精度、およびサンプリング周期をパラメータとして種々の組み合わせに対応して算出される最適なシステム雑音をシステム雑音分散設定表としてあらかじめ記憶する記憶部と、
   リアルタイムで入力される前記目標の想定加速度、前記目標の観測精度、およびサンプリング周期に対応する最適なシステム雑音を、前記記憶部に記憶された前記システム雑音分散設定表から取り出す最適システム雑音選択部と
   を備えることを特徴とする目標追尾装置。
4. 請求項１ないし３のいずれか１項に記載の目標追尾装置において、
   前記追尾フィルタ部から出力された前記残差および前記残差推定値に基づいて残差推定誤差を算出し、算出した前記残差推定誤差の大きさに応じて前記目標の想定加速度を修正するための修正量を算出する修正量設定部をさらに備え、
   前記最適システム雑音設定部は、前記修正量設定部で算出された前記修正量により前記目標の想定加速度を修正して前記最適なシステム雑音を算出する
   ことを特徴とする目標追尾装置。
5. 請求項４に記載の目標追尾装置において、
   前記修正量設定部は、
   前記追尾フィルタ部から出力された前記残差と前記残差推定値との差分から残差推定誤差を算出する残差推定誤差算出部と、
   前記残差推定誤差算出部で算出された前記残差推定誤差と所定の閾値との比較結果に基づいて前記修正量を算出する修正判定部と
   を備えることを特徴とする目標追尾装置。
6. 請求項４に記載の目標追尾装置において、
   前記修正量設定部は、
   前記追尾フィルタ部から出力された前記残差および前記残差推定値に基づいて、前記残差推定値により正規化した残差推定誤差を算出する残差正規化部と、
   前記残差正規化部で算出された前記残差推定誤差と所定の閾値との比較結果に基づいて前記修正量を算出する修正判定部と
   を備えることを特徴とする目標追尾装置。

Images