目標追尾装置及び目標追尾方法
【課題】ゲートが不必要に拡大するのを防ぎ、高速移動目標に対しても正確にゲート内に目標を捕捉できる目標追尾装置および目標追尾方法を得る。
【解決手段】本発明の追尾装置及び追尾方法は、北基準直交座標での目標運動における加速度ベクトルを駆動雑音とせずに、極座標での速度ベクトルのふらつきを駆動雑音としている。すなわち、平滑値ベクトルから速度ノルムと駆動雑音ベクトルの座標変換行列を算出し、その値と駆動雑音分散の設定値とから駆動雑音共分散行列を算出し、それを用いて追尾処理を行う。これにより、サンプリング間隔が大きい時にも、ゲートの極端な拡大を防ぐとともに、１サンプリング前の目標の速度成分を引用するゲート設定により、目標速度の大小に合わせた駆動雑音を設定し、目標速度に応じてゲートの広さを変化させ、最適なゲート設定を行うことが可能となる。
【解決手段】本発明の追尾装置及び追尾方法は、北基準直交座標での目標運動における加速度ベクトルを駆動雑音とせずに、極座標での速度ベクトルのふらつきを駆動雑音としている。すなわち、平滑値ベクトルから速度ノルムと駆動雑音ベクトルの座標変換行列を算出し、その値と駆動雑音分散の設定値とから駆動雑音共分散行列を算出し、それを用いて追尾処理を行う。これにより、サンプリング間隔が大きい時にも、ゲートの極端な拡大を防ぐとともに、１サンプリング前の目標の速度成分を引用するゲート設定により、目標速度の大小に合わせた駆動雑音を設定し、目標速度に応じてゲートの広さを変化させ、最適なゲート設定を行うことが可能となる。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は目標追尾装置及び目標追尾方法に関し、特に、レーダなどのセンサからなる観測装置を用いて、航空機、船舶、車両などの移動体の位置を観測し、その観測結果に基づいて移動体の真の位置、速度などの運動諸元を高精度に推定する追尾装置および追尾方法に関するものである。
【背景技術】
【０００２】
一般に、観測装置による目標観測結果を用いた目標追尾処理では、カルマンフィルタを使用する（例えば、特許文献１参照）。
【０００３】
従来技術においては、３次元空間用の等速直線運動モデルや等加速度運動モデルを使用し、目標運動のあいまいさを表す駆動雑音ベクトルをｘ，ｙ，ｚの各方向の加速度項で定義している。以下、等速直線運動モデルを使用した場合を例に、従来の目標追尾方法を説明する。
【０００４】
カルマンフィルタを使用した追尾処理では、設定する座標系を、図８のようにセンサを原点、東方向をｘ軸の正、北方向をｙ軸の正、水平面（ｘ−ｙ面）上方向をｚ軸の正に取った北基準直交座標とする。つぎに、目標の運動モデルを以下のように仮定する。ただし、サンプリング時刻（以下、単に時刻）をｔ（ｋ）で表す。
【０００５】
【数１】
【０００６】
但し、
ｘ（ｋ）：時刻ｔ（ｋ）における目標運動諸元の真値を表す状態変数ベクトル
Φ：時刻ｔ（ｋ−１）から時刻ｔ（ｋ）への状態ベクトルの既知の推移行列
ｗ（ｋ）：北基準直交座標の時刻ｔ（ｋ）における駆動雑音ベクトル
Γ：駆動雑音ベクトルの変換行列
とする。
【０００７】
従来の一般的な追尾装置では、目標の基本的な運動をｘ−ｙ−ｚの３次元空間における北基準直交座標の等速直線運動と仮定して上記運動モデルを記述する。たとえば、目標の位置と速度を推定する場合、状態ベクトルｘ（ｋ）、状態推移行列Φを次式のように定義する。
【０００８】
【数２】
【０００９】
但し、Ｔはベクトル及び行列の転置、ドットは時間微分を表す。また、Iｎ×ｎ及び０ｎ×ｎはそれぞれｎ行ｎ列の単位行列及び零行列であり、τは一定のサンプリング周期を表す。
【００１０】
さらに、ｗ（ｋ）は現実の目標運動を北基準直交座標の等速直線で近似したことによるｘ（ｋ）の誤差を表現するために設けられたｘ，ｙ，ｚの各方向の加速度項の雑音成分で構成されており、平均零ベクトルで駆動雑音共分散行列Ｑ（ｋ）の３変量白色正規分布に従う駆動雑音ベクトルであると仮定する。
【００１１】
つぎに、状態ベクトル及び観測雑音ベクトルを使用して目標位置の観測ベクトルを表現する際の、観測装置の観測モデルを以下のように仮定する。
【００１２】
【数３】
【００１３】
但し、
ｚ（ｋ）：時刻ｔ（ｋ）における目標位置の観測ベクトル
Ｈ：観測行列
ｖ（ｋ）：時刻ｔ（ｋ）における北基準直交座標による観測雑音ベクトル
とする。
【００１４】
たとえば、観測装置によりｘ−ｙ−ｚの３次元空間の北基準直交座標系における目標位置の観測結果が得られるとする。このとき、観測ベクトルｚ（ｋ）、観測行列Ｈを次式のように定義する。
【００１５】
【数４】
【００１６】
また、ｖ（ｋ）は観測装置の観測誤差を表すための雑音成分であり、平均零ベクトルで観測雑音共分散行列Ｂ（ｋ）の３変量白色正規分布に従う北基準直交座標による観測雑音ベクトルであると仮定する。
【００１７】
上記のような運動モデル、観測モデルの仮定のもと、カルマンフィルタは以下に示すアルゴリズムで、状態ベクトルの予測を行う。時刻ｔ（ｋ−１）までの観測結果に基づいて時刻ｔ（ｋ）の状態ベクトルを推定した予測値ベクトルをｘハット（ｋ｜ｋ−１）と表し、また、ｘハット（ｋ｜ｋ−１）の誤差共分散行列を表す予測誤差共分散行列をＰ（ｋ｜ｋ−１）と表す。
【００１８】
【数５】
【００１９】
なお、駆動雑音共分散行列Ｑ（ｋ）はパラメータとして与える。
【００２０】
つぎに、ゲートの設定を以下のように仮定する。ゲートとは、図９に示すように、追尾目標が存在すると予測される範囲であり、ゲート内の観測ベクトルを追尾に使用するものである。ゲートの中心は、１サンプリング前までの観測ベクトルの情報に基づく、時刻ｔｋの観測ベクトルｚ（ｋ）の推定値である。これを考慮し、上記式（５）に、予測ベクトルの定義及び観測雑音ベクトルは平均零ベクトルの白色雑音であるとの仮定を使用することで、以下の式が得られる。
【００２１】
【数６】
【００２２】
但し、
Ｓ（ｋ）：残差ベクトルｚ（ｋ）−ｚ（ｋ−１）の共分散行列
ｄ：ゲートの大きさを決めるパラメータ
とする。
【００２３】
式（１０）がゲートの中心、式（１１）がゲートの広がり、式（１２）がゲートの判定を示す。ここで、式（７），（４），（９）及び（１１）より、次式（１３）を得る。
【００２４】
【数７】
【００２５】
式（１３）の右辺の第２項では、ゲートの広がりを判定する要素の一つが駆動雑音共分散行列のサンプリング間隔の影響であるということを示している。
【００２６】
ゲート内に追尾目標の観測ベクトルが得られると、つぎに、状態ベクトルの更新を行う。時刻ｔ（ｋ）までの観測結果に基づいて時刻ｔ（ｋ）の状態ベクトルを推定した平滑ベクトルをｘハット（ｋ｜ｋ）と表し、ｘハット（ｋ｜ｋ）の誤差共分散行列を表す平滑誤差共分散行列をＰ（ｋ｜ｋ）と表す。さらに、Ｋ（ｋ）はフィルタのゲイン行列であり、上記式（２）〜（４），（６），（７）の設定を行った場合、６行３列の行列である。
【００２７】
【数８】
【００２８】
なお、ｘハット（ｋ｜ｋ）の初期値ｘハット（０｜０）と、Ｐ（ｋ｜ｋ）の初期値Ｐ（０｜０）が別途算出され、与えられるものとする。また、駆動雑音共分散行列Ｑ（ｋ）、観測雑音共分散行列Ｂ（ｋ）はパラメータとして与える。
【００２９】
カルマンフィルタは、式（８），（１５）により、平均的な偏りのない不偏推定値を算出するとともに、式（１４）でゲイン行列を算出することにより、平均２乗誤差を最小化する最適なフィルタとして知られている。
【００３０】
尚、従来技術による一般的な追尾装置の構成を図１０に示す。
図１０において、１は平滑値ベクトル記憶手段に、目標の位置、速度等の運動諸元で構成される状態ベクトルの平滑値ベクトルの初期値を設定する平滑値ベクトル初期値設定手段、２は平滑値ベクトルの値を記憶する平滑値ベクトル記憶手段、３は前時刻の平滑値ベクトルより現時刻の予測値ベクトルを算出する予測手段、４は前記予測値ベクトルと観測装置からの観測値ベクトルを入力し、ゲートを設定するゲート範囲設定手段、５は前記ゲートと予測値ベクトルとゲイン行列設定手段よりゲイン行列を入力して、平滑値ベクトルを算出する平滑手段である。
６は目標速度の分散値を設定する駆動雑音分散設定手段、７は前記駆動雑音分散より駆動雑音共分散行列を算出する駆動雑音共分散行列算出手段、８は平滑誤差共分散行列記憶手段に、平滑値ベクトルの誤差を推定した平滑誤差共分散行列の初期値を設定する平滑誤差共分散行列初期値設定手段、９は平滑誤差共分散行列の値を記憶する平滑誤差共分散行列記憶手段、１０は前時刻の平滑誤差共分散行列と駆動雑音共分散行列より現時刻の予測誤差共分散行列を算出する予測誤差共分散行列算出手段、１１は前記予測誤差共分散行列とゲイン行列設定手段よりのゲイン行列を入力し、平滑誤差共分散行列を算出する平滑誤差共分散行列算出手段である。
また、１２は観測雑音共分散行列が与えられる観測雑音共分散行列算出手段、１３は予測誤差共分散行列の値と観測雑音共分散行列より残差共分散行列を算出する残差共分散行列算出手段、１４は前記残差共分散行列の値と予測誤差共分散行列の値よりゲイン行列が設定されるゲイン行列設定手段である。
【００３１】
【特許文献１】特開平１１−４４７５４号公報
【発明の開示】
【発明が解決しようとする課題】
【００３２】
上述したように、カルマンフィルタは、適度なサンプリング間隔で目標の運動モデルと観測装置の観測モデルが正しく表現されていれば、最適なゲートの広がりの設定が行われ、追尾性能を発揮することができる。しかしながら、式（１３）の右辺の第２項では、ゲートの広がりを決める要素の一つが駆動雑音共分散行列のサンプリング間隔の影響であるということを示したように、ここでの値（τ４／４）×Ｑ（ｋ−１）は、目標速度、速度ベクトルの向きに関係無く、サンプリング間隔τが大きくなれば、ゲートが極端に拡大していくことを表している。
【００３３】
このため、たとえば等速直線運動を行っていた目標が等速円運動で９０度旋回したのち、再び等速直線運動を行う場合でも、ｘ，ｙ，ｚ方向おのおのにおいて、サンプリング間隔が大きくなるほど、ゲートは拡大していく。また、単純に目標が等速直線運動を行っている時でも、サンプリング間隔が大きくなると、非現実的な大きさにゲートが拡大してしまう。ゲートが不必要に拡大してしまうと、不要信号を受信する確率が高まり、目標の追尾を外す可能性が高くなるという問題点がある。
【００３４】
さらに、目標の加速度ベクトルを駆動雑音として設定するため、速度成分を引用しない一定の大きさのゲート設定となり、目標速度が大きい時にその速度に適したゲートの範囲が割り当てられず、高速移動目標に対してゲート内に目標を捕捉できず、追尾を外してしまう危険性を含むという問題点があった。
【００３５】
本発明は、かかる問題点を解決するためになされたものであり、サンプリング間隔が大きい時にゲートが不必要に拡大するのを防ぎ、また、目標速度の大小に合わせたゲート設定により、高速移動目標に対しても適切なゲートが設定される目標追尾装置および目標追尾方法を得ることを目的としている。
【課題を解決するための手段】
【００３６】
この発明は、目標の位置、速度等の運動諸元で構成される状態ベクトルの平滑値ベクトルの初期値を設定する平滑値ベクトル初期値設定手段と、前記平滑値ベクトルの初期値および算出される平滑値ベクトルの値を記憶する平滑値ベクトル記憶手段と、前記平滑値ベクトル記憶手段に記憶されている前時刻の平滑値ベクトルより現時刻の予測値ベクトルを算出する予測手段と、前記平滑値ベクトル記憶手段に記憶されている前記平滑値ベクトルに基づいて速度ノルムを算出する速度ノルム計算手段と、算出された前記速度ノルムと前記平滑値ベクトル記憶手段に記憶されている前記平滑値ベクトルとを入力し、駆動雑音ベクトルの座標変換行列を算出する駆動雑音ベクトルの座標変換行列算出手段と、駆動雑音分散として目標速度の分散を設定する駆動雑音分散設定手段と、設定された前記駆動雑音分散と前記駆動雑音ベクトルの座標変換行列算出手段により算出された駆動雑音ベクトルの座標変換行列とにより、駆動雑音共分散行列を算出する駆動雑音共分散行列算出手段と、平滑ベクトルの誤差を推定する平滑誤差共分散行列の初期値を設定する平滑誤差共分散行列初期値設定手段と、前記平滑誤差共分散行列の初期値および算出される平滑誤差共分散行列の値を記憶する平滑誤差共分散行列記憶手段と、前記平滑誤差共分散行列記憶手段に記憶されている前時刻の平滑誤差共分散行列と前記駆動雑音共分散行列算出手段により算出された前記駆動雑音共分散行列とにより、現時刻の予測誤差共分散行列を算出する予測誤差共分散行列算出手段と、算出された前記予測誤差共分散行列の値と観測装置から得られる観測雑音共分散行列とにより、残差共分散行列を算出する残差共分散行列算出手段と、算出された前記残差共分散行列の値と前記予測誤差共分散行列算出手段により算出された前記予測誤差共分散行列の値とによりゲイン行列を設定するゲイン行列設定手段と、前記予測手段により算出された前記予測値ベクトルと前記観測装置から得られる観測値ベクトルとを入力し、ゲートを設定するゲート範囲設定手段と、設定された前記ゲートと前記予測手段により算出された前記予測値ベクトルと前記ゲイン行列設定手段により設定された前記ゲイン行列とを入力して、平滑値ベクトルを算出する平滑手段と、前記予測誤差共分散行列算出手段により算出された前記予測誤差共分散行列と前記ゲイン行列設定手段により設定されたゲイン行列とを入力し、平滑誤差共分散行列を算出する平滑誤差共分散行列算出手段とを備えた目標追尾装置である。
【発明の効果】
【００３７】
この発明は、目標の位置、速度等の運動諸元で構成される状態ベクトルの平滑値ベクトルの初期値を設定する平滑値ベクトル初期値設定手段と、前記平滑値ベクトルの初期値および算出される平滑値ベクトルの値を記憶する平滑値ベクトル記憶手段と、前記平滑値ベクトル記憶手段に記憶されている前時刻の平滑値ベクトルより現時刻の予測値ベクトルを算出する予測手段と、前記平滑値ベクトル記憶手段に記憶されている前記平滑値ベクトルに基づいて速度ノルムを算出する速度ノルム計算手段と、算出された前記速度ノルムと前記平滑値ベクトル記憶手段に記憶されている前記平滑値ベクトルとを入力し、駆動雑音ベクトルの座標変換行列を算出する駆動雑音ベクトルの座標変換行列算出手段と、駆動雑音分散として目標速度の分散を設定する駆動雑音分散設定手段と、設定された前記駆動雑音分散と前記駆動雑音ベクトルの座標変換行列算出手段により算出された駆動雑音ベクトルの座標変換行列とにより、駆動雑音共分散行列を算出する駆動雑音共分散行列算出手段と、平滑ベクトルの誤差を推定する平滑誤差共分散行列の初期値を設定する平滑誤差共分散行列初期値設定手段と、前記平滑誤差共分散行列の初期値および算出される平滑誤差共分散行列の値を記憶する平滑誤差共分散行列記憶手段と、前記平滑誤差共分散行列記憶手段に記憶されている前時刻の平滑誤差共分散行列と前記駆動雑音共分散行列算出手段により算出された前記駆動雑音共分散行列とにより、現時刻の予測誤差共分散行列を算出する予測誤差共分散行列算出手段と、算出された前記予測誤差共分散行列の値と観測装置から得られる観測雑音共分散行列とにより、残差共分散行列を算出する残差共分散行列算出手段と、算出された前記残差共分散行列の値と前記予測誤差共分散行列算出手段により算出された前記予測誤差共分散行列の値とによりゲイン行列を設定するゲイン行列設定手段と、前記予測手段により算出された前記予測値ベクトルと前記観測装置から得られる観測値ベクトルとを入力し、ゲートを設定するゲート範囲設定手段と、設定された前記ゲートと前記予測手段により算出された前記予測値ベクトルと前記ゲイン行列設定手段により設定された前記ゲイン行列とを入力して、平滑値ベクトルを算出する平滑手段と、前記予測誤差共分散行列算出手段により算出された前記予測誤差共分散行列と前記ゲイン行列設定手段により設定されたゲイン行列とを入力し、平滑誤差共分散行列を算出する平滑誤差共分散行列算出手段とを備えた目標追尾装置であるので、サンプリング間隔が大きい時にゲートが不必要に拡大するのを防ぐとともに、高速移動目標に対しても速度に応じた最適なゲート設定をすることにより正確にゲート内に目標を捕捉することができる。
【発明を実施するための最良の形態】
【００３８】
実施の形態１．
以下、図面に基づき、本発明の実施の形態１に係る目標追尾装置について以下に説明する。図１は、本実施の形態１に係る目標追尾装置の構成を示した図である。
図１において、１５は平滑値ベクトルより速度ノルムを計算する速度ノルム計算手段、１６は前記速度ノルムと平滑値ベクトルを入力し、駆動雑音ベクトルの座標変換行列を算出する駆動雑音ベクトルの座標変換行列算出手段、１７は駆動雑音共分散行列算出手段に対する目標速度の分散を設定する駆動雑音分散設定手段、１８は前記駆動雑音分散と駆動雑音ベクトルの座標変換行列より駆動雑音共分散行列を算出する駆動雑音共分散行列算出手段である。他の構成については、上述の図１０と同じであるため、ここでは、同一符号を付して示し、それらの説明については省略する。なお、図１０との相違点としては、図１の構成においては、図１０の符号６，７で示す構成が無くなり、代わりに、符号１５〜１８で示す構成が設けられている。
【００３９】
本発明の追尾装置では、極座標での目標速度ベクトルの大きさと向きのふらつきを駆動雑音とする等速直線運動モデルを用いた新たなカルマンフィルタを使用している。極座標とは、図２に示すように、センサより目標までの距離をＲ、水平面より目標までの仰角をＥ、水平面内で北方向より目標までの方位角をＢｙとした座標のことを示す。
【００４０】
ここで、本発明において使用する座標系の説明を図３で示す。本発明での座標系では、目標速度ベクトルの向きを示す角度α，βと、速度ベクトルの大きさＶを使用し、システム雑音を設定する。なお、北基準直交座標表示と極座標表示の間には、次の関係が成り立つ。
【００４１】
【数９】
【００４２】
つぎに、速度ベクトルのふらつきの北基準直交座標表示を式（１８）で、極座標表示を式（１９）で定義する。
【００４３】
【数１０】
【００４４】
すると、ふらつきが微少量であると仮定すれば、線形近似により、式（１７）をＴａｉｌｏｒ展開して、２次以上の項を無視すれば、式（１８）及び式（１９）より、式（２０）及び式（２１）を得る。
【００４５】
【数１１】
【００４６】
ここで、Ｇは駆動雑音ベクトルの座標変換行列である。本発明では、駆動雑音設定時は極座標系を使用するが、カルマンフィルタ内での各計算時は北基準直交座標系のパラメータとして引用するため、このような座標変換行列が必要となる。なお、この駆動雑音ベクトルの座標変換行列こそが、本発明の特徴である。
【００４７】
式（２１）及び式（１７）より、次式（２２）を得る。
【００４８】
【数１２】
【００４９】
目標速度は式（２３）で表され、また、式（１７），（２０）及び水平面内の速度の定義の式（２４）より、次式（２５），（２６），（２７）及び（２８）を得る。
【００５０】
【数１３】
【００５１】
式（２２）に式（２５）〜（２８）を代入し、駆動雑音ベクトルの座標変換行列として、式（２９）を得る。
【００５２】
【数１４】
【００５３】
つぎに、運動モデルの定義を説明する。
式（１）では、駆動雑音ベクトルとして北基準直交座標の加速度ベクトルを使用しているが、本発明においては、駆動雑音ベクトルとして極座標の速度ベクトルのふらつきを使用し、それを北基準直交座標に座標変換を行い、処理を行うものである。そこで、運動モデルは次式（３０）となる。
【００５４】
【数１５】
【００５５】
ここで、Γは駆動雑音ベクトルの変換行列で、北基準直交座標系の駆動雑音ベクトルｗ（ｋ−１｜ｖｃ）が速度のディメンションのため、式（３１）となり、式（３０）に式（２０）を代入し、近似誤差を無視すると、式（３２）となる。
【００５６】
【数１６】
【００５７】
但し、
Ｗｖｐ（ｋ）：平均零ベクトル駆動雑音共分散行列Ｑｖｐ（ｋ）の３変量白色正規分布に従う極座標系駆動雑音ベクトル
とする。
【００５８】
上記のような運動モデルと、式（５）の観測モデルのもと、カルマンフィルタは式（９）を次式（３４）に変更することにより状態ベクトルの予測を行う。
【００５９】
【数１７】
【００６０】
なお、上述した従来方式による式（９）は駆動雑音ベクトルを北基準直交座標の加速度ベクトルとした場合の予測誤差共分散行列であり、また、式（３４）は駆動雑音ベクトルを極座標の速度ベクトルのふらつきとした場合の予測誤差共分散行列である。ここで、従来方式である式（９）はあらかじめ設定されたパラメータのみで算出されるのに対し、本発明の方式である式（３４）は、式（２９）からもわかるように、追尾フィルタに算出すべき目標速度が反映されている。
【００６１】
つぎに、ゲートの算出式は、駆動雑音ベクトルを極座標の速度ベクトルのふらつきとしても、駆動雑音ベクトルを北基準直交座標の加速度ベクトルとした場合と同一である。ただし、予測誤差共分散行列が式（３４）で算出されるため、式（１３）は、式（７），（３１），（３４）及び（１１）より、式（３５）となる。
【００６２】
【数１８】
【００６３】
式（３５）の右辺の第２項は、ゲートの広がりを決める要素の一つであり、駆動雑音共分散行列のサンプリング間での影響であることを示す。なお、この値τ２ＧＱｖｐ（ｋ−１）ＧＴは、式（２９），（１９）、及び、（３３）より、目標速度のあいまいさであるΔＶ及び速度ベクトルの向きのあいまいさΔα、Δβを反映している。また、サンプリング間隔τが大きくなれば、算出した目標速度ベクトルに応じて大きくなっていく。
【００６４】
つぎに、駆動雑音ベクトルを極座標の速度ベクトルのふらつきとする本発明の追尾フィルタの各要素の特徴を述べる。
【００６５】
予測誤差共分散行列の導出について、次式（３６）が定義される。
【００６６】
【数１９】
【００６７】
ここで、従来方式と比較し、本発明の特徴が、Γの項（駆動雑音ベクトルの変換行列）と、Ｑの項（誤差共分散行列）に含まれる。
【００６８】
まず、変換行列Γの項の特徴を表示する。
【００６９】
従来方式では、式（４）が示すように、式（９）における６×３の変換行列Γの内の上部の３×３の行列部は、サンプリング間隔の二乗τ２に比例しており、また変換行列Γの内の下部の３×３の行列部は、サンプリング間隔τに比例している。これに対し、本発明の方式では、式（３１）が示すように、式（３６）における６×３の変換行列Γの内の上部の３×３の行列部は、サンプリング間隔τに比例しており、また、変換行列Γの内の下部の３×３の行列部は、サンプリング間隔に無関係な定数であることがわかる。
【００７０】
すなわち、従来方式ではサンプリング間隔の二乗の値に比例するゲート設定となっており、サンプリング間隔が広がると極端に大きなゲートが設定されていたが、本発明の方式では、サンプリング間隔の一乗の値に比例するゲート設定であり、サンプリング間隔が広がってもゲートの設定は線形に拡大するので、従来方式よりもサンプリング間隔の大きさの変化を受けにくいことを示している（図４参照）。これにより、サンプリング間隔が大きくなっても極端にゲートが拡大することはなく、適度な大きさのゲート設定が保たれるため、不要信号の受信確率が低くなり、追尾を外す危険性が少なくなる。
【００７１】
つぎに、Ｑの項（誤差共分散行列）の特徴を表示する。
【００７２】
従来方式では、式（９）に示すように平均零ベクトルの駆動雑音共分散行列であり、速度に無関係なパラメータである。これに対し、本発明の方式では、式（２９）の座標変換行列が組み込まれるため、次式（３７）で表される。
【００７３】
【数２０】
【００７４】
ここで、式（３７）の各要素は、次式（３８）〜（４３）で表す。
【００７５】
【数２１】
【００７６】
上下方向の速度ｚドットは微少で、速度ベクトルはｘ軸上にあるとすると、次式（４４）〜（４６）も仮定される。
【００７７】
【数２２】
【００７８】
ここで、式（４４）〜（４６）を式（３７）〜（４３）に代入したのち、式（２３）及び（２４）を使用し、次式（４７）が得られる。
【００７９】
【数２３】
【００８０】
但し、
ｑｖ：目標速度のふらつきの分散
Ｖ２ｑα：水平面内の速度ベクトルの向きのふらつきの分散に速度の二乗が乗算された値
Ｖ２ｑβ：鉛直面内の速度ベクトルの向きのふらつきの分散に速度の二乗が乗算された値
とする。
【００８１】
ここで、（２３）成分及び（３４）成分に速度の二乗が組み込まれることになる。すなわち、従来方式では目標速度の大小にかかわらず、一定の大きさのゲートが設定されていたため、速度が大きな目標に対しても、速度の小さい目標と同じ範囲のゲートが設定され、適切な範囲のゲート設定とはならず、追尾を外す可能性が大きかった。本発明の方式では、１サンプリング前の目標速度がパラメータとして毎回引用されるため、目標速度の大小に比例したゲート設定が可能となり、速度が大きな目標に対してはゲート範囲を拡大することが可能となった。すなわち、高速移動目標に対して適切なゲート設定が可能となり、追尾を外す可能性を小さくすることが可能となる（図５参照）。
【００８２】
次に、図６のフローチャートに基づいて、本発明の追尾方法について説明する。
図６に示すように、初期化ステップであるステップＳ１において、目標の位置、速度等の運動諸元で構成される状態ベクトルの平滑値ベクトルとその誤差を推定した平滑誤差共分散行列の初期値を設定する。以降、ステップＳ２〜ステップＳ１２までの各サンプリング時刻における追尾処理ステップに入る。まず、ステップＳ２の速度ノルム算出ステップにおいて、目標速度のノルムを算出する。
次に、ステップＳ３の駆動雑音の座標変換行列算出ステップにおいて、駆動雑音ベクトルの座標変換行列を算出する。次に、ステップＳ４の駆動雑音分散設定ステップにおいて、駆動雑音分散を設定する。次に、ステップＳ５の駆動雑音共分散行列算出ステップにおいて、駆動雑音共分散行列を算出する。次に、ステップＳ６の予測処理ステップにおいて、予測値ベクトルと予測誤差共分散行列を算出する。次に、ステップＳ７の観測諸元入力ステップにおいて、観測値ベクトルと観測雑音共分散行列を入力する。次に、ステップＳ８の残差共分散行列算出ステップにおいて、残差共分散行列を算出する。次に、ステップＳ９のゲート設定ステップにおいて、ゲートの設定を行う。次に、ステップＳ１０のゲイン行列設定ステップにおいて、ゲイン行列を設定する。次に、ステップＳ１１の平滑処理ステップにおいて、平滑値ベクトルと平滑誤差共分散行列を算出する。最後に、ステップＳ１２の追尾終了判定ステップにおいて、追尾終了か否かを判断し、追尾終了の場合は処理を終了し、そうでなければ、ステップＳ２の処理に戻る。
【００８３】
以下、図１および図６を用いて、本発明の追尾装置と追尾方法の動作手順をさらに詳細に説明する。
まず、平滑値ベクトル初期値設定手段１が、平滑値ベクトル記憶手段２に、平滑値ベクトルの初期値ｘハット（０｜０）を設定し、また、平滑誤差共分散行列初期値設定手段８が、平滑誤差共分散行列記憶手段９に、平滑誤差共分散行列の初期値Ｐ（０｜０）を設定する（ステップＳ１の初期化ステップ）。以下、各サンプリング時刻の追尾処理のループに入る。
【００８４】
サンプリング時刻ｔ（ｋ）における一連の処理では、まず、速度ノルム算出手段１５が平滑値ベクトル記憶手段２から入力した前時刻の平滑値ベクトルｘハット（ｋ−１｜ｋ−１）を使用し、速度ベクトルのふらつきを算出し（ステップＳ２の速度ノルム算出ステップ）、その値と、前時刻の平滑値ベクトルｘハット（ｋ−１｜ｋ−１）とを使用し、駆動雑音ベクトルの座標変換行列算出手段１６により、式（２９）の駆動雑音ベクトルの座標変換行列を算出する（ステップＳ３の駆動雑音ベクトルの座標変換行列算出ステップ）。
【００８５】
【数２４】
【００８６】
つぎに、駆動雑音分散設定手段１７が、式（３３）で示される駆動雑音分散を設定する（ステップＳ４の駆動雑音分散設定ステップ）。つぎに、駆動雑音共分散行列算出手段１８が、駆動雑音ベクトルの座標変換行列算出手段１６で算出した駆動雑音ベクトルの座標変換行列Ｇと、駆動雑音分散設定手段１７で設定された式（３３）で示される駆動雑音分散を使用し、式（４７）で示す駆動雑音共分散行列を算出する（ステップＳ５の駆動雑音共分散行列算出ステップ）。
【００８７】
【数２５】
【００８８】
つぎに、予測手段３が平滑値ベクトル記憶手段２から入力した前時刻の平滑値ベクトルｘハット（ｋ−１｜ｋ−１）を使用し、式（８）に従って、現時刻の予測値ベクトルｘハット（ｋ｜ｋ−１）を算出する。
【００８９】
【数２６】
【００９０】
また、予測誤差共分散行列算出手段１０が、平滑誤差共分散行列記憶手段９から入力した前時刻の平滑誤差共分散行列Ｐ（ｋ−１｜ｋ−１）と、前時刻に算出した式（４７）の駆動雑音共分散行列を使用し、式（３４）に従って、現時刻の予測誤差共分散行列Ｐ（ｋ｜ｋ−１）を算出する（ステップＳ６の予測処理ステップ）。
【００９１】
【数２７】
【００９２】
つぎに、観測装置１０２より観測雑音共分散行列算出手段１２を介して観測値ベクトルｚ（ｋ）と観測雑音共分散行列Ｂ（ｋ）が入力されると（ステップＳ７の観測諸元入力ステップ）、残差共分散行列算出手段１３が、予測誤差共分散行列算出手段１０で前時刻に算出した予測誤差共分散行列Ｐ（ｋ｜ｋ−１）と、入力された観測雑音共分散行列Ｂ（ｋ）より、残差共分散行列を算出し（ステップＳ８の残差共分散行列算出ステップ）、その残差共分散行列と予測誤差共分散行列により、ゲイン行列設定手段１４によってゲイン行列が設定される（ステップＳ１０のゲイン行列設定ステップ）。
【００９３】
【数２８】
【００９４】
つぎに、ゲート範囲設定手段４が、予測手段３が算出した予測値ベクトルｘハット（ｋ｜ｋ−１）と、観測装置１０２より入力された観測値ベクトルｚ（ｋ）を使用し、式（１３）に示すゲート範囲を設定した（ステップＳ９のゲート設定ステップ）後、平滑手段５が、式（１５）に従って、前期予測値ベクトルｘハット（ｋ｜ｋ−１）、ゲート範囲Ｓ（ｋ）、ゲイン行列Ｋ（ｋ）を使用して、現時刻の平滑値ベクトルｘハット（ｋ｜ｋ）を算出する。この平滑値ベクトルは平滑値ベクトル記憶手段２に格納する（ステップＳ１１の平滑処理ステップの一部）。
【００９５】
【数２９】
【００９６】
また、平滑誤差共分散行列算出手段１１は、式（１６）に従って、前記予測誤差共分散行列Ｐ（ｋ｜ｋ−１）、ゲイン行列Ｋ（ｋ）及び観測行列Ｈを使用して、現時刻の平滑誤差共分散行列Ｐ（ｋ｜ｋ）を算出する。この平滑誤差共分散行列は平滑誤差共分散行列記憶手段９に格納する（ステップＳ１１の平滑処理ステップの残りの部分）。
【００９７】
【数３０】
【００９８】
以上で、時刻ｔ（ｋ）における一連の処理が終了する。追尾終了判定ステップにおいて追尾終了との判定がなされる（ステップＳ１２の追尾終了判定ステップ）まで、これら一連の処理を繰り返して実行する。
【００９９】
本発明の追尾装置及び追尾方法を使用した極座標表示の速度のふらつきを駆動雑音とするカルマンフィルタによれば、従来のカルマンフィルタのように、サンプリング間隔の大きさに比例してゲートが爆発的に拡大することがないため、それぞれのサンプリング間隔に応じた適度な大きさのゲートが設定され、不要信号の受信率を少なくし、追尾を外す危険性を低くすることが可能となる利点を有する。さらに、速度ベクトルの北基準直交座標表示と極座標表示の変換行列内に、１サンプリング前の速度要因が引用されることとなり、従来方式ではゲートが目標速度の大小に関係なく一定の大きさで設定されていたものが、本発明方式では、目標速度の大小に応じてゲートの範囲が変動し、設定されることとなった。これにより、目標速度が大きければ、その速度に対応した大きさのゲートが設定されることになり、目標速度が大きい時に不釣合いに小さいゲートを設定し、目標の追尾を外してしまうような危険性を低くすることが可能となる利点を有する。
【０１００】
実施の形態２．
以下、本発明の実施の形態２に係る目標追尾装置および目標追尾方法について説明する。本実施の形態２においては、構成および動作は、基本的に、図１及び図６に示した実施の形態１と同じであるため、それらを参照することとし、以下の説明においては、実施の形態１と異なる部分のみを説明することとする。
【０１０１】
図７は、本実施の形態２にかかる目標追尾装置の駆動雑音分散設定手段１７の内部構成を示したブロック図である。図７に示すように、本実施の形態２にかかる駆動雑音分散設定手段１７には、目標別の最大加速度を算出する最大加速度算出手段２０と、目標別の最小速度を算出する最小速度算出手段２１と、算出された最大加速度及び最小速度より示される最大角加速度を算出する最大角加速度算出手段２２と、算出された最大角加速度により示される最大旋回半径を算出する最大旋回半径算出手段２３と、算出された最大旋回半径より駆動雑音共分散行列内のパラメータを算出する駆動雑音共分散行列内パラメータ算出手段２４とが設けられている。
【０１０２】
本実施の形態においては、速度ベクトルを駆動雑音とすることにより、目標の最大旋回範囲が機体別（目標別）によって導かれる。まず、目標の機種別により、式（４８）に示す速度の最大値と最小値及び加速度の最大値と最小値が与えられるとする。
【０１０３】
【数３１】
【０１０４】
ここで、式（４９）の定義より、角加速度の最大値と最小値が式（５０）で求められる。
【０１０５】
【数３２】
【０１０６】
式（５０）で求められた最大角加速度を使用し、式（５１）で示す最大旋回半径が求められる。
【０１０７】
【数３３】
【０１０８】
但し、
Δβ：旋回半径
Ｔ：サンプリングレート
とする。
【０１０９】
つまり、本実施の形態においては、駆動雑音分散設定手段１７において、図７に示すように、目標別の最大加速度および最小速度を、それぞれ、最大加速度算出手段２０と最小速度算出手段２１とにより求め、それらの値を用いて、最大角加速度を最大角加速度算出手段２２より算出する。算出された最大角加速度により最大旋回半径を最大旋回半径算出手段２３により算出し、その値に基づいて駆動雑音共分散行列内のパラメータを駆動雑音共分散行列内パラメータ算出手段２４より算出する。
【０１１０】
以上のように、本実施の形態２においては、上述の実施の形態１と同様の効果が得られるとともに、さらに、速度ベクトルを駆動雑音とすることにより、目標の最大旋回範囲が目標別によって導かれ、その値に基づいて駆動雑音共分散行列内のパラメータを算出する。それにより、たとえば等速直線運動を行っていた目標が等速円運動で９０度旋回したのち、再び等速直線運動を行う場合でも、ｘ，ｙ，ｚ方向おのおのにおいて、サンプリング間隔が大きくなるほど、ゲートが不必要に拡大してしまうことを防止し、適度な大きさのゲートの設定を可能として、効率よく目標の追尾を行うことができるという効果が得られる。
【図面の簡単な説明】
【０１１１】
【図１】本発明の実施の形態１に係る目標追尾装置の構成を示したブロック図である。
【図２】一般的な極座標系を示した説明図である。
【図３】本発明の実施の形態１に係る目標追尾装置で用いる座標系を示した説明図である。
【図４】本発明と従来技術とのゲート設定を比較した説明図である。
【図５】本発明と従来技術とのゲート設定を比較した説明図である。
【図６】本発明の実施の形態１に係る目標追尾方法の処理の流れを示したフローチャートである。
【図７】本発明の実施の形態２に係る目標追尾装置における駆動雑音分散設定手段の構成を示したブロック図である。
【図８】従来のカルマンフィルタを使用した目標追尾処理で用いる座標系を示した説明図である。
【図９】目標追尾処理における一般的なゲートを示した模式図である。
【図１０】従来の目標追尾装置の構成を示したブロック図である。
【符号の説明】
【０１１２】
１ 平滑値ベクトル初期値設定手段、２ 平滑値ベクトル記憶手段、３ 予測手段、４ ゲート範囲設定手段、５ 平滑手段、６ 駆動雑音分散設定手段、７ 駆動雑音共分散行列算出手段、８ 平滑誤差共分散行列初期値設定手段、９ 平滑誤差共分散行列記憶手段、１０ 予測誤差共分散行列算出手段、１１ 平滑誤差共分散行列算出手段、１２ 観測雑音共分散行列算出手段、１３ 残差共分散行列算出手段、１４ ゲイン行列設定手段、１５ 速度ノルム計算手段、１６ 駆動雑音ベクトルの座標変換行列算出手段、１７ 駆動雑音分散設定手段、１８ 駆動雑音共分散行列算出手段、２０ 最大加速度算出手段、２１ 最小速度算出手段、２２ 最大角加速度算出手段、２３ 最大旋回半径算出手段、２４ 駆動雑音共分散行列内パラメータ算出手段。
【技術分野】
【０００１】
本発明は目標追尾装置及び目標追尾方法に関し、特に、レーダなどのセンサからなる観測装置を用いて、航空機、船舶、車両などの移動体の位置を観測し、その観測結果に基づいて移動体の真の位置、速度などの運動諸元を高精度に推定する追尾装置および追尾方法に関するものである。
【背景技術】
【０００２】
一般に、観測装置による目標観測結果を用いた目標追尾処理では、カルマンフィルタを使用する（例えば、特許文献１参照）。
【０００３】
従来技術においては、３次元空間用の等速直線運動モデルや等加速度運動モデルを使用し、目標運動のあいまいさを表す駆動雑音ベクトルをｘ，ｙ，ｚの各方向の加速度項で定義している。以下、等速直線運動モデルを使用した場合を例に、従来の目標追尾方法を説明する。
【０００４】
カルマンフィルタを使用した追尾処理では、設定する座標系を、図８のようにセンサを原点、東方向をｘ軸の正、北方向をｙ軸の正、水平面（ｘ−ｙ面）上方向をｚ軸の正に取った北基準直交座標とする。つぎに、目標の運動モデルを以下のように仮定する。ただし、サンプリング時刻（以下、単に時刻）をｔ（ｋ）で表す。
【０００５】
【数１】
【０００６】
但し、
ｘ（ｋ）：時刻ｔ（ｋ）における目標運動諸元の真値を表す状態変数ベクトル
Φ：時刻ｔ（ｋ−１）から時刻ｔ（ｋ）への状態ベクトルの既知の推移行列
ｗ（ｋ）：北基準直交座標の時刻ｔ（ｋ）における駆動雑音ベクトル
Γ：駆動雑音ベクトルの変換行列
とする。
【０００７】
従来の一般的な追尾装置では、目標の基本的な運動をｘ−ｙ−ｚの３次元空間における北基準直交座標の等速直線運動と仮定して上記運動モデルを記述する。たとえば、目標の位置と速度を推定する場合、状態ベクトルｘ（ｋ）、状態推移行列Φを次式のように定義する。
【０００８】
【数２】
【０００９】
但し、Ｔはベクトル及び行列の転置、ドットは時間微分を表す。また、Iｎ×ｎ及び０ｎ×ｎはそれぞれｎ行ｎ列の単位行列及び零行列であり、τは一定のサンプリング周期を表す。
【００１０】
さらに、ｗ（ｋ）は現実の目標運動を北基準直交座標の等速直線で近似したことによるｘ（ｋ）の誤差を表現するために設けられたｘ，ｙ，ｚの各方向の加速度項の雑音成分で構成されており、平均零ベクトルで駆動雑音共分散行列Ｑ（ｋ）の３変量白色正規分布に従う駆動雑音ベクトルであると仮定する。
【００１１】
つぎに、状態ベクトル及び観測雑音ベクトルを使用して目標位置の観測ベクトルを表現する際の、観測装置の観測モデルを以下のように仮定する。
【００１２】
【数３】
【００１３】
但し、
ｚ（ｋ）：時刻ｔ（ｋ）における目標位置の観測ベクトル
Ｈ：観測行列
ｖ（ｋ）：時刻ｔ（ｋ）における北基準直交座標による観測雑音ベクトル
とする。
【００１４】
たとえば、観測装置によりｘ−ｙ−ｚの３次元空間の北基準直交座標系における目標位置の観測結果が得られるとする。このとき、観測ベクトルｚ（ｋ）、観測行列Ｈを次式のように定義する。
【００１５】
【数４】
【００１６】
また、ｖ（ｋ）は観測装置の観測誤差を表すための雑音成分であり、平均零ベクトルで観測雑音共分散行列Ｂ（ｋ）の３変量白色正規分布に従う北基準直交座標による観測雑音ベクトルであると仮定する。
【００１７】
上記のような運動モデル、観測モデルの仮定のもと、カルマンフィルタは以下に示すアルゴリズムで、状態ベクトルの予測を行う。時刻ｔ（ｋ−１）までの観測結果に基づいて時刻ｔ（ｋ）の状態ベクトルを推定した予測値ベクトルをｘハット（ｋ｜ｋ−１）と表し、また、ｘハット（ｋ｜ｋ−１）の誤差共分散行列を表す予測誤差共分散行列をＰ（ｋ｜ｋ−１）と表す。
【００１８】
【数５】
【００１９】
なお、駆動雑音共分散行列Ｑ（ｋ）はパラメータとして与える。
【００２０】
つぎに、ゲートの設定を以下のように仮定する。ゲートとは、図９に示すように、追尾目標が存在すると予測される範囲であり、ゲート内の観測ベクトルを追尾に使用するものである。ゲートの中心は、１サンプリング前までの観測ベクトルの情報に基づく、時刻ｔｋの観測ベクトルｚ（ｋ）の推定値である。これを考慮し、上記式（５）に、予測ベクトルの定義及び観測雑音ベクトルは平均零ベクトルの白色雑音であるとの仮定を使用することで、以下の式が得られる。
【００２１】
【数６】
【００２２】
但し、
Ｓ（ｋ）：残差ベクトルｚ（ｋ）−ｚ（ｋ−１）の共分散行列
ｄ：ゲートの大きさを決めるパラメータ
とする。
【００２３】
式（１０）がゲートの中心、式（１１）がゲートの広がり、式（１２）がゲートの判定を示す。ここで、式（７），（４），（９）及び（１１）より、次式（１３）を得る。
【００２４】
【数７】
【００２５】
式（１３）の右辺の第２項では、ゲートの広がりを判定する要素の一つが駆動雑音共分散行列のサンプリング間隔の影響であるということを示している。
【００２６】
ゲート内に追尾目標の観測ベクトルが得られると、つぎに、状態ベクトルの更新を行う。時刻ｔ（ｋ）までの観測結果に基づいて時刻ｔ（ｋ）の状態ベクトルを推定した平滑ベクトルをｘハット（ｋ｜ｋ）と表し、ｘハット（ｋ｜ｋ）の誤差共分散行列を表す平滑誤差共分散行列をＰ（ｋ｜ｋ）と表す。さらに、Ｋ（ｋ）はフィルタのゲイン行列であり、上記式（２）〜（４），（６），（７）の設定を行った場合、６行３列の行列である。
【００２７】
【数８】
【００２８】
なお、ｘハット（ｋ｜ｋ）の初期値ｘハット（０｜０）と、Ｐ（ｋ｜ｋ）の初期値Ｐ（０｜０）が別途算出され、与えられるものとする。また、駆動雑音共分散行列Ｑ（ｋ）、観測雑音共分散行列Ｂ（ｋ）はパラメータとして与える。
【００２９】
カルマンフィルタは、式（８），（１５）により、平均的な偏りのない不偏推定値を算出するとともに、式（１４）でゲイン行列を算出することにより、平均２乗誤差を最小化する最適なフィルタとして知られている。
【００３０】
尚、従来技術による一般的な追尾装置の構成を図１０に示す。
図１０において、１は平滑値ベクトル記憶手段に、目標の位置、速度等の運動諸元で構成される状態ベクトルの平滑値ベクトルの初期値を設定する平滑値ベクトル初期値設定手段、２は平滑値ベクトルの値を記憶する平滑値ベクトル記憶手段、３は前時刻の平滑値ベクトルより現時刻の予測値ベクトルを算出する予測手段、４は前記予測値ベクトルと観測装置からの観測値ベクトルを入力し、ゲートを設定するゲート範囲設定手段、５は前記ゲートと予測値ベクトルとゲイン行列設定手段よりゲイン行列を入力して、平滑値ベクトルを算出する平滑手段である。
６は目標速度の分散値を設定する駆動雑音分散設定手段、７は前記駆動雑音分散より駆動雑音共分散行列を算出する駆動雑音共分散行列算出手段、８は平滑誤差共分散行列記憶手段に、平滑値ベクトルの誤差を推定した平滑誤差共分散行列の初期値を設定する平滑誤差共分散行列初期値設定手段、９は平滑誤差共分散行列の値を記憶する平滑誤差共分散行列記憶手段、１０は前時刻の平滑誤差共分散行列と駆動雑音共分散行列より現時刻の予測誤差共分散行列を算出する予測誤差共分散行列算出手段、１１は前記予測誤差共分散行列とゲイン行列設定手段よりのゲイン行列を入力し、平滑誤差共分散行列を算出する平滑誤差共分散行列算出手段である。
また、１２は観測雑音共分散行列が与えられる観測雑音共分散行列算出手段、１３は予測誤差共分散行列の値と観測雑音共分散行列より残差共分散行列を算出する残差共分散行列算出手段、１４は前記残差共分散行列の値と予測誤差共分散行列の値よりゲイン行列が設定されるゲイン行列設定手段である。
【００３１】
【特許文献１】特開平１１−４４７５４号公報
【発明の開示】
【発明が解決しようとする課題】
【００３２】
上述したように、カルマンフィルタは、適度なサンプリング間隔で目標の運動モデルと観測装置の観測モデルが正しく表現されていれば、最適なゲートの広がりの設定が行われ、追尾性能を発揮することができる。しかしながら、式（１３）の右辺の第２項では、ゲートの広がりを決める要素の一つが駆動雑音共分散行列のサンプリング間隔の影響であるということを示したように、ここでの値（τ４／４）×Ｑ（ｋ−１）は、目標速度、速度ベクトルの向きに関係無く、サンプリング間隔τが大きくなれば、ゲートが極端に拡大していくことを表している。
【００３３】
このため、たとえば等速直線運動を行っていた目標が等速円運動で９０度旋回したのち、再び等速直線運動を行う場合でも、ｘ，ｙ，ｚ方向おのおのにおいて、サンプリング間隔が大きくなるほど、ゲートは拡大していく。また、単純に目標が等速直線運動を行っている時でも、サンプリング間隔が大きくなると、非現実的な大きさにゲートが拡大してしまう。ゲートが不必要に拡大してしまうと、不要信号を受信する確率が高まり、目標の追尾を外す可能性が高くなるという問題点がある。
【００３４】
さらに、目標の加速度ベクトルを駆動雑音として設定するため、速度成分を引用しない一定の大きさのゲート設定となり、目標速度が大きい時にその速度に適したゲートの範囲が割り当てられず、高速移動目標に対してゲート内に目標を捕捉できず、追尾を外してしまう危険性を含むという問題点があった。
【００３５】
本発明は、かかる問題点を解決するためになされたものであり、サンプリング間隔が大きい時にゲートが不必要に拡大するのを防ぎ、また、目標速度の大小に合わせたゲート設定により、高速移動目標に対しても適切なゲートが設定される目標追尾装置および目標追尾方法を得ることを目的としている。
【課題を解決するための手段】
【００３６】
この発明は、目標の位置、速度等の運動諸元で構成される状態ベクトルの平滑値ベクトルの初期値を設定する平滑値ベクトル初期値設定手段と、前記平滑値ベクトルの初期値および算出される平滑値ベクトルの値を記憶する平滑値ベクトル記憶手段と、前記平滑値ベクトル記憶手段に記憶されている前時刻の平滑値ベクトルより現時刻の予測値ベクトルを算出する予測手段と、前記平滑値ベクトル記憶手段に記憶されている前記平滑値ベクトルに基づいて速度ノルムを算出する速度ノルム計算手段と、算出された前記速度ノルムと前記平滑値ベクトル記憶手段に記憶されている前記平滑値ベクトルとを入力し、駆動雑音ベクトルの座標変換行列を算出する駆動雑音ベクトルの座標変換行列算出手段と、駆動雑音分散として目標速度の分散を設定する駆動雑音分散設定手段と、設定された前記駆動雑音分散と前記駆動雑音ベクトルの座標変換行列算出手段により算出された駆動雑音ベクトルの座標変換行列とにより、駆動雑音共分散行列を算出する駆動雑音共分散行列算出手段と、平滑ベクトルの誤差を推定する平滑誤差共分散行列の初期値を設定する平滑誤差共分散行列初期値設定手段と、前記平滑誤差共分散行列の初期値および算出される平滑誤差共分散行列の値を記憶する平滑誤差共分散行列記憶手段と、前記平滑誤差共分散行列記憶手段に記憶されている前時刻の平滑誤差共分散行列と前記駆動雑音共分散行列算出手段により算出された前記駆動雑音共分散行列とにより、現時刻の予測誤差共分散行列を算出する予測誤差共分散行列算出手段と、算出された前記予測誤差共分散行列の値と観測装置から得られる観測雑音共分散行列とにより、残差共分散行列を算出する残差共分散行列算出手段と、算出された前記残差共分散行列の値と前記予測誤差共分散行列算出手段により算出された前記予測誤差共分散行列の値とによりゲイン行列を設定するゲイン行列設定手段と、前記予測手段により算出された前記予測値ベクトルと前記観測装置から得られる観測値ベクトルとを入力し、ゲートを設定するゲート範囲設定手段と、設定された前記ゲートと前記予測手段により算出された前記予測値ベクトルと前記ゲイン行列設定手段により設定された前記ゲイン行列とを入力して、平滑値ベクトルを算出する平滑手段と、前記予測誤差共分散行列算出手段により算出された前記予測誤差共分散行列と前記ゲイン行列設定手段により設定されたゲイン行列とを入力し、平滑誤差共分散行列を算出する平滑誤差共分散行列算出手段とを備えた目標追尾装置である。
【発明の効果】
【００３７】
この発明は、目標の位置、速度等の運動諸元で構成される状態ベクトルの平滑値ベクトルの初期値を設定する平滑値ベクトル初期値設定手段と、前記平滑値ベクトルの初期値および算出される平滑値ベクトルの値を記憶する平滑値ベクトル記憶手段と、前記平滑値ベクトル記憶手段に記憶されている前時刻の平滑値ベクトルより現時刻の予測値ベクトルを算出する予測手段と、前記平滑値ベクトル記憶手段に記憶されている前記平滑値ベクトルに基づいて速度ノルムを算出する速度ノルム計算手段と、算出された前記速度ノルムと前記平滑値ベクトル記憶手段に記憶されている前記平滑値ベクトルとを入力し、駆動雑音ベクトルの座標変換行列を算出する駆動雑音ベクトルの座標変換行列算出手段と、駆動雑音分散として目標速度の分散を設定する駆動雑音分散設定手段と、設定された前記駆動雑音分散と前記駆動雑音ベクトルの座標変換行列算出手段により算出された駆動雑音ベクトルの座標変換行列とにより、駆動雑音共分散行列を算出する駆動雑音共分散行列算出手段と、平滑ベクトルの誤差を推定する平滑誤差共分散行列の初期値を設定する平滑誤差共分散行列初期値設定手段と、前記平滑誤差共分散行列の初期値および算出される平滑誤差共分散行列の値を記憶する平滑誤差共分散行列記憶手段と、前記平滑誤差共分散行列記憶手段に記憶されている前時刻の平滑誤差共分散行列と前記駆動雑音共分散行列算出手段により算出された前記駆動雑音共分散行列とにより、現時刻の予測誤差共分散行列を算出する予測誤差共分散行列算出手段と、算出された前記予測誤差共分散行列の値と観測装置から得られる観測雑音共分散行列とにより、残差共分散行列を算出する残差共分散行列算出手段と、算出された前記残差共分散行列の値と前記予測誤差共分散行列算出手段により算出された前記予測誤差共分散行列の値とによりゲイン行列を設定するゲイン行列設定手段と、前記予測手段により算出された前記予測値ベクトルと前記観測装置から得られる観測値ベクトルとを入力し、ゲートを設定するゲート範囲設定手段と、設定された前記ゲートと前記予測手段により算出された前記予測値ベクトルと前記ゲイン行列設定手段により設定された前記ゲイン行列とを入力して、平滑値ベクトルを算出する平滑手段と、前記予測誤差共分散行列算出手段により算出された前記予測誤差共分散行列と前記ゲイン行列設定手段により設定されたゲイン行列とを入力し、平滑誤差共分散行列を算出する平滑誤差共分散行列算出手段とを備えた目標追尾装置であるので、サンプリング間隔が大きい時にゲートが不必要に拡大するのを防ぐとともに、高速移動目標に対しても速度に応じた最適なゲート設定をすることにより正確にゲート内に目標を捕捉することができる。
【発明を実施するための最良の形態】
【００３８】
実施の形態１．
以下、図面に基づき、本発明の実施の形態１に係る目標追尾装置について以下に説明する。図１は、本実施の形態１に係る目標追尾装置の構成を示した図である。
図１において、１５は平滑値ベクトルより速度ノルムを計算する速度ノルム計算手段、１６は前記速度ノルムと平滑値ベクトルを入力し、駆動雑音ベクトルの座標変換行列を算出する駆動雑音ベクトルの座標変換行列算出手段、１７は駆動雑音共分散行列算出手段に対する目標速度の分散を設定する駆動雑音分散設定手段、１８は前記駆動雑音分散と駆動雑音ベクトルの座標変換行列より駆動雑音共分散行列を算出する駆動雑音共分散行列算出手段である。他の構成については、上述の図１０と同じであるため、ここでは、同一符号を付して示し、それらの説明については省略する。なお、図１０との相違点としては、図１の構成においては、図１０の符号６，７で示す構成が無くなり、代わりに、符号１５〜１８で示す構成が設けられている。
【００３９】
本発明の追尾装置では、極座標での目標速度ベクトルの大きさと向きのふらつきを駆動雑音とする等速直線運動モデルを用いた新たなカルマンフィルタを使用している。極座標とは、図２に示すように、センサより目標までの距離をＲ、水平面より目標までの仰角をＥ、水平面内で北方向より目標までの方位角をＢｙとした座標のことを示す。
【００４０】
ここで、本発明において使用する座標系の説明を図３で示す。本発明での座標系では、目標速度ベクトルの向きを示す角度α，βと、速度ベクトルの大きさＶを使用し、システム雑音を設定する。なお、北基準直交座標表示と極座標表示の間には、次の関係が成り立つ。
【００４１】
【数９】
【００４２】
つぎに、速度ベクトルのふらつきの北基準直交座標表示を式（１８）で、極座標表示を式（１９）で定義する。
【００４３】
【数１０】
【００４４】
すると、ふらつきが微少量であると仮定すれば、線形近似により、式（１７）をＴａｉｌｏｒ展開して、２次以上の項を無視すれば、式（１８）及び式（１９）より、式（２０）及び式（２１）を得る。
【００４５】
【数１１】
【００４６】
ここで、Ｇは駆動雑音ベクトルの座標変換行列である。本発明では、駆動雑音設定時は極座標系を使用するが、カルマンフィルタ内での各計算時は北基準直交座標系のパラメータとして引用するため、このような座標変換行列が必要となる。なお、この駆動雑音ベクトルの座標変換行列こそが、本発明の特徴である。
【００４７】
式（２１）及び式（１７）より、次式（２２）を得る。
【００４８】
【数１２】
【００４９】
目標速度は式（２３）で表され、また、式（１７），（２０）及び水平面内の速度の定義の式（２４）より、次式（２５），（２６），（２７）及び（２８）を得る。
【００５０】
【数１３】
【００５１】
式（２２）に式（２５）〜（２８）を代入し、駆動雑音ベクトルの座標変換行列として、式（２９）を得る。
【００５２】
【数１４】
【００５３】
つぎに、運動モデルの定義を説明する。
式（１）では、駆動雑音ベクトルとして北基準直交座標の加速度ベクトルを使用しているが、本発明においては、駆動雑音ベクトルとして極座標の速度ベクトルのふらつきを使用し、それを北基準直交座標に座標変換を行い、処理を行うものである。そこで、運動モデルは次式（３０）となる。
【００５４】
【数１５】
【００５５】
ここで、Γは駆動雑音ベクトルの変換行列で、北基準直交座標系の駆動雑音ベクトルｗ（ｋ−１｜ｖｃ）が速度のディメンションのため、式（３１）となり、式（３０）に式（２０）を代入し、近似誤差を無視すると、式（３２）となる。
【００５６】
【数１６】
【００５７】
但し、
Ｗｖｐ（ｋ）：平均零ベクトル駆動雑音共分散行列Ｑｖｐ（ｋ）の３変量白色正規分布に従う極座標系駆動雑音ベクトル
とする。
【００５８】
上記のような運動モデルと、式（５）の観測モデルのもと、カルマンフィルタは式（９）を次式（３４）に変更することにより状態ベクトルの予測を行う。
【００５９】
【数１７】
【００６０】
なお、上述した従来方式による式（９）は駆動雑音ベクトルを北基準直交座標の加速度ベクトルとした場合の予測誤差共分散行列であり、また、式（３４）は駆動雑音ベクトルを極座標の速度ベクトルのふらつきとした場合の予測誤差共分散行列である。ここで、従来方式である式（９）はあらかじめ設定されたパラメータのみで算出されるのに対し、本発明の方式である式（３４）は、式（２９）からもわかるように、追尾フィルタに算出すべき目標速度が反映されている。
【００６１】
つぎに、ゲートの算出式は、駆動雑音ベクトルを極座標の速度ベクトルのふらつきとしても、駆動雑音ベクトルを北基準直交座標の加速度ベクトルとした場合と同一である。ただし、予測誤差共分散行列が式（３４）で算出されるため、式（１３）は、式（７），（３１），（３４）及び（１１）より、式（３５）となる。
【００６２】
【数１８】
【００６３】
式（３５）の右辺の第２項は、ゲートの広がりを決める要素の一つであり、駆動雑音共分散行列のサンプリング間での影響であることを示す。なお、この値τ２ＧＱｖｐ（ｋ−１）ＧＴは、式（２９），（１９）、及び、（３３）より、目標速度のあいまいさであるΔＶ及び速度ベクトルの向きのあいまいさΔα、Δβを反映している。また、サンプリング間隔τが大きくなれば、算出した目標速度ベクトルに応じて大きくなっていく。
【００６４】
つぎに、駆動雑音ベクトルを極座標の速度ベクトルのふらつきとする本発明の追尾フィルタの各要素の特徴を述べる。
【００６５】
予測誤差共分散行列の導出について、次式（３６）が定義される。
【００６６】
【数１９】
【００６７】
ここで、従来方式と比較し、本発明の特徴が、Γの項（駆動雑音ベクトルの変換行列）と、Ｑの項（誤差共分散行列）に含まれる。
【００６８】
まず、変換行列Γの項の特徴を表示する。
【００６９】
従来方式では、式（４）が示すように、式（９）における６×３の変換行列Γの内の上部の３×３の行列部は、サンプリング間隔の二乗τ２に比例しており、また変換行列Γの内の下部の３×３の行列部は、サンプリング間隔τに比例している。これに対し、本発明の方式では、式（３１）が示すように、式（３６）における６×３の変換行列Γの内の上部の３×３の行列部は、サンプリング間隔τに比例しており、また、変換行列Γの内の下部の３×３の行列部は、サンプリング間隔に無関係な定数であることがわかる。
【００７０】
すなわち、従来方式ではサンプリング間隔の二乗の値に比例するゲート設定となっており、サンプリング間隔が広がると極端に大きなゲートが設定されていたが、本発明の方式では、サンプリング間隔の一乗の値に比例するゲート設定であり、サンプリング間隔が広がってもゲートの設定は線形に拡大するので、従来方式よりもサンプリング間隔の大きさの変化を受けにくいことを示している（図４参照）。これにより、サンプリング間隔が大きくなっても極端にゲートが拡大することはなく、適度な大きさのゲート設定が保たれるため、不要信号の受信確率が低くなり、追尾を外す危険性が少なくなる。
【００７１】
つぎに、Ｑの項（誤差共分散行列）の特徴を表示する。
【００７２】
従来方式では、式（９）に示すように平均零ベクトルの駆動雑音共分散行列であり、速度に無関係なパラメータである。これに対し、本発明の方式では、式（２９）の座標変換行列が組み込まれるため、次式（３７）で表される。
【００７３】
【数２０】
【００７４】
ここで、式（３７）の各要素は、次式（３８）〜（４３）で表す。
【００７５】
【数２１】
【００７６】
上下方向の速度ｚドットは微少で、速度ベクトルはｘ軸上にあるとすると、次式（４４）〜（４６）も仮定される。
【００７７】
【数２２】
【００７８】
ここで、式（４４）〜（４６）を式（３７）〜（４３）に代入したのち、式（２３）及び（２４）を使用し、次式（４７）が得られる。
【００７９】
【数２３】
【００８０】
但し、
ｑｖ：目標速度のふらつきの分散
Ｖ２ｑα：水平面内の速度ベクトルの向きのふらつきの分散に速度の二乗が乗算された値
Ｖ２ｑβ：鉛直面内の速度ベクトルの向きのふらつきの分散に速度の二乗が乗算された値
とする。
【００８１】
ここで、（２３）成分及び（３４）成分に速度の二乗が組み込まれることになる。すなわち、従来方式では目標速度の大小にかかわらず、一定の大きさのゲートが設定されていたため、速度が大きな目標に対しても、速度の小さい目標と同じ範囲のゲートが設定され、適切な範囲のゲート設定とはならず、追尾を外す可能性が大きかった。本発明の方式では、１サンプリング前の目標速度がパラメータとして毎回引用されるため、目標速度の大小に比例したゲート設定が可能となり、速度が大きな目標に対してはゲート範囲を拡大することが可能となった。すなわち、高速移動目標に対して適切なゲート設定が可能となり、追尾を外す可能性を小さくすることが可能となる（図５参照）。
【００８２】
次に、図６のフローチャートに基づいて、本発明の追尾方法について説明する。
図６に示すように、初期化ステップであるステップＳ１において、目標の位置、速度等の運動諸元で構成される状態ベクトルの平滑値ベクトルとその誤差を推定した平滑誤差共分散行列の初期値を設定する。以降、ステップＳ２〜ステップＳ１２までの各サンプリング時刻における追尾処理ステップに入る。まず、ステップＳ２の速度ノルム算出ステップにおいて、目標速度のノルムを算出する。
次に、ステップＳ３の駆動雑音の座標変換行列算出ステップにおいて、駆動雑音ベクトルの座標変換行列を算出する。次に、ステップＳ４の駆動雑音分散設定ステップにおいて、駆動雑音分散を設定する。次に、ステップＳ５の駆動雑音共分散行列算出ステップにおいて、駆動雑音共分散行列を算出する。次に、ステップＳ６の予測処理ステップにおいて、予測値ベクトルと予測誤差共分散行列を算出する。次に、ステップＳ７の観測諸元入力ステップにおいて、観測値ベクトルと観測雑音共分散行列を入力する。次に、ステップＳ８の残差共分散行列算出ステップにおいて、残差共分散行列を算出する。次に、ステップＳ９のゲート設定ステップにおいて、ゲートの設定を行う。次に、ステップＳ１０のゲイン行列設定ステップにおいて、ゲイン行列を設定する。次に、ステップＳ１１の平滑処理ステップにおいて、平滑値ベクトルと平滑誤差共分散行列を算出する。最後に、ステップＳ１２の追尾終了判定ステップにおいて、追尾終了か否かを判断し、追尾終了の場合は処理を終了し、そうでなければ、ステップＳ２の処理に戻る。
【００８３】
以下、図１および図６を用いて、本発明の追尾装置と追尾方法の動作手順をさらに詳細に説明する。
まず、平滑値ベクトル初期値設定手段１が、平滑値ベクトル記憶手段２に、平滑値ベクトルの初期値ｘハット（０｜０）を設定し、また、平滑誤差共分散行列初期値設定手段８が、平滑誤差共分散行列記憶手段９に、平滑誤差共分散行列の初期値Ｐ（０｜０）を設定する（ステップＳ１の初期化ステップ）。以下、各サンプリング時刻の追尾処理のループに入る。
【００８４】
サンプリング時刻ｔ（ｋ）における一連の処理では、まず、速度ノルム算出手段１５が平滑値ベクトル記憶手段２から入力した前時刻の平滑値ベクトルｘハット（ｋ−１｜ｋ−１）を使用し、速度ベクトルのふらつきを算出し（ステップＳ２の速度ノルム算出ステップ）、その値と、前時刻の平滑値ベクトルｘハット（ｋ−１｜ｋ−１）とを使用し、駆動雑音ベクトルの座標変換行列算出手段１６により、式（２９）の駆動雑音ベクトルの座標変換行列を算出する（ステップＳ３の駆動雑音ベクトルの座標変換行列算出ステップ）。
【００８５】
【数２４】
【００８６】
つぎに、駆動雑音分散設定手段１７が、式（３３）で示される駆動雑音分散を設定する（ステップＳ４の駆動雑音分散設定ステップ）。つぎに、駆動雑音共分散行列算出手段１８が、駆動雑音ベクトルの座標変換行列算出手段１６で算出した駆動雑音ベクトルの座標変換行列Ｇと、駆動雑音分散設定手段１７で設定された式（３３）で示される駆動雑音分散を使用し、式（４７）で示す駆動雑音共分散行列を算出する（ステップＳ５の駆動雑音共分散行列算出ステップ）。
【００８７】
【数２５】
【００８８】
つぎに、予測手段３が平滑値ベクトル記憶手段２から入力した前時刻の平滑値ベクトルｘハット（ｋ−１｜ｋ−１）を使用し、式（８）に従って、現時刻の予測値ベクトルｘハット（ｋ｜ｋ−１）を算出する。
【００８９】
【数２６】
【００９０】
また、予測誤差共分散行列算出手段１０が、平滑誤差共分散行列記憶手段９から入力した前時刻の平滑誤差共分散行列Ｐ（ｋ−１｜ｋ−１）と、前時刻に算出した式（４７）の駆動雑音共分散行列を使用し、式（３４）に従って、現時刻の予測誤差共分散行列Ｐ（ｋ｜ｋ−１）を算出する（ステップＳ６の予測処理ステップ）。
【００９１】
【数２７】
【００９２】
つぎに、観測装置１０２より観測雑音共分散行列算出手段１２を介して観測値ベクトルｚ（ｋ）と観測雑音共分散行列Ｂ（ｋ）が入力されると（ステップＳ７の観測諸元入力ステップ）、残差共分散行列算出手段１３が、予測誤差共分散行列算出手段１０で前時刻に算出した予測誤差共分散行列Ｐ（ｋ｜ｋ−１）と、入力された観測雑音共分散行列Ｂ（ｋ）より、残差共分散行列を算出し（ステップＳ８の残差共分散行列算出ステップ）、その残差共分散行列と予測誤差共分散行列により、ゲイン行列設定手段１４によってゲイン行列が設定される（ステップＳ１０のゲイン行列設定ステップ）。
【００９３】
【数２８】
【００９４】
つぎに、ゲート範囲設定手段４が、予測手段３が算出した予測値ベクトルｘハット（ｋ｜ｋ−１）と、観測装置１０２より入力された観測値ベクトルｚ（ｋ）を使用し、式（１３）に示すゲート範囲を設定した（ステップＳ９のゲート設定ステップ）後、平滑手段５が、式（１５）に従って、前期予測値ベクトルｘハット（ｋ｜ｋ−１）、ゲート範囲Ｓ（ｋ）、ゲイン行列Ｋ（ｋ）を使用して、現時刻の平滑値ベクトルｘハット（ｋ｜ｋ）を算出する。この平滑値ベクトルは平滑値ベクトル記憶手段２に格納する（ステップＳ１１の平滑処理ステップの一部）。
【００９５】
【数２９】
【００９６】
また、平滑誤差共分散行列算出手段１１は、式（１６）に従って、前記予測誤差共分散行列Ｐ（ｋ｜ｋ−１）、ゲイン行列Ｋ（ｋ）及び観測行列Ｈを使用して、現時刻の平滑誤差共分散行列Ｐ（ｋ｜ｋ）を算出する。この平滑誤差共分散行列は平滑誤差共分散行列記憶手段９に格納する（ステップＳ１１の平滑処理ステップの残りの部分）。
【００９７】
【数３０】
【００９８】
以上で、時刻ｔ（ｋ）における一連の処理が終了する。追尾終了判定ステップにおいて追尾終了との判定がなされる（ステップＳ１２の追尾終了判定ステップ）まで、これら一連の処理を繰り返して実行する。
【００９９】
本発明の追尾装置及び追尾方法を使用した極座標表示の速度のふらつきを駆動雑音とするカルマンフィルタによれば、従来のカルマンフィルタのように、サンプリング間隔の大きさに比例してゲートが爆発的に拡大することがないため、それぞれのサンプリング間隔に応じた適度な大きさのゲートが設定され、不要信号の受信率を少なくし、追尾を外す危険性を低くすることが可能となる利点を有する。さらに、速度ベクトルの北基準直交座標表示と極座標表示の変換行列内に、１サンプリング前の速度要因が引用されることとなり、従来方式ではゲートが目標速度の大小に関係なく一定の大きさで設定されていたものが、本発明方式では、目標速度の大小に応じてゲートの範囲が変動し、設定されることとなった。これにより、目標速度が大きければ、その速度に対応した大きさのゲートが設定されることになり、目標速度が大きい時に不釣合いに小さいゲートを設定し、目標の追尾を外してしまうような危険性を低くすることが可能となる利点を有する。
【０１００】
実施の形態２．
以下、本発明の実施の形態２に係る目標追尾装置および目標追尾方法について説明する。本実施の形態２においては、構成および動作は、基本的に、図１及び図６に示した実施の形態１と同じであるため、それらを参照することとし、以下の説明においては、実施の形態１と異なる部分のみを説明することとする。
【０１０１】
図７は、本実施の形態２にかかる目標追尾装置の駆動雑音分散設定手段１７の内部構成を示したブロック図である。図７に示すように、本実施の形態２にかかる駆動雑音分散設定手段１７には、目標別の最大加速度を算出する最大加速度算出手段２０と、目標別の最小速度を算出する最小速度算出手段２１と、算出された最大加速度及び最小速度より示される最大角加速度を算出する最大角加速度算出手段２２と、算出された最大角加速度により示される最大旋回半径を算出する最大旋回半径算出手段２３と、算出された最大旋回半径より駆動雑音共分散行列内のパラメータを算出する駆動雑音共分散行列内パラメータ算出手段２４とが設けられている。
【０１０２】
本実施の形態においては、速度ベクトルを駆動雑音とすることにより、目標の最大旋回範囲が機体別（目標別）によって導かれる。まず、目標の機種別により、式（４８）に示す速度の最大値と最小値及び加速度の最大値と最小値が与えられるとする。
【０１０３】
【数３１】
【０１０４】
ここで、式（４９）の定義より、角加速度の最大値と最小値が式（５０）で求められる。
【０１０５】
【数３２】
【０１０６】
式（５０）で求められた最大角加速度を使用し、式（５１）で示す最大旋回半径が求められる。
【０１０７】
【数３３】
【０１０８】
但し、
Δβ：旋回半径
Ｔ：サンプリングレート
とする。
【０１０９】
つまり、本実施の形態においては、駆動雑音分散設定手段１７において、図７に示すように、目標別の最大加速度および最小速度を、それぞれ、最大加速度算出手段２０と最小速度算出手段２１とにより求め、それらの値を用いて、最大角加速度を最大角加速度算出手段２２より算出する。算出された最大角加速度により最大旋回半径を最大旋回半径算出手段２３により算出し、その値に基づいて駆動雑音共分散行列内のパラメータを駆動雑音共分散行列内パラメータ算出手段２４より算出する。
【０１１０】
以上のように、本実施の形態２においては、上述の実施の形態１と同様の効果が得られるとともに、さらに、速度ベクトルを駆動雑音とすることにより、目標の最大旋回範囲が目標別によって導かれ、その値に基づいて駆動雑音共分散行列内のパラメータを算出する。それにより、たとえば等速直線運動を行っていた目標が等速円運動で９０度旋回したのち、再び等速直線運動を行う場合でも、ｘ，ｙ，ｚ方向おのおのにおいて、サンプリング間隔が大きくなるほど、ゲートが不必要に拡大してしまうことを防止し、適度な大きさのゲートの設定を可能として、効率よく目標の追尾を行うことができるという効果が得られる。
【図面の簡単な説明】
【０１１１】
【図１】本発明の実施の形態１に係る目標追尾装置の構成を示したブロック図である。
【図２】一般的な極座標系を示した説明図である。
【図３】本発明の実施の形態１に係る目標追尾装置で用いる座標系を示した説明図である。
【図４】本発明と従来技術とのゲート設定を比較した説明図である。
【図５】本発明と従来技術とのゲート設定を比較した説明図である。
【図６】本発明の実施の形態１に係る目標追尾方法の処理の流れを示したフローチャートである。
【図７】本発明の実施の形態２に係る目標追尾装置における駆動雑音分散設定手段の構成を示したブロック図である。
【図８】従来のカルマンフィルタを使用した目標追尾処理で用いる座標系を示した説明図である。
【図９】目標追尾処理における一般的なゲートを示した模式図である。
【図１０】従来の目標追尾装置の構成を示したブロック図である。
【符号の説明】
【０１１２】
１ 平滑値ベクトル初期値設定手段、２ 平滑値ベクトル記憶手段、３ 予測手段、４ ゲート範囲設定手段、５ 平滑手段、６ 駆動雑音分散設定手段、７ 駆動雑音共分散行列算出手段、８ 平滑誤差共分散行列初期値設定手段、９ 平滑誤差共分散行列記憶手段、１０ 予測誤差共分散行列算出手段、１１ 平滑誤差共分散行列算出手段、１２ 観測雑音共分散行列算出手段、１３ 残差共分散行列算出手段、１４ ゲイン行列設定手段、１５ 速度ノルム計算手段、１６ 駆動雑音ベクトルの座標変換行列算出手段、１７ 駆動雑音分散設定手段、１８ 駆動雑音共分散行列算出手段、２０ 最大加速度算出手段、２１ 最小速度算出手段、２２ 最大角加速度算出手段、２３ 最大旋回半径算出手段、２４ 駆動雑音共分散行列内パラメータ算出手段。
【特許請求の範囲】
【請求項１】
目標の位置、速度等の運動諸元で構成される状態ベクトルの平滑値ベクトルの初期値を設定する平滑値ベクトル初期値設定手段と、
前記平滑値ベクトルの初期値および算出される平滑値ベクトルの値を記憶する平滑値ベクトル記憶手段と、
前記平滑値ベクトル記憶手段に記憶されている前時刻の平滑値ベクトルより現時刻の予測値ベクトルを算出する予測手段と、
前記平滑値ベクトル記憶手段に記憶されている前記平滑値ベクトルに基づいて速度ノルムを算出する速度ノルム計算手段と、
算出された前記速度ノルムと前記平滑値ベクトル記憶手段に記憶されている前記平滑値ベクトルとを入力し、駆動雑音ベクトルの座標変換行列を算出する駆動雑音ベクトルの座標変換行列算出手段と、
駆動雑音分散として目標速度の分散を設定する駆動雑音分散設定手段と、
設定された前記駆動雑音分散と前記駆動雑音ベクトルの座標変換行列算出手段により算出された駆動雑音ベクトルの座標変換行列とにより、駆動雑音共分散行列を算出する駆動雑音共分散行列算出手段と、
平滑ベクトルの誤差を推定する平滑誤差共分散行列の初期値を設定する平滑誤差共分散行列初期値設定手段と、
前記平滑誤差共分散行列の初期値および算出される平滑誤差共分散行列の値を記憶する平滑誤差共分散行列記憶手段と、
前記平滑誤差共分散行列記憶手段に記憶されている前時刻の平滑誤差共分散行列と前記駆動雑音共分散行列算出手段により算出された前記駆動雑音共分散行列とにより、現時刻の予測誤差共分散行列を算出する予測誤差共分散行列算出手段と、
算出された前記予測誤差共分散行列の値と観測装置から得られる観測雑音共分散行列とにより、残差共分散行列を算出する残差共分散行列算出手段と、
算出された前記残差共分散行列の値と前記予測誤差共分散行列算出手段により算出された前記予測誤差共分散行列の値とによりゲイン行列を設定するゲイン行列設定手段と、
前記予測手段により算出された前記予測値ベクトルと前記観測装置から得られる観測値ベクトルとを入力し、ゲートを設定するゲート範囲設定手段と、
設定された前記ゲートと前記予測手段により算出された前記予測値ベクトルと前記ゲイン行列設定手段により設定された前記ゲイン行列とを入力して、平滑値ベクトルを算出する平滑手段と、
前記予測誤差共分散行列算出手段により算出された前記予測誤差共分散行列と前記ゲイン行列設定手段により設定されたゲイン行列とを入力し、平滑誤差共分散行列を算出する平滑誤差共分散行列算出手段と
を備えたことを特徴とする目標追尾装置。
【請求項２】
前記駆動雑音分散設定手段は、
目標別の最大加速度を算出する最大加速度算出手段と、
前記目標別の最小速度を算出する最小速度算出手段と、
算出された前記最大加速度及び前記最小速度に基づいて、前記目標別の最大角加速度を算出する最大角加速度算出手段と、
算出された前記最大角加速度に基づいて、前記目標別の最大旋回半径を算出する最大旋回半径算出手段と、
算出された前記最大旋回半径に基づいて、前記駆動雑音共分散行列算出手段における前記駆動雑音共分散行列内のパラメータを算出する駆動雑音共分散行列内パラメータ算出手段と
を備えていることを特徴とする請求項１に記載の目標追尾装置。
【請求項３】
目標の位置、速度等の運動諸元で構成される状態ベクトルの平滑値ベクトルの初期値を設定する平滑値ベクトル初期値設定ステップと、
前記平滑値ベクトルの初期値および算出される平滑値ベクトルの値を記憶する平滑値ベクトル記憶ステップと、
前記平滑値ベクトル記憶ステップにより記憶された前時刻の平滑値ベクトルより現時刻の予測値ベクトルを算出する予測ステップと、
前記平滑値ベクトル記憶ステップにより記憶された前記平滑値ベクトルに基づいて速度ノルムを算出する速度ノルム計算ステップと、
算出された前記速度ノルムと、前記平滑値ベクトル記憶ステップで記憶された前記平滑値ベクトルとを入力し、駆動雑音ベクトルの座標変換行列を算出する駆動雑音ベクトルの座標変換行列算出ステップと、
駆動雑音分散として目標速度の分散を設定する駆動雑音分散設定ステップと、
設定された前記駆動雑音分散と前記駆動雑音ベクトルの座標変換行列算出ステップにより算出された駆動雑音ベクトルの座標変換行列とにより、駆動雑音共分散行列を算出する駆動雑音共分散行列算出ステップと、
平滑ベクトルの誤差を推定する平滑誤差共分散行列の初期値を設定する平滑誤差共分散行列初期値設定ステップと、
前記平滑誤差共分散行列の初期値および算出される平滑誤差共分散行列の値を記憶する平滑誤差共分散行列記憶ステップと、
前記平滑誤差共分散行列記憶ステップにより記憶された前時刻の平滑誤差共分散行列と前記駆動雑音共分散行列算出ステップにより算出された前記駆動雑音共分散行列とにより、現時刻の予測誤差共分散行列を算出する予測誤差共分散行列算出ステップと、
算出された前記予測誤差共分散行列の値と観測装置から得られる観測雑音共分散行列とにより、残差共分散行列を算出する残差共分散行列算出ステップと、
算出された前記残差共分散行列の値と前記予測誤差共分散行列算出ステップにより算出された前記予測誤差共分散行列の値とによりゲイン行列を設定するゲイン行列設定ステップと、
前記予測ステップにより算出された前記予測値ベクトルと前記観測装置から得られる観測値ベクトルとを入力し、ゲートを設定するゲート範囲設定ステップと、
設定された前記ゲートと前記予測ステップにより算出された前記予測値ベクトルと前記ゲイン行列設定ステップにより設定された前記ゲイン行列とを入力して、平滑値ベクトルを算出する平滑ステップと、
前記予測誤差共分散行列算出ステップにより算出された前記予測誤差共分散行列と前記ゲイン行列設定ステップにより設定されたゲイン行列とを入力し、平滑誤差共分散行列を算出する平滑誤差共分散行列算出ステップと
を備えたことを特徴とする目標追尾方法。
【請求項４】
前記駆動雑音分散設定ステップは、
目標別の最大加速度を算出する最大加速度算出ステップと、
前記目標別の最小速度を算出する最小速度算出ステップと、
算出された前記最大加速度及び前記最小速度に基づいて、前記目標別の最大角加速度を算出する最大角加速度算出ステップと、
算出された前記最大角加速度に基づいて、前記目標別の最大旋回半径を算出する最大旋回半径算出ステップと、
算出された前記最大旋回半径に基づいて、前記駆動雑音共分散行列算出ステップにおける前記駆動雑音共分散行列内のパラメータを算出する駆動雑音共分散行列内パラメータ算出ステップと
を備えていることを特徴とする請求項３に記載の目標追尾方法。
【請求項１】
目標の位置、速度等の運動諸元で構成される状態ベクトルの平滑値ベクトルの初期値を設定する平滑値ベクトル初期値設定手段と、
前記平滑値ベクトルの初期値および算出される平滑値ベクトルの値を記憶する平滑値ベクトル記憶手段と、
前記平滑値ベクトル記憶手段に記憶されている前時刻の平滑値ベクトルより現時刻の予測値ベクトルを算出する予測手段と、
前記平滑値ベクトル記憶手段に記憶されている前記平滑値ベクトルに基づいて速度ノルムを算出する速度ノルム計算手段と、
算出された前記速度ノルムと前記平滑値ベクトル記憶手段に記憶されている前記平滑値ベクトルとを入力し、駆動雑音ベクトルの座標変換行列を算出する駆動雑音ベクトルの座標変換行列算出手段と、
駆動雑音分散として目標速度の分散を設定する駆動雑音分散設定手段と、
設定された前記駆動雑音分散と前記駆動雑音ベクトルの座標変換行列算出手段により算出された駆動雑音ベクトルの座標変換行列とにより、駆動雑音共分散行列を算出する駆動雑音共分散行列算出手段と、
平滑ベクトルの誤差を推定する平滑誤差共分散行列の初期値を設定する平滑誤差共分散行列初期値設定手段と、
前記平滑誤差共分散行列の初期値および算出される平滑誤差共分散行列の値を記憶する平滑誤差共分散行列記憶手段と、
前記平滑誤差共分散行列記憶手段に記憶されている前時刻の平滑誤差共分散行列と前記駆動雑音共分散行列算出手段により算出された前記駆動雑音共分散行列とにより、現時刻の予測誤差共分散行列を算出する予測誤差共分散行列算出手段と、
算出された前記予測誤差共分散行列の値と観測装置から得られる観測雑音共分散行列とにより、残差共分散行列を算出する残差共分散行列算出手段と、
算出された前記残差共分散行列の値と前記予測誤差共分散行列算出手段により算出された前記予測誤差共分散行列の値とによりゲイン行列を設定するゲイン行列設定手段と、
前記予測手段により算出された前記予測値ベクトルと前記観測装置から得られる観測値ベクトルとを入力し、ゲートを設定するゲート範囲設定手段と、
設定された前記ゲートと前記予測手段により算出された前記予測値ベクトルと前記ゲイン行列設定手段により設定された前記ゲイン行列とを入力して、平滑値ベクトルを算出する平滑手段と、
前記予測誤差共分散行列算出手段により算出された前記予測誤差共分散行列と前記ゲイン行列設定手段により設定されたゲイン行列とを入力し、平滑誤差共分散行列を算出する平滑誤差共分散行列算出手段と
を備えたことを特徴とする目標追尾装置。
【請求項２】
前記駆動雑音分散設定手段は、
目標別の最大加速度を算出する最大加速度算出手段と、
前記目標別の最小速度を算出する最小速度算出手段と、
算出された前記最大加速度及び前記最小速度に基づいて、前記目標別の最大角加速度を算出する最大角加速度算出手段と、
算出された前記最大角加速度に基づいて、前記目標別の最大旋回半径を算出する最大旋回半径算出手段と、
算出された前記最大旋回半径に基づいて、前記駆動雑音共分散行列算出手段における前記駆動雑音共分散行列内のパラメータを算出する駆動雑音共分散行列内パラメータ算出手段と
を備えていることを特徴とする請求項１に記載の目標追尾装置。
【請求項３】
目標の位置、速度等の運動諸元で構成される状態ベクトルの平滑値ベクトルの初期値を設定する平滑値ベクトル初期値設定ステップと、
前記平滑値ベクトルの初期値および算出される平滑値ベクトルの値を記憶する平滑値ベクトル記憶ステップと、
前記平滑値ベクトル記憶ステップにより記憶された前時刻の平滑値ベクトルより現時刻の予測値ベクトルを算出する予測ステップと、
前記平滑値ベクトル記憶ステップにより記憶された前記平滑値ベクトルに基づいて速度ノルムを算出する速度ノルム計算ステップと、
算出された前記速度ノルムと、前記平滑値ベクトル記憶ステップで記憶された前記平滑値ベクトルとを入力し、駆動雑音ベクトルの座標変換行列を算出する駆動雑音ベクトルの座標変換行列算出ステップと、
駆動雑音分散として目標速度の分散を設定する駆動雑音分散設定ステップと、
設定された前記駆動雑音分散と前記駆動雑音ベクトルの座標変換行列算出ステップにより算出された駆動雑音ベクトルの座標変換行列とにより、駆動雑音共分散行列を算出する駆動雑音共分散行列算出ステップと、
平滑ベクトルの誤差を推定する平滑誤差共分散行列の初期値を設定する平滑誤差共分散行列初期値設定ステップと、
前記平滑誤差共分散行列の初期値および算出される平滑誤差共分散行列の値を記憶する平滑誤差共分散行列記憶ステップと、
前記平滑誤差共分散行列記憶ステップにより記憶された前時刻の平滑誤差共分散行列と前記駆動雑音共分散行列算出ステップにより算出された前記駆動雑音共分散行列とにより、現時刻の予測誤差共分散行列を算出する予測誤差共分散行列算出ステップと、
算出された前記予測誤差共分散行列の値と観測装置から得られる観測雑音共分散行列とにより、残差共分散行列を算出する残差共分散行列算出ステップと、
算出された前記残差共分散行列の値と前記予測誤差共分散行列算出ステップにより算出された前記予測誤差共分散行列の値とによりゲイン行列を設定するゲイン行列設定ステップと、
前記予測ステップにより算出された前記予測値ベクトルと前記観測装置から得られる観測値ベクトルとを入力し、ゲートを設定するゲート範囲設定ステップと、
設定された前記ゲートと前記予測ステップにより算出された前記予測値ベクトルと前記ゲイン行列設定ステップにより設定された前記ゲイン行列とを入力して、平滑値ベクトルを算出する平滑ステップと、
前記予測誤差共分散行列算出ステップにより算出された前記予測誤差共分散行列と前記ゲイン行列設定ステップにより設定されたゲイン行列とを入力し、平滑誤差共分散行列を算出する平滑誤差共分散行列算出ステップと
を備えたことを特徴とする目標追尾方法。
【請求項４】
前記駆動雑音分散設定ステップは、
目標別の最大加速度を算出する最大加速度算出ステップと、
前記目標別の最小速度を算出する最小速度算出ステップと、
算出された前記最大加速度及び前記最小速度に基づいて、前記目標別の最大角加速度を算出する最大角加速度算出ステップと、
算出された前記最大角加速度に基づいて、前記目標別の最大旋回半径を算出する最大旋回半径算出ステップと、
算出された前記最大旋回半径に基づいて、前記駆動雑音共分散行列算出ステップにおける前記駆動雑音共分散行列内のパラメータを算出する駆動雑音共分散行列内パラメータ算出ステップと
を備えていることを特徴とする請求項３に記載の目標追尾方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【公開番号】特開２００９−１４５９６（Ｐ２００９−１４５９６Ａ）
【公開日】平成２１年１月２２日（２００９．１．２２）
【国際特許分類】
【出願番号】特願２００７−１７８３１７（Ｐ２００７−１７８３１７）
【出願日】平成１９年７月６日（２００７．７．６）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
【公開日】平成２１年１月２２日（２００９．１．２２）
【国際特許分類】
【出願日】平成１９年７月６日（２００７．７．６）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
[ Back to top ]