JP4977492B2 - 目標追尾装置及び目標追尾方法 - Google Patents

Description

本発明は、航空機等の目標の位置をレーダ等のセンサで観測して得られる観測値から、その目標の位置及び速度の諸元を推定する目標追尾方法および目標追尾装置に関する。

従来から種々の目標追尾装置が提案されている。例えば、特許文献１（特開２０００−２４１５３９号公報）は、目標を観測する間隔（サンプリング間隔）を制御することで、機動力の高い目標に対する追尾精度の向上を図った「目標追尾装置」を開示している。特許文献１に開示された目標追尾装置は、Ｎ個の運動モデルによる信頼度算出器と、Ｎ個の運動モデルによる平滑器と、Ｎ個の運動モデルによる予測器と、運動モデルごとの予測器と、Ｎ個の運動モデルによる平滑誤差評価器と、運動モデルごとの予測誤差評価器と、Ｎ個の運動モデルによる予測誤差評価器と、ゲイン行列算出器と、観測時刻算出器と、運動モデル間遷移確率算出器とを備えている。Ｎ個の運動モデルによる信頼度算出器は、目標位置観測情報、運動モデルごとの予測値、運動モデルごとの予測誤差、および１サンプリング前の各運動モデルの信頼度を基に、Ｎ個の運動モデルによる信頼度を算出する。Ｎ個の運動モデルによる平滑器は、目標位置観測情報、Ｎ個の運動モデルによる信頼度、ゲイン行列、および１サンプリング前のＮ個の運動モデルによる平滑値を基に、Ｎ個の運動モデルによる平滑値を算出する。Ｎ個の運動モデルによる予測器は、Ｎ個の運動モデルによる平滑値、Ｎ個の運動モデルによる信頼度、および１サンプリング前の観測時刻に基づいて、Ｎ個の運動モデルによる予測値を算出する。運動モデルごとの予測器は、Ｎ個の運動モデルによる平滑値、および１サンプリング後の観測時刻に基づいて、運動モデルごとの予測値を算出する。Ｎ個の運動モデルによる平滑誤差評価器は、Ｎ個の運動モデルによる信頼度、運動モデルごとの予測誤差、およびゲイン行列に基づいて、Ｎ個の運動モデルによる平滑誤差を算出する。運動モデルごとの予測誤差評価器は、Ｎ個の運動モデルによる平滑誤差、および１サンプリング後の観測時刻に基づいて、運動モデルごとの予測誤差を算出する。Ｎ個の運動モデルによる予測誤差評価器は、運動モデルごとの予測誤差、Ｎ個の運動モデルによる信頼度、および１サンプリング後の観測時刻に基づいて、Ｎ個の運動モデルによる予測誤差を算出する。ゲイン行列算出器は、運動モデルごとの予測誤差を基に、ゲイン行列を算出する。観測時刻算出器は、Ｎ個の運動モデルによる信頼度、Ｎ個の運動モデルによる平滑値、およびＮ個の運動モデルによる平滑誤差に基づいて、次回のサンプリング（目標観測）が行われる時刻（観測時刻）を算出する。運動モデル間推移確率算出器は、観測時刻算出器の出力する観測時刻に基づいて、現在の観測時刻と次回の観測時刻との差からサンプリング間隔を算出し、このサンプリング間隔における運動モデル間の推移確率を算出する。

とにかく、特許文献１は、目標諸元予測値と目標諸元誤差予測値とを算出することを開示している。また、特許文献１では、目標追尾精度に基づき観測情報のサンプリング間隔を決定することを開示している。

また、特許文献２（特開２００２−１７４６８１号公報）は、フィルタ平滑効果をコントロール可能な「目標追尾装置」を開示している。この特許文献２の図３に開示された目標追尾装置は、平滑ベクトル用メモリと、擬似平滑誤差共分散行列用メモリと、予測ベクトル算出部と、駆動雑音共分散行列設定部と、擬似予測誤差共分散行列算出部と、観測雑音共分散行列設定部と、擬似残差共分散行列算出部と、ゲート判定部と、ゲイン行列算出部と、平滑ベクトル算出部と、係数制御部と、係数乗算部と、擬似平滑誤差共分散行列算出部と、平滑誤差共分散行列用メモリと、予測誤差共分散行列算出部と、残差共分散行列算出部と、平滑誤差共分散行列算出部とを備えている。

平滑ベクトル用メモリは、平滑ベクトル算出部による平滑ベクトルの計算結果を格納する。平滑誤差共分散行列用メモリは、平滑誤差共分散行列算出部による平滑誤差共分散行列の計算結果を格納する。予測ベクトル算出部は、平滑ベクトル用メモリに格納された前時刻の平滑ベクトルから現時刻の予測ベクトルを算出する。予測誤差共分散行列算出部は、平滑誤差共分散行列用メモリに格納された前時刻の平滑誤差共分散行列の計算結果と、駆動雑音共分散行列設定部により設定された駆動雑音共分散行列の値とから、現時刻の予測誤差共分散行列を算出する。観測雑音共分散行列設定部は、目標観測装置１から入力された観測ベクトルの精度評価に関連したパラメータに基づいて、観測雑音共分散行列を設定する。残差共分散行列算出部は、予測誤差共分散行列算出部により算出された予測誤差共分散行列と、観測雑音共分散行列設定部により設定された観測雑音共分散行列とから、残差共分散行列を算出する。ゲート判定部は、予測ベクトル算出部により算出された予測ベクトルと、目標観測装置からの観測ベクトルとに基づいて、残差ベクトルを算出するとともに、残差共分散行列算出部により算出された残差共分散行列を用いて、観測ベクトルが追尾目標の探知データであるか否かのゲート判定を行う。擬似平滑誤差共分散行列用メモリは、擬似平滑誤差共分散行列算出部による擬似平滑誤差共分散行列の計算結果を格納する。擬似予測誤差共分散行列算出部は、前時刻の擬似平滑誤差共分散行列の計算結果と、駆動雑音共分散行列設定部からの駆動雑音共分散行列の値とを用いて、現時刻の擬似予測誤差共分散行列を算出する。擬似残差共分散行列算出部は、擬似予測誤差共分散行列算出部からの擬似予測誤差共分散行列と、観測雑音共分散行列設定部からの観測雑音共分散行列とを用いて、擬似残差共分散行列を算出する。ゲイン行列算出部は、擬似残差共分散行列算出部からの擬似予測誤差共分散行列と、擬似予測誤差共分散行列計算部からの擬似残差共分散行列とを用いて、フィルタのゲイン行列を算出する。係数制御部は、観測雑音共分散行列に乗算されるスカラ係数の値を追尾目標の追尾状態に応じて決定し、係数乗算部は、上記スカラ係数を観測雑音共分散行列に乗算する。擬似平滑誤差共分散行列算出部は、上記スカラ係数が乗算された観測雑音共分散行列と、擬似予測誤差共分散行列算出部からの擬似予測誤差共分散行列とを用いて、擬似平滑誤差共分散行列を算出する。

