JP2005077167A - 目標追尾装置 - Google Patents

Abstract

【課題】 推定精度が低下しやすいジャーク項の処理を行わずに、蛇行する追尾目標の追尾性能を向上し、蛇行軌跡の中心軸に沿った追尾処理を可能とする。
【解決手段】 等速直線運動成分と正弦運動成分とを有する追尾目標の運動モデルに基づいて、カルマンフィルタにより前記追尾目標の運動諸元平滑値を算出する運動諸元予測器１５−１を備え、必要に応じて蛇行軌跡の中心軸上、あるいは蛇行軌跡上に追尾目標が存在するような予測処理を行うようにした。
【選択図】 図１

Description

この発明は、追尾目標の運動諸元の観測値に基づいて、その追尾目標の運動諸元を予測する目標追尾装置に係るものであり、特に追尾目標が蛇行している場合にその運動諸元を精度よく予測する技術に関する。

従来の目標追尾装置は、追尾目標が蛇行して運動する場合に、加速度ｘ"(t)を正弦関数ｘ"(t)＝ｎ_ｔｓｉｎ（ωｔ＋φ）で表した運動モデルに基づくカルマンフィルタを用いて、追尾目標の運動諸元を予測していた（例えば、特許文献１や非特許文献１）。ただしここで、ｘは追尾目標の位置であり、ｘ"は時間ｔによる二階微分を表す（ｘ"＝ｄ^２ｘ／ｄｔ^２）。またｎ_ｔは蛇行運動の最大振幅、ωは蛇行周波数、φは位相差である。

特開２００２−１８９５１９「追跡システム、及び、追跡方法」公報 第４図、段落００３８ "旋回（蛇行）目標追尾を行う多重運動モデルを用いたカルマンフィルタ"、電子情報通信学会総合大会、Ｂ−２−２０、２００３年３月

従来の目標追尾装置は、上記のように構成されているので、各サンプリング間における状態変数の時間変位を扱おうとすると、状態変数の一つである加速度の時間変化、すなわち、ジャーク項（加速度の時間による一階微分項）まで扱う必要がある。ところが、観測雑音が大きい場合やサンプリング間隔が長い場合には、ジャーク項の推定精度は劣化する傾向にある。また、蛇行周波数が小さい場合（蛇行軌跡の波長が長い場合）には、ジャーク項の推定まで行う必要が本来ないにも関わらず、上記の運動モデルを用いると、必然的にジャーク項を推定することとなって無駄な処理を行うことになる。さらに、蛇行振幅が大きい場合に、ある時刻の推定値（位置及び速度）を採用し、その時刻の速度で等速直線運動をすると仮定して数サンプリング後の位置予測を行うと、実際の目標位置との乖離が大きくなってしまう。この発明は、かかる問題を解決することを目的とする。

この発明に係る目標追尾装置は、前回サンプル時の運動諸元平滑値から今回サンプル時の運動諸元平滑値をカルマンフィルタによって算出する目標追尾装置であって、
等速直線運動成分と正弦運動成分とを有する追尾目標の運動モデルに基づいて、前回サンプル時の運動諸元平滑値から今回サンプル時の運動諸元平滑値を算出する運動諸元予測手段を備えたものである。

この発明は、従来技術で用いる「加速度を基準とする運動モデル」ではなく、位置を基準とする運動モデルを採用し、さらに正弦運動成分だけでなく、等速直線運動成分を有する運動モデルを採り入れることによって、蛇行運動を行う追尾目標の追尾処理を精度よく行うことが可能となり、追尾目標が蛇行するために観測値と予測値が大きくずれるといった問題を少なくする効果を奏する。

以下、この発明の実施の形態について、図を用いて説明する。
実施の形態１．
図１は、この発明の実施の形態１による目標追尾装置の構成を示すブロック図である。図において、ゲート判定器１０は、観測値が追尾に有効なデータであるか否かを判定する部位である。予測諸元統合器１１は、後述する運動諸元予測器によって算出される予測ベクトルが複数ある場合に、それらを統合して統合予測ベクトルとその誤差共分散行列である統合予測誤差共分散行列を算出する部位である。平滑諸元統合器１２は、算出された平滑運動諸元が複数ある場合に、それらを統合して、統合平滑値と統合平滑誤差共分散行列を算出する部位である。統合平滑値メモリ１３は、統合平滑値、統合平滑誤差共分散行列および後述する運動諸元予測処理によって算出される各運動モデルの事後確率を格納する記憶素子または回路によって構成される。

また、運動モデル制御器１４は、追尾目標に対して追尾処理を行うにあたり、採用する運動モデルの数と各運動モデルに設定する蛇行周波数を決定する部位である。運動諸元予測器１５−１〜１５−Ｎ（Ｎは１以上の自然数）は、それぞれ所定の運動モデルに基づいてカルマンフィルタを実行することにより、追尾目標の運動諸元を予測する部位である。また運動諸元予測器１５−１〜１５−Ｎは、それぞれの運動諸元予測値の確からしさについて評価するために、運動モデルの評価値である尤度を算出するようになっている。混合平滑器１６は、運動諸元予測器が複数ある場合、すなわちＮが２以上の場合に、それぞれの運動諸元予測器が算出した運動諸元平滑値に対して、他の運動諸元予測器によって算出された運動諸元平滑値に基づく調整処理を行う部位である。より具体的にいえば、混合平滑器１６は、各運動モデルの平滑値と平滑誤差共分散行列をミキシングと呼ばれる処理を行うようになっている。モデル尤度集計器１７は、運動諸元予測器１５−１〜１５−Ｎが算出した追尾結果を評価するために用いる尤度の総和を算出する部位である。未来位置予測器１８は、平滑諸元統合器１２が算出した運動諸元平滑値に基づいて、追尾目標の未来位置を予測する部位である。

ここで、運動諸元予測器１５−１〜１５−Ｎの構成について補足する。Ｎが２以上の場合、すなわち、複数の運動諸元予測器を備える場合にあっては、各運動諸元予測器は他の運動諸元予測器とは異なる運動モデルを用いるようにした方が有利である。運動諸元予測器１５−１〜１５−Ｎのそれぞれが、種類の異なる複数の運動モデルを用いることで、各運動モデルによる追尾処理結果と現実の追尾目標についての観測値との差異の大小が生ずることになる。当然ながら、この場合には観測値と近い追尾処理結果を算出する運動モデルの処理精度が、それ以外の運動モデルの処理精度に優ることになる。複数の種類の運動モデルを採用することで、単一の運動モデルによる追尾処理より処理精度を向上させることができるのである。このように、運動諸元予測器１５−１〜１５−Ｎのそれぞれがどの運動モデルに基づく追尾処理を行うかを決定する役割を担うのが、運動モデル制御器１４である。

しかしながら、運動諸元予測器の個数は２以上であることは必須ではなく、少なくとも１つ備えるだけでも、以降の説明から明らかとなるように、従来の技術にはないこの発明の特徴が達成されるのである。したがって、運動諸元予測器の個数、すなわちＮの値は、設計事項である。すなわち、Ｎ＝１（単一の運動モデル）として、簡易な構成を採用してもよい。その場合には、運動諸元予測器１５−１の算出する予測値をこの発明の実施の形態１による目標追尾装置の出力値として出力すればよく、予測諸元統合器１１、平滑諸元統合器１２、混合平滑器１６、モデル尤度集計器１７に相当する機能を省略して構成するようにする。

一方、追尾する目標がさまざまな蛇行周波数による蛇行運動をすることが想定される場合には、複数の運動モデルに基づく運動諸元予測をした方が有利になる。そして運動諸元予測器の個数、すなわちＮの数が大きければ大きいほど、精度の高い追尾処理を行うことができるし、幅広い蛇行周波数をカバーすることもできるのである。ここで、Ｎ個の運動諸元予測器を備える、とは、物理的に別体のＮ個の運動諸元予測器を備えることはもちろん、Ｎ個に満たない個数の運動諸元予測器のそれぞれが、１サンプル時間を時分割して複数の運動モデルに基づく追尾処理を行い、結果としてＮ個の運動モデルに基づく追尾処理を行うようにしておいてもよい。

