(57)【要約】【課題】  移動体の運動状態により観測誤差の大きさが変化する状況にあっても、目標を高精度に追尾する目標追尾装置を提供する。【解決手段】  目標の運動モデルと、モデル誤差の大きさを表すプロセスノイズ行列と、センサによる観測誤差の大きさを表す観測ノイズ行列の値を用いたカルマンフィルタにおいて、観測ノイズ行列に自らの移動体の運動状態に合わせて変化する値を設定する。観測ノイズ行列計算処理によってフィルタ特性をその場に応じて適応的に変化させることができ、結果として、目標の位置等の状態量を高精度に推定可能となる。【選択図】      図１
2015197403.tif 000002
【特許請求の範囲】
【請求項１】
移動体に搭載され、目標を追尾する目標追尾装置であって、
目標の方向、距離を観測した観測量を観測更新計算部に出力する第１のセンサ部と、
前記移動体の運動状態を計測し、運動状態量を観測ノイズ行列計算部に出力する第２のセンサ部と、
前記運動状態量を入力し、前記移動体の運動状態を表す観測ノイズ行列を観測更新計算部に出力する観測ノイズ行列計算部と、
観測更新計算部が出力する状態量（＋）と誤差共分散行列（＋）を入力し、所定のモデルに基づき計算した時間外挿計算後の状態量（−）と誤差共分散行列（−）を観測更新計算装置に出力する時間外挿計算部と、
前記観測ノイズ行列計算部が出力する観測ノイズ行列と、前記第１のセンサ部が出力する観測量と、前記時間外挿計算部が出力する時間外挿計算後の状態量（−）と誤差共分散行列（−）を入力し、前記目標の観測量の観測更新計算を行い、観測更新計算後の状態量（＋）と誤差共分散行列（＋）を出力する観測更新計算部と、
を備えることを特徴とする目標追尾装置。
【請求項２】
観測ノイズ行列計算部は、前記観測ノイズ行列Ｒを以下の式により計算することを特徴とする請求項１記載の目標追尾装置。
2015197403.tif 000006
ここで、
Ｒ：観測ノイズ行列、
Ｒ０：観測ノイズ行列初期値、 
ｗｋ：運動状態量、 
Ｇ：ゲイン行列、
ｔｋ：現在時間、
Ｍ：サンプリング数
である。 
【請求項３】
前記観測ノイズ行列初期値Ｒ０は、前記第１のセンサ部のＳ／Ｎ比を用いて設定される初期値であることを特徴とする請求項２記載の目標追尾装置。
【請求項４】
前記観測更新計算部は、前記第１のセンサ部から前記観測量を入力する時刻と、前記観測ノイズ行列計算部から前記観測ノイズ行列を入力する時刻が異なる場合に、前記第１のセンサ部から観測量を入力したタイミングにおいて、当該観測量と、前記タイミングにおける最新の観測ノイズ行列を用いて観測更新計算を行うことを特徴とする請求項１記載の目標追尾装置。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、車両や飛しょう体等の移動体に搭載され、自身の運動量に基づいて、観測ノイズ行列の値を更新可能な目標追尾装置に関するものである。
【背景技術】
【０００２】
従来、車両や飛しょう体等の移動体において目標を追尾する際、目標の運動を推定する計算としてカルマンフィルタが広く用いられている。
目標を捕捉するためのセンサであるシーカから得られる目標の観測結果には、観測誤差が含まれており、その観測誤差を取り除く手段として、目標の運動モデルに基づいて状態量を推定するカルマンフィルタは非常に有効である。カルマンフィルタには、線形カルマンフィルタ（例えば、非特許文献１のP.163〜P.184参照）や、非線形カルマンフィルタ(例えば、非特許文献１のP.373〜P.388参照)等の種類がある。
また、従来、目標の状態量を推定する装置が開示されている（例えば、特許文献１参照）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１０−９６６４７号公報
【非特許文献】
【０００４】
【非特許文献１】"Tactical and Strategic Missile Guidance Fifth Edition", Paul Zarchan, ISBN-10:1-56347-874-9
【発明の概要】
【発明が解決しようとする課題】
【０００５】
カルマンフィルタの特性は、目標の運動モデルに加えて、モデル誤差の大きさを表すプロセスノイズ行列と、シーカによる観測誤差の大きさを表す観測ノイズ行列の値によって決定される。このうち、目標の観測結果に含まれる観測誤差の大きさは、装置の搭載された移動体の運動状態によって変化しうる。そのため、カルマンフィルタの観測ノイズ行列の値も、移動体の運動状態に合わせて変化させるのが望ましい。
しかしながら、従来は移動体の運動状態に関わらず、観測ノイズ行列の値は固定されており、観測誤差の大きさの変化が考慮されていなかった。
そのため、必ずしもその場に応じた適切なフィルタ特性となっていないことがあり、このことが、目標状態量の推定精度の向上を図る上での課題となっていた。
【０００６】
この発明は係る課題を解決するためになされたものであり、移動体の運動状態により観測誤差の大きさが変化する状況にあっても、目標を高精度に追尾する目標追尾装置を提供することを目標とする。
【課題を解決するための手段】
【０００７】
この発明に係る目標追尾装置は、移動体に搭載され、目標を追尾する目標追尾装置であって、目標の方向、距離を観測した観測量を観測更新計算部に出力する第１のセンサ部と、前記移動体の運動状態を計測し、運動状態量を観測ノイズ行列計算部に出力する第２のセンサ部と、前記運動状態量を入力し、前記移動体の運動状態を表す観測ノイズ行列を観測更新計算部に出力する観測ノイズ行列計算部と、観測更新計算部が出力する状態量（＋）と誤差共分散行列（＋）を入力し、所定のモデルに基づき計算した時間外挿計算後の状態量（−）と誤差共分散行列（−）を観測更新計算装置に出力する時間外挿計算部と、前記観測ノイズ行列計算部が出力する観測ノイズ行列と、前記第１のセンサ部が出力する観測量と、前記時間外挿計算部が出力する時間外挿計算後の状態量（−）と誤差共分散行列（−）を入力し、前記目標の観測量の観測更新計算を行い、観測更新計算後の状態量（＋）と誤差共分散行列（＋）を出力する観測更新計算部とを備える。
【発明の効果】
【０００８】
この発明に係る目標追尾装置によれば、カルマンフィルタのフィルタ特性を状態に応じて適切に変化させることが可能であり、目標追尾性能の向上を図ることができる。
【図面の簡単な説明】
【０００９】
【図１】この発明の実施の形態１に係る目標追尾装置の構成を示す図である。
【発明を実施するための形態】
【００１０】
実施の形態１．
図１は、本実施の形態に係る目標追尾装置１００の構成を示す図である。
目標追尾装置１００は、車両や飛しょう体等の移動体に搭載され、目標の方向・距離等を観測するためのシーカ１と、自身の速度・加速度等の状態量を計測するためのセンサ２と、観測更新計算装置３と、時間外挿計算装置４と、観測ノイズ行列計算装置５から構成される。
以下、図１を参照しながら、実施の形態１における目標追尾装置１００の動作を説明する。
【００１１】
シーカ１は、目標に向けて電波を送信しその反射波を受信し、あるいは目標が放射する赤外線を受信すること等により、目標の動きを観測する。
シーカ１は、観測された目標の観測量６を観測更新計算装置３へ出力する。
【００１２】
センサ２は、自身の目標追尾装置１００が搭載される移動体の運動状態量１０を計測する。センサ２は、計測により得られた運動状態量１０を観測ノイズ行列計算装置５に出力する。
【００１３】
観測更新計算装置３は、シーカ１から得られる観測量６と、時間外挿装置から得られる「状態量（−）・誤差共分散行列（−）」９を用い、観測ノイズ行列計算装置５から得られる観測ノイズ行列７の値に基づいて観測更新計算を行う。
【００１４】
観測更新計算の処理を下記の式（１）に示す。
式（１）において、シーカ１から観測量６が出力される時刻と、観測ノイズ行列計算装置５から観測ノイズ行列７が出力される時刻はそれぞれ異なる。そのため、観測更新計算は新たな観測量６が得られた時点で行うものとし、その際に使用する観測ノイズ行列７は、その時点で得られている最新の値を使用するものとする。
【００１５】
【数１】
2015197403.tif 000003
【００１６】
ここで、
出力方程式：ｈ
観測量：ｚ
観測ノイズ行列：Ｒ
状態量（観測更新計算前）：ｘ−
状態量（観測更新計算後）：ｘ＋
誤差共分散行列（観測更新計算前）：Ｐ−
誤差共分散行列（観測更新計算後）：Ｐ＋
とする。
【００１７】
出力方程式ｈは、シーカの特性をあらかじめ適当に同定した数式モデルから構成される。誤差共分散行列Ｐの初期値は、事前の想定に基づいて適当に設定されるものとする。
【００１８】
以上の計算で得られた観測更新計算後の状態量（Ｘ+）と観測更新計算後の誤差共分散行列（Ｐ+）を「状態量（＋）・誤差共分散行列（＋）」８として時間外挿計算装置４へ出力する。
【００１９】
時間外挿計算装置４は、観測更新計算装置３からの入力「状態量（＋）・誤差共分散行列（＋）」８を用いて時間外挿計算を行う。次の式（２）に時間外挿計算の処理を示す。
【００２０】
【数２】
2015197403.tif 000004
【００２１】
ここで、
運動方程式：ｆ
プロセスノイズ：ｗ
プロセスノイズ行列：Ｑ
現在時間：ｔｋ
とする。
【００２２】
運動方程式ｆは、対象をあらかじめ適当に同定した数式モデルから構成される。プロセスノイズ行列Ｑの値はあらかじめ想定されるモデル化誤差から適当に設定される。
【００２３】
時間外挿計算装置４は、時間外挿計算の結果として得られる時間外挿計算後の状態量と時間外挿計算後の誤差共分散行列を、観測更新計算前の「状態量（−）・誤差共分散行列（−）」９として観測更新計算装置３へ出力する。
【００２４】
ここで、観測ノイズ行列計算装置５は、センサ２から得られた自身の目標追尾装置１００が搭載される移動体の運動状態量１０を用いて、観測ノイズ行列７の値を計算する。
観測ノイズ行列７の値は次の式（３）に示す処理で計算される。
【００２５】
【数３】
2015197403.tif 000005
【００２６】
ここで
観測ノイズ行列：Ｒ
観測ノイズ行列初期値：Ｒ０
運動状態量：ｗｋ
ゲイン行列：Ｇ
現在時間：ｔｋ
サンプリング数：Ｍ
とする。
【００２７】
Ｖは与えられたデータ列から分散を算出する関数、ｍは与えられたデータ列から平均を算出する関数である。各時刻において、センサ２から得られた運動状態量ｗｋの過去Ｍサンプリング分の分散を算出する。これにゲイン行列Ｇを乗じた上で、観測ノイズ行列初期値Ｒ０に乗じて観測ノイズ行列Ｒを計算する。これにより、移動体の運動状態を反映した観測ノイズ行列Ｒを算出することができる。
【００２８】
なお、観測ノイズ行列初期値Ｒ０は、シーカ１のＳ／Ｎ比（singal-noise ratio）等をもとに設定される初期値である。
この計算はセンサ２から新たな値が送信される毎に行われ、式（３）により計算された観測ノイズ行列Ｒは、観測ノイズ行列７として観測更新計算装置３に送信される。
【００２９】
以上の数式１〜数式３の処理を繰り返し計算し、「状態量（−）・誤差共分散行列（−）」９を毎サンプリング計算する。サンプリング毎に、「状態量（−）・誤差共分散行列（−）」９を目標追尾装置の最終的な出力とする。
【００３０】
このように、本実施の形態に係る目標追尾装置によれば、移動体の運動状態により観測誤差の大きさが変化する状況にあっても、フィルタ特性をその場に応じて適応的に変化させることができ、ロバストな推定が可能になる。その結果、目標の位置等の状態量を高精度に推定することができ、ひいてはその推定された状態量を用いて、目標を高精度に追尾することができる。
【００３１】
例えば、自身の目標追尾装置１００が搭載される移動体が車両であり、目標を追尾している場合に、目標の追尾精度は移動体が走行している路面の状態に大きく影響される。
従来は路面の状態に関わらず観測ノイズ行列が固定されていたが、本発明によれば固定されていた観測ノイズ行列の値を観測ノイズの大きさと相関のある自らの移動体の運動状態を示す値によって変化させることが可能であるため、フィルタ特性をその場に応じて適応的に変化させることができ、目標の追尾精度を格段に向上させることが可能となる。例えば舗装路面を走行する際は観測ノイズが小さく目標の方向・距離等を観測する観測量６に対する応答性能が高い特性となる。一方、不整地を走行する際には観測ノイズが大きく観測量６に対する外乱抑制性能が高い特性となる。
【符号の説明】
【００３２】
１  シーカ、２  センサ、３  観測更新計算装置、４  時間外挿計算装置、５  観測ノイズ行列計算装置、６  観測量、７  観測ノイズ行列、８  状態量（＋）・誤差共分散行列（＋）、９  状態量（−）・誤差共分散行列（−）、１０  運動状態量、１００  目標追尾装置。
【図１】
2015197403.tif 000007