とにかく、特許文献２は、1)前時刻の平滑ベクトル（諸元）から予測ベクトル（諸元）を算出すること、2)前時刻の誤差共分散から予測誤差共分散を算出すること、3)観測雑音に、目標の機動（直進、曲進）に基づくスカラ係数を乗算して擬似予測誤差共分散（新目標諸元誤差予測値）を算出すること、4)擬似予測誤差共分散からゲインを算出すること、予測ベクトルとゲインより平滑ベクトル（推定目標）を算出すること、および5)擬似予測誤差共分散、観測雑音、ゲインから平滑誤差共分散（目標諸元誤差推定値）を算出することを開示している。

特許文献３（特開２００３−１３０９４７号公報）は、誤差楕円体の重複度判定によりフィルタのモデルでは目標の運動を表現できないと判定された場合、信頼度計算における残差共分散行列を再計算することにより、所望の誤差楕円体重複度を実現し、追尾性能の向上を図ることができる「目標追尾装置及び方法」を開示している。特許文献３において、予測器は、平滑器からの平滑値と、各運動モデル信頼度、定数加速度ベクトルを基に、予測値及び予測誤差共分散行列を算出する。また、特許文献３では、誤差楕円体を各軸方向について任意の大きさに制御して、等速直線運動目標に対して誤差を低減しつつ、旋回目標に対しても追尾精度を向上させている。

特許文献４（特開平７−３５８５０号公報）は、目標追尾を行う際に必要とする計算機の負荷の増大を防ぎ、短いサンプリング間隔のままで精度の高い追尾処理を実行すると共に、送信ビーム諸元の算出時間を確保できる「目標追尾装置」を開示している。特許文献４では、フィルタゲイン定数および観測値をもとに位置と速度の平滑値を算出する平滑値算出装置と、上記平滑値から次のサンプリングタイムである予測時刻における位置と速度の予測値を算出する予測値算出装置とを開示している。また、特許文献４では、次サンプリング時刻における予測ベクトルを算出する予測ベクトル算出装置と、予測誤差共分散行列を算出する予測誤差共分散算出装置とを開示している。

以降の説明の数式中において、ベクトル及び行列の数式中にてベクトル及び行列の肩に付した記号Ｔは、そのベクトル及び行列の転置を示す。さらに、数式中で使用する記号において、記号の上に〜が付された要素（ベクトル及び行列）は予測値を示し、記号の上に＾が付された要素（ベクトル及び行列）は推定値を示す。一方、明細書中で使用する記号に対しては、記号の前に〜が付された要素（ベクトル及び行列）は予測値を示し、記号の前に＾が付された要素（ベクトル及び行列）は推定値を示す。

また、図４に、以降のブロック図の中で使用する要素の記号を示している。図４（１）に示す要素１は、時点ｋにＡ_ｋを入力し、その一時点前に入力したＡ_ｋ−１を遅延結果Ｙとして出力する遅延要素である。図４（２）に示す要素２は、ＡとＢとを入力し、（Ａ＋Ｂ）を加算結果Ｙとして出力する加算器要素である。図４（３）に示す要素３は、ＡとＢとを入力し、（Ａ−Ｂ）を減算結果Ｙとして出力する減算器要素である。

図５に第１の従来の目標追尾装置１Ｃの構成を示す。先ず、第１の従来の目標追尾装置１Ｃとその外部の装置との接続関係、並びに第１の従来の目標追尾装置１Ｃの内部構成について説明する。

第１の従来の目標追尾装置１Ｃには、２つの外部の装置として、観測値入力部２と追尾結果出力部３とが接続されている。目標追尾装置１Ｃは、予測諸元算出部１０と、推定諸元算出部２０と、第１の遅延要素３０と、予測諸元誤差算出部４０と、推定諸元誤差算出部５０と、ゲイン値算出部６０と、第２の遅延要素７０とから構成されている。

次に、第１の従来の目標追尾装置１Ｃとその外部の装置との間の入出力データとその処理内容について説明する。

目標追尾装置１Ｃは、観測値入力部２から、観測結果のデータとして、時点ｋで時刻ｔ_ｋ、観測値ｚ_ｋ、及び観測値誤差Ｗ_ｋを入力する。ただし、ｋは１〜ｎの自然数であり、時点は１、２、・・・の順に進むとする。これら入力に基づき、目標追尾装置１Ｃは、目標推定諸元（目標諸元推定値）＾Ｘ_ｋおよび目標推定諸元誤差（目標諸元誤差推定値）＾Ｐ_ｋを算出し、これらを追尾結果出力部３に出力する。

なお、ここで、時刻ｔ_ｋは例えば任意の起算時刻からの経過時間である。観測値ｚ_ｋは、例えば、ＸＹ直交座標系での目標の位置をＸ座標値、Ｙ座標値を要素とする、下記数１のように表記されるベクトルである。

観測値誤差Ｗ_ｋは、下記数２のように定義される観測値ｚ_ｋの誤差共分散行列である。

目標推定諸元＾Ｘ_ｋは、例えば目標の位置及び速度の推定値を表すＸ座標値、Ｙ座標値、Ｘ座標速度値、Ｙ座標速度値を要素とする、下記数３のように表記されるベクトルである。

目標推定諸元誤差＾Ｐ_ｋは、下記数４のように定義される目標推定諸元＾Ｘ_ｋの誤差共分散行列である。

最後に、第１の従来の目標追尾装置１Ｃの内部の各部分間のデータの関係及びその処理内容について説明する。