図２は、運動諸元予測器１５−１の詳細な構成を示すブロック図である。図に示すように、運動諸元予測器１５−１は平滑諸元メモリ２１、設定器２２，予測器２３、平滑器２４を備えており、これらの構成要素によって運動モデル制御器１４が設定した運動モデルに基づいてカルマンフィルタを実行するようになっている。なお一般に、カルマンフィルタは所定の時間毎（これをここではサンプルと呼ぶ）における予測値を算出すると共に、各時刻における観測値をサンプリングして取得し、予測値と観測値から平滑値を求め、次回のサンプルに予測値算出に供するというループ処理を行うものである。運動諸元予測器１５−１は、外部の予測諸元統合器１１、平滑諸元統合器１２、混合平滑器１６、モデル尤度集計器１７と協働しながら、これらのループ処理を実行するようになっている。

平滑諸元メモリ２１は、運動諸元予測器１５−１が追尾処理の基礎としている運動モデルについて、前回サンプル時の平滑値と平滑誤差共分散行列を記憶する記憶回路または素子である。また、ハードディスク装置やＣＤ−ＲＯＭドライブ装置のように記憶媒体を用いた記憶装置により構成してもよい。ここで、前回サンプルの平滑値と平滑誤差共分散行列は、混合平滑器１６によって算出され、平滑諸元メモリ２１に記憶されるようになっている。設定器２２は、運動モデル制御器１４が運動諸元予測器１５−１に割り当てた運動モデルから各種パラメータを算出する部位である。なお、各種パラメータとは、状態遷移行列、蛇行周波数などを初めとするパラメータであって、追尾処理を開始するのに必要となるパラメータをいう。

予測器２３は、前回サンプルの平滑値と平滑誤差共分散行列、および運動モデルから、今回サンプルにおける運動諸元予測値を算出する部位である。ここで算出された運動諸元予測値は予測諸元統合器１１に出力されるようになっている。ここで用いられる前回サンプルの平滑値と平滑誤差共分散行列は、平滑諸元メモリ２１によって記憶されているものであり、また運動モデルは設定器２２によって記憶されているものである。

平滑器２４は、予測器２３が算出した今回サンプルにおける運動諸元予測値と、ゲート判定器１０が相関ありと判断して出力した追尾目標の観測値とを用いて平滑処理を行う部位である。ここで算出された運動諸元平滑値は平滑諸元統合器１２に出力されるようになっている。

さらに、運動諸元予測器１５−１は、確率メモリ２５、事前確率算出器２６、尤度算出器２７、事後確率算出器２８を備えており、これらの構成要素によって運動諸元予測器１５−１が基礎とする運動モデルの評価値（事前確率、尤度、事後確率など）を算出するようになっている。確率メモリ２５は、前回サンプルにおける事後確率を記憶する記憶回路または素子である。事前確率算出器２６は、確率メモリ２５に記憶されている前回サンプルの事後確率に基づいて、今回サンプルの事前確率を算出する部位である。ここで算出された事前確率は、予測諸元統合器１１に出力されるようになっている。尤度算出器２７は、事前確率算出器２６が算出した事前確率と、予測器２３が算出した運動諸元予測値、ゲート判定器１０が相関ありと判断して出力した追尾目標の観測値とから尤度を算出する部位である。ここで算出された尤度はモデル尤度集計器１７に出力されるようになっている。事後確率算出器２８は、尤度算出器２７が算出した尤度から事後確率を算出する部位である。ここで算出された事後確率は、今回サンプルの事後確率として確率メモリ２５に記憶され、次回サンプルにおける事前確率や尤度、事後確率の算出に供されることとなる。

なお、運動諸元予測器１５−２〜１５−Ｎにおいても、平滑諸元メモリ２１、設定器２２、予測器２３、平滑器２４、確率メモリ２５、事前確率算出器２６、尤度算出器２７、事後確率算出器２８に相当する構成要素が存在するが、これらの構成については、運動諸元予測器１５−１と同様であるので、説明を省略する。また、以後の説明において、特に運動諸元予測器１５−２〜１５−Ｎの構成要素に言及する場合は、例えば、「運動諸元予測器１５−２の平滑諸元メモリ２１」のように、明示的にその運動諸元予測器の符号を示すこととする。また、運動諸元予測器の符号を特定せずに、単に「平滑諸元メモリ２１」といった場合には、運動諸元予測器１５−１における確率メモリ２５を指すものとする。このような用法は、設定器２２、予測器２３、平滑器２４、確率メモリ２５、事前確率算出器２６、尤度算出器２７、事後確率算出器２８についても同様とする。

また、ここまでの説明において、例えば「予測値を算出する部位である」というように、何らかの「部位」であるとした構成要素は、そのための専用回路または素子として構成されることを意味する。ただし、同様の機能をコンピュータプログラムによって、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ：中央演算装置）を備えたコンピュータ若しくはマイコンチップに実行させる構成を採ってもよい。

さらに、ゲート判定器１０はゲート判定手段の例であり、予測諸元統合器１１は予測諸元統合手段の例である。また、平滑諸元統合器１２は、平滑諸元統合手段の例であって、統合平滑値メモリ１３は、統合平滑値記憶手段の例である。運動モデル制御器１４は運動モデル制御手段の例であって、運動諸元予測器１５−１〜１５−Ｎは運動諸元予測手段の例である。混合平滑器１６は混合平滑手段の例であって、モデル尤度集計器１７はモデル尤度集計手段の例である。

次に、この発明の実施の形態１による目標追尾装置の動作について説明する。これまでの説明からも理解されるように、この目標追尾装置は、運動諸元予測器１５−１〜１５−Ｎのそれぞれに運動モデルを割り当ててカルマンフィルタによる追尾処理を行い、これらが算出した運動諸元予測値、運動諸元平滑値や確率を、予測諸元統合器１１、平滑諸元統合器１２、統合平滑値メモリ１３、運動モデル制御器１４、混合平滑器１６、モデル尤度集計器１７によって調整するようになっている。そこで、以降の説明では、主として運動諸元予測器１５−１における追尾処理を順を追って説明することとし、予測諸元統合器１１、平滑諸元統合器１２、統合平滑値メモリ１３、運動モデル制御器１４、混合平滑器１６、モデル尤度集計器１７における処理については、運動諸元予測器１５−１が算出した運動諸元や確率値と運動諸元予測器１５−２〜１５−Ｎが算出した運動諸元や確率値との調整が必要となる局面において説明することとする。

まず、追尾処理を開始するにあたって、統合平滑値メモリ１３に各運動モデルの平滑値の初期値と平滑誤差共分散行列の初期値を設定しておく。また、運動諸元予測器１５−１〜１５−Ｎのそれぞれの確率メモリ２５に、その運動諸元予測器に割り当てられる運動モデルの事後確率の初期値を設定しておく。これらの準備が整った状態で、追尾処理が開始する。以降、新たな観測値が得られるたびに、サンプリング時間を１つずつ進めながら、追尾処理を実行していく。

運動モデル制御器１４は、平滑諸元統合器１２より統合平滑値メモリ１３を介して追尾目標の状態変数及びモデル確率を取得する。そして、安定した追尾精度を得るのに必要なモデル数と設定する蛇行周波数を決定し、運動諸元予測器１５−１〜１５−Ｎの設定器２２に運動モデルを設定する。ここで運動モデル制御器１４が設定する運動モデルは、式（１）〜式（３）による正弦運動成分と等速直線運動成分とを有する運動モデルｘ(t)である。

式（１）に示すように、ｘ(t)は成分ｘ₁(t)と成分ｘ₂(t)とを有している。成分ｘ₁(t)は正弦運動を表す成分であり、ｘ₂(t)は等速直線運動を表す成分である。ｎ_ｔは正弦運動の最大振幅であり、ωは正弦運動の蛇行周波数である。またａは等速直線運動成分における速度であり、ｂはｔ＝０における初期速度である。

