目標追尾装置
【課題】複数のｎ次元角度センサと目標との交会角が小さな場合でも、追尾安定性を向上させた目標追尾装置を得る。
【解決手段】目標の追尾維持処理開始時の初期値算出にあたって、目標との仮説距離を用いた第１の追尾維持処理と、交会法により算出された距離を用いた第２の追尾維持処理とを並行して動作させ、それぞれの処理で目標航跡を取得しながら、それぞれの目標航跡に対する事後確率を算出し、この事後確率に基づいて２つの目標航跡を選択／統合した統合航跡を継続取得することによって目標の追尾を維持する。
【解決手段】目標の追尾維持処理開始時の初期値算出にあたって、目標との仮説距離を用いた第１の追尾維持処理と、交会法により算出された距離を用いた第２の追尾維持処理とを並行して動作させ、それぞれの処理で目標航跡を取得しながら、それぞれの目標航跡に対する事後確率を算出し、この事後確率に基づいて２つの目標航跡を選択／統合した統合航跡を継続取得することによって目標の追尾を維持する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、複数の角度センサからのｎ次元角度情報を統合して、目標のｎ＋１次元追尾を行う目標追尾装置に関する。
【背景技術】
【０００２】
図２０は、従来の目標追尾装置の構成の一例を示すブロック図である。この図２０に例示した目標追尾装置は、２つのｎ次元（ｎは１または２）角度センサを含み構成された事例であり、同一に構成された第１のｎ次元角度センサ１（＃１）、及び第２のｎ次元角度センサ１（＃２）、同一に構成された第１の角度追尾部２（＃１）、及び第２の角度追尾部２（＃２）、航跡統合部３、ならびに第１の航跡表示部４（＃１）、及び第２の航跡表示部４（＃２）から構成されている。
【０００３】
第１のｎ次元角度センサ１（＃１）及び第２のｎ次元角度センサ１（＃２）は、探知した目標の角度情報を取得し、それぞれ対応する角度追尾部２（＃１）または２（＃２）、ならびに航跡統合部３に送出する。第１の角度追尾部２（＃１）及び第２の角度追尾部２（＃２）は、それぞれに対応するｎ次元角度センサ１からの角度情報に基づいて、第１の角度追尾部２（＃１）は、目標の角度及び角速度からなる角度航跡ｚ（ｋ，１）及びその誤差共分散行列Ｐｚ（ｋ，１）を、また第２の角度追尾部２（＃２）は、目標の角度及び角速度からなる角度航跡ｚ（ｋ，２）及びその誤差共分散行列Ｐｚ（ｋ，２）を算出し、航跡統合部３に送出する。
【０００４】
航跡統合部３は、これら２つの角度追尾部２（＃１）及び２（＃２）での算出結果から、同一の目標に対する航跡を統合して追尾する。ここに、航跡統合部３は、相関部３１、距離／距離変化率算出部３２、ｎ＋１次元航跡初期値算出部３３、及び航跡維持部３４から構成されている。相関部３１は、２つの角度追尾部２（＃１）及び２（＃２）からの角度航跡を同一の目標毎に対をなすように関連づける。距離／距離変化率算出部３２は、これら角度航跡に基づき、交会法の手法により２つのｎ次元角度センサ１（＃１）及び１（＃２）のそれぞれから同一の目標までの距離及び距離変化率を算出する。ｎ＋１次元航跡初期値算出部３３は、距離／距離変化率算出部３２の算出結果をもとに、後段の航跡維持部３４でこの目標の追尾に必要な初期値となる、この目標のｎ＋１次元航跡とその誤差評価量としての誤差共分散行列を算出する。航跡維持部３４は、これら算出結果を初期値として追尾維持処理を実行し、その航跡を継続的に取得する。
【０００５】
航跡維持部３４における追尾維持処理は、例えば、式（１）に示した目標の運動モデル、及び式（２）に示した目標の観測モデルに対して、拡張カルマンフィルタ等によって算出することができる。
【数１】
【数２】
【０００６】
ここで、ｘ（ｋ）は、観測時刻ｔｋにおける目標の状態ベクトル、Ｆ（ｋ，ｋ−１）とＧ（ｋ，ｋ−１）は、観測時刻ｔｋ−１からｔｋにおける遷移行列と駆動行列、ｗ（ｋ−１）は、観測時刻ｔｋ−１における平均０、共分散行列Ｑ（ｋ−１）の正規分布に従うシステム雑音ベクトル、ｙ（ｋ，ｉ）は、観測時刻ｔｋにおけるｎ次元角度センサ１（＃ｉ）の観測ベクトル、ｈ（・）は、観測関数、ｖ（ｋ，ｉ）は、観測時刻ｔｋにおける平均０、共分散行列Ｒ（ｋ，ｉ）の正規分布に従う観測雑音ベクトルである。
【０００７】
また、航跡維持部３４を拡張カルマンフィルタで構成した場合、式（３）〜式（４）に示す予測処理、ならびに式（５）〜式（９）に示す平滑処理を実施する。
【数３】
【数４】
【数５】
【数６】
【数７】
【数８】
【数９】
【０００８】
ここで、ｘ＾（ｋ｜ｋ−１）とＰ（ｋ｜ｋ−１）は、観測時刻ｔｋにおける予測ベクトルと予測誤差共分散行列、Ｓ（ｋ）、ｄ（ｋ）およびＫ（ｋ）は、観測時刻ｔｋにおける残差共分散行列、マハラノビス距離およびカルマンゲイン行列、ｘ＾（ｋ｜ｋ）とＰ（ｋ｜ｋ）は、観測時刻ｔｋにおける平滑ベクトルと平滑誤差共分散行列である。また、ＡＴは、ベクトルまたは行列Ａの転置、Ａ−１は、行列Ａの逆行列、Ｉは、単位行列を示す。
【０００９】
ｎ次元角度センサ１が方位角のみを出力し、航跡維持部３４が２次元航跡を維持する場合、上記ベクトルおよび行列は、以下の式（１０）〜式（１６）のようになる。
【数１０】
【数１１】
【数１２】
【数１３】
【数１４】
【数１５】
【数１６】
【００１０】
ここで、ｐｘ（ｋ）とｐｙ（ｋ）は、目標のｘ軸とｙ軸の位置成分、ｖｘ（ｋ）とｖｙ（ｋ）は、目標のｘ軸とｙ軸の速度成分、σｘ（ｋ）とσｙ（ｋ）は、目標のｘ軸とｙ軸のシステム雑音の標準偏差、θ（ｋ，ｉ）は、ｎ次元角度センサ１（＃ｉ）の方位角の観測値、ｐｘ（ｉ）とｐｙ（ｉ）は、ｎ次元角度センサ１（＃ｉ）のｘ軸とｙ軸の位置成分、σθ（ｋ，ｉ）は、ｎ次元角度センサ１（＃ｉ）の方位角の観測雑音の標準偏差、（全て、観測時刻ｔｋにおける値）である。また、Ｉｎは、ｎ行ｎ列の単位行列、Ｏｎは、ｎ行ｎ列の零行列を示す。
【００１１】
第１の航跡表示部４（＃１）及び第２の航跡表示部４（＃２）は、航跡統合部３で取得した目標の航跡を、２つのｎ次元角度センサ１（＃１）及び１（＃２）のそれぞれに対応させて表示する。
【００１２】
次に、図２０に例示した従来の目標追尾装置の動作を、航跡統合部３における処理の流れを中心に説明する。図２１は、航跡統合部３の処理の流れの一例を示すフローチャートである。まず、航跡統合部３は、２つの角度追尾部２（＃１）及び２（＃２）から新たな目標の角度航跡、すなわち航跡の初期値を算出済みでない角度航跡を受けとると（ＳＴ６０１のＮ）、相関部３１がこれら角度航跡を入力し（ＳＴ６０２）、次いで、追尾維持処理に必要な一連の初期値算出処理を行う（ＳＴ６０３）。
【００１３】
すなわち、相関部３１において、２つの角度追尾部２（＃１）及び２（＃２）からの角度航跡のどの対が同一目標であるかを判別する相関処理を行なった結果（ＳＴ６０７）、同一目標と判別された角度航跡の対がある場合には（ＳＴ６０８のＹ）、距離／距離変化率算出部３２において、これら角度航跡に基づき、交会法を用いてそれぞれのｎ次元角度センサ１（＃１）及び１（＃２）からこの目標までの距離及び距離変化率を算出する（ＳＴ６０９）。さらに、この算出結果をもとに、ｎ＋１次元航跡初期値算出部３３において、後段の航跡維持部３４での追尾維持処理の初期値となる、目標のｎ＋１次元航跡とその誤差評価量としての誤差共分散行列を算出し、この目標の追尾開始に備える（ＳＴ６１０）。
【００１４】
次いで、再びＳＴ６０１のステップにより、対象の航跡初期値が算出済みである旨の判定がなされると（ＳＴ６０１のＹ）、この後は、算出済みの初期値に基づき目標の追尾に移行すべく、航跡維持部３４において、２つのｎ次元角度センサ１（＃１）及び１（＃２）で探知した角度情報を直接入力し（ＳＴ６０４）、追尾維持処理を開始して、目標のｎ＋１次元航跡の取得を維持継続する（ＳＴ６０５）。そして、その結果を、２つのｎ次元角度センサ１（＃１）及び１（＃２）のそれぞれに対応させて２つの航跡表示部４（＃１）及び４（＃２）に表示する。
【先行技術文献】
【特許文献】
【００１５】
【特許文献１】特開平１０−１４２３２５号公報（第２８ページ、図４）
【非特許文献】
【００１６】
【非特許文献１】Ｓ．Ｓ．Ｂｌａｃｋｍａｎ，“Ｍｕｌｔｉｔａｒｇｅｔ−Ｍｕｌｔｉｓｅｎｓｏｒ Ｔｒａｃｋｉｎｇ Ａｄｖａｎｃｅｄ Ａｐｐｌｉｃａｔｉｏｎｓ” Ａｒｔｅｃｈ Ｈｏｕｓｅ，Ｃｈａｐｔｅｒ６，ｐｐ．２１２−２１４，Ｊａｎ． １９８９
【非特許文献２】Ｎ．Ｐｅａｃｈ，“Ｂｅａｒｉｎｇｓ−ｏｎｌｙ ｔｒａｃｋｉｎｇ ｕｓｉｎｇ ａ ｓｅｔ ｏｆ ｒａｎｇｅ−ｐａｒａｍｅｔｅｒｉｓｅｄ ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ ｆｉｌｔｅｒｓ”，ＩＥＥＥ Ｐｒｏｃ−Ｃｏｎｔｒｏｌ Ｔｈｅｏｒｙ Ａｐｐｌ．，ｖｏｌ１４２，ｎｏ．１，ｐｐ．７３−８０，Ｊａｎ． １９９５
【非特許文献３】系 正義，辻道信吾，小菅義夫，"拡張カルマンフィルタを用いた複数パッシブセンサによる３次元運動目標の追尾，"信学論（Ｂ）、ｖｏｌ．Ｊ８２−Ｂ、ｎｏ．５、ｐｐ．１０６３−１０７２、Ｍａｙ １９９９
【発明の概要】
【発明が解決しようとする課題】
【００１７】
上述したように、従来の目標追尾装置では、２つのｎ次元角度センサ１で取得した角度航跡の対から、交会法により各角度センサから目標までの距離及び距離変化率を算出し、これらの距離及び距離変化率と角度航跡とに基づいて、目標のｎ＋１次元空間での位置及び速度よりなるｎ＋１次元航跡とその誤差共分散行列を算出し、さらにこの算出結果を初期値として拡張カルマンフィルタによる追尾維持処理に移行している。
【００１８】
ところで、交会法により各角度センサから目標までの距離を算出する場合、算出した距離に対する誤差は、２つのｎ次元角度センサ１で取得した角度航跡の対が作る交会角（０°〜９０°の場合）に概ね反比例する。このため、交会角が小さな場合、目標との距離に大きな誤差を含むことになり、これらを用いて算出される、目標のｎ＋１次元空間での位置及び速度等の追尾維持処理の初期値にも、その誤差の影響が及ぶ。しかしながら、拡張カルマンフィルタでは、初期値が真値から大きく外れている場合、推定値が収束しないケースがある。この結果、交会角が小さな場合、追尾維持処理の初期値が真値から大きく外れるケースが多くなり、拡張カルマンフィルタによる追尾維持処理において推定値が収束しにくくなって正常な追尾維持が困難となり、追尾安定性が低くなるという課題があった。
【００１９】
本発明は、上述の課題を解決するためになされたものであり、複数のｎ次元角度センサと目標との交会角が小さな場合でも、追尾安定性を向上させた目標追尾装置を提供することを目的とする。
【課題を解決するための手段】
【００２０】
上記目的を達成するために、第１の発明の目標追尾装置は、互いに位置の異なる複数の角度センサにより探知された複数の目標のｎ次元（ｎは１または２）角度情報を同一の目標毎に対をなすように関連づける相関手段と、前記相関手段により同一の目標に関連づけられた角度情報と、この目標に対して設定されたｍ個（ｍは１以上）の仮説距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｍ個のｎ＋１次元航跡とその誤差評価量とを算出する第１の初期値算出手段と、前記第１の初期値算出手段によって算出されたｍ個のｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第１の追尾維持手段と、前記相関手段により同一の目標に関連づけられた角度情報と、これら角度情報の対を用いて交会法に基づき算出した前記複数の各角度センサとこの目標までの距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｎ＋１次元航跡とその誤差評価量とを算出する第２の初期値算出手段と、前記第２の初期値算出手段によって算出されたｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第２の追尾維持手段と、前記第１の追尾維持手段、及び前記第２の追尾維持手段の事後確率を算出する事後確率算出手段と、前記第１の追尾維持手段、及び前記第２の追尾維持手段のそれぞれにより算出された前記目標のｎ＋１次元航跡を統合する目標航跡統合手段とを備え、前記目標航跡統合手段は、前記事後確率算出手段により算出された第１及び第２の追尾維持手段の事後確率に基づいて、それぞれの追尾維持手段により算出された前記目標のｎ＋１次元航跡を統合することを特徴とする。
【００２１】
また、第２の発明の目標追尾装置は、互いに位置の異なる複数の角度センサにより探知された複数の目標のｎ次元（ｎは１または２）角度情報を同一の目標毎に対をなすように関連づける相関手段と、前記相関手段により同一の目標に関連づけられた角度情報と、この目標に対して設定されたｍ個（ｍは１以上）の仮説距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｍ個のｎ＋１次元航跡とその誤差評価量とを算出する第１の初期値算出手段と、前記第１の初期値算出手段によって算出されたｍ個のｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第１の追尾維持手段と、前記相関手段により同一の目標に関連づけられた角度情報と、これら角度情報の対を用いて交会法に基づき算出した前記複数の各角度センサとこの目標までの距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｎ＋１次元航跡とその誤差評価量とを算出する第２の初期値算出手段と、前記第２の初期値算出手段によって算出されたｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第２の追尾維持手段と、前記複数の角度センサと前記目標との交会角に基づいて前記第１の追尾維持手段または前記第２の追尾維持手段のいずれかを選択的に動作させる追尾事前選択手段とを備え、前記追尾事前選択手段は、前記交会角が所定のしきい値未満の場合には、前記第１の追尾維持手段を動作させるとともに、この第１の追尾維持手段により算出された前記目標のｎ＋１次元航跡を採用し、前記交会角がこの所定のしきい値以上の場合には、前記第２の追尾維持手段を動作させるとともに、この第２の追尾維持手段により算出された前記目標のｎ＋１次元航跡を採用することを特徴とする。
【００２２】
また、第３の発明の目標追尾装置は、互いに位置の異なる複数の角度センサにより探知された複数の目標のｎ次元（ｎは１または２）角度情報を同一の目標毎に対をなすように関連づける相関手段と、前記相関手段により同一の目標に関連づけられた角度情報と、この目標に対して設定されたｍ個（ｍは１以上）の仮説距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｍ個のｎ＋１次元航跡とその誤差評価量とを算出する第１の初期値算出手段と、前記第１の初期値算出手段によって算出されたｍ個のｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第１の追尾維持手段と、前記相関手段により同一の目標に関連づけられた角度情報と、これら角度情報の対を用いて交会法に基づき算出した前記複数の各角度センサとこの目標までの距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｎ＋１次元航跡とその誤差評価量とを算出する第２の初期値算出手段と、前記第２の初期値算出手段によって算出されたｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第２の追尾維持手段と、前記第１の追尾維持手段、及び前記第２の追尾維持手段の事後確率を算出する事後確率算出手段と、前記事後確率算出手段からの事後確率に基づいて、前記第１の追尾維持手段、及び前記第２の追尾維持手段のそれぞれにより算出された前記目標のｎ＋１次元航跡を統合する目標航跡統合手段と、前記複数の角度センサと前記目標との交会角に基づいて前記第１の追尾維持手段または前記第２の追尾維持手段のいずれか一方または両者を選択して動作させる追尾事前選択手段とを備え、前記追尾事前選択手段は、前記交会角が第１のしきい値未満の場合には、前記第１の追尾維持手段を動作させるとともに、この第１の追尾維持手段により算出された前記目標のｎ＋１次元航跡を採用し、前記交会角が第１のしきい値以上かつ第２のしきい値（第２のしきい値＞第１のしきい値）未満の場合には、前記第１の追尾維持手段及び前記第２の追尾維持手段の両者を動作させるとともに、前記目標航跡統合手段により統合された前記目標のｎ＋１次元航跡を採用し、前記交会角が第２のしきい値以上の場合には、前記第２の追尾維持手段を動作させるとともに、この第２の追尾維持手段により算出された前記目標のｎ＋１次元航跡を採用することを特徴とする。
【００２３】
また、第４の発明の目標追尾装置は、互いに位置の異なる複数の角度センサにより探知された複数の目標のｎ次元（ｎは１または２）角度情報を同一の目標毎に対をなすように関連づける相関手段と、前記相関手段により同一の目標に関連づけられた角度情報と、この目標に対して設定されたｍ個（ｍは１以上）の仮説距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｍ個のｎ＋１次元航跡とその誤差評価量とを算出する第１の初期値算出手段と、前記第１の初期値算出手段によって算出されたｍ個のｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第１の追尾維持手段と、前記相関手段により同一の目標に関連づけられた角度情報と、これら角度情報の対を用いて交会法に基づき算出した前記複数の各角度センサとこの目標までの距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｎ＋１次元航跡とその誤差評価量とを算出する第２の初期値算出手段と、前記第２の初期値算出手段によって算出されたｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第２の追尾維持手段と、前記第１の追尾維持手段、及び前記第２の追尾維持手段の事後確率を算出する事後確率算出手段と、前記事後確率算出手段からの事後確率に基づいて、前記第１の追尾維持手段、及び前記第２の追尾維持手段のそれぞれにより算出された前記目標のｎ＋１次元航跡を統合する目標航跡統合手段と、前記事後確率算出手段からの事後確率に基づいて前記第１の追尾維持手段、前記第２の追尾維持手段、または前記目標航跡統合手段のいずれかにより算出された前記目標のｎ＋１次元航跡を選択して採用する追尾事後選択手段とを備え、前記追尾事後選択手段は、所定の観測回数または観測時間が経過するまでは、前記目標航跡統合手段により統合された前記目標のｎ＋１次元航跡を採用し、その後は、この所定の観測回数または観測時間が経過した時点で算出した、前記第１の追尾維持手段及び前記第２の追尾維持手段の事後確率を参照し、その値の高いいずれか一方の追尾維持手段により算出された前記目標のｎ＋１次元航跡を採用することを特徴とする。
【００２４】
また、第５の発明の目標追尾装置は、互いに位置の異なる複数の角度センサにより探知された複数の目標のｎ次元（ｎは１または２）角度情報を同一の目標毎に対をなすように関連づける相関手段と、前記相関手段により同一の目標に関連づけられた角度情報と、この目標に対して設定されたｍ個（ｍは１以上）の仮説距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｍ個のｎ＋１次元航跡とその誤差評価量とを算出する第１の初期値算出手段と、前記第１の初期値算出手段によって算出されたｍ個のｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第１の追尾維持手段と、前記相関手段により同一の目標に関連づけられた角度情報と、これら角度情報の対を用いて交会法に基づき算出した前記複数の各角度センサとこの目標までの距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｎ＋１次元航跡とその誤差評価量とを算出する第２の初期値算出手段と、前記第２の初期値算出手段によって算出されたｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第２の追尾維持手段と、前記第１の追尾維持手段、及び前記第２の追尾維持手段の事後確率を算出する事後確率算出手段と、前記事後確率算出手段からの事後確率に基づいて、前記第１の追尾維持手段、及び前記第２の追尾維持手段のそれぞれにより算出された前記目標のｎ＋１次元航跡を統合する目標航跡統合手段と、前記複数の角度センサと前記目標との交会角に基づいて前記第１の追尾維持手段または前記第２の追尾維持手段のいずれか一方または両者を選択して動作させる追尾事前選択手段と、前記事後確率算出手段からの事後確率に基づいて前記第１の追尾維持手段、前記第２の追尾維持手段、または前記目標航跡統合手段のいずれかにより算出された前記目標のｎ＋１次元航跡を選択して採用する追尾事後選択手段とを備え、前記追尾事後選択手段は、所定の観測回数または観測時間が経過するまでは、前記目標航跡統合手段により統合された前記目標のｎ＋１次元航跡を採用し、その後は、この所定の観測回数または観測時間が経過した時点で算出した、前記第１の追尾維持手段及び前記第２の追尾維持手段の事後確率を参照し、その値の高いいずれか一方の追尾維持手段により算出された前記目標のｎ＋１次元航跡を採用し、前記追尾事前選択手段は、前記交会角が第１のしきい値未満の場合には、前記第１の追尾維持手段を動作させるとともに、この第１の追尾維持手段により算出された前記目標のｎ＋１次元航跡を採用し、前記交会角が第１のしきい値以上かつ第２のしきい値（第２のしきい値＞第１のしきい値）未満の場合には、前記第１の追尾維持手段及び前記第２の追尾維持手段の両者を動作させるとともに、前記追尾事後選択手段による前記目標のｎ＋１次元航跡を採用し、前記交会角が第２のしきい値以上の場合には、前記第２の追尾維持手段を動作させるとともに、この第２の追尾維持手段により算出された前記目標のｎ＋１次元航跡を採用することを特徴とする。
【発明の効果】
【００２５】
本発明によれば、交会角が小さな場合でも、良好な追尾安定性を有する目標追尾装置を得ることができる。
【図面の簡単な説明】
【００２６】
【図１】本発明に係る目標追尾装置の第１の実施例の構成を示すブロック図。
【図２】図１に例示した目標追尾装置の動作を説明するための第１の部分のフローチャート。
【図３】図１に例示した目標追尾装置の動作を説明するための第２の部分のフローチャート。
【図４】航跡統合部３ａ内主要部の動作タイミングを時間経過に沿ってモデル化して例示する説明図。
【図５】図１の目標追尾装置の変形例の構成を示すブロック図。
【図６】本発明に係る目標追尾装置の第２の実施例の構成を示すブロック図。
【図７】図６に例示した目標追尾装置の動作を説明するための第１の部分のフローチャート。
【図８】図６に例示した目標追尾装置の動作を説明するための第２の部分のフローチャート。
【図９】航跡統合部３ｂ内主要部の動作タイミングを時間経過に沿ってモデル化して例示する説明図。
【図１０】本発明に係る目標追尾装置の第３の実施例の構成を示すブロック図。
【図１１】図１０に例示した目標追尾装置の動作を説明するための第１の部分のフローチャート。
【図１２】図１０に例示した目標追尾装置の動作を説明するための第２の部分のフローチャート。
【図１３】本発明に係る目標追尾装置の第４の実施例の構成を示すブロック図。
【図１４】図１３に例示した目標追尾装置の動作を説明するための第１の部分のフローチャート。
【図１５】図１３に例示した目標追尾装置の動作を説明するための第２の部分のフローチャート。
【図１６】航跡統合部３ｄ内主要部の動作タイミングを時間経過に沿ってモデル化して例示する説明図。
【図１７】本発明に係る目標追尾装置の第５の実施例の構成を示すブロック図。
【図１８】図１７に例示した目標追尾装置の動作を説明するための第１の部分のフローチャート。
【図１９】図１７に例示した目標追尾装置の動作を説明するための第２の部分のフローチャート。
【図２０】従来の目標追尾装置の構成の一例を示すブロック図。
【図２１】図１４における航跡統合部３の処理の流れの一例を示すフローチャート。
【発明を実施するための形態】
【００２７】
以下に、本発明に係る目標追尾装置を実施するための最良の形態について、図１乃至図１９を参照して説明する。なお、以下の説明においては、背景技術の欄で説明した従来の目標追尾装置の各構成に相当する部分には、同一の符号を用いて説明する。
【実施例１】
【００２８】
図１は、本発明に係る目標追尾装置の第１の実施例の構成を示すブロック図である。図１に例示したように、この目標追尾装置は、２つのｎ次元（ｎは１または２）角度センサを含み構成された事例であり、同一に構成された第１のｎ次元角度センサ１（＃１）、及び第２のｎ次元角度センサ１（＃２）、同一に構成された第１の角度追尾部２（＃１）、及び第２の角度追尾部２（＃２）、航跡統合部３ａ、ならびに第１の航跡表示部４（＃１）、及び第２の航跡表示部４（＃２）から構成されている。
【００２９】
第１のｎ次元角度センサ１（＃１）及び第２のｎ次元角度センサ１（＃２）は、探知した目標の角度情報を取得し、それぞれ対応する角度追尾部２（＃１）または２（＃２）、ならびに航跡統合部３ａに送出する。第１の角度追尾部２（＃１）及び第２の角度追尾部２（＃２）は、それぞれに対応するｎ次元角度センサ１からの角度情報に基づいて、第１の角度追尾部２（＃１）は、目標の角度航跡ｚ（ｋ，１）及びその誤差共分散行列Ｐｚ（ｋ，１）を、また第２の角度追尾部２（＃２）は、目標の角度航跡ｚ（ｋ，２）及びその誤差共分散行列Ｐｚ（ｋ，２）を算出し、航跡統合部３ａに送出する。
【００３０】
航跡統合部３ａは、後述するが、追尾フィルタの異なる２つの航跡維持部を有しており、角度追尾部２（＃１）及び２（＃２）での算出結果を受けて、同一の目標に対してそれぞれの航跡維持部で算出した目標の航跡を統合し、第１の航跡表示部４（＃１）及び第２の航跡表示部４（＃２）に送出する。ここに、航跡統合部３ａは、相関部３１ａ、仮説距離生成部３５、航跡初期値算出部（１）３６、航跡維持部（１）３７、距離算出部３２ａ、航跡初期値算出部（２）３３ａ、航跡維持部（２）３４ａ、事後確率算出部３８、及び統合航跡算出部３９から構成されている。
【００３１】
相関部３１ａは、２つの角度追尾部２（＃１）及び２（＃２）からの角度航跡を入力し、それぞれで取得した角度航跡の関連づけを行ってどの対が同一目標のものであるかを判定する相関処理を行い、同一目標の角度航跡の対を後段に送出する。また、同一目標の角度航跡の対から、新目標の出現についても判定する。なお、この相関部３１ａでの処理には、例えば、特許文献１に記載の手法等を適用することができる。
【００３２】
仮説距離生成部３５は、相関部３１ａからの角度航跡に基づいて、２つのｎ次元角度センサ１（＃１）及び１（＃２）から目標までの仮説距離を算出し、航跡初期値算出部（１）３６に送出する。仮説距離の算出にあたっては、例えば、非特許文献２等に詳述される手法を適用できる。この手法の主要な処理部分を、以下の式（１７）〜式（１９）に示す。
【数１７】
【数１８】
【数１９】
【００３３】
ここで、ｒｍｉｎは目標の最小距離、ｒｍａｘは目標の最大距離、ｍは仮説距離の数（＝サブフィルタの数ｍ、ｍは１以上）、ｒ（ｊ）（ｊ＝１〜ｍ）はｍ個の仮説距離、σｒ（ｊ）（ｊ＝１〜ｍ）は距離誤差の標準偏差である。
【００３４】
航跡初期値算出部（１）３６は、これら仮説距離をもとに、後段の航跡維持部（１）３７でこの目標の追尾に必要な初期値となる、目標のｎ＋１次元航跡とその誤差評価量としての誤差共分散行列を算出し、航跡維持部（１）３７に送出する。これら初期値となるｎ＋１次元航跡とその誤差評価量の算出にあたっては、例えば、非特許文献２等に詳述される手法を適用できる。この手法の主要な処理部分については、例えば、航跡維持部（１）３７が２次元航跡を維持する場合、航跡としてのｊ番目（ｊ＝１〜ｍ）のサブフィルタの平滑ベクトルｘ＾（ｋ｜ｋ，ｊ）と平滑誤差共分散行列Ｐ（ｋ｜ｋ，ｊ）は、以下の式（２０）〜式（２１）で表される。
【数２０】
【数２１】
【００３５】
ここで、ｘ＾（ｋ，ｉ）は、ｎ次元角度センサ１（＃ｉ）の状態ベクトル、ｆｐ２ｃは、共分散行列の極座標・直交座標変換関数、σｖは、速度誤差の標準偏差を示す。
【００３６】
航跡維持部（１）３７は、航跡初期値算出部（１）３６での算出結果を初期値として、２つのｎ次元角度センサ１（＃１）及び１（＃２）からの探知情報に基づき目標の追尾維持処理を実行し、その航跡を継続的に取得する。ここでは、目標の運動モデル及び観測モデルを、それぞれ式（２２）及び式（２３）として、目標の追尾維持処理を、レンジパラメタライズド拡張カルマンフィルタにより構成した場合について説明する。
【数２２】
【数２３】
【００３７】
ここで、ｘ（ｋ）は、観測時刻ｔｋにおける目標の状態ベクトル、Ｆ（ｋ，ｋ−１）とＧ（ｋ，ｋ−１）は、観測時刻ｔｋ−１からｔｋにおける遷移行列と駆動行列、ｗ（ｋ−１）は、観測時刻ｔｋ−１における平均０、共分散行列Ｑ（ｋ−１）の正規分布に従うシステム雑音ベクトル、ｙ（ｋ，ｉ）は、観測時刻ｔｋにおけるｎ次元角度センサ１（＃ｉ）の観測ベクトル、ｈ（・）は、観測関数、ｖ（ｋ，ｉ）は、観測時刻ｔｋにおける平均０、共分散行列Ｒ（ｋ，ｉ）の正規分布に従う観測雑音ベクトルである。
【００３８】
航跡維持部（１）３７をレンジパラメタライズド拡張カルマンフィルタで構成した場合、ｍ個のサブフィルタを構成してこれらによる処理が行なわれる。ｊ番目（ｊ＝１〜ｍ）のサブフィルタでは、式（２４）〜式（２５）に示す予測処理、及び式（２６）〜式（３０）に示す平滑処理を実施する。
【数２４】
【数２５】
【数２６】
【数２７】
【数２８】
【数２９】
【数３０】
【００３９】
ここで、ｘ＾（ｋ｜ｋ−１，ｊ）とＰ（ｋ｜ｋ−１，ｊ）は、予測ベクトルと予測誤差共分散行列、Ｓ（ｋ，ｊ）、ｄ（ｋ，ｊ）およびＫ（ｋ，ｊ）は、残差共分散行列、マハラノビス距離およびカルマンゲイン行列、ｘ＾（ｋ｜ｋ，ｊ）とＰ（ｋ｜ｋ，ｊ）は、平滑ベクトルと平滑誤差共分散行列である（全て、観測時刻ｔｋのｊ番目のサブフィルタにおける値）。
【００４０】
観測時刻ｔｋにおける平滑ベクトルｘ＾（ｋ｜ｋ）と平滑誤差共分散行列Ｐ（ｋ｜ｋ）は、以下の式で算出される。
【数３１】
【数３２】
【数３３】
【数３４】
【００４１】
ここで、ｌ（ｋ，ｊ）は尤度、ｐ（ｋ，ｊ）とｗ（ｋ，ｊ）は、事後確率と重みである（全て、観測時刻ｔｋのｊ番目のサブフィルタにおける値）。
【００４２】
なお、上記した事例では、航跡維持部（１）３７をレンジパラメタライズド拡張カルマンフィルタで構成した場合を詳述したが、航跡維持部（１）３７は、レンジパラメタライズド・アンセンテッドカルマンフィルタやレンジパラメタライズド・パーティクルフィルタで構成しても良い。また、航跡維持部（１）３７がレンジパラメタライズド拡張カルマンフィルタまたはレンジパラメタライズド・アンセンテッドカルマンフィルタで構成される場合、航跡維持部（１）３７は、ｍ個のサブフィルタにより算出・更新されるｍ個の目標のｎ＋１次元航跡の中から、所定の観測回数または観測時間が経過した時点で最も事後確率の高い１つを選択し、以降はこの選択したサブフィルタによる目標のｎ＋１次元航跡を出力するように構成しても良い。
【００４３】
距離算出部３２ａは、相関部３１ａからの角度航跡に基づいて、２つのｎ次元角度センサ１（＃１）及び１（＃２）から目標までのそれぞれの距離を算出し、航跡初期値算出部（２）３３ａに送出する。距離の算出にあたっては、例えば、非特許文献１に記載の手法等を適用することができる。
【００４４】
航跡初期値算出部（２）３３ａは、目標の角度航跡及び距離の算出結果をもとに、後段の航跡維持部（２）３４ａでこの目標の追尾に必要な初期値となる、この目標のｎ＋１次元航跡とその誤差評価量としての誤差共分散行列を算出する。算出にあたっては、距離算出部３２ａにより算出された距離と各ｎ次元角度センサ１との角度航跡から、位置のみを含み速度を含まないｎ＋１次元航跡を算出し、複数回（２回以上）のｎ＋１次元航跡とその誤差評価量から、これら航跡初期値を算出することができる。また、距離算出部３２ａにより算出された距離と各ｎ次元角度センサ１との角度航跡から、位置のみを含み速度を含まないｎ＋１次元航跡を算出し、前述したレンジパラメタライズド拡張カルマンフィルタの初期値のように、航跡初期速度を０として、航跡初期値を算出するように構成することができる。なお、上述した距離算出部３２ａ及び航跡初期値算出部（２）３３ａにおける各値の算出手法の代わりに、特許文献１に記載の手法等を適用することができる。
【００４５】
航跡維持部（２）３４ａは、航跡初期値算出部（２）３３ａでの算出結果を初期値として、２つのｎ次元角度センサ１（＃１）及び１（＃２）からの探知情報に基づき目標の追尾維持処理を実行し、その航跡を継続的に取得する。この航跡維持部（２）３４ａで実行される追尾維持処理は、図２０に例示した従来の目標追尾装置における航跡維持部３４と同じ内容であり、説明を省略する。なお、拡張カルマンフィルタの代わりに、アンセンテッドカルマンフィルタやパーティクルフィルタで構成しても良い。
【００４６】
事後確率算出部３８は、航跡維持部（１）３７、及び航跡維持部（２）３４ａから目標のｎ＋１次元航跡を受けとり、それぞれからのｎ＋１次元航跡に対する事後確率及び重みを算出する。事後確率及び重みの算出は、以下の式（３５）〜式（３８）に示した処理手法による。
【数３５】
【数３６】
【数３７】
【数３８】
【００４７】
ここで、ｌ（ｋ，ＲＰ）、ｐ（ｋ，ＲＰ）及びｗ（ｋ，ＲＰ）は、それぞれ、航跡維持部（１）３７の尤度、事後確率及び重み、ｌ（ｋ，ＥＫＦ）、ｐ（ｋ，ＥＫＦ）及びｗ（ｋ，ＥＫＦ）は、航跡維持部（２）３４ａの尤度、事後確率および重みである（全て、観測時刻ｔｋにおける値）。また、Ｓ（ｋ，ＥＫＦ）とｄ（ｋ，ＥＫＦ）は、それぞれ式（５）と式（６）との値を用い、Ｓ（ｋ，ＲＰ）及びｄ（ｋ，ＲＰ）は、以下の式（３９）〜式（４２）で算出する。
【数３９】
【数４０】
【数４１】
【数４２】
【００４８】
統合航跡算出部３９は、航跡維持部（１）３７、及び航跡維持部（２）３４ａのそれぞれで取得された目標のｎ＋１次元航跡を、事後確率算出部３８で算出されたそれぞれの事後確率及び重みに基づき統合し、統合航跡を算出する。統合航跡の算出にあたっては、事後確率に基づく重み付けにより、以下の式（４３）〜式（４４）を用いて算出する。
【数４３】
【数４４】
【００４９】
ここで、ｘ＾（ｋ，ＲＰ）とＰ（ｋ，ＲＰ）は、式（３３）と式（３４）の値を用い、ｘ＾（ｋ，ＥＫＦ）とＰ（ｋ，ＥＫＦ）は、式（８）と式（９）の値を用いる。また、事後確率により重み付けせずに、航跡維持部（１）３７、または航跡維持部（２）３４ａのそれぞれで取得された目標のｎ＋１次元航跡のうち、事後確率の高いいずれか一方を選択してこれを統合航跡とするように構成することもできる。
【００５０】
航跡表示部４（＃１）及び航跡表示部４（＃２）は、このようにして航跡統合部３ａで算出された目標の統合航跡を、それぞれｎ次元角度センサ１（＃１）及び１（＃２）を基準にして表示する。
【００５１】
次に、前出の図１、ならびに図２〜図３のフローチャート及び図４の説明図を参照して、上述のように構成された本実施例の目標追尾装置の動作について、航跡統合部３ａにおける処理の流れを中心に説明する。図２及び図３は、図１に例示した目標追尾装置の第１の実施例の動作を説明するためのフローチャートである。
【００５２】
まず、航跡統合部３ａは、２つの角度追尾部２（＃１）及び２（＃２）から新たな目標の角度航跡、すなわち航跡の初期値を算出済みでない角度航跡を受けとると（ＳＴ１０１のＮ）、相関部３１ａがこれら角度航跡を入力し（ＳＴ１０２）、次いで、追尾維持処理に必要な一連の初期値算出処理を行う（ＳＴ１０３）。このＳＴ１０３での初期値算出では、最初に２つの角度追尾部２（＃１）及び２（＃２）からの角度航跡間のどの対が同一目標であるかを判定する相関処理を行ない（ＳＴ１０７）、その結果、同一目標と判定された新たな角度航跡の対がある場合には（ＳＴ１０８のＹ）、後段の航跡維持部（１）３７、及び航跡維持部（２）３４ａでのそれぞれの追尾維持処理における初期値を、同時並行的に算出する。（ＳＴ１０９）。
【００５３】
すなわち、相関部３１ａからの新たな角度航跡の対に基づいて、仮説距離生成部３５においては、２つのｎ次元角度センサ１（＃１）及び１（＃２）から目標までの仮説距離を算出し（ＳＴ１１４）、さらに、これら仮説距離をもとに、航跡初期値算出部（１）３６において、航跡維持部（１）３７での追尾維持処理の初期値となる、この目標のｎ＋１次元航跡とその誤差評価量としての誤差共分散行列を算出し、航跡維持部（１）３７に送出する（ＳＴ１１５）。一方、これと並行して距離算出部３２ａでは、２つのｎ次元角度センサ１（＃１）及び１（＃２）から目標までのそれぞれの距離を算出し（ＳＴ１１６）、さらに、この算出結果をもとに、航跡初期値算出部（２）３３ａにおいて、航跡維持部（２）３４ａでの追尾維持処理の初期値となる、目標のｎ＋１次元航跡とその誤差評価量としての誤差共分散行列を算出し、航跡維持部（２）３４ａに送出する（ＳＴ１１７）。
【００５４】
次いで、再びＳＴ１０１のステップにより、対象目標の航跡の初期値が算出済である旨の判定がなされると（ＳＴ１０１のＹ）、航跡維持部（１）３７、及び航跡維持部（２）３４ａは、追尾維持処理に移行すべく、２つのｎ次元角度センサ１（＃１）及び１（＃２）で探知した角度情報を直接入力し始める（ＳＴ１０４）。そして、ＳＴ１０３のステップで算出済みの初期値に基づいて、それぞれの追尾維持処理を同時進行させながら目標のｎ＋１次元航跡の取得を維持継続し、さらに、これら２つの航跡維持部からの航跡を事後確率に基づいて統合し、統合航跡を得ている（ＳＴ１０５）。
【００５５】
すなわち、ＳＴ１０３のステップで算出済みの初期値、及び２つのｎ次元角度センサ１（＃１）及び１（＃２）からの角度情報に基づいて、航跡維持部（１）３７は、上述した手法により追尾維持処理を実行して目標のｎ＋１次元航跡を継続的に取得し、その結果を事後確率算出部３８、及び統合航跡算出部３９に送出する（ＳＴ１１０）。一方、これと並行して航跡維持部（２）３４ａも、自身の追尾維持処理を実行して目標のｎ＋１次元航跡を継続的に取得し、その結果を事後確率算出部３８、及び統合航跡算出部３９に送出する（ＳＴ１１１）。次いで、事後確率算出部３８は、これら２つのｎ＋１次元航跡のそれぞれについて事後確率を算出し、統合航跡算出部３９に送出すると（ＳＴ１１２）、統合航跡算出部３９では、事後確率を用いた重み付け、または事後確率の大小関係により２つのｎ＋１次元航跡を統合して、目標の統合航跡を得る。そして、その結果を航跡表示部４に送出し、航跡表示部４は、これを表示する（ＳＴ１１３）。この後は、動作終了が指示されるまで、上記した動作ステップを繰り返す（ＳＴ１０６）。
【００５６】
図４は、上記した航跡統合部３ａ内での処理の流れに対応した航跡統合部３ａ内主要部の動作タイミングの一例を、時間経過に沿ってモデル化して示す説明図である。相関部３１ａにより、同一目標の角度航跡の対から新たな目標と判定した角度航跡の対が抽出されると（Ｔ１１）、航跡初期値算出部（１）３６では、仮説距離生成部３５からの仮説距離を用いて、航跡維持部（１）３７での追尾維持処理に必要な航跡初期値として、目標のｎ＋１次元航跡とその誤差評価量が算出される（Ｔ１２）。同様に、航跡初期値算出部（２）３３ａでは、距離算出部３２ａで算出された距離を用いて、航跡維持部（２）３４ａでの追尾維持処理に必要な目標の航跡の初期値が算出される（Ｔ１３）。これらの初期値をもとに、航跡維持部（１）３７、及び航跡維持部（２）３４ａは、目標の追尾維持処理に移行し、それぞれの追尾維持処理が開始されて、目標のｎ＋１次元航跡が継続して取得される。この時に、航跡維持部（１）３７の追尾維持処理と航跡維持部（２）３４ａの追尾維持処理は、同時並行して進行する（Ｔ１４、及びＴ１５）。さらに、これらと並行して動作する統合航跡算出部３９では、航跡維持部（１）３７、及び航跡維持部（２）３４ａからの目標航跡が、事後確率算出部３８で算出した事後確率に基づいて統合され、統合航跡として航跡表示部４に送出される（Ｔ１６）。
【００５７】
なお、上記した実施例においては、距離算出部３２ａにおいて距離変化率を算出する必要がないため、２つの角度追尾部２（＃１）及び２（＃２）において各速度を算出する必要がなく、この結果、２つの角度追尾部２（＃１）及び２（＃２）を省略することができる。この時の目標追尾装置の構成のブロック図を図５に例示する。
【００５８】
以上説明したように、本実施例においては、２つのｎ次元角度センサからの目標の角度情報に対して、相関手段によって同一目標と判定された角度情報とｍ個（ｍは、１以上）の仮説距離に基づき、目標のｎ＋１次元空間での位置および速度よりなるｍ個のｎ＋１次元航跡とその誤差評価量を算出し、これを初期値として、２つのｎ次元角度センサのいずれかよりの新たな探知情報が得られるたびに、この探知情報を反映してｎ＋１次元航跡およびその誤差評価量を更新して、目標のｎ＋１次元追尾を行なう第１の追尾維持手段としての航跡維持部（１）と、相関手段によって同一目標と判定された角度情報から、交会法に基づいて目標の距離を算出し、この距離と２つのｎ次元角度センサの角度航跡とに基づき、目標のｎ＋１次元空間での位置および速度よりなるｎ＋１次元航跡とその誤差評価量を算出し、これを初期値として、２つのｎ次元角度センサのいずれかよりの新たな探知情報が得られるたびに、この探知情報を反映してｎ＋１次元航跡およびその誤差評価量を更新して、目標のｎ＋１次元追尾を行なう第２の追尾維持手段としての航跡維持部（２）を備え、さらに、これら２つの航跡維持部で取得された目標航跡を事後確率により統合／選択している。
【００５９】
これにより、２つの航跡維持部（１）及び（２）が目標の追尾維持処理に移行する際に、その初期値に仮説距離を用いて処理を開始し継続取得した目標航跡と、交会法により算出された距離を用いて処理を開始し継続取得した目標航跡とが統合されるので、目標に対する２つのｎ次元角度センサからの交会角が小さな場合でも、追尾安定性を向上することができる。
【実施例２】
【００６０】
図６は、本発明に係る目標追尾装置の第２の実施例の構成を示すブロック図である。また、図７〜図９は、この目標追尾装置の第２の実施例の動作を説明するためのフローチャート及び説明図である。この第２の実施例について、図１〜図５に示した第１の実施例の各部と同一の部分は同一の符号で示し、その説明は省略する。第２の実施例が第１の実施例と異なる点は、第１の実施例においては、相関部において新しい目標と判定した角度航跡の対に対して、２つの追尾維持処理、すなわち、航跡初期値の算出に目標との仮説距離を用いた、航跡維持部（１）による第１の追尾維持処理と、航跡初期値の算出に交会法による算出距離を用いた、航跡維持部（２）による第２の追尾維持処理とを、同時並行に進行させたのに対し、第２の実施例においては、相関部おいて新しい目標と判定した角度航跡の対に対して、２つのｎ次元角度センサによる交会角を算出し、この交会角の値の範囲によって、上記した２つの追尾維持処理のいずれか一方を選択してその後の目標の航跡を取得するようにした点である。以下、前出の図１〜図５、ならびに図６〜図９を参照して、その相違点のみを説明する。
【００６１】
図６に例示したように、この目標追尾装置は、同一に構成された第１のｎ次元角度センサ１（＃１）、及び第２のｎ次元角度センサ１（＃２）、同一に構成された第１の角度追尾部２（＃１）、及び第２の角度追尾部２（＃２）、航跡統合部３ｂ、ならびに第１の航跡表示部４（＃１）、及び第２の航跡表示部４（＃２）から構成されている。２つのｎ次元角度センサ１（＃１）及び１（＃２）、２つの角度追尾部２（＃１）及び２（＃２）、ならびに、２つの航跡表示部４（＃１）及び４（＃２）は、第１の実施例と同じであり、説明を省略する。また、航跡統合部３ｂは、相関部３１ａ、事前選択部４０、仮説距離生成部３５、航跡初期値算出部（１）３６、航跡維持部（１）３７、距離算出部３２ａ、航跡初期値算出部（２）３３ａ、及び航跡維持部（２）３４ａから構成されている。ここに、事前選択部４０を除く各構成は、第１の実施例と同じであり、説明を省略する。
【００６２】
事前選択部４０は、相関部３１ａにおいて新目標と判定した角度航跡の対に基づいて、２つのｎ次元角度センサ１（＃１）及び１（＃２）による目標の交会角を算出するとともに、この算出結果をあらかじめ設定されたしきい値と比較し、算出した交会角がしきい値未満の場合、すなわちこの場合は交会角が小さく、交会法により算出した目標との距離に大きな誤差が含まれるおそれがあるため、仮説距離生成部３５からの仮説距離に基づき航跡初期値算出部（１）３６で算出した初期値を用いた、航跡維持部（１）３７での追尾維持処理により、その後の目標のｎ＋１次元航跡を継続して取得するように選択制御を行う。一方、算出した交会角がしきい値以上の場合には、距離算出部３２ａからの交会法による算出距離に基づき航跡初期値算出部（２）３３ａで算出した初期値を用いた、航跡維持部（２）３４ａでの追尾維持処理により目標の航跡を継続して取得するように選択制御を行う。交会角の算出にあたっては、例えば、２つの角度追尾部２（＃１）及び２（＃２）からの目標の角度航跡、または２つのｎ次元角度センサ１（＃１）及び１（＃２）からの探知情報を、方向単位ベクトルｕ（ｋ，１）とｕ（ｋ，２）に変換し、２つの方向単位ベクトルの内積の逆余弦から算出する手法等を適用できる。なお、交会角としきい値の代わりに、交会角の余弦成分（すなわち、方向単位ベクトルの内積）としきい値の余弦成分を用いるように構成することもできる。
【００６３】
次に、図７及び図８のフローチャートを参照して、上述のように構成された第２の実施例の目標追尾装置の動作について、航跡統合部３ｂにおける処理の流れを中心に説明する。まず、航跡統合部３ｂは、２つの角度追尾部２（＃１）及び２（＃２）から航跡の初期値を算出済みでない角度航跡を受けとると（ＳＴ２０１のＮ）、相関部３１ａがこれら角度航跡を入力し（ＳＴ２０２）、次いで、追尾維持処理に必要な一連の初期値算出処理を行う（ＳＴ２０３）。
【００６４】
このＳＴ２０３での初期値算出では、最初に２つの角度追尾部２（＃１）及び２（＃２）からの角度航跡間のどの対が同一目標であるかを判定する相関処理を行ない（ＳＴ２０７）、その結果、同一目標と判定された新たな角度航跡の対がある場合には、これら角度航跡を事前選択部４０に送出する（ＳＴ２０８のＹ）。次いで、事前選択部４０は、この新たな目標に対する交会角を算出し、その算出結果をあらかじめ設定されたしきい値と比較する（ＳＴ２０９）。そして、その比較結果に基づいて、後段の追尾維持処理に必要な航跡初期値の算出を行う（ＳＴ２１０）。
【００６５】
すなわち、交会角がしきい値未満の場合には（ＳＴ２１４のしきい値未満）、角度航跡の対は仮説距離生成部３５に送られ、仮説距離生成部３５は、目標までの仮説距離を算出する（ＳＴ２１５）。さらに、この仮説距離と角度航跡とから、航跡初期値算出部（１）３６は、航跡初期値としての目標のｎ＋１次元航跡とその誤差評価量を算出して、航跡維持部（１）３７による以降の追尾維持処理に備える（ＳＴ２１６）。一方、交会角がしきい値以上の場合には（ＳＴ２１４のしきい値以上）、角度航跡の対は距離算出部３２ａに送られ、距離算出部３２ａは、交会法により目標との距離を算出する（ＳＴ２１７）。そして、航跡初期値算出部（２）３３ａにおいて航跡の初期値を算出し、航跡維持部（２）３４ａによる追尾維持処理に備える（ＳＴ２１８）。
【００６６】
次いで、再びＳＴ２０１のステップにより、対象目標の航跡の初期値が算出済みである旨の判定がなされると（ＳＴ２０１のＹ）、航跡維持部（１）３７、及び航跡維持部（２）３４ａは、追尾維持処理に移行すべく、２つのｎ次元角度センサ１（＃１）及び１（＃２）で探知した角度情報を直接入力し始める（ＳＴ２０４）。そして、ＳＴ２０９での事前選択の結果によって選択した、航跡維持部（１）３７、または航跡維持部（２）３４ａのどちらか一方の追尾維持処理で目標の航跡を継続取得して、追尾を維持する（ＳＴ２０５）。
【００６７】
すなわち、事前選択部４０において算出した交会角がしきい値未満の場合には（ＳＴ２１１のしきい値未満）、ＳＴ２１６のステップで算出した航跡初期値算出部（１）３６の算出結果をもとに、航跡維持部（１）３７が、２つのｎ次元角度センサ１（＃１）及び１（＃２）で探知した角度情報を入力して目標の航跡の取得を維持継続し、その結果を航跡表示部４に送出する（ＳＴ２１２）。一方、事前選択部４０において算出した交会角がしきい値以上の場合には（ＳＴ２１１のしきい値以上）、ＳＴ２１８のステップで算出した航跡初期値算出部（２）３３ａの算出結果をもとに、航跡維持部（２）３４ａが、２つのｎ次元角度センサ１（＃１）及び１（＃２）で探知した角度情報を入力して目標の航跡の取得を維持継続し、その結果を航跡表示部４に送出する（ＳＴ２１３）。この後は、動作終了が指示されるまで、上記した動作ステップを繰り返す（ＳＴ２０６）。
【００６８】
図９は、上記した航跡統合部３ｂ内での処理の流れに対応した航跡統合部３ｂ内主要部の動作タイミングの一例を、時間経過に沿ってモデル化して示す説明図である。相関部３１ａにより、同一目標の角度航跡の対から新たな目標と判定した角度航跡の対が抽出されると（Ｔ２１）、事前選択部４０により、交会角が算出され、その算出結果に基づいて以降の追尾維持処理として、航跡維持部（１）３７と航跡維持部（２）３４ａの何れか一方が選択される。事前選択部４０により、航跡維持部（１）３７による追尾維持処理が選択された場合、航跡初期値算出部（１）３６は、仮説距離生成部３５による距離算出結果に基づき航跡初期値を算出し（Ｔ２２）、航跡維持部（１）３７による追尾維持処理に移行する。一方、事前選択部４０により、航跡維持部（２）３４ａによる追尾維持処理が選択された場合、航跡初期値算出部（２）３３ａは、距離算出部３２ａによる距離算出結果に基づき航跡初期値を算出し（Ｔ２２）、航跡維持部（２）による追尾維持処理に移行する。そして、その後は、追尾終了まで、航跡維持部（１）３７と航跡維持部（２）３４ａの何れか一方の追尾維持処理が動作し、目標の航跡が継続取得される（Ｔ２３）。
【００６９】
以上説明したように、本実施例においては、相関部において新しい目標と判定した角度航跡の対に対して、事前選択部において２つのｎ次元角度センサによる交会角を算出し、この交会角の値の範囲によって、２つの追尾維持処理のいずれか一方を選択している。すなわち、算出した交会角がしきい値未満の場合、交会角がより小さいので、交会法により算出した目標との距離に、より大きな誤差が含まれるおそれがあるため、航跡初期値の算出に目標との仮説距離を用いた、航跡維持部（１）による第１の追尾維持処理を選択し、それ以外の場合は、航跡初期値の算出に交会法による算出距離を用いた、航跡維持部（２）による第２の追尾維持処理を選択して、その後の目標の航跡を継続取得している。これにより、第１の実施例と同様に追尾安定性の向上に加え、２つの追尾維持処理を同時並行に動作させず、どちらか一方を動作させることにより追尾維持しているので、装置の処理負荷を軽減することができる。
【実施例３】
【００７０】
図１０は、本発明に係る目標追尾装置の第３の実施例の構成を示すブロック図である。また、図１１〜図１２は、この目標追尾装置の第３の実施例の動作を説明するためのフローチャートである。この第３の実施例について、図１〜図９に示した第１の実施例及び第２の実施例の各部と同一の部分は同一の符号で示し、その説明は省略する。第３の実施例が第２の実施例と異なる点は、事前選択部において算出した目標の交会角に基づいて後段で目標の航跡を継続取得するための追尾維持処理を選択する際に、第２の実施例においては、２つの選択肢、すなわち、航跡初期値の算出に目標との仮説距離を用いた、航跡維持部（１）による第１の追尾維持処理と、航跡初期値の算出に交会法による算出距離を用いた、航跡維持部（２）による第２の追尾維持処理とを設け、いずれか一方を選択したのに対し、第３の実施例においては、さらに選択肢を増やし、これら２つの追尾維持処理を同時に選択して、第１の実施例で示したように、それぞれで取得した航跡を統合した統合航跡も選択できるようにしており、交会角の範囲に基づいて、３つの選択肢からいずれか１つを選択するようにした点である。以下、前出の図１〜９、ならびに図１０〜１２を参照して、その相違点のみを説明する。
【００７１】
図１０に例示したように、この目標追尾装置は、同一に構成された第１のｎ次元角度センサ１（＃１）、及び第２のｎ次元角度センサ１（＃２）、同一に構成された第１の角度追尾部２（＃１）、及び第２の角度追尾部２（＃２）、航跡統合部３ｃ、ならびに第１の航跡表示部４（＃１）、及び第２の航跡表示部４（＃２）から構成されている。２つのｎ次元角度センサ１（＃１）及び１（＃２）、２つの角度追尾部２（＃１）及び２（＃２）、ならびに、２つの航跡表示部４（＃１）及び４（＃２）は、前出の第１及び第２の実施例と同じであり、説明を省略する。
【００７２】
また、航跡統合部３ｃは、相関部３１ａ、事前選択部４０ａ、仮説距離生成部３５、航跡初期値算出部（１）３６、航跡維持部（１）３７、距離算出部３２ａ、航跡初期値算出部（２）３３ａ、航跡維持部（２）３４ａ、事後確率算出部３８、及び統合航跡算出部３９ａから構成されている。ここに、事前選択部４０ａ及び統合航跡算出部３９ａを除く各構成は、第１及び第２の実施例と同じであり、説明を省略する。
【００７３】
事前選択部４０ａは、相関部３１ａにおいて新目標と判定した角度航跡の対に基づいて、２つのｎ次元角度センサ１（＃１）及び１（＃２）による目標の交会角を算出するとともに、この算出結果をあらかじめ設定された２つのしきい値である、しきい値１及びしきい値２（しきい値２＞しきい値１）と比較し、その結果により、角度航跡の送出及び目標の航跡の継続取得について次のような選択制御を行う。すなわち、交会角が第１のしきい値未満（以下、第１の交会角範囲と表す）の場合、仮説距離生成部３５からの仮説距離に基づき航跡初期値算出部（１）３６で算出した初期値を用いた、航跡維持部（１）３７での追尾維持処理により、その後の目標のｎ＋１次元航跡を継続して取得するように選択制御を行う。また、交会角が第１のしきい値以上、第２のしきい値未満（以下、第２の交会角範囲と表す）の場合、上記した航跡維持部（１）３７での追尾維持処理と、距離算出部３２ａからの交会法による算出距離に基づき航跡初期値算出部（２）３３ａで算出した初期値を用いた、航跡維持部（２）３４ａでの追尾維持処理とを共に動作させ、これら２つの追尾維持処理により取得した航跡を統合して目標の航跡を継続取得するように選択制御を行う。また、交会角が第２のしきい値以上（以下、第３の交会角範囲と表す）の場合、上記した航跡維持部（２）３４ａでの追尾維持処理により、その後の目標の航跡を継続して取得するように選択制御を行う。
【００７４】
統合航跡算出部３９ａは、第１の実施例における統合航跡算出部３９と同様の処理機能に加え、事前選択部４０ａからの選択制御に基づいて、目標の航跡を、航跡維持部（１）３７での追尾維持処理による航跡、航跡維持部（２）３４ａでの追尾維持処理による航跡、または、これらを統合した航跡のいずれかから選択し、後段の航跡表示部４に送出する。
【００７５】
次に、図１１及び図１２のフローチャートを参照して、上述のように構成された第３の実施例の目標追尾装置の動作について、航跡統合部３ｃにおける処理の流れを中心に説明する。まず、航跡統合部３ｃは、２つの角度追尾部２（＃１）及び２（＃２）から航跡の初期値を算出済みでない角度航跡を受けとると（ＳＴ３０１のＮ）、相関部３１ａがこれら角度航跡を入力し（ＳＴ３０２）、次いで、追尾維持処理に必要な一連の初期値算出処理を行う（ＳＴ３０３）。
【００７６】
このＳＴ３０３での初期値算出では、最初に２つの角度追尾部２（＃１）及び２（＃２）からの角度航跡間のどの対が同一目標であるかを判定する相関処理を行ない（ＳＴ３０７）、その結果、同一目標と判定された新たな角度航跡の対がある場合には、これら角度航跡を事前選択部４０ａに送出する（ＳＴ３０８のＹ）。次いで、事前選択部４０ａは、この新たな目標に対する交会角を算出し、その算出結果をあらかじめ設定された２つのしきい値と比較する（ＳＴ３０９）。そして、その比較結果に基づいて、以下の３つの手順のいずれかにより、後段の追尾維持処理に必要な航跡初期値の算出を行う（ＳＴ３１０）。
【００７７】
すなわち、交会角が第１の交会角範囲の場合には（ＳＴ３１５の第１の交会角範囲）、角度航跡の対は仮説距離生成部３５に送られ、仮説距離生成部３５は、目標までの仮説距離を算出する（ＳＴ３１６）。さらに、この仮説距離と角度航跡とから、航跡初期値算出部（１）３６は、航跡初期値としての目標のｎ＋１次元航跡とその誤差評価量を算出して、航跡維持部（１）３７による以降の追尾維持処理に備える（ＳＴ３１７）。また、交会角が第３の交会角範囲の場合には（ＳＴ３１５の第３の交会角範囲）、角度航跡の対は距離算出部３２ａに送られ、距離算出部３２ａは、交会法により目標との距離を算出する（ＳＴ３１８）。さらに、航跡初期値算出部（２）３３ａにおいて航跡の初期値を算出し、航跡維持部（２）３４ａによる追尾維持処理に備える（ＳＴ３１９）。また、交会角が、第２の交会角範囲の場合には（ＳＴ３１５の第２の交会角範囲）、上記した２つの初期値算出を共に行って、航跡維持部（１）３７及び航跡維持部（２）３４ａによる２つの追尾維持処理の同時動作による統合航跡の取得に備える。
【００７８】
次いで、再びＳＴ３０１のステップにより、対象目標の航跡の初期値が算出済みである旨の判定がなされると（ＳＴ３０１のＹ）、航跡維持部（１）３７、及び航跡維持部（２）３４ａは、追尾維持処理に移行すべく、２つのｎ次元角度センサ１（＃１）及び１（＃２）で探知した角度情報を直接入力し始める（ＳＴ３０４）。そして、ＳＴ３０９での事前選択の結果によって選択した、航跡維持部（１）３７の追尾維持処理、航跡維持部（２）３４ａの追尾維持処理、または、これらの統合結果のいずれか１つにより目標の航跡を継続取得して、追尾を維持する（ＳＴ３０５）。
【００７９】
すなわち、事前選択部４０ａにおいて算出した交会角が第１の交会角範囲の場合には（ＳＴ３１１の第１の交会角範囲）、ＳＴ３１７のステップで算出した航跡初期値算出部（１）３６の算出結果をもとに、航跡維持部（１）３７が、２つのｎ次元角度センサ１（＃１）及び１（＃２）で探知した角度情報を入力して目標の航跡の取得を維持継続し、その結果を統合航跡算出部３９ａを経由して航跡表示部４に送出する（ＳＴ３１２）。一方、事前選択部４０ａにおいて算出した交会角が第３の交会角範囲の場合には（ＳＴ３１１の第３の交会角範囲）、ＳＴ３１９のステップで算出した航跡初期値算出部（２）３３ａの算出結果をもとに、航跡維持部（２）３４ａが、２つのｎ次元角度センサ１（＃１）及び１（＃２）で探知した角度情報を入力して目標の航跡の取得を維持継続し、その結果を統合航跡算出部３９ａを経由して航跡表示部４に送出する（ＳＴ３１４）。これに対して、事前選択部４０ａにおいて算出した交会角が第２の交会角範囲の場合には（ＳＴ３１１の第２の交会角範囲）、図３（ｃ）に示したＳＴ１０５と同様の動作ステップにより、航跡維持部（１）３７及び航跡維持部（２）３４ａでそれぞれに継続取得した目標の航跡を、事後確率算出部３８での算出結果に基づいて統合航跡算出部３９ａにて統合し、この統合航跡を航跡表示部４に送出する（ＳＴ３１３）。この後は、動作終了が指示されるまで、上記した動作ステップを繰り返す（ＳＴ３０６）。
【００８０】
上述した航跡統合部３ｃ内での処理の流れに対応した航跡統合部３ｃ内の主要部の動作タイミングについては、第１の実施例の図４、及び第２の実施例の図９を組み合わせたものとなる。すなわち、事前選択部４０ａにおける交会角の判定が第２の交会角範囲の場合は、図４と同様な動作タイミングとなり、第１の交会角範囲、または第３の交会角範囲の場合は、図９と同様な動作タイミングとなる。
【００８１】
以上説明したように、本実施例においては、相関部において新しい目標と判定した角度航跡の対に対して、事前選択部において２つのｎ次元角度センサによる交会角を算出し、この交会角の値の範囲によって、目標の航跡を継続取得する追尾維持処理を選択している。すなわち、交会角が最も小さな範囲である第１の交会角範囲の場合は、航跡初期値の算出に目標との仮説距離を用いた、航跡維持部（１）による第１の追尾維持処理を選択し、交会角が最も大きい範囲である第３の交会角範囲の場合は、航跡初期値の算出に交会法による算出距離を用いた、航跡維持部（２）による第２の追尾維持処理を選択し、交会角がこれらの中間値となる第２の交会角範囲の場合は、航跡維持部（１）及び航跡維持部（２）のそれぞれの追尾維持処理で取得した航跡を統合して、その後の目標の航跡を継続取得している。これにより、交会角が小さな場合でも目標に対する追尾安定性を向上させることができるとともに、交会角の範囲によっては追尾維持処理を同時並行に動作させないので、装置の処理負荷を軽減することができる。
【実施例４】
【００８２】
図１３は、本発明に係る目標追尾装置の第４の実施例の構成を示すブロック図である。また、図１４〜図１６は、この目標追尾装置の第４の実施例の動作を説明するためのフローチャート及び説明図である。この第４の実施例について、上述した第１〜第３の実施例の各部と同一の部分は同一の符号で示し、その説明は省略する。第４の実施例が第１の実施例と異なる点は、第１の実施例においては、航跡維持部（１）、及び航跡維持部（２）のそれぞれの追尾維持処理からの目標航跡を事後確率算出部で算出した事後確率に基づいて統合し、この統合航跡を追尾終了まで継続取得することによって追尾維持するのに対し、第４の実施例においては、追尾維持処理に移行後、所定の経過時間までは同様に統合航跡を継続取得するが、その後は、航跡維持部（１）、及び航跡維持部（２）のそれぞれの追尾維持処理による目標航跡のうち、事後確率の大きなどちらか一方を選択し、この選択した側の目標航跡を継続取得することによって追尾維持するように構成した点である。
【００８３】
以下、前出の図１〜図５、ならびに図１３〜図１６を参照して、その相違点のみを説明する。
【００８４】
図１３に例示したように、この目標追尾装置は、同一に構成された第１のｎ次元角度センサ１（＃１）、及び第２のｎ次元角度センサ１（＃２）、同一に構成された第１の角度追尾部２（＃１）、及び第２の角度追尾部２（＃２）、航跡統合部３ｄ、ならびに第１の航跡表示部４（＃１）、及び第２の航跡表示部４（＃２）から構成されている。２つのｎ次元角度センサ１（＃１）及び１（＃２）、２つの角度追尾部２（＃１）及び２（＃２）、ならびに、２つの航跡表示部４（＃１）及び４（＃２）は、第１の実施例と同じであり、説明を省略する。また、航跡統合部３ｄは、相関部３１ａ、仮説距離生成部３５、航跡初期値算出部（１）３６、航跡維持部（１）３７、距離算出部３２ａ、航跡初期値算出部（２）３３ａ、及び航跡維持部（２）３４ａ、事後確率算出部３８、事後選択部４１、及び統合航跡算出部３９ｂから構成されている。ここに、事後選択部４１、及び統合航跡算出部３９ｂを除く各構成は、第１の実施例と同じであり、説明を省略する。
【００８５】
事後選択部４１は、追尾維持処理の初期値が算出されて追尾維持処理に移行後、所定の時間が経過した時点で、事後確率算出部３８で算出した、航跡維持部（１）３７、及び航跡維持部（２）３４ａのそれぞれの追尾維持処理による目標航跡の事後確率を参照し、所定の時間経過以降は、事後確率の大きなどちらか一方からの目標航跡を採用して追尾維持を継続するように選択制御を行う。統合航跡算出部３９ｂは、第１の実施例における統合航跡算出部３９と同様の処理機能に加え、事後選択部４１からの選択制御に基づいて、所定の時間が経過するまでは統合航跡を、また所定の時間経過後は、航跡維持部（１）３７、または、航跡維持部（２）３４ａのそれぞれの追尾維持処理による目標航跡のうちで、どちらか一方を選択し、後段の航跡表示部４に送出する。
【００８６】
次に、図１４及び図１５のフローチャートを参照して、上述のように構成された第４の実施例の目標追尾装置の動作について、航跡統合部３ｄにおける処理の流れを中心に説明する。まず、航跡統合部３ｄは、２つの角度追尾部２（＃１）及び２（＃２）から航跡の初期値を算出済みでない角度航跡を受けとると（ＳＴ４０１のＮ）、相関部３１ａがこれら角度航跡を入力し（ＳＴ４０２）、次いで、追尾維持処理に必要な一連の初期値算出処理を行う（ＳＴ４０３）。このＳＴ４０３での初期値算出手順は、図２（ｂ）、及び図３（ｄ）に示した第１の実施例における手順と同様である。
【００８７】
次いで、再びＳＴ４０１のステップにより、対象目標の航跡の初期値が算出済である旨の判定がなされると（ＳＴ４０１のＹ）、航跡維持部（１）３７、及び航跡維持部（２）３４ａは、追尾維持処理に移行すべく、２つのｎ次元角度センサ１（＃１）及び１（＃２）で探知した角度情報を直接入力し始める（ＳＴ４０４）。そして、ＳＴ４０３のステップで算出済みの初期値に基づいて、それぞれの追尾維持処理を同時進行させながら目標航跡の取得を維持継続し、さらに、これら２つの航跡維持部からの航跡を事後確率に基づき統合した統合航跡を得るとともに、所定の時間経過した時点で事後選択を行い、事後確率の大きなどちらか一方の目標航跡を選択して追尾維持する（ＳＴ４０５）。
【００８８】
次に、このＳＴ４０５のステップについて、図１５を参照して詳述する。事後選択は、追尾維持処理に移行後、所定の時間経過した時点で行われるため、所定の時間が経過するまでは、事後選択がなされていないので（ＳＴ４０７のＮ）、航跡維持部（１）３７、及び航跡維持部（２）３４ａにおけるそれぞれの追尾維持処理を同時進行させながら、これらを統合した統合航跡を継続取得し、後段の航跡表示部４に送出する（ＳＴ４０８）。このＳＴ４０８での統合航跡の算出手順は、図３（ｃ）に示した第１の実施例における手順と同様である。所定の時間が経過するまでは（ＳＴ４０９のＮ）、この統合航跡を繰り返し取得することによって追尾維持する。そして、所定時間が経過すると（ＳＴ４０９のＹ）、事後選択部４１は、事後確率算出部３８からの事後確率に基づいて、事後選択を行う。すなわち、以降は統合航跡により追尾維持するのではなく、同時並列に動作していた航跡維持部（１）３７、及び航跡維持部（２）３４ａのそれぞれの追尾維持処理による目標航跡の事後確率を参照し、事後確率の大きなどちらか一方からの目標航跡を採用して追尾維持を継続するように選択制御を行う（ＳＴ４１０）。
【００８９】
このようにして所定の時間が経過し事後選択がなされた後は（ＳＴ４０７のＹ）、再びＳＴ４０４のステップで次の角度情報が探知されると、これら探知情報は、事後選択された航跡維持部（１）３７、または航跡維持部３４ａのどちらかに送られる（ＳＴ４１１）。そして、事後選択されたどちらかの航跡維持部（１）３７、または航跡維持部（２）３４ａは、それぞれの航跡維持処理により目標航跡の取得を継続して追尾維持するとともに、取得した目標航跡を後段の航跡表示部４に送出し表示する（ＳＴ４１２、またはＳＴ４１３）。この後は、動作終了が指示されるまで、上記した動作ステップを繰り返す（ＳＴ４０６）。
【００９０】
図１６は、上記した航跡統合部３ｄ内での処理の流れに対応した航跡統合部３ｄ内主要部の動作タイミングの一例を、時間経過に沿ってモデル化して示す説明図である。相関部３１ａにより、同一目標の角度航跡の対から新目標と判定した角度航跡の対が抽出されると（Ｔ４１）、航跡初期値算出部（１）３６では、仮説距離生成部３５からの仮説距離を用いて、航跡維持部（１）３７での追尾維持処理に必要な航跡初期値として、目標のｎ＋１次元航跡とその誤差評価量が算出される（Ｔ４２）。同様に、航跡初期値算出部（２）３３ａでは、距離算出部３２ａで算出された距離を用いて、航跡維持部（２）３４ａでの追尾維持処理に必要な、目標の航跡の初期値が算出される（Ｔ４３）。
【００９１】
これらの初期値をもとに、航跡維持部（１）３７、及び航跡維持部（２）３４ａは、目標の追尾維持処理に移行し、それぞれの追尾維持処理が開始されて、目標のｎ＋１次元航跡が継続して取得される。この時に、航跡維持部（１）３７の追尾維持処理と航跡維持部（２）３４ａの追尾維持処理は、同時並行して進行する（Ｔ４４、及びＴ４５）。さらに、これらと並行して動作する統合航跡算出部３９では、航跡維持部（１）３７、及び航跡維持部（２）３４ａからの目標航跡が、事後確率算出部３８で算出した事後確率に基づいて統合され、統合航跡として航跡表示部４に送出される（Ｔ４６）。その後、所定の時間が経過すると、統合航跡に換えて、事後選択部４１により選択された航跡維持部（１）３７または航跡維持部（２）３４ａのいずれか一方が、追尾終了まで動作する。図１６では、事後選択部４１により、航跡維持部（１）３７が選択された場合を例示している（Ｔ４７）。
【００９２】
以上説明したように、本実施例においては、目標の追尾維持処理に移行後、所定の時間が経過して追尾安定性が高まるまでは統合航跡を継続取得して追尾維持し、その後、追尾安定性が高まった以降は、航跡維持部（１）、及び航跡維持部（２）のそれぞれの追尾維持処理による目標航跡のうち、事後確率の大きなどちらか一方を選択し、この選択した側の目標航跡を継続取得することによって追尾維持している。これにより、交会角が小さな場合でも目標に対する追尾安定性を向上させることができるとともに、追尾維持処理に移行して所定の時間経過後は、同時並行に動作する２つの追尾維持処理のうち、どちらか一方を動作させて目標の航跡を継続取得しているので、時間経過に伴い追尾維持処理が収束するに従って、装置の処理負荷を軽減することができる。
【００９３】
なお、本実施例では、事後選択４１が事後選択を行なうタイミングとして、経過時間を用いる場合を説明したが、事後選択を行なうタイミングは、これ以外に、観測回数が所定の回数を超えた場合とすることもできる。また、事後確率のどちらか一方が所定の確率を超えた場合、さらには、それらの条件の組み合わせを満足するタイミング等、種々の中から選択することができる。
【実施例５】
【００９４】
図１７は、本発明に係る目標追尾装置の第５の実施例の構成を示すブロック図である。また、図１８〜図１９は、この目標追尾装置の第５の実施例の動作を説明するためのフローチャートである。この第５の実施例について、上述した第１〜第４の実施例の各部と同一の部分は同一の符号で示し、その説明は省略する。第５の実施例においては、第３の実施例における事前選択部の処理機能と、第４の実施例における事後選択部の処理機能とを同時に具備した構成としている。すなわち、新たな目標に対しては、事前選択として、各ｎ次元角度センサからの交会角の範囲に基づき目標の航跡を継続取得するための追尾維持処理を選択するとともに、この事前選択で航跡維持部（１）と航跡維持部（２）からの航跡を統合した統合航跡が採用された場合には、事後選択として、追尾維持処理に移行後、所定の経過時間までは統合航跡を継続取得し、その後は、事後確率に基づき航跡維持部（１）または航跡維持部（２）のどちらか一方の追尾維持処理を選択して追尾維持するように構成している。以下、前出の図１０〜図１６、ならびに図１７〜１９を参照して、その相違点のみを説明する。
【００９５】
図１７に例示したように、この目標追尾装置は、同一に構成された第１のｎ次元角度センサ１（＃１）、及び第２のｎ次元角度センサ１（＃２）、同一に構成された第１の角度追尾部２（＃１）、及び第２の角度追尾部２（＃２）、航跡統合部３ｅ、ならびに第１の航跡表示部４（＃１）、及び第２の航跡表示部４（＃２）から構成されている。２つのｎ次元角度センサ１（＃１）及び１（＃２）、２つの角度追尾部２（＃１）及び２（＃２）、ならびに、２つの航跡表示部４（＃１）及び４（＃２）は、前出の各実施例と同じであり、説明を省略する。また、航跡統合部３ｅは、相関部３１ａ、事前選択部４０ａ、仮説距離生成部３５、航跡初期値算出部（１）３６、航跡維持部（１）３７、距離算出部３２ａ、航跡初期値算出部（２）３３ａ、及び航跡維持部（２）３４ａ、事後確率算出部３８、事後選択部４１ａ、及び統合航跡算出部３９ｃから構成されている。ここに、事後選択部４１ａ、及び統合航跡算出部３９ｃを除く各構成は、前出の第３及び第４の実施例と同じであり、説明を省略する。
【００９６】
事後選択部４１ａは、事前選択部４０ａにおける選択制御において、目標の交会角が第２の交会角範囲の場合に対応する、統合航跡による目標の追尾維持が選択された場合に、前出の第４の実施例における事後選択部４１と同様の処理機能を果たすよう構成される。また、統合航跡算出部３９ｃは、事前選択部４０ａにおける選択制御において、統合航跡による目標の追尾維持が選択された場合には、前出の第４の実施例における統合航跡算出部３９ｂと同様の処理機能を果たすとともに、事前選択部４０ａにおける選択制御において、航跡維持部（１）３７、または航跡維持部（２）３４ａの追尾維持処理による目標の追尾維持が選択された場合には、これら選択制御に該当する追尾維持処理による目標航跡を後段の航跡表示部４に送出する。
【００９７】
次に、図１８及び図１９のフローチャートを参照して、上述のように構成された第５の実施例の目標追尾装置の動作について、航跡統合部３ｅにおける処理の流れを中心に説明する。まず、航跡統合部３ｅは、２つの角度追尾部２（＃１）及び２（＃２）から航跡の初期値を算出済みでない角度航跡を受けとると（ＳＴ５０１のＮ）、相関部３１ａがこれら角度航跡を入力し（ＳＴ５０２）、次いで、追尾維持処理に必要な一連の初期値算出処理を行う（ＳＴ５０３）。このＳＴ５０３での初期値算出手順は、図１１（ｂ）、及び図１２（ｄ）に示した第３の実施例における手順と同様である。すなわち、事前選択部４０ａにおける交会角の判定において、交会角が第１の交会角範囲の場合には、航跡初期値算出部（１）３６により初期値を算出し、また、交会角が第２の交会角範囲の場合には、航跡初期値算出部（１）３６、及び航跡初期値算出部（２）３３ａの２つの初期値算出部で共に初期値を算出し、さらに、交会角が第３の交会角範囲の場合には航跡初期値算出部（２）３３ａにより初期値を算出して、以降の航跡維持部（１）３７及び航跡維持部（２）３４ａによる追尾維持処理に備える。
【００９８】
次いで、再びＳＴ５０１のステップにより、対象目標の航跡の初期値が算出済みである旨の判定がなされると（ＳＴ５０１のＹ）、航跡維持部（１）３７、及び航跡維持部（２）３４ａは、追尾維持処理に移行すべく、２つのｎ次元角度センサ１（＃１）及び１（＃２）で探知した角度情報を直接入力し始める（ＳＴ５０４）。そして、事前選択部４０ａにおける交会角による選択制御に基づいて、航跡維持部（１）３７の追尾維持処理、航跡維持部（２）３４ａの追尾維持処理、または、これらの統合結果のいずれか１つにより目標の航跡を継続取得して、追尾を維持する（ＳＴ５０７）。
【００９９】
すなわち、目標の交会角が第１の交会角範囲の場合には（ＳＴ５０７の第１の交会角範囲）、航跡維持部（１）３７が、２つのｎ次元角度センサ１（＃１）及び１（＃２）で探知した角度情報を入力して目標の航跡の取得を維持継続し、その結果を統合航跡算出部３９ｃを経由して航跡表示部４に送出する（ＳＴ５０８）。また、目標の交会角が第２の交会角範囲の場合には（ＳＴ５０７の第２の交会角範囲）、所定の経過時間まで統合航跡を継続取得し、所定の時間を経過した後は、航跡維持部（１）３７、及び航跡維持部（２）３４ａのそれぞれの追尾維持処理による目標航跡のうち、事後確率算出部３８で算出した事後確率の大きなどちらか一方を選択し、この選択した側の目標航跡を継続取得することによって追尾維持するとともに、この目標航跡を航跡表示部４に送出する（ＳＴ５０９）。このＳＴ５０９での処理手順は、図１５（ｂ）に示した第４の実施例におけるＳＴ４０５の詳細な処理手順と同様である。
【０１００】
また、目標の交会角が第３の交会角範囲の場合には（ＳＴ５０７の第３の交会角範囲）、航跡維持部（１）３４ａが、２つのｎ次元角度センサ１（＃１）及び１（＃２）で探知した角度情報を入力して目標の航跡の取得を維持継続し、その結果を統合航跡算出部３９ｃを経由して航跡表示部４に送出する（ＳＴ５１０）。この後は、動作終了が指示されるまで、上記した動作ステップを繰り返す（ＳＴ５０６）。
【０１０１】
上述した航跡統合部３ｅ内での処理の流れに対応した航跡統合部３ｅ内の主要部の動作タイミングについては、第２の実施例の図９、及び第４の実施例の図１６を組み合わせたものとなる。すなわち、事前選択部４０ａにおける交会角の判定が、第１の交会角範囲、または第３の交会角範囲の場合は、第２の実施例の図９と同様な動作タイミングとなり、第２の交会角範囲の場合は、第４の実施例の図１６と同様な動作タイミングとなる。
【０１０２】
以上説明したように、本実施例においては、相関部において新しい目標と判定した角度航跡の対に対して、事前選択部において２つのｎ次元角度センサによる交会角を算出した結果、交会角が最も小さな範囲である第１の交会角範囲の場合は、航跡初期値の算出に目標との仮説距離を用いた、航跡維持部（１）による第１の追尾維持処理を選択し、交会角が最も大きい範囲である第３の交会角範囲の場合は、航跡初期値の算出に交会法による算出距離を用いた、航跡維持部（２）による第２の追尾維持処理を選択し、交会角がこれらの中間値となる第２の交会角範囲の場合は、航跡維持部（１）及び航跡維持部（２）のそれぞれの追尾維持処理で取得した航跡を統合して、その後の目標の航跡を継続取得している。加えて、統合航跡を選択した場合は、追尾維持処理に移行後、追尾安定性が高まるまでは、２つの追尾維持処理による統合航跡を継続取得して追尾維持し、その後、所定の時間が経過して追尾安定性が高まった以降は、航跡維持部（１）、及び航跡維持部（２）のそれぞれの追尾維持処理による目標航跡のうち、事後確率の大きなどちらか一方を選択し、この選択した側の目標航跡を継続取得することによって追尾維持している。これにより、交会角が小さな場合でも目標に対する追尾安定性を向上させることができるとともに、目標の交会角の範囲によって１つの追尾維持処理のみを選択するか、あるいは、時間経過に伴い追尾維持処理が収束するに従って同時並行動作させた２つの追尾維持処理のどちらか一方を選択して目標の航跡を継続取得しているので、装置の処理負荷を軽減することができる。
【０１０３】
なお、本発明は、上記した実施形態そのままに限定されるものではなく、実施段階ではその要旨を逸脱しない範囲で構成要素を変形して具体化できる。また、上記実施形態に開示されている複数の構成要素の適宜な組み合せにより種々の発明を形成できる。例えば、実施形態に示される全構成要素から幾つかの構成要素を削除してもよい。さらに、異なる実施形態に亘る構成要素を適宜組み合せてもよい。
【符号の説明】
【０１０４】
１ ｎ次元角度センサ
２ 角度追尾部
３、３ａ、３ｂ、３ｃ、３ｄ、３ｅ 航跡統合部
４ 航跡表示部
３１、３１ａ 相関部
３２ 距離／距離変化率算出部
３２ａ 距離算出部
３３ ｎ＋１次元航跡初期値算出部
３３ａ 航跡初期値算出部（２）
３４ 航跡維持部
３４ａ 航跡維持部（２）
３５ 仮説距離生成部
３６ 航跡初期値算出部（１）
３７ 航跡維持部（１）
３８ 事後確率算出部
３９、３９ａ、３９ｂ、３９ｃ 統合航跡算出部
４０、４０ａ 事前選択部
４１、４１ａ 事後選択部
【技術分野】
【０００１】
本発明は、複数の角度センサからのｎ次元角度情報を統合して、目標のｎ＋１次元追尾を行う目標追尾装置に関する。
【背景技術】
【０００２】
図２０は、従来の目標追尾装置の構成の一例を示すブロック図である。この図２０に例示した目標追尾装置は、２つのｎ次元（ｎは１または２）角度センサを含み構成された事例であり、同一に構成された第１のｎ次元角度センサ１（＃１）、及び第２のｎ次元角度センサ１（＃２）、同一に構成された第１の角度追尾部２（＃１）、及び第２の角度追尾部２（＃２）、航跡統合部３、ならびに第１の航跡表示部４（＃１）、及び第２の航跡表示部４（＃２）から構成されている。
【０００３】
第１のｎ次元角度センサ１（＃１）及び第２のｎ次元角度センサ１（＃２）は、探知した目標の角度情報を取得し、それぞれ対応する角度追尾部２（＃１）または２（＃２）、ならびに航跡統合部３に送出する。第１の角度追尾部２（＃１）及び第２の角度追尾部２（＃２）は、それぞれに対応するｎ次元角度センサ１からの角度情報に基づいて、第１の角度追尾部２（＃１）は、目標の角度及び角速度からなる角度航跡ｚ（ｋ，１）及びその誤差共分散行列Ｐｚ（ｋ，１）を、また第２の角度追尾部２（＃２）は、目標の角度及び角速度からなる角度航跡ｚ（ｋ，２）及びその誤差共分散行列Ｐｚ（ｋ，２）を算出し、航跡統合部３に送出する。
【０００４】
航跡統合部３は、これら２つの角度追尾部２（＃１）及び２（＃２）での算出結果から、同一の目標に対する航跡を統合して追尾する。ここに、航跡統合部３は、相関部３１、距離／距離変化率算出部３２、ｎ＋１次元航跡初期値算出部３３、及び航跡維持部３４から構成されている。相関部３１は、２つの角度追尾部２（＃１）及び２（＃２）からの角度航跡を同一の目標毎に対をなすように関連づける。距離／距離変化率算出部３２は、これら角度航跡に基づき、交会法の手法により２つのｎ次元角度センサ１（＃１）及び１（＃２）のそれぞれから同一の目標までの距離及び距離変化率を算出する。ｎ＋１次元航跡初期値算出部３３は、距離／距離変化率算出部３２の算出結果をもとに、後段の航跡維持部３４でこの目標の追尾に必要な初期値となる、この目標のｎ＋１次元航跡とその誤差評価量としての誤差共分散行列を算出する。航跡維持部３４は、これら算出結果を初期値として追尾維持処理を実行し、その航跡を継続的に取得する。
【０００５】
航跡維持部３４における追尾維持処理は、例えば、式（１）に示した目標の運動モデル、及び式（２）に示した目標の観測モデルに対して、拡張カルマンフィルタ等によって算出することができる。
【数１】
【数２】
【０００６】
ここで、ｘ（ｋ）は、観測時刻ｔｋにおける目標の状態ベクトル、Ｆ（ｋ，ｋ−１）とＧ（ｋ，ｋ−１）は、観測時刻ｔｋ−１からｔｋにおける遷移行列と駆動行列、ｗ（ｋ−１）は、観測時刻ｔｋ−１における平均０、共分散行列Ｑ（ｋ−１）の正規分布に従うシステム雑音ベクトル、ｙ（ｋ，ｉ）は、観測時刻ｔｋにおけるｎ次元角度センサ１（＃ｉ）の観測ベクトル、ｈ（・）は、観測関数、ｖ（ｋ，ｉ）は、観測時刻ｔｋにおける平均０、共分散行列Ｒ（ｋ，ｉ）の正規分布に従う観測雑音ベクトルである。
【０００７】
また、航跡維持部３４を拡張カルマンフィルタで構成した場合、式（３）〜式（４）に示す予測処理、ならびに式（５）〜式（９）に示す平滑処理を実施する。
【数３】
【数４】
【数５】
【数６】
【数７】
【数８】
【数９】
【０００８】
ここで、ｘ＾（ｋ｜ｋ−１）とＰ（ｋ｜ｋ−１）は、観測時刻ｔｋにおける予測ベクトルと予測誤差共分散行列、Ｓ（ｋ）、ｄ（ｋ）およびＫ（ｋ）は、観測時刻ｔｋにおける残差共分散行列、マハラノビス距離およびカルマンゲイン行列、ｘ＾（ｋ｜ｋ）とＰ（ｋ｜ｋ）は、観測時刻ｔｋにおける平滑ベクトルと平滑誤差共分散行列である。また、ＡＴは、ベクトルまたは行列Ａの転置、Ａ−１は、行列Ａの逆行列、Ｉは、単位行列を示す。
【０００９】
ｎ次元角度センサ１が方位角のみを出力し、航跡維持部３４が２次元航跡を維持する場合、上記ベクトルおよび行列は、以下の式（１０）〜式（１６）のようになる。
【数１０】
【数１１】
【数１２】
【数１３】
【数１４】
【数１５】
【数１６】
【００１０】
ここで、ｐｘ（ｋ）とｐｙ（ｋ）は、目標のｘ軸とｙ軸の位置成分、ｖｘ（ｋ）とｖｙ（ｋ）は、目標のｘ軸とｙ軸の速度成分、σｘ（ｋ）とσｙ（ｋ）は、目標のｘ軸とｙ軸のシステム雑音の標準偏差、θ（ｋ，ｉ）は、ｎ次元角度センサ１（＃ｉ）の方位角の観測値、ｐｘ（ｉ）とｐｙ（ｉ）は、ｎ次元角度センサ１（＃ｉ）のｘ軸とｙ軸の位置成分、σθ（ｋ，ｉ）は、ｎ次元角度センサ１（＃ｉ）の方位角の観測雑音の標準偏差、（全て、観測時刻ｔｋにおける値）である。また、Ｉｎは、ｎ行ｎ列の単位行列、Ｏｎは、ｎ行ｎ列の零行列を示す。
【００１１】
第１の航跡表示部４（＃１）及び第２の航跡表示部４（＃２）は、航跡統合部３で取得した目標の航跡を、２つのｎ次元角度センサ１（＃１）及び１（＃２）のそれぞれに対応させて表示する。
【００１２】
次に、図２０に例示した従来の目標追尾装置の動作を、航跡統合部３における処理の流れを中心に説明する。図２１は、航跡統合部３の処理の流れの一例を示すフローチャートである。まず、航跡統合部３は、２つの角度追尾部２（＃１）及び２（＃２）から新たな目標の角度航跡、すなわち航跡の初期値を算出済みでない角度航跡を受けとると（ＳＴ６０１のＮ）、相関部３１がこれら角度航跡を入力し（ＳＴ６０２）、次いで、追尾維持処理に必要な一連の初期値算出処理を行う（ＳＴ６０３）。
【００１３】
すなわち、相関部３１において、２つの角度追尾部２（＃１）及び２（＃２）からの角度航跡のどの対が同一目標であるかを判別する相関処理を行なった結果（ＳＴ６０７）、同一目標と判別された角度航跡の対がある場合には（ＳＴ６０８のＹ）、距離／距離変化率算出部３２において、これら角度航跡に基づき、交会法を用いてそれぞれのｎ次元角度センサ１（＃１）及び１（＃２）からこの目標までの距離及び距離変化率を算出する（ＳＴ６０９）。さらに、この算出結果をもとに、ｎ＋１次元航跡初期値算出部３３において、後段の航跡維持部３４での追尾維持処理の初期値となる、目標のｎ＋１次元航跡とその誤差評価量としての誤差共分散行列を算出し、この目標の追尾開始に備える（ＳＴ６１０）。
【００１４】
次いで、再びＳＴ６０１のステップにより、対象の航跡初期値が算出済みである旨の判定がなされると（ＳＴ６０１のＹ）、この後は、算出済みの初期値に基づき目標の追尾に移行すべく、航跡維持部３４において、２つのｎ次元角度センサ１（＃１）及び１（＃２）で探知した角度情報を直接入力し（ＳＴ６０４）、追尾維持処理を開始して、目標のｎ＋１次元航跡の取得を維持継続する（ＳＴ６０５）。そして、その結果を、２つのｎ次元角度センサ１（＃１）及び１（＃２）のそれぞれに対応させて２つの航跡表示部４（＃１）及び４（＃２）に表示する。
【先行技術文献】
【特許文献】
【００１５】
【特許文献１】特開平１０−１４２３２５号公報（第２８ページ、図４）
【非特許文献】
【００１６】
【非特許文献１】Ｓ．Ｓ．Ｂｌａｃｋｍａｎ，“Ｍｕｌｔｉｔａｒｇｅｔ−Ｍｕｌｔｉｓｅｎｓｏｒ Ｔｒａｃｋｉｎｇ Ａｄｖａｎｃｅｄ Ａｐｐｌｉｃａｔｉｏｎｓ” Ａｒｔｅｃｈ Ｈｏｕｓｅ，Ｃｈａｐｔｅｒ６，ｐｐ．２１２−２１４，Ｊａｎ． １９８９
【非特許文献２】Ｎ．Ｐｅａｃｈ，“Ｂｅａｒｉｎｇｓ−ｏｎｌｙ ｔｒａｃｋｉｎｇ ｕｓｉｎｇ ａ ｓｅｔ ｏｆ ｒａｎｇｅ−ｐａｒａｍｅｔｅｒｉｓｅｄ ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ ｆｉｌｔｅｒｓ”，ＩＥＥＥ Ｐｒｏｃ−Ｃｏｎｔｒｏｌ Ｔｈｅｏｒｙ Ａｐｐｌ．，ｖｏｌ１４２，ｎｏ．１，ｐｐ．７３−８０，Ｊａｎ． １９９５
【非特許文献３】系 正義，辻道信吾，小菅義夫，"拡張カルマンフィルタを用いた複数パッシブセンサによる３次元運動目標の追尾，"信学論（Ｂ）、ｖｏｌ．Ｊ８２−Ｂ、ｎｏ．５、ｐｐ．１０６３−１０７２、Ｍａｙ １９９９
【発明の概要】
【発明が解決しようとする課題】
【００１７】
上述したように、従来の目標追尾装置では、２つのｎ次元角度センサ１で取得した角度航跡の対から、交会法により各角度センサから目標までの距離及び距離変化率を算出し、これらの距離及び距離変化率と角度航跡とに基づいて、目標のｎ＋１次元空間での位置及び速度よりなるｎ＋１次元航跡とその誤差共分散行列を算出し、さらにこの算出結果を初期値として拡張カルマンフィルタによる追尾維持処理に移行している。
【００１８】
ところで、交会法により各角度センサから目標までの距離を算出する場合、算出した距離に対する誤差は、２つのｎ次元角度センサ１で取得した角度航跡の対が作る交会角（０°〜９０°の場合）に概ね反比例する。このため、交会角が小さな場合、目標との距離に大きな誤差を含むことになり、これらを用いて算出される、目標のｎ＋１次元空間での位置及び速度等の追尾維持処理の初期値にも、その誤差の影響が及ぶ。しかしながら、拡張カルマンフィルタでは、初期値が真値から大きく外れている場合、推定値が収束しないケースがある。この結果、交会角が小さな場合、追尾維持処理の初期値が真値から大きく外れるケースが多くなり、拡張カルマンフィルタによる追尾維持処理において推定値が収束しにくくなって正常な追尾維持が困難となり、追尾安定性が低くなるという課題があった。
【００１９】
本発明は、上述の課題を解決するためになされたものであり、複数のｎ次元角度センサと目標との交会角が小さな場合でも、追尾安定性を向上させた目標追尾装置を提供することを目的とする。
【課題を解決するための手段】
【００２０】
上記目的を達成するために、第１の発明の目標追尾装置は、互いに位置の異なる複数の角度センサにより探知された複数の目標のｎ次元（ｎは１または２）角度情報を同一の目標毎に対をなすように関連づける相関手段と、前記相関手段により同一の目標に関連づけられた角度情報と、この目標に対して設定されたｍ個（ｍは１以上）の仮説距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｍ個のｎ＋１次元航跡とその誤差評価量とを算出する第１の初期値算出手段と、前記第１の初期値算出手段によって算出されたｍ個のｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第１の追尾維持手段と、前記相関手段により同一の目標に関連づけられた角度情報と、これら角度情報の対を用いて交会法に基づき算出した前記複数の各角度センサとこの目標までの距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｎ＋１次元航跡とその誤差評価量とを算出する第２の初期値算出手段と、前記第２の初期値算出手段によって算出されたｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第２の追尾維持手段と、前記第１の追尾維持手段、及び前記第２の追尾維持手段の事後確率を算出する事後確率算出手段と、前記第１の追尾維持手段、及び前記第２の追尾維持手段のそれぞれにより算出された前記目標のｎ＋１次元航跡を統合する目標航跡統合手段とを備え、前記目標航跡統合手段は、前記事後確率算出手段により算出された第１及び第２の追尾維持手段の事後確率に基づいて、それぞれの追尾維持手段により算出された前記目標のｎ＋１次元航跡を統合することを特徴とする。
【００２１】
また、第２の発明の目標追尾装置は、互いに位置の異なる複数の角度センサにより探知された複数の目標のｎ次元（ｎは１または２）角度情報を同一の目標毎に対をなすように関連づける相関手段と、前記相関手段により同一の目標に関連づけられた角度情報と、この目標に対して設定されたｍ個（ｍは１以上）の仮説距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｍ個のｎ＋１次元航跡とその誤差評価量とを算出する第１の初期値算出手段と、前記第１の初期値算出手段によって算出されたｍ個のｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第１の追尾維持手段と、前記相関手段により同一の目標に関連づけられた角度情報と、これら角度情報の対を用いて交会法に基づき算出した前記複数の各角度センサとこの目標までの距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｎ＋１次元航跡とその誤差評価量とを算出する第２の初期値算出手段と、前記第２の初期値算出手段によって算出されたｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第２の追尾維持手段と、前記複数の角度センサと前記目標との交会角に基づいて前記第１の追尾維持手段または前記第２の追尾維持手段のいずれかを選択的に動作させる追尾事前選択手段とを備え、前記追尾事前選択手段は、前記交会角が所定のしきい値未満の場合には、前記第１の追尾維持手段を動作させるとともに、この第１の追尾維持手段により算出された前記目標のｎ＋１次元航跡を採用し、前記交会角がこの所定のしきい値以上の場合には、前記第２の追尾維持手段を動作させるとともに、この第２の追尾維持手段により算出された前記目標のｎ＋１次元航跡を採用することを特徴とする。
【００２２】
また、第３の発明の目標追尾装置は、互いに位置の異なる複数の角度センサにより探知された複数の目標のｎ次元（ｎは１または２）角度情報を同一の目標毎に対をなすように関連づける相関手段と、前記相関手段により同一の目標に関連づけられた角度情報と、この目標に対して設定されたｍ個（ｍは１以上）の仮説距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｍ個のｎ＋１次元航跡とその誤差評価量とを算出する第１の初期値算出手段と、前記第１の初期値算出手段によって算出されたｍ個のｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第１の追尾維持手段と、前記相関手段により同一の目標に関連づけられた角度情報と、これら角度情報の対を用いて交会法に基づき算出した前記複数の各角度センサとこの目標までの距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｎ＋１次元航跡とその誤差評価量とを算出する第２の初期値算出手段と、前記第２の初期値算出手段によって算出されたｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第２の追尾維持手段と、前記第１の追尾維持手段、及び前記第２の追尾維持手段の事後確率を算出する事後確率算出手段と、前記事後確率算出手段からの事後確率に基づいて、前記第１の追尾維持手段、及び前記第２の追尾維持手段のそれぞれにより算出された前記目標のｎ＋１次元航跡を統合する目標航跡統合手段と、前記複数の角度センサと前記目標との交会角に基づいて前記第１の追尾維持手段または前記第２の追尾維持手段のいずれか一方または両者を選択して動作させる追尾事前選択手段とを備え、前記追尾事前選択手段は、前記交会角が第１のしきい値未満の場合には、前記第１の追尾維持手段を動作させるとともに、この第１の追尾維持手段により算出された前記目標のｎ＋１次元航跡を採用し、前記交会角が第１のしきい値以上かつ第２のしきい値（第２のしきい値＞第１のしきい値）未満の場合には、前記第１の追尾維持手段及び前記第２の追尾維持手段の両者を動作させるとともに、前記目標航跡統合手段により統合された前記目標のｎ＋１次元航跡を採用し、前記交会角が第２のしきい値以上の場合には、前記第２の追尾維持手段を動作させるとともに、この第２の追尾維持手段により算出された前記目標のｎ＋１次元航跡を採用することを特徴とする。
【００２３】
また、第４の発明の目標追尾装置は、互いに位置の異なる複数の角度センサにより探知された複数の目標のｎ次元（ｎは１または２）角度情報を同一の目標毎に対をなすように関連づける相関手段と、前記相関手段により同一の目標に関連づけられた角度情報と、この目標に対して設定されたｍ個（ｍは１以上）の仮説距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｍ個のｎ＋１次元航跡とその誤差評価量とを算出する第１の初期値算出手段と、前記第１の初期値算出手段によって算出されたｍ個のｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第１の追尾維持手段と、前記相関手段により同一の目標に関連づけられた角度情報と、これら角度情報の対を用いて交会法に基づき算出した前記複数の各角度センサとこの目標までの距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｎ＋１次元航跡とその誤差評価量とを算出する第２の初期値算出手段と、前記第２の初期値算出手段によって算出されたｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第２の追尾維持手段と、前記第１の追尾維持手段、及び前記第２の追尾維持手段の事後確率を算出する事後確率算出手段と、前記事後確率算出手段からの事後確率に基づいて、前記第１の追尾維持手段、及び前記第２の追尾維持手段のそれぞれにより算出された前記目標のｎ＋１次元航跡を統合する目標航跡統合手段と、前記事後確率算出手段からの事後確率に基づいて前記第１の追尾維持手段、前記第２の追尾維持手段、または前記目標航跡統合手段のいずれかにより算出された前記目標のｎ＋１次元航跡を選択して採用する追尾事後選択手段とを備え、前記追尾事後選択手段は、所定の観測回数または観測時間が経過するまでは、前記目標航跡統合手段により統合された前記目標のｎ＋１次元航跡を採用し、その後は、この所定の観測回数または観測時間が経過した時点で算出した、前記第１の追尾維持手段及び前記第２の追尾維持手段の事後確率を参照し、その値の高いいずれか一方の追尾維持手段により算出された前記目標のｎ＋１次元航跡を採用することを特徴とする。
【００２４】
また、第５の発明の目標追尾装置は、互いに位置の異なる複数の角度センサにより探知された複数の目標のｎ次元（ｎは１または２）角度情報を同一の目標毎に対をなすように関連づける相関手段と、前記相関手段により同一の目標に関連づけられた角度情報と、この目標に対して設定されたｍ個（ｍは１以上）の仮説距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｍ個のｎ＋１次元航跡とその誤差評価量とを算出する第１の初期値算出手段と、前記第１の初期値算出手段によって算出されたｍ個のｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第１の追尾維持手段と、前記相関手段により同一の目標に関連づけられた角度情報と、これら角度情報の対を用いて交会法に基づき算出した前記複数の各角度センサとこの目標までの距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｎ＋１次元航跡とその誤差評価量とを算出する第２の初期値算出手段と、前記第２の初期値算出手段によって算出されたｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第２の追尾維持手段と、前記第１の追尾維持手段、及び前記第２の追尾維持手段の事後確率を算出する事後確率算出手段と、前記事後確率算出手段からの事後確率に基づいて、前記第１の追尾維持手段、及び前記第２の追尾維持手段のそれぞれにより算出された前記目標のｎ＋１次元航跡を統合する目標航跡統合手段と、前記複数の角度センサと前記目標との交会角に基づいて前記第１の追尾維持手段または前記第２の追尾維持手段のいずれか一方または両者を選択して動作させる追尾事前選択手段と、前記事後確率算出手段からの事後確率に基づいて前記第１の追尾維持手段、前記第２の追尾維持手段、または前記目標航跡統合手段のいずれかにより算出された前記目標のｎ＋１次元航跡を選択して採用する追尾事後選択手段とを備え、前記追尾事後選択手段は、所定の観測回数または観測時間が経過するまでは、前記目標航跡統合手段により統合された前記目標のｎ＋１次元航跡を採用し、その後は、この所定の観測回数または観測時間が経過した時点で算出した、前記第１の追尾維持手段及び前記第２の追尾維持手段の事後確率を参照し、その値の高いいずれか一方の追尾維持手段により算出された前記目標のｎ＋１次元航跡を採用し、前記追尾事前選択手段は、前記交会角が第１のしきい値未満の場合には、前記第１の追尾維持手段を動作させるとともに、この第１の追尾維持手段により算出された前記目標のｎ＋１次元航跡を採用し、前記交会角が第１のしきい値以上かつ第２のしきい値（第２のしきい値＞第１のしきい値）未満の場合には、前記第１の追尾維持手段及び前記第２の追尾維持手段の両者を動作させるとともに、前記追尾事後選択手段による前記目標のｎ＋１次元航跡を採用し、前記交会角が第２のしきい値以上の場合には、前記第２の追尾維持手段を動作させるとともに、この第２の追尾維持手段により算出された前記目標のｎ＋１次元航跡を採用することを特徴とする。
【発明の効果】
【００２５】
本発明によれば、交会角が小さな場合でも、良好な追尾安定性を有する目標追尾装置を得ることができる。
【図面の簡単な説明】
【００２６】
【図１】本発明に係る目標追尾装置の第１の実施例の構成を示すブロック図。
【図２】図１に例示した目標追尾装置の動作を説明するための第１の部分のフローチャート。
【図３】図１に例示した目標追尾装置の動作を説明するための第２の部分のフローチャート。
【図４】航跡統合部３ａ内主要部の動作タイミングを時間経過に沿ってモデル化して例示する説明図。
【図５】図１の目標追尾装置の変形例の構成を示すブロック図。
【図６】本発明に係る目標追尾装置の第２の実施例の構成を示すブロック図。
【図７】図６に例示した目標追尾装置の動作を説明するための第１の部分のフローチャート。
【図８】図６に例示した目標追尾装置の動作を説明するための第２の部分のフローチャート。
【図９】航跡統合部３ｂ内主要部の動作タイミングを時間経過に沿ってモデル化して例示する説明図。
【図１０】本発明に係る目標追尾装置の第３の実施例の構成を示すブロック図。
【図１１】図１０に例示した目標追尾装置の動作を説明するための第１の部分のフローチャート。
【図１２】図１０に例示した目標追尾装置の動作を説明するための第２の部分のフローチャート。
【図１３】本発明に係る目標追尾装置の第４の実施例の構成を示すブロック図。
【図１４】図１３に例示した目標追尾装置の動作を説明するための第１の部分のフローチャート。
【図１５】図１３に例示した目標追尾装置の動作を説明するための第２の部分のフローチャート。
【図１６】航跡統合部３ｄ内主要部の動作タイミングを時間経過に沿ってモデル化して例示する説明図。
【図１７】本発明に係る目標追尾装置の第５の実施例の構成を示すブロック図。
【図１８】図１７に例示した目標追尾装置の動作を説明するための第１の部分のフローチャート。
【図１９】図１７に例示した目標追尾装置の動作を説明するための第２の部分のフローチャート。
【図２０】従来の目標追尾装置の構成の一例を示すブロック図。
【図２１】図１４における航跡統合部３の処理の流れの一例を示すフローチャート。
【発明を実施するための形態】
【００２７】
以下に、本発明に係る目標追尾装置を実施するための最良の形態について、図１乃至図１９を参照して説明する。なお、以下の説明においては、背景技術の欄で説明した従来の目標追尾装置の各構成に相当する部分には、同一の符号を用いて説明する。
【実施例１】
【００２８】
図１は、本発明に係る目標追尾装置の第１の実施例の構成を示すブロック図である。図１に例示したように、この目標追尾装置は、２つのｎ次元（ｎは１または２）角度センサを含み構成された事例であり、同一に構成された第１のｎ次元角度センサ１（＃１）、及び第２のｎ次元角度センサ１（＃２）、同一に構成された第１の角度追尾部２（＃１）、及び第２の角度追尾部２（＃２）、航跡統合部３ａ、ならびに第１の航跡表示部４（＃１）、及び第２の航跡表示部４（＃２）から構成されている。
【００２９】
第１のｎ次元角度センサ１（＃１）及び第２のｎ次元角度センサ１（＃２）は、探知した目標の角度情報を取得し、それぞれ対応する角度追尾部２（＃１）または２（＃２）、ならびに航跡統合部３ａに送出する。第１の角度追尾部２（＃１）及び第２の角度追尾部２（＃２）は、それぞれに対応するｎ次元角度センサ１からの角度情報に基づいて、第１の角度追尾部２（＃１）は、目標の角度航跡ｚ（ｋ，１）及びその誤差共分散行列Ｐｚ（ｋ，１）を、また第２の角度追尾部２（＃２）は、目標の角度航跡ｚ（ｋ，２）及びその誤差共分散行列Ｐｚ（ｋ，２）を算出し、航跡統合部３ａに送出する。
【００３０】
航跡統合部３ａは、後述するが、追尾フィルタの異なる２つの航跡維持部を有しており、角度追尾部２（＃１）及び２（＃２）での算出結果を受けて、同一の目標に対してそれぞれの航跡維持部で算出した目標の航跡を統合し、第１の航跡表示部４（＃１）及び第２の航跡表示部４（＃２）に送出する。ここに、航跡統合部３ａは、相関部３１ａ、仮説距離生成部３５、航跡初期値算出部（１）３６、航跡維持部（１）３７、距離算出部３２ａ、航跡初期値算出部（２）３３ａ、航跡維持部（２）３４ａ、事後確率算出部３８、及び統合航跡算出部３９から構成されている。
【００３１】
相関部３１ａは、２つの角度追尾部２（＃１）及び２（＃２）からの角度航跡を入力し、それぞれで取得した角度航跡の関連づけを行ってどの対が同一目標のものであるかを判定する相関処理を行い、同一目標の角度航跡の対を後段に送出する。また、同一目標の角度航跡の対から、新目標の出現についても判定する。なお、この相関部３１ａでの処理には、例えば、特許文献１に記載の手法等を適用することができる。
【００３２】
仮説距離生成部３５は、相関部３１ａからの角度航跡に基づいて、２つのｎ次元角度センサ１（＃１）及び１（＃２）から目標までの仮説距離を算出し、航跡初期値算出部（１）３６に送出する。仮説距離の算出にあたっては、例えば、非特許文献２等に詳述される手法を適用できる。この手法の主要な処理部分を、以下の式（１７）〜式（１９）に示す。
【数１７】
【数１８】
【数１９】
【００３３】
ここで、ｒｍｉｎは目標の最小距離、ｒｍａｘは目標の最大距離、ｍは仮説距離の数（＝サブフィルタの数ｍ、ｍは１以上）、ｒ（ｊ）（ｊ＝１〜ｍ）はｍ個の仮説距離、σｒ（ｊ）（ｊ＝１〜ｍ）は距離誤差の標準偏差である。
【００３４】
航跡初期値算出部（１）３６は、これら仮説距離をもとに、後段の航跡維持部（１）３７でこの目標の追尾に必要な初期値となる、目標のｎ＋１次元航跡とその誤差評価量としての誤差共分散行列を算出し、航跡維持部（１）３７に送出する。これら初期値となるｎ＋１次元航跡とその誤差評価量の算出にあたっては、例えば、非特許文献２等に詳述される手法を適用できる。この手法の主要な処理部分については、例えば、航跡維持部（１）３７が２次元航跡を維持する場合、航跡としてのｊ番目（ｊ＝１〜ｍ）のサブフィルタの平滑ベクトルｘ＾（ｋ｜ｋ，ｊ）と平滑誤差共分散行列Ｐ（ｋ｜ｋ，ｊ）は、以下の式（２０）〜式（２１）で表される。
【数２０】
【数２１】
【００３５】
ここで、ｘ＾（ｋ，ｉ）は、ｎ次元角度センサ１（＃ｉ）の状態ベクトル、ｆｐ２ｃは、共分散行列の極座標・直交座標変換関数、σｖは、速度誤差の標準偏差を示す。
【００３６】
航跡維持部（１）３７は、航跡初期値算出部（１）３６での算出結果を初期値として、２つのｎ次元角度センサ１（＃１）及び１（＃２）からの探知情報に基づき目標の追尾維持処理を実行し、その航跡を継続的に取得する。ここでは、目標の運動モデル及び観測モデルを、それぞれ式（２２）及び式（２３）として、目標の追尾維持処理を、レンジパラメタライズド拡張カルマンフィルタにより構成した場合について説明する。
【数２２】
【数２３】
【００３７】
ここで、ｘ（ｋ）は、観測時刻ｔｋにおける目標の状態ベクトル、Ｆ（ｋ，ｋ−１）とＧ（ｋ，ｋ−１）は、観測時刻ｔｋ−１からｔｋにおける遷移行列と駆動行列、ｗ（ｋ−１）は、観測時刻ｔｋ−１における平均０、共分散行列Ｑ（ｋ−１）の正規分布に従うシステム雑音ベクトル、ｙ（ｋ，ｉ）は、観測時刻ｔｋにおけるｎ次元角度センサ１（＃ｉ）の観測ベクトル、ｈ（・）は、観測関数、ｖ（ｋ，ｉ）は、観測時刻ｔｋにおける平均０、共分散行列Ｒ（ｋ，ｉ）の正規分布に従う観測雑音ベクトルである。
【００３８】
航跡維持部（１）３７をレンジパラメタライズド拡張カルマンフィルタで構成した場合、ｍ個のサブフィルタを構成してこれらによる処理が行なわれる。ｊ番目（ｊ＝１〜ｍ）のサブフィルタでは、式（２４）〜式（２５）に示す予測処理、及び式（２６）〜式（３０）に示す平滑処理を実施する。
【数２４】
【数２５】
【数２６】
【数２７】
【数２８】
【数２９】
【数３０】
【００３９】
ここで、ｘ＾（ｋ｜ｋ−１，ｊ）とＰ（ｋ｜ｋ−１，ｊ）は、予測ベクトルと予測誤差共分散行列、Ｓ（ｋ，ｊ）、ｄ（ｋ，ｊ）およびＫ（ｋ，ｊ）は、残差共分散行列、マハラノビス距離およびカルマンゲイン行列、ｘ＾（ｋ｜ｋ，ｊ）とＰ（ｋ｜ｋ，ｊ）は、平滑ベクトルと平滑誤差共分散行列である（全て、観測時刻ｔｋのｊ番目のサブフィルタにおける値）。
【００４０】
観測時刻ｔｋにおける平滑ベクトルｘ＾（ｋ｜ｋ）と平滑誤差共分散行列Ｐ（ｋ｜ｋ）は、以下の式で算出される。
【数３１】
【数３２】
【数３３】
【数３４】
【００４１】
ここで、ｌ（ｋ，ｊ）は尤度、ｐ（ｋ，ｊ）とｗ（ｋ，ｊ）は、事後確率と重みである（全て、観測時刻ｔｋのｊ番目のサブフィルタにおける値）。
【００４２】
なお、上記した事例では、航跡維持部（１）３７をレンジパラメタライズド拡張カルマンフィルタで構成した場合を詳述したが、航跡維持部（１）３７は、レンジパラメタライズド・アンセンテッドカルマンフィルタやレンジパラメタライズド・パーティクルフィルタで構成しても良い。また、航跡維持部（１）３７がレンジパラメタライズド拡張カルマンフィルタまたはレンジパラメタライズド・アンセンテッドカルマンフィルタで構成される場合、航跡維持部（１）３７は、ｍ個のサブフィルタにより算出・更新されるｍ個の目標のｎ＋１次元航跡の中から、所定の観測回数または観測時間が経過した時点で最も事後確率の高い１つを選択し、以降はこの選択したサブフィルタによる目標のｎ＋１次元航跡を出力するように構成しても良い。
【００４３】
距離算出部３２ａは、相関部３１ａからの角度航跡に基づいて、２つのｎ次元角度センサ１（＃１）及び１（＃２）から目標までのそれぞれの距離を算出し、航跡初期値算出部（２）３３ａに送出する。距離の算出にあたっては、例えば、非特許文献１に記載の手法等を適用することができる。
【００４４】
航跡初期値算出部（２）３３ａは、目標の角度航跡及び距離の算出結果をもとに、後段の航跡維持部（２）３４ａでこの目標の追尾に必要な初期値となる、この目標のｎ＋１次元航跡とその誤差評価量としての誤差共分散行列を算出する。算出にあたっては、距離算出部３２ａにより算出された距離と各ｎ次元角度センサ１との角度航跡から、位置のみを含み速度を含まないｎ＋１次元航跡を算出し、複数回（２回以上）のｎ＋１次元航跡とその誤差評価量から、これら航跡初期値を算出することができる。また、距離算出部３２ａにより算出された距離と各ｎ次元角度センサ１との角度航跡から、位置のみを含み速度を含まないｎ＋１次元航跡を算出し、前述したレンジパラメタライズド拡張カルマンフィルタの初期値のように、航跡初期速度を０として、航跡初期値を算出するように構成することができる。なお、上述した距離算出部３２ａ及び航跡初期値算出部（２）３３ａにおける各値の算出手法の代わりに、特許文献１に記載の手法等を適用することができる。
【００４５】
航跡維持部（２）３４ａは、航跡初期値算出部（２）３３ａでの算出結果を初期値として、２つのｎ次元角度センサ１（＃１）及び１（＃２）からの探知情報に基づき目標の追尾維持処理を実行し、その航跡を継続的に取得する。この航跡維持部（２）３４ａで実行される追尾維持処理は、図２０に例示した従来の目標追尾装置における航跡維持部３４と同じ内容であり、説明を省略する。なお、拡張カルマンフィルタの代わりに、アンセンテッドカルマンフィルタやパーティクルフィルタで構成しても良い。
【００４６】
事後確率算出部３８は、航跡維持部（１）３７、及び航跡維持部（２）３４ａから目標のｎ＋１次元航跡を受けとり、それぞれからのｎ＋１次元航跡に対する事後確率及び重みを算出する。事後確率及び重みの算出は、以下の式（３５）〜式（３８）に示した処理手法による。
【数３５】
【数３６】
【数３７】
【数３８】
【００４７】
ここで、ｌ（ｋ，ＲＰ）、ｐ（ｋ，ＲＰ）及びｗ（ｋ，ＲＰ）は、それぞれ、航跡維持部（１）３７の尤度、事後確率及び重み、ｌ（ｋ，ＥＫＦ）、ｐ（ｋ，ＥＫＦ）及びｗ（ｋ，ＥＫＦ）は、航跡維持部（２）３４ａの尤度、事後確率および重みである（全て、観測時刻ｔｋにおける値）。また、Ｓ（ｋ，ＥＫＦ）とｄ（ｋ，ＥＫＦ）は、それぞれ式（５）と式（６）との値を用い、Ｓ（ｋ，ＲＰ）及びｄ（ｋ，ＲＰ）は、以下の式（３９）〜式（４２）で算出する。
【数３９】
【数４０】
【数４１】
【数４２】
【００４８】
統合航跡算出部３９は、航跡維持部（１）３７、及び航跡維持部（２）３４ａのそれぞれで取得された目標のｎ＋１次元航跡を、事後確率算出部３８で算出されたそれぞれの事後確率及び重みに基づき統合し、統合航跡を算出する。統合航跡の算出にあたっては、事後確率に基づく重み付けにより、以下の式（４３）〜式（４４）を用いて算出する。
【数４３】
【数４４】
【００４９】
ここで、ｘ＾（ｋ，ＲＰ）とＰ（ｋ，ＲＰ）は、式（３３）と式（３４）の値を用い、ｘ＾（ｋ，ＥＫＦ）とＰ（ｋ，ＥＫＦ）は、式（８）と式（９）の値を用いる。また、事後確率により重み付けせずに、航跡維持部（１）３７、または航跡維持部（２）３４ａのそれぞれで取得された目標のｎ＋１次元航跡のうち、事後確率の高いいずれか一方を選択してこれを統合航跡とするように構成することもできる。
【００５０】
航跡表示部４（＃１）及び航跡表示部４（＃２）は、このようにして航跡統合部３ａで算出された目標の統合航跡を、それぞれｎ次元角度センサ１（＃１）及び１（＃２）を基準にして表示する。
【００５１】
次に、前出の図１、ならびに図２〜図３のフローチャート及び図４の説明図を参照して、上述のように構成された本実施例の目標追尾装置の動作について、航跡統合部３ａにおける処理の流れを中心に説明する。図２及び図３は、図１に例示した目標追尾装置の第１の実施例の動作を説明するためのフローチャートである。
【００５２】
まず、航跡統合部３ａは、２つの角度追尾部２（＃１）及び２（＃２）から新たな目標の角度航跡、すなわち航跡の初期値を算出済みでない角度航跡を受けとると（ＳＴ１０１のＮ）、相関部３１ａがこれら角度航跡を入力し（ＳＴ１０２）、次いで、追尾維持処理に必要な一連の初期値算出処理を行う（ＳＴ１０３）。このＳＴ１０３での初期値算出では、最初に２つの角度追尾部２（＃１）及び２（＃２）からの角度航跡間のどの対が同一目標であるかを判定する相関処理を行ない（ＳＴ１０７）、その結果、同一目標と判定された新たな角度航跡の対がある場合には（ＳＴ１０８のＹ）、後段の航跡維持部（１）３７、及び航跡維持部（２）３４ａでのそれぞれの追尾維持処理における初期値を、同時並行的に算出する。（ＳＴ１０９）。
【００５３】
すなわち、相関部３１ａからの新たな角度航跡の対に基づいて、仮説距離生成部３５においては、２つのｎ次元角度センサ１（＃１）及び１（＃２）から目標までの仮説距離を算出し（ＳＴ１１４）、さらに、これら仮説距離をもとに、航跡初期値算出部（１）３６において、航跡維持部（１）３７での追尾維持処理の初期値となる、この目標のｎ＋１次元航跡とその誤差評価量としての誤差共分散行列を算出し、航跡維持部（１）３７に送出する（ＳＴ１１５）。一方、これと並行して距離算出部３２ａでは、２つのｎ次元角度センサ１（＃１）及び１（＃２）から目標までのそれぞれの距離を算出し（ＳＴ１１６）、さらに、この算出結果をもとに、航跡初期値算出部（２）３３ａにおいて、航跡維持部（２）３４ａでの追尾維持処理の初期値となる、目標のｎ＋１次元航跡とその誤差評価量としての誤差共分散行列を算出し、航跡維持部（２）３４ａに送出する（ＳＴ１１７）。
【００５４】
次いで、再びＳＴ１０１のステップにより、対象目標の航跡の初期値が算出済である旨の判定がなされると（ＳＴ１０１のＹ）、航跡維持部（１）３７、及び航跡維持部（２）３４ａは、追尾維持処理に移行すべく、２つのｎ次元角度センサ１（＃１）及び１（＃２）で探知した角度情報を直接入力し始める（ＳＴ１０４）。そして、ＳＴ１０３のステップで算出済みの初期値に基づいて、それぞれの追尾維持処理を同時進行させながら目標のｎ＋１次元航跡の取得を維持継続し、さらに、これら２つの航跡維持部からの航跡を事後確率に基づいて統合し、統合航跡を得ている（ＳＴ１０５）。
【００５５】
すなわち、ＳＴ１０３のステップで算出済みの初期値、及び２つのｎ次元角度センサ１（＃１）及び１（＃２）からの角度情報に基づいて、航跡維持部（１）３７は、上述した手法により追尾維持処理を実行して目標のｎ＋１次元航跡を継続的に取得し、その結果を事後確率算出部３８、及び統合航跡算出部３９に送出する（ＳＴ１１０）。一方、これと並行して航跡維持部（２）３４ａも、自身の追尾維持処理を実行して目標のｎ＋１次元航跡を継続的に取得し、その結果を事後確率算出部３８、及び統合航跡算出部３９に送出する（ＳＴ１１１）。次いで、事後確率算出部３８は、これら２つのｎ＋１次元航跡のそれぞれについて事後確率を算出し、統合航跡算出部３９に送出すると（ＳＴ１１２）、統合航跡算出部３９では、事後確率を用いた重み付け、または事後確率の大小関係により２つのｎ＋１次元航跡を統合して、目標の統合航跡を得る。そして、その結果を航跡表示部４に送出し、航跡表示部４は、これを表示する（ＳＴ１１３）。この後は、動作終了が指示されるまで、上記した動作ステップを繰り返す（ＳＴ１０６）。
【００５６】
図４は、上記した航跡統合部３ａ内での処理の流れに対応した航跡統合部３ａ内主要部の動作タイミングの一例を、時間経過に沿ってモデル化して示す説明図である。相関部３１ａにより、同一目標の角度航跡の対から新たな目標と判定した角度航跡の対が抽出されると（Ｔ１１）、航跡初期値算出部（１）３６では、仮説距離生成部３５からの仮説距離を用いて、航跡維持部（１）３７での追尾維持処理に必要な航跡初期値として、目標のｎ＋１次元航跡とその誤差評価量が算出される（Ｔ１２）。同様に、航跡初期値算出部（２）３３ａでは、距離算出部３２ａで算出された距離を用いて、航跡維持部（２）３４ａでの追尾維持処理に必要な目標の航跡の初期値が算出される（Ｔ１３）。これらの初期値をもとに、航跡維持部（１）３７、及び航跡維持部（２）３４ａは、目標の追尾維持処理に移行し、それぞれの追尾維持処理が開始されて、目標のｎ＋１次元航跡が継続して取得される。この時に、航跡維持部（１）３７の追尾維持処理と航跡維持部（２）３４ａの追尾維持処理は、同時並行して進行する（Ｔ１４、及びＴ１５）。さらに、これらと並行して動作する統合航跡算出部３９では、航跡維持部（１）３７、及び航跡維持部（２）３４ａからの目標航跡が、事後確率算出部３８で算出した事後確率に基づいて統合され、統合航跡として航跡表示部４に送出される（Ｔ１６）。
【００５７】
なお、上記した実施例においては、距離算出部３２ａにおいて距離変化率を算出する必要がないため、２つの角度追尾部２（＃１）及び２（＃２）において各速度を算出する必要がなく、この結果、２つの角度追尾部２（＃１）及び２（＃２）を省略することができる。この時の目標追尾装置の構成のブロック図を図５に例示する。
【００５８】
以上説明したように、本実施例においては、２つのｎ次元角度センサからの目標の角度情報に対して、相関手段によって同一目標と判定された角度情報とｍ個（ｍは、１以上）の仮説距離に基づき、目標のｎ＋１次元空間での位置および速度よりなるｍ個のｎ＋１次元航跡とその誤差評価量を算出し、これを初期値として、２つのｎ次元角度センサのいずれかよりの新たな探知情報が得られるたびに、この探知情報を反映してｎ＋１次元航跡およびその誤差評価量を更新して、目標のｎ＋１次元追尾を行なう第１の追尾維持手段としての航跡維持部（１）と、相関手段によって同一目標と判定された角度情報から、交会法に基づいて目標の距離を算出し、この距離と２つのｎ次元角度センサの角度航跡とに基づき、目標のｎ＋１次元空間での位置および速度よりなるｎ＋１次元航跡とその誤差評価量を算出し、これを初期値として、２つのｎ次元角度センサのいずれかよりの新たな探知情報が得られるたびに、この探知情報を反映してｎ＋１次元航跡およびその誤差評価量を更新して、目標のｎ＋１次元追尾を行なう第２の追尾維持手段としての航跡維持部（２）を備え、さらに、これら２つの航跡維持部で取得された目標航跡を事後確率により統合／選択している。
【００５９】
これにより、２つの航跡維持部（１）及び（２）が目標の追尾維持処理に移行する際に、その初期値に仮説距離を用いて処理を開始し継続取得した目標航跡と、交会法により算出された距離を用いて処理を開始し継続取得した目標航跡とが統合されるので、目標に対する２つのｎ次元角度センサからの交会角が小さな場合でも、追尾安定性を向上することができる。
【実施例２】
【００６０】
図６は、本発明に係る目標追尾装置の第２の実施例の構成を示すブロック図である。また、図７〜図９は、この目標追尾装置の第２の実施例の動作を説明するためのフローチャート及び説明図である。この第２の実施例について、図１〜図５に示した第１の実施例の各部と同一の部分は同一の符号で示し、その説明は省略する。第２の実施例が第１の実施例と異なる点は、第１の実施例においては、相関部において新しい目標と判定した角度航跡の対に対して、２つの追尾維持処理、すなわち、航跡初期値の算出に目標との仮説距離を用いた、航跡維持部（１）による第１の追尾維持処理と、航跡初期値の算出に交会法による算出距離を用いた、航跡維持部（２）による第２の追尾維持処理とを、同時並行に進行させたのに対し、第２の実施例においては、相関部おいて新しい目標と判定した角度航跡の対に対して、２つのｎ次元角度センサによる交会角を算出し、この交会角の値の範囲によって、上記した２つの追尾維持処理のいずれか一方を選択してその後の目標の航跡を取得するようにした点である。以下、前出の図１〜図５、ならびに図６〜図９を参照して、その相違点のみを説明する。
【００６１】
図６に例示したように、この目標追尾装置は、同一に構成された第１のｎ次元角度センサ１（＃１）、及び第２のｎ次元角度センサ１（＃２）、同一に構成された第１の角度追尾部２（＃１）、及び第２の角度追尾部２（＃２）、航跡統合部３ｂ、ならびに第１の航跡表示部４（＃１）、及び第２の航跡表示部４（＃２）から構成されている。２つのｎ次元角度センサ１（＃１）及び１（＃２）、２つの角度追尾部２（＃１）及び２（＃２）、ならびに、２つの航跡表示部４（＃１）及び４（＃２）は、第１の実施例と同じであり、説明を省略する。また、航跡統合部３ｂは、相関部３１ａ、事前選択部４０、仮説距離生成部３５、航跡初期値算出部（１）３６、航跡維持部（１）３７、距離算出部３２ａ、航跡初期値算出部（２）３３ａ、及び航跡維持部（２）３４ａから構成されている。ここに、事前選択部４０を除く各構成は、第１の実施例と同じであり、説明を省略する。
【００６２】
事前選択部４０は、相関部３１ａにおいて新目標と判定した角度航跡の対に基づいて、２つのｎ次元角度センサ１（＃１）及び１（＃２）による目標の交会角を算出するとともに、この算出結果をあらかじめ設定されたしきい値と比較し、算出した交会角がしきい値未満の場合、すなわちこの場合は交会角が小さく、交会法により算出した目標との距離に大きな誤差が含まれるおそれがあるため、仮説距離生成部３５からの仮説距離に基づき航跡初期値算出部（１）３６で算出した初期値を用いた、航跡維持部（１）３７での追尾維持処理により、その後の目標のｎ＋１次元航跡を継続して取得するように選択制御を行う。一方、算出した交会角がしきい値以上の場合には、距離算出部３２ａからの交会法による算出距離に基づき航跡初期値算出部（２）３３ａで算出した初期値を用いた、航跡維持部（２）３４ａでの追尾維持処理により目標の航跡を継続して取得するように選択制御を行う。交会角の算出にあたっては、例えば、２つの角度追尾部２（＃１）及び２（＃２）からの目標の角度航跡、または２つのｎ次元角度センサ１（＃１）及び１（＃２）からの探知情報を、方向単位ベクトルｕ（ｋ，１）とｕ（ｋ，２）に変換し、２つの方向単位ベクトルの内積の逆余弦から算出する手法等を適用できる。なお、交会角としきい値の代わりに、交会角の余弦成分（すなわち、方向単位ベクトルの内積）としきい値の余弦成分を用いるように構成することもできる。
【００６３】
次に、図７及び図８のフローチャートを参照して、上述のように構成された第２の実施例の目標追尾装置の動作について、航跡統合部３ｂにおける処理の流れを中心に説明する。まず、航跡統合部３ｂは、２つの角度追尾部２（＃１）及び２（＃２）から航跡の初期値を算出済みでない角度航跡を受けとると（ＳＴ２０１のＮ）、相関部３１ａがこれら角度航跡を入力し（ＳＴ２０２）、次いで、追尾維持処理に必要な一連の初期値算出処理を行う（ＳＴ２０３）。
【００６４】
このＳＴ２０３での初期値算出では、最初に２つの角度追尾部２（＃１）及び２（＃２）からの角度航跡間のどの対が同一目標であるかを判定する相関処理を行ない（ＳＴ２０７）、その結果、同一目標と判定された新たな角度航跡の対がある場合には、これら角度航跡を事前選択部４０に送出する（ＳＴ２０８のＹ）。次いで、事前選択部４０は、この新たな目標に対する交会角を算出し、その算出結果をあらかじめ設定されたしきい値と比較する（ＳＴ２０９）。そして、その比較結果に基づいて、後段の追尾維持処理に必要な航跡初期値の算出を行う（ＳＴ２１０）。
【００６５】
すなわち、交会角がしきい値未満の場合には（ＳＴ２１４のしきい値未満）、角度航跡の対は仮説距離生成部３５に送られ、仮説距離生成部３５は、目標までの仮説距離を算出する（ＳＴ２１５）。さらに、この仮説距離と角度航跡とから、航跡初期値算出部（１）３６は、航跡初期値としての目標のｎ＋１次元航跡とその誤差評価量を算出して、航跡維持部（１）３７による以降の追尾維持処理に備える（ＳＴ２１６）。一方、交会角がしきい値以上の場合には（ＳＴ２１４のしきい値以上）、角度航跡の対は距離算出部３２ａに送られ、距離算出部３２ａは、交会法により目標との距離を算出する（ＳＴ２１７）。そして、航跡初期値算出部（２）３３ａにおいて航跡の初期値を算出し、航跡維持部（２）３４ａによる追尾維持処理に備える（ＳＴ２１８）。
【００６６】
次いで、再びＳＴ２０１のステップにより、対象目標の航跡の初期値が算出済みである旨の判定がなされると（ＳＴ２０１のＹ）、航跡維持部（１）３７、及び航跡維持部（２）３４ａは、追尾維持処理に移行すべく、２つのｎ次元角度センサ１（＃１）及び１（＃２）で探知した角度情報を直接入力し始める（ＳＴ２０４）。そして、ＳＴ２０９での事前選択の結果によって選択した、航跡維持部（１）３７、または航跡維持部（２）３４ａのどちらか一方の追尾維持処理で目標の航跡を継続取得して、追尾を維持する（ＳＴ２０５）。
【００６７】
すなわち、事前選択部４０において算出した交会角がしきい値未満の場合には（ＳＴ２１１のしきい値未満）、ＳＴ２１６のステップで算出した航跡初期値算出部（１）３６の算出結果をもとに、航跡維持部（１）３７が、２つのｎ次元角度センサ１（＃１）及び１（＃２）で探知した角度情報を入力して目標の航跡の取得を維持継続し、その結果を航跡表示部４に送出する（ＳＴ２１２）。一方、事前選択部４０において算出した交会角がしきい値以上の場合には（ＳＴ２１１のしきい値以上）、ＳＴ２１８のステップで算出した航跡初期値算出部（２）３３ａの算出結果をもとに、航跡維持部（２）３４ａが、２つのｎ次元角度センサ１（＃１）及び１（＃２）で探知した角度情報を入力して目標の航跡の取得を維持継続し、その結果を航跡表示部４に送出する（ＳＴ２１３）。この後は、動作終了が指示されるまで、上記した動作ステップを繰り返す（ＳＴ２０６）。
【００６８】
図９は、上記した航跡統合部３ｂ内での処理の流れに対応した航跡統合部３ｂ内主要部の動作タイミングの一例を、時間経過に沿ってモデル化して示す説明図である。相関部３１ａにより、同一目標の角度航跡の対から新たな目標と判定した角度航跡の対が抽出されると（Ｔ２１）、事前選択部４０により、交会角が算出され、その算出結果に基づいて以降の追尾維持処理として、航跡維持部（１）３７と航跡維持部（２）３４ａの何れか一方が選択される。事前選択部４０により、航跡維持部（１）３７による追尾維持処理が選択された場合、航跡初期値算出部（１）３６は、仮説距離生成部３５による距離算出結果に基づき航跡初期値を算出し（Ｔ２２）、航跡維持部（１）３７による追尾維持処理に移行する。一方、事前選択部４０により、航跡維持部（２）３４ａによる追尾維持処理が選択された場合、航跡初期値算出部（２）３３ａは、距離算出部３２ａによる距離算出結果に基づき航跡初期値を算出し（Ｔ２２）、航跡維持部（２）による追尾維持処理に移行する。そして、その後は、追尾終了まで、航跡維持部（１）３７と航跡維持部（２）３４ａの何れか一方の追尾維持処理が動作し、目標の航跡が継続取得される（Ｔ２３）。
【００６９】
以上説明したように、本実施例においては、相関部において新しい目標と判定した角度航跡の対に対して、事前選択部において２つのｎ次元角度センサによる交会角を算出し、この交会角の値の範囲によって、２つの追尾維持処理のいずれか一方を選択している。すなわち、算出した交会角がしきい値未満の場合、交会角がより小さいので、交会法により算出した目標との距離に、より大きな誤差が含まれるおそれがあるため、航跡初期値の算出に目標との仮説距離を用いた、航跡維持部（１）による第１の追尾維持処理を選択し、それ以外の場合は、航跡初期値の算出に交会法による算出距離を用いた、航跡維持部（２）による第２の追尾維持処理を選択して、その後の目標の航跡を継続取得している。これにより、第１の実施例と同様に追尾安定性の向上に加え、２つの追尾維持処理を同時並行に動作させず、どちらか一方を動作させることにより追尾維持しているので、装置の処理負荷を軽減することができる。
【実施例３】
【００７０】
図１０は、本発明に係る目標追尾装置の第３の実施例の構成を示すブロック図である。また、図１１〜図１２は、この目標追尾装置の第３の実施例の動作を説明するためのフローチャートである。この第３の実施例について、図１〜図９に示した第１の実施例及び第２の実施例の各部と同一の部分は同一の符号で示し、その説明は省略する。第３の実施例が第２の実施例と異なる点は、事前選択部において算出した目標の交会角に基づいて後段で目標の航跡を継続取得するための追尾維持処理を選択する際に、第２の実施例においては、２つの選択肢、すなわち、航跡初期値の算出に目標との仮説距離を用いた、航跡維持部（１）による第１の追尾維持処理と、航跡初期値の算出に交会法による算出距離を用いた、航跡維持部（２）による第２の追尾維持処理とを設け、いずれか一方を選択したのに対し、第３の実施例においては、さらに選択肢を増やし、これら２つの追尾維持処理を同時に選択して、第１の実施例で示したように、それぞれで取得した航跡を統合した統合航跡も選択できるようにしており、交会角の範囲に基づいて、３つの選択肢からいずれか１つを選択するようにした点である。以下、前出の図１〜９、ならびに図１０〜１２を参照して、その相違点のみを説明する。
【００７１】
図１０に例示したように、この目標追尾装置は、同一に構成された第１のｎ次元角度センサ１（＃１）、及び第２のｎ次元角度センサ１（＃２）、同一に構成された第１の角度追尾部２（＃１）、及び第２の角度追尾部２（＃２）、航跡統合部３ｃ、ならびに第１の航跡表示部４（＃１）、及び第２の航跡表示部４（＃２）から構成されている。２つのｎ次元角度センサ１（＃１）及び１（＃２）、２つの角度追尾部２（＃１）及び２（＃２）、ならびに、２つの航跡表示部４（＃１）及び４（＃２）は、前出の第１及び第２の実施例と同じであり、説明を省略する。
【００７２】
また、航跡統合部３ｃは、相関部３１ａ、事前選択部４０ａ、仮説距離生成部３５、航跡初期値算出部（１）３６、航跡維持部（１）３７、距離算出部３２ａ、航跡初期値算出部（２）３３ａ、航跡維持部（２）３４ａ、事後確率算出部３８、及び統合航跡算出部３９ａから構成されている。ここに、事前選択部４０ａ及び統合航跡算出部３９ａを除く各構成は、第１及び第２の実施例と同じであり、説明を省略する。
【００７３】
事前選択部４０ａは、相関部３１ａにおいて新目標と判定した角度航跡の対に基づいて、２つのｎ次元角度センサ１（＃１）及び１（＃２）による目標の交会角を算出するとともに、この算出結果をあらかじめ設定された２つのしきい値である、しきい値１及びしきい値２（しきい値２＞しきい値１）と比較し、その結果により、角度航跡の送出及び目標の航跡の継続取得について次のような選択制御を行う。すなわち、交会角が第１のしきい値未満（以下、第１の交会角範囲と表す）の場合、仮説距離生成部３５からの仮説距離に基づき航跡初期値算出部（１）３６で算出した初期値を用いた、航跡維持部（１）３７での追尾維持処理により、その後の目標のｎ＋１次元航跡を継続して取得するように選択制御を行う。また、交会角が第１のしきい値以上、第２のしきい値未満（以下、第２の交会角範囲と表す）の場合、上記した航跡維持部（１）３７での追尾維持処理と、距離算出部３２ａからの交会法による算出距離に基づき航跡初期値算出部（２）３３ａで算出した初期値を用いた、航跡維持部（２）３４ａでの追尾維持処理とを共に動作させ、これら２つの追尾維持処理により取得した航跡を統合して目標の航跡を継続取得するように選択制御を行う。また、交会角が第２のしきい値以上（以下、第３の交会角範囲と表す）の場合、上記した航跡維持部（２）３４ａでの追尾維持処理により、その後の目標の航跡を継続して取得するように選択制御を行う。
【００７４】
統合航跡算出部３９ａは、第１の実施例における統合航跡算出部３９と同様の処理機能に加え、事前選択部４０ａからの選択制御に基づいて、目標の航跡を、航跡維持部（１）３７での追尾維持処理による航跡、航跡維持部（２）３４ａでの追尾維持処理による航跡、または、これらを統合した航跡のいずれかから選択し、後段の航跡表示部４に送出する。
【００７５】
次に、図１１及び図１２のフローチャートを参照して、上述のように構成された第３の実施例の目標追尾装置の動作について、航跡統合部３ｃにおける処理の流れを中心に説明する。まず、航跡統合部３ｃは、２つの角度追尾部２（＃１）及び２（＃２）から航跡の初期値を算出済みでない角度航跡を受けとると（ＳＴ３０１のＮ）、相関部３１ａがこれら角度航跡を入力し（ＳＴ３０２）、次いで、追尾維持処理に必要な一連の初期値算出処理を行う（ＳＴ３０３）。
【００７６】
このＳＴ３０３での初期値算出では、最初に２つの角度追尾部２（＃１）及び２（＃２）からの角度航跡間のどの対が同一目標であるかを判定する相関処理を行ない（ＳＴ３０７）、その結果、同一目標と判定された新たな角度航跡の対がある場合には、これら角度航跡を事前選択部４０ａに送出する（ＳＴ３０８のＹ）。次いで、事前選択部４０ａは、この新たな目標に対する交会角を算出し、その算出結果をあらかじめ設定された２つのしきい値と比較する（ＳＴ３０９）。そして、その比較結果に基づいて、以下の３つの手順のいずれかにより、後段の追尾維持処理に必要な航跡初期値の算出を行う（ＳＴ３１０）。
【００７７】
すなわち、交会角が第１の交会角範囲の場合には（ＳＴ３１５の第１の交会角範囲）、角度航跡の対は仮説距離生成部３５に送られ、仮説距離生成部３５は、目標までの仮説距離を算出する（ＳＴ３１６）。さらに、この仮説距離と角度航跡とから、航跡初期値算出部（１）３６は、航跡初期値としての目標のｎ＋１次元航跡とその誤差評価量を算出して、航跡維持部（１）３７による以降の追尾維持処理に備える（ＳＴ３１７）。また、交会角が第３の交会角範囲の場合には（ＳＴ３１５の第３の交会角範囲）、角度航跡の対は距離算出部３２ａに送られ、距離算出部３２ａは、交会法により目標との距離を算出する（ＳＴ３１８）。さらに、航跡初期値算出部（２）３３ａにおいて航跡の初期値を算出し、航跡維持部（２）３４ａによる追尾維持処理に備える（ＳＴ３１９）。また、交会角が、第２の交会角範囲の場合には（ＳＴ３１５の第２の交会角範囲）、上記した２つの初期値算出を共に行って、航跡維持部（１）３７及び航跡維持部（２）３４ａによる２つの追尾維持処理の同時動作による統合航跡の取得に備える。
【００７８】
次いで、再びＳＴ３０１のステップにより、対象目標の航跡の初期値が算出済みである旨の判定がなされると（ＳＴ３０１のＹ）、航跡維持部（１）３７、及び航跡維持部（２）３４ａは、追尾維持処理に移行すべく、２つのｎ次元角度センサ１（＃１）及び１（＃２）で探知した角度情報を直接入力し始める（ＳＴ３０４）。そして、ＳＴ３０９での事前選択の結果によって選択した、航跡維持部（１）３７の追尾維持処理、航跡維持部（２）３４ａの追尾維持処理、または、これらの統合結果のいずれか１つにより目標の航跡を継続取得して、追尾を維持する（ＳＴ３０５）。
【００７９】
すなわち、事前選択部４０ａにおいて算出した交会角が第１の交会角範囲の場合には（ＳＴ３１１の第１の交会角範囲）、ＳＴ３１７のステップで算出した航跡初期値算出部（１）３６の算出結果をもとに、航跡維持部（１）３７が、２つのｎ次元角度センサ１（＃１）及び１（＃２）で探知した角度情報を入力して目標の航跡の取得を維持継続し、その結果を統合航跡算出部３９ａを経由して航跡表示部４に送出する（ＳＴ３１２）。一方、事前選択部４０ａにおいて算出した交会角が第３の交会角範囲の場合には（ＳＴ３１１の第３の交会角範囲）、ＳＴ３１９のステップで算出した航跡初期値算出部（２）３３ａの算出結果をもとに、航跡維持部（２）３４ａが、２つのｎ次元角度センサ１（＃１）及び１（＃２）で探知した角度情報を入力して目標の航跡の取得を維持継続し、その結果を統合航跡算出部３９ａを経由して航跡表示部４に送出する（ＳＴ３１４）。これに対して、事前選択部４０ａにおいて算出した交会角が第２の交会角範囲の場合には（ＳＴ３１１の第２の交会角範囲）、図３（ｃ）に示したＳＴ１０５と同様の動作ステップにより、航跡維持部（１）３７及び航跡維持部（２）３４ａでそれぞれに継続取得した目標の航跡を、事後確率算出部３８での算出結果に基づいて統合航跡算出部３９ａにて統合し、この統合航跡を航跡表示部４に送出する（ＳＴ３１３）。この後は、動作終了が指示されるまで、上記した動作ステップを繰り返す（ＳＴ３０６）。
【００８０】
上述した航跡統合部３ｃ内での処理の流れに対応した航跡統合部３ｃ内の主要部の動作タイミングについては、第１の実施例の図４、及び第２の実施例の図９を組み合わせたものとなる。すなわち、事前選択部４０ａにおける交会角の判定が第２の交会角範囲の場合は、図４と同様な動作タイミングとなり、第１の交会角範囲、または第３の交会角範囲の場合は、図９と同様な動作タイミングとなる。
【００８１】
以上説明したように、本実施例においては、相関部において新しい目標と判定した角度航跡の対に対して、事前選択部において２つのｎ次元角度センサによる交会角を算出し、この交会角の値の範囲によって、目標の航跡を継続取得する追尾維持処理を選択している。すなわち、交会角が最も小さな範囲である第１の交会角範囲の場合は、航跡初期値の算出に目標との仮説距離を用いた、航跡維持部（１）による第１の追尾維持処理を選択し、交会角が最も大きい範囲である第３の交会角範囲の場合は、航跡初期値の算出に交会法による算出距離を用いた、航跡維持部（２）による第２の追尾維持処理を選択し、交会角がこれらの中間値となる第２の交会角範囲の場合は、航跡維持部（１）及び航跡維持部（２）のそれぞれの追尾維持処理で取得した航跡を統合して、その後の目標の航跡を継続取得している。これにより、交会角が小さな場合でも目標に対する追尾安定性を向上させることができるとともに、交会角の範囲によっては追尾維持処理を同時並行に動作させないので、装置の処理負荷を軽減することができる。
【実施例４】
【００８２】
図１３は、本発明に係る目標追尾装置の第４の実施例の構成を示すブロック図である。また、図１４〜図１６は、この目標追尾装置の第４の実施例の動作を説明するためのフローチャート及び説明図である。この第４の実施例について、上述した第１〜第３の実施例の各部と同一の部分は同一の符号で示し、その説明は省略する。第４の実施例が第１の実施例と異なる点は、第１の実施例においては、航跡維持部（１）、及び航跡維持部（２）のそれぞれの追尾維持処理からの目標航跡を事後確率算出部で算出した事後確率に基づいて統合し、この統合航跡を追尾終了まで継続取得することによって追尾維持するのに対し、第４の実施例においては、追尾維持処理に移行後、所定の経過時間までは同様に統合航跡を継続取得するが、その後は、航跡維持部（１）、及び航跡維持部（２）のそれぞれの追尾維持処理による目標航跡のうち、事後確率の大きなどちらか一方を選択し、この選択した側の目標航跡を継続取得することによって追尾維持するように構成した点である。
【００８３】
以下、前出の図１〜図５、ならびに図１３〜図１６を参照して、その相違点のみを説明する。
【００８４】
図１３に例示したように、この目標追尾装置は、同一に構成された第１のｎ次元角度センサ１（＃１）、及び第２のｎ次元角度センサ１（＃２）、同一に構成された第１の角度追尾部２（＃１）、及び第２の角度追尾部２（＃２）、航跡統合部３ｄ、ならびに第１の航跡表示部４（＃１）、及び第２の航跡表示部４（＃２）から構成されている。２つのｎ次元角度センサ１（＃１）及び１（＃２）、２つの角度追尾部２（＃１）及び２（＃２）、ならびに、２つの航跡表示部４（＃１）及び４（＃２）は、第１の実施例と同じであり、説明を省略する。また、航跡統合部３ｄは、相関部３１ａ、仮説距離生成部３５、航跡初期値算出部（１）３６、航跡維持部（１）３７、距離算出部３２ａ、航跡初期値算出部（２）３３ａ、及び航跡維持部（２）３４ａ、事後確率算出部３８、事後選択部４１、及び統合航跡算出部３９ｂから構成されている。ここに、事後選択部４１、及び統合航跡算出部３９ｂを除く各構成は、第１の実施例と同じであり、説明を省略する。
【００８５】
事後選択部４１は、追尾維持処理の初期値が算出されて追尾維持処理に移行後、所定の時間が経過した時点で、事後確率算出部３８で算出した、航跡維持部（１）３７、及び航跡維持部（２）３４ａのそれぞれの追尾維持処理による目標航跡の事後確率を参照し、所定の時間経過以降は、事後確率の大きなどちらか一方からの目標航跡を採用して追尾維持を継続するように選択制御を行う。統合航跡算出部３９ｂは、第１の実施例における統合航跡算出部３９と同様の処理機能に加え、事後選択部４１からの選択制御に基づいて、所定の時間が経過するまでは統合航跡を、また所定の時間経過後は、航跡維持部（１）３７、または、航跡維持部（２）３４ａのそれぞれの追尾維持処理による目標航跡のうちで、どちらか一方を選択し、後段の航跡表示部４に送出する。
【００８６】
次に、図１４及び図１５のフローチャートを参照して、上述のように構成された第４の実施例の目標追尾装置の動作について、航跡統合部３ｄにおける処理の流れを中心に説明する。まず、航跡統合部３ｄは、２つの角度追尾部２（＃１）及び２（＃２）から航跡の初期値を算出済みでない角度航跡を受けとると（ＳＴ４０１のＮ）、相関部３１ａがこれら角度航跡を入力し（ＳＴ４０２）、次いで、追尾維持処理に必要な一連の初期値算出処理を行う（ＳＴ４０３）。このＳＴ４０３での初期値算出手順は、図２（ｂ）、及び図３（ｄ）に示した第１の実施例における手順と同様である。
【００８７】
次いで、再びＳＴ４０１のステップにより、対象目標の航跡の初期値が算出済である旨の判定がなされると（ＳＴ４０１のＹ）、航跡維持部（１）３７、及び航跡維持部（２）３４ａは、追尾維持処理に移行すべく、２つのｎ次元角度センサ１（＃１）及び１（＃２）で探知した角度情報を直接入力し始める（ＳＴ４０４）。そして、ＳＴ４０３のステップで算出済みの初期値に基づいて、それぞれの追尾維持処理を同時進行させながら目標航跡の取得を維持継続し、さらに、これら２つの航跡維持部からの航跡を事後確率に基づき統合した統合航跡を得るとともに、所定の時間経過した時点で事後選択を行い、事後確率の大きなどちらか一方の目標航跡を選択して追尾維持する（ＳＴ４０５）。
【００８８】
次に、このＳＴ４０５のステップについて、図１５を参照して詳述する。事後選択は、追尾維持処理に移行後、所定の時間経過した時点で行われるため、所定の時間が経過するまでは、事後選択がなされていないので（ＳＴ４０７のＮ）、航跡維持部（１）３７、及び航跡維持部（２）３４ａにおけるそれぞれの追尾維持処理を同時進行させながら、これらを統合した統合航跡を継続取得し、後段の航跡表示部４に送出する（ＳＴ４０８）。このＳＴ４０８での統合航跡の算出手順は、図３（ｃ）に示した第１の実施例における手順と同様である。所定の時間が経過するまでは（ＳＴ４０９のＮ）、この統合航跡を繰り返し取得することによって追尾維持する。そして、所定時間が経過すると（ＳＴ４０９のＹ）、事後選択部４１は、事後確率算出部３８からの事後確率に基づいて、事後選択を行う。すなわち、以降は統合航跡により追尾維持するのではなく、同時並列に動作していた航跡維持部（１）３７、及び航跡維持部（２）３４ａのそれぞれの追尾維持処理による目標航跡の事後確率を参照し、事後確率の大きなどちらか一方からの目標航跡を採用して追尾維持を継続するように選択制御を行う（ＳＴ４１０）。
【００８９】
このようにして所定の時間が経過し事後選択がなされた後は（ＳＴ４０７のＹ）、再びＳＴ４０４のステップで次の角度情報が探知されると、これら探知情報は、事後選択された航跡維持部（１）３７、または航跡維持部３４ａのどちらかに送られる（ＳＴ４１１）。そして、事後選択されたどちらかの航跡維持部（１）３７、または航跡維持部（２）３４ａは、それぞれの航跡維持処理により目標航跡の取得を継続して追尾維持するとともに、取得した目標航跡を後段の航跡表示部４に送出し表示する（ＳＴ４１２、またはＳＴ４１３）。この後は、動作終了が指示されるまで、上記した動作ステップを繰り返す（ＳＴ４０６）。
【００９０】
図１６は、上記した航跡統合部３ｄ内での処理の流れに対応した航跡統合部３ｄ内主要部の動作タイミングの一例を、時間経過に沿ってモデル化して示す説明図である。相関部３１ａにより、同一目標の角度航跡の対から新目標と判定した角度航跡の対が抽出されると（Ｔ４１）、航跡初期値算出部（１）３６では、仮説距離生成部３５からの仮説距離を用いて、航跡維持部（１）３７での追尾維持処理に必要な航跡初期値として、目標のｎ＋１次元航跡とその誤差評価量が算出される（Ｔ４２）。同様に、航跡初期値算出部（２）３３ａでは、距離算出部３２ａで算出された距離を用いて、航跡維持部（２）３４ａでの追尾維持処理に必要な、目標の航跡の初期値が算出される（Ｔ４３）。
【００９１】
これらの初期値をもとに、航跡維持部（１）３７、及び航跡維持部（２）３４ａは、目標の追尾維持処理に移行し、それぞれの追尾維持処理が開始されて、目標のｎ＋１次元航跡が継続して取得される。この時に、航跡維持部（１）３７の追尾維持処理と航跡維持部（２）３４ａの追尾維持処理は、同時並行して進行する（Ｔ４４、及びＴ４５）。さらに、これらと並行して動作する統合航跡算出部３９では、航跡維持部（１）３７、及び航跡維持部（２）３４ａからの目標航跡が、事後確率算出部３８で算出した事後確率に基づいて統合され、統合航跡として航跡表示部４に送出される（Ｔ４６）。その後、所定の時間が経過すると、統合航跡に換えて、事後選択部４１により選択された航跡維持部（１）３７または航跡維持部（２）３４ａのいずれか一方が、追尾終了まで動作する。図１６では、事後選択部４１により、航跡維持部（１）３７が選択された場合を例示している（Ｔ４７）。
【００９２】
以上説明したように、本実施例においては、目標の追尾維持処理に移行後、所定の時間が経過して追尾安定性が高まるまでは統合航跡を継続取得して追尾維持し、その後、追尾安定性が高まった以降は、航跡維持部（１）、及び航跡維持部（２）のそれぞれの追尾維持処理による目標航跡のうち、事後確率の大きなどちらか一方を選択し、この選択した側の目標航跡を継続取得することによって追尾維持している。これにより、交会角が小さな場合でも目標に対する追尾安定性を向上させることができるとともに、追尾維持処理に移行して所定の時間経過後は、同時並行に動作する２つの追尾維持処理のうち、どちらか一方を動作させて目標の航跡を継続取得しているので、時間経過に伴い追尾維持処理が収束するに従って、装置の処理負荷を軽減することができる。
【００９３】
なお、本実施例では、事後選択４１が事後選択を行なうタイミングとして、経過時間を用いる場合を説明したが、事後選択を行なうタイミングは、これ以外に、観測回数が所定の回数を超えた場合とすることもできる。また、事後確率のどちらか一方が所定の確率を超えた場合、さらには、それらの条件の組み合わせを満足するタイミング等、種々の中から選択することができる。
【実施例５】
【００９４】
図１７は、本発明に係る目標追尾装置の第５の実施例の構成を示すブロック図である。また、図１８〜図１９は、この目標追尾装置の第５の実施例の動作を説明するためのフローチャートである。この第５の実施例について、上述した第１〜第４の実施例の各部と同一の部分は同一の符号で示し、その説明は省略する。第５の実施例においては、第３の実施例における事前選択部の処理機能と、第４の実施例における事後選択部の処理機能とを同時に具備した構成としている。すなわち、新たな目標に対しては、事前選択として、各ｎ次元角度センサからの交会角の範囲に基づき目標の航跡を継続取得するための追尾維持処理を選択するとともに、この事前選択で航跡維持部（１）と航跡維持部（２）からの航跡を統合した統合航跡が採用された場合には、事後選択として、追尾維持処理に移行後、所定の経過時間までは統合航跡を継続取得し、その後は、事後確率に基づき航跡維持部（１）または航跡維持部（２）のどちらか一方の追尾維持処理を選択して追尾維持するように構成している。以下、前出の図１０〜図１６、ならびに図１７〜１９を参照して、その相違点のみを説明する。
【００９５】
図１７に例示したように、この目標追尾装置は、同一に構成された第１のｎ次元角度センサ１（＃１）、及び第２のｎ次元角度センサ１（＃２）、同一に構成された第１の角度追尾部２（＃１）、及び第２の角度追尾部２（＃２）、航跡統合部３ｅ、ならびに第１の航跡表示部４（＃１）、及び第２の航跡表示部４（＃２）から構成されている。２つのｎ次元角度センサ１（＃１）及び１（＃２）、２つの角度追尾部２（＃１）及び２（＃２）、ならびに、２つの航跡表示部４（＃１）及び４（＃２）は、前出の各実施例と同じであり、説明を省略する。また、航跡統合部３ｅは、相関部３１ａ、事前選択部４０ａ、仮説距離生成部３５、航跡初期値算出部（１）３６、航跡維持部（１）３７、距離算出部３２ａ、航跡初期値算出部（２）３３ａ、及び航跡維持部（２）３４ａ、事後確率算出部３８、事後選択部４１ａ、及び統合航跡算出部３９ｃから構成されている。ここに、事後選択部４１ａ、及び統合航跡算出部３９ｃを除く各構成は、前出の第３及び第４の実施例と同じであり、説明を省略する。
【００９６】
事後選択部４１ａは、事前選択部４０ａにおける選択制御において、目標の交会角が第２の交会角範囲の場合に対応する、統合航跡による目標の追尾維持が選択された場合に、前出の第４の実施例における事後選択部４１と同様の処理機能を果たすよう構成される。また、統合航跡算出部３９ｃは、事前選択部４０ａにおける選択制御において、統合航跡による目標の追尾維持が選択された場合には、前出の第４の実施例における統合航跡算出部３９ｂと同様の処理機能を果たすとともに、事前選択部４０ａにおける選択制御において、航跡維持部（１）３７、または航跡維持部（２）３４ａの追尾維持処理による目標の追尾維持が選択された場合には、これら選択制御に該当する追尾維持処理による目標航跡を後段の航跡表示部４に送出する。
【００９７】
次に、図１８及び図１９のフローチャートを参照して、上述のように構成された第５の実施例の目標追尾装置の動作について、航跡統合部３ｅにおける処理の流れを中心に説明する。まず、航跡統合部３ｅは、２つの角度追尾部２（＃１）及び２（＃２）から航跡の初期値を算出済みでない角度航跡を受けとると（ＳＴ５０１のＮ）、相関部３１ａがこれら角度航跡を入力し（ＳＴ５０２）、次いで、追尾維持処理に必要な一連の初期値算出処理を行う（ＳＴ５０３）。このＳＴ５０３での初期値算出手順は、図１１（ｂ）、及び図１２（ｄ）に示した第３の実施例における手順と同様である。すなわち、事前選択部４０ａにおける交会角の判定において、交会角が第１の交会角範囲の場合には、航跡初期値算出部（１）３６により初期値を算出し、また、交会角が第２の交会角範囲の場合には、航跡初期値算出部（１）３６、及び航跡初期値算出部（２）３３ａの２つの初期値算出部で共に初期値を算出し、さらに、交会角が第３の交会角範囲の場合には航跡初期値算出部（２）３３ａにより初期値を算出して、以降の航跡維持部（１）３７及び航跡維持部（２）３４ａによる追尾維持処理に備える。
【００９８】
次いで、再びＳＴ５０１のステップにより、対象目標の航跡の初期値が算出済みである旨の判定がなされると（ＳＴ５０１のＹ）、航跡維持部（１）３７、及び航跡維持部（２）３４ａは、追尾維持処理に移行すべく、２つのｎ次元角度センサ１（＃１）及び１（＃２）で探知した角度情報を直接入力し始める（ＳＴ５０４）。そして、事前選択部４０ａにおける交会角による選択制御に基づいて、航跡維持部（１）３７の追尾維持処理、航跡維持部（２）３４ａの追尾維持処理、または、これらの統合結果のいずれか１つにより目標の航跡を継続取得して、追尾を維持する（ＳＴ５０７）。
【００９９】
すなわち、目標の交会角が第１の交会角範囲の場合には（ＳＴ５０７の第１の交会角範囲）、航跡維持部（１）３７が、２つのｎ次元角度センサ１（＃１）及び１（＃２）で探知した角度情報を入力して目標の航跡の取得を維持継続し、その結果を統合航跡算出部３９ｃを経由して航跡表示部４に送出する（ＳＴ５０８）。また、目標の交会角が第２の交会角範囲の場合には（ＳＴ５０７の第２の交会角範囲）、所定の経過時間まで統合航跡を継続取得し、所定の時間を経過した後は、航跡維持部（１）３７、及び航跡維持部（２）３４ａのそれぞれの追尾維持処理による目標航跡のうち、事後確率算出部３８で算出した事後確率の大きなどちらか一方を選択し、この選択した側の目標航跡を継続取得することによって追尾維持するとともに、この目標航跡を航跡表示部４に送出する（ＳＴ５０９）。このＳＴ５０９での処理手順は、図１５（ｂ）に示した第４の実施例におけるＳＴ４０５の詳細な処理手順と同様である。
【０１００】
また、目標の交会角が第３の交会角範囲の場合には（ＳＴ５０７の第３の交会角範囲）、航跡維持部（１）３４ａが、２つのｎ次元角度センサ１（＃１）及び１（＃２）で探知した角度情報を入力して目標の航跡の取得を維持継続し、その結果を統合航跡算出部３９ｃを経由して航跡表示部４に送出する（ＳＴ５１０）。この後は、動作終了が指示されるまで、上記した動作ステップを繰り返す（ＳＴ５０６）。
【０１０１】
上述した航跡統合部３ｅ内での処理の流れに対応した航跡統合部３ｅ内の主要部の動作タイミングについては、第２の実施例の図９、及び第４の実施例の図１６を組み合わせたものとなる。すなわち、事前選択部４０ａにおける交会角の判定が、第１の交会角範囲、または第３の交会角範囲の場合は、第２の実施例の図９と同様な動作タイミングとなり、第２の交会角範囲の場合は、第４の実施例の図１６と同様な動作タイミングとなる。
【０１０２】
以上説明したように、本実施例においては、相関部において新しい目標と判定した角度航跡の対に対して、事前選択部において２つのｎ次元角度センサによる交会角を算出した結果、交会角が最も小さな範囲である第１の交会角範囲の場合は、航跡初期値の算出に目標との仮説距離を用いた、航跡維持部（１）による第１の追尾維持処理を選択し、交会角が最も大きい範囲である第３の交会角範囲の場合は、航跡初期値の算出に交会法による算出距離を用いた、航跡維持部（２）による第２の追尾維持処理を選択し、交会角がこれらの中間値となる第２の交会角範囲の場合は、航跡維持部（１）及び航跡維持部（２）のそれぞれの追尾維持処理で取得した航跡を統合して、その後の目標の航跡を継続取得している。加えて、統合航跡を選択した場合は、追尾維持処理に移行後、追尾安定性が高まるまでは、２つの追尾維持処理による統合航跡を継続取得して追尾維持し、その後、所定の時間が経過して追尾安定性が高まった以降は、航跡維持部（１）、及び航跡維持部（２）のそれぞれの追尾維持処理による目標航跡のうち、事後確率の大きなどちらか一方を選択し、この選択した側の目標航跡を継続取得することによって追尾維持している。これにより、交会角が小さな場合でも目標に対する追尾安定性を向上させることができるとともに、目標の交会角の範囲によって１つの追尾維持処理のみを選択するか、あるいは、時間経過に伴い追尾維持処理が収束するに従って同時並行動作させた２つの追尾維持処理のどちらか一方を選択して目標の航跡を継続取得しているので、装置の処理負荷を軽減することができる。
【０１０３】
なお、本発明は、上記した実施形態そのままに限定されるものではなく、実施段階ではその要旨を逸脱しない範囲で構成要素を変形して具体化できる。また、上記実施形態に開示されている複数の構成要素の適宜な組み合せにより種々の発明を形成できる。例えば、実施形態に示される全構成要素から幾つかの構成要素を削除してもよい。さらに、異なる実施形態に亘る構成要素を適宜組み合せてもよい。
【符号の説明】
【０１０４】
１ ｎ次元角度センサ
２ 角度追尾部
３、３ａ、３ｂ、３ｃ、３ｄ、３ｅ 航跡統合部
４ 航跡表示部
３１、３１ａ 相関部
３２ 距離／距離変化率算出部
３２ａ 距離算出部
３３ ｎ＋１次元航跡初期値算出部
３３ａ 航跡初期値算出部（２）
３４ 航跡維持部
３４ａ 航跡維持部（２）
３５ 仮説距離生成部
３６ 航跡初期値算出部（１）
３７ 航跡維持部（１）
３８ 事後確率算出部
３９、３９ａ、３９ｂ、３９ｃ 統合航跡算出部
４０、４０ａ 事前選択部
４１、４１ａ 事後選択部
【特許請求の範囲】
【請求項１】
互いに位置の異なる複数の角度センサにより探知された複数の目標のｎ次元（ｎは１または２）角度情報を同一の目標毎に対をなすように関連づける相関手段と、
前記相関手段により同一の目標に関連づけられた角度情報と、この目標に対して設定されたｍ個（ｍは１以上）の仮説距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｍ個のｎ＋１次元航跡とその誤差評価量とを算出する第１の初期値算出手段と、
前記第１の初期値算出手段によって算出されたｍ個のｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第１の追尾維持手段と、
前記相関手段により同一の目標に関連づけられた角度情報と、これら角度情報の対を用いて交会法に基づき算出した前記複数の各角度センサとこの目標までの距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｎ＋１次元航跡とその誤差評価量とを算出する第２の初期値算出手段と、
前記第２の初期値算出手段によって算出されたｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第２の追尾維持手段と、
前記第１の追尾維持手段、及び前記第２の追尾維持手段の事後確率を算出する事後確率算出手段と、
前記第１の追尾維持手段、及び前記第２の追尾維持手段のそれぞれにより算出された前記目標のｎ＋１次元航跡を統合する目標航跡統合手段とを備え、
前記目標航跡統合手段は、前記事後確率算出手段により算出された第１及び第２の追尾維持手段の事後確率に基づいて、それぞれの追尾維持手段により算出された前記目標のｎ＋１次元航跡を統合することを特徴とする目標追尾装置。
【請求項２】
互いに位置の異なる複数の角度センサにより探知された複数の目標のｎ次元（ｎは１または２）角度情報を同一の目標毎に対をなすように関連づける相関手段と、
前記相関手段により同一の目標に関連づけられた角度情報と、この目標に対して設定されたｍ個（ｍは１以上）の仮説距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｍ個のｎ＋１次元航跡とその誤差評価量とを算出する第１の初期値算出手段と、
前記第１の初期値算出手段によって算出されたｍ個のｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第１の追尾維持手段と、
前記相関手段により同一の目標に関連づけられた角度情報と、これら角度情報の対を用いて交会法に基づき算出した前記複数の各角度センサとこの目標までの距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｎ＋１次元航跡とその誤差評価量とを算出する第２の初期値算出手段と、
前記第２の初期値算出手段によって算出されたｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第２の追尾維持手段と、
前記複数の角度センサと前記目標との交会角に基づいて前記第１の追尾維持手段または前記第２の追尾維持手段のいずれかを選択的に動作させる追尾事前選択手段とを備え、
前記追尾事前選択手段は、
前記交会角が所定のしきい値未満の場合には、前記第１の追尾維持手段を動作させるとともに、この第１の追尾維持手段により算出された前記目標のｎ＋１次元航跡を採用し、
前記交会角がこの所定のしきい値以上の場合には、前記第２の追尾維持手段を動作させるとともに、この第２の追尾維持手段により算出された前記目標のｎ＋１次元航跡を採用する
ことを特徴とする目標追尾装置。
【請求項３】
互いに位置の異なる複数の角度センサにより探知された複数の目標のｎ次元（ｎは１または２）角度情報を同一の目標毎に対をなすように関連づける相関手段と、
前記相関手段により同一の目標に関連づけられた角度情報と、この目標に対して設定されたｍ個（ｍは１以上）の仮説距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｍ個のｎ＋１次元航跡とその誤差評価量とを算出する第１の初期値算出手段と、
前記第１の初期値算出手段によって算出されたｍ個のｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第１の追尾維持手段と、
前記相関手段により同一の目標に関連づけられた角度情報と、これら角度情報の対を用いて交会法に基づき算出した前記複数の各角度センサとこの目標までの距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｎ＋１次元航跡とその誤差評価量とを算出する第２の初期値算出手段と、
前記第２の初期値算出手段によって算出されたｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第２の追尾維持手段と、
前記第１の追尾維持手段、及び前記第２の追尾維持手段の事後確率を算出する事後確率算出手段と、
前記事後確率算出手段からの事後確率に基づいて、前記第１の追尾維持手段、及び前記第２の追尾維持手段のそれぞれにより算出された前記目標のｎ＋１次元航跡を統合する目標航跡統合手段と、
前記複数の角度センサと前記目標との交会角に基づいて前記第１の追尾維持手段または前記第２の追尾維持手段のいずれか一方または両者を選択して動作させる追尾事前選択手段とを備え、
前記追尾事前選択手段は、
前記交会角が第１のしきい値未満の場合には、前記第１の追尾維持手段を動作させるとともに、この第１の追尾維持手段により算出された前記目標のｎ＋１次元航跡を採用し、
前記交会角が第１のしきい値以上かつ第２のしきい値（第２のしきい値＞第１のしきい値）未満の場合には、前記第１の追尾維持手段及び前記第２の追尾維持手段の両者を動作させるとともに、前記目標航跡統合手段により統合された前記目標のｎ＋１次元航跡を採用し、
前記交会角が第２のしきい値以上の場合には、前記第２の追尾維持手段を動作させるとともに、この第２の追尾維持手段により算出された前記目標のｎ＋１次元航跡を採用する
ことを特徴とする目標追尾装置。
【請求項４】
互いに位置の異なる複数の角度センサにより探知された複数の目標のｎ次元（ｎは１または２）角度情報を同一の目標毎に対をなすように関連づける相関手段と、
前記相関手段により同一の目標に関連づけられた角度情報と、この目標に対して設定されたｍ個（ｍは１以上）の仮説距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｍ個のｎ＋１次元航跡とその誤差評価量とを算出する第１の初期値算出手段と、
前記第１の初期値算出手段によって算出されたｍ個のｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第１の追尾維持手段と、
前記相関手段により同一の目標に関連づけられた角度情報と、これら角度情報の対を用いて交会法に基づき算出した前記複数の各角度センサとこの目標までの距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｎ＋１次元航跡とその誤差評価量とを算出する第２の初期値算出手段と、
前記第２の初期値算出手段によって算出されたｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第２の追尾維持手段と、
前記第１の追尾維持手段、及び前記第２の追尾維持手段の事後確率を算出する事後確率算出手段と、
前記事後確率算出手段からの事後確率に基づいて、前記第１の追尾維持手段、及び前記第２の追尾維持手段のそれぞれにより算出された前記目標のｎ＋１次元航跡を統合する目標航跡統合手段と、
前記事後確率算出手段からの事後確率に基づいて前記第１の追尾維持手段、前記第２の追尾維持手段、または前記目標航跡統合手段のいずれかにより算出された前記目標のｎ＋１次元航跡を選択して採用する追尾事後選択手段とを備え、
前記追尾事後選択手段は、
所定の観測回数または観測時間が経過するまでは、前記目標航跡統合手段により統合された前記目標のｎ＋１次元航跡を採用し、
その後は、この所定の観測回数または観測時間が経過した時点で算出した、前記第１の追尾維持手段及び前記第２の追尾維持手段の事後確率を参照し、その値の高いいずれか一方の追尾維持手段により算出された前記目標のｎ＋１次元航跡を採用する
ことを特徴とする目標追尾装置。
【請求項５】
互いに位置の異なる複数の角度センサにより探知された複数の目標のｎ次元（ｎは１または２）角度情報を同一の目標毎に対をなすように関連づける相関手段と、
前記相関手段により同一の目標に関連づけられた角度情報と、この目標に対して設定されたｍ個（ｍは１以上）の仮説距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｍ個のｎ＋１次元航跡とその誤差評価量とを算出する第１の初期値算出手段と、
前記第１の初期値算出手段によって算出されたｍ個のｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第１の追尾維持手段と、
前記相関手段により同一の目標に関連づけられた角度情報と、これら角度情報の対を用いて交会法に基づき算出した前記複数の各角度センサとこの目標までの距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｎ＋１次元航跡とその誤差評価量とを算出する第２の初期値算出手段と、
前記第２の初期値算出手段によって算出されたｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第２の追尾維持手段と、
前記第１の追尾維持手段、及び前記第２の追尾維持手段の事後確率を算出する事後確率算出手段と、
前記事後確率算出手段からの事後確率に基づいて、前記第１の追尾維持手段、及び前記第２の追尾維持手段のそれぞれにより算出された前記目標のｎ＋１次元航跡を統合する目標航跡統合手段と、
前記複数の角度センサと前記目標との交会角に基づいて前記第１の追尾維持手段または前記第２の追尾維持手段のいずれか一方または両者を選択して動作させる追尾事前選択手段と、
前記事後確率算出手段からの事後確率に基づいて前記第１の追尾維持手段、前記第２の追尾維持手段、または前記目標航跡統合手段のいずれかにより算出された前記目標のｎ＋１次元航跡を選択して採用する追尾事後選択手段とを備え、
前記追尾事後選択手段は、
所定の観測回数または観測時間が経過するまでは、前記目標航跡統合手段により統合された前記目標のｎ＋１次元航跡を採用し、
その後は、この所定の観測回数または観測時間が経過した時点で算出した、前記第１の追尾維持手段及び前記第２の追尾維持手段の事後確率を参照し、その値の高いいずれか一方の追尾維持手段により算出された前記目標のｎ＋１次元航跡を採用し、
前記追尾事前選択手段は、
前記交会角が第１のしきい値未満の場合には、前記第１の追尾維持手段を動作させるとともに、この第１の追尾維持手段により算出された前記目標のｎ＋１次元航跡を採用し、
前記交会角が第１のしきい値以上かつ第２のしきい値（第２のしきい値＞第１のしきい値）未満の場合には、前記第１の追尾維持手段及び前記第２の追尾維持手段の両者を動作させるとともに、前記追尾事後選択手段による前記目標のｎ＋１次元航跡を採用し、
前記交会角が第２のしきい値以上の場合には、前記第２の追尾維持手段を動作させるとともに、この第２の追尾維持手段により算出された前記目標のｎ＋１次元航跡を採用する
ことを特徴とする目標追尾装置。
【請求項６】
前記第１の追尾維持手段は、レンジパラメタライズド拡張カルマンフィルタ（Ｒａｎｇｅ Ｐａｒａｍｅｔｅｒｉｚｅｄ Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ）、レンジパラメタライズド・アンセンテッドカルマンフィルタ（Ｒａｎｇｅ Ｐａｒａｍｅｔｅｒｉｚｅｄ Ｕｎｓｃｅｎｔｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ）、または、レンジパラメタライズド・パーティクルフィルタ（Ｒａｎｇｅ Ｐａｒａｍｅｔｅｒｉｚｅｄ Ｐａｒｔｉｃｌｅ Ｆｉｌｔｅｒ）で構成されることを特徴とする請求項１乃至請求項５のいずれか１項に記載の目標追尾装置。
【請求項７】
前記第１の初期値算出手段、及び前記第２の初期値算出手段は、前記追尾事前選択手段により選択された前記第１の追尾維持手段、または前記第２の追尾維持手段に対応するいずれか一方のみ動作することを特徴とする請求項２、請求項３、及び請求項５のいずれか１項に記載の目標追尾装置。
【請求項８】
前記目標航跡統合手段は、
前記事後確率算出手段で算出した、前記第１の追尾維持手段及び前記第２の追尾維持手段のそれぞれに対する事後確率の高い、いずれか一方に対応した追尾維持手段からの前記目標のｎ＋１次元航跡を出力する、または、
前記第１の追尾維持手段及び前記第２の追尾維持手段からの前記目標のｎ＋１次元航跡を、それぞれの事後確率に基づき重み付き平均して出力することを特徴とする請求項１、及び請求項３乃至請求項５のいずれか１項に記載の目標追尾装置。
【請求項９】
前記第１の追尾維持手段がレンジパラメタライズド拡張カルマンフィルタ（Ｒａｎｇｅ Ｐａｒａｍｅｔｅｒｉｚｅｄ Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ）、またはレンジパラメタライズド・アンセンテッドカルマンフィルタ（Ｒａｎｇｅ Ｐａｒａｍｅｔｅｒｉｚｅｄ Ｕｎｓｃｅｎｔｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ）で構成される場合、
前記第１の追尾維持手段は、前記ｍ個の仮説距離のそれぞれに対応して設けられたサブフィルタにより算出・更新されるｍ個の前記目標のｎ＋１次元航跡の中から、所定の観測回数または観測時間が経過した時点で最も事後確率の高い１つを選択し、以降はこの選択したサブフィルタによる前記目標のｎ＋１次元航跡を出力することを特徴とする請求項１乃至請求項５のいずれか１項に記載の目標追尾装置。
【請求項１】
互いに位置の異なる複数の角度センサにより探知された複数の目標のｎ次元（ｎは１または２）角度情報を同一の目標毎に対をなすように関連づける相関手段と、
前記相関手段により同一の目標に関連づけられた角度情報と、この目標に対して設定されたｍ個（ｍは１以上）の仮説距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｍ個のｎ＋１次元航跡とその誤差評価量とを算出する第１の初期値算出手段と、
前記第１の初期値算出手段によって算出されたｍ個のｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第１の追尾維持手段と、
前記相関手段により同一の目標に関連づけられた角度情報と、これら角度情報の対を用いて交会法に基づき算出した前記複数の各角度センサとこの目標までの距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｎ＋１次元航跡とその誤差評価量とを算出する第２の初期値算出手段と、
前記第２の初期値算出手段によって算出されたｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第２の追尾維持手段と、
前記第１の追尾維持手段、及び前記第２の追尾維持手段の事後確率を算出する事後確率算出手段と、
前記第１の追尾維持手段、及び前記第２の追尾維持手段のそれぞれにより算出された前記目標のｎ＋１次元航跡を統合する目標航跡統合手段とを備え、
前記目標航跡統合手段は、前記事後確率算出手段により算出された第１及び第２の追尾維持手段の事後確率に基づいて、それぞれの追尾維持手段により算出された前記目標のｎ＋１次元航跡を統合することを特徴とする目標追尾装置。
【請求項２】
互いに位置の異なる複数の角度センサにより探知された複数の目標のｎ次元（ｎは１または２）角度情報を同一の目標毎に対をなすように関連づける相関手段と、
前記相関手段により同一の目標に関連づけられた角度情報と、この目標に対して設定されたｍ個（ｍは１以上）の仮説距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｍ個のｎ＋１次元航跡とその誤差評価量とを算出する第１の初期値算出手段と、
前記第１の初期値算出手段によって算出されたｍ個のｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第１の追尾維持手段と、
前記相関手段により同一の目標に関連づけられた角度情報と、これら角度情報の対を用いて交会法に基づき算出した前記複数の各角度センサとこの目標までの距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｎ＋１次元航跡とその誤差評価量とを算出する第２の初期値算出手段と、
前記第２の初期値算出手段によって算出されたｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第２の追尾維持手段と、
前記複数の角度センサと前記目標との交会角に基づいて前記第１の追尾維持手段または前記第２の追尾維持手段のいずれかを選択的に動作させる追尾事前選択手段とを備え、
前記追尾事前選択手段は、
前記交会角が所定のしきい値未満の場合には、前記第１の追尾維持手段を動作させるとともに、この第１の追尾維持手段により算出された前記目標のｎ＋１次元航跡を採用し、
前記交会角がこの所定のしきい値以上の場合には、前記第２の追尾維持手段を動作させるとともに、この第２の追尾維持手段により算出された前記目標のｎ＋１次元航跡を採用する
ことを特徴とする目標追尾装置。
【請求項３】
互いに位置の異なる複数の角度センサにより探知された複数の目標のｎ次元（ｎは１または２）角度情報を同一の目標毎に対をなすように関連づける相関手段と、
前記相関手段により同一の目標に関連づけられた角度情報と、この目標に対して設定されたｍ個（ｍは１以上）の仮説距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｍ個のｎ＋１次元航跡とその誤差評価量とを算出する第１の初期値算出手段と、
前記第１の初期値算出手段によって算出されたｍ個のｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第１の追尾維持手段と、
前記相関手段により同一の目標に関連づけられた角度情報と、これら角度情報の対を用いて交会法に基づき算出した前記複数の各角度センサとこの目標までの距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｎ＋１次元航跡とその誤差評価量とを算出する第２の初期値算出手段と、
前記第２の初期値算出手段によって算出されたｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第２の追尾維持手段と、
前記第１の追尾維持手段、及び前記第２の追尾維持手段の事後確率を算出する事後確率算出手段と、
前記事後確率算出手段からの事後確率に基づいて、前記第１の追尾維持手段、及び前記第２の追尾維持手段のそれぞれにより算出された前記目標のｎ＋１次元航跡を統合する目標航跡統合手段と、
前記複数の角度センサと前記目標との交会角に基づいて前記第１の追尾維持手段または前記第２の追尾維持手段のいずれか一方または両者を選択して動作させる追尾事前選択手段とを備え、
前記追尾事前選択手段は、
前記交会角が第１のしきい値未満の場合には、前記第１の追尾維持手段を動作させるとともに、この第１の追尾維持手段により算出された前記目標のｎ＋１次元航跡を採用し、
前記交会角が第１のしきい値以上かつ第２のしきい値（第２のしきい値＞第１のしきい値）未満の場合には、前記第１の追尾維持手段及び前記第２の追尾維持手段の両者を動作させるとともに、前記目標航跡統合手段により統合された前記目標のｎ＋１次元航跡を採用し、
前記交会角が第２のしきい値以上の場合には、前記第２の追尾維持手段を動作させるとともに、この第２の追尾維持手段により算出された前記目標のｎ＋１次元航跡を採用する
ことを特徴とする目標追尾装置。
【請求項４】
互いに位置の異なる複数の角度センサにより探知された複数の目標のｎ次元（ｎは１または２）角度情報を同一の目標毎に対をなすように関連づける相関手段と、
前記相関手段により同一の目標に関連づけられた角度情報と、この目標に対して設定されたｍ個（ｍは１以上）の仮説距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｍ個のｎ＋１次元航跡とその誤差評価量とを算出する第１の初期値算出手段と、
前記第１の初期値算出手段によって算出されたｍ個のｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第１の追尾維持手段と、
前記相関手段により同一の目標に関連づけられた角度情報と、これら角度情報の対を用いて交会法に基づき算出した前記複数の各角度センサとこの目標までの距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｎ＋１次元航跡とその誤差評価量とを算出する第２の初期値算出手段と、
前記第２の初期値算出手段によって算出されたｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第２の追尾維持手段と、
前記第１の追尾維持手段、及び前記第２の追尾維持手段の事後確率を算出する事後確率算出手段と、
前記事後確率算出手段からの事後確率に基づいて、前記第１の追尾維持手段、及び前記第２の追尾維持手段のそれぞれにより算出された前記目標のｎ＋１次元航跡を統合する目標航跡統合手段と、
前記事後確率算出手段からの事後確率に基づいて前記第１の追尾維持手段、前記第２の追尾維持手段、または前記目標航跡統合手段のいずれかにより算出された前記目標のｎ＋１次元航跡を選択して採用する追尾事後選択手段とを備え、
前記追尾事後選択手段は、
所定の観測回数または観測時間が経過するまでは、前記目標航跡統合手段により統合された前記目標のｎ＋１次元航跡を採用し、
その後は、この所定の観測回数または観測時間が経過した時点で算出した、前記第１の追尾維持手段及び前記第２の追尾維持手段の事後確率を参照し、その値の高いいずれか一方の追尾維持手段により算出された前記目標のｎ＋１次元航跡を採用する
ことを特徴とする目標追尾装置。
【請求項５】
互いに位置の異なる複数の角度センサにより探知された複数の目標のｎ次元（ｎは１または２）角度情報を同一の目標毎に対をなすように関連づける相関手段と、
前記相関手段により同一の目標に関連づけられた角度情報と、この目標に対して設定されたｍ個（ｍは１以上）の仮説距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｍ個のｎ＋１次元航跡とその誤差評価量とを算出する第１の初期値算出手段と、
前記第１の初期値算出手段によって算出されたｍ個のｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第１の追尾維持手段と、
前記相関手段により同一の目標に関連づけられた角度情報と、これら角度情報の対を用いて交会法に基づき算出した前記複数の各角度センサとこの目標までの距離とに基づいて、この目標のｎ＋１次元空間での位置及び速度からなるｎ＋１次元航跡とその誤差評価量とを算出する第２の初期値算出手段と、
前記第２の初期値算出手段によって算出されたｎ＋１次元航跡とその誤差評価量とを初期値として、前記複数の角度センサのいずれかにより前記目標に対する新たな角度情報が取得されるたび毎に、この新たな角度情報を反映してｎ＋１次元航跡とその誤差評価量とを更新し、前記目標のｎ＋１次元追尾を行う第２の追尾維持手段と、
前記第１の追尾維持手段、及び前記第２の追尾維持手段の事後確率を算出する事後確率算出手段と、
前記事後確率算出手段からの事後確率に基づいて、前記第１の追尾維持手段、及び前記第２の追尾維持手段のそれぞれにより算出された前記目標のｎ＋１次元航跡を統合する目標航跡統合手段と、
前記複数の角度センサと前記目標との交会角に基づいて前記第１の追尾維持手段または前記第２の追尾維持手段のいずれか一方または両者を選択して動作させる追尾事前選択手段と、
前記事後確率算出手段からの事後確率に基づいて前記第１の追尾維持手段、前記第２の追尾維持手段、または前記目標航跡統合手段のいずれかにより算出された前記目標のｎ＋１次元航跡を選択して採用する追尾事後選択手段とを備え、
前記追尾事後選択手段は、
所定の観測回数または観測時間が経過するまでは、前記目標航跡統合手段により統合された前記目標のｎ＋１次元航跡を採用し、
その後は、この所定の観測回数または観測時間が経過した時点で算出した、前記第１の追尾維持手段及び前記第２の追尾維持手段の事後確率を参照し、その値の高いいずれか一方の追尾維持手段により算出された前記目標のｎ＋１次元航跡を採用し、
前記追尾事前選択手段は、
前記交会角が第１のしきい値未満の場合には、前記第１の追尾維持手段を動作させるとともに、この第１の追尾維持手段により算出された前記目標のｎ＋１次元航跡を採用し、
前記交会角が第１のしきい値以上かつ第２のしきい値（第２のしきい値＞第１のしきい値）未満の場合には、前記第１の追尾維持手段及び前記第２の追尾維持手段の両者を動作させるとともに、前記追尾事後選択手段による前記目標のｎ＋１次元航跡を採用し、
前記交会角が第２のしきい値以上の場合には、前記第２の追尾維持手段を動作させるとともに、この第２の追尾維持手段により算出された前記目標のｎ＋１次元航跡を採用する
ことを特徴とする目標追尾装置。
【請求項６】
前記第１の追尾維持手段は、レンジパラメタライズド拡張カルマンフィルタ（Ｒａｎｇｅ Ｐａｒａｍｅｔｅｒｉｚｅｄ Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ）、レンジパラメタライズド・アンセンテッドカルマンフィルタ（Ｒａｎｇｅ Ｐａｒａｍｅｔｅｒｉｚｅｄ Ｕｎｓｃｅｎｔｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ）、または、レンジパラメタライズド・パーティクルフィルタ（Ｒａｎｇｅ Ｐａｒａｍｅｔｅｒｉｚｅｄ Ｐａｒｔｉｃｌｅ Ｆｉｌｔｅｒ）で構成されることを特徴とする請求項１乃至請求項５のいずれか１項に記載の目標追尾装置。
【請求項７】
前記第１の初期値算出手段、及び前記第２の初期値算出手段は、前記追尾事前選択手段により選択された前記第１の追尾維持手段、または前記第２の追尾維持手段に対応するいずれか一方のみ動作することを特徴とする請求項２、請求項３、及び請求項５のいずれか１項に記載の目標追尾装置。
【請求項８】
前記目標航跡統合手段は、
前記事後確率算出手段で算出した、前記第１の追尾維持手段及び前記第２の追尾維持手段のそれぞれに対する事後確率の高い、いずれか一方に対応した追尾維持手段からの前記目標のｎ＋１次元航跡を出力する、または、
前記第１の追尾維持手段及び前記第２の追尾維持手段からの前記目標のｎ＋１次元航跡を、それぞれの事後確率に基づき重み付き平均して出力することを特徴とする請求項１、及び請求項３乃至請求項５のいずれか１項に記載の目標追尾装置。
【請求項９】
前記第１の追尾維持手段がレンジパラメタライズド拡張カルマンフィルタ（Ｒａｎｇｅ Ｐａｒａｍｅｔｅｒｉｚｅｄ Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ）、またはレンジパラメタライズド・アンセンテッドカルマンフィルタ（Ｒａｎｇｅ Ｐａｒａｍｅｔｅｒｉｚｅｄ Ｕｎｓｃｅｎｔｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ）で構成される場合、
前記第１の追尾維持手段は、前記ｍ個の仮説距離のそれぞれに対応して設けられたサブフィルタにより算出・更新されるｍ個の前記目標のｎ＋１次元航跡の中から、所定の観測回数または観測時間が経過した時点で最も事後確率の高い１つを選択し、以降はこの選択したサブフィルタによる前記目標のｎ＋１次元航跡を出力することを特徴とする請求項１乃至請求項５のいずれか１項に記載の目標追尾装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【公開番号】特開２０１０−２０３７９９（Ｐ２０１０−２０３７９９Ａ）
【公開日】平成２２年９月１６日（２０１０．９．１６）
【国際特許分類】
【出願番号】特願２００９−４６８００（Ｐ２００９−４６８００）
【出願日】平成２１年２月２７日（２００９．２．２７）
【出願人】（０００００３０７８）株式会社東芝 (54,554)
【Ｆターム（参考）】
【公開日】平成２２年９月１６日（２０１０．９．１６）
【国際特許分類】
【出願日】平成２１年２月２７日（２００９．２．２７）
【出願人】（０００００３０７８）株式会社東芝 (54,554)
【Ｆターム（参考）】
[ Back to top ]