予測諸元算出部１０は、観測値入力部２から時刻ｔ_ｋを入力し、同時に推定諸元算出部２０から第１の遅延要素３０を介して１時点前の目標諸元推定値＾Ｘ_ｋ−１を入力して、入力に対応する時点（現時点）での目標諸元予測値〜Ｘ_ｋを算出して出力するものである。

予測諸元算出部１０は、状態遷移行列作成部１１と状態遷移部１２とから構成されている。状態遷移行列作成部１１は、時刻ｔ_ｋに基づき、下記の数５によって状態遷移行列Φ_ｋを作成する。

状態遷移部１２は、１時点前の目標諸元推定値＾Ｘ_ｋ−１と状態遷移行列Φ_ｋとに基づき、下記の数６によって現時点の目標諸元予測値〜Ｘ_ｋを算出する。

また、予測諸元誤差算出部４０は、観測値入力部２から時刻ｔ_ｋを入力し、同時に推定諸元誤差算出部５０から第２の遅延要素７０を介して１時点前の目標諸元誤差推定値＾Ｐ_ｋ−１を入力し、入力に対応する時点（現時点）での目標諸元誤差予測値〜Ｐ_ｋを算出して出力するものである。

予測諸元誤差算出部４０は、状態遷移行列作成部４１と、状態遷移分散誤差算出部４２と、変動誤差伝播部４３と、加算器４４とから構成される。状態遷移行列作成部４１は、時刻ｔ_ｋに基づき、上記数５によって状態遷移行列Φ_ｋを作成する。

状態遷移分散誤差算出部４２は、１時点前の目標諸元誤差推定値＾Ｐ_ｋ−１と状態遷移行列Φ_ｋとに基づき、下記の数７によって目標諸元の遷移に対応する目標諸元誤差推定値φ（＾Ｐ_ｋ−１）を算出する。

変動誤差伝播部４３は、時刻ｔ_ｋと変動誤差想定値σ_ｕに基づき変動誤差による目標諸元誤差の増加分Ｕを、下記の数８によって算出する。

加算器４４は、下記の数９によって、目標諸元誤差推定値φ（＾Ｐ_ｋ−１）に目標諸元誤差の増加分Ｕを加えて現時点の目標諸元誤差予測値〜Ｐ_ｋを算出する。

なお、この目標追尾装置１Ｃでは、初回の目標諸元推定値＾Ｘ_ｋ−１と目標諸元誤差推定値＾Ｐ_ｋ−１とは既に求まっているとして動作が始まる構成である。すなわち、初回の目標諸元推定値＾Ｘ_ｋ−１及び目標諸元誤差推定値＾Ｐ_ｋ−１の算出は、別途にこの目標追尾装置１Ｃの外で算出されているものとする。

ゲイン値算出部６０は、観測値誤差Ｗ_ｋと目標諸元誤差予測値〜Ｐ_ｋとを入力して、目標諸元予測値〜Ｘ_ｋに対して観測値ｚ_ｋを適用して目標推定諸元を推定する際の観測値ｚ_ｋの適用割合、つまりゲイン値Ｋ_ｋを算出するものである。この算出は、下記の数１０による。

推定諸元算出部２０は、現時点の目標諸元予測値〜Ｘ_ｋ、観測値ｚ_ｋ、及びゲイン値Ｋ_ｋを入力して、入力に対応する時点（現時点）での目標諸元推定値＾Ｘ_ｋを算出して出力するものである。

推定諸元算出部２０は、残差推定部２１と、ゲイン適用部２２と、加算器２３とから構成されている。残差推定部２１は、現時点の目標諸元予測値〜Ｘ_ｋと観測値ｚ_ｋとに基づき、下記の数１１により残差Δを算出する。

ゲイン適用部２２は、残差Δとゲイン値Ｋ_ｋとに基づき、下記の数１２により残差にゲインを適用した分Δ_ｋを算出する。

加算器２３は、下記の数１３によって、現時点の目標諸元予測値〜Ｘ_ｋにΔ_ｋを加えて、現時点の目標推定諸元（目標諸元推定値）＾Ｘ_ｋを算出する。

推定諸元誤差算出部５０は、現時点の目標諸元誤差予測値〜Ｐ_ｋ、観測値誤差Ｗ_ｋ、及びゲイン値Ｋ_ｋを入力して、入力に対応する時点（現時点）での目標諸元誤差推定値＾Ｐ_ｋを算出して出力するものである。

推定諸元誤差算出部５０は、予測諸元誤差分ゲイン適用部５１と、観測誤差分ゲイン適用部５２と、加算器５３とから構成される。予測諸元誤差分ゲイン適用部５１は、現時点の目標諸元誤差の予測値〜Ｐ_ｋとゲイン値Ｋ_ｋとに基づき、下記の数１４により、予測諸元誤差にゲインを適用した分Ｐを算出する。

観測誤差分ゲイン適用部５２は、観測値誤差Ｗ_ｋとゲイン値Ｋ_ｋとに基づき、下記の数１５により、観測誤差にゲインを適用した分Ｗを算出する。

加算器５３は、下記の数１６によって、ＰにＷを加えて、現時点の目標諸元誤差推定値＾Ｐ_ｋを算出する。

図５に示した第１の従来の目標追尾装置１Ｃは、レーダ等のセンサが検知した観測データを使用した航空機の目標追尾装置に広く使用され、等速直進する目標だけでなく、旋回や加減速する目標にも適用できる。

ただし、予測諸元誤差算出部４０で使用する変動誤差想定値σ_ｕの設定値によって追尾誤差の特性が変わる。

図６を参照して、この追尾誤差の特性が変わる様子について詳細に説明する。目標が等速直進している場合には、変動誤差想定値σ_ｕが小さいほど、センサの観測データに含まれる観測誤差の影響を防いで追尾誤差が小さくなる。これに対して、旋回等の機動を行っている場合には、変動誤差想定値σ_ｕが小さいほど、機動に対する追従の遅れが大きくなる。

目標追尾装置の性能として、等速直進する目標に対する追尾誤差と、旋回等の機動時の追従の遅れとについて特段の要望がない場合には、図５に示した第１の従来の目標追尾装置１Ｃの構成で何等不都合はない。一方、これらに特定の要望がある場合には、要望の性能を満たすような変動誤差想定値σ_ｕを探して設定する必要がある。しかしながら、場合によっては、これら２つの要望の性能を同時に満たす変動誤差想定値σ_ｕが存在しない場合がある。このような場合には、図５に示した目標追尾装置１Ｃでは対応できない。

