目標追跡システムとそのプログラム及び方法、角度追跡装置とそのプログラム及び方法、目標追跡装置とそのプログラム及び方法

本発明は、目標の角度を観測する複数のＮ次元角度センサの情報を統合し、距離情報を算出・追加した目標の（Ｎ＋１）次元航跡の生成・維持を行う目標追跡システムとこのシステムに用いられる角度追跡装置及び目標追跡装置とそれらのプログラム及び方法に関する。

目標追跡システムにあっては、２個の２次元角度センサの情報を統合し、２目標の追跡を行う場合に、２次元航跡の４通りの組み合わせの中から、どの２次元航跡の組み合わせが同一目標のものであるかを判定する処理が提案されている。 さらに、ｎ個の２次元角度センサの情報を統合し、ｍ目標の追跡を行う場合に、２次元航跡の（ｎ・（ｎ−１）／２）・ｍ ^２通りの組み合わせの中から、どの２次元航跡の組み合わせが同一目標のものであるかを判定することも検討されている。

しかしながら、上記のような従来の目標追跡システムでは、２次元角度センサ数ｎや目標数ｍが増加するにつれて、２次元航跡の組み合わせ数が概ね２乗の関係で増加することになるため、精度向上のための２次元角度センサ数増大や目標数増大の要求に伴って装置規模や処理時間が増大してしまうという課題がある。

以上述べたように、目標追跡装置では、２次元角度センサ数や目標数が増加するにつれて、２次元航跡の組み合わせ数が概ね２乗の関係で増加するため、精度向上のための２次元角度センサ数増大や目標数増大の要求に伴って装置規模や処理時間が増大してしまうという課題がある。

本実施形態は前記のような課題に鑑みなされたもので、精度向上のための２次元角度センサ数増大や目標数増大の要求に伴う装置規模や処理時間の増大を抑制し、装置規模の縮小、処理時間の短縮を実現することのできる目標追跡システムとこのシステムに用いられる角度追跡装置及び目標追跡装置とそれらのプログラム及び方法を提供することを目的とする。

本実施形態によれば、複数のＮ（Ｎは１または２）次元角度センサでそれぞれ観測される目標毎のＮ次元角度観測値を入力し、前記センサで得られる目標毎のＮ次元角度観測値から前記目標毎のＮ次元航跡を算出すると共に、距離情報を算出・追加して前記目標毎の（Ｎ＋１）次元航跡を生成し維持する場合に、前記維持している目標毎の（Ｎ＋１）次元航跡からそれぞれのＮ次元予測値を算出し、前記目標毎のＮ次元予測値と前記目標毎に入力されるＮ次元角度観測値との相関の有無を判定し、前記相関の有無の判定で相関がないと判定された場合に、前記相関がないと判定されたＮ次元角度観測値に対応するＮ次元航跡に基づいて新たな目標の（Ｎ＋１）次元航跡を生成し、前記相関の有無の判定で相関があると判定された場合に、前記相関があると判定されたＮ次元角度観測値を用いてＮ次元予測値に対応して維持されている（Ｎ＋１）次元航跡を更新するようにしている。

本実施形態に係る目標追跡システムの構成を示すブロック図。

図１に示す目標追跡装置の処理の流れを示すフローチャート。

本実施形態に係る目標追跡システムの変形例を示すブロック図。

以下、図面を参照して本実施形態に係る目標追跡システムを説明する。

図１は本実施形態に係る目標追跡システムの構成を示すブロック図である。 この目標追跡システムは、それぞれ目標の２次元角度を観測するｎ個の２次元角度センサ１１−１〜１１−ｎと、各２次元角度センサ１１−１〜１１−ｎで得られる２次元角度観測値（以下、単に２次元観測値と称する）からそれぞれの目標の２次元航跡を算出することで目標毎の角度追跡を行うｎ個の角度追跡装置１２−１〜１２−ｎと、各角度追跡装置１２−１〜１２−ｎで得られる目標毎の角度追跡情報から目標毎の３次元航跡を生成・維持することで各目標を追跡する目標追跡装置１３とを備える。

