(57)【特許請求の範囲】
【請求項１】
複数のセンサにより観測された目標の観測値を出力する観測値出力部と、
前記観測値出力部から出力された観測値に基づき、前記複数のセンサに含まれる２つのセンサをセンサ組として、異なるセンサ組のそれぞれに対して、センサ組を構成する２つのセンサと前記目標との距離差であるセンサ組の距離差を算出する距離差算出部と、
前記異なるセンサ組それぞれの距離差ごとの観測精度を算出する観測精度算出部及び、前記観測精度算出部で算出された前記異なるセンサ組それぞれの距離差ごとの観測精度に基づいて前記距離差算出部により算出された前記異なるセンサ組それぞれの距離差の中から少なくとも一つのセンサ組の距離差を選択する優先選択部を含む距離差観測値選択部と、
前記距離差観測値選択部で選択された少なくとも一つのセンサ組の距離差に基づき、前記目標の位置及び速度を算出する追尾処理を行う追尾処理部と、
を備えたことを特徴とする目標追尾装置。
【請求項２】
前記距離差観測値選択部は
前記目標の将来時刻における予測位置を算出する予測部を含み、
前記観測精度算出部で算出される前記異なるセンサ組それぞれの距離差ごとの観測精度は、前記予測部で算出された予測位置での前記異なるセンサ組それぞれの距離差ごとの観測精度であることを特徴とする請求項１に記載の目標追尾装置。
【請求項３】
前記観測精度は一つの空間方向または水平面における観測精度であることを特徴とする請求項１または請求項２に記載の目標追尾装置。
【請求項４】
前記観測精度は、前記観測精度に対応するセンサ組に含まれる２つのセンサと前記目標との位置関係に基づき定義されるパラメータであることを特徴とする請求項１乃至３のいずれか１項に記載の目標追尾装置。
【請求項５】
前記パラメータは、前記観測精度に対応するセンサ組に含まれる２つのセンサの前記目標からの距離差の推定誤差の一つの空間方向または水平面に対する勾配である
ことを特徴とする請求項４に記載の目標追尾装置。
【請求項６】
前記パラメータは、前記観測精度に対応するセンサ組に対して定義される疑似的なDOPであることを特徴とする請求項４または請求項５に記載の目標追尾装置。
【請求項７】
前記追尾処理部は複数の空間方向に対して前記目標の推定位置誤差を算出する手段を含み、前記距離差観測値選択部は前記追尾処理部より出力される複数の空間方向に対する前記目標の推定位置誤差の中から前記推定位置誤差の大きい空間方向を選択し、該選択された空間方向に対する前記異なるセンサ組それぞれの距離差ごとの観測精度に基づき前記少なくとも一つのセンサ組の距離差を選択する
ことを特徴とする請求項１乃至６のいずれか１項に記載の目標追尾装置。
【請求項８】
前記距離差観測値選択部は
前記目標の将来時刻における前記異なるセンサ組それぞれの距離差ごとの観測精度の一つの空間方向または水平面に対する勾配を算出する勾配予測部と、
前記勾配予測部で算出された勾配に基づき前記異なるセンサ組それぞれの距離差ごとの観測精度を算出する観測精度算出部と、
前記観測精度算出部で算出された前記異なるセンサ組それぞれの距離差ごとの観測精度に基づき、前記異なるセンサ組それぞれの距離差の中から前記少なくとも一つのセンサ組の距離差を選択する優先選択部と、
を備えたことを特徴とする請求項１に記載の目標追尾装置。
【請求項９】
前記距離差観測値選択部において選択される前記少なくとも一つのセンサ組の距離差は、２以上の所定数のセンサ組の距離差であり、
前記追尾処理部は同期型追尾処理部である
ことを特徴とする請求項１乃至８のいずれか１項に記載の目標追尾装置。
【請求項１０】
前記追尾処理部は非同期型追尾処理部である
ことを特徴とする請求項１乃至８のいずれか１項に記載の目標追尾装置。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、目標の運動諸元を推定し、目標を追尾する目標追尾装置に関するものである。
【背景技術】
【０００２】
複数センサによる目標との距離差の観測結果を用いて目標の運動諸元を推定する従来の目標追尾装置には、同期型目標追尾装置と非同期型目標追尾装置がある。同期型目標追尾では、複数センサにより同一時刻に得られた観測情報を用いて目標追尾が行われ、非同期型目標追尾では複数センサにより異なる時刻に得られた観測情報を用いて目標追尾が行われる。
【０００３】
従来の同期型目標追尾装置は、例えば特許文献１、非特許文献１に記載されている。この従来の同期型目標追尾装置の動作について説明する。まず、観測モデルを式(1)のように仮定する。
0006358006.tif 000002
ここで、zi,j(t)は時刻tにおける目標とセンサi、センサjとの距離差の観測値、ｒi,j(x(t))は時刻tにおける目標とセンサi、センサjとの距離差真値を表す観測方程式、vi,j(t)は時刻tにおける目標とセンサi、センサjとの距離差の観測誤差、x(t)は目標の状態ベクトル（式(4)）を表す。観測誤差vi,j(t)は受信時の雑音の影響等により発生する。
【０００４】
たとえば、x−yの2次元直交座標系における目標位置とセンサi、センサjとの距離差の観測値が得られるとする。このとき、距離差真値ｒi,j(x(t))を式(2)のように定義する。ここで、Si、Sjはそれぞれセンサi、jの位置ベクトルを表す。
0006358006.tif 000003
ここで
0006358006.tif 000004
0006358006.tif 000005
であり、Inxnはn行n列の単位行列、Onxnはn行n列の零行列、上付き添え字のTは転置の記号を意味する。
【０００５】
上記のような観測モデルの仮定のもと、従来の目標追尾装置は以下に示すアルゴリズムで、状態ベクトルの推定を行う。
【０００６】
まず、センサが３つ以上の場合、式(2)の方程式が２式以上得られるため、この連立方程式を解くことで状態ベクトルを求めることができる。しかし、式(2)は非線形な方程式であるため、非線形最小二乗法を用いて状態ベクトルの位置推定値を算出する。
【０００７】
式(2)を目標の位置を位置ベクトル推定値（あるいは初期値）
0006358006.tif 000006
で近似すると、真値への修正ベクトルδxとの関係式が式(5)により得られる。
0006358006.tif 000007
ここで、距離差観測値として、センサ1を基準とし、センサ2〜Mまでの距離差観測値を使用する場合、δrはM-1行1列の残差ベクトル、GはM-1行2列のヤコビアン、δxは2行1列の修正ベクトルとなる。残差とは、観測値と推定値の差を意味する。また、各変数は下記式で表現される。
0006358006.tif 000008
0006358006.tif 000009
0006358006.tif 000010
0006358006.tif 000011
0006358006.tif 000012
【０００８】
式(5)より最小二乗法を用いて修正ベクトルを式(11)で算出し、式(12)のように目標の位置ベクトルを更新する。
0006358006.tif 000013
0006358006.tif 000014
以上の処理を修正ベクトルδxが収束する（例えば、所定値以下となる）まで繰り返すと、位置ベクトルが推定される。ここまでの処理を測位処理とし、さらに、各時刻で得られる前記位置ベクトルを用いて追尾処理を実施することで位置及び速度を含む目標状態ベクトルを更新する。
【０００９】
また、位置ベクトルの推定誤差共分散行列Pは式(13)のようになる。ここで、σは距離差観測値の精度である。
0006358006.tif 000015
【００１０】
さらに、水平面位置精度σHは水平面のDOP(Dilution of Precision、誤差倍増率)であるHDOPより算出できる。
0006358006.tif 000016
0006358006.tif 000017
上記に示した従来の同期型目標追尾装置は、複数のセンサが同じ時刻に観測した場合に成立する。しかし、実際には観測タイミングのずれ、マルチパス干渉による観測値欠落などにより、複数センサ間で同時に観測できない状況が想定される。
【００１１】
一方、非同期型目標追尾装置として、例えば非特許文献２がある。非特許文献２では同期型目標追尾装置のように測位処理は実施せず、直接距離差観測値を入力して、目標の状態ベクトルを更新する方法が開示されている。
【００１２】
非特許文献２は非同期型目標追尾装置のように異なる時刻に観測した距離差観測値を直接用いるため、同じ時刻に距離差観測値が得られず、測位できない場合でも目標の状態ベクトルを更新することが可能となる。
【００１３】
この場合、同期型目標追尾装置のように位置ベクトルではなく、距離差を用いて追尾処理を行う。
【先行技術文献】
【特許文献】
【００１４】
【特許文献１】特開２００２−２６７７３２号公報
【非特許文献】
【００１５】
【非特許文献１】D．J．Torrieri,“Statistical Theory of Passive Location Systems,”, IEEE Trans．Aersp．Syst．,vol．AES-20,pp．183-198, Mar．1984．
【非特許文献２】N．Okello,“Comparison of Recursive Algorithms for Emitter Localisation using TDOA Measurements from a Pair of UAVs,” IEEE Trans．Aersp．Syst．,vol．47, No．3, pp．1723-1732, Jul．2011．
【発明の概要】
【発明が解決しようとする課題】
【００１６】
同期型目標追尾装置及び非同期型目標追尾装置を含む従来の目標追尾装置では、複数のセンサで観測された距離差観測値を全て追尾処理へ入力し、目標の位置及び速度を更新することにより追尾処理を行う。この場合、観測精度が悪く追尾処理への貢献度の低い観測値を含めて追尾処理が行われるため、追尾処理への貢献度の低い観測値に対しても演算を行う必要があり、処理負荷が大きくなる課題があった。
この発明は、上記課題を解決するためになされたもので、目標追尾装置における追尾処理において追尾精度を確保しつつ、処理負荷を低減することを目的とする。
【課題を解決するための手段】
【００１７】
この発明に係る目標追尾装置は、複数のセンサにより観測された目標の観測値を出力する観測値出力部と、前記観測値出力部から出力された観測値に基づき、前記複数のセンサに含まれる２つのセンサをセンサ組として、センサ組を構成する２つのセンサと前記目標との距離差であるセンサ組の距離差を異なるセンサ組に対して算出する距離差算出部と、前記異なるセンサ組それぞれの距離差ごとの観測精度を算出する観測精度算出部及び、前記観測精度算出部で算出された前記異なるセンサ組それぞれの距離差ごとの観測精度に基づいて前記距離差算出部により算出された異なるセンサ組それぞれの距離差の中から少なくとも一つのセンサ組の距離差を選択する優先選択部を含む距離差観測値選択部と、前記距離差観測値選択部で選択された少なくとも一つのセンサ組の距離差に基づき、前記目標の位置及び速度を算出する追尾処理を行う追尾処理部と、を備えたことを特徴とする。
【発明の効果】
【００１８】
この発明によれば、観測精度の高い距離差観測値を優先的に選択して用いるため、追尾精度を確保しつつ、追尾処理負荷を低減することが可能となる。
【図面の簡単な説明】
【００１９】
【図１】本発明の実施の形態１の目標追尾装置を示す構成図。
【図２】本発明の実施の形態１の目標追尾装置における追尾処理の概念図。
【図３】本発明の実施の形態１の目標追尾装置における距離差観測値選択部４及び非同期追尾処理部５の処理フローを示す図。
【図４】本発明の実施の形態１の目標追尾装置における各距離差観測時刻において優先センサ組による距離差観測値を選択する追尾処理の概念図。
【図５】本発明の実施の形態２の目標追尾装置を示す構成図。
【図６】本発明の実施の形態３の目標追尾装置を示す構成図。
【発明を実施するための最良の形態】
【００２０】
以下、この発明の実施の形態について説明する。
実施の形態１．
図１は、実施の形態１の目標追尾装置１を示す構成図である。図１に示すように、本実施例の目標追尾装置１は、観測値出力部２、距離差算出部３、距離差観測値選択部４、追尾処理部である非同期追尾処理部５を備えている。なお、以降の各図において、同一符号は同一または相当部分を示す。
【００２１】
観測値出力部２は、センサ１１〜１Ｍ（Ｍは、１より大きい整数）を備える。距離差算出部３は、観測値出力部２から出力された観測値に基づき目標とセンサ１１〜１Ｍのうちの任意の２センサをセンサ組として、センサ組に含まれる２つのセンサと目標とのそれぞれの距離の差をセンサ組の距離差として算出する。距離差算出部３はセンサ組ごとの距離差を距離差観測値選択部４に出力する。  距離差観測値選択部４は、予測部４１、センサ組分の疑似DOP(Dilution of Precision、誤差倍増率)算出部４２、センサ組優先選択部４３を備える。疑似DOP算出部４２はより一般的には観測精度算出部として扱うことができる。また、センサ組優先選択部４３はより一般的には優先選択部として扱うことができる。
【００２２】
次に、実施の形態１の目標追尾装置１の動作を説明する。観測値出力部２は、センサ１１〜１Ｍの全てまたは一部の観測値を出力する。距離差算出部３は、観測値出力部２から出力された観測値に基づき、センサ１１〜１Ｍのうちの任意の２センサをセンサ組として、異なるセンサ組のそれぞれに対してセンサ組に含まれる２センサと目標との距離の差分を距離差として算出する。図１では、センサ１１とセンサ１２をセンサ組１、・・・、センサ１（Ｍ−１）とセンサ１Ｍをセンサ組M-1として、各センサ組における２センサと目標との距離の差を距離差として算出している。ここで、距離差算出部３はセンサ組ごとの距離差を同一時刻に算出してもよいし、異なる時刻に算出してもよい。同一時刻に算出されるセンサ組ごとの距離差は、ともに距離差観測値選択部４に出力される。なお、同一時刻に入力される各センサ組で得られた距離差観測値は、ある所定時間帯（例えば数秒以内）に入力される各センサ組で得られた距離差観測値であっても構わない。また、距離差算出部３は異なるセンサ組ごとの距離差の値を、異なる時刻に距離差観測値選択部４に出力しても構わない。
【００２３】
距離差観測値選択部４における予測部４１では非同期追尾処理部５で行われる追尾処理により算出される推定位置・速度より予測時刻ΔTi(i=1、…、L)秒後における予測位置を算出する。疑似DOP算出部４２は各センサ組（図１ではセンサ組1、・・・、センサ組M-1）に対応した算出部を備えている。疑似DOP算出部４２は予測部４１から予測時刻ΔTi(i=1、…、L)秒後の予測位置情報を受け取り、その予測位置における疑似DOPをセンサ組毎に算出する。ここで、疑似DOPとは測位位置のDOPと異なり、本実施の形態によって定義されるセンサ組毎の位置精度を表した値である。疑似DOPの詳細は後に説明するが、疑似DOPの値が小さいほど位置精度は高くなる。
【００２４】
センサ組優先選択部４３は、距離差算出部３から同一時刻に入力される各センサ組で得られた距離差観測値を受け取る。また、センサ組優先選択部４３は、疑似DOP算出部４２から出力される各センサ組の疑似DOPを受け取ると、疑似DOPが小さい順に各センサ組で得られた距離差観測値を非同期追尾処理部５へ出力する。非同期追尾処理部５は疑似DOPが小さい各センサ組で得られた距離差観測値を用いて追尾処理を行う。このような処理により、観測精度のよい観測値の情報を用いて、少ない演算量で効率的に追尾処理を行うことができる。なお、センサ組優先選択部４３は疑似DOPが小さい順に各センサ組で得られた距離差観測値を非同期追尾処理部５へ出力するとしたが、疑似DOPが小さい観測値を優先的に非同期追尾処理部５へ出力してもよい。
【００２５】
以下では、本実施の形態で定義される疑似DOPの詳細を説明する。
【００２６】
まず、従来から総合的な位置精度を表すパラメータとして式(15)のHDOPが知られているが、このHDOPを近似を用いて変形し、各要素に分解してみる。具体的には、2次元位置の測位において、式(15)のHDOPは式(16)の第２式となり、さらに近似することで式(16)の第３式となる。式(16)の第３式において、平方根内の第1項はX方向のDOP（以下、XDOP）、第2項はY方向のDOP（YDOP）である。従って、HDOP、XDOP、YDOPの関係式は式(17)のようになる。
【００２７】
この表記の考え方にならい、あるセンサi、jの組に注目し、センサ組i、jにおけるX方向の疑似DOP（以下、XDOPtrk(i、j)）を式(18)、Y方向の疑似DOP（以下、YDOPtrk(i、j)）を式(19)で定義する。さらに、水平面の疑似DOPであるHDOPtrk(i、j)を式(17)の表記にならい、式(20)で定義する。このように疑似DOPを定義すると、式（１６）の第３式で示されるHDOPはセンサ組の疑似DOPを用いて表すことができることが分かる。
【００２８】
0006358006.tif 000018
0006358006.tif 000019
0006358006.tif 000020
0006358006.tif 000021
0006358006.tif 000022
ここで、
0006358006.tif 000023
0006358006.tif 000024
0006358006.tif 000025
0006358006.tif 000026
0006358006.tif 000027
0006358006.tif 000028
0006358006.tif 000029
である。ここで、
0006358006.tif 000030
はri、jのxに関する勾配と呼んでも構わない。
【００２９】
このように、総合的な位置精度を表すパラメータであるHDOPを、本実施の形態で定義したセンサ組ごとの疑似DOPによって表すことができる。言い換えれば、総合的な位置精度を表すパラメータはセンサ組ごとの疑似DOPにより決定され、疑似DOPが小さく観測精度の高いセンサ組ごとの観測値を選定することにより、少ない数の観測値を用いて目標の位置推定を高精度に行えることが分かる。
【００３０】
本実施の形態では、センサ組ごとの疑似DOP算出部４２から出力される疑似DOPはXDOPtrk(i、j)、YDOPtrk(i、j)、HDOPtrk(i、j)のいずれかとする。センサ組優先選択部４３は各センサ組の疑似DOP算出部４２から出力される疑似DOPを受け取ると、距離差算出部３から受け取った同一時刻に入力される各センサ組で得られた距離差観測値に対して、疑似DOP、例えばHDOPtrkが小さい順に非同期追尾処理部５へ出力する。
【００３１】
また、疑似DOP算出部４２は精度を向上したい方向についてXDOPtrkあるいはYDOPtrkの小さい順に非同期追尾処理部５へ出力し、センサ組の優先度を決める方法もある。この際、X、Yどちらの方向を改善するか否かを判断するために、後段の非同期追尾処理部５より出力される推定誤差共分散行列（式(24)）のX方向の位置誤差分散σxx2、Y方向の位置誤差分散σyy2を比較し、誤差が大きい方向を改善することとしてもよい。
【００３２】
一例として、図２に本実施の形態においての目標追尾処理を行う場合の追尾処理の概念図を示す。図２では、各時刻におけるX、Y方向の誤差分布が示されており、Y方向の推定誤差がX方向の推定誤差よりも大きくなっている。例えば、非同期追尾処理部５における追尾処理で算出される式(24)の推定誤差共分散行列より、Y方向の精度向上に貢献する距離差観測値を利用することでY方向の精度を向上することが可能となる。
0006358006.tif 000031
σxx2：X方向の位置誤差分散
σyy2：Y方向の位置誤差分散
σvxvx2：X方向の速度誤差分散
σvyvy2：Y方向の速度誤差分散  
【００３３】
図３に式(24)の推定誤差共分散行列を用いてセンサ組の優先度を決める場合の距離差観測値選択部４及び非同期追尾処理部５の処理フローを示す。まず、非同期追尾処理部５は追尾処理を実施し、最新時刻tsにおいて目標状態ベクトルの推定値及び推定誤差共分散行列を算出する（ST1）。以降の処理では、追尾処理周期内で異なる時刻に距離差観測値が複数得られた場合を想定し、前記距離差観測値のそれぞれの取得時刻における予測位置を算出し、以下のST3〜ST8の一連の処理を繰り返すこととする。また、予め予測時刻ΔTi(i=1、…、L)を設定しておき、ΔTi秒後におけるST2〜ST6の処理をL回分繰り返し処理しておいても構わない。すなわち、まず、i=1とする（ST2）。次に、予測部４１は距離差観測値が取得される時刻tiと最新時刻tsとの時刻差（式(25)）を算出し、式(26)によって距離差観測値が取得される時刻tiにおける予測位置xp(ti)、yp(ti)を算出する（ST3）。 
0006358006.tif 000032
0006358006.tif 000033
0006358006.tif 000034
0006358006.tif 000035
0006358006.tif 000036
疑似DOP算出部４２は前記予測位置における疑似DOPを算出する（ST4）。疑似DOP算出部４２は前記推定誤差共分散行列のX方向誤差分散とY方向誤差分散を比較し、X方向誤差分散がY方向誤差分散より大きい場合（ST5）は、X方向の疑似DOPが小さい順に追尾処理に入力するようにTDOA観測値を優先的に並び替える（ST6）。ST４において、Y方向誤差分散がX方向誤差分散より大きい場合は、疑似DOP算出部４２はY方向の疑似DOPが小さい順に追尾処理に入力するようにTDOA観測値を優先的に並び替える（ST7）。また、ST5では距離差観測時刻ti(i=1、…、L)に跨がった全距離差観測値を並び替えても良いこととする。疑似DOP算出部４２はセンサ組の優先選択を行う。具体的には、ST6またはST7で並び替えた順に非同期追尾処理部５へ入力する（ST8）。ここで、予め処理上限数を決めておけば上限数に達した時点で処理を打ち切っても良い。 最後に、パラメータiがLであれば(ST9)終了し、パラメータiがそれ以外の値であれば、ST3の処理に戻る。
【００３４】
本処理によって、図4のように各距離差観測時刻において、優先センサ組による距離差観測値を選択して追尾処理を実施する。図４では、予測時刻Δt1秒後、 Δt2秒後の予測位置を算出するにあたって、それぞれ異なるセンサ組による観測値が選択されている。
【００３５】
非同期追尾処理部５は、距離差観測値選択部３より出力される各センサ組の距離差を入力とし、非線形フィルタによる追尾処理を実施し、位置及び速度を出力することとする。ここで、非線形フィルタとして、拡張カルマンフィルタ、アンセンテッドカルマンフィルタ、粒子フィルタ等を用いても良い。
【００３６】
以上のように、実施の形態１による目標追尾装置１によれば、同時に追尾処理に入力される距離差観測値について、追尾位置精度に貢献する順に距離差観測値を選択し、入力することで、演算負荷の制約がある場合においても追尾精度を確保することができる。特に多目標環境では、処理すべき距離差観測値数が多く、本実施の形態が有効となる。
【００３７】
また、ここまで2次元位置で説明したが、3次元位置についても同様の考え方で容易に拡張できる。さらに距離差だけではなくドップラ速度差が得られる場合も同様の考えで拡張しても良い。
【００３８】
このように、本実施の形態の目標追尾装置は、複数のセンサにより観測された目標の観測値を出力する観測値出力部２と、観測値出力部２から出力された観測値に基づき、複数のセンサに含まれる２つのセンサをセンサ組として、センサ組を構成する２つのセンサと目標との距離差であるセンサ組の距離差を異なるセンサ組に対して算出する距離差算出部３と、異なるセンサ組の距離差の観測精度を算出する観測精度算出部である疑似DOP算出部４２及び、疑似DOP算出部４２で算出された異なるセンサ組の距離差の観測精度に基づいて距離差算出部３により算出された前記異なるセンサ組の距離差の中から少なくとも一つのセンサ組の距離差を選択する優先選択部を含む距離差観測値選択部４と、距離差観測値選択部４で選択された少なくとも一つのセンサ組の距離差に基づき、目標の位置及び速度を算出する追尾処理を行う追尾処理部と、を備えたことを特徴とする。このような構成により、追尾精度に貢献する距離差観測値を選択して用いるため、追尾精度を確保しつつ、追尾処理負荷を低減することが可能となる。
【００３９】
また、本実施の形態の目標追尾装置は、距離差観測値選択部４は目標の将来時刻における予測位置を算出する予測部４１と、予測部４１で算出された予測位置での異なるセンサ組の距離差の観測精度を算出する観測精度算出部である疑似DOP算出部４２と、疑似DOP算出部４２で算出された異なるセンサ組の距離差の観測精度に基づき、異なるセンサ組の距離差の中から少なくとも一つのセンサ組の距離差を選択する優先選択部４３と、を備えたことを特徴とする。このような構成により、追尾処理に必要となる将来時刻を想定して、追尾精度に貢献する距離差観測値を選択することができる。その結果、追尾に必要となる観測値を効率的に選択することが可能となる。
【００４０】
また、本実施の形態の目標追尾装置では、センサ組の距離差の観測精度は一つの空間方向または水平面における観測精度であることを特徴とする。ここで、一つの空間方向の代表的な例はX方向、Y方向などがあげられる。また、水平面における観測精度としては、水平面のDOPであるHDOPが代表的な例である。このような構成により、一つの空間方向、または水平方向に絞って、観測精度を向上するための追尾処理を行うことが可能となる。
【００４１】
また、本実施の形態の目標追尾装置では、センサ組の距離差の観測精度は、センサ組に含まれる２つのセンサと目標との位置関係に基づき定義されるパラメータであることを特徴とする。この関係は式(21)(22)によって表されている。このような構成により、センサ組に含まれるセンサと目標から定まる観測精度を用いて、追尾精度に貢献する距離差観測値を効率的に選択することができる。また、このパラメータは、センサ組に含まれる２つのセンサの目標からの距離差の推定誤差の一つの空間方向または水平面に対する勾配とすることもできる。このような構成により、一つの空間方向、または水平方向に絞って、観測精度を向上するための追尾処理を行うことが可能となる。
【００４２】
また、このパラメータは、センサ組に対して定義される疑似的なDOPとすることができる。このような構成により、従来から総合的な位置精度を表すパラメータとして知られているHDOPと同様の形態でセンサ組ごとの位置精度を円滑に表すことができ、追尾精度に貢献する距離差観測値の観測精度を効率的に演算することができる。
【００４３】
また、実施の形態の目標追尾装置では、追尾処理部は複数の空間方向に対して前記目標の推定位置誤差を算出する手段を含み、距離差観測値選択部４はこの追尾処理部より出力される複数の空間方向に対する目標の推定位置誤差の中から推定位置誤差の大きい空間方向を選択し、選択された空間方向に対するセンサ組の距離差の観測精度に基づき前記少なくとも一つのセンサ組の距離差を選択する構成を備えることを特徴とする。このような構成により、精度を向上したい方向に対して観測精度の良いセンサ組の距離差観測値を用いて追尾処理部で追尾処理を行うことができる。  
【００４４】
実施の形態２．
実施の形態２では、実施の形態１において予測位置ごとに疑似DOPを計算していた処理を簡略化するために、疑似DOPを予測し、予測された疑似DOPを用いて距離差観測値の優先度を決定する。
【００４５】
図５は、実施の形態２の目標追尾装置１を示す構成図である。図５に示すように、本実施例の目標追尾装置１は、観測値出力部２、距離差算出部３、距離差観測値選択部６、非同期追尾処理部５を備えている。
【００４６】
本実施の形態では、センサ組i、jの疑似DOP であるXDOPtrk(i、j)のxに関する勾配
0006358006.tif 000037
を式(30)のように定義する。式(31)のように時刻Δt秒後にX軸方向にΔX、Y軸方向にΔY分位置が変化した場合、本来は式(21)を用いて算出する必要がある。
0006358006.tif 000038
0006358006.tif 000039
【００４７】
ここで、式(31)を簡略化して算出する方法を以下に示す。まず、式(31)について2次元位置x、yについてΔXおよびΔY分ずれた位置における勾配を式(32)で近似する。
0006358006.tif 000040
さらに、位置変化量ΔXおよびΔYを追尾処理から得たサンプル時刻kにおける推定速度ベクトル
0006358006.tif 000041
と時間差Δtで表す。このとき、式(33)、 (34)が成り立つ。
0006358006.tif 000042
0006358006.tif 000043
式(32)、 (33)、 (34)より、式(35)が導出される。
0006358006.tif 000044
ここで、式(35)のA、Bは式(36)、 (37)に示すように時間差Δtに依存しない固定値である。
0006358006.tif 000045
0006358006.tif 000046
【００４８】
以上のように、任意の時間差ΔｔにおけるXDOPtrk(i、j)のxに関する勾配について、式(35)を用いて算出する。また、YDOPtrk(i、j)のyに関する勾配についても同様の手順で算出する。
【００４９】
次に、本実施の形態に目標追尾装置の動作を説明する。ここで、観測値出力部２、距離差算出部３、非同期追尾処理部５は実施の形態１と同様のため省略する。
【００５０】
距離差観測値選択部６は、勾配予測部６１、疑似DOP算出部６２、センサ組優先選択部６３を備え、勾配予測部６１では、まず非同期追尾処理部５の追尾処理により得られるサンプル時刻kにおける推定位置ベクトルxs、ysを入力とし、各センサ組の勾配初期値を算出する。さらに、勾配初期値と式(35)を用いて勾配を予測する。疑似DOP算出部６２では勾配予測部６１で予測された勾配に基づいて疑似DOPを算出する。他は実施の形態と同様のため、省略する。
【００５１】
また、ここまで2次元位置で説明したが、3次元位置についても同様の考え方で算出しても構わない。
【００５２】
本実施の形態の構成では、疑似DOPを計算していた処理を実施の形態１よりも簡略化するために、疑似DOPを予測し、予測された疑似DOPを用いて距離差観測値の優先度を決定する。その結果、実施の形態１に比べて更に演算負荷を抑えることが可能となる。
【００５３】
このように、本実施の形態の目標追尾装置では、目標の将来時刻におけるセンサ組の距離差の観測精度の一つの空間方向または水平面に対する勾配を算出する勾配予測部６１と、勾配予測部６１で算出された勾配に基づき異なるセンサ組の距離差の観測精度を算出する観測精度算出部である疑似DOP算出部６２と、前記疑似DOP算出部６２で算出された前記異なるセンサ組の距離差の観測精度に基づき、前記異なるセンサ組の距離差の中から前記少なくとも一つのセンサ組の距離差を選択する優先選択部であるセンサ組優先選択部６３と、を備えたことを特徴とする。このような構成を用いることにより、実施の形態１に比べて更に演算負荷を抑えることが可能となる。
【００５４】
実施の形態３．
実施の形態３では、実施の形態１と構成が異なり、非同期追尾処理の代わりに測位部及び同期追尾処理を備えた構成を示す。
【００５５】
図６は、実施の形態２の目標追尾装置１を示す構成図である。図６に示すように、本実施例の目標追尾装置１は、観測値出力部２、距離差算出部３、距離差観測値選択部４、測位部７、同期追尾処理部８を備えている。
【００５６】
測位部７では、背景技術で述べたように同時に観測した全ての距離観測値を用いて式(8)を構成し、式(11)によって修正ベクトルを算出する。しかし、距離観測値数が多い場合には行列計算時の負荷が大きくなる。実施の形態３では、実施の形態１で述べた距離差観測値の貢献度を用いて距離差観測値選択部４がセンサ組毎の距離差観測値の優先度を決定し、測位部７が優先度順に上位N個の距離観測値に限定し、測位処理を実施することで演算負荷を低減する。
【００５７】
次に、動作を説明する。実施の形態３の測位部７では、距離差観測値選択部４より出力される各センサ組の距離差の中から疑似DOPの値が小さい距離差観測値から上位N個までの観測値に限定し、式(8)のヤコビアンの行列サイズを縮小し、測位位置を算出することができる。行列サイズが小さくなることで測位部７の演算負荷を抑えることになる。同期追尾処理部８では、測位部７より出力される測位位置を入力として、目標の位置及び速度を算出する。
【００５８】
他の処理は実施の形態１、２と同様のため省略する。
【００５９】
このように、本実施の形態の目標追尾装置１は、疑似DOPの値が小さい距離差観測値から上位N個までの観測値に限定して同期追尾処理を実行する。
【００６０】
すなわち、本実施の形態の目標追尾装置では、距離差観測値選択部４において２以上の所定数のセンサ組の距離差が選択され、追尾処理部は同期型追尾処理部８であることを特徴とする。このような構成を用いることにより、その結果、全てのセンサ組の距離差観測値を用いる従来の目標追尾処理と比較して、位置観測精度を同程度に保ちつつ演算量を低減することが可能である。
【符号の説明】
【００６１】
１：目標追尾装置、２：観測値出力部、３：距離差算出部、４：距離差観測値選択部、５：非同期追尾処理部、６：距離差観測値選択部、７：測位部、８：同期追尾処理部、４１：予測部、４２：疑似DOP算出部、４３：センサ組優先選択部、６１：勾配予測部、６２：疑似DOP算出部、６３：センサ組優先選択部
【図１】
0006358006.tif 000047
【図２】
0006358006.tif 000048
【図３】
0006358006.tif 000049
【図４】
0006358006.tif 000050
【図５】
0006358006.tif 000051
【図６】
0006358006.tif 000052