図７に、図５に示した第１の従来の目標追尾装置１Ｃに工夫を加えた、第２の従来の目標追尾装置１Ｄを示す。第２の従来の目標追尾装置１Ｄは、予測諸元誤差算出部４０と推定諸元誤差算出部５０との間に新たに機動検知時点予測誤差調整部８０Ａを追加した点を除いて、図５に示した第１の従来の目標追尾装置１Ｃと同様の構成を有する。

機動検知時点予測誤差調整部８０Ａは、目標が旋回等の機動を行っていることを検知した場合に、その時にのみ１回動作させるものである。機動検知時点予測誤差調整部８０Ａは加算器８１から構成される。加算器８１は、予測諸元誤差算出部４０が算出した現時点の目標諸元誤差予測値〜Ｐ_ｋに予め設定された予測誤差調整値Ｖを加えて、下記の数１７により、新たな目標諸元誤差予測値〜Ｐ_ｋを算出する。

なお、目標が機動していることを検知する部位は、図７に示す目標追尾装置１Ｄには特に含まれておらず、目標追尾装置１Ｄの外の何らかの別の手段によって検知されるものとする。

次に、図７に示す第２の従来の目標追尾装置１Ｄの効果について説明する。目標追尾装置１Ｄでは、現時点の目標諸元誤差予測値〜Ｐ_ｋが予測誤差調整値Ｖの分だけ大きくなることにより、以降のゲイン値算出部６０において、図５に示した第１の従来の目標追尾装置１Ｃの時に比較して、大き目のゲイン値Ｋ_ｋが算出される。その結果として、推定諸元算出部２０で残差に大き目のゲインが適用され、図５に示した第１の従来の目標追尾装置１Ｃに比較して、観測値ｚ_ｋに近い位置に現時点の目標諸元推定値＾Ｘ_ｋを算出するので、追従遅れをより小さくすることができるという効果がある。この効果の様子を図８に示す。

なお、予測誤差調整値Ｖは、予測諸元誤差を増加させる誤差共分散行列であり、その要素は、下記の数１８に示す誤差共分散行列としての値の条件を満たす必要がある。

また、図７に示した目標追尾装置１Ｄでは、予測誤差調整値Ｖは予め設定するものであったが、同等のものを動的に作る形で機動検知時点予測誤差調整部を構成することも出来る。

図９に、そのように構成した、第３の従来の目標追尾装置１Ｅを示す。すなわち、図９に示す目標追尾装置１Ｅは、機動検知時点予測誤差調整部が後述するように変更されている点を除いて、図７に示した第２の従来の目標追尾装置１Ｄと同様の構成を有する。従って、機動検知時点予測誤差調整部に８０Ｂの参照符号を付してある。

機動検知時点予測誤差調整部８０Ｂは、加算器８１と機動誤差形成部８２とから構成されている。機動誤差形成部８２は、予め設定された機動誤差想定値σ^ｒ _ｕおよび機動検知時用機動誤差伝播時間ΔＴとを入力し、下記の数１９により、予測誤差調整値Ｖを算出する。

このような構成の目標追尾装置１Ｅは、図７に示した目標追尾装置１Ｄと同様の効果を得ることができる。

ところで、図７および図９に図示した第２及び第３の従来の目標追尾装置１Ｄおよび１Ｅは、等速直進する目標に対する追尾誤差を小さく保ちつつ、同時に等速旋回等の機動時の追従の遅れを、図５に図示した第１の従来の目標追尾装置１Ｄに比較して小さくすることができる。

特開２０００−２４１５３９号公報（図１） 特開２００２−１７４６８１号公報（図３） 特開２００３−１３０９４７号公報 特開平０７−０３５８５０号公報

上述した特許文献１〜４のいずれも、図７および図９に図示した目標追尾装置１Ｄおよび１Ｅに開示されているような、機動検知時点予測誤差調整部については開示せず、示唆する記載もない。

次に、図７および図９に図示した第２及び第３の従来の目標追尾装置１Ｄおよび１Ｅの課題について説明する。

図１０を参照して、等速旋回の追尾状況における機動の検知時の速度の誤差の特性について説明する。等速旋回の機動は、速さ（速度の絶対値）は一定で針路（速度の方向）が変化する機動である。等速旋回の機動の検知時には、推定目標諸元の速度において、速さは誤差が小さく針路のみ追従遅れが出ている状況である。そのため、図７および図９に図示した第２及び第３の従来の目標追尾装置１Ｄおよび１Ｅでは、機動検知時に針路の遅れの追従に合わせて速さの誤差が大きくなるという課題がある。

また同様に、直進加減速の機動は、針路（速度の方向）は一定で速さ（速度の絶対値）が変化する機動である。直進加減速の機動の検知時には、推定目標諸尾元の速度において、針路は誤差が小さく速度のみ追従遅れが出ている状況である。そのため、図７および図９に図示した第２及び第３の従来の目標追尾装置１Ｄおよび１Ｅでは、機動検知時に速さの遅れの追従に合わせて針路の誤差が大きくなるという課題がある。

したがって、本発明の課題は、機動を検知時にその機動が等速旋回である場合には、旋回方向には針路の追従の遅れを早く回復させ、針路方向には速さの誤差を小さいままに保ち、機動を検知時にその機動が直進加減速である場合には、針路方向には速さの追従遅れを早く回復させ、旋回方向には針路の誤差を小さいままに保つことができる、目標追尾装置を提供することにある。

本発明に想到するまでの着眼点について、以下に説明する。換言すれば、図７および図９に示した第２及び第３の従来の目標追尾装置１Ｄおよび１Ｅにおける上記課題の原因について考察する。

図７および図９に示した第２及び第３の従来の目標追尾装置１Ｄおよび１Ｅにおいて、機動検知時に追従遅れを、図５に示した第１の従来の目標追尾装置１Ｃに比較して小さくできたのは、ゲイン値算出部６０と推定諸元算出部２０における算出が、現時点の目標諸元誤差予測値〜Ｐ_ｋと観測誤差値Ｗ_ｋの関係で定まることを利用しているからである。