なお、式（１）は１軸における運動モデルを表現するものであるが、現実の追尾目標では、自動車や船舶などのように平面運動をする場合は２次元運動となり、航空機の追尾などの場合は３次元運動となる。そこで、それぞれの次元について独立して式（１）で与えられる運動モデルを設定することになる。しかし、ここでは理解を容易にするために、１次元運動モデルに基づいて説明を行うこととする。

運動諸元予測器が複数ある場合には、運動モデル制御器１４は、式（２）のωの値を変えた複数の運動モデルを運動諸元予測器１５−１〜１５−Ｎの設定器２２にそれぞれ設定する。例えば、対象となる追尾目標がとりうる蛇行周期が１０秒〜６０秒である場合、蛇行周波数は１／１０〜１／６０Ｈｚとなる。したがってＮ＝１１とした場合（１１個の運動諸元予測器を備えた場合）には、１／１０、１／１５、１／２０、１／２５、１／３０、１／３５、１／４０、１／４５、１／５０、１／５５、１／６０のようなωを運動諸元予測器１５−１〜１５−１１のそれぞれの設定器２２に設定すればよい。ただし、その他の蛇行周波数を選択しても構わないのであって、例えば、１／１０、１／２０、１／２５、１／３０、１／３３、１／３６、１／３９、１／４２、１／４５、１／５０、１／６０のように一部の周波数区間の分解能を細かくするようにしてもよい。

引き続き、運動モデルについて説明する。式（１）による運動モデルは連続時間における運動モデルであるが、カルマンフィルタではサンプル時間ごと、すなわち離散時間における運動モデルを設定する方が適している。そこで、式（１）から離散時間における運動モデルを導出することとする。式（２）、式（３）より、式（４）、式（５）の微分方程式が得られる。

これらの微分方程式を状態空間において表現すると、正弦運動成分については、

等速直線運動成分については、

となる。ここで、ｘ₁'(t)、ｘ₂'(t)はそれぞれｘ₁(t)、ｘ₂(t)の時間による１階微分を表す。さらにｘ ₁(t)、ｘ ₂(t)は正弦運動成分と等速直線運動成分それぞれの状態変数を表しており、ｘ ₁'(t)、ｘ ₂'(t)はｘ ₁(t)、ｘ ₂(t)の各成分を時間微分したものである。Ｆ₁とＦ₂はそれぞれの成分の連続時間における状態遷移行列を表しており、Ｄ₁とＤ₂は駆動雑音の変換行列を表している。ｗ₁(t)とｗ₂(t)はそれぞれの成分の駆動雑音である。

ここで、正弦運動成分の状態変数ｘ₁(t)と等速直線運動成分の状態変数ｘ₂(t)とを一つの状態変数ｘ(t)にまとめる。すなわち
ｘ(t)＝[ｘ ₁(t) ｘ ₁'(t) ｘ ₂(t) ｘ ₂'(t)]^Ｔ
である。そうすると式（６）〜式（１３）より、ｘ(t)に関する次のような連続微分状態方程式が得られる。

ただし、
ｗ(t)＝[０ ｗ₁(t) ０ ｗ₂(t)]^Ｔ
であって、ｗ(t)は駆動雑音であり、式（１７）、式（１８）を満たす連続時間の白色雑音過程である。

上記において、δ(t)はディラックのデルタ関数であり、τは次の条件を満たす時刻である。すなわち、ｔ≠τの場合にディラックのデルタ関数の値が０となり、ｔ＝τの場合にディラックのデルタ関数の値が無限大となる。また式（１９）において、ｑ_１、ｑ_２は白色雑音過程のパワースペクトル密度であり、［ｍ^２／ｓ^３］の次元を持つ（ｍは距離の次元であり、ｓは時間の次元である）。

一方、連続時間における追尾目標の観測方程式は、式（２０）及び式（２１）によって与えられる。

ここで、Ｈは観測行列を、Ｇ_２は観測が行われる極座標から北基準直交座標への座標変換行列を表している。またｚ(t)は観測値である。ｖ(t)は観測雑音ベクトルであって、平均０、共分散Ｒの白色ガウス雑音である。

式（１４）〜式（２１）を離散時間のブロック線図で表すと、例えば図３のようになる。図３においてｓは、ラプラス変換における複素変数、つまり微分演算子を表す。またｙ_sinは正弦運動成分における追尾目標の位置、ｙ_stは等速直線運動成分における追尾目標の位置、ｖ_nは観測雑音であって、ｖ_sin、ｖ_stは正弦運動成分および等速直線運動成分のそれぞれにおける駆動雑音を表している。さらに、ｚは観測値である。この図から明らかなように、蛇行目標の位置の時間変動を等速直線運動成分と正弦運動成分とを有する運動モデルで表現することにより、加速度及びジャーク項を推定する処理を必要とせずに、蛇行周波数の推定を行うことができるのである。また、等速直線運動成分の推定値（位置および速度）を用いることで、Ｋサンプル後（Ｋは自然数）の追尾目標の位置を、蛇行軌跡の中心軸上に推定することが可能となる。

ｋサンプル目（ｋは自然数）の離散時間における運動モデルは、式（４）と式（５）の微分方程式の解を求めることで、

となる。なお式（２２）、（２４）において、Φは離散時間における状態遷移行列である。またｘ_1,k、ｘ_2,kは、それぞれ正弦運動成分ｘ₁と等速直線運動成分ｘ₂のｋサンプル目の状態変数を表している。さらにｗ _kは、４次元の離散時間白色雑音系列であって、以下の性質を有する。

ここで、δ_ｋｌはクロネッカーのデルタ関数であって、ｋ＝ｌの場合に値が１となり、ｋ≠ｌで値が０となる。

また、式（２０）と式（２１）による観測方程式を離散時間で表現すると、

となる。ここで、ｖ _kは観測雑音ベクトルであり、平均０、共分散Ｒ_kの白色ガウス雑音となる。またＧ_2,kは観測雑音の極座標から北基準直交座標への座標変換行列を表す。以上により、離散時間における運動モデルが導出された。

次に、設定器２２は、運動モデル制御器１４が設定した運動モデルに基づいて、状態遷移行列、駆動雑音の誤差共分散行列および蛇行周波数を計算し、予測器２３に出力する。

予測器２３は、平滑諸元メモリ２１が記憶している混合平滑値と混合平滑誤差共分散行列を読み出し、これらの値と設定器２２が算出した状態遷移行列、駆動雑音の誤差共分散行列および蛇行周波数とに基づいて、現在の運動モデルの予測ベクトルｘ＾_{ｋ，ａ(−)}と予測誤差共分散行列Ｐ _{ｋ，ａ(−)}を式（３０）と式（３１）に基づいて算出する。なお、ここでｘ＾の右肩添字＾は、数式表現上は、ｘの真上添字で表される。また添字ａは運動モデルａについての状態変数であることを示すものである。

なお、平滑諸元メモリ２１は、初期状態では混合平滑値と混合平滑誤差共分散行列の初期値を記憶しており、また定常状態（少なくとも１サンプル以上の観測値を処理した状態）では、前回サンプルの混合平滑値と混合平滑誤差共分散行列を記憶している。ここで求められた予測ベクトルと予測誤差共分散行列は、予測諸元統合器１１、平滑器２４、尤度算出器２７にそれぞれ出力される。

次に、事前確率算出器２６は確率メモリ２５から前回サンプル時の事後確率を読み出して、式（３２）によって今回サンプルの事前確率Ｐｒ［Ψ_k,a｜Ｚ^k-1］を算出して、予測諸元統合器１１と尤度算出器２７に出力する。

ここで、μ_k,a(−)はｋサンプル目の運動モデルａについての事前確率であり、μ_k,a(＋)はｋサンプル目の運動モデルａについての事後確率であって、Ｐ_abは運動モデルｂから運動モデルａに推移する確率を表す。

続いて、予測諸元統合器１１は、運動諸元予測器１５−１〜１５−Ｎの予測器２３と事前確率算出器２６とがそれぞれ算出した各運動モデルについての運動諸元予測値と、予測誤差共分散行列、事前確率を取得して、各運動モデルの仮説ごとの予測ベクトル及び予測誤差共分散行列を統合し、１サンプリング前までの観測値に基づく統合予測ベクトルｘ＾_k(−)と統合予測誤差共分散行列Ｐ_k(−)とを、式（３３）と式（３４）によって計算し、これらをゲート判定器１０に出力する。

