JP2007086039A - 目標体運動解析方法及びその装置 - Google Patents

Abstract

【課題】 受波センサ位置の誤差によって生じる目標体状態量の推定値における誤差を抑制する。
【解決手段】 本発明は、目標体から放射される信号を、目標体と相対的に運動する観測体に取り付けた、空間的に離れた３個以上の受波センサで受信し、各受波センサ間の信号到来時間差の測定結果である観測時間差時系列を得、観測時間差時系列に対して所定演算を実行して、目標体に関する状態量を推定する。そして、観測時間差時系列に基づき、目標体の位置及び速度と共に、受波センサ位置をも評価対象としたセンサ位置含有評価関数を構成し、この評価関数を最小化する目標体の位置及び速度、並びに、受波センサ位置を探索して、これらパラメータの推定値を得ると共に、受波センサ位置の推定値で、評価関数の構成に適用する受波センサ位置を補正することを特徴とする。
【選択図】 図１

Description

本発明は目標体運動解析方法及び装置に関し、例えば、目標体から発信された信号（音響）を、水中観測体又は海上観測体などでなる観測体に取り付けた３個以上の受波センサで受信し、各受波センサ間の信号の到来時間差から、発信源（目標体）の方位、距離、針路、速力などの状態量を推定する方法及び装置に適用し得るものである。

従来の目標体運動解析方法として、非特許文献１に記載のものがある。図２は、非特許文献１に記載の従来の目標体運動解析方法における観測系及び運動系を示す幾何学的説明図である。

図２において、（Ｘ，Ｙ，Ｚ）は原点ｏの固定座標系、１は観測体、２は目標体、Ｃ１〜Ｃ３はそれぞれ、観測体１に取り付けられた受波センサである。Ｘ_Ｏ（ｔ）は、時刻ｔにおける観測体１の基準位置の位置ベクトル（以下、観測体位置ベクトルと略記する）、Ｓ_１（ｔ）、Ｓ_２（ｔ）、Ｓ_３（ｔ）はそれぞれ、時刻ｔにおける受波センサＣ１、Ｃ２、Ｃ３の位置ベクトル、Ｘ_Ｔ（ｔ）は、時刻ｔにおける目標体２の位置ベクトルである。また、（ｘ_ｏ，ｙ_ｏ，ｚ_ｏ）は観測体１に固定され、観測体１の基準位置を原点とする座標系（以下、観測体固定座標系と略記する）である。観測体固定座標系において、受波センサＣ１、Ｃ２、Ｃ３のそれぞれの位置ベクトルをΔＳ_１、ΔＳ_２、ΔＳ_３とする。

従来の目標体運動解析方法は、目標体２から発信された信号を３個以上の受波センサ（Ｃ１〜Ｃ３）で受信し、雑音に乱された各受波センサ間の信号の観測時間差時系列から、目標体２が等速直線運動を行っていると仮定し、目標体２の位置、速度などの状態量を推定する。

Ｘ_Ｔ（ｔ_ｒ）を任意の基準時刻ｔ_ｒにおける目標体位置ベクトル、Ｖ_Ｔを等速直線運動する目標体２の速度ベクトルとすると、時刻ｔ_ｋにおける目標体位置ベクトルＸ_Ｔ（ｔ_ｋ）は、（１）式に示すように、Ｘ_Ｔ（ｔ_ｒ）、Ｖ_Ｔ、ｔ_ｋ及びｔ_ｒの関数となる。時刻ｔ_ｋにおける観測体位置ベクトルは既知であり、Ｘ_Ｏ（ｔ_ｋ）とする。観測体固定座標系は、観測体１の針路の変更に伴って回転する。時刻ｔ_ｋにおいて、固定座標系（Ｘ，Ｙ，Ｚ）に対する観測体固定座標系（ｘ_ｏ，ｙ_ｏ，ｚ_ｏ）の回転行列をＣ（ｔ_ｋ）とする。このとき、時刻ｔ_ｋにおける受波センサＣｉ（ｉは１〜３のいずれか）の位置ベクトルＳ_ｉ（ｔ_ｋ）は、(２)式で表すことができる。よって、時刻ｔ_ｋにおける受波センサＣｉと目標体２間の距離Ｒ_ｉ（ｔ_ｋ）は、（３）式に示すようになる。なお、（３）式における‖ ‖はベクトルのノルムを表している。従って、信号の伝搬速度をｃとしたとき、目標体２から発信された信号を、受波センサＣｉと受波センサＣｊで受波したとき、両受波センサ間の信号の到来時間差τ_ｉ，ｊ（ｔ_ｋ）は、（４）式に示すようになる。受波センサがＮ個の場合を考え、時刻ｔ_ｋにおける独立な時間差ベクトルτ（ｔ_ｋ）を（５）式で定義する。なお、（５）式における中括弧の右肩に付与されているような括弧の右肩に付与されているＴは、ベクトル、行列の転置を表している。

しかしながら、実際に観測される受波センサ間の到来時間差（以下、観測時間差と略記する）のベクトルτ_ｍ（ｔ_ｋ）は、周囲の雑音等により誤差を持ち、（６）式に示すようになる。（６）式において、Ｗ（ｔ_ｋ）は時間的に無相関で、平均０、共分散行列Σ（ｔ_ｋ）のガウス雑音ベクトルとする。

時刻ｔ_１〜ｔ_Ｋにおいて得られた観測時間差ベクトルτ_ｍ（ｔ_１）〜τ_ｍ（ｔ_Ｋ）に対する目標体評価関数Ｊ（Ｘ_Ｔ（ｔ_ｒ），Ｖ_Ｔ）を（７）式で表す。なお、（７）式の行列に対する右肩に付与している−１は付与されていない行列の逆行列であることを表している。目標体評価関数Ｊ（Ｘ_Ｔ（ｔ_ｒ），Ｖ_Ｔ）を最小にするようなＸ_Ｔ（ｔ_ｒ）及びＶ_Ｔを、基準時刻ｔ_ｒにおける目標体位置ベクトルの推定値＾Ｘ_Ｔ（ｔ_ｒ）及び目標体の速度ベクトルの推定値＾Ｖ_Ｔとする。

D. Van Cappel, "Target Motion Analysis using Time Delays Measured from a nonlinear Array", in Proc. of Int. Conf. on Acoust., Speech, Signal Processing, pp.2724-2727, 1989.

従来の目標体運動解析方法では、観測体に取り付けられた受波センサの位置の微小な誤差が推定値の大きな誤差になるため、事前にセンサ位置を正確に測定するなどの方法がとられている。

しかしながら、受波センサ位置の測定誤差を完全に０に押さえ込むことは難しく、また、周囲の環境等の変化で受波センサ位置が測定位置からずれることがある。

本発明は、以上の点を考慮してなされたものであり、受波センサ位置の誤差によって生じる目標体位置及び速度の推定値の誤差を抑制する、優れた目標体運動解析方法及び装置を提供しようとしたものである。

かかる課題を解決するため、第１の本発明は、目標体から放射される信号を、上記目標体と相対的に運動可能な観測体に取り付けた、空間的に離れた３個以上の受波センサで受信し、上記各受波センサ間の信号到来時間差の測定結果である観測時間差時系列を得、観測時間差時系列に対して所定演算を実行して、上記目標体に関する状態量を推定する目標体運動解析方法において、（１）上記観測時間差時系列に基づき、上記目標体の位置及び速度と共に、上記受波センサ位置をも評価対象としたセンサ位置含有評価関数を構成した後、そのセンサ位置含有評価関数を最小化する上記目標体の位置及び速度、並びに、受波センサ位置を探索して、これらパラメータの推定値を得る状態量・センサ位置推定工程と、（２）上記受波センサ位置の推定値で、上記評価関数の構成に適用する受波センサ位置を補正するセンサ位置補正工程とを含むことを特徴とする。

第２の本発明は、目標体から放射される信号を、上記目標体と相対的に運動可能な観測体に取り付けた、空間的に離れた３個以上の受波センサで受信し、上記各受波センサ間の信号到来時間差の測定結果である観測時間差時系列を得、観測時間差時系列に対して所定演算を実行して、上記目標体に関する状態量を推定する目標体運動解析方法において、（１）複数の目標体の上記各観測時間差時系列に基づき、上記目標体の位置及び速度と共に、上記受波センサ位置をも評価対象としたセンサ位置含有目標体統合評価関数を構成した後、そのセンサ位置含有目標体統合評価関数を最小化する上記各目標体の位置及び速度、並びに、受波センサ位置を探索して、これらパラメータの推定値を得る目標体別状態量・センサ位置推定工程と、（２）上記受波センサ位置の推定値で、上記評価関数の構成に適用する受波センサ位置を補正するセンサ位置補正工程とを含むことを特徴とする。

