JP3970585B2 - 目標追尾装置及び方法 - Google Patents
目標追尾装置及び方法 Download PDFInfo
- Publication number
- JP3970585B2 JP3970585B2 JP2001359939A JP2001359939A JP3970585B2 JP 3970585 B2 JP3970585 B2 JP 3970585B2 JP 2001359939 A JP2001359939 A JP 2001359939A JP 2001359939 A JP2001359939 A JP 2001359939A JP 3970585 B2 JP3970585 B2 JP 3970585B2
- Authority
- JP
- Japan
- Prior art keywords
- vector
- prediction
- observation
- acceleration
- residual
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Images
Landscapes
- Radar Systems Or Details Thereof (AREA)
Description
【０００１】
【発明の属する技術分野】
この発明は、レーダ、レーザ、カメラ等にて観測した、車両、航空機等の移動目標の距離、俯角（仰角）、方位角、位置、またはこれらの時間変化率等の運動諸元を推定する目標追尾装置及び方法に関するものである。
【０００２】
【従来の技術】
図６は、例えば、"An Adaptive Two-Dimensional Kalman Tracking Filter", IEEE Transaction on Aerospace and Electronic Systems Vol.AES-16,No.6,1980.に示された従来の目標追尾装置を示す構成図である。
【０００３】
図６において、１は目標の位置を観測するレーダ装置、２は予測器１３から追尾対象の予測ベクトルを入力し、レーダ装置１により観測された観測ベクトルのうち、予測ベクトルに最も近い観測ベクトルを選択する目標相関器、３は目標相関器２で選択した観測ベクトルから予測器１３で算出した予測観測ベクトルを減算する残差演算器である。
【０００４】
４は第２の遅延回路９から入力した１サンプリング前の平滑誤差共分散行列と、駆動雑音調整器１２から入力した駆動雑音から、予測誤差共分散行列を算出する予測誤差評価器、５は予測誤差評価器４から入力した予測誤差共分散行列と、予め設定されたレーダ装置１の観測誤差共分散行列とからゲイン行列を算出するゲイン行列算出器、６は残差算出器３を介して予測器１３から入力した予測ベクトルと、残差算出器３から入力した残差ベクトルと、ゲイン行列算出器５から入力したゲイン行列とから平滑ベクトルを算出する平滑器、７は予測誤差評価器４から入力した予測誤差共分散行列と、ゲイン行列算出器５から入力したゲイン行列とから平滑誤差共分散行列を算出する平滑誤差評価器、８は平滑器６から入力した平滑ベクトルを１サンプリング時間だけ遅延する第１の遅延回路、９は平滑誤差評価器７から入力した平滑誤差共分散行列を１サンプリング時間だけ遅延する第２の遅延回路である。
【０００５】
１０は残差算出器３から入力した残差ベクトルを、予測器１３から入力した予測誤差共分散行列と予め設定されたレーダ装置１の観測誤差共分散行列で正規化する正規化演算器、１１は正規化演算器１０から入力した正規化後の残差を１サンプリング時間だけ遅延する第３の遅延回路、１２は第３の遅延回路１１から入力した１サンプリング前の正規化後の残差をもとに目標の加速度運動を判定し駆動雑音の値を決定する駆動雑音調整器、１３は第１の遅延回路８から入力した１サンプリング前の平滑ベクトルから、予め定義された等速直線運動モデルに基づき予測ベクトルを算出する予測器である。
【０００６】
次に動作について説明する。目標相関器２は、レーダ装置１から観測ベクトルを受け取ると、その観測ベクトルが目標に係る観測ベクトルである可能性が高い場合にのみ有効なデータとして採用するため、予測器１３から入力した予測ベクトルに最も近い観測ベクトルを出力する。
【０００７】
残差算出器３は、目標相関器２で採用した観測ベクトルと、予測器１３で算出した予測ベクトルを入力し、これらの差ベクトルである残差ベクトルを算出し、平滑器６と正規化演算器１０にそれぞれ送出する。
【０００８】
そして、ゲイン行列算出器５は、予測誤差評価器４から入力した予測誤差共分散行列と、予め設定されたレーダ装置１の観測誤差共分散行列とからゲイン行列を算出する。平滑器６は、残差算出器３から残差ベクトルと予測ベクトルを受け取ると、ゲイン行列算出器５からゲイン行列を入力し、目標の位置及び速度の平滑ベクトルを算出する。
【０００９】
ここで、平滑誤差評価器７は、１サンプリング前において、予測誤差評価器４から入力した予測誤差共分散行列と、ゲイン行列算出器５から入力したゲイン行列とから平滑誤差共分散行列を算出し、予測誤差評価器４は第２の遅延回路９から入力した１サンプリング前の平滑誤差共分散行列と、駆動雑音調整器１２から入力した駆動雑音から、予測誤差共分散行列を算出する。
【００１０】
正規化演算器１０は、残差算出器３から残差ベクトル入力すると、予測器１３から入力した予測誤差共分散行列と予め設定されたレーダ装置１の観測誤差共分散行列の和で残差ベクトルを正規化し、第３の遅延回路１１に送出する。駆動雑音調整器１２は、第３の遅延回路から１サンプリング前の正規化後の残差を入力し、その値が予め設定された値より大きい場合にのみ、目標が加速度運動をしていると判断して駆動雑音の値を大きな値に変更し、予測誤差評価器４に送出する。
【００１１】
前記のようにして、平滑器６が目標の位置及び速度の平滑ベクトルを算出すると、予測器１３は、平滑ベクトルと予め定義された目標の運動モデルを用いて、現時刻より１サンプリング後の目標の予測位置及び速度からなる予測ベクトルを算出する。
【００１２】
【発明が解決しようとする課題】
従来の目標追尾装置は以上のように構成されているので、目標が等速直線運動をしているときでも、ランダム性の観測雑音が突発的に大きくなった場合に、正規化後の残差が大きくなって駆動雑音調整器１２が加速度運動を誤って検出してしまい、追尾性能が劣化するという課題があった。
【００１３】
この発明は前記のような課題を解決するためになされたもので、例えば直線道路を走行する車両のように、目標の運動の特徴として、ある特定の方向の速度変化が大きくそれ以外の方向の速度変化は無視できるような場合に、残差の目標の加減速方向の成分の時間平均もしくは連続するサンプルの複数残差ベクトル間の内積の時間平均を利用して特定の方向の残差成分のみを加速度検出の対象とすることで、ランダム性の観測雑音による加速度運動の誤検出の少ない目標追尾装置及び方法を得ることを目的とする。
【００１４】
【課題を解決しようとする手段】
この発明に係る目標追尾装置は、目標を観測して観測ベクトルを得る観測手段と、目標の状態ベクトルの予測ベクトルを演算する予測手段と、目標の状態ベクトルの平滑ベクトルを演算する平滑手段と、所望の方向の単位ベクトルを格納した方向ベクトルデータベースと、前記データベースから入力した所望方向の単位ベクトルと、前記観測手段により観測された観測ベクトルのうち、前記予測手段で演算された予測ベクトルに最も近い観測ベクトルから前記予測ベクトルを減算することで得られる残差ベクトルを正規化した正規化後の残差ベクトルとの内積を算出する方向ベクトル内積演算手段と、前記方向ベクトル内積演算手段により算出された内積をスレッショルド判定して各単位ベクトルの方向に対する加速度運動の有無を判定することにより、特定方向の速度変化を検出する加速度検出手段とを備えたものである。
【００１５】
また、前記加速度検出手段にて速度変化が検出された場合に前記予測手段で用いる運動モデルを速度変化を考慮した運動モデルに切り換える等加速度運動モデル切り換え手段をさらに備えたことを特徴とするものである。
【００１６】
また、他の発明に係る目標追尾装置は、目標を観測して観測ベクトルを得る観測手段と、目標の状態ベクトルの予測ベクトルを演算する予測手段と、目標の状態ベクトルの平滑ベクトルを演算する平滑手段と、所望の方向の単位ベクトルを格納した方向ベクトルデータベースと、前記データベースから入力した所望方向の単位ベクトルと、前記観測手段により観測された観測ベクトルのうち、前記予測手段で演算された予測ベクトルに最も近い観測ベクトルから前記予測ベクトルを減算することで得られる残差ベクトルを正規化した正規化後の残差ベクトルとの内積を算出する方向ベクトル内積演算手段と、前記方向ベクトル内積演算手段により算出された隣り合うサンプリングにおける内積の変化量をスレッショルド判定して特定方向の加速度変化または速度変化を検出する加速度変化検出手段と、前記加速度変化検出手段により加速度変化または速度変化が検出された場合に前記予測手段で用いる運動モデルを加速度変化または速度変化を考慮した運動モデルに切り換える加速度変化運動モデル切り換え手段とを備えたものである。
【００１７】
また、さらに他の発明に係る目標追尾装置は、目標を観測して観測ベクトルを得る観測手段と、目標の状態ベクトルの予測ベクトルを演算する予測手段と、目標の状態ベクトルの平滑ベクトルを演算する平滑手段と、前記観測手段により観測された観測ベクトルのうち、前記予測手段で演算された予測ベクトルに最も近い観測ベクトルから前記予測ベクトルを減算することで得られる残差ベクトルを観測ベクトルの予測誤差共分散行列を用いて正規化後、連続する過去２サンプルの正規化後の残差ベクトル間の内積を算出する隣接残差内積演算手段と、前記隣接残差内積演算手段により算出された内積の値によりある一定方向の速度変化を検出する加速度検出手段とを備えたものである。
【００１８】
また、前記加速度検出手段にて速度変化が検出された場合に前記予測手段で用いる運動モデルを速度変化を考慮した運動モデルに切り換える等加速度運動モデル切り換え手段をさらに備えたことを特徴とするものである。
【００１９】
また、さらに他の発明に係る目標追尾装置は、目標を観測して観測ベクトルを得る観測手段と、目標の状態ベクトルの予測ベクトルを演算する予測手段と、目標の状態ベクトルの平滑ベクトルを演算する平滑手段と、前記観測手段により観測された観測ベクトルのうち、前記予測手段で演算された予測ベクトルに最も近い観測ベクトルから前記予測ベクトルを減算することで得られる残差ベクトルを観測ベクトルの予測誤差共分散行列を用いて正規化後、連続する過去２サンプルの正規化後の残差ベクトル間の内積を算出する隣接残差内積演算手段と、前記隣接残差内積演算手段により算出された隣り合うサンプリングにおける内積の変化量をスレッショルド判定して特定方向の加速度変化または速度変化を検出する加速度変化検出手段と、前記加速度変化検出手段にて加速度変化または速度変化が検出された場合に前記予測手段で用いる運動モデルを加速度変化または速度変化を考慮した運動モデルに切り換える加速度変化運動モデル切り換え手段とを備えたものである。
【００２０】
また、この発明に係る目標追尾方法は、目標を観測して観測ベクトルを得る観測ステップと、目標の状態ベクトルの予測ベクトルを演算する予測ステップと、目標の状態ベクトルの平滑ベクトルを演算する平滑ステップと、所望の方向の単位ベクトルを格納した方向ベクトルデータベースから入力した所望方向の単位ベクトルと、前記観測ステップにより観測された観測ベクトルのうち、前記予測ステップで演算された予測ベクトルに最も近い観測ベクトルから前記予測ベクトルを減算することで得られる残差ベクトルを正規化した正規化後の残差ベクトルとの内積を算出する方向ベクトル内積演算ステップと、前記方向ベクトル内積演算ステップにより算出された内積をスレッショルド判定して各単位ベクトルの方向に対する加速度運動の有無を判定することにより、特定方向の速度変化を検出する加速度検出ステップとを備えたものである。
【００２１】
また、前記加速度検出ステップにて速度変化が検出された場合に前記予測ステップで用いる運動モデルを速度変化を考慮した運動モデルに切り換える等加速度運動モデル切り換えステップをさらに備えたことを特徴とするものである。
【００２２】
また、他の発明に係る目標追尾方法は、目標を観測して観測ベクトルを得る観測ステップと、目標の状態ベクトルの予測ベクトルを演算する予測ステップと、目標の状態ベクトルの平滑ベクトルを演算する平滑ステップと、所望の方向の単位ベクトルを格納した方向ベクトルデータベースから入力した所望方向の単位ベクトルと、前記観測ステップにより観測された観測ベクトルのうち、前記予測ステップで演算された予測ベクトルに最も近い観測ベクトルから前記予測ベクトルを減算することで得られる残差ベクトルを正規化した正規化後の残差ベクトルとの内積を算出する方向ベクトル内積演算ステップと、前記方向ベクトル内積演算ステップにより算出された隣り合うサンプリングにおける内積の変化量をスレショルド判定して特定方向の加速度変化または速度変化を検出する加速度変化検出ステップと、前記加速度変化検出ステップにて加速度変化または速度変化が検出された場合に前記予測ステップで用いる運動モデルを加速度変化または速度変化を考慮した運動モデルに切り換える加速度変化運動モデル切り換えステップとを備えたものである。
【００２３】
また、さらに他の発明に係る目標追尾方法は、目標を観測して観測ベクトルを得る観測ステップと、目標の状態ベクトルの予測ベクトルを演算する予測ステップと、目標の状態ベクトルの平滑ベクトルを演算する平滑ステップと、前記観測ステップにより観測された観測ベクトルのうち、前記予測ステップで演算された予測ベクトルに最も近い観測ベクトルから前記予測ベクトルを減算することで得られる残差ベクトルを観測ベクトルの予測誤差共分散行列を用いて正規化後、連続する過去２サンプルの正規化後の残差ベクトル間の内積を算出する隣接残差内積演算ステップと、前記隣接残差内積演算ステップにより算出された内積の値によりある一定方向速度変化を検出する加速度検出ステップとを備えたものである。
【００２４】
また、前記加速度検出ステップにて速度変化が検出された場合に前記予測ステップで用いる運動モデルを速度変化を考慮した運動モデルに切り換える等加速度運動モデル切り換えステップをさらに備えたことを特徴とするものである。
【００２５】
また、さらに他の発明に係る目標追尾方法は、目標を観測して観測ベクトルを得る観測ステップと、目標の状態ベクトルの予測ベクトルを演算する予測ステップと、目標の状態ベクトルの平滑ベクトルを演算する平滑ステップと、前記観測ステップにより観測された観測ベクトルのうち、前記予測ステップで演算された予測ベクトルに最も近い観測ベクトルから前記予測ベクトルを減算することで得られる残差ベクトルを観測ベクトルの予測誤差共分散行列を用いて正規化後、連続する過去２サンプルの正規化後の残差ベクトル間の内積を算出する隣接残差内積演算ステップと、前記隣接残差内積演算ステップにより算出された隣り合うサンプリングにおける内積の変化量をスレショルド判定して特定方向の加速度変化または速度変化を検出する加速度変化検出ステップと、前記加速度変化検出ステップにて加速度変化または速度変化が検出された場合に前記予測ステップで用いる運動モデルを加速度変化または速度変化を考慮した運動モデルに切り換える加速度変化運動モデル切り換えステップとを備えたものである。
【００２６】
【発明の実施の形態】
実施の形態１．
図１は、この実施の形態１による目標追尾装置を示す構成図である。図１において、１は目標の位置を観測するレーダ装置、２は予測器１３から追尾対象の予測ベクトルを入力し、レーダ装置１により観測された観測ベクトルのうち、予測ベクトルに最も近い観測ベクトルを選択する目標相関器、３は目標相関器２で選択した観測ベクトルから予測器１３で算出した予測観測ベクトルを減算する残差演算器である。
【００２７】
４は第２の遅延回路９から入力した１サンプリング前の平滑誤差共分散行列と、予め設定された駆動雑音から、予測誤差共分散行列を算出する予測誤差評価器、５は予測誤差評価器４から入力した予測誤差共分散行列と、予め設定されたレーダ装置１の観測誤差共分散行列とからゲイン行列を算出するゲイン行列算出器、６は残差算出器３を介して予測器１３から入力した予測ベクトルと、残差算出器３から入力した残差ベクトルと、ゲイン行列算出器５から入力したゲイン行列とから平滑ベクトルを算出する平滑器である。
【００２８】
７は予測誤差評価器４から入力した予測誤差共分散行列と、ゲイン行列算出器５から入力したゲイン行列とから平滑誤差共分散行列を算出する平滑誤差評価器、８は平滑器６から入力した平滑ベクトルを１サンプリング時間だけ遅延する第１の遅延回路、９は平滑誤差評価器７から入力した平滑誤差共分散行列を１サンプリング時間だけ遅延する第２の遅延回路である。
【００２９】
１０は残差算出器３から入力した残差ベクトルを、予測器１３から入力した予測誤差共分散行列と予め設定されたレーダ装置１の観測誤差共分散行列で正規化する正規化演算器、１１は正規化演算器１０から入力した正規化後の残差を１サンプリング時間だけ遅延する第３の遅延回路、１３は第１の遅延回路８から入力した１サンプリング前の平滑ベクトルから、等加速度運動モデル切り換え器１７により選択された運動モデルに基づき予測ベクトルを算出する予測器である。
【００３０】
１４は所望の方向の単位ベクトルが格納されている方向ベクトルデータベース、１５は方向ベクトルデータベース１４から入力した所望の方向の単位ベクトルと、第３の遅延回路１１から入力した１サンプリング前の正規化後の残差と残差ベクトルを入力し、各単位ベクトルと正規化後の残差ベクトルの内積を演算する方向ベクトル内積演算器、１６は方向ベクトル内積演算器１５から入力した各単位ベクトルと正規化後の残差ベクトルの内積の値をスレッショルド判定し、各単位ベクトルの方向に対する加速度運動の有無を判定する加速度検出器、１７は加速度検出器１６で目標の加速度運動があると判定された場合に等加速度運動モデルに、ないと判定された場合に等速直線運動モデルに切り換える等加速度運動モデル切り換え器である。
【００３１】
次に動作について説明する。最初に、この実施の形態１による目標追尾装置の動作原理を説明する。目標は２次元平面内を運動するものとする。目標の運動する平面をｘｙ平面に取った３次元直交座標を基準座標と呼ぶことにする。図２に示すように、レーダ装置１はｚ軸上に設置され、その座標を（０，０，ｚr）とする。レーダ装置１は、目標の距離Ｒ及び方位角Ａｚ（ｙ軸正の方向より時計回りを正とする）を観測する。
【００３２】
目標が等速直線運動をしていることを想定した、ｘｙ座標における等速直線運動モデルを式（１）に示す。ただし、アンダーバーｘkはサンプリング時刻ｔkにおける目標運動諸元の真値を表す状態ベクトルであり、基準座標における目標位置ベクトルを式（２）とし、同速度ベクトルを式（３）とすると、目標の状態ベクトルは式（４）で表される。なお、アンダーバーＡTはベクトルアンダーバーＡの転置ベクトルを表す。
【００３３】
【数１】
【００３４】
ここで、Φk-1はサンプリング時刻ｔk-1よりｔkへの状態ベクトルアンダーバーｘkの推移行列であり、式（５）で表される。また、アンダーバーｗkはサンプリング時刻ｔkにおける駆動雑音ベクトルであり、Γ1（ｋ）はサンプリング時刻ｔkにおける駆動雑音ベクトルの変換行列であり、例えば、目標の運動モデルを等速直線運動と仮定したことによる打ち切り誤差項をΓ1（ｋ−１）アンダーバーｗk-1とみれば、アンダーバーｗkは加速度ベクトル相当であり、Γ1（ｋ−１）は式（６）で表される。なお、Ｔはレーダ装置１のサンプリング間隔、Ｉは２行２列の単位行列である。
【００３５】
【数２】
【００３６】
また、平均を表す記号としてＥを用いると、アンダーバーｗkは平均の２次元正規分布白色雑音であり、式（７）及び（８）とする。ただし、アンダーバー０は零ベクトルであり、Ｑkはサンプリング時刻ｔkにおける駆動雑音共分散行列である。
【００３７】
【数３】
【００３８】
次に、目標の距離Ｒ及び方位角Ａｚがサンプリング時刻ｔkにレーダ装置１より観測される場合のｘｙ座標におけるレーダ装置１の観測モデルを式（９）とする。ここで、距離Ｒ及び方位角Ａｚは式（１０）及び式（１１）によりｘｙ座標に変換される。ここで、アンダーバーｚkはサンプリング時刻ｔkにおけるレーダ観測装置１の観測ベクトル、Ｈは観測行列で、式（１２）で表される。
【００３９】
また、アンダーバーνkはサンプリング時刻ｔkにおけるレーダ装置１の観測雑音ベクトルであり、平均アンダーバー０の２次元正規分布白色雑音で、式（１３）及び（１４）で表される。なお、Ｒkはサンプリング時刻ｔkにおけるレーダ装置１の観測雑音共分散行列である。Γ2（ｋ）は極座標よりｘｙ座標への観測雑音ベクトルの変換行列で、式（１５）で表される。ここで、式（１６）は目標の状態ベクトルアンダーバーｘkの予測ベクトルである。
【００４０】
サンプリング時刻ｔkまでに目標の追尾に用いたレーダ装置１の観測ベクトル全体をＺkとする（式（１７）を参照）。
【００４１】
【数４】
【００４２】
次に、サンプリング時刻ｔk-1までのレーダ装置１の観測ベクトルＺk-1が得られているときの予測処理について述べる。目標が等速直線運動をしていると仮定した場合のサンプリング時刻ｔkにおけるアンダーバーｘkの予測ベクトルをアンダーバーｘkハット（−）、予測誤差共分散行列をＰk（−）とすると、それぞれ条件付平均ベクトル、条件付共分散行列で定義され、式（１８）及び式（１９）で表される。ここで、アンダーバーｘkハット（＋）及びＰk（＋）はそれぞれ前サンプリング時刻ｔk-1の平滑ベクトル及び平滑誤差共分散行列である。これらの算出方法については後で述べる。
【００４３】
一方、目標が等加速度運動をしていると仮定した場合のサンプリング時刻ｔkにおけるアンダーバーｘkの予測ベクトルをアンダーバーｘ’kハットとすると、式（２０）で表すことができる。ここで、式（２１）とおく。
【００４４】
【数５】
【００４５】
次に、目標の位置ベクトルがサンプリング時刻ｔkにおいて、レーダ装置１より観測される場合の目標相関処理について述べる。サンプリング時刻ｔkにおいてレーダ装置１より観測されたｍ個の観測ベクトルをアンダーバーｚk,i（ｉ＝１，２，・・・，ｍ）とすると、目標からの観測ベクトルアンダーバーｚkは、式（２２）で与えられる予測位置ベクトルアンダーバーｚk（−）との距離が最も小さい観測ベクトル、すなわち、式（２３）を満たす観測ベクトルとする。
【００４６】
【数６】
【００４７】
次に、目標の位置ベクトルがサンプリング時刻ｔkにおいて、レーダ装置１により観測される場合の平滑処理について述べる。ゲイン行列Ｋk、平滑ベクトルアンダーバーｘハット（＋）及び平滑誤差共分散行列Ｐk（＋）は通常のカルマンフィルタの理論により、式（２４）〜（２６）で与えられる。ここで、アンダーバーνkは式（２７）で与えられる残差ベクトルである。
【００４８】
【数７】
【００４９】
残差ベクトルアンダーバーνkは、目標が仮定した等速直線運動モデルと異なる運動を行うほど大きくなるため、その大きさにより目標の速度変化を判定することが可能である。しかし、残差ベクトルアンダーバーνkは同時にランダム性の観測雑音の影響も受ける。しかも、レーダによる観測では、観測誤差はｘｙ座標において一様ではなく、目標の位置によって大きさが異なる（レーダ装置と目標の距離が離れるほど観測誤差は大きくなる）。従って、この目標位置による観測誤差の相違の影響を排除するため、式（２８）を用いて、残差ベクトルを観測ベクトルの予測誤差共分散Ｓkで正規化する。ここで、観測ベクトルの予測誤差共分散行列Ｓkは式（２９）で与えられる。
【００５０】
【数８】
【００５１】
次に、正規化した残差ベクトルの大きさＶkから目標の速度変化を判定し、その結果に応じて予測処理を準最適化する方法を示す。ある特定の方向の速度変化が大きく、それ以外の方向の速度変化が小さい目標の場合、速度変化が大きい方向の速度変化のみ検出すればよい。例えば、直線道路を走行する車両は、道路に沿った方向の速度変化が大きく、それ以外の方向の速度変化は小さいため、道路や滑走路に沿った方向の残差ベクトルの大きさを判定すればよい。ｊ番目の所望の方向の単位ベクトルをアンダーバーｕj（ｊ＝１，２，・・・，ｎ）とすると、正規化した残差ベクトルの大きさＶkのアンダーバーｕj方向の成分Ｕk,jは、内積を利用して式（３０）により得られる。ここで、θjは残差ベクトルアンダーバーνkとアンダーバーｕjのなす角である。
【００５２】
そして、このＵk,jのｓサンプルの平均オーババーＵk,jが、予め設定されたパラメータαに対して式（３１）を満たすときに目標が速度変化をしたと判定し、予測処理において、式（１８）の代わりに式（２０）を用いる。ここで、平均の母数ｓが大きいほど、加速度検出に遅れを生じるが、観測雑音の方向はランダムであるため、逆に観測雑音によって速度変化を誤検出する危険性が少なくなる。
【００５３】
【数９】
【００５４】
次に、この実施の形態１による目標追尾装置の動作を具体的に説明する。なお、カルマンフィルタを目標追尾装置に通常適用する場合と同様にして平滑ベクトルの初期値は別途定まっているものとする。
【００５５】
レーダ装置１では、図２に示すように、目標の距離Ｒ及び方位角Ａｚを観測し、式（９）及び式（１０）に従いｘｙ座標に変換する（ステップＳＴ１）。
【００５６】
目標相関器２では、予測器１３から追尾対象の予測ベクトルアンダーバーｘkハット（−）を入力して予測位置ベクトルをアンダーバーｚkハット（−）式（２２）に従い算出し、レーダ装置１から入力した観測ベクトルのうち、観測ベクトルの予測ベクトルアンダーバーｚk（−）に最も近い観測ベクトルを式（２３）に従い選択する（ステップＳＴ２）。
【００５７】
残差演算器３では、目標相関器２で選択した観測ベクトルと予測器１３で算出した予測観測ベクトルを入力し、式（２７）に残差ベクトルアンダーバーνkを算出する（ステップＳＴ３）。
【００５８】
一方、予測誤差評価器４では、第２の遅延回路９から１サンプリング前の平滑誤差共分散行列Ｐk-1（＋）を入力し、予め設定された駆動雑音Ｑk-1に基づい、予測誤差共分散行列Ｐk（−）を式（１９）に従い算出する（ステップＳＴ４）。
【００５９】
ゲイン行列算出器５では、予測誤差評価器４から入力した予測誤差共分散行列Ｐk（−）と、予め設定されたレーダ装置１の観測誤差共分散行列Ｒkとから式（２４）に従いゲイン行列Ｋkを算出する（ステップＳＴ５）。
【００６０】
平滑器６では、予測器１３の出力した予測ベクトルアンダーバーｘkハット（−）を残差演算器３を経由して入力し、また、残差算出器３から残差ベクトルアンダーバーνkを、ゲイン行列算出器５からゲイン行列Ｋkをそれぞれ入力し、式（２５）に従い平滑ベクトルアンダーバーｘkハット（＋）を算出する（ステップＳＴ６）。
【００６１】
平滑誤差評価器７では、予測誤差評価器４から予測誤差共分散行列Ｐk（−）を、ゲイン行列算出器５からゲイン行列Ｋkをそれぞれ入力し、式（２６）に従い平滑誤差共分散行列Ｐk（＋）を算出する（ステップＳＴ７）。
【００６２】
残差演算器３が残差ベクトルアンダーバーνkを算出すると、正規化演算器１０では、まず、予測器１３から予測誤差共分散行列Ｐk（−）を入力し、これと予め設定されたレーダ装置１の観測誤差共分散行列Ｒkを用いて式（２９）に従い、観測ベクトルの予測誤差共分散行列Ｓkを算出する。次に、残差算出器３から残差ベクトルアンダバーνkを入力し、これを、先に算出した観測ベクトルの予測誤差共分散行列Ｓkを用いて式（２８）に従い正規化する（ステップＳＴ８）。
【００６３】
方向ベクトル内積演算器１５では、方向ベクトルデータベース１４から所望の方向の単位ベクトルアンダバーｕjを、第３の遅延回路１１から１サンプリング前の正規化後の残差Ｖk-1と残差ベクトルアンダバーνk-1をそれぞれ入力し、各単位ベクトルアンダバーｕjと正規化後の残差ベクトルの内積Ｕk,jの過去ｓサンプルの平均オーババーＵk,jを式（３０）及び式（３１）に従い演算する（ステップＳＴ９）。
【００６４】
加速度検出器１６では、方向ベクトル内積演算器１５から各単位ベクトルと正規化後の残差ベクトルの内積の過去ｓサンプルの平均オーババーＵk,jを入力し、予め設定されたパラメータαについて式（３１）を満たす場合にのみ目標の加速度運動があると判定する（ステップＳＴ９）。
【００６５】
等加速度運動モデル切り換え器１７では、加速度検出器１６で目標の加速度運動があると判定された場合に式（２０）の等加速度運動モデルに、ないと判定された場合に式（１８）の等速直線運動モデルに切り換える（ステップＳＴ１０）。
【００６６】
予測器１３では１サンプリング前の平滑ベクトルアンダバーｘkハット（＋）を第１の遅延回路８を介して入力し、等加速度運動モデル切り換え器１７により選択された運動モデルに基づき予測ベクトルアンダバーｘkハット（−）を算出する（ステップＳＴ１１）。
以上、追尾終了になるまでこの一連の処理を繰り返す。
【００６７】
以上で明らかなように、この実施の形態１によれば、方向ベクトル内積演算器１５で所望の方向の単位ベクトルと正規化後の残差ベクトルの内積の過去ｓサンプルの平均オーババーＵk,jを算出し、加速度検出器１６において過去ｓサンプルの平均オーババーＵk,jのスレッショルド判定により速度変化を検出しているので、ランダム性の観測誤差の影響によって速度変化が誤って検出されるのを防ぐことができる。
【００６８】
また、目標の速度変化を検出した場合に、等加速度運動モデル切り換え器１７により、目標の運動モデルを等加速度運動モデルに切り換える構成としているので、目標が加速度運動することにより生じる追従遅れの少ない安定した追尾が可能となる効果を奏する。
【００６９】
実施の形態２．
図３は、この実施の形態２による目標追尾装置を示す構成図である。図３において、図１と同一符号である１〜１５は実施の形態１と同一または相当部分を示すのでその説明を省略する。
【００７０】
１８は方向ベクトル内積演算器１５から入力した各単位ベクトルと正規化後の残差ベクトルの内積の値を１サンプリング時間だけ遅延する第４の遅延回路、１９は方向ベクトル内積演算器１５から入力した各単位ベクトルと正規化後の残差ベクトルの内積の値と、第４の遅延回路１８から入力した現時刻から１サンプリング前の各単位ベクトルと正規化後の残差ベクトルの内積の値との差をスレッショルド判定し、各単位ベクトルの方向に対する加速度運動または加速度変化運動の有無を判定する加速度変化検出器である。
【００７１】
２０は加速度変化検出器１９で目標の加速度変化運動があると判定された場合に加速度変化運動モデルに、加速度変化運動がないと判定されかつ加速度運動があると判定された場合に等加速度運動モデルに、どちらもないと判定された場合に等速直線運動モデルに切り換える加速度変化運動モデル切り換え器である。
【００７２】
次に動作について説明する。最初に、この実施の形態２による目標追尾装置の動作原理を説明する。目標の等速直線運動モデル、レーダ装置１の観測モデルは、実施の形態１における式（１）〜式（１７）までの原理と同じであるので省略する。
【００７３】
次に、サンプリング時刻ｔk-1までのレーダ装置１の観測ベクトルＺk-1が得られているときの予測処理について述べる。目標が等速直線運動をしていると仮定した場合のサンプリング時刻ｔkにおけるアンダバーｘkの予測ベクトルをアンダバーｘkハット（−）、予測誤差共分散行列をＰk（−）とすると、それぞれ条件付平均ベクトル、条件付共分散行列で定義され、式（１８）及び式（１９）で表される。
【００７４】
一方、目標が等加速度運動をしていると仮定した場合のサンプリング時刻ｔkにおけるアンダバーｘkの予測ベクトルをアンダバーｘ’kハットとすると、式（２０）で表すことができる。ここで、式（２１）とおく。また、目標が加速度変化運動をしていると仮定した場合の同時刻のアンダバーｘkの予測ベクトルをアンダバーｘ”k（−）とすると、式（３２）及び式（３３）で表すことができる。
【００７５】
【数１０】
【００７６】
目標の位置ベクトルがサンプリング時刻ｔkにおいて、レーダ装置１より観測される場合の目標相関処理及び平滑処理は、実施の形態１における式（２２）〜式（２７）までの原理と同じであるので省略する。
【００７７】
残差ベクトルアンダバーνkは、目標が仮定した等速直線運動モデルと異なる運動を行うほど大きくなるため、その大きさにより目標の速度変化を判定することが可能である。しかし、残差ベクトルアンダバーνkは同時にランダム性の観測雑音の影響も受ける。しかも、レーダによる観測では、観測誤差はｘｙ座標において一様ではなく、目標の位置によって大きさが異なる（レーダ装置と目標の距離が離れるほど観測誤差は大きくなる）。従って、この目標位置による観測誤差の相違の影響を排除するため、式（２８）を用いて、残差ベクトルを観測ベクトルの予測誤差共分散Ｓkで正規化する。ここで、観測ベクトルの予測誤差共分散行列Ｓkは式（２９）で与えられる。
【００７８】
次に、正規化した残差ベクトルの大きさＶkまたはその変化量から目標の速度変化または加速度変化を判定し、その結果に応じて予測処理を準最適化する方法を示す。ある特定の方向の速度変化が大きく、それ以外の方向の速度変化が小さい目標の場合、速度変化が大きい方向の速度変化のみ検出すればよい。例えば、直線道路を走行する車両は、道路に沿った方向の速度変化が大きく、それ以外の方向の速度変化は小さいため、道路や滑走路に沿った方向の残差ベクトルの大きさを判定すればよい。
【００７９】
ｊ番目の所望の方向の単位ベクトルをアンダバーｕj（ｊ＝１，２，・・・，ｎ）とすると、正規化した残差ベクトルの大きさＶkのアンダバーｕj方向の成分Ｕk,jは、内積を利用して式（３０）により得られる。ここで、θjは残差ベクトルアンダバーνkとアンダバーｕjのなす角である。そして、隣り合うサンプリングにおけるＵk,jの変化量ΔＵk,jが、予め設定されたパラメータｂに対して式（３４）を満たすときに目標が加速度変化をしたと判定し、予測処理において、式（１８）の代わりに式（３２）を用いる。ΔＵk,jが、式（３４）を満たさず、Ｕk,jのｓサンプルの平均オーババーＵk,jが、予め設定されたパラメータαに対して式（３１）を満たすときに目標が速度変化をしたと判定し、予測処理において、式（１８）の代わりに式（２０）を用いる。ここで、平均の母数ｓが大きいほど、加速度検出に遅れを生じるが、観測雑音の方向はランダムであるため、逆に観測雑音によって速度変化を誤検出する危険性が少なくなる。
【００８０】
【数１１】
【００８１】
次に、この実施の形態２による目標追尾装置の動作を具体的に説明する。なお、カルマンフィルタを目標追尾装置に通常適用する場合と同様にして平滑ベクトルの初期値は別途定まっているものとする。
【００８２】
レーダ装置１では、図２に示すように目標の距離Ｒ及び方位角Ａｚを観測し、式（９）及び式（１０）に従いｘｙ座標に変換する（ステップＳＴ１）。目標相関器２では、予測器１３から追尾対象の予測ベクトルアンダバーｘkハット（−）を入力して予測位置ベクトルをアンダバーｚk（−）を式（２２）に従い算出し、レーダ装置１から入力した観測ベクトルのうち、観測ベクトルの予測ベクトルアンダバーｚk（−）に最も近い観測ベクトルを式（２３）に従い選択する（ステップＳＴ２）。残差演算器３では、目標相関器２で選択した観測ベクトルと予測器１３で算出した予測観測ベクトルを入力し、式（２７）に残差ベクトルアンダバーνkを算出する（ステップＳＴ３）。
【００８３】
一方、予測誤差評価器４では、第２の遅延回路９から１サンプリング前の平滑誤差共分散行列Ｐk-1（＋）を入力し、予め設定された駆動雑音Ｑk-1に基づいて、予測誤差共分散行列Ｐk（−）を式（１９）に従い算出する（ステップＳＴ４）。
【００８４】
ゲイン行列算出器５では、予測誤差評価器４から入力した予測誤差共分散行列Ｐk（−）と、予め設定されたレーダ装置１の観測誤差共分散行列Ｒkとから式（２４）に従いゲイン行列Ｋkを算出する（ステップＳＴ５）。
【００８５】
平滑器６では、予測器１３の出力した予測ベクトルアンダバーｘkハット（−）を残差演算器３を経由して入力し、また、残差算出器３から残差ベクトルアンダバーνkを、ゲイン行列算出器５からゲイン行列Ｋkをそれぞれ入力し、式（２５）に従い平滑ベクトルアンダバーｘkハット（＋）を算出する（ステップＳＴ６）。
【００８６】
平滑誤差評価器７では、予測誤差評価器４から予測誤差共分散行列Ｐk（−）を、ゲイン行列算出器５からゲイン行列Ｋkをそれぞれ入力し、式（２６）に従い平滑誤差共分散行列Ｐk（＋）を算出する（ステップＳＴ７）。
【００８７】
残差演算器３が残差ベクトルアンダバーνkを算出すると、正規化演算器１０では、まず、予測器１３から予測誤差共分散行列Ｐk（−）を入力し、これと予め設定されたレーダ装置１の観測誤差共分散行列Ｒkを用いて式（２９）に従い、観測ベクトルの予測誤差共分散行列Ｓkを算出する。
【００８８】
次に、残差算出器３から残差ベクトルアンダバーνkを入力し、これを、先に算出した観測ベクトルの予測誤差共分散行列Ｓkを用いて式（２８）に従い正規化する（ステップＳＴ８）。
【００８９】
方向ベクトル内積演算器１５では、方向ベクトルデータベース１４から所望の方向の単位ベクトルアンダバーｕjを、第３の遅延回路１１から１サンプリング前の正規化後の残差Ｖk-1と残差ベクトルアンダバーνk-1をそれぞれ入力し、各単位ベクトルアンダバーｕjと正規化後の残差ベクトルの内積Ｕk,jの過去ｓサンプルの平均オーババーＵk,jを式（３０）及び式（３１）に従い演算する（ステップＳＴ９）。
【００９０】
加速度変化検出器１９では、方向ベクトル内積演算器１５から各単位ベクトルと正規化後の残差ベクトルの内積の値Ｕk,jと、その過去ｓサンプルの平均オーババーＵk,jを入力し、第４の遅延回路１８から現時刻から１サンプリング前の各単位ベクトルと正規化後の残差ベクトルの内積の値Ｕk-1,jを入力し、まず、式（３４）のΔＵk,jが予め設定されたパラメータｂに対して式（３４）を満たすか否かを判定し、満たす場合に目標が加速度変化をしたと判定する。式（３４）を満たさない場合は、オーババーＵk、jが予め設定されたパラメータαについて式（３１）を満たすか否かを判定し、満たす場合に目標の速度変化があると判定する。そして何れも満たさない場合に目標は等速直線運動をしていると判定する（ステップＳＴ１０）。
【００９１】
加速度変化運動モデル切り換え器２０では、加速度変化検出器１９で目標の加速度変化運動があると判定された場合に式（３２）の加速度変化運動モデルに、目標の加速度変化運動がないと判定され、かつ加速度運動があると判定された場合に式（２０）の等加速度運動モデルに、どちらもないと判定された場合に式（１８）の等速直線運動モデルに切り換える（ステップＳＴ１１）。予測器１３では１サンプリング前の平滑ベクトルアンダバーｘk-1ハット（＋）を第１の遅延回路８を介して入力し、加速度変化運動モデル切り換え器２０により選択された運動モデルに基づき予測ベクトルアンダバーｘkハット（−）を算出する（ステップＳＴ１２）。
以上、追尾終了になるまでこの一連の処理を繰り返す。
【００９２】
以上で明らかなように、この実施の形態２によれば、方向ベクトル内積演算器１５で所望の方向の単位ベクトルと正規化後の残差ベクトルの内積の過去ｓサンプルの平均オーババーＵk、jを算出し、加速度変化検出器１９において過去ｓサンプルの平均オーババーＵk、jのスレッショルド判定により速度変化を検出しているので、ランダム性の観測誤差の影響によって速度変化が誤って検出されるのを防ぐことができる。
【００９３】
また、目標の速度変化を検出した場合に、加速度変化運動モデル切り換え器２０により、目標の運動モデルを等加速度運動モデルに切り換える構成としているので、目標が加速度運動することにより生じる追従遅れの少ない安定した追尾が可能となる効果を奏する。
【００９４】
さらに、実施の形態２では、加速度変化検出器１９において、隣り合うサンプリングにおけるＵk,jの変化量ΔＵk,jのスレッショルド判定により加速度の変化を検出しているので、ランダム性の観測誤差の影響によって加速度変化が誤って検出されるのを防ぐことができる。
【００９５】
また、目標の加速度変化を検出した場合に、加速度変化運動モデル切り換え器２０により、目標の運動モデルを加速度変化運動モデルに切り換える構成としているので、目標が加速度変化運動することにより生じる追従遅れの少ない安定した追尾が可能となる効果を奏する。
【００９６】
実施の形態３．
図４は、この実施の形態３による目標追尾装置を示す構成図である。図４において、図１と同一符号である１〜１１及び１３は実施の形態１と同一または相当部分を示すのでその説明を省略する。
【００９７】
１８は第３の遅延回路１１から入力した１サンプリング前の正規化後の残差と残差ベクトルを１サンプリング時間だけ遅延する第４の遅延回路、２１は第３の遅延回路１１から入力した１サンプリング前の正規化後の残差と残差ベクトルと、第４の遅延回路１８から入力した２サンプリング前の正規化後の残差と残差ベクトルとから、過去２サンプルの残差ベクトルの内積を演算する隣接残差内積演算器である。
【００９８】
１６は隣接残差内積演算器２１から入力した過去２サンプルの残差ベクトルの内積の値をスレッショルド判定し、加速度運動の有無を判定する加速度検出器、１７は加速度検出器１６で目標の加速度運動があると判定された場合に等加速度運動モデルに、ないと判定された場合に等速直線運動モデルに切り換える等加速度運動モデル切り換え器、１３は第１の遅延回路８から入力した１サンプリング前の平滑ベクトルから、等加速度運動モデル切り換え器１７により選択された運動モデルに基づき予測ベクトルを算出する予測器である。
【００９９】
次に動作について説明する。最初に、この実施の形態３による目標追尾装置の動作原理を説明する。目標の等速直線運動モデル、レーダ装置１の観測モデルは、実施の形態１における式（１）から式（１７）までの原理と同じであるので省略する。
【０１００】
次に、サンプリング時刻ｔk-1までのレーダ装置１の観測ベクトルＺk-1が得られているときの予測処理は、実施の形態１における式（１８）〜式（２１）までの原理と同じであるので省略する。また、目標の位置ベクトルがサンプリング時刻ｔkにおいて、レーダ装置１より観測される場合の目標相関処理及び平滑処理は、実施の形態１における式（２２）〜式（２７）までの原理と同じであるので省略する。
【０１０１】
残差ベクトルアンダバーνkは、目標が仮定した等速直線運動モデルと異なる運動を行うほど大きくなるため、その大きさにより目標の速度変化を判定することが可能である。しかし、残差ベクトルアンダバーνkは同時にランダム性の観測雑音の影響も受ける。しかも、レーダによる観測では、観測誤差はｘｙ座標において一様ではなく、目標の位置によって大きさが異なる（レーダ装置と目標の距離が離れるほど観測誤差は大きくなる）。従って、この目標位置による観測誤差の相違の影響を排除するため、式（２８）を用いて、残差ベクトルを観測ベクトルの予測誤差共分散Ｓkで正規化する。ここで、観測ベクトルの予測誤差共分散行列Ｓkは式（２９）で与えられる。
【０１０２】
次に、正規化した残差ベクトルの大きさＶkから目標の速度変化を判定し、その結果に応じて予測処理を準最適化する方法を示す。ある特定の方向の速度変化が大きく、それ以外の方向の速度変化が小さい目標の場合、速度変化が大きい方向の速度変化のみ検出すればよい。例えば、直線道路を走行する車両は、道路に沿った方向の速度変化が大きく、それ以外の方向の速度変化は小さいため、道路や滑走路に沿った方向の残差ベクトルの大きさを判定すればよい。大きな加速度の発生する方向が限られているものの、その方向が予め得られない場合について考えると、加速度が生じたときには、ほぼ一定方向の残差が大きくなるため、連続する２サンプルの残差ベクトルの内積は大きな値となる。
【０１０３】
一方、加速度が生じていないときにはランダム誤差の影響により残差が発生するため、連続する２サンプルの残差ベクトルの内積は先程に比べて小さな値となる。そこで、式（３５）で表される連続する２サンプルの残差ベクトルの内積の大きさＧkにより、目標の速度変化を判定する。すなわち、２サンプルの残差ベクトルの内積の大きさＧkのｓサンプルの平均オーババーＧkが、予め設定されたパラメータｃに対して式（３６）を満たすときに目標が速度変化をしたと判定し、予測処理において、式（１８）の代わりに式（２０）を用いる。ここで、平均の母数ｓが大きいほど、加速度検出に遅れを生じるが、逆に観測雑音によって速度変化を誤検出する危険性が少なくなる。
【０１０４】
【数１２】
【０１０５】
次に、この実施の形態３による目標追尾装置の動作を具体的に説明する。なお、カルマンフィルタを目標追尾装置に通常適用する場合と同様にして平滑ベクトルの初期値は別途定まっているものとする。
【０１０６】
レーダ装置１では、図４に示すように目標の距離Ｒ及び方位角Ａｚを観測し、式（９）及び式（１０）に従いｘｙ座標に変換する（ステップＳＴ１）。目標相関器２では、予測器１３から追尾対象の予測ベクトルアンダバーｘkハット（−）を入力して予測位置ベクトルアンダバーｚk（−）を式（２２）に従い算出し、レーダ装置１から入力した観測ベクトルのうち、観測ベクトルの予測ベクトルアンダバーｚk（−）に最も近い観測ベクトルを式（２３）に従い選択する（ステップＳＴ２）。残差演算器３では、目標相関器２で選択した観測ベクトルと予測器１３で算出した予測観測ベクトルを入力し、式（２７）に残差ベクトルアンダバーνkを算出する（ステップＳＴ３）。
【０１０７】
一方、予測誤差評価器４では、第２の遅延回路９から１サンプリング前の平滑誤差共分散行列Ｐk-1（＋）を入力し、予め設定された駆動雑音Ｑk-1に基づいて、予測誤差共分散行列Ｐk（−）を式（１９）に従い算出する（ステップＳＴ４）。
【０１０８】
ゲイン行列算出器５では、予測誤差評価器４から入力した予測誤差共分散行列Ｐk（−）と、予め設定されたレーダ装置１の観測誤差共分散行列Ｒkとから式（２４）に従いゲイン行列Ｋkを算出する（ステップＳＴ５）。
【０１０９】
平滑器６では、予測器１３の出力した予測ベクトルアンダバーｘkハット（−）を残差演算器３を経由して入力し、また、残差算出器３から残差ベクトルアンダバーνkを、ゲイン行列算出器５からゲイン行列Ｋkをそれぞれ入力し、式（２５）に従い平滑ベクトルアンダバーｘkハット（＋）を算出する（ステップＳＴ６）。
【０１１０】
平滑誤差評価器７では、予測誤差評価器４から予測誤差共分散行列Ｐk（−）を、ゲイン行列算出器５からゲイン行列Ｋkをそれぞれ入力し、式（２６）に従い平滑誤差共分散行列Ｐk（+）を算出する（ステップＳＴ７）。
【０１１１】
残差演算器３が残差ベクトルアンダバーνkを算出すると、正規化演算器１０では、まず、予測器１３から予測誤差共分散行列Ｐk（−）を入力し、これと予め設定されたレーダ装置１の観測誤差共分散行列Ｒkを用いて式（２９）に従い、観測ベクトルの予測誤差共分散行列Ｓkを算出する。次に、残差算出器３から残差ベクトルアンダバーνkを入力し、これを、先に算出した観測ベクトルの予測誤差共分散行列Ｓkを用いて式（２８）に従い正規化する（ステップＳＴ８）。
【０１１２】
隣接残差内積演算器２１では、第３の遅延回路１１から入力した１サンプリング前の正規化後の残差Ｖk-1と残差ベクトルアンダバーνk-1と、第４の遅延回路１８から入力した２サンプリング前の正規化後の残差Ｖk-2と残差ベクトルアンダバーνk-2とから、過去２サンプルの残差ベクトルの内積Ｇkの過去ｓサンプルの平均オーババーＧkを式（３５）及び式（３６）に従い演算する（ステップＳＴ９）。
【０１１３】
加速度検出器１６では、隣接残差内積演算器２１から過去２サンプルの残差ベクトルの内積Ｇkの過去ｓサンプルの平均オーババーＧkを入力し、予め設定されたパラメータｃについて式（３６）を満たす場合にのみ目標の加速度運動があると判定する（ステップＳＴ１０）。
【０１１４】
等加速度運動モデル切り換え器１７では、加速度検出器１６で目標の加速度運動があると判定された場合に式（２０）の等加速度運動モデルに、ないと判定された場合に式（１８）の等速直線運動モデルに切り換える（ステップＳＴ１１）。予測器１３では１サンプリング前の平滑ベクトルアンダバーｘk-1ハット（＋）を第１の遅延回路８を介して入力し、等加速度運動モデル切り換え器１７により選択された運動モデルに基づき予測ベクトルアンダバーｘkハット（−）を算出する（ステップＳＴ１２）。
以上、追尾終了になるまでこの一連の処理を繰り返す。
【０１１５】
以上で明らかなように、この実施の形態３によれば、隣接残差内積演算器２１で過去２サンプルの残差ベクトルの内積Ｇkの過去ｓサンプルの平均オーババーＧkを算出し、加速度検出器１６において過去ｓサンプルの平均オーババーＧkのスレッショルド判定により速度変化を検出しているので、検出すべき加速度の方向を与えることなく、ランダム性の観測誤差の影響によって速度変化が誤って検出されるのを防ぐことができる。
【０１１６】
また、目標の速度変化を検出した場合に、等加速度運動モデル切り換え器１７により、目標の運動モデルを等加速度運動モデルに切り換える構成としているので、目標が加速度運動することにより生じる追従遅れの少ない安定した追尾が可能となる効果を奏する。
【０１１７】
実施の形態４．
図５は、この実施の形態４による目標追尾装置を示す構成図である。図５において、図１と同一符号である１〜１１及び１３は実施の形態１と同一または相当部分を示すのでその説明を省略する。
【０１１８】
１８は第３の遅延回路１１から入力した１サンプリング前の正規化後の残差と残差ベクトルを１サンプリング時間だけ遅延する第４の遅延回路、２１は第３の遅延回路１１から入力した１サンプリング前の正規化後の残差と残差ベクトルと、第４の遅延回路１８から入力した２サンプリング前の正規化後の残差と残差ベクトルとから、過去２サンプルの残差ベクトルの内積を演算する隣接残差内積演算器である。
【０１１９】
２２は隣接残差内積演算器２１の出力する過去２サンプルの残差ベクトルの内積を１サンプリング時間だけ遅延する第５の遅延回路、１９は隣接残差内積演算器２１から入力した過去２サンプルの残差ベクトルの内積の値と、第５の遅延回路２２から入力した現時刻から１サンプリング前に算出したの過去２サンプルの残差ベクトルの内積の値との差をスレッショルド判定し、各単位ベクトルの方向に対する加速度運動または加速度変化運動の有無を判定する加速度変化検出器である。
【０１２０】
２０は加速度変化検出器１９で目標の加速度変化運動があると判定された場合に加速度変化運動モデルに、加速度変化運動がないと判定されかつ加速度運動があると判定された場合に等加速度運動モデルに、どちらもないと判定された場合に等速直線運動モデルに切り換える加速度変化運動モデル切り換え器である、
【０１２１】
次に動作について説明する。最初に、この実施の形態４による目標追尾装置の動作原理を説明する。目標の等速直線運動モデル、レーダ装置１の観測モデルは、実施の形態１における式（１）〜式（１７）までの原理と同じであるので省略する。
【０１２２】
次に、サンプリング時刻ｔk-1までのレーダ装置１の観測ベクトルＺk-1が得られているときの予測処理は、実施の形態１における式（１８）〜式（２１）までの原理と同じであるので省略する。また、目標の位置ベクトルがサンプリング時刻ｔkにおいて、レーダ装置１より観測される場合の目標相関処理及び平滑処理は、実施の形態１における式（２２）〜式（２７）までの原理と同じであるので省略する。
【０１２３】
残差ベクトルアンダバーνkは、目標が仮定した等速直線運動モデルと異なる運動を行うほど大きくなるため、その大きさにより目標の速度変化を判定することが可能である。しかし、残差ベクトルアンダバーνkは同時にランダム性の観測雑音の影響も受ける。しかも、レーダによる観測では、観測誤差はｘｙ座標において一様ではなく、目標の位置によって大きさが異なる（レーダ装置と目標の距離が離れるほど観測誤差は大きくなる）。従って、この目標位置による観測誤差の相違の影響を排除するため、式（２８）を用いて、残差ベクトルを観測ベクトルの予測誤差共分散Ｓkで正規化する。ここで、観測ベクトルの予測誤差共分散行列Ｓkは式（２９）で与えられる。
【０１２４】
次に、正規化した残差ベクトルの大きさＶkから目標の速度変化を判定し、その結果に応じて予測処理を準最適化する方法を示す。ある特定の方向の速度変化が大きく、それ以外の方向の速度変化が小さい目標の場合、速度変化が大きい方向の速度変化のみ検出すればよい。例えば、直線道路を走行する車両は、道路に沿った方向の速度変化が大きく、それ以外の方向の速度変化は小さいため、道路や滑走路に沿った方向の残差ベクトルの大きさを判定すればよい。大きな加速度の発生する方向が限られているものの、その方向が予め得られない場合について考えると、加速度が生じたときには、ほぼ一定方向の残差が大きくなるため、連続する２サンプルの残差ベクトルの内積は大きな値となる。
【０１２５】
一方、加速度が生じていないときにはランダム誤差の影響により残差が発生するため、連続する２サンプルの残差ベクトルの内積は先程に比べて小さな値となる。そこで、式（３５）で表される連続する２サンプルの残差ベクトルの内積の大きさＧkにより、目標の速度変化を判定する。さらに、式（３７）で表される隣り合うサンプリングにおけるＧkの変化量ΔＧkにより、目標の加速度変化を判定する。すなわち、予め設定されたパラメータｄに対して式（３７）を満たすときに目標が加速度変化をしたと判定し、予測処理において、式（１８）の代わりに式（３２）を用いる。ΔＧkが、式（３７）を満たさず、Ｇkのｓサンプルの平均オーババーＧkが、予め設定されたパラメータｃに対して式（３６）を満たすときに目標が速度変化をしたと判定し、予測処理において、式（１８）の代わりに式（２０）を用いる。ここで、平均の母数ｓが大きいほど、加速度検出に遅れを生じるが、観測雑音の方向はランダムであるため、逆に観測雑音によって速度変化を誤検出する危険性が少なくなる。
【０１２６】
【数１３】
【０１２７】
次に、この実施の形態４による目標追尾装置の動作を具体的に説明する。なお、カルマンフィルタを目標追尾装置に通常適用する場合と同様にして平滑ベクトルの初期値は別途定まっているものとする。
【０１２８】
レーダ装置１では、図４に示すように目標の距離Ｒ及び方位角Ａｚを観測し、式（９）及び式（１０）に従いｘｙ座標に変換する（ステップＳＴ１）。目標相関器２では、予測器１３から追尾対象の予測ベクトルアンダバーｘkハット（−）を入力して予測位置ベクトルアンダバーｚk（−）を式（２２）に従い算出し、レーダ装置１から入力した観測ベクトルのうち、観測ベクトルの予測ベクトルアンダバーｚk（−）に最も近い観測ベクトルを式（２３）に従い選択する（ステップＳＴ２）。残差演算器３では、目標相関器２で選択した観測ベクトルと予測器１３で算出した予測観測ベクトルを入力し、式（２７）に残差ベクトルアンダバーνkを算出する（ステップＳＴ３）。
【０１２９】
一方、予測誤差評価器４では、第２の遅延回路９から１サンプリング前の平滑誤差共分散行列Ｐk-1（＋）を入力し、予め設定された駆動雑音Ｑk-1を入力し、予測誤差共分散行列Ｐk（−）を式（１９）に従い算出する（ステップＳＴ４）。
【０１３０】
ゲイン行列算出器５では、予測誤差評価器４から入力した予測誤差共分散行列Ｐk（−）と、予め設定されたレーダ装置１の観測誤差共分散行列Ｒkとから式（２４）に従いゲイン行列Ｋkを算出する（ステップＳＴ５）。
【０１３１】
平滑器６では、予測器１３の出力した予測ベクトルアンダバーｘkハット（−）を残差演算器３を経由して入力し、また、残差算出器３から残差ベクトルアンダバーνkを、ゲイン行列算出器５からゲイン行列Ｋkをそれぞれ入力し、式（２５）に従い平滑ベクトルアンダバーｘkハット（＋）を算出する（ステップＳＴ６）。
【０１３２】
平滑誤差評価器７では、予測誤差評価器４から予測誤差共分散行列Ｐk（−）を、ゲイン行列算出器５からゲイン行列Ｋkをそれぞれ入力し、式（２６）に従い平滑誤差共分散行列Ｐk（＋）を算出する（ステップＳＴ７）。
【０１３３】
残差演算器３が残差ベクトルアンダバーνkを算出すると、正規化演算器１０では、まず、予測器１３から予測誤差共分散行列Ｐk（−）を入力し、これと予め設定されたレーダ装置１の観測誤差共分散行列Ｒkを用いて式（２９）に従い、観測ベクトルの予測誤差共分散行列Ｓkを算出する。次に、残差算出器３から残差ベクトルアンダバーνkを入力し、これを、先に算出した観測ベクトルの予測誤差共分散行列Ｓkを用いて式（２８）に従い正規化する（ステップＳＴ８）。
【０１３４】
隣接残差内積演算器２１では、第３の遅延回路１１から入力した１サンプリング前の正規化後の残差Ｖk-1と残差ベクトルアンダバーνk-1と、第４の遅延回路１８から入力した２サンプリング前の正規化後の残差Ｖk-2と残差ベクトルアンダバーνk-2とから、過去２サンプルの残差ベクトルの内積Ｇkの過去ｓサンプルの平均オーババーＧkを式（３５）及び式（３６）に従い演算する（ステップＳＴ９）。
【０１３５】
加速度変化検出器１９では、隣接残差内積演算器２１から過去２サンプルの残差ベクトルの内積Ｇkと、その過去ｓサンプルの平均オーババーＧkを入力し、第５の遅延回路２２から現時刻から１サンプリング前に算出した過去２サンプルの残差ベクトルの内積Ｇk-1を入力し、まず、式（３７）のΔＧkが予め設定されたパラメータｄに対して式（３７）を満たすか否かを判定し、満たす場合に目標が加速度変化をしたと判定する。式（３７）を満たさない場合は、オーババーＧkが予め設定されたパラメータｃについて式（３６）を満たすか否かを判定し、満たす場合に目標の速度変化があると判定する。そして何れも満たさない場合に目標は等速直線運動をしていると判定する（ステップＳＴ１０）。
【０１３６】
加速度変化運動モデル切り換え器２０では、加速度変化検出器１９で目標の加速度変化運動があると判定された場合に式（３２）の加速度変化運動モデルに、目標の加速度変化運動がないと判定され、かつ加速度運動があると判定された場合に式（２０）の等加速度運動モデルに、どちらもないと判定された場合に式（１８）の等速直線運動モデルに切り換える（ステップＳＴ１１）。
【０１３７】
予測器１３では１サンプリング前の平滑ベクトルアンダバーｘk-1ハット（＋）を第１の遅延回路８を介して入力し、等加速度運動モデル切り換え器１７により選択された運動モデルに基づき予測ベクトルアンダバーｘkハット（−）を算出する（ステップＳＴ１２）。
以上、追尾終了になるまでこの一連の処理を繰り返す。
【０１３８】
以上で明らかなように、この実施の形態４によれば、隣接残差内積演算器２１で過去２サンプルの残差ベクトルの内積Ｇkの過去ｓサンプルの平均オーババーＧkを算出し、加速度検出器１６において過去ｓサンプルの平均オーババーＧkのスレッショルド判定により速度変化を検出しているので、検出すべき加速度の方向を与えることなく、ランダム性の観測誤差の影響によって速度変化が誤って検出されるのを防ぐことができる。
【０１３９】
また、目標の速度変化を検出した場合に、等加速度運動モデル切り換え器１７により、目標の運動モデルを等加速度運動モデルに切り換える構成としているので、目標が加速度運動することにより生じる追従遅れの少ない安定した追尾が可能となる効果を奏する。
【０１４０】
さらに、実施の形態４では、加速度変化検出器１９において、隣り合うサンプリングにおけるＧkの変化量ΔＧkのスレッショルド判定により加速度の変化を検出しているので、検出すべき加速度の方向を与えることなく、ランダム性の観測誤差の影響によって加速度変化が誤って検出されるのを防ぐことができる。
【０１４１】
また、目標の加速度変化を検出した場合に、加速度変化運動モデル切り換え器２０により、目標の運動モデルを加速度変化運動モデルに切り換える構成としているので、目標が加速度変化運動することにより生じる追従遅れの少ない安定した追尾が可能となる効果を奏する。
【０１４２】
【発明の効果】
以上のように、この発明によれば、方向ベクトル内積演算手段により所望の方向の単位ベクトルと正規化後の残差ベクトルの内積の過去ｓサンプルの平均を算出し、加速度検出手段において過去ｓサンプルの平均のスレッショルド判定により速度変化を検出しているので、ランダム性の観測誤差の影響によって速度変化が誤って検出されるのを防ぐことができる。
【０１４３】
また、目標の速度変化を検出した場合に、等加速度運動モデル切り換え手段により、目標の運動モデルを等加速度運動モデルに切り換える構成としているので、目標が加速度運動することにより生じる追従遅れの少ない安定した追尾が可能となる効果を奏する。
【０１４４】
また、加速度変化検出手段において、隣り合うサンプリングにおける残差の変化量のスレッショルド判定により加速度の変化を検出しているので、ランダム性の観測誤差の影響によって加速度変化が誤って検出されるのを防ぐことができる。
【０１４５】
また、隣接残差内積演算手段により過去２サンプルの残差ベクトルの内積の過去ｓサンプルの平均を算出し、加速度検出手段において過去ｓサンプルの平均のスレッショルド判定により速度変化を検出しているので、検出すべき加速度の方向を与えることなく、ランダム性の観測誤差の影響によって速度変化が誤って検出されるのを防ぐことができる。
【０１４６】
また、目標の速度変化を検出した場合に、等加速度運動モデル切り換え手段により、目標の運動モデルを等加速度運動モデルに切り換える構成としているので、目標が加速度運動することにより生じる追従遅れの少ない安定した追尾が可能となる効果を奏する。
【０１４７】
さらに、隣接残差内積演算手段により過去２サンプルの残差ベクトルの内積の過去ｓサンプルの平均を算出し、加速度変化検出手段において、隣り合うサンプリングにおける残差の変化量のスレッショルド判定により加速度の変化を検出し、目標の加速度変化を検出した場合に、加速度変化運動モデル切り換え手段により、目標の運動モデルを加速度変化運動モデルに切り換える構成としているので、検出すべき加速度の方向を与えることなく、ランダム性の観測誤差の影響によって加速度変化が誤って検出されるのを防ぐことができると共に、目標が加速度変化運動することにより生じる追従遅れの少ない安定した追尾が可能となる効果を奏する。
【図面の簡単な説明】
【図１】 この発明の実施の形態１による目標追尾装置を示す構成図である。
【図２】 ｚ軸上に設置されたレーダ装置１の座標と目標の距離Ｒ及び方位角Ａｚの観測に係る説明図である。
【図３】 この発明の実施の形態２による目標追尾装置を示す構成図である。
【図４】 この発明の実施の形態３による目標追尾装置を示す構成図である。
【図５】 この発明の実施の形態４による目標追尾装置を示す構成図である。
【図６】 "An Adaptive Two-Dimensional Kalman Tracking Filter", IEEE Transaction on Aerospace and Electronic Systems Vol.AES-16,No.6,1980.に示された従来の目標追尾装置を示す構成図である。
【符号の説明】
１ レーダ装置、２ 目標相関器、３ 残差算出器、４ 予測誤差評価器、５ゲイン行列算出器、６ 平滑器、７ 平滑誤差評価器、８ 第１の遅延回路、９ 第２の遅延回路、１０ 正規化演算器、１１ 第３の遅延回路、１３ 予測器、１４ 方向ベクトルデータベース、１５ 方向ベクトル内積演算器、１６ 加速度検出器、１７ 等加速度運動モデル切り換え器、１８ 第４の遅延回路、１９ 加速度変化検出器、２０ 加速度変化運動モデル切り換え器、２１ 隣接残差内積演算器、２２ 第５の遅延回路。
【発明の属する技術分野】
この発明は、レーダ、レーザ、カメラ等にて観測した、車両、航空機等の移動目標の距離、俯角（仰角）、方位角、位置、またはこれらの時間変化率等の運動諸元を推定する目標追尾装置及び方法に関するものである。
【０００２】
【従来の技術】
図６は、例えば、"An Adaptive Two-Dimensional Kalman Tracking Filter", IEEE Transaction on Aerospace and Electronic Systems Vol.AES-16,No.6,1980.に示された従来の目標追尾装置を示す構成図である。
【０００３】
図６において、１は目標の位置を観測するレーダ装置、２は予測器１３から追尾対象の予測ベクトルを入力し、レーダ装置１により観測された観測ベクトルのうち、予測ベクトルに最も近い観測ベクトルを選択する目標相関器、３は目標相関器２で選択した観測ベクトルから予測器１３で算出した予測観測ベクトルを減算する残差演算器である。
【０００４】
４は第２の遅延回路９から入力した１サンプリング前の平滑誤差共分散行列と、駆動雑音調整器１２から入力した駆動雑音から、予測誤差共分散行列を算出する予測誤差評価器、５は予測誤差評価器４から入力した予測誤差共分散行列と、予め設定されたレーダ装置１の観測誤差共分散行列とからゲイン行列を算出するゲイン行列算出器、６は残差算出器３を介して予測器１３から入力した予測ベクトルと、残差算出器３から入力した残差ベクトルと、ゲイン行列算出器５から入力したゲイン行列とから平滑ベクトルを算出する平滑器、７は予測誤差評価器４から入力した予測誤差共分散行列と、ゲイン行列算出器５から入力したゲイン行列とから平滑誤差共分散行列を算出する平滑誤差評価器、８は平滑器６から入力した平滑ベクトルを１サンプリング時間だけ遅延する第１の遅延回路、９は平滑誤差評価器７から入力した平滑誤差共分散行列を１サンプリング時間だけ遅延する第２の遅延回路である。
【０００５】
１０は残差算出器３から入力した残差ベクトルを、予測器１３から入力した予測誤差共分散行列と予め設定されたレーダ装置１の観測誤差共分散行列で正規化する正規化演算器、１１は正規化演算器１０から入力した正規化後の残差を１サンプリング時間だけ遅延する第３の遅延回路、１２は第３の遅延回路１１から入力した１サンプリング前の正規化後の残差をもとに目標の加速度運動を判定し駆動雑音の値を決定する駆動雑音調整器、１３は第１の遅延回路８から入力した１サンプリング前の平滑ベクトルから、予め定義された等速直線運動モデルに基づき予測ベクトルを算出する予測器である。
【０００６】
次に動作について説明する。目標相関器２は、レーダ装置１から観測ベクトルを受け取ると、その観測ベクトルが目標に係る観測ベクトルである可能性が高い場合にのみ有効なデータとして採用するため、予測器１３から入力した予測ベクトルに最も近い観測ベクトルを出力する。
【０００７】
残差算出器３は、目標相関器２で採用した観測ベクトルと、予測器１３で算出した予測ベクトルを入力し、これらの差ベクトルである残差ベクトルを算出し、平滑器６と正規化演算器１０にそれぞれ送出する。
【０００８】
そして、ゲイン行列算出器５は、予測誤差評価器４から入力した予測誤差共分散行列と、予め設定されたレーダ装置１の観測誤差共分散行列とからゲイン行列を算出する。平滑器６は、残差算出器３から残差ベクトルと予測ベクトルを受け取ると、ゲイン行列算出器５からゲイン行列を入力し、目標の位置及び速度の平滑ベクトルを算出する。
【０００９】
ここで、平滑誤差評価器７は、１サンプリング前において、予測誤差評価器４から入力した予測誤差共分散行列と、ゲイン行列算出器５から入力したゲイン行列とから平滑誤差共分散行列を算出し、予測誤差評価器４は第２の遅延回路９から入力した１サンプリング前の平滑誤差共分散行列と、駆動雑音調整器１２から入力した駆動雑音から、予測誤差共分散行列を算出する。
【００１０】
正規化演算器１０は、残差算出器３から残差ベクトル入力すると、予測器１３から入力した予測誤差共分散行列と予め設定されたレーダ装置１の観測誤差共分散行列の和で残差ベクトルを正規化し、第３の遅延回路１１に送出する。駆動雑音調整器１２は、第３の遅延回路から１サンプリング前の正規化後の残差を入力し、その値が予め設定された値より大きい場合にのみ、目標が加速度運動をしていると判断して駆動雑音の値を大きな値に変更し、予測誤差評価器４に送出する。
【００１１】
前記のようにして、平滑器６が目標の位置及び速度の平滑ベクトルを算出すると、予測器１３は、平滑ベクトルと予め定義された目標の運動モデルを用いて、現時刻より１サンプリング後の目標の予測位置及び速度からなる予測ベクトルを算出する。
【００１２】
【発明が解決しようとする課題】
従来の目標追尾装置は以上のように構成されているので、目標が等速直線運動をしているときでも、ランダム性の観測雑音が突発的に大きくなった場合に、正規化後の残差が大きくなって駆動雑音調整器１２が加速度運動を誤って検出してしまい、追尾性能が劣化するという課題があった。
【００１３】
この発明は前記のような課題を解決するためになされたもので、例えば直線道路を走行する車両のように、目標の運動の特徴として、ある特定の方向の速度変化が大きくそれ以外の方向の速度変化は無視できるような場合に、残差の目標の加減速方向の成分の時間平均もしくは連続するサンプルの複数残差ベクトル間の内積の時間平均を利用して特定の方向の残差成分のみを加速度検出の対象とすることで、ランダム性の観測雑音による加速度運動の誤検出の少ない目標追尾装置及び方法を得ることを目的とする。
【００１４】
【課題を解決しようとする手段】
この発明に係る目標追尾装置は、目標を観測して観測ベクトルを得る観測手段と、目標の状態ベクトルの予測ベクトルを演算する予測手段と、目標の状態ベクトルの平滑ベクトルを演算する平滑手段と、所望の方向の単位ベクトルを格納した方向ベクトルデータベースと、前記データベースから入力した所望方向の単位ベクトルと、前記観測手段により観測された観測ベクトルのうち、前記予測手段で演算された予測ベクトルに最も近い観測ベクトルから前記予測ベクトルを減算することで得られる残差ベクトルを正規化した正規化後の残差ベクトルとの内積を算出する方向ベクトル内積演算手段と、前記方向ベクトル内積演算手段により算出された内積をスレッショルド判定して各単位ベクトルの方向に対する加速度運動の有無を判定することにより、特定方向の速度変化を検出する加速度検出手段とを備えたものである。
【００１５】
また、前記加速度検出手段にて速度変化が検出された場合に前記予測手段で用いる運動モデルを速度変化を考慮した運動モデルに切り換える等加速度運動モデル切り換え手段をさらに備えたことを特徴とするものである。
【００１６】
また、他の発明に係る目標追尾装置は、目標を観測して観測ベクトルを得る観測手段と、目標の状態ベクトルの予測ベクトルを演算する予測手段と、目標の状態ベクトルの平滑ベクトルを演算する平滑手段と、所望の方向の単位ベクトルを格納した方向ベクトルデータベースと、前記データベースから入力した所望方向の単位ベクトルと、前記観測手段により観測された観測ベクトルのうち、前記予測手段で演算された予測ベクトルに最も近い観測ベクトルから前記予測ベクトルを減算することで得られる残差ベクトルを正規化した正規化後の残差ベクトルとの内積を算出する方向ベクトル内積演算手段と、前記方向ベクトル内積演算手段により算出された隣り合うサンプリングにおける内積の変化量をスレッショルド判定して特定方向の加速度変化または速度変化を検出する加速度変化検出手段と、前記加速度変化検出手段により加速度変化または速度変化が検出された場合に前記予測手段で用いる運動モデルを加速度変化または速度変化を考慮した運動モデルに切り換える加速度変化運動モデル切り換え手段とを備えたものである。
【００１７】
また、さらに他の発明に係る目標追尾装置は、目標を観測して観測ベクトルを得る観測手段と、目標の状態ベクトルの予測ベクトルを演算する予測手段と、目標の状態ベクトルの平滑ベクトルを演算する平滑手段と、前記観測手段により観測された観測ベクトルのうち、前記予測手段で演算された予測ベクトルに最も近い観測ベクトルから前記予測ベクトルを減算することで得られる残差ベクトルを観測ベクトルの予測誤差共分散行列を用いて正規化後、連続する過去２サンプルの正規化後の残差ベクトル間の内積を算出する隣接残差内積演算手段と、前記隣接残差内積演算手段により算出された内積の値によりある一定方向の速度変化を検出する加速度検出手段とを備えたものである。
【００１８】
また、前記加速度検出手段にて速度変化が検出された場合に前記予測手段で用いる運動モデルを速度変化を考慮した運動モデルに切り換える等加速度運動モデル切り換え手段をさらに備えたことを特徴とするものである。
【００１９】
また、さらに他の発明に係る目標追尾装置は、目標を観測して観測ベクトルを得る観測手段と、目標の状態ベクトルの予測ベクトルを演算する予測手段と、目標の状態ベクトルの平滑ベクトルを演算する平滑手段と、前記観測手段により観測された観測ベクトルのうち、前記予測手段で演算された予測ベクトルに最も近い観測ベクトルから前記予測ベクトルを減算することで得られる残差ベクトルを観測ベクトルの予測誤差共分散行列を用いて正規化後、連続する過去２サンプルの正規化後の残差ベクトル間の内積を算出する隣接残差内積演算手段と、前記隣接残差内積演算手段により算出された隣り合うサンプリングにおける内積の変化量をスレッショルド判定して特定方向の加速度変化または速度変化を検出する加速度変化検出手段と、前記加速度変化検出手段にて加速度変化または速度変化が検出された場合に前記予測手段で用いる運動モデルを加速度変化または速度変化を考慮した運動モデルに切り換える加速度変化運動モデル切り換え手段とを備えたものである。
【００２０】
また、この発明に係る目標追尾方法は、目標を観測して観測ベクトルを得る観測ステップと、目標の状態ベクトルの予測ベクトルを演算する予測ステップと、目標の状態ベクトルの平滑ベクトルを演算する平滑ステップと、所望の方向の単位ベクトルを格納した方向ベクトルデータベースから入力した所望方向の単位ベクトルと、前記観測ステップにより観測された観測ベクトルのうち、前記予測ステップで演算された予測ベクトルに最も近い観測ベクトルから前記予測ベクトルを減算することで得られる残差ベクトルを正規化した正規化後の残差ベクトルとの内積を算出する方向ベクトル内積演算ステップと、前記方向ベクトル内積演算ステップにより算出された内積をスレッショルド判定して各単位ベクトルの方向に対する加速度運動の有無を判定することにより、特定方向の速度変化を検出する加速度検出ステップとを備えたものである。
【００２１】
また、前記加速度検出ステップにて速度変化が検出された場合に前記予測ステップで用いる運動モデルを速度変化を考慮した運動モデルに切り換える等加速度運動モデル切り換えステップをさらに備えたことを特徴とするものである。
【００２２】
また、他の発明に係る目標追尾方法は、目標を観測して観測ベクトルを得る観測ステップと、目標の状態ベクトルの予測ベクトルを演算する予測ステップと、目標の状態ベクトルの平滑ベクトルを演算する平滑ステップと、所望の方向の単位ベクトルを格納した方向ベクトルデータベースから入力した所望方向の単位ベクトルと、前記観測ステップにより観測された観測ベクトルのうち、前記予測ステップで演算された予測ベクトルに最も近い観測ベクトルから前記予測ベクトルを減算することで得られる残差ベクトルを正規化した正規化後の残差ベクトルとの内積を算出する方向ベクトル内積演算ステップと、前記方向ベクトル内積演算ステップにより算出された隣り合うサンプリングにおける内積の変化量をスレショルド判定して特定方向の加速度変化または速度変化を検出する加速度変化検出ステップと、前記加速度変化検出ステップにて加速度変化または速度変化が検出された場合に前記予測ステップで用いる運動モデルを加速度変化または速度変化を考慮した運動モデルに切り換える加速度変化運動モデル切り換えステップとを備えたものである。
【００２３】
また、さらに他の発明に係る目標追尾方法は、目標を観測して観測ベクトルを得る観測ステップと、目標の状態ベクトルの予測ベクトルを演算する予測ステップと、目標の状態ベクトルの平滑ベクトルを演算する平滑ステップと、前記観測ステップにより観測された観測ベクトルのうち、前記予測ステップで演算された予測ベクトルに最も近い観測ベクトルから前記予測ベクトルを減算することで得られる残差ベクトルを観測ベクトルの予測誤差共分散行列を用いて正規化後、連続する過去２サンプルの正規化後の残差ベクトル間の内積を算出する隣接残差内積演算ステップと、前記隣接残差内積演算ステップにより算出された内積の値によりある一定方向速度変化を検出する加速度検出ステップとを備えたものである。
【００２４】
また、前記加速度検出ステップにて速度変化が検出された場合に前記予測ステップで用いる運動モデルを速度変化を考慮した運動モデルに切り換える等加速度運動モデル切り換えステップをさらに備えたことを特徴とするものである。
【００２５】
また、さらに他の発明に係る目標追尾方法は、目標を観測して観測ベクトルを得る観測ステップと、目標の状態ベクトルの予測ベクトルを演算する予測ステップと、目標の状態ベクトルの平滑ベクトルを演算する平滑ステップと、前記観測ステップにより観測された観測ベクトルのうち、前記予測ステップで演算された予測ベクトルに最も近い観測ベクトルから前記予測ベクトルを減算することで得られる残差ベクトルを観測ベクトルの予測誤差共分散行列を用いて正規化後、連続する過去２サンプルの正規化後の残差ベクトル間の内積を算出する隣接残差内積演算ステップと、前記隣接残差内積演算ステップにより算出された隣り合うサンプリングにおける内積の変化量をスレショルド判定して特定方向の加速度変化または速度変化を検出する加速度変化検出ステップと、前記加速度変化検出ステップにて加速度変化または速度変化が検出された場合に前記予測ステップで用いる運動モデルを加速度変化または速度変化を考慮した運動モデルに切り換える加速度変化運動モデル切り換えステップとを備えたものである。
【００２６】
【発明の実施の形態】
実施の形態１．
図１は、この実施の形態１による目標追尾装置を示す構成図である。図１において、１は目標の位置を観測するレーダ装置、２は予測器１３から追尾対象の予測ベクトルを入力し、レーダ装置１により観測された観測ベクトルのうち、予測ベクトルに最も近い観測ベクトルを選択する目標相関器、３は目標相関器２で選択した観測ベクトルから予測器１３で算出した予測観測ベクトルを減算する残差演算器である。
【００２７】
４は第２の遅延回路９から入力した１サンプリング前の平滑誤差共分散行列と、予め設定された駆動雑音から、予測誤差共分散行列を算出する予測誤差評価器、５は予測誤差評価器４から入力した予測誤差共分散行列と、予め設定されたレーダ装置１の観測誤差共分散行列とからゲイン行列を算出するゲイン行列算出器、６は残差算出器３を介して予測器１３から入力した予測ベクトルと、残差算出器３から入力した残差ベクトルと、ゲイン行列算出器５から入力したゲイン行列とから平滑ベクトルを算出する平滑器である。
【００２８】
７は予測誤差評価器４から入力した予測誤差共分散行列と、ゲイン行列算出器５から入力したゲイン行列とから平滑誤差共分散行列を算出する平滑誤差評価器、８は平滑器６から入力した平滑ベクトルを１サンプリング時間だけ遅延する第１の遅延回路、９は平滑誤差評価器７から入力した平滑誤差共分散行列を１サンプリング時間だけ遅延する第２の遅延回路である。
【００２９】
１０は残差算出器３から入力した残差ベクトルを、予測器１３から入力した予測誤差共分散行列と予め設定されたレーダ装置１の観測誤差共分散行列で正規化する正規化演算器、１１は正規化演算器１０から入力した正規化後の残差を１サンプリング時間だけ遅延する第３の遅延回路、１３は第１の遅延回路８から入力した１サンプリング前の平滑ベクトルから、等加速度運動モデル切り換え器１７により選択された運動モデルに基づき予測ベクトルを算出する予測器である。
【００３０】
１４は所望の方向の単位ベクトルが格納されている方向ベクトルデータベース、１５は方向ベクトルデータベース１４から入力した所望の方向の単位ベクトルと、第３の遅延回路１１から入力した１サンプリング前の正規化後の残差と残差ベクトルを入力し、各単位ベクトルと正規化後の残差ベクトルの内積を演算する方向ベクトル内積演算器、１６は方向ベクトル内積演算器１５から入力した各単位ベクトルと正規化後の残差ベクトルの内積の値をスレッショルド判定し、各単位ベクトルの方向に対する加速度運動の有無を判定する加速度検出器、１７は加速度検出器１６で目標の加速度運動があると判定された場合に等加速度運動モデルに、ないと判定された場合に等速直線運動モデルに切り換える等加速度運動モデル切り換え器である。
【００３１】
次に動作について説明する。最初に、この実施の形態１による目標追尾装置の動作原理を説明する。目標は２次元平面内を運動するものとする。目標の運動する平面をｘｙ平面に取った３次元直交座標を基準座標と呼ぶことにする。図２に示すように、レーダ装置１はｚ軸上に設置され、その座標を（０，０，ｚr）とする。レーダ装置１は、目標の距離Ｒ及び方位角Ａｚ（ｙ軸正の方向より時計回りを正とする）を観測する。
【００３２】
目標が等速直線運動をしていることを想定した、ｘｙ座標における等速直線運動モデルを式（１）に示す。ただし、アンダーバーｘkはサンプリング時刻ｔkにおける目標運動諸元の真値を表す状態ベクトルであり、基準座標における目標位置ベクトルを式（２）とし、同速度ベクトルを式（３）とすると、目標の状態ベクトルは式（４）で表される。なお、アンダーバーＡTはベクトルアンダーバーＡの転置ベクトルを表す。
【００３３】
【数１】
ここで、Φk-1はサンプリング時刻ｔk-1よりｔkへの状態ベクトルアンダーバーｘkの推移行列であり、式（５）で表される。また、アンダーバーｗkはサンプリング時刻ｔkにおける駆動雑音ベクトルであり、Γ1（ｋ）はサンプリング時刻ｔkにおける駆動雑音ベクトルの変換行列であり、例えば、目標の運動モデルを等速直線運動と仮定したことによる打ち切り誤差項をΓ1（ｋ−１）アンダーバーｗk-1とみれば、アンダーバーｗkは加速度ベクトル相当であり、Γ1（ｋ−１）は式（６）で表される。なお、Ｔはレーダ装置１のサンプリング間隔、Ｉは２行２列の単位行列である。
【００３５】
【数２】
また、平均を表す記号としてＥを用いると、アンダーバーｗkは平均の２次元正規分布白色雑音であり、式（７）及び（８）とする。ただし、アンダーバー０は零ベクトルであり、Ｑkはサンプリング時刻ｔkにおける駆動雑音共分散行列である。
【００３７】
【数３】
次に、目標の距離Ｒ及び方位角Ａｚがサンプリング時刻ｔkにレーダ装置１より観測される場合のｘｙ座標におけるレーダ装置１の観測モデルを式（９）とする。ここで、距離Ｒ及び方位角Ａｚは式（１０）及び式（１１）によりｘｙ座標に変換される。ここで、アンダーバーｚkはサンプリング時刻ｔkにおけるレーダ観測装置１の観測ベクトル、Ｈは観測行列で、式（１２）で表される。
【００３９】
また、アンダーバーνkはサンプリング時刻ｔkにおけるレーダ装置１の観測雑音ベクトルであり、平均アンダーバー０の２次元正規分布白色雑音で、式（１３）及び（１４）で表される。なお、Ｒkはサンプリング時刻ｔkにおけるレーダ装置１の観測雑音共分散行列である。Γ2（ｋ）は極座標よりｘｙ座標への観測雑音ベクトルの変換行列で、式（１５）で表される。ここで、式（１６）は目標の状態ベクトルアンダーバーｘkの予測ベクトルである。
【００４０】
サンプリング時刻ｔkまでに目標の追尾に用いたレーダ装置１の観測ベクトル全体をＺkとする（式（１７）を参照）。
【００４１】
【数４】
次に、サンプリング時刻ｔk-1までのレーダ装置１の観測ベクトルＺk-1が得られているときの予測処理について述べる。目標が等速直線運動をしていると仮定した場合のサンプリング時刻ｔkにおけるアンダーバーｘkの予測ベクトルをアンダーバーｘkハット（−）、予測誤差共分散行列をＰk（−）とすると、それぞれ条件付平均ベクトル、条件付共分散行列で定義され、式（１８）及び式（１９）で表される。ここで、アンダーバーｘkハット（＋）及びＰk（＋）はそれぞれ前サンプリング時刻ｔk-1の平滑ベクトル及び平滑誤差共分散行列である。これらの算出方法については後で述べる。
【００４３】
一方、目標が等加速度運動をしていると仮定した場合のサンプリング時刻ｔkにおけるアンダーバーｘkの予測ベクトルをアンダーバーｘ’kハットとすると、式（２０）で表すことができる。ここで、式（２１）とおく。
【００４４】
【数５】
次に、目標の位置ベクトルがサンプリング時刻ｔkにおいて、レーダ装置１より観測される場合の目標相関処理について述べる。サンプリング時刻ｔkにおいてレーダ装置１より観測されたｍ個の観測ベクトルをアンダーバーｚk,i（ｉ＝１，２，・・・，ｍ）とすると、目標からの観測ベクトルアンダーバーｚkは、式（２２）で与えられる予測位置ベクトルアンダーバーｚk（−）との距離が最も小さい観測ベクトル、すなわち、式（２３）を満たす観測ベクトルとする。
【００４６】
【数６】
次に、目標の位置ベクトルがサンプリング時刻ｔkにおいて、レーダ装置１により観測される場合の平滑処理について述べる。ゲイン行列Ｋk、平滑ベクトルアンダーバーｘハット（＋）及び平滑誤差共分散行列Ｐk（＋）は通常のカルマンフィルタの理論により、式（２４）〜（２６）で与えられる。ここで、アンダーバーνkは式（２７）で与えられる残差ベクトルである。
【００４８】
【数７】
残差ベクトルアンダーバーνkは、目標が仮定した等速直線運動モデルと異なる運動を行うほど大きくなるため、その大きさにより目標の速度変化を判定することが可能である。しかし、残差ベクトルアンダーバーνkは同時にランダム性の観測雑音の影響も受ける。しかも、レーダによる観測では、観測誤差はｘｙ座標において一様ではなく、目標の位置によって大きさが異なる（レーダ装置と目標の距離が離れるほど観測誤差は大きくなる）。従って、この目標位置による観測誤差の相違の影響を排除するため、式（２８）を用いて、残差ベクトルを観測ベクトルの予測誤差共分散Ｓkで正規化する。ここで、観測ベクトルの予測誤差共分散行列Ｓkは式（２９）で与えられる。
【００５０】
【数８】
次に、正規化した残差ベクトルの大きさＶkから目標の速度変化を判定し、その結果に応じて予測処理を準最適化する方法を示す。ある特定の方向の速度変化が大きく、それ以外の方向の速度変化が小さい目標の場合、速度変化が大きい方向の速度変化のみ検出すればよい。例えば、直線道路を走行する車両は、道路に沿った方向の速度変化が大きく、それ以外の方向の速度変化は小さいため、道路や滑走路に沿った方向の残差ベクトルの大きさを判定すればよい。ｊ番目の所望の方向の単位ベクトルをアンダーバーｕj（ｊ＝１，２，・・・，ｎ）とすると、正規化した残差ベクトルの大きさＶkのアンダーバーｕj方向の成分Ｕk,jは、内積を利用して式（３０）により得られる。ここで、θjは残差ベクトルアンダーバーνkとアンダーバーｕjのなす角である。
【００５２】
そして、このＵk,jのｓサンプルの平均オーババーＵk,jが、予め設定されたパラメータαに対して式（３１）を満たすときに目標が速度変化をしたと判定し、予測処理において、式（１８）の代わりに式（２０）を用いる。ここで、平均の母数ｓが大きいほど、加速度検出に遅れを生じるが、観測雑音の方向はランダムであるため、逆に観測雑音によって速度変化を誤検出する危険性が少なくなる。
【００５３】
【数９】
次に、この実施の形態１による目標追尾装置の動作を具体的に説明する。なお、カルマンフィルタを目標追尾装置に通常適用する場合と同様にして平滑ベクトルの初期値は別途定まっているものとする。
【００５５】
レーダ装置１では、図２に示すように、目標の距離Ｒ及び方位角Ａｚを観測し、式（９）及び式（１０）に従いｘｙ座標に変換する（ステップＳＴ１）。
【００５６】
目標相関器２では、予測器１３から追尾対象の予測ベクトルアンダーバーｘkハット（−）を入力して予測位置ベクトルをアンダーバーｚkハット（−）式（２２）に従い算出し、レーダ装置１から入力した観測ベクトルのうち、観測ベクトルの予測ベクトルアンダーバーｚk（−）に最も近い観測ベクトルを式（２３）に従い選択する（ステップＳＴ２）。
【００５７】
残差演算器３では、目標相関器２で選択した観測ベクトルと予測器１３で算出した予測観測ベクトルを入力し、式（２７）に残差ベクトルアンダーバーνkを算出する（ステップＳＴ３）。
【００５８】
一方、予測誤差評価器４では、第２の遅延回路９から１サンプリング前の平滑誤差共分散行列Ｐk-1（＋）を入力し、予め設定された駆動雑音Ｑk-1に基づい、予測誤差共分散行列Ｐk（−）を式（１９）に従い算出する（ステップＳＴ４）。
【００５９】
ゲイン行列算出器５では、予測誤差評価器４から入力した予測誤差共分散行列Ｐk（−）と、予め設定されたレーダ装置１の観測誤差共分散行列Ｒkとから式（２４）に従いゲイン行列Ｋkを算出する（ステップＳＴ５）。
【００６０】
平滑器６では、予測器１３の出力した予測ベクトルアンダーバーｘkハット（−）を残差演算器３を経由して入力し、また、残差算出器３から残差ベクトルアンダーバーνkを、ゲイン行列算出器５からゲイン行列Ｋkをそれぞれ入力し、式（２５）に従い平滑ベクトルアンダーバーｘkハット（＋）を算出する（ステップＳＴ６）。
【００６１】
平滑誤差評価器７では、予測誤差評価器４から予測誤差共分散行列Ｐk（−）を、ゲイン行列算出器５からゲイン行列Ｋkをそれぞれ入力し、式（２６）に従い平滑誤差共分散行列Ｐk（＋）を算出する（ステップＳＴ７）。
【００６２】
残差演算器３が残差ベクトルアンダーバーνkを算出すると、正規化演算器１０では、まず、予測器１３から予測誤差共分散行列Ｐk（−）を入力し、これと予め設定されたレーダ装置１の観測誤差共分散行列Ｒkを用いて式（２９）に従い、観測ベクトルの予測誤差共分散行列Ｓkを算出する。次に、残差算出器３から残差ベクトルアンダバーνkを入力し、これを、先に算出した観測ベクトルの予測誤差共分散行列Ｓkを用いて式（２８）に従い正規化する（ステップＳＴ８）。
【００６３】
方向ベクトル内積演算器１５では、方向ベクトルデータベース１４から所望の方向の単位ベクトルアンダバーｕjを、第３の遅延回路１１から１サンプリング前の正規化後の残差Ｖk-1と残差ベクトルアンダバーνk-1をそれぞれ入力し、各単位ベクトルアンダバーｕjと正規化後の残差ベクトルの内積Ｕk,jの過去ｓサンプルの平均オーババーＵk,jを式（３０）及び式（３１）に従い演算する（ステップＳＴ９）。
【００６４】
加速度検出器１６では、方向ベクトル内積演算器１５から各単位ベクトルと正規化後の残差ベクトルの内積の過去ｓサンプルの平均オーババーＵk,jを入力し、予め設定されたパラメータαについて式（３１）を満たす場合にのみ目標の加速度運動があると判定する（ステップＳＴ９）。
【００６５】
等加速度運動モデル切り換え器１７では、加速度検出器１６で目標の加速度運動があると判定された場合に式（２０）の等加速度運動モデルに、ないと判定された場合に式（１８）の等速直線運動モデルに切り換える（ステップＳＴ１０）。
【００６６】
予測器１３では１サンプリング前の平滑ベクトルアンダバーｘkハット（＋）を第１の遅延回路８を介して入力し、等加速度運動モデル切り換え器１７により選択された運動モデルに基づき予測ベクトルアンダバーｘkハット（−）を算出する（ステップＳＴ１１）。
以上、追尾終了になるまでこの一連の処理を繰り返す。
【００６７】
以上で明らかなように、この実施の形態１によれば、方向ベクトル内積演算器１５で所望の方向の単位ベクトルと正規化後の残差ベクトルの内積の過去ｓサンプルの平均オーババーＵk,jを算出し、加速度検出器１６において過去ｓサンプルの平均オーババーＵk,jのスレッショルド判定により速度変化を検出しているので、ランダム性の観測誤差の影響によって速度変化が誤って検出されるのを防ぐことができる。
【００６８】
また、目標の速度変化を検出した場合に、等加速度運動モデル切り換え器１７により、目標の運動モデルを等加速度運動モデルに切り換える構成としているので、目標が加速度運動することにより生じる追従遅れの少ない安定した追尾が可能となる効果を奏する。
【００６９】
実施の形態２．
図３は、この実施の形態２による目標追尾装置を示す構成図である。図３において、図１と同一符号である１〜１５は実施の形態１と同一または相当部分を示すのでその説明を省略する。
【００７０】
１８は方向ベクトル内積演算器１５から入力した各単位ベクトルと正規化後の残差ベクトルの内積の値を１サンプリング時間だけ遅延する第４の遅延回路、１９は方向ベクトル内積演算器１５から入力した各単位ベクトルと正規化後の残差ベクトルの内積の値と、第４の遅延回路１８から入力した現時刻から１サンプリング前の各単位ベクトルと正規化後の残差ベクトルの内積の値との差をスレッショルド判定し、各単位ベクトルの方向に対する加速度運動または加速度変化運動の有無を判定する加速度変化検出器である。
【００７１】
２０は加速度変化検出器１９で目標の加速度変化運動があると判定された場合に加速度変化運動モデルに、加速度変化運動がないと判定されかつ加速度運動があると判定された場合に等加速度運動モデルに、どちらもないと判定された場合に等速直線運動モデルに切り換える加速度変化運動モデル切り換え器である。
【００７２】
次に動作について説明する。最初に、この実施の形態２による目標追尾装置の動作原理を説明する。目標の等速直線運動モデル、レーダ装置１の観測モデルは、実施の形態１における式（１）〜式（１７）までの原理と同じであるので省略する。
【００７３】
次に、サンプリング時刻ｔk-1までのレーダ装置１の観測ベクトルＺk-1が得られているときの予測処理について述べる。目標が等速直線運動をしていると仮定した場合のサンプリング時刻ｔkにおけるアンダバーｘkの予測ベクトルをアンダバーｘkハット（−）、予測誤差共分散行列をＰk（−）とすると、それぞれ条件付平均ベクトル、条件付共分散行列で定義され、式（１８）及び式（１９）で表される。
【００７４】
一方、目標が等加速度運動をしていると仮定した場合のサンプリング時刻ｔkにおけるアンダバーｘkの予測ベクトルをアンダバーｘ’kハットとすると、式（２０）で表すことができる。ここで、式（２１）とおく。また、目標が加速度変化運動をしていると仮定した場合の同時刻のアンダバーｘkの予測ベクトルをアンダバーｘ”k（−）とすると、式（３２）及び式（３３）で表すことができる。
【００７５】
【数１０】
目標の位置ベクトルがサンプリング時刻ｔkにおいて、レーダ装置１より観測される場合の目標相関処理及び平滑処理は、実施の形態１における式（２２）〜式（２７）までの原理と同じであるので省略する。
【００７７】
残差ベクトルアンダバーνkは、目標が仮定した等速直線運動モデルと異なる運動を行うほど大きくなるため、その大きさにより目標の速度変化を判定することが可能である。しかし、残差ベクトルアンダバーνkは同時にランダム性の観測雑音の影響も受ける。しかも、レーダによる観測では、観測誤差はｘｙ座標において一様ではなく、目標の位置によって大きさが異なる（レーダ装置と目標の距離が離れるほど観測誤差は大きくなる）。従って、この目標位置による観測誤差の相違の影響を排除するため、式（２８）を用いて、残差ベクトルを観測ベクトルの予測誤差共分散Ｓkで正規化する。ここで、観測ベクトルの予測誤差共分散行列Ｓkは式（２９）で与えられる。
【００７８】
次に、正規化した残差ベクトルの大きさＶkまたはその変化量から目標の速度変化または加速度変化を判定し、その結果に応じて予測処理を準最適化する方法を示す。ある特定の方向の速度変化が大きく、それ以外の方向の速度変化が小さい目標の場合、速度変化が大きい方向の速度変化のみ検出すればよい。例えば、直線道路を走行する車両は、道路に沿った方向の速度変化が大きく、それ以外の方向の速度変化は小さいため、道路や滑走路に沿った方向の残差ベクトルの大きさを判定すればよい。
【００７９】
ｊ番目の所望の方向の単位ベクトルをアンダバーｕj（ｊ＝１，２，・・・，ｎ）とすると、正規化した残差ベクトルの大きさＶkのアンダバーｕj方向の成分Ｕk,jは、内積を利用して式（３０）により得られる。ここで、θjは残差ベクトルアンダバーνkとアンダバーｕjのなす角である。そして、隣り合うサンプリングにおけるＵk,jの変化量ΔＵk,jが、予め設定されたパラメータｂに対して式（３４）を満たすときに目標が加速度変化をしたと判定し、予測処理において、式（１８）の代わりに式（３２）を用いる。ΔＵk,jが、式（３４）を満たさず、Ｕk,jのｓサンプルの平均オーババーＵk,jが、予め設定されたパラメータαに対して式（３１）を満たすときに目標が速度変化をしたと判定し、予測処理において、式（１８）の代わりに式（２０）を用いる。ここで、平均の母数ｓが大きいほど、加速度検出に遅れを生じるが、観測雑音の方向はランダムであるため、逆に観測雑音によって速度変化を誤検出する危険性が少なくなる。
【００８０】
【数１１】
次に、この実施の形態２による目標追尾装置の動作を具体的に説明する。なお、カルマンフィルタを目標追尾装置に通常適用する場合と同様にして平滑ベクトルの初期値は別途定まっているものとする。
【００８２】
レーダ装置１では、図２に示すように目標の距離Ｒ及び方位角Ａｚを観測し、式（９）及び式（１０）に従いｘｙ座標に変換する（ステップＳＴ１）。目標相関器２では、予測器１３から追尾対象の予測ベクトルアンダバーｘkハット（−）を入力して予測位置ベクトルをアンダバーｚk（−）を式（２２）に従い算出し、レーダ装置１から入力した観測ベクトルのうち、観測ベクトルの予測ベクトルアンダバーｚk（−）に最も近い観測ベクトルを式（２３）に従い選択する（ステップＳＴ２）。残差演算器３では、目標相関器２で選択した観測ベクトルと予測器１３で算出した予測観測ベクトルを入力し、式（２７）に残差ベクトルアンダバーνkを算出する（ステップＳＴ３）。
【００８３】
一方、予測誤差評価器４では、第２の遅延回路９から１サンプリング前の平滑誤差共分散行列Ｐk-1（＋）を入力し、予め設定された駆動雑音Ｑk-1に基づいて、予測誤差共分散行列Ｐk（−）を式（１９）に従い算出する（ステップＳＴ４）。
【００８４】
ゲイン行列算出器５では、予測誤差評価器４から入力した予測誤差共分散行列Ｐk（−）と、予め設定されたレーダ装置１の観測誤差共分散行列Ｒkとから式（２４）に従いゲイン行列Ｋkを算出する（ステップＳＴ５）。
【００８５】
平滑器６では、予測器１３の出力した予測ベクトルアンダバーｘkハット（−）を残差演算器３を経由して入力し、また、残差算出器３から残差ベクトルアンダバーνkを、ゲイン行列算出器５からゲイン行列Ｋkをそれぞれ入力し、式（２５）に従い平滑ベクトルアンダバーｘkハット（＋）を算出する（ステップＳＴ６）。
【００８６】
平滑誤差評価器７では、予測誤差評価器４から予測誤差共分散行列Ｐk（−）を、ゲイン行列算出器５からゲイン行列Ｋkをそれぞれ入力し、式（２６）に従い平滑誤差共分散行列Ｐk（＋）を算出する（ステップＳＴ７）。
【００８７】
残差演算器３が残差ベクトルアンダバーνkを算出すると、正規化演算器１０では、まず、予測器１３から予測誤差共分散行列Ｐk（−）を入力し、これと予め設定されたレーダ装置１の観測誤差共分散行列Ｒkを用いて式（２９）に従い、観測ベクトルの予測誤差共分散行列Ｓkを算出する。
【００８８】
次に、残差算出器３から残差ベクトルアンダバーνkを入力し、これを、先に算出した観測ベクトルの予測誤差共分散行列Ｓkを用いて式（２８）に従い正規化する（ステップＳＴ８）。
【００８９】
方向ベクトル内積演算器１５では、方向ベクトルデータベース１４から所望の方向の単位ベクトルアンダバーｕjを、第３の遅延回路１１から１サンプリング前の正規化後の残差Ｖk-1と残差ベクトルアンダバーνk-1をそれぞれ入力し、各単位ベクトルアンダバーｕjと正規化後の残差ベクトルの内積Ｕk,jの過去ｓサンプルの平均オーババーＵk,jを式（３０）及び式（３１）に従い演算する（ステップＳＴ９）。
【００９０】
加速度変化検出器１９では、方向ベクトル内積演算器１５から各単位ベクトルと正規化後の残差ベクトルの内積の値Ｕk,jと、その過去ｓサンプルの平均オーババーＵk,jを入力し、第４の遅延回路１８から現時刻から１サンプリング前の各単位ベクトルと正規化後の残差ベクトルの内積の値Ｕk-1,jを入力し、まず、式（３４）のΔＵk,jが予め設定されたパラメータｂに対して式（３４）を満たすか否かを判定し、満たす場合に目標が加速度変化をしたと判定する。式（３４）を満たさない場合は、オーババーＵk、jが予め設定されたパラメータαについて式（３１）を満たすか否かを判定し、満たす場合に目標の速度変化があると判定する。そして何れも満たさない場合に目標は等速直線運動をしていると判定する（ステップＳＴ１０）。
【００９１】
加速度変化運動モデル切り換え器２０では、加速度変化検出器１９で目標の加速度変化運動があると判定された場合に式（３２）の加速度変化運動モデルに、目標の加速度変化運動がないと判定され、かつ加速度運動があると判定された場合に式（２０）の等加速度運動モデルに、どちらもないと判定された場合に式（１８）の等速直線運動モデルに切り換える（ステップＳＴ１１）。予測器１３では１サンプリング前の平滑ベクトルアンダバーｘk-1ハット（＋）を第１の遅延回路８を介して入力し、加速度変化運動モデル切り換え器２０により選択された運動モデルに基づき予測ベクトルアンダバーｘkハット（−）を算出する（ステップＳＴ１２）。
以上、追尾終了になるまでこの一連の処理を繰り返す。
【００９２】
以上で明らかなように、この実施の形態２によれば、方向ベクトル内積演算器１５で所望の方向の単位ベクトルと正規化後の残差ベクトルの内積の過去ｓサンプルの平均オーババーＵk、jを算出し、加速度変化検出器１９において過去ｓサンプルの平均オーババーＵk、jのスレッショルド判定により速度変化を検出しているので、ランダム性の観測誤差の影響によって速度変化が誤って検出されるのを防ぐことができる。
【００９３】
また、目標の速度変化を検出した場合に、加速度変化運動モデル切り換え器２０により、目標の運動モデルを等加速度運動モデルに切り換える構成としているので、目標が加速度運動することにより生じる追従遅れの少ない安定した追尾が可能となる効果を奏する。
【００９４】
さらに、実施の形態２では、加速度変化検出器１９において、隣り合うサンプリングにおけるＵk,jの変化量ΔＵk,jのスレッショルド判定により加速度の変化を検出しているので、ランダム性の観測誤差の影響によって加速度変化が誤って検出されるのを防ぐことができる。
【００９５】
また、目標の加速度変化を検出した場合に、加速度変化運動モデル切り換え器２０により、目標の運動モデルを加速度変化運動モデルに切り換える構成としているので、目標が加速度変化運動することにより生じる追従遅れの少ない安定した追尾が可能となる効果を奏する。
【００９６】
実施の形態３．
図４は、この実施の形態３による目標追尾装置を示す構成図である。図４において、図１と同一符号である１〜１１及び１３は実施の形態１と同一または相当部分を示すのでその説明を省略する。
【００９７】
１８は第３の遅延回路１１から入力した１サンプリング前の正規化後の残差と残差ベクトルを１サンプリング時間だけ遅延する第４の遅延回路、２１は第３の遅延回路１１から入力した１サンプリング前の正規化後の残差と残差ベクトルと、第４の遅延回路１８から入力した２サンプリング前の正規化後の残差と残差ベクトルとから、過去２サンプルの残差ベクトルの内積を演算する隣接残差内積演算器である。
【００９８】
１６は隣接残差内積演算器２１から入力した過去２サンプルの残差ベクトルの内積の値をスレッショルド判定し、加速度運動の有無を判定する加速度検出器、１７は加速度検出器１６で目標の加速度運動があると判定された場合に等加速度運動モデルに、ないと判定された場合に等速直線運動モデルに切り換える等加速度運動モデル切り換え器、１３は第１の遅延回路８から入力した１サンプリング前の平滑ベクトルから、等加速度運動モデル切り換え器１７により選択された運動モデルに基づき予測ベクトルを算出する予測器である。
【００９９】
次に動作について説明する。最初に、この実施の形態３による目標追尾装置の動作原理を説明する。目標の等速直線運動モデル、レーダ装置１の観測モデルは、実施の形態１における式（１）から式（１７）までの原理と同じであるので省略する。
【０１００】
次に、サンプリング時刻ｔk-1までのレーダ装置１の観測ベクトルＺk-1が得られているときの予測処理は、実施の形態１における式（１８）〜式（２１）までの原理と同じであるので省略する。また、目標の位置ベクトルがサンプリング時刻ｔkにおいて、レーダ装置１より観測される場合の目標相関処理及び平滑処理は、実施の形態１における式（２２）〜式（２７）までの原理と同じであるので省略する。
【０１０１】
残差ベクトルアンダバーνkは、目標が仮定した等速直線運動モデルと異なる運動を行うほど大きくなるため、その大きさにより目標の速度変化を判定することが可能である。しかし、残差ベクトルアンダバーνkは同時にランダム性の観測雑音の影響も受ける。しかも、レーダによる観測では、観測誤差はｘｙ座標において一様ではなく、目標の位置によって大きさが異なる（レーダ装置と目標の距離が離れるほど観測誤差は大きくなる）。従って、この目標位置による観測誤差の相違の影響を排除するため、式（２８）を用いて、残差ベクトルを観測ベクトルの予測誤差共分散Ｓkで正規化する。ここで、観測ベクトルの予測誤差共分散行列Ｓkは式（２９）で与えられる。
【０１０２】
次に、正規化した残差ベクトルの大きさＶkから目標の速度変化を判定し、その結果に応じて予測処理を準最適化する方法を示す。ある特定の方向の速度変化が大きく、それ以外の方向の速度変化が小さい目標の場合、速度変化が大きい方向の速度変化のみ検出すればよい。例えば、直線道路を走行する車両は、道路に沿った方向の速度変化が大きく、それ以外の方向の速度変化は小さいため、道路や滑走路に沿った方向の残差ベクトルの大きさを判定すればよい。大きな加速度の発生する方向が限られているものの、その方向が予め得られない場合について考えると、加速度が生じたときには、ほぼ一定方向の残差が大きくなるため、連続する２サンプルの残差ベクトルの内積は大きな値となる。
【０１０３】
一方、加速度が生じていないときにはランダム誤差の影響により残差が発生するため、連続する２サンプルの残差ベクトルの内積は先程に比べて小さな値となる。そこで、式（３５）で表される連続する２サンプルの残差ベクトルの内積の大きさＧkにより、目標の速度変化を判定する。すなわち、２サンプルの残差ベクトルの内積の大きさＧkのｓサンプルの平均オーババーＧkが、予め設定されたパラメータｃに対して式（３６）を満たすときに目標が速度変化をしたと判定し、予測処理において、式（１８）の代わりに式（２０）を用いる。ここで、平均の母数ｓが大きいほど、加速度検出に遅れを生じるが、逆に観測雑音によって速度変化を誤検出する危険性が少なくなる。
【０１０４】
【数１２】
次に、この実施の形態３による目標追尾装置の動作を具体的に説明する。なお、カルマンフィルタを目標追尾装置に通常適用する場合と同様にして平滑ベクトルの初期値は別途定まっているものとする。
【０１０６】
レーダ装置１では、図４に示すように目標の距離Ｒ及び方位角Ａｚを観測し、式（９）及び式（１０）に従いｘｙ座標に変換する（ステップＳＴ１）。目標相関器２では、予測器１３から追尾対象の予測ベクトルアンダバーｘkハット（−）を入力して予測位置ベクトルアンダバーｚk（−）を式（２２）に従い算出し、レーダ装置１から入力した観測ベクトルのうち、観測ベクトルの予測ベクトルアンダバーｚk（−）に最も近い観測ベクトルを式（２３）に従い選択する（ステップＳＴ２）。残差演算器３では、目標相関器２で選択した観測ベクトルと予測器１３で算出した予測観測ベクトルを入力し、式（２７）に残差ベクトルアンダバーνkを算出する（ステップＳＴ３）。
【０１０７】
一方、予測誤差評価器４では、第２の遅延回路９から１サンプリング前の平滑誤差共分散行列Ｐk-1（＋）を入力し、予め設定された駆動雑音Ｑk-1に基づいて、予測誤差共分散行列Ｐk（−）を式（１９）に従い算出する（ステップＳＴ４）。
【０１０８】
ゲイン行列算出器５では、予測誤差評価器４から入力した予測誤差共分散行列Ｐk（−）と、予め設定されたレーダ装置１の観測誤差共分散行列Ｒkとから式（２４）に従いゲイン行列Ｋkを算出する（ステップＳＴ５）。
【０１０９】
平滑器６では、予測器１３の出力した予測ベクトルアンダバーｘkハット（−）を残差演算器３を経由して入力し、また、残差算出器３から残差ベクトルアンダバーνkを、ゲイン行列算出器５からゲイン行列Ｋkをそれぞれ入力し、式（２５）に従い平滑ベクトルアンダバーｘkハット（＋）を算出する（ステップＳＴ６）。
【０１１０】
平滑誤差評価器７では、予測誤差評価器４から予測誤差共分散行列Ｐk（−）を、ゲイン行列算出器５からゲイン行列Ｋkをそれぞれ入力し、式（２６）に従い平滑誤差共分散行列Ｐk（+）を算出する（ステップＳＴ７）。
【０１１１】
残差演算器３が残差ベクトルアンダバーνkを算出すると、正規化演算器１０では、まず、予測器１３から予測誤差共分散行列Ｐk（−）を入力し、これと予め設定されたレーダ装置１の観測誤差共分散行列Ｒkを用いて式（２９）に従い、観測ベクトルの予測誤差共分散行列Ｓkを算出する。次に、残差算出器３から残差ベクトルアンダバーνkを入力し、これを、先に算出した観測ベクトルの予測誤差共分散行列Ｓkを用いて式（２８）に従い正規化する（ステップＳＴ８）。
【０１１２】
隣接残差内積演算器２１では、第３の遅延回路１１から入力した１サンプリング前の正規化後の残差Ｖk-1と残差ベクトルアンダバーνk-1と、第４の遅延回路１８から入力した２サンプリング前の正規化後の残差Ｖk-2と残差ベクトルアンダバーνk-2とから、過去２サンプルの残差ベクトルの内積Ｇkの過去ｓサンプルの平均オーババーＧkを式（３５）及び式（３６）に従い演算する（ステップＳＴ９）。
【０１１３】
加速度検出器１６では、隣接残差内積演算器２１から過去２サンプルの残差ベクトルの内積Ｇkの過去ｓサンプルの平均オーババーＧkを入力し、予め設定されたパラメータｃについて式（３６）を満たす場合にのみ目標の加速度運動があると判定する（ステップＳＴ１０）。
【０１１４】
等加速度運動モデル切り換え器１７では、加速度検出器１６で目標の加速度運動があると判定された場合に式（２０）の等加速度運動モデルに、ないと判定された場合に式（１８）の等速直線運動モデルに切り換える（ステップＳＴ１１）。予測器１３では１サンプリング前の平滑ベクトルアンダバーｘk-1ハット（＋）を第１の遅延回路８を介して入力し、等加速度運動モデル切り換え器１７により選択された運動モデルに基づき予測ベクトルアンダバーｘkハット（−）を算出する（ステップＳＴ１２）。
以上、追尾終了になるまでこの一連の処理を繰り返す。
【０１１５】
以上で明らかなように、この実施の形態３によれば、隣接残差内積演算器２１で過去２サンプルの残差ベクトルの内積Ｇkの過去ｓサンプルの平均オーババーＧkを算出し、加速度検出器１６において過去ｓサンプルの平均オーババーＧkのスレッショルド判定により速度変化を検出しているので、検出すべき加速度の方向を与えることなく、ランダム性の観測誤差の影響によって速度変化が誤って検出されるのを防ぐことができる。
【０１１６】
また、目標の速度変化を検出した場合に、等加速度運動モデル切り換え器１７により、目標の運動モデルを等加速度運動モデルに切り換える構成としているので、目標が加速度運動することにより生じる追従遅れの少ない安定した追尾が可能となる効果を奏する。
【０１１７】
実施の形態４．
図５は、この実施の形態４による目標追尾装置を示す構成図である。図５において、図１と同一符号である１〜１１及び１３は実施の形態１と同一または相当部分を示すのでその説明を省略する。
【０１１８】
１８は第３の遅延回路１１から入力した１サンプリング前の正規化後の残差と残差ベクトルを１サンプリング時間だけ遅延する第４の遅延回路、２１は第３の遅延回路１１から入力した１サンプリング前の正規化後の残差と残差ベクトルと、第４の遅延回路１８から入力した２サンプリング前の正規化後の残差と残差ベクトルとから、過去２サンプルの残差ベクトルの内積を演算する隣接残差内積演算器である。
【０１１９】
２２は隣接残差内積演算器２１の出力する過去２サンプルの残差ベクトルの内積を１サンプリング時間だけ遅延する第５の遅延回路、１９は隣接残差内積演算器２１から入力した過去２サンプルの残差ベクトルの内積の値と、第５の遅延回路２２から入力した現時刻から１サンプリング前に算出したの過去２サンプルの残差ベクトルの内積の値との差をスレッショルド判定し、各単位ベクトルの方向に対する加速度運動または加速度変化運動の有無を判定する加速度変化検出器である。
【０１２０】
２０は加速度変化検出器１９で目標の加速度変化運動があると判定された場合に加速度変化運動モデルに、加速度変化運動がないと判定されかつ加速度運動があると判定された場合に等加速度運動モデルに、どちらもないと判定された場合に等速直線運動モデルに切り換える加速度変化運動モデル切り換え器である、
【０１２１】
次に動作について説明する。最初に、この実施の形態４による目標追尾装置の動作原理を説明する。目標の等速直線運動モデル、レーダ装置１の観測モデルは、実施の形態１における式（１）〜式（１７）までの原理と同じであるので省略する。
【０１２２】
次に、サンプリング時刻ｔk-1までのレーダ装置１の観測ベクトルＺk-1が得られているときの予測処理は、実施の形態１における式（１８）〜式（２１）までの原理と同じであるので省略する。また、目標の位置ベクトルがサンプリング時刻ｔkにおいて、レーダ装置１より観測される場合の目標相関処理及び平滑処理は、実施の形態１における式（２２）〜式（２７）までの原理と同じであるので省略する。
【０１２３】
残差ベクトルアンダバーνkは、目標が仮定した等速直線運動モデルと異なる運動を行うほど大きくなるため、その大きさにより目標の速度変化を判定することが可能である。しかし、残差ベクトルアンダバーνkは同時にランダム性の観測雑音の影響も受ける。しかも、レーダによる観測では、観測誤差はｘｙ座標において一様ではなく、目標の位置によって大きさが異なる（レーダ装置と目標の距離が離れるほど観測誤差は大きくなる）。従って、この目標位置による観測誤差の相違の影響を排除するため、式（２８）を用いて、残差ベクトルを観測ベクトルの予測誤差共分散Ｓkで正規化する。ここで、観測ベクトルの予測誤差共分散行列Ｓkは式（２９）で与えられる。
【０１２４】
次に、正規化した残差ベクトルの大きさＶkから目標の速度変化を判定し、その結果に応じて予測処理を準最適化する方法を示す。ある特定の方向の速度変化が大きく、それ以外の方向の速度変化が小さい目標の場合、速度変化が大きい方向の速度変化のみ検出すればよい。例えば、直線道路を走行する車両は、道路に沿った方向の速度変化が大きく、それ以外の方向の速度変化は小さいため、道路や滑走路に沿った方向の残差ベクトルの大きさを判定すればよい。大きな加速度の発生する方向が限られているものの、その方向が予め得られない場合について考えると、加速度が生じたときには、ほぼ一定方向の残差が大きくなるため、連続する２サンプルの残差ベクトルの内積は大きな値となる。
【０１２５】
一方、加速度が生じていないときにはランダム誤差の影響により残差が発生するため、連続する２サンプルの残差ベクトルの内積は先程に比べて小さな値となる。そこで、式（３５）で表される連続する２サンプルの残差ベクトルの内積の大きさＧkにより、目標の速度変化を判定する。さらに、式（３７）で表される隣り合うサンプリングにおけるＧkの変化量ΔＧkにより、目標の加速度変化を判定する。すなわち、予め設定されたパラメータｄに対して式（３７）を満たすときに目標が加速度変化をしたと判定し、予測処理において、式（１８）の代わりに式（３２）を用いる。ΔＧkが、式（３７）を満たさず、Ｇkのｓサンプルの平均オーババーＧkが、予め設定されたパラメータｃに対して式（３６）を満たすときに目標が速度変化をしたと判定し、予測処理において、式（１８）の代わりに式（２０）を用いる。ここで、平均の母数ｓが大きいほど、加速度検出に遅れを生じるが、観測雑音の方向はランダムであるため、逆に観測雑音によって速度変化を誤検出する危険性が少なくなる。
【０１２６】
【数１３】
次に、この実施の形態４による目標追尾装置の動作を具体的に説明する。なお、カルマンフィルタを目標追尾装置に通常適用する場合と同様にして平滑ベクトルの初期値は別途定まっているものとする。
【０１２８】
レーダ装置１では、図４に示すように目標の距離Ｒ及び方位角Ａｚを観測し、式（９）及び式（１０）に従いｘｙ座標に変換する（ステップＳＴ１）。目標相関器２では、予測器１３から追尾対象の予測ベクトルアンダバーｘkハット（−）を入力して予測位置ベクトルアンダバーｚk（−）を式（２２）に従い算出し、レーダ装置１から入力した観測ベクトルのうち、観測ベクトルの予測ベクトルアンダバーｚk（−）に最も近い観測ベクトルを式（２３）に従い選択する（ステップＳＴ２）。残差演算器３では、目標相関器２で選択した観測ベクトルと予測器１３で算出した予測観測ベクトルを入力し、式（２７）に残差ベクトルアンダバーνkを算出する（ステップＳＴ３）。
【０１２９】
一方、予測誤差評価器４では、第２の遅延回路９から１サンプリング前の平滑誤差共分散行列Ｐk-1（＋）を入力し、予め設定された駆動雑音Ｑk-1を入力し、予測誤差共分散行列Ｐk（−）を式（１９）に従い算出する（ステップＳＴ４）。
【０１３０】
ゲイン行列算出器５では、予測誤差評価器４から入力した予測誤差共分散行列Ｐk（−）と、予め設定されたレーダ装置１の観測誤差共分散行列Ｒkとから式（２４）に従いゲイン行列Ｋkを算出する（ステップＳＴ５）。
【０１３１】
平滑器６では、予測器１３の出力した予測ベクトルアンダバーｘkハット（−）を残差演算器３を経由して入力し、また、残差算出器３から残差ベクトルアンダバーνkを、ゲイン行列算出器５からゲイン行列Ｋkをそれぞれ入力し、式（２５）に従い平滑ベクトルアンダバーｘkハット（＋）を算出する（ステップＳＴ６）。
【０１３２】
平滑誤差評価器７では、予測誤差評価器４から予測誤差共分散行列Ｐk（−）を、ゲイン行列算出器５からゲイン行列Ｋkをそれぞれ入力し、式（２６）に従い平滑誤差共分散行列Ｐk（＋）を算出する（ステップＳＴ７）。
【０１３３】
残差演算器３が残差ベクトルアンダバーνkを算出すると、正規化演算器１０では、まず、予測器１３から予測誤差共分散行列Ｐk（−）を入力し、これと予め設定されたレーダ装置１の観測誤差共分散行列Ｒkを用いて式（２９）に従い、観測ベクトルの予測誤差共分散行列Ｓkを算出する。次に、残差算出器３から残差ベクトルアンダバーνkを入力し、これを、先に算出した観測ベクトルの予測誤差共分散行列Ｓkを用いて式（２８）に従い正規化する（ステップＳＴ８）。
【０１３４】
隣接残差内積演算器２１では、第３の遅延回路１１から入力した１サンプリング前の正規化後の残差Ｖk-1と残差ベクトルアンダバーνk-1と、第４の遅延回路１８から入力した２サンプリング前の正規化後の残差Ｖk-2と残差ベクトルアンダバーνk-2とから、過去２サンプルの残差ベクトルの内積Ｇkの過去ｓサンプルの平均オーババーＧkを式（３５）及び式（３６）に従い演算する（ステップＳＴ９）。
【０１３５】
加速度変化検出器１９では、隣接残差内積演算器２１から過去２サンプルの残差ベクトルの内積Ｇkと、その過去ｓサンプルの平均オーババーＧkを入力し、第５の遅延回路２２から現時刻から１サンプリング前に算出した過去２サンプルの残差ベクトルの内積Ｇk-1を入力し、まず、式（３７）のΔＧkが予め設定されたパラメータｄに対して式（３７）を満たすか否かを判定し、満たす場合に目標が加速度変化をしたと判定する。式（３７）を満たさない場合は、オーババーＧkが予め設定されたパラメータｃについて式（３６）を満たすか否かを判定し、満たす場合に目標の速度変化があると判定する。そして何れも満たさない場合に目標は等速直線運動をしていると判定する（ステップＳＴ１０）。
【０１３６】
加速度変化運動モデル切り換え器２０では、加速度変化検出器１９で目標の加速度変化運動があると判定された場合に式（３２）の加速度変化運動モデルに、目標の加速度変化運動がないと判定され、かつ加速度運動があると判定された場合に式（２０）の等加速度運動モデルに、どちらもないと判定された場合に式（１８）の等速直線運動モデルに切り換える（ステップＳＴ１１）。
【０１３７】
予測器１３では１サンプリング前の平滑ベクトルアンダバーｘk-1ハット（＋）を第１の遅延回路８を介して入力し、等加速度運動モデル切り換え器１７により選択された運動モデルに基づき予測ベクトルアンダバーｘkハット（−）を算出する（ステップＳＴ１２）。
以上、追尾終了になるまでこの一連の処理を繰り返す。
【０１３８】
以上で明らかなように、この実施の形態４によれば、隣接残差内積演算器２１で過去２サンプルの残差ベクトルの内積Ｇkの過去ｓサンプルの平均オーババーＧkを算出し、加速度検出器１６において過去ｓサンプルの平均オーババーＧkのスレッショルド判定により速度変化を検出しているので、検出すべき加速度の方向を与えることなく、ランダム性の観測誤差の影響によって速度変化が誤って検出されるのを防ぐことができる。
【０１３９】
また、目標の速度変化を検出した場合に、等加速度運動モデル切り換え器１７により、目標の運動モデルを等加速度運動モデルに切り換える構成としているので、目標が加速度運動することにより生じる追従遅れの少ない安定した追尾が可能となる効果を奏する。
【０１４０】
さらに、実施の形態４では、加速度変化検出器１９において、隣り合うサンプリングにおけるＧkの変化量ΔＧkのスレッショルド判定により加速度の変化を検出しているので、検出すべき加速度の方向を与えることなく、ランダム性の観測誤差の影響によって加速度変化が誤って検出されるのを防ぐことができる。
【０１４１】
また、目標の加速度変化を検出した場合に、加速度変化運動モデル切り換え器２０により、目標の運動モデルを加速度変化運動モデルに切り換える構成としているので、目標が加速度変化運動することにより生じる追従遅れの少ない安定した追尾が可能となる効果を奏する。
【０１４２】
【発明の効果】
以上のように、この発明によれば、方向ベクトル内積演算手段により所望の方向の単位ベクトルと正規化後の残差ベクトルの内積の過去ｓサンプルの平均を算出し、加速度検出手段において過去ｓサンプルの平均のスレッショルド判定により速度変化を検出しているので、ランダム性の観測誤差の影響によって速度変化が誤って検出されるのを防ぐことができる。
【０１４３】
また、目標の速度変化を検出した場合に、等加速度運動モデル切り換え手段により、目標の運動モデルを等加速度運動モデルに切り換える構成としているので、目標が加速度運動することにより生じる追従遅れの少ない安定した追尾が可能となる効果を奏する。
【０１４４】
また、加速度変化検出手段において、隣り合うサンプリングにおける残差の変化量のスレッショルド判定により加速度の変化を検出しているので、ランダム性の観測誤差の影響によって加速度変化が誤って検出されるのを防ぐことができる。
【０１４５】
また、隣接残差内積演算手段により過去２サンプルの残差ベクトルの内積の過去ｓサンプルの平均を算出し、加速度検出手段において過去ｓサンプルの平均のスレッショルド判定により速度変化を検出しているので、検出すべき加速度の方向を与えることなく、ランダム性の観測誤差の影響によって速度変化が誤って検出されるのを防ぐことができる。
【０１４６】
また、目標の速度変化を検出した場合に、等加速度運動モデル切り換え手段により、目標の運動モデルを等加速度運動モデルに切り換える構成としているので、目標が加速度運動することにより生じる追従遅れの少ない安定した追尾が可能となる効果を奏する。
【０１４７】
さらに、隣接残差内積演算手段により過去２サンプルの残差ベクトルの内積の過去ｓサンプルの平均を算出し、加速度変化検出手段において、隣り合うサンプリングにおける残差の変化量のスレッショルド判定により加速度の変化を検出し、目標の加速度変化を検出した場合に、加速度変化運動モデル切り換え手段により、目標の運動モデルを加速度変化運動モデルに切り換える構成としているので、検出すべき加速度の方向を与えることなく、ランダム性の観測誤差の影響によって加速度変化が誤って検出されるのを防ぐことができると共に、目標が加速度変化運動することにより生じる追従遅れの少ない安定した追尾が可能となる効果を奏する。
【図面の簡単な説明】
【図１】 この発明の実施の形態１による目標追尾装置を示す構成図である。
【図２】 ｚ軸上に設置されたレーダ装置１の座標と目標の距離Ｒ及び方位角Ａｚの観測に係る説明図である。
【図３】 この発明の実施の形態２による目標追尾装置を示す構成図である。
【図４】 この発明の実施の形態３による目標追尾装置を示す構成図である。
【図５】 この発明の実施の形態４による目標追尾装置を示す構成図である。
【図６】 "An Adaptive Two-Dimensional Kalman Tracking Filter", IEEE Transaction on Aerospace and Electronic Systems Vol.AES-16,No.6,1980.に示された従来の目標追尾装置を示す構成図である。
【符号の説明】
１ レーダ装置、２ 目標相関器、３ 残差算出器、４ 予測誤差評価器、５ゲイン行列算出器、６ 平滑器、７ 平滑誤差評価器、８ 第１の遅延回路、９ 第２の遅延回路、１０ 正規化演算器、１１ 第３の遅延回路、１３ 予測器、１４ 方向ベクトルデータベース、１５ 方向ベクトル内積演算器、１６ 加速度検出器、１７ 等加速度運動モデル切り換え器、１８ 第４の遅延回路、１９ 加速度変化検出器、２０ 加速度変化運動モデル切り換え器、２１ 隣接残差内積演算器、２２ 第５の遅延回路。
Claims (12)
- 目標を観測して観測ベクトルを得る観測手段と、
目標の状態ベクトルの予測ベクトルを演算する予測手段と、
目標の状態ベクトルの平滑ベクトルを演算する平滑手段と、
所望の方向の単位ベクトルを格納した方向ベクトルデータベースと、
前記データベースから入力した所望方向の単位ベクトルと、前記観測手段により観測された観測ベクトルのうち、前記予測手段で演算された予測ベクトルに最も近い観測ベクトルから前記予測ベクトルを減算することで得られる残差ベクトルを正規化した正規化後の残差ベクトルとの内積を算出する方向ベクトル内積演算手段と、
前記方向ベクトル内積演算手段により算出された内積をスレッショルド判定して各単位ベクトルの方向に対する加速度運動の有無を判定することにより、特定方向の速度変化を検出する加速度検出手段と
を備えた目標追尾装置。 - 請求項１に記載の目標追尾装置において、
前記加速度検出手段にて速度変化が検出された場合に前記予測手段で用いる運動モデルを速度変化を考慮した運動モデルに切り換える等加速度運動モデル切り換え手段をさらに備えた
ことを特徴とする目標追尾装置。 - 目標を観測して観測ベクトルを得る観測手段と、
目標の状態ベクトルの予測ベクトルを演算する予測手段と、
目標の状態ベクトルの平滑ベクトルを演算する平滑手段と、
所望の方向の単位ベクトルを格納した方向ベクトルデータベースと、
前記データベースから入力した所望方向の単位ベクトルと、前記観測手段により観測された観測ベクトルのうち、前記予測手段で演算された予測ベクトルに最も近い観測ベクトルから前記予測ベクトルを減算することで得られる残差ベクトルを正規化した正規化後の残差ベクトルとの内積を算出する方向ベクトル内積演算手段と、
前記方向ベクトル内積演算手段により算出された隣り合うサンプリングにおける内積の変化量をスレッショルド判定して特定方向の加速度変化または速度変化を検出する加速度変化検出手段と、
前記加速度変化検出手段により加速度変化または速度変化が検出された場合に前記予測手段で用いる運動モデルを加速度変化または速度変化を考慮した運動モデルに切り換える加速度変化運動モデル切り換え手段と
を備えた目標追尾装置。 - 目標を観測して観測ベクトルを得る観測手段と、
目標の状態ベクトルの予測ベクトルを演算する予測手段と、
目標の状態ベクトルの平滑ベクトルを演算する平滑手段と、
前記観測手段により観測された観測ベクトルのうち、前記予測手段で演算された予測ベクトルに最も近い観測ベクトルから前記予測ベクトルを減算することで得られる残差ベクトルを観測ベクトルの予測誤差共分散行列を用いて正規化後、連続する過去２サンプルの正規化後の残差ベクトル間の内積を算出する隣接残差内積演算手段と、
前記隣接残差内積演算手段により算出された内積の値によりある一定方向の速度変化を検出する加速度検出手段と
を備えた目標追尾装置。 - 請求項４に記載の目標追尾装置において、
前記加速度検出手段にて速度変化が検出された場合に前記予測手段で用いる運動モデルを速度変化を考慮した運動モデルに切り換える等加速度運動モデル切り換え手段をさらに備えた
ことを特徴とする目標追尾装置。 - 目標を観測して観測ベクトルを得る観測手段と、
目標の状態ベクトルの予測ベクトルを演算する予測手段と、
目標の状態ベクトルの平滑ベクトルを演算する平滑手段と、
前記観測手段により観測された観測ベクトルのうち、前記予測手段で演算された予測ベクトルに最も近い観測ベクトルから前記予測ベクトルを減算することで得られる残差ベクトルを観測ベクトルの予測誤差共分散行列を用いて正規化後、連続する過去２サンプルの正規化後の残差ベクトル間の内積を算出する隣接残差内積演算手段と、
前記隣接残差内積演算手段により算出された隣り合うサンプリングにおける内積の変化量をスレッショルド判定して特定方向の加速度変化または速度変化を検出する加速度変化検出手段と、
前記加速度変化検出手段にて加速度変化または速度変化が検出された場合に前記予測手段で用いる運動モデルを加速度変化または速度変化を考慮した運動モデルに切り換える加速度変化運動モデル切り換え手段と
を備えた目標追尾装置。 - 目標を観測して観測ベクトルを得る観測ステップと、
目標の状態ベクトルの予測ベクトルを演算する予測ステップと、
目標の状態ベクトルの平滑ベクトルを演算する平滑ステップと、
所望の方向の単位ベクトルを格納した方向ベクトルデータベースから入力した所望方向の単位ベクトルと、前記観測ステップにより観測された観測ベクトルのうち、前記予測ステップで演算された予測ベクトルに最も近い観測ベクトルから前記予測ベクトルを減算することで得られる残差ベクトルを正規化した正規化後の残差ベクトルとの内積を算出する方向ベクトル内積演算ステップと、
前記方向ベクトル内積演算ステップにより算出された内積をスレッショルド判定して各単位ベクトルの方向に対する加速度運動の有無を判定することにより、特定方向の速度変化を検出する加速度検出ステップと
を備えた目標追尾方法。 - 請求項７に記載の目標追尾方法において、
前記加速度検出ステップにて速度変化が検出された場合に前記予測ステップで用いる運動モデルを速度変化を考慮した運動モデルに切り換える等加速度運動モデル切り換えステップをさらに備えた
ことを特徴とする目標追尾方法。 - 目標を観測して観測ベクトルを得る観測ステップと、
目標の状態ベクトルの予測ベクトルを演算する予測ステップと、
目標の状態ベクトルの平滑ベクトルを演算する平滑ステップと、
所望の方向の単位ベクトルを格納した方向ベクトルデータベースから入力した所望方向の単位ベクトルと、前記観測ステップにより観測された観測ベクトルのうち、前記予測ステップで演算された予測ベクトルに最も近い観測ベクトルから前記予測ベクトルを減算することで得られる残差ベクトルを正規化した正規化後の残差ベクトルとの内積を算出する方向ベクトル内積演算ステップと、
前記方向ベクトル内積演算ステップにより算出された隣り合うサンプリングにおける内積の変化量をスレショルド判定して特定方向の加速度変化または速度変化を検出する加速度変化検出ステップと、
前記加速度変化検出ステップにて加速度変化または速度変化が検出された場合に前記予測ステップで用いる運動モデルを加速度変化または速度変化を考慮した運動モデルに切り換える加速度変化運動モデル切り換えステップと
を備えた目標追尾方法。 - 目標を観測して観測ベクトルを得る観測ステップと、
目標の状態ベクトルの予測ベクトルを演算する予測ステップと、
目標の状態ベクトルの平滑ベクトルを演算する平滑ステップと、
前記観測ステップにより観測された観測ベクトルのうち、前記予測ステップで演算された予測ベクトルに最も近い観測ベクトルから前記予測ベクトルを減算することで得られる残差ベクトルを観測ベクトルの予測誤差共分散行列を用いて正規化後、連続する過去２サ ンプルの正規化後の残差ベクトル間の内積を算出する隣接残差内積演算ステップと、
前記隣接残差内積演算ステップにより算出された内積の値によりある一定方向速度変化を検出する加速度検出ステップと
を備えた目標追尾方法。 - 請求項１０に記載の目標追尾方法において、
前記加速度検出ステップにて速度変化が検出された場合に前記予測ステップで用いる運動モデルを速度変化を考慮した運動モデルに切り換える等加速度運動モデル切り換えステップをさらに備えた
ことを特徴とする目標追尾方法。 - 目標を観測して観測ベクトルを得る観測ステップと、
目標の状態ベクトルの予測ベクトルを演算する予測ステップと、
目標の状態ベクトルの平滑ベクトルを演算する平滑ステップと、
前記観測ステップにより観測された観測ベクトルのうち、前記予測ステップで演算された予測ベクトルに最も近い観測ベクトルから前記予測ベクトルを減算することで得られる残差ベクトルを観測ベクトルの予測誤差共分散行列を用いて正規化後、連続する過去２サンプルの正規化後の残差ベクトル間の内積を算出する隣接残差内積演算ステップと、
前記隣接残差内積演算ステップにより算出された隣り合うサンプリングにおける内積の変化量をスレショルド判定して特定方向の加速度変化または速度変化を検出する加速度変化検出ステップと、
前記加速度変化検出ステップにて加速度変化または速度変化が検出された場合に前記予測ステップで用いる運動モデルを加速度変化または速度変化を考慮した運動モデルに切り換える加速度変化運動モデル切り換えステップと
を備えた目標追尾方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2001359939A JP3970585B2 (ja) | 2001-11-26 | 2001-11-26 | 目標追尾装置及び方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2001359939A JP3970585B2 (ja) | 2001-11-26 | 2001-11-26 | 目標追尾装置及び方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2003161778A JP2003161778A (ja) | 2003-06-06 |
| JP3970585B2 true JP3970585B2 (ja) | 2007-09-05 |
Family
ID=19170845
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2001359939A Expired - Fee Related JP3970585B2 (ja) | 2001-11-26 | 2001-11-26 | 目標追尾装置及び方法 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP3970585B2 (ja) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP5196971B2 (ja) * | 2007-11-27 | 2013-05-15 | 三菱電機株式会社 | 目標追尾装置 |
| JP2014041547A (ja) * | 2012-08-23 | 2014-03-06 | Nippon Telegr & Teleph Corp <Ntt> | 時系列データ解析装置、方法、及びプログラム |
-
2001
- 2001-11-26 JP JP2001359939A patent/JP3970585B2/ja not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| JP2003161778A (ja) | 2003-06-06 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US6581007B2 (en) | System, method, and program for detecting approach to object | |
| JP5784526B2 (ja) | 目標追跡装置 | |
| US20070211917A1 (en) | Obstacle tracking apparatus and method | |
| JP4116898B2 (ja) | 目標追尾装置 | |
| JP4966794B2 (ja) | 水中航走体誘導方法及び装置 | |
| CN112912759A (zh) | 物体跟踪装置 | |
| JP3970585B2 (ja) | 目標追尾装置及び方法 | |
| WO2020148894A1 (ja) | 運動状態判定装置 | |
| JP2002341024A (ja) | 多目標追尾装置 | |
| JP3774093B2 (ja) | 追尾処理装置及び方法 | |
| JP3926602B2 (ja) | 目標追尾装置及び方法 | |
| CN113280821A (zh) | 基于斜率约束和回溯搜索的水下多目标跟踪方法 | |
| JP3440010B2 (ja) | 目標追尾装置 | |
| JP3848846B2 (ja) | 追尾装置 | |
| JP2001228245A (ja) | 目標追尾装置および目標追尾方法 | |
| JP2843910B2 (ja) | 多目標追尾装置 | |
| JP2002286838A (ja) | 目標追尾装置及び目標追尾方法 | |
| JP2013253760A (ja) | 誘導装置 | |
| JPH0643241A (ja) | 多目標追尾方法及びその装置 | |
| Hashirao et al. | Maneuver target tracking with an acceleration estimator using target past positions | |
| JP3583666B2 (ja) | 追尾装置 | |
| JPH05297947A (ja) | 多目標追尾装置 | |
| JP3411485B2 (ja) | 目標追尾装置 | |
| JP2005098840A (ja) | 飛しょう体の誘導装置 | |
| JP2001289944A (ja) | 航跡追尾方法およびその装置 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20041018 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20060627 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20060704 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20060901 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20070320 |
|
| A521 | Written amendment |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20070516 |
|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20070605 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20070606 |
|
| R150 | Certificate of patent or registration of utility model |
Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20100615 Year of fee payment: 3 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20110615 Year of fee payment: 4 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20120615 Year of fee payment: 5 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20130615 Year of fee payment: 6 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| LAPS | Cancellation because of no payment of annual fees |