次にゲート判定器１０は、図示せぬセンサが取得した観測値から極端に観測精度の悪いデータを除外するために、統合予測ベクトルｘ＾_k(−)と統合予測誤差共分散行列Ｐ_k(−)から、残差２次形式を用いて観測値が有効なデータであるかどうかの判定を行う。ゲート判定器１０により観測値が有効なデータであると判定された場合、ゲート判定器１０は尤度算出器２７と平滑器２４にｋサンプル目の観測値ｚ _kを出力する。

尤度算出器２７は、ｋサンプル目の観測値ｚ _kを取得すると、事前確率算出器２６が算出した今回サンプルの事前確率Ｐｒ[Ψ_k,a｜Ｚ^k-1]と、予測ベクトルｘ＾_k,a(−)と予測誤差共分散行列Ｐ _k,a(−)を用いて、式（３５）より運動モデルの尤度を算出し、事後確率算出器２８とモデル尤度集計器１７に出力する。

ここで、ｇ（ｚ；ａｖ，Ａ）は平均ａｖ、共分散行列Ａの３変量正規分布のｚにおける確率密度関数を表す。またＲ_kは観測雑音ベクトルの共分散行列である。

次に、モデル尤度集計器１７は、運動諸元予測器１５−１〜１５−Ｎが算出した各運動モデルの尤度を合計して尤度合計値を求め、運動諸元予測器１５−１〜１５−Ｎのそれぞれの事後確率算出器２８に出力する。事後確率算出器２８は、式（３６）によって事後確率を計算して、平滑諸元統合器１２、混合平滑器１６に出力するとともに、確率メモリ２５に記憶させる。

ここで、右辺の分母はモデル尤度算出器１７が算出した尤度合計値を表しており、また右辺の分子は尤度算出器２７が算出した運動モデルａについてのｋサンプル目の尤度である。

一方、平滑器２４は、予測器２３が算出した予測ベクトルｘ＾_k.a(−)と予測誤差共分散行列Ｐ _k,a(−)から、現在の運動モデルの平滑ベクトルと平滑誤差共分散行列およびカルマンゲインを、式（３７）、式（３８）、式（３９）により計算し、平滑諸元統合器１２と混合平滑器１６に出力する。

次に混合平滑器１６は、運動諸元予測器１５−１〜１５−Ｎのそれぞれの平滑器２４が算出した平滑ベクトルと平滑誤差共分散行列、そして運動諸元予測器１５−１〜１５−Ｎのそれぞれの事後確率算出器２８が算出した事後確率から、式（４０）と式（４１）に基づいてｋ−１サンプル目の混合平滑ベクトルｘ＾_k-1,mix(+)および混合平滑誤差共分散行列Ｐ _k-1,mix(+)を計算し、運動諸元予測器１５−１〜１５−Ｎのそれぞれの平滑諸元メモリ２１に記憶させる。

ｋ−１サンプル目の混合平滑ベクトルおよび混合平滑誤差共分散行列は、ｋサンプル目のループにおける予測ベクトルや予測誤差共分散行列の算出に供される。このように、混合平滑器１６が、運動モデルａ以外の運動モデルｂの平滑値を用いて、運動モデルａによるｋ−１サンプル目の平滑値を調整し、この平滑値に基づいて運動モデルａによるｋサンプル目の予測値や平滑値を算出するようにすることで、他の運動モデルにおける予測処理の尤度を運動モデルａの予測処理に反映して、運動モデルａによる予測処理の適正化を図ることができるのである。ただし、このような処理は必須ではなく、平滑器２４が算出した平滑ベクトルおよび平滑誤差共分散行列を直接平滑諸元メモリに書き込む構成を採ってもよい。

平滑諸元統合器１２は、運動諸元予測器１５−１〜１５−Ｎのそれぞれの運動モデルについて平滑ベクトルと平滑誤差共分散行列、および事後確率を取得して、式（４３）にしたがって統合平滑ベクトルと統合平滑誤差共分散行列とを算出する。

さらに、平滑諸元統合器１２は、運動諸元予測器１５−１〜１５−Ｎの事後確率算出器２８が算出した事後確率と各運動モデルにおいて定数として与えられている蛇行周波数を用いて、式（４５）から蛇行周波数推定値を算出するようにしてもよい。

統合平滑ベクトルｘ＾_k(＋)と統合平滑誤差共分散行列Ｐ _k(＋)は、ジャーク項の推定を行わないために、すでに十分な推定精度を確保している。したがって、これらの値をこの発明の実施の形態１による目標追尾装置の出力値としてもよい。目標の追尾を維持し、１サンプル先に追尾目標がどの位置に移動するか予測して、レーダのビームを照射する用途においては、統合平滑ベクトルｘ＾k(＋)と統合平滑誤差共分散行列Ｐk(＋)を出力値とすれば十分である。

しかし、ここではさらに、未来位置予測器１８を設けることによって、Ｋサンプル後の位置を予測して追尾目標との衝突を回避したり、Ｋサンプル後の追尾目標の予測位置に誘導物（例えば飛翔体など）を誘導する用途に用いることができるようにする。すなわち、この目標追尾装置は、等速直線運動成分と正弦運動成分とを有する運動モデルに基づいて目標追尾を行うので、条件によって追尾目標が蛇行運動の中心軸上に存在するものと仮定して目標追尾を行ったり、蛇行軌跡上に存在するものと仮定して目標追尾を行うことができるのである。

そのために、例えば所定の点と追尾目標とが離れている場合、すなわち所定の点と追尾目標との距離が一定値以上の場合に、蛇行軌跡の中心軸上の点を追尾目標の予測位置とする。こうすることで、航空機の場合であれば、現在の飛行を維持して時間や燃料のロスを抑えながら、衝突回避することができる。具体的には次のようにする。すなわち、追尾目標の状態変数は式（２３）で与えられるように正弦運動成分ｘ_１と等速直線運動成分ｘ_２とを有しており、このことから、追尾目標のｋサンプル目の平滑位置ｘ＾_{ｔｇｔ，ｋ}(＋)は式（４６）で表しうる。

所定の点と追尾目標との距離が一定値以上の場合は、追尾目標のＫサンプル後の予測位置を蛇行軌跡の中心軸上の点とするので、ｘ−ｙ平面上の２次元追尾において追尾目標のＫサンプル後の予測位置（ｘ＾_c,k+K(−)、ｙ＾_c,k+K(−)）は、正弦運動成分を無視することにより、式（４７）〜式（４９）によって与えられる。

なお、式（４７）において、Ｔはサンプリング間隔であり、またｘ＾_ｋ(＋)とｙ＾_ｋ(＋)はそれぞれ、式（４３）で与えられる統合平滑値のｘ及びｙ座標成分を表している。

一方、所定の点と追尾目標とが近接している場合、すなわち所定の点と追尾目標との距離が一定値未満の場合には、追尾目標の未来位置を蛇行軸上の点として算出するよりも、
、蛇行軌跡上の点として算出した方が望ましい。これは、例えば所定の点が追尾目標の運動の障害物である場合に、衝突の危険性を予見するために、より詳細な追尾目標の予測航跡が必要となるからである。たとえば追尾目標が他の航空機や船舶であり、所定の点が自航空機や自船舶の場合、衝突の危険性を予測し、衝突回避を図る場合に有用となる。

このような場合には、Ｋサンプル後まで追尾目標の蛇行周波数、最大加速度が一定であると仮定し、蛇行軌跡上に存在する未来位置の予測値（ｘ＾_w,k+K(−)，ｙ＾_w,k+K(−)）を式（５０）によって算出する。

ここで、ｎ＾_px及びｎ＾_pyは正弦運動成分推定値の最大振幅を、ω_x及びω_yはこの目標追尾装置により推定された蛇行周波数を表す。各座標軸で動作するフィルタの正弦運動成分推定値の最大振幅ｎ＾_px及びｎ＾_pyは、蛇行運動の１周期の間（蛇行周期は推定蛇行数波数より換算してＴ＝１／ωとなる）における正弦運動成分の推定位置の最大値が、蛇行運動の最大振幅となる。したがって、最大振幅は蛇行運動の１周期毎に更新され、更新後は１周期の間、その大きさが不変であるものと仮定する。
また、φ_k,x及びφ_k,yはｋサンプル目の正弦関数の位相であって、式（５１）より算出する。