第３の本発明は、目標体から放射される信号を、上記目標体と相対的に運動可能な観測体に取り付けた、空間的に離れた３個以上の受波センサで受信し、上記各受波センサ間の信号到来時間差の測定結果である観測時間差時系列を得、観測時間差時系列に対して所定演算を実行して、上記目標体に関する状態量を推定する目標体運動解析装置において、（１）上記観測時間差時系列に基づき、上記目標体の位置及び速度と共に、上記受波センサ位置をも評価対象としたセンサ位置含有評価関数を構成した後、そのセンサ位置含有評価関数を最小化する上記目標体の位置及び速度、並びに、受波センサ位置を探索して、これらパラメータの推定値を得る状態量・センサ位置推定手段と、（２）上記受波センサ位置の推定値で、上記評価関数の構成に適用する受波センサ位置を補正するセンサ位置補正手段と有することを特徴とする。

第４の本発明は、目標体から放射される信号を、上記目標体と相対的に運動可能な観測体に取り付けた、空間的に離れた３個以上の受波センサで受信し、上記各受波センサ間の信号到来時間差の測定結果である観測時間差時系列を得、観測時間差時系列に対して所定演算を実行して、上記目標体に関する状態量を推定する目標体運動解析装置において、（１）複数の目標体の上記各観測時間差時系列に基づき、上記目標体の位置及び速度と共に、上記受波センサ位置をも評価対象としたセンサ位置含有目標体統合評価関数を構成した後、そのセンサ位置含有目標体統合評価関数を最小化する上記各目標体の位置及び速度、並びに、受波センサ位置を探索して、これらパラメータの推定値を得る目標体別状態量・センサ位置推定手段と、（２）上記受波センサ位置の推定値で、上記評価関数の構成に適用する受波センサ位置を補正するセンサ位置補正手段とを有することを特徴とする。

本発明の目標体運動解析方法及び装置によれば、目標体の位置及び速度と共に、受波センサ位置をも評価対象とした評価関数を構成し、目標体の位置及び速度、受波センサ位置の推定値を得ると共に、受波センサ位置の推定値に基づいて、評価関数を構成する際の受波センサ位置の情報を見直すようにしたので、受波センサ位置の誤差によって生じる目標体位置及び速度の推定値の誤差を抑制することができる。

（Ａ）第１の実施形態
以下、本発明による目標体運動解析方法及び装置の第１の実施形態を説明する。

（Ａ−１）第１の実施形態の目標体運動解析の原理
第１の実施形態の具体的な説明に先立って、図２を参照しながら、第１の実施形態における目標体運動解析方法の原理を説明する。

第１の実施形態及び後述する実施形態は、従来における目標体位置及び速度の推定値に大きな誤差が生じる問題を解決するため、観測時間差に基づいて目標体位置と速度を推定すると共に、受波センサ位置も同時に推定し、この推定した受波センサ位置で受波センサ位置を補正するものである。

従来の目標体運動解析方法では、観測体１に取り付けられた各受波センサＣｉの観測体固定座標系での位置ベクトルΔＳ_ｉ（ｉ＝１,…,Ｎ）には誤差がないとして、時刻ｔ_ｋにおける受波センサｉの位置ベクトルを、上記（２）式を用いて算出している。ここで、受波センサＣｉ_ｍにε_ｍの誤差があり、観測体固定座標系での実際の位置ベクトルが（ΔＳ_ｉ＋ε_ｍ）とすると、時刻ｔ_ｋにおける受波センサＣｉ_ｍの実際の位置ベクトルＳ^εｍ _ｉｍ（ｔ_ｋ）は、（８）式で表される。よって、時刻ｔ_ｋにおける受波センサＣｉ_ｍと目標体２との間の距離Ｒ^εｍ _ｉｍ（ｔ_ｋ）は、（９）式のようになる。

ここで、全ての受波センサ位置に誤差があるとすると、受波センサ位置の推定ができない。そこで、Ｎ個の受波センサ位置のうち、２個には誤差がないと仮定する。以下では、受波センサＣ１と受波センサＣＮに誤差がないとして説明する。そうすると、目標体２から発信された信号を、受波センサＣ１と受波センサＣ２で受波したとき、これら受波センサ間の信号の到来時間差τ^ε _１，２（ｔ_ｋ）は、（１０）式で表され、受波センサＣｉ（但し、２＜ｉ＜Ｎ−２）と受波センサＣｊ（但し、３＜ｊ＜Ｎ−１，ｊ≠ｉ）で受波したとき、これら受信センサ間の信号の到来時間差τ^ε _ｉ，ｊ（ｔ_ｋ）は、（１１）で表され、受波センサＣ（Ｎ−１）と受波センサＣＮで受波したとき、これら受波センサ間の信号の到来時間差τ^ε _{Ｎ−１，Ｎ}（ｔ_ｋ）は、（１２）式で表される。

時刻ｔ_ｋにおける独立な時間差ベクトルτ^ε（ｔ_ｋ）を（１３）式で定義する。ここで、実際に観測される受波センサ間の観測時間差τ_ｍ（ｔ_ｋ）は、（１４）式のように、周囲の雑音等により、本来の時間差ベクトルτ^ε（ｔ_ｋ）に対して誤差Ｗ（ｔ_ｋ）を持っている。そこで、時刻ｔ_１、…、ｔ_Ｋにおいて得られた観測時間差τ_ｍ（ｔ_１）、τ_ｍ（ｔ_２）、…、τ_ｍ（ｔ_Ｋ）に対して、（１６）式に示す受波センサ位置誤差ベクトルＥを利用した（１５）式に示す目標体・センサ位置評価関数Ｊ（Ｘ_Ｔ（ｔ_ｒ），Ｖ_Ｔ，Ｅ）を、最小にするようなＸ_Ｔ（ｔ_ｒ）、Ｖ_Ｔ及びＥをそれぞれ、基準時刻ｔ_ｒにおける目標体位置ベクトルの推定値＾Ｘ_Ｔ（ｔ_ｒ）、目標体速度ベクトルの推定値＾Ｖ_Ｔ及び受波センサ位置誤差の推定値＾Ｅとする。

なお、誤差がないと仮定する２個の受波センサは必ずしも受波センサＣ１と受波センサＣＮである必要はない。

（Ａ−２）第１の実施形態の構成
図１は、第１の実施形態の目標体運動解析装置の構成を示すブロック図である。

図１において、第１の実施形態の目標体運動解析装置は、Ｎ個の受波センサＣ１〜ＣＮ、Ｎ−１個のセンサ間時間差算出部３−１〜３−（Ｎ−１）、センサ間時間差誤差共分散行列算出部４、Ｎ−１個のセンサ間時間差蓄積部５−１〜５−（Ｎ−１）、センサ間時間差誤差共分散行列蓄積部６、目標体・センサ位置評価関数構成部７、目標体状態量・センサ位置推定部８、目標体位置・速度推定値出力端子９及びセンサ位置補正部１０を有する。

受波センサＣ１〜ＣＮはそれぞれ、観測体１に取り付けられ、目標体２からの信号（例えば音波）を捕捉するものである。

各センサ間時間差算出部３−ｘ（但しｘは１〜Ｎ−１）は、受波センサＣｘと受波センサＣｘ＋１の受信信号の到来時間差を算出するものである。

センサ間時間差誤差共分散行列算出部４は、受波センサＣ１〜ＣＮの受信信号から各センサ間時間差の推定誤差共分散行列Σ（ｔ_ｋ）を算出するものである。

各センサ間時間差蓄積部５−ｘは、各時刻の受波センサＣｘと受波センサＣｘ＋１の受信信号の到来時間差を蓄積するものである。

センサ間時間差誤差共分散行列蓄積部６は、各時刻のセンサ間時間差誤差共分散行列を蓄積するものである。

目標体・センサ位置評価関数構成部７は、センサ間時間差蓄積部５−１〜５−（Ｎ−１）に蓄積された各センサ間時間差や、センサ間時間差誤差共分散行列蓄積部６に蓄積されたセンサ間時間差誤差共分散行列から目標体の位置、速度及びセンサ位置に関する評価関数を構成するものである。

目標体状態量・センサ位置推定部８は、目標体・センサ位置評価関数構成部７によって構成された目標体・センサ位置評価関数を最小にするような目標体２の位置、速度及びセンサ位置（センサ位置誤差）を推定するものである。

目標体状態量・センサ位置推定値出力端子９は、目標体状態量・センサ位置推定部８によって推定された目標体２の位置及び速度（センサ位置を含めても良い）を出力する端子である。

センサ位置補正部１０は、目標体状態量・センサ位置推定部８によるセンサ位置誤差の推定値に基づいて、センサ位置を補正するものである。

（Ａ−３）第１の実施形態の動作
次に、第１の実施形態の目標体運動解析装置の動作（第１の実施形態の目標体運動解析方法）について説明する。

