追尾装置
【課題】バイアス誤差推定精度を向上させた追尾装置を得る。
【解決手段】目標Ｔの動態データを取得する第１のセンサ１１と、目標Ｔの距離、仰角および方位角を取得する第２のセンサ１２と、動態データを第２のセンサ１２の航跡出力時刻に時刻合わせを行う補正処理部４０と、時刻合わせが行われた動態データと第２のセンサ１２の航跡とを比較して一致度を出力する一致度判定処理部５０と、一致度に基づき、バイアス誤差推定処理部７０における予測処理で使用する予測ベクトル算出式を切替える運動モデル設定処理部６０と、運動モデル設定処理部６０の設定結果に基づき、カルマンフィルタによるバイアス誤差推定処理を行うバイアス誤差推定処理部７０と、バイアス誤差ベクトル推定値を蓄積するバイアス誤差データベース８０と、バイアス誤差ベクトル推定値を用いてバイアス誤差補正値を算出するバイアス誤差補正処理部９０とを備える。
【解決手段】目標Ｔの動態データを取得する第１のセンサ１１と、目標Ｔの距離、仰角および方位角を取得する第２のセンサ１２と、動態データを第２のセンサ１２の航跡出力時刻に時刻合わせを行う補正処理部４０と、時刻合わせが行われた動態データと第２のセンサ１２の航跡とを比較して一致度を出力する一致度判定処理部５０と、一致度に基づき、バイアス誤差推定処理部７０における予測処理で使用する予測ベクトル算出式を切替える運動モデル設定処理部６０と、運動モデル設定処理部６０の設定結果に基づき、カルマンフィルタによるバイアス誤差推定処理を行うバイアス誤差推定処理部７０と、バイアス誤差ベクトル推定値を蓄積するバイアス誤差データベース８０と、バイアス誤差ベクトル推定値を用いてバイアス誤差補正値を算出するバイアス誤差補正処理部９０とを備える。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、センサのバイアス誤差を推定する機能を備えた追尾装置に関するものである。
【背景技術】
【０００２】
従来から、追尾装置においては、センサのバイアス誤差を推定する際に、目標の運動モデルを使用せずにバイアス誤差の推定が行われている（たとえば、特許文献１参照）。
ここで、図１１を参照しながら、追尾装置に用いられるセンサのバイアス誤差を補正する目的について説明する。
【０００３】
図１１は２つのセンサＡ、Ｂの各々のバイアスを補正するＰＰＩ（Ｐｌａｎ Ｐｏｓｉｔｉｏｎ Ｉｎｄｉｃａｔｏｒ）の表示例を示す説明図である。
図１１（ａ）において、２つのセンサＡ、Ｂは、それぞれ、方位方向（破線座標参照）にバイアス誤差を有するものとし、３つの目標Ｔ１、Ｔ２、Ｔ３（○印、△印、□印参照）を同時に観測しているものとする。
【０００４】
このとき、センサＡ、Ｂで同一目標を観測していたとしても、各センサＡ、Ｂがバイアス誤差を有することから、センサＡ、Ｂで表示された目標同士を重ね合わせた場合に、目標同士が重ならず、図１１（ｂ）に示すように、各目標Ｔ１、Ｔ２、Ｔ３がだぶって見える状況となる。
【０００５】
そこで、図１１（ｂ）のようなバイアス誤差により目標Ｔ１、Ｔ２、Ｔ３がたぶって見える状況を改善するために、バイアス誤差を推定して、バイアス誤差推定値を用いた補正を行うことにより、図１１（ｃ）に示すように、２つのセンサＡ、Ｂの表示を重ね合わせることが要求される。
つまり、各センサＡ、Ｂで観測した目標の状態量（目標の位置、速度、加速度など）を一致させることが必要となる。
【０００６】
ここで、バイアス誤差としては、たとえば距離および角度による極座標で定義してもよい。また、距離、仰角および方位角を得る３次元レーダの場合には、距離、仰角および方位角でバイアス誤差を定義してもよい。
また、バイアス誤差として、距離および方位角、仰角および方位角、距離および仰角、のいずれで構成してもよく、さらに、距離のみ、仰角のみ、または方位角のみで構成してもよい。
【０００７】
つまり、バイアス誤差の成分は、事前に自由に定義することができる。
同様に、バイアス誤差は、直交座標Ｘ、Ｙ、Ｚの成分からなるバイアス誤差としてもよく、また、バイアス誤差の成分は、上記距離、仰角および方位角の極座標の組合せと同様に、直交座標、または緯度、経度および高度からなる座標、または地球中心座標など、他の種々の座標で定義することができる。
【０００８】
さらに、バイアス誤差の成分は、極座標のみで定義することもでき、極座標の３成分のうちの２成分または１成分で定義することもでき、極座標と直交座標との組合せで定義することもでき、種々の組合せで定義することができる。
【０００９】
従来の追尾装置において、上記２台のセンサＡ、Ｂのバイアス誤差を推定する際には、２台のセンサＡ、Ｂの時刻同期が取れていることが前提である。
しかし、従来装置においては、目標の運動を考慮して時刻同期を取るような処理は行われていないので、従来技術で時刻同期を取る方法を実現しようとする場合を考える。
【００１０】
たとえば、各目標Ｔ１、Ｔ２、Ｔ３が等速直線運動を行うなどの仮定を前提として、各センサＡ、Ｂの或る時刻における観測値または航跡を、基準時刻まで外挿するなどの時刻同期を取る方法が考えられる。
ただし、実際には、目標Ｔ１、Ｔ２、Ｔ３の運動は未知であることから、等速直線運動の仮定が崩れると、各センサＡ、Ｂの観測値または航跡を外挿した結果が悪くなるので、かえってバイアス誤差推定精度が劣化するといった問題が考えられる。
【先行技術文献】
【特許文献】
【００１１】
【特許文献１】特許第３３０２８７０号公報
【発明の概要】
【発明が解決しようとする課題】
【００１２】
従来の追尾装置は、目標が等速直線運動を行うなどの仮定を前提として、各センサの或る時刻における観測値または航跡を、基準時刻まで外挿するなどの時刻同期方法を適用してバイアス誤差を推定しているので、等速直線運動の仮定が崩れると、かえってバイアス誤差推定精度が劣化するという課題があった。
【００１３】
この発明は、上記のような課題を解決するためになされたものであり、バイアス誤差推定精度を向上させた追尾装置を得ることを目的とする。
【課題を解決するための手段】
【００１４】
この発明に係る追尾装置は、目標の動態データを取得する第１のセンサと、目標の距離、仰角および方位角を取得する第２のセンサと、動態データを第２のセンサの航跡出力時刻に時刻合わせを行う補正処理部と、補正処理部により時刻合わせが行われた動態データと第２のセンサの航跡とを比較して両者の一致度を出力する一致度判定処理部と、一致度に基づき、バイアス誤差推定処理部における予測処理で使用する予測ベクトル算出式を切替える運動モデル設定処理部と、運動モデル設定処理部の設定結果に基づき、カルマンフィルタによるバイアス誤差推定器を用いてバイアス誤差推定処理を行うバイアス誤差推定処理部と、バイアス誤差推定処理部により得られるバイアス誤差ベクトル推定値を時系列に蓄積するバイアス誤差データベースと、バイアス誤差データベース内のバイアス誤差ベクトル推定値を用いてバイアス誤差補正値を算出するバイアス誤差補正処理部と、制御工学における遅延部と、を備えたものである。
【発明の効果】
【００１５】
この発明によれば、カルマンフィルタによるバイアス誤差推定処理を行う際に、第１のセンサの動態データと第２のセンサの航跡との一致度を比較して、予測ベクトル算出式を適切に選択することにより、目標が旋回運動を行う場合であっても高精度のバイアス誤差推定値を得ることができる。
【図面の簡単な説明】
【００１６】
【図１】この発明の実施の形態１に係る追尾装置を示すブロック構成図である。
【図２】ＸＹＺ座標と姿勢角との関係を示す説明図である。
【図３】ＵＶＷ座標を示す説明図である。
【図４】第２のセンサが有するバイアス誤差を直交座標で示す説明図である。
【図５】第２のセンサが有するバイアス誤差を極座標で示す説明図である。
【図６】第２のセンサが有するバイアス誤差を回転角で示す説明図である。
【図７】図１内の補正処理部による時刻合わせ処理を示す説明図である。
【図８】この発明の実施の形態４に係る追尾装置を示すブロック構成図である。
【図９】この発明の実施の形態５に係る追尾装置を示すブロック構成図である。
【図１０】この発明の実施の形態６に係る追尾装置を示すブロック構成図である。
【図１１】一般的な追尾装置のバイアス誤差を補正する目的を示す説明図である。
【発明を実施するための形態】
【００１７】
実施の形態１．
以下、図面を参照しながら、この発明の好適な実施の形態について説明する。
図１はこの発明の実施の形態１に係る追尾装置を示すブロック構成図である。
図１において、追尾装置は、複数のセンサを含むセンサ群１０と、補正処理部４０と、一致度判定処理部５０と、運動モデル設定処理部６０と、カルマンフィルタによるバイアス誤差推定器（図示せず）を含むバイアス誤差推定処理部７０と、バイアス誤差データベース８０と、バイアス誤差補正処理部９０と、制御工学における遅延部１００とを備えている。
【００１８】
センサ群１０は、第１のセンサ１１と、任意数の第２のセンサ１２とを備えており、第１および第２のセンサ１１、１２は、目標Ｔを同時に観測している。
第１のセンサ１１は、基準座標に対するバイアス誤差が存在しないセンサからなり、目標に搭載されたＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ：全地球測位システム）／ＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ：慣性計測装置）からの目標Ｔの動態データを取得し、動態データを補正処理部４０に入力する。
【００１９】
なお、動態データとは、目標Ｔの位置、速度、姿勢角、姿勢角速度、機体加速度からなるデータとする。また、以下の説明において、特に断りがなければ、各データにはすべて時刻が付与されているものとする。
動態データは、以下の式（１）で表される状態ベクトルを有するものとする。
【００２０】
Ｘ（Ｏ）＝［Ｐ、Ｖ、δＰ、δＶ、Ａ］’ ・・・（１）
【００２１】
式（１）において、Ｘ（０）は、第１のセンサ１１における状態ベクトルを表す。
また、記号「’」は、ベクトルおよび行列の転置を表しており、このことは後述の数式においても同様である。
【００２２】
さらに、式（１）において、ＰはＸＹＺ座標における３次元の位置成分を表し、ＶはＸＹＺ座標における３次元の速度成分を表し、δＰはＸＹＺ座標のＸ、Ｙ、Ｚ各軸からの基準の軸からの姿勢角を表し、δＶはＸＹＺ座標のＸ、Ｙ、Ｚ各軸からの姿勢角の速度を表し、ＡはＵＶＷ座標における３次元加速度を表す。
【００２３】
図２はＸＹＺ座標と姿勢角［ｄｘ、ｄｙ、ｄｚ］との関係を示す説明図である。
図２において、姿勢角［ｄｘ、ｄｙ、ｄｚ］は、ｘ軸、ｙ軸およびｚ軸周りの回転角に相当する。
【００２４】
図３はＵＶＷ座標を示す説明図である。
図３において、ＵＶＷ座標は、レーダ位置を原点Ｏとし、目標Ｔの速度ベクトル方向をｕ軸の正方向とし、水平面に平行な面内で速度ベクトルに対して垂直で右手の方向をｖ軸の正方向とし、速度ベクトルに対して垂直で下向きの方向をｗ軸の正方向とした座標系である。
【００２５】
図１に戻り、第２のセンサ１２は、たとえば基準座標に対するバイアス誤差を有するセンサからなり、目標Ｔの距離、仰角および方位角を取得する。
たとえば、第２のセンサ１２を原点とする北基準の直交座標系において、距離、仰角および方位角のバイアス誤差を有するものとする。
【００２６】
つまり、第２のセンサ１２の観測値（距離、仰角および方位角）には、基準座標に対するバイアス誤差が付加されている。
また、第２のセンサ１２は、基準座標に対するバイアス誤差が付加された観測値を入力情報とした追尾を行い、追尾結果として、基準座標に対するバイアス誤差が付加された航跡を算出する。
【００２７】
ここで、第２のセンサ１２の航跡は、たとえば、以下の式（２）のように、第２のセンサ１２における状態ベクトルＸ（Ａ）（位置成分Ｐおよび速度成分Ｖからなる）を有するものとする。
【００２８】
Ｘ（Ａ）＝［Ｐ、Ｖ］’ ・・・（２）
【００２９】
式（２）において、位置成分ＰはＸＹＺ座標における３次元位置、速度成分ＶはＸＹＺ座標における３次元速度をそれぞれ表す。
また、第２のセンサ１２の航跡は、状態ベクトルＸ（Ａ）の誤差共分散行列を有する。
第２のセンサ１２の航跡の誤差共分散行列は、ＥＲＲＭＡＴ（Ａ）で表される。
【００３０】
ここで、第２のセンサ１２が有するバイアス誤差は、図４のように直交座標で定義することもでき、図５のように極座標で定義することもでき、図６のように回転角で定義することもできる。
【００３１】
図４、図５、図６において、第２のセンサ１２が有するバイアス誤差は、それぞれ、１点鎖線で示されている。
また、ここでは図示しないが、前述と同様に、バイアス誤差は、緯度、経度および高度で定義することもでき、どのような座標でもバイアス誤差を定義することは可能である。
【００３２】
第２のセンサ１２は、観測値として距離、仰角および方位角を補正処理部４０に入力するとともに、第２のセンサ１２で追尾フィルタにより算出した航跡（第２のセンサ１２の航跡）を補正処理部４０に入力する。
【００３３】
補正処理部４０は、第１のセンサ１１から得られる動態データに対して、第２のセンサ１２の航跡が得られた最新時刻まで時刻合わせ処理を行い、時刻合わせが行われた動態データＸ＃（Ｏ）を一致度判定処理部５０に入力する。
時刻合わせが行われた動態データＸ＃（Ｏ）は、以下の式（３）のように定義される。
【００３４】
Ｘ＃（Ｏ）＝［Ｐ＃、Ｖ＃］’ ・・・（３）
【００３５】
式（３）において、右辺のＰ＃、Ｖ＃は、第１のセンサ１１から得られる動態データに対して、第２のセンサ１２の航跡が得られた最新時刻まで時刻合わせ処理を施した動態データの位置成分および速度成分を、それぞれ表す。
【００３６】
図７は補正処理部４０による時刻合わせ処理を示す説明図である。
図７において、第１のセンサ１１から補正処理部４０への動態データの入力タイミングは時刻ｔ１であり、第２のセンサ１２の航跡の補正処理部４０への入力タイミングは時刻ｔ２であり、第２のセンサ１２の航跡と動態データとの時刻差Δｔ（＝ｔ２−ｔ１）が生じるが、補正処理部４０の出力タイミングは、時刻ｔ２に合わせられる。
【００３７】
このように、時刻合わせが行われた（第２のセンサ１２の航跡が得られた最新時刻まで時刻合わせ処理が施された）動態データＸ＃（Ｏ）は、以下の式（４）のように求められる。
【００３８】
Ｘ♯（Ｏ）＝ΦＸ（Ｏ）＋α（δＰ、δＶ、Ａ） ・・・（４）
【００３９】
式（４）において、Ｘ（Ｏ）は第１のセンサ１１から得られる動態データであり、Φは動態データＸ（Ｏ）を第２のセンサ１２の航跡が得られた最新時刻まで等速直線運動モデルにしたがい時刻あわせを行う行列である。
【００４０】
また、α（δＰ、δＶ、Ａ）は、姿勢角δＰ、姿勢角速度δＶおよびＵＶＷ座標における３次元加速度Ａに対して、第２のセンサ１２の航跡が得られた最新時刻まで時刻あわせ処理が施されたときの、ＸＹＺ座標における位置成分Ｐおよび速度成分Ｖへの運動補償係数を表す。
【００４１】
したがって、式（４）から分かるように、時刻合わせが行われた動態データＸ＃（０）は、等速直線運動モデルにしたがい外挿された動態データＸ（Ｏ）の位置成分Ｐおよび速度成分Ｖに、運動補償係数α（δＰ、δＶ、Ａ）を加算した値となる。
このように、補正処理部４０からは、第２のセンサ１２の航跡すなわち状態ベクトルＸ（Ａ）と、第２のセンサ１２の航跡が得られた最新時刻まで時刻合わせが行われた動態データＸ＃（Ｏ）とが出力される。
【００４２】
一致度判定処理部５０は、第２のセンサ１２の航跡（状態ベクトル）Ｘ（Ａ）と、第２のセンサ１２の航跡が得られた最新時刻まで時刻合わせが行われた動態データＸ＃（Ｏ）との一致度を算出する。
このとき、両者が一致しているか否かは、以下の式（５）、式（６）のように判定される。
【００４３】
｜Ｘ（Ａ）−Ｘ＃（Ｏ）｜≦ＴＨ、であれば「一致フラグ」を出力 ・・・（５）
｜Ｘ（Ａ）−Ｘ＃（Ｏ）｜＞ＴＨ、であれば「不一致フラグ」を出力 ・・・（６）
【００４４】
式（５）、式（６）において、｜ ｜はノルムを表しており、ユークリッドノルムであっても、最大値を取るノルムであってもよく、ユーザが自由に事前に定義することができる。
なお、一致度は、式（５）、式（６）の左辺｜Ｘ（Ａ）−Ｘ＃（Ｏ）｜で定義される。
また、式（５）、式（６）の右辺は、事前に設定された閾値ＴＨを示している。
【００４５】
一致度判定処理部５０は、式（５）、式（６）を用いて、第２のセンサ１２の航跡（状態ベクトル）Ｘ（Ａ）と、第２のセンサ１２の航跡が得られた最新時刻まで時刻合わせが行われた動態データＸ＃（Ｏ）とが一致するか否かを判定し、一致する場合には一致フラグを生成し、不一致の場合には不一致フラグを生成し、一致度（判定結果）として運動モデル設定処理部６０に入力する。
【００４６】
運動モデル設定処理部６０は、一致度判定処理部５０から得られる一致度（一致フラグまたは不一致フラグ）に基づき、後段のバイアス誤差推定処理部７０で使用されるカルマンフィルタによるバイアス誤差推定器による予測ベクトル算出用の制御信号を、バイアス誤差推定処理部７０に入力する。
【００４７】
すなわち、運動モデル設定処理部６０は、一致度判定処理部５０から一致フラグが得られた場合には、以下の式（７）により予測ベクトルＸｐを算出するための制御信号を、バイアス誤差推定処理部７０に入力する。
【００４８】
Ｘｐ＝ΦＸｓ ・・・（７）
【００４９】
一方、一致度判定処理部５０から不一致フラグが得られた場合には、運動モデル設定処理部６０は、以下の式（８）により予測ベクトルを算出するための制御信号を、バイアス誤差推定処理部７０に入力する。
【００５０】
Ｘｐ＝ΦＸｓ＋α（δＰ、δＶ、Ａ） ・・・（８）
【００５１】
式（７）、式（８）において、Ｘｓは、バイアス誤差推定処理部７０（バイアス誤差推定器）における１サンプリング前の目標Ｔの状態ベクトル平滑値であり、Ｘｐは、バイアス誤差推定処理部７０における現時刻の目標Ｔの状態ベクトル予測値である。
【００５２】
バイアス誤差推定処理部７０は、第１のセンサ１１から得られる動態データの位置観測値と、第２のセンサ１２から得られる位置観測値とを、バイアス誤差推定処理部７０内のバイアス誤差推定器に入力する。
【００５３】
カルマンフィルタによるバイアス誤差推定器は、運動モデル設定処理部６０から得られる制御信号にしたがい、式（７）または式（８）を用いて予測ベクトルを算出し、予測ベクトルの算出値を用いて、第２のセンサ１２のバイアス誤差ベクトル推定値を算出し、バイアス誤差データベース８０に格納する。
【００５４】
このように、バイアス誤差推定処理部７０内のカルマンフィルタによるバイアス誤差推定器を用いてバイアス誤差ベクトルを推定する場合、バイアス誤差ベクトル推定値ｂｓとしては、以下の式（９）のように、第２のセンサ１２の距離、仰角および方位角の各バイアス誤差成分が得られる。
【００５５】
ｂｓ＝［ΔＲ、ΔＥ、ΔＡｚ］’ ・・・（９）
【００５６】
式（９）において、ΔＲ、ΔＥ、ΔＡｚは、バイアス誤差ベクトル推定値ｂｓの距離、仰角および方位角の各バイアス誤差成分である。
【００５７】
なお、距離バイアス誤差成分ΔＲを除いて、第２のセンサ１２の仰角および方位角のバイアス誤差成分ΔＥ、ΔＡｚを、バイアス誤差推定処理部７０内のバイアス誤差推定器で仮定する場合、バイアス誤差ベクトル推定値ｂｓは、以下の式（１０）から得られる。
【００５８】
ｂｓ＝［ΔＥ、ΔＡｚ］’ ・・・（１０）
【００５９】
バイアス誤差データベース８０は、バイアス誤差推定処理部７０で算出されたバイアス誤差ベクトル推定値ｂｓを、時系列的に、かつ複数の第２のセンサ１２ごとに蓄積する。
【００６０】
バイアス誤差補正処理部９０は、バイアス誤差データベース８０から得られる時系列のバイアス誤差ベクトル推定値ｂｓのＮサンプリング分の平均値または最新値（いずれを用いるかは事前にユーザにより決定される）を用いて、第２のセンサ１２のバイアス誤差補正処理に用いられるバイアス誤差を算出し、バイアス誤差補正値を遅延部１００に入力する。
【００６１】
すなわち、バイアス誤差補正処理部９０は、バイアス誤差補正処理をリアルタイム（バイアス誤差ベクトル推定値ｂｓの最新値）で行うことをユーザが決定した場合には、現時刻の第２のセンサ１２の観測値のバイアス誤差を補正するためのバイアス誤差補正値を遅延部１００に入力する。
【００６２】
一方、バイアス誤差補正処理を平均値で行う（リアルタイムで行わない）ことをユーザが決定した場合には、バイアス誤差補正処理部９０は、現時刻の第２のセンサ１２の観測値のバイアス誤差を補正するためのバイアス誤差補正値を出力せずに、ユーザがバイアス誤差推定の開始を指示した場合のみに、バイアス誤差補正値を遅延部１００に入力する。
【００６３】
最後に、遅延部１００は、バイアス誤差補正処理部９０から得られるバイアス誤差補正値を、第２のセンサ１２の最新時刻まで遅延させ、遅延後のバイアス誤差補正値をセンサ群１０に入力する。
【００６４】
これにより、センサ群１０内の第２のセンサ１２は、バイアス誤差補正処理部９０から遅延部１００を介して得られるバイアス誤差補正値に基づき、第２のセンサ１２で得られる観測値のバイアス誤差補正を行う。
【００６５】
以上のように、この発明の実施の形態１（図１）に係る追尾装置は、目標Ｔの動態データを取得する第１のセンサ１１と、目標Ｔの距離、仰角および方位角を取得する第２のセンサ１２と、動態データを第２のセンサの航跡出力時刻に時刻合わせを行う補正処理部４０と、補正処理部４０により時刻合わせが行われた動態データと第２のセンサ１２の航跡とを比較して両者の一致度を出力する一致度判定処理部５０と、一致度（判定フラグ）に基づき、バイアス誤差推定処理部７０における予測処理で使用する予測ベクトル算出式を切替える運動モデル設定処理部６０と、運動モデル設定処理部６０の設定結果（制御信号）に基づき、カルマンフィルタによるバイアス誤差推定器を用いてバイアス誤差推定処理を行うバイアス誤差推定処理部７０と、バイアス誤差推定処理部７０により得られるバイアス誤差ベクトル推定値を時系列に蓄積するバイアス誤差データベース８０と、バイアス誤差データベース８０内のバイアス誤差ベクトル推定値を用いてバイアス誤差補正値を算出するバイアス誤差補正処理部９０と、制御工学における遅延部１００と、を備えている。
【００６６】
一致度判定処理部５０は、動態データの３次元加速度Ａ、または、補正処理部４０から出力される運動補償係数に基づき、一致度を判定する。
このように、カルマンフィルタによるバイアス誤差推定処理を行う際に、第１のセンサ１１の動態データと、第２のセンサ１２の航跡との一致度を比較して、予測ベクトル算出式を適切に選択することにより、目標Ｔが旋回運動を行う場合であっても、高精度のバイアス誤差推定値を得ることが可能となる。
【００６７】
実施の形態２．
なお、上記実施の形態１（図１）では、一致度判定処理部５０において、第２のセンサ１２の航跡（状態ベクトル）Ｘ（Ａ）と、時刻合わせが行われた動態データＸ＃（Ｏ）との比較に基づき一致度を判定したが、ＵＶＷ座標における３次元加速度Ａのユークリッドノルムによる大きさに基づき一致度を判定してもよい。
【００６８】
この場合、一致度判定処理部５０は、ＵＶＷ座標における３次元加速度Ａのユークリッドノルムによる大きさが、事前に決めた閾値ＴＨ以下の場合には、「一致フラグ」を出力し、閾値ＴＨよりも大きい場合には、「不一致フラグ」を出力する。
【００６９】
以上のように、この発明の実施の形態２によれば、バイアス誤差推定処理部７０においてカルマンフィルタによるバイアス誤差推定処理を行う際に、第１のセンサ１１の動態データと第２のセンサ１２の航跡との一致度を、動態データのＵＶＷ座標における３次元加速度Ａのユークリッドノルムによる大きさと閾値ＴＨとの比較に基づいて判定することにより、前述と同様に、カルマンフィルタによるバイアス誤差推定処理部７０で使用する予測ベクトル算出式を適切に選択することができる。
したがって、目標Ｔが旋回運動を行う場合でも、高精度なバイアス誤差推定値を得ることが可能となる。
【００７０】
実施の形態３．
なお、上記実施の形態２では、一致度判定処理部５０において、ＵＶＷ座標における３次元加速度Ａのユークリッドノルムによる大きさに基づき一致度を判定したが、運動補償係数のユークリッドノルムによる大きさに基づき一致度を判定してもよい。
この場合、一致度判定処理部５０は、運動補償係数のユークリッドノルムによる大きさが事前に決めた閾値ＴＨ以下の場合には、「一致フラグ」を出力し、閾値ＴＨよりも大きい場合には、「不一致フラグ」を出力する。
【００７１】
以上のように、この発明の実施の形態３によれば、バイアス誤差推定処理部７０においてカルマンフィルタによるバイアス誤差推定処理を行う際に、第１のセンサ１１の動態データと第２のセンサ１２の航跡との一致度を、運動補償係数のユークリッドノルムによる大きさに基づく加速度と閾値ＴＨとの比較に基づいて判定することにより、前述と同様に、カルマンフィルタによるバイアス誤差推定処理部７０で使用する予測ベクトル算出式を適切に選択することができる。
したがって、目標Ｔが旋回運動を行う場合でも、高精度なバイアス誤差推定値を得ることが可能となる。
【００７２】
実施の形態４．
なお、上記実施の形態１〜３（図１）では、一致度（一致フラグまたは不一致フラグ）に応答する運動モデル設定処理部６０のみを用いて、バイアス誤差推定処理部７０（カルマンフィルタによるバイアス誤差推定器）で使用される予測ベクトル算出用の制御信号を生成したが、図８のように、ユーザの設定条件にしたがう運動モデル制御処理部１３０を追加してもよい。
【００７３】
図８はこの発明の実施の形態４に係る追尾装置を示すブロック構成図であり、前述（図１参照）と同様のものについては、前述と同一符号を付して詳述を省略する。
図８において、運動モデル設定処理部６０には、運動モデル制御処理部１３０が接続されている。
【００７４】
運動モデル制御処理部１３０は、所定の運動モデル（事前にユーザが動態データで設定する）を常に使い続けるための制御信号を生成し、これを運動モデル設定処理部６０に入力する。すなわち、運動モデル制御処理部１３０は、運動モデル設定処理部６０において動態データを使用し続けるための制御信号を出力する。
【００７５】
これにより、運動モデル設定処理部６０は、運動モデル制御処理部１３０からの制御信号に応答して、前述の式（８）に基づき、予測ベクトルを算出するための制御信号をバイアス誤差推定処理部７０に入力する。
【００７６】
以上のように、この発明の実施の形態４（図８）によれば、目標Ｔが常に旋回している状況において、その都度の毎回にわたり、式（７）、式（８）の予測ベクトル算出式の切替が頻繁に行われる可能性がある場合であっても、バイアス誤差推定処理部７０における予測ベクトル算出式として、式（８）に基づく制御信号にしたがう算出式を常に使い続けることができるので、目標Ｔが常に旋回運動を行う場合であっても、高精度なバイアス誤差推定値を得ることが可能となる。
【００７７】
実施の形態５．
なお、上記実施の形態４（図８）では、動態データを使用し続けるための制御信号を運動モデル設定処理部６０に入力する運動モデル制御処理部１３０を追加したが、図９のように、第２のセンサ１２から得られるサンプリングレートに基づく制御信号を運動モデル設定処理部６０に入力する運動モデル制御処理部１４０を追加してもよい。
【００７８】
図９はこの発明の実施の形態５に係る追尾装置を示すブロック構成図であり、前述（図１、図８参照）と同様のものについては、前述と同一符号を付して詳述を省略する。
図９において、補正処理部４０、一致度判定処理部５０、運動モデル設定処理部６０およびバイアス誤差推定処理部７０には、運動モデル制御処理部１４０が接続されている。
【００７９】
運動モデル制御処理部１４０は、第２のセンサ１２から得られるサンプリングレートが事前に決めたサンプリング閾値よりも小さい場合には、等速直線運動に基づく予測ベクトル算出処理によりバイアス誤差推定を行うための制御信号を運動モデル設定処理部６０に入力する。
【００８０】
すなわち、運動モデル制御処理部１４０は、第２のセンサ１２から得られるサンプリングレートがサンプリング閾値よりも小さい場合には、等速直線運動に基づく予測ベクトル算出処理を行うため、つまり、前述の式（７）に基づくバイアス誤差推定をバイアス誤差推定処理部７０で行うための制御信号を、運動モデル設定処理部６０に入力する。
【００８１】
これにより、バイアス誤差推定処理部７０は、式（７）による等速直線運動モデルを用いてバイアス誤差推定を行う。
また、運動モデル制御処理部１４０は、補正処理部４０、一致度判定処理部５０および運動モデル設定処理部６０に対して、処理停止信号を入力して各処理を中止し、センサ群１０からの観測値を、バイアス誤差推定処理部７０で使用する処理のみ動作させるようにする。
【００８２】
以上のように、この発明の実施の形態５（図９）によれば、バイアス誤差推定処理部７０においてカルマンフィルタによるバイアス誤差推定処理を行う際に、第２のセンサ１２のサンプリングレートが小さい場合には、実際の目標Ｔが旋回していたとしても、等速直線運動モデルに基づくバイアス誤差推定精度はあまり劣化しないと考えられるので、目標Ｔの旋回判定に関わる補正処理部４０、一致度判定処理部５０および運動モデル設定処理部６０の計算を中止することにより、演算負荷の低減を実現することができる。
【００８３】
また、一致度判定処理部５０における閾値ＴＨの誤設定に起因して、運動モデルの切替が頻繁に起こるような状況が生じた場合でも、等速直線運動モデルに固定することができるので、バイアス誤差推定精度の劣化を防ぐことができる。
さらに、前述と同様に、目標Ｔが旋回運動を行う場合でも、高精度なバイアス誤差推定値を得ることが可能となる。
【００８４】
実施の形態６．
なお、上記実施の形態１〜５（図１、図８、図９）では、言及しなかったが、図１０のように、第１のセンサ１１からの動態データに基づき、第２のセンサ１２の航跡を上書きするセンサ航跡更新処理部１５０を追加してもよい。
【００８５】
図１０はこの発明の実施の形態６に係る追尾装置を示すブロック構成図であり、前述（図１、図８、図９参照）と同様のものについては、前述と同一符号を付して詳述を省略する。
図１０において、補正処理部４０には、第１のセンサ１１の動態データに基づき、第２のセンサ１２の航跡を上書きするセンサ航跡更新処理部１５０が接続されている。
【００８６】
以上のように、この発明の実施の形態６によれば、第２のセンサ１２の航跡よりも精度が高い第１のセンサ１１の動態データが得られる状況下において、精度の低い（または、劣化している）第２のセンサ１２の航跡を、精度の高い第１のセンサ１１の動態データで上書きすることにより改善し、目標Ｔの追尾維持率を向上させることができる。
【００８７】
また、第２のセンサ１２の目標Ｔの追尾維持率が向上するので、第２のセンサ１２が目標Ｔの探知に失敗して、第２のセンサ１２から目標Ｔの観測値が出力されない状況を防ぐことが可能となり、結果として、バイアス誤差推定値を継続して計算し続けることができる。
さらに、前述と同様に、目標Ｔが旋回運動を行う場合でも、高精度なバイアス誤差推定値を得ることが可能となる。
【符号の説明】
【００８８】
１１ 第１のセンサ、１２ 第２のセンサ、４０ 補正処理部、５０ 一致度判定処理部、６０ 運動モデル設定処理部、７０ バイアス誤差推定処理部、８０ バイアス誤差データベース、９０ バイアス誤差補正処理部、１００ 遅延部、１３０、１４０ 運動モデル制御処理部、１５０ センサ航跡更新処理部、Ｔ 目標。
【技術分野】
【０００１】
この発明は、センサのバイアス誤差を推定する機能を備えた追尾装置に関するものである。
【背景技術】
【０００２】
従来から、追尾装置においては、センサのバイアス誤差を推定する際に、目標の運動モデルを使用せずにバイアス誤差の推定が行われている（たとえば、特許文献１参照）。
ここで、図１１を参照しながら、追尾装置に用いられるセンサのバイアス誤差を補正する目的について説明する。
【０００３】
図１１は２つのセンサＡ、Ｂの各々のバイアスを補正するＰＰＩ（Ｐｌａｎ Ｐｏｓｉｔｉｏｎ Ｉｎｄｉｃａｔｏｒ）の表示例を示す説明図である。
図１１（ａ）において、２つのセンサＡ、Ｂは、それぞれ、方位方向（破線座標参照）にバイアス誤差を有するものとし、３つの目標Ｔ１、Ｔ２、Ｔ３（○印、△印、□印参照）を同時に観測しているものとする。
【０００４】
このとき、センサＡ、Ｂで同一目標を観測していたとしても、各センサＡ、Ｂがバイアス誤差を有することから、センサＡ、Ｂで表示された目標同士を重ね合わせた場合に、目標同士が重ならず、図１１（ｂ）に示すように、各目標Ｔ１、Ｔ２、Ｔ３がだぶって見える状況となる。
【０００５】
そこで、図１１（ｂ）のようなバイアス誤差により目標Ｔ１、Ｔ２、Ｔ３がたぶって見える状況を改善するために、バイアス誤差を推定して、バイアス誤差推定値を用いた補正を行うことにより、図１１（ｃ）に示すように、２つのセンサＡ、Ｂの表示を重ね合わせることが要求される。
つまり、各センサＡ、Ｂで観測した目標の状態量（目標の位置、速度、加速度など）を一致させることが必要となる。
【０００６】
ここで、バイアス誤差としては、たとえば距離および角度による極座標で定義してもよい。また、距離、仰角および方位角を得る３次元レーダの場合には、距離、仰角および方位角でバイアス誤差を定義してもよい。
また、バイアス誤差として、距離および方位角、仰角および方位角、距離および仰角、のいずれで構成してもよく、さらに、距離のみ、仰角のみ、または方位角のみで構成してもよい。
【０００７】
つまり、バイアス誤差の成分は、事前に自由に定義することができる。
同様に、バイアス誤差は、直交座標Ｘ、Ｙ、Ｚの成分からなるバイアス誤差としてもよく、また、バイアス誤差の成分は、上記距離、仰角および方位角の極座標の組合せと同様に、直交座標、または緯度、経度および高度からなる座標、または地球中心座標など、他の種々の座標で定義することができる。
【０００８】
さらに、バイアス誤差の成分は、極座標のみで定義することもでき、極座標の３成分のうちの２成分または１成分で定義することもでき、極座標と直交座標との組合せで定義することもでき、種々の組合せで定義することができる。
【０００９】
従来の追尾装置において、上記２台のセンサＡ、Ｂのバイアス誤差を推定する際には、２台のセンサＡ、Ｂの時刻同期が取れていることが前提である。
しかし、従来装置においては、目標の運動を考慮して時刻同期を取るような処理は行われていないので、従来技術で時刻同期を取る方法を実現しようとする場合を考える。
【００１０】
たとえば、各目標Ｔ１、Ｔ２、Ｔ３が等速直線運動を行うなどの仮定を前提として、各センサＡ、Ｂの或る時刻における観測値または航跡を、基準時刻まで外挿するなどの時刻同期を取る方法が考えられる。
ただし、実際には、目標Ｔ１、Ｔ２、Ｔ３の運動は未知であることから、等速直線運動の仮定が崩れると、各センサＡ、Ｂの観測値または航跡を外挿した結果が悪くなるので、かえってバイアス誤差推定精度が劣化するといった問題が考えられる。
【先行技術文献】
【特許文献】
【００１１】
【特許文献１】特許第３３０２８７０号公報
【発明の概要】
【発明が解決しようとする課題】
【００１２】
従来の追尾装置は、目標が等速直線運動を行うなどの仮定を前提として、各センサの或る時刻における観測値または航跡を、基準時刻まで外挿するなどの時刻同期方法を適用してバイアス誤差を推定しているので、等速直線運動の仮定が崩れると、かえってバイアス誤差推定精度が劣化するという課題があった。
【００１３】
この発明は、上記のような課題を解決するためになされたものであり、バイアス誤差推定精度を向上させた追尾装置を得ることを目的とする。
【課題を解決するための手段】
【００１４】
この発明に係る追尾装置は、目標の動態データを取得する第１のセンサと、目標の距離、仰角および方位角を取得する第２のセンサと、動態データを第２のセンサの航跡出力時刻に時刻合わせを行う補正処理部と、補正処理部により時刻合わせが行われた動態データと第２のセンサの航跡とを比較して両者の一致度を出力する一致度判定処理部と、一致度に基づき、バイアス誤差推定処理部における予測処理で使用する予測ベクトル算出式を切替える運動モデル設定処理部と、運動モデル設定処理部の設定結果に基づき、カルマンフィルタによるバイアス誤差推定器を用いてバイアス誤差推定処理を行うバイアス誤差推定処理部と、バイアス誤差推定処理部により得られるバイアス誤差ベクトル推定値を時系列に蓄積するバイアス誤差データベースと、バイアス誤差データベース内のバイアス誤差ベクトル推定値を用いてバイアス誤差補正値を算出するバイアス誤差補正処理部と、制御工学における遅延部と、を備えたものである。
【発明の効果】
【００１５】
この発明によれば、カルマンフィルタによるバイアス誤差推定処理を行う際に、第１のセンサの動態データと第２のセンサの航跡との一致度を比較して、予測ベクトル算出式を適切に選択することにより、目標が旋回運動を行う場合であっても高精度のバイアス誤差推定値を得ることができる。
【図面の簡単な説明】
【００１６】
【図１】この発明の実施の形態１に係る追尾装置を示すブロック構成図である。
【図２】ＸＹＺ座標と姿勢角との関係を示す説明図である。
【図３】ＵＶＷ座標を示す説明図である。
【図４】第２のセンサが有するバイアス誤差を直交座標で示す説明図である。
【図５】第２のセンサが有するバイアス誤差を極座標で示す説明図である。
【図６】第２のセンサが有するバイアス誤差を回転角で示す説明図である。
【図７】図１内の補正処理部による時刻合わせ処理を示す説明図である。
【図８】この発明の実施の形態４に係る追尾装置を示すブロック構成図である。
【図９】この発明の実施の形態５に係る追尾装置を示すブロック構成図である。
【図１０】この発明の実施の形態６に係る追尾装置を示すブロック構成図である。
【図１１】一般的な追尾装置のバイアス誤差を補正する目的を示す説明図である。
【発明を実施するための形態】
【００１７】
実施の形態１．
以下、図面を参照しながら、この発明の好適な実施の形態について説明する。
図１はこの発明の実施の形態１に係る追尾装置を示すブロック構成図である。
図１において、追尾装置は、複数のセンサを含むセンサ群１０と、補正処理部４０と、一致度判定処理部５０と、運動モデル設定処理部６０と、カルマンフィルタによるバイアス誤差推定器（図示せず）を含むバイアス誤差推定処理部７０と、バイアス誤差データベース８０と、バイアス誤差補正処理部９０と、制御工学における遅延部１００とを備えている。
【００１８】
センサ群１０は、第１のセンサ１１と、任意数の第２のセンサ１２とを備えており、第１および第２のセンサ１１、１２は、目標Ｔを同時に観測している。
第１のセンサ１１は、基準座標に対するバイアス誤差が存在しないセンサからなり、目標に搭載されたＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ：全地球測位システム）／ＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ：慣性計測装置）からの目標Ｔの動態データを取得し、動態データを補正処理部４０に入力する。
【００１９】
なお、動態データとは、目標Ｔの位置、速度、姿勢角、姿勢角速度、機体加速度からなるデータとする。また、以下の説明において、特に断りがなければ、各データにはすべて時刻が付与されているものとする。
動態データは、以下の式（１）で表される状態ベクトルを有するものとする。
【００２０】
Ｘ（Ｏ）＝［Ｐ、Ｖ、δＰ、δＶ、Ａ］’ ・・・（１）
【００２１】
式（１）において、Ｘ（０）は、第１のセンサ１１における状態ベクトルを表す。
また、記号「’」は、ベクトルおよび行列の転置を表しており、このことは後述の数式においても同様である。
【００２２】
さらに、式（１）において、ＰはＸＹＺ座標における３次元の位置成分を表し、ＶはＸＹＺ座標における３次元の速度成分を表し、δＰはＸＹＺ座標のＸ、Ｙ、Ｚ各軸からの基準の軸からの姿勢角を表し、δＶはＸＹＺ座標のＸ、Ｙ、Ｚ各軸からの姿勢角の速度を表し、ＡはＵＶＷ座標における３次元加速度を表す。
【００２３】
図２はＸＹＺ座標と姿勢角［ｄｘ、ｄｙ、ｄｚ］との関係を示す説明図である。
図２において、姿勢角［ｄｘ、ｄｙ、ｄｚ］は、ｘ軸、ｙ軸およびｚ軸周りの回転角に相当する。
【００２４】
図３はＵＶＷ座標を示す説明図である。
図３において、ＵＶＷ座標は、レーダ位置を原点Ｏとし、目標Ｔの速度ベクトル方向をｕ軸の正方向とし、水平面に平行な面内で速度ベクトルに対して垂直で右手の方向をｖ軸の正方向とし、速度ベクトルに対して垂直で下向きの方向をｗ軸の正方向とした座標系である。
【００２５】
図１に戻り、第２のセンサ１２は、たとえば基準座標に対するバイアス誤差を有するセンサからなり、目標Ｔの距離、仰角および方位角を取得する。
たとえば、第２のセンサ１２を原点とする北基準の直交座標系において、距離、仰角および方位角のバイアス誤差を有するものとする。
【００２６】
つまり、第２のセンサ１２の観測値（距離、仰角および方位角）には、基準座標に対するバイアス誤差が付加されている。
また、第２のセンサ１２は、基準座標に対するバイアス誤差が付加された観測値を入力情報とした追尾を行い、追尾結果として、基準座標に対するバイアス誤差が付加された航跡を算出する。
【００２７】
ここで、第２のセンサ１２の航跡は、たとえば、以下の式（２）のように、第２のセンサ１２における状態ベクトルＸ（Ａ）（位置成分Ｐおよび速度成分Ｖからなる）を有するものとする。
【００２８】
Ｘ（Ａ）＝［Ｐ、Ｖ］’ ・・・（２）
【００２９】
式（２）において、位置成分ＰはＸＹＺ座標における３次元位置、速度成分ＶはＸＹＺ座標における３次元速度をそれぞれ表す。
また、第２のセンサ１２の航跡は、状態ベクトルＸ（Ａ）の誤差共分散行列を有する。
第２のセンサ１２の航跡の誤差共分散行列は、ＥＲＲＭＡＴ（Ａ）で表される。
【００３０】
ここで、第２のセンサ１２が有するバイアス誤差は、図４のように直交座標で定義することもでき、図５のように極座標で定義することもでき、図６のように回転角で定義することもできる。
【００３１】
図４、図５、図６において、第２のセンサ１２が有するバイアス誤差は、それぞれ、１点鎖線で示されている。
また、ここでは図示しないが、前述と同様に、バイアス誤差は、緯度、経度および高度で定義することもでき、どのような座標でもバイアス誤差を定義することは可能である。
【００３２】
第２のセンサ１２は、観測値として距離、仰角および方位角を補正処理部４０に入力するとともに、第２のセンサ１２で追尾フィルタにより算出した航跡（第２のセンサ１２の航跡）を補正処理部４０に入力する。
【００３３】
補正処理部４０は、第１のセンサ１１から得られる動態データに対して、第２のセンサ１２の航跡が得られた最新時刻まで時刻合わせ処理を行い、時刻合わせが行われた動態データＸ＃（Ｏ）を一致度判定処理部５０に入力する。
時刻合わせが行われた動態データＸ＃（Ｏ）は、以下の式（３）のように定義される。
【００３４】
Ｘ＃（Ｏ）＝［Ｐ＃、Ｖ＃］’ ・・・（３）
【００３５】
式（３）において、右辺のＰ＃、Ｖ＃は、第１のセンサ１１から得られる動態データに対して、第２のセンサ１２の航跡が得られた最新時刻まで時刻合わせ処理を施した動態データの位置成分および速度成分を、それぞれ表す。
【００３６】
図７は補正処理部４０による時刻合わせ処理を示す説明図である。
図７において、第１のセンサ１１から補正処理部４０への動態データの入力タイミングは時刻ｔ１であり、第２のセンサ１２の航跡の補正処理部４０への入力タイミングは時刻ｔ２であり、第２のセンサ１２の航跡と動態データとの時刻差Δｔ（＝ｔ２−ｔ１）が生じるが、補正処理部４０の出力タイミングは、時刻ｔ２に合わせられる。
【００３７】
このように、時刻合わせが行われた（第２のセンサ１２の航跡が得られた最新時刻まで時刻合わせ処理が施された）動態データＸ＃（Ｏ）は、以下の式（４）のように求められる。
【００３８】
Ｘ♯（Ｏ）＝ΦＸ（Ｏ）＋α（δＰ、δＶ、Ａ） ・・・（４）
【００３９】
式（４）において、Ｘ（Ｏ）は第１のセンサ１１から得られる動態データであり、Φは動態データＸ（Ｏ）を第２のセンサ１２の航跡が得られた最新時刻まで等速直線運動モデルにしたがい時刻あわせを行う行列である。
【００４０】
また、α（δＰ、δＶ、Ａ）は、姿勢角δＰ、姿勢角速度δＶおよびＵＶＷ座標における３次元加速度Ａに対して、第２のセンサ１２の航跡が得られた最新時刻まで時刻あわせ処理が施されたときの、ＸＹＺ座標における位置成分Ｐおよび速度成分Ｖへの運動補償係数を表す。
【００４１】
したがって、式（４）から分かるように、時刻合わせが行われた動態データＸ＃（０）は、等速直線運動モデルにしたがい外挿された動態データＸ（Ｏ）の位置成分Ｐおよび速度成分Ｖに、運動補償係数α（δＰ、δＶ、Ａ）を加算した値となる。
このように、補正処理部４０からは、第２のセンサ１２の航跡すなわち状態ベクトルＸ（Ａ）と、第２のセンサ１２の航跡が得られた最新時刻まで時刻合わせが行われた動態データＸ＃（Ｏ）とが出力される。
【００４２】
一致度判定処理部５０は、第２のセンサ１２の航跡（状態ベクトル）Ｘ（Ａ）と、第２のセンサ１２の航跡が得られた最新時刻まで時刻合わせが行われた動態データＸ＃（Ｏ）との一致度を算出する。
このとき、両者が一致しているか否かは、以下の式（５）、式（６）のように判定される。
【００４３】
｜Ｘ（Ａ）−Ｘ＃（Ｏ）｜≦ＴＨ、であれば「一致フラグ」を出力 ・・・（５）
｜Ｘ（Ａ）−Ｘ＃（Ｏ）｜＞ＴＨ、であれば「不一致フラグ」を出力 ・・・（６）
【００４４】
式（５）、式（６）において、｜ ｜はノルムを表しており、ユークリッドノルムであっても、最大値を取るノルムであってもよく、ユーザが自由に事前に定義することができる。
なお、一致度は、式（５）、式（６）の左辺｜Ｘ（Ａ）−Ｘ＃（Ｏ）｜で定義される。
また、式（５）、式（６）の右辺は、事前に設定された閾値ＴＨを示している。
【００４５】
一致度判定処理部５０は、式（５）、式（６）を用いて、第２のセンサ１２の航跡（状態ベクトル）Ｘ（Ａ）と、第２のセンサ１２の航跡が得られた最新時刻まで時刻合わせが行われた動態データＸ＃（Ｏ）とが一致するか否かを判定し、一致する場合には一致フラグを生成し、不一致の場合には不一致フラグを生成し、一致度（判定結果）として運動モデル設定処理部６０に入力する。
【００４６】
運動モデル設定処理部６０は、一致度判定処理部５０から得られる一致度（一致フラグまたは不一致フラグ）に基づき、後段のバイアス誤差推定処理部７０で使用されるカルマンフィルタによるバイアス誤差推定器による予測ベクトル算出用の制御信号を、バイアス誤差推定処理部７０に入力する。
【００４７】
すなわち、運動モデル設定処理部６０は、一致度判定処理部５０から一致フラグが得られた場合には、以下の式（７）により予測ベクトルＸｐを算出するための制御信号を、バイアス誤差推定処理部７０に入力する。
【００４８】
Ｘｐ＝ΦＸｓ ・・・（７）
【００４９】
一方、一致度判定処理部５０から不一致フラグが得られた場合には、運動モデル設定処理部６０は、以下の式（８）により予測ベクトルを算出するための制御信号を、バイアス誤差推定処理部７０に入力する。
【００５０】
Ｘｐ＝ΦＸｓ＋α（δＰ、δＶ、Ａ） ・・・（８）
【００５１】
式（７）、式（８）において、Ｘｓは、バイアス誤差推定処理部７０（バイアス誤差推定器）における１サンプリング前の目標Ｔの状態ベクトル平滑値であり、Ｘｐは、バイアス誤差推定処理部７０における現時刻の目標Ｔの状態ベクトル予測値である。
【００５２】
バイアス誤差推定処理部７０は、第１のセンサ１１から得られる動態データの位置観測値と、第２のセンサ１２から得られる位置観測値とを、バイアス誤差推定処理部７０内のバイアス誤差推定器に入力する。
【００５３】
カルマンフィルタによるバイアス誤差推定器は、運動モデル設定処理部６０から得られる制御信号にしたがい、式（７）または式（８）を用いて予測ベクトルを算出し、予測ベクトルの算出値を用いて、第２のセンサ１２のバイアス誤差ベクトル推定値を算出し、バイアス誤差データベース８０に格納する。
【００５４】
このように、バイアス誤差推定処理部７０内のカルマンフィルタによるバイアス誤差推定器を用いてバイアス誤差ベクトルを推定する場合、バイアス誤差ベクトル推定値ｂｓとしては、以下の式（９）のように、第２のセンサ１２の距離、仰角および方位角の各バイアス誤差成分が得られる。
【００５５】
ｂｓ＝［ΔＲ、ΔＥ、ΔＡｚ］’ ・・・（９）
【００５６】
式（９）において、ΔＲ、ΔＥ、ΔＡｚは、バイアス誤差ベクトル推定値ｂｓの距離、仰角および方位角の各バイアス誤差成分である。
【００５７】
なお、距離バイアス誤差成分ΔＲを除いて、第２のセンサ１２の仰角および方位角のバイアス誤差成分ΔＥ、ΔＡｚを、バイアス誤差推定処理部７０内のバイアス誤差推定器で仮定する場合、バイアス誤差ベクトル推定値ｂｓは、以下の式（１０）から得られる。
【００５８】
ｂｓ＝［ΔＥ、ΔＡｚ］’ ・・・（１０）
【００５９】
バイアス誤差データベース８０は、バイアス誤差推定処理部７０で算出されたバイアス誤差ベクトル推定値ｂｓを、時系列的に、かつ複数の第２のセンサ１２ごとに蓄積する。
【００６０】
バイアス誤差補正処理部９０は、バイアス誤差データベース８０から得られる時系列のバイアス誤差ベクトル推定値ｂｓのＮサンプリング分の平均値または最新値（いずれを用いるかは事前にユーザにより決定される）を用いて、第２のセンサ１２のバイアス誤差補正処理に用いられるバイアス誤差を算出し、バイアス誤差補正値を遅延部１００に入力する。
【００６１】
すなわち、バイアス誤差補正処理部９０は、バイアス誤差補正処理をリアルタイム（バイアス誤差ベクトル推定値ｂｓの最新値）で行うことをユーザが決定した場合には、現時刻の第２のセンサ１２の観測値のバイアス誤差を補正するためのバイアス誤差補正値を遅延部１００に入力する。
【００６２】
一方、バイアス誤差補正処理を平均値で行う（リアルタイムで行わない）ことをユーザが決定した場合には、バイアス誤差補正処理部９０は、現時刻の第２のセンサ１２の観測値のバイアス誤差を補正するためのバイアス誤差補正値を出力せずに、ユーザがバイアス誤差推定の開始を指示した場合のみに、バイアス誤差補正値を遅延部１００に入力する。
【００６３】
最後に、遅延部１００は、バイアス誤差補正処理部９０から得られるバイアス誤差補正値を、第２のセンサ１２の最新時刻まで遅延させ、遅延後のバイアス誤差補正値をセンサ群１０に入力する。
【００６４】
これにより、センサ群１０内の第２のセンサ１２は、バイアス誤差補正処理部９０から遅延部１００を介して得られるバイアス誤差補正値に基づき、第２のセンサ１２で得られる観測値のバイアス誤差補正を行う。
【００６５】
以上のように、この発明の実施の形態１（図１）に係る追尾装置は、目標Ｔの動態データを取得する第１のセンサ１１と、目標Ｔの距離、仰角および方位角を取得する第２のセンサ１２と、動態データを第２のセンサの航跡出力時刻に時刻合わせを行う補正処理部４０と、補正処理部４０により時刻合わせが行われた動態データと第２のセンサ１２の航跡とを比較して両者の一致度を出力する一致度判定処理部５０と、一致度（判定フラグ）に基づき、バイアス誤差推定処理部７０における予測処理で使用する予測ベクトル算出式を切替える運動モデル設定処理部６０と、運動モデル設定処理部６０の設定結果（制御信号）に基づき、カルマンフィルタによるバイアス誤差推定器を用いてバイアス誤差推定処理を行うバイアス誤差推定処理部７０と、バイアス誤差推定処理部７０により得られるバイアス誤差ベクトル推定値を時系列に蓄積するバイアス誤差データベース８０と、バイアス誤差データベース８０内のバイアス誤差ベクトル推定値を用いてバイアス誤差補正値を算出するバイアス誤差補正処理部９０と、制御工学における遅延部１００と、を備えている。
【００６６】
一致度判定処理部５０は、動態データの３次元加速度Ａ、または、補正処理部４０から出力される運動補償係数に基づき、一致度を判定する。
このように、カルマンフィルタによるバイアス誤差推定処理を行う際に、第１のセンサ１１の動態データと、第２のセンサ１２の航跡との一致度を比較して、予測ベクトル算出式を適切に選択することにより、目標Ｔが旋回運動を行う場合であっても、高精度のバイアス誤差推定値を得ることが可能となる。
【００６７】
実施の形態２．
なお、上記実施の形態１（図１）では、一致度判定処理部５０において、第２のセンサ１２の航跡（状態ベクトル）Ｘ（Ａ）と、時刻合わせが行われた動態データＸ＃（Ｏ）との比較に基づき一致度を判定したが、ＵＶＷ座標における３次元加速度Ａのユークリッドノルムによる大きさに基づき一致度を判定してもよい。
【００６８】
この場合、一致度判定処理部５０は、ＵＶＷ座標における３次元加速度Ａのユークリッドノルムによる大きさが、事前に決めた閾値ＴＨ以下の場合には、「一致フラグ」を出力し、閾値ＴＨよりも大きい場合には、「不一致フラグ」を出力する。
【００６９】
以上のように、この発明の実施の形態２によれば、バイアス誤差推定処理部７０においてカルマンフィルタによるバイアス誤差推定処理を行う際に、第１のセンサ１１の動態データと第２のセンサ１２の航跡との一致度を、動態データのＵＶＷ座標における３次元加速度Ａのユークリッドノルムによる大きさと閾値ＴＨとの比較に基づいて判定することにより、前述と同様に、カルマンフィルタによるバイアス誤差推定処理部７０で使用する予測ベクトル算出式を適切に選択することができる。
したがって、目標Ｔが旋回運動を行う場合でも、高精度なバイアス誤差推定値を得ることが可能となる。
【００７０】
実施の形態３．
なお、上記実施の形態２では、一致度判定処理部５０において、ＵＶＷ座標における３次元加速度Ａのユークリッドノルムによる大きさに基づき一致度を判定したが、運動補償係数のユークリッドノルムによる大きさに基づき一致度を判定してもよい。
この場合、一致度判定処理部５０は、運動補償係数のユークリッドノルムによる大きさが事前に決めた閾値ＴＨ以下の場合には、「一致フラグ」を出力し、閾値ＴＨよりも大きい場合には、「不一致フラグ」を出力する。
【００７１】
以上のように、この発明の実施の形態３によれば、バイアス誤差推定処理部７０においてカルマンフィルタによるバイアス誤差推定処理を行う際に、第１のセンサ１１の動態データと第２のセンサ１２の航跡との一致度を、運動補償係数のユークリッドノルムによる大きさに基づく加速度と閾値ＴＨとの比較に基づいて判定することにより、前述と同様に、カルマンフィルタによるバイアス誤差推定処理部７０で使用する予測ベクトル算出式を適切に選択することができる。
したがって、目標Ｔが旋回運動を行う場合でも、高精度なバイアス誤差推定値を得ることが可能となる。
【００７２】
実施の形態４．
なお、上記実施の形態１〜３（図１）では、一致度（一致フラグまたは不一致フラグ）に応答する運動モデル設定処理部６０のみを用いて、バイアス誤差推定処理部７０（カルマンフィルタによるバイアス誤差推定器）で使用される予測ベクトル算出用の制御信号を生成したが、図８のように、ユーザの設定条件にしたがう運動モデル制御処理部１３０を追加してもよい。
【００７３】
図８はこの発明の実施の形態４に係る追尾装置を示すブロック構成図であり、前述（図１参照）と同様のものについては、前述と同一符号を付して詳述を省略する。
図８において、運動モデル設定処理部６０には、運動モデル制御処理部１３０が接続されている。
【００７４】
運動モデル制御処理部１３０は、所定の運動モデル（事前にユーザが動態データで設定する）を常に使い続けるための制御信号を生成し、これを運動モデル設定処理部６０に入力する。すなわち、運動モデル制御処理部１３０は、運動モデル設定処理部６０において動態データを使用し続けるための制御信号を出力する。
【００７５】
これにより、運動モデル設定処理部６０は、運動モデル制御処理部１３０からの制御信号に応答して、前述の式（８）に基づき、予測ベクトルを算出するための制御信号をバイアス誤差推定処理部７０に入力する。
【００７６】
以上のように、この発明の実施の形態４（図８）によれば、目標Ｔが常に旋回している状況において、その都度の毎回にわたり、式（７）、式（８）の予測ベクトル算出式の切替が頻繁に行われる可能性がある場合であっても、バイアス誤差推定処理部７０における予測ベクトル算出式として、式（８）に基づく制御信号にしたがう算出式を常に使い続けることができるので、目標Ｔが常に旋回運動を行う場合であっても、高精度なバイアス誤差推定値を得ることが可能となる。
【００７７】
実施の形態５．
なお、上記実施の形態４（図８）では、動態データを使用し続けるための制御信号を運動モデル設定処理部６０に入力する運動モデル制御処理部１３０を追加したが、図９のように、第２のセンサ１２から得られるサンプリングレートに基づく制御信号を運動モデル設定処理部６０に入力する運動モデル制御処理部１４０を追加してもよい。
【００７８】
図９はこの発明の実施の形態５に係る追尾装置を示すブロック構成図であり、前述（図１、図８参照）と同様のものについては、前述と同一符号を付して詳述を省略する。
図９において、補正処理部４０、一致度判定処理部５０、運動モデル設定処理部６０およびバイアス誤差推定処理部７０には、運動モデル制御処理部１４０が接続されている。
【００７９】
運動モデル制御処理部１４０は、第２のセンサ１２から得られるサンプリングレートが事前に決めたサンプリング閾値よりも小さい場合には、等速直線運動に基づく予測ベクトル算出処理によりバイアス誤差推定を行うための制御信号を運動モデル設定処理部６０に入力する。
【００８０】
すなわち、運動モデル制御処理部１４０は、第２のセンサ１２から得られるサンプリングレートがサンプリング閾値よりも小さい場合には、等速直線運動に基づく予測ベクトル算出処理を行うため、つまり、前述の式（７）に基づくバイアス誤差推定をバイアス誤差推定処理部７０で行うための制御信号を、運動モデル設定処理部６０に入力する。
【００８１】
これにより、バイアス誤差推定処理部７０は、式（７）による等速直線運動モデルを用いてバイアス誤差推定を行う。
また、運動モデル制御処理部１４０は、補正処理部４０、一致度判定処理部５０および運動モデル設定処理部６０に対して、処理停止信号を入力して各処理を中止し、センサ群１０からの観測値を、バイアス誤差推定処理部７０で使用する処理のみ動作させるようにする。
【００８２】
以上のように、この発明の実施の形態５（図９）によれば、バイアス誤差推定処理部７０においてカルマンフィルタによるバイアス誤差推定処理を行う際に、第２のセンサ１２のサンプリングレートが小さい場合には、実際の目標Ｔが旋回していたとしても、等速直線運動モデルに基づくバイアス誤差推定精度はあまり劣化しないと考えられるので、目標Ｔの旋回判定に関わる補正処理部４０、一致度判定処理部５０および運動モデル設定処理部６０の計算を中止することにより、演算負荷の低減を実現することができる。
【００８３】
また、一致度判定処理部５０における閾値ＴＨの誤設定に起因して、運動モデルの切替が頻繁に起こるような状況が生じた場合でも、等速直線運動モデルに固定することができるので、バイアス誤差推定精度の劣化を防ぐことができる。
さらに、前述と同様に、目標Ｔが旋回運動を行う場合でも、高精度なバイアス誤差推定値を得ることが可能となる。
【００８４】
実施の形態６．
なお、上記実施の形態１〜５（図１、図８、図９）では、言及しなかったが、図１０のように、第１のセンサ１１からの動態データに基づき、第２のセンサ１２の航跡を上書きするセンサ航跡更新処理部１５０を追加してもよい。
【００８５】
図１０はこの発明の実施の形態６に係る追尾装置を示すブロック構成図であり、前述（図１、図８、図９参照）と同様のものについては、前述と同一符号を付して詳述を省略する。
図１０において、補正処理部４０には、第１のセンサ１１の動態データに基づき、第２のセンサ１２の航跡を上書きするセンサ航跡更新処理部１５０が接続されている。
【００８６】
以上のように、この発明の実施の形態６によれば、第２のセンサ１２の航跡よりも精度が高い第１のセンサ１１の動態データが得られる状況下において、精度の低い（または、劣化している）第２のセンサ１２の航跡を、精度の高い第１のセンサ１１の動態データで上書きすることにより改善し、目標Ｔの追尾維持率を向上させることができる。
【００８７】
また、第２のセンサ１２の目標Ｔの追尾維持率が向上するので、第２のセンサ１２が目標Ｔの探知に失敗して、第２のセンサ１２から目標Ｔの観測値が出力されない状況を防ぐことが可能となり、結果として、バイアス誤差推定値を継続して計算し続けることができる。
さらに、前述と同様に、目標Ｔが旋回運動を行う場合でも、高精度なバイアス誤差推定値を得ることが可能となる。
【符号の説明】
【００８８】
１１ 第１のセンサ、１２ 第２のセンサ、４０ 補正処理部、５０ 一致度判定処理部、６０ 運動モデル設定処理部、７０ バイアス誤差推定処理部、８０ バイアス誤差データベース、９０ バイアス誤差補正処理部、１００ 遅延部、１３０、１４０ 運動モデル制御処理部、１５０ センサ航跡更新処理部、Ｔ 目標。
【特許請求の範囲】
【請求項１】
目標の動態データを取得する第１のセンサと、
前記目標の距離、仰角および方位角を取得する第２のセンサと、
前記動態データを前記第２のセンサの航跡出力時刻に時刻合わせを行う補正処理部と、
前記補正処理部により時刻合わせが行われた動態データと前記第２のセンサの航跡とを比較して両者の一致度を出力する一致度判定処理部と、
前記一致度に基づき、後記バイアス誤差推定処理部における予測処理で使用する予測ベクトル算出式を切替える運動モデル設定処理部と、
前記運動モデル設定処理部の設定結果に基づき、カルマンフィルタによるバイアス誤差推定器を用いてバイアス誤差推定処理を行うバイアス誤差推定処理部と、
バイアス誤差推定処理部により得られるバイアス誤差ベクトル推定値を時系列に蓄積するバイアス誤差データベースと、
前記バイアス誤差データベース内のバイアス誤差ベクトル推定値を用いてバイアス誤差補正値を算出するバイアス誤差補正処理部と、
制御工学における遅延部と、
を備えたことを特徴とする追尾装置。
【請求項２】
前記一致度判定処理部は、前記動態データの加速度に基づき前記一致度を判定することを特徴とする請求項１に記載の追尾装置。
【請求項３】
前記一致度判定処理部は、前記補正処理部から出力される運動補償係数に基づき前記一致度を判定することを特徴とする請求項１に記載の追尾装置。
【請求項４】
前記運動モデル設定処理部に接続された運動モデル制御処理部を備え、
前記運動モデル制御処理部は、前記運動モデル設定処理部において前記動態データを使用し続けるための制御信号を、前記運動モデル制御処理部に入力することを特徴とする請求項１から請求項３までのいずれか１項に記載の追尾装置。
【請求項５】
前記補正処理部、前記一致度判定処理部および前記運動モデル設定処理部に接続された運動モデル制御処理部を備え、
前記運動モデル制御処理部は、前記第２のセンサから得られるサンプリングレートが事前に決めたサンプリング閾値よりも小さい場合には、等速直線運動に基づく予測ベクトル算出処理によりバイアス誤差推定を行うための制御信号を前記運動モデル制御処理部に入力するとともに、前記補正処理部、前記一致度判定処理部および前記運動モデル設定処理部に処理停止信号を入力することを特徴とする請求項１から請求項３までのいずれか１項に記載の追尾装置。
【請求項６】
前記動態データに基づき、前記第２のセンサの航跡を上書きするセンサ航跡更新処理部を備えたことを特徴とする請求項１から請求項５までのいずれか１項に記載の追尾装置。
【請求項１】
目標の動態データを取得する第１のセンサと、
前記目標の距離、仰角および方位角を取得する第２のセンサと、
前記動態データを前記第２のセンサの航跡出力時刻に時刻合わせを行う補正処理部と、
前記補正処理部により時刻合わせが行われた動態データと前記第２のセンサの航跡とを比較して両者の一致度を出力する一致度判定処理部と、
前記一致度に基づき、後記バイアス誤差推定処理部における予測処理で使用する予測ベクトル算出式を切替える運動モデル設定処理部と、
前記運動モデル設定処理部の設定結果に基づき、カルマンフィルタによるバイアス誤差推定器を用いてバイアス誤差推定処理を行うバイアス誤差推定処理部と、
バイアス誤差推定処理部により得られるバイアス誤差ベクトル推定値を時系列に蓄積するバイアス誤差データベースと、
前記バイアス誤差データベース内のバイアス誤差ベクトル推定値を用いてバイアス誤差補正値を算出するバイアス誤差補正処理部と、
制御工学における遅延部と、
を備えたことを特徴とする追尾装置。
【請求項２】
前記一致度判定処理部は、前記動態データの加速度に基づき前記一致度を判定することを特徴とする請求項１に記載の追尾装置。
【請求項３】
前記一致度判定処理部は、前記補正処理部から出力される運動補償係数に基づき前記一致度を判定することを特徴とする請求項１に記載の追尾装置。
【請求項４】
前記運動モデル設定処理部に接続された運動モデル制御処理部を備え、
前記運動モデル制御処理部は、前記運動モデル設定処理部において前記動態データを使用し続けるための制御信号を、前記運動モデル制御処理部に入力することを特徴とする請求項１から請求項３までのいずれか１項に記載の追尾装置。
【請求項５】
前記補正処理部、前記一致度判定処理部および前記運動モデル設定処理部に接続された運動モデル制御処理部を備え、
前記運動モデル制御処理部は、前記第２のセンサから得られるサンプリングレートが事前に決めたサンプリング閾値よりも小さい場合には、等速直線運動に基づく予測ベクトル算出処理によりバイアス誤差推定を行うための制御信号を前記運動モデル制御処理部に入力するとともに、前記補正処理部、前記一致度判定処理部および前記運動モデル設定処理部に処理停止信号を入力することを特徴とする請求項１から請求項３までのいずれか１項に記載の追尾装置。
【請求項６】
前記動態データに基づき、前記第２のセンサの航跡を上書きするセンサ航跡更新処理部を備えたことを特徴とする請求項１から請求項５までのいずれか１項に記載の追尾装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【公開番号】特開２０１２−４７４９０（Ｐ２０１２−４７４９０Ａ）
【公開日】平成２４年３月８日（２０１２．３．８）
【国際特許分類】
【出願番号】特願２０１０−１８７４７２（Ｐ２０１０−１８７４７２）
【出願日】平成２２年８月２４日（２０１０．８．２４）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
【公開日】平成２４年３月８日（２０１２．３．８）
【国際特許分類】
【出願日】平成２２年８月２４日（２０１０．８．２４）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
[ Back to top ]