以上から明らかなように、この発明の実施の形態１の目標追尾装置によれば、正弦運動成分と等速直線運動成分とを有する運動モデルに基づいて目標追尾を行うこととしたので、蛇行運動を行う追尾目標の追尾処理を精度よく予測することができる。

実施の形態２．
Paul Zarchan "Tactical and Strategic Missile Guidance Third Edition" Volume 176, American Institute of Aeronautics and Astronauticsn, Inc.. p.12〜p.29 p.433〜p.439では、波状蛇行目標の運動として、速度ベクトルに対して垂直方向に正弦関数により表現される加速度が加えられるモデルが検討されている。そこで、図４のように（ｖ_ｘ，ｖ_ｙ）を成分とする２次元の速度ベクトルにより移動する追尾目標を考えることにする。ここで図のように、ｔａｎθ＝ｖ_ｙ／ｖ_ｘとし、またベクトル（ｖ_ｘ，ｖ_ｙ）と垂直をなす大きさａの加速度ベクトルが作用しているものとする。さらに速度ベクトル（ｖ_ｘ，ｖ_ｙ）の大きさは一定であるものとする。

この場合、加速度ベクトルの成分を（ａ_ｘ(t)，ａ_ｙ(t)）とすると、
ａ_ｘ(t)＝−ｎ_ｔ ｃｏｓωｔ ｓｉｎθ(t)
ａ_ｙ(t)＝−ｎ_ｔ ｃｏｓωｔ ｃｏｓθ(t)
で与えられる。ただしｎ_ｔは最大振幅である。速度ベクトルを時間の関数とみなした場合の成分（ｖ_x(t)，ｖ_y(t)）は、図４におけるθの定義から、
ｖ_x(t)＝ｖ ｃｏｓθ(t)
ｖ_y(t)＝ｖ ｓｉｎθ(t)
となるから、これらの式を時間ｔについて微分すると、
ａ_x(t)＝ｖ_x'(t)＝−ｖ ｓｉｎθ(t)・θ'(t)
ａ_y(t)＝ｖ_y'(t)＝−ｖ ｃｏｓθ(t)・θ'(t)
となる。よって、θ(t)の時間ｔによる一階微分値θ'(t)は、式（５３）によって与えられる。

式（５３）を時間ｔで積分することにより、角度θ(t)は、

ここで、φは初期位相である。初期位相φ＝０とした場合、ｖ_ｘ(t)とｖ_ｙ(t)は式（５５）によって与えられる。

式（５４）から角度θ(t)とｖ_ｘ、ｖ_ｙとの時間変化の関係を理解する上では、これ以上フーリエ級数等により展開するよりも、図５に示した概念図で各変数の関係を把握した方が容易である。図において、上段のグラフはθ(t)の時間変化の様子を表したものであり、中段のグラフはｖ_ｘの時間変化、下段のグラフはｖ_ｙの時間変化を表したものである。中段と下段のグラフを対比すれば明らかなように、ｘ軸方向の速度変化はｙ軸方向の２倍高調波となっている。これは、速度ベクトルが加速度の変化により図４の上下方向に振れる場合に、ｖ_y(t)が速度０を中心に増減を繰り返す間にｖ_x(t)はｖ_y(t)の２倍の周波数で変動するためである。

以上より、加速度の蛇行周波数をωとした場合に、波状蛇行目標の速度変動はωと２ωの２つの周波数による正弦運動成分を有することがわかる。蛇行周波数２ωの正弦運動成分は、進行方向についての位置の変動を与えるものであるため、蛇行周期が短い場合にはその振幅を無視しても影響は小さい。しかし蛇行周期が長くなると、振幅も増大するので影響が無視できなくなり、蛇行周波数２ωの正弦運動成分を有する運動モデルが必要となる。この発明の実施の形態２による目標追尾装置は、かかる運動モデルにより運動する追尾目標を追尾することを想定したものであり、蛇行周波数２ωの正弦運動成分を有する運動モデルを用いることを特徴を有するものである。

この発明の実施の形態２による目標追尾装置の構成を示すブロック図としては実施の形態１と同様に図１を用いるが、この発明の実施の形態２による目標追尾装置は、実施の形態１とは運動モデルが異なるのみであるので、各構成要素の構成については説明を省略する。

次に、この発明の実施の形態２による目標追尾装置の動作について説明する。この発明の実施の形態２による目標追尾装置においても、実施の形態１と同様の初期処理を行い、運動モデル制御器１４は運動諸元予測器１５−１〜１５−Ｎのそれぞれの設定器２２に運動モデルを設定する。すでに述べたとおり、この目標追尾装置は、蛇行周波数２ωの正弦運動成分を有する運動モデルを用いる。この運動モデルは式（５６）によって表される。

式（５６）〜式（５９）において、ｘ₁(t)は蛇行周波数ωの正弦運動成分、ｘ₂(t)は蛇行周波数２ωの正弦運動成分、ｘ₃(t)は等速直線運動成分を表しており、ｎ_t1、ｎ_t2はそれぞれｘ₁(t)、ｘ₂(t)の最大振幅を表すものである。またａは等速直線運動の速度成分（傾き）であり、ｂは初期速度（切片）である。

次に、実施の形態１と同様に、この発明の実施の形態２においても、式（５６）〜式（５９）から離散時間における運動モデルを導出する。式（５６）〜式（５９）を微分方程式として表すと、式（６０）〜式（６２）のようになる。

となる。これらの微分方程式を状態空間において表現すると、

となる。ここで、Ｆ_１、Ｆ_２及びＦ_３はそれぞれ蛇行周波数ωの正弦運動成分、蛇行周波数２ωの正弦運動成分、等速直線運動成分の連続時間における状態遷移行列を、Ｄ_１、Ｄ_２及びＤ_３は駆動雑音の変換行列を表す。またｗ₁(t)、ｗ₂(t)及びｗ₃(t)は、各成分における駆動雑音である。

式（６３）〜式（７１）を１つにまとめることにより、次の連続微分方程式が得られる。

ｗ(t)は各成分における駆動雑音であり、式（７７）〜式（７９）の性質を満たす連続時間の白色雑音過程である。

ここで、δ(t)はディラックのデルタ関数である。また、ｑ_１、ｑ_２、ｑ_３は白色雑音過程のパワースペクトル密度で［ｍ^２／ｓ^３］（ｍは距離、ｓは時間）の次元を持つ。

一方、連続時間における追尾目標の観測方程式は、式（８０）及び式（８１）によって与えられる。

ここで、Ｈは観測行列、Ｇ_２は観測が行われる極座標から北基準直交座標への座標変換行列を表している。またｙ(t)は観測値である。ｖ(t)は観測雑音ベクトルであって、平均０、共分散Ｒの白色ガウス雑音である。

式（６３）から式（８１）を離散時間のブロック線図で表すと、例えば図６のようになる。図６において、実施の形態１のブロック線図（図３）と同じ符号又は変数を付している部分については図３と同じであるので説明を省略する。ｙ_sin2は蛇行周波数２ωの正弦運動成分における追尾目標の位置を表している。この図から分かるように、実施の形態１と同様に加速度及びジャーク項を推定する処理は不要であり、また等速直線運動成分の推定値（位置および速度）を用いることで、Ｋサンプル後の追尾目標の位置を、蛇行軌跡の中心軸上に推定することが可能となる。

ｋサンプル目の離散時間における運動モデルは、式（６０）〜式（６２）の微分方程式の解を求めることで、

となる。なお式（８２）、（８４）において、Φは離散時間における状態遷移行列である。またｘ_1,k、ｘ_2,k、ｘ_3,kは、それぞれ正弦運動成分ｘ₁、ｘ₂と等速直線運動成分ｘ₃のｋサンプル目の状態変数を表している。さらにｗ _kは、４次元の離散時間白色雑音系列であって、以下の性質を有する。

