JP4097126B2 - Target tracking apparatus and method - Google Patents

Description

【００２８】
次に、目標の運動モデルを式（３）に示す。ただし、アンダバーｘ_ｋはサンプリング時刻における目標運動諸元の真値を表す状態ベクトルであり、ｘｙ座標における目標位置ベクトルを式（４）、速度ベクトルを式（５）とすると、目標の状態ベクトルは式（6）で表される。なお、アンダバーＡ^ＴはベクトルアンダバーＡの転置ベクトルを表す。
【００２９】
【数２】

【００３０】
ここで、Φ_ｋ−１はサンプリング時刻ｔ_ｋ−１よりｔ_ｋへの状態ベクトルの推移行列であり、式（７）で表される。また、アンダバーｗ_ｋはサンプリング時刻ｔ_ｋにおける駆動雑音ベクトルであり、Γ_１（ｋ）はサンプリング時刻ｔ_ｋにおける駆動雑音ベクトルの変換行列である。例えば、目標の運動モデルを等速直線運動と仮定したことによる打ち切り誤差項をΓ_１（ｋ−１）アンダバーｗ_ｋ−１とみれば、アンダバーｗ_ｋは加速度ベクトル相当であり、Γ_１（ｋ−１）は式（８）で表される。なお、Ｔはレーダ装置１のサンプリング間隔、Ｉは２行２列の単位行列である。
【００３１】
【数３】

【００３２】
また、平均を表す記号としてＥを用いると、アンダバーｗ_ｋは平均の２次元正規分布白色雑音であり、式（７）及び（８）とする。ただし、アンダバー０は零ベクトルであり、Ｑ_ｋはサンプリング時刻ｔ_ｋにおける駆動雑音共分散行列である。
【００３３】
【数４】

【００３４】
次に目標の距離Ｒ及び方位角Ａｚがサンプリング時刻ｔ_ｋにレーダ装置１より観測される場合のｘｙ座標におけるレーダ装置１の観測モデルを式（１１）とする。ここで、距離Ｒ及び方位角Ａｚは式（１）及び式（２）によりｘｙ座標に変換される。また、アンダバーｚ_ｋはサンプリング時刻ｔ_ｋにおけるレーダ観測装置１の観測ベクトル、Ｈは観測行列で、式（１２）で表される。
【００３５】
また、アンダバーν_ｋはサンプリング時刻ｔ_ｋにおけるレーダ装置１の観測雑音ベクトルであり、平均アンダバー０の２次元正規分布白色雑音で、式（１３）及び（１４）で表される。なお、Ｒ_ｋはサンプリング時刻ｔ_ｋにおけるレーダ装置１の観測誤差共分散行列である。Γ_２（ｋ）は極座標よりｘｙ座標への観測雑音ベクトルの変換行列で、式（１５）で表される。サンプリング時刻ｔ_ｋまでに目標の追尾に用いたレーダ装置１の観測ベクトル全体をＺ_ｋとする（式（１６）を参照）。
【００３６】
【数５】

【００３７】
次に、サンプリング時刻ｔ_ｋ−１までのレーダ装置１の観測ベクトルＺ_ｋ−１が得られているときの予測処理について述べる。目標が等速直線運動をしていると仮定した場合のサンプリング時刻ｔ_ｋにおける追尾目標の状態ベクトルアンダバーｘ_ｋの予測ベクトルをアンダバーｘ_ｋハット（−）、予測誤差共分散行列をＰ_ｋ（−）とすると、それぞれ条件付平均ベクトル、条件付共分散行列で定義され、式（１７）及び式（１８）で表される。ここで、アンダバーｘ_ｋハット（＋）及びＰ_ｋ−１（＋）はそれぞれ前サンプリング時刻ｔ_ｋ−１の平滑ベクトル及び平滑誤差共分散行列である。これらの算出方法については後で述べる。
【００３８】
【数６】

【００３９】
次に、相関ゲート判定処理について述べる。追尾目標の観測ベクトルの予測ベクトルは式（１９）で表される。サンプリング時刻ｔ_ｋにおいてレーダ装置１より観測されたｍ個の観測ベクトルをアンダバーｚ_ｋ，ｉ（ｉ＝１，２，・・・，ｍ）とすると、当該追尾目標との残差ε_ｋ，ｉが予め設定されたゲートサイズパラメータρに対して式（２０）を満たすとき、その観測ベクトルの全てを相関ゲート内の観測ベクトルとし、当該追尾目標と相関があるとみなす。この相関ゲート判定は全ての追尾目標について行う。
【００４０】
【数７】

【００４１】
次に、追尾の対象となる道路領域における追尾車両の走行順序を判定する方法について述べる。サンプリング時刻ｔ_ｋにおいて、１サンプリング前に算出されたj番目の追尾目標の平滑ベクトルの位置ベクトルアンダバーξ_ｋ−１ ^ｊを式（２１）とおく。一方、車両の進行方向であるｙ軸正方向の単位ベクトルをアンダバーｅ_ｄとおくと、式（２２）で表されるこれらのベクトルの内積Ψ_ｋ−１ ^ｊは車両の位置ベクトルの車両進行方向の成分となるので、車両の前後関係は内積Ψ_ｋ−１ ^ｊの大小関係を見ればよい。すなわち、内積Ψ_ｋ−１ ^ｊが大きい順序に車両ｊを並べた結果が車両の走行順序となる。
【００４２】
【数８】

【００４３】
次に、追尾車両のグループを判定する方法について述べる。前述の処理により判定された追尾車両の走行順序に対応して、改めて追尾車両の平滑ベクトルの位置ベクトルの順序を先頭から順に、アンダバーξ_ｋ−１ ^１，アンダバーξ_ｋ−１ ^２，アンダバーξ_ｋ−１ ^３，・・・と記述し直す。先頭から順に、隣り合う平滑ベクトルの位置ベクトルの距離ｒが予め設定されたパラメータｒ_ｍｉｎに対して式（２３）を満たすか否かを調べ、式（２３）を満たす航跡同士を同一グループと判定する。
【００４４】
【数９】

【００４５】
次に、追尾の対象となる道路領域における観測ベクトルの存在順序を判定する方法について述べる。観測ベクトルの存在順序は、追尾車両の走行順序を判定する方法と同様に、式（２４）で表される観測ベクトルと車両進行方向の単位ベクトルアンダバーｅ_ｄとの内積λ_ｋ−１ ^ｉの大小関係を見ればよい。すなわち、内積λ_ｋ−１ ^ｉが大きい順序に観測ベクトルを並べた結果が観測ベクトルの存在順序となる。
【００４６】
【数１０】

【００４７】
次に、追尾目標と観測ベクトルの順序情報を利用した相関判定方法について述べる。本処理では追尾車両の追い越しがないように、追尾目標の順序と観測ベクトルの順序を逆転させないように相関判定を行う。相関判定は前述の追尾車両のグループ判定において分類されたグループ毎に以下の処理を行う。
【００４８】
先頭の追尾車両から順番に、その追尾車両の相関ゲート内の観測ベクトルのうち存在順序が先頭側の観測ベクトルを当該追尾車両と相関があるものとしてその組み合わせを記憶する。このとき、相関があると判定された観測ベクトルは以後の追尾車両の相関対象から除く。例えば、図３に示すように、３つの追尾車両が同一グループと判定され、これらの追尾車両の相関ゲート内と判定された観測ベクトルが４つあるとする。
【００４９】
ここで、追尾車両の走行順序は先頭から、アンダバーξ_ｋ−１ ^１，アンダバーξ_ｋ−１ ^２，アンダバーξ_ｋ−１ ^３であり、観測ベクトルの存在順序は先頭側から、アンダバーｚ_ｋ，１，アンダバーｚ_ｋ，２，アンダバーｚ_ｋ，３，アンダバーｚ_ｋ，４，とする。図３は、これらの相関ゲート内外関係を示しており、相関ゲート内と判定された場合は○、相関ゲート外と判定された場合は×で表している。まず、先頭車両であるアンダバーξ_ｋ−１ ^１に着目する。
【００５０】
この車両の相関ゲート内に存在する観測ベクトルのうち存在順序が先頭側なのはアンダバーｚ_ｋ，１であるから、アンダバーξ_ｋ−１ ^１はアンダバーｚ_ｋ，１と相関があると判定する。次に、アンダバーｚ_ｋ，１を消去し、後続の走行車両であるアンダバーξ_ｋ−１ ^２に着目する。この車両の相関ゲート内に存在する観測ベクトルのうち、存在順序が先頭側なのはアンダバーｚ_ｋ，２であるから、アンダバーξ_ｋ−１ ^２はアンダバーｚ_ｋ，２と相関があると判定する。同様にしてアンダバーξ_ｋ−１ ^３はアンダバーｚ_ｋ，３と相関があると判定する。
【００５１】
次に、平滑処理について述べる。前述の相関処理により、追尾目標と相関があると判定された観測ベクトルをアンダバーｚ_ｋと置き直すと、ゲイン行列Ｋ_ｋ、平滑ベクトルアンダバーｘ_ｋハット（＋）及び平滑誤差共分散行列Ｐ_ｋ（＋）は通常のカルマンフィルタの理論により、式（２５）〜式（２７）で与えられる。ここで、Ｒ_ｋは式（１７）で表される、サンプリング時刻ｔ_ｋにおけるレーダ装置１の観測誤差共分散行列であり、アンダバーν_ｋは式（２８）で与えられる残差ベクトルである。
【００５２】
【数１１】