図１１を参照してその理由について説明する。図１１は、現時点の目標諸元誤差予測値〜Ｐ_ｋと観測誤差値Ｗ_ｋの大小を図上での範囲の大小で表す。すなわち、現時点の目標諸元誤差予測値〜Ｐ_ｋが観測誤差値Ｗ_ｋに比較して大きいほど、観測値ｚ_ｋを重視して現時点の目標諸元推定値＾Ｘ_ｋを算出し、逆に現時点の目標諸元誤差予測値〜Ｐ_ｋが観測誤差値Ｗ_ｋに比較して小さいほど、現時点の目標諸元予測値〜Ｘ_ｋを重視して現時点の目標諸元推定値＾Ｘ_ｋを算出する関係にある。

図１１に示した誤差の範囲は一般に楕円となり、その大きさは、現時点の目標諸元誤差予測値〜Ｐ_ｋと観測誤差値Ｗ_ｋの要素の値で定まる。例えば、それぞれの１行１列要素、１行２列要素、２行１列要素、２行２列要素を、それぞれ、σ_１１、σ_１２、σ_２１、σ_２２とする。この場合、ＸＹ直交座標での楕円のＸ方向の範囲の寸法とＹ方向の範囲の寸法の比が√σ_１１：√σ_２２となり、楕円の傾きがσ_１２を含む各要素の関係で定まる。

したがって、図７および図９に示した第２及び第３の従来の目標追尾装置１Ｄおよび１Ｅでは、機動検知時点予測誤差調整部８０Ａおよび８０Ｂにより、現時点の目標諸元誤差予測値〜Ｐ_ｋを予測誤差調整値Ｖの分だけ大きくしていたので、その分だけ観測値ｚ_ｋを重視して現時点の目標諸元推定値＾Ｘ_ｋを算出することにより追従遅れを小さくできた。ところで、図１１に示したように、第２及び第３の従来の目標追尾装置１Ｄおよび１Ｅの機動検知時点予測誤差調整部８０Ａおよび８０Ｂが形成する予測誤差調整値Ｖに対応する範囲の形状は円形である。すなわち、目標の針路に関わらない等方向性のものである。

そこで、本発明では、上記の課題を解決するために、図７および図９に示した第２及び第３の従来の目標追尾装置１Ｄおよび１Ｅの機動検知時点予測誤差調整部８０Ａおよび８０Ｂが形成する予測誤差調整値Ｖが目標の針路に関わらない等方向性のものであったのを、次のように改めるようにしている。

すなわち、目標の機動を検知時にその機動が等速旋回である場合には、目標が機動した方向である旋回方向、つまり針路に直交する方向に大きく、目標の針路方向に小さい予測誤差調整値を形成する。これにより、目標の旋回方向には針路の追従遅れを早く回復させ、目標の針路方向には速さの誤差を小さいままに保つようにしている。一方、目標の機動を検知時にその機動が直進加減速である場合には、目標が機動した方向である針路方向に大きく、目標の旋回方向に小さい予測誤差調整値を形成する。これにより、目標の針路方向には速さの追従遅れを早く回復させ、目標の旋回方向には針路の誤差を小さいままに保つようにしている。

本発明の第１の態様によれば、目標の観測値及び観測値誤差と前記観測値の時刻とを入力して、現時点の目標諸元推定値と現時点の目標諸元誤差推定値とを出力する目標追尾装置であって、前記観測値の時刻と前回時点の目標諸元推定値とを入力し、前記観測値の時刻に対応する現時点の目標諸元予測値を算出する予測緒元算出部と、前記観測値の時刻と前回時点の目標諸元誤差推定値とを入力し、前記観測値の時刻に対応する現時点の目標諸元誤差予測値を算出する予測諸元誤差算出部と、前記観測値誤差と前記現時点の目標諸元誤差予測値とを入力して、ゲイン値を算出するゲイン算出部と、前記現時点の目標諸元予測値と前記観測値と前記ゲイン値とを入力して、前記現時点の目標諸元推定値を算出する推定諸元算出部と、前記現時点の目標諸元誤差予測値と前記観測値誤差と前記ゲイン値とを入力して、前記現時点の目標諸元誤差推定値を算出する推定諸元誤差算出部と、前記予測諸元誤差算出部と前記推定諸元誤差算出部及び前記ゲイン値算出部との間に設けられて、前記目標が機動したことを検知した時点にのみ動作する機動検知時点予測誤差調整部であって、前記現時点の目標諸元誤差予測値を入力して、前記現時点の目標諸元予測値に基づいて、機動誤差を前記現時点の目標諸元予測値に含まれる予測速度の針路方向に回転させた予測誤差調整値を求め、前記現時点の目標諸元誤差予測値の代わりに、前記予測誤差調整値を前記現時点の目標諸元誤差予測値に加算したものを新たな目標諸元誤差予測値として、前記推定諸元誤差算出部及び前記ゲイン値算出部へ供給する、前記機動検知時点予測誤差調整部と、から成る目標追尾装置が得られる。

上記目標追尾装置において、前記機動検知時点予測誤差調整部は、前記現時点の目標諸元予測値に基づいて、予め用意された機動誤差を前記現時点の目標諸元予測値に含まれる予測速度の針路方向に回転させて、前記予測誤差調整値を求める機動誤差針路方向回転部と、前記予測誤差調整値を前記現時点の目標諸元誤差予測値に加算して、前記新たな目標諸元誤差予測値を出力する加算器と、から構成されてよい。その代わりに、前記機動検知時点予測誤差調整部は、予め用意された機動誤差旋回方向想定値と機動誤差針路方向想定値と機動検知時用機動誤差伝播時間とを入力して機動誤差を算出し、算出した機動誤差を出力する機動誤差形成部と、前記現時点の目標諸元予測値に基づいて、前記算出した機動誤差を前記現時点の目標諸元予測値に含まれる予測速度の針路方向に回転させて、前記予測誤差調整値を求める機動誤差針路方向回転部と、前記予測誤差調整値を前記現時点の目標諸元誤差予測値に加算して、前記新たな目標諸元誤差予測値を出力する加算器と、から構成されてよい。或いは、前記機動検知時点予測誤差調整部は、予め用意された機動誤差旋回方向想定値と機動誤差針路方向想定値と前記現時点の目標諸元予測値とを入力して、前記目標諸元予測値に含まれる予測速度の針路方向に向いた機動誤差要素を算出する機動誤差要素針路方向回転部と、前記機動誤差要素と機動検知時用機動誤差伝播時間とを入力して、前記予測誤差調整値を算出する機動誤差形成部と、前記予測誤差調整値を前記現時点の目標諸元誤差予測値に加算して、前記新たな目標諸元誤差予測値を出力する加算器と、から構成されてよい。