また式（８０）と式（８１）の観測方程式を離散時間で表現すると、

となる。ここで、ｖ _ｋは観測雑音ベクトルであり、平均０、共分散Ｒ_ｋの白色ガウス雑音となる。またＧ_2,kは観測雑音の極座標から北基準直交座標への座標変換行列を表す。以上により離散時間における運動モデルが導出された。

続いて運動諸元予測器１５−１〜１５−Ｎは、実施の形態１と同様にカルマンフィルタによる追尾処理を実行する。また予測諸元統合器１１、平滑諸元統合器１２、統合平滑値メモリ１３、運動モデル制御器１４、混合平滑器１６、モデル尤度集計器１７は、運動諸元予測器１５−１〜１５−Ｎが算出する予測ベクトルや予測誤差共分散行列、平滑ベクトル、平滑誤差共分散行列、事前確率、尤度、事後確率の調整、重み付けを行うという点についても、実施の形態１と同様である。

次に、未来位置予測器１８は、所定の点と追尾目標が一定値以上離れている場合に、蛇行軌跡の中心軸上の点を追尾目標の予測位置とし、また所定の点と追尾目標との距離が一定値未満の場合に、蛇行軌跡上の点を追尾目標の予測位置とする。実施の形態２による運動モデルを用いれば、追尾目標のｋサンプル目の平滑位置ｘ＾_tgt,k(＋)は式（８９）で表される。

ここで、ｘ＾_1,k(＋)及びｘ＾'_1,k(＋)は蛇行周波数ωの正弦運動成分の位置と速度の統合平滑値を表している。また、ｘ＾_2,k(＋)及びｘ＾'_2,k(＋)は蛇行周波数２ωの正弦運動成分の位置と速度の統合平滑値を表している。ｘ＾_3,k(＋)及びｘ＾'_3,k(＋)は等速直線運動成分の位置と速度の統合平滑値を表すものである。

所定の点と追尾目標との距離が一定値以上の場合は、ｘ−ｙ平面上の２次元追尾において追尾目標のＫサンプル後の予測位置（ｘ＾_{ｃ，ｋ＋Ｋ}(−)、ｙ＾_{ｃ，ｋ＋Ｋ}(−)）は、実施の形態１と同様に正弦運動成分を無視することにより、式（９１）〜式（９３）によって与えられる。

なお、式（９１）において、Ｔはサンプリング間隔であり、またｘ＾_k(＋)とｙ＾_k(＋)はそれぞれ、式（４３）で与えられる統合平滑値のｘ及びｙ座標成分を表している。

所定の点と追尾目標との距離が一定値未満の場合は、ｘ−ｙ平面上の２次元追尾において追尾目標のＫサンプル後の予測位置（ｘ＾_w,k+K(−)、ｙ＾_w,k+K(−)）は、式（９４）によって算出する。

ここで、ｎ＾_1p,x及びｎ＾_1p,yは蛇行周波数ωの正弦運動成分についての推定値の最大振幅を表す。また、ｎ＾_2p,x及びｎ＾_2p,yは蛇行周波数２ωの正弦運動成分についての推定値の最大振幅を表す。ω_ｘ及びω_ｙはこの目標追尾装置により推定された蛇行周波数を表す。そしてφ_k,x及びφ_k,yはｋサンプル目の正弦関数の位相であって、式（９５）より算出する。

以上から明らかなように、この発明の実施の形態２の目標追尾装置によれば、正弦運動成分（蛇行周波数ω）と等速直線運動成分に加えて、蛇行周波数２ωの正弦運動成分を有する運動モデルに基づいて目標追尾を行うこととしたので、波状蛇行運動を行う追尾目標の追尾処理を精度よく予測することができる。

なお、実施の形態２による目標追尾装置においても、未来位置予測器１８を経ずに、平滑諸元統合器１２の出力をこの目標追尾装置の出力とする構成を採ってもよい。この目標追尾装置は、正弦運動成分（蛇行周波数ω）と等速直線運動成分と蛇行周波数２ωの正弦運動成分をも合わせ持つ運動モデルに基づいて追尾処理を行うことで、波状蛇行運動を精度よく追尾できるという特徴を有するものであり、平滑諸元統合器１２が統合平滑値を出力する段階でも、すでにその効果が確保されているのである。

実施の形態３．
実施の形態１及び２による目標追尾装置においては、３次元追尾を行う場合に、運動モデル制御器１４が、各軸に対応するように運動モデルを運動諸元予測器１５−１〜１５−Ｎのそれぞれの設定器２２に設定していた。しかし、蛇行目標の中心軸の傾きと切片を状態変数ベクトルの成分に含めて、ｘ軸から蛇行中心軸へ座標変換することにより、蛇行目標の運動諸元を推定して、未来位置予測を行うようにしてもよい。この発明の実施の形態３による目標追尾装置は、このような特徴を有するものである。

この発明の実施の形態３による目標追尾装置の構成は、実施の形態１と同様に図１のブロック図によって示される。実施の形態３による目標追尾装置の運動諸元予測器１５−１〜１５−Ｎは、非線形運動モデルから導出される拡張カルマンフィルタによって追尾処理を行う点で、実施の形態１とは異なるものである。しかしその他の点については、実施の形態１と同様であるので、構成については説明を省略することとする。

次にこの発明の実施の形態３による目標追尾装置の動作について説明する。ここでも、実施の形態１と同様の初期処理を行い、運動モデル制御器１４によって運動諸元予測器１５−１〜１５−Ｎのそれぞれの設定器２２に運動モデルが設定される。ここで、蛇行目標がｘ軸に対して傾きθ(t)、切片ρ(t)の蛇行軸上を移動しているとした場合、蛇行中心軸に対して垂直方向の位置時間変動（中心軸に対する揺動成分）を正弦関数により表現すると、運動モデルの連続微分方程式は、式（９８）〜式（１０２）によって与えられる。

ｗ(t)は駆動雑音であって、式（１０３）、式（１０４）を満たす連続時間の白色雑音過程である。

ここで、δ(t)はディラックのデルタ関数、Ｑは駆動雑音の誤差共分散行列を表す。

一方、連続時間における追尾目標の観測方程式は、式（１０５）、式（１０６）によって与えられる。

ここで、Ｈは観測行列、Ｇ_２は観測が行われる極座標から北基準直交座標への座標変換行列を表している。またｙ(t)は観測値である。ｖ(t)は観測雑音ベクトルであって、平均０、共分散Ｒの白色ガウス雑音である。

上記連続微分方程式において、サンプリング周期をＴ、サンプリング時刻ｔ＝ｋＴ（ｋ＝０，１，２，…）におけるｘ(t)の値をｘ _ｋとすると、式（１０７）の離散時間運動モデルが導かれる。

ここで、式（１０７）の状態変数ベクトルｘ _ｋ及び状態遷移行列Φは、以下の式により得られる。

式（１１０）のｗ _ｋは、４次元の離散時間白色雑音系列であって、以下の性質を有する。

式（１０７）の運動方程式は目標がｘ軸上を移動していると仮定しているため、状態変数ベクトルｘ _ｋをｘ軸方向へ角度−θ回転させた後、切片ρを差し引いた場合、以下の式が得られる。

ここでＡは以下の座標変換行列を表す。

式（１１３）から、

となる。ここでｘ _k＝ｇ(ｘ _k-1)とすれば、予測ベクトルｘ＾_k+1(−)及び予測誤差共分散行列Ｐ_k(−)は、

と表される。ただしｘ＾_k-1(＋)はｋ−１サンプル目の平滑ベクトルであって、Ｐ_k(−)は同じくｋ−１サンプル目の平滑誤差共分散行列、Ｑ_k-1は駆動雑音共分散行列を表している。また、Φ_p,k-1は座標変換後の状態遷移行列であり、関数ｇ(ｘ＾_k(＋))の傾きを表すために、以下の式より求められる。

また、式（１０５）による観測方程式を離散時間で表すと

となる。ここで、ｖ _ｋは観測雑音ベクトルであり、平均０、共分散Ｒ_ｋの白色ガウス雑音である。