ここで、上記角度追跡装置１２−１〜１２−ｎは、２次元角度センサ１１−１〜１１−ｎからの２次元観測値とその観測値を用いて算出した２次元航跡を対にして、角度追跡情報として目標追跡装置１３へ出力するペアリング機能を備える。

上記構成による目標追跡システムにおいて、目標追跡装置１３は、上記角度追跡装置１２−１〜１２−ｎで得られた角度追跡情報（２次元観測値と２次元航跡）を入力し、距離情報を算出・追加して３次元航跡を生成・維持するもので、具体的には、予測処理部１３１、相関処理部１３２、２次元航跡抽出部１３３、３次元航跡生成部１３４及び平滑処理部１３５から構成される。

予測処理部１３１は、角度追跡装置１２−１〜１２−ｎから２次元観測値が入力された時点で、維持している３次元航跡から２次元予測値を算出する。

相関処理部１３２は、予測処理部１３１からの２次元予測値と角度追跡装置１２−１〜１２−ｎからの２次元観測値との相関の有無を判定する。

２次元航跡抽出部１３３は、相関処理部１３２において２次元予測値と２次元観測値との間に相関がないと判定された場合に、相関がないと判定された２次元観測値の対となる２次元航跡を抽出する。

３次元航跡生成部１３４は、２次元航跡抽出部１３３で抽出された２次元航跡に基づいて３次元航跡を生成する。

平滑処理部１３５は、相関処理部１３２において２次元予測値と２次元観測値との間に相関があると判定された場合に、相関があると判定された２次元観測値を用いて２次元予測値に対応する３次元航跡を更新する。

本実施形態の構成における目標追跡装置１３の処理内容について、ここでは拡張カルマンフィルタを用いた場合を例にして簡単に説明する。 尚、目標毎に、以下の処理が実施されるが、ここでは、説明が繁雑になるため各目標を区別する記号は用いないものとする。

まず、拡張カルマンフィルタでは、以下の運動モデルと観測モデルを用いる。

ここで、ｘ(ｋ)は観測時刻ｔ _ｋにおける目標の状態ベクトル、Ｆ(ｋ)とＧ(ｋ)はそれぞれ観測時刻ｔ _ｋ−１からｔ _ｋにおける遷移行列と駆動行列、ｗ(ｋ−１)は観測時刻ｔ _ｋ−１における平均０、共分散行列Ｑ(ｋ−１)の正規分布に従うシステム雑音ベクトル、ｙ(ｋ，ｉ)は、観測時刻ｔ _ｋにおける角度センサＳ _ｉの観測ベクトル、ｈ(・)は観測関数、ｖ(ｋ，ｉ)は、観測時刻ｔ _ｋにおける平均０、共分散行列Ｒ(ｋ，ｉ)の正規分布に従う観測雑音ベクトルである。

予測処理部１３１は以下の式に示す値を算出する。

ここで、ｘ(ｋ｜ｋ−１)とＰ(ｋ｜ｋ−１)はそれぞれ観測時刻ｔ _ｋにおける予測ベクトルと予測誤差共分散行列、ｘ(ｋ−１｜ｋ−１)とＰ(ｋ−１｜ｋ−１)は観測時刻ｔ _ｋ−１における平滑ベクトルと平滑誤差共分散行列、ｙ(ｋ｜ｋ−１，ｉ)は観測時刻ｔ _ｋにおける角度センサＳ _ｉの予測位置ベクトルである。 また、Ａ ^Ｔはベクトルまたは行列Ａの転置を示す。 なお、平滑ベクトルｘ(ｋ−１｜ｋ−１)は維持している３次元航跡に対応し、予測位置ベクトルｙ(ｋ｜ｋ−１，ｉ)は２次元予測値に対応する。

尚、上記の説明において「３次元航跡」とは、３次元空間における航跡であることを意味しており、「平滑ベクトル…３次元航跡に対応し」との記載において、平滑ベクトルの次数（次元と呼ばれることもある）は、速度成分や加速度成分を加えて、３より大きくすることができるのは勿論である。