時刻ｔ_ｋにおいて受波センサＣ１で受信された信号は、センサ間時間差算出部３−１に送られると共に、センサ間時間差誤差共分散行列算出部４に送られる。時刻ｔ_ｋにおいて受波センサＣ２で受信された信号は、センサ間時間差算出部３−１及びセンサ間時間差算出部３−２に送られると共に、センサ間時間差誤差共分散行列算出部４に送られる。以下、同様に、時刻ｔ_ｋにおいて受波センサＣｘで受信された信号は、センサ間時間差算出部３−ｘ及びセンサ間時間差算出部３−（ｘ＋１）に送られると共に、センサ間時間差誤差共分散行列算出部４に送られる。時刻ｔ_ｋにおいて受波センサＣＮで受信された信号は、センサ間時間差算出部３−（Ｎ−１）に送られると共に、センサ間時間差誤差共分散行列算出部４に送られる。

各センサ間時間差算出部３−１、…、３−（Ｎ−１）はそれぞれ、２個の受波センサから送られた受信信号間の到来時間差観測値τm_１，２（ｔ_ｋ）、…、τm_Ｎ−１，_Ｎ（ｔ_ｋ）を算出し、対応するセンサ間時間差蓄積部５−１、…、５−（Ｎ−１）に送る。

一方、センサ間時間差誤差共分散行列算出部４は、受波センサＣ１〜ＣＮから送られた受信信号から、各センサ間時間差の誤差共分散行列Σ（ｔ_ｋ）を算出し、センサ間時間差誤差共分散行列蓄積部６に送る。

各センサ間時間差蓄積部５−１、…、５−（Ｎ−１）はそれぞれ、対応するセンサ間時間差算出部３−１、…、３−（Ｎ−１）から送られた各時刻のセンサ間到来時間差観測値τm_１，２（ｔ_ｋ）、…、τm_{Ｎ−１，Ｎ}（ｔ_ｋ）を蓄積し、蓄積したセンサ間到来時間差観測値τm_１，２（ｔ_ｋ）、…、τm_{Ｎ−１，Ｎ}（ｔ_ｋ）（ｋ＝１，…，Ｋ）を目標体・センサ位置評価関数構成部７に送る。

センサ間時間差誤差共分散行列蓄積部６は、センサ間時間差誤差共分散行列算出部４から送られた各時刻の各センサ間時間差の推定誤差共分散行列Σ（ｔ_ｋ）を蓄積し、蓄積した各センサ間時間差の推定誤差共分散行列Σ（ｔ_ｋ）（ｋ＝１，…，Ｋ）を目標体・センサ位置評価関数構成部７に送る。

目標体・センサ位置評価関数構成部７は、各センサ間時間差蓄積部５−１、…、５−（Ｎ−１）から送られたセンサ間到来時間差観測値τm_１，２（ｔ_ｋ）、…、τm_{Ｎ−１，Ｎ}（ｔ_ｋ）（ｋ＝１，…，Ｋ）、センサ間時間差誤差共分散行列蓄積部６から送られた各センサ間時間差の推定誤差共分散行列Σ（ｔ_ｋ）（ｋ＝１，…，Ｋ）及びセンサ位置補正部１０から送られた観測体固定座標系における各受波センサの位置ベクトルΔＳ_１，…，ΔＳ_Ｎから、上述した（１５）式の目標体・センサ位置評価関数Ｊ（Ｘ_Ｔ（ｔ_ｒ），Ｖ_Ｔ，Ｅ）を構成し、目標体状態量・センサ位置推定部８に送る。

ここで、（１５）式において必要な観測体位置ベクトルＸ_Ｏ（ｔ_ｋ）（ｋ＝１，…，Ｋ）及び固定座標系に対する観測体固定座標系の回転行列Ｃ（ｔ_ｋ）（ｋ＝１，…，Ｋ）は、自身（観測体１）の位置、運動に関する情報であるので、ジャイロやコンパス等のセンサを用いて測定した値を用いる。

目標体状態量・センサ位置推定部８は、目標体・センサ位置評価関数構成部７から送られた目標体・センサ位置評価関数Ｊ（Ｘ_Ｔ（ｔ_ｒ），Ｖ_Ｔ，Ｅ）が最小となるような基準時刻ｔ_ｒにおける目標体位置ベクトルＸ_Ｔ（ｔ_ｒ）、目標体速度ベクトルＶ_Ｔ及び受波センサ位置誤差Ｅを探索し、目標体・センサ位置評価関数Ｊ（Ｘ_Ｔ（ｔ_ｒ），Ｖ_Ｔ，Ｅ）が最小となったＸ_Ｔ（ｔ_ｒ）、Ｖ_Ｔ及びＥをそれぞれ、基準時刻ｔ_ｒにおける目標体位置ベクトルの推定値＾Ｘ_Ｔ（ｔ_ｒ）、目標体速度ベクトルの推定値＾Ｖ_Ｔ及び受波センサ位置誤差の推定値＾Ｅとし、目標体位置ベクトルの推定値＾Ｘ_Ｔ（ｔ_ｒ）及び目標体速度ベクトルの推定値＾Ｖ_Ｔを出力端子９から出力すると共に、受波センサ位置誤差の推定値＾Ｅをセンサ位置補正部１０に送る。ここで、目標体・センサ位置評価関数Ｊ（Ｘ_Ｔ（ｔ_ｒ），Ｖ_Ｔ，Ｅ）を最小とするＸ_Ｔ（ｔ_ｒ）、Ｖ_Ｔ及びＥの探索には、例えば、非特許文献１に記載されているガウス−ニュートンアルゴリズムなどを用いれば良い。

センサ位置補正部１０は、目標体状態量・センサ位置推定部８から送られた、（１７）式で表される受波センサ位置誤差の推定値＾Ｅを用いて、それまで設定されていた観測体１の固定座標系における各センサの位置ベクトルΔＳ^{（ｏｌｄ）} _ｉ（ｉ＝２，…，Ｎ−１）を、（１８）式に従って補正して、新たな観測体１の固定座標系における受波センサの位置ベクトルとしてΔＳ_ｉ（ｉ＝１，…，Ｎ）を、目標体・センサ位置評価関数構成部７に送る。ここで、センサ位置を補正しない受波センサＣ１及びＣＮについては、センサ位置補正部１０は、予め設定した観測体１の固定座標系における受波センサの位置ベクトルΔＳ_１、ΔＳ_Ｎを目標体・センサ位置評価関数構成部７に送る。

なお、センサ位置補正部１０は、受波センサ位置誤差が推定されていない初期状態では、予め設定した観測体固定座標系における受波センサの位置ベクトルΔＳ_ｉ（ｉ＝１，…，Ｎ）を目標体・センサ位置評価関数構成部７に送る。

（Ａ−４）第１の実施形態の効果
以上詳細に説明したように、第１の実施形態によれば、は目標体位置、速度と共に受波センサの位置誤差の推定を行うようにしたため、受波センサの位置誤差によって生じる目標体位置、速度の推定誤差を抑えることが可能となり、解析精度の向上を実現することが可能となる。

（Ｂ）第２の実施形態
次に、本発明による目標体運動解析方法及び装置の第２の実施形態を説明する。

（Ｂ−１）第２の実施形態の目標体運動解析の原理
第２の実施形態の具体的な説明に先立って、図２を参照しながら、第２の実施形態における目標体運動解析方法の原理を説明する。

上述した第１の実施形態では、目標体位置ベクトル、目標体速度ベクトルを推定する共に、受波センサ位置誤差も同時に推定するようにしている。ところが、受波センサ位置に誤差がない場合に、受波センサ位置誤差も同時に推定すると、受波センサ位置誤差を推定しない場合に比べ、目標体位置ベクトル及び目標体速度ベクトルの推定精度が悪くなる。

そこで、この第２の実施形態では、受波センサ位置誤差が十分小さい場合には、受波センサ位置誤差の推定は行わず、目標体位置ベクトル及び目標体速度ベクトルのみを推定することで、推定精度を良くしようとしたものである。