本発明の第２の態様によれば、目標の位置を観測して得られる観測値から、前記目標の位置及び速度の諸元を推定する目標追尾方法であって、前記観測値の時刻と前回時点の目標諸元推定値とを受けて、前記観測値の時刻に対応する現時点の目標諸元予測値を算出するステップと、前記観測値の時刻と前回時点の目標諸元誤差推定値とを受けて、前記観測値の時刻に対応する現時点の目標諸元誤差予測値を算出するステップと、前記観測値誤差と前記現時点の目標諸元誤差予測値とを入力して、ゲイン値を算出するステップと、前記現時点の目標諸元予測値と前記観測値と前記ゲイン値とを入力して、現時点の目標諸元推定値を算出するステップと、前記現時点の目標諸元誤差予測値と前記観測値誤差と前記ゲイン値とを入力して、現時点の目標諸元誤差推定値を算出するステップと、前記目標が機動したことを検知した時点にのみ動作する機動検知時点予測誤差調整ステップとを含み、前記機動検知時点予測誤差調整ステップは、前記現時点の目標諸元誤差予測値を入力して、前記現時点の目標諸元予測値に基づいて、機動誤差を前記現時点の目標諸元予測値に含まれる予測速度の針路方向に回転させた予測誤差調整値を求め、前記予測誤差調整値を前記現時点の目標諸元誤差予測値に加算したものを新たな目標諸元誤差予測値として出力する、ことを特徴とする目標追尾方法が得られる。

上記目標追尾方法において、前記機動検知時点予測誤差調整ステップは、前記現時点の目標諸元予測値に基づいて、予め用意された機動誤差を前記現時点の目標諸元予測値に含まれる予測速度の針路方向に回転させて、前記予測誤差調整値を求めるステップと、前記予測誤差調整値を前記現時点の目標諸元誤差予測値に加算して、前記新たな目標諸元誤差予測値を出力するステップと、から構成されてよい。その代わりに、前記機動検知時点予測誤差調整ステップは、予め用意された機動誤差旋回方向想定値と機動誤差針路方向想定値と機動検知時用機動誤差伝播時間とを入力して機動誤差を算出するステップと、前記現時点の目標諸元予測値に基づいて、前記算出した機動誤差を前記現時点の目標諸元予測値に含まれる予測速度の針路方向に回転させて、前記予測誤差調整値を求めるステップと、前記予測誤差調整値を前記現時点の目標諸元誤差予測値に加算して、前記新たな目標諸元誤差予測値を出力するステップと、から構成されてよい。或いは、前記機動検知時点予測誤差調整ステップは、予め用意された機動誤差旋回方向想定値と機動誤差針路方向想定値と前記現時点の目標諸元予測値とを入力して、前記目標諸元予測値に含まれる予測速度の針路方向に向いた機動誤差要素を算出するステップと、前記機動誤差要素と機動検知時用機動誤差伝播時間とを入力して、前記予測誤差調整値を算出するステップと、前記予測誤差調整値を前記現時点の目標諸元誤差予測値に加算して、前記新たな目標諸元誤差予測値を出力するステップと、から構成されてよい。

本発明による目標追従装置は、機動を検知時にその機動が等速旋回である場合には、旋回方向には針路の追従の遅れを早く回復させ、針路方向には速さの誤差を小さいままに保ち、また機動を検知時にその機動が直進加減速である場合には、針路方向には速さの追従遅れを早く回復させ、旋回方向には針路の誤差を小さいままに保つことを可能にすることができる。

以下、本発明の実施の形態について図面を参照して詳細に説明する。

図１を参照して、本発明の第１の実施の形態に係る目標追尾装置１について説明する。図示の目標追尾装置１は、機動検知時点予測誤差調整部の構成が図７および図９に図示したものと相違している点を除いて、図７および図９に図示した目標追尾装置１Ｄおよび１Ｅと同様の構成を有する。したがって、機動検知時点予測誤差調整部に９０の参照符号を付してある。図７および図９に示したものと同様の機能を有するものには同一の参照符号を付して、説明の簡略化のためにそれらの説明については省略する。

機動検知時点予測誤差調整部９０は、目標が旋回等の機動を行っていることを検知した場合に、その時にのみ１回動作させるものである。機動検知時点予測誤差調整部９０は、加算器９１と機動誤差針路方向回転部９２とから構成されている。機動誤差針路方向回転部９２は、予め設定された機動誤差Ｕ^ｒ _ｎｐと現時点の目標諸元予測値〜Ｘ_ｋとを入力して、機動誤差Ｕ^ｒ _ｎｐの表す誤差の範囲の縦軸方向を現時点の目標諸元予測値〜Ｘ_ｋの針路方向に合わせるように機動誤差Ｕ^ｒ _ｎｐを回転させた予測誤差調整値〜Ｖ_ｋを下記の数２０により算出する。

この予測誤差調整値〜Ｖ_ｋを加算器９１で、予測誤差諸元誤差算出部４０が算出した現時点の目標諸元誤差予測値〜Ｐ_ｋに加えて、下記の数２１により新たな目標諸元誤差予測値〜Ｐ_ｋを算出する。

なお、目標が機動していることを検知する部位は、本発明の第１の実施の形態に特に含まれておらず、目標追尾装置１の外の何等かの別の手段によって検知されているものとする。また、機動誤差Ｕ^ｒ _ｎｐは予測諸元誤差を増加させる誤差共分散行列であり、その要素は下記の数２２に示す誤差共分散行列としての値の条件を満たす必要がある。