上記相関処理部１３２は、相関処理として以下の式に示す値を算出する。

ここで、Ｓ（ｋ）とｄ（ｋ）は観測時刻ｔ _ｋにおける残差共分散行列とマハラノビス距離である。 また、Ａ ^−１は行列Ａの逆行列を示す。 マハラノビス距離（又はマハラノビス二乗距離）が基準値よりも小さい場合、相関があると判定され、平滑処理部１３５は平滑処理として以下の式に示す値を算出する。

ここで、Ｋ(ｋ)は観測時刻ｔ _ｋにおけるカルマンゲイン行列、ｘ(ｋ｜ｋ)とＰ(ｋ｜ｋ)はそれぞれ観測時刻ｔ _ｋにおける平滑ベクトルと平滑誤差共分散行列である。 また、Ｉは単位行列を示す。

また、相関処理部１３２における相関処理の結果として、マハラノビス距離（又はマハラノビス二乗距離）が基準値よりも大きい場合には相関がないと判定される。 このとき、２次元航跡抽出部１３３は、相関がないと判定された２次元観測値の対となる２次元航跡を抽出して、３次元航跡生成部１３４に出力する。

続いて、図２（ａ）を参照して、本実施形態の目標追跡装置１３の処理の流れを説明する。

目標追跡装置１３では、角度追跡装置１２−１〜１２−ｎから２次元観測値と２次元航跡の対が入力されると（ＳＴ１０１）、予測処理部１３１により、維持している３次元航跡から２次元予測値を算出する（ＳＴ１０２）。 次に、相関処理部１３２により、予測処理部１３１からの２次元予測値と角度追跡装置１２−１〜１２−ｎからの２次元観測値との相関をとり（ＳＴ１０３）、両者の相関の有無を判定する（ＳＴ１０４）。

ＳＴ１０４の結果、予測処理部１３１からの２次元予測値と角度追跡装置１２−１〜１２−ｎからの２次元観測値との間に相関がないと判定された場合（ＳＴ１０４のＮ）、２次元航跡抽出部１３３により、相関がないと判定された２次元観測値の対となる２次元航跡を抽出する（ＳＴ１０５）。 次に、３次元航跡生成部１３４により、２次元航跡抽出部１３３で抽出した異なる角度追跡装置からの２次元航跡に基づいて、３次元航跡を生成する（ＳＴ１０６）。

一方、予測処理部１３１からの２次元予測値と角度追跡装置１２−１〜１２−ｎからの２次元観測値との間に相関があると判定された場合（ＳＴ１０４のＹ）、平滑処理部１３５により相関があると判定された２次元観測値を用いて２次元予測値に対応する３次元航跡を更新する（ＳＴ１０７）。 この後は、動作終了が指示されるまで、上記したＳＴ１０１からの動作ステップを繰り返す（ＳＴ１０８）。

続いて、図２（ｂ）を参照して、ＳＴ１０６の３次元航跡生成処理の流れを説明する。

３次元航跡生成部１３４は、２次元航跡抽出部１３３で抽出した異なる角度追跡装置１２−１〜１２−ｎからの２次元航跡に基づいて２次元航跡の組み合わせを作成し、２次元航跡の組み合わせが、同一目標に対する２次元航跡の組みあわせであるか否かを判定する相関処理を実施する（ＳＴ１１１）。 ＳＴ１１１の判定の結果、同一目標に対する２次元航跡の組み合わせが見つかった場合（ＳＴ１１２のＹ）、同一目標に対する２次元航跡の組み合わせを用いて、距離や距離変化率の算出が行われる（ＳＴ１１３）。 ＳＴ１１３で距離や距離変化率が算出されると、２次元航跡に前記距離や距離変化率を組み合わせて３次元航跡を算出し（ＳＴ１１４）、ＳＴ１０６の処理を終了する。 また、ＳＴ１１２において、同一目標に対する２次元航跡の組み合わせが見つからなかった場合には、そのままＳＴ１０６の処理を終了する。

以上の３次元航跡生成処理では、２次元航跡抽出部１３３で抽出した異なる角度追跡装置からの２次元航跡を用いることを除き、特許文献１や非特許文献１〜３に記載の目標追跡装置と同じ処理を実施することができる。