第１の実施形態で上述した（１５）式の目標体・センサ位置評価関数Ｊ（Ｘ_Ｔ（ｔ_ｒ），Ｖ_Ｔ，Ｅ）を最小にするように推定した目標体位置ベクトルの推定値＾Ｘ_Ｔ（ｔ_ｒ）、目標体速度ベクトルの推定値＾Ｖ_Ｔ及び受波センサ位置誤差の推定値＾Ｅの推定誤差共分散行列Ｐ_{ＸＴ（ｔｒ），ＶＴ，Ｅ}は、（１９）式で表される。この推定誤差共分散行列Ｐ_{ＸＴ（ｔｒ），ＶＴ，Ｅ}を（２０）式に示すように複数の部分行列に分けことができる。部分行列Ｐ_Ｅは、受波センサ位置誤差の推定値＾Ｅの推定誤差共分散行列を表すものとなっている。但し、ｎ_ｘを目標体位置ベクトルＸ_Ｔ（ｔ_ｒ）の次元、ｎ_ｖを目標体速度ベクトルＶ_Ｔの次元、ｎ_Ｅを受波センサ位置誤差ベクトルＥの次元とすると、受波センサ位置誤差の推定値＾Ｅの推定誤差共分散行列Ｐ_Ｅは、行列Ｐ_{ＸＴ（ｔｒ），ＶＴ，Ｅ}のｎ_ｘ＋ｎ_ｖ＋１行〜ｎ_ｘ＋ｎ_ｖ＋ｎ_Ｅ行、ｎ_ｘ＋ｎ_ｖ＋１列〜ｎ_ｘ＋ｎ_ｖ＋ｎ_Ｅ列の部分行列である。

推定誤差共分散行列Ｐ_Ｅから受波センサ位置誤差の推定値＾Ｅが−定以上の精度で推定できているか判断し、受波センサ位置を補正するかどうかを判断する。

（Ｂ−２）第２の実施形態の構成
図３は、第２の実施形態の目標体運動解析装置の構成を示すブロック図であり、第１の実施形態に係る上述した図１との同一、対応部分には同一符号を付して示している。

第２の実施形態の目標体運動解析装置は、第１の実施形態に比較すると、センサ位置推定判定部１１、目標体評価関数構成部１２及び目標体状態量推定部１３が追加して設けられている点が異なっており、その他の構成は、第１の実施形態とほぼ同様である。

センサ位置推定判定部１１は、受波センサ位置の推定精度を算出し、この推定精度から次周期で受波センサ位置の推定を行うかどうかを判定するものである。

目標体評価関数構成部１２は、各センサ間時間差蓄積部５−１、…、５−（Ｎ−１）に蓄積された各センサ間時間差や、センサ間時間差誤差共分散行列蓄積部６に蓄積されたセンサ間時間差誤差共分散行列から、目標体２の位置、速度に関する評価関数を構成するものである。

目標体状態量推定部１３は、目標体評価関数構成部１２が構成した目標体評価関数を最小にするような目標体位置、速度を推定するものである。

これら目標体評価関数構成部１２及び目標体状態量推定部１３は、センサ位置推定判定部１１が次周期で受波センサ位置の推定を行なわないと判定したときに有効に機能するものである。

（Ｂ−３）第２の実施形態の動作
次に、第２の実施形態の目標体運動解析装置の動作（第２の実施形態の目標体運動解析方法）について説明する。

受波センサＣ１〜ＣＮ、センサ間時間差算出部３−１〜３−（Ｎ−１）、センサ間時間差誤差共分散行列算出部４、センサ間時間差蓄積部５−１〜５−（Ｎ−１）、センサ間時間差誤差共分散行列蓄積部６の動作は、第１の実施形態と同様である。

但し、各センサ間時間差蓄積部５−１、…、５−（Ｎ−１）は、蓄積したセンサ間到来時間差観測値τm_１，２（ｔ_ｋ）、…、τm_{Ｎ−１，Ｎ}（ｔ_ｋ）（ｋ＝１，…，Ｋ）を目標体・センサ位置評価関数構成部７に送ると共に、目標体評価関数構成部１２にも送る。また、センサ間時間差誤差共分散行列蓄積部６は、蓄積した各センサ間時間差の推定誤差共分散行列Σ（ｔ_ｋ）（ｋ＝１，…，Ｋ）を目標体・センサ位置評価関数構成部７に送ると共に、センサ位置推定判定部１１及び目標体評価関数構成部１２にも送る。

第２の実施形態の目標体・センサ位置評価関数構成部７は、センサ位置推定判定部１１から評価関数構成の指示があった場合にのみ、第１の実施形態で説明した動作を実行するものである。

第２の実施形態の目標体状態量・センサ位置推定部８も、推定動作自体は第１の実施形態のものと同様である。第２の実施形態の目標体状態量・センサ位置推定部８は、目標体位置ベクトルの推定値＾Ｘ_Ｔ（ｔ_ｒ）及び目標体速度ベクトルの推定値＾Ｖ_Ｔを出力端子９から出力すると共に、目標体位置ベクトルの推定値＾Ｘ_Ｔ（ｔ_ｒ）、目標体速度ベクトルの推定値＾Ｖ_Ｔ及び受波センサ位置誤差の推定値＾Ｅをセンサ位置推定判定部１１に送り、受波センサ位置誤差の推定値＾Ｅをセンサ位置補正部１０に送る。

第２の実施形態のセンサ位置補正部１０は、目標体状態量・センサ位置推定部８から受波センサ位置誤差の推定値＾Ｅが送られた場合には第１の実施形態と同様な補正動作を行い、補正後の位置ベクトルΔＳ_ｉ（ｉ＝１，…，Ｎ）を、目標体・センサ位置評価関数構成部７及び目標体評価関数構成部１２に送る。ここで、センサ位置補正部１０は、センサ位置を補正しない受波センサＣ１及びＣＮについては、予め設定した観測体１の固定座標系における受波センサの位置ベクトルΔＳ_１、ΔＳ_Ｎを目標体・センサ位置評価関数構成部７及び目標体評価関数構成部１２に送る。また、センサ位置補正部１０は、受波センサ位置誤差が推定されていない初期状態では、予め設定した観測体１の固定座標系における受波センサの位置ベクトルΔＳ_ｉ（ｉ＝１，…，Ｎ）を目標体・センサ位置評価関数構成部７及び目標体評価関数構成部１２に送る。さらに、センサ位置補正部１０は、目標体状態量・センサ位置推定部８から受波センサ位置誤差の推定値＾Ｅが送られない場合には、それまでに補正された最新の各受波センサの位置ベクトルを観測体１の固定座標系における受波センサの位置ベクトルとして目標体評価関数構成部１２に送る。

第２の実施形態で新たに設けられたセンサ位置推定判定部１１は、目標体状態量・センサ位置推定部８から目標体位置ベクトルの推定値＾Ｘ_Ｔ（ｔ_ｒ）、目標体の速度ベクトルの推定値＾Ｖ_Ｔ及び受波センサ位置誤差の推定値＾Ｅを送られた場合に、これらの推定値と、センサ間時間差誤差共分散行列蓄積部６から送られた各センサ間時間差の推定誤差共分散行列Σ（ｔ_ｋ）（ｋ＝１，…，Ｋ）を用いて、上述の（１９）式及び（２０）式に従って、受波センサ位置誤差の推定値＾Ｅの推定誤差共分散行列Ｐ_Ｅを算出し、この推定誤差共分散行列Ｐ_Ｅの各分散値が予め設定された閾値以下であるかどうかを判断し、閾値以下であった分散値の個数が予め設定した個数以上あった場合には目標体評価関数構成部１２に評価関数構成の指示を送り、閾値以下であった分散値の個数が予め設定した個数未満であった場合には目標体・センサ位置評価関数構成部７に評価関数構成の指示を送る。

目標体評価関数構成部１２は、センサ位置推定判定部１１から評価関数構成の指示があった場合に、各センサ間時間差蓄積部５−１、…、５−（Ｎ−１）から送られたτm_１，２（ｔ_ｋ）、…、τm_{Ｎ−１，Ｎ}（ｔ_ｋ）（ｋ＝１，…，Ｋ）、センサ間時間差誤差共分散行列蓄積部６から送られた各センサ間時間差の推定誤差共分散行列Σ（ｔ_ｋ）（ｋ＝１，…，Ｋ）及びセンサ位置補正部１０から送られた観測体１の固定座標系における各受波センサの位置ベクトルΔＳ_１、ΔＳ_２、…、ΔＳ_Ｎから上述した（７）式の目標体評価関数Ｊ（Ｘ_Ｔ（ｔ_ｒ），Ｖ_Ｔ）を構成し、目標体状態量推定部１３に送る。ここで、（７）式で必要となる観測体位置ベクトルＸ_ｏ（ｔ_ｋ）（ｋ＝１，…，Ｋ）及び固定座標系に対する観測体１の固定座標系の回転行列Ｃ（ｔ_ｋ）（ｋ＝１，…，Ｋ）は自身（観測体１）の位置、運動に関する情報であるので、ジャイロやコンパス等のセンサを用いて測定した値を用いる。