また、機動誤差Ｕ^ｒ _ｎｐは、さらに、機動の種類に応じて、等速旋回時にはＵ_１１＞Ｕ_２２、Ｕ_３３＞Ｕ_４４、Ｕ_１２＞Ｕ_２４、Ｕ_３１＞Ｕ_４２であることが適切であり、直進加減速時にはＵ_１１＜Ｕ_２２、Ｕ_３３＜Ｕ_４４、Ｕ_１２＜Ｕ_２４、Ｕ_３１＜Ｕ_４２であることが適切である。

また、図１に示した目標追尾装置１においては、機動誤差Ｕ^ｒ _ｎｐは、予め設定されているが、同等のものを動的に作る形で機動検知時点予測誤差調整部を構成することも出来る。

図２は、そのように構成した、本発明の第２の実施の形態に係る目標追尾装置１Ａを示すブロック図である。機動検知時点予測誤差調整部に９０Ａの参照符号を付してある。

機動検知時点予測誤差調整部９０Ａは、加算器９１および機動誤差針路方向回転部９２の他に機動誤差形成部９３をも備えている。機動誤差形成部９３は、予め設定された機動誤差旋回方向想定値σ^ｒ _ｕｎ、機動誤差針路方向想定値σ^ｒ _ｕｐ、及び機動検知時用機動誤差伝播時間ΔＴを入力し、下記の数２３により機動誤差Ｕ^ｒ _ｎｐを算出する。

このような構成の目標追尾装置１Ａでは、図１に図示した目標追尾装置１において機動誤差Ｕ^ｒ _ｎｐの値を予め用意する際に、上述したような、その要素の条件を満たすことを留意する手間を省くことができる。そして、目標追尾装置１Ａは図１に図示した目標追尾装置１と同じ効果を得ることができる。

図２に示した目標追尾装置１Ａでは、機動誤差Ｕ^ｒ _ｎｐを算出した後に、これを目標の針路に基づいて回転させているが、予め設定された機動誤差旋回方向想定値σ^ｒ _ｕｎ、機動誤差針路方向想定値σ^ｒ _ｕｐを目標の針路に基づいて回転させた後に、これを機動検知時用機動誤差伝播時間ΔＴにより伝播させても、図２に示した目標追尾装置１Ａの機動検知時点予測誤差調整部９０Ａと同じ予測誤差調整値〜Ｖ_ｋを算出することができる。

図３は、このように機動検知時点予測誤差調整部を構成した、本発明の第３の実施の形態に係る目標追尾装置１Ｂを示すブロック図である。機動検知時点予測誤差調整部に９０Ｂの参照符号を付してある。

機動検知時点予測誤差調整部９０Ｂは、加算器９１と、機動誤差要素針路方向回転部９４と、機動誤差形成部９５とから構成されている。機動誤差要素針路方向回転部９４は、機動誤差旋回方向想定値σ^ｒ _ｕｎ、機動誤差針路方向想定値σ^ｒ _ｕｐ、及び現時点の目標諸元予測値〜Ｘ_ｋを入力して、機動誤差旋回方向想定値σ^ｒ _ｕｎと機動誤差針路方向想定値σ^ｒ _ｕｐとにより構成する誤差の範囲の縦軸方向を現時点の目標諸元予測値〜Ｘ_ｋの針路に合わせるように回転させた機動誤差値（機動誤差要素）Σ^ｒ _ｘｙを下記の数２４により算出する。

この機動誤差値（機動誤差要素）Σ^ｒ _ｘｙと機動検知時用機動誤差伝播時間ΔＴとを機動誤差形成部９５に入力して、下記の数２５により予測誤差調整値〜Ｖ_ｋを算出する。

このような構成の目標追尾装置１Ｂは、図２に示した目標追尾装置１Ａと同じ効果を得ることができる。

以上、本発明について好ましい実施の形態について説明してきたが、本発明は上述した実施の形態に限定されず、本発明の趣旨（主題）を逸脱しない範囲内で種々の変形・変更が可能なのは勿論である。

本発明の第１の実施の形態による目標追尾装置を示すブロック図である。 本発明の第２の実施の形態による目標追尾装置を示すブロック図である。 本発明の第３の実施の形態による目標追尾装置を示すブロック図である。 ブロック図に使用する要素の説明図である。 第１の従来の目標追尾装置を示すブロック図である。 変動誤差想定値と追尾誤差との間の関係を説明するための図である。 第２の従来の目標追尾装置を示すブロック図である。 図７に示した第２の従来の目標追尾装置による機動検知時点予測誤差調整の効果を説明するための図である。 第３の従来の目標追尾装置を示すブロック図である。 従来の目標追尾装置による機動検知時点予測誤差調整の課題を説明するための図である。 目標諸元推定における目標諸元誤差予測値と観測誤差値との間の関係を説明するための図である。

符号の説明

１、１Ａ、１Ｂ 目標追尾装置
２ 観測値入力部
３ 追尾結果出力部
１０ 予測諸元算出部
１１ 状態遷移行列作成部
１２ 状態遷移部
２０ 推定諸元算出部
２１ 残差算出部
２２ ゲイン適用部
２３ 加算器
３０ 遅延要素
４０ 予測諸元誤差算出部
４１ 状態遷移行列作成部
４２ 状態遷移分誤差算出部
４３ 変動誤差伝播部
４４ 加算器
５０ 推定諸元誤差算出部
５１ 予測諸元誤差分ゲイン適用部
５２ 観測誤差分ゲイン適用部
５３ 加算器
７０ 遅延要素
９０、９０Ａ、９０Ｂ 機動検知時点予測誤差調整部
９１ 加算器
９２ 機動誤差針路方向回転部
９３ 機動誤差形成部
９４ 機動誤差要素針路方向回転部
９５ 機動誤差形成部

Claims (8)

