センサ情報融合装置
【課題】２Ｄ角度観測値を用いた距離推定値初期値と距離変化率推定値初期値の高精度化、２Ｄ角度観測値を用いたバッチ処理後の距離推定値と距離変化率推定値の高精度化を可能にする。
【解決手段】センサによる２Ｄ角度観測情報に基づいて２Ｄ角度航跡を算出する２Ｄ角度追尾手段と、２Ｄ角度観測情報と２Ｄ角度航跡について事前設定された圧縮間隔に基づいて最小２乗統合により圧縮処理して統合２Ｄ角度観測情報を算出する２Ｄ角度観測情報圧縮手段と、３点の統合２Ｄ角度観測情報に基づいて距離推定値初期値および距離変化率推定値初期値を算出する距離推定値初期値算出手段と、上記算出初期値を初期値として、２Ｄ角度観測情報を事前に設定したバッチデータ数分蓄積してバッチ処理して距離推定値および距離変化率推定値を算出する距離算出バッチ処理手段を備える。
【解決手段】センサによる２Ｄ角度観測情報に基づいて２Ｄ角度航跡を算出する２Ｄ角度追尾手段と、２Ｄ角度観測情報と２Ｄ角度航跡について事前設定された圧縮間隔に基づいて最小２乗統合により圧縮処理して統合２Ｄ角度観測情報を算出する２Ｄ角度観測情報圧縮手段と、３点の統合２Ｄ角度観測情報に基づいて距離推定値初期値および距離変化率推定値初期値を算出する距離推定値初期値算出手段と、上記算出初期値を初期値として、２Ｄ角度観測情報を事前に設定したバッチデータ数分蓄積してバッチ処理して距離推定値および距離変化率推定値を算出する距離算出バッチ処理手段を備える。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、例えば２次元センサを用いて観測した飛翔体（以下、目標と呼ぶ）の角度観測値から距離を高精度に推定するセンサ情報融合装置に関するものである。
【背景技術】
【０００２】
センサで観測した飛翔体（以下、目標と呼ぶ）の角度情報のみを用いてその目標の軌道を推定する技術がある（例えば非特許文献１および非特許文献２参照）。これらの文献に開示された技術を適用して、角度観測値から直交座標の位置の推定を行う、あるいは角度観測値から距離の推定を行う従来の方法について説明する。
なお、以下の説明では、この発明も含めて、２次元の角度観測値から３次元の直交座標の位置を推定することを例にするが、１次元の角度観測値から２次元の直交座標の位置を推定する場合も含む。また、以降、特に断りのない限り、１Ｄは１次元、２Ｄは２次元、３Ｄは３次元を表すこととし、１Ｄセンサは仰角または方位角を観測するセンサ、２Ｄセンサは仰角および方位角を観測するセンサ、３Ｄセンサは距離、仰角および方位角を観測するセンサとする。そして、１Ｄセンサで観測する仰角または方位角をまとめて１Ｄ角度観測値、２Ｄセンサで観測する仰角および方位角をまとめて２Ｄ角度観測値、２Ｄセンサで観測する距離と仰角、あるいは距離と方位角をまとめて２Ｄ観測値、３Ｄセンサで観測する距離、仰角および方位角をまとめて３Ｄ観測値と呼ぶこととする。
【０００３】
図１０は２Ｄセンサから得られる２Ｄ角度観測値を用いて、距離を推定する従来方法のイメージを表し、図１１はこの推定処理を行う従来のセンサ情報融合装置の機能構成を示す。
２Ｄセンサ１０により目標を観測して３点の２Ｄ角度観測情報（角度観測値、観測誤差分散）を取得する。距離初期値算出手段４０は、例えば非特許文献１に関する技術を適用したもので、２Ｄセンサ１０で取得した２Ｄ角度観測情報に基づいて距離推定値初期値および距離変化率推定値初期値の算出を行う。この距離推定値初期値を算出するイメージを図１０（ａ）に示す。距離算出バッチ処理手段５０では、２Ｄセンサ１０で観測した１点以上の複数の２Ｄ角度観測値を蓄積し、バッチ処理により、目標の距離推定値および距離変化率推定値を算出し出力する。この場合、距離算出バッチ処理手段５０は、バッチ処理の初期値として、距離初期値算出手段４０で３点の角度観測値から求めた距離推定値および距離変化率推定値初期値を用いる。このバッチ処理のイメージは図１０（ｂ）に示されるようになる。
【０００４】
さて、上記処理において、２Ｄセンサ１０から、２Ｄ角度観測値がある決まった送信レートで得られた場合、距離初期値算出手段４０は２Ｄセンサ１０の送信レートのままで、距離推定値初期値および距離変化率推定値初期値を算出する。そして、距離算出バッチ処理手段５０は、同様に、２Ｄセンサ１０の送信レートのままで、距離推定値および距離変化率初期値をバッチ処理により算出することが想定される。ここで、距離推定値初期値および距離変化率推定値初期値を算出する際、２Ｄセンサ１０の送信レート、つまり２Ｄセンサ１０から得る２Ｄ角度観測値のサンプリングレートの高低によっては距離推定値および距離変化率推定値の推定精度に影響を及ぼす場合がある。このケースを図１２により説明すると、２Ｄ角度観測値の観測精度が悪い場合の２Ｄ角度観測値は、観測精度が良い場合に比べてばらついている。このようなとき、高サンプリングの３点の２Ｄ角度観測値で距離推定値および距離変化率推定値を算出すると、低サンプリングの３点の２Ｄ角度観測値で距離推定値および距離変化率推定値を算出する場合に比べ、距離推定値および距離変化率推定値が距離真値および距離変化率真値からかけ離れている。すなわち、低サンプリングレートで得られる場合の方が、距離推定値および距離変化率推定値の推定精度が良くなっている。
【０００５】
【非特許文献１】S. Blackman and R. Popoli, “Design and Analysis of Modern Tracking Systems”, Chapter 5, Section 5.2, Attech House, 1999.
【非特許文献２】C.B.Chang, “Ballistic Trajectory Estimation with Angle-Only Measurements”, IEEE TRANSACTIONS ON AUTOMATIC CONTROL, AC, vol.AC-25, JUNE 1980
【発明の開示】
【発明が解決しようとする課題】
【０００６】
従来のセンサ情報融合装置では、以上のように角度情報から距離を推定する際の距離推定値初期値算出の際、センサからの角度情報の入力タイミングのままで距離推定値初期値を計算しているので、高サンプリングレートで得た３点の２Ｄ角度観測値により、距離推定値初期値および距離変化率推定値初期値を算出した場合、距離推定値初期値が劣化し、その結果、２Ｄ角度観測値のバッチ処理による距離推定値および距離変化率推定値の算出結果も劣化するといった問題がある。
【０００７】
この発明は、上記問題点を解決するためになされたもので、２Ｄ角度観測値を用いた距離推定値初期値と距離変化率推定値初期値の高精度化、並びに２Ｄ角度観測値を用いたバッチ処理後の距離推定値および距離変化率推定値の高精度化を可能にするセンサ情報融合装置を得ることを目的とする。
【課題を解決するための手段】
【０００８】
この発明に係るセンサ情報融合装置は、センサにより観測した２Ｄ角度観測値と２Ｄ角度観測誤差分散からなる２Ｄ角度観測情報に基づいて２Ｄ角度航跡を算出する２Ｄ角度追尾手段と、２Ｄ角度観測情報と２Ｄ角度航跡について事前設定された圧縮間隔に基づいて最小２乗統合することにより、２Ｄ角度観測情報を圧縮処理した統合２Ｄ角度観測情報を算出する２Ｄ角度観測情報圧縮手段と、算出された３点の統合２Ｄ角度観測情報に基づいて距離推定値初期値と距離変化率推定値初期値を算出する距離推定値初期値算出手段と、距離推定値初期値と前記距離変化率推定値初期値を初期値として、２Ｄ角度観測情報を事前に設定したバッチデータ数分蓄積してバッチ処理することにより、距離推定値および距離変化率推定値を算出する距離算出バッチ処理手段を備えたものである。
【発明の効果】
【０００９】
この発明によれば、高サンプリングレートで得られる２Ｄ角度観測情報を間引いて捨てて、あたかも低サンプリングレートで２Ｄ角度観測情報が得られる状況にすることにより、距離推定値初期値および距離変化率推定値初期値の精度を向上させることができる。加えて、単に２Ｄ角度観測情報を捨てるだけではなく、２Ｄ角度観測値を最小２乗統合によりまとめた統合２Ｄ角度観測情報を用いており、統合２Ｄ角度観測情報は２Ｄ角度観測情報に比べて精度が良いから、この面においても、算出された距離推定値初期値および距離変化率初期値の精度を向上させることができる。したがって、最終的に、２Ｄ角度観測値を用いた３Ｄ位置の高精度化が可能となる。
【発明を実施するための最良の形態】
【００１０】
実施の形態１．
図１はこの発明の実施の形態１によるセンサ情報融合装置の機能構成を示すブロック図である。
図１において、センサ情報融合装置は、２Ｄセンサ１０、２Ｄ角度追尾手段２０、２Ｄ角度観測情報圧縮手段３０、距離推定値初期値算出手段４０および距離算出バッチ処理手段５０を備えている。
２Ｄセンサ１０では、目標をパッシブまたはアクティブに観測して、仰角、方位角の角度観測値、つまり２Ｄ角度観測値と、この２Ｄ角度観測値の観測誤差分散を生成し、この２Ｄ角度観測値および２Ｄ角度観測誤差分散からなる２Ｄ角度観測情報を出力する。２Ｄセンサ１０には、例えばレーダ、ＩＲ（InfraRed）センサ等が使用される。レーダの場合は、周知のように、パッシブに角度を観測するもの、アクティブに距離、角度を観測するものである。しかし、距離を観測するレーダは、距離の精度が悪いため、距離を有効な観測情報として使えない場合や距離が妨害波により有効な観測情報として使えない場合が多いため、ここでは角度のみ使用する。また、２Ｄセンサ１０としては、複数台のセンサを使用する場合を含むものとする。
【００１１】
２Ｄ角度追尾手段２０では、２Ｄセンサ１０から得られた２Ｄ角度観測情報から、追尾フィルタを用いて仰角、仰角速度、仰角加速度、方位角、方位角速度および方位角加速度の６変数からなる状態量を推定し、その推定結果を２Ｄ角度航跡として出力する。ここで、２Ｄ角度航跡の平滑位置は２Ｄ角度航跡の仰角、方位角成分とし、２Ｄ角度航跡の平滑速度は２Ｄ角度航跡の仰角速度、方位角度速度成分とし、また、２Ｄ角度航跡の平滑加速度は２Ｄ角度航跡の仰角角加速度、方位角加速度とする。
【００１２】
２Ｄ角度観測情報圧縮手段３０では、２Ｄセンサ１０から得られる２Ｄ角度観測情報と２Ｄ角度追尾手段２０から得られる２Ｄ角度航跡について事前設定された圧縮間隔に基づいて最小２乗統合することにより、２Ｄ角度観測情報を圧縮処理した統合２Ｄ角度観測情報を算出する。この２Ｄ角度観測情報圧縮手段３０の処理方法について、以下図２を用いて説明する。
図２において、圧縮前の２Ｄ角度観測値が時刻ｔk-1 から時刻ｔk までの処理フレーム間でｎ個得られているとする。そして、その処理フレーム間における時刻ｔk,i と時刻ｔk の間隔をΔｔki＝（ｔk −ｔk,i ）とする。ここで、処理フレームの長さは事前設定する圧縮間隔Ｔｉｎｔにより決まるものとする。
それから、時刻ｔk,i （ｉ≦ｎ，ｎ≧１）における観測ベクトルを
【数１】
と定義し、観測雑音共分散行列をＢk,i とする。ここで、観測ベクトルは２Ｄ角度観測値に対応し、観測誤差共分散行列は２Ｄ角度観測値の観測誤差分散に対応し、それぞれ２Ｄ角度観測情報から得られるものである。
【００１３】
また、２Ｄ角度追尾手段２０で得られる２Ｄ角度航跡から、状態ベクトル
【数２】
が得られることから、ｔk-1 から時刻ｔk における速度ベクトル
【数３】
が得られる。ここで、ベクトルおよび行列の右上添字Ｔは、代数学における行列およびベクトルの転置を表す。
そうすると、上記速度ベクトル
【数４】
および観測行列Ｈ＝［Ｉ2×2 Ｏ2×2 Ｏ2×2 ］を用いて、時刻ｔk における観測ベクトル（圧縮前の２Ｄ角度観測値）は、（３）式のようにモデル化できる。
【数５】
ここで、時刻ｔk の観測雑音ベクトルを
【数６】
【００１４】
上記観測雑音ベクトルは、平均零ベクトル、分散Ｂk,i の白色正規分布に従う。Ｂk,i は２Ｄ角度観測値の誤差共分散行列である。また、Ｉ2×2 は２行２列の単位行列、そして、Ｏ2×2 は２行２列の零行列である。
そして、時刻ｔk-1 から時刻ｔk までのｎ個の観測ベクトルを、最小２乗統合により圧縮し、時刻ｔk における統合２Ｄ角度観測値を、（４）式により算出する。
【数７】
また、（４）式の統合２Ｄ角度観測値の誤差共分散行列も（５）式として同時に算出する。
【数８】
２Ｄ角度観測値圧縮手段３０では、上記処理フレーム内の１つ以上の２Ｄ角度観測値を上記（４）式により、１つの統合２Ｄ角度観測値にまとめる。そして、２Ｄ角度観測値の誤差共分散行列Ｂk,i を上記（５）式により、１つの統合２Ｄ角度観測値の誤差共分散行列にまとめる。
【００１５】
２Ｄ角度観測値圧縮手段３０は、以上のようにして得られた、統合２Ｄ角度観測値および統合２Ｄ角度観測値の誤差共分散行列を、統合２Ｄ角度観測情報として出力する。
また、２Ｄ角度観測値圧縮手段３０では、統合２Ｄ角度観測値の算出の際、処理フレーム内の観測値を１つ選択して、そのときの２Ｄ角度観測値と２Ｄ角度観測値の誤差共分散行列を、統合２Ｄ角度観測情報として出力する。
ここで、最小２乗統合により２Ｄ角度観測情報を圧縮したものを統合２Ｄ角度観測情報として出力するか、または処理フレーム内の観測情報を１つ選択したものを統合２Ｄ角度観測情報で出力するかは、事前に設定する。
【００１６】
距離推定値初期値算出手段４０では、２Ｄ角度観測値圧縮手段３０から入力される統合２Ｄ角度観測情報から、３点の統合２Ｄ角度観測情報に基づいて距離推定値初期値および距離変化率推定値初期値を算出する。
距離算出バッチ処理手段５０では、距離推定値初期値算出手段４０により算出された距離推定値初期値と距離変化率初期値を、距離算出バッチ処理の初期値として設定を行う。そして、距離算出バッチ処理初期値の設定後は、２Ｄセンサ１０から得られる２Ｄ角度観測情報を、事前に設定したバッチデータ数分蓄積してバッチ処理することにより、距離推定値および距離変化率推定値を算出して出力する。
【００１７】
以上のように、この実施の形態１によれば、図１２に示したような高サンプリングレートで得られる２Ｄ角度観測情報を間引いて捨てて、あたかも低サンプリングレートで２Ｄ角度観測情報が得られる状況にすることにより、距離推定値初期値および距離変化率推定値初期値の精度を向上させることができる。加えて、単に２Ｄ角度観測情報を捨てるだけではなく、２Ｄ角度観測値を最小２乗統合によりまとめた統合２Ｄ角度観測情報を用いており、統合２Ｄ角度観測情報は２Ｄ角度観測情報に比べて精度が良いから、この面においても算出された距離推定値初期値および距離変化率初期値の精度を向上させることができる。
【００１８】
実施の形態２．
図３はこの発明の実施の形態２によるセンサ情報融合装置の機能構成を示すブロック図である。図において、実施の形態１の図１に相当する部分には同一符号を付し、その説明は原則として省略する。この実施の形態２の場合は、２Ｄ角度観測情報選択手段６０を設けた点が実施の形態１の構成と異なっている。
２Ｄ角度観測情報選択手段６０では、２Ｄ角度追尾手段２０から得られる２Ｄ角度航跡の平滑位置と、２Ｄセンサ１０から得られる２Ｄ角度観測情報の２Ｄ角度観測値の残差を取り、その残差ベクトルのノルムが事前に決めたしきい値（残差ベクトルのノルムのしきい値と呼ぶ）よりも小さくなる２Ｄ角度観測値を選択する。この算出された選択後２Ｄ角度観測値に紐付く２Ｄ角度観測情報が、上記実施の形態１の場合の２Ｄセンサ１０で得られた２Ｄ角度観測情報と２Ｄ角度追尾手段２０で得られた２Ｄ角度航跡に代えて、２Ｄ角度観測情報圧縮手段３０に入力される。したがって、２Ｄ角度観測情報圧縮手段３０では、選択後２Ｄ角度観測値に紐付く選択後２Ｄ角度観測情報について事前に設定した圧縮間隔に基づいて圧縮処理した統合２Ｄ角度観測情報を算出する。
なお、上記残差ベクトルのノルムは、代数学におけるユークリッドノルムに基づくノルムでも良いし、マハラノビス距離に基づくノルムでも良い。
【００１９】
以上のように、この実施の形態２によれば、実施の形態１の場合の２Ｄセンサ１０から２Ｄ角度観測情報圧縮手段３０に入力する前の２Ｄ角度観測情報について、２Ｄ角度航跡に基づいて残差を考慮して選択し選択後２Ｄ角度観測情報として得、この選択後２Ｄ角度観測情報について２Ｄ角度観測情報圧縮手段３０で圧縮処理しているので、元の２Ｄ角度観測情報の異常値については排除することができる。結果として、距離推定値初期値および距離変化率推定値初期値の精度を向上させることが可能となる。
【００２０】
実施の形態３．
図４は、この発明の実施の形態３によるセンサ情報融合装置の機能構成を示すブロック図である。図において、実施の形態２の図３に相当する部分には同一符号を付し、その説明は原則として省略する。この実施の形態３の場合は、統合２Ｄ角度観測情報選択手段７０を設けた点が実施の形態２の構成と異なっている。
統合２Ｄ角度観測値選択手段７０では、２Ｄ角度追尾手段２０で得られた２Ｄ角度航跡の平滑位置と、２Ｄ角度観測情報圧縮手段３０で得られた統合２Ｄ角度観測情報の統合２Ｄ角度観測値の残差を取り、その残差ベクトルのノルムが事前に決めたしきい値よりも小さくなる統合２Ｄ角度観測値を選択する。この算出された選択後統合２Ｄ角度観測値に紐付く統合２Ｄ角度観測情報は、上記実施の形態２の場合の２Ｄ角度観測情報圧縮手段３０からの統合２Ｄ角度観測情報に代えて、距離推定値初期値算出手段４０に入力される。したがって、距離推定値初期値算出手段４０では、選択後統合２Ｄ角度観測情報を用いて、３点の統合２Ｄ角度観測情報から距離推定値初期値および距離変化率推定値初期値を算出することになる。
なお、上記残差ベクトルのノルムは、代数学におけるユークリッドノルムに基づくノルムでも良いし、マハラノビス距離に基づくノルムでも良い。
【００２１】
以上のように、この実施の形態３によれば、距離推定値初期値算出の処理前に、統合２Ｄ角度観測情報を２Ｄ角度航跡に基づいて選択するようにしたので、統合２Ｄ角度観測情報の異常値については排除することができる。結果として、距離推定値初期値および距離変化率推定値初期値の精度を向上させることができる。
【００２２】
実施の形態４．
目標の旋回性、機動性が大きい場合は、２Ｄ角度観測情報圧縮手段３０で圧縮処理を実施すると、統合２Ｄ角度観測情報の精度が悪くなることが想定される。目標の旋回性、機動性の大きさは、２Ｄ角度航跡の速度の大きさおよび加速度の大きさとなって現れる。この実施の形態４では、２Ｄ角度航跡の速度および加速度に基づいて目標の旋回性、機動性の大きさを判断し、上記目標の旋回性、機動性により起こる統合２Ｄ角度観測情報の精度の劣化を低減する手段について説明する。
【００２３】
図５は、この発明の実施の形態４によるセンサ情報融合装置の機能構成を示すブロック図である。図において、実施の形態１の図１に相当する部分には同一符号を付し、その説明は原則として省略する。この実施の形態４の場合は、圧縮間隔制御手段８０を設けた点が実施の形態１の構成と異なっている。
圧縮間隔制御手段８０には、２Ｄ角度追尾手段２０から得られる２Ｄ角度航跡
が入力される。また、圧縮間隔制御手段８０は、２Ｄ角度航跡の速度の大きさおよび加速度の大きさの２つの要素のしきい値とそれに対応して設定する圧縮間隔からなる圧縮間隔テーブルを事前に準備して保有している。
圧縮間隔制御手段８０では、２Ｄ角度追尾手段２０から入力された２Ｄ角度航跡の速度の大きさまたは加速度の大きさが、圧縮間隔テーブルで決めてある速度の大きさのしきい値および加速度の大きさのしきい値よりも大きい場合、目標の機動性、旋回性が有ると判断し、既設定してある圧縮間隔をそれよりも小さな圧縮間隔に変更設定して制御信号として２Ｄ角度観測情報圧縮手段３０に出力する。
【００２４】
圧縮間隔をどの程度に設定するかは、圧縮間隔テーブルに基づいて決定することになるが、このテーブルには、例えば圧縮間隔を２Ｄセンサの送信レートと同じにするといった内容も入れておく。設定する圧縮間隔を２Ｄセンサの送信レートと同じにするということは、圧縮をしないということと同じである。このように圧縮間隔テーブルにより変更設定した圧縮間隔と２Ｄセンサの送信レートが同じ場合は、圧縮間隔制御手段８０は、圧縮間隔と２Ｄ角度観測情報圧縮処理を行わないといった制御信号を２Ｄ角度観測情報圧縮手段３０に出力する。一方、圧縮間隔テーブルにより変更設定した圧縮間隔と２Ｄセンサの送信レートが異なる場合は、その圧縮間隔と２Ｄ角度観測情報圧縮処理を行うといった制御信号を２Ｄ角度観測情報圧縮手段３０に出力することになる。
【００２５】
以上のように、この実施の形態４によれば、圧縮間隔制御手段８０により、２Ｄ角度航跡の速度の大きさまたは加速度の大きさを、目標の機動性、旋回性の有無を判断する事前に決めたしきい値と比較し、比較結果に応じて、２Ｄ角度観測情報圧縮手段３０が用いる既設定された圧縮間隔を変更制御するようにしている。
したがって、目標の旋回性、機動性が大きい場合には小さな圧縮間隔に変更設定するため、統合２Ｄ角度観測情報の精度の劣化を低減できる。結果として、距離推定値初期値および距離変化率推定値初期値の精度の劣化も低減できる。
【００２６】
実施の形態５．
図３の距離算出バッチ処理手段５０において、２Ｄ角度観測情報に異常値が含まれていると、算出する距離推定値および距離変化率推定値の精度が悪化することになる。この実施の形態４では、この問題を解決する手段について説明する。
図６は、この発明の実施の形態４によるセンサ情報融合装置の機能構成を示すブロック図である。図において、実施の形態２の図３に相当する部分には同一符号を付し、その説明は原則として省略する。この実施の形態５の場合は、距離推定値選択手段９０を設けた点が実施の形態２の構成と異なっている。
距離推定値選択手段９０には、２Ｄ角度観測情報の異常値を用いて距離推定値および距離変化率推定値を算出しているとみなすための事前に設定した距離推定値しきい値および距離変化率推定値しきい値が事前に設定されている。そして、距離推定値選択手段９０では、距離算出バッチ処理手段５０で算出された距離推定値および距離変化率推定値が、事前に設定した距離推定値しきい値および距離変化率推定値しきい値以下の場合は、妥当な選択後距離推定値および距離変化率推定値として出力する。
【００２７】
一方、距離推定値および距離変化率推定値が、事前に設定した距離推定値しきい値および距離変化率推定値しきい値よりも大きい場合は、選択後距離推定値および距離変化率推定値として出力されない。また、距離推定値選択手段９０は、このとき、再選択の制御信号を２Ｄ角度観測情報選択手段６０に与える。２Ｄ角度観測情報選択手段６０では、距離推定値選択手段９０から再選択の制御信号が入力された場合には、その制御信号に基づいて、２Ｄ角度航跡と２Ｄ角度観測情報の残差ベクトルのノルムのしきい値を既設値よりも小さい値に変更設定する。このことによって、距離推定値初期値および距離変化率推定値初期値を求めるために用いる２Ｄ角度観測情報の異常値も排除される。
【００２８】
以上のように、この実施の形態５によれば、２Ｄ角度観測情報選択手段９０において、距離算出バッチ処理手段５０で算出された距離推定値と距離変化率推定値を、２Ｄ角度観測情報の異常値を用いて算出していると判断する事前設定のしきい値と比較し、当該比較結果に応じて前記距離推定値と距離変化率推定値を出力するかどうかを選択し、出力しない場合には２Ｄ角度観測情報選択手段６０における２Ｄ角度航跡と２Ｄ角度観測情報の残差の値を判定するしきい値を既設値よりも小さい値に変更設定するよう制御するようにしている。したがって、２Ｄ角度観測情報の異常値を使って算出された距離推定値および距離変化率推定値は使わないようにし、また、距離推定値初期値および距離変化率推定値初期値の精度も向上させることができる。
【００２９】
実施の形態６．
図７は、この発明の実施の形態６によるセンサ情報融合装置の機能構成を示すブロック図である。図において、実施の形態１の図１に相当する部分には同一符号を付し、その説明は原則として省略する。この実施の形態６の場合は、距離推定値平均手段１００を設けた点が実施の形態１の構成と異なっている。
距離推定値平均手段１００では、２Ｄ角度追尾手段２０で得られた２Ｄ角度航跡と、距離推定値初期値算出手段４０で得られた距離推定値初期値および距離変化率推定値初期値を用いて、一旦３Ｄ位置・速度情報に変換する。次に、変換して得られた３Ｄ位置・速度情報を、時系列にＭサンプリング分移動平均して、平均化した３Ｄ位置・速度情報を算出する。この平均化した３Ｄ位置・速度情報と２Ｄ角度航跡を用いて、距離推定値初期値と距離変化率推定値初期値を算出する。
【００３０】
ここで、各時刻の３Ｄ位置・速度情報を時系列にＭサンプリング分移動平均する場合は、事前に仮定した軌道モデルにより、ある時刻に合わせて外挿を行う。例えば、時刻ｋの３Ｄ位置・速度情報をＸ（ｋ）、時刻ｋ＋１の３Ｄ位置・速度情報をＸ（ｋ＋１）とした場合に、時刻ｋ＋１を平均化した３Ｄ位置・速度を求めることを考える。この場合、時刻ｋの３Ｄ位置・速度情報Ｘ（ｋ）と時刻ｋ＋１の３Ｄ位置・速度情報Ｘ（ｋ＋１）の平均を算出する場合は、事前に仮定した軌道モデルを線形１次近似するなどして得た、制御工学における時刻ｋから時刻ｋ＋１への状態遷移行列Ｆを用いて、時刻ｋの３Ｄ位置・速度情報に時刻ｋから時刻ｋ＋１への状態遷移行列Ｆをかけることにより、時刻合わせをしたＦ・Ｘ（ｋ）を算出する。そして、Ｆ・Ｘ（ｋ）とＸ（ｋ＋１）の平均をとったものを平均化した３Ｄ位置・速度情報とする。
この平均化した３Ｄ位置・速度情報と２Ｄ角度航跡を用いて、距離推定値と距離変化率推定値を逆算する。この逆算したものを、それぞれ、平均処理後距離推定値初期値および平均処理後距離変化率推定値初期値と呼ぶ。そして、この平均処理後距離推定初期値と平均処理後距離変化率推定値初期値を、距離算出バッチ処理手段５０に入力する。距離算出バッチ処理手段５０では、距離推定値初期値と距離変化率推定値初期値を初期値として２Ｄ角度観測情報のバッチ処理を行う。
【００３１】
以上のように、この実施の形態６によれば、距離推定値初期値および距離変化率推定値初期値を移動平均することにより、距離推定値初期値および距離変化率推定値初期値の安定化が図れるため、結果として、距離推定値および距離変化率推定値の精度が向上する。
【００３２】
実施の形態７．
図１の距離算出バッチ処理手段５０において、バッチ処理データ数が多ければ多い程、距離推定値および距離変化率推定値の精度は良くなる。しかし、バッチ処理データ数がある値を超えた場合は、距離推定値および距離変化率推定値の推定精度はさほど変わらなくなる筈である。このようなケースでは、バッチ処理データ数が不当に多い場合、バッチ処理計算結果である距離推定値および距離変化率推定値の精度は良くなるものの、バッチ処理計算の演算負荷が高くなる。逆に、バッチ処理データ数が少なければ、バッチ処理計算の演算負荷は低くなるが、距離推定値および距離変化率推定値としては十分な精度は得られなくなる。また、２Ｄセンサの観測精度、サンプリングレートによって、最適なバッチ処理データ数は異なる。この実施の形態７では、バッチ処理の推定精度の高精度化と演算量の低減を両立させる手段について説明する。
【００３３】
図８は、この発明の実施の形態７によるセンサ情報融合装置の機能構成を示すブロック図である。図において、実施の形態１の図１に相当する部分には同一符号を付し、その説明は原則として省略する。この実施の形態７の場合は、バッチ処理データ数制御ＤＢ（データベース）１１０、バッチ処理データ数制御手段１２０を設けた点が実施の形態１の構成と異なっている。
バッチ処理データ数制御ＤＢ１１０には、距離および距離推定値の推定精度とバッチ処理計算の演算量に関連するデータ、すなわち２Ｄ角度観測情報、サンプリングレート、観測精度、距離および距離変化率、２Ｄ角度航跡、バッチ処理データ数などを含むテーブルを事前に作成し、データベース化して保管しておく。バッチ処理データ数制御手段１２０では、２Ｄセンサ１０で得られる２Ｄ角度観測情報、この２Ｄ角度観測情報を得るサンプリングレートおよび観測精度、距離算出バッチ処理手段５０で得られる距離推定値および距離変化率推定値、さらに２Ｄ角度追尾手段２０で得られる２Ｄ角度航跡を検索キーとして、バッチ処理データ数制御ＤＢ１１０を参照してバッチ処理データ数を得る。そして、検索したバッチ処理データ数を距離算出バッチ処理手段５０に設定する。
【００３４】
以上のように、この実施の形態７によれば、バッチ処理データ数制御手段１２０により、２Ｄセンサ１０の２Ｄ角度観測情報、そして２Ｄセンサ１０の観測精度、サンプリングレート、２Ｄ角度航跡および距離推定値および距離変化率推定値を検索キーとして、バッチ処理データ数制御ＤＢ１１０から最適なバッチ処理データ数を検索して距離算出バッチ処理手段５０に設定するようにしたので、バッチ処理の推定精度の高精度化と演算量の低減の両立化を可能にする。
【００３５】
実施の形態８．
図９は、この発明の実施の形態８によるセンサ情報融合装置の機能構成を示すブロック図である。図において、実施の形態１の図１に相当する部分には同一符号を付し、その説明は原則として省略する。この実施の形態７場合は、擬似３Ｄ観測情報生成手段１３０、３Ｄセンサ１４０、観測モデル選択手段１５０、３Ｄ追尾手段１６０を設けた点が実施の形態１の構成と異なっている。
擬似３Ｄ観測情報生成手段１３０では、距離算出バッチ処理手段５０で得られた距離推定値と２Ｄセンサ１０で得られた２Ｄ角度観測情報から、距離推定値と仰角観測値と方位角観測値からなる組を擬似３Ｄ観測値として生成する。ここで、距離推定値に対応する距離観測誤差分散を事前設定しておく。ここでは、距離推定値を擬似距離観測値と呼び、この擬似距離に対応する距離観測誤差分散を擬似距離観測誤差分散と呼ぶことにする。擬似３Ｄ観測情報生成手段１３０では、擬似距離観測誤差分散と、２Ｄ角度観測情報から得られる仰角観測誤差分散、方位角度観測誤差分散からなる組である、擬似３Ｄ観測誤差分散を生成する。最終的に、擬似３Ｄ観測情報生成手段１３０は、擬似３Ｄ観測値と擬似３Ｄ観測誤差分散をペアとした擬似３Ｄ観測情報を出力する。
【００３６】
３Ｄセンサ１４０では、目標を観測することにより、距離、仰角、方位角からなる３Ｄ観測値と、距離観測誤差分散、仰角観測誤差分散、方位角観測誤差分散からなる３Ｄ観測誤差分散を取得して、３Ｄ観測値と３Ｄ観測誤差分散をペアとした３Ｄ観測情報を出力する。
２Ｄセンサ１０からの２Ｄ角度観測情報、擬似３Ｄ観測情報生成手段１３０からの擬似３Ｄ観測情報、３Ｄセンサ１４０からの３Ｄ観測情報は、観測モデル選択手段１５０に入力される。観測モデル選択手段１５０では、これらの入力される観測情報の種類に応じて、制御工学における観測モデルを制御する信号（観測モデル制御信号）を生成する。例えば、２Ｄ角度観測情報が入力された場合は、３Ｄ追尾手段１６０で２Ｄ角度観測情報用の観測モデルを用いるような観測モデル制御信号を生成する。また、３Ｄ観測情報または擬似３Ｄ観測情報が入力された場合も同様で、各観測情報に応じた観測モデルを用いるような観測モデル制御信号を生成する。よって、観測モデル選択手段１５０では、生成したいずれかの観測モデル制御信号と、当該制御信号に対応した２Ｄ角度観測情報、３Ｄ観測情報、擬似３Ｄ観測情報を３Ｄ追尾手段１６０に出力する。
【００３７】
３Ｄ追尾手段１６０では、観測モデル選択手段１５０から入力される観測モデル制御信号と、当該観測モデル制御信号に対応した２Ｄ角度観測情報、３Ｄ観測情報、擬似３Ｄ観測情報に基づいて３Ｄ追尾処理を行う。
ここで、３Ｄ観測情報が得られる場合の観測モデル（観測ベクトル）は、（６）式のようになる。
【数９】
とする。観測ベクトルは、３Ｄ観測情報の３Ｄ観測値を極座標から直交座標に座標変換することにより求める。
【数１０】
また、（６）式におけるＨは、観測行列であり、Ｈ＝［Ｉ3×3 Ｏ3×3 ]である。この観測行列はカルマンフィルタにおけるデータ更新処理等に用いる。ここで、Ｉ3×3 は３行３列の単位行列、Ｏ3×3 は３行３列の零行列である。
また、
【数１１】
は距離、仰角、方位角を成分に持つ観測雑音ベクトルであり、観測雑音ベクトルの共分散行列の対角成分が、３Ｄ観測情報の距離観測誤差分散σＲk2、仰角観測誤差分散σＥk2、方位角観測誤差分散σＡｚk2にそれぞれ対応する。具体的には、観測雑音ベクトルの共分散行列をＢk とすると、（９）式で表される。
Ｂk ＝ｄｉａｇ［σＲk2，σＥk2，σＡｚk2］ （９）
ここで、ｄｉａｇ（ａ，ｂ，ｃ）は、ａ，ｂ，ｃを対角成分に持つ対角行列を表す。
さらに、Ａk は極座標から直交座標への座標変換行列を表し、直交座標（ｘ，ｙ，ｚ）を極座標（Ｒ，Ｅ，Ａｚ）で線形１次近似したヤコビアン行列（１０）式で得られる。
【数１２】
【００３８】
また、擬似３Ｄ観測情報が得られる場合の観測モデルも、（６）式で定義する。
但し、この場合の観測雑音ベクトルの共分散行列Ｂk は（１１）式のように定義する。
【数１３】
【００３９】
一方、２Ｄ角度観測情報が得られる場合の観測モデル（観測ベクトル）は、（１２）式となる。
【数１４】
また、２Ｄ角度情報が得られる場合の観測行列はＨk ＝［Ａk Ｏ3×3 ］である。この観測行列はカルマンフィルタにおけるデータ更新処理等に用いる。また、Ａk は（１４）式で算出される。
【数１５】
Ａk は極座標（Ｅ，Ａｚ）を直交座標（ｘ，ｙ，ｚ）で線形１次近似したヤコビアン行列で得られる。
また、（１１）式の
【数１６】
は仰角、方位角を成分に持つ観測雑音ベクトルであり、観測雑音ベクトルの共分散行列の対角成分が、２Ｄ角度観測情報の仰角観測誤差分散σＥk2、方位角観測誤差分散σＡＥk2にそれぞれ対応する。具体的には、観測雑音ベクトルの共分散行列をＢk とすると、（１５）式のようになる。
Ｂk ＝ｄｉａｇ［σＥk2，σＡｚk2］ （１５）
【００４０】
３Ｄ追尾手段１６０は、３Ｄ観測情報、擬似３Ｄ観測情報または２Ｄ角度観測情報に応じた、以上の（６）式または（１２）式で定義したような観測モデルが入力されると、その観測モデルに基づいて３Ｄ追尾を行い、（８）式で表される状態ベクトルの成分を持つ３Ｄ航跡を出力する。
【００４１】
以上のように、実施の形態８によれば、入力される観測情報に応じた観測モデルを選択して３Ｄ追尾を行うようにしたので、３Ｄ航跡の収束を早くすることができる。
【図面の簡単な説明】
【００４２】
【図１】この発明の実施の形態１によるセンサ情報融合装置の機能構成を示すブロック図である。
【図２】この発明の実施の形態１に係る２Ｄ角度観測情報圧縮手段の処理方法を示す説明図である。
【図３】この発明の実施の形態２によるセンサ情報融合装置の機能構成を示すブロック図である。
【図４】この発明の実施の形態３によるセンサ情報融合装置の機能構成を示すブロック図である。
【図５】この発明の実施の形態４によるセンサ情報融合装置の機能構成を示すブロック図である。
【図６】この発明の実施の形態５によるセンサ情報融合装置の機能構成を示すブロック図である。
【図７】この発明の実施の形態６によるセンサ情報融合装置の機能構成を示すブロック図である。
【図８】この発明の実施の形態７によるセンサ情報融合装置の機能構成を示すブロック図である。
【図９】この発明の実施の形態８によるセンサ情報融合装置の機能構成を示すブロック図である。
【図１０】角度観測値から直交座標の位置の推定を行う従来方法を示す説明図である。
【図１１】従来のセンサ情報融合装置の機能構成を示すブロック図である。
【図１２】２Ｄ角度観測値のサンプリングレートの高低に対応する距離推定値等の推定精度を表す説明図である。
【符号の説明】
【００４３】
１０ ２Ｄセンサ、２０ ２Ｄ角度追尾手段、３０ ２Ｄ角度観測情報圧縮手段、４０ 距離推定値初期値算出手段、５０ 距離算出バッチ処理手段、６０ ２Ｄ角度観測情報選択手段、７０ 統合２Ｄ角度観測情報選択手段、８０ 圧縮間隔制御手段、９０ 距離推定値選択手段、１００ 距離推定値平均手段、１１０ バッチ処理データ数制御ＤＢ、１２０ バッチ処理データ数制御処理、１３０ 擬似３Ｄ観測情報生成手段、１４０ ３Ｄセンサ、１５０ 観測モデル選択手段、１６０ ３Ｄ追尾手段。
【技術分野】
【０００１】
この発明は、例えば２次元センサを用いて観測した飛翔体（以下、目標と呼ぶ）の角度観測値から距離を高精度に推定するセンサ情報融合装置に関するものである。
【背景技術】
【０００２】
センサで観測した飛翔体（以下、目標と呼ぶ）の角度情報のみを用いてその目標の軌道を推定する技術がある（例えば非特許文献１および非特許文献２参照）。これらの文献に開示された技術を適用して、角度観測値から直交座標の位置の推定を行う、あるいは角度観測値から距離の推定を行う従来の方法について説明する。
なお、以下の説明では、この発明も含めて、２次元の角度観測値から３次元の直交座標の位置を推定することを例にするが、１次元の角度観測値から２次元の直交座標の位置を推定する場合も含む。また、以降、特に断りのない限り、１Ｄは１次元、２Ｄは２次元、３Ｄは３次元を表すこととし、１Ｄセンサは仰角または方位角を観測するセンサ、２Ｄセンサは仰角および方位角を観測するセンサ、３Ｄセンサは距離、仰角および方位角を観測するセンサとする。そして、１Ｄセンサで観測する仰角または方位角をまとめて１Ｄ角度観測値、２Ｄセンサで観測する仰角および方位角をまとめて２Ｄ角度観測値、２Ｄセンサで観測する距離と仰角、あるいは距離と方位角をまとめて２Ｄ観測値、３Ｄセンサで観測する距離、仰角および方位角をまとめて３Ｄ観測値と呼ぶこととする。
【０００３】
図１０は２Ｄセンサから得られる２Ｄ角度観測値を用いて、距離を推定する従来方法のイメージを表し、図１１はこの推定処理を行う従来のセンサ情報融合装置の機能構成を示す。
２Ｄセンサ１０により目標を観測して３点の２Ｄ角度観測情報（角度観測値、観測誤差分散）を取得する。距離初期値算出手段４０は、例えば非特許文献１に関する技術を適用したもので、２Ｄセンサ１０で取得した２Ｄ角度観測情報に基づいて距離推定値初期値および距離変化率推定値初期値の算出を行う。この距離推定値初期値を算出するイメージを図１０（ａ）に示す。距離算出バッチ処理手段５０では、２Ｄセンサ１０で観測した１点以上の複数の２Ｄ角度観測値を蓄積し、バッチ処理により、目標の距離推定値および距離変化率推定値を算出し出力する。この場合、距離算出バッチ処理手段５０は、バッチ処理の初期値として、距離初期値算出手段４０で３点の角度観測値から求めた距離推定値および距離変化率推定値初期値を用いる。このバッチ処理のイメージは図１０（ｂ）に示されるようになる。
【０００４】
さて、上記処理において、２Ｄセンサ１０から、２Ｄ角度観測値がある決まった送信レートで得られた場合、距離初期値算出手段４０は２Ｄセンサ１０の送信レートのままで、距離推定値初期値および距離変化率推定値初期値を算出する。そして、距離算出バッチ処理手段５０は、同様に、２Ｄセンサ１０の送信レートのままで、距離推定値および距離変化率初期値をバッチ処理により算出することが想定される。ここで、距離推定値初期値および距離変化率推定値初期値を算出する際、２Ｄセンサ１０の送信レート、つまり２Ｄセンサ１０から得る２Ｄ角度観測値のサンプリングレートの高低によっては距離推定値および距離変化率推定値の推定精度に影響を及ぼす場合がある。このケースを図１２により説明すると、２Ｄ角度観測値の観測精度が悪い場合の２Ｄ角度観測値は、観測精度が良い場合に比べてばらついている。このようなとき、高サンプリングの３点の２Ｄ角度観測値で距離推定値および距離変化率推定値を算出すると、低サンプリングの３点の２Ｄ角度観測値で距離推定値および距離変化率推定値を算出する場合に比べ、距離推定値および距離変化率推定値が距離真値および距離変化率真値からかけ離れている。すなわち、低サンプリングレートで得られる場合の方が、距離推定値および距離変化率推定値の推定精度が良くなっている。
【０００５】
【非特許文献１】S. Blackman and R. Popoli, “Design and Analysis of Modern Tracking Systems”, Chapter 5, Section 5.2, Attech House, 1999.
【非特許文献２】C.B.Chang, “Ballistic Trajectory Estimation with Angle-Only Measurements”, IEEE TRANSACTIONS ON AUTOMATIC CONTROL, AC, vol.AC-25, JUNE 1980
【発明の開示】
【発明が解決しようとする課題】
【０００６】
従来のセンサ情報融合装置では、以上のように角度情報から距離を推定する際の距離推定値初期値算出の際、センサからの角度情報の入力タイミングのままで距離推定値初期値を計算しているので、高サンプリングレートで得た３点の２Ｄ角度観測値により、距離推定値初期値および距離変化率推定値初期値を算出した場合、距離推定値初期値が劣化し、その結果、２Ｄ角度観測値のバッチ処理による距離推定値および距離変化率推定値の算出結果も劣化するといった問題がある。
【０００７】
この発明は、上記問題点を解決するためになされたもので、２Ｄ角度観測値を用いた距離推定値初期値と距離変化率推定値初期値の高精度化、並びに２Ｄ角度観測値を用いたバッチ処理後の距離推定値および距離変化率推定値の高精度化を可能にするセンサ情報融合装置を得ることを目的とする。
【課題を解決するための手段】
【０００８】
この発明に係るセンサ情報融合装置は、センサにより観測した２Ｄ角度観測値と２Ｄ角度観測誤差分散からなる２Ｄ角度観測情報に基づいて２Ｄ角度航跡を算出する２Ｄ角度追尾手段と、２Ｄ角度観測情報と２Ｄ角度航跡について事前設定された圧縮間隔に基づいて最小２乗統合することにより、２Ｄ角度観測情報を圧縮処理した統合２Ｄ角度観測情報を算出する２Ｄ角度観測情報圧縮手段と、算出された３点の統合２Ｄ角度観測情報に基づいて距離推定値初期値と距離変化率推定値初期値を算出する距離推定値初期値算出手段と、距離推定値初期値と前記距離変化率推定値初期値を初期値として、２Ｄ角度観測情報を事前に設定したバッチデータ数分蓄積してバッチ処理することにより、距離推定値および距離変化率推定値を算出する距離算出バッチ処理手段を備えたものである。
【発明の効果】
【０００９】
この発明によれば、高サンプリングレートで得られる２Ｄ角度観測情報を間引いて捨てて、あたかも低サンプリングレートで２Ｄ角度観測情報が得られる状況にすることにより、距離推定値初期値および距離変化率推定値初期値の精度を向上させることができる。加えて、単に２Ｄ角度観測情報を捨てるだけではなく、２Ｄ角度観測値を最小２乗統合によりまとめた統合２Ｄ角度観測情報を用いており、統合２Ｄ角度観測情報は２Ｄ角度観測情報に比べて精度が良いから、この面においても、算出された距離推定値初期値および距離変化率初期値の精度を向上させることができる。したがって、最終的に、２Ｄ角度観測値を用いた３Ｄ位置の高精度化が可能となる。
【発明を実施するための最良の形態】
【００１０】
実施の形態１．
図１はこの発明の実施の形態１によるセンサ情報融合装置の機能構成を示すブロック図である。
図１において、センサ情報融合装置は、２Ｄセンサ１０、２Ｄ角度追尾手段２０、２Ｄ角度観測情報圧縮手段３０、距離推定値初期値算出手段４０および距離算出バッチ処理手段５０を備えている。
２Ｄセンサ１０では、目標をパッシブまたはアクティブに観測して、仰角、方位角の角度観測値、つまり２Ｄ角度観測値と、この２Ｄ角度観測値の観測誤差分散を生成し、この２Ｄ角度観測値および２Ｄ角度観測誤差分散からなる２Ｄ角度観測情報を出力する。２Ｄセンサ１０には、例えばレーダ、ＩＲ（InfraRed）センサ等が使用される。レーダの場合は、周知のように、パッシブに角度を観測するもの、アクティブに距離、角度を観測するものである。しかし、距離を観測するレーダは、距離の精度が悪いため、距離を有効な観測情報として使えない場合や距離が妨害波により有効な観測情報として使えない場合が多いため、ここでは角度のみ使用する。また、２Ｄセンサ１０としては、複数台のセンサを使用する場合を含むものとする。
【００１１】
２Ｄ角度追尾手段２０では、２Ｄセンサ１０から得られた２Ｄ角度観測情報から、追尾フィルタを用いて仰角、仰角速度、仰角加速度、方位角、方位角速度および方位角加速度の６変数からなる状態量を推定し、その推定結果を２Ｄ角度航跡として出力する。ここで、２Ｄ角度航跡の平滑位置は２Ｄ角度航跡の仰角、方位角成分とし、２Ｄ角度航跡の平滑速度は２Ｄ角度航跡の仰角速度、方位角度速度成分とし、また、２Ｄ角度航跡の平滑加速度は２Ｄ角度航跡の仰角角加速度、方位角加速度とする。
【００１２】
２Ｄ角度観測情報圧縮手段３０では、２Ｄセンサ１０から得られる２Ｄ角度観測情報と２Ｄ角度追尾手段２０から得られる２Ｄ角度航跡について事前設定された圧縮間隔に基づいて最小２乗統合することにより、２Ｄ角度観測情報を圧縮処理した統合２Ｄ角度観測情報を算出する。この２Ｄ角度観測情報圧縮手段３０の処理方法について、以下図２を用いて説明する。
図２において、圧縮前の２Ｄ角度観測値が時刻ｔk-1 から時刻ｔk までの処理フレーム間でｎ個得られているとする。そして、その処理フレーム間における時刻ｔk,i と時刻ｔk の間隔をΔｔki＝（ｔk −ｔk,i ）とする。ここで、処理フレームの長さは事前設定する圧縮間隔Ｔｉｎｔにより決まるものとする。
それから、時刻ｔk,i （ｉ≦ｎ，ｎ≧１）における観測ベクトルを
【数１】
と定義し、観測雑音共分散行列をＢk,i とする。ここで、観測ベクトルは２Ｄ角度観測値に対応し、観測誤差共分散行列は２Ｄ角度観測値の観測誤差分散に対応し、それぞれ２Ｄ角度観測情報から得られるものである。
【００１３】
また、２Ｄ角度追尾手段２０で得られる２Ｄ角度航跡から、状態ベクトル
【数２】
が得られることから、ｔk-1 から時刻ｔk における速度ベクトル
【数３】
が得られる。ここで、ベクトルおよび行列の右上添字Ｔは、代数学における行列およびベクトルの転置を表す。
そうすると、上記速度ベクトル
【数４】
および観測行列Ｈ＝［Ｉ2×2 Ｏ2×2 Ｏ2×2 ］を用いて、時刻ｔk における観測ベクトル（圧縮前の２Ｄ角度観測値）は、（３）式のようにモデル化できる。
【数５】
ここで、時刻ｔk の観測雑音ベクトルを
【数６】
【００１４】
上記観測雑音ベクトルは、平均零ベクトル、分散Ｂk,i の白色正規分布に従う。Ｂk,i は２Ｄ角度観測値の誤差共分散行列である。また、Ｉ2×2 は２行２列の単位行列、そして、Ｏ2×2 は２行２列の零行列である。
そして、時刻ｔk-1 から時刻ｔk までのｎ個の観測ベクトルを、最小２乗統合により圧縮し、時刻ｔk における統合２Ｄ角度観測値を、（４）式により算出する。
【数７】
また、（４）式の統合２Ｄ角度観測値の誤差共分散行列も（５）式として同時に算出する。
【数８】
２Ｄ角度観測値圧縮手段３０では、上記処理フレーム内の１つ以上の２Ｄ角度観測値を上記（４）式により、１つの統合２Ｄ角度観測値にまとめる。そして、２Ｄ角度観測値の誤差共分散行列Ｂk,i を上記（５）式により、１つの統合２Ｄ角度観測値の誤差共分散行列にまとめる。
【００１５】
２Ｄ角度観測値圧縮手段３０は、以上のようにして得られた、統合２Ｄ角度観測値および統合２Ｄ角度観測値の誤差共分散行列を、統合２Ｄ角度観測情報として出力する。
また、２Ｄ角度観測値圧縮手段３０では、統合２Ｄ角度観測値の算出の際、処理フレーム内の観測値を１つ選択して、そのときの２Ｄ角度観測値と２Ｄ角度観測値の誤差共分散行列を、統合２Ｄ角度観測情報として出力する。
ここで、最小２乗統合により２Ｄ角度観測情報を圧縮したものを統合２Ｄ角度観測情報として出力するか、または処理フレーム内の観測情報を１つ選択したものを統合２Ｄ角度観測情報で出力するかは、事前に設定する。
【００１６】
距離推定値初期値算出手段４０では、２Ｄ角度観測値圧縮手段３０から入力される統合２Ｄ角度観測情報から、３点の統合２Ｄ角度観測情報に基づいて距離推定値初期値および距離変化率推定値初期値を算出する。
距離算出バッチ処理手段５０では、距離推定値初期値算出手段４０により算出された距離推定値初期値と距離変化率初期値を、距離算出バッチ処理の初期値として設定を行う。そして、距離算出バッチ処理初期値の設定後は、２Ｄセンサ１０から得られる２Ｄ角度観測情報を、事前に設定したバッチデータ数分蓄積してバッチ処理することにより、距離推定値および距離変化率推定値を算出して出力する。
【００１７】
以上のように、この実施の形態１によれば、図１２に示したような高サンプリングレートで得られる２Ｄ角度観測情報を間引いて捨てて、あたかも低サンプリングレートで２Ｄ角度観測情報が得られる状況にすることにより、距離推定値初期値および距離変化率推定値初期値の精度を向上させることができる。加えて、単に２Ｄ角度観測情報を捨てるだけではなく、２Ｄ角度観測値を最小２乗統合によりまとめた統合２Ｄ角度観測情報を用いており、統合２Ｄ角度観測情報は２Ｄ角度観測情報に比べて精度が良いから、この面においても算出された距離推定値初期値および距離変化率初期値の精度を向上させることができる。
【００１８】
実施の形態２．
図３はこの発明の実施の形態２によるセンサ情報融合装置の機能構成を示すブロック図である。図において、実施の形態１の図１に相当する部分には同一符号を付し、その説明は原則として省略する。この実施の形態２の場合は、２Ｄ角度観測情報選択手段６０を設けた点が実施の形態１の構成と異なっている。
２Ｄ角度観測情報選択手段６０では、２Ｄ角度追尾手段２０から得られる２Ｄ角度航跡の平滑位置と、２Ｄセンサ１０から得られる２Ｄ角度観測情報の２Ｄ角度観測値の残差を取り、その残差ベクトルのノルムが事前に決めたしきい値（残差ベクトルのノルムのしきい値と呼ぶ）よりも小さくなる２Ｄ角度観測値を選択する。この算出された選択後２Ｄ角度観測値に紐付く２Ｄ角度観測情報が、上記実施の形態１の場合の２Ｄセンサ１０で得られた２Ｄ角度観測情報と２Ｄ角度追尾手段２０で得られた２Ｄ角度航跡に代えて、２Ｄ角度観測情報圧縮手段３０に入力される。したがって、２Ｄ角度観測情報圧縮手段３０では、選択後２Ｄ角度観測値に紐付く選択後２Ｄ角度観測情報について事前に設定した圧縮間隔に基づいて圧縮処理した統合２Ｄ角度観測情報を算出する。
なお、上記残差ベクトルのノルムは、代数学におけるユークリッドノルムに基づくノルムでも良いし、マハラノビス距離に基づくノルムでも良い。
【００１９】
以上のように、この実施の形態２によれば、実施の形態１の場合の２Ｄセンサ１０から２Ｄ角度観測情報圧縮手段３０に入力する前の２Ｄ角度観測情報について、２Ｄ角度航跡に基づいて残差を考慮して選択し選択後２Ｄ角度観測情報として得、この選択後２Ｄ角度観測情報について２Ｄ角度観測情報圧縮手段３０で圧縮処理しているので、元の２Ｄ角度観測情報の異常値については排除することができる。結果として、距離推定値初期値および距離変化率推定値初期値の精度を向上させることが可能となる。
【００２０】
実施の形態３．
図４は、この発明の実施の形態３によるセンサ情報融合装置の機能構成を示すブロック図である。図において、実施の形態２の図３に相当する部分には同一符号を付し、その説明は原則として省略する。この実施の形態３の場合は、統合２Ｄ角度観測情報選択手段７０を設けた点が実施の形態２の構成と異なっている。
統合２Ｄ角度観測値選択手段７０では、２Ｄ角度追尾手段２０で得られた２Ｄ角度航跡の平滑位置と、２Ｄ角度観測情報圧縮手段３０で得られた統合２Ｄ角度観測情報の統合２Ｄ角度観測値の残差を取り、その残差ベクトルのノルムが事前に決めたしきい値よりも小さくなる統合２Ｄ角度観測値を選択する。この算出された選択後統合２Ｄ角度観測値に紐付く統合２Ｄ角度観測情報は、上記実施の形態２の場合の２Ｄ角度観測情報圧縮手段３０からの統合２Ｄ角度観測情報に代えて、距離推定値初期値算出手段４０に入力される。したがって、距離推定値初期値算出手段４０では、選択後統合２Ｄ角度観測情報を用いて、３点の統合２Ｄ角度観測情報から距離推定値初期値および距離変化率推定値初期値を算出することになる。
なお、上記残差ベクトルのノルムは、代数学におけるユークリッドノルムに基づくノルムでも良いし、マハラノビス距離に基づくノルムでも良い。
【００２１】
以上のように、この実施の形態３によれば、距離推定値初期値算出の処理前に、統合２Ｄ角度観測情報を２Ｄ角度航跡に基づいて選択するようにしたので、統合２Ｄ角度観測情報の異常値については排除することができる。結果として、距離推定値初期値および距離変化率推定値初期値の精度を向上させることができる。
【００２２】
実施の形態４．
目標の旋回性、機動性が大きい場合は、２Ｄ角度観測情報圧縮手段３０で圧縮処理を実施すると、統合２Ｄ角度観測情報の精度が悪くなることが想定される。目標の旋回性、機動性の大きさは、２Ｄ角度航跡の速度の大きさおよび加速度の大きさとなって現れる。この実施の形態４では、２Ｄ角度航跡の速度および加速度に基づいて目標の旋回性、機動性の大きさを判断し、上記目標の旋回性、機動性により起こる統合２Ｄ角度観測情報の精度の劣化を低減する手段について説明する。
【００２３】
図５は、この発明の実施の形態４によるセンサ情報融合装置の機能構成を示すブロック図である。図において、実施の形態１の図１に相当する部分には同一符号を付し、その説明は原則として省略する。この実施の形態４の場合は、圧縮間隔制御手段８０を設けた点が実施の形態１の構成と異なっている。
圧縮間隔制御手段８０には、２Ｄ角度追尾手段２０から得られる２Ｄ角度航跡
が入力される。また、圧縮間隔制御手段８０は、２Ｄ角度航跡の速度の大きさおよび加速度の大きさの２つの要素のしきい値とそれに対応して設定する圧縮間隔からなる圧縮間隔テーブルを事前に準備して保有している。
圧縮間隔制御手段８０では、２Ｄ角度追尾手段２０から入力された２Ｄ角度航跡の速度の大きさまたは加速度の大きさが、圧縮間隔テーブルで決めてある速度の大きさのしきい値および加速度の大きさのしきい値よりも大きい場合、目標の機動性、旋回性が有ると判断し、既設定してある圧縮間隔をそれよりも小さな圧縮間隔に変更設定して制御信号として２Ｄ角度観測情報圧縮手段３０に出力する。
【００２４】
圧縮間隔をどの程度に設定するかは、圧縮間隔テーブルに基づいて決定することになるが、このテーブルには、例えば圧縮間隔を２Ｄセンサの送信レートと同じにするといった内容も入れておく。設定する圧縮間隔を２Ｄセンサの送信レートと同じにするということは、圧縮をしないということと同じである。このように圧縮間隔テーブルにより変更設定した圧縮間隔と２Ｄセンサの送信レートが同じ場合は、圧縮間隔制御手段８０は、圧縮間隔と２Ｄ角度観測情報圧縮処理を行わないといった制御信号を２Ｄ角度観測情報圧縮手段３０に出力する。一方、圧縮間隔テーブルにより変更設定した圧縮間隔と２Ｄセンサの送信レートが異なる場合は、その圧縮間隔と２Ｄ角度観測情報圧縮処理を行うといった制御信号を２Ｄ角度観測情報圧縮手段３０に出力することになる。
【００２５】
以上のように、この実施の形態４によれば、圧縮間隔制御手段８０により、２Ｄ角度航跡の速度の大きさまたは加速度の大きさを、目標の機動性、旋回性の有無を判断する事前に決めたしきい値と比較し、比較結果に応じて、２Ｄ角度観測情報圧縮手段３０が用いる既設定された圧縮間隔を変更制御するようにしている。
したがって、目標の旋回性、機動性が大きい場合には小さな圧縮間隔に変更設定するため、統合２Ｄ角度観測情報の精度の劣化を低減できる。結果として、距離推定値初期値および距離変化率推定値初期値の精度の劣化も低減できる。
【００２６】
実施の形態５．
図３の距離算出バッチ処理手段５０において、２Ｄ角度観測情報に異常値が含まれていると、算出する距離推定値および距離変化率推定値の精度が悪化することになる。この実施の形態４では、この問題を解決する手段について説明する。
図６は、この発明の実施の形態４によるセンサ情報融合装置の機能構成を示すブロック図である。図において、実施の形態２の図３に相当する部分には同一符号を付し、その説明は原則として省略する。この実施の形態５の場合は、距離推定値選択手段９０を設けた点が実施の形態２の構成と異なっている。
距離推定値選択手段９０には、２Ｄ角度観測情報の異常値を用いて距離推定値および距離変化率推定値を算出しているとみなすための事前に設定した距離推定値しきい値および距離変化率推定値しきい値が事前に設定されている。そして、距離推定値選択手段９０では、距離算出バッチ処理手段５０で算出された距離推定値および距離変化率推定値が、事前に設定した距離推定値しきい値および距離変化率推定値しきい値以下の場合は、妥当な選択後距離推定値および距離変化率推定値として出力する。
【００２７】
一方、距離推定値および距離変化率推定値が、事前に設定した距離推定値しきい値および距離変化率推定値しきい値よりも大きい場合は、選択後距離推定値および距離変化率推定値として出力されない。また、距離推定値選択手段９０は、このとき、再選択の制御信号を２Ｄ角度観測情報選択手段６０に与える。２Ｄ角度観測情報選択手段６０では、距離推定値選択手段９０から再選択の制御信号が入力された場合には、その制御信号に基づいて、２Ｄ角度航跡と２Ｄ角度観測情報の残差ベクトルのノルムのしきい値を既設値よりも小さい値に変更設定する。このことによって、距離推定値初期値および距離変化率推定値初期値を求めるために用いる２Ｄ角度観測情報の異常値も排除される。
【００２８】
以上のように、この実施の形態５によれば、２Ｄ角度観測情報選択手段９０において、距離算出バッチ処理手段５０で算出された距離推定値と距離変化率推定値を、２Ｄ角度観測情報の異常値を用いて算出していると判断する事前設定のしきい値と比較し、当該比較結果に応じて前記距離推定値と距離変化率推定値を出力するかどうかを選択し、出力しない場合には２Ｄ角度観測情報選択手段６０における２Ｄ角度航跡と２Ｄ角度観測情報の残差の値を判定するしきい値を既設値よりも小さい値に変更設定するよう制御するようにしている。したがって、２Ｄ角度観測情報の異常値を使って算出された距離推定値および距離変化率推定値は使わないようにし、また、距離推定値初期値および距離変化率推定値初期値の精度も向上させることができる。
【００２９】
実施の形態６．
図７は、この発明の実施の形態６によるセンサ情報融合装置の機能構成を示すブロック図である。図において、実施の形態１の図１に相当する部分には同一符号を付し、その説明は原則として省略する。この実施の形態６の場合は、距離推定値平均手段１００を設けた点が実施の形態１の構成と異なっている。
距離推定値平均手段１００では、２Ｄ角度追尾手段２０で得られた２Ｄ角度航跡と、距離推定値初期値算出手段４０で得られた距離推定値初期値および距離変化率推定値初期値を用いて、一旦３Ｄ位置・速度情報に変換する。次に、変換して得られた３Ｄ位置・速度情報を、時系列にＭサンプリング分移動平均して、平均化した３Ｄ位置・速度情報を算出する。この平均化した３Ｄ位置・速度情報と２Ｄ角度航跡を用いて、距離推定値初期値と距離変化率推定値初期値を算出する。
【００３０】
ここで、各時刻の３Ｄ位置・速度情報を時系列にＭサンプリング分移動平均する場合は、事前に仮定した軌道モデルにより、ある時刻に合わせて外挿を行う。例えば、時刻ｋの３Ｄ位置・速度情報をＸ（ｋ）、時刻ｋ＋１の３Ｄ位置・速度情報をＸ（ｋ＋１）とした場合に、時刻ｋ＋１を平均化した３Ｄ位置・速度を求めることを考える。この場合、時刻ｋの３Ｄ位置・速度情報Ｘ（ｋ）と時刻ｋ＋１の３Ｄ位置・速度情報Ｘ（ｋ＋１）の平均を算出する場合は、事前に仮定した軌道モデルを線形１次近似するなどして得た、制御工学における時刻ｋから時刻ｋ＋１への状態遷移行列Ｆを用いて、時刻ｋの３Ｄ位置・速度情報に時刻ｋから時刻ｋ＋１への状態遷移行列Ｆをかけることにより、時刻合わせをしたＦ・Ｘ（ｋ）を算出する。そして、Ｆ・Ｘ（ｋ）とＸ（ｋ＋１）の平均をとったものを平均化した３Ｄ位置・速度情報とする。
この平均化した３Ｄ位置・速度情報と２Ｄ角度航跡を用いて、距離推定値と距離変化率推定値を逆算する。この逆算したものを、それぞれ、平均処理後距離推定値初期値および平均処理後距離変化率推定値初期値と呼ぶ。そして、この平均処理後距離推定初期値と平均処理後距離変化率推定値初期値を、距離算出バッチ処理手段５０に入力する。距離算出バッチ処理手段５０では、距離推定値初期値と距離変化率推定値初期値を初期値として２Ｄ角度観測情報のバッチ処理を行う。
【００３１】
以上のように、この実施の形態６によれば、距離推定値初期値および距離変化率推定値初期値を移動平均することにより、距離推定値初期値および距離変化率推定値初期値の安定化が図れるため、結果として、距離推定値および距離変化率推定値の精度が向上する。
【００３２】
実施の形態７．
図１の距離算出バッチ処理手段５０において、バッチ処理データ数が多ければ多い程、距離推定値および距離変化率推定値の精度は良くなる。しかし、バッチ処理データ数がある値を超えた場合は、距離推定値および距離変化率推定値の推定精度はさほど変わらなくなる筈である。このようなケースでは、バッチ処理データ数が不当に多い場合、バッチ処理計算結果である距離推定値および距離変化率推定値の精度は良くなるものの、バッチ処理計算の演算負荷が高くなる。逆に、バッチ処理データ数が少なければ、バッチ処理計算の演算負荷は低くなるが、距離推定値および距離変化率推定値としては十分な精度は得られなくなる。また、２Ｄセンサの観測精度、サンプリングレートによって、最適なバッチ処理データ数は異なる。この実施の形態７では、バッチ処理の推定精度の高精度化と演算量の低減を両立させる手段について説明する。
【００３３】
図８は、この発明の実施の形態７によるセンサ情報融合装置の機能構成を示すブロック図である。図において、実施の形態１の図１に相当する部分には同一符号を付し、その説明は原則として省略する。この実施の形態７の場合は、バッチ処理データ数制御ＤＢ（データベース）１１０、バッチ処理データ数制御手段１２０を設けた点が実施の形態１の構成と異なっている。
バッチ処理データ数制御ＤＢ１１０には、距離および距離推定値の推定精度とバッチ処理計算の演算量に関連するデータ、すなわち２Ｄ角度観測情報、サンプリングレート、観測精度、距離および距離変化率、２Ｄ角度航跡、バッチ処理データ数などを含むテーブルを事前に作成し、データベース化して保管しておく。バッチ処理データ数制御手段１２０では、２Ｄセンサ１０で得られる２Ｄ角度観測情報、この２Ｄ角度観測情報を得るサンプリングレートおよび観測精度、距離算出バッチ処理手段５０で得られる距離推定値および距離変化率推定値、さらに２Ｄ角度追尾手段２０で得られる２Ｄ角度航跡を検索キーとして、バッチ処理データ数制御ＤＢ１１０を参照してバッチ処理データ数を得る。そして、検索したバッチ処理データ数を距離算出バッチ処理手段５０に設定する。
【００３４】
以上のように、この実施の形態７によれば、バッチ処理データ数制御手段１２０により、２Ｄセンサ１０の２Ｄ角度観測情報、そして２Ｄセンサ１０の観測精度、サンプリングレート、２Ｄ角度航跡および距離推定値および距離変化率推定値を検索キーとして、バッチ処理データ数制御ＤＢ１１０から最適なバッチ処理データ数を検索して距離算出バッチ処理手段５０に設定するようにしたので、バッチ処理の推定精度の高精度化と演算量の低減の両立化を可能にする。
【００３５】
実施の形態８．
図９は、この発明の実施の形態８によるセンサ情報融合装置の機能構成を示すブロック図である。図において、実施の形態１の図１に相当する部分には同一符号を付し、その説明は原則として省略する。この実施の形態７場合は、擬似３Ｄ観測情報生成手段１３０、３Ｄセンサ１４０、観測モデル選択手段１５０、３Ｄ追尾手段１６０を設けた点が実施の形態１の構成と異なっている。
擬似３Ｄ観測情報生成手段１３０では、距離算出バッチ処理手段５０で得られた距離推定値と２Ｄセンサ１０で得られた２Ｄ角度観測情報から、距離推定値と仰角観測値と方位角観測値からなる組を擬似３Ｄ観測値として生成する。ここで、距離推定値に対応する距離観測誤差分散を事前設定しておく。ここでは、距離推定値を擬似距離観測値と呼び、この擬似距離に対応する距離観測誤差分散を擬似距離観測誤差分散と呼ぶことにする。擬似３Ｄ観測情報生成手段１３０では、擬似距離観測誤差分散と、２Ｄ角度観測情報から得られる仰角観測誤差分散、方位角度観測誤差分散からなる組である、擬似３Ｄ観測誤差分散を生成する。最終的に、擬似３Ｄ観測情報生成手段１３０は、擬似３Ｄ観測値と擬似３Ｄ観測誤差分散をペアとした擬似３Ｄ観測情報を出力する。
【００３６】
３Ｄセンサ１４０では、目標を観測することにより、距離、仰角、方位角からなる３Ｄ観測値と、距離観測誤差分散、仰角観測誤差分散、方位角観測誤差分散からなる３Ｄ観測誤差分散を取得して、３Ｄ観測値と３Ｄ観測誤差分散をペアとした３Ｄ観測情報を出力する。
２Ｄセンサ１０からの２Ｄ角度観測情報、擬似３Ｄ観測情報生成手段１３０からの擬似３Ｄ観測情報、３Ｄセンサ１４０からの３Ｄ観測情報は、観測モデル選択手段１５０に入力される。観測モデル選択手段１５０では、これらの入力される観測情報の種類に応じて、制御工学における観測モデルを制御する信号（観測モデル制御信号）を生成する。例えば、２Ｄ角度観測情報が入力された場合は、３Ｄ追尾手段１６０で２Ｄ角度観測情報用の観測モデルを用いるような観測モデル制御信号を生成する。また、３Ｄ観測情報または擬似３Ｄ観測情報が入力された場合も同様で、各観測情報に応じた観測モデルを用いるような観測モデル制御信号を生成する。よって、観測モデル選択手段１５０では、生成したいずれかの観測モデル制御信号と、当該制御信号に対応した２Ｄ角度観測情報、３Ｄ観測情報、擬似３Ｄ観測情報を３Ｄ追尾手段１６０に出力する。
【００３７】
３Ｄ追尾手段１６０では、観測モデル選択手段１５０から入力される観測モデル制御信号と、当該観測モデル制御信号に対応した２Ｄ角度観測情報、３Ｄ観測情報、擬似３Ｄ観測情報に基づいて３Ｄ追尾処理を行う。
ここで、３Ｄ観測情報が得られる場合の観測モデル（観測ベクトル）は、（６）式のようになる。
【数９】
とする。観測ベクトルは、３Ｄ観測情報の３Ｄ観測値を極座標から直交座標に座標変換することにより求める。
【数１０】
また、（６）式におけるＨは、観測行列であり、Ｈ＝［Ｉ3×3 Ｏ3×3 ]である。この観測行列はカルマンフィルタにおけるデータ更新処理等に用いる。ここで、Ｉ3×3 は３行３列の単位行列、Ｏ3×3 は３行３列の零行列である。
また、
【数１１】
は距離、仰角、方位角を成分に持つ観測雑音ベクトルであり、観測雑音ベクトルの共分散行列の対角成分が、３Ｄ観測情報の距離観測誤差分散σＲk2、仰角観測誤差分散σＥk2、方位角観測誤差分散σＡｚk2にそれぞれ対応する。具体的には、観測雑音ベクトルの共分散行列をＢk とすると、（９）式で表される。
Ｂk ＝ｄｉａｇ［σＲk2，σＥk2，σＡｚk2］ （９）
ここで、ｄｉａｇ（ａ，ｂ，ｃ）は、ａ，ｂ，ｃを対角成分に持つ対角行列を表す。
さらに、Ａk は極座標から直交座標への座標変換行列を表し、直交座標（ｘ，ｙ，ｚ）を極座標（Ｒ，Ｅ，Ａｚ）で線形１次近似したヤコビアン行列（１０）式で得られる。
【数１２】
【００３８】
また、擬似３Ｄ観測情報が得られる場合の観測モデルも、（６）式で定義する。
但し、この場合の観測雑音ベクトルの共分散行列Ｂk は（１１）式のように定義する。
【数１３】
【００３９】
一方、２Ｄ角度観測情報が得られる場合の観測モデル（観測ベクトル）は、（１２）式となる。
【数１４】
また、２Ｄ角度情報が得られる場合の観測行列はＨk ＝［Ａk Ｏ3×3 ］である。この観測行列はカルマンフィルタにおけるデータ更新処理等に用いる。また、Ａk は（１４）式で算出される。
【数１５】
Ａk は極座標（Ｅ，Ａｚ）を直交座標（ｘ，ｙ，ｚ）で線形１次近似したヤコビアン行列で得られる。
また、（１１）式の
【数１６】
は仰角、方位角を成分に持つ観測雑音ベクトルであり、観測雑音ベクトルの共分散行列の対角成分が、２Ｄ角度観測情報の仰角観測誤差分散σＥk2、方位角観測誤差分散σＡＥk2にそれぞれ対応する。具体的には、観測雑音ベクトルの共分散行列をＢk とすると、（１５）式のようになる。
Ｂk ＝ｄｉａｇ［σＥk2，σＡｚk2］ （１５）
【００４０】
３Ｄ追尾手段１６０は、３Ｄ観測情報、擬似３Ｄ観測情報または２Ｄ角度観測情報に応じた、以上の（６）式または（１２）式で定義したような観測モデルが入力されると、その観測モデルに基づいて３Ｄ追尾を行い、（８）式で表される状態ベクトルの成分を持つ３Ｄ航跡を出力する。
【００４１】
以上のように、実施の形態８によれば、入力される観測情報に応じた観測モデルを選択して３Ｄ追尾を行うようにしたので、３Ｄ航跡の収束を早くすることができる。
【図面の簡単な説明】
【００４２】
【図１】この発明の実施の形態１によるセンサ情報融合装置の機能構成を示すブロック図である。
【図２】この発明の実施の形態１に係る２Ｄ角度観測情報圧縮手段の処理方法を示す説明図である。
【図３】この発明の実施の形態２によるセンサ情報融合装置の機能構成を示すブロック図である。
【図４】この発明の実施の形態３によるセンサ情報融合装置の機能構成を示すブロック図である。
【図５】この発明の実施の形態４によるセンサ情報融合装置の機能構成を示すブロック図である。
【図６】この発明の実施の形態５によるセンサ情報融合装置の機能構成を示すブロック図である。
【図７】この発明の実施の形態６によるセンサ情報融合装置の機能構成を示すブロック図である。
【図８】この発明の実施の形態７によるセンサ情報融合装置の機能構成を示すブロック図である。
【図９】この発明の実施の形態８によるセンサ情報融合装置の機能構成を示すブロック図である。
【図１０】角度観測値から直交座標の位置の推定を行う従来方法を示す説明図である。
【図１１】従来のセンサ情報融合装置の機能構成を示すブロック図である。
【図１２】２Ｄ角度観測値のサンプリングレートの高低に対応する距離推定値等の推定精度を表す説明図である。
【符号の説明】
【００４３】
１０ ２Ｄセンサ、２０ ２Ｄ角度追尾手段、３０ ２Ｄ角度観測情報圧縮手段、４０ 距離推定値初期値算出手段、５０ 距離算出バッチ処理手段、６０ ２Ｄ角度観測情報選択手段、７０ 統合２Ｄ角度観測情報選択手段、８０ 圧縮間隔制御手段、９０ 距離推定値選択手段、１００ 距離推定値平均手段、１１０ バッチ処理データ数制御ＤＢ、１２０ バッチ処理データ数制御処理、１３０ 擬似３Ｄ観測情報生成手段、１４０ ３Ｄセンサ、１５０ 観測モデル選択手段、１６０ ３Ｄ追尾手段。
【特許請求の範囲】
【請求項１】
センサにより観測した２Ｄ角度観測値と２Ｄ角度観測誤差分散からなる２Ｄ角度観測情報に基づいて２Ｄ角度航跡を算出する２Ｄ角度追尾手段と、
前記２Ｄ角度観測情報と前記２Ｄ角度航跡について事前設定された圧縮間隔に基づいて最小２乗統合することにより、前記２Ｄ角度観測情報を圧縮処理した統合２Ｄ角度観測情報を算出する２Ｄ角度観測情報圧縮手段と、
前記算出された３点の統合２Ｄ角度観測情報に基づいて距離推定値初期値と距離変化率推定値初期値を算出する距離推定値初期値算出手段と、
前記距離推定値初期値と前記距離変化率推定値初期値を初期値として、前記２Ｄ角度観測情報を事前に設定したバッチデータ数分蓄積してバッチ処理することにより、距離推定値と距離変化率推定値を算出する距離算出バッチ処理手段を備えたことを特徴とするセンサ情報融合装置。
【請求項２】
２Ｄ角度追尾手段で得られた２Ｄ角度航跡とセンサで得られた２Ｄ角度観測情報の残差を取り、当該残差が事前に決めたしきい値よりも小さくなる２Ｄ角度観測情報を選択し選択後２Ｄ角度観測情報として出力する２Ｄ角度観測情報選択手段を備え、
２Ｄ角度観測情報圧縮手段は、センサで得られた２Ｄ角度観測情報と２Ｄ角度追尾手段で得られた２Ｄ角度航跡に代えて、前記選択後２Ｄ角度観測情報について事前に設定した圧縮間隔に基づいて圧縮処理して統合２Ｄ角度観測情報を算出するようにしたことを特徴とする請求項１記載のセンサ情報融合装置。
【請求項３】
２Ｄ角度追尾手段で得られた２Ｄ角度航跡と２Ｄ角度観測情報圧縮手段で得られた統合２Ｄ角度観測情報の残差を取り、当該残差が事前に決めたしきい値より小さくなる統合２Ｄ角度観測情報を選択する統合２Ｄ角度選択手段を備え、
前記距離推定初期値算出手段は、前記選択された統合２Ｄ角度観測情報に基づいて距離推定値初期値と距離変化率推定値初期値を算出するようにしたことを特徴とする請求項２記載のセンサ情報融合装置。
【請求項４】
２Ｄ角度追尾手段で得られた２Ｄ角度航跡の速度の大きさまたは加速度の大きさを、目標の機動性、旋回性の有無を判断する事前に決めたしきい値と比較し、当該比較結果に応じて、２Ｄ角度観測情報圧縮手段が用いる既設された圧縮間隔を変更制御する圧縮間隔制御手段を備えたことを特徴とする請求項１記載のセンサ情報融合装置。
【請求項５】
距離算出バッチ処理手段で得られた距離推定値と距離変化率推定値を、２Ｄ角度観測情報の異常値を用いて算出していると判断する事前設定のしきい値と比較し、当該比較結果に応じて前記距離推定値と距離変化率推定値を出力するかどうかを選択し、出力しない場合には２Ｄ角度観測情報選択手段における２Ｄ角度航跡と２Ｄ角度観測情報の残差の値を判定するしきい値を既設値よりも小さい値に変更設定するよう制御する距離推定値選択手段を備えたことを特徴とする請求項２記載のセンサ情報融合装置。
【請求項６】
２Ｄ角度追尾手段で得られた２Ｄ角度航跡と距離推定値初期値算出手段で得られた距離推定値と距離変化率推定値初期値を用いて、時系列の距離推定値および距離変化率推定値を移動平均して平均処理後距離推定値初期値と平均処理後距離変化率推定値初期値を算出する距離推定値初期値平均手段を備え、
距離算出バッチ処理手段は、前記平均処理後距離推定値初期値と前記平均処理後距離変化率推定値初期値を初期値として２Ｄ角度観測情報のバッチ処理を行うようにしたことを特徴とする請求項１記載のセンサ情報融合装置。
【請求項７】
２Ｄ角度観測情報、サンプリングレート、観測精度、距離および距離変化率、２Ｄ角度航跡およびバッチ処理データ数をデータベース化して保有するバッチ処理データ数制御ＤＢと、
センサで得られた２Ｄ角度観測情報、この２Ｄ角度観測情報を得るサンプリングレートと観測精度、距離算出バッチ処理手段で得られた距離推定値および距離変化率推定値、２Ｄ角度追尾手段で得られた２Ｄ角度航跡を検索キーとして、バッチ処理データ数制御ＤＢを参照して、対応するバッチ処理データ数を検索し前記距離算出バッチ処理手段に設定するバッチ処理データ数制御手段を備えたことを特徴とする請求項１記載のセンサ情報融合装置。
【請求項８】
距離算出バッチ処理手段で得られた距離推定値とセンサで得られた２Ｄ角度観測情報に基づいて擬似３Ｄ観測情報を生成する擬似３Ｄ観測情報生成手段と、
距離観測値、仰角観測値、方位角観測値およびその観測誤差分散を３Ｄ観測情報として出力する３Ｄセンサと、
入力される前記２Ｄ角度観測情報、前記３Ｄ観測情報および前記擬似３Ｄ観測情報に応じて対応する観測モデルを選択する観測モデル選択手段と、
前記観測モデル選択手段で選択された観測モデルに対応する、２Ｄ角度観測情報、３Ｄ観測情報または擬似３Ｄ観測情報に基づいて３Ｄ追尾処理を行って３Ｄ航跡を算出する３Ｄ追尾手段を備えたことを特徴とする請求項１記載のセンサ情報融合装置。
【請求項１】
センサにより観測した２Ｄ角度観測値と２Ｄ角度観測誤差分散からなる２Ｄ角度観測情報に基づいて２Ｄ角度航跡を算出する２Ｄ角度追尾手段と、
前記２Ｄ角度観測情報と前記２Ｄ角度航跡について事前設定された圧縮間隔に基づいて最小２乗統合することにより、前記２Ｄ角度観測情報を圧縮処理した統合２Ｄ角度観測情報を算出する２Ｄ角度観測情報圧縮手段と、
前記算出された３点の統合２Ｄ角度観測情報に基づいて距離推定値初期値と距離変化率推定値初期値を算出する距離推定値初期値算出手段と、
前記距離推定値初期値と前記距離変化率推定値初期値を初期値として、前記２Ｄ角度観測情報を事前に設定したバッチデータ数分蓄積してバッチ処理することにより、距離推定値と距離変化率推定値を算出する距離算出バッチ処理手段を備えたことを特徴とするセンサ情報融合装置。
【請求項２】
２Ｄ角度追尾手段で得られた２Ｄ角度航跡とセンサで得られた２Ｄ角度観測情報の残差を取り、当該残差が事前に決めたしきい値よりも小さくなる２Ｄ角度観測情報を選択し選択後２Ｄ角度観測情報として出力する２Ｄ角度観測情報選択手段を備え、
２Ｄ角度観測情報圧縮手段は、センサで得られた２Ｄ角度観測情報と２Ｄ角度追尾手段で得られた２Ｄ角度航跡に代えて、前記選択後２Ｄ角度観測情報について事前に設定した圧縮間隔に基づいて圧縮処理して統合２Ｄ角度観測情報を算出するようにしたことを特徴とする請求項１記載のセンサ情報融合装置。
【請求項３】
２Ｄ角度追尾手段で得られた２Ｄ角度航跡と２Ｄ角度観測情報圧縮手段で得られた統合２Ｄ角度観測情報の残差を取り、当該残差が事前に決めたしきい値より小さくなる統合２Ｄ角度観測情報を選択する統合２Ｄ角度選択手段を備え、
前記距離推定初期値算出手段は、前記選択された統合２Ｄ角度観測情報に基づいて距離推定値初期値と距離変化率推定値初期値を算出するようにしたことを特徴とする請求項２記載のセンサ情報融合装置。
【請求項４】
２Ｄ角度追尾手段で得られた２Ｄ角度航跡の速度の大きさまたは加速度の大きさを、目標の機動性、旋回性の有無を判断する事前に決めたしきい値と比較し、当該比較結果に応じて、２Ｄ角度観測情報圧縮手段が用いる既設された圧縮間隔を変更制御する圧縮間隔制御手段を備えたことを特徴とする請求項１記載のセンサ情報融合装置。
【請求項５】
距離算出バッチ処理手段で得られた距離推定値と距離変化率推定値を、２Ｄ角度観測情報の異常値を用いて算出していると判断する事前設定のしきい値と比較し、当該比較結果に応じて前記距離推定値と距離変化率推定値を出力するかどうかを選択し、出力しない場合には２Ｄ角度観測情報選択手段における２Ｄ角度航跡と２Ｄ角度観測情報の残差の値を判定するしきい値を既設値よりも小さい値に変更設定するよう制御する距離推定値選択手段を備えたことを特徴とする請求項２記載のセンサ情報融合装置。
【請求項６】
２Ｄ角度追尾手段で得られた２Ｄ角度航跡と距離推定値初期値算出手段で得られた距離推定値と距離変化率推定値初期値を用いて、時系列の距離推定値および距離変化率推定値を移動平均して平均処理後距離推定値初期値と平均処理後距離変化率推定値初期値を算出する距離推定値初期値平均手段を備え、
距離算出バッチ処理手段は、前記平均処理後距離推定値初期値と前記平均処理後距離変化率推定値初期値を初期値として２Ｄ角度観測情報のバッチ処理を行うようにしたことを特徴とする請求項１記載のセンサ情報融合装置。
【請求項７】
２Ｄ角度観測情報、サンプリングレート、観測精度、距離および距離変化率、２Ｄ角度航跡およびバッチ処理データ数をデータベース化して保有するバッチ処理データ数制御ＤＢと、
センサで得られた２Ｄ角度観測情報、この２Ｄ角度観測情報を得るサンプリングレートと観測精度、距離算出バッチ処理手段で得られた距離推定値および距離変化率推定値、２Ｄ角度追尾手段で得られた２Ｄ角度航跡を検索キーとして、バッチ処理データ数制御ＤＢを参照して、対応するバッチ処理データ数を検索し前記距離算出バッチ処理手段に設定するバッチ処理データ数制御手段を備えたことを特徴とする請求項１記載のセンサ情報融合装置。
【請求項８】
距離算出バッチ処理手段で得られた距離推定値とセンサで得られた２Ｄ角度観測情報に基づいて擬似３Ｄ観測情報を生成する擬似３Ｄ観測情報生成手段と、
距離観測値、仰角観測値、方位角観測値およびその観測誤差分散を３Ｄ観測情報として出力する３Ｄセンサと、
入力される前記２Ｄ角度観測情報、前記３Ｄ観測情報および前記擬似３Ｄ観測情報に応じて対応する観測モデルを選択する観測モデル選択手段と、
前記観測モデル選択手段で選択された観測モデルに対応する、２Ｄ角度観測情報、３Ｄ観測情報または擬似３Ｄ観測情報に基づいて３Ｄ追尾処理を行って３Ｄ航跡を算出する３Ｄ追尾手段を備えたことを特徴とする請求項１記載のセンサ情報融合装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【公開番号】特開２００９−１８６２６９（Ｐ２００９−１８６２６９Ａ）
【公開日】平成２１年８月２０日（２００９．８．２０）
【国際特許分類】
【出願番号】特願２００８−２５２４３（Ｐ２００８−２５２４３）
【出願日】平成２０年２月５日（２００８．２．５）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
【公開日】平成２１年８月２０日（２００９．８．２０）
【国際特許分類】
【出願日】平成２０年２月５日（２００８．２．５）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
[ Back to top ]