目標体状態量推定部１３は、目標体評価関数構成部１２から目標体評価関数Ｊ（Ｘ_Ｔ（ｔ_ｒ），Ｖ_Ｔ）が送られた場合に、この目標体評価関数Ｊ（Ｘ_Ｔ（ｔ_ｒ），Ｖ_Ｔ）が最小となるような基準時刻ｔ_ｒにおける目標体位置ベクトルＸ_Ｔ（ｔ_ｒ）、目標体速度ベクトルＶ_Ｔを探索し、目標体評価関数Ｊ（Ｘ_Ｔ（ｔ_ｒ），Ｖ_Ｔ）が最小となった目標体位置ベクトルＸ_Ｔ（ｔ_ｒ）、目標体速度ベクトルＶ_Ｔをそれぞれ基準時刻ｔ_ｒにおける目標体位置ベクトルの推定値＾Ｘ_Ｔ（ｔ_ｒ）、目標体速度ベクトルの推定値＾Ｖ_Ｔとし、出力端子９から出力する。

（Ｂ−４）第２の実施形態の効果
以上詳細に説明したように、第２の実施形態によれば、目標体位置、速度と共に受波センサの位置誤差の推定を行うようにしているため、受波センサの位置誤差によって生じる目標体位置、速度の推定誤差を抑えることが可能となり、解析精度の向上を実現することが可能となる。

また、受波センサ位置誤差の推定誤差が十分小さい場合には、受波センサ位置誤差の推定は行わず、目標体位置ベクトル及び目標体速度ベクトルのみを推定するようにしているので、さらに、目標体位置、速度の解析精度の向上を実現することが可能となる。

（Ｃ）第３の実施形態
次に、本発明による目標体運動解析方法及び装置の第３の実施形態を、図４を参照しながら説明する。

第３の実施形態は、以下の考え方によってなされた。センサ間時間差の変化が小さい場合には、センサ位置誤差の推定が困難になり、センサ位置誤差の推定値に基づいてセンサ位置を補正すると、却って、センサ位置誤差が大きくなる可能性がある。このような不都合を排除するため、第３の実施形態では、センサ間時間差の変化が十分大きい場合に、目標体位置、速度と共に受波センサの位置誤差の推定を行うこととした。

（Ｃ−１）第３の実施形態の構成
図４は、第３の実施形態の目標体運動解析装置の構成を示すブロック図であり、第１の実施形態に係る上述した図１や、第２の実施形態に係る上述した図３との同一、対応部分には同一符号を付して示している。

第３の実施形態の目標体運動解析装置は、第１の実施形態に比較すると、目標体評価関数構成部１２、目標体状態量推定部１３及びセンサ間時間差変化判定部１４が追加して設けられている点が異なっており、その他の構成は、第１の実施形態とほぼ同様である。また、第３の実施形態の目標体運動解析装置は、第２の実施形態に比較すると、センサ位置判定推定部１１に代えて、センサ間時間差変化判定部１４が設けられている点が異なっており、その他の構成は、第２の実施形態とほぼ同様である。すなわち、第１及び第２の実施形態と比較した場合には、センサ間時間差変化判定部１４が設けられていることが相違点である。

センサ間時間差変化判定部１４は、各センサ間時間差蓄積部５−１、…、５−（Ｎ−１）に蓄積された各センサ間時間差から時間差の変化を判定するものであり、この判定結果に応じて、目標体・センサ位置評価関数構成部７又は目標体評価関数構成部１２の一方が有効に機能する。

（Ｃ−２）第３の実施形態の動作
次に、第３の実施形態の目標体運動解析装置の動作（第３の実施形態の目標体運動解析方法）について説明する。

受波センサＣ１〜ＣＮ、センサ間時間差算出部３−１〜３−（Ｎ−１）、センサ間時間差誤差共分散行列算出部４、センサ間時間差蓄積部５−１〜５−（Ｎ−１）、センサ間時間差誤差共分散行列蓄積部６の動作は、第１の実施形態と同様である。

但し、各センサ間時間差蓄積部５−１、…、５−（Ｎ−１）は、蓄積したセンサ間到来時間差観測値τm_１，２（ｔ_ｋ）、…、τm_{Ｎ−１，Ｎ}（ｔ_ｋ）（ｋ＝１，…，Ｋ）をセンサ間時間差変化判定部１４に送る。

センサ間時間差変化判定部１４は、各センサ間時間差蓄積部５−１、…、５−（Ｎ−１）から送られたセンサ間到来時間差観測値τm_１，２（ｔ_ｋ）、…、τm_{Ｎ−１，Ｎ}（ｔ_ｋ）（ｋ＝１，…，Ｋ）の最小時間差と最大時間差を検索し、最大時間差−最小時間差が予め設定された閾値以上か否かを判別する。センサ間時間差変化判定部１４は、最大時間差−最小時間差が予め設定された閾値以上の場合には、各センサ間到来時間差観測値τm_１，２（ｔ_ｋ）、…、τm_{Ｎ−１，Ｎ}（ｔ_ｋ）（ｋ＝１，…，Ｋ）を目標体・センサ位置評価関数構成部７に送り、最大時間差−最小時間差が予め設定された閾値未満の場合には、各センサ間到来時間差観測値τm_１，２（ｔ_ｋ）、…、τm_{Ｎ−１，Ｎ}（ｔ_ｋ）（ｋ＝１，…，Ｋ）を目標体評価関数構成部１２に送る。

目標体・センサ位置評価関数構成部７は、センサ間時間差変化判定部１４から各センサ間到来時間差観測値τm_１，２（ｔ_ｋ）、…、τm_{Ｎ−１，Ｎ}（ｔ_ｋ）（ｋ＝１，…，Ｋ）を送られた場合には、第１の実施形態と同様にして、目標体・センサ位置評価関数Ｊ（Ｘ_Ｔ（ｔ_ｒ），Ｖ_Ｔ，Ｅ）を構成し、目標体状態量・センサ位置推定部８に送る。

目標体状態量・センサ位置推定部８も、第１の実施形態と同様にして、目標体・センサ位置評価関数Ｊ（Ｘ_Ｔ（ｔ_ｒ），Ｖ_Ｔ，Ｅ）に基づいて、基準時刻ｔ_ｒにおける目標体位置ベクトルの推定値＾Ｘ_Ｔ（ｔ_ｒ）、目標体速度ベクトルの推定値＾Ｖ_Ｔ及び受波センサ位置誤差の推定値＾Ｅを求め、目標体位置ベクトルの推定値＾Ｘ_Ｔ（ｔ_ｒ）及び目標体速度ベクトルの推定値＾Ｖ_Ｔを出力端子９から出力すると共に、受波センサ位置誤差の推定値Ｅ＾をセンサ位置補正部１０に送る。

センサ位置補正部１０は、第１や第２の実施形態と同様にして、各受波センサの位置ベクトルΔＳ_ｉ（ｉ＝１，…，Ｎ）を、上述した（１８）式に従って補正し、補正後の各受波センサの位置ベクトルΔＳ_ｉ（ｉ＝１，…，Ｎ）を、目標体・センサ位置評価関数構成部７及び目標体評価関数構成部１２に送る。

目標体評価関数構成部１２は、センサ間時間差変化判定部１４から各センサ間到来時間差観測値τm_１，２（ｔ_ｋ）、…、τm_{Ｎ−１，Ｎ}（ｔ_ｋ）（ｋ＝１，…，Ｋ）を送られた場合には、第２の実施形態と同様にして、目標体評価関数Ｊ（Ｘ_Ｔ（ｔ_ｒ），Ｖ_Ｔ）を構成し、目標体状態量推定部１３に送り、目標体状態量推定部１３も、第２の実施形態と同様にして、基準時刻ｔ_ｒにおける目標体位置ベクトルの推定値＾Ｘ_Ｔ（ｔ_ｒ）、目標体速度ベクトルの推定値＾Ｖ_Ｔを求め、出力端子９から出力する。

（Ｃ−３）第３の実施形態の効果
センサ間時間差の変化が小さい場合には、センサ位置誤差の推定が困難になり、センサ位置誤差の推定値に基づいてセンサ位置を補正すると却ってセンサ位置誤差が大きくなる可能性があるが、第３の実施形態によれば、このような不都合を未然に防止することができる。

すなわち、第３の実施形態によれば、センサ間時間差の変化が十分に大きい場合には、目標体位置、速度と共に受波センサの位置誤差の推定を行うようにしているため、受波センサの位置誤差によって生じる目標体位置、速度の推定誤差を抑えることが可能となり、解析精度の向上を実現することが可能となる。一方、センサ間時間差の変化が小さい場合には、受波センサ位置誤差の推定は行わず、目標体位置ベクトル及び目標体速度ベクトルのみを推定するようにしているので、センサ位置誤差が大きくなる可能性を排除できる。

（Ｄ）第４の実施形態
次に、本発明による目標体運動解析方法及び装置の第４の実施形態を説明する。

（Ｄ−１）第４の実施形態の目標体運動解析の原理
第４の実施形態の具体的な説明に先立って、第４の実施形態における目標体運動解析方法の原理を説明する。