すなわち、上記文献記載の目標追跡装置では、ｎ個の２次元角度センサの情報を統合し、ｍ目標の追跡を行う場合、２次元航跡の（ｎ・（ｎ−１）／２）・ｍ ^２通りの組み合わせの中から、どの２次元航跡の組み合わせが同一目標のものであるかを判定する必要があり、２次元角度センサ数ｎや目標数ｍが増加するにつれて、２次元航跡の組み合わせ数が概ね２乗の関係で増加していた。

これに対して、本実施形態の目標追跡装置１３では、ｎ個の２次元角度センサ１１−１〜１１−ｎの情報を統合し、ｍ目標の追跡を行う場合、目標数ｍと維持している航跡（目標）数との差分をΔｍとすると、２次元航跡の（ｎ・（ｎ−１）／２）・（Δｍ） ^２通りの組み合わせの中から、どの２次元航跡の組み合わせが同一目標のものであるかを判定するだけで済むため、２次元航跡の組み合わせ数を大幅に削減することができる。 また、目標数ｍが変化しない状況では、Δｍは最終的に０となり、ｍ目標の３次元航跡を生成・維持する場合でも、２次元航跡の組み合わせ数を０にすることができる。 この結果、本実施形態では、目標追跡装置の装置規模を小さくすることや、処理時間を短縮することができる。

尚、上記の実施形態では、角度追跡装置１２−１〜１２−ｎにおいて２次元角度センサ１１−１〜１１−ｎからの２次元観測値とその観測値から算出した２次元航跡を対にして目標追跡装置１３に入力するように構成したが、図３に示すように目標追跡装置１３ａを変形しても実施可能である。 以下の図３の説明において、図１と同一部分には同一符号を付して示し、ここでは異なる部分について説明する。

すなわち、目標追跡装置１３ａでは、２次元角度センサ１１−１〜１１−ｎからの２次元観測値を直接、相関処理部１３２ａに入力し、２次元観測値との対応情報を付加した角度追跡装置１２−１〜１２−ｎからの２次元航跡を直接、２次元航跡抽出部１３３ａに入力するように構成することもできる。 この場合、相関処理部１３２ａは、２次元航跡と対をなしていない２次元角度センサ１１−１〜１１−ｎからの２次元観測値のみを入力するように変形され、２次元航跡抽出部１３３ａは、相関処理部１３２ａにおける２次元観測値の相関判定の結果と、角度追跡装置１２−１〜１２−ｎからの２次元観測値との対応情報を付加した２次元航跡に基づいて、２次元航跡を抽出するように変形される。

したがって、上記のように変形された目標追跡装置１３ａでも、図１に示した実施形態と同様に、２次元航跡の組み合わせ数を大幅に削減することができ、装置規模を小さくすることや、処理時間を短縮することができる。

また、上記実施形態において、角度追跡装置１２−１〜１２−ｎは、２次元角度センサ１１−１〜１１−ｎのそれぞれに対応させるように構成したが、角度追跡装置１２は、複数（２個からｎ個）の２次元角度センサに対応させるように構成することができる。

尚、上記実施形態において、目標の角度を観測する複数のＮ（Ｎは１または２）次元角度センサ１１−１〜１１−ｎとして、画像センサ、ソナー装置、パッシブ・レーダ装置等が利用可能であり、これらの情報を統合し、目標の（Ｎ＋１）次元航跡の生成・維持を行う目標追跡装置に適用可能である。

また、これらの実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。 これら実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。 これら実施形態やその変形は、発明の範囲や要旨に含まれると同様に、特許請求の範囲に記載された発明とその均等の範囲に含まれるものである。

１１−１〜１１−ｎ…２次元角度センサ、１２−１〜１２−ｎ…角度追跡装置、１３，１３ａ…目標追跡装置、１３１…予測処理部、１３２，１３２ａ…相関処理部、１３３，１３３ａ…２次元航跡抽出部、１３４…３次元航跡生成部、１３５…平滑処理部。