(57)【特許請求の範囲】
【請求項１】
目標の位置を観測する観測手段と、
目標の状態の予測値を演算する予測手段と、
目標の状態の平滑値を演算する平滑手段と
を備えた目標追尾装置において、
前記平滑手段により演算された平滑値を用いて所望の方向に対する航跡の位置の並びの順序を判定する航跡順序判定手段と、
前記航跡順序判定手段の判定結果に基づいて平滑位置の近い航跡同士をグループとして認識するグループ判定手段と、
前記グループ判定手段によって判定されたグループ毎に、そのグループに所属する追尾目標の相関ゲート内に得られた観測ベクトルについて、所望方向に対する位置の並びの順序を判定する観測データ順序判定手段と、
前記グループ判定手段と前記観測データ順序判定手段の判定結果に基づいて同一のグループに所属する追尾車両の走行順序の順番に、その追尾車両の相関ゲート内の観測ベクトルのうち存在順序が先頭側の観測ベクトルを追尾車両と相関があると判定する順序による相関手段と
を備え、
前記平滑手段は、前記相関手段と前記予測手段の出力に基づいて目標の状態の平滑値を演算する
ことを特徴とする目標追尾装置。
【請求項２】
請求項１に記載の目標追尾装置において、
前記相関手段は、航跡と観測データの位置の並びの順序と残差をもとに航跡と観測データの相関を判定する
ことを特徴とする目標追尾装置。
【請求項３】
請求項１または２に記載の目標追尾装置において、
前記相関手段において相関する観測データのない航跡の平滑を補間する擬似的な観測データを生成する擬似データ生成手段をさらに備えた
ことを特徴とする目標追尾装置。
【請求項４】
請求項１に記載の目標追尾装置において、
前記相関手段として、航跡と観測データの位置の並びの順序をもとに航跡と観測データの相関を判定する第１の相関手段と、航跡と観測データの位置の並びの順序と残差をもとに航跡と観測データの相関を判定する第２の相関手段とを備えると共に、
航跡数と観測データ数の大小関係により前記第１の相関手段と前記第２の相関手段とを切り換える相関処理切り換え手段と、
前記第１の相関手段もしくは前記第２の相関手段において相関する観測データのない航跡の平滑を補間する擬似的な観測データを生成する擬似データ生成手段と
をさらに備えたことを特徴とする目標追尾装置。
【請求項５】
目標の位置を観測する観測ステップと、
目標の状態の予測値を演算する予測ステップと、
目標の状態の平滑値を演算する平滑ステップと
を備えた目標追尾方法において、
前記平滑ステップにより演算された平滑値を用いて所望の方向に対する航跡の位置の並びの順序を判定する航跡順序判定ステップと、
前記航跡順序判定ステップの判定結果に基づいて平滑位置の近い航跡同士をグループとして認識するグループ判定ステップと、
前記グループ判定ステップによって判定されたグループ毎に、そのグループに所属する追尾目標の相関ゲート内に得られた観測ベクトルについて、所望方向に対する位置の並びの順序を判定する観測データ順序判定ステップと、
前記グループ判定ステップと前記観測データ順序判定ステップの判定結果に基づいて同一のグループに所属する追尾車両の走行順序の順番に、その追尾車両の相関ゲート内の観測ベクトルのうち存在順序が先頭側の観測ベクトルを追尾車両と相関があると判定する相関ステップと
を備え、
前記平滑ステップは、前記相関ステップと前記予測ステップの出力に基づいて目標の状態の平滑値を演算する
ことを特徴とする目標追尾方法。
【請求項６】
請求項５に記載の目標追尾方法において、
前記相関ステップは、航跡と観測データの位置の並びの順序と残差をもとに航跡と観測データの相関を判定する
ことを特徴とする目標追尾方法。
【請求項７】
請求項５または６に記載の目標追尾方法において、
前記相関ステップにおいて相関する観測データのない航跡の平滑を補間する擬似観測データを生成する擬似データ生成ステップをさらに備えた
ことを特徴とする目標追尾方法。
【請求項８】
請求項５に記載の目標追尾方法において、
前記相関ステップとして、航跡と観測データの位置の並びの順序をもとに航跡と観測データの相関を判定する第１の相関ステップと、航跡と観測データの位置の並びの順序と残差をもとに航跡と観測データの相関を判定する第２の相関ステップとを備えると共に、
航跡数と観測データ数の大小関係により前記第１の相関ステップと前記第２の相関ステップを切り換える相関処理切り換えステップと、
前記第１の相関ステップもしくは前記第２の相関ステップにおいて相関する観測データのない航跡の平滑を補間する擬似観測データを生成する擬似データ生成ステップと
をさらに備えたことを特徴とする目標追尾方法。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
この発明は、レーダ、レーザ、カメラ等にて観測した、人、自転車、車両、航空機等の移動目標の残差、俯角（仰角）、方位角、位置、またはこれらの時間変化率等の運動諸元を推定する目標追尾装置に関するものである。
【０００２】
【従来の技術】
図９は、例えば、Multiple-Target Tracking with Radar Application, ARTECK HOUSE, 1986, pp299-302に示された従来の目標追尾装置を示す構成図である。図９において、１は目標の位置を観測するレーダ装置、２はレーダ装置１により観測された観測ベクトルのうち、追尾目標と相関のある観測ベクトルを残差をもとに選択する相関ゲート判定器、１７は相関ゲート判定器２で選択された観測データの信頼度を算出する信頼度算出器、５は予測誤差評価器１０により算出された予測誤差共分散行列と予め設定されたレーダ装置１の観測誤差共分散行列からゲイン行列を算出するゲイン行列算出器、１８は、信頼度算出器１７により算出された観測ベクトルの信頼度と、予測器１１で算出された予測ベクトルと、ゲイン行列算出器５により算出されたゲイン行列とを用いて、目標の位置及び速度の平滑ベクトルを算出する信頼度による平滑器、７は信頼度による平滑器１８により算出された平滑ベクトルを１サンプリング時間だけ遅延する第１の遅延回路である。
【０００３】
８は、信頼度による平滑器１８で算出された残差ベクトル及び信頼度による重み付き残差ベクトルと、ゲイン行列算出器５により算出されたゲイン行列と、予測誤差評価器１０により算出された予測誤差共分散行列とを用いて、平滑誤差共分散行列を算出する平滑誤差評価器、９は平滑誤差評価器８により算出された平滑誤差共分散行列を１サンプリング時間だけ遅延する第２の遅延回路、１０は平滑誤差評価器８により算出された平滑誤差共分散行列から現時刻より１サンプリング後の予測誤差共分散行列を算出する予測誤差評価器、１１は、信頼度による平滑器１８から出力される目標の位置及び速度から成る平滑ベクトルを第１の遅延回路７を介して入力し、これと予め設定された目標の運動モデルを用いて現時刻より１サンプリング後の目標の予測位置及び予測速度から成る予測ベクトルを算出する予測器である。
【０００４】
次に動作について説明する。相関ゲート判定器２は、レーダ装置１から観測ベクトルを受け取ると、その観測ベクトルが目標に係る観測ベクトルである可能性が高い場合にのみ有効なデータとして採用するため、予測誤差評価器１０から出力される予測誤差共分散行列と予め設定されたレーダ装置１の観測誤差共分散行列とから目標の予測存在確率分布を算出し、この分布の所望の等確率範囲内に存在する観測ベクトルを追尾目標と相関があるとみなし、信頼度算出器１７に出力する。
【０００５】
信頼度算出器１７は、相関ゲート判定器２から出力された目標と相関のある観測ベクトルを受け取ると、予測器１１の出力する予測ベクトルを入力し、これらと予め設定されたレーダ装置１の検出確率及び不要信号発生頻度とから上記観測ベクトルの信頼度を算出する。
【０００６】
ゲイン行列算出器５は、予測誤差評価器１０から出力された予測誤差共分散行列と、予め設定されたレーダ装置１の観測誤差共分散行列とからゲイン行列を算出する。
【０００７】
信頼度による平滑器１８は、信頼度算出器１７により算出された観測ベクトルの信頼度と、予測器１１により算出された予測ベクトルとから信頼度による重み付き残差ベクトルを算出し、これとゲイン行列算出器５により算出されたゲイン行列とを用いて、目標の位置及び速度から成る平滑ベクトルを算出する。
【０００８】
平滑誤差評価器８は、信頼度による平滑器１８により算出された残差ベクトル及び信頼度による重み付き残差ベクトルと、ゲイン行列算出器５により算出されたゲイン行列と、予測誤差評価器１０により算出された予測誤差共分散行列とを用いて、平滑誤差を評価する平滑誤差共分散行列を算出する。
【０００９】
そして、予測誤差評価器１０は、平滑誤差評価器８から出力された平滑誤差共分散行列から予測誤差を評価する予測誤差共分散行列を算出する。
【００１０】
上記のようにして、信頼度による平滑器１８が目標の位置及び速度から成る平滑ベクトルを算出すると、予測器１１は、予め設定された目標の運動モデルを用いて、現時刻より１サンプリング後の目標の予測ベクトルを算出する。
【００１１】
【発明が解決しようとする課題】
従来の目標追尾装置は以上のように構成されており、追尾目標と観測データの相関は、追尾目標の予測位置に近い観測データほど相関が高いと判定されて平滑処理が行われるため、複数の目標が接近している場合に、目標観測装置の観測誤差の影響で生じる偽りの加減速運動により、複数の目標に対する予測位置が隣り合う目標間で入れ替わる現象が生じ、実際の目標の位置関係と異なる場合が発生する。
【００１２】
この発明は上記のような課題を解決するためになされたもので、追い越しや追い抜きができないまたは運行上ありえない状況において、追尾した航跡が実際の目標の順序に対して入れ替わりにくい目標追尾装置及び方法を得ることを目的とする。
【００１３】
【課題を解決するための手段】
この発明に係る目標追尾装置は、目標の位置を観測する観測手段と、目標の状態の予測値を演算する予測手段と、目標の状態の平滑値を演算する平滑手段とを備えた目標追尾装置において、前記平滑手段により演算された平滑値を用いて所望の方向に対する航跡の位置の並びの順序を判定する航跡順序判定手段と、前記航跡順序判定手段の判定結果に基づいて平滑位置の近い航跡同士をグループとして認識するグループ判定手段と、前記グループ判定手段によって判定されたグループ毎に、そのグループに所属する追尾目標の相関ゲート内に得られた観測ベクトルについて、所望方向に対する位置の並びの順序を判定する観測データ順序判定手段と、前記グループ判定手段と前記観測データ順序判定手段の判定結果に基づいて同一のグループに所属する追尾車両の走行順序の順番に、その追尾車両の相関ゲート内の観測ベクトルのうち存在順序が先頭側の観測ベクトルを追尾車両と相関があると判定する順序による相関手段とを備え、前記平滑手段は、前記相関手段と前記予測手段の出力に基づいて目標の状態の平滑値を演算することを特徴とするものである。
【００１４】
また、前記相関手段は、航跡と観測データの位置の並びの順序と残差をもとに航跡と観測データの相関を判定することを特徴とするものである。
【００１５】
また、前記相関手段において相関する観測データのない航跡の平滑を補間する擬似的な観測データを生成する擬似データ生成手段をさらに備えたことを特徴とするものである。
【００１６】
また、前記相関手段として、航跡と観測データの位置の並びの順序をもとに航跡と観測データの相関を判定する第１の相関手段と、航跡と観測データの位置の並びの順序と残差をもとに航跡と観測データの相関を判定する第２の相関手段とを備えると共に、航跡数と観測データ数の大小関係により前記第１の相関手段と前記第２の相関手段とを切り換える相関処理切り換え手段と、前記第１の相関手段もしくは前記第２の相関手段において相関する観測データのない航跡の平滑を補間する擬似的な観測データを生成する擬似データ生成手段とをさらに備えたことを特徴とするものである。
【００１７】
また、この発明に係る目標追尾方法は、目標の位置を観測する観測ステップと、目標の状態の予測値を演算する予測ステップと、目標の状態の平滑値を演算する平滑ステップとを備えた目標追尾方法において、前記平滑ステップにより演算された平滑値を用いて所望の方向に対する航跡の位置の並びの順序を判定する航跡順序判定ステップと、前記航跡順序判定ステップの判定結果に基づいて平滑位置の近い航跡同士をグループとして認識するグループ判定ステップと、前記グループ判定ステップによって判定されたグループ毎に、そのグループに所属する追尾目標の相関ゲート内に得られた観測ベクトルについて、所望方向に対する位置の並びの順序を判定する観測データ順序判定ステップと、前記グループ判定ステップと前記観測データ順序判定ステップの判定結果に基づいて同一のグループに所属する追尾車両の走行順序の順番に、その追尾車両の相関ゲート内の観測ベクトルのうち存在順序が先頭側の観測ベクトルを追尾車両と相関があると判定する相関ステップとを備え、前記平滑ステップは、前記相関ステップと前記予測ステップの出力に基づいて目標の状態の平滑値を演算することを特徴とするものである。
【００１８】
また、前記相関ステップは、航跡と観測データの位置の並びの順序と残差をもとに航跡と観測データの相関を判定することを特徴とするものである。
【００１９】
また、前記相関ステップにおいて相関する観測データのない航跡の平滑を補間する擬似観測データを生成する擬似データ生成ステップをさらに備えたことを特徴とするものである。
【００２０】
さらに、前記相関ステップとして、航跡と観測データの位置の並びの順序をもとに航跡と観測データの相関を判定する第１の相関ステップと、航跡と観測データの位置の並びの順序と残差をもとに航跡と観測データの相関を判定する第２の相関ステップとを備えると共に、航跡数と観測データ数の大小関係により前記第１の相関ステップと前記第２の相関ステップを切り換える相関処理切り換えステップと、前記第１の相関ステップもしくは前記第２の相関ステップにおいて相関する観測データのない航跡の平滑を補間する擬似観測データを生成する擬似データ生成ステップとをさらに備えたことを特徴とするものである。
【００２１】
【発明の実施の形態】
実施の形態１．
図１は、この発明の実施の形態１による目標追尾装置を示す構成図である。図において、１は目標の存在する領域に電波を放射し、その反射信号から目標を検出してその位置を出力する観測手段としてのレーダ装置、２はレーダ装置１により観測された観測ベクトルのうち、追尾目標と相関のある観測ベクトルを残差をもとに選択する相関ゲート判定器、３は、グループ判定器１３で判定した追尾目標のグループ情報と、相関ゲート判定器２で選択された追尾目標と観測データの相関結果と、予め設定された目標進行方向ベクトルとを用いて、各グループに対応する観測データの順序を判定する観測データの順序判定器、４は、相関ゲート判定器２で判定された追尾目標の相関ゲート内の観測データと、観測データの順序判定器３で判定された各グループに対応する観測データの順序情報と、グループ判定器１３で判定した追尾目標のグループ情報を用いて、追尾目標と観測データの相関を判定する順序による相関判定器である。
【００２２】
５は予測誤差評価器１０により算出された予測誤差共分散行列と予め設定されたレーダ装置１の観測誤差共分散行列とからゲイン行列を算出するゲイン行列算出器、６は、順序による相関判定器４で判定された追尾目標と観測データの相関結果と、予測器１１で算出された予測ベクトルと、ゲイン行列算出器４で算出されたゲイン行列を用いて、目標の位置及び速度の平滑ベクトルを算出する平滑器、７は平滑器６で算出された平滑ベクトルを１サンプリング時間だけ遅延する第１の遅延回路である。
【００２３】
８は、平滑器６で算出された残差ベクトル及び信頼度による重み付き残差ベクトルと、ゲイン行列算出器５により算出されたゲイン行列と、予測誤差評価器１０により算出された予測誤差共分散行列とを用いて平滑誤差共分散行列を算出する平滑誤差評価器、９は平滑誤差評価器８により算出された平滑誤差共分散行列を１サンプリング時間だけ遅延する第２の遅延回路、１０は平滑誤差評価器８で算出された平滑誤差共分散行列から現時刻より１サンプリング後の予測誤差共分散行列を算出する予測誤差評価器、１１は、平滑器６で算出された平滑ベクトルを第１の遅延回路７を介して入力し、これと予め設定された目標の運動モデルを用いて現時刻より１サンプリング後の目標の予測位置及び予測速度から成る予測ベクトルを算出する予測器である。
【００２４】
１２は、平滑器６で算出された平滑ベクトルを第１の遅延回路７を介して入力し、これと予め設定された目標進行方向ベクトルとを用いて、追尾目標の順序を判定する航跡の順序判定器、１３は航跡の順序判定器１２で判定された追尾目標の順序に従い、平滑位置の近い追尾目標同士をグループ化するグループ判定器である。
【００２５】
次に動作について説明する。最初に、この実施の形態１による目標追尾装置の動作原理を説明する。図２に示すように、レーダ装置１が地上から高さｚｒの位置に設置され、直線道路上を一定方向に走行する車両を追尾する場合を仮定する。レーダ装置１のアンテナ取り付け位置の鉛直線と地上面との交点を原点とし、道路交差方向をｘ軸、道路に沿った方向をｙ軸、 鉛直上向きをｚ軸の正とするｏ−ｘｙｚ座標を基準座標にとる。基準座標におけるレーダ装置１のアンテナ取り付け位置は（０，０，ｚｒ）とする。
【００２６】
レーダ装置１はアンテナ取り付け位置に対する目標の距離Ｒ及び方位角Ａｚ（ｙ軸正の方向より反時計回りを正とする）を観測する。この極座標（Ｒ，Ａｚ）をレーダ観測座標とする。図１より、ｘｙ座標とレーダ座標の間には式（１）及び式（２）の関係がある。
【００２７】
【数１】
0004097126.tif 000002
【００２８】
次に、目標の運動モデルを式（３）に示す。ただし、アンダバーｘｋはサンプリング時刻における目標運動諸元の真値を表す状態ベクトルであり、ｘｙ座標における目標位置ベクトルを式（４）、速度ベクトルを式（５）とすると、目標の状態ベクトルは式（6）で表される。なお、アンダバーＡＴはベクトルアンダバーＡの転置ベクトルを表す。
【００２９】
【数２】
0004097126.tif 000003
【００３０】
ここで、Φｋ−１はサンプリング時刻ｔｋ−１よりｔｋへの状態ベクトルの推移行列であり、式（７）で表される。また、アンダバーｗｋはサンプリング時刻ｔｋにおける駆動雑音ベクトルであり、Γ１（ｋ）はサンプリング時刻ｔｋにおける駆動雑音ベクトルの変換行列である。例えば、目標の運動モデルを等速直線運動と仮定したことによる打ち切り誤差項をΓ１（ｋ−１）アンダバーｗｋ−１とみれば、アンダバーｗｋは加速度ベクトル相当であり、Γ１（ｋ−１）は式（８）で表される。なお、Ｔはレーダ装置１のサンプリング間隔、Ｉは２行２列の単位行列である。
【００３１】
【数３】
0004097126.tif 000004
【００３２】
また、平均を表す記号としてＥを用いると、アンダバーｗｋは平均の２次元正規分布白色雑音であり、式（７）及び（８）とする。ただし、アンダバー０は零ベクトルであり、Ｑｋはサンプリング時刻ｔｋにおける駆動雑音共分散行列である。
【００３３】
【数４】
0004097126.tif 000005
【００３４】
次に目標の距離Ｒ及び方位角Ａｚがサンプリング時刻ｔｋにレーダ装置１より観測される場合のｘｙ座標におけるレーダ装置１の観測モデルを式（１１）とする。ここで、距離Ｒ及び方位角Ａｚは式（１）及び式（２）によりｘｙ座標に変換される。また、アンダバーｚｋはサンプリング時刻ｔｋにおけるレーダ観測装置１の観測ベクトル、Ｈは観測行列で、式（１２）で表される。
【００３５】
また、アンダバーνｋはサンプリング時刻ｔｋにおけるレーダ装置１の観測雑音ベクトルであり、平均アンダバー０の２次元正規分布白色雑音で、式（１３）及び（１４）で表される。なお、Ｒｋはサンプリング時刻ｔｋにおけるレーダ装置１の観測誤差共分散行列である。Γ２（ｋ）は極座標よりｘｙ座標への観測雑音ベクトルの変換行列で、式（１５）で表される。サンプリング時刻ｔｋまでに目標の追尾に用いたレーダ装置１の観測ベクトル全体をＺｋとする（式（１６）を参照）。
【００３６】
【数５】
0004097126.tif 000006
【００３７】
次に、サンプリング時刻ｔｋ−１までのレーダ装置１の観測ベクトルＺｋ−１が得られているときの予測処理について述べる。目標が等速直線運動をしていると仮定した場合のサンプリング時刻ｔｋにおける追尾目標の状態ベクトルアンダバーｘｋの予測ベクトルをアンダバーｘｋハット（−）、予測誤差共分散行列をＰｋ（−）とすると、それぞれ条件付平均ベクトル、条件付共分散行列で定義され、式（１７）及び式（１８）で表される。ここで、アンダバーｘｋハット（＋）及びＰｋ−１（＋）はそれぞれ前サンプリング時刻ｔｋ−１の平滑ベクトル及び平滑誤差共分散行列である。これらの算出方法については後で述べる。
【００３８】
【数６】
0004097126.tif 000007
【００３９】
次に、相関ゲート判定処理について述べる。追尾目標の観測ベクトルの予測ベクトルは式（１９）で表される。サンプリング時刻ｔｋにおいてレーダ装置１より観測されたｍ個の観測ベクトルをアンダバーｚｋ，ｉ（ｉ＝１，２，・・・，ｍ）とすると、当該追尾目標との残差εｋ，ｉが予め設定されたゲートサイズパラメータρに対して式（２０）を満たすとき、その観測ベクトルの全てを相関ゲート内の観測ベクトルとし、当該追尾目標と相関があるとみなす。この相関ゲート判定は全ての追尾目標について行う。
【００４０】
【数７】
0004097126.tif 000008
【００４１】
次に、追尾の対象となる道路領域における追尾車両の走行順序を判定する方法について述べる。サンプリング時刻ｔｋにおいて、１サンプリング前に算出されたj番目の追尾目標の平滑ベクトルの位置ベクトルアンダバーξｋ−１ｊを式（２１）とおく。一方、車両の進行方向であるｙ軸正方向の単位ベクトルをアンダバーｅｄとおくと、式（２２）で表されるこれらのベクトルの内積Ψｋ−１ｊは車両の位置ベクトルの車両進行方向の成分となるので、車両の前後関係は内積Ψｋ−１ｊの大小関係を見ればよい。すなわち、内積Ψｋ−１ｊが大きい順序に車両ｊを並べた結果が車両の走行順序となる。
【００４２】
【数８】
0004097126.tif 000009
【００４３】
次に、追尾車両のグループを判定する方法について述べる。前述の処理により判定された追尾車両の走行順序に対応して、改めて追尾車両の平滑ベクトルの位置ベクトルの順序を先頭から順に、アンダバーξｋ−１１，アンダバーξｋ−１２，アンダバーξｋ−１３，・・・と記述し直す。先頭から順に、隣り合う平滑ベクトルの位置ベクトルの距離ｒが予め設定されたパラメータｒｍｉｎに対して式（２３）を満たすか否かを調べ、式（２３）を満たす航跡同士を同一グループと判定する。
【００４４】
【数９】
0004097126.tif 000010
【００４５】
次に、追尾の対象となる道路領域における観測ベクトルの存在順序を判定する方法について述べる。観測ベクトルの存在順序は、追尾車両の走行順序を判定する方法と同様に、式（２４）で表される観測ベクトルと車両進行方向の単位ベクトルアンダバーｅｄとの内積λｋ−１ｉの大小関係を見ればよい。すなわち、内積λｋ−１ｉが大きい順序に観測ベクトルを並べた結果が観測ベクトルの存在順序となる。
【００４６】
【数１０】
0004097126.tif 000011
【００４７】
次に、追尾目標と観測ベクトルの順序情報を利用した相関判定方法について述べる。本処理では追尾車両の追い越しがないように、追尾目標の順序と観測ベクトルの順序を逆転させないように相関判定を行う。相関判定は前述の追尾車両のグループ判定において分類されたグループ毎に以下の処理を行う。
【００４８】
先頭の追尾車両から順番に、その追尾車両の相関ゲート内の観測ベクトルのうち存在順序が先頭側の観測ベクトルを当該追尾車両と相関があるものとしてその組み合わせを記憶する。このとき、相関があると判定された観測ベクトルは以後の追尾車両の相関対象から除く。例えば、図３に示すように、３つの追尾車両が同一グループと判定され、これらの追尾車両の相関ゲート内と判定された観測ベクトルが４つあるとする。
【００４９】
ここで、追尾車両の走行順序は先頭から、アンダバーξｋ−１１，アンダバーξｋ−１２，アンダバーξｋ−１３であり、観測ベクトルの存在順序は先頭側から、アンダバーｚｋ，１，アンダバーｚｋ，２，アンダバーｚｋ，３，アンダバーｚｋ，４，とする。図３は、これらの相関ゲート内外関係を示しており、相関ゲート内と判定された場合は○、相関ゲート外と判定された場合は×で表している。まず、先頭車両であるアンダバーξｋ−１１に着目する。
【００５０】
この車両の相関ゲート内に存在する観測ベクトルのうち存在順序が先頭側なのはアンダバーｚｋ，１であるから、アンダバーξｋ−１１はアンダバーｚｋ，１と相関があると判定する。次に、アンダバーｚｋ，１を消去し、後続の走行車両であるアンダバーξｋ−１２に着目する。この車両の相関ゲート内に存在する観測ベクトルのうち、存在順序が先頭側なのはアンダバーｚｋ，２であるから、アンダバーξｋ−１２はアンダバーｚｋ，２と相関があると判定する。同様にしてアンダバーξｋ−１３はアンダバーｚｋ，３と相関があると判定する。
【００５１】
次に、平滑処理について述べる。前述の相関処理により、追尾目標と相関があると判定された観測ベクトルをアンダバーｚｋと置き直すと、ゲイン行列Ｋｋ、平滑ベクトルアンダバーｘｋハット（＋）及び平滑誤差共分散行列Ｐｋ（＋）は通常のカルマンフィルタの理論により、式（２５）〜式（２７）で与えられる。ここで、Ｒｋは式（１７）で表される、サンプリング時刻ｔｋにおけるレーダ装置１の観測誤差共分散行列であり、アンダバーνｋは式（２８）で与えられる残差ベクトルである。
【００５２】
【数１１】
0004097126.tif 000012
【００５３】
次に、この実施の形態１による目標追尾装置の動作を具体的に説明する。なお、カルマンフィルタを目標追尾装置に通常適用する場合と同様にして状態ベクトルの初期値は別途定まっているものとする。
【００５４】
レーダ装置１では、図２に示すように目標の距離Ｒ及び方位角Ａｚを観測し、式（１）及び式（２）に従いｘｙ座標に変換する（ステップＳＴ１）。相関ゲート判定器２では、予測器１１から追尾対象の予測ベクトルアンダバーｘｋハット（−）を入力して観測ベクトルのベクトルアンダバーｚｋハット（−）を式（１９）に従い算出し、レーダ装置１から入力した観測ベクトルのうち、予め設定されたゲートサイズパラメータρに対して式（２０）を満たす観測ベクトルの全てを当該追尾目標の相関ゲート内の観測ベクトルとして選択する。これを全ての追尾目標に対して行う（ステップＳＴ２）。
【００５５】
観測データの順序判定器３では、グループ判定器１３により判定されたグループ毎に、そのグループに所属する追尾目標の相関ゲート内の観測ベクトルに対し、予め設定された車両の進行方向の単位ベクトルアンダバーｅｄとの内積λｋ−１ｉを式（２４）に従い算出する。そして、内積λｋ−１ｉの大きい順番を観測ベクトルの存在順序として記憶する（ステップＳＴ３）。
【００５６】
順序による相関判定器４では、グループ判定器１３において判定されたグループ毎に、観測データの順序判定器３により判定されたそのグループに所属する追尾目標の相関ゲート内の観測ベクトルの存在順序の情報と、航跡の順序判定器１２で判定されたそのグループに所属する追尾車両の走行順序の情報を入力し、走行順序の順番に、その追尾車両の相関ゲート内の観測ベクトルのうち存在順序が先頭側の観測ベクトルを当該追尾車両と相関があると判定してその組み合わせを記憶する。このとき、相関があると判定された観測ベクトルは後に続く追尾車両の相関対象から除く。そして、全てのグループに対して同様の処理を行う。（ステップＳＴ４）。
【００５７】
ゲイン行列算出器５では、追尾目標毎に、予測誤差評価器１０から入力した予測誤差共分散行列Ｐｋ（−）と、予め設定されたレーダ装置１の観測誤差共分散行列Ｒｋとから式（２５）に従いゲイン行列Ｋｋを算出する（ステップＳＴ５）。平滑器６では、追尾目標毎に、順序による相関判定器４で判定した相関結果と、予測器１１の出力した予測ベクトルアンダバーｘｋハット（−）と、ゲイン行列算出器５からゲイン行列Ｋｋとをそれぞれ入力し、式（２６）に従い平滑ベクトルアンダバーｘｋハット（＋）を算出する（ステップＳＴ６）。
【００５８】
一方、平滑誤差評価器８では、予測誤差評価器１０から予測誤差共分散行列Ｐｋ（−）を、ゲイン行列算出器５からゲイン行列Ｋｋをそれぞれ入力し、式（２７）に従い平滑誤差共分散行列Ｐｋ（＋）を算出する（ステップＳＴ７）。予測誤差評価器１０では、第２の遅延回路９から１サンプリング前の平滑誤差共分散行列Ｐｋ−１（＋）を、予め設定された駆動雑音共分散行列Ｑｋ−１をそれぞれ入力し、予測誤差共分散行列Ｐｋ（−）を式（１８）に従い算出する（ステップＳＴ８）。予測器１１では、１サンプリング前の平滑ベクトルアンダバーｘｋ−１ハット（＋）を第１の遅延回路７を介して入力し、式（１７）に従い予測ベクトルアンダバーｘｋハット（−）を算出する（ステップＳＴ９）。
【００５９】
航跡の順序判定器１２では、平滑器６において１サンプリング前に算出された追尾目標の平滑ベクトルを第１の遅延回路７を介して入力し、その位置ベクトルアンダバーξｋ−１ｊと、予め設定された車両の進行方向の単位ベクトルアンダバーｅｄとの内積Ψｋ−１ｊを式（２２）に従い算出する。そして、内積Ψｋ−１ｊの大きい順番を航跡の走行順序として記憶する（ステップＳＴ１０）。グループ判定機１３では、航跡の順序判定機１２で判定された追尾車両の走行順序の順番に隣り合う平滑ベクトルの位置ベクトルの距離ｒが予め設定されたパラメータｒｍｉｎに対して式（２３）を満たすか否かを調べ、式（２３）を満たす航跡同士を同一グループと判定する（ステップＳＴ１１）。
【００６０】
以上で明らかなように、この実施の形態１によれば、観測データの順序判定器３により、車両の進行方向に対する観測ベクトルの存在順序を、予め設定された車両の進行方向の単位ベクトルと観測ベクトルの内積の大きさから判定し、一方、航跡の順序判定器１２により、車両の走行順序を、上記単位ベクトルと追尾車両の平滑ベクトルの位置ベクトルの内積の大きさから判定し、順序による相関判定器４において、走行順序の順番に、追尾車両の相関ゲート内の観測ベクトルのうち存在順序が先頭側の観測ベクトルを当該追尾車両と相関があるものと判定する構成としているので、追尾車両の航跡の順序が入れ替わりにくい効果を奏する。
【００６１】
実施の形態２．
図４は、この実施の形態２による目標追尾装置を示す構成図であり、図において、図１と同一符号である１〜３及び５〜１３は実施の形態１と同一または相当部分を示すので、その説明を省略する。
【００６２】
１４は相関ゲート判定器２で判定された追尾目標の相関ゲート内の観測データと、その残差を入力し、これらと、観測データの順序判定器３で判定された各グループに対応する観測データの順序情報と、グループ判定器１３で判定した追尾目標のグループ情報を用いて、追尾目標と観測データの相関を判定する順序と残差による相関判定器である。
【００６３】
次に動作について説明する。最初に、この実施の形態２による目標追尾装置の動作原理を説明する。レーダ装置１による車両の観測方法、目標の運動モデル、レーダ装置１の観測モデルは、実施の形態１における式（１）〜式（１６）までの原理と同じであるので省略する。
【００６４】
また、予測処理及び相関ゲート判定処理は、実施の形態１における式（１７）〜式（２０）までの原理と同じであるので省略する。また、追尾車両の走行順序を判定する方法、追尾車両のグループを判定する方法、そして、観測ベクトルの存在順序を判定する方法も、実施の形態１における式（２１）〜式（２４）までの原理と同じであるので省略する。
【００６５】
次に、追尾目標と観測ベクトルの順序情報を利用した相関判定方法について述べる。本処理では追尾車両の追い越しがないように、追尾目標の順序と観測ベクトルの順序を逆転させないように相関判定を行う。相関判定は前述の追尾車両のグループ判定において分類されたグループ毎に以下の処理を行う。
【００６６】
先頭の追尾車両から順番に、その追尾車両の相関ゲート内の観測ベクトルのうち式（２０）で表される残差εｋ，ｉが最も小さい観測ベクトルを当該追尾車両と相関があるものと判定してその組み合わせを記憶する。このとき、相関があると判定された観測ベクトルとそれよりも存在順序が前の観測ベクトルは以後の追尾車両の相関対象から除く。
【００６７】
例えば、図５に示すように、３つの追尾車両が同一グループと判定され、これらの追尾車両の相関ゲート内と判定された観測ベクトルが４つあるとする。ここで、追尾車両の走行順序は先頭から、アンダバーξｋ−１１，アンダバーξｋ−１２，アンダバーξｋ−１３であり、観測ベクトルの存在順序は先頭側から、アンダバーｚｋ，１，アンダバーｚｋ，２，アンダバーｚｋ，３，アンダバーｚｋ，４とする。図５は、これらの相関ゲート内外関係及び残差を示しており、相関ゲート内と判定された場合は残差を、相関ゲート外と判定された場合は×で表している。
【００６８】
まず、先頭車両であるアンダバーξｋ−１１に着目する。この車両の相関ゲート内に存在する観測ベクトルのうち残差が最も小さい観測ベクトルはアンダバーｚｋ，２であるから、アンダバーξｋ−１１はアンダバーｚｋ，２と相関があると判定する。次に、アンダバーｚｋ，２とこれより前方に存在するアンダバーｚｋ，１を消去し、後続の走行車両であるアンダバーξｋ−１２に着目する。この車両の相関ゲート内に存在する観測ベクトルのうち、残差が最も小さいのはアンダバーｚｋ，３であるから、アンダバーξｋ−１２はアンダバーｚｋ，３と相関があると判定する。同様にしてアンダバーξｋ−１３はアンダバーｚｋ，４と相関があると判定する。
【００６９】
次に、平滑処理について述べる。前述の相関処理により、追尾目標と相関があると判定された観測ベクトルをアンダバーｚｋと置き直すと、ゲイン行列Ｋｋ、平滑ベクトルアンダバーｘｋハット（＋）及び平滑誤差共分散行列Ｐｋ（＋）は通常のカルマンフィルタの理論により、式（２５）〜式（２７）で与えられる。ここで、Ｒｋは式（１７）で表される、サンプリング時刻ｔｋにおけるレーダ装置１の観測誤差共分散行列であり、アンダバーνｋは式（２８）で与えられる残差ベクトルである。
【００７０】
次に、この実施の形態２による目標追尾装置の動作を具体的に説明する。なお、カルマンフィルタを目標追尾装置に通常適用する場合と同様にして状態ベクトルの初期値は別途定まっているものとする。
【００７１】
レーダ装置１では、図２に示すように目標の距離Ｒ及び方位角Ａｚを観測し、式（１）及び式（２）に従いｘｙ座標に変換する（ステップＳＴ１）。相関ゲート判定器２では、予測器１１から追尾対象の予測ベクトルアンダバーｘｋハット（−）を入力して観測ベクトルのベクトルアンダバーｚｋ（−）を式（１９）に従い算出し、レーダ装置１から入力した観測ベクトルのうち、予め設定されたゲートサイズパラメータρに対して式（２０）を満たす観測ベクトルの全てを当該追尾目標の相関ゲート内の観測ベクトルとして選択する。これを全ての追尾目標に対して行う（ステップＳＴ２）。
【００７２】
観測データの順序判定器３では、グループ判定器１３により判定されたグループ毎に、そのグループに所属する追尾目標の相関ゲート内の観測ベクトルに対し、予め設定された車両の進行方向の単位ベクトルアンダバーｅｄとの内積λｋ−１ｉを式（２４）に従い算出する。そして、内積λｋ−１ｉの大きい順番を観測ベクトルの存在順序として記憶する（ステップＳＴ３）。
【００７３】
順序と残差による相関判定器１４では、グループ判定器１３において判定されたグループ毎に、観測データの順序判定器３により判定されたそのグループに所属する追尾目標の相関ゲート内の観測ベクトルの存在順序の情報及び残差と、航跡の順序判定器１２で判定されたそのグループに所属する追尾車両の走行順序の情報を入力し、走行順序の順番に、その追尾車両の相関ゲート内の観測ベクトルのうち残差が最も小さい観測ベクトルを当該追尾車両と相関があると判定してその組み合わせを記憶する。このとき、相関があると判定された観測ベクトルは後に続く追尾車両の相関対象から除く。そして、全てのグループに対して同様の処理を行う（ステップＳＴ４）。
【００７４】
ゲイン行列算出器５では、追尾目標毎に、予測誤差評価器１０から入力した予測誤差共分散行列Ｐｋ（−）と、予め設定されたレーダ装置１の観測誤差共分散行列Ｒｋとから式（２５）に従いゲイン行列Ｋｋを算出する（ステップＳＴ５）。平滑器６では、追尾目標毎に、順序と残差による相関判定器１４で判定した相関結果と、予測器１１の出力した予測ベクトルアンダバーｘｋハット（−）と、ゲイン行列算出器５からゲイン行列Ｋｋとをそれぞれ入力し、式（２６）に従い平滑ベクトルアンダバーｘｋハット（＋）を算出する（ステップＳＴ６）。
【００７５】
一方、平滑誤差評価器８では、予測誤差評価器１０から予測誤差共分散行列Ｐｋ（−）を、ゲイン行列算出器５からゲイン行列Ｋｋをそれぞれ入力し、式（２７）に従い平滑誤差共分散行列Ｐｋ（＋）を算出する（ステップＳＴ７）。予測誤差評価器１０では、第２の遅延回路９から１サンプリング前の平滑誤差共分散行列Ｐｋ−１（＋）を、予め設定された駆動雑音Ｑｋ−１をそれぞれ入力し、予測誤差共分散行列Ｐｋ（−）を式（１８）に従い算出する（ステップＳＴ８）。予測器１１では、１サンプリング前の平滑ベクトルアンダバーｘｋ−１ハット（＋）を第１の遅延回路７を介して入力し、式（１７）に従い予測ベクトルアンダバーｘｋハット（−）を算出する（ステップＳＴ９）。
【００７６】
航跡の順序判定器１２では、平滑器６において１サンプリング前に算出された追尾目標の平滑ベクトルを第１の遅延回路７を介して入力し、その位置ベクトルアンダバーξｋ−１ｊと、予め設定された車両の進行方向の単位ベクトルアンダバーｅｄとの内積Ψｋ−１ｊを式（２２）に従い算出する。そして、内積Ψｋ−１ｊの大きい順番を航跡の走行順序として記憶する（ステップＳＴ１０）。グループ判定機１３では、航跡の順序判定機１２で判定された追尾車両の走行順序の順番に隣り合う平滑ベクトルの位置ベクトルの距離ｒが予め設定されたパラメータｒｍｉｎに対して式（２３）を満たすか否かを調べ、式（２３）を満たす航跡同士を同一グループと判定する（ステップＳＴ１１）。
【００７７】
以上で明らかなように、この実施の形態２によれば、観測データの順序判定器３により、車両の進行方向に対する観測ベクトルの存在順序を、予め設定された車両の進行方向の単位ベクトルと観測ベクトルの内積の大きさから判定し、一方、航跡の順序判定器１２により、車両の走行順序を、上記単位ベクトルと追尾車両の平滑ベクトルの位置ベクトルの内積の大きさから判定し、順序と残差による相関判定器１４において、走行順序の順番に、追尾車両の相関ゲート内の観測ベクトルのうち残差が最も小さい観測ベクトルを当該追尾車両と相関があるものと判定し、それより前方に存在する観測ベクトルを以降の追尾目標の相関対象から除く構成としているので、追尾車両の航跡の順序が入れ替わりにくく、かつ、レーダ装置１により誤検出が検出される場合においても誤った相関結果となりにくい効果を奏する。
【００７８】
実施の形態３．
図６は、この実施の形態３による目標追尾装置を示す構成図であり、図において、図１と同一符号である１〜１３は実施の形態１と同一または相当部分を示すので、その説明を省略する。１５は順序による相関判定器４で判定された追尾目標と観測データの相関結果において、相関する観測データのない追尾目標に対して、擬似的な観測データを生成する擬似データ生成器である。
【００７９】
次に動作について説明する。最初に、この実施の形態３による目標追尾装置の動作原理を説明する。レーダ装置１による車両の観測方法、目標の運動モデル、レーダ装置１の観測モデルは、実施の形態１における式（１）〜式（１６）までの原理と同じであるので省略する。
【００８０】
また、相関判定方法は実施の形態１と同じ原理であるので省略する。また、予測処理及び相関ゲート判定処理は、実施の形態１における式（１７）〜式（２０）までの原理と同じであるので省略する。また、追尾車両の走行順序を判定する方法、追尾車両のグループを判定する方法、そして、観測ベクトルの存在順序を判定する方法も、実施の形態１における式（２１）〜式（２４）までの原理と同じであるので省略する。
【００８１】
次に相関する観測データのない追尾目標が存在する場合の擬似的な観測データの生成方法について述べる。相関判定器４において、どの観測ベクトルとも相関の得られない追尾目標がある場合、通常のメモリートラック（予測位置をそのまま平滑位置とする方法）を行うと、その前後の車両が加減速を行っている場合に前方の車両を追い越す、あるいは後方の車両に追い抜かれる等の可能性が高くなる。
【００８２】
そこで、相関の得られない追尾目標は、同一グループ内で相関の得られた前後の車両の１サンプル前の平滑位置の間隔と同じ比率で、前後車両と相関の得られた観測ベクトルを内分した位置を用いて平滑を行う。相関の得られない追尾目標の１サンプリング前の平滑ベクトルの位置ベクトルをアンダバーξｋ−１ｊとする。そして、そのａ台前の車両とｂ台前の車両の１サンプリング前の平滑ベクトルの位置ベクトルをそれぞれアンダバーξｋ−１ｊ−ａおよびアンダバーξｋ−１ｊ＋ｂとし、これらと相関の得られた現時刻の観測ベクトルをそれぞれアンダバーｚｋ，ａ及びアンダバーｚｋ，ｂとすると、１サンプル前の平滑位置の間隔と同じ比率で、前後車両と相関の得られた観測ベクトルを内分した位置アンダバーｚｋ，ｊは式（２９）で表すことができる。但し、相関の得られない追尾目標の前後の何れかにしか相関の得られた追尾目標がない場合は、その追尾目標の１サンプリング前の平滑ベクトルの位置ベクトルをアンダバーξｋ−１ｌとすると、式（３０）で表される平滑ベクトルの差ベクトルの位置で外挿した位置を用いる。
【００８３】
【数１２】
0004097126.tif 000013
【００８４】
次に、平滑処理について述べる。前述の相関処理により、相関対象が存在する追尾目標はその相関対象となる観測ベクトルをアンダバーｚｋと置き直し、相関対象が存在しない追尾目標は、前述の擬似的な観測データの生成方法で得られた位置ベクトルをアンダバーｚｋと置き直すと、ゲイン行列Ｋｋ、平滑ベクトルアンダバーｘｋハット（＋）及び平滑誤差共分散行列Ｐｋ（＋）は通常のカルマンフィルタの理論により、式（２５）〜式（２７）で与えられる。ここで、Ｒｋは式（１７）で表される、サンプリング時刻ｔｋにおけるレーダ装置１の観測誤差共分散行列であり、アンダバーνｋは式（２８）で与えられる残差ベクトルである。
【００８５】
次に、この実施の形態３による目標追尾装置の動作を具体的に説明する。なお、カルマンフィルタを目標追尾装置に通常適用する場合と同様にして平滑ベクトルの初期値は別途定まっているものとする。
【００８６】
レーダ装置１では、図２に示すように目標の距離Ｒ及び方位角Ａｚを観測し、式（１）及び式（２）に従いｘｙ座標に変換する（ステップＳＴ１）。相関ゲート判定器２では、予測器１１から追尾対象の予測ベクトルアンダバーｘｋハット（−）を入力して観測ベクトルのベクトルアンダバーｚｋ（−）を式（１９）に従い算出し、レーダ装置１から入力した観測ベクトルのうち、予め設定されたゲートサイズパラメータρに対して式（２０）を満たす観測ベクトルの全てを当該追尾目標の相関ゲート内の観測ベクトルとして選択する。これを全ての追尾目標に対して行う（ステップＳＴ２）。
【００８７】
観測データの順序判定器３では、グループ判定器１３により判定されたグループ毎に、そのグループに所属する追尾目標の相関ゲート内の観測ベクトルに対し、予め設定された車両の進行方向の単位ベクトルアンダバーｅｄとの内積λｋ−１ｉを式（２４）に従い算出する。そして、内積λｋ−１ｉの大きい順番を観測ベクトルの存在順序として記憶する（ステップＳＴ３）。
【００８８】
順序による相関判定器４では、グループ判定器１３において判定されたグループ毎に、観測データの順序判定器３により判定されたそのグループに所属する追尾目標の相関ゲート内の観測ベクトルの存在順序の情報と、航跡の順序判定器１２で判定されたそのグループに所属する追尾車両の走行順序の情報を入力し、走行順序の順番に、その追尾車両の相関ゲート内の観測ベクトルのうち存在順序が先頭側の観測ベクトルを当該追尾車両と相関があると判定してその組み合わせを記憶する。このとき、相関があると判定された観測ベクトルは後に続く追尾車両の相関対象から除く。そして、全てのグループに対して同様の処理を行う。（ステップＳＴ４）。
【００８９】
擬似データ生成器１５では、順序による相関判定器４で判定された追尾目標と観測データの相関結果と、原時刻より１サンプリング前に平滑器６で算出された平滑ベクトルを第１の遅延回路を介して入力し、グループ毎に、相関する観測データのない追尾目標に対して、その前後に相関の取れた追尾目標がある場合には式（２９）に従い、前後のどちらか１方に追尾目標がある場合には式（３０）に従い擬似的な観測ベクトルを生成する（ステップＳＴ５）。
【００９０】
ゲイン行列算出器５では、追尾目標毎に、予測誤差評価器１０から入力した予測誤差共分散行列Ｐｋ（−）と、予め設定されたレーダ装置１の観測誤差共分散行列Ｒｋとから式（２５）に従いゲイン行列Ｋｋを算出する（ステップＳＴ６）。平滑器６では、追尾目標毎に、順序による相関判定器４で判定した相関結果と、擬似データ生成器１５で生成した擬似的な観測ベクトルと、予測器１１の出力した予測ベクトルアンダバーｘｋハット（−）と、ゲイン行列算出器５からゲイン行列Ｋｋをそれぞれ入力し、式（２６）に従い平滑ベクトルアンダバーｘｋハット（＋）を算出する（ステップＳＴ７）。
【００９１】
一方、平滑誤差評価器８では、予測誤差評価器１０から予測誤差共分散行列Ｐｋ（−）を、ゲイン行列算出器５からゲイン行列Ｋｋをそれぞれ入力し、式（２７）に従い平滑誤差共分散行列Ｐｋ（＋）を算出する（ステップＳＴ８）。予測誤差評価器１０では、第２の遅延回路９から１サンプリング前の平滑誤差共分散行列Ｐｋ−１（＋）を、予め設定された駆動雑音Ｑｋ−１をそれぞれ入力し、予測誤差共分散行列Ｐｋ（−）を式（１８）に従い算出する（ステップＳＴ９）。予測器１１では、１サンプリング前の平滑ベクトルアンダバーｘｋ−１ハット（＋）を第１の遅延回路７を介して入力し、式（１７）に従い予測ベクトルアンダバーｘｋハット（−）を算出する（ステップＳＴ１０）。
【００９２】
航跡の順序判定器１２では、平滑器６において１サンプリング前に算出された追尾目標の平滑ベクトルを第１の遅延回路７を介して入力し、その位置ベクトルアンダバーξｋ−１ｊと、予め設定された車両の進行方向の単位ベクトルアンダバーｅｄとの内積Ψｋ−１ｊを式（２２）に従い算出する。そして、内積Ψｋ−１ｊの大きい順番を航跡の走行順序として記憶する（ステップＳＴ１１）。グループ判定機１３では、航跡の順序判定機１２で判定された追尾車両の走行順序の順番に隣り合う平滑ベクトルの位置ベクトルの距離ｒが予め設定されたパラメータｒｍｉｎに対して式（２３）を満たすか否かを調べ、式（２３）を満たす航跡同士を同一グループと判定する（ステップＳＴ１２）。
【００９３】
以上で明らかなように、この実施の形態３によれば、観測データの順序判定器３により、車両の進行方向に対する観測ベクトルの存在順序を、予め設定された車両の進行方向の単位ベクトルと観測ベクトルの内積の大きさから判定し、一方、航跡の順序判定器１２により、車両の走行順序を、上記単位ベクトルと追尾車両の平滑ベクトルの位置ベクトルの内積の大きさから判定し、順序による相関判定器４において、走行順序の順番に、追尾車両の相関ゲート内の観測ベクトルのうち存在順序が先頭側の観測ベクトルを当該追尾車両と相関があるものと判定する構成としているので、追尾車両の航跡の順序が入れ替わりにくい効果を奏する。更に、擬似データ生成器１５において、相関する観測データのない追尾目標に対して、その前後もしくはどちらか一方の相関の取れた追尾目標の１サンプリング前の平滑位置と現時刻に相関の取れた観測ベクトルを用いて擬似的な観測ベクトルを生成して平滑する構成としているので、レーダ装置１が失検出した場合においても追尾車両の航跡の順序が入れ替わりにくい効果を奏する。
【００９４】
実施の形態４．
図７は、この実施の形態４による目標追尾装置を示す構成図であり、図において、図４と同一符号である１〜３及び５〜１４は実施の形態２と同一または相当部分を示すので、その説明を省略する。１５は順序と残差による相関判定器１４で判定された追尾目標と観測データの相関結果において、相関する観測データのない追尾目標に対して、擬似的な観測データを生成する擬似データ生成器である。
【００９５】
次に動作について説明する。最初に、この実施の形態４による目標追尾装置の動作原理を説明する。レーダ装置１による車両の観測方法、目標の運動モデル、レーダ装置１の観測モデルは、実施の形態１における式（１）〜式（１６）までの原理と同じであるので省略する。
【００９６】
また、相関判定方法は実施の形態２と同じ原理であるので省略する。また、予測処理及び相関ゲート判定処理は、実施の形態１における式（１７）〜式（２０）までの原理と同じであるので省略する。また、追尾車両の走行順序を判定する方法、追尾車両のグループを判定する方法、そして、観測ベクトルの存在順序を判定する方法も、実施の形態１における式（２１）〜式（２４）までの原理と同じであるので省略する。
【００９７】
相関する観測データのない追尾目標が存在する場合の擬似的な観測データの生成方法は実施の形態３における式（２９）〜式（３０）までの原理と同じであるので省略する。
【００９８】
次に、この実施の形態４による目標追尾装置の動作を具体的に説明する。なお、カルマンフィルタを目標追尾装置に通常適用する場合と同様にして状態ベクトルの初期値は別途定まっているものとする。
【００９９】
レーダ装置１では、図２に示すように目標の距離Ｒ及び方位角Ａｚを観測し、式（１）及び式（２）に従いｘｙ座標に変換する（ステップＳＴ１）。相関ゲート判定器２では、予測器１１から追尾対象の予測ベクトルアンダバーｘｋハット（−）を入力して観測ベクトルのベクトルアンダバーｚｋ（−）を式（１９）に従い算出し、レーダ装置１から入力した観測ベクトルのうち、予め設定されたゲートサイズパラメータρに対して式（２０）を満たす観測ベクトルの全てを当該追尾目標の相関ゲート内の観測ベクトルとして選択する。これを全ての追尾目標に対して行う（ステップＳＴ２）。観測データの順序判定器３では、グループ判定器１３により判定されたグループ毎に、そのグループに所属する追尾目標の相関ゲート内の観測ベクトルに対し、予め設定された車両の進行方向の単位ベクトルアンダバーｅｄとの内積λｋ−１ｉを式（２４）に従い算出する。そして、内積λｋ−１ｉの大きい順番を観測ベクトルの存在順序として記憶する（ステップＳＴ３）。
【０１００】
順序と残差による相関判定器１４では、グループ判定器１３において判定されたグループ毎に、観測データの順序判定器３により判定されたそのグループに所属する追尾目標の相関ゲート内の観測ベクトルの存在順序の情報及び残差と、航跡の順序判定器１２で判定されたそのグループに所属する追尾車両の走行順序の情報を入力し、走行順序の順番に、その追尾車両の相関ゲート内の観測ベクトルのうち残差が最も小さい観測ベクトルを当該追尾車両と相関があると判定してその組み合わせを記憶する。このとき、相関があると判定された観測ベクトルは後に続く追尾車両の相関対象から除く。そして、全てのグループに対して同様の処理を行う。（ステップＳＴ４）。
【０１０１】
擬似データ生成器１５では、順序と残差による相関判定器１４で判定された追尾目標と観測データの相関結果と、現在時刻より１サンプリング前に平滑器６で算出された平滑ベクトルを第１の遅延回路を介して入力し、グループ毎に、相関する観測データのない追尾目標に対して、その前後に相関の取れた追尾目標がある場合には式（２９）に従い、前後のどちらか一方に追尾目標がある場合には式（３０）に従い擬似的な観測ベクトルを生成する（ステップＳＴ５）。
【０１０２】
ゲイン行列算出器５では、追尾目標毎に、予測誤差評価器１０から入力した予測誤差共分散行列Ｐｋ（−）と、予め設定されたレーダ装置１の観測誤差共分散行列Ｒｋとから式（２５）に従いゲイン行列Ｋｋを算出する（ステップＳＴ６）。平滑器６では、追尾目標毎に、順序による相関判定器４で判定した相関結果と、擬似データ生成器１５で生成した擬似的な観測ベクトルと、予測器１１の出力した予測ベクトルアンダバーｘｋハット（−）と、ゲイン行列算出器５からゲイン行列Ｋｋをそれぞれ入力し、式（２６）に従い平滑ベクトルアンダバーｘｋハット（＋）を算出する（ステップＳＴ７）。
【０１０３】
一方、平滑誤差評価器８では、予測誤差評価器１０から予測誤差共分散行列Ｐｋ（−）を、ゲイン行列算出器５からゲイン行列Ｋｋをそれぞれ入力し、式（２７）に従い平滑誤差共分散行列Ｐｋ（＋）を算出する（ステップＳＴ８）。予測誤差評価器１０では、第２の遅延回路９から１サンプリング前の平滑誤差共分散行列Ｐｋ−１（＋）を、予め設定された駆動雑音Ｑｋ−１をそれぞれ入力し、予測誤差共分散行列Ｐｋ（−）を式（１８）に従い算出する（ステップＳＴ９）。予測器１１では、１サンプリング前の平滑ベクトルアンダバーｘｋ−１ハット（＋）を第１の遅延回路７を介して入力し、式（１７）に従い予測ベクトルアンダバーｘｋハット（−）を算出する（ステップＳＴ１０）。
【０１０４】
航跡の順序判定器１２では、平滑器６において１サンプリング前に算出された追尾目標の平滑ベクトルを第１の遅延回路７を介して入力し、その位置ベクトルアンダバーξｋ−１ｊと、予め設定された車両の進行方向の単位ベクトルアンダバーｅｄとの内積Ψｋ−１ｊを式（２２）に従い算出する。そして、内積Ψｋ−１ｊの大きい順番を航跡の走行順序として記憶する（ステップＳＴ１１）。グループ判定機１３では、航跡の順序判定機１２で判定された追尾車両の走行順序の順番に隣り合う平滑ベクトルの位置ベクトルの距離ｒが予め設定されたパラメータｒｍｉｎに対して式（２３）を満たすか否かを調べ、式（２３）を満たす航跡同士を同一グループと判定する（ステップＳＴ１２）。
【０１０５】
以上で明らかなように、この実施の形態４によれば、観測データの順序判定器３により、車両の進行方向に対する観測ベクトルの存在順序を、予め設定された車両の進行方向の単位ベクトルと観測ベクトルの内積の大きさから判定し、一方、航跡の順序判定器１２により、車両の走行順序を、上記単位ベクトルと追尾車両の平滑ベクトルの位置ベクトルの内積の大きさから判定し、順序と残差による相関判定器１４において、走行順序の順番に、追尾車両の相関ゲート内の観測ベクトルのうち残差が最も小さい観測ベクトルを当該追尾車両と相関があるものと判定し、それより前方に存在する観測ベクトルを以降の追尾目標の相関対象から除く構成としているので、追尾車両の航跡の順序が入れ替わりにくく、かつ、レーダ装置１により誤検出が検出される場合においても誤った相関結果となりにくい効果を奏する。更に、擬似データ生成器１５において、相関する観測データのない追尾目標に対して、その前後もしくはどちらか一方の相関の取れた追尾目標の１サンプリング前の平滑位置と現時刻に相関の取れた観測ベクトルを用いて擬似的な観測ベクトルを生成して平滑する構成としているので、レーダ装置１が失検出した場合においても追尾車両の航跡の順序が入れ替わりにくい効果を奏する。
【０１０６】
実施の形態５．
図８はこの実施の形態５による目標追尾装置を示す構成図であり、図において、図６と同一符号である１〜１３、１５は実施の形態３と同一または相当部分を示すので、その説明を省略する。また、図４と同一符号である１４は実施の形態２と同一または相当部分を示すので、その説明を省略する。
【０１０７】
１６はグループ判定器１３で判定された追尾目標のグループと、相関ゲート判定器２で判定したそのグループに属する追尾目標の相関ゲート内の観測データを観測データの順序判定器３を介して入力し、同一グループ内の追尾目標数と、その追尾目標の相関ゲート内に存在する観測データ数に応じて、相関判定処理を順序による相関判定器１５あるいは順序と残差による相関判定器１４の何れかに切り換える相関処理切り換え器１６である。
【０１０８】
次に動作について説明する。最初に、この実施の形態５による目標追尾装置の動作原理を説明する。レーダ装置１による車両の観測方法、目標の運動モデル、レーダ装置１の観測モデルは、実施の形態１における式（１）〜式（１６）までの原理と同じであるので省略する。
【０１０９】
また、相関判定方法は実施の形態２と同じ原理であるので省略する。また、予測処理及び相関ゲート判定処理は、実施の形態１における式（１７）〜式（２０）までの原理と同じであるので省略する。また、追尾車両の走行順序を判定する方法、追尾車両のグループを判定する方法、そして、観測ベクトルの存在順序を判定する方法も、実施の形態１における式（２１）〜式（２４）までの原理と同じであるので省略する。
【０１１０】
相関する観測データのない追尾目標が存在する場合の擬似的な観測データの生成方法は実施の形態３における式（２９）〜式（３０）までの原理と同じであるので省略する。
【０１１１】
次に、相関処理の切り換え方法について説明する。各グループにおいて、追尾車両数と観測データ数が等しい場合には、レーダ装置１の検出状態がよいと考えられる。この場合は、追尾車両と観測データの残差の情報を用いた相関判定を行うよりも、追尾目標と観測データの順序を合わせて１対１対応をとる方が簡単かつ相関ミスがないと考えられる。一方、追尾車両より観測データ数が多い場合にはレーダ装置１が誤検出をしていると考えられるので、順序と残差の情報を用いて相関判定を行う方法がよいと考えられる。逆に追尾車両より観測データ数が少ない場合にはレーダ装置１が失検出していると考えられるので、順序と残差の情報を用いて相関判定を行い、更に、相関の取れない追尾目標に対する擬似観測データを生成して平滑する方法がよいと考えられる。
【０１１２】
次に、この実施の形態５による目標追尾装置の動作を具体的に説明する。なお、カルマンフィルタを目標追尾装置に通常適用する場合と同様にして状態ベクトルの初期値は別途定まっているものとする。
【０１１３】
レーダ装置１では、図２に示すように目標の距離Ｒ及び方位角Ａｚを観測し、式（１）及び式（２）に従いｘｙ座標に変換する（ステップＳＴ１）。相関ゲート判定器２では、予測器１１から追尾対象の予測ベクトルアンダバーｘｋハット（−）を入力して観測ベクトルのベクトルアンダバーｚｋ（−）を式（１９）に従い算出し、レーダ装置１から入力した観測ベクトルのうち、予め設定されたゲートサイズパラメータρに対して式（２０）を満たす観測ベクトルの全てを当該追尾目標の相関ゲート内の観測ベクトルとして選択する。これを全ての追尾目標に対して行う（ステップＳＴ２）。観測データの順序判定器３では、グループ判定器１３により判定されたグループ毎に、そのグループに所属する追尾目標の相関ゲート内の観測ベクトルに対し、予め設定された車両の進行方向の単位ベクトルアンダバーｅｄとの内積λｋ−１ｉを式（２４）に従い算出する。そして、内積λｋ−１ｉの大きい順番を観測ベクトルの存在順序として記憶する（ステップＳＴ３）。
【０１１４】
相関処理切り換え器１６では、グループ判定器１３で判定された追尾目標のグループと、相関ゲート判定器２で判定したそのグループに属する追尾目標の相関ゲート内の観測データを観測データの順序判定器３を介して入力し、各グループ毎に、グループの属する追尾目標数と、その追尾目標の相関ゲート内に存在する観測データ数に応じて次の処理分岐を行う。追尾車両数と観測データ数が等しい場合には、順序による相関判定器４の処理に移る。追尾車両と観測データ数が異なる場合には順序と残差による相関判定器１４の処理に移る（ステップＳＴ４）。
【０１１５】
順序による相関判定器４では、グループ判定器１３において判定されたグループ毎に、観測データの順序判定器３により判定されたそのグループに所属する追尾目標の相関ゲート内の観測ベクトルの存在順序の情報を相関処理切り換え器１６を介して入力し、また、航跡の順序判定器１２で判定されたそのグループに所属する追尾車両の走行順序の情報を入力し、走行順序の順番に、その追尾車両の相関ゲート内の観測ベクトルのうち存在順序が先頭側の観測ベクトルを当該追尾車両と相関があると判定してその組み合わせを記憶する。このとき、相関があると判定された観測ベクトルは後に続く追尾車両の相関対象から除く。そして、全てのグループに対して同様の処理を行う。（ステップＳＴ５）。
【０１１６】
順序と残差による相関判定器１４では、グループ判定器１３において判定されたグループ毎に、観測データの順序判定器３により判定されたそのグループに所属する追尾目標の相関ゲート内の観測ベクトルの存在順序の情報及び残差を相関処理切り換え器１６を介して入力し、また、航跡の順序判定器１２で判定されたそのグループに所属する追尾車両の走行順序の情報を入力し、走行順序の順番に、その追尾車両の相関ゲート内の観測ベクトルのうち残差が最も小さい観測ベクトルを当該追尾車両と相関があると判定してその組み合わせを記憶する。このとき、相関があると判定された観測ベクトルは後に続く追尾車両の相関対象から除く。そして、全てのグループに対して同様の処理を行う。（ステップＳＴ６）。
【０１１７】
擬似データ生成器１５では、順序と残差による相関判定器１４で判定された追尾目標と観測データの相関結果と、原時刻より１サンプリング前に平滑器６で算出された平滑ベクトルを第１の遅延回路を介して入力し、グループ毎に、相関する観測データのない追尾目標に対して、その前後に相関の取れた追尾目標がある場合には式（２９）に従い、前後のどちらか１方に追尾目標がある場合には式（３０）に従い擬似的な観測ベクトルを生成する（ステップＳＴ７）。
【０１１８】
ゲイン行列算出器５では、追尾目標毎に、予測誤差評価器１０から入力した予測誤差共分散行列Ｐｋ（−）と、予め設定されたレーダ装置１の観測誤差共分散行列Ｒｋとから式（２５）に従いゲイン行列Ｋｋを算出する（ステップＳＴ８）。平滑器６では、追尾目標毎に、順序による相関判定器４で判定した相関結果と、擬似データ生成器１５で生成した擬似的な観測ベクトルと、予測器１１の出力した予測ベクトルアンダバーｘｋハット（−）と、ゲイン行列算出器５からゲイン行列Ｋｋをそれぞれ入力し、式（２６）に従い平滑ベクトルアンダバーｘｋハット（＋）を算出する（ステップＳＴ９）。
【０１１９】
一方、平滑誤差評価器８では、予測誤差評価器１０から予測誤差共分散行列Ｐｋ（−）を、ゲイン行列算出器５からゲイン行列Ｋｋをそれぞれ入力し、式（２７）に従い平滑誤差共分散行列Ｐｋ（＋）を算出する（ステップＳＴ１０）。予測誤差評価器１０では、第２の遅延回路９から１サンプリング前の平滑誤差共分散行列Ｐｋ−１（＋）を、予め設定された駆動雑音Ｑｋ−１をそれぞれ入力し、予測誤差共分散行列Ｐｋ（−）を式（１８）に従い算出する（ステップＳＴ１１）。予測器１１では、１サンプリング前の平滑ベクトルアンダバーｘｋ−１ハット（＋）を第１の遅延回路７を介して入力し、式（１７）に従い予測ベクトルアンダバーｘｋハット（−）を算出する（ステップＳＴ１２）。
【０１２０】
航跡の順序判定器１２では、平滑器６において１サンプリング前に算出された追尾目標の平滑ベクトルを第１の遅延回路７を介して入力し、その位置ベクトルアンダバーξｋ−１ｊと、予め設定された車両の進行方向の単位ベクトルアンダバーｅｄとの内積Ψｋ−１ｊを式（２２）に従い算出する。そして、内積Ψｋ−１ｊの大きい順番を航跡の走行順序として記憶する（ステップＳＴ１３）。グループ判定機１３では、航跡の順序判定機１２で判定された追尾車両の走行順序の順番に隣り合う平滑ベクトルの位置ベクトルの距離ｒが予め設定されたパラメータｒｍｉｎに対して式（２３）を満たすか否かを調べ、式（２３）を満たす航跡同士を同一グループと判定する（ステップＳＴ１４）。
【０１２１】
以上で明らかなように、この実施の形態５によれば、観測データの順序判定器３により、車両の進行方向に対する観測ベクトルの存在順序を、予め設定された車両の進行方向の単位ベクトルと観測ベクトルの内積の大きさから判定し、一方、航跡の順序判定器１２により、車両の走行順序を、上記単位ベクトルと追尾車両の平滑ベクトルの位置ベクトルの内積の大きさから判定し、相関処理切り換え器１６により、追尾車両と観測データ数が等しく、レーダ装置１の検出状態が良い場合には、順序による相関判定器４において、走行順序の順番に、追尾車両の相関ゲート内の観測ベクトルのうち存在順序が先頭側の観測ベクトルを当該追尾車両と相関があるものと判定し、追尾車両と観測データが等しくない場合には、順序と残差による相関判定器１４において、走行順序の順番に、追尾車両の相関ゲート内の観測ベクトルのうち残差が最も小さい観測ベクトルを当該追尾車両と相関があるものと判定し、それより前方に存在する観測ベクトルを以降の追尾目標の相関対象から除く構成としているので、レーダ装置１により誤検出がある場合や、失検出がある場合においても誤った相関結果となりにくい効果を奏する。
【０１２２】
【発明の効果】
以上のように、この発明によれば、例えば、空港面の誘導路や滑走路を走行する航空機や、車線幅の狭い一方通行路を走行する車両のように、追尾対象となる複数の目標の運動がレールの上を走る電車のようにあるコース上に限定され、追い越しや追い抜きができないまたは運行上ありえない状況において追尾した航跡が実際の目標の順序と入れ替わりにくい目標追尾装置及び方法を得ることができるという効果がある。
【図面の簡単な説明】
【図１】  この発明の実施の形態１による目標追尾装置を示す構成図である。
【図２】  この発明の実施の形態１による目標追尾装置の動作原理を説明する図である。
【図３】  この発明の実施の形態１による目標追尾装置における追尾目標と観測ベクトルの順序情報を利用した相関判定方法の説明図である。
【図４】  この発明の実施の形態２による目標追尾装置を示す構成図である。
【図５】  この発明の実施の形態２による目標追尾装置における追尾目標と観測ベクトルの順序情報を利用した相関判定方法の説明図である。
【図６】  この発明の実施の形態３による目標追尾装置を示す構成図である。
【図７】  この発明の実施の形態４による目標追尾装置を示す構成図である。
【図８】  この発明の実施の形態５による目標追尾装置を示す構成図である。
【図９】  Multiple-Target Tracking with Radar Application, ARTECK HOUSE, 1986, pp299-302に示された従来の目標追尾装置を示す構成図である。
【符号の説明】
１  レーダ装置、２  相関ゲート判定器、３  観測データ順序判定器、４  順序による相関器、５  ゲイン行列算出器、６  平滑器、７  第１の遅延器、８  平滑誤差評価器、９  第２の遅延器、１０  予測誤差評価器、１１  予測器、１２  航跡順序判定器、１３  グループ判定器、１４  順序と残差による相関器、１５  疑似データ生成器、１６  相関処理切り換え器。
【図１】
0004097126.tif 000014
【図２】
0004097126.tif 000015
【図３】
0004097126.tif 000016
【図４】
0004097126.tif 000017
【図５】
0004097126.tif 000018
【図６】
0004097126.tif 000019
【図７】
0004097126.tif 000020
【図８】
0004097126.tif 000021
【図９】
0004097126.tif 000022