上述した第１の実施形態から第３の実施形態では、１個の目標体のみの観測時間差を用いて目標体位置、速度及び受波センサの位置誤差を推定するようにしている。しかし、複数の目標体に対してセンサ間時間差の観測値、センサ間時間差誤差共分散行列を蓄積しておけば、これらを用いて、各目標体位置、速度を推定すると共に受波センサの位置誤差を推定することも可能となる。第４の実施形態は、このような観点からなされたものである。

Ｘ^（ｐ） _Ｔ（ｔ^（ｐ） _ｒ）を第ｐ目標体の任意の基準時刻ｔ^（ｐ） _ｒにおける目標体位置ベクトル、Ｖ^（ｐ） _Ｔを等速直線運動する第ｐ目標体の速度ベクトルとすると、時刻ｔ^（ｐ） _ｋにおける第ｐ目標体の目標体位置ベクトルＸ^（ｐ） _Ｔ（ｔ^（ｐ） _ｋ）は、上述した（１）式から、（２１）式となる。

時刻ｔ^（ｐ） _ｋにおける観測体位置ベクトルは既知であり、Ｘ_Ｏ（ｔ^（ｐ） _ｋ）とする。観測体固定座標系は、観測体１の針路の変更に伴い、回転する。時刻ｔ^（ｐ） _ｋにおいて、固定座標系に対する観測体固定座標系の回転行列をＣ（ｔ^（ｐ） _ｋ）とする。時刻ｔ^（ｐ） _ｋにおける受波センサＣｉの位置ベクトルＳ_ｉ（ｔ^（ｐ） _ｋ）は、（２２）式となる。

ここで、受波センサＣｎにε_ｎの誤差があり、観測体固定座標系での実際の位置ベクトルが（ΔＳ_ｎ＋ε_ｎ）とすると、時刻ｔ^（ｐ） _ｋにおける受波センサＣｎの実際の位置ベクトルＳ^εｎ _ｎ（ｔ^（ｐ） _ｋ）は、（２３）式のようになる。よって、時刻ｔ^（ｐ） _ｋにおける受波センサＣｎと第ｐ目標体間の距離Ｒ^{εｎ（ｐ）} _ｎ（ｔ^（ｐ） _ｋ）は（２４）式に示すようになる。

ここで、全ての受波センサ位置に誤差があるとすると、受波センサ位置の推定ができない。そこで、Ｎ個の受波センサ位置のうち、２個は誤差がないと仮定する。ここでは、受波センサＣ１と受波センサＣＮに誤差がないとして説明する。このようにすると、第ｐ目標体から発信された信号を、受波センサＣ１と受波センサＣ２で受波したとき、センサ間の信号の到来時間差τ^ε（ｐ） _１，２（ｔ^（ｐ） _ｋ）は、（２５）式で表される。また、第ｐ目標体から発信された信号を、受波センサＣｉ（但し、２＜ｉ＜Ｎ−２）と受波センサＣｊ（但し、３＜ｊ＜Ｎ−１，ｊ≠ｉ）で受波したとき、センサ間の信号の到来時間差τ^ε（ｐ） _ｉ，ｊ（ｔ^（ｐ） _ｋ）は、（２６）式で表される。さらに、受波センサＣ（Ｎ−１）と受波センサＣＮで受波したとき、センサ間の信号の到来時間差τ^ε（ｐ） _{Ｎ−１，Ｎ}（ｔ^（ｐ） _ｋ）は、（２７）式で表される。受波センサがＮ個の場合を考え、時刻ｔ^（ｐ） _ｋにおける独立な時間差ベクトルτ^ε（ｐ）（ｔ^（ｐ） _ｋ）を（２８）式で定義する。

実際に観測されるセンサ間の到来時間差τ^ε（ｐ） _ｍ（ｔ^（ｐ） _ｋ）は、周囲の雑音により誤差を持ち、（２９）式に示すようになる。（２９）式において、誤差ベクトルＷ^（ｐ）（ｔ^（ｐ） _ｋ）は、時間的に無相関で、平均０、共分散行列Σ^（ｐ）（ｔ^（ｐ） _ｋ）のガウス雑音ベクトルとする。

第ｐ目標体について、時刻ｔ^（ｐ） _１〜ｔ^（ｐ） _Ｋ（ｐ）において得られた観測時間差τ^ε（ｐ） _ｍ（ｔ^（ｐ） _１）、τ^ε（ｐ） _ｍ（ｔ^（ｐ） _２）、…、τ^ε（ｐ） _ｍ（ｔ^（ｐ） _Ｋ（ｐ））に対して、誤差ベクトルＥを（３１）式のように定めて第ｐ目標体についての目標体・センサ位置評価関数Ｊ（Ｘ^（ｐ） _Ｔ（ｔ^（ｐ） _ｒ），Ｖ^（ｐ） _Ｔ，Ｅ）を（３０）式のように定義すると、（３２）式で表される第１〜第Ｐ目標体・センサ位置評価関数Ｊ（Ｘ^（１） _Ｔ（ｔ^（１） _ｒ），…，Ｘ^（Ｐ） _Ｔ（ｔ^（Ｐ） _ｒ），Ｖ^（１） _Ｔ，…，Ｖ^（Ｐ） _Ｔ，Ｅ）を最小にするようなＸ^（ｐ） _Ｔ（ｔ^（ｐ） _ｒ）、Ｖ^（ｐ） _Ｔ（ｐ＝１，…，Ｐ）及びＥをそれぞれ、基準時刻ｔ^（ｐ） _ｒにおける第ｐ目標体の位置ベクトルの推定値＾Ｘ^（ｐ） _Ｔ（ｔ^（ｐ） _ｒ）、＾Ｖ^（ｐ） _Ｔ（ｐ＝１，…，Ｐ）及び受波センサ位置誤差の推定値＾Ｅとする。

なお、誤差がないと仮定する２個の受波センサは必ずしも受波センサ１と受波センサＮである必要はない。

（Ｄ−２）第４の実施形態の構成
図５は、第４の実施形態の目標体運動解析装置の構成を示すブロック図であり、第１の実施形態に係る上述した図１との同一、対応部分には同一符号を付して示している。

図５において、第４の実施形態の目標体運動解析装置は、Ｎ個の受波センサＣ１〜ＣＮ、Ｎ−１個のセンサ間時間差算出部３−１〜３−（Ｎ−１）、センサ間時間差誤差共分散行列算出部４、Ｎ−１個のセンサ間時間差蓄積部５−１〜５−（Ｎ−１）、センサ間時間差誤差共分散行列蓄積部６、第２の目標体・センサ位置評価関数構成部７−２、目標体状態量・センサ位置推定部８−２、目標体位置・速度推定値出力端子９、センサ位置補正部１０及び目標体別データ蓄積部１５を有する。

受波センサＣ１〜ＣＮ、センサ間時間差算出部３−１〜３−（Ｎ−１）、センサ間時間差誤差共分散行列算出部４、センサ間時間差蓄積部５−１〜５−（Ｎ−１）、センサ間時間差誤差共分散行列蓄積部６、出力端子９及びセンサ位置補正部１０は、第１の実施形態のものと同様なものである。

目標体別データ蓄積部１５は、各目標体毎の各センサ間時間差及びセンサ間時間差誤差共分散行列を蓄積するものである。

第２の目標体・センサ位置評価関数構成部７−２は、目標体別データ蓄積部１５に蓄積された各目標体についての各センサ間時間差及びセンサ間時間差誤差共分散行列から、各目標体の位置、速度及びセンサ位置に関する第１〜第Ｐ目標体・センサ位置評価関数評価関数を構成するものである。

第２の目標体及びセンサ位置推定部８−２は、第１〜第Ｐ目標体・センサ位置評価関数を最小にするような各目標体の位置、速度及びセンサ位置を推定するものである。

（Ｄ−３）第４の実施形態の動作
次に、第４の実施形態の目標体運動解析装置の動作（第４の実施形態の目標体運動解析方法）について説明する。

受波センサＣ１〜ＣＮ、センサ間時間差算出部３−１〜３−（Ｎ−１）、センサ間時間差誤差共分散行列算出部４、センサ間時間差蓄積部５−１〜５−（Ｎ−１）、センサ間時間差誤差共分散行列蓄積部６の動作は、第１の実施形態と同様である。

但し、各センサ間時間差蓄積部５−１、…、５−（Ｎ−１）は、蓄積したセンサ間到来時間差観測値τm_１，２（ｔ_ｋ）、…、τm_{Ｎ−１，Ｎ}（ｔ_ｋ）（ｋ＝１，…，Ｋ）を目標体別データ蓄積部１５に送り、また、センサ間時間差誤差共分散行列蓄積部６は、蓄積した各センサ間時間差の推定誤差共分散行列Σ（ｔ_ｋ）（ｋ＝１，…，Ｋ）を目標体別データ蓄積部１５に送る。