【００５３】
次に、この実施の形態１による目標追尾装置の動作を具体的に説明する。なお、カルマンフィルタを目標追尾装置に通常適用する場合と同様にして状態ベクトルの初期値は別途定まっているものとする。
【００５４】
レーダ装置１では、図２に示すように目標の距離Ｒ及び方位角Ａｚを観測し、式（１）及び式（２）に従いｘｙ座標に変換する（ステップＳＴ１）。相関ゲート判定器２では、予測器１１から追尾対象の予測ベクトルアンダバーｘ_ｋハット（−）を入力して観測ベクトルのベクトルアンダバーｚ_ｋハット（−）を式（１９）に従い算出し、レーダ装置１から入力した観測ベクトルのうち、予め設定されたゲートサイズパラメータρに対して式（２０）を満たす観測ベクトルの全てを当該追尾目標の相関ゲート内の観測ベクトルとして選択する。これを全ての追尾目標に対して行う（ステップＳＴ２）。
【００５５】
観測データの順序判定器３では、グループ判定器１３により判定されたグループ毎に、そのグループに所属する追尾目標の相関ゲート内の観測ベクトルに対し、予め設定された車両の進行方向の単位ベクトルアンダバーｅ_ｄとの内積λ_ｋ−１ ^ｉを式（２４）に従い算出する。そして、内積λ_ｋ−１ ^ｉの大きい順番を観測ベクトルの存在順序として記憶する（ステップＳＴ３）。
【００５６】
順序による相関判定器４では、グループ判定器１３において判定されたグループ毎に、観測データの順序判定器３により判定されたそのグループに所属する追尾目標の相関ゲート内の観測ベクトルの存在順序の情報と、航跡の順序判定器１２で判定されたそのグループに所属する追尾車両の走行順序の情報を入力し、走行順序の順番に、その追尾車両の相関ゲート内の観測ベクトルのうち存在順序が先頭側の観測ベクトルを当該追尾車両と相関があると判定してその組み合わせを記憶する。このとき、相関があると判定された観測ベクトルは後に続く追尾車両の相関対象から除く。そして、全てのグループに対して同様の処理を行う。（ステップＳＴ４）。
【００５７】
ゲイン行列算出器５では、追尾目標毎に、予測誤差評価器１０から入力した予測誤差共分散行列Ｐ_ｋ（−）と、予め設定されたレーダ装置１の観測誤差共分散行列Ｒ_ｋとから式（２５）に従いゲイン行列Ｋ_ｋを算出する（ステップＳＴ５）。平滑器６では、追尾目標毎に、順序による相関判定器４で判定した相関結果と、予測器１１の出力した予測ベクトルアンダバーｘ_ｋハット（−）と、ゲイン行列算出器５からゲイン行列Ｋ_ｋとをそれぞれ入力し、式（２６）に従い平滑ベクトルアンダバーｘ_ｋハット（＋）を算出する（ステップＳＴ６）。
【００５８】
一方、平滑誤差評価器８では、予測誤差評価器１０から予測誤差共分散行列Ｐ_ｋ（−）を、ゲイン行列算出器５からゲイン行列Ｋ_ｋをそれぞれ入力し、式（２７）に従い平滑誤差共分散行列Ｐ_ｋ（＋）を算出する（ステップＳＴ７）。予測誤差評価器１０では、第２の遅延回路９から１サンプリング前の平滑誤差共分散行列Ｐ_ｋ−１（＋）を、予め設定された駆動雑音共分散行列Ｑ_ｋ−１をそれぞれ入力し、予測誤差共分散行列Ｐ_ｋ（−）を式（１８）に従い算出する（ステップＳＴ８）。予測器１１では、１サンプリング前の平滑ベクトルアンダバーｘ_{ｋ− １}ハット（＋）を第１の遅延回路７を介して入力し、式（１７）に従い予測ベクトルアンダバーｘ_ｋハット（−）を算出する（ステップＳＴ９）。
【００５９】
航跡の順序判定器１２では、平滑器６において１サンプリング前に算出された追尾目標の平滑ベクトルを第１の遅延回路７を介して入力し、その位置ベクトルアンダバーξ_ｋ−１ ^ｊと、予め設定された車両の進行方向の単位ベクトルアンダバーｅ_ｄとの内積Ψ_ｋ−１ ^ｊを式（２２）に従い算出する。そして、内積Ψ_ｋ−１ ^ｊの大きい順番を航跡の走行順序として記憶する（ステップＳＴ１０）。グループ判定機１３では、航跡の順序判定機１２で判定された追尾車両の走行順序の順番に隣り合う平滑ベクトルの位置ベクトルの距離ｒが予め設定されたパラメータｒ_ｍｉｎに対して式（２３）を満たすか否かを調べ、式（２３）を満たす航跡同士を同一グループと判定する（ステップＳＴ１１）。
【００６０】
以上で明らかなように、この実施の形態１によれば、観測データの順序判定器３により、車両の進行方向に対する観測ベクトルの存在順序を、予め設定された車両の進行方向の単位ベクトルと観測ベクトルの内積の大きさから判定し、一方、航跡の順序判定器１２により、車両の走行順序を、上記単位ベクトルと追尾車両の平滑ベクトルの位置ベクトルの内積の大きさから判定し、順序による相関判定器４において、走行順序の順番に、追尾車両の相関ゲート内の観測ベクトルのうち存在順序が先頭側の観測ベクトルを当該追尾車両と相関があるものと判定する構成としているので、追尾車両の航跡の順序が入れ替わりにくい効果を奏する。
【００６１】
実施の形態２．
図４は、この実施の形態２による目標追尾装置を示す構成図であり、図において、図１と同一符号である１〜３及び５〜１３は実施の形態１と同一または相当部分を示すので、その説明を省略する。
【００６２】
１４は相関ゲート判定器２で判定された追尾目標の相関ゲート内の観測データと、その残差を入力し、これらと、観測データの順序判定器３で判定された各グループに対応する観測データの順序情報と、グループ判定器１３で判定した追尾目標のグループ情報を用いて、追尾目標と観測データの相関を判定する順序と残差による相関判定器である。
【００６３】
次に動作について説明する。最初に、この実施の形態２による目標追尾装置の動作原理を説明する。レーダ装置１による車両の観測方法、目標の運動モデル、レーダ装置１の観測モデルは、実施の形態１における式（１）〜式（１６）までの原理と同じであるので省略する。
【００６４】
また、予測処理及び相関ゲート判定処理は、実施の形態１における式（１７）〜式（２０）までの原理と同じであるので省略する。また、追尾車両の走行順序を判定する方法、追尾車両のグループを判定する方法、そして、観測ベクトルの存在順序を判定する方法も、実施の形態１における式（２１）〜式（２４）までの原理と同じであるので省略する。
【００６５】
次に、追尾目標と観測ベクトルの順序情報を利用した相関判定方法について述べる。本処理では追尾車両の追い越しがないように、追尾目標の順序と観測ベクトルの順序を逆転させないように相関判定を行う。相関判定は前述の追尾車両のグループ判定において分類されたグループ毎に以下の処理を行う。
【００６６】
先頭の追尾車両から順番に、その追尾車両の相関ゲート内の観測ベクトルのうち式（２０）で表される残差ε_ｋ，ｉが最も小さい観測ベクトルを当該追尾車両と相関があるものと判定してその組み合わせを記憶する。このとき、相関があると判定された観測ベクトルとそれよりも存在順序が前の観測ベクトルは以後の追尾車両の相関対象から除く。
【００６７】
例えば、図５に示すように、３つの追尾車両が同一グループと判定され、これらの追尾車両の相関ゲート内と判定された観測ベクトルが４つあるとする。ここで、追尾車両の走行順序は先頭から、アンダバーξ_ｋ−１ ^１，アンダバーξ_ｋ−１ ^２，アンダバーξ_ｋ−１ ^３であり、観測ベクトルの存在順序は先頭側から、アンダバーｚ_ｋ，１，アンダバーｚ_ｋ，２，アンダバーｚ_ｋ，３，アンダバーｚ_ｋ，４とする。図５は、これらの相関ゲート内外関係及び残差を示しており、相関ゲート内と判定された場合は残差を、相関ゲート外と判定された場合は×で表している。
【００６８】
まず、先頭車両であるアンダバーξ_ｋ−１ ^１に着目する。この車両の相関ゲート内に存在する観測ベクトルのうち残差が最も小さい観測ベクトルはアンダバーｚ_ｋ，２であるから、アンダバーξ_ｋ−１ ^１はアンダバーｚ_ｋ，２と相関があると判定する。次に、アンダバーｚ_ｋ，２とこれより前方に存在するアンダバーｚ_ｋ，１を消去し、後続の走行車両であるアンダバーξ_ｋ−１ ^２に着目する。この車両の相関ゲート内に存在する観測ベクトルのうち、残差が最も小さいのはアンダバーｚ_ｋ，３であるから、アンダバーξ_ｋ−１ ^２はアンダバーｚ_ｋ，３と相関があると判定する。同様にしてアンダバーξ_ｋ−１ ^３はアンダバーｚ_ｋ，４と相関があると判定する。
【００６９】
次に、平滑処理について述べる。前述の相関処理により、追尾目標と相関があると判定された観測ベクトルをアンダバーｚ_ｋと置き直すと、ゲイン行列Ｋ_ｋ、平滑ベクトルアンダバーｘ_ｋハット（＋）及び平滑誤差共分散行列Ｐ_ｋ（＋）は通常のカルマンフィルタの理論により、式（２５）〜式（２７）で与えられる。ここで、Ｒ_ｋは式（１７）で表される、サンプリング時刻ｔ_ｋにおけるレーダ装置１の観測誤差共分散行列であり、アンダバーν_ｋは式（２８）で与えられる残差ベクトルである。
【００７０】
次に、この実施の形態２による目標追尾装置の動作を具体的に説明する。なお、カルマンフィルタを目標追尾装置に通常適用する場合と同様にして状態ベクトルの初期値は別途定まっているものとする。
【００７１】
レーダ装置１では、図２に示すように目標の距離Ｒ及び方位角Ａｚを観測し、式（１）及び式（２）に従いｘｙ座標に変換する（ステップＳＴ１）。相関ゲート判定器２では、予測器１１から追尾対象の予測ベクトルアンダバーｘ_ｋハット（−）を入力して観測ベクトルのベクトルアンダバーｚ_ｋ（−）を式（１９）に従い算出し、レーダ装置１から入力した観測ベクトルのうち、予め設定されたゲートサイズパラメータρに対して式（２０）を満たす観測ベクトルの全てを当該追尾目標の相関ゲート内の観測ベクトルとして選択する。これを全ての追尾目標に対して行う（ステップＳＴ２）。
【００７２】
観測データの順序判定器３では、グループ判定器１３により判定されたグループ毎に、そのグループに所属する追尾目標の相関ゲート内の観測ベクトルに対し、予め設定された車両の進行方向の単位ベクトルアンダバーｅ_ｄとの内積λ_ｋ−１ ^ｉを式（２４）に従い算出する。そして、内積λ_ｋ−１ ^ｉの大きい順番を観測ベクトルの存在順序として記憶する（ステップＳＴ３）。
【００７３】
順序と残差による相関判定器１４では、グループ判定器１３において判定されたグループ毎に、観測データの順序判定器３により判定されたそのグループに所属する追尾目標の相関ゲート内の観測ベクトルの存在順序の情報及び残差と、航跡の順序判定器１２で判定されたそのグループに所属する追尾車両の走行順序の情報を入力し、走行順序の順番に、その追尾車両の相関ゲート内の観測ベクトルのうち残差が最も小さい観測ベクトルを当該追尾車両と相関があると判定してその組み合わせを記憶する。このとき、相関があると判定された観測ベクトルは後に続く追尾車両の相関対象から除く。そして、全てのグループに対して同様の処理を行う（ステップＳＴ４）。
【００７４】
ゲイン行列算出器５では、追尾目標毎に、予測誤差評価器１０から入力した予測誤差共分散行列Ｐ_ｋ（−）と、予め設定されたレーダ装置１の観測誤差共分散行列Ｒ_ｋとから式（２５）に従いゲイン行列Ｋ_ｋを算出する（ステップＳＴ５）。平滑器６では、追尾目標毎に、順序と残差による相関判定器１４で判定した相関結果と、予測器１１の出力した予測ベクトルアンダバーｘ_ｋハット（−）と、ゲイン行列算出器５からゲイン行列Ｋ_ｋとをそれぞれ入力し、式（２６）に従い平滑ベクトルアンダバーｘ_ｋハット（＋）を算出する（ステップＳＴ６）。
【００７５】
一方、平滑誤差評価器８では、予測誤差評価器１０から予測誤差共分散行列Ｐ_ｋ（−）を、ゲイン行列算出器５からゲイン行列Ｋ_ｋをそれぞれ入力し、式（２７）に従い平滑誤差共分散行列Ｐ_ｋ（＋）を算出する（ステップＳＴ７）。予測誤差評価器１０では、第２の遅延回路９から１サンプリング前の平滑誤差共分散行列Ｐ_ｋ−１（＋）を、予め設定された駆動雑音Ｑ_ｋ−１をそれぞれ入力し、予測誤差共分散行列Ｐ_ｋ（−）を式（１８）に従い算出する（ステップＳＴ８）。予測器１１では、１サンプリング前の平滑ベクトルアンダバーｘ_ｋ−１ハット（＋）を第１の遅延回路７を介して入力し、式（１７）に従い予測ベクトルアンダバーｘ_ｋハット（−）を算出する（ステップＳＴ９）。
【００７６】
航跡の順序判定器１２では、平滑器６において１サンプリング前に算出された追尾目標の平滑ベクトルを第１の遅延回路７を介して入力し、その位置ベクトルアンダバーξ_ｋ−１ ^ｊと、予め設定された車両の進行方向の単位ベクトルアンダバーｅ_ｄとの内積Ψ_ｋ−１ ^ｊを式（２２）に従い算出する。そして、内積Ψ_ｋ−１ ^ｊの大きい順番を航跡の走行順序として記憶する（ステップＳＴ１０）。グループ判定機１３では、航跡の順序判定機１２で判定された追尾車両の走行順序の順番に隣り合う平滑ベクトルの位置ベクトルの距離ｒが予め設定されたパラメータｒ_ｍｉｎに対して式（２３）を満たすか否かを調べ、式（２３）を満たす航跡同士を同一グループと判定する（ステップＳＴ１１）。
【００７７】
以上で明らかなように、この実施の形態２によれば、観測データの順序判定器３により、車両の進行方向に対する観測ベクトルの存在順序を、予め設定された車両の進行方向の単位ベクトルと観測ベクトルの内積の大きさから判定し、一方、航跡の順序判定器１２により、車両の走行順序を、上記単位ベクトルと追尾車両の平滑ベクトルの位置ベクトルの内積の大きさから判定し、順序と残差による相関判定器１４において、走行順序の順番に、追尾車両の相関ゲート内の観測ベクトルのうち残差が最も小さい観測ベクトルを当該追尾車両と相関があるものと判定し、それより前方に存在する観測ベクトルを以降の追尾目標の相関対象から除く構成としているので、追尾車両の航跡の順序が入れ替わりにくく、かつ、レーダ装置１により誤検出が検出される場合においても誤った相関結果となりにくい効果を奏する。
【００７８】
実施の形態３．
図６は、この実施の形態３による目標追尾装置を示す構成図であり、図において、図１と同一符号である１〜１３は実施の形態１と同一または相当部分を示すので、その説明を省略する。１５は順序による相関判定器４で判定された追尾目標と観測データの相関結果において、相関する観測データのない追尾目標に対して、擬似的な観測データを生成する擬似データ生成器である。
【００７９】
次に動作について説明する。最初に、この実施の形態３による目標追尾装置の動作原理を説明する。レーダ装置１による車両の観測方法、目標の運動モデル、レーダ装置１の観測モデルは、実施の形態１における式（１）〜式（１６）までの原理と同じであるので省略する。
【００８０】
また、相関判定方法は実施の形態１と同じ原理であるので省略する。また、予測処理及び相関ゲート判定処理は、実施の形態１における式（１７）〜式（２０）までの原理と同じであるので省略する。また、追尾車両の走行順序を判定する方法、追尾車両のグループを判定する方法、そして、観測ベクトルの存在順序を判定する方法も、実施の形態１における式（２１）〜式（２４）までの原理と同じであるので省略する。
【００８１】
次に相関する観測データのない追尾目標が存在する場合の擬似的な観測データの生成方法について述べる。相関判定器４において、どの観測ベクトルとも相関の得られない追尾目標がある場合、通常のメモリートラック（予測位置をそのまま平滑位置とする方法）を行うと、その前後の車両が加減速を行っている場合に前方の車両を追い越す、あるいは後方の車両に追い抜かれる等の可能性が高くなる。
【００８２】
そこで、相関の得られない追尾目標は、同一グループ内で相関の得られた前後の車両の１サンプル前の平滑位置の間隔と同じ比率で、前後車両と相関の得られた観測ベクトルを内分した位置を用いて平滑を行う。相関の得られない追尾目標の１サンプリング前の平滑ベクトルの位置ベクトルをアンダバーξ_ｋ−１ ^ｊとする。そして、そのａ台前の車両とｂ台前の車両の１サンプリング前の平滑ベクトルの位置ベクトルをそれぞれアンダバーξ_ｋ−１ ^ｊ−ａおよびアンダバーξ_ｋ−１ ^ｊ＋ｂとし、これらと相関の得られた現時刻の観測ベクトルをそれぞれアンダバーｚ_ｋ，ａ及びアンダバーｚ_ｋ，ｂとすると、１サンプル前の平滑位置の間隔と同じ比率で、前後車両と相関の得られた観測ベクトルを内分した位置アンダバーｚ_ｋ，ｊは式（２９）で表すことができる。但し、相関の得られない追尾目標の前後の何れかにしか相関の得られた追尾目標がない場合は、その追尾目標の１サンプリング前の平滑ベクトルの位置ベクトルをアンダバーξ_ｋ−１ ^ｌとすると、式（３０）で表される平滑ベクトルの差ベクトルの位置で外挿した位置を用いる。
【００８３】
【数１２】

