JP4618627B2 - 目標追尾装置 - Google Patents
目標追尾装置 Download PDFInfo
- Publication number
- JP4618627B2 JP4618627B2 JP2002369839A JP2002369839A JP4618627B2 JP 4618627 B2 JP4618627 B2 JP 4618627B2 JP 2002369839 A JP2002369839 A JP 2002369839A JP 2002369839 A JP2002369839 A JP 2002369839A JP 4618627 B2 JP4618627 B2 JP 4618627B2
- Authority
- JP
- Japan
- Prior art keywords
- value
- calculated
- detection data
- change rate
- distance change
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Images
Landscapes
- Radar Systems Or Details Thereof (AREA)
Description
【０００１】
【発明の属する技術分野】
本発明はレーダから得られた探知データから、目標の軌跡を推定する目標追尾装置に関するものであり、特に不要信号が同時に観測される状況や、複数の目標が混在する状況下で、目標と探知データの組み合わせである相関の決定を、正確かつ効率的に処理可能な様に工夫したものである。
【０００２】
【従来の技術】
レーダから得られる探知データを使って目標の軌道である航跡を推定する目標追尾では、どの探知データを用いて航跡を形成するか、その決定方法により追尾の性能は大きく左右される。各々の探知データは、以下に挙げる3通りの解釈のうち何れかが当てはまる。
（ａ）何れかの既存の航跡と対応付ける
（ｂ）新たに発生した目標である
（ｂ）不要信号である
どの解釈を正しいとするか、この相関決定の際は、判断の指標として、個々の既存航跡と探知データの組み合わせについての相関の度合いを示す指標が必要となる。この相関の度合いを表す指標としてよく用いられるのが、予測値からの距離である。
【０００３】
この種の従来の追尾方式では、相関の有無の判定および相関の度合い計算について、探知データの位置情報しか用いない。従ってレーダから得られるドップラ周波数の観測値より計算する距離変化率（ドップラ速度）が得られる場合でも、この情報を活用できず、間違った相関決定を行ってしまう可能性が高かった。
【０００４】
そのため、この距離変化率の観測値を利用し、位置情報のフィルタとは別に距離変化率のフィルタを構成し、既存航跡と探知データの相関に対して２重の検査を行う追尾技術が考案されている（例えば、特許文献１参照）。
【０００５】
【特許文献１】
特願２００２−１６７６６４号
【０００６】
【発明が解決しようとする課題】
しかしながら、上記の特許文献１に記載されている従来の追尾技術では、距離変化率のフィルタによって計算された距離変化率と、位置情報のフィルタによって計算された速度から求めた距離変化率の一致を判定する機能がないため、これらの２つの距離変化率に関する推定結果が全くかけ離れた場合でも「相関有り」と判定されてしまうため、追尾結果の誤りと、計算負荷の増加を起こす可能性があった。
【０００７】
この発明は、かかる問題点を解決するためになされたものであり、位置情報の観測値を使って推定した距離変化率と、距離変化率の観測値から得られる値との照合を行うことよって、相関の有無に関する判定の精度の向上を図る目標追尾装置を得ることを目的とする。
【０００８】
【課題を解決するための手段】
この発明は、観測された探知データを使って目標の軌道である航跡を推定するための目標追尾装置であって、現在の１つ前の時刻の位置の平滑値を用いて現在の位置の予測値を計算し、この計算した位置の予測値および現在の探知データの位置の観測値を用いて現在の位置の平滑値を計算する位置情報フィルタ部と、現在の１つ前の時刻の距離変化率の平滑値を用いて現在の距離変化率の予測値を計算し、この計算した距離変化率の予測値および現在の探知データの距離変化率の観測値を用いて、現在の距離変化率の平滑値を計算する距離変化率フィルタ部と、上記位置情報フィルタ部で計算された上記位置の予測値に基づいて現在の探知データの存在可能領域を算出し、探知データと既存航跡とが相関可能かを判定するゲート内外判定部と、上記ゲート内外判定部により相関可能と判定された探知データに関して、上記位置情報フィルタ部により計算された上記位置の平滑値に基づいて距離変化率を算出し、上記距離変化率フィルタ部により計算された上記距離変化率の平滑値と照合することによって、探知データと既存航跡が相関可能かを判定する距離変化率照合部と、上記距離変化率照合部により相関可能と判定された既存航跡と探知データとの相関の度合いを計算する距離変化率一致度計算部と、上記距離変化率一致度計算部で計算された既存航跡と探知データの上記相関の度合いを基に、目標の追尾航跡を決定する追尾航跡決定部とを備えた目標追尾装置である。
【０００９】
【発明の実施の形態】
実施の形態１．
以下、本発明の実施の形態１に係る目標追尾装置について説明を行う。図１にその構成を示す。本発明の目標追尾装置は、センサ１から探知されたデータを使って目標を追尾することを目的としている。図１に示すように、本発明の目標追尾装置は、センサ１から得られた探知データを用いて、既存航跡と探知データとの個別の相関を決定する個別相関決定部２と、航跡と探知データの相関の度合いを基に、目標の追尾航跡を決定する追尾航跡決定部３と、既存航跡が相関した探知データを用いて、目標の位置、速度、距離変化率を推定する航跡諸元計算部４と、探知データや予測値等の計算結果が格納される航跡データ格納部５とから構成されている。
【００１０】
個別相関決定部２は、図１に示すように、既存航跡の予測値から探知データの存在可能領域を算出し、探知データと航跡が相関可能かを判定するゲート内外判定部２１と、後述する位置情報フィルタ部４１で計算された平滑値より距離変化率を算出し、同じく後述する距離変化率フィルタ部４２で計算された距離変化率の平滑値と照合することによって、探知データと航跡が相関可能かを判定する距離変化率照合部２２と、上記の照合の度合いによって既存航跡と探知データの相関の度合いを計算する距離変化率一致度計算部２３とを備えている。
【００１１】
また、航跡諸元計算部４は、図１に示すように、既存航跡が相関した探知データの位置情報の観測値を使って、位置および速度を推定する位置情報フィルタ部４１と、既存航跡が相関した探知データの距離変化率の観測値を使って、距離変化率を推定する距離変化率フィルタ部４２とを備えている。
【００１２】
次に、動作について説明する。本実施の形態における追尾処理の手順の概要を図２に示す。図２に示すように、本実施の形態においては、まず、センサ１からの次サンプリングデータの読み込みを行い（ステップＳ１００）、個別相関部２により個別相関を決定し（ステップＳ１０１）、次に、追尾航跡決定部３により追尾航跡を決定する（ステップＳ１０２）。ステップＳ１０１の個別相関決定ステップと、ステップＳ１０２の追尾航跡決定ステップの詳細について以下に説明する。なお、この図２の流れ図は、センサによる同一時刻の観測で得られた探知データ群１サンプル分の処理過程を示している。すなわち、サンプル毎にこの図２に示す以下の処理を繰り返す。
【００１３】
まず、はじめに、ステップＳ１０１の個別相関決定ステップについて説明する。図３に個別相関決定ステップの詳細手順を示す。以下、この手順図に従って個別相関決定の処理内容を説明する。
【００１４】
まず、ステップＳ１において、ゲート内外判定部２１は、既存航跡の予測値と、残差共分散行列から探知データが観測される可能性が高い領域すなわちゲートを設定する。このゲート内外判定方法を以下の式（１）に示す。次に、ステップＳ２において、ゲート内外判定部２１は、観測値の位置ベクトルｚｋがこの条件を満たせばゲート内、満たさなければゲート外と判定する。
【００１５】
【数１】
【００１６】
ここで、ｄは事前に設定するパラメータ、Ｓ ｋ は残差共分散行列であり、ｚｋ｜ｋ−１は予測位置で、以下の（２）式により計算される。
【００１７】
【数２】
【００１８】
ここで、Ｈ ｋ は観測行列である。ｘ ｋ｜ｋ−１ は予測値である。
ゲート外と判定された場合は、その探知データと既存航跡の組み合わせは「相関の可能性がない」ものとして、以降の相関決定では考慮の対象から外れ、ステップＳ８で棄却される。
【００１９】
次に、ステップＳ３において、位置情報フィルタ部４１により、位置情報に関するフィルタの平滑計算を行う。位置情報フィルタ部４１は、現在の１つ前のサンプルにおける処理で、そのサンプルで相関した探知データ位置情報を使って、既存航跡の運動諸元を更新しておく。すなわち、最新時刻の平滑値とその次の時刻の予測値を計算する。なお、位置情報フィルタ部４１における予測値および平滑値の計算方法については、後述の式（１１）で示される平滑値ｘ ｋ｜ｋ ≡（ｘ ｋ｜ｋ ｙ ｋ｜ｋ ｚ ｋ｜ｋ ｖｘ ｋ｜ｋ ｖｙ ｋ｜ｋ ｖｚ ｋ｜ｋ ） Ｔ が導き出せるような、カルマンフィルタを用いた、従来周知の計算方法により行う。
【００２０】
次に、ステップＳ４における距離変化率フィルタ部４２の処理について説明する。
まず、平滑値の計算について式（３）〜（６）により説明する。式（３）〜（６）において、ｘＤ ｋ｜ｋが平滑値、ＰＤ ｋ｜ｋが平滑誤差共分散行列、ＫＤ ｋがカルマンゲイン、ＳＤ ｋが残差共分散行列である。
【００２１】
【数３】
【００２２】
ここで、ＨＤ ｋは観測行列、ＲＤ ｋは観測誤差共分散行列、Ｄｒｋ，ｊは観測値ｚｋ，ｊに付随するドップラ速度の計測値である。
【００２３】
次に、予測値の計算について説明する。以下の（７）式および（８）式において、ｘＤ ｋ｜ｋ−１、ＰＤ ｋ｜ｋ−１はそれぞれ予測値および予測誤差共分散行列であり、それぞれ、それらの式により計算される。
【００２４】
【数４】
【００２５】
ここで、ΦＤ ｋ−１は推移行列、ＱＤ ｋ−１は駆動雑音共分散行列である。
【００２６】
この距離変化率フィルタ部４２は、以下の（９）式に示す距離変化率とその変化率で構成される状態変数を、距離変化率の観測値を用いて推定する。
【００２７】
【数５】
【００２８】
次に、ステップＳ５において、距離変化率照合部２２により、距離変化率の照合を行う。距離変化率フィルタ４２によって計算された平滑値と、位置情報フィルタ４１が計算した位置および速度の平滑値から距離変化率を計算し、両者を照合し、ステップＳ６において、それらが一致したか否かを判定する。
【００２９】
位置情報フィルタの平滑値から距離変化率は、以下の式で計算する。
【００３０】
【数６】
【００３１】
距離変化率フィルタ４２の平滑値ｘＤ ｋ｜ｋの第一成分Ｄｋ｜ｋと、式（１０）により得られる距離変化率Ｒドットとが一致するか否かを判定するため、両者の差による評価関数を以下の通り設定する。
【００３２】
【数７】
【００３３】
この評価関数は以下の７変数を引数とする。
【００３４】
【数８】
【００３５】
この評価関数が０に等しい、すなわち「距離変化率フィルタ部４２の平滑値と、位置情報フィルタ部４１の平滑値より計算した距離変化率が、誤差の範囲内で一致する」という仮説を建てる。この仮説が正しく、かつ二つのフィルタの出力である平滑値が真値
【００３６】
【数９】
【００３７】
となっているならば、
Ｆ（ａ０）＝０ （１５）
が成立するはずである。しかしながら、位置情報フィルタ部４１および距離変化率フィルタ部４２の平滑値は平滑誤差を含んでいるため、評価関数は０にはならない。仮説が正しければ
Ｆ（ａ）＝Ｆ（ａ０）＋Ｌ・Δａ
＝Ｌ・Δａ （１６）
が成立するので、評価関数にフィルタの平滑値を入力した計算結果は、各成分の誤差が影響する範囲に収まるはずである。ただし、
【００３８】
【数１０】
【００３９】
Ｌはａの関数で、真値ａ０における値を設定するのが理想的である。しかし真値は得られないので、フィルタの平滑値を用いて近似する。以上より、評価関数Ｆ（ａ）の値は平均０、共分散ＬＰ’ＬＴの正規分布になる。ただし、共分散行列Ｐ’は二つのフィルタの平滑誤差共分散行列（以下の（１８）式と上述の（４）式）を使って、以下の様に計算する。
【００４０】
【数１１】
【００４１】
すると、以下の（２０）式による統計量は、自由度１のカイ平方分布となる。
【００４２】
【数１２】
【００４３】
以上より、評価関数が０となるという仮説は、
（Ｉ）η≧ｄｔｈのとき、仮説を棄却する。すなわち二つのフィルタの出力は一致しておらず、航跡と探知データに相関はないとする。
（ＩＩ）η＜ｄｔｈのとき、仮説を採択する。すなわち、二つのフィルタの出力は一致しており、航跡と探知データの間に相関があるとする。
ここで、ｄｔｈは判定のための閾値である。
【００４４】
次に、ステップＳ７において、距離変化率一致度計算部２３により、距離変化率照合度の計算を行う。航跡と探知データの組み合わせについて相関があると判定された場合、その相関の度合いを、式（２１）の様に計算する。
【００４５】
【数１３】
【００４６】
以上が、図２におけるステップＳ１０１の「個別相関決定」ステップの詳細説明である。
【００４７】
次に、図２におけるステップＳ１０２の「追尾航跡決定」ステップの詳細について説明する。
【００４８】
当該ステップは、追尾航跡決定部３が行う。追尾航跡決定部３は、
・相関の可能性のある既存航跡と探知データの組み合わせ
・上記の組み合わせについて計算された相関の度合い
を使って、航跡と探知データの組み合わせを選択し、全体での相関を決定する。この相関決定には既存航跡と探知データの相関のみならず、既存航跡から探知データが得られない状況、探知データが新目標あるいは不要信号である可能性が考慮される。この全体での相関決定は、様々なやり方があるが、一例として、以下に挙げる（１）〜（３）の何れの方式によっても容易にかつ精度よく実現可能である。
（１）各航跡に、相関度合いが最も高い探知データを割り当てるＮＮ(Nearest Neighbour)と呼ばれる方式を用いる。Samuel S.Blackman著 "Multiple-Target Tracking with Radar Application"(ARTECH HOUSE)の4章3節にその記述がある。
（２）航跡と探知データの組み合わせをさらに組み合わせて仮説を構成し、その仮説を複数維持しながら処理を継続し、1つの仮説の確からしさが他を圧倒するまで決定を保留する方式を用いる（例えば、特開平８−２７１６１７号公報にその方式についての記述がある）。
（３）航跡と探知データによって計算される平滑値を、相関の度合いによって重み付け平均して1つの航跡に統合するJPDA(Joint Probabilistic Data Association)と呼ばれる方式を用いる。Samuel S. Blackman著 "Multiple-Target Tracking with Radar Application"(ARTECH HOUSE)の10章3節にその記述がある。
【００４９】
以上のように、本実施の形態によれば、位置情報フィルタ部４１で計算された平滑値より距離変化率を算出し、距離変化率フィルタ部４２で計算された距離変化率の平滑値と照合することによって、探知データと航跡の相関判定を行うため、相関判定をより正確に行うことができる。
【００５０】
実施の形態２．
以下、本発明の実施の形態２にかかる目標追尾装置について説明する。図４にその構成を示す。図４において、上述の図１と同様の構成については同一符号を付して示し、ここではその説明を省略する。なお、図１と図４との異なる点は、図１の距離変化率一致度計算部２３の代わりに、尤度計算部２４を設けた点である。尤度計算部２４は、センサ１により探知された探知データと予測値の距離を用いて、航跡と探知データとの相関の度合いを計算するものである。
【００５１】
次に動作について説明する。本実施の形態における追尾処理の手順の概要は、図２に示した通りであるが、ステップＳ１０１における個別相関決定ステップの内容が、上述の実施の形態１と異なる。
【００５２】
図５に、本実施の形態における個別相関決定ステップの詳細手順を示す。図５を実施の形態１の図３と比較すると、図５においては、図１のステップＳ７の距離変化率一致度計算ステップが削除されていることと、相関尤度計算ステップ（ステップＳ１０）が、図１のステップＳ１，Ｓ２のゲート内外判定ステップの後に追加されている点が異なる。他のステップは図３と同じであるため、ここでは説明を省略する。
【００５３】
本実施の形態におけるステップＳ１０の相関尤度計算ステップは、追尾航跡決定部３に入力する相関の度合いの計算を尤度計算により行う。ゲート内外判定部２１によりゲート内として判定された場合に、その探知データと既存航跡の組み合わせは「相関の可能性がある」とする。このとき、尤度計算部２４が、その組み合わせにおける相関の度合いを、以下の式（２２）に従って計算する。この計算方式は、探知データの位置が、予測位置を中心とした共分散行列Ｓｋのガウス分布に従う統計量であることを仮定しており、計算結果である相関の度合い（尤度）は、探知データの位置が、予測値にどれだけ近いかを示している。すなわち、ここでは探知データの位置の予測値との一致度を、相関の度合いとして使用する。
【００５４】
【数１４】
【００５５】
以上のように、本実施の形態によれば、位置情報フィルタ部４１で計算された平滑値より距離変化率を算出し、距離変化率フィルタ部４２で計算された距離変化率の平滑値と照合することによって、探知データと航跡の相関判定を行うため、相関判定をより正確に行うことができる。
【００５６】
実施の形態３．
以下、本発明の実施の形態３にかかる目標追尾装置について説明する。図６にその構成を示す。図６において、上述の図１と同様の構成については同一符号を付して示し、ここではその説明を省略する。なお、図１と図６との異なる点は、図１の距離変化率照合部２２が削除されている点である。
【００５７】
次に動作について説明する。本実施の形態における追尾処理の手順の概要は、図２に示した通りであるが、ステップＳ１０１における個別相関決定ステップの内容が、上述の実施の形態１と異なる。
【００５８】
図７に、本実施の形態における個別相関決定ステップの詳細手順を示す。図７を実施の形態１の図３と比較すると、図７においては、図１のステップＳ５，Ｓ６の距離変化率照合ステップが削除されている。従って、ここでは、相関の棄却判定をゲート内外判定部２１のみで行う。他のステップは、実施の形態１と同様であるため、ここではその説明を省略する。なお、本実施の形態において、追尾航跡決定部３に入力する相関の度合いとしては、実施の形態１の場合と同様に、ステップＳ７において距離変化率一致度計算部２３により上述の式（２１）を用いて計算される距離変化率の照合の一致度を入力する。
【００５９】
以上のように、本実施の形態によれば、位置情報フィルタ部４１で計算された平滑値より距離変化率を算出し、距離変化率フィルタ部４２で計算された距離変化率の平滑値と照合することによって、探知データと航跡の相関判定を行うため、相関判定をより正確に行うことができる。
【００６０】
実施の形態４．
以下、本発明の実施の形態４にかかる目標追尾装置について説明する。図８にその構成を示す。図８において、上述の実施の形態２で示した図４と同様の構成については同一符号を付して示し、ここではその説明を省略する。なお、図４と図８との異なる点は、図４のゲート内外判定部２１が削除されている点である。
【００６１】
次に動作について説明する。本実施の形態における追尾処理の手順の概要は、図２に示した通りであるが、ステップＳ１０１における個別相関決定ステップの内容が、上述の実施の形態２と異なる。
【００６２】
図９に、本実施の形態における個別相関決定ステップの詳細手順を示す。図９を実施の形態２の図５と比較すると、図９においては、図５のステップＳ１，Ｓ２の位置情報ゲート内外判定ステップが削除されている。従って、ここでは、相関の棄却判定を距離変化率照合部２２による距離変化率の一致具合のみで行う。他のステップは実施の形態２と同様であるため、ここでは説明を省略する。なお、本実施の形態において、追尾航跡決定部３に入力する相関の度合いとしては、実施の形態２の場合と同様に、ステップＳ１０においてにより尤度計算部２４により上述の式（２２）を用いて計算される探知データの位置の予測値との一致度を入力する。
【００６３】
以上のように、本実施の形態によれば、位置情報フィルタ部４１で計算された平滑値より距離変化率を算出し、距離変化率フィルタ部４２で計算された距離変化率の平滑値と照合することによって、探知データと航跡の相関判定を行うため、相関判定をより正確に行うことができる。
【００６４】
実施の形態５．
以下、本発明の実施の形態５にかかる目標追尾装置について説明する。図１０にその構成を示す。図１０において、上述の図１と同様の構成については同一符号を付して示し、ここではその説明を省略する。なお、図１と図１０との異なる点は、図１の距離変化率フィルタ部４２が削除されている点である。
【００６５】
次に動作について説明する。本実施の形態における追尾処理の手順の概要は、図２に示した通りであるが、ステップＳ１０１における個別相関決定ステップの内容が、上述の実施の形態１と異なる。ステップＳ１０２における「追尾航跡決定」ステップの処理内容は実施の形態１と同様である。
【００６６】
図１１に、本実施の形態における個別相関決定ステップの詳細手順を示す。図１１を実施の形態１の図３と比較すると、図１１においては、図１のステップＳ４の距離変化率情報平滑計算ステップが削除されている。また、本実施の形態５においては、位置情報フィルタ部４１Ａによる位置情報平滑計算ステップＳ３Ａ、距離変化率照合部２２による距離変化率照合ステップＳ５Ａ，Ｓ６Ａ、および距離変化率一致度計算部２３による距離変化率一致度計算ステップＳ７Ａの処理内容が実施の形態１と異なる。ステップＳ１およびＳ２については実施の形態１と同様であるので、ここではその説明は省略する。
【００６７】
ステップＳ１およびＳ２は実施の形態１と同じであるため、ステップＳ３Ａの位置情報平滑計算ステップの説明を行う。本実施の形態においては、位置情報フィルタ部４１Ａは、現在の１つ前のサンプルにおける処理で、そのサンプルで相関した探知データ位置情報を使って、既存航跡の運動諸元を更新しておく。すなわち最新時刻の平滑値とその次の時刻の予測値を計算する。予測値および平滑値の計算方法を以下の式（２３）〜（２６）に示す。
【００６８】
まず、平滑値計算について説明する。ｘｋ｜ｋが平滑値、Ｐｋ｜ｋが平滑誤差共分散行列、Ｋｋはカルマンゲイン、Ｓｋは残差共分散行列である。
【００６９】
【数１５】
【００７０】
ここで、Ｈｋは観測行列、Ｒｋは観測誤差共分散行列である。またｘｋ｜ｋ―１、Ｐｋ｜ｋ―１は予測値及び予測誤差共分散行列で、以下の（２７），（２８）式で計算する。
【００７１】
【数１６】
【００７２】
ここで、Φｋ―１は推移行列、Ｑｋ―１は駆動雑音共分散行列である。
【００７３】
次に、距離変化率照合部２２による距離変化率照合ステップＳ５Ａ，Ｓ６Ａについて説明する。実施の形態１との違いは、ここでは、距離変化率フィルタ部４２を使わないことである。位置情報フィルタ部４１が計算した位置および速度の平滑値から距離変化率を計算し、距離変化率の観測値と照合する。
【００７４】
位置情報フィルタ部４１の平滑値からの距離変化率の計算は、既出の（１０）式に従う。探知データより得られる距離変化率の観測値と、（１０）式より得られた値とが一致するか否かを判定するため、両者の差による評価関数を以下の（２９）式により設定する。
【００７５】
【数１７】
【００７６】
この評価関数は引数として以下の７変数を設定する。
【００７７】
【数１８】
【００７８】
この評価関数が０、すなわち「距離変化率の観測値と位置情報フィルタの平滑値より計算する距離変化率が等しい」という仮説を建てる。この仮説が正しく、引数が真値に等しいならば、以下の（３１）式が成立する。
【００７９】
【数１９】
【００８０】
しかし真値は分からないので、引数ａとして位置情報フィルタ部４１Ａの平滑値を代入する。この場合平滑値にはフィルタの推定による誤差を含んでいるため、評価関数は０にはならない。しかし仮説が正しければ、以下の（３２）式が成立するので、評価関数の計算結果は各成分の誤差が影響する範囲に収まるはずである。
【００８１】
【数２０】
【００８２】
Ｌはａの関数であり、真値ａ０における値を設定するのが理想的であるが、真値は得られないので、位置情報フィルタ部４１Ａの平滑値と距離変化率の観測値を代入して近似する。以上より、評価関数の値は平均０、共分散ＬＰ’ＬＴの正規分布になる。ただし、共分散行列ｐ’Ｐ’は位置情報フィルタ部４１の平滑誤差共分散行列（式（２４））と、距離変化率の観測誤差
【数２１】
を使って、以下の様に計算する。
【００８３】
【数２２】
【００８４】
すると、以下の（３５）式で示される統計量は、自由度１のカイ平方分布となる。
【００８５】
【数２３】
【００８６】
以上より、評価関数が０、すなわち位置情報フィルタ部４１Ａの平滑値と距離変化率の観測値が一致するという仮説は、
（Ｉ）η≧ｄｔｈのとき、仮説を棄却する。すなわち位置情報フィルタ部４１Ａの出力と距離情報の観測値は一致しておらず、航跡と探知データに相関はないとする。
（ＩＩ）η＜ｄｔｈのとき、仮説を採択する。すなわち位置情報フィルタ部４１Ａの出力と距離情報の観測値は一致しており、航跡と探知データの間に相関があるとする。ここで、ｄｔｈは判定のための閾値である。
【００８７】
次に、距離変化率一致度計算部２３Ａによる距離変化率一致度計算ステップＳ７Ａについて説明する。本実施の形態においては、航跡と探知データの組み合わせについて相関があると判定された場合、その相関の度合いを、式（３６）の様に計算する。
【００８８】
【数２４】
【００８９】
以上の様に、本実施の形態によれば、探知データの位置情報による独立した位置情報フィルタ部４１で計算された平滑値と、距離変化率の観測値とを照合することによって、探知データと航跡の相関判定を行うため、相関判定をより正確に行うことができる。また、距離変化率のフィルタを用いない分、実施の形態１よりも計算量が少なくなる。
【００９０】
【発明の効果】
この発明は、観測された探知データを使って目標の軌道である航跡を推定するための目標追尾装置であって、現在の１つ前の時刻の位置の平滑値を用いて現在の位置の予測値を計算し、この計算した位置の予測値および現在の探知データの位置の観測値を用いて現在の位置の平滑値を計算する位置情報フィルタ部と、現在の１つ前の時刻の距離変化率の平滑値を用いて現在の距離変化率の予測値を計算し、この計算した距離変化率の予測値および現在の探知データの距離変化率の観測値を用いて、現在の距離変化率の平滑値を計算する距離変化率フィルタ部と、上記位置情報フィルタ部で計算された上記位置の予測値に基づいて現在の探知データの存在可能領域を算出し、探知データと既存航跡とが相関可能かを判定するゲート内外判定部と、上記ゲート内外判定部により相関可能と判定された探知データに関して、上記位置情報フィルタ部により計算された上記位置の平滑値に基づいて距離変化率を算出し、上記距離変化率フィルタ部により計算された上記距離変化率の平滑値と照合することによって、探知データと既存航跡が相関可能かを判定する距離変化率照合部と、上記距離変化率照合部により相関可能と判定された既存航跡と探知データとの相関の度合いを計算する距離変化率一致度計算部と、上記距離変化率一致度計算部で計算された既存航跡と探知データの上記相関の度合いを基に、目標の追尾航跡を決定する追尾航跡決定部とを備えた目標追尾装置であるので、位置情報の観測値を使って推定した距離変化率と、距離変化率の観測値から得られる値との照合を行うことよって、相関の有無に関する判定の精度の向上を図ることができる。
【図面の簡単な説明】
【図１】 本発明の実施の形態１に係る目標追尾装置の構成を示したブロック図である。
【図２】 本発明の実施の形態１〜５に係る目標追尾装置の動作の概略を示した流れ図である。
【図３】 本発明の実施の形態１に係る目標追尾装置の動作を示した流れ図である。
【図４】 本発明の実施の形態２に係る目標追尾装置の構成を示したブロック図である。
【図５】 本発明の実施の形態２に係る目標追尾装置の動作を示した流れ図である。
【図６】 本発明の実施の形態３に係る目標追尾装置の構成を示したブロック図である。
【図７】 本発明の実施の形態３に係る目標追尾装置の動作を示した流れ図である。
【図８】 本発明の実施の形態４に係る目標追尾装置の構成を示したブロック図である。
【図９】 本発明の実施の形態４に係る目標追尾装置の動作を示した流れ図である。
【図１０】 本発明の実施の形態５に係る目標追尾装置の構成を示したブロック図である。
【図１１】 本発明の実施の形態５に係る目標追尾装置の動作を示した流れ図である。
【符号の説明】
１ センサ、２ 個別相関決定部、３ 追尾航跡決定部、４ 航跡諸元計算部、５ 航跡データ、２１ ゲート内外判定部、２２，２２Ａ 距離変化率照合部、２３，２３Ａ 距離変化率一致度計算部、２４ 尤度計算部、４１，４１Ａ 位置情報フィルタ部、４２ 距離変化率フィルタ部。
【発明の属する技術分野】
本発明はレーダから得られた探知データから、目標の軌跡を推定する目標追尾装置に関するものであり、特に不要信号が同時に観測される状況や、複数の目標が混在する状況下で、目標と探知データの組み合わせである相関の決定を、正確かつ効率的に処理可能な様に工夫したものである。
【０００２】
【従来の技術】
レーダから得られる探知データを使って目標の軌道である航跡を推定する目標追尾では、どの探知データを用いて航跡を形成するか、その決定方法により追尾の性能は大きく左右される。各々の探知データは、以下に挙げる3通りの解釈のうち何れかが当てはまる。
（ａ）何れかの既存の航跡と対応付ける
（ｂ）新たに発生した目標である
（ｂ）不要信号である
どの解釈を正しいとするか、この相関決定の際は、判断の指標として、個々の既存航跡と探知データの組み合わせについての相関の度合いを示す指標が必要となる。この相関の度合いを表す指標としてよく用いられるのが、予測値からの距離である。
【０００３】
この種の従来の追尾方式では、相関の有無の判定および相関の度合い計算について、探知データの位置情報しか用いない。従ってレーダから得られるドップラ周波数の観測値より計算する距離変化率（ドップラ速度）が得られる場合でも、この情報を活用できず、間違った相関決定を行ってしまう可能性が高かった。
【０００４】
そのため、この距離変化率の観測値を利用し、位置情報のフィルタとは別に距離変化率のフィルタを構成し、既存航跡と探知データの相関に対して２重の検査を行う追尾技術が考案されている（例えば、特許文献１参照）。
【０００５】
【特許文献１】
特願２００２−１６７６６４号
【０００６】
【発明が解決しようとする課題】
しかしながら、上記の特許文献１に記載されている従来の追尾技術では、距離変化率のフィルタによって計算された距離変化率と、位置情報のフィルタによって計算された速度から求めた距離変化率の一致を判定する機能がないため、これらの２つの距離変化率に関する推定結果が全くかけ離れた場合でも「相関有り」と判定されてしまうため、追尾結果の誤りと、計算負荷の増加を起こす可能性があった。
【０００７】
この発明は、かかる問題点を解決するためになされたものであり、位置情報の観測値を使って推定した距離変化率と、距離変化率の観測値から得られる値との照合を行うことよって、相関の有無に関する判定の精度の向上を図る目標追尾装置を得ることを目的とする。
【０００８】
【課題を解決するための手段】
この発明は、観測された探知データを使って目標の軌道である航跡を推定するための目標追尾装置であって、現在の１つ前の時刻の位置の平滑値を用いて現在の位置の予測値を計算し、この計算した位置の予測値および現在の探知データの位置の観測値を用いて現在の位置の平滑値を計算する位置情報フィルタ部と、現在の１つ前の時刻の距離変化率の平滑値を用いて現在の距離変化率の予測値を計算し、この計算した距離変化率の予測値および現在の探知データの距離変化率の観測値を用いて、現在の距離変化率の平滑値を計算する距離変化率フィルタ部と、上記位置情報フィルタ部で計算された上記位置の予測値に基づいて現在の探知データの存在可能領域を算出し、探知データと既存航跡とが相関可能かを判定するゲート内外判定部と、上記ゲート内外判定部により相関可能と判定された探知データに関して、上記位置情報フィルタ部により計算された上記位置の平滑値に基づいて距離変化率を算出し、上記距離変化率フィルタ部により計算された上記距離変化率の平滑値と照合することによって、探知データと既存航跡が相関可能かを判定する距離変化率照合部と、上記距離変化率照合部により相関可能と判定された既存航跡と探知データとの相関の度合いを計算する距離変化率一致度計算部と、上記距離変化率一致度計算部で計算された既存航跡と探知データの上記相関の度合いを基に、目標の追尾航跡を決定する追尾航跡決定部とを備えた目標追尾装置である。
【０００９】
【発明の実施の形態】
実施の形態１．
以下、本発明の実施の形態１に係る目標追尾装置について説明を行う。図１にその構成を示す。本発明の目標追尾装置は、センサ１から探知されたデータを使って目標を追尾することを目的としている。図１に示すように、本発明の目標追尾装置は、センサ１から得られた探知データを用いて、既存航跡と探知データとの個別の相関を決定する個別相関決定部２と、航跡と探知データの相関の度合いを基に、目標の追尾航跡を決定する追尾航跡決定部３と、既存航跡が相関した探知データを用いて、目標の位置、速度、距離変化率を推定する航跡諸元計算部４と、探知データや予測値等の計算結果が格納される航跡データ格納部５とから構成されている。
【００１０】
個別相関決定部２は、図１に示すように、既存航跡の予測値から探知データの存在可能領域を算出し、探知データと航跡が相関可能かを判定するゲート内外判定部２１と、後述する位置情報フィルタ部４１で計算された平滑値より距離変化率を算出し、同じく後述する距離変化率フィルタ部４２で計算された距離変化率の平滑値と照合することによって、探知データと航跡が相関可能かを判定する距離変化率照合部２２と、上記の照合の度合いによって既存航跡と探知データの相関の度合いを計算する距離変化率一致度計算部２３とを備えている。
【００１１】
また、航跡諸元計算部４は、図１に示すように、既存航跡が相関した探知データの位置情報の観測値を使って、位置および速度を推定する位置情報フィルタ部４１と、既存航跡が相関した探知データの距離変化率の観測値を使って、距離変化率を推定する距離変化率フィルタ部４２とを備えている。
【００１２】
次に、動作について説明する。本実施の形態における追尾処理の手順の概要を図２に示す。図２に示すように、本実施の形態においては、まず、センサ１からの次サンプリングデータの読み込みを行い（ステップＳ１００）、個別相関部２により個別相関を決定し（ステップＳ１０１）、次に、追尾航跡決定部３により追尾航跡を決定する（ステップＳ１０２）。ステップＳ１０１の個別相関決定ステップと、ステップＳ１０２の追尾航跡決定ステップの詳細について以下に説明する。なお、この図２の流れ図は、センサによる同一時刻の観測で得られた探知データ群１サンプル分の処理過程を示している。すなわち、サンプル毎にこの図２に示す以下の処理を繰り返す。
【００１３】
まず、はじめに、ステップＳ１０１の個別相関決定ステップについて説明する。図３に個別相関決定ステップの詳細手順を示す。以下、この手順図に従って個別相関決定の処理内容を説明する。
【００１４】
まず、ステップＳ１において、ゲート内外判定部２１は、既存航跡の予測値と、残差共分散行列から探知データが観測される可能性が高い領域すなわちゲートを設定する。このゲート内外判定方法を以下の式（１）に示す。次に、ステップＳ２において、ゲート内外判定部２１は、観測値の位置ベクトルｚｋがこの条件を満たせばゲート内、満たさなければゲート外と判定する。
【００１５】
【数１】
ここで、ｄは事前に設定するパラメータ、Ｓ ｋ は残差共分散行列であり、ｚｋ｜ｋ−１は予測位置で、以下の（２）式により計算される。
【００１７】
【数２】
ここで、Ｈ ｋ は観測行列である。ｘ ｋ｜ｋ−１ は予測値である。
ゲート外と判定された場合は、その探知データと既存航跡の組み合わせは「相関の可能性がない」ものとして、以降の相関決定では考慮の対象から外れ、ステップＳ８で棄却される。
【００１９】
次に、ステップＳ３において、位置情報フィルタ部４１により、位置情報に関するフィルタの平滑計算を行う。位置情報フィルタ部４１は、現在の１つ前のサンプルにおける処理で、そのサンプルで相関した探知データ位置情報を使って、既存航跡の運動諸元を更新しておく。すなわち、最新時刻の平滑値とその次の時刻の予測値を計算する。なお、位置情報フィルタ部４１における予測値および平滑値の計算方法については、後述の式（１１）で示される平滑値ｘ ｋ｜ｋ ≡（ｘ ｋ｜ｋ ｙ ｋ｜ｋ ｚ ｋ｜ｋ ｖｘ ｋ｜ｋ ｖｙ ｋ｜ｋ ｖｚ ｋ｜ｋ ） Ｔ が導き出せるような、カルマンフィルタを用いた、従来周知の計算方法により行う。
【００２０】
次に、ステップＳ４における距離変化率フィルタ部４２の処理について説明する。
まず、平滑値の計算について式（３）〜（６）により説明する。式（３）〜（６）において、ｘＤ ｋ｜ｋが平滑値、ＰＤ ｋ｜ｋが平滑誤差共分散行列、ＫＤ ｋがカルマンゲイン、ＳＤ ｋが残差共分散行列である。
【００２１】
【数３】
ここで、ＨＤ ｋは観測行列、ＲＤ ｋは観測誤差共分散行列、Ｄｒｋ，ｊは観測値ｚｋ，ｊに付随するドップラ速度の計測値である。
【００２３】
次に、予測値の計算について説明する。以下の（７）式および（８）式において、ｘＤ ｋ｜ｋ−１、ＰＤ ｋ｜ｋ−１はそれぞれ予測値および予測誤差共分散行列であり、それぞれ、それらの式により計算される。
【００２４】
【数４】
ここで、ΦＤ ｋ−１は推移行列、ＱＤ ｋ−１は駆動雑音共分散行列である。
【００２６】
この距離変化率フィルタ部４２は、以下の（９）式に示す距離変化率とその変化率で構成される状態変数を、距離変化率の観測値を用いて推定する。
【００２７】
【数５】
次に、ステップＳ５において、距離変化率照合部２２により、距離変化率の照合を行う。距離変化率フィルタ４２によって計算された平滑値と、位置情報フィルタ４１が計算した位置および速度の平滑値から距離変化率を計算し、両者を照合し、ステップＳ６において、それらが一致したか否かを判定する。
【００２９】
位置情報フィルタの平滑値から距離変化率は、以下の式で計算する。
【００３０】
【数６】
距離変化率フィルタ４２の平滑値ｘＤ ｋ｜ｋの第一成分Ｄｋ｜ｋと、式（１０）により得られる距離変化率Ｒドットとが一致するか否かを判定するため、両者の差による評価関数を以下の通り設定する。
【００３２】
【数７】
この評価関数は以下の７変数を引数とする。
【００３４】
【数８】
この評価関数が０に等しい、すなわち「距離変化率フィルタ部４２の平滑値と、位置情報フィルタ部４１の平滑値より計算した距離変化率が、誤差の範囲内で一致する」という仮説を建てる。この仮説が正しく、かつ二つのフィルタの出力である平滑値が真値
【００３６】
【数９】
となっているならば、
Ｆ（ａ０）＝０ （１５）
が成立するはずである。しかしながら、位置情報フィルタ部４１および距離変化率フィルタ部４２の平滑値は平滑誤差を含んでいるため、評価関数は０にはならない。仮説が正しければ
Ｆ（ａ）＝Ｆ（ａ０）＋Ｌ・Δａ
＝Ｌ・Δａ （１６）
が成立するので、評価関数にフィルタの平滑値を入力した計算結果は、各成分の誤差が影響する範囲に収まるはずである。ただし、
【００３８】
【数１０】
Ｌはａの関数で、真値ａ０における値を設定するのが理想的である。しかし真値は得られないので、フィルタの平滑値を用いて近似する。以上より、評価関数Ｆ（ａ）の値は平均０、共分散ＬＰ’ＬＴの正規分布になる。ただし、共分散行列Ｐ’は二つのフィルタの平滑誤差共分散行列（以下の（１８）式と上述の（４）式）を使って、以下の様に計算する。
【００４０】
【数１１】
すると、以下の（２０）式による統計量は、自由度１のカイ平方分布となる。
【００４２】
【数１２】
以上より、評価関数が０となるという仮説は、
（Ｉ）η≧ｄｔｈのとき、仮説を棄却する。すなわち二つのフィルタの出力は一致しておらず、航跡と探知データに相関はないとする。
（ＩＩ）η＜ｄｔｈのとき、仮説を採択する。すなわち、二つのフィルタの出力は一致しており、航跡と探知データの間に相関があるとする。
ここで、ｄｔｈは判定のための閾値である。
【００４４】
次に、ステップＳ７において、距離変化率一致度計算部２３により、距離変化率照合度の計算を行う。航跡と探知データの組み合わせについて相関があると判定された場合、その相関の度合いを、式（２１）の様に計算する。
【００４５】
【数１３】
以上が、図２におけるステップＳ１０１の「個別相関決定」ステップの詳細説明である。
【００４７】
次に、図２におけるステップＳ１０２の「追尾航跡決定」ステップの詳細について説明する。
【００４８】
当該ステップは、追尾航跡決定部３が行う。追尾航跡決定部３は、
・相関の可能性のある既存航跡と探知データの組み合わせ
・上記の組み合わせについて計算された相関の度合い
を使って、航跡と探知データの組み合わせを選択し、全体での相関を決定する。この相関決定には既存航跡と探知データの相関のみならず、既存航跡から探知データが得られない状況、探知データが新目標あるいは不要信号である可能性が考慮される。この全体での相関決定は、様々なやり方があるが、一例として、以下に挙げる（１）〜（３）の何れの方式によっても容易にかつ精度よく実現可能である。
（１）各航跡に、相関度合いが最も高い探知データを割り当てるＮＮ(Nearest Neighbour)と呼ばれる方式を用いる。Samuel S.Blackman著 "Multiple-Target Tracking with Radar Application"(ARTECH HOUSE)の4章3節にその記述がある。
（２）航跡と探知データの組み合わせをさらに組み合わせて仮説を構成し、その仮説を複数維持しながら処理を継続し、1つの仮説の確からしさが他を圧倒するまで決定を保留する方式を用いる（例えば、特開平８−２７１６１７号公報にその方式についての記述がある）。
（３）航跡と探知データによって計算される平滑値を、相関の度合いによって重み付け平均して1つの航跡に統合するJPDA(Joint Probabilistic Data Association)と呼ばれる方式を用いる。Samuel S. Blackman著 "Multiple-Target Tracking with Radar Application"(ARTECH HOUSE)の10章3節にその記述がある。
【００４９】
以上のように、本実施の形態によれば、位置情報フィルタ部４１で計算された平滑値より距離変化率を算出し、距離変化率フィルタ部４２で計算された距離変化率の平滑値と照合することによって、探知データと航跡の相関判定を行うため、相関判定をより正確に行うことができる。
【００５０】
実施の形態２．
以下、本発明の実施の形態２にかかる目標追尾装置について説明する。図４にその構成を示す。図４において、上述の図１と同様の構成については同一符号を付して示し、ここではその説明を省略する。なお、図１と図４との異なる点は、図１の距離変化率一致度計算部２３の代わりに、尤度計算部２４を設けた点である。尤度計算部２４は、センサ１により探知された探知データと予測値の距離を用いて、航跡と探知データとの相関の度合いを計算するものである。
【００５１】
次に動作について説明する。本実施の形態における追尾処理の手順の概要は、図２に示した通りであるが、ステップＳ１０１における個別相関決定ステップの内容が、上述の実施の形態１と異なる。
【００５２】
図５に、本実施の形態における個別相関決定ステップの詳細手順を示す。図５を実施の形態１の図３と比較すると、図５においては、図１のステップＳ７の距離変化率一致度計算ステップが削除されていることと、相関尤度計算ステップ（ステップＳ１０）が、図１のステップＳ１，Ｓ２のゲート内外判定ステップの後に追加されている点が異なる。他のステップは図３と同じであるため、ここでは説明を省略する。
【００５３】
本実施の形態におけるステップＳ１０の相関尤度計算ステップは、追尾航跡決定部３に入力する相関の度合いの計算を尤度計算により行う。ゲート内外判定部２１によりゲート内として判定された場合に、その探知データと既存航跡の組み合わせは「相関の可能性がある」とする。このとき、尤度計算部２４が、その組み合わせにおける相関の度合いを、以下の式（２２）に従って計算する。この計算方式は、探知データの位置が、予測位置を中心とした共分散行列Ｓｋのガウス分布に従う統計量であることを仮定しており、計算結果である相関の度合い（尤度）は、探知データの位置が、予測値にどれだけ近いかを示している。すなわち、ここでは探知データの位置の予測値との一致度を、相関の度合いとして使用する。
【００５４】
【数１４】
以上のように、本実施の形態によれば、位置情報フィルタ部４１で計算された平滑値より距離変化率を算出し、距離変化率フィルタ部４２で計算された距離変化率の平滑値と照合することによって、探知データと航跡の相関判定を行うため、相関判定をより正確に行うことができる。
【００５６】
実施の形態３．
以下、本発明の実施の形態３にかかる目標追尾装置について説明する。図６にその構成を示す。図６において、上述の図１と同様の構成については同一符号を付して示し、ここではその説明を省略する。なお、図１と図６との異なる点は、図１の距離変化率照合部２２が削除されている点である。
【００５７】
次に動作について説明する。本実施の形態における追尾処理の手順の概要は、図２に示した通りであるが、ステップＳ１０１における個別相関決定ステップの内容が、上述の実施の形態１と異なる。
【００５８】
図７に、本実施の形態における個別相関決定ステップの詳細手順を示す。図７を実施の形態１の図３と比較すると、図７においては、図１のステップＳ５，Ｓ６の距離変化率照合ステップが削除されている。従って、ここでは、相関の棄却判定をゲート内外判定部２１のみで行う。他のステップは、実施の形態１と同様であるため、ここではその説明を省略する。なお、本実施の形態において、追尾航跡決定部３に入力する相関の度合いとしては、実施の形態１の場合と同様に、ステップＳ７において距離変化率一致度計算部２３により上述の式（２１）を用いて計算される距離変化率の照合の一致度を入力する。
【００５９】
以上のように、本実施の形態によれば、位置情報フィルタ部４１で計算された平滑値より距離変化率を算出し、距離変化率フィルタ部４２で計算された距離変化率の平滑値と照合することによって、探知データと航跡の相関判定を行うため、相関判定をより正確に行うことができる。
【００６０】
実施の形態４．
以下、本発明の実施の形態４にかかる目標追尾装置について説明する。図８にその構成を示す。図８において、上述の実施の形態２で示した図４と同様の構成については同一符号を付して示し、ここではその説明を省略する。なお、図４と図８との異なる点は、図４のゲート内外判定部２１が削除されている点である。
【００６１】
次に動作について説明する。本実施の形態における追尾処理の手順の概要は、図２に示した通りであるが、ステップＳ１０１における個別相関決定ステップの内容が、上述の実施の形態２と異なる。
【００６２】
図９に、本実施の形態における個別相関決定ステップの詳細手順を示す。図９を実施の形態２の図５と比較すると、図９においては、図５のステップＳ１，Ｓ２の位置情報ゲート内外判定ステップが削除されている。従って、ここでは、相関の棄却判定を距離変化率照合部２２による距離変化率の一致具合のみで行う。他のステップは実施の形態２と同様であるため、ここでは説明を省略する。なお、本実施の形態において、追尾航跡決定部３に入力する相関の度合いとしては、実施の形態２の場合と同様に、ステップＳ１０においてにより尤度計算部２４により上述の式（２２）を用いて計算される探知データの位置の予測値との一致度を入力する。
【００６３】
以上のように、本実施の形態によれば、位置情報フィルタ部４１で計算された平滑値より距離変化率を算出し、距離変化率フィルタ部４２で計算された距離変化率の平滑値と照合することによって、探知データと航跡の相関判定を行うため、相関判定をより正確に行うことができる。
【００６４】
実施の形態５．
以下、本発明の実施の形態５にかかる目標追尾装置について説明する。図１０にその構成を示す。図１０において、上述の図１と同様の構成については同一符号を付して示し、ここではその説明を省略する。なお、図１と図１０との異なる点は、図１の距離変化率フィルタ部４２が削除されている点である。
【００６５】
次に動作について説明する。本実施の形態における追尾処理の手順の概要は、図２に示した通りであるが、ステップＳ１０１における個別相関決定ステップの内容が、上述の実施の形態１と異なる。ステップＳ１０２における「追尾航跡決定」ステップの処理内容は実施の形態１と同様である。
【００６６】
図１１に、本実施の形態における個別相関決定ステップの詳細手順を示す。図１１を実施の形態１の図３と比較すると、図１１においては、図１のステップＳ４の距離変化率情報平滑計算ステップが削除されている。また、本実施の形態５においては、位置情報フィルタ部４１Ａによる位置情報平滑計算ステップＳ３Ａ、距離変化率照合部２２による距離変化率照合ステップＳ５Ａ，Ｓ６Ａ、および距離変化率一致度計算部２３による距離変化率一致度計算ステップＳ７Ａの処理内容が実施の形態１と異なる。ステップＳ１およびＳ２については実施の形態１と同様であるので、ここではその説明は省略する。
【００６７】
ステップＳ１およびＳ２は実施の形態１と同じであるため、ステップＳ３Ａの位置情報平滑計算ステップの説明を行う。本実施の形態においては、位置情報フィルタ部４１Ａは、現在の１つ前のサンプルにおける処理で、そのサンプルで相関した探知データ位置情報を使って、既存航跡の運動諸元を更新しておく。すなわち最新時刻の平滑値とその次の時刻の予測値を計算する。予測値および平滑値の計算方法を以下の式（２３）〜（２６）に示す。
【００６８】
まず、平滑値計算について説明する。ｘｋ｜ｋが平滑値、Ｐｋ｜ｋが平滑誤差共分散行列、Ｋｋはカルマンゲイン、Ｓｋは残差共分散行列である。
【００６９】
【数１５】
ここで、Ｈｋは観測行列、Ｒｋは観測誤差共分散行列である。またｘｋ｜ｋ―１、Ｐｋ｜ｋ―１は予測値及び予測誤差共分散行列で、以下の（２７），（２８）式で計算する。
【００７１】
【数１６】
ここで、Φｋ―１は推移行列、Ｑｋ―１は駆動雑音共分散行列である。
【００７３】
次に、距離変化率照合部２２による距離変化率照合ステップＳ５Ａ，Ｓ６Ａについて説明する。実施の形態１との違いは、ここでは、距離変化率フィルタ部４２を使わないことである。位置情報フィルタ部４１が計算した位置および速度の平滑値から距離変化率を計算し、距離変化率の観測値と照合する。
【００７４】
位置情報フィルタ部４１の平滑値からの距離変化率の計算は、既出の（１０）式に従う。探知データより得られる距離変化率の観測値と、（１０）式より得られた値とが一致するか否かを判定するため、両者の差による評価関数を以下の（２９）式により設定する。
【００７５】
【数１７】
この評価関数は引数として以下の７変数を設定する。
【００７７】
【数１８】
この評価関数が０、すなわち「距離変化率の観測値と位置情報フィルタの平滑値より計算する距離変化率が等しい」という仮説を建てる。この仮説が正しく、引数が真値に等しいならば、以下の（３１）式が成立する。
【００７９】
【数１９】
しかし真値は分からないので、引数ａとして位置情報フィルタ部４１Ａの平滑値を代入する。この場合平滑値にはフィルタの推定による誤差を含んでいるため、評価関数は０にはならない。しかし仮説が正しければ、以下の（３２）式が成立するので、評価関数の計算結果は各成分の誤差が影響する範囲に収まるはずである。
【００８１】
【数２０】
Ｌはａの関数であり、真値ａ０における値を設定するのが理想的であるが、真値は得られないので、位置情報フィルタ部４１Ａの平滑値と距離変化率の観測値を代入して近似する。以上より、評価関数の値は平均０、共分散ＬＰ’ＬＴの正規分布になる。ただし、共分散行列ｐ’Ｐ’は位置情報フィルタ部４１の平滑誤差共分散行列（式（２４））と、距離変化率の観測誤差
【数２１】
【００８３】
【数２２】
すると、以下の（３５）式で示される統計量は、自由度１のカイ平方分布となる。
【００８５】
【数２３】
以上より、評価関数が０、すなわち位置情報フィルタ部４１Ａの平滑値と距離変化率の観測値が一致するという仮説は、
（Ｉ）η≧ｄｔｈのとき、仮説を棄却する。すなわち位置情報フィルタ部４１Ａの出力と距離情報の観測値は一致しておらず、航跡と探知データに相関はないとする。
（ＩＩ）η＜ｄｔｈのとき、仮説を採択する。すなわち位置情報フィルタ部４１Ａの出力と距離情報の観測値は一致しており、航跡と探知データの間に相関があるとする。ここで、ｄｔｈは判定のための閾値である。
【００８７】
次に、距離変化率一致度計算部２３Ａによる距離変化率一致度計算ステップＳ７Ａについて説明する。本実施の形態においては、航跡と探知データの組み合わせについて相関があると判定された場合、その相関の度合いを、式（３６）の様に計算する。
【００８８】
【数２４】
以上の様に、本実施の形態によれば、探知データの位置情報による独立した位置情報フィルタ部４１で計算された平滑値と、距離変化率の観測値とを照合することによって、探知データと航跡の相関判定を行うため、相関判定をより正確に行うことができる。また、距離変化率のフィルタを用いない分、実施の形態１よりも計算量が少なくなる。
【００９０】
【発明の効果】
この発明は、観測された探知データを使って目標の軌道である航跡を推定するための目標追尾装置であって、現在の１つ前の時刻の位置の平滑値を用いて現在の位置の予測値を計算し、この計算した位置の予測値および現在の探知データの位置の観測値を用いて現在の位置の平滑値を計算する位置情報フィルタ部と、現在の１つ前の時刻の距離変化率の平滑値を用いて現在の距離変化率の予測値を計算し、この計算した距離変化率の予測値および現在の探知データの距離変化率の観測値を用いて、現在の距離変化率の平滑値を計算する距離変化率フィルタ部と、上記位置情報フィルタ部で計算された上記位置の予測値に基づいて現在の探知データの存在可能領域を算出し、探知データと既存航跡とが相関可能かを判定するゲート内外判定部と、上記ゲート内外判定部により相関可能と判定された探知データに関して、上記位置情報フィルタ部により計算された上記位置の平滑値に基づいて距離変化率を算出し、上記距離変化率フィルタ部により計算された上記距離変化率の平滑値と照合することによって、探知データと既存航跡が相関可能かを判定する距離変化率照合部と、上記距離変化率照合部により相関可能と判定された既存航跡と探知データとの相関の度合いを計算する距離変化率一致度計算部と、上記距離変化率一致度計算部で計算された既存航跡と探知データの上記相関の度合いを基に、目標の追尾航跡を決定する追尾航跡決定部とを備えた目標追尾装置であるので、位置情報の観測値を使って推定した距離変化率と、距離変化率の観測値から得られる値との照合を行うことよって、相関の有無に関する判定の精度の向上を図ることができる。
【図面の簡単な説明】
【図１】 本発明の実施の形態１に係る目標追尾装置の構成を示したブロック図である。
【図２】 本発明の実施の形態１〜５に係る目標追尾装置の動作の概略を示した流れ図である。
【図３】 本発明の実施の形態１に係る目標追尾装置の動作を示した流れ図である。
【図４】 本発明の実施の形態２に係る目標追尾装置の構成を示したブロック図である。
【図５】 本発明の実施の形態２に係る目標追尾装置の動作を示した流れ図である。
【図６】 本発明の実施の形態３に係る目標追尾装置の構成を示したブロック図である。
【図７】 本発明の実施の形態３に係る目標追尾装置の動作を示した流れ図である。
【図８】 本発明の実施の形態４に係る目標追尾装置の構成を示したブロック図である。
【図９】 本発明の実施の形態４に係る目標追尾装置の動作を示した流れ図である。
【図１０】 本発明の実施の形態５に係る目標追尾装置の構成を示したブロック図である。
【図１１】 本発明の実施の形態５に係る目標追尾装置の動作を示した流れ図である。
【符号の説明】
１ センサ、２ 個別相関決定部、３ 追尾航跡決定部、４ 航跡諸元計算部、５ 航跡データ、２１ ゲート内外判定部、２２，２２Ａ 距離変化率照合部、２３，２３Ａ 距離変化率一致度計算部、２４ 尤度計算部、４１，４１Ａ 位置情報フィルタ部、４２ 距離変化率フィルタ部。
Claims (7)
- 観測された探知データを使って目標の軌道である航跡を推定するための目標追尾装置であって、
現在の１つ前の時刻の位置の平滑値を用いて現在の位置の予測値を計算し、この計算した位置の予測値および現在の探知データの位置の観測値を用いて現在の位置の平滑値を計算する位置情報フィルタ部と、
現在の１つ前の時刻の距離変化率の平滑値を用いて現在の距離変化率の予測値を計算し、この計算した距離変化率の予測値および現在の探知データの距離変化率の観測値を用いて、現在の距離変化率の平滑値を計算する距離変化率フィルタ部と、
上記位置情報フィルタ部で計算された上記位置の予測値に基づいて現在の探知データの存在可能領域を算出し、探知データと既存航跡とが相関可能かを判定するゲート内外判定部と、
上記ゲート内外判定部により相関可能と判定された探知データに関して、上記位置情報フィルタ部により計算された上記位置の平滑値に基づいて距離変化率を算出し、上記距離変化率フィルタ部により計算された上記距離変化率の平滑値と照合することによって、探知データと既存航跡が相関可能かを判定する距離変化率照合部と、
上記距離変化率照合部により相関可能と判定された既存航跡と探知データとの相関の度合いを計算する距離変化率一致度計算部と、
上記距離変化率一致度計算部で計算された既存航跡と探知データの上記相関の度合いを基に、目標の追尾航跡を決定する追尾航跡決定部と
を備えたことを特徴とする目標追尾装置。 - 観測された探知データを使って目標の軌道である航跡を推定するための目標追尾装置であって、
現在の１つ前の時刻の位置の平滑値を用いて現在の位置の予測値を計算し、この計算した位置の予測値および現在の探知データの位置の観測値を用いて現在の位置の平滑値を計算する位置情報フィルタ部と、
現在の１つ前の時刻の距離変化率の平滑値を用いて現在の距離変化率の予測値を計算し、この計算した距離変化率の予測値および現在の探知データの距離変化率の観測値を用いて、現在の距離変化率の平滑値を計算する距離変化率フィルタ部と、
上記位置情報フィルタ部で計算された上記位置の予測値に基づいて現在の探知データの存在可能領域を算出し、探知データと既存航跡とが相関可能かを判定するゲート内外判定部と、
上記ゲート内外判定部により相関可能と判定された探知データと上記位置の予測値の距離を用いて探知データと既存航跡の相関の度合いを計算する尤度計算部と、
上記位置情報フィルタ部で計算された上記位置の平滑値に基づいて距離変化率を算出し、上記距離変化率フィルタ部で計算された上記距離変化率の平滑値と照合することによって、探知データと既存航跡とが相関可能かを判定する距離変化率照合部と、
上記距離変化率照合部により相関可能と判定された既存航跡と探知データに関して、前記尤度計算部で計算された上記相関の度合いを基に、目標の追尾航跡を決定する追尾航跡決定部と
を備えたことを特徴とする目標追尾装置。 - 観測された探知データを使って目標の軌道である航跡を推定するための目標追尾装置であって、
現在の１つ前の時刻の位置の平滑値を用いて現在の位置の予測値を計算し、この計算した位置の予測値および現在の探知データの位置の観測値を用いて現在の位置の平滑値を計算する位置情報フィルタ部と、
現在の１つ前の時刻の距離変化率の平滑値を用いて現在の距離変化率の予測値を計算し、この計算した距離変化率の予測値および現在の探知データの距離変化率の観測値を用いて、現在の距離変化率の平滑値を計算する距離変化率フィルタ部と、
上記位置情報フィルタ部により計算された上記位置の予測値に基づいて現在の探知データの存在可能領域を算出し、探知データと既存航跡とが相関可能かを判定するゲート内外判定部と、
上記ゲート内外判定部により相関可能と判定された探知データに関して、上記位置情報フィルタ部により計算された上記位置の平滑値に基づいて距離変化率を算出し、上記距離変化率フィルタ部により計算された距離変化率の平滑値と照合することによって、既存航跡と探知データの相関の度合いを計算する距離変化率一致度計算部と、
上記距離変化率一致度計算部で計算された既存航跡と探知データの上記相関の度合いを基に、目標の追尾航跡を決定する追尾航跡決定部と
を備えたことを特徴とする目標追尾装置。 - 観測された探知データを使って目標の軌道である航跡を推定するための目標追尾装置であって、
現在の１つ前の時刻の位置の平滑値を用いて現在の位置の予測値を計算し、この計算した位置の予測値および現在の探知データの位置の観測値を用いて現在の位置の平滑値を計算する位置情報フィルタ部と、
現在の１つ前の時刻の距離変化率の平滑値を用いて現在の距離変化率の予測値を計算し、この計算した距離変化率の予測値および現在の探知データの距離変化率の観測値を用いて、現在の距離変化率の平滑値を計算する距離変化率フィルタ部と、
既存航跡の予測値と探知データの距離から、相関の度合いを計算する尤度計算部と、
上記位置情報フィルタ部により計算された上記位置の平滑値に基づいて距離変化率を算出し、上記距離変化率フィルタ部で計算された距離変化率の平滑値と照合することによって、探知データと既存航跡が相関可能かを判定する距離変化率照合部と、
上記距離変化率照合部により相関可能と判定された既存航跡と探知データに関して、前記尤度計算部で計算された上記相関の度合いを基に、目標の追尾航跡を決定する追尾航跡決定部と
を備えたことを特徴とする目標追尾装置。 - 観測された探知データを使って目標の軌道である航跡を推定するための目標追尾装置であって、
現在の１つ前の時刻の位置の平滑値を用いて現在の位置の予測値を計算し、この計算した位置の予測値および現在の探知データの位置の観測値を用いて現在の位置の平滑値を計算する位置情報フィルタ部と、
上記位置情報フィルタ部で計算された上記位置の予測値に基づいて現在の探知データの存在可能領域を算出し、探知データと既存航跡とが相関可能かを判定するゲート内外判定部と、
上記ゲート内外判定部により相関可能と判定された探知データに関して、上記位置情報フィルタ部で計算された上記位置の平滑値より距離変化率を算出し、探知データより得られる距離変化率の観測値と照合することによって、探知データと既存航跡が相関可能かを判定する距離変化率照合部と、
上記距離変化率照合部により相関可能と判定された既存航跡と探知データとの相関の度合いを計算する距離変化率一致度計算部と、
上記距離変化率一致度計算部で計算された既存航跡と探知データの上記相関の度合いを基に、目標の追尾航跡を決定する追尾航跡決定部と
を備えたことを特徴とする目標追尾装置。 - 上記追尾航跡決定部は、既存航跡と探知データの相関について、相関度合いが最大となる組み合わせを選択することを特徴とする請求項１ないし５のいずれか１項に記載の目標追尾装置。
- 上記追尾航跡決定部は、既存航跡と探知データの相関について、複数の仮説を構成し、各々の仮説について相関の度合いを計算し、その相関の度合いに応じて仮説中の航跡を重み付け平均して統合し、その統合によって生成する航跡を目標の航跡と決定することを特徴とする請求項１ないし５のいずれか１項に記載の目標追尾装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2002369839A JP4618627B2 (ja) | 2002-12-20 | 2002-12-20 | 目標追尾装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2002369839A JP4618627B2 (ja) | 2002-12-20 | 2002-12-20 | 目標追尾装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2004198345A JP2004198345A (ja) | 2004-07-15 |
| JP4618627B2 true JP4618627B2 (ja) | 2011-01-26 |
Family
ID=32765941
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2002369839A Expired - Fee Related JP4618627B2 (ja) | 2002-12-20 | 2002-12-20 | 目標追尾装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP4618627B2 (ja) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP5526826B2 (ja) * | 2010-02-03 | 2014-06-18 | 富士通株式会社 | 目標物探知装置および目標物探知方法 |
| CN114859339B (zh) * | 2022-04-29 | 2023-06-02 | 电子科技大学 | 一种基于毫米波雷达的多目标跟踪方法 |
-
2002
- 2002-12-20 JP JP2002369839A patent/JP4618627B2/ja not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| JP2004198345A (ja) | 2004-07-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109143224B (zh) | 一种多目标关联方法和装置 | |
| EP2495583B1 (en) | Target tracking system and target tracking method | |
| CN109946671B (zh) | 一种基于双门限判决的水下机动弱目标检测跟踪方法 | |
| CN110501671B (zh) | 一种基于测量分配的目标跟踪方法及装置 | |
| CN106291498B (zh) | 一种基于粒子滤波的检测跟踪联合优化方法 | |
| CN111259332A (zh) | 一种杂波环境下的模糊数据关联方法及多目标跟踪方法 | |
| JP4618627B2 (ja) | 目標追尾装置 | |
| JP4166651B2 (ja) | 目標追尾装置 | |
| JP4196684B2 (ja) | 目標追尾装置 | |
| WO2019239524A1 (ja) | 航跡推定装置及び携帯情報端末 | |
| CN111142101B (zh) | 一种数据关联方法 | |
| JP2005337732A (ja) | レーダ装置 | |
| CN116953692A (zh) | 一种主动雷达和被动雷达协同跟踪下的航迹关联方法 | |
| Mobus et al. | Multi-target multi-object radar tracking | |
| CN115220002A (zh) | 一种固定单站的多目标数据关联跟踪方法和相关装置 | |
| Trehag et al. | Onboard estimation and classification of a railroad curvature | |
| JP2012112807A (ja) | 多目標追尾装置 | |
| CN112068121A (zh) | 基于随机有限集的编队目标跟踪方法 | |
| JP3881214B2 (ja) | 目標追尾装置 | |
| WO2022113478A1 (ja) | 物標検出装置及び物標検出方法 | |
| CN112346479B (zh) | 一种基于集中式卡尔曼滤波的无人航行器状态估计方法 | |
| CN117168455A (zh) | 带航迹维护的序贯融合双传感器jpda多目标跟踪方法 | |
| JP2017227640A (ja) | 位置確定装置、方法及び電子機器 | |
| CN114858169A (zh) | 一种基于离线mbm滤波器的多目标跟踪方法和装置 | |
| CN115375992A (zh) | 一种基于联合决策与估计的多目标联合检测与跟踪方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20050926 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20070802 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20071225 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20080222 |
|
| A02 | Decision of refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A02 Effective date: 20080527 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20100917 |
|
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20101020 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20131105 Year of fee payment: 3 |
|
| R150 | Certificate of patent or registration of utility model |
Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| LAPS | Cancellation because of no payment of annual fees |