目標体別データ蓄積部１５は、各センサ間時間差蓄積部５−１、…、５−（Ｎ−１）から送られた第ｐ目標体の観測時間差τ^（ｐ）m_１，２（ｔ^（ｐ） _ｋ）、…、τ^（ｐ）m_{Ｎ−１，Ｎ}（ｔ^（ｐ） _ｋ）（ｋ＝１，…，Ｋ^（ｐ））、センサ間時間差誤差共分散行列蓄積部６から送られた第ｐ目標体の各センサ間時間差の推定誤差共分散行列Σ^（ｐ）（ｔ^（ｐ） _ｋ）（ｋ＝１，…，Ｋ（ｐ））を各目標体ｐ（ｐ＝１〜Ｐ）について蓄積し、この蓄積した目標体別のデータを第２の目標体・センサ位置評価関数構成部７−２に送る。

第２の目標体・センサ位置評価関数構成部７−２は、目標体別データ蓄積部１５から送られた各目標体ｐ（ｐ＝１〜Ｐ）についての観測時間差τ^（ｐ）m_１，２（ｔ^（ｐ） _ｋ）、…、τ^（ｐ）m_{Ｎ−１，Ｎ}（ｔ^（ｐ） _ｋ）（ｋ＝１，…，Ｋ^（ｐ））、センサ間時間差の推定誤差共分散行列Σ^（ｐ）（ｔ^（ｐ） _ｋ）（ｋ＝１，…，Ｋ^（ｐ））及びセンサ位置補正部１０から送られた観測体固定座標系における各センサの位置ベクトルΔＳ_１、ΔＳ_２、…、ΔＳ_Ｎから上述した（３２）式の第１〜第Ｐ目標体・センサ位置評価関数Ｊ（Ｘ^（１） _Ｔ（ｔ^（１） _ｒ），…，Ｘ^（Ｐ） _Ｔ（ｔ^（Ｐ） _ｒ），Ｖ^（１） _Ｔ，…，Ｖ^（Ｐ） _Ｔ，Ｅ）を構成し、第２の目標体状態量・センサ位置推定部８−２に送る。ここで、（３２）式で必要となる観測体位置ベクトルＸ_ｏ（ｔ^（ｐ） _ｋ）（ｋ＝１，…，Ｋ^（ｐ）、ｐ＝１，…，Ｐ）及び固定座標系に対する観測体固定座標系の回転行列Ｃ（ｔ^（ｐ） _ｋ）（ｋ＝１，…，Ｋ、ｐ＝１，…，Ｐ）は自身（観測体１）の位置、運動に関する情報であるので、ジャイロやコンパス等のセンサを用いて測定した値を用いる。

第２の目標体状態量・センサ位置推定部８−２は、第２の目標体・センサ位置評価関数構成部７−２から送られた第１〜第Ｐ目標体・センサ位置評価関数Ｊ（Ｘ^（１） _Ｔ（ｔ^（１） _ｒ），…，Ｘ^（Ｐ） _Ｔ（ｔ^（Ｐ） _ｒ），Ｖ^（１） _Ｔ，…，Ｖ^（Ｐ） _Ｔ，Ｅ）が最小となるような目標体毎の基準時刻ｔ^（ｐ） _ｒにおける目標体位置ベクトルＸ^（ｐ） _Ｔ（ｔ^（ｐ） _ｒ）（ｐ＝１〜Ｐ）、目標体速度ベクトルＶ^（ｐ） _Ｔ（ｐ＝１〜Ｐ）及び受波センサ位置誤差Ｅを探索し、評価関数Ｊ（Ｘ^（１） _Ｔ（ｔ^（１） _ｒ），…，Ｘ^（Ｐ） _Ｔ（ｔ^（Ｐ） _ｒ），Ｖ^（１） _Ｔ，…，Ｖ^（Ｐ） _Ｔ，Ｅ）が最小となった目標体位置ベクトルＸ^（ｐ） _Ｔ（ｔ^（ｐ） _ｒ）（ｐ＝１〜Ｐ）、目標体速度ベクトルＶ^（ｐ） _Ｔ（ｐ＝１〜Ｐ）及び受波センサ位置誤差Ｅをそれぞれ、目標体毎の基準時刻ｔ^（ｐ） _ｒにおける目標体位置ベクトルの推定値＾Ｘ^（ｐ） _Ｔ（ｔ^（ｐ） _ｒ）（ｐ＝１〜Ｐ）、目標体速度ベクトルの推定値＾Ｖ^（ｐ） _Ｔ（ｐ＝１〜Ｐ）及び受波センサ位置誤差の推定値＾Ｅとし、各目標体の目標体位置ベクトルの推定値＾Ｘ^（ｐ） _Ｔ（ｔ^（ｐ） _ｒ）（ｐ＝１〜Ｐ）及び目標体速度ベクトルの推定値＾Ｖ^（ｐ） _Ｔ（ｐ＝１〜Ｐ）を出力端子９から出力すると共に、受波センサ位置誤差の推定値＾Ｅをセンサ位置補正部１０に送る。

ここで、第１〜第Ｐ目標体・センサ位置評価関数Ｊ（Ｘ^（１） _Ｔ（ｔ^（１） _ｒ），…，Ｘ^（Ｐ） _Ｔ（ｔ^（Ｐ） _ｒ），Ｖ^（１） _Ｔ，…，Ｖ^（Ｐ） _Ｔ，Ｅ）を最小とする目標体位置ベクトルＸ^（ｐ） _Ｔ（ｔ^（ｐ） _ｒ）（ｐ＝１〜Ｐ）、目標体速度ベクトルＶ^（ｐ） _Ｔ（ｐ＝１〜Ｐ）及び受波センサ位置誤差Ｅの探索には、例えば、非特許文献１に記載されているガウス−ニュートンアルゴリズムなどを用いれば良い。

センサ位置補正部１０は、第２の目標体状態量・センサ位置推定部８−２から送られた、（３３）式で表される受波センサ位置誤差の推定値＾Ｅを用いて、それまで設定されていた観測体１の固定座標系における各センサの位置ベクトルΔＳ^{（ｏｌｄ）} _ｉ（ｉ＝２，…，Ｎ−１）を、（３４）式に従って補正して、新たな観測体１の固定座標系における受波センサの位置ベクトルとして、補正後のΔＳ_ｉ（ｉ＝１，…，Ｎ）を、第２の目標体・センサ位置評価関数構成部７−２に送る。ここで、センサ位置を補正しない受波センサＣ１及びＣＮについては、センサ位置補正部１０は、予め設定した観測体１の固定座標系における受波センサの位置ベクトルΔＳ_１、ΔＳ_Ｎを第２の目標体・センサ位置評価関数構成部７−２に送る。

なお、センサ位置補正部１０は、受波センサ位置誤差が推定されていない初期状態では、予め設定した観測体固定座標系における受波センサの位置ベクトルΔＳ_ｉ（ｉ＝１，…，Ｎ）を第２の目標体・センサ位置評価関数構成部７−２に送る。

（Ｄ−３）第４の実施形態の効果
以上詳細に説明したように、第４の実施形態では、推定した複数目標体の観測時間差を用いて受波センサの位置誤差を推定し、推定した受波センサの位置誤差に基づいて受波センサ位置を補正し、次の周期での目標体位置、速度の推定を行うようにしているため、１個の目標体のみの観測時間差を用いて受波センサの位置誤差を推定した場合よりも、受波センサの位置誤差の推定精度が向上する。

これによって、受波センサの位置誤差によって生じる目標体位置、速度の推定誤差を抑えることが可能となり、解析精度の向上を実現することが可能となる。

（Ｅ）他の実施形態
上記第３の実施形態において、センサ間時間差変化判定部１４は、全てのセンサ間時間差の中で最小時間差と最大時間差を検索し、最大時間差−最小時間差に基づき、センサ間時間差の変化が小さいか否かを判定するようにしているが、全てのセンサ間時間差の中から時間差変化判定に用いるセンサ間時間差を選択し（例えば、２個の受波センサ間の距離が所定以上のものに対応するセンサ間時間差を選択）、選択したセンサ間時間差の中で最小時間差と最大時間差を検索し、最大時間差−最小時間差に基づき、センサ間時間差の変化が小さいか否かを判定するようにしても良い。

上記第４の実施形態は、上記第１の実施形態の技術思想に対し、複数目標体の位置、速度に基づいて、受波センサの位置誤差を推定するという手法を適用したものであったが、上記第２や第３の実施形態の技術思想に対し、第４の実施形態の技術思想を組み合わせるようにしても良い。

