JP3792927B2 - 目標追尾装置 - Google Patents
目標追尾装置 Download PDFInfo
- Publication number
- JP3792927B2 JP3792927B2 JP03913899A JP3913899A JP3792927B2 JP 3792927 B2 JP3792927 B2 JP 3792927B2 JP 03913899 A JP03913899 A JP 03913899A JP 3913899 A JP3913899 A JP 3913899A JP 3792927 B2 JP3792927 B2 JP 3792927B2
- Authority
- JP
- Japan
- Prior art keywords
- motion models
- motion
- observation
- target
- models
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Images
Landscapes
- Radar Systems Or Details Thereof (AREA)
Description
【０００１】
【発明の属する技術分野】
本発明は、センサ等からの目標の位置情報に関する観測データに基づいて目標の位置や速度などの目標運動諸元を推定する目標追尾装置に係り、目標を観測する間隔(サンプリング間隔)を制御することで、機動力の高い目標に対する追尾精度を向上させた目標追尾装置に関する。
【０００２】
【従来の技術】
図７は、特開平１−９２６７８号公報に開示された従来の目標追尾装置を示すブロック図である。また、図８は、図７に示す目標追尾装置の動作を説明するためのフローチャートを示す。以下、図７を参照して従来の目標追尾装置の構成を説明する。
【０００３】
目標観測装置１は、目標の予測位置をセンサ（図示せず）に指示すると共に、センサが観測する目標位置に関する観測情報（以下、「目標位置観測情報」と称す）を取得するものである。
【０００４】
Ｎ個の運動モデルによる信頼度算出器２（以下、単に「信頼度算出器」と称す）は、上述した目標位置観測情報、運動モデルごとの予測値、運動モデルごとの予測誤差、および１サンプリング前の各運動モデルの信頼度を基に、Ｎ個の運動モデルによる信頼度を算出するものである。
１３は遅延回路であり、信頼度算出器２によって算出された信頼度を単位時間だけ遅延させて信頼度算出器２に入力するものである。
【０００５】
Ｎ個の運動モデルによる平滑器３（以下、単に「平滑器３」と称す）は、上述した目標位置観測情報、各運動モデルの信頼度、運動モデルごとの予測値、ゲイン行列、および１サンプリング前のＮ個の運動モデルによる平滑値を基に、Ｎ個の運動モデルによる平滑値を算出するものである。
４は遅延回路であり、平滑器３によって算出された平滑値を単位時間だけ遅延させて平滑器３に入力するものである。
【０００６】
Ｎ個の運動モデルによる予測器５（以下、単に「予測器５」と称す）は、上記Ｎ個の運動モデルによる平滑値を基に、Ｎ個の運動モデルによる予測位置を算出するものである。
１４は遅延回路であり、予測器５によって算出された予測位置を単位時間だけ遅延させて目標観測装置１に入力するものである。
【０００７】
運動モデルごとの予測器６は、上述したＮ個の運動モデルによる平滑値を基に、
運動モデルごとの予測値を算出するものである。
７は遅延回路であり、運動モデルごとの予測器６によって算出された運動モデルごとの予測値を遅延させて信頼度算出器２に入力するものである。
【０００８】
Ｎ個の運動モデルによる平滑誤差評価器９（以下、単に「平滑誤差評価器９」と称す）は、上述した各運動モデルの信頼度、運動モデルごとの予測誤差、およびゲイン行列を基に、Ｎ個の運動モデルによる平滑誤差を算出するものである。
【０００９】
運動モデルごとの予測誤差評価器１０は、上記Ｎ個の運動モデルによる平滑誤差を基に、運動モデルごとの予測誤差を算出するものである。
８は遅延回路であり、運動モデルごとの予測誤差評価器10によって算出された運動モデルごとの予測誤差を遅延させて、信頼度算出器２、平滑誤差評価器９、およびゲイン行列算出器12に入力するものである。
【００１０】
Ｎ個の運動モデルによる予測誤差評価器１１（以下、単に「予測誤差評価器１１」と称す）は、上述した運動モデルごとの予測誤差、および各運動モデルの信頼度を基に、Ｎ個の運動モデルによる予測誤差を算出するものである。
ゲイン行列算出器12は、上述した運動モデルごとの予測誤差を基に、ゲイン行列を算出するものである。
【００１１】
次に図８に示すフローチャートを参照して従来の目標追尾装置の動作について説明する。
目標追尾装置では、先ず、目標観測装置１より得られる目標位置観測情報を基に、通常のカルマンフィルタ理論に基づき、目標位置・速度の平滑値、および平滑誤差共分散行列の初期値が設定される（ＳＴ１）。
【００１２】
次に、目標の運動モデルとして、図９に示すようなＮ個の定数加速度ベクトル（零加速度ベクトルを含む）よりなるＮ個の運動モデルが設定設定される（ＳＴ２）。
【００１３】
次に、運動モデルごとの予測器６において、平滑器３より入力されるＮ個の運動モデルによる平滑値、およびＮ個の運動モデルを構成している定数加速度ベクトルに基づいて、運動モデルごとの予測値が算出される。更に、運動モデルごとの予測誤差評価器１０において、平滑誤差評価器９より入力されるＮ個の運動モデルによる平滑誤差共分散行列、および予め設定された駆動雑音共分散行列に基づいて、運動モデルごとの予測誤差共分散行列が算出される（ＳＴ３）。
【００１４】
次に、予測器５において、平滑器３から出力されるＮ個の運動モデルによる平滑値、および信頼度算出器２から出力される各運動モデルの信頼度が読み込まれる。予測器５は、それらの入力データ、予め設定されている推移確率（目標の運動状態に適合する運動モデルが推移する確率）、および定数加速度ベクトルに基づいて、Ｎ個の運動モデルによる予測値を算出する。
【００１５】
また、予測誤差評価器１１においては、運動モデルごとの予測誤差評価器１０から出力される運動モデルごとの予測誤差共分散行列、および信頼度算出器２から出力される各運動モデルの信頼度が読み込まれる。予測誤差評価器１１は、それらの入力データ、予め設定されている推移確率、および定数加速度ベクトルに基づいて、Ｎ個の運動モデルによる予測誤差共分散行列が算出される（ＳＴ４）。
【００１６】
次に、目標観測装置１において、観測値が読み込まれる（ＳＴ５）。上記の観測値は、目標からの信号の検出結果であり、位置情報で構成されている。
【００１７】
目標観測装置１で観測された観測値は、信頼度算出器２に供給される。信頼度算出器２は、更に、現時刻より１サンプリング前に算出した各運動モデルの信頼度を遅延回路１３を介して読み込み、運動モデルごとの予測器６が現時刻より１サンプリング前に算出した運動モデルごとの予測値を遅延回路７を介して読み込み、運動モデルごとの予測誤差評価器１０が現時刻より１サンプリング前に算出した運動モデルごとの予測誤差共分散行列を遅延回路8を介して読み込む。信頼度算出器２は、それらの入力データと、予め設定されている観測雑音共分散行列および運動モデルの推移確率とに基づいて、運動モデルの信頼度を算出する（ＳＴ６）。
【００１８】
次に、ゲイン行列算出器１２において、運動モデルごとの予測誤差評価器１０が現時刻より１サンプリング前に算出した運動モデルごとの予測誤差共分散行列が遅延回路8を介して読み込まれる。ゲイン行列算出器１２は、その入力データと、予め設定された観測雑音共分散行列とに基づいてゲイン行列を算出する（ＳＴ７）。
【００１９】
次に、平滑器３において、現時刻より１サンプリング前に算出しておいたＮ個の運動モデルによる平滑値が遅延回路4を介して読み込まれる。平滑器３は、更に、信頼度算出器２が出力する各運動モデルの信頼度、ゲイン行列算出器１２が出力するゲイン行列、および目標観測装置１が出力する観測値を読み込む。平滑器３は、それらの入力データと、予め設定されている定数加速度ベクトルとに基づいてＮ個の運動モデルによる平滑値を算出する。
【００２０】
また、平滑誤差評価器９においては、信頼度算出器２から出力される各運動モデルの信頼度が読み込まれる。平滑誤差評価器９は、更に、運動モデルごとの予測誤差評価器１０が現時刻より１サンプリング前に算出した運動モデルごとの予測誤差共分散行列を遅延回路８を介して読み込み、ゲイン行列算出器１２から出力されるゲイン行列を読み込む。平滑誤差評価器９は、それらの入力データと、あらかじめ設定されている定数加速度ベクトルとに基づいて、Ｎ個の運動モデルによる平滑誤差共分散行列を算出する（ＳＴ８）。
【００２１】
上述した一連の処理が終了すると、追尾処理の終了が要求されているか否かが判別される（ＳＴ１０）。その結果、処理の終了が要求されていないと判別される場合は、所定のサンプリング間隔で、上記ＳＴ３以降の処理が繰り返し実行される。
【００２２】
【発明が解決しようとする課題】
従来の目標追尾装置は以上のように、目標追尾に必要な一連の処理を所定のサンプリング間隔で繰り返すように構成されている。サンプリング間隔が常に一定であると、旋回運動や直進運動をおり混ぜた運動を行う機動力の高い目標が、精度良く追尾できない事態が生じ得る。この点、従来の目標追尾装置は、機動性に優れた目標に対する追尾精度を劣化させ易いという問題を有するものであった。
【００２３】
本発明は、上記のような課題を解決するためになされたもので、目標追尾精度に合わせて適応的にサンプリング間隔を制御することで、追尾対象が機動力の高い目標であっても、追尾精度をある水準に保ちながら追尾を行うことができる目標追尾装置を提供することを目的とする。
【００２４】
【課題を解決するための手段】
請求項１記載の発明は、目標観測時刻と目標予測位置とをセンサに指示すると共に、センサによって観測される目標位置の観測情報を取得する目標観測装置を備える目標追尾装置であって、
目標追尾精度を検出する追尾精度検出手段と、
前記目標追尾精度に基づいて前記観測情報のサンプリング間隔を決定すると共に、そのサンプリング間隔を用いて、次回の観測時刻を算出する観測時刻算出手段とを備え、
前記追尾精度検出手段は、
目標位置観測情報、運動モデルごとの予測値、運動モデルごとの予測誤差、運動モデル間推移確率、および１サンプリング前のＮ個の運動モデルによる信頼度を基に、Ｎ個の運動モデルによる信頼度を算出するＮ個の運動モデルによる信頼度算出器と、
目標位置観測情報、Ｎ個の運動モデルによる信頼度、ゲイン行列、および１サンプリング前のＮ個の運動モデルによる平滑値を基に、Ｎ個の運動モデルによる平滑値を算出するＮ個の運動モデルによる平滑器と、
Ｎ個の運動モデルによる平滑値、Ｎ個の運動モデルによる信頼度、１サンプリング後の観測時刻、および運動モデル間推移確率を基に、Ｎ個の運動モデルによる予測値を算出するＮ個の運動モデルによる予測器と、
Ｎ個の運動モデルによる平滑値、および１サンプリング後の観測時刻を基に、運動モデルごとの予測値を算出する運動モデルごとの予測器と、
Ｎ個の運動モデルによる信頼度、運動モデルごとの予測誤差、およびゲイン行列を基に、Ｎ個の運動モデルによる平滑誤差を算出するＮ個の運動モデルによる平滑誤差評価器と、
Ｎ個の運動モデルによる平滑誤差および１サンプリング後の観測時刻を基に、運動モデルごとの予測誤差を算出する運動モデルごとの予測誤差評価器と、
運動モデルごとの予測誤差を基に、ゲイン行列を算出するゲイン行列算出器と、
運動モデルごとの予測誤差、Ｎ個の運動モデルによる信頼度、１サンプリング後の観測時刻および運動モデル間推移確率を基に、Ｎ個の運動モデルによる予測誤差を算出するＮ個の運動モデルによる予測誤差評価器と、
現在の観測時刻と次の観測時刻との間における運動モデル間の推移確率を算出する運動モデル間推移確率算出器と、を備え、
Ｎ個の運動モデルによる予測値と、Ｎ個の運動モデルによる予測誤差とに基づいて、目標追尾精度を求めることを特徴とするものである。
【００２６】
請求項２記載の発明は、請求項１記載の目標追尾装置であって、
前記追尾精度検出手段は、
センサの状態を表すセンサ諸元を読み込むセンサ諸元入力器を備えると共に、
Ｎ個の運動モデルによる予測値と、Ｎ個の運動モデルによる予測誤差と、センサ諸元とに基づいて、目標追尾精度求めることを特徴とするものである。
【００２７】
請求項３記載の発明は、請求項１または２記載の目標追尾装置であって、
前記観測時刻算出手段は、
サンプリング間隔を最小サンプリング間隔ごとに延長させる間隔再設定手段と、
サンプリング間隔が延長される毎に、Ｎ個の運動モデルによる予測値と、Ｎ個の運動モデルによる予測誤差とを更新する予測更新手段と、を備え、
前記目標追尾精度算出手段は、
前記Ｎ個の運動モデルによる予測値、および前記Ｎ個の運動モデルによる予測誤差が更新される毎に、目標追尾精度の評価値を算出する評価値算出手段と、
前記評価値と所定のしきい値とを比較するしきい値判定手段と、を備え、
前記観測時刻算出手段は、更に、前記しきい値判定手段によって前記評価値と前記しきい値の大小関係の逆転が判別された際のサンプリング間隔を、次のサンプリング間隔とすることを特徴とするものである。
【００２８】
請求項４記載の発明は、請求項１記載の目標追尾装置であって、
前記追尾精度算出手段は、
Ｎ個の運動モデルによる予測誤差共分散行列に基づいて演算される誤差楕円体の体積を評価値とする評価値設定手段と、
観測誤差共分散行列に基づいて演算される誤差楕円体の体積をしきい値とするしきい値設定手段と、
前記評価値と前記しきい値とを比較することにより追尾精度の状態を判断するしきい値判定手段と、
を備えることを特徴とするものである。
【００２９】
請求項５記載の発明は、請求項２記載の目標追尾装置であって、
前記センサ諸元は、極座標面におけるセンサの観測領域の面積を含み、
前記追尾精度算出手段は、
Ｎ個の運動モデルによる予測誤差共分散行列に基づいて演算される誤差楕円体の極座標面での面積を評価値とする評価値設定手段と、
前記センサ諸元に含まれる観測領域の面積に基づいてしきい値を設定するしきい値設定手段と、
前記評価値と前記しきい値とを比較することにより追尾精度の状態を判断するしきい値判定手段と、
を備えることを特徴とするものである。
【００３０】
請求項６記載の発明は、請求項２記載の目標追尾装置であって、
前記センサ諸元は、極座標面におけるセンサの観測領域の幅を含み、
前記追尾精度算出手段は、
Ｎ個の運動モデルによる予測誤差共分散行列に基づいて演算される誤差楕円体の極座標面での長軸直径を評価値とする評価値設定手段と、
前記センサ諸元に含まれる観測領域の幅に基づいてしきい値を設定するしきい値設定手段と、
前記評価値と前記しきい値とを比較することにより追尾精度の状態を判断するしきい値判定手段と、
を備えることを特徴とするものである。
【００３１】
【発明の実施の形態】
以下、図面を参照してこの発明の実施の形態について説明する。尚、各図において共通する要素には、同一の符号を付して重複する説明を省略する。
【００３２】
実施の形態１。
図１は、本発明の目標追尾装置の実施の形態１を示すブロック図である。図１において、目標観測装置１は、センサ（図示せず）に対して目標の予測位置を指示すると共に、センサが観測する目標位置に関する観測情報（以下、「目標位置観測情報」と称す）を取得する。
【００３３】
Ｎ個の運動モデルによる信頼度算出器２（以下、単に「信頼度算出器」と称す）は、上述した目標位置観測情報、運動モデルごとの予測値、運動モデルごとの予測誤差、および１サンプリング前の各運動モデルの信頼度を基に、Ｎ個の運動モデルによる信頼度を算出する。
【００３４】
Ｎ個の運動モデルによる平滑器３（以下、単に「平滑器３」と称す）は、上述した目標位置観測情報、Ｎ個の運動モデルによる信頼度、ゲイン行列、および１サンプリング前のＮ個の運動モデルによる平滑値を基に、Ｎ個の運動モデルによる平滑値を算出する。
【００３５】
Ｎ個の運動モデルによる予測器５（以下、単に「予測器５」と称す）は、上述したＮ個の運動モデルによる平滑値、Ｎ個の運動モデルによる信頼度、および１サンプリング後の観測時刻に基づいて、Ｎ個の運動モデルによる予測値を算出する。
【００３６】
運動モデルごとの予測器６は、Ｎ個の運動モデルによる平滑値、および１サンプリング後の観測時刻に基づいて、運動モデルごとの予測値を算出する。
【００３７】
Ｎ個の運動モデルによる平滑誤差評価器９（以下、単に「平滑誤差評価器９」と称す）は、Ｎ個の運動モデルによる信頼度、運動モデルごとの予測誤差、およびゲイン行列に基づいて、Ｎ個の運動モデルによる平滑誤差を算出する。
【００３８】
運動モデルごとの予測誤差評価器１０は、Ｎ個の運動モデルによる平滑誤差、および１サンプリング後の観測時刻に基づいて、運動モデルごとの予測誤差を算出する。
【００３９】
Ｎ個の運動モデルによる予測誤差評価器１１は、運動モデルごとの予測誤差、Ｎ個の運動モデルによる信頼度、および１サンプリング後の観測時刻に基づいて、Ｎ個の運動モデルによる予測誤差を算出する。
【００４０】
ゲイン行列算出器１２は、運動モデルごとの予測誤差を基に、ゲイン行列を算出する。
【００４１】
観測時刻算出器１５は、Ｎ個の運動モデルによる信頼度、Ｎ個の運動モデルによる平滑値、およびＮ個の運動モデルによる平滑誤差に基づいて、次回のサンプリング（目標観測）が行われる時刻（観測時刻）を算出する。
【００４２】
運動モデル間推移確率算出器１６は、観測時刻算出器１５の出力する観測時刻に基づいて、現在の観測時刻と次回の観測時刻との差からサンプリング間隔を算出し、このサンプリング間隔における運動モデル間の推移確率を算出する。
【００４３】
次に、本実施形態の目標追尾装置において、追尾フィルタとして用いられるＮ個の運動モデルを持つカルマンフィルタについて説明する。
カルマンフィルタにおいては、目標の運動モデルを式(15)のように定義する。
Ｘk＝Φk-1・Ｘk-1＋Γk-1・ωk-1＋Γ′k-1・ｕk-1 (15)
【００４４】
上記(15)式において、Ｘkは、サンプリング時刻ｔkにおける目標運動諸元の真値をあらわす次元状態ベクトルである。例えば、xyzの直交座標系における目標位置ベクトルを式(16)、直交座標系における目標速度ベクトルを式(17)とした時、Ｘkは式(18)で表される。
【００４５】
【数１】
【００４６】
上記(15)式において、Φk-1は、サンプリング時刻の変化（ｔk-1からｔk）に伴う状態ベクトルＸkの推移を表す推移行列で、目標が等速直線運動を行うと仮定した場合式(19)のように表される。尚、式(19)で用いられるＩは、式(20)で表される単位行列である。
【００４７】
【数２】
【００４８】
上記(15)式において、ωkは、サンプリング時刻ｔkにおける駆動雑音ベクトルである。また、式(15)において、Γkは、サンプリング時刻ｔkにおける駆動雑音ベクトルの変換行列である。目標の運動モデルを等速直線運動と仮定したことによる打切り誤差項をΓk-1・ωk-1とみれば、ωkは加速度ベクトル相当であり、Γk-1は式(21)のように表される。
【００４９】
【数３】
【００５０】
ωkは平均0の3次元正規分布白色雑音で、式(22)、式(23)のように表されるものとする。ここで0は零ベクトルを表し、Ｑkはサンプリング時刻ｔkにおける駆動雑音共分散行列である。
【００５１】
【数４】
【００５２】
上記(15)式において、ｕkは、サンプリング時刻ｔkにおいてＮ個の運動モデルを構成する定数加速度ベクトルであり、式(24)のように表すことができる。また、Γ′kはサンプリング時刻ｔkにおける定数加速度ベクトルの変換行列であり、式(25)のように表すことができる。
【００５３】
【数５】
【００５４】
図９は、定数加速度ベクトルを説明する図である。図９において、Oは座標O-xyzの原点である。座標0-xyzは、目標観測装置の位置を原点とする座標である。また、図９において、Ｘは東方向を正方向とする座標O-xyzのx軸、Ｙは北方向を正方向とする座標O-xyzのy軸、Ｚは上方向を正方向とする座標O-xyzのz軸を示す。更に、α１はy軸正方向の定数加速度ベクトル、α２はy軸負方向の定数加速度ベクトル、α３はx軸正方向の定数加速度ベクトル、α４はx軸負方向の定数加速度ベクトル、α５はz軸正方向の定数加速度ベクトル、α６はz軸負方向の定数加速度ベクトルである。この他に、加速度0の定数加速度ベクトルα７を考えた運動モデルの場合、モデル数Ｎは７となる。
【００５５】
サンプリング時刻ｔkにおいて式(26)が真であるとの仮説を式(27)で表す。
ｕk-1＝αa （ａ＝１，２，・・，Ｎ） (26)
Ψk,a （ａ＝１，２，・・，Ｎ） (27)
【００５６】
運動モデルの推移にマルコフ性を仮定する。すなわち運動モデルΨk,aはサンプリング時刻ｔk-1の運動モデルより決まり、サンプリング時刻ｔk-2までの運動モデルには依存しないと仮定する。
【００５７】
時刻ｔkにおける運動モデル間推移確率を式(28)のように表す。サンプリング間隔が可変の場合には、サンプリング間隔に合わせて運動モデル間推移確率を変える必要がある。例えば、最小サンプリング間隔をｔsとし、サンプリング間隔ｔk - ｔk-1 = ｔsの場合の運動モデル間推移確率を式(28)と同様の意味でｐabとする。さらに、Ｎ×Ｎ行列でa行b列の要素をｐabとしたものを運動モデル間推移確率行列Πと表す。サンプリング間隔がｔk - ｔk-1 = m・ｔs、 (m = 1、2、・・)と表せる場合、サンプリング時刻ｔkでの運動モデル間推移確率行列Πkは式(29)で表せる。このときのΠkのa行b列の要素が運動モデル間推移確率ｐk,abである。
【００５８】
【００５９】
サンプリング時刻ｔkまでの観測情報Ｚkに基づく、サンプリング時刻ｔkでの運動モデルの信頼度を条件付き確率密度関数により式(30)と定義すると、その信頼度は式(31)で計算される。
【００６０】
【数６】
【００６１】
ただし、νk,aは、観測ベクトルの正規分布近似P[Ｚk|Ψk,a，Ｚk-1]を多変量正規分布で近似したもので、式(32)で表される。
νk,a＝ｇ（Ｚk；Ｈ・Ｘk,a(-)，Ｈ・Ｐk,a(-)・ＨT＋Ｒk） (32)
尚、上記(32)式において、ｇ（ａ；ｂ，ｃ）は、平均ｂ、共分散行列ｃの３変量正規分布のａにおける確率密度関数である。また、上記(32)式において、記号Tは、その行列が転置行列であることを表す記号である。
【００６２】
サンプリング時刻ｔk-1までの観測情報Ｚk-1に基づくサンプリング時刻ｔkでの運動モデルの事前信頼度を条件付き確率密度関数により、式(33)と定義すると、その信頼度は式(34)と書ける。
【００６３】
【数７】
【００６４】
また、サンプリング時刻ｔkまでの観測情報Ｚkに基づく、定数加速度ベクトルの推定値を式(35)として定義すると、その推定値は式(36)と書ける。これを推定加速度ベクトルと呼ぶ。
【００６５】
【数８】
【００６６】
更に、サンプリング時刻ｔk-1までの観測情報Ｚk-1に基づく、定数加速度ベクトルの推定値を式(37)として定義すると、その推定値は式(38)と書ける。これを予測加速度ベクトルと呼ぶ。
【００６７】
【数９】
【００６８】
追尾対象目標に起因する観測値はサンプリング時刻ｔkにおいて1つ得られるとし、その観測系モデルを式(39)とする。ここでＺkはサンプリング時刻ｔkにおける位置情報の観測値で構成される直交座標による位置観測ベクトル、Ｈはサンプリング時刻ｔkにおける観測行列で、式(40)で表される。ｖkはサンプリング時刻ｔkでの観測ベクトルＺkに対応した観測雑音ベクトルであり、平均0の3次元正規分布白色雑音で、式(41)、式(42)のように表すことができる。尚、Ｒkはサンプリング時刻ｔkにおける観測雑音共分散行列で、運動モデルによらない値とする。また、駆動雑音ベクトルと観測雑音ベクトルは互いに独立であるとする。
【００６９】
【数１０】
【００７０】
またサンプリング時刻ｔ1からｔkまでの観測値ベクトルの集積を式(43)のように表す。
Ｚk＝［Ｚ1，Ｚ2，Ｚ3，・・，Ｚk］ (43)
【００７１】
カルマンフィルタの理論によれば、上記モデルに従いサンプリング時刻ｔkで観測値が得られた場合の、状態ベクトルＸkの推定値Ｘk(+)は式(44)〜(52)によって計算される。
【００７２】
【数１１】
【００７３】
ここで、Ｘk,a(-)、 Ｘk,a(+)、 Ｘk(-)、 Ｘk(+)、 Ｐk,a(-)、Ｐk,a(+)、 Ｐk(-)、Ｐk(+)は、それぞれ式(53)〜(60)のように定義される。尚、上記(48)式で表されるＫkは、サンプリング時刻ｔkにおけるゲイン行列である。
【００７４】
【数１２】
【００７５】
上記(53)式で表されるＸk,a(-)は、サンプリング時刻ｔk-1までの観測情報Ｚk-1と運動モデルの仮説Ψk,aとに基づく状態ベクトルＸkの条件付き平均値であり、サンプリング時刻ｔk-1までの観測情報Ｚk-1と運動モデルの仮説Ψk,aとに基づいてサンプリング時刻ｔkでの真値を推定した予測ベクトルに相当する。
【００７６】
上記(54)式で表されるＸk,a(+)は、サンプリング時刻ｔkまでの観測情報Ｚkと運動モデルの仮説Ψk,aとに基づく状態ベクトルＸkの条件付き平均値であり、サンプリング時刻ｔkまでの観測情報Ｚkと運動モデルの仮説Ψk,aとに基づいてサンプリング時刻ｔkでの真値を推定した平滑ベクトルに相当する。
【００７７】
上記(55)式で表されるＸk(-)は、サンプリング時刻ｔk-1までの観測情報Ｚk-1に基づくＸkの条件付き平均値で、サンプリング時刻ｔk-1までの観測情報Ｚk-1に基づいてサンプリング時刻ｔkでの真値を推定した予測ベクトルに相当する。
【００７８】
上記(56)式で表されるＸk(+)は、サンプリング時刻ｔkまでの観測情報Ｚkに基づくＸkの条件付き平均値で、サンプリング時刻ｔkまでの観測情報Ｚkに基づいてサンプリング時刻ｔkでの真値を推定した平滑ベクトルに相当する。
【００７９】
また、上記(57)式〜(60)式で表されるＰk,a(-)、Ｐk,a(+)、 Ｐk(-)、 Ｐk(+)は、それぞれＸk,a(-)、 Ｘk,a(+)、 Ｘk(-)、 Ｘk(+)の誤差共分散行列を表す。すなわち、Ｘk,a(-)は運動モデルごとの予測誤差共分散行列を、Ｘk,a(+)は運動モデルごとの平滑誤差共分散行列を、Ｘk(-)はＮ個の運動モデルによる予測誤差共分散行列を、また、Ｘk(+)はＮ個の運動モデルによる平滑誤差共分散行列を、それぞれ表している。
【００８０】
本実施形態において、平滑ベクトルの初期値Ｘ0(+)、および、平滑誤差共分散行列の初期値Ｐ0(+)は、カルマンフィルタが通常適用される場合と同様に、別途定められているものとする。
ところで、上述した処理において、Ｐk,a(-)は、式(45)に示す如く、仮説Ψk,aによらない値である。このため、式(48)で表されるＫk、および、式(50)で表されるＰk,a(+)も、同様に仮説Ψk,aによらない値となる。
【００８１】
次に、図２に示すフローチャートを参照して本実施形態の目標追尾装置の動作について説明する。
本実施形態の目標追尾装置では、先ず、目標観測装置１より得られる目標位置観測情報を基に、通常のカルマンフィルタ理論に基づき、目標位置・速度の平滑値の初期値Ｘ0(+)、および平滑誤差共分散行列の初期値Ｐ0(+)が設定される（ＳＴ１）。
【００８２】
次に、目標の運動モデルとして、図９に示すようなＮ個の定数加速度ベクトル（零加速度ベクトルを含む）よりなるＮ個の運動モデルが設定設定される（ＳＴ２）。
【００８３】
次に、運動モデルごとの予測器６において、平滑器３より入力されるＮ個の運動モデルによる平滑値Ｘk-1(+)（式(56)）、およびＮ個の運動モデルを構成している定数加速度ベクトルαa（式(24)）に基づいて、式(44)に従って、式(53)の運動モデルごとの予測値Ｘk,a(-)が算出される。更に、運動モデルごとの予測誤差評価器１０において、平滑誤差評価器９より入力されるＮ個の運動モデルによる平滑誤差共分散行列Ｐk-1(+)（式(60))、および予め設定された駆動雑音共分散行列Ｑk-1（式(23))に基づいて、式(45)に従って、式(57)の運動モデルごとの予測誤差共分散行列Ｐk,a(-)が算出される（ＳＴ３）。
【００８４】
次に、予測器５において、平滑器３から出力されるＮ個の運動モデルによる平滑値Ｘk-1(+)（式(56)）、信頼度算出器２から出力される各運動モデルの信頼度μk-1,a(+)（式(30)）、観測時刻算出器１５から出力される次の観測時刻、および運動モデル間推移確率算出器１６から出力される運動モデル間推移確率ｐk,ab（式(28)）が読み込まれる。予測器５は、それらの入力データ、および予め設定されている定数加速度ベクトルαa（式(24)）に基づいて、式(46)に従い、式(55)のＮ個の運動モデルによる予測値Ｘk(-)を算出する。
【００８５】
更に、予測誤差評価器１１において、運動モデルごとの予測誤差評価器１０から出力される運動モデルごとの予測誤差共分散行列Ｐk,a(-)（式(57)）、信頼度算出器２から出力される各運動モデルの信頼度μk-1,a(+)（式(30)）、運動モデル間推移確率算出器１６から出力される運動モデル間推移確率ｐk,ab（式(28)）、および観測時刻算出器１５から出力される次の観測時刻が読み込まれる。予測誤差評価器１１は、それらの入力データ、および定数加速度ベクトルαa（式(24)）に基づいて、式(47)に従い、式(59)のＮ個の運動モデルによる予測誤差共分散行列Ｐk(-)を算出する（ＳＴ４）。
【００８６】
次に、目標観測装置１において、観測値Ｚkが読み込まれる（ＳＴ５）。上記の観測値は、目標からの信号の検出結果であり、位置情報で構成されている。
【００８７】
目標観測装置１で観測された観測値は、信頼度算出器２に供給される。信頼度算出器２は、更に、現時刻より１サンプリング前に算出した各運動モデルの信頼度μk-1,a(+)（式(30)）を遅延回路１３を介して読み込み、運動モデルごとの予測器６が現時刻より１サンプリング前に算出した運動モデルごとの予測値Ｘk,a(-) （式(53)）を遅延回路７を介して読み込み、運動モデルごとの予測誤差評価器１０が現時刻より１サンプリング前に算出した運動モデルごとの予測誤差共分散行列Ｐk,a(-)（式(57)）を遅延回路8を介して読み込み、運動モデル間推移確率算出器１６が現時刻より１サンプリング前に算出した運動モデル間推移確率ｐk,ab（式(28)）を遅延回路１７を介して読み込む。信頼度算出器２は、それらの入力データと、予め設定されている観測雑音共分散行列Ｒk（式(42)）とに基づいて、式(31)に従い、式(30)の運動モデルの信頼度μk,a(+)を算出する（ＳＴ６）。
【００８８】
次に、ゲイン行列算出器１２において、運動モデルごとの予測誤差評価器１０が現時刻より１サンプリング前に算出した運動モデルごとの予測誤差共分散行列Ｐk,a(-)（式(57)）が遅延回路8を介して読み込まれる。ゲイン行列算出器１２は、その入力データと、予め設定された観測雑音共分散行列Ｒk（式(42)）とに基づいて、式(48)に従いゲイン行列Ｋkを算出する（ＳＴ７）。
【００８９】
次に、平滑器３において、現時刻より１サンプリング前に算出しておいたＮ個の運動モデルによる平滑値Ｘk-1(+)（式(56)）が遅延回路4を介して読み込まれる。平滑器３は、更に、信頼度算出器２が出力する各運動モデルの信頼度μk,a(+)（式(30)）、ゲイン行列算出器１２が出力するゲイン行列Ｋk（式(48)）、および目標観測装置１が出力する観測値Ｚkを読み込む。平滑器３は、それらの入力データと、予め設定されている定数加速度ベクトルαa（式(24)）とに基づいて、式(51)に従い、式(56)のＮ個の運動モデルによる平滑値Ｘk(+)を算出する。
【００９０】
また、平滑誤差評価器９においては、信頼度算出器２から出力される各運動モデルの信頼度μk,a(+)（式(30)）が読み込まれる。平滑誤差評価器９は、更に、運動モデルごとの予測誤差評価器１０が現時刻より１サンプリング前に算出した運動モデルごとの予測誤差共分散行列Ｐk,a(-)（式(57)）を遅延回路８を介して読み込み、ゲイン行列算出器１２から出力されるゲイン行列Ｋk（式(48)）を読み込む。平滑誤差評価器９は、それらの入力データと、あらかじめ設定されている定数加速度ベクトルαa（式(24)）とに基づいて、式(52)に従い、式(60)のＮ個の運動モデルによる平滑誤差共分散行列Ｐk(+)を算出する（ＳＴ８）。
【００９１】
次に、観測時刻算出器１５において、信頼度算出器２から出力される運動モデルごとの信頼度μk,a(+)（式(30)）、平滑器３から出力されるＮ個の運動モデルによる平滑値Ｘk(+)（式(56)）、平滑誤差評価器９から出力されるＮ個の運動モデルによる平滑誤差共分散行列Ｐk(+)（式(60)）が読み込まれると共に、次回観測を行う観測時刻ｔk+1が算出される。
【００９２】
また、運動モデル間推移確率算出器１６において、観測時刻算出器１５で算出される次回の観測時刻と、現観測時刻との差よりサンプリング間隔が算出される。運動モデル間推移確率算出器１６は、更に、算出されたサンプリング間隔に合わせた運動モデル間推移確率を、例えば式(29)により算出する（ＳＴ９）。
【００９３】
上述した一連の処理が終了すると、追尾処理の終了が要求されているか否かが判別される（ＳＴ１０）。その結果、処理の終了が要求されていないと判別される場合は、所定のサンプリング間隔で、上記ＳＴ３以降の処理が繰り返し実行される。
【００９４】
次に、図３に示すフローチャートを参照して、上述したＳＴ９（図２）における観測時刻算出器１５の動作について説明する。
図３は、上述したＳＴ９において実行される一例の処理の内容を示すフローチャートである。図３に示す処理においては、先ず、ＳＴ１１の処理が実行される。
【００９５】
ＳＴ１１では、サンプリング間隔Ｔの初期化が行われる。本ステップＳＴ１１の処理により、サンプリング間隔Ｔは、予め設定されている最小サンプリング間隔ｔsに設定される。
【００９６】
ＳＴ１２では、式(46)、(47)に従って、時刻ｔk＋Ｔ−ｔｓにおけるＮ個の運動モデルによる平滑値および平滑誤差共分散行列に基づいて、最小サンプリング間隔ｔsだけ先の時刻、すなわち、未来の時刻ｔk＋ＴにおけるＮ個の運動モデルによる予測値および予測誤差共分散行列が算出される。
【００９７】
ＳＴ１３では、ＳＴ１２で算出された時刻ｔk＋ＴにおけるＮ個のモデルによる予測値および予測誤差分散行列に基づいて、誤差楕円体が計算されると共に、その誤差楕円体の体積が、評価値として求められる。
【００９８】
上記の処理を具体的に説明する。ここで、時刻ｔk＋ＴにおけるＮ個のモデルによる予測値（式(46)）および予測誤差共分散行列（式(47)）を、それぞれＸk+T(-)、 Ｐk+T(-)と表す。この場合、上記の誤差楕円体は、次式(1)により求めることができる。
【００９９】
［Ｚk+T−Ｈ・Ｘk+T(-)］TＳk+T-1［Ｚk+T−Ｈ・Ｘk+T(-)］≦ｄ (1)
式(1)の左辺は自由度3のカイ2乗分布となる。ｄをパラメータとして式(1)の時刻ｔk+Tにおける観測値Ｚk+Tのなす領域は楕円体となる。この楕円体を誤差楕円体と呼ぶ。
ただし、上記(1)式中、Ｓk+Tは次式(2)で表される値であり、Ｈは式(40)の観測行列、Ｒk+Tは時刻ｔk+Tにおける式(23)の観測雑音共分散行列である。
Ｓk+T＝Ｈ・Ｐk+T(-)・ＨT＋Ｒk+T (2)
【０１００】
誤差楕円体の体積ＶGは次式(3)で計算される。本実施形態においては、その計算値ＶGが評価値として用いられる。尚、式(3)に示すΓ(・)はガンマ関数である。
【０１０１】
【数１３】
【０１０２】
ＳＴ１４では、次式(4)に従って、観測誤差共分散行列Ｒk+Tに基づく観測誤差楕円体の体積が算出される。本実施形態においては、その値がしきい値Ｖthとして用いられる。
【０１０３】
【数１４】
【０１０４】
尚、本実施形態において用いられるしきい値は、上記の値に限定されるものではなく、パラメータλ１を設定して、λ１・Ｖth をしきい値としてもよい。
【０１０５】
ＳＴ１９では、上述した評価値ＶGと、上述したしきい値Ｖthとを比較するしきい値判定が行われる。その結果、評価値ＶG＜しきい値Ｖthが成立すると判別されると、次にＳＴ２０の処理が実行される。一方、上記の条件が成立しないと判別される場合は、次にＳＴ２１の処理が実行される。
【０１０６】
ＳＴ２０では、サンプリング間隔Ｔがｔsだけ延長される、すなわちＴ＝Ｔ＋ｔsとする処理が実行される。以後、ＳＴ１９において、評価値ＶG＜しきい値Ｖthが成立しないと判定されるまで、繰り返し上記ＳＴ１２以降の処理が実行される。本ステップＳＴ２０では、また、ＳＴ１２で算出された時刻ｔk+TにおけるＮ個の運動モデルによる予測値および予測誤差共分散行列を、そのまま時刻ｔk+TにおけるＮ個の運動モデルによる平滑値および平滑誤差共分散行列とする処理が行われる。ＳＴ１２以降の処理が繰り返し実行される間、ＳＴ１２の処理は、このようにして前回の処理サイクルで設定された平滑値および平滑誤差共分散行列を用いて行われる。
【０１０７】
ＳＴ２１では、次の観測時刻ｔk+1が、時刻ｔk＋Ｔに、すなわちＳＴ１１〜ＳＴ２０の処理により設定されたサンプリング間隔Ｔだけ先の時刻に設定される。以後、このようにして設定された観測時刻ｔｋ＋Ｔを用いて、上記図２に示すルーチンが実行される。
【０１０８】
誤差楕円体の大きさは、その時刻での目標追尾精度を表している。つまり、追尾精度がよければ誤差楕円体は小さく、追尾精度が悪ければ誤差楕円体は大きくなる。上述の如く、本実施形態の目標追尾装置によれば、この原理を利用して、追尾精度がよいときはサンプリング間隔Ｔを長くし、追尾精度が悪いときは観測間隔を短くすることができる。
【０１０９】
また、図３に示すルーチンによれば、サンプリング間隔Ｔが延長される毎に、Ｎ個の運動モデルによる予測値および予測誤差共分散行列を逐次更新しながら、評価値およびしきい値を算出して、しきい値判定を行うことができる。すなわち、サンプリング間隔Ｔを、最小間隔ｔs単位で少しずつ延長しながらしきい値判定を行うことができる。このため、本実施形態の目標追尾装置によれば、未来の各時刻における目標追尾精度を予測しながら、最適なサンプリング間隔Ｔを算出することができ、所望の目標追尾精度を保つことができる。
【０１１０】
このように構成された実施の形態1の目標追尾装置によれば、目標追尾精度に合わせて観測間隔を適応的に制御できるので、機動力の高い目標でも等速直線運動を行う目標でも一定の水準の追尾精度を保つことができる。
【０１１１】
尚、上記の実施形態においては、観測時刻算出器１５が、上記ＳＴ１３、ＳＴ１４、およびＳＴ１９の処理を実行することで、前記請求項１記載の「追尾精度検出手段」が、また、上記ＳＴ１１、ＳＴ１２、およびＳＴ２０の処理を実行することで、前記請求項１記載の「観測時刻算出手段」が、それぞれ実現されている。
【０１１２】
また、上記の実施形態においては、観測時刻算出器１５が、上記ＳＴ２０の処理を行うことにより前記請求項４記載の「間隔再設定手段」が、ＳＴ１２の処理を行うことにより前記請求項４記載の「予測更新手段」が、ＳＴ１３の処理を行うことにより前記請求項４または５記載の「評価値算出手段」が、ＳＴ１４の処理を行うことにより請求項５記載の「しきい値設定手段」が、ＳＴ１９の処理を行うことにより前記請求項４または５記載の「しきい値判定手段」が、それぞれ実現されている。
【０１１３】
実施の形態２．
次に、図４および図５を参照して、本発明の実施の形態２について説明する。図４は本発明の目標追尾装置の実施の形態２を示すブロック図である。図４において、１〜１４、１６および１７は、実施の形態1の場合と同等の構成要素である。
【０１１４】
本実施形態の目標追尾装置は、センサ諸元入力器１８を備えている。センサ諸元入力器１８は、観測時刻算出器１５に対して目標追尾装置が備えるセンサの諸元を入力する。
【０１１５】
本実施形態において、観測時刻算出器１５は、Ｎ個の運動モデルによる信頼度、Ｎ個の運動モデルによる平滑値、およびＮ個の運動モデルによる平滑誤差に、センサ諸元を加えた入力データに基づいて次回のサンプリングを行う観測時刻を算出する。
【０１１６】
本実施形態の目標追尾装置の全体的な動作は、実施の形態１の装置と同様であり、図２の如く表される。以下、本実施形態の特徴部である観測時刻算出器１５の動作を、図５に示すフローチャートを参照して説明する。
観測時刻算出器１５は、図５に示す一連の処理を実行することで、サンプリング時刻ｔkにおける追尾精度とセンサ諸元とを考慮に入れて、次の観測時刻ｔk+1を算出する。ここでは、特に、極座標Ｅ-Ｂｙ面において算出される目標の誤差楕円の面積を評価値とし、かつ、センサの観測領域の面積から求めたしきい値を使ってしきい値判定を行う方法を示す。
【０１１７】
図５に示すフローチャートにおいて、ＳＴ１１、ＳＴ１２、およびＳＴ１９〜２１の処理は、実施の形態１の場合と同様である。
図５に示すＳＴ１５の評価値算出処理、およびＳＴ１６に示すしきい値算出処理においては、以下の理論に従って評価値およびしきい値が算出される。
【０１１８】
まず、時刻ｔk+Tにおいて、レーダより見た目標角度の真値を表すベクトルθk+Tを、式(5)のように定義する。
【０１１９】
【数１５】
【０１２０】
時刻ｔk+Tにおける目標角度の予測ベクトル（「θチルダk+T」と称す）は、Ｎ個の運動モデルによる予測値Ｘk+T(-)を使用し、式(6)で与えられる。
【０１２１】
【数１６】
【０１２２】
ただし、次式(7)の関係が成立するものとする。
【０１２３】
【数１７】
【０１２４】
ここで、式(7)に示す[ａr ｂr ｃr]は、センサのxyz直交座標系における位置ベクトルであり、また、[x y z]は、目標予測値Ｘk+T(-)の位置成分である。
【０１２５】
上記(6)式で表される角度予測ベクトルθチルダk+Tの平均値をθk+Tmとすると、角度予測ベクトルの予測誤差“θチルダk+T -θk+Tm”の共分散行列は式(8)のように表せる。尚、式(8)に示すＥ［・］は平均を表す。
【０１２６】
【数１８】
【０１２７】
ただし、Ｆk+Tは次式(9)とする。また、Ｐk+T(-)は、時刻ｔk+TにおけるＮ個の運動モデルによる予測誤差共分散行列である。
【０１２８】
【数１９】
【０１２９】
次に、誤差楕円の面積を計算する。次式(10)に示す変量は、自由度2のカイ2乗分布に従う値である。式(10)の変量とパラメータｄ′とを用いると、条件式 (11)を生成することができる。式(11)を満たすθチルダk+Tの領域は、極座標系Ｅ-Ｂｙ面内において楕円を成す。
【０１３０】
【数２０】
【０１３１】
上記誤差楕円の面積σGは、式(12)により算出される。本実施形態においては、その値σGが評価値として用いられる（ＳＴ１５）。
【０１３２】
【数２１】
【０１３３】
一方、センサの観測領域の角度幅をφとするとき、観測領域のＥ-Ｂｙ面内における面積は、次式(13)のように定義することができる。本実施形態においては、式(13)に従って演算される値がしきい値σthとして用いられる（ＳＴ１６）。
【０１３４】
【数２２】
【０１３５】
ここで、Ｅチルダk+Tは、時刻ｔk+Tにおける目標位置予測値の仰角成分である。尚、本実施形態においては、式(13)の演算値σthがしきい値として用いられているが、しきい値は、これに限定されるものではなく、パラメータλ２を設定して、λ２・σthをしきい値としてもよい。
【０１３６】
上述した方式では、センサ位置やセンサの観測領域を考慮にいれた追尾精度が評価される。このため、本実施形態の目標追尾装置によれば、センサからみた追尾精度がよいときはサンプリング間隔を長くし、その精度が悪いときはサンプリング間隔を短くすることができる。
【０１３７】
本発明の実施の形態２によれば、センサ諸元を入力データとして追加した観測時刻算出器１５を備えているので、センサと目標との位置関係やセンサの観測領域を考慮にいれた追尾精度が評価され、その評価結果に基づいて次のサンプリング時刻が算出される。このため、本実施形態の目標追尾装置は、機動力の高い目標でも、等速直線運動を行う目標でも、センサから見た追尾精度を一定の水準に保ったまま追尾を行うことができる。
【０１３８】
尚、上記の実施形態においては、観測領域の角度幅φ、およびセンサ位置などがセンサ諸元に相当する。
【０１３９】
また、上記の実施形態においては、観測時刻算出器１５が、上記ＳＴ１５の処理を行うことにより前記請求項６記載の「評価値設定手段」が、ＳＴ１６の処理を行うことにより前記請求項６記載の「しきい値設定手段」が、ＳＴ１９の処理を行うことにより前記請求項６記載の「しきい値判定手段」が、それぞれ実現されている。
【０１４０】
実施の形態３．
次に、図６を参照して、本発明の実施の形態３について説明する。実施の形態３の目標追尾装置の構成および全体の動きは実施の形態２と同等であり、それぞれ図４および図２で表される。
【０１４１】
図６は、実施の形態３における観測時刻算出器１５において実行される一連の処理のフローチャートを示す。ここでは、極座標Ｅ-Ｂｙ面における目標の誤差楕円の長軸径を評価値とし、かつ、センサの観測領域の幅(例えば、レーダにおけるビーム幅)から求めたしきい値を使ってしきい値判定を行う方法を示す。
【０１４２】
図６においてＳＴ１１、ＳＴ１２、およびＳＴ１９〜２１の処理は、実施の形態２の場合と同等である。
図６に示すＳＴ１７の評価値算出処理、およびＳＴ１８に示すしきい値算出処理においては、以下の方法に従って評価値およびしきい値が算出される。
【０１４３】
ＳＴ１７では、先ず、上記(8)式で表される行列の逆行列Ａk+T-1の固有値ｃ１およびｃ２ (ｃ１ ≦ ｃ２)が求められる。次に、次式(14)に従って、誤差楕円の長軸の直径φGが算出される。本実施形態においては、その直径φGが評価値として用いられる。
φG＝２√（ｄ′／ｃ１） (14)
【０１４４】
ＳＴ１８では、センサの観測領域の幅がしきい値φthとされる。センサの観測領域幅とは、例えばセンサとしてレーダを仮定すると、そのレーダのビーム幅を角度で表したものなどである。本実施形態においては、観測領域幅φthがしきい値として用いられているが、しきい値は、これに限定されるものではなく、パラメータλ３を設定して、λ３・φthをしきい値としてもよい。
【０１４５】
本発明の実施の形態３によれば、センサ諸元を入力として追加した観測時刻算出器１５を備えるので、実施の形態２と同様に、センサと目標との位置関係やセンサのビーム幅を考慮にいれた追尾精度を評価し、その結果に基づいてサンプリング間隔を算出することができる。このため、本実施形態の目標追尾装置によれば、機動力の高い目標でも、等速直線運動を行う目標でも、センサから見た追尾精度を一定の水準に保ったまま追尾することができる。
【０１４６】
尚、上記の実施形態においては、観測時刻算出器１５が、上記ＳＴ１７の処理を行うことにより前記請求項７記載の「評価値設定手段」が、ＳＴ１８の処理を行うことにより前記請求項７記載の「しきい値設定手段」が、ＳＴ１９の処理を行うことにより前記請求項７記載の「しきい値判定手段」が、それぞれ実現されている。
【０１４７】
【発明の効果】
この発明は以上説明したように構成されているので、以下に示すような効果を奏する。
請求項１記載の発明によれば、目標追尾精度に基づいてサンプリング間隔を適応的に決定することができるため、機動力が高く追尾精度が劣化しやすい目標の場合は観測間隔を短くし、等速直線運動を行う目標の場合は観測間隔を長くすることができる。これにより、如何なる目標に対しても追尾精度を一定の水準に保つことができる。
【０１４８】
請求項２記載の発明によれば、サンプリング間隔を、センサ諸元をも考慮して適応的に決定することができるため、如何なるセンサで追尾を行っても、そのセンサの性能に合わせたサンプリング間隔を決定することができる。従って、本発明の目標追尾装置によれば、センサの特性や目標の特性に関わらず、常に良好な追尾精度を得ることができる。
【０１４９】
請求項３記載の発明によれば、サンプリング間隔を、最小のサンプリング間隔単位で延長しながら、Ｎ個の運動モデルによる予測値および予測誤差を算出し、更に、それらが更新される毎に追尾精度の状態を判断することができる。そして、その判断に基づいて最適なサンプリング間隔を設定し、次回の観測時刻を最適な時刻に設定することができる。従って、本発明によれば、所望の追尾精度を安定に保持することができる。
【０１５０】
請求項４記載の発明によれば、Ｎ個の運動モデルによる予測誤差共分散行列より求めた誤差楕円体の体積を評価値とし、その評価値と、観測誤差共分散行列より求めたしきい値とを比較することでしきい値判定が行われる。追尾目標が如何なるものであっても、その誤差楕円体の体積には、その目標に対する追尾精度が反映される。従って、本発明によれば、追尾目標の特性に関わらず、目標追尾精度を安定に一定の水準に保つことができる。
【０１５１】
請求項５記載の発明によれば、Ｎ個の運動モデルによる予測誤差共分散行列より求めた誤差楕円体の極座標面での面積が評価値とされると共に、センサ諸元に含まれる極座標面内でのセンサの観測領域の面積に基づいてしきい値が設定される。このため、本発明によれば、センサの特性に応じたサンプリング間隔を設定することができ、センサの特性に関わらず、目標追尾精度を一定の水準に保つことができる。
【０１５２】
請求項６記載の発明によれば、Ｎ個の運動モデルによる予測誤差共分散行列より求めた誤差楕円体の極座標面での長軸直径が評価値とされると共に、センサ諸元に含まれる極座標面内でのセンサの観測領域の幅に基づいてしきい値が設定される。このため、本発明によれば、センサの特性に応じたサンプリング間隔を設定することができ、センサの特性に関わらず、目標追尾精度を一定の水準に保つことができる。
【図面の簡単な説明】
【図１】 本発明の実施の形態1に係わる目標追尾装置のブロック図である。
【図２】 本発明の実施の形態１、２または３に係わる目標追尾装置の動作を説明するためのフローチャートである。
【図３】 本発明の実施の形態１の観測時刻算出器の動作を説明するためのフローチャートである。
【図４】 本発明の実施の形態２または３に係わる目標追尾装置のブロック図である。
【図５】 本発明の実施の形態２の観測時刻算出器の動作を説明するフローチャートである。
【図６】 本発明の実施の形態３の観測時刻算出器の動作を説明するフローチャートである。
【図７】 従来の目標追尾装置のブロック図である。
【図８】 従来の目標追尾装置の動作を説明するためのフローチャートである。
【図９】 定数加速度のベクトルの一例である。
【符号の説明】
１ 目標観測装置、 ２ Ｎ個の運動モデルによる信頼度算出器、 ３ Ｎ個の運動モデルによる平滑器、 ４，７，８，１３，１４，１７ 遅延回路、 ５ Ｎ個の運動モデルによる予測器、 ６ 運動モデルごとの予測器、９ Ｎ個の運動モデルによる平滑誤差評価器、 １０ 運動モデルごとの予測誤差評価器、 １１ Ｎ個の運動モデルによる予測誤差評価器、 １２ゲイン行列算出器、 １５ 観測時刻算出器、 １６ 運動モデル間推移確率算出器、 １８ センサ諸元入力器。
【発明の属する技術分野】
本発明は、センサ等からの目標の位置情報に関する観測データに基づいて目標の位置や速度などの目標運動諸元を推定する目標追尾装置に係り、目標を観測する間隔(サンプリング間隔)を制御することで、機動力の高い目標に対する追尾精度を向上させた目標追尾装置に関する。
【０００２】
【従来の技術】
図７は、特開平１−９２６７８号公報に開示された従来の目標追尾装置を示すブロック図である。また、図８は、図７に示す目標追尾装置の動作を説明するためのフローチャートを示す。以下、図７を参照して従来の目標追尾装置の構成を説明する。
【０００３】
目標観測装置１は、目標の予測位置をセンサ（図示せず）に指示すると共に、センサが観測する目標位置に関する観測情報（以下、「目標位置観測情報」と称す）を取得するものである。
【０００４】
Ｎ個の運動モデルによる信頼度算出器２（以下、単に「信頼度算出器」と称す）は、上述した目標位置観測情報、運動モデルごとの予測値、運動モデルごとの予測誤差、および１サンプリング前の各運動モデルの信頼度を基に、Ｎ個の運動モデルによる信頼度を算出するものである。
１３は遅延回路であり、信頼度算出器２によって算出された信頼度を単位時間だけ遅延させて信頼度算出器２に入力するものである。
【０００５】
Ｎ個の運動モデルによる平滑器３（以下、単に「平滑器３」と称す）は、上述した目標位置観測情報、各運動モデルの信頼度、運動モデルごとの予測値、ゲイン行列、および１サンプリング前のＮ個の運動モデルによる平滑値を基に、Ｎ個の運動モデルによる平滑値を算出するものである。
４は遅延回路であり、平滑器３によって算出された平滑値を単位時間だけ遅延させて平滑器３に入力するものである。
【０００６】
Ｎ個の運動モデルによる予測器５（以下、単に「予測器５」と称す）は、上記Ｎ個の運動モデルによる平滑値を基に、Ｎ個の運動モデルによる予測位置を算出するものである。
１４は遅延回路であり、予測器５によって算出された予測位置を単位時間だけ遅延させて目標観測装置１に入力するものである。
【０００７】
運動モデルごとの予測器６は、上述したＮ個の運動モデルによる平滑値を基に、
運動モデルごとの予測値を算出するものである。
７は遅延回路であり、運動モデルごとの予測器６によって算出された運動モデルごとの予測値を遅延させて信頼度算出器２に入力するものである。
【０００８】
Ｎ個の運動モデルによる平滑誤差評価器９（以下、単に「平滑誤差評価器９」と称す）は、上述した各運動モデルの信頼度、運動モデルごとの予測誤差、およびゲイン行列を基に、Ｎ個の運動モデルによる平滑誤差を算出するものである。
【０００９】
運動モデルごとの予測誤差評価器１０は、上記Ｎ個の運動モデルによる平滑誤差を基に、運動モデルごとの予測誤差を算出するものである。
８は遅延回路であり、運動モデルごとの予測誤差評価器10によって算出された運動モデルごとの予測誤差を遅延させて、信頼度算出器２、平滑誤差評価器９、およびゲイン行列算出器12に入力するものである。
【００１０】
Ｎ個の運動モデルによる予測誤差評価器１１（以下、単に「予測誤差評価器１１」と称す）は、上述した運動モデルごとの予測誤差、および各運動モデルの信頼度を基に、Ｎ個の運動モデルによる予測誤差を算出するものである。
ゲイン行列算出器12は、上述した運動モデルごとの予測誤差を基に、ゲイン行列を算出するものである。
【００１１】
次に図８に示すフローチャートを参照して従来の目標追尾装置の動作について説明する。
目標追尾装置では、先ず、目標観測装置１より得られる目標位置観測情報を基に、通常のカルマンフィルタ理論に基づき、目標位置・速度の平滑値、および平滑誤差共分散行列の初期値が設定される（ＳＴ１）。
【００１２】
次に、目標の運動モデルとして、図９に示すようなＮ個の定数加速度ベクトル（零加速度ベクトルを含む）よりなるＮ個の運動モデルが設定設定される（ＳＴ２）。
【００１３】
次に、運動モデルごとの予測器６において、平滑器３より入力されるＮ個の運動モデルによる平滑値、およびＮ個の運動モデルを構成している定数加速度ベクトルに基づいて、運動モデルごとの予測値が算出される。更に、運動モデルごとの予測誤差評価器１０において、平滑誤差評価器９より入力されるＮ個の運動モデルによる平滑誤差共分散行列、および予め設定された駆動雑音共分散行列に基づいて、運動モデルごとの予測誤差共分散行列が算出される（ＳＴ３）。
【００１４】
次に、予測器５において、平滑器３から出力されるＮ個の運動モデルによる平滑値、および信頼度算出器２から出力される各運動モデルの信頼度が読み込まれる。予測器５は、それらの入力データ、予め設定されている推移確率（目標の運動状態に適合する運動モデルが推移する確率）、および定数加速度ベクトルに基づいて、Ｎ個の運動モデルによる予測値を算出する。
【００１５】
また、予測誤差評価器１１においては、運動モデルごとの予測誤差評価器１０から出力される運動モデルごとの予測誤差共分散行列、および信頼度算出器２から出力される各運動モデルの信頼度が読み込まれる。予測誤差評価器１１は、それらの入力データ、予め設定されている推移確率、および定数加速度ベクトルに基づいて、Ｎ個の運動モデルによる予測誤差共分散行列が算出される（ＳＴ４）。
【００１６】
次に、目標観測装置１において、観測値が読み込まれる（ＳＴ５）。上記の観測値は、目標からの信号の検出結果であり、位置情報で構成されている。
【００１７】
目標観測装置１で観測された観測値は、信頼度算出器２に供給される。信頼度算出器２は、更に、現時刻より１サンプリング前に算出した各運動モデルの信頼度を遅延回路１３を介して読み込み、運動モデルごとの予測器６が現時刻より１サンプリング前に算出した運動モデルごとの予測値を遅延回路７を介して読み込み、運動モデルごとの予測誤差評価器１０が現時刻より１サンプリング前に算出した運動モデルごとの予測誤差共分散行列を遅延回路8を介して読み込む。信頼度算出器２は、それらの入力データと、予め設定されている観測雑音共分散行列および運動モデルの推移確率とに基づいて、運動モデルの信頼度を算出する（ＳＴ６）。
【００１８】
次に、ゲイン行列算出器１２において、運動モデルごとの予測誤差評価器１０が現時刻より１サンプリング前に算出した運動モデルごとの予測誤差共分散行列が遅延回路8を介して読み込まれる。ゲイン行列算出器１２は、その入力データと、予め設定された観測雑音共分散行列とに基づいてゲイン行列を算出する（ＳＴ７）。
【００１９】
次に、平滑器３において、現時刻より１サンプリング前に算出しておいたＮ個の運動モデルによる平滑値が遅延回路4を介して読み込まれる。平滑器３は、更に、信頼度算出器２が出力する各運動モデルの信頼度、ゲイン行列算出器１２が出力するゲイン行列、および目標観測装置１が出力する観測値を読み込む。平滑器３は、それらの入力データと、予め設定されている定数加速度ベクトルとに基づいてＮ個の運動モデルによる平滑値を算出する。
【００２０】
また、平滑誤差評価器９においては、信頼度算出器２から出力される各運動モデルの信頼度が読み込まれる。平滑誤差評価器９は、更に、運動モデルごとの予測誤差評価器１０が現時刻より１サンプリング前に算出した運動モデルごとの予測誤差共分散行列を遅延回路８を介して読み込み、ゲイン行列算出器１２から出力されるゲイン行列を読み込む。平滑誤差評価器９は、それらの入力データと、あらかじめ設定されている定数加速度ベクトルとに基づいて、Ｎ個の運動モデルによる平滑誤差共分散行列を算出する（ＳＴ８）。
【００２１】
上述した一連の処理が終了すると、追尾処理の終了が要求されているか否かが判別される（ＳＴ１０）。その結果、処理の終了が要求されていないと判別される場合は、所定のサンプリング間隔で、上記ＳＴ３以降の処理が繰り返し実行される。
【００２２】
【発明が解決しようとする課題】
従来の目標追尾装置は以上のように、目標追尾に必要な一連の処理を所定のサンプリング間隔で繰り返すように構成されている。サンプリング間隔が常に一定であると、旋回運動や直進運動をおり混ぜた運動を行う機動力の高い目標が、精度良く追尾できない事態が生じ得る。この点、従来の目標追尾装置は、機動性に優れた目標に対する追尾精度を劣化させ易いという問題を有するものであった。
【００２３】
本発明は、上記のような課題を解決するためになされたもので、目標追尾精度に合わせて適応的にサンプリング間隔を制御することで、追尾対象が機動力の高い目標であっても、追尾精度をある水準に保ちながら追尾を行うことができる目標追尾装置を提供することを目的とする。
【００２４】
【課題を解決するための手段】
請求項１記載の発明は、目標観測時刻と目標予測位置とをセンサに指示すると共に、センサによって観測される目標位置の観測情報を取得する目標観測装置を備える目標追尾装置であって、
目標追尾精度を検出する追尾精度検出手段と、
前記目標追尾精度に基づいて前記観測情報のサンプリング間隔を決定すると共に、そのサンプリング間隔を用いて、次回の観測時刻を算出する観測時刻算出手段とを備え、
前記追尾精度検出手段は、
目標位置観測情報、運動モデルごとの予測値、運動モデルごとの予測誤差、運動モデル間推移確率、および１サンプリング前のＮ個の運動モデルによる信頼度を基に、Ｎ個の運動モデルによる信頼度を算出するＮ個の運動モデルによる信頼度算出器と、
目標位置観測情報、Ｎ個の運動モデルによる信頼度、ゲイン行列、および１サンプリング前のＮ個の運動モデルによる平滑値を基に、Ｎ個の運動モデルによる平滑値を算出するＮ個の運動モデルによる平滑器と、
Ｎ個の運動モデルによる平滑値、Ｎ個の運動モデルによる信頼度、１サンプリング後の観測時刻、および運動モデル間推移確率を基に、Ｎ個の運動モデルによる予測値を算出するＮ個の運動モデルによる予測器と、
Ｎ個の運動モデルによる平滑値、および１サンプリング後の観測時刻を基に、運動モデルごとの予測値を算出する運動モデルごとの予測器と、
Ｎ個の運動モデルによる信頼度、運動モデルごとの予測誤差、およびゲイン行列を基に、Ｎ個の運動モデルによる平滑誤差を算出するＮ個の運動モデルによる平滑誤差評価器と、
Ｎ個の運動モデルによる平滑誤差および１サンプリング後の観測時刻を基に、運動モデルごとの予測誤差を算出する運動モデルごとの予測誤差評価器と、
運動モデルごとの予測誤差を基に、ゲイン行列を算出するゲイン行列算出器と、
運動モデルごとの予測誤差、Ｎ個の運動モデルによる信頼度、１サンプリング後の観測時刻および運動モデル間推移確率を基に、Ｎ個の運動モデルによる予測誤差を算出するＮ個の運動モデルによる予測誤差評価器と、
現在の観測時刻と次の観測時刻との間における運動モデル間の推移確率を算出する運動モデル間推移確率算出器と、を備え、
Ｎ個の運動モデルによる予測値と、Ｎ個の運動モデルによる予測誤差とに基づいて、目標追尾精度を求めることを特徴とするものである。
【００２６】
請求項２記載の発明は、請求項１記載の目標追尾装置であって、
前記追尾精度検出手段は、
センサの状態を表すセンサ諸元を読み込むセンサ諸元入力器を備えると共に、
Ｎ個の運動モデルによる予測値と、Ｎ個の運動モデルによる予測誤差と、センサ諸元とに基づいて、目標追尾精度求めることを特徴とするものである。
【００２７】
請求項３記載の発明は、請求項１または２記載の目標追尾装置であって、
前記観測時刻算出手段は、
サンプリング間隔を最小サンプリング間隔ごとに延長させる間隔再設定手段と、
サンプリング間隔が延長される毎に、Ｎ個の運動モデルによる予測値と、Ｎ個の運動モデルによる予測誤差とを更新する予測更新手段と、を備え、
前記目標追尾精度算出手段は、
前記Ｎ個の運動モデルによる予測値、および前記Ｎ個の運動モデルによる予測誤差が更新される毎に、目標追尾精度の評価値を算出する評価値算出手段と、
前記評価値と所定のしきい値とを比較するしきい値判定手段と、を備え、
前記観測時刻算出手段は、更に、前記しきい値判定手段によって前記評価値と前記しきい値の大小関係の逆転が判別された際のサンプリング間隔を、次のサンプリング間隔とすることを特徴とするものである。
【００２８】
請求項４記載の発明は、請求項１記載の目標追尾装置であって、
前記追尾精度算出手段は、
Ｎ個の運動モデルによる予測誤差共分散行列に基づいて演算される誤差楕円体の体積を評価値とする評価値設定手段と、
観測誤差共分散行列に基づいて演算される誤差楕円体の体積をしきい値とするしきい値設定手段と、
前記評価値と前記しきい値とを比較することにより追尾精度の状態を判断するしきい値判定手段と、
を備えることを特徴とするものである。
【００２９】
請求項５記載の発明は、請求項２記載の目標追尾装置であって、
前記センサ諸元は、極座標面におけるセンサの観測領域の面積を含み、
前記追尾精度算出手段は、
Ｎ個の運動モデルによる予測誤差共分散行列に基づいて演算される誤差楕円体の極座標面での面積を評価値とする評価値設定手段と、
前記センサ諸元に含まれる観測領域の面積に基づいてしきい値を設定するしきい値設定手段と、
前記評価値と前記しきい値とを比較することにより追尾精度の状態を判断するしきい値判定手段と、
を備えることを特徴とするものである。
【００３０】
請求項６記載の発明は、請求項２記載の目標追尾装置であって、
前記センサ諸元は、極座標面におけるセンサの観測領域の幅を含み、
前記追尾精度算出手段は、
Ｎ個の運動モデルによる予測誤差共分散行列に基づいて演算される誤差楕円体の極座標面での長軸直径を評価値とする評価値設定手段と、
前記センサ諸元に含まれる観測領域の幅に基づいてしきい値を設定するしきい値設定手段と、
前記評価値と前記しきい値とを比較することにより追尾精度の状態を判断するしきい値判定手段と、
を備えることを特徴とするものである。
【００３１】
【発明の実施の形態】
以下、図面を参照してこの発明の実施の形態について説明する。尚、各図において共通する要素には、同一の符号を付して重複する説明を省略する。
【００３２】
実施の形態１。
図１は、本発明の目標追尾装置の実施の形態１を示すブロック図である。図１において、目標観測装置１は、センサ（図示せず）に対して目標の予測位置を指示すると共に、センサが観測する目標位置に関する観測情報（以下、「目標位置観測情報」と称す）を取得する。
【００３３】
Ｎ個の運動モデルによる信頼度算出器２（以下、単に「信頼度算出器」と称す）は、上述した目標位置観測情報、運動モデルごとの予測値、運動モデルごとの予測誤差、および１サンプリング前の各運動モデルの信頼度を基に、Ｎ個の運動モデルによる信頼度を算出する。
【００３４】
Ｎ個の運動モデルによる平滑器３（以下、単に「平滑器３」と称す）は、上述した目標位置観測情報、Ｎ個の運動モデルによる信頼度、ゲイン行列、および１サンプリング前のＮ個の運動モデルによる平滑値を基に、Ｎ個の運動モデルによる平滑値を算出する。
【００３５】
Ｎ個の運動モデルによる予測器５（以下、単に「予測器５」と称す）は、上述したＮ個の運動モデルによる平滑値、Ｎ個の運動モデルによる信頼度、および１サンプリング後の観測時刻に基づいて、Ｎ個の運動モデルによる予測値を算出する。
【００３６】
運動モデルごとの予測器６は、Ｎ個の運動モデルによる平滑値、および１サンプリング後の観測時刻に基づいて、運動モデルごとの予測値を算出する。
【００３７】
Ｎ個の運動モデルによる平滑誤差評価器９（以下、単に「平滑誤差評価器９」と称す）は、Ｎ個の運動モデルによる信頼度、運動モデルごとの予測誤差、およびゲイン行列に基づいて、Ｎ個の運動モデルによる平滑誤差を算出する。
【００３８】
運動モデルごとの予測誤差評価器１０は、Ｎ個の運動モデルによる平滑誤差、および１サンプリング後の観測時刻に基づいて、運動モデルごとの予測誤差を算出する。
【００３９】
Ｎ個の運動モデルによる予測誤差評価器１１は、運動モデルごとの予測誤差、Ｎ個の運動モデルによる信頼度、および１サンプリング後の観測時刻に基づいて、Ｎ個の運動モデルによる予測誤差を算出する。
【００４０】
ゲイン行列算出器１２は、運動モデルごとの予測誤差を基に、ゲイン行列を算出する。
【００４１】
観測時刻算出器１５は、Ｎ個の運動モデルによる信頼度、Ｎ個の運動モデルによる平滑値、およびＮ個の運動モデルによる平滑誤差に基づいて、次回のサンプリング（目標観測）が行われる時刻（観測時刻）を算出する。
【００４２】
運動モデル間推移確率算出器１６は、観測時刻算出器１５の出力する観測時刻に基づいて、現在の観測時刻と次回の観測時刻との差からサンプリング間隔を算出し、このサンプリング間隔における運動モデル間の推移確率を算出する。
【００４３】
次に、本実施形態の目標追尾装置において、追尾フィルタとして用いられるＮ個の運動モデルを持つカルマンフィルタについて説明する。
カルマンフィルタにおいては、目標の運動モデルを式(15)のように定義する。
Ｘk＝Φk-1・Ｘk-1＋Γk-1・ωk-1＋Γ′k-1・ｕk-1 (15)
【００４４】
上記(15)式において、Ｘkは、サンプリング時刻ｔkにおける目標運動諸元の真値をあらわす次元状態ベクトルである。例えば、xyzの直交座標系における目標位置ベクトルを式(16)、直交座標系における目標速度ベクトルを式(17)とした時、Ｘkは式(18)で表される。
【００４５】
【数１】
上記(15)式において、Φk-1は、サンプリング時刻の変化（ｔk-1からｔk）に伴う状態ベクトルＸkの推移を表す推移行列で、目標が等速直線運動を行うと仮定した場合式(19)のように表される。尚、式(19)で用いられるＩは、式(20)で表される単位行列である。
【００４７】
【数２】
上記(15)式において、ωkは、サンプリング時刻ｔkにおける駆動雑音ベクトルである。また、式(15)において、Γkは、サンプリング時刻ｔkにおける駆動雑音ベクトルの変換行列である。目標の運動モデルを等速直線運動と仮定したことによる打切り誤差項をΓk-1・ωk-1とみれば、ωkは加速度ベクトル相当であり、Γk-1は式(21)のように表される。
【００４９】
【数３】
ωkは平均0の3次元正規分布白色雑音で、式(22)、式(23)のように表されるものとする。ここで0は零ベクトルを表し、Ｑkはサンプリング時刻ｔkにおける駆動雑音共分散行列である。
【００５１】
【数４】
上記(15)式において、ｕkは、サンプリング時刻ｔkにおいてＮ個の運動モデルを構成する定数加速度ベクトルであり、式(24)のように表すことができる。また、Γ′kはサンプリング時刻ｔkにおける定数加速度ベクトルの変換行列であり、式(25)のように表すことができる。
【００５３】
【数５】
図９は、定数加速度ベクトルを説明する図である。図９において、Oは座標O-xyzの原点である。座標0-xyzは、目標観測装置の位置を原点とする座標である。また、図９において、Ｘは東方向を正方向とする座標O-xyzのx軸、Ｙは北方向を正方向とする座標O-xyzのy軸、Ｚは上方向を正方向とする座標O-xyzのz軸を示す。更に、α１はy軸正方向の定数加速度ベクトル、α２はy軸負方向の定数加速度ベクトル、α３はx軸正方向の定数加速度ベクトル、α４はx軸負方向の定数加速度ベクトル、α５はz軸正方向の定数加速度ベクトル、α６はz軸負方向の定数加速度ベクトルである。この他に、加速度0の定数加速度ベクトルα７を考えた運動モデルの場合、モデル数Ｎは７となる。
【００５５】
サンプリング時刻ｔkにおいて式(26)が真であるとの仮説を式(27)で表す。
ｕk-1＝αa （ａ＝１，２，・・，Ｎ） (26)
Ψk,a （ａ＝１，２，・・，Ｎ） (27)
【００５６】
運動モデルの推移にマルコフ性を仮定する。すなわち運動モデルΨk,aはサンプリング時刻ｔk-1の運動モデルより決まり、サンプリング時刻ｔk-2までの運動モデルには依存しないと仮定する。
【００５７】
時刻ｔkにおける運動モデル間推移確率を式(28)のように表す。サンプリング間隔が可変の場合には、サンプリング間隔に合わせて運動モデル間推移確率を変える必要がある。例えば、最小サンプリング間隔をｔsとし、サンプリング間隔ｔk - ｔk-1 = ｔsの場合の運動モデル間推移確率を式(28)と同様の意味でｐabとする。さらに、Ｎ×Ｎ行列でa行b列の要素をｐabとしたものを運動モデル間推移確率行列Πと表す。サンプリング間隔がｔk - ｔk-1 = m・ｔs、 (m = 1、2、・・)と表せる場合、サンプリング時刻ｔkでの運動モデル間推移確率行列Πkは式(29)で表せる。このときのΠkのa行b列の要素が運動モデル間推移確率ｐk,abである。
【００５８】
サンプリング時刻ｔkまでの観測情報Ｚkに基づく、サンプリング時刻ｔkでの運動モデルの信頼度を条件付き確率密度関数により式(30)と定義すると、その信頼度は式(31)で計算される。
【００６０】
【数６】
ただし、νk,aは、観測ベクトルの正規分布近似P[Ｚk|Ψk,a，Ｚk-1]を多変量正規分布で近似したもので、式(32)で表される。
νk,a＝ｇ（Ｚk；Ｈ・Ｘk,a(-)，Ｈ・Ｐk,a(-)・ＨT＋Ｒk） (32)
尚、上記(32)式において、ｇ（ａ；ｂ，ｃ）は、平均ｂ、共分散行列ｃの３変量正規分布のａにおける確率密度関数である。また、上記(32)式において、記号Tは、その行列が転置行列であることを表す記号である。
【００６２】
サンプリング時刻ｔk-1までの観測情報Ｚk-1に基づくサンプリング時刻ｔkでの運動モデルの事前信頼度を条件付き確率密度関数により、式(33)と定義すると、その信頼度は式(34)と書ける。
【００６３】
【数７】
また、サンプリング時刻ｔkまでの観測情報Ｚkに基づく、定数加速度ベクトルの推定値を式(35)として定義すると、その推定値は式(36)と書ける。これを推定加速度ベクトルと呼ぶ。
【００６５】
【数８】
更に、サンプリング時刻ｔk-1までの観測情報Ｚk-1に基づく、定数加速度ベクトルの推定値を式(37)として定義すると、その推定値は式(38)と書ける。これを予測加速度ベクトルと呼ぶ。
【００６７】
【数９】
追尾対象目標に起因する観測値はサンプリング時刻ｔkにおいて1つ得られるとし、その観測系モデルを式(39)とする。ここでＺkはサンプリング時刻ｔkにおける位置情報の観測値で構成される直交座標による位置観測ベクトル、Ｈはサンプリング時刻ｔkにおける観測行列で、式(40)で表される。ｖkはサンプリング時刻ｔkでの観測ベクトルＺkに対応した観測雑音ベクトルであり、平均0の3次元正規分布白色雑音で、式(41)、式(42)のように表すことができる。尚、Ｒkはサンプリング時刻ｔkにおける観測雑音共分散行列で、運動モデルによらない値とする。また、駆動雑音ベクトルと観測雑音ベクトルは互いに独立であるとする。
【００６９】
【数１０】
またサンプリング時刻ｔ1からｔkまでの観測値ベクトルの集積を式(43)のように表す。
Ｚk＝［Ｚ1，Ｚ2，Ｚ3，・・，Ｚk］ (43)
【００７１】
カルマンフィルタの理論によれば、上記モデルに従いサンプリング時刻ｔkで観測値が得られた場合の、状態ベクトルＸkの推定値Ｘk(+)は式(44)〜(52)によって計算される。
【００７２】
【数１１】
ここで、Ｘk,a(-)、 Ｘk,a(+)、 Ｘk(-)、 Ｘk(+)、 Ｐk,a(-)、Ｐk,a(+)、 Ｐk(-)、Ｐk(+)は、それぞれ式(53)〜(60)のように定義される。尚、上記(48)式で表されるＫkは、サンプリング時刻ｔkにおけるゲイン行列である。
【００７４】
【数１２】
上記(53)式で表されるＸk,a(-)は、サンプリング時刻ｔk-1までの観測情報Ｚk-1と運動モデルの仮説Ψk,aとに基づく状態ベクトルＸkの条件付き平均値であり、サンプリング時刻ｔk-1までの観測情報Ｚk-1と運動モデルの仮説Ψk,aとに基づいてサンプリング時刻ｔkでの真値を推定した予測ベクトルに相当する。
【００７６】
上記(54)式で表されるＸk,a(+)は、サンプリング時刻ｔkまでの観測情報Ｚkと運動モデルの仮説Ψk,aとに基づく状態ベクトルＸkの条件付き平均値であり、サンプリング時刻ｔkまでの観測情報Ｚkと運動モデルの仮説Ψk,aとに基づいてサンプリング時刻ｔkでの真値を推定した平滑ベクトルに相当する。
【００７７】
上記(55)式で表されるＸk(-)は、サンプリング時刻ｔk-1までの観測情報Ｚk-1に基づくＸkの条件付き平均値で、サンプリング時刻ｔk-1までの観測情報Ｚk-1に基づいてサンプリング時刻ｔkでの真値を推定した予測ベクトルに相当する。
【００７８】
上記(56)式で表されるＸk(+)は、サンプリング時刻ｔkまでの観測情報Ｚkに基づくＸkの条件付き平均値で、サンプリング時刻ｔkまでの観測情報Ｚkに基づいてサンプリング時刻ｔkでの真値を推定した平滑ベクトルに相当する。
【００７９】
また、上記(57)式〜(60)式で表されるＰk,a(-)、Ｐk,a(+)、 Ｐk(-)、 Ｐk(+)は、それぞれＸk,a(-)、 Ｘk,a(+)、 Ｘk(-)、 Ｘk(+)の誤差共分散行列を表す。すなわち、Ｘk,a(-)は運動モデルごとの予測誤差共分散行列を、Ｘk,a(+)は運動モデルごとの平滑誤差共分散行列を、Ｘk(-)はＮ個の運動モデルによる予測誤差共分散行列を、また、Ｘk(+)はＮ個の運動モデルによる平滑誤差共分散行列を、それぞれ表している。
【００８０】
本実施形態において、平滑ベクトルの初期値Ｘ0(+)、および、平滑誤差共分散行列の初期値Ｐ0(+)は、カルマンフィルタが通常適用される場合と同様に、別途定められているものとする。
ところで、上述した処理において、Ｐk,a(-)は、式(45)に示す如く、仮説Ψk,aによらない値である。このため、式(48)で表されるＫk、および、式(50)で表されるＰk,a(+)も、同様に仮説Ψk,aによらない値となる。
【００８１】
次に、図２に示すフローチャートを参照して本実施形態の目標追尾装置の動作について説明する。
本実施形態の目標追尾装置では、先ず、目標観測装置１より得られる目標位置観測情報を基に、通常のカルマンフィルタ理論に基づき、目標位置・速度の平滑値の初期値Ｘ0(+)、および平滑誤差共分散行列の初期値Ｐ0(+)が設定される（ＳＴ１）。
【００８２】
次に、目標の運動モデルとして、図９に示すようなＮ個の定数加速度ベクトル（零加速度ベクトルを含む）よりなるＮ個の運動モデルが設定設定される（ＳＴ２）。
【００８３】
次に、運動モデルごとの予測器６において、平滑器３より入力されるＮ個の運動モデルによる平滑値Ｘk-1(+)（式(56)）、およびＮ個の運動モデルを構成している定数加速度ベクトルαa（式(24)）に基づいて、式(44)に従って、式(53)の運動モデルごとの予測値Ｘk,a(-)が算出される。更に、運動モデルごとの予測誤差評価器１０において、平滑誤差評価器９より入力されるＮ個の運動モデルによる平滑誤差共分散行列Ｐk-1(+)（式(60))、および予め設定された駆動雑音共分散行列Ｑk-1（式(23))に基づいて、式(45)に従って、式(57)の運動モデルごとの予測誤差共分散行列Ｐk,a(-)が算出される（ＳＴ３）。
【００８４】
次に、予測器５において、平滑器３から出力されるＮ個の運動モデルによる平滑値Ｘk-1(+)（式(56)）、信頼度算出器２から出力される各運動モデルの信頼度μk-1,a(+)（式(30)）、観測時刻算出器１５から出力される次の観測時刻、および運動モデル間推移確率算出器１６から出力される運動モデル間推移確率ｐk,ab（式(28)）が読み込まれる。予測器５は、それらの入力データ、および予め設定されている定数加速度ベクトルαa（式(24)）に基づいて、式(46)に従い、式(55)のＮ個の運動モデルによる予測値Ｘk(-)を算出する。
【００８５】
更に、予測誤差評価器１１において、運動モデルごとの予測誤差評価器１０から出力される運動モデルごとの予測誤差共分散行列Ｐk,a(-)（式(57)）、信頼度算出器２から出力される各運動モデルの信頼度μk-1,a(+)（式(30)）、運動モデル間推移確率算出器１６から出力される運動モデル間推移確率ｐk,ab（式(28)）、および観測時刻算出器１５から出力される次の観測時刻が読み込まれる。予測誤差評価器１１は、それらの入力データ、および定数加速度ベクトルαa（式(24)）に基づいて、式(47)に従い、式(59)のＮ個の運動モデルによる予測誤差共分散行列Ｐk(-)を算出する（ＳＴ４）。
【００８６】
次に、目標観測装置１において、観測値Ｚkが読み込まれる（ＳＴ５）。上記の観測値は、目標からの信号の検出結果であり、位置情報で構成されている。
【００８７】
目標観測装置１で観測された観測値は、信頼度算出器２に供給される。信頼度算出器２は、更に、現時刻より１サンプリング前に算出した各運動モデルの信頼度μk-1,a(+)（式(30)）を遅延回路１３を介して読み込み、運動モデルごとの予測器６が現時刻より１サンプリング前に算出した運動モデルごとの予測値Ｘk,a(-) （式(53)）を遅延回路７を介して読み込み、運動モデルごとの予測誤差評価器１０が現時刻より１サンプリング前に算出した運動モデルごとの予測誤差共分散行列Ｐk,a(-)（式(57)）を遅延回路8を介して読み込み、運動モデル間推移確率算出器１６が現時刻より１サンプリング前に算出した運動モデル間推移確率ｐk,ab（式(28)）を遅延回路１７を介して読み込む。信頼度算出器２は、それらの入力データと、予め設定されている観測雑音共分散行列Ｒk（式(42)）とに基づいて、式(31)に従い、式(30)の運動モデルの信頼度μk,a(+)を算出する（ＳＴ６）。
【００８８】
次に、ゲイン行列算出器１２において、運動モデルごとの予測誤差評価器１０が現時刻より１サンプリング前に算出した運動モデルごとの予測誤差共分散行列Ｐk,a(-)（式(57)）が遅延回路8を介して読み込まれる。ゲイン行列算出器１２は、その入力データと、予め設定された観測雑音共分散行列Ｒk（式(42)）とに基づいて、式(48)に従いゲイン行列Ｋkを算出する（ＳＴ７）。
【００８９】
次に、平滑器３において、現時刻より１サンプリング前に算出しておいたＮ個の運動モデルによる平滑値Ｘk-1(+)（式(56)）が遅延回路4を介して読み込まれる。平滑器３は、更に、信頼度算出器２が出力する各運動モデルの信頼度μk,a(+)（式(30)）、ゲイン行列算出器１２が出力するゲイン行列Ｋk（式(48)）、および目標観測装置１が出力する観測値Ｚkを読み込む。平滑器３は、それらの入力データと、予め設定されている定数加速度ベクトルαa（式(24)）とに基づいて、式(51)に従い、式(56)のＮ個の運動モデルによる平滑値Ｘk(+)を算出する。
【００９０】
また、平滑誤差評価器９においては、信頼度算出器２から出力される各運動モデルの信頼度μk,a(+)（式(30)）が読み込まれる。平滑誤差評価器９は、更に、運動モデルごとの予測誤差評価器１０が現時刻より１サンプリング前に算出した運動モデルごとの予測誤差共分散行列Ｐk,a(-)（式(57)）を遅延回路８を介して読み込み、ゲイン行列算出器１２から出力されるゲイン行列Ｋk（式(48)）を読み込む。平滑誤差評価器９は、それらの入力データと、あらかじめ設定されている定数加速度ベクトルαa（式(24)）とに基づいて、式(52)に従い、式(60)のＮ個の運動モデルによる平滑誤差共分散行列Ｐk(+)を算出する（ＳＴ８）。
【００９１】
次に、観測時刻算出器１５において、信頼度算出器２から出力される運動モデルごとの信頼度μk,a(+)（式(30)）、平滑器３から出力されるＮ個の運動モデルによる平滑値Ｘk(+)（式(56)）、平滑誤差評価器９から出力されるＮ個の運動モデルによる平滑誤差共分散行列Ｐk(+)（式(60)）が読み込まれると共に、次回観測を行う観測時刻ｔk+1が算出される。
【００９２】
また、運動モデル間推移確率算出器１６において、観測時刻算出器１５で算出される次回の観測時刻と、現観測時刻との差よりサンプリング間隔が算出される。運動モデル間推移確率算出器１６は、更に、算出されたサンプリング間隔に合わせた運動モデル間推移確率を、例えば式(29)により算出する（ＳＴ９）。
【００９３】
上述した一連の処理が終了すると、追尾処理の終了が要求されているか否かが判別される（ＳＴ１０）。その結果、処理の終了が要求されていないと判別される場合は、所定のサンプリング間隔で、上記ＳＴ３以降の処理が繰り返し実行される。
【００９４】
次に、図３に示すフローチャートを参照して、上述したＳＴ９（図２）における観測時刻算出器１５の動作について説明する。
図３は、上述したＳＴ９において実行される一例の処理の内容を示すフローチャートである。図３に示す処理においては、先ず、ＳＴ１１の処理が実行される。
【００９５】
ＳＴ１１では、サンプリング間隔Ｔの初期化が行われる。本ステップＳＴ１１の処理により、サンプリング間隔Ｔは、予め設定されている最小サンプリング間隔ｔsに設定される。
【００９６】
ＳＴ１２では、式(46)、(47)に従って、時刻ｔk＋Ｔ−ｔｓにおけるＮ個の運動モデルによる平滑値および平滑誤差共分散行列に基づいて、最小サンプリング間隔ｔsだけ先の時刻、すなわち、未来の時刻ｔk＋ＴにおけるＮ個の運動モデルによる予測値および予測誤差共分散行列が算出される。
【００９７】
ＳＴ１３では、ＳＴ１２で算出された時刻ｔk＋ＴにおけるＮ個のモデルによる予測値および予測誤差分散行列に基づいて、誤差楕円体が計算されると共に、その誤差楕円体の体積が、評価値として求められる。
【００９８】
上記の処理を具体的に説明する。ここで、時刻ｔk＋ＴにおけるＮ個のモデルによる予測値（式(46)）および予測誤差共分散行列（式(47)）を、それぞれＸk+T(-)、 Ｐk+T(-)と表す。この場合、上記の誤差楕円体は、次式(1)により求めることができる。
【００９９】
［Ｚk+T−Ｈ・Ｘk+T(-)］TＳk+T-1［Ｚk+T−Ｈ・Ｘk+T(-)］≦ｄ (1)
式(1)の左辺は自由度3のカイ2乗分布となる。ｄをパラメータとして式(1)の時刻ｔk+Tにおける観測値Ｚk+Tのなす領域は楕円体となる。この楕円体を誤差楕円体と呼ぶ。
ただし、上記(1)式中、Ｓk+Tは次式(2)で表される値であり、Ｈは式(40)の観測行列、Ｒk+Tは時刻ｔk+Tにおける式(23)の観測雑音共分散行列である。
Ｓk+T＝Ｈ・Ｐk+T(-)・ＨT＋Ｒk+T (2)
【０１００】
誤差楕円体の体積ＶGは次式(3)で計算される。本実施形態においては、その計算値ＶGが評価値として用いられる。尚、式(3)に示すΓ(・)はガンマ関数である。
【０１０１】
【数１３】
ＳＴ１４では、次式(4)に従って、観測誤差共分散行列Ｒk+Tに基づく観測誤差楕円体の体積が算出される。本実施形態においては、その値がしきい値Ｖthとして用いられる。
【０１０３】
【数１４】
尚、本実施形態において用いられるしきい値は、上記の値に限定されるものではなく、パラメータλ１を設定して、λ１・Ｖth をしきい値としてもよい。
【０１０５】
ＳＴ１９では、上述した評価値ＶGと、上述したしきい値Ｖthとを比較するしきい値判定が行われる。その結果、評価値ＶG＜しきい値Ｖthが成立すると判別されると、次にＳＴ２０の処理が実行される。一方、上記の条件が成立しないと判別される場合は、次にＳＴ２１の処理が実行される。
【０１０６】
ＳＴ２０では、サンプリング間隔Ｔがｔsだけ延長される、すなわちＴ＝Ｔ＋ｔsとする処理が実行される。以後、ＳＴ１９において、評価値ＶG＜しきい値Ｖthが成立しないと判定されるまで、繰り返し上記ＳＴ１２以降の処理が実行される。本ステップＳＴ２０では、また、ＳＴ１２で算出された時刻ｔk+TにおけるＮ個の運動モデルによる予測値および予測誤差共分散行列を、そのまま時刻ｔk+TにおけるＮ個の運動モデルによる平滑値および平滑誤差共分散行列とする処理が行われる。ＳＴ１２以降の処理が繰り返し実行される間、ＳＴ１２の処理は、このようにして前回の処理サイクルで設定された平滑値および平滑誤差共分散行列を用いて行われる。
【０１０７】
ＳＴ２１では、次の観測時刻ｔk+1が、時刻ｔk＋Ｔに、すなわちＳＴ１１〜ＳＴ２０の処理により設定されたサンプリング間隔Ｔだけ先の時刻に設定される。以後、このようにして設定された観測時刻ｔｋ＋Ｔを用いて、上記図２に示すルーチンが実行される。
【０１０８】
誤差楕円体の大きさは、その時刻での目標追尾精度を表している。つまり、追尾精度がよければ誤差楕円体は小さく、追尾精度が悪ければ誤差楕円体は大きくなる。上述の如く、本実施形態の目標追尾装置によれば、この原理を利用して、追尾精度がよいときはサンプリング間隔Ｔを長くし、追尾精度が悪いときは観測間隔を短くすることができる。
【０１０９】
また、図３に示すルーチンによれば、サンプリング間隔Ｔが延長される毎に、Ｎ個の運動モデルによる予測値および予測誤差共分散行列を逐次更新しながら、評価値およびしきい値を算出して、しきい値判定を行うことができる。すなわち、サンプリング間隔Ｔを、最小間隔ｔs単位で少しずつ延長しながらしきい値判定を行うことができる。このため、本実施形態の目標追尾装置によれば、未来の各時刻における目標追尾精度を予測しながら、最適なサンプリング間隔Ｔを算出することができ、所望の目標追尾精度を保つことができる。
【０１１０】
このように構成された実施の形態1の目標追尾装置によれば、目標追尾精度に合わせて観測間隔を適応的に制御できるので、機動力の高い目標でも等速直線運動を行う目標でも一定の水準の追尾精度を保つことができる。
【０１１１】
尚、上記の実施形態においては、観測時刻算出器１５が、上記ＳＴ１３、ＳＴ１４、およびＳＴ１９の処理を実行することで、前記請求項１記載の「追尾精度検出手段」が、また、上記ＳＴ１１、ＳＴ１２、およびＳＴ２０の処理を実行することで、前記請求項１記載の「観測時刻算出手段」が、それぞれ実現されている。
【０１１２】
また、上記の実施形態においては、観測時刻算出器１５が、上記ＳＴ２０の処理を行うことにより前記請求項４記載の「間隔再設定手段」が、ＳＴ１２の処理を行うことにより前記請求項４記載の「予測更新手段」が、ＳＴ１３の処理を行うことにより前記請求項４または５記載の「評価値算出手段」が、ＳＴ１４の処理を行うことにより請求項５記載の「しきい値設定手段」が、ＳＴ１９の処理を行うことにより前記請求項４または５記載の「しきい値判定手段」が、それぞれ実現されている。
【０１１３】
実施の形態２．
次に、図４および図５を参照して、本発明の実施の形態２について説明する。図４は本発明の目標追尾装置の実施の形態２を示すブロック図である。図４において、１〜１４、１６および１７は、実施の形態1の場合と同等の構成要素である。
【０１１４】
本実施形態の目標追尾装置は、センサ諸元入力器１８を備えている。センサ諸元入力器１８は、観測時刻算出器１５に対して目標追尾装置が備えるセンサの諸元を入力する。
【０１１５】
本実施形態において、観測時刻算出器１５は、Ｎ個の運動モデルによる信頼度、Ｎ個の運動モデルによる平滑値、およびＮ個の運動モデルによる平滑誤差に、センサ諸元を加えた入力データに基づいて次回のサンプリングを行う観測時刻を算出する。
【０１１６】
本実施形態の目標追尾装置の全体的な動作は、実施の形態１の装置と同様であり、図２の如く表される。以下、本実施形態の特徴部である観測時刻算出器１５の動作を、図５に示すフローチャートを参照して説明する。
観測時刻算出器１５は、図５に示す一連の処理を実行することで、サンプリング時刻ｔkにおける追尾精度とセンサ諸元とを考慮に入れて、次の観測時刻ｔk+1を算出する。ここでは、特に、極座標Ｅ-Ｂｙ面において算出される目標の誤差楕円の面積を評価値とし、かつ、センサの観測領域の面積から求めたしきい値を使ってしきい値判定を行う方法を示す。
【０１１７】
図５に示すフローチャートにおいて、ＳＴ１１、ＳＴ１２、およびＳＴ１９〜２１の処理は、実施の形態１の場合と同様である。
図５に示すＳＴ１５の評価値算出処理、およびＳＴ１６に示すしきい値算出処理においては、以下の理論に従って評価値およびしきい値が算出される。
【０１１８】
まず、時刻ｔk+Tにおいて、レーダより見た目標角度の真値を表すベクトルθk+Tを、式(5)のように定義する。
【０１１９】
【数１５】
時刻ｔk+Tにおける目標角度の予測ベクトル（「θチルダk+T」と称す）は、Ｎ個の運動モデルによる予測値Ｘk+T(-)を使用し、式(6)で与えられる。
【０１２１】
【数１６】
ただし、次式(7)の関係が成立するものとする。
【０１２３】
【数１７】
ここで、式(7)に示す[ａr ｂr ｃr]は、センサのxyz直交座標系における位置ベクトルであり、また、[x y z]は、目標予測値Ｘk+T(-)の位置成分である。
【０１２５】
上記(6)式で表される角度予測ベクトルθチルダk+Tの平均値をθk+Tmとすると、角度予測ベクトルの予測誤差“θチルダk+T -θk+Tm”の共分散行列は式(8)のように表せる。尚、式(8)に示すＥ［・］は平均を表す。
【０１２６】
【数１８】
ただし、Ｆk+Tは次式(9)とする。また、Ｐk+T(-)は、時刻ｔk+TにおけるＮ個の運動モデルによる予測誤差共分散行列である。
【０１２８】
【数１９】
次に、誤差楕円の面積を計算する。次式(10)に示す変量は、自由度2のカイ2乗分布に従う値である。式(10)の変量とパラメータｄ′とを用いると、条件式 (11)を生成することができる。式(11)を満たすθチルダk+Tの領域は、極座標系Ｅ-Ｂｙ面内において楕円を成す。
【０１３０】
【数２０】
上記誤差楕円の面積σGは、式(12)により算出される。本実施形態においては、その値σGが評価値として用いられる（ＳＴ１５）。
【０１３２】
【数２１】
一方、センサの観測領域の角度幅をφとするとき、観測領域のＥ-Ｂｙ面内における面積は、次式(13)のように定義することができる。本実施形態においては、式(13)に従って演算される値がしきい値σthとして用いられる（ＳＴ１６）。
【０１３４】
【数２２】
ここで、Ｅチルダk+Tは、時刻ｔk+Tにおける目標位置予測値の仰角成分である。尚、本実施形態においては、式(13)の演算値σthがしきい値として用いられているが、しきい値は、これに限定されるものではなく、パラメータλ２を設定して、λ２・σthをしきい値としてもよい。
【０１３６】
上述した方式では、センサ位置やセンサの観測領域を考慮にいれた追尾精度が評価される。このため、本実施形態の目標追尾装置によれば、センサからみた追尾精度がよいときはサンプリング間隔を長くし、その精度が悪いときはサンプリング間隔を短くすることができる。
【０１３７】
本発明の実施の形態２によれば、センサ諸元を入力データとして追加した観測時刻算出器１５を備えているので、センサと目標との位置関係やセンサの観測領域を考慮にいれた追尾精度が評価され、その評価結果に基づいて次のサンプリング時刻が算出される。このため、本実施形態の目標追尾装置は、機動力の高い目標でも、等速直線運動を行う目標でも、センサから見た追尾精度を一定の水準に保ったまま追尾を行うことができる。
【０１３８】
尚、上記の実施形態においては、観測領域の角度幅φ、およびセンサ位置などがセンサ諸元に相当する。
【０１３９】
また、上記の実施形態においては、観測時刻算出器１５が、上記ＳＴ１５の処理を行うことにより前記請求項６記載の「評価値設定手段」が、ＳＴ１６の処理を行うことにより前記請求項６記載の「しきい値設定手段」が、ＳＴ１９の処理を行うことにより前記請求項６記載の「しきい値判定手段」が、それぞれ実現されている。
【０１４０】
実施の形態３．
次に、図６を参照して、本発明の実施の形態３について説明する。実施の形態３の目標追尾装置の構成および全体の動きは実施の形態２と同等であり、それぞれ図４および図２で表される。
【０１４１】
図６は、実施の形態３における観測時刻算出器１５において実行される一連の処理のフローチャートを示す。ここでは、極座標Ｅ-Ｂｙ面における目標の誤差楕円の長軸径を評価値とし、かつ、センサの観測領域の幅(例えば、レーダにおけるビーム幅)から求めたしきい値を使ってしきい値判定を行う方法を示す。
【０１４２】
図６においてＳＴ１１、ＳＴ１２、およびＳＴ１９〜２１の処理は、実施の形態２の場合と同等である。
図６に示すＳＴ１７の評価値算出処理、およびＳＴ１８に示すしきい値算出処理においては、以下の方法に従って評価値およびしきい値が算出される。
【０１４３】
ＳＴ１７では、先ず、上記(8)式で表される行列の逆行列Ａk+T-1の固有値ｃ１およびｃ２ (ｃ１ ≦ ｃ２)が求められる。次に、次式(14)に従って、誤差楕円の長軸の直径φGが算出される。本実施形態においては、その直径φGが評価値として用いられる。
φG＝２√（ｄ′／ｃ１） (14)
【０１４４】
ＳＴ１８では、センサの観測領域の幅がしきい値φthとされる。センサの観測領域幅とは、例えばセンサとしてレーダを仮定すると、そのレーダのビーム幅を角度で表したものなどである。本実施形態においては、観測領域幅φthがしきい値として用いられているが、しきい値は、これに限定されるものではなく、パラメータλ３を設定して、λ３・φthをしきい値としてもよい。
【０１４５】
本発明の実施の形態３によれば、センサ諸元を入力として追加した観測時刻算出器１５を備えるので、実施の形態２と同様に、センサと目標との位置関係やセンサのビーム幅を考慮にいれた追尾精度を評価し、その結果に基づいてサンプリング間隔を算出することができる。このため、本実施形態の目標追尾装置によれば、機動力の高い目標でも、等速直線運動を行う目標でも、センサから見た追尾精度を一定の水準に保ったまま追尾することができる。
【０１４６】
尚、上記の実施形態においては、観測時刻算出器１５が、上記ＳＴ１７の処理を行うことにより前記請求項７記載の「評価値設定手段」が、ＳＴ１８の処理を行うことにより前記請求項７記載の「しきい値設定手段」が、ＳＴ１９の処理を行うことにより前記請求項７記載の「しきい値判定手段」が、それぞれ実現されている。
【０１４７】
【発明の効果】
この発明は以上説明したように構成されているので、以下に示すような効果を奏する。
請求項１記載の発明によれば、目標追尾精度に基づいてサンプリング間隔を適応的に決定することができるため、機動力が高く追尾精度が劣化しやすい目標の場合は観測間隔を短くし、等速直線運動を行う目標の場合は観測間隔を長くすることができる。これにより、如何なる目標に対しても追尾精度を一定の水準に保つことができる。
【０１４８】
請求項２記載の発明によれば、サンプリング間隔を、センサ諸元をも考慮して適応的に決定することができるため、如何なるセンサで追尾を行っても、そのセンサの性能に合わせたサンプリング間隔を決定することができる。従って、本発明の目標追尾装置によれば、センサの特性や目標の特性に関わらず、常に良好な追尾精度を得ることができる。
【０１４９】
請求項３記載の発明によれば、サンプリング間隔を、最小のサンプリング間隔単位で延長しながら、Ｎ個の運動モデルによる予測値および予測誤差を算出し、更に、それらが更新される毎に追尾精度の状態を判断することができる。そして、その判断に基づいて最適なサンプリング間隔を設定し、次回の観測時刻を最適な時刻に設定することができる。従って、本発明によれば、所望の追尾精度を安定に保持することができる。
【０１５０】
請求項４記載の発明によれば、Ｎ個の運動モデルによる予測誤差共分散行列より求めた誤差楕円体の体積を評価値とし、その評価値と、観測誤差共分散行列より求めたしきい値とを比較することでしきい値判定が行われる。追尾目標が如何なるものであっても、その誤差楕円体の体積には、その目標に対する追尾精度が反映される。従って、本発明によれば、追尾目標の特性に関わらず、目標追尾精度を安定に一定の水準に保つことができる。
【０１５１】
請求項５記載の発明によれば、Ｎ個の運動モデルによる予測誤差共分散行列より求めた誤差楕円体の極座標面での面積が評価値とされると共に、センサ諸元に含まれる極座標面内でのセンサの観測領域の面積に基づいてしきい値が設定される。このため、本発明によれば、センサの特性に応じたサンプリング間隔を設定することができ、センサの特性に関わらず、目標追尾精度を一定の水準に保つことができる。
【０１５２】
請求項６記載の発明によれば、Ｎ個の運動モデルによる予測誤差共分散行列より求めた誤差楕円体の極座標面での長軸直径が評価値とされると共に、センサ諸元に含まれる極座標面内でのセンサの観測領域の幅に基づいてしきい値が設定される。このため、本発明によれば、センサの特性に応じたサンプリング間隔を設定することができ、センサの特性に関わらず、目標追尾精度を一定の水準に保つことができる。
【図面の簡単な説明】
【図１】 本発明の実施の形態1に係わる目標追尾装置のブロック図である。
【図２】 本発明の実施の形態１、２または３に係わる目標追尾装置の動作を説明するためのフローチャートである。
【図３】 本発明の実施の形態１の観測時刻算出器の動作を説明するためのフローチャートである。
【図４】 本発明の実施の形態２または３に係わる目標追尾装置のブロック図である。
【図５】 本発明の実施の形態２の観測時刻算出器の動作を説明するフローチャートである。
【図６】 本発明の実施の形態３の観測時刻算出器の動作を説明するフローチャートである。
【図７】 従来の目標追尾装置のブロック図である。
【図８】 従来の目標追尾装置の動作を説明するためのフローチャートである。
【図９】 定数加速度のベクトルの一例である。
【符号の説明】
１ 目標観測装置、 ２ Ｎ個の運動モデルによる信頼度算出器、 ３ Ｎ個の運動モデルによる平滑器、 ４，７，８，１３，１４，１７ 遅延回路、 ５ Ｎ個の運動モデルによる予測器、 ６ 運動モデルごとの予測器、９ Ｎ個の運動モデルによる平滑誤差評価器、 １０ 運動モデルごとの予測誤差評価器、 １１ Ｎ個の運動モデルによる予測誤差評価器、 １２ゲイン行列算出器、 １５ 観測時刻算出器、 １６ 運動モデル間推移確率算出器、 １８ センサ諸元入力器。
Claims (6)
- 目標観測時刻と目標予測位置とをセンサに指示すると共に、センサによって観測される目標位置の観測情報を取得する目標観測装置を備える目標追尾装置であって、
目標追尾精度を検出する追尾精度検出手段と、
前記目標追尾精度に基づいて前記観測情報のサンプリング間隔を決定すると共に、そのサンプリング間隔を用いて、次回の観測時刻を算出する観測時刻算出手段とを備え、
前記追尾精度検出手段は、
目標位置観測情報、運動モデルごとの予測値、運動モデルごとの予測誤差、運動モデル間推移確率、および１サンプリング前のＮ個の運動モデルによる信頼度を基に、Ｎ個の運動モデルによる信頼度を算出するＮ個の運動モデルによる信頼度算出器と、
目標位置観測情報、Ｎ個の運動モデルによる信頼度、ゲイン行列、および１サンプリング前のＮ個の運動モデルによる平滑値を基に、Ｎ個の運動モデルによる平滑値を算出するＮ個の運動モデルによる平滑器と、
Ｎ個の運動モデルによる平滑値、Ｎ個の運動モデルによる信頼度、１サンプリング後の観測時刻、および運動モデル間推移確率を基に、Ｎ個の運動モデルによる予測値を算出するＮ個の運動モデルによる予測器と、
Ｎ個の運動モデルによる平滑値、および１サンプリング後の観測時刻を基に、運動モデルごとの予測値を算出する運動モデルごとの予測器と、
Ｎ個の運動モデルによる信頼度、運動モデルごとの予測誤差、およびゲイン行列を基に、Ｎ個の運動モデルによる平滑誤差を算出するＮ個の運動モデルによる平滑誤差評価器と、
Ｎ個の運動モデルによる平滑誤差および１サンプリング後の観測時刻を基に、運動モデルごとの予測誤差を算出する運動モデルごとの予測誤差評価器と、
運動モデルごとの予測誤差を基に、ゲイン行列を算出するゲイン行列算出器と、
運動モデルごとの予測誤差、Ｎ個の運動モデルによる信頼度、１サンプリング後の観測時刻および運動モデル間推移確率を基に、Ｎ個の運動モデルによる予測誤差を算出するＮ個の運動モデルによる予測誤差評価器と、
現在の観測時刻と次の観測時刻との間における運動モデル間の推移確率を算出する運動モデル間推移確率算出器と、を備え、
Ｎ個の運動モデルによる予測値と、Ｎ個の運動モデルによる予測誤差とに基づいて、目標追尾精度を求めることを特徴とする目標追尾装置。 - 前記追尾精度検出手段は、
センサの状態を表すセンサ諸元を読み込むセンサ諸元入力器を備えると共に、
Ｎ個の運動モデルによる予測値と、Ｎ個の運動モデルによる予測誤差と、センサ諸元とに基づいて、目標追尾精度を求めることを特徴とする請求項１記載の目標追尾装置。 - 前記観測時刻算出手段は、
サンプリング間隔を最小サンプリング間隔ごとに延長させる間隔再設定手段と、
サンプリング間隔が延長される毎に、Ｎ個の運動モデルによる予測値と、Ｎ個の運動モデルによる予測誤差とを更新する予測更新手段と、を備え、
前記目標追尾精度算出手段は、
前記Ｎ個の運動モデルによる予測値、および前記Ｎ個の運動モデルによる予測誤差が更新される毎に、目標追尾精度の評価値を算出する評価値算出手段と、
前記評価値と所定のしきい値とを比較するしきい値判定手段と、を備え、
前記観測時刻算出手段は、更に、前記しきい値判定手段によって前記評価値と前記しきい値の大小関係の逆転が判別された際のサンプリング間隔を、次のサンプリング間隔とすることを特徴とする請求項１または２記載の目標追尾装置。 - 前記追尾精度算出手段は、
Ｎ個の運動モデルによる予測誤差共分散行列に基づいて演算される誤差楕円体の体積を評価値とする評価値設定手段と、
観測誤差共分散行列に基づいて演算される誤差楕円体の体積をしきい値とするしきい値設定手段と、
前記評価値と前記しきい値とを比較することにより追尾精度の状態を判断するしきい値判定手段と、
を備えることを特徴とする請求項１記載の目標追尾装置。 - 前記センサ諸元は、極座標面におけるセンサの観測領域の面積を含み、
前記追尾精度算出手段は、
Ｎ個の運動モデルによる予測誤差共分散行列に基づいて演算される誤差楕円体の極座標面での面積を評価値とする評価値設定手段と、
前記センサ諸元に含まれる観測領域の面積に基づいてしきい値を設定するしきい値設定手段と、
前記評価値と前記しきい値とを比較することにより追尾精度の状態を判断するしきい値判定手段と、
を備えることを特徴とする請求項２記載の目標追尾装置。 - 前記センサ諸元は、極座標面におけるセンサの観測領域の幅を含み、
前記追尾精度算出手段は、
Ｎ個の運動モデルによる予測誤差共分散行列に基づいて演算される誤差楕円体の極座標面での長軸直径を評価値とする評価値設定手段と、
前記センサ諸元に含まれる観測領域の幅に基づいてしきい値を設定するしきい値設定手段と、
前記評価値と前記しきい値とを比較することにより追尾精度の状態を判断するしきい値判定手段と、
を備えることを特徴とする請求項２記載の目標追尾装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP03913899A JP3792927B2 (ja) | 1999-02-17 | 1999-02-17 | 目標追尾装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP03913899A JP3792927B2 (ja) | 1999-02-17 | 1999-02-17 | 目標追尾装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2000241539A JP2000241539A (ja) | 2000-09-08 |
| JP3792927B2 true JP3792927B2 (ja) | 2006-07-05 |
Family
ID=12544758
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP03913899A Expired - Fee Related JP3792927B2 (ja) | 1999-02-17 | 1999-02-17 | 目標追尾装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP3792927B2 (ja) |
Families Citing this family (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP4977492B2 (ja) | 2007-02-26 | 2012-07-18 | 日本電気株式会社 | 目標追尾装置及び目標追尾方法 |
| JP5183543B2 (ja) * | 2009-03-24 | 2013-04-17 | 三菱電機株式会社 | レーダ装置 |
| CN109446729B (zh) * | 2018-12-05 | 2023-03-28 | 明阳智慧能源集团股份公司 | 一种不依赖于模型的极短期风预测方法 |
| CN110363165B (zh) * | 2019-07-18 | 2023-04-14 | 深圳大学 | 基于tsk模糊***的多目标跟踪方法、装置及存储介质 |
-
1999
- 1999-02-17 JP JP03913899A patent/JP3792927B2/ja not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| JP2000241539A (ja) | 2000-09-08 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| WO2019114807A1 (zh) | 多传感器目标信息融合 | |
| JP4348535B2 (ja) | 目標追尾装置 | |
| JP4116898B2 (ja) | 目標追尾装置 | |
| JP3792927B2 (ja) | 目標追尾装置 | |
| JP4966794B2 (ja) | 水中航走体誘導方法及び装置 | |
| JP5230132B2 (ja) | 目標追尾装置及び目標追尾方法 | |
| WO2024120187A1 (zh) | 一种用于信息拒止环境下的无人机动态目标估计方法 | |
| JP3330852B2 (ja) | 目標追尾方法及び装置 | |
| JP4882544B2 (ja) | 追尾処理装置及びその方法並びにプログラム | |
| CN114894222B (zh) | Imu-gnss天线的外参数标定方法和相关方法、设备 | |
| JP4166651B2 (ja) | 目標追尾装置 | |
| CN110007298B (zh) | 一种目标超前预测跟踪方法 | |
| JP2004219300A (ja) | 目標追尾装置 | |
| CN108230371B (zh) | 基于云台的跟踪目标速度预测方法及存储介质 | |
| JP3926602B2 (ja) | 目標追尾装置及び方法 | |
| Crocoll et al. | Laser-aided navigation with loop closure capabilities for Micro Aerial Vehicles in indoor and urban environments | |
| JP4281473B2 (ja) | 目標追尾装置 | |
| JP3323415B2 (ja) | 目標追尾装置及びその目標追尾方法 | |
| CN111123323A (zh) | 一种提高便携设备定位精度的方法 | |
| JPH0797136B2 (ja) | 多目標追尾方法及びその装置 | |
| JP2005308428A (ja) | 位置推定装置 | |
| JP3440010B2 (ja) | 目標追尾装置 | |
| JPH09113610A (ja) | 目標追尾レーダ装置 | |
| WO2023157157A1 (ja) | 目標追尾装置及び目標追尾方法 | |
| JP2006153736A (ja) | 目標追尾装置 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20040601 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20051003 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20051220 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20060126 |
|
| RD03 | Notification of appointment of power of attorney |
Free format text: JAPANESE INTERMEDIATE CODE: A7423 Effective date: 20060126 |
|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20060404 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20060406 |
|
| R150 | Certificate of patent or registration of utility model |
Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20100414 Year of fee payment: 4 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20100414 Year of fee payment: 4 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20110414 Year of fee payment: 5 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120414 Year of fee payment: 6 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120414 Year of fee payment: 6 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20130414 Year of fee payment: 7 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20130414 Year of fee payment: 7 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20140414 Year of fee payment: 8 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| LAPS | Cancellation because of no payment of annual fees |