(57)【特許請求の範囲】
【請求項１】
センサから入力された観測データを観測時刻ごとに蓄積する観測データ蓄積部と、
前記観測データ蓄積部に蓄積された観測時刻ごとの観測データのうち、航跡確立判定結果に基づいてどの観測時刻の観測データを推定に用いるかを選択する観測データ管理部と、
前記観測データ管理部により選択された観測時刻をすべて基準時刻として設定し、各基準時刻における航跡の候補を状態ベクトル候補として生成する選択時刻状態候補生成部と、
前記観測データ管理部により選択された観測データを用いて、前記状態ベクトル候補の尤度比関数を計算して、各基準時刻における尤度比関数が最大の状態ベクトル候補を探索する状態空間概探索部と、
前記状態空間概探索部により探索された各基準時刻における尤度比関数が最大の状態ベクトル候補を初期値として、前記観測データ管理部により選択された観測データを用いて、反復計算により各基準時刻における尤度比関数が最大の状態ベクトル推定値を探索する状態空間詳細探索部と、
前記各基準時刻における尤度比関数が最大の状態ベクトル推定値の対応関係から最新時刻の最も確からしい状態ベクトル推定値を決定する状態相関処理部と、
前記最新時刻の最も確からしい状態ベクトル推定値から航跡の確立を判定して前記航跡確立判定結果を出力する航跡確立判定部と
を備えたことを特徴とする追尾装置。
【請求項２】
前記状態相関処理部は、
前記各基準時刻における尤度比関数が最大の状態ベクトル推定値を、予め仮定した目標運動に従って最新時刻に至るまで前向きに外挿して状態ベクトル予測値を計算する予測処理部と、
前記各基準時刻における状態ベクトル推定値と前記予測処理部により計算された状態ベクトル予測値の対応関係の有無を判定するゲート内外判定部と、
前記各基準時刻における状態ベクトル推定値について、前記ゲート内外判定部により対応関係が有と判定された状態ベクトル推定値を１つのグループとしてまとめるグルーピング処理部と、
前記グルーピング処理部によりグループ化された状態ベクトル推定値を平滑処理することによって各グループの最新時刻における状態ベクトル平滑値を求める平滑処理部と、
前記各グループの最新時刻における状態ベクトル平滑値について、前記観測データ管理部により選択された観測データを用いて、尤度比関数を計算して尤度比関数が最大のグループの状態ベクトル平滑値を選択して航跡を決定する航跡決定部とを有する
ことを特徴とする請求項１記載の追尾装置。
【請求項３】
前記観測データ管理部により選択された観測時刻のうち、前記航跡決定部からの航跡決定結果に基づいて所定の観測時刻を現在の基準時刻として設定する基準時刻設定部をさらに備え、
前記選択時刻状態候補生成部は、前記現在の基準時刻における航跡の候補を状態ベクトル候補として生成し、
前記状態空間概探索部は、前記観測データ管理部により選択された観測データを用いて、前記状態ベクトル候補の尤度比関数を計算して、前記現在の基準時刻における尤度比関数が最大の状態ベクトル候補を探索し、
前記状態空間詳細探索部は、前記状態空間概探索部により探索された現在の基準時刻における尤度比関数が最大の状態ベクトル候補を初期値として、前記観測データ管理部により選択された観測データを用いて、反復計算により現在の基準時刻における尤度比関数が最大の状態ベクトル推定値を探索し、
前記状態相関処理部は、
前記状態空間詳細探索部により探索された現在の基準時刻における状態ベクトル推定値を蓄積する状態ベクトル推定値記憶部をさらに有し、
前記予測処理部は、前記状態ベクトル推定値記憶部に蓄積された過去の基準時刻における状態ベクトル推定値群を、予め仮定した目標運動に従って現在の基準時刻まで前向きに外挿して状態ベクトル予測値を計算し、
前記ゲート内外判定部は、過去及び現在の基準時刻における状態ベクトル推定値と前記予測処理部により計算された状態ベクトル予測値の対応関係の有無を判定し、
前記グルーピング処理部は、過去及び現在の基準時刻における状態ベクトル推定値について、前記ゲート内外判定部により対応関係が有と判定された状態ベクトル推定値を１つのグループとしてまとめる
ことを特徴とする請求項２記載の追尾装置。
【請求項４】
前記状態相関処理部により決定された最新時刻の最も確からしい状態ベクトル推定値を蓄積する最新時刻状態ベクトル推定値記憶部と、
前記最新時刻状態ベクトル推定値記憶部に蓄積された過去及び最新時刻の状態ベクトル推定値の対応関係から最新時刻の最も確からしい状態ベクトル推定値を決定する最新時刻状態相関処理部をさらに備え、
前記最新時刻状態相関処理部は、
前記過去及び最新時刻における尤度比関数が最大の状態ベクトル推定値を、予め仮定した目標運動に従って最新時刻まで前向きに外挿して状態ベクトル予測値を計算する予測処理部と、
前記過去及び最新時刻における状態ベクトル推定値と前記予測処理部により計算された状態ベクトル予測値の対応関係の有無を判定するゲート内外判定部と、
前記過去及び最新時刻における状態ベクトル推定値について、前記ゲート内外判定部により対応関係が有と判定された状態ベクトル推定値を１つのグループとしてまとめるグルーピング処理部と、
前記グルーピング処理部によりグループ化された状態ベクトル推定値を平滑処理することによって各グループの最新時刻における状態ベクトル平滑値を求める平滑処理部と、
前記各グループの最新時刻における状態ベクトル平滑値について、前記観測データ管理部により選択された観測データを用いて、尤度比関数を計算して尤度比関数が最大のグループの状態ベクトル平滑値を選択して航跡を決定する航跡決定部とを有する
ことを特徴とする請求項１記載の追尾装置。
【請求項５】
前記状態相関処理部は、
前記各基準時刻における尤度比関数が最大の状態ベクトル推定値を、予め仮定した目標運動に従って最新時刻に至るまで前向きに外挿して状態ベクトル予測値を計算する第１の予測処理部と、
前記各基準時刻における状態ベクトル推定値と前記第１の予測処理部により計算された状態ベクトル予測値の対応関係の有無を判定するゲート内外判定部と、
前記各基準時刻における状態ベクトル推定値について、前記ゲート内外判定部により対応関係が有と判定された状態ベクトル推定値を１つのグループとしてまとめるグルーピング処理部と、
前記グルーピング処理部によりグループ化された状態ベクトル推定値を平滑処理することによって各グループの最新時刻における状態ベクトル平滑値を求める平滑処理部とを
有し、
前記平滑処理部により求めた各グループの最新時刻における状態ベクトル平滑値を、予め仮定した目標運動に従って最新時刻より１サンプル未来時刻まで前向きに外挿して、各グループの状態ベクトル予測値を計算する第２の予測処理部と、
前記観測データ蓄積部から最新時刻より１サンプル未来時刻の観測データを取得し、前記最新時刻より１サンプル未来時刻の観測データと前記各グループの状態ベクトル予測値との対応関係の有無に基づいて最も確からしい状態ベクトル推定値を決定する検定処理部とをさらに備えた
ことを特徴とする請求項１記載の追尾装置。
【請求項６】
前記ゲート内外判定部は、前記状態ベクトル推定値と前記状態ベクトル予測値との間の距離と予め定めた閾値の大小関係に基づいて、前記対応関係の有無を判定する
ことを特徴とする請求項２から請求項５までのいずれか１項に記載の追尾装置。
【請求項７】
前記状態空間概探索部及び前記状態空間詳細探索部は、前記各基準時刻における状態ベクトル推定値を尤度比関数の大きいものから複数個抽出し、
前記状態相関処理部は、各基準時刻における複数個の状態ベクトル推定値の対応関係から最も確からしい状態ベクトル推定値を決定する
ことを特徴とする請求項１記載の追尾装置。
【請求項８】
前記検定処理部は、前記観測データと前記状態ベクトル予測値との間の距離と予め定めた閾値の大小関係に基づいて最も確からしい状態ベクトル推定値を決定する
ことを特徴とする請求項５記載の追尾装置。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、センサからの観測データを入力として、移動目標を追尾する追尾装置に関するものであり、特に、移動目標の航跡を最尤推定することにより追尾する追尾装置に関するものである。
【背景技術】
【０００２】
センサからの観測データを入力として、移動目標の最も確からしい航跡を推定する追尾装置としてＭＬ−ＰＤＡ（Maximum Likelihood-Probabilistic Data Association）を用いたものが知られている（例えば、非特許文献１参照）。
【０００３】
ＭＬ−ＰＤＡを用いた追尾装置は、移動目標の航跡を最尤推定法によって推定する。まず、移動目標の航跡をある時刻（以下、これを基準時刻と呼ぶ。）における目標の位置や速度といった状態ベクトルｘｋとして表現する。次に、状態ベクトルを予め仮定した目標運動に従って前向きあるいは後ろ向きに外挿し、観測データ群Ｚに基づいた状態ベクトルの確からしさを尤度比関数φ（Ｚ、ｘｋ）として表現する。そして、得られた観測データから尤度比関数φ（Ｚ、ｘｋ）の大域的最大点ｘｋＭＬを求めることで目標航跡を推定する。
【０００４】
以下、ＭＬ−ＰＤＡを用いた従来の追尾装置について図１３及び図１４を参照しながら説明する。図１３は、ＭＬ−ＰＤＡを用いた従来の追尾装置の構成を示すブロック図である。また、図１４は、ＭＬ−ＰＤＡを用いた従来の追尾装置の動作を示すフローチャートである。
【０００５】
図１３において、ＭＬ−ＰＤＡを用いた従来の追尾装置１Ｘは、観測データ蓄積部９１と、観測データ管理部９２と、状態候補生成部９３と、状態空間概探索部９４と、状態空間詳細探索部９５と、航跡確立判定部９６とが設けられている。
【０００６】
まず、ステップ９０１において、観測データ蓄積部９１は、センサから観測データを取得し、次のステップ９０２において、観測データ蓄積部９１は、取得した観測データを観測時刻ごとに蓄積する。
【０００７】
次に、ステップ９０３において、観測データ管理部９２は、観測データ蓄積部９１に蓄積された各時刻の観測データ群のうち、どの時刻の観測データ群を状態空間概探索部９４及び状態空間詳細探索部９５に入力するかを、航跡確立判定部９６の航跡確立判定結果を基に選択する。以下、選択した観測時刻を時刻ｔ１からｔＮまでとする。
【０００８】
次に、ステップ９０４において、状態候補生成部９３は、基準時刻を適当に設定したのち、基準時刻における状態ベクトルの候補を多数生成し、この状態ベクトル候補群を状態空間概探索部９４に出力する。
【０００９】
次に、ステップ９０５において、状態空間概探索部９４は、状態候補生成部９３から入力された状態ベクトル候補群を入力し、観測データ管理部９２で選択した時刻ｔ１からｔＮまでの観測データ群を用いて、状態ベクトル候補の中から尤度比関数φ（Ｚ、ｘｋ）が最大となるｘｋ＊を１個探索し、状態空間詳細探索部９５に出力する。ここで、ｘｋ＊はφ（Ｚ、ｘｋ）の大域的最大点ｘｋＭＬに十分近いと仮定する。
【００１０】
次に、ステップ９０６において、状態空間詳細探索部９５は、状態空間概探索部９４から入力された状態ベクトル候補ｘｋ＊を初期点として、観測データ管理部９２で選択した時刻ｔ１からｔＮまでの観測データ群を用いて、前記仮定の下でｘｋＭＬに収束する反復処理法を適用し、収束点ｘｋ＊＊及びφ（Ｚ、ｘｋ＊＊）を航跡確立判定部９６に出力する。この反復処理法としては、最急降下法、準ニュートン法など、数多くの手法が提案されている。
【００１１】
そして、ステップ９０７において、航跡確立判定部９６は、予め定めた判定条件に従って航跡確立を判定し、判定結果を観測データ管理部９２に出力する。また、航跡確立判定部９６では、航跡確立の判定結果にかかわらず、入力された状態ベクトル推定値ｘｋ＊＊を出力する。
【００１２】
ところで、状態候補生成部９３では、状態ベクトル候補を、センサによって観測できる位置範囲や、想定される速度方向などを基に生成するが、一般に膨大な候補数となる。しかし、尤度比関数φ（Ｚ、ｘｋ）は状態ベクトルの位置が観測データに近いほど高くなる性質を利用すると、観測データを状態空間に写像したものを状態ベクトル候補の位置成分として用いることができる（例えば、非特許文献２参照）。
【００１３】
図１５は、状態ベクトル候補の位置成分として観測データを使用するＭＬ−ＰＤＡを用いた従来の別の追尾装置の構成を示すブロック図である。
【００１４】
図１５において、追尾装置１Ｙは、状態候補生成部９３Ａにおいて観測データ管理部９２で選択した観測データ群が入力されることを除いては、図１３におけるＭＬ−ＰＤＡを用いた従来の追尾装置１Ｘと同じである。
【００１５】
追尾装置１Ｙは、状態候補生成部９３Ａにおいて、状態ベクトル候補の位置成分として観測データを使用する。ただし、定めた基準時刻と観測時刻が異なる場合は、速度成分の候補と組み合わせて観測時刻における状態ベクトルを定め、その状態ベクトルを予め仮定した目標運動に従って、基準時刻まで前向きあるいは後ろ向きに外挿したものを状態ベクトル候補として生成する。
【００１６】
このように、追尾装置１Ｙでは、状態空間概探索部９４で探索する範囲を絞り込むことにより、生成する状態ベクトルの候補の数を、追尾装置１Ｘの状態候補生成部９３で生成する状態ベクトルの候補数よりも少なくすることができ、探索を高速化することができる。
【先行技術文献】
【非特許文献】
【００１７】
【非特許文献１】T. Kirubarajan and Y. Bar-Shalom, Low observable target motion analysis using amplitude information, IEEE Transactions on Aerospace and Electronic Systems, vol.32, no.4, pp.1367-1384, 1996.
【非特許文献２】W. R. Blanding, P. K. Willett, Y. Bar-Shalom, and R. S. Lynch, Directed subspace search ML-PDA with application to active sonar tracking, IEEE Transactions on Aerospace and Electronic Systems, vol.44, no.1, pp.201-216, 2008.
【発明の概要】
【発明が解決しようとする課題】
【００１８】
上述したように、ＭＬ−ＰＤＡを用いた従来の追尾装置では、基準時刻を前もって設定する必要がある。この基準時刻は任意に定めることができ、例えば、最新時刻が設定される。
【００１９】
ところで、センサから観測データを取得する際の観測環境、すなわち、目標が観測されているかどうかや、目標の観測誤差は時刻ごとに異なるため、基準時刻によって状態ベクトルの推定誤差は異なる。したがって、推定誤差が最小となる基準時刻を求める必要がある。
【００２０】
しかし、ＭＬ−ＰＤＡを用いた従来の追尾装置では、基準時刻を固定的に設定するため、その基準時刻において推定誤差が最小とは限らず、基準時刻を設定する根拠が曖昧であるという問題点があった。
【００２１】
さらに、センサの観測環境は未知であるため、推定誤差が最小となる基準時刻を求めることは困難であるという問題点があった。
【００２２】
また、ＭＬ−ＰＤＡを用いた従来の追尾装置では、状態空間概探索部９４において探索した状態ベクトルの候補ｘｋ＊は尤度比関数φ（Ｚ、ｘｋ）の大域的最大点ｘｋＭＬに十分近いと仮定する。状態空間詳細探索部９５における反復処理法は、前記仮定の下でのみ有効である。このため、前記仮定に反する状態ベクトル候補ｘｋ＊が得られた場合は、反復処理法によって尤度比関数の局所的最大点に反復が収束してしまい、推定誤差が増大するという問題点があった。
【００２３】
本発明は、前記のような課題を解決するためになされたものであり、基準時刻の設定根拠の曖昧さを解消し、なおかつ局所的最大点に収束した状態ベクトル推定値を排除することにより、正確な追尾結果を得ることができる追尾装置を得ることを目的とする。
【課題を解決するための手段】
【００２４】
本発明に係る追尾装置は、センサから入力された観測データを観測時刻ごとに蓄積する観測データ蓄積部と、前記観測データ蓄積部に蓄積された観測時刻ごとの観測データのうち、航跡確立判定結果に基づいてどの観測時刻の観測データを推定に用いるかを選択する観測データ管理部と、前記観測データ管理部により選択された観測時刻をすべて基準時刻として設定し、各基準時刻における航跡の候補を状態ベクトル候補として生成する選択時刻状態候補生成部と、前記観測データ管理部により選択された観測データを用いて、前記状態ベクトル候補の尤度比関数を計算して、各基準時刻における尤度比関数が最大の状態ベクトル候補を探索する状態空間概探索部と、前記状態空間概探索部により探索された各基準時刻における尤度比関数が最大の状態ベクトル候補を初期値として、前記観測データ管理部により選択された観測データを用いて、反復計算により各基準時刻における尤度比関数が最大の状態ベクトル推定値を探索する状態空間詳細探索部と、前記各基準時刻における尤度比関数が最大の状態ベクトル推定値の対応関係から最新時刻の最も確からしい状態ベクトル推定値を決定する状態相関処理部と、前記最新時刻の最も確からしい状態ベクトル推定値から航跡の確立を判定して前記航跡確立判定結果を出力する航跡確立判定部とを備えるものである。
【発明の効果】
【００２５】
本発明に係る追尾装置によれば、基準時刻の設定根拠の曖昧さを解消し、なおかつ局所的最大点に収束した状態ベクトル推定値を排除することにより、正確な追尾結果を得ることができるという効果を奏する。
【図面の簡単な説明】
【００２６】
【図１】この発明の実施の形態１に係る追尾装置の構成を示すブロック図である。
【図２】この発明の実施の形態１に係る追尾装置の動作を示すフローチャートである。
【図３】この発明の実施の形態１に係る追尾装置の基準時刻ごとの状態ベクトル推定値を示す図である。
【図４】この発明の実施の形態１に係る追尾装置の予測処理及びゲート内外判定処理の結果を表す図である。
【図５】この発明の実施の形態１に係る追尾装置の状態ベクトル推定値のグルーピング処理結果を示す図である。
【図６】この発明の実施の形態１に係る追尾装置のグループごとの状態ベクトル推定値の平滑処理結果及び航跡決定結果を示す図である。
【図７】この発明の実施の形態２に係る追尾装置の構成を示すブロック図である。
【図８】この発明の実施の形態２に係る追尾装置の動作を示すフローチャートである。
【図９】この発明の実施の形態３に係る追尾装置の構成を示すブロック図である。
【図１０】この発明の実施の形態３に係る追尾装置の動作を示すフローチャートである。
【図１１】この発明の実施の形態４に係る追尾装置の構成を示すブロック図である。
【図１２】この発明の実施の形態４に係る追尾装置の動作を示すフローチャートである。
【図１３】ＭＬ−ＰＤＡを用いた従来の追尾装置の構成を示すブロック図である。
【図１４】ＭＬ−ＰＤＡを用いた従来の追尾装置の動作を示すフローチャートである。
【図１５】状態ベクトル候補の位置成分として観測データを使用するＭＬ−ＰＤＡを用いた従来の別の追尾装置の構成を示すブロック図である。
【発明を実施するための形態】
【００２７】
以下、本発明の追尾装置の好適な実施の形態につき図面を用いて説明する。
【００２８】
実施の形態１．
この発明の実施の形態１に係る追尾装置について図１から図６までを参照しながら説明する。図１は、この発明の実施の形態１に係る追尾装置の構成を示すブロック図である。なお、以降では、各図中、同一符号は同一又は相当部分を示す。
【００２９】
図１において、この発明の実施の形態１に係る追尾装置１は、観測データ蓄積部１１と、観測データ管理部１２と、選択時刻状態候補生成部１３と、状態空間概探索部１４と、状態空間詳細探索部１５と、状態相関処理部１６と、航跡確立判定部２２とが設けられている。
【００３０】
また、図１において、状態相関処理部１６は、予測処理部１７と、ゲート内外判定部１８と、グルーピング処理部１９と、平滑処理部２０と、航跡決定部２１とが設けられている。
【００３１】
つぎに、この実施の形態１に係る追尾装置の動作について図面を参照しながら説明する。
【００３２】
図２は、この発明の実施の形態１に係る追尾装置の動作を示すフローチャートである。
【００３３】
まず、ステップ１０１において、観測データ蓄積部１１は、センサから観測データを取得する。
【００３４】
次に、ステップ１０２において、観測データ蓄積部１１は、取得した観測データを観測時刻ごとに蓄積する。
【００３５】
次に、ステップ１０３において、観測データ管理部１２は、観測データ蓄積部１１に蓄積された各時刻の観測データ群のうち、どの時刻の観測データ群を選択時刻状態候補生成部１３、状態空間概探索部１４、状態空間詳細探索部１５、及び状態相関処理部１６に出力するかを、航跡確立判定部２２の航跡確立判定結果を基に選択する。
【００３６】
次に、ステップ１０４において、選択時刻状態候補生成部１３は、観測データ管理部１２で選択した時刻列ｔ１からｔＮまでのすべてを基準時刻として設定し、この基準時刻ごとに状態ベクトル候補群を生成し、状態空間概探索部１４に出力する。ただし、計算負荷の低減のため、状態ベクトル候補の位置成分として観測データを状態空間に写像したものを使用する。
【００３７】
次に、ステップ１０５において、状態空間概探索部１４は、選択時刻状態候補生成部１３から入力された基準時刻ｔ１からｔＮまでの状態ベクトル候補群を入力し、基準時刻ｔ１からｔＮまでの時刻ごとに、観測データ管理部１２で選択した時刻ｔ１からｔＮまでの観測データ群を用いて、状態ベクトル候補の中で尤度比関数φ（Ｚ、ｘｋ）が最大となるｘ１＊からｘＮ＊までを１個ずつ探索し、状態空間詳細探索部１５に出力する。
【００３８】
次に、ステップ１０６において、状態空間詳細探索部１５は、状態空間概探索部１４から入力された基準時刻ｔ１からｔＮまでの状態ベクトル候補ｘ１＊からｘＮ＊までをそれぞれ初期値として、基準時刻ｔ１からｔＮまでの時刻ごとに、観測データ管理部１２で選択した時刻ｔ１からｔＮまでの観測データ群を用いて、反復処理法により尤度比関数φ（Ｚ、ｘｋ）を最大化する状態ベクトルｘ１＊＊からｘＮ＊＊までを探索し、状態相関処理部１６に状態ベクトル推定値ｘ１＊＊からｘＮ＊＊までを出力する。
【００３９】
なお、以上の選択時刻状態候補生成部１３、状態空間概探索部１４、及び状態空間詳細探索部１５の各機能は、従来の追尾装置における各機能について基準時刻を複数個設定した場合に相当し、設定した基準時刻の数だけ従来の追尾装置における各機能の動作を反復することで実現できる。
【００４０】
つづいて、状態相関処理部１６の機能の詳細について図３から図６までを参照しながら説明する。
【００４１】
図３は、この発明の実施の形態１に係る追尾装置の基準時刻ごとの状態ベクトル推定値を示す図である。また、図４は、この発明の実施の形態１に係る追尾装置の予測処理及びゲート内外判定処理の結果を表す図である。また、図５は、この発明の実施の形態１に係る追尾装置の状態ベクトル推定値のグルーピング処理結果を示す図である。さらに、図６は、この発明の実施の形態１に係る追尾装置のグループごとの状態ベクトル推定値の平滑処理結果及び航跡決定結果を示す図である。
【００４２】
まず、ステップ１０７において、予測処理部１７は、図３に示すような、状態空間詳細探索部１５から入力された基準時刻ｔ１からｔＮまでの状態ベクトル推定値ｘ１＊＊からｘＮ＊＊までを順番に、図４に示すように、予め仮定した目標運動に従って最新時刻ｔＮまで前向きに外挿する。ここで、状態ベクトル推定値ｘｉ＊＊を時刻ｔｋ（ただし１≦ｉ＜ｋ≦Ｎとする）まで外挿したものを状態ベクトル予測値ｘｉ、ｋ＊＊と呼ぶ。
【００４３】
次に、ステップ１０８において、ゲート内外判定部１８は、図４に示すように、ある基準時刻ｔｋにおける状態ベクトル推定値ｘｋ＊＊と、予測処理部１７で求めた状態ベクトル予測値ｘｉ、ｋ＊＊（ただし１≦ｉ＜ｋ≦Ｎとする）との対応関係を、式（１）に示すような、予め定めた行列Ｃｋに基づく距離ｄｉ、ｋにより評価する。距離ｄｉ、ｋが、予め定めた閾値ｄＧ以下であれば、状態ベクトル推定値ｘｋ＊＊と状態ベクトル予測値ｘｉ＊＊は対応が有り、さもなければ、つまり距離ｄｉ、ｋが、予め定めた閾値ｄＧより大きい（長い）場合には、状態ベクトル推定値ｘｋ＊＊と状態ベクトル予測値ｘｉ＊＊は対応が無いと判定する。この判定を、基準時刻ｔ１からｔＮまでのすべてについて行う。
【００４４】
【数１】
0005618566.tif 000002
【００４５】
次に、ステップ１０９において、グルーピング処理部１９は、図５に示すように、状態ベクトル推定値ｘ１＊＊からｘＮ＊＊までについて、ゲート内外判定部１８において対応ありと判定されたものを１つのグループΓｊとしてまとめる。ここで、状態ベクトル推定値ｘ１＊＊からｘＮ＊＊はグループΓ１からΓｎまでに分割されたとする。
【００４６】
次に、ステップ１１０において、平滑処理部２０は、図６に示すように、グループΓ１からΓｎまでに属する状態ベクトル推定値をフィルタで処理することによって、各グループの最新時刻ｔＮにおける状態ベクトル平滑値ｘ１＊＊＊からｘｎ＊＊＊を求める。
【００４７】
ただし、一般に、あるグループΓｊの属する状態ベクトルの時刻は、時刻ｔ１からｔＮまでのうちいくつかを取り除いたものとなっている。取り除かれた時刻における状態ベクトルの推定値は、その直前の時刻における状態ベクトル推定値を予め仮定した目標運動に従って前向きに外挿することによって求める。
【００４８】
そして、ステップ１１１において、航跡決定部２１は、グループΓ１からΓｎまでの最新時刻ｔＮにおける状態ベクトル平滑値ｘ１＊＊＊からｘｎ＊＊＊までについて、観測データ管理部１２で選択した時刻ｔ１からｔＮまでの観測データ群を用いて、尤度比関数φ（Ｚ、ｘｋ）を計算し、尤度比関数値が最も大きいグループの状態ベクトル平滑値ｘｊ＊＊＊と尤度比関数φ（Ｚ、ｘｊ＊＊＊）とを選択して航跡を決定し、状態ベクトル平滑値ｘｊ＊＊＊と尤度比関数φ（Ｚ、ｘｊ＊＊＊）とを、航跡確立判定部２２に出力する。
【００４９】
ただし、尤度比関数値が最も大きいグループが複数個存在した場合は、グループに属する状態ベクトル平滑値の数が最も多いグループの状態ベクトル平滑値ｘｊ＊＊＊とφ（Ｚ、ｘｊ＊＊＊）とを選択し、航跡確立判定部２２に出力する。
【００５０】
最後に、ステップ１１２において、航跡確立判定部２２では、予め定めた判定条件に従って航跡確立を判定し、判定結果を観測データ管理部１２に出力する。また、航跡確立判定部２２では、航跡確立の判定結果にかかわらず、状態相関処理部１６から入力された状態ベクトル平滑値ｘｊ＊＊＊を推定値として出力する。
【００５１】
以上のように、本実施の形態１によれば、状態ベクトルの推定に使用する観測データの時刻すべてを基準時刻として設定して状態ベクトルを推定することで、基準時刻の設定根拠の曖昧さを解消することが可能となる。
【００５２】
また、異なる複数の基準時刻における状態ベクトルを推定し、グルーピング処理によりそれらの対応関係を判別することで、ある基準時刻において局所的最大点に収束した状態ベクトル推定値を抽出し、そのような状態ベクトル推定値を排除することが可能となる。
【００５３】
さらに、異なる複数の基準時刻における状態ベクトル推定値をフィルタで処理することによって、推定誤差の増大をさらに抑圧するという効果を奏する。
【００５４】
なお、本実施の形態１では、ステップ１０８において、状態ベクトル推定値ｘｋ＊＊と状態ベクトル予測値ｘｉ、ｋ＊＊の距離ｄｉ、ｋを式（１）で与えるものとして説明したが、状態ベクトル推定値の位置成分Ｈｘｋ＊＊、Ｈｘｉ、ｋ＊＊（Ｈは観測行列であり、状態空間から観測データ空間への写像を表す）と、予め定めた行列Ｄｋに基づく式（２）の距離ｄｉ、ｋ'で与えることもできる。この場合、計算する行列のサイズが式（１）よりも小さくなり、計算時間を短縮することが可能となる。
【００５５】
【数２】
0005618566.tif 000003
【００５６】
また、本実施の形態１では、ステップ１１０において、各グループの最新時刻ｔＮにおける状態ベクトル平滑値の計算にフィルタを用いるものとして説明したが、各グループに含まれる最も新しい時刻の状態ベクトル推定値を基準として、予め仮定した目標運動に従って最新時刻ｔＮまで外挿することによって求めることもできる。この場合、複雑なフィルタ計算を省略することによって、計算時間を短縮することが可能となる。
【００５７】
また、本実施の形態１では、状態空間概探索部１４において、基準時刻ごとに尤度比関数が最大となる状態ベクトル候補を１個ずつ探索し、状態空間詳細探索部１５においてそれらを初期値として反復処理法を適用して収束点を１個ずつ探索するものとして説明したが、最終的にＫ個の収束点を求め、得られたＫ×Ｎ個の状態ベクトル推定値の対応関係を判別する方法でも良く、収束点は１個に限られない。ただし、収束点を１個に限ることにより、対応関係の判別問題のサイズが小さくなり、計算時間を短縮することが可能となる。
【００５８】
実施の形態２．
この発明の実施の形態２に係る追尾装置について図７及び図８を参照しながら説明する。図７は、この発明の実施の形態２に係る追尾装置の構成を示すブロック図である。
【００５９】
図７において、この発明の実施の形態２に係る追尾装置１Ａの構成は、上記の実施の形態１の構成と比較すると、基準時刻設定部２３と、状態ベクトル推定値記憶部２４とをさらに備えている点が異なっている。
【００６０】
つぎに、この実施の形態２に係る追尾装置の動作について図面を参照しながら説明する。
【００６１】
図８は、この発明の実施の形態２に係る追尾装置の動作を示すフローチャートである。この図８のステップ２０１−２０３、２０５−２０７、２０９−２１２、２１５及び２１６は、図２のステップ１０１−１１２にそれぞれに相当する。
【００６２】
図８において、この発明の実施の形態２に係る追尾装置１Ａの動作は、上記の実施の形態１の動作と比較すると、ステップ１０４−１０５の「基準時刻におけるループ」の代わりに、ステップ２０４（基準時刻設定）と、ステップ２０８（状態ベクトル推定値記憶）とを付加している点と、ステップ２１２とステップ２１５の間に条件分岐（ステップ２１３、２１４）を付加している点が異なっている。そこで、本実施の形態２に係る追尾装置１Ａの動作について、これらの相違点を中心に、以下に説明する。
【００６３】
まず、ステップ２０４において、基準時刻設定部２３は、観測データ管理部１２で選択した時刻列ｔ１からｔＮまでについて、最初に時刻ｔ１を基準時刻に設定する。また、航跡決定部２１において航跡が決定されない場合、ｔ２、ｔ３、．．．、と基準時刻を１つずつ進める。以下、基準時刻設定部２３で設定する基準時刻を「現在の基準時刻」と呼び、現在の基準時刻より過去の基準時刻を「過去の基準時刻」と呼ぶ。
【００６４】
次に、ステップ２０５からステップ２０７までにおいて、選択時刻状態候補生成部１３、状態空間概探索部１４、及び状態空間詳細探索部１５は、現在の基準時刻に関する状態空間探索を行い、ゲート内外判定部１８に出力する。
【００６５】
また、ステップ２０８において、状態ベクトル推定値記憶部２４は、現在の基準時刻における状態ベクトル推定値を記憶する。
【００６６】
次に、ステップ２０９において、予測処理部１７は、状態ベクトル推定値記憶部２４に蓄積された過去の基準時刻における状態ベクトル推定値群を、予め仮定した目標運動に従って現在の基準時刻まで前向きに外挿し、状態ベクトル予測値群を計算する。
【００６７】
次に、ステップ２１０、ステップ２１１、及びステップ２１２は、過去及び現在の基準時刻における状態ベクトル推定値に対する動作である点を除いては、上記の実施の形態１における動作と同様である。
【００６８】
次に、ステップ２１３において、航跡決定部２１は、グループに属する状態ベクトル平滑値の数が予め定めたＭ個以上となるグループが存在するならば、航跡決定可能と判定する。そして、ステップ２１５において、状態ベクトル平滑値の数がＭ個以上のグループの状態ベクトル平滑値を選択し、航跡を決定する。
【００６９】
一方、ステップ２１４において、航跡決定可能と判定されない場合、現在の基準時刻が、観測データ管理部１２で選択した最新時刻ｔＮと等しいならば、ステップ２１５において、グループに属する状態ベクトル平滑値の数が最も多いグループの状態ベクトル平滑値を選択し、航跡を決定する。
【００７０】
さらに、航跡決定部２１は、航跡を決定した場合、選択したグループの状態ベクトル平滑値を最新時刻ｔＮまで前向きに外挿する。
【００７１】
以上のように、本実施の形態２では、観測データの時刻列から基準時刻を順番に設定し、基準時刻に関して逐次的に状態ベクトルを推定し、状態ベクトル推定値の対応がＭ個以上となった場合は以降の基準時刻において推定しない。これにより、上記の実施の形態１において、観測データの時刻列をすべて基準時刻に設定し、基準時刻に関して同時並行的に状態ベクトルを推定するよりも計算負荷を軽減させることが可能となる。
【００７２】
なお、本実施の形態２では、ステップ２１０において、上記の実施の形態１と同様、状態ベクトル推定値ｘｋ＊＊と状態ベクトル予測値ｘｉ、ｋ＊＊の距離を式（１）で与えるものとして説明したが、式（２）で与えることもできることは言うまでもない。
【００７３】
実施の形態３．
この発明の実施の形態３に係る追尾装置について図９及び図１０を参照しながら説明する。図９は、この発明の実施の形態３に係る追尾装置の構成を示すブロック図である。
【００７４】
図９において、この発明の実施の形態３に係る追尾装置１Ｂの構成は、上記の実施の形態１の構成と比較すると、状態相関処理部１６と航跡確立判定部２２との間に、最新時刻状態ベクトル推定値記憶部２５と、最新時刻状態相関処理部２６とをさらに備えている点が異なっている。
【００７５】
つぎに、この実施の形態３に係る追尾装置の動作について図面を参照しながら説明する。
【００７６】
図１０は、この発明の実施の形態３に係る追尾装置の動作を示すフローチャートである。この図１０のステップ３０１−３１１及び３１８は、図２のステップ１０１−１１２にそれぞれに相当する。
【００７７】
図１０において、この発明の実施の形態３に係る追尾装置１Ｂの動作は、上記の実施の形態１の動作と比較すると、ステップ３１１とステップ３１８の間に、ステップ３１２（最新時刻状態ベクトル推定値記憶）からステップ３１７（航跡決定）までを付加している点が異なっている。しかし、ステップ３１２（最新時刻状態ベクトル推定値記憶）を除いて、ステップ３１３からステップ３１７までは、上記の実施の形態１のステップ１０７（予測処理）からステップ１１１（航跡決定）までと同様である。そこで、本実施の形態３に係る追尾装置１Ｂの動作について、これらの相違点を中心に、以下に説明する。
【００７８】
まず、ステップ３０１からステップ３１１までの動作により、追尾装置１Ｂは、上記の実施の形態１の構成及び動作と同様、最新時刻の状態ベクトル推定値を出力する。
【００７９】
次に、ステップ３１２において、最新時刻状態ベクトル推定値記憶部２５は、最新時刻の状態ベクトル推定値を記憶する。また、航跡確立判定部２２の航跡確立判定結果を基に、過去何時刻分の状態ベクトル推定値を記憶するかを決定し、その時刻分よりも過去の状態ベクトル推定値を最新時刻状態ベクトル推定値記憶部２５から削除する。
【００８０】
次に、ステップ３１３からステップ３１７までにおいて、最新時刻状態相関処理部２６の動作は、上述したように、ステップ１０７からステップ１１１までの状態相関処理部１６の動作と同様である。
【００８１】
ただし、状態相関処理部１６における入力値は、時刻ｔ１からｔＮまでの観測データから求めた基準時刻ｔ１からｔＮまでの状態ベクトル推定値であり、これらの状態ベクトル推定値は、用いた観測データの時刻はすべて同じである。一方、最新時刻状態相関処理部２６における入力値は、状態相関処理部１６から出力された過去及び最新時刻の状態ベクトル推定値であり、これらの状態ベクトル推定値は、用いた観測データの時刻列が異なっている。すなわち、最新時刻状態相関処理部２６においては、異なる時刻列の観測データからの状態ベクトル推定値の対応関係を判別している。
【００８２】
以上のように、本実施の形態３によれば、異なる複数の時刻列の観測データから状態ベクトルを推定し、グルーピング処理によりそれらの対応関係を判別することで、ある時刻列の観測データを用いた推定で局所的最大点への収束を排除できなかった場合についても、そのような状態ベクトル推定値を排除することが可能となる。
【００８３】
なお、本実施の形態３では、ステップ３０８及びステップ３１４において、上記の実施の形態１と同様、状態ベクトル推定値ｘｋ＊＊と状態ベクトル予測値ｘｉ、ｋ＊＊の距離を式（１）で与えるものとして説明したが、式（２）で与えることもできることは言うまでもない。
【００８４】
また、本実施の形態３では、ステップ３０１からステップ３１１までにおいて、上記の実施の形態１の構成と動作を用いるものとして説明したが、上記の実施の形態２の構成と図８の動作を用いても、同様の効果を得ることができる。
【００８５】
実施の形態４．
この発明の実施の形態４に係る追尾装置について図１１及び図１２を参照しながら説明する。図１１は、この発明の実施の形態４に係る追尾装置の構成を示すブロック図である。
【００８６】
図１１において、この発明の実施の形態４に係る追尾装置１Ｃの構成は、上記の実施の形態１の構成と比較すると、状態相関処理部１６から航跡決定部２１を取り除いた構成を状態相関処理部１６Ｂとしている点と、予測処理部３２と、検定処理部３３とをさらに備えている点が異なっている。
【００８７】
つぎに、この実施の形態４に係る追尾装置の動作について図面を参照しながら説明する。
【００８８】
図１２は、この発明の実施の形態４に係る追尾装置の動作を示すフローチャートである。この図１２のステップ４０１−４１０及び４１４は、図２のステップ１０１−１１０及び１１２にそれぞれに相当する。
【００８９】
図１２において、この発明の実施の形態４に係る追尾装置１Ｃの動作は、上記の実施の形態１の動作と比較すると、ステップ１１１の代わりに、ステップ４１１（予測処理）からステップ４１３（検定処理）までを付加している点が異なっている。そこで、本実施の形態４に係る追尾装置１Ｃの動作について、これらの相違点を中心に、以下に説明する。
【００９０】
まず、ステップ４１０までの動作により、状態相関処理部１６Ｂは、グループΓ１からΓｎまでの状態ベクトル平滑値ｘ１＊＊＊からｘｎ＊＊＊までを出力する。この状態ベクトル平滑値ｘ１＊＊＊からｘｎ＊＊＊までは最新時刻ｔＮにおける状態ベクトルを推定するものである。
【００９１】
次に、ステップ４１１において、予測処理部３２は、入力された各グループの状態ベクトル平滑値ｘ１＊＊＊からｘｎ＊＊＊までを、予め仮定した目標運動に従って最新時刻より１サンプル未来時刻ｔＮ＋１まで前向きに外挿し、各グループの状態ベクトル予測値ｘ１、Ｎ＋１＊＊＊からｘｎ、Ｎ＋１＊＊＊までを計算して出力する。
【００９２】
次に、ステップ４１２において、観測データ管理部１２は、最新時刻より１サンプル未来時刻ｔＮ＋１の観測データｚＮ＋１、１からｚＮ＋１、ｍまでを観測データ蓄積部１１から取得し、検定処理部３３に出力する。
【００９３】
次に、ステップ４１３において、検定処理部３３は、グループΓｊの状態ベクトル予測値ｘｊ、Ｎ＋１＊＊＊と、時刻ｔＮ＋１のある観測データｚＮ＋１、ｋとの対応関係を、式（３）に示すような、予め定めた行列ＥＮ＋１に基づく距離ｄｊ、ｋ''により評価する。そして、検定処理部３３は、距離ｄｊ、ｋ''が予め定めた閾値ｄＧ'以下となるような観測データｚＮ＋１、ｋが存在するグループΓｊの状態ベクトル平滑値ｘｊ＊＊＊とφ（Ｚ、ｘｉ＊＊＊）とを航跡確立判定部２２に出力する。
【００９４】
【数３】
0005618566.tif 000004
【００９５】
ただし、すべてのグループΓｊにおいて距離ｄｊ、ｋ''が閾値ｄＧ'以下となる観測データｚＮ＋１、ｋが存在しなかった場合や、距離ｄｊ、ｋ''が閾値ｄＧ'以下となる観測データｚＮ＋１、ｋが存在するグループが複数存在した場合は、グループに属する状態ベクトル推定値の数が最も多いグループの状態ベクトル平滑値ｘｊ＊＊＊とφ（Ｚ、ｘｊ＊＊＊）とを選択し、航跡確立判定部２２に出力する。
【００９６】
以上のように、本実施の形態４によれば、時刻ｔ１からｔＮまでの観測データから推定した状態ベクトルを、時刻ｔＮ＋１における観測データを用いて検定することで、局所最大点に収束した状態ベクトル推定値を判別し、排除することが可能となる。
【００９７】
なお、本実施の形態４では、ステップ４０８において、上記の実施の形態１と同様、状態ベクトル推定値ｘｋ＊＊と状態ベクトル予測値ｘｉ、ｋ＊＊の距離を式（１）で与えるものとして説明したが、式（２）で与えることもできることは言うまでもない。
【符号の説明】
【００９８】
１、１Ａ、１Ｂ、１Ｃ  追尾装置、１１  観測データ蓄積部、１２  観測データ管理部、１３  選択時刻状態候補生成部、１４  状態空間概探索部、１５  状態空間詳細探索部、１６  状態相関処理部、１６Ａ  状態相関処理部、１６Ｂ  状態相関処理部、１７  予測処理部、１８  ゲート内外判定部、１９  グルーピング処理部、２０  平滑処理部、２１  航跡決定部、２２  航跡確立判定部、２３  基準時刻設定部、２４  状態ベクトル推定値記憶部、２５  最新時刻状態ベクトル推定値記憶部、２６  最新時刻状態相関処理部、２７  予測処理部、２８  ゲート内外判定部、２９  グルーピング処理部、３０  平滑処理部、３１  航跡決定部、３２  予測処理部、３３  検定処理部。
【図１】
0005618566.tif 000005
【図２】
0005618566.tif 000006
【図３】
0005618566.tif 000007
【図４】
0005618566.tif 000008
【図５】
0005618566.tif 000009
【図６】
0005618566.tif 000010
【図７】
0005618566.tif 000011
【図８】
0005618566.tif 000012
【図９】
0005618566.tif 000013
【図１０】
0005618566.tif 000014
【図１１】
0005618566.tif 000015
【図１２】
0005618566.tif 000016
【図１３】
0005618566.tif 000017
【図１４】
0005618566.tif 000018
【図１５】
0005618566.tif 000019