また、上記第４の実施形態の説明では、物理的に別体の複数の目標体からの信号を処理するイメージで説明したが、同一の目標体からの信号も、その受信タイミングなどが異なる場合であれば、異なる目標体からの信号として処理に含めるようにしても良い。

さらに、上記各実施形態では、観測体１及び目標体２共に運動物体であるように説明したが、いずれか一方が固定物であっても良い。

さらにまた、上記各実施形態においては、目標体２に関する状態量として、位置及び速度を推定するものを示したが、一方だけを推定するものであっても良く、さらには、位置及び加速度（速度の微分）を推定するものであっても良く、推定される状態量は上記実施形態のものに限定されない。

第１の実施形態から第４の実施形態では受波センサＮ個に対して位置誤差を推定するセンサをＮ−２個にしているが、位置誤差を推定するセンサは１〜Ｎ−２個の範囲であれば任意に設定できる。例えば、上記（１６）、（１７）、（３１）式において、受波センサ位置誤差ベクトルＥを推定したい受波センサだけを選択して構成すれば良い。センサＣ２だけを推定したいとすれば、誤差ベクトルＥを（３５）式のように構成して、目標体・センサ位置評価関数Ｊ（Ｘ_Ｔ（ｔ_ｒ），Ｖ_Ｔ，Ｅ）あるいは第１〜第Ｐ目標体・センサ位置評価関数Ｊ（Ｘ^（１） _Ｔ（ｔ^（１） _ｒ），…，Ｘ^（Ｐ） _Ｔ（ｔ^（Ｐ） _ｒ），Ｖ^（１） _Ｔ，…，Ｖ^（Ｐ） _Ｔ，Ｅ）を構成すれば良い。

また、受波センサの推定位置の精度の評価方法として、上記第２の実施形態の評価方法と、上記第３の実施形態の評価方法とを併用するようにしても良い。

第１の実施形態の目標体運動解析装置の構成を示すブロック図である。 従来の目標体運動解析方法における観測系及び運動系を示す幾何学的説明図である。 第２の実施形態の目標体運動解析装置の構成を示すブロック図である。 第３の実施形態の目標体運動解析装置の構成を示すブロック図である。 第４の実施形態の目標体運動解析装置の構成を示すブロック図である。

符号の説明

Ｃ１〜ＣＮ…受波センサ、１…観測体、２…目標体、３−１〜３−（Ｎ−１）…センサ間時間差算出部、４…センサ間時間差誤差共分散行列算出部、５−１〜５−（Ｎ−１）…センサ間時間差蓄積部、６…センサ間時間差誤差共分散行列蓄積部、７、７−２…目標体・センサ位置評価関数構成部、８、８−２…目標体状態量・センサ位置推定部、９…目標体状態量・センサ位置推定値出力端子、１０…センサ位置補正部、１１…センサ位置推定判定部、１２…目標体評価関数構成部、１３…目標体状態量推定部、１４…センサ間時間差変化判定部、１５…目標体別データ蓄積部。

Claims (8)

1. 目標体から放射される信号を、上記目標体と相対的に運動可能な観測体に取り付けた、空間的に離れた３個以上の受波センサで受信し、上記各受波センサ間の信号到来時間差の測定結果である観測時間差時系列を得、観測時間差時系列に対して所定演算を実行して、上記目標体に関する状態量を推定する目標体運動解析方法において、
   上記観測時間差時系列に基づき、上記目標体の位置及び速度と共に、上記受波センサ位置をも評価対象としたセンサ位置含有評価関数を構成した後、そのセンサ位置含有評価関数を最小化する上記目標体の位置及び速度、並びに、受波センサ位置を探索して、これらパラメータの推定値を得る状態量・センサ位置推定工程と、
   上記受波センサ位置の推定値で、上記評価関数の構成に適用する受波センサ位置を補正するセンサ位置補正工程と
   を含むことを特徴とする目標体運動解析方法。
2. 上記受波センサ位置の推定値の推定精度を算出するセンサ位置推定精度算出工程と、
   この推定精度が予め設定した閾値より良い場合には、上記観測時間差時系列に基づき、上記目標体の位置及び速度を評価対象としたセンサ位置除外評価関数を構成した後、そのセンサ位置除外評価関数を最小化する上記目標体の位置及び速度を探索して、推定値を得る状態量推定工程と
   をさらに含むことを特徴とする請求項１に記載の目標体運動解析方法。
3. 上記観測時間差時系列に基づき、センサ間時間差の時間変化を測定するセンサ間時間差変化判定工程と、
   センサ間時間差の時間変化が予め設定した閾値未満であった場合には、上記状態量・センサ位置推定工程に代って機能し、上記観測時間差時系列に基づき、上記目標体の位置及び速度を評価対象としたセンサ位置除外評価関数を構成した後、そのセンサ位置除外評価関数を最小化する上記目標体の位置及び速度を探索して推定値を得る状態量推定工程と
   をさらに含むことを特徴とする請求項１に記載の目標体運動解析方法。
4. 目標体から放射される信号を、上記目標体と相対的に運動可能な観測体に取り付けた、空間的に離れた３個以上の受波センサで受信し、上記各受波センサ間の信号到来時間差の測定結果である観測時間差時系列を得、観測時間差時系列に対して所定演算を実行して、上記目標体に関する状態量を推定する目標体運動解析方法において、
   複数の目標体の上記各観測時間差時系列に基づき、上記目標体の位置及び速度と共に、上記受波センサ位置をも評価対象としたセンサ位置含有目標体統合評価関数を構成した後、そのセンサ位置含有目標体統合評価関数を最小化する上記各目標体の位置及び速度、並びに、受波センサ位置を探索して、これらパラメータの推定値を得る目標体別状態量・センサ位置推定工程と、
   上記受波センサ位置の推定値で、上記評価関数の構成に適用する受波センサ位置を補正するセンサ位置補正工程と
   を含むことを特徴とする目標体運動解析方法。
5. 目標体から放射される信号を、上記目標体と相対的に運動可能な観測体に取り付けた、空間的に離れた３個以上の受波センサで受信し、上記各受波センサ間の信号到来時間差の測定結果である観測時間差時系列を得、観測時間差時系列に対して所定演算を実行して、上記目標体に関する状態量を推定する目標体運動解析装置において、
   上記観測時間差時系列に基づき、上記目標体の位置及び速度と共に、上記受波センサ位置をも評価対象としたセンサ位置含有評価関数を構成した後、そのセンサ位置含有評価関数を最小化する上記目標体の位置及び速度、並びに、受波センサ位置を探索して、これらパラメータの推定値を得る状態量・センサ位置推定手段と、
   上記受波センサ位置の推定値で、上記評価関数の構成に適用する受波センサ位置を補正するセンサ位置補正手段と
   を有することを特徴とする目標体運動解析装置。
6. 上記受波センサ位置の推定値の推定精度を算出するセンサ位置推定精度算出手段と、
   この推定精度が予め設定した閾値より良い場合には、上記観測時間差時系列に基づき、上記目標体の位置及び速度を評価対象としたセンサ位置除外評価関数を構成した後、そのセンサ位置除外評価関数を最小化する上記目標体の位置及び速度を探索して、推定値を得る状態量推定手段と
   をさらに有することを特徴とする請求項５に記載の目標体運動解析装置。
7. 上記観測時間差時系列に基づき、センサ間時間差の時間変化を測定するセンサ間時間差変化判定手段と、
   センサ間時間差の時間変化が予め設定した閾値未満であった場合には、上記状態量・センサ位置推定手段に代って機能し、上記観測時間差時系列に基づき、上記目標体の位置及び速度を評価対象としたセンサ位置除外評価関数を構成した後、そのセンサ位置除外評価関数を最小化する上記目標体の位置及び速度を探索して推定値を得る状態量推定手段と
   をさらに有することを特徴とする請求項５に記載の目標体運動解析装置。
8. 目標体から放射される信号を、上記目標体と相対的に運動可能な観測体に取り付けた、空間的に離れた３個以上の受波センサで受信し、上記各受波センサ間の信号到来時間差の測定結果である観測時間差時系列を得、観測時間差時系列に対して所定演算を実行して、上記目標体に関する状態量を推定する目標体運動解析装置において、
   複数の目標体の上記各観測時間差時系列に基づき、上記目標体の位置及び速度と共に、上記受波センサ位置をも評価対象としたセンサ位置含有目標体統合評価関数を構成した後、そのセンサ位置含有目標体統合評価関数を最小化する上記各目標体の位置及び速度、並びに、受波センサ位置を探索して、これらパラメータの推定値を得る目標体別状態量・センサ位置推定手段と、
   上記受波センサ位置の推定値で、上記評価関数の構成に適用する受波センサ位置を補正するセンサ位置補正手段と
   を有することを特徴とする目標体運動解析装置。
   

Images