1. 目標の観測値及び観測値誤差と前記観測値の時刻とを入力して、現時点の目標諸元推定値と現時点の目標諸元誤差推定値とを出力する目標追尾装置であって、
   前記観測値の時刻と前回時点の目標諸元推定値とを入力し、前記観測値の時刻に対応する現時点の目標諸元予測値を算出する予測緒元算出部と、
   前記観測値の時刻と前回時点の目標諸元誤差推定値とを入力し、前記観測値の時刻に対応する現時点の目標諸元誤差予測値を算出する予測諸元誤差算出部と、
   前記観測値誤差と前記現時点の目標諸元誤差予測値とを入力して、ゲイン値を算出するゲイン算出部と、
   前記現時点の目標諸元予測値と前記観測値と前記ゲイン値とを入力して、前記現時点の目標諸元推定値を算出する推定諸元算出部と、
   前記現時点の目標諸元誤差予測値と前記観測値誤差と前記ゲイン値とを入力して、前記現時点の目標諸元誤差推定値を算出する推定諸元誤差算出部と、
   前記予測諸元誤差算出部と前記推定諸元誤差算出部及び前記ゲイン値算出部との間に設けられて、前記目標が機動したことを検知した時点にのみ動作する機動検知時点予測誤差調整部であって、前記現時点の目標諸元誤差予測値を入力して、前記現時点の目標諸元予測値に基づいて、機動誤差を前記現時点の目標諸元予測値に含まれる予測速度の針路方向に回転させた予測誤差調整値を求め、前記現時点の目標諸元誤差予測値の代わりに、前記予測誤差調整値を前記現時点の目標諸元誤差予測値に加算したものを新たな目標諸元誤差予測値として、前記推定諸元誤差算出部及び前記ゲイン値算出部へ供給する、前記機動検知時点予測誤差調整部と、
   から成る目標追尾装置。
2. 前記機動検知時点予測誤差調整部は、
   前記現時点の目標諸元予測値に基づいて、予め用意された機動誤差を前記現時点の目標諸元予測値に含まれる予測速度の針路方向に回転させて、前記予測誤差調整値を求める機動誤差針路方向回転部と、
   前記予測誤差調整値を前記現時点の目標諸元誤差予測値に加算して、前記新たな目標諸元誤差予測値を出力する加算器と、
   から構成される、請求項１に記載の目標追尾装置。
3. 前記機動検知時点予測誤差調整部は、
   予め用意された機動誤差旋回方向想定値と機動誤差針路方向想定値と機動検知時用機動誤差伝播時間とを入力して機動誤差を算出し、算出した機動誤差を出力する機動誤差形成部と、
   前記現時点の目標諸元予測値に基づいて、前記算出した機動誤差を前記現時点の目標諸元予測値に含まれる予測速度の針路方向に回転させて、前記予測誤差調整値を求める機動誤差針路方向回転部と、
   前記予測誤差調整値を前記現時点の目標諸元誤差予測値に加算して、前記新たな目標諸元誤差予測値を出力する加算器と、
   から構成される、請求項１に記載の目標追尾装置。
4. 前記機動検知時点予測誤差調整部は、
   予め用意された機動誤差旋回方向想定値と機動誤差針路方向想定値と前記現時点の目標諸元予測値とを入力して、前記目標諸元予測値に含まれる予測速度の針路方向に向いた機動誤差要素を算出する機動誤差要素針路方向回転部と、
   前記機動誤差要素と機動検知時用機動誤差伝播時間とを入力して、前記予測誤差調整値を算出する機動誤差形成部と、
   前記予測誤差調整値を前記現時点の目標諸元誤差予測値に加算して、前記新たな目標諸元誤差予測値を出力する加算器と、
   から構成される、請求項１に記載の目標追尾装置。
5. 目標の位置を観測して得られる観測値から、前記目標の位置及び速度の諸元を推定する目標追尾方法であって、
   前記観測値の時刻と前回時点の目標諸元推定値とを受けて、前記観測値の時刻に対応する現時点の目標諸元予測値を算出するステップと、
   前記観測値の時刻と前回時点の目標諸元誤差推定値とを受けて、前記観測値の時刻に対応する現時点の目標諸元誤差予測値を算出するステップと、
   前記観測値誤差と前記現時点の目標諸元誤差予測値とを入力して、ゲイン値を算出するステップと、
   前記現時点の目標諸元予測値と前記観測値と前記ゲイン値とを入力して、現時点の目標諸元推定値を算出するステップと、
   前記現時点の目標諸元誤差予測値と前記観測値誤差と前記ゲイン値とを入力して、現時点の目標諸元誤差推定値を算出するステップと、
   前記目標が機動したことを検知した時点にのみ動作する機動検知時点予測誤差調整ステップとを含み、
   前記機動検知時点予測誤差調整ステップは、前記現時点の目標諸元誤差予測値を入力して、前記現時点の目標諸元予測値に基づいて、機動誤差を前記現時点の目標諸元予測値に含まれる予測速度の針路方向に回転させた予測誤差調整値を求め、前記予測誤差調整値を前記現時点の目標諸元誤差予測値に加算したものを新たな目標諸元誤差予測値として出力する、ことを特徴とする目標追尾方法。
6. 前記機動検知時点予測誤差調整ステップは、
   前記現時点の目標諸元予測値に基づいて、予め用意された機動誤差を前記現時点の目標諸元予測値に含まれる予測速度の針路方向に回転させて、前記予測誤差調整値を求めるステップと、
   前記予測誤差調整値を前記現時点の目標諸元誤差予測値に加算して、前記新たな目標諸元誤差予測値を出力するステップと、
   を有する、請求項５に記載の目標追尾方法。
7. 前記機動検知時点予測誤差調整ステップは、
   予め用意された機動誤差旋回方向想定値と機動誤差針路方向想定値と機動検知時用機動誤差伝播時間とを入力して機動誤差を算出するステップと、
   前記現時点の目標諸元予測値に基づいて、前記算出した機動誤差を前記現時点の目標諸元予測値に含まれる予測速度の針路方向に回転させて、前記予測誤差調整値を求めるステップと、
   前記予測誤差調整値を前記現時点の目標諸元誤差予測値に加算して、前記新たな目標諸元誤差予測値を出力するステップと、
   を有する、請求項５に記載の目標追尾方法。
8. 前記機動検知時点予測誤差調整ステップは、
   予め用意された機動誤差旋回方向想定値と機動誤差針路方向想定値と前記現時点の目標諸元予測値とを入力して、前記目標諸元予測値に含まれる予測速度の針路方向に向いた機動誤差要素を算出するステップと、
   前記機動誤差要素と機動検知時用機動誤差伝播時間とを入力して、前記予測誤差調整値を算出するステップと、
   前記予測誤差調整値を前記現時点の目標諸元誤差予測値に加算して、前記新たな目標諸元誤差予測値を出力するステップと、
   を有する、請求項５に記載の目標追尾方法。

Images