以上の運動方程式と観測方程式とをもとに、この発明の実施の形態３による目標追尾装置で使用する拡張カルマンフィルタを導出する。時刻ｋにおける各運動モデルの予測ベクトル及び予測誤差共分散行列は、

となる。予測器２３は、実施の形態１の式（３０）と式（３１）に替えて、式（１２０）と式（１２１）を用いて予測ベクトルと予測誤差共分散行列とを算出する。

次に平滑器２４は、平滑ベクトルと平滑誤差共分散行列、カルマンゲインを算出する。平滑値、平滑誤差共分散行列およびカルマンゲインはそれぞれ、

となる。

以上のようにして、平滑器２４が算出した平滑値を実施の形態３による目標追尾装置の出力値としてもよいが、実施の形態１および２と同様に未来位置予測器１８を用いて、追尾目標が蛇行軌跡の中心軸上、または蛇行軌跡上に存在すると仮定して、Ｋサンプル後の位置を算出する。まず、蛇行軌跡の中心軸上に存在すると仮定した場合、ｘ−ｙ平面上の２次元追尾において追尾目標のＫサンプル後の予測位置（ｘ＾_c,k+K(−)、ｙ＾_c,k+K(−)）は、正弦運動成分を無視することにより、式（１２３）〜式（１２６）によって与えられる。

なお、Ｔはサンプリング間隔である。

また、追尾目標が蛇行軌跡上に存在すると仮定した場合、Ｋサンプル後の未来位置の予測値（ｘ＾_c,k+K(−)、ｙ＾_c,k+K(−)）は、式（１２７）で与えられる。

ここで、ｎ＾_pyは正弦運動成分推定値の最大振幅であって、ω＾_yはこの目標追尾装置によって推定された蛇行周波数を表している。またφ_k,yはｋサンプル目の正弦関数の位相であって、式（１２４）から算出される。

以上から明らかなように、この発明の実施の形態３による目標追尾装置によれば、拡張カルマンフィルタを用いることで、実施の形態１及び２のように各座標軸ごとにフィルタを独立して動作させなくても、各軸の観測雑音等の相関を考慮した３次元追尾や未来位置予測が可能となる。

実施の形態４．
実施の形態１乃至３の目標追尾装置は、等速直線運動成分と正弦運動成分とを有する運動モデルに基づいているが、等速直線運動モデルのみに基づいて追尾処理を行う専用の運動諸元予測器を新たに設け、この運動諸元予測器の算出する平滑値の信頼度が所定値以上の場合に、運動諸元予測器１５−１〜１５−Ｎの動作を停止させたり、計算結果を使用しないようにしてもよい。実施の形態４による目標追尾装置は、かかる特徴を有するものである。

図８は、この発明の実施の形態４による目標追尾装置の構成を示すブロック図である。図において、等速直線運動諸元予測器１９は、等速直線運動モデルに基づいて追尾処理を行う部位である。等速直線運動諸元予測器１９による追尾処理の結果（予測値と平滑値）は予測諸元統合器１１と平滑諸元統合器１２、そして混合平滑器１６にそれぞれ出力されるようになっている。

また信頼度判定器２０は、等速直線運動諸元予測器１９及びその他の運動諸元予測器の算出する事後確率に基づいて信頼度判定を行う部位である。その他、図１と同一の符号を付した構成要素については、実施の形態１と同様であるので説明を省略するが、運動諸元予測器１５−１〜１５−Ｎが算出する事後確率が信頼度判定器２０にも出力される点、および信頼度判定器による判定結果が平滑諸元統合器１２に出力されるようになっている点が実施の形態１とは相違している。

次に、この発明の実施の形態４による目標追尾装置の運動諸元予測器１５−１〜１５−Ｎおよび等速直線運動諸元予測器１９の詳細な構成について説明する。ここで、運動諸元予測器１５−１〜１５−Ｎと等速直線運動諸元予測器１９とは、依拠する運動モデルが異なっているのみであって、構成要素については同様であるので、運動諸元予測器１５−１を代表して説明する。図９は、この発明の実施の形態４による目標追尾装置の運動諸元予測器１５−１の詳細な構成を示すブロック図である。

図において、事後確率算出器２８は、実施の形態１と同様に事後確率を算出するが、ここで計算された事後確率は、平滑諸元統合器１２、混合平滑器１６、確率メモリ２５に加えて、信頼度判定器２０にも出力されるようになっている。その他、図２と同じ符号を付した構成要素については、実施の形態１と同様であるので説明を省略する。

次に、この発明の実施の形態４による目標追尾装置の動作について説明する。運動諸元予測器１５−１〜１５−Ｎの動作については、実施の形態１と同様であり、さらに等速直線運動諸元予測器１９については、等速直線運動モデルに基づくカルマンフィルタを用いた目標追尾装置がすでに公知であるので、説明を省略する。ここでは、実施の形態１と同様の処理によって、各運動諸元予測器（等速直線運動諸元予測器１９を含む）によって事後確率が算出されたものとし、その事後確率を信頼度として、信頼度判定器２０が、等速直線運動諸元予測器１９の出力を採用するか否かを判断する。

例えば、等速直線運動諸元予測器１９が算出した事後確率をμ_k,1(＋)とした場合に、Ｍサンプル間（Ｍは自然数であって、望ましくは２以上の整数とする）、継続的にしきい値ｄ_μを上回る場合には、等速直線運動諸元予測器１９の出力した予測値と平滑値を、この目標追尾装置の出力として採用する。ここで、しきい値ｄ_μを一定値としてもよいし、運動諸元予測器１５−１〜１５−Ｎの算出する事後確率に基づいて算出するようにしてもよい。

この場合、予測諸元統合器１１は、運動諸元予測器１５−１〜１５−Ｎの予測値の算出処理完了を待たずに、等速直線運動諸元予測器１９の予測値を取得し次第、動作を開始するようにする。また、平滑諸元統合器１２は、運動諸元予測器１５−１〜１５−Ｎの平滑値の出力完了を待たずに、等速直線運動諸元予測器１９が平滑値を算出し次第、平滑値の出力や未来位置予測処理を開始するようにする。こうすることにより、運動諸元予測器１５−１〜１５−Ｎの計算処理の待機に要する時間が不要となるので、目標追尾装置全体のスループットが向上する。さらに、このことからサンプル時間を短縮できるので、多くの観測値を処理できるようになり、予測精度が向上する。

また、等速直線運動諸元予測器１９の出力結果を採用する条件が満たされた場合に、運動諸元予測器１５−１〜１５−Ｎの動作を停止するような制御を行ってもよい。こうすることによって、例えば運動諸元予測器の個数よりも少ない数の中央演算装置を用いて時分割により複数の運動モデルに基づく追尾処理を行う構成としている場合には、中央演算装置の負荷を軽減できる。

なお、実施の形態４において、運動諸元予測器１５−１〜１５−Ｎに割り当てる運動モデルとして、実施の形態２で用いた蛇行周波数ωと２ωの２つの正弦運動成分を有する運動モデルを用いてもよい。

また、実施の形態４において、実施の形態３で用いた拡張カルマンフィルタを用いてもよいことはいうまでもない。

実施の形態５．
実施の形態４では、等速直線運動モデルを用いて追尾処理を行う等速直線運動諸元予測器１９の信頼度に基づいて、運動諸元予測器１５−１〜１５−Ｎの処理を省略することとした。しかしこの他にも、蛇行運動を行う追尾目標の最大振幅が所定値以下の場合に、運動諸元予測器１５−１〜１５−Ｎの処理を省略して、等速直線運動諸元予測器１９の出力値を採用するようにしてもよい。この発明の実施の形態５による目標追尾装置は、かかる特徴を有するものである。

図１０は、この発明の実施の形態５による目標追尾装置の構成を示すブロック図である。図に示すように、この発明の実施の形態５による目標追尾装置は、実施の形態４における信頼度判定器２０を廃し、それに替えて振幅判定器３１を設けたものである。振幅判定器３１は、平滑諸元統合器１２が算出した統合平滑値のうちの蛇行軌跡の最大振幅が所定値以下か否かを判定する部位である。平滑諸元統合器１２は、振幅判定器３１が蛇行軌跡の最大振幅が所定値以下と判断した場合に、等速直線運動諸元予測器１９の算出した平滑値を統合平滑値として採用するようになっている。