【００８４】
次に、平滑処理について述べる。前述の相関処理により、相関対象が存在する追尾目標はその相関対象となる観測ベクトルをアンダバーｚ_ｋと置き直し、相関対象が存在しない追尾目標は、前述の擬似的な観測データの生成方法で得られた位置ベクトルをアンダバーｚ_ｋと置き直すと、ゲイン行列Ｋ_ｋ、平滑ベクトルアンダバーｘ_ｋハット（＋）及び平滑誤差共分散行列Ｐ_ｋ（＋）は通常のカルマンフィルタの理論により、式（２５）〜式（２７）で与えられる。ここで、Ｒ_ｋは式（１７）で表される、サンプリング時刻ｔ_ｋにおけるレーダ装置１の観測誤差共分散行列であり、アンダバーν_ｋは式（２８）で与えられる残差ベクトルである。
【００８５】
次に、この実施の形態３による目標追尾装置の動作を具体的に説明する。なお、カルマンフィルタを目標追尾装置に通常適用する場合と同様にして平滑ベクトルの初期値は別途定まっているものとする。
【００８６】
レーダ装置１では、図２に示すように目標の距離Ｒ及び方位角Ａｚを観測し、式（１）及び式（２）に従いｘｙ座標に変換する（ステップＳＴ１）。相関ゲート判定器２では、予測器１１から追尾対象の予測ベクトルアンダバーｘ_ｋハット（−）を入力して観測ベクトルのベクトルアンダバーｚ_ｋ（−）を式（１９）に従い算出し、レーダ装置１から入力した観測ベクトルのうち、予め設定されたゲートサイズパラメータρに対して式（２０）を満たす観測ベクトルの全てを当該追尾目標の相関ゲート内の観測ベクトルとして選択する。これを全ての追尾目標に対して行う（ステップＳＴ２）。
【００８７】
観測データの順序判定器３では、グループ判定器１３により判定されたグループ毎に、そのグループに所属する追尾目標の相関ゲート内の観測ベクトルに対し、予め設定された車両の進行方向の単位ベクトルアンダバーｅ_ｄとの内積λ_ｋ−１ ^ｉを式（２４）に従い算出する。そして、内積λ_ｋ−１ ^ｉの大きい順番を観測ベクトルの存在順序として記憶する（ステップＳＴ３）。
【００８８】
順序による相関判定器４では、グループ判定器１３において判定されたグループ毎に、観測データの順序判定器３により判定されたそのグループに所属する追尾目標の相関ゲート内の観測ベクトルの存在順序の情報と、航跡の順序判定器１２で判定されたそのグループに所属する追尾車両の走行順序の情報を入力し、走行順序の順番に、その追尾車両の相関ゲート内の観測ベクトルのうち存在順序が先頭側の観測ベクトルを当該追尾車両と相関があると判定してその組み合わせを記憶する。このとき、相関があると判定された観測ベクトルは後に続く追尾車両の相関対象から除く。そして、全てのグループに対して同様の処理を行う。（ステップＳＴ４）。
【００８９】
擬似データ生成器１５では、順序による相関判定器４で判定された追尾目標と観測データの相関結果と、原時刻より１サンプリング前に平滑器６で算出された平滑ベクトルを第１の遅延回路を介して入力し、グループ毎に、相関する観測データのない追尾目標に対して、その前後に相関の取れた追尾目標がある場合には式（２９）に従い、前後のどちらか１方に追尾目標がある場合には式（３０）に従い擬似的な観測ベクトルを生成する（ステップＳＴ５）。
【００９０】
ゲイン行列算出器５では、追尾目標毎に、予測誤差評価器１０から入力した予測誤差共分散行列Ｐ_ｋ（−）と、予め設定されたレーダ装置１の観測誤差共分散行列Ｒ_ｋとから式（２５）に従いゲイン行列Ｋ_ｋを算出する（ステップＳＴ６）。平滑器６では、追尾目標毎に、順序による相関判定器４で判定した相関結果と、擬似データ生成器１５で生成した擬似的な観測ベクトルと、予測器１１の出力した予測ベクトルアンダバーｘ_ｋハット（−）と、ゲイン行列算出器５からゲイン行列Ｋ_ｋをそれぞれ入力し、式（２６）に従い平滑ベクトルアンダバーｘ_ｋハット（＋）を算出する（ステップＳＴ７）。
【００９１】
一方、平滑誤差評価器８では、予測誤差評価器１０から予測誤差共分散行列Ｐ_ｋ（−）を、ゲイン行列算出器５からゲイン行列Ｋ_ｋをそれぞれ入力し、式（２７）に従い平滑誤差共分散行列Ｐ_ｋ（＋）を算出する（ステップＳＴ８）。予測誤差評価器１０では、第２の遅延回路９から１サンプリング前の平滑誤差共分散行列Ｐ_ｋ−１（＋）を、予め設定された駆動雑音Ｑ_ｋ−１をそれぞれ入力し、予測誤差共分散行列Ｐ_ｋ（−）を式（１８）に従い算出する（ステップＳＴ９）。予測器１１では、１サンプリング前の平滑ベクトルアンダバーｘ_ｋ−１ハット（＋）を第１の遅延回路７を介して入力し、式（１７）に従い予測ベクトルアンダバーｘ_ｋハット（−）を算出する（ステップＳＴ１０）。
【００９２】
航跡の順序判定器１２では、平滑器６において１サンプリング前に算出された追尾目標の平滑ベクトルを第１の遅延回路７を介して入力し、その位置ベクトルアンダバーξ_ｋ−１ ^ｊと、予め設定された車両の進行方向の単位ベクトルアンダバーｅ_ｄとの内積Ψ_ｋ−１ ^ｊを式（２２）に従い算出する。そして、内積Ψ_ｋ−１ ^ｊの大きい順番を航跡の走行順序として記憶する（ステップＳＴ１１）。グループ判定機１３では、航跡の順序判定機１２で判定された追尾車両の走行順序の順番に隣り合う平滑ベクトルの位置ベクトルの距離ｒが予め設定されたパラメータｒ_ｍｉｎに対して式（２３）を満たすか否かを調べ、式（２３）を満たす航跡同士を同一グループと判定する（ステップＳＴ１２）。
【００９３】
以上で明らかなように、この実施の形態３によれば、観測データの順序判定器３により、車両の進行方向に対する観測ベクトルの存在順序を、予め設定された車両の進行方向の単位ベクトルと観測ベクトルの内積の大きさから判定し、一方、航跡の順序判定器１２により、車両の走行順序を、上記単位ベクトルと追尾車両の平滑ベクトルの位置ベクトルの内積の大きさから判定し、順序による相関判定器４において、走行順序の順番に、追尾車両の相関ゲート内の観測ベクトルのうち存在順序が先頭側の観測ベクトルを当該追尾車両と相関があるものと判定する構成としているので、追尾車両の航跡の順序が入れ替わりにくい効果を奏する。更に、擬似データ生成器１５において、相関する観測データのない追尾目標に対して、その前後もしくはどちらか一方の相関の取れた追尾目標の１サンプリング前の平滑位置と現時刻に相関の取れた観測ベクトルを用いて擬似的な観測ベクトルを生成して平滑する構成としているので、レーダ装置１が失検出した場合においても追尾車両の航跡の順序が入れ替わりにくい効果を奏する。
【００９４】
実施の形態４．
図７は、この実施の形態４による目標追尾装置を示す構成図であり、図において、図４と同一符号である１〜３及び５〜１４は実施の形態２と同一または相当部分を示すので、その説明を省略する。１５は順序と残差による相関判定器１４で判定された追尾目標と観測データの相関結果において、相関する観測データのない追尾目標に対して、擬似的な観測データを生成する擬似データ生成器である。
【００９５】
次に動作について説明する。最初に、この実施の形態４による目標追尾装置の動作原理を説明する。レーダ装置１による車両の観測方法、目標の運動モデル、レーダ装置１の観測モデルは、実施の形態１における式（１）〜式（１６）までの原理と同じであるので省略する。
【００９６】
また、相関判定方法は実施の形態２と同じ原理であるので省略する。また、予測処理及び相関ゲート判定処理は、実施の形態１における式（１７）〜式（２０）までの原理と同じであるので省略する。また、追尾車両の走行順序を判定する方法、追尾車両のグループを判定する方法、そして、観測ベクトルの存在順序を判定する方法も、実施の形態１における式（２１）〜式（２４）までの原理と同じであるので省略する。
【００９７】
相関する観測データのない追尾目標が存在する場合の擬似的な観測データの生成方法は実施の形態３における式（２９）〜式（３０）までの原理と同じであるので省略する。
【００９８】
次に、この実施の形態４による目標追尾装置の動作を具体的に説明する。なお、カルマンフィルタを目標追尾装置に通常適用する場合と同様にして状態ベクトルの初期値は別途定まっているものとする。
【００９９】
レーダ装置１では、図２に示すように目標の距離Ｒ及び方位角Ａｚを観測し、式（１）及び式（２）に従いｘｙ座標に変換する（ステップＳＴ１）。相関ゲート判定器２では、予測器１１から追尾対象の予測ベクトルアンダバーｘ_ｋハット（−）を入力して観測ベクトルのベクトルアンダバーｚ_ｋ（−）を式（１９）に従い算出し、レーダ装置１から入力した観測ベクトルのうち、予め設定されたゲートサイズパラメータρに対して式（２０）を満たす観測ベクトルの全てを当該追尾目標の相関ゲート内の観測ベクトルとして選択する。これを全ての追尾目標に対して行う（ステップＳＴ２）。観測データの順序判定器３では、グループ判定器１３により判定されたグループ毎に、そのグループに所属する追尾目標の相関ゲート内の観測ベクトルに対し、予め設定された車両の進行方向の単位ベクトルアンダバーｅ_ｄとの内積λ_ｋ−１ ^ｉを式（２４）に従い算出する。そして、内積λ_ｋ−１ ^ｉの大きい順番を観測ベクトルの存在順序として記憶する（ステップＳＴ３）。
【０１００】
順序と残差による相関判定器１４では、グループ判定器１３において判定されたグループ毎に、観測データの順序判定器３により判定されたそのグループに所属する追尾目標の相関ゲート内の観測ベクトルの存在順序の情報及び残差と、航跡の順序判定器１２で判定されたそのグループに所属する追尾車両の走行順序の情報を入力し、走行順序の順番に、その追尾車両の相関ゲート内の観測ベクトルのうち残差が最も小さい観測ベクトルを当該追尾車両と相関があると判定してその組み合わせを記憶する。このとき、相関があると判定された観測ベクトルは後に続く追尾車両の相関対象から除く。そして、全てのグループに対して同様の処理を行う。（ステップＳＴ４）。
【０１０１】
擬似データ生成器１５では、順序と残差による相関判定器１４で判定された追尾目標と観測データの相関結果と、現在時刻より１サンプリング前に平滑器６で算出された平滑ベクトルを第１の遅延回路を介して入力し、グループ毎に、相関する観測データのない追尾目標に対して、その前後に相関の取れた追尾目標がある場合には式（２９）に従い、前後のどちらか一方に追尾目標がある場合には式（３０）に従い擬似的な観測ベクトルを生成する（ステップＳＴ５）。
【０１０２】
ゲイン行列算出器５では、追尾目標毎に、予測誤差評価器１０から入力した予測誤差共分散行列Ｐ_ｋ（−）と、予め設定されたレーダ装置１の観測誤差共分散行列Ｒ_ｋとから式（２５）に従いゲイン行列Ｋ_ｋを算出する（ステップＳＴ６）。平滑器６では、追尾目標毎に、順序による相関判定器４で判定した相関結果と、擬似データ生成器１５で生成した擬似的な観測ベクトルと、予測器１１の出力した予測ベクトルアンダバーｘ_ｋハット（−）と、ゲイン行列算出器５からゲイン行列Ｋ_ｋをそれぞれ入力し、式（２６）に従い平滑ベクトルアンダバーｘ_ｋハット（＋）を算出する（ステップＳＴ７）。
【０１０３】
一方、平滑誤差評価器８では、予測誤差評価器１０から予測誤差共分散行列Ｐ_ｋ（−）を、ゲイン行列算出器５からゲイン行列Ｋ_ｋをそれぞれ入力し、式（２７）に従い平滑誤差共分散行列Ｐ_ｋ（＋）を算出する（ステップＳＴ８）。予測誤差評価器１０では、第２の遅延回路９から１サンプリング前の平滑誤差共分散行列Ｐ_ｋ−１（＋）を、予め設定された駆動雑音Ｑ_ｋ−１をそれぞれ入力し、予測誤差共分散行列Ｐ_ｋ（−）を式（１８）に従い算出する（ステップＳＴ９）。予測器１１では、１サンプリング前の平滑ベクトルアンダバーｘ_ｋ−１ハット（＋）を第１の遅延回路７を介して入力し、式（１７）に従い予測ベクトルアンダバーｘ_ｋハット（−）を算出する（ステップＳＴ１０）。
【０１０４】
航跡の順序判定器１２では、平滑器６において１サンプリング前に算出された追尾目標の平滑ベクトルを第１の遅延回路７を介して入力し、その位置ベクトルアンダバーξ_ｋ−１ ^ｊと、予め設定された車両の進行方向の単位ベクトルアンダバーｅ_ｄとの内積Ψ_ｋ−１ ^ｊを式（２２）に従い算出する。そして、内積Ψ_ｋ−１ ^ｊの大きい順番を航跡の走行順序として記憶する（ステップＳＴ１１）。グループ判定機１３では、航跡の順序判定機１２で判定された追尾車両の走行順序の順番に隣り合う平滑ベクトルの位置ベクトルの距離ｒが予め設定されたパラメータｒ_ｍｉｎに対して式（２３）を満たすか否かを調べ、式（２３）を満たす航跡同士を同一グループと判定する（ステップＳＴ１２）。
【０１０５】
以上で明らかなように、この実施の形態４によれば、観測データの順序判定器３により、車両の進行方向に対する観測ベクトルの存在順序を、予め設定された車両の進行方向の単位ベクトルと観測ベクトルの内積の大きさから判定し、一方、航跡の順序判定器１２により、車両の走行順序を、上記単位ベクトルと追尾車両の平滑ベクトルの位置ベクトルの内積の大きさから判定し、順序と残差による相関判定器１４において、走行順序の順番に、追尾車両の相関ゲート内の観測ベクトルのうち残差が最も小さい観測ベクトルを当該追尾車両と相関があるものと判定し、それより前方に存在する観測ベクトルを以降の追尾目標の相関対象から除く構成としているので、追尾車両の航跡の順序が入れ替わりにくく、かつ、レーダ装置１により誤検出が検出される場合においても誤った相関結果となりにくい効果を奏する。更に、擬似データ生成器１５において、相関する観測データのない追尾目標に対して、その前後もしくはどちらか一方の相関の取れた追尾目標の１サンプリング前の平滑位置と現時刻に相関の取れた観測ベクトルを用いて擬似的な観測ベクトルを生成して平滑する構成としているので、レーダ装置１が失検出した場合においても追尾車両の航跡の順序が入れ替わりにくい効果を奏する。
【０１０６】
実施の形態５．
図８はこの実施の形態５による目標追尾装置を示す構成図であり、図において、図６と同一符号である１〜１３、１５は実施の形態３と同一または相当部分を示すので、その説明を省略する。また、図４と同一符号である１４は実施の形態２と同一または相当部分を示すので、その説明を省略する。
【０１０７】
１６はグループ判定器１３で判定された追尾目標のグループと、相関ゲート判定器２で判定したそのグループに属する追尾目標の相関ゲート内の観測データを観測データの順序判定器３を介して入力し、同一グループ内の追尾目標数と、その追尾目標の相関ゲート内に存在する観測データ数に応じて、相関判定処理を順序による相関判定器１５あるいは順序と残差による相関判定器１４の何れかに切り換える相関処理切り換え器１６である。
【０１０８】
次に動作について説明する。最初に、この実施の形態５による目標追尾装置の動作原理を説明する。レーダ装置１による車両の観測方法、目標の運動モデル、レーダ装置１の観測モデルは、実施の形態１における式（１）〜式（１６）までの原理と同じであるので省略する。
【０１０９】
また、相関判定方法は実施の形態２と同じ原理であるので省略する。また、予測処理及び相関ゲート判定処理は、実施の形態１における式（１７）〜式（２０）までの原理と同じであるので省略する。また、追尾車両の走行順序を判定する方法、追尾車両のグループを判定する方法、そして、観測ベクトルの存在順序を判定する方法も、実施の形態１における式（２１）〜式（２４）までの原理と同じであるので省略する。
【０１１０】
相関する観測データのない追尾目標が存在する場合の擬似的な観測データの生成方法は実施の形態３における式（２９）〜式（３０）までの原理と同じであるので省略する。
【０１１１】
次に、相関処理の切り換え方法について説明する。各グループにおいて、追尾車両数と観測データ数が等しい場合には、レーダ装置１の検出状態がよいと考えられる。この場合は、追尾車両と観測データの残差の情報を用いた相関判定を行うよりも、追尾目標と観測データの順序を合わせて１対１対応をとる方が簡単かつ相関ミスがないと考えられる。一方、追尾車両より観測データ数が多い場合にはレーダ装置１が誤検出をしていると考えられるので、順序と残差の情報を用いて相関判定を行う方法がよいと考えられる。逆に追尾車両より観測データ数が少ない場合にはレーダ装置１が失検出していると考えられるので、順序と残差の情報を用いて相関判定を行い、更に、相関の取れない追尾目標に対する擬似観測データを生成して平滑する方法がよいと考えられる。
【０１１２】
次に、この実施の形態５による目標追尾装置の動作を具体的に説明する。なお、カルマンフィルタを目標追尾装置に通常適用する場合と同様にして状態ベクトルの初期値は別途定まっているものとする。
【０１１３】
レーダ装置１では、図２に示すように目標の距離Ｒ及び方位角Ａｚを観測し、式（１）及び式（２）に従いｘｙ座標に変換する（ステップＳＴ１）。相関ゲート判定器２では、予測器１１から追尾対象の予測ベクトルアンダバーｘ_ｋハット（−）を入力して観測ベクトルのベクトルアンダバーｚ_ｋ（−）を式（１９）に従い算出し、レーダ装置１から入力した観測ベクトルのうち、予め設定されたゲートサイズパラメータρに対して式（２０）を満たす観測ベクトルの全てを当該追尾目標の相関ゲート内の観測ベクトルとして選択する。これを全ての追尾目標に対して行う（ステップＳＴ２）。観測データの順序判定器３では、グループ判定器１３により判定されたグループ毎に、そのグループに所属する追尾目標の相関ゲート内の観測ベクトルに対し、予め設定された車両の進行方向の単位ベクトルアンダバーｅ_ｄとの内積λ_ｋ−１ ^ｉを式（２４）に従い算出する。そして、内積λ_ｋ−１ ^ｉの大きい順番を観測ベクトルの存在順序として記憶する（ステップＳＴ３）。
【０１１４】
相関処理切り換え器１６では、グループ判定器１３で判定された追尾目標のグループと、相関ゲート判定器２で判定したそのグループに属する追尾目標の相関ゲート内の観測データを観測データの順序判定器３を介して入力し、各グループ毎に、グループの属する追尾目標数と、その追尾目標の相関ゲート内に存在する観測データ数に応じて次の処理分岐を行う。追尾車両数と観測データ数が等しい場合には、順序による相関判定器４の処理に移る。追尾車両と観測データ数が異なる場合には順序と残差による相関判定器１４の処理に移る（ステップＳＴ４）。
【０１１５】
順序による相関判定器４では、グループ判定器１３において判定されたグループ毎に、観測データの順序判定器３により判定されたそのグループに所属する追尾目標の相関ゲート内の観測ベクトルの存在順序の情報を相関処理切り換え器１６を介して入力し、また、航跡の順序判定器１２で判定されたそのグループに所属する追尾車両の走行順序の情報を入力し、走行順序の順番に、その追尾車両の相関ゲート内の観測ベクトルのうち存在順序が先頭側の観測ベクトルを当該追尾車両と相関があると判定してその組み合わせを記憶する。このとき、相関があると判定された観測ベクトルは後に続く追尾車両の相関対象から除く。そして、全てのグループに対して同様の処理を行う。（ステップＳＴ５）。
【０１１６】
順序と残差による相関判定器１４では、グループ判定器１３において判定されたグループ毎に、観測データの順序判定器３により判定されたそのグループに所属する追尾目標の相関ゲート内の観測ベクトルの存在順序の情報及び残差を相関処理切り換え器１６を介して入力し、また、航跡の順序判定器１２で判定されたそのグループに所属する追尾車両の走行順序の情報を入力し、走行順序の順番に、その追尾車両の相関ゲート内の観測ベクトルのうち残差が最も小さい観測ベクトルを当該追尾車両と相関があると判定してその組み合わせを記憶する。このとき、相関があると判定された観測ベクトルは後に続く追尾車両の相関対象から除く。そして、全てのグループに対して同様の処理を行う。（ステップＳＴ６）。
【０１１７】
擬似データ生成器１５では、順序と残差による相関判定器１４で判定された追尾目標と観測データの相関結果と、原時刻より１サンプリング前に平滑器６で算出された平滑ベクトルを第１の遅延回路を介して入力し、グループ毎に、相関する観測データのない追尾目標に対して、その前後に相関の取れた追尾目標がある場合には式（２９）に従い、前後のどちらか１方に追尾目標がある場合には式（３０）に従い擬似的な観測ベクトルを生成する（ステップＳＴ７）。
【０１１８】
ゲイン行列算出器５では、追尾目標毎に、予測誤差評価器１０から入力した予測誤差共分散行列Ｐ_ｋ（−）と、予め設定されたレーダ装置１の観測誤差共分散行列Ｒ_ｋとから式（２５）に従いゲイン行列Ｋ_ｋを算出する（ステップＳＴ８）。平滑器６では、追尾目標毎に、順序による相関判定器４で判定した相関結果と、擬似データ生成器１５で生成した擬似的な観測ベクトルと、予測器１１の出力した予測ベクトルアンダバーｘ_ｋハット（−）と、ゲイン行列算出器５からゲイン行列Ｋ_ｋをそれぞれ入力し、式（２６）に従い平滑ベクトルアンダバーｘ_ｋハット（＋）を算出する（ステップＳＴ９）。
【０１１９】
一方、平滑誤差評価器８では、予測誤差評価器１０から予測誤差共分散行列Ｐ_ｋ（−）を、ゲイン行列算出器５からゲイン行列Ｋ_ｋをそれぞれ入力し、式（２７）に従い平滑誤差共分散行列Ｐ_ｋ（＋）を算出する（ステップＳＴ１０）。予測誤差評価器１０では、第２の遅延回路９から１サンプリング前の平滑誤差共分散行列Ｐ_ｋ−１（＋）を、予め設定された駆動雑音Ｑ_ｋ−１をそれぞれ入力し、予測誤差共分散行列Ｐ_ｋ（−）を式（１８）に従い算出する（ステップＳＴ１１）。予測器１１では、１サンプリング前の平滑ベクトルアンダバーｘ_ｋ−１ハット（＋）を第１の遅延回路７を介して入力し、式（１７）に従い予測ベクトルアンダバーｘ_ｋハット（−）を算出する（ステップＳＴ１２）。
【０１２０】
航跡の順序判定器１２では、平滑器６において１サンプリング前に算出された追尾目標の平滑ベクトルを第１の遅延回路７を介して入力し、その位置ベクトルアンダバーξ_ｋ−１ ^ｊと、予め設定された車両の進行方向の単位ベクトルアンダバーｅ_ｄとの内積Ψ_ｋ−１ ^ｊを式（２２）に従い算出する。そして、内積Ψ_ｋ−１ ^ｊの大きい順番を航跡の走行順序として記憶する（ステップＳＴ１３）。グループ判定機１３では、航跡の順序判定機１２で判定された追尾車両の走行順序の順番に隣り合う平滑ベクトルの位置ベクトルの距離ｒが予め設定されたパラメータｒ_ｍｉｎに対して式（２３）を満たすか否かを調べ、式（２３）を満たす航跡同士を同一グループと判定する（ステップＳＴ１４）。
【０１２１】
以上で明らかなように、この実施の形態５によれば、観測データの順序判定器３により、車両の進行方向に対する観測ベクトルの存在順序を、予め設定された車両の進行方向の単位ベクトルと観測ベクトルの内積の大きさから判定し、一方、航跡の順序判定器１２により、車両の走行順序を、上記単位ベクトルと追尾車両の平滑ベクトルの位置ベクトルの内積の大きさから判定し、相関処理切り換え器１６により、追尾車両と観測データ数が等しく、レーダ装置１の検出状態が良い場合には、順序による相関判定器４において、走行順序の順番に、追尾車両の相関ゲート内の観測ベクトルのうち存在順序が先頭側の観測ベクトルを当該追尾車両と相関があるものと判定し、追尾車両と観測データが等しくない場合には、順序と残差による相関判定器１４において、走行順序の順番に、追尾車両の相関ゲート内の観測ベクトルのうち残差が最も小さい観測ベクトルを当該追尾車両と相関があるものと判定し、それより前方に存在する観測ベクトルを以降の追尾目標の相関対象から除く構成としているので、レーダ装置１により誤検出がある場合や、失検出がある場合においても誤った相関結果となりにくい効果を奏する。
【０１２２】
【発明の効果】
以上のように、この発明によれば、例えば、空港面の誘導路や滑走路を走行する航空機や、車線幅の狭い一方通行路を走行する車両のように、追尾対象となる複数の目標の運動がレールの上を走る電車のようにあるコース上に限定され、追い越しや追い抜きができないまたは運行上ありえない状況において追尾した航跡が実際の目標の順序と入れ替わりにくい目標追尾装置及び方法を得ることができるという効果がある。
【図面の簡単な説明】
【図１】 この発明の実施の形態１による目標追尾装置を示す構成図である。
【図２】 この発明の実施の形態１による目標追尾装置の動作原理を説明する図である。
【図３】 この発明の実施の形態１による目標追尾装置における追尾目標と観測ベクトルの順序情報を利用した相関判定方法の説明図である。
【図４】 この発明の実施の形態２による目標追尾装置を示す構成図である。
【図５】 この発明の実施の形態２による目標追尾装置における追尾目標と観測ベクトルの順序情報を利用した相関判定方法の説明図である。
【図６】 この発明の実施の形態３による目標追尾装置を示す構成図である。
【図７】 この発明の実施の形態４による目標追尾装置を示す構成図である。
【図８】 この発明の実施の形態５による目標追尾装置を示す構成図である。
【図９】 Multiple-Target Tracking with Radar Application, ARTECK HOUSE, 1986, pp299-302に示された従来の目標追尾装置を示す構成図である。
【符号の説明】
１ レーダ装置、２ 相関ゲート判定器、３ 観測データ順序判定器、４ 順序による相関器、５ ゲイン行列算出器、６ 平滑器、７ 第１の遅延器、８ 平滑誤差評価器、９ 第２の遅延器、１０ 予測誤差評価器、１１ 予測器、１２ 航跡順序判定器、１３ グループ判定器、１４ 順序と残差による相関器、１５ 疑似データ生成器、１６ 相関処理切り換え器。[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a variety of motions such as residuals, depression angles (elevation angles), azimuth angles, positions, or time-varying rates thereof observed by a radar, a laser, a camera, etc. The present invention relates to a target tracking device that estimates a source.
[0002]
[Prior art]
FIG. 9 is a block diagram showing a conventional target tracking device shown in, for example, Multiple-Target Tracking with Radar Application, ARTECK HOUSE, 1986, pp299-302. In FIG. 9, reference numeral 1 denotes a radar apparatus that observes the position of a target, and reference numeral 2 denotes a correlation gate determination unit that selects an observation vector correlated with a tracking target from observation vectors observed by the radar apparatus 1 based on the residual. , 17 is a reliability calculator for calculating the reliability of the observation data selected by the correlation gate determination unit 2, and 5 is a prediction error covariance matrix calculated by the prediction error evaluator 10 and a preset radar apparatus 1. A gain matrix calculator 18 for calculating a gain matrix from the observation error covariance matrix, 18 is a reliability of the observation vector calculated by the reliability calculator 17, a prediction vector calculated by the predictor 11, and a gain matrix calculator 5 is a smoother based on the reliability for calculating the smooth vector of the target position and velocity using the gain matrix calculated by 5, and 7 is a smoothing vector calculated by the smoother 18 based on the reliability. A torque only one sampling time is a first delay circuit for delaying.
[0003]
8 is a residual vector calculated by the smoother 18 based on reliability, a weighted residual vector based on reliability, a gain matrix calculated by the gain matrix calculator 5, and a prediction calculated by the prediction error evaluator 10. A smoothing error evaluator that calculates a smoothing error covariance matrix using the error covariance matrix, and 9 is a second delay circuit that delays the smoothing error covariance matrix calculated by the smoothing error evaluator 8 by one sampling time. 10 is a prediction error evaluator that calculates a prediction error covariance matrix after one sampling from the current time from the smoothing error covariance matrix calculated by the smoothing error evaluator 8, and 11 is output from the smoother 18 based on reliability. A smooth vector consisting of the target position and velocity to be input is input via the first delay circuit 7, and one sampling is performed from the current time using this and a preset target motion model. Prediction vector comprising from predicted position and predicted velocity of the target is a predictor for calculating a.
[0004]
Next, the operation will be described. When the correlation gate determination unit 2 receives the observation vector from the radar apparatus 1, the correlation gate determination unit 2 employs the observation vector as valid data only when the observation vector is likely to be an observation vector related to the target. The target predicted existence probability distribution is calculated from the predicted prediction error covariance matrix and the preset observation error covariance matrix of the radar apparatus 1, and the observation target existing within the desired equiprobability range of this distribution is tracked. Are output to the reliability calculator 17.
[0005]
When the reliability calculator 17 receives the observation vector correlated with the target output from the correlation gate determination unit 2, the reliability calculator 17 inputs the prediction vector output from the predictor 11, and detects these with the radar apparatus 1 set in advance. The reliability of the observation vector is calculated from the probability and the frequency of unnecessary signal generation.
[0006]
The gain matrix calculator 5 calculates a gain matrix from the prediction error covariance matrix output from the prediction error evaluator 10 and a preset observation error covariance matrix of the radar apparatus 1.
[0007]
The smoother 18 based on reliability calculates a weighted residual vector based on reliability from the reliability of the observation vector calculated by the reliability calculator 17 and the prediction vector calculated by the predictor 11, and the gain. Using the gain matrix calculated by the matrix calculator 5, a smooth vector composed of the target position and velocity is calculated.
[0008]
The smoothing error evaluator 8 includes a residual vector calculated by the smoother 18 based on reliability, a weighted residual vector based on reliability, a gain matrix calculated by the gain matrix calculator 5, and a prediction error evaluator 10. Using the calculated prediction error covariance matrix, a smooth error covariance matrix for evaluating the smooth error is calculated.
[0009]
Then, the prediction error evaluator 10 calculates a prediction error covariance matrix for evaluating the prediction error from the smooth error covariance matrix output from the smooth error evaluator 8.
[0010]
When the smoother 18 based on the reliability calculates the smooth vector composed of the target position and velocity as described above, the predictor 11 uses the preset target motion model and samples one sample after the current time. A target prediction vector is calculated.
[0011]
[Problems to be solved by the invention]
The conventional target tracking device is configured as described above, and the correlation between the tracking target and the observation data is determined to be higher for the observation data closer to the predicted position of the tracking target, and smoothing processing is performed. When the target is approaching, a false acceleration / deceleration motion caused by the observation error of the target observation device causes a phenomenon in which the predicted positions for multiple targets are switched between adjacent targets, and the actual positional relationship between the targets. Different cases occur.
[0012]
The present invention has been made to solve the above-described problems, and provides a target tracking device and method in which a track that is tracked is less likely to be replaced with an actual target order in a situation where overtaking or overtaking is impossible or impossible in operation. The purpose is to obtain.
[0013]
[Means for Solving the Problems]
  A target tracking device according to the present invention includes an observation unit that observes a target position, a prediction unit that calculates a predicted value of a target state, and a smoothing unit that calculates a smooth value of the target state. , The wake order determining means for determining the order of the arrangement of wake positions with respect to a desired direction using the smoothed value calculated by the smoothing means, and the wake having a smooth position close to the wake order based on the determination result of the wake order determining means Group determination means for recognizing each other as a group, and the group determination meansFor each group determined by, the observation vector obtained in the correlation gate of the tracking target belonging to that groupBased on the determination results of the observation data order determination means for determining the arrangement order of the positions, the group determination means and the observation data order determination meansWhen the tracking order of a tracking vehicle belonging to the same group is correlated with the tracking vehicle, the observation vector in the order of presence among the observation vectors in the correlation gate of the tracking vehicle is correlated with the tracking vehicle.Correlation means according to the order of determination, wherein the smoothing means calculates a smooth value of a target state based on outputs of the correlation means and the prediction means.
[0014]
The correlation means determines the correlation between the wake and the observation data based on the order of the positions of the wake and the observation data and the residual.
[0015]
The correlation means further includes pseudo data generation means for generating pseudo observation data for interpolating smoothness of a wake having no correlated observation data.
[0016]
  Further, as the correlation means, a first correlation means for determining a correlation between the wake and the observation data based on an order of arrangement of the wake and the observation data, an order of the arrangement of the wake and the observation data, and a residual Second correlation means for determining the correlation between the wake and the observation data based on the first correlation means according to the magnitude relationship between the number of wakes and the number of observation data.And beforeCorrelation processing switching means for switching between the second correlation means, and pseudo observation data for interpolating smoothness of a wake having no correlated observation data in the first correlation means or the second correlation means. And a data generation means.
[0017]
  In addition, the target tracking method according to the present invention includes an observation step for observing a target position, a prediction step for calculating a predicted value of the target state, and a smoothing step for calculating a smooth value of the target state. In the tracking method, the smooth position calculated based on the determination result of the wake order determination step and the wake order determination step for determining the order of the arrangement of the wake positions with respect to a desired direction using the smooth value calculated by the smoothing step. A group determination step for recognizing close wakes as a group, and the group determination stepFor each group determined by, the observation vector obtained in the correlation gate of the tracking target belonging to that groupBased on the determination results of the observation data order determination step for determining the arrangement order of the positions, the group determination step and the observation data order determination stepWhen the tracking order of a tracking vehicle belonging to the same group is correlated with the tracking vehicle, the observation vector in the order of presence among the observation vectors in the correlation gate of the tracking vehicle is correlated with the tracking vehicle.A correlation step for determining, and the smoothing step calculates a smooth value of a target state based on outputs of the correlation step and the prediction step.
[0018]
The correlation step is characterized in that the correlation between the wake and the observation data is determined based on the sequence of the positions of the wake and the observation data and the residual.
[0019]
The correlation step further includes a pseudo data generation step for generating pseudo observation data for interpolating smoothness of a wake having no correlated observation data.
[0020]
Further, as the correlation step, a first correlation step for determining a correlation between the wake and the observation data based on an order of positions of the wake and the observation data, an order of the position of the wake and the observation data, and a residual And a second correlation step for determining the correlation between the wake and the observation data based on the above, and a correlation process for switching between the first correlation step and the second correlation step according to the magnitude relationship between the number of wakes and the number of observation data The method further comprises a switching step and a pseudo data generation step of generating pseudo observation data for interpolating smoothness of a wake having no observation data correlated in the first correlation step or the second correlation step. Is.
[0021]
DETAILED DESCRIPTION OF THE INVENTION
Embodiment 1 FIG.
FIG. 1 is a block diagram showing a target tracking device according to Embodiment 1 of the present invention. In the figure, reference numeral 1 denotes a radar device as an observing means for radiating a radio wave to an area where a target is present, detects the target from the reflected signal, and outputs the position, and 2 is an observation vector observed by the radar device 1. A correlation gate determination unit that selects an observation vector correlated with the tracking target based on the residual, and 3, a tracking target group information determined by the group determination unit 13 and a tracking selected by the correlation gate determination unit 2 Using the correlation result between the target and the observation data and a preset target traveling direction vector, the observation data order determiner 4 determines the order of the observation data corresponding to each group. The observation data in the correlation gate of the determined tracking target, the order information of the observation data corresponding to each group determined by the observation data order determination unit 3, and the group determination unit 13 Using the group information of the tracking target, a correlation determination unit according to the order determined correlation of the observation data and tracking targets.
[0022]
5 is a gain matrix calculator that calculates a gain matrix from the prediction error covariance matrix calculated by the prediction error evaluator 10 and a preset observation error covariance matrix of the radar apparatus 1, and 6 is a correlation determination unit by order. Using the correlation result between the tracking target determined in 4 and the observation data, the prediction vector calculated by the predictor 11, and the gain matrix calculated by the gain matrix calculator 4, a smooth vector of the target position and velocity is obtained. A smoothing device 7 for calculating is a first delay circuit for delaying the smoothing vector calculated by the smoothing device 6 by one sampling time.
[0023]
8 is a residual vector calculated by the smoother 6 and a weighted residual vector based on reliability, a gain matrix calculated by the gain matrix calculator 5, and a prediction error covariance calculated by the prediction error evaluator 10. A smoothing error evaluator that calculates a smoothing error covariance matrix using the matrix, 9 is a second delay circuit that delays the smoothing error covariance matrix calculated by the smoothing error evaluator 8 by one sampling time, and 10 is a smoothing A prediction error evaluator that calculates a prediction error covariance matrix after one sampling from the current time from the smooth error covariance matrix calculated by the error evaluator 8, and 11 is a smoothing vector calculated by the smoother 6 as a first A prediction vector which is input via the delay circuit 7 and calculates a prediction vector composed of a predicted position and a predicted speed of the target after one sampling from the current time using this and a preset target motion model. It is a vessel.
[0024]
12 is a sequence of wakes that inputs the smoothed vector calculated by the smoother 6 via the first delay circuit 7 and determines the order of the tracking target using this and a preset target traveling direction vector. A determinator 13 is a group determinator that groups tracking targets having close smooth positions in accordance with the order of tracking targets determined by the wake order determining unit 12.
[0025]
Next, the operation will be described. First, the operation principle of the target tracking device according to the first embodiment will be described. As shown in FIG. 2, the radar apparatus 1 is at a height z from the ground._rIt is assumed that a vehicle that is installed at a position and runs on a straight road in a certain direction is tracked. O-xyz coordinates with the intersection of the vertical line of the antenna mounting position of the radar apparatus 1 and the ground plane as the origin, the road crossing direction as the x axis, the direction along the road as the y axis, and the vertical upward as the z axis are positive. Take reference coordinates. The antenna mounting position of the radar device 1 at the reference coordinates is (0, 0, z_r).
[0026]
The radar apparatus 1 observes a target distance R and an azimuth angle Az with respect to the antenna mounting position (counterclockwise is positive from the positive y-axis direction). These polar coordinates (R, Az) are taken as radar observation coordinates. From FIG. 1, there is a relationship of Expression (1) and Expression (2) between the xy coordinates and the radar coordinates.
[0027]
[Expression 1]

[0028]
Next, the target motion model is shown in Equation (3). However, underbar x_kIs a state vector representing the true value of the target motion specification at the sampling time. When the target position vector in the xy coordinates is expressed by equation (4) and the velocity vector is expressed by equation (5), the target state vector is expressed by equation (6). expressed. Underbar A^TRepresents a transposed vector of the vector underbar A.
[0029]
[Expression 2]

[0030]
Where Φ_k-1Is the sampling time t_k-1From t_kIs a transition matrix of the state vector to, and is expressed by Equation (7). Also, underbar w_kIs the sampling time t_kIs the drive noise vector at Γ₁(K) is the sampling time t_kIs a conversion matrix of a drive noise vector in FIG. For example, if the target motion model is assumed to be constant velocity linear motion, the truncation error term is₁(K-1) Underbar w_k-1And underbar w_kIs equivalent to the acceleration vector, Γ₁(K-1) is represented by Formula (8). T is a sampling interval of the radar apparatus 1 and I is a unit matrix of 2 rows and 2 columns.
[0031]
[Equation 3]

[0032]
If E is used as a symbol representing the average, the underbar w_kIs an average two-dimensional normal distribution white noise, and is represented by equations (7) and (8). However, underbar 0 is a zero vector and Q_kIs the sampling time t_kIs a driving noise covariance matrix.
[0033]
[Expression 4]

[0034]
Next, the target distance R and azimuth angle Az are determined by sampling time t._kThe observation model of the radar apparatus 1 in the xy coordinates when observed from the radar apparatus 1 is expressed by Equation (11). Here, the distance R and the azimuth angle Az are converted into xy coordinates by the equations (1) and (2). Also, underbar z_kIs the sampling time t_kThe observation vector H of the radar observation apparatus 1 in FIG. 1 is an observation matrix, which is expressed by Expression (12).
[0035]
And underbar ν_kIs the sampling time t_kIs a two-dimensional normal distribution white noise with an average underbar 0, and is expressed by equations (13) and (14). R_kIs the sampling time t_k2 is an observation error covariance matrix of the radar apparatus 1 in FIG. Γ₂(K) is a conversion matrix of an observation noise vector from polar coordinates to xy coordinates, and is expressed by Expression (15). Sampling time t_kThe entire observation vector of the radar device 1 used for tracking the target by Z_k(See equation (16)).
[0036]
[Equation 5]

[0037]
Next, sampling time t_k-1Observation vector Z of radar device 1 up to_k-1The prediction process when is obtained is described. Sampling time t on the assumption that the target is moving at a constant linear velocity_kState vector underbar x of the tracking target in_kPredictor vector of underbar x_kHat (-), prediction error covariance matrix P_k(−) Is defined by a conditional mean vector and a conditional covariance matrix, respectively, and is expressed by Expression (17) and Expression (18). Where Underbar x_kHat (+) and P_k-1(+) Is the previous sampling time t_k-1The smoothing vector and the smoothing error covariance matrix. These calculation methods will be described later.
[0038]
[Formula 6]

[0039]
Next, correlation gate determination processing will be described. The prediction vector of the tracking target observation vector is expressed by Equation (19). Sampling time t_kM observation vectors observed by the radar apparatus 1 at underbar z_{k, i}(I = 1, 2,..., M), the residual ε from the tracking target_{k, i}Satisfy the equation (20) with respect to a preset gate size parameter ρ, all of the observation vectors are regarded as observation vectors in the correlation gate, and are considered to be correlated with the tracking target. This correlation gate determination is performed for all tracking targets.
[0040]
[Expression 7]

[0041]
Next, a method for determining the traveling order of the tracking vehicle in the road area to be tracked will be described. Sampling time t_kThe position vector underbar ξ of the smooth vector of the j-th tracking target calculated one sampling before_k-1 ^jIs given by equation (21). On the other hand, the unit vector in the positive y-axis direction, which is the traveling direction of the vehicle, is_dThen, the inner product Ψ of these vectors expressed by the equation (22)_k-1 ^jIs the component of the vehicle position vector in the vehicle traveling direction,_k-1 ^jJust look at the magnitude relationship. That is, the inner product Ψ_k-1 ^jThe result of arranging the vehicles j in the descending order is the traveling order of the vehicles.
[0042]
[Equation 8]

[0043]
Next, a method for determining a group of tracking vehicles will be described. Corresponding to the traveling order of the tracking vehicle determined by the above-described processing, the order of the position vector of the smooth vector of the tracking vehicle is changed from the top to the underbar ξ_k-1 ¹, Underbar ξ_k-1 ², Underbar ξ_k-1 ³Rewrite as ... In order from the top, the distance r between the position vectors of adjacent smooth vectors is a preset parameter r_minWhether or not the equation (23) is satisfied is determined, and wakes satisfying the equation (23) are determined as the same group.
[0044]
[Equation 9]

[0045]
Next, a method for determining the existence order of observation vectors in a road area to be tracked will be described. Similar to the method of determining the traveling order of the tracking vehicle, the observation vector exists in the same order as the observation vector represented by Expression (24) and the unit vector underbar e in the vehicle traveling direction._dInner product λ_k-1 ⁱJust look at the magnitude relationship. That is, the inner product λ_k-1 ⁱThe result of arranging the observation vectors in the descending order is the existence order of the observation vectors.
[0046]
[Expression 10]

[0047]
Next, a correlation determination method using order information of the tracking target and the observation vector will be described. In this process, the correlation is determined so as not to reverse the order of the tracking target and the order of the observation vectors so that the tracking vehicle is not overtaken. In the correlation determination, the following processing is performed for each group classified in the group determination of the tracking vehicle.
[0048]
In order from the first tracking vehicle, among the observation vectors in the correlation gate of the tracking vehicle, the observation vector whose existence order is the first side is stored as being correlated with the tracking vehicle. At this time, the observation vector determined to have a correlation is excluded from the subsequent correlation objects of the tracking vehicle. For example, as shown in FIG. 3, it is assumed that three tracking vehicles are determined to be in the same group, and that there are four observation vectors determined to be within the correlation gate of these tracking vehicles.
[0049]
Here, the traveling order of the tracking vehicle is from the top to the underbar ξ_k-1 ¹, Underbar ξ_k-1 ², Underbar ξ_k-1 ³The observation vector exists in the order of the underbar z_{k, 1}, Underbar z_{k, 2}, Underbar z_{k, 3}, Underbar z_{k, 4}, And. FIG. 3 shows the internal / external relationship of these correlation gates. When it is determined that the correlation gate is inside, it is indicated by ○, and when it is determined that it is outside the correlation gate, it is indicated by ×. First, underbar ξ, the leading vehicle_k-1 ¹Pay attention to.
[0050]
Of the observation vectors existing in the correlation gate of this vehicle, the order of existence is the underbar z_{k, 1}So, underbar ξ_k-1 ¹Is underbar z_{k, 1}It is determined that there is a correlation. Next, underbar z_{k, 1}And underbar ξ which is the following traveling vehicle_k-1 ²Pay attention to. Among the observation vectors existing in the correlation gate of this vehicle, the existence order is the underbar z_{k, 2}So, underbar ξ_k-1 ²Is underbar z_{k, 2}It is determined that there is a correlation. Similarly, underbar ξ_k-1 ³Is underbar z_{k, 3}It is determined that there is a correlation.
[0051]
Next, the smoothing process will be described. The observation vector determined to have a correlation with the tracking target by the correlation processing described above is used as the underbar z_kAnd the gain matrix K_k, Smooth vector underbar x_kHat (+) and smoothing error covariance matrix P_k(+) Is given by equations (25) to (27) according to the theory of ordinary Kalman filter. Where R_kIs the sampling time t expressed by equation (17)_kIs an observation error covariance matrix of the radar apparatus 1 in FIG._kIs a residual vector given by equation (28).
[0052]
## EQU11 ##

[0053]
Next, the operation of the target tracking device according to the first embodiment will be specifically described. It is assumed that the initial value of the state vector is determined separately as in the case where the Kalman filter is normally applied to the target tracking device.
[0054]
The radar apparatus 1 observes the target distance R and azimuth angle Az as shown in FIG. 2, and converts them into xy coordinates according to the equations (1) and (2) (step ST1). In the correlation gate determination unit 2, the prediction vector underbar x of the tracking target is output from the predictor 11._kEnter the hat (-) and observe the vector underbar z of the observation vector_kThe hat (−) is calculated according to the equation (19), and among the observation vectors input from the radar apparatus 1, all of the observation vectors satisfying the equation (20) with respect to the gate size parameter ρ set in advance are set as the tracking target. Select as the observation vector in the correlation gate. This is performed for all tracking targets (step ST2).
[0055]
In the observation data order determination unit 3, for each group determined by the group determination unit 13, a preset unit vector under direction of the vehicle with respect to the observation vector in the correlation gate of the tracking target belonging to the group. Bar e_dInner product λ_k-1 ⁱIs calculated according to equation (24). And the inner product λ_k-1 ⁱIs stored as the observation vector existence order (step ST3).
[0056]
In the correlation determination unit 4 by order, for each group determined by the group determination unit 13, information on the existence order of observation vectors in the correlation gate of the tracking target belonging to that group determined by the observation data order determination unit 3. And the information on the traveling order of the tracking vehicle belonging to the group determined by the wake order determination unit 12 is input, and the existence order is first among the observation vectors in the correlation gate of the tracking vehicle in the order of the traveling order. The observation vector on the side is determined to be correlated with the tracking vehicle, and the combination is stored. At this time, the observation vector determined to have correlation is excluded from the correlation target of the following tracking vehicle. Then, the same processing is performed for all groups. (Step ST4).
[0057]
In the gain matrix calculator 5, the prediction error covariance matrix P input from the prediction error evaluator 10 for each tracking target._k(−) And a preset observation error covariance matrix R of the radar apparatus 1_kAnd gain matrix K according to equation (25)_kIs calculated (step ST5). In the smoother 6, for each tracking target, the correlation result determined by the correlation determiner 4 according to the order and the predicted vector underbar x output from the predictor 11_kHat (-) and gain matrix K from gain matrix calculator 5_kAnd the smooth vector underbar x according to the equation (26)_kHat (+) is calculated (step ST6).
[0058]
On the other hand, the smoothing error evaluator 8 receives the prediction error covariance matrix P from the prediction error evaluator 10._k(−) From the gain matrix calculator 5 to the gain matrix K_k, And the smoothing error covariance matrix P according to equation (27)_k(+) Is calculated (step ST7). In the prediction error evaluator 10, the smoothing error covariance matrix P one sampling before from the second delay circuit 9._k-1(+) Represents a preset driving noise covariance matrix Q_k-1, And the prediction error covariance matrix P_k(−) Is calculated according to equation (18) (step ST8). In the predictor 11, the smoothed vector underbar x before one sampling_{k- 1}The hat (+) is input through the first delay circuit 7 and the prediction vector underbar x according to the equation (17)_kA hat (-) is calculated (step ST9).
[0059]
In the wake order determining unit 12, the smoothing vector of the tracking target calculated one sampling before by the smoother 6 is input via the first delay circuit 7, and the position vector underbar ξ_k-1 ^jAnd a preset unit vector underbar e of the traveling direction of the vehicle_dInner product Ψ with_k-1 ^jIs calculated according to equation (22). And inner product Ψ_k-1 ^jIs stored as the wake traveling order (step ST10). In the group determination device 13, the distance r between the position vectors of the smooth vectors adjacent to each other in the order of the tracking vehicle traveling order determined by the wake order determination device 12 is a preset parameter r._minWhether or not the equation (23) is satisfied is determined, and wakes satisfying the equation (23) are determined as the same group (step ST11).
[0060]
As is apparent from the above, according to the first embodiment, the observation data order determiner 3 determines the observation vector presence order with respect to the traveling direction of the vehicle and the preset unit vector of the traveling direction of the vehicle and the observation. Judgment is based on the magnitude of the inner product of the vectors. On the other hand, the wake order judging unit 12 judges the vehicle running order from the magnitude of the inner product of the unit vector and the position vector of the smooth vector of the tracking vehicle, The determination unit 4 is configured to determine, in the order of traveling order, the observation vector in the order of existence among the observation vectors in the correlation gate of the tracking vehicle that is correlated with the tracking vehicle. There is an effect that the order of wakes is not easily changed.
[0061]
Embodiment 2. FIG.
FIG. 4 is a block diagram showing the target tracking device according to the second embodiment. In the figure, reference numerals 1 to 3 and 5 to 13 which are the same as those in FIG. 1 indicate the same or corresponding parts as those in the first embodiment. The description is omitted.
[0062]
14 is input with the observation data in the correlation gate of the tracking target determined by the correlation gate determination unit 2 and its residual, and the observation data corresponding to each group determined by the order determination unit 3 of the observation data This is a correlation determiner based on the order and the residual for determining the correlation between the tracking target and the observation data using the order information of the tracking target and the group information of the tracking target determined by the group determiner 13.
[0063]
Next, the operation will be described. First, the operation principle of the target tracking device according to the second embodiment will be described. The vehicle observation method, the target motion model, and the observation model of the radar device 1 by the radar apparatus 1 are the same as the principles (1) to (16) in the first embodiment, and therefore will be omitted.
[0064]
Moreover, since the prediction process and the correlation gate determination process are the same as the principles of Expression (17) to Expression (20) in Embodiment 1, they are omitted. In addition, the method for determining the traveling order of the tracking vehicle, the method for determining the group of the tracking vehicle, and the method for determining the order in which the observation vectors exist are also expressed by Equations (21) to (24) in the first embodiment. Since it is the same as the principle, it is omitted.
[0065]
Next, a correlation determination method using order information of the tracking target and the observation vector will be described. In this process, the correlation is determined so as not to reverse the order of the tracking target and the order of the observation vectors so that the tracking vehicle is not overtaken. In the correlation determination, the following processing is performed for each group classified in the group determination of the tracking vehicle.
[0066]
In order from the first tracking vehicle, the residual ε represented by equation (20) among the observed vectors in the correlation gate of the tracking vehicle_{k, i}Is determined to have a correlation with the tracking vehicle, and the combination is stored. At this time, the observation vector determined to have a correlation and the observation vector whose order of existence is earlier than that are excluded from the correlation object of the following tracking vehicle.
[0067]
For example, as shown in FIG. 5, it is assumed that three tracking vehicles are determined to be in the same group, and there are four observation vectors determined to be within the correlation gate of these tracking vehicles. Here, the traveling order of the tracking vehicle is from the top to the underbar ξ_k-1 ¹, Underbar ξ_k-1 ², Underbar ξ_k-1 ³The observation vector exists in the order of the underbar z_{k, 1}, Underbar z_{k, 2}, Underbar z_{k, 3}, Underbar z_{k, 4}And FIG. 5 shows these correlation gate internal / external relations and residuals, and the residual is represented by “x” when it is determined that the correlation gate is determined to be within the correlation gate.
[0068]
First, underbar ξ, the leading vehicle_k-1 ¹Pay attention to. The observation vector with the smallest residual among the observation vectors existing in the correlation gate of this vehicle is underbar z_{k, 2}So, underbar ξ_k-1 ¹Is underbar z_{k, 2}It is determined that there is a correlation. Next, underbar z_{k, 2}And underbar z that exists ahead of this_{k, 1}And underbar ξ which is the following traveling vehicle_k-1 ²Pay attention to. Among the observation vectors existing in the correlation gate of this vehicle, the smallest residual is the underbar z_{k, 3}So, underbar ξ_k-1 ²Is underbar z_{k, 3}It is determined that there is a correlation. Similarly, underbar ξ_k-1 ³Is underbar z_{k, 4}It is determined that there is a correlation.
[0069]
Next, the smoothing process will be described. The observation vector determined to have a correlation with the tracking target by the correlation processing described above is used as the underbar z_kAnd the gain matrix K_k, Smooth vector underbar x_kHat (+) and smoothing error covariance matrix P_k(+) Is given by equations (25) to (27) according to the theory of ordinary Kalman filter. Where R_kIs the sampling time t expressed by equation (17)_kIs an observation error covariance matrix of the radar apparatus 1 in FIG._kIs a residual vector given by equation (28).
[0070]
Next, the operation of the target tracking device according to the second embodiment will be specifically described. It is assumed that the initial value of the state vector is determined separately as in the case where the Kalman filter is normally applied to the target tracking device.
[0071]
The radar apparatus 1 observes the target distance R and azimuth angle Az as shown in FIG. 2, and converts them into xy coordinates according to the equations (1) and (2) (step ST1). In the correlation gate determination unit 2, the prediction vector underbar x of the tracking target is output from the predictor 11._kEnter the hat (-) and observe the vector underbar z of the observation vector_k(−) Is calculated according to the equation (19), and among the observation vectors input from the radar apparatus 1, all of the observation vectors satisfying the equation (20) with respect to the preset gate size parameter ρ are correlated with the tracking target. Select as the observation vector in the gate. This is performed for all tracking targets (step ST2).
[0072]
In the observation data order determination unit 3, for each group determined by the group determination unit 13, a preset unit vector under direction of the vehicle with respect to the observation vector in the correlation gate of the tracking target belonging to the group. Bar e_dInner product λ_k-1 ⁱIs calculated according to equation (24). And the inner product λ_k-1 ⁱIs stored as the observation vector existence order (step ST3).
[0073]
In the correlation determination unit 14 based on the order and the residual, for each group determined by the group determination unit 13, the presence of an observation vector in the correlation target of the tracking target belonging to the group determined by the observation data order determination unit 3 Input the order information and the residual, and the information of the traveling order of the tracking vehicle belonging to the group determined by the wake order determination unit 12, and the observation vector in the correlation gate of the tracking vehicle in the order of the traveling order The observation vector having the smallest residual is determined to be correlated with the tracking vehicle, and the combination is stored. At this time, the observation vector determined to have correlation is excluded from the correlation target of the following tracking vehicle. Then, the same process is performed for all groups (step ST4).
[0074]
In the gain matrix calculator 5, the prediction error covariance matrix P input from the prediction error evaluator 10 for each tracking target._k(−) And a preset observation error covariance matrix R of the radar apparatus 1_kAnd gain matrix K according to equation (25)_kIs calculated (step ST5). In the smoother 6, for each tracking target, the correlation result determined by the correlation determiner 14 based on the order and the residual, and the predicted vector underbar x output from the predictor 11_kHat (-) and gain matrix K from gain matrix calculator 5_kAnd the smooth vector underbar x according to the equation (26)_kHat (+) is calculated (step ST6).
[0075]
On the other hand, the smoothing error evaluator 8 receives the prediction error covariance matrix P from the prediction error evaluator 10._k(−) From the gain matrix calculator 5 to the gain matrix K_k, And the smoothing error covariance matrix P according to equation (27)_k(+) Is calculated (step ST7). In the prediction error evaluator 10, the smoothing error covariance matrix P one sampling before from the second delay circuit 9._k-1(+) Represents a preset driving noise Q_k-1, And the prediction error covariance matrix P_k(−) Is calculated according to equation (18) (step ST8). In the predictor 11, the smoothed vector underbar x before one sampling_k-1The hat (+) is input through the first delay circuit 7 and the prediction vector underbar x according to the equation (17)_kA hat (-) is calculated (step ST9).
[0076]
In the wake order determining unit 12, the smoothing vector of the tracking target calculated one sampling before by the smoother 6 is input via the first delay circuit 7, and the position vector underbar ξ_k-1 ^jAnd a preset unit vector underbar e of the traveling direction of the vehicle_dInner product Ψ with_k-1 ^jIs calculated according to equation (22). And inner product Ψ_k-1 ^jIs stored as the wake traveling order (step ST10). In the group determination device 13, the distance r between the position vectors of the smooth vectors adjacent to each other in the order of the tracking vehicle traveling order determined by the wake order determination device 12 is a preset parameter r._minWhether or not the equation (23) is satisfied is determined, and wakes satisfying the equation (23) are determined as the same group (step ST11).
[0077]
As is apparent from the above, according to the second embodiment, the observation data order determining unit 3 determines the observation vector presence order with respect to the traveling direction of the vehicle and the preset unit vector of the traveling direction of the vehicle and the observation. Judgment is made from the magnitude of the inner product of the vectors. On the other hand, the wake order judging unit 12 judges the traveling order of the vehicle from the magnitude of the inner product of the unit vector and the position vector of the smooth vector of the tracking vehicle. In the correlation determination unit 14 based on the difference, the observation vector having the smallest residual among the observation vectors in the correlation gate of the tracking vehicle is determined to be correlated with the tracking vehicle in the order of the traveling order, and exists ahead of it. Since the observation vector to be tracked is excluded from the correlation target of the tracking target thereafter, the tracking order of the tracking vehicle is difficult to be switched, and the radar device 1 makes a false detection. There exhibits hardly effect becomes correlation erroneous results even when detected.
[0078]
Embodiment 3 FIG.
FIG. 6 is a block diagram showing the target tracking device according to the third embodiment. In the figure, reference numerals 1 to 13 which are the same as those in FIG. 1 denote the same or corresponding parts as those in the first embodiment. Omitted. A pseudo data generator 15 generates pseudo observation data for a tracking target having no correlated observation data in the correlation result of the tracking target and the observation data determined by the correlation determination unit 4 according to the order.
[0079]
Next, the operation will be described. First, the operation principle of the target tracking device according to the third embodiment will be described. The vehicle observation method, the target motion model, and the observation model of the radar device 1 by the radar apparatus 1 are the same as the principles (1) to (16) in the first embodiment, and therefore will be omitted.
[0080]
Further, the correlation determination method is the same principle as that of the first embodiment, and therefore will be omitted. Moreover, since the prediction process and the correlation gate determination process are the same as the principles of Expression (17) to Expression (20) in Embodiment 1, they are omitted. In addition, the method for determining the traveling order of the tracking vehicle, the method for determining the group of the tracking vehicle, and the method for determining the order in which the observation vectors exist are also expressed by Equations (21) to (24) in the first embodiment. Since it is the same as the principle, it is omitted.
[0081]
Next, a method for generating pseudo observation data when there is a tracking target without correlated observation data will be described. When there is a tracking target in which correlation cannot be obtained with any observation vector in the correlation determination unit 4, when a normal memory track (a method in which the predicted position is set as a smooth position) is performed, the vehicles before and after that perform acceleration / deceleration The possibility of overtaking the vehicle ahead or overtaking the vehicle behind is high.
[0082]
Therefore, the tracking target for which the correlation cannot be obtained is obtained by dividing the observation vector obtained by the correlation with the preceding and following vehicles at the same ratio as the interval of the smooth position one sample before the preceding and succeeding vehicles within the same group. Smoothing is performed using the determined position. The position vector of the smooth vector before the sampling of the tracking target for which no correlation is obtained is the underbar ξ_k-1 ^jAnd Then, the position vector of the smooth vector before one sampling of the vehicle before a and the vehicle before b is respectively underbar ξ_k-1 ^j-aAnd underbar ξ_k-1 ^{j + b}And the observed vectors at the current time correlated with these are underbars z_{k, a}And underbar z_{k, b}Then, a position underbar z that internally divides the observation vector obtained by the correlation with the preceding and following vehicles at the same ratio as the interval between the smooth positions one sample before_{k, j}Can be represented by formula (29). However, if there is a tracking target that is correlated only before or after the tracking target for which no correlation is obtained, the position vector of the smooth vector one sampling before the tracking target is used as the underbar ξ_k-1 ^lThen, the position extrapolated at the position of the difference vector of the smooth vector represented by Expression (30) is used.
[0083]
[Expression 12]

[0084]
Next, the smoothing process will be described. By the above-described correlation processing, the tracking target where the correlation target exists has the observation vector to be correlated with the underbar z_kThe tracking target for which there is no correlation target is obtained by substituting the position vector obtained by the above-described pseudo observation data generation method with the underbar z_kAnd the gain matrix K_k, Smooth vector underbar x_kHat (+) and smoothing error covariance matrix P_k(+) Is given by equations (25) to (27) according to the theory of ordinary Kalman filter. Where R_kIs the sampling time t expressed by equation (17)_kIs an observation error covariance matrix of the radar apparatus 1 in FIG._kIs a residual vector given by equation (28).
[0085]
Next, the operation of the target tracking device according to the third embodiment will be specifically described. It is assumed that the initial value of the smooth vector is determined separately as in the case where the Kalman filter is normally applied to the target tracking device.
[0086]
The radar apparatus 1 observes the target distance R and azimuth angle Az as shown in FIG. 2, and converts them into xy coordinates according to the equations (1) and (2) (step ST1). In the correlation gate determination unit 2, the prediction vector underbar x of the tracking target is output from the predictor 11._kEnter the hat (-) and observe the vector underbar z of the observation vector_k(−) Is calculated according to the equation (19), and among the observation vectors input from the radar apparatus 1, all of the observation vectors satisfying the equation (20) with respect to the preset gate size parameter ρ are correlated with the tracking target. Select as the observation vector in the gate. This is performed for all tracking targets (step ST2).
[0087]
In the observation data order determination unit 3, for each group determined by the group determination unit 13, a preset unit vector under direction of the vehicle with respect to the observation vector in the correlation gate of the tracking target belonging to the group. Bar e_dInner product λ_k-1 ⁱIs calculated according to equation (24). And the inner product λ_k-1 ⁱIs stored as the observation vector existence order (step ST3).
[0088]
In the correlation determination unit 4 by order, for each group determined by the group determination unit 13, information on the existence order of observation vectors in the correlation gate of the tracking target belonging to that group determined by the observation data order determination unit 3. And the information on the traveling order of the tracking vehicle belonging to the group determined by the wake order determination unit 12 is input, and the existence order is first among the observation vectors in the correlation gate of the tracking vehicle in the order of the traveling order. The observation vector on the side is determined to be correlated with the tracking vehicle, and the combination is stored. At this time, the observation vector determined to have correlation is excluded from the correlation target of the following tracking vehicle. Then, the same processing is performed for all groups. (Step ST4).
[0089]
In the pseudo data generator 15, the correlation result between the tracking target and the observation data determined by the correlation determination unit 4 according to the order, and the smooth vector calculated by the smoother 6 one sampling before the original time are passed through the first delay circuit. If there is a correlated tracking target before and after the tracking target without correlated observation data for each group, the tracking target is either in the front or back according to equation (29). If there is, a pseudo observation vector is generated according to equation (30) (step ST5).
[0090]
In the gain matrix calculator 5, the prediction error covariance matrix P input from the prediction error evaluator 10 for each tracking target._k(−) And a preset observation error covariance matrix R of the radar apparatus 1_kAnd gain matrix K according to equation (25)_kIs calculated (step ST6). In the smoother 6, for each tracking target, the correlation result determined by the correlation determination unit 4 according to the order, the pseudo observation vector generated by the pseudo data generator 15, and the prediction vector underbar x output from the predictor 11._kHat (-) and gain matrix K from gain matrix calculator 5_k, And smooth vector underbar x according to equation (26)_kHat (+) is calculated (step ST7).
[0091]
On the other hand, the smoothing error evaluator 8 receives the prediction error covariance matrix P from the prediction error evaluator 10._k(−) From the gain matrix calculator 5 to the gain matrix K_k, And the smoothing error covariance matrix P according to equation (27)_k(+) Is calculated (step ST8). In the prediction error evaluator 10, the smoothing error covariance matrix P one sampling before from the second delay circuit 9._k-1(+) Represents a preset driving noise Q_k-1, And the prediction error covariance matrix P_k(−) Is calculated according to equation (18) (step ST9). In the predictor 11, the smoothed vector underbar x before one sampling_k-1The hat (+) is input through the first delay circuit 7 and the prediction vector underbar x according to the equation (17)_kA hat (-) is calculated (step ST10).
[0092]
In the wake order determining unit 12, the smoothing vector of the tracking target calculated one sampling before by the smoother 6 is input via the first delay circuit 7, and the position vector underbar ξ_k-1 ^jAnd a preset unit vector underbar e of the traveling direction of the vehicle_dInner product Ψ with_k-1 ^jIs calculated according to equation (22). And inner product Ψ_k-1 ^jIs stored as the travel order of the wake (step ST11). In the group determination device 13, the distance r between the position vectors of the smooth vectors adjacent to each other in the order of the tracking vehicle traveling order determined by the wake order determination device 12 is a preset parameter r._minWhether or not the equation (23) is satisfied is examined, and wakes satisfying the equation (23) are determined as the same group (step ST12).
[0093]
As is apparent from the above, according to the third embodiment, the observation data order determining unit 3 determines the observation vector presence order with respect to the vehicle traveling direction and the preset unit vector in the vehicle traveling direction and the observation. Judgment is based on the magnitude of the inner product of the vectors. On the other hand, the wake order judging unit 12 judges the order of travel of the vehicle from the magnitude of the inner product of the unit vector and the position vector of the smoothing vector of the tracking vehicle. The determination unit 4 is configured to determine, in the order of traveling order, the observation vector in the order of existence among the observation vectors in the correlation gate of the tracking vehicle that is correlated with the tracking vehicle. There is an effect that the order of wakes is not easily changed. Further, in the pseudo data generator 15, for a tracking target having no correlated observation data, an observation correlated with the current position and the smooth position one sampling before or after one of the tracking targets with which correlation was obtained. Since the pseudo observation vector is generated using the vector and smoothed, the wake order of the tracking vehicle is not easily changed even when the radar apparatus 1 detects a failure.
[0094]
Embodiment 4 FIG.
FIG. 7 is a block diagram showing the target tracking device according to the fourth embodiment. In the figure, reference numerals 1 to 3 and 5 to 14 which are the same as those in FIG. 4 indicate the same or corresponding parts as those in the second embodiment. The description is omitted. 15 is a pseudo data generator that generates pseudo observation data for a tracking target having no correlated observation data in the correlation result between the tracking target and the observation data determined by the correlation determination unit 14 based on the order and residual. is there.
[0095]
Next, the operation will be described. First, the operation principle of the target tracking device according to the fourth embodiment will be described. The vehicle observation method, the target motion model, and the observation model of the radar device 1 by the radar apparatus 1 are the same as the principles (1) to (16) in the first embodiment, and therefore will be omitted.
[0096]
The correlation determination method is the same as that of the second embodiment, and will not be described. Moreover, since the prediction process and the correlation gate determination process are the same as the principles of Expression (17) to Expression (20) in Embodiment 1, they are omitted. In addition, the method for determining the traveling order of the tracking vehicle, the method for determining the group of the tracking vehicle, and the method for determining the order in which the observation vectors exist are also expressed by Equations (21) to (24) in the first embodiment. Since it is the same as the principle, it is omitted.
[0097]
A method for generating pseudo observation data when there is a tracking target having no correlated observation data is the same as the principles (29) to (30) in the third embodiment, and a description thereof will be omitted.
[0098]
Next, the operation of the target tracking device according to the fourth embodiment will be specifically described. It is assumed that the initial value of the state vector is determined separately as in the case where the Kalman filter is normally applied to the target tracking device.
[0099]
The radar apparatus 1 observes the target distance R and azimuth angle Az as shown in FIG. 2, and converts them into xy coordinates according to the equations (1) and (2) (step ST1). In the correlation gate determination unit 2, the prediction vector underbar x of the tracking target is output from the predictor 11._kEnter the hat (-) and observe the vector underbar z of the observation vector_k(−) Is calculated according to the equation (19), and among the observation vectors input from the radar apparatus 1, all of the observation vectors satisfying the equation (20) with respect to the preset gate size parameter ρ are correlated with the tracking target. Select as the observation vector in the gate. This is performed for all tracking targets (step ST2). In the observation data order determination unit 3, for each group determined by the group determination unit 13, a preset unit vector under direction of the vehicle with respect to the observation vector in the correlation gate of the tracking target belonging to the group. Bar e_dInner product λ_k-1 ⁱIs calculated according to equation (24). And the inner product λ_k-1 ⁱIs stored as the observation vector existence order (step ST3).
[0100]
In the correlation determination unit 14 based on the order and the residual, for each group determined by the group determination unit 13, the presence of an observation vector in the correlation target of the tracking target belonging to the group determined by the observation data order determination unit 3 Input the order information and the residual, and the information of the traveling order of the tracking vehicle belonging to the group determined by the wake order determination unit 12, and the observation vector in the correlation gate of the tracking vehicle in the order of the traveling order The observation vector having the smallest residual is determined to be correlated with the tracking vehicle, and the combination is stored. At this time, the observation vector determined to have correlation is excluded from the correlation target of the following tracking vehicle. Then, the same processing is performed for all groups. (Step ST4).
[0101]
In the pseudo data generator 15, the correlation result between the tracking target determined by the correlation determining unit 14 based on the order and the residual and the observation data, and the smooth vector calculated by the smoother 6 one sampling before the current time are the first. When there is a correlated tracking target before and after a tracking target that is input via a delay circuit and does not have correlated observation data for each group, it is placed either before or after according to equation (29). If there is a tracking target, a pseudo observation vector is generated according to equation (30) (step ST5).
[0102]
In the gain matrix calculator 5, the prediction error covariance matrix P input from the prediction error evaluator 10 for each tracking target._k(−) And a preset observation error covariance matrix R of the radar apparatus 1_kAnd gain matrix K according to equation (25)_kIs calculated (step ST6). In the smoother 6, for each tracking target, the correlation result determined by the correlation determination unit 4 according to the order, the pseudo observation vector generated by the pseudo data generator 15, and the prediction vector underbar x output from the predictor 11._kHat (-) and gain matrix K from gain matrix calculator 5_k, And smooth vector underbar x according to equation (26)_kHat (+) is calculated (step ST7).
[0103]
On the other hand, the smoothing error evaluator 8 receives the prediction error covariance matrix P from the prediction error evaluator 10._k(−) From the gain matrix calculator 5 to the gain matrix K_k, And the smoothing error covariance matrix P according to equation (27)_k(+) Is calculated (step ST8). In the prediction error evaluator 10, the smoothing error covariance matrix P one sampling before from the second delay circuit 9._k-1(+) Represents a preset driving noise Q_k-1, And the prediction error covariance matrix P_k(−) Is calculated according to equation (18) (step ST9). In the predictor 11, the smoothed vector underbar x before one sampling_k-1The hat (+) is input through the first delay circuit 7 and the prediction vector underbar x according to the equation (17)_kA hat (-) is calculated (step ST10).
[0104]
In the wake order determining unit 12, the smoothing vector of the tracking target calculated one sampling before by the smoother 6 is input via the first delay circuit 7, and the position vector underbar ξ_k-1 ^jAnd a preset unit vector underbar e of the traveling direction of the vehicle_dInner product Ψ with_k-1 ^jIs calculated according to equation (22). And inner product Ψ_k-1 ^jIs stored as the travel order of the wake (step ST11). In the group determination device 13, the distance r between the position vectors of the smooth vectors adjacent to each other in the order of the tracking vehicle traveling order determined by the wake order determination device 12 is a preset parameter r._minWhether or not the equation (23) is satisfied is examined, and wakes satisfying the equation (23) are determined as the same group (step ST12).
[0105]
As is apparent from the above, according to the fourth embodiment, the observation data order determination unit 3 determines the observation vector presence order with respect to the traveling direction of the vehicle and the preset unit vector of the traveling direction of the vehicle and the observation. Judgment is made from the magnitude of the inner product of the vectors. On the other hand, the wake order judging unit 12 judges the traveling order of the vehicle from the magnitude of the inner product of the unit vector and the position vector of the smooth vector of the tracking vehicle. In the correlation determination unit 14 based on the difference, the observation vector having the smallest residual among the observation vectors in the correlation gate of the tracking vehicle is determined to be correlated with the tracking vehicle in the order of the traveling order, and exists ahead of it. Since the observation vector to be tracked is excluded from the correlation target of the subsequent tracking target, the track order of the tracking vehicle is difficult to be switched, and the radar apparatus 1 makes a false detection. There exhibits hardly effect becomes correlation erroneous results even when detected. Further, in the pseudo data generator 15, for a tracking target having no correlated observation data, an observation correlated with the current position and the smooth position one sampling before or after one of the tracking targets with which correlation was obtained. Since the pseudo observation vector is generated using the vector and smoothed, the wake order of the tracking vehicle is not easily changed even when the radar apparatus 1 detects a failure.
[0106]
Embodiment 5. FIG.
FIG. 8 is a block diagram showing the target tracking apparatus according to the fifth embodiment. In the figure, reference numerals 1 to 13 and 15 that are the same as those in FIG. 6 denote the same or corresponding parts as those in the third embodiment. Is omitted. Further, reference numeral 14 which is the same as that in FIG.
[0107]
Reference numeral 16 inputs the tracking target group determined by the group determination unit 13 and the observation data in the correlation gate of the tracking target belonging to the group determined by the correlation gate determination unit 2 via the observation data order determination unit 3. Depending on the number of tracking targets in the same group and the number of observation data existing in the correlation gate of the tracking target, either the correlation determination unit 15 by order or the correlation determination unit 14 by order and residual is used. The correlation processing switcher 16 switches to
[0108]
Next, the operation will be described. First, the operation principle of the target tracking device according to the fifth embodiment will be described. The vehicle observation method, the target motion model, and the observation model of the radar device 1 by the radar apparatus 1 are the same as the principles (1) to (16) in the first embodiment, and therefore will be omitted.
[0109]
The correlation determination method is the same as that of the second embodiment, and will not be described. Moreover, since the prediction process and the correlation gate determination process are the same as the principles of Expression (17) to Expression (20) in Embodiment 1, they are omitted. In addition, the method for determining the traveling order of the tracking vehicle, the method for determining the group of the tracking vehicle, and the method for determining the order in which the observation vectors exist are also expressed by Equations (21) to (24) in the first embodiment. Since it is the same as the principle, it is omitted.
[0110]
A method for generating pseudo observation data when there is a tracking target having no correlated observation data is the same as the principles (29) to (30) in the third embodiment, and a description thereof will be omitted.
[0111]
Next, a correlation processing switching method will be described. In each group, when the number of tracking vehicles is equal to the number of observation data, the detection state of the radar device 1 is considered good. In this case, it is simpler to have a one-to-one correspondence by matching the order of the tracking target and the observation data than to perform the correlation determination using the information on the residual between the tracking vehicle and the observation data. It is done. On the other hand, when the number of observation data is larger than that of the tracking vehicle, it is considered that the radar apparatus 1 has made a false detection. Therefore, a method of performing correlation determination using information on the order and the residual is considered to be good. On the contrary, when the number of observation data is smaller than that of the tracking vehicle, the radar apparatus 1 is considered to have lost detection. Therefore, the correlation determination is performed using the order and residual information, and further, the tracking target for which the correlation cannot be obtained is determined. A method of generating and smoothing pseudo-observation data is considered good.
[0112]
Next, the operation of the target tracking device according to the fifth embodiment will be specifically described. It is assumed that the initial value of the state vector is determined separately as in the case where the Kalman filter is normally applied to the target tracking device.
[0113]
The radar apparatus 1 observes the target distance R and azimuth angle Az as shown in FIG. 2, and converts them into xy coordinates according to the equations (1) and (2) (step ST1). In the correlation gate determination unit 2, the prediction vector underbar x of the tracking target is output from the predictor 11._kEnter the hat (-) and observe the vector underbar z of the observation vector_k(−) Is calculated according to the equation (19), and among the observation vectors input from the radar apparatus 1, all of the observation vectors satisfying the equation (20) with respect to the preset gate size parameter ρ are correlated with the tracking target. Select as the observation vector in the gate. This is performed for all tracking targets (step ST2). In the observation data order determination unit 3, for each group determined by the group determination unit 13, a preset unit vector under direction of the vehicle with respect to the observation vector in the correlation gate of the tracking target belonging to the group. Bar e_dInner product λ_k-1 ⁱIs calculated according to equation (24). And the inner product λ_k-1 ⁱIs stored as the observation vector existence order (step ST3).
[0114]
In the correlation processing switcher 16, the group of tracking targets determined by the group determiner 13 and the observation data in the correlation gate of the tracking target belonging to the group determined by the correlation gate determiner 2 are converted into an observation data order determiner 3. For each group, the next processing branch is performed according to the number of tracking targets to which the group belongs and the number of observation data existing in the correlation gate of the tracking target. When the number of tracking vehicles is equal to the number of observation data, the process proceeds to the processing of the correlation determination unit 4 according to the order. If the number of observation data is different from that of the tracking vehicle, the process proceeds to the processing of the correlation determination unit 14 based on the order and the residual (step ST4).
[0115]
In the correlation determination unit 4 by order, for each group determined by the group determination unit 13, information on the existence order of observation vectors in the correlation gate of the tracking target belonging to that group determined by the observation data order determination unit 3. Is input via the correlation processing switching unit 16, and information on the traveling order of the tracking vehicle belonging to the group determined by the wake order determining unit 12 is input. Of the observation vectors in the correlation gate, the observation vector with the leading order is determined to have a correlation with the tracking vehicle, and the combination is stored. At this time, the observation vector determined to have correlation is excluded from the correlation target of the following tracking vehicle. Then, the same processing is performed for all groups. (Step ST5).
[0116]
In the correlation determination unit 14 based on the order and the residual, for each group determined by the group determination unit 13, the presence of an observation vector in the correlation target of the tracking target belonging to the group determined by the observation data order determination unit 3 Information on the order and the residual are input via the correlation processing switcher 16, and information on the traveling order of the tracking vehicles belonging to the group determined by the wake order determining unit 12 is input. Then, the observation vector having the smallest residual among the observation vectors in the correlation gate of the tracking vehicle is determined to have a correlation with the tracking vehicle, and the combination is stored. At this time, the observation vector determined to have correlation is excluded from the correlation target of the following tracking vehicle. Then, the same processing is performed for all groups. (Step ST6).
[0117]
In the pseudo data generator 15, the correlation result between the tracking target and the observation data determined by the correlation determination unit 14 based on the order and the residual, and the smooth vector calculated by the smoother 6 one sampling before the original time are the first. If there is a tracking target that is correlated before and after a tracking target that is input via a delay circuit and does not have correlated observation data for each group, either one of the front or the rear according to equation (29) If there is a tracking target, a pseudo observation vector is generated according to equation (30) (step ST7).
[0118]
In the gain matrix calculator 5, the prediction error covariance matrix P input from the prediction error evaluator 10 for each tracking target._k(−) And a preset observation error covariance matrix R of the radar apparatus 1_kAnd gain matrix K according to equation (25)_kIs calculated (step ST8). In the smoother 6, for each tracking target, the correlation result determined by the correlation determination unit 4 according to the order, the pseudo observation vector generated by the pseudo data generator 15, and the prediction vector underbar x output from the predictor 11._kHat (-) and gain matrix K from gain matrix calculator 5_k, And smooth vector underbar x according to equation (26)_kHat (+) is calculated (step ST9).
[0119]
On the other hand, the smoothing error evaluator 8 receives the prediction error covariance matrix P from the prediction error evaluator 10._k(−) From the gain matrix calculator 5 to the gain matrix K_k, And the smoothing error covariance matrix P according to equation (27)_k(+) Is calculated (step ST10). In the prediction error evaluator 10, the smoothing error covariance matrix P one sampling before from the second delay circuit 9._k-1(+) Represents a preset driving noise Q_k-1, And the prediction error covariance matrix P_k(−) Is calculated according to the equation (18) (step ST11). In the predictor 11, the smoothed vector underbar x before one sampling_k-1The hat (+) is input through the first delay circuit 7 and the prediction vector underbar x according to the equation (17)_kA hat (-) is calculated (step ST12).
[0120]
In the wake order determining unit 12, the smoothing vector of the tracking target calculated one sampling before by the smoother 6 is input via the first delay circuit 7, and the position vector underbar ξ_k-1 ^jAnd a preset unit vector underbar e of the traveling direction of the vehicle_dInner product Ψ with_k-1 ^jIs calculated according to equation (22). And inner product Ψ_k-1 ^jIs stored as the wake traveling order (step ST13). In the group determination device 13, the distance r between the position vectors of the smooth vectors adjacent to each other in the order of the tracking vehicle traveling order determined by the wake order determination device 12 is a preset parameter r._minWhether or not the equation (23) is satisfied is checked, and wakes satisfying the equation (23) are determined as the same group (step ST14).
[0121]
As is apparent from the above, according to the fifth embodiment, the observation data order determination unit 3 determines the observation vector presence order with respect to the traveling direction of the vehicle and the preset unit vector of the traveling direction of the vehicle and the observation. Judgment is based on the magnitude of the inner product of the vectors, while the wake order judging unit 12 judges the traveling order of the vehicle from the magnitude of the inner product of the unit vector and the position vector of the smoothing vector of the tracking vehicle. When the number of observation data is the same as that of the tracking vehicle and the detection state of the radar apparatus 1 is good, the correlation determination unit 4 in order determines the number of observation vectors in the correlation gate of the tracking vehicle in the order of the traveling order. If the observation vector whose existence order is at the top is determined to be correlated with the tracking vehicle, and the tracking vehicle and observation data are not equal, the correlation between the order and the residual In the setter 14, the observation vector having the smallest residual among the observation vectors in the correlation gate of the tracking vehicle is determined to have a correlation with the tracking vehicle in the order of the traveling order, and the observation vector existing ahead of it is determined. Is excluded from the correlation target of the tracking target thereafter, there is an effect that an erroneous correlation result is less likely to occur even when there is an erroneous detection by the radar apparatus 1 or when there is a missed detection.
[0122]
【The invention's effect】
As described above, according to the present invention, for example, a plurality of target targets to be tracked, such as an aircraft traveling on a taxiway or a runway on an airport surface, or a vehicle traveling on a one-way street with a narrow lane width. It is possible to obtain a target tracking device and method in which movement is limited to a course such as a train running on a rail, and the track that is tracked is difficult to replace the actual target order in a situation where overtaking or overtaking is impossible or impossible in operation. There is an effect that can be done.
[Brief description of the drawings]
FIG. 1 is a configuration diagram illustrating a target tracking device according to a first embodiment of the present invention.
FIG. 2 is a diagram for explaining the operation principle of the target tracking device according to the first embodiment of the present invention.
FIG. 3 is an explanatory diagram of a correlation determination method using order information of a tracking target and an observation vector in the target tracking device according to Embodiment 1 of the present invention.
FIG. 4 is a block diagram showing a target tracking device according to Embodiment 2 of the present invention.
FIG. 5 is an explanatory diagram of a correlation determination method using order information of a tracking target and an observation vector in a target tracking device according to Embodiment 2 of the present invention.
FIG. 6 is a block diagram showing a target tracking apparatus according to Embodiment 3 of the present invention.
FIG. 7 is a block diagram showing a target tracking apparatus according to Embodiment 4 of the present invention.
FIG. 8 is a block diagram showing a target tracking apparatus according to Embodiment 5 of the present invention.
FIG. 9 is a block diagram showing a conventional target tracking device shown in Multiple-Target Tracking with Radar Application, ARTECK HOUSE, 1986, pp299-302.
[Explanation of symbols]
DESCRIPTION OF SYMBOLS 1 Radar apparatus, 2 Correlation gate determination device, 3 Observation data order determination device, 4 Correlator by order, 5 Gain matrix calculation device, 6 Smoother, 7 1st delay device, 8 Smoothing error evaluator, 9 2nd Delay unit, 10 prediction error evaluator, 11 predictor, 12 track order determiner, 13 group determiner, 14 correlator based on order and residual, 15 pseudo data generator, 16 correlation processing switcher.

Claims (8)

An observation means for observing the target position;
A prediction means for calculating a predicted value of the target state;
In a target tracking device comprising: smoothing means for calculating a smooth value of a target state;
Wake order determination means for determining the order of the arrangement of wake positions with respect to a desired direction using the smoothed value calculated by the smoothing means;
Group determination means for recognizing wakes close to smooth positions as a group based on the determination result of the wake order determination means;
For each group determined by the group determination unit , an observation data order determination unit that determines the order of the arrangement of positions with respect to a desired direction for the observation vector obtained in the correlation gate of the tracking target belonging to the group ;
Based on the determination result of the group determination means and the observation data order determination means, the order of existence of the observation vectors in the correlation gate of the tracking vehicle is the leading order of the tracking vehicles belonging to the same group. Correlating means according to the order in which the observation vector is determined to be correlated with the tracking vehicle ,
The smoothing unit calculates a smooth value of a target state based on outputs of the correlation unit and the prediction unit. The target tracking device according to claim 1,
The correlation means determines the correlation between the track and the observation data based on the order of the position of the track and the position of the observation data and the residual. The target tracking device according to claim 1 or 2,
A target tracking device, further comprising: pseudo data generation means for generating pseudo observation data for interpolating smoothness of a wake having no correlated observation data in the correlation means. The target tracking device according to claim 1,
As the correlation means, a first correlation means for determining a correlation between the wake and the observation data based on the order of the arrangement of the positions of the wake and the observation data; And a second correlation means for determining the correlation between the wake and the observation data,
A correlation processing switching means for switching between said first correlation means and a front Stories second correlation means by track number and the observed data number magnitude relation,
The target tracking further comprising: pseudo data generation means for generating pseudo observation data for interpolating smoothness of a wake having no observation data correlated in the first correlation means or the second correlation means apparatus. An observation step for observing the target position;
A prediction step for calculating a predicted value of the target state;
A target tracking method comprising: a smoothing step for calculating a smooth value of a target state;
A wake order determination step for determining the order of arrangement of wake positions with respect to a desired direction using the smoothed value calculated by the smoothing step;
A group determination step for recognizing wakes close to smooth positions as a group based on the determination result of the wake order determination step;
For each group determined by the group determination step , an observation data order determination step for determining the order of the arrangement of positions with respect to a desired direction for the observation vector obtained in the correlation gate of the tracking target belonging to the group ;
The order of the running order of the tracking vehicle belonging to the same group based on the group determining step and the observed data order determination step of the determination result, the top side and the order of the observation vectors in the correlation gate of the tracking vehicle A correlation step for determining that the observed vector is correlated with the tracking vehicle ,
The smoothing step calculates a smooth value of a target state based on outputs of the correlation step and the prediction step. In the target tracking method according to claim 5,
The correlation step determines the correlation between the wake and the observation data based on the order of the position of the wake and the observation data and the residual. In the target tracking method according to claim 5 or 6,
A target tracking method, further comprising a pseudo data generation step for generating pseudo observation data for interpolating smoothness of a wake having no correlated observation data in the correlation step. In the target tracking method according to claim 5,
As the correlation step, a first correlation step for determining a correlation between the wake and the observation data based on an order of arrangement of the wake and the observation data, an order of the arrangement of the wake and the observation data, and a residual are provided. And a second correlation step for determining the correlation between the wake and the observation data,
A correlation processing switching step of switching between the first correlation step and the second correlation step according to the magnitude relationship between the number of wakes and the number of observation data;
And a pseudo data generation step of generating pseudo observation data for interpolating smoothness of a wake having no observation data correlated in the first correlation step or the second correlation step.

Images