次に、この発明の実施の形態５による目標追尾装置の動作について説明する。実施の形態５においても、運動諸元予測器１５−１〜１５−Ｎにおける平滑値の算出および事後確率の算出は、実施の形態４と同様なので説明を省略する。平滑諸元統合器１２は、運動諸元予測器１５−１〜１５−Ｎの算出した平滑値から、例えば式（４５）などを用いて、統合推定値の最大振幅を算出すると、この値を振幅判定器３１に出力する。

振幅判定器３１は、例えば以下の条件式をｎ＾_p,x（ｘ軸方向の蛇行振幅）、ｎ＾_p,y（ｙ軸方向の蛇行振幅）が満たすかどうかを判定し、その結果を平滑諸元統合器１２に出力する。

平滑諸元統合器１２は、振幅判定器３１より式（１２６）を満たすという判定結果を受け取った場合には、実施の形態４と同様に等速直線運動諸元予測器１９の算出した平滑値を採用し、運動諸元予測器１５−１〜１５−Ｎの算出結果を省略する。またその場合において、実施の形態４と同様に運動諸元予測器１５−１〜１５−Ｎの動作を停止するようにしてもよい。

以上から明らかなように、この発明の実施の形態５の目標追尾装置によれば、追尾目標の運動が等速直線運動に近似できる場合には、等速直線運動モデルを採用して、正弦運動成分を有する運動モデルによる追尾処理を省略するので、計算負荷を低減することができるという効果を奏する。

この発明は、例えば航空機や船舶、自動車などの移動体の追尾を行う装置やシステムに適用することができる。

この発明の実施の形態１による目標追尾装置の構成を示すブロック図である。 この発明の実施の形態１による目標追尾装置の詳細な構成を示すブロック図である。 この発明の実施の形態１による目標追尾装置の運動モデルの離散時間によるブロック線図である。 この発明の実施の形態２による目標追尾装置の対象とする追尾目標の速度と加速度の関係を説明する説明図である。 この発明の実施の形態２による目標追尾装置の対象とする追尾目標のｘ軸方向、ｙ軸方向の各速度成分の時間変動の関係を説明する説明図である。 この発明の実施の形態２による目標追尾装置の運動モデルの離散時間によるブロック線図である。 この発明の実施の形態３における追尾目標の航跡の例を示す概念図である。 この発明の実施の形態４による目標追尾装置の構成を示すブロック図である。 この発明の実施の形態４による目標追尾装置の詳細な構成を示すブロック図である。 この発明の実施の形態５による目標追尾装置の構成を示すブロック図である。

符号の説明

１０ ゲート判定器、
１１ 予測諸元統合器、
１２ 平滑諸元統合器、
１３ 統合平滑値メモリ、
１４ 運動モデル制御器、
１５−１〜１５−Ｎ 運動諸元予測器、
１６ 混合平滑器、
１７ モデル尤度集計器、
１８ 未来位置予測器、
１９ 等速直線運動諸元予測器、
２１ 平滑諸元メモリ、
２２ 設定器、
２３ 予測器、
２４ 平滑器、
２５ 確率メモリ、
２６ 事前確率算出器、
２７ 尤度算出器、
２８ 事後確率算出器、
３１ 振幅判定器。

Claims (11)

1. 前回サンプル時の運動諸元平滑値から今回サンプル時の運動諸元平滑値をカルマンフィルタによって算出する目標追尾装置において、
   等速直線運動成分と正弦運動成分とを有する追尾目標の運動モデルに基づいて、前回サンプル時の運動諸元平滑値から今回サンプル時の運動諸元平滑値を算出する運動諸元予測手段を備えたことを特徴とする目標追尾装置。
2. 前記運動諸元予測手段は、前記追尾目標の位置を等速直線運動成分と正弦運動成分との和で表した前記運動モデルに基づいて、前記運動諸元平滑値を算出することを特徴とする請求項１に記載の目標追尾装置。
3. 前記運動諸元予測手段は、前記正弦運動成分の周波数をωとした場合に、周波数を２ωとする正弦運動成分をさらに有する前記運動モデルに基づいて、前記運動諸元平滑値を算出することを特徴とする請求項１又は２に記載の目標追尾装置。
4. 前回サンプル時の運動諸元平滑値から今回サンプル時の運動諸元平滑値を拡張カルマンフィルタによって算出する目標追尾装置において、
   蛇行軌跡の中心軸に対する揺動成分を正弦運動に近似することにより追尾目標の運動を表現するとともに、前記中心軸の傾き角と切片とを状態変数ベクトルとする運動モデルに基づいて、前回サンプル時の運動諸元平滑値から今回サンプル時の運動諸元平滑値を算出する運動諸元予測手段を備えたことを特徴とする目標追尾装置。
5. 前記運動諸元予測手段は、正弦運動成分の周波数が相異なる複数の前記運動モデルに基づいて、前記運動諸元平滑値を算出することを特徴とする請求項１乃至４のいずれか一に記載の目標追尾装置。
6. 前記運動諸元予測手段により前記各運動モデルについて算出された複数の運動諸元平滑値を統合する平滑諸元統合手段を備えたことを特徴とする請求項５に記載の目標追尾装置。
7. 前記運動諸元予測手段が用いる前記複数の運動モデルの信頼度をそれぞれ算出するモデル信頼度算出手段を備え、
   前記平滑諸元統合手段は、前記モデル信頼度算出手段の算出した信頼度に基づいて前記複数の運動諸元平滑値を統合することを特徴とする請求項６に記載の目標追尾装置。
8. 前回サンプル時に、前記運動諸元予測手段が算出した運動諸元平滑値を、前記平滑諸元統合手段が統合する他の運動モデルによる運動諸元平滑値に基づいて調整する混合平滑手段を備え、
   前記運動諸元予測手段は、今回サンプル時の運動諸元平滑値の算出にあたり、前回サンプル時の運動諸元平滑値に替えて、前記混同平滑手段が調整した運動諸元平滑値を用いることを特徴とする請求項６又は７に記載の目標追尾装置。
9. 等速直線運動モデルに基づいて前記追尾目標の運動諸元平滑値を算出する等速直線運動諸元予測手段を備え、
   前記モデル信頼度算出手段は、前記等速直線運動諸元予測手段が算出した運動諸元平滑値についても信頼度を算出し、
   前記平滑諸元統合手段は、前記モデル信頼度算出手段が算出した信頼度が所定値以上の場合に、前記等速直前運動諸元予測手段が算出した運動諸元平滑値を統合結果とすることを特徴とする請求項７記載の目標追尾装置。
10. 等速直線運動モデルに基づいて前記追尾目標の運動諸元平滑値を算出する等速直線運動諸元予測手段を備え、
    前記平滑諸元統合手段は、前記追尾目標の航跡が有する正弦運動成分の振幅が所定値以下の場合に、前記等速直前運動諸元予測手段の運動諸元平滑値を統合結果とすることを特徴とする請求項６乃至８のいずれか一に記載の目標追尾装置。
11. 前記運動諸元予測手段が算出した今回サンプルの運動諸元平滑値に含まれる等速直線運動による位置成分の時間変位にＫ回（Ｋは自然数）分のサンプリング間隔を乗じてＫサンプリング間の位置成分時間変位を求めるとともに、今回サンプルの前記追尾目標の位置に前記Ｋサンプリング間の位置成分時間変位を加えてＫサンプリング後の予測位置を算出し、かつ所定の点と前記追尾目標との距離が一定値未満である場合には、前記運動諸元平滑値に含まれる正弦運動の周波数と位相からその正弦運動のＫサンプリング後の時間変位を求めて前記Ｋサンプリング後の予測位置に加えたものを前記Ｋサンプリング後の予測位置として出力する未来位置予測手段を備えたことを特徴とする請求項１乃至１０のいずれか一に記載の目標追尾装置。

Images