追尾装置
【課題】センサと目標との距離が大きい場合には追尾維持性能を発揮し、センサと目標との距離が小さい場合には、各追尾目標に対する追尾精度を向上させる。
【解決手段】センサを介して2つの観測値を取得した場合に、センサ位置を基準点とした2つの観測値のなす角度を算出する角度算出部21と、角度算出部21により算出された2つの観測値のなす角度に基づいて2つの観測値に対する重み係数を算出し、重み係数に基づいて各観測値の各観測値間での重心をそれぞれ算出する角度基準重心算出部22と、角度基準重心算出部22により算出された各観測値の重心および各追尾目標に対する予測値に基づいて、各追尾目標に対する平滑値を算出する平滑処理部24とを備えた。
【解決手段】センサを介して2つの観測値を取得した場合に、センサ位置を基準点とした2つの観測値のなす角度を算出する角度算出部21と、角度算出部21により算出された2つの観測値のなす角度に基づいて2つの観測値に対する重み係数を算出し、重み係数に基づいて各観測値の各観測値間での重心をそれぞれ算出する角度基準重心算出部22と、角度基準重心算出部22により算出された各観測値の重心および各追尾目標に対する予測値に基づいて、各追尾目標に対する平滑値を算出する平滑処理部24とを備えた。
【発明の詳細な説明】
【技術分野】
【0001】
この発明は、近接した複数の追尾目標に対する運動諸元の観測値をセンサを介して取得し、当該追尾目標に対する平滑値および予測値を算出することで追尾を行う追尾装置に関するものである。
【背景技術】
【0002】
従来から、レーダセンサを用いて、近接した複数の追尾目標を追尾する追尾装置が知られている(例えば、非特許文献1参照)。この非特許文献1に開示された追尾装置では、まず、各追尾目標と、追尾ゲート内で複数得られた観測値との対応関係を示す各仮説の信頼度を算出する。この際、各仮説の信頼度として、各追尾目標の予測値(予測位置)と各観測値(観測位置)との距離に基づいて各観測値に対する重み係数を算出する。そして、各信頼度に基づいて各観測値の加重平均値を算出し、それぞれの追尾目標に対応付けることで複数の目標追尾を行っている。
ここで、複数の目標のうち、例えばi番目の目標を追尾する場合、追尾ゲート内での観測値数をNiとし、その観測値をzi,j(j=1,・・・,Ni)とし、これらの観測値がi番目の追尾目標である場合の信頼度をβi,j(j=1,・・・,Ni)とすると、i番目の追尾目標に対応付けられる各観測値の加重平均値は、式(1)で表される。
【先行技術文献】
【非特許文献】
【0003】
【非特許文献1】Fortmann,Bar−Shalom and Scheffe,“Multi−Target Tracking Using Joint Probabilistic Data Association,” Proc.of the IEEE Conf. on Decision and Control,2,pp.807−812,Feb.,1980.
【発明の概要】
【発明が解決しようとする課題】
【0004】
ここで、非特許文献1に開示された従来の追尾装置では、上式(1)のようにNi個の観測値の加重平均値をi番目の追尾目標に対応付けているが、この加重平均値は追尾ゲート内に存在する全観測値の中間寄りに常に位置する。このため、従来の追尾装置では、追尾維持は可能であるが、レーダセンサと目標との距離が小さくなった場合であっても、追尾目標ごとの追尾精度を向上させることができないという課題があった。
【0005】
この発明は、上記のような課題を解決するためになされたもので、近接した複数の追尾目標に対する観測値をセンサを介して取得することで追尾を行う追尾装置において、複数の観測値を検出するセンサと目標との距離が大きい場合には、追尾目標候補となる位置を安定化させることで追尾維持性能を発揮することができ、センサと目標との距離が小さい場合には、センサにより得られた各観測値をそれぞれの追尾目標に対応付けることで、各追尾目標に対する追尾精度を向上させることができる追尾装置を提供することを目的としている。
【課題を解決するための手段】
【0006】
この発明に係る追尾装置は、センサを介して2つの観測値を取得した場合に、当該センサ位置を基準点とした当該2つの観測値のなす角度を算出する角度算出部と、角度算出部により算出された2つの観測値のなす角度に基づいて当該2つの観測値に対する重み係数を算出し、当該重み係数に基づいて当該各観測値の当該各観測値間での重心をそれぞれ算出する角度基準重心算出部と、角度基準重心算出部により算出された各観測値の重心および各追尾目標に対する予測値に基づいて、各追尾目標に対する平滑値を算出する平滑処理部とを備えたものである。
【発明の効果】
【0007】
この発明によれば、上記のように構成したので、センサと目標との距離が大きく、センサにより得られた2つの観測値のなす角度が小さい場合には、追尾目標候補となる位置を安定化させることで各追尾目標に対する追尾維持が可能となり、センサと目標との距離が小さく、センサにより得られた観測値のなす角度が大きい場合には、各観測値をそれぞれの追尾目標に対応付けることで、各追尾目標に対する追尾精度を向上させることができる。
【図面の簡単な説明】
【0008】
【図1】この発明の実施の形態1に係る追尾装置の構成の一例を示すブロック図である。
【図2】この発明の実施の形態1に係る追尾装置による追尾処理の流れを示すフローチャートである。
【図3】この発明の実施の形態1に係る追尾装置が想定する観測状況を示す説明図である。
【図4】この発明の実施の形態1において使用する関数の一例を示した説明図である。
【図5】この発明の実施の形態1に係る追尾装置によって実現される効果を示した概念図である。
【図6】この発明の実施の形態2に係る追尾装置の構成の一例を示すブロック図である。
【図7】この発明の実施の形態2に係る追尾装置による追尾処理の流れを示すフローチャートである。
【図8】この本発明の実施の形態2に係る追尾装置によって実現される効果を示した概念図である。
【図9】この発明の実施の形態3に係る追尾装置の構成の一例を示すブロック図である。
【図10】この発明の実施の形態3に係る追尾装置による追尾処理の流れを示すフローチャートである。
【図11】この発明の実施の形態3に係る追尾装置が想定する観測状況を示す説明図である。
【発明を実施するための形態】
【0009】
以下、この発明の実施の形態について図面を参照しながら詳細に説明する。
実施の形態1.
図1はこの発明の実施の形態1に係る追尾装置の構成の一例を示すブロック図である。
追尾装置は、近接した複数の追尾目標に対する観測値をレーダセンサ(不図示)を介して取得し、この追尾目標に対する平滑値および予測値を算出することで追尾を行うものである。この追尾装置は、図1に示すように、目標観測手段1および追尾手段2から構成されている。なお以下で説明する実施の形態1に係る追尾装置では、図3に示すように、レーダセンサにより2つの追尾目標に対する観測値が検出された状況を想定して説明を行う。
また、レーダセンサの種類は、追尾目標の位置や速度などの観測値が検出可能なレーダセンサであれば何でもよく、例えば測距用のレーダや、パルスドップラ周波数を利用する測速度用のレーダを用いる。また、赤外センサなどの光学センサ、画像レーダなどを用いてもよい。
【0010】
目標観測手段1は、アンテナ・受信系(不図示)を介してレーダセンサから取得した受信信号に対して、公知の方法による信号処理を行い、2つの観測値を生成するものである。この目標観測手段1により生成された各観測値を示す情報は追尾手段2内の角度算出部21に出力される。
【0011】
追尾手段2は、目標観測手段1により生成された各観測値に基づいて、追尾目標に対する追尾処理を行うものである。この追尾手段2は、角度算出部21、角度基準重心算出部22、予測処理部23、平滑処理部24および平滑諸元ファイル25から構成されている。
【0012】
角度算出部21は、目標観測手段1により生成された2つの観測値に基づいて、レーダセンサ位置を基準点とした2つの観測値のなす角度を算出するものである。この角度算出部21により算出された2つの観測値のなす角度を示す情報は角度基準重心算出部22に出力される。
【0013】
角度基準重心算出部22は、角度算出部21により算出された2つの観測値のなす角度に基づいてこの2つの観測値に対する重み係数を算出し、この重み係数に基づいて、各観測値の各観測値間での重心をそれぞれ算出するものである。この角度基準重心算出部22により算出された各観測値の重心を示す情報は平滑処理部24に出力される。
【0014】
予測処理部23は、1観測周期前の観測時刻における各追尾目標の平滑値(平滑位置および平滑速度)を示す情報を平滑諸元ファイル25から取得し、この平滑値に基づいて、現観測時刻における各追尾目標の予測値(予測位置および予測速度)を算出するものである。また、予測処理部23は、1観測周期前の観測時刻における各追尾目標の平滑誤差共分散行列を平滑諸元ファイル25から取得し、この平滑誤差共分散行列に基づいて、現観測時刻における各追尾目標の予測誤差共分散行列を算出する。この予測処理部23により算出された現観測時刻における各追尾目標の予測値および予測誤差共分散行列を示す情報は平滑処理部24に出力される。
【0015】
平滑処理部24は、角度基準重心算出部22により算出された各観測値の重心、および、予測処理部23より算出された現観測時刻における各追尾目標の予測値に基づいて、現観測時刻における各追尾目標の平滑値を算出するものである。また、平滑処理部24は、予測処理部23より算出された現観測時刻における予測誤差共分散行列、および、各追尾目標に対するカルマンゲインに基づいて、現観測時刻における各追尾目標の平滑誤差共分散行列を算出する。この平滑処理部24により算出された現観測時刻における各追尾目標の平滑値および平滑誤差共分散行列を示す情報は平滑諸元ファイル25に出力される。
【0016】
平滑諸元ファイル25は、平滑処理部24により算出された現観測時刻における各追尾目標の平滑値および平滑誤差共分散行列を示す情報を記憶するものである。また、平滑諸元ファイル25は、各観測時刻において、記憶している1観測周期前の観測時刻における各追尾目標の平滑値および平滑誤差共分散行列を示す情報を予測処理部23に出力する。
【0017】
次に、上記のように構成された追尾装置による追尾処理の流れについて説明する。図2はこの発明の実施の形態1に係る追尾装置による追尾処理の流れを示すフローチャートである。なお以下では、図3に示すように、レーダセンサにより2つの追尾目標に対する観測値(観測位置)z1,z2が検出された状況を想定して説明を行う。
【0018】
追尾装置による追尾処理では、図2に示すように、まず、目標観測手段1は、アンテナ・受信系を介してレーダセンサから取得した受信信号に対して、公知の方法による信号処理を行い、2つの観測値z1,z2を生成する(ステップST21)。この目標観測手段1により生成された各観測値z1,z2を示す情報は角度算出部21に出力される。
【0019】
次いで、角度算出部21は、目標観測手段1により生成された各観測値z1,z2に基づいて、レーダセンサ位置を基準点とした2つの観測値z1,z2のなす角度θを算出する(ステップST22)。この角度算出部21により算出された2つの観測値z1,z2のなす角度θを示す情報は角度基準重心算出部22に出力される。
【0020】
次いで、角度基準重心算出部22は、角度算出部21により算出された2つの観測値z1,z2のなす角度θを変数とする関数f(θ)を用いて、この2つの観測値z1,z2に対する重み係数を算出する。そして、この重み係数に基づいて、式(2)から観測値z1,z2に対する各観測値z1,z2間での重心z1θ,z2θをそれぞれ算出する(ステップST23)。
この角度基準重心算出部22により算出された各観測値z1,z2の重心z1θ,z2θを示す情報は平滑処理部24に出力される。
【0021】
ここで、各観測値z1,z2に対する重み係数を算出する際に用いる関数f(θ)としては、例えば図4に示すように、2つの観測値z1,z2のなす角度θに応じて変化する関数を使用する。なお、関数f(θ)は、図4に示す関数に限定されるものではなく、その他の関数を用いてもよい。これは、以降の実施の形態においても同様である。
【0022】
このステップST23の角度基準重心算出処理によって、図5(a)に示すように、レーダセンサと各追尾目標との距離が大きく、2つの観測値z1,z2(図5の×印位置)のなす角度θが小さい場合には、各追尾目標に対応付けられる各観測値z1,z2の重心z1θ,z2θ(図5の□,■印位置)が各観測値z1,z2に対して中間寄りの位置となる。そのため、追尾目標候補となる位置を安定化させることができ、各追尾目標に対する追尾維持が可能となる。
また、図5(b)に示すように、レーダセンサと各追尾目標との距離が小さくなり、2つの観測値z1,z2のなす角度θが大きくなるに連れて、各追尾目標に対応付けられる各観測値z1,z2の重心z1θ,z2θが各観測値z1,z2側に近づく。そのため、各追尾目標に対する追尾精度を向上させることができる。
なお図5の○,●印位置は、現観測時刻tkにおける各追尾目標の予測位置x(ハット)i,k|k−1(i=1,2)である。
【0023】
一方、予測処理部23は予測処理を行う(ステップST24)。具体的には、予測処理部23は、まず、1観測周期前の観測時刻tk−1における各追尾目標の平滑値(平滑位置および平滑速度)x(ハット)i,k−1|k−1(i=1,2)を示す情報を平滑諸元ファイル25から取得する。そして、この平滑値x(ハット)i,k−1|k−1に基づいて、式(3)から現観測時刻tkにおける各追尾目標の予測値(予測位置および予測速度)x(ハット)i,k|k−1を算出する。
【0024】
次に、予測処理部23は、1観測周期前の時刻tk−1における各追尾目標の平滑誤差共分散行列を平滑諸元ファイル25から取得する。そして、この平滑誤差共分散行列に基づいて、現観測時刻tkにおける各追尾目標の予測誤差共分散行列を算出する。
この予測処理部23により算出された現観測時刻tkにおける各追尾目標の予測値x(ハット)i,k|k−1および予測誤差共分散行列を示す情報は平滑処理部24に出力される。
【0025】
次いで、平滑処理部24は平滑処理を行う(ステップST25)。具体的には、平滑処理部24は、まず、角度基準重心算出部22により算出された各観測値z1,z2の重心z1θ,z2θを、予測処理部23より算出された現観測時刻tkにおける各追尾目標の予測値x(ハット)i,k|k−1に対応付け、各追尾目標に対するカルマンゲインKi,k(i=1,2)を算出する。そして、式(4)に示すカルマンフィルタアルゴリズムに従って現観測時刻tkにおける各追尾目標の平滑値x(ハット)i,k|k−1を算出する。
【0026】
次に、平滑処理部24は、予測処理部23により算出された予測誤差共分散行列、および、各追尾目標に対するカルマンゲインKi,kに基づいて、現観測時刻tkにおける各追尾目標の平滑誤差共分散行列を算出する。
この平滑処理部24により算出された現観測時刻tkにおける各追尾目標の平滑値x(ハット)i,k|k−1および平滑誤差共分散行列を示す情報は平滑諸元ファイル25に出力されて記憶される。
【0027】
次いで、平滑諸元ファイル25は、1観測周期後の観測時刻tk+1において、記憶している観測時刻tkにおける各追尾目標の平滑値x(ハット)i,k|kおよび平滑誤差共分散行列を示す情報を予測処理部23に出力する(ステップST26)。
【0028】
以上のように、この実施の形態1によれば、レーザセンサ位置を基準点とした2つの観測値のなす角度に基づいてこの2つの観測値に対する重み係数を算出し、この重み係数に基づいて算出した各観測値の重心を各追尾目標に対応付けるように構成したので、レーダセンサと各追尾目標との距離が大きく、2つの観測値のなす角度が小さい場合には、追尾目標候補となる位置を安定化させることで各追尾目標に対する追尾維持が可能となり、レーダセンサと各追尾目標との距離が小さく、2つの観測値のなす角度が大きい場合には、複数得られている各観測値のそれぞれを各追尾目標に対応付けることで、各追尾目標に対する追尾精度を向上させることができる。
【0029】
実施の形態2.
図6はこの発明の実施の形態2に係る追尾装置の構成の一例を示すブロック図である。図6に示す実施の形態2に係る追尾装置の構成は、図1に示す実施の形態1に係る追尾装置に加重平均部26bを追加したものであり、同様の構成についてはその説明を省略する。なお以下で説明する実施の形態2に係る追尾装置では、図3に示すように、レーダセンサにより2つの追尾目標に対する観測値が検出された状況を想定して説明を行う。
【0030】
加重平均部26bは、目標観測手段1bにより生成された各観測値、および、予測処理部23bにより算出された現観測時刻における各追尾目標の予測値に基づいて、追尾目標ごとに、各観測値に対する信頼度を算出するものである。また、加重平均部26bは、目標観測手段1により生成された各観測値、および、算出した各観測値に対する信頼度に基づいて、追尾目標ごとに、各観測値の加重平均値を算出する。この加重平均部26bにより算出された追尾目標ごとの加重平均値を示す情報は角度基準重心算出部22bに出力される。
【0031】
なお、角度基準重心算出部22bは、実施の形態1における角度基準重心算出部22と同様の処理によって、角度算出部21bにより算出された2つの観測値のなす角度を変数とする関数に基づいて、この2つの観測値に対する重み係数を算出する。さらに、角度基準重心算出部22bは、加重平均部26bにより算出された追尾目標ごとの加重平均値、および、算出した各観測値に対する重み係数に基づいて、各観測値の信頼度付き重心を算出する。この角度基準重心算出部22bにより算出された各観測値の信頼度付き重心を示す情報は平滑処理部24bに出力される。
【0032】
次に、上記のように構成された追尾装置による追尾処理の流れについて説明する。図7はこの発明の実施の形態2に係る追尾装置による追尾処理の流れを示すフローチャートである。なお以下では、図3に示すように、レーダセンサにより2つの追尾目標に対する観測値(観測位置)z1,z2が検出された状況を想定して説明を行う。
【0033】
追尾装置による追尾処理では、図7に示すように、まず、目標観測手段1bは、実施の形態1における目標観測手段1と同様の処理によって、アンテナ・受信系を介してレーダセンサから取得した受信信号から2つの観測値z1,z2を生成する(ステップST71)。この目標観測手段1bにより生成された各観測値z1,z2を示す情報は角度算出部21bおよび加重平均部26bに出力される。
【0034】
次いで、角度算出部21bは、実施の形態1における角度算出部21と同様の処理によって、目標観測手段1bにより生成された各観測値z1,z2に基づいて、レーダセンサ位置を基準点とした2つの観測値z1,z2のなす角度θを算出する(ステップST72)。この角度算出部21bにより算出された2つの観測値z1,z2のなす角度θを示す情報は角度基準重心算出部22bに出力される。
【0035】
一方、加重平均部26bは観測値加重平均算出処理を行う(ステップST73)。具体的には、加重平均部26bは、まず、目標観測手段1bにより生成された各観測値z1,z2、および、予測処理部23bにより算出された現観測時刻tkにおける各追尾目標の予測値(予測位置および予測速度)x(ハット)i,k|k−1(i=1,2)に基づいて、追尾目標ごとに、各観測値z1,z2に対する信頼度βi,j(i=1,2, j=1,2)を算出する。ここで例えば、現観測時刻tkにおける各追尾目標の予測位置x(ハット)i,k|k−1と各観測値z1,z2との距離を変数とする関数を用い、式(5)から、追尾目標ごとに各観測値z1,z2に対する信頼度βi,jを算出する。
【0036】
ここで、上式(5)では、追尾目標ごとの各観測値z1,z2に対する信頼度βi,jを算出する際に用いる関数として、各予測位置x(ハット)i,k|k−1と各観測値z1,z2との距離を変数とする関数を用いているが、これに限定されるものではなく、その他の関数を用いてもよい。
【0037】
次に、加重平均部26bは、目標観測手段1bにより生成された各観測値z1,z2、および、算出した追尾目標ごとの各観測値z1,z2に対する信頼度βi,jに基づいて、式(6)から、追尾目標ごとに、各観測値z1,z2の加重平均値z(バー)1,z(バー)2を算出する。
この加重平均部26bにより算出された追尾目標ごとの加重平均値z(バー)1,z(バー)2を示す情報は角度基準重心算出部22bに出力される。
【0038】
次いで、角度基準重心算出部22bは、実施の形態1における角度基準重心算出部22と同様の処理によって、角度算出部21bにより算出された2つの観測値z1,z2のなす角度θを変数とする関数f(θ)を用いて、各観測値z1,z2に対する重み係数を算出する。そして、この重み係数および加重平均部26bにより算出された追尾目標ごとの加重平均値z(バー)1,z(バー)2に基づいて、式(7)から各観測値z1,z2の信頼度付き重心z(バー)1θ,z(バー)2θを算出する(ステップST74)。
この角度基準重心算出部22bにより算出された各観測値z1,z2の信頼度付き重心z(バー)1θ,z(バー)2θを示す情報は平滑処理部24bに出力される。
【0039】
このステップST74の角度基準重心算出処理によって、図8(a)に示すように、レーダセンサと各追尾目標との距離が大きく、2つの観測値z1,z2(図8の×印位置)のなす角度θが小さい場合には、各追尾目標に対応付けられる各観測値z1,z2の信頼度付き重心z(バー)1θ,z(バー)2θ(図8の□,■印位置)が2つの加重平均値z(バー)1,z(バー)2(図8の△,▲印位置)の中間寄りの位置となる。そのため、追尾目標候補となる位置を安定化させることができ、各追尾目標に対する追尾維持が可能となる。
また、図8(b)に示すように、レーダセンサと各追尾目標との距離が小さくなり、2つの観測値z1,z2のなす角度θが大きくなるに連れて、各追尾目標に対応付けられる各観測値z1,z2の信頼度付き重心z(バー)1θ,z(バー)2θが各加重平均値z(バー)1,z(バー)2側に近づく。そのため、各追尾目標に対する追尾精度を向上させることができる。
なお図8の○,●印位置は、現観測時刻tkにおける各追尾目標の予測位置x(ハット)i,k|k−1(i=1,2)である。
【0040】
一方、予測処理部23bは、実施の形態1における予測処理部23と同様の処理によって予測処理を行い、現観測時刻tkにおける各追尾目標の予測値x(ハット)i,k|k−1および予測誤差共分散行列を算出する(ステップST75)。この予測処理部23bにより算出された現観測時刻tkにおける各追尾目標の予測値x(ハット)i,k|k−1および予測誤差共分散行列を示す情報は平滑処理部24bおよび加重平均部26bに出力される。
【0041】
次いで、平滑処理部24bは、実施の形態1における平滑処理部24と同様の処理によって平滑処理を行い、現観測時刻tkにおける各追尾目標の平滑値x(ハット)i,k|kおよび平滑誤差共分散行列を算出する(ステップST76)。この平滑処理部24bにより算出された現観測時刻tkにおける各追尾目標の平滑値x(ハット)i,k|kおよび平滑誤差共分散行列を示す情報は平滑諸元ファイル25bに出力されて記憶される。
【0042】
次いで、平滑諸元ファイル25bは、実施の形態1における平滑諸元ファイル25と同様の処理によって、1観測周期後の時刻tk+1において、記憶している時刻tkにおける各追尾目標の平滑値x(ハット)i,k|kおよび平滑誤差共分散行列を示す情報を予測処理部23bに出力する(ステップST77)。
【0043】
以上のように、この実施の形態2によれば、追尾目標ごとに各観測値に対する信頼度を算出し、この信頼度に基づいて各観測値の信頼度付き重心を算出して各追尾目標に対応付けるように構成したので、実施の形態1における効果に対して、レーダセンサと各追尾目標との距離が大きく、2つの観測値のなす角度が小さい場合には、追尾目標候補となる位置をより安定化させることができ、レーダセンサと各追尾目標との距離が小さく、2つの観測値のなす角度が大きい場合には、各追尾目標に対する追尾精度をより向上させることができる。
【0044】
実施の形態3.
図9はこの発明の実施の形態3に係る追尾装置の構成の一例を示すブロック図である。図9に示す実施の形態3に係る追尾装置の構成は、図6に示す実施の形態2に係る追尾装置の角度算出部21bを多目標角度算出部21cに変更したものであり、同様の構成についてはその説明を省略する。なお以下で説明する実施の形態3に係る追尾装置では、図11に示すように、レーダセンサによりN個の追尾目標に対する観測値が検出された状況を想定して説明を行う。
【0045】
なお、目標観測手段1cは、アンテナ・受信系(不図示)を介してレーダセンサから取得した受信信号に対して、公知の方法による信号処理を行い、N個の観測値を生成する。この目標観測手段1cにより生成された各観測値を示す情報は多目標角度算出部21cに出力される。
【0046】
多目標角度算出部21cは、目標観測手段1cにより生成されたN個の観測値に基づいて、レーザセンサ位置を基準点とした2つの観測値のなす角度を全ての組み合わせについて算出するものである。この多目標角度算出部21cにより算出された各組み合わせでの2つの観測値のなす角度を示す情報は角度基準重心算出部22cに出力される。
【0047】
次に、上記のように構成された追尾装置による追尾処理の流れについて説明する。図10はこの発明の実施の形態3に係る追尾装置による追尾処理の流れを示すフローチャートである。なお以下では、図11に示すように、レーダセンサによりN個の追尾目標に対する観測値zj(j=1,2,・・・,N)が検出された状況を想定して説明を行う。
【0048】
追尾装置による追尾処理では、図10に示すように、まず、目標観測手段1cは、アンテナ・受信系を介してレーダセンサから取得した受信信号に対して、公知の方法による信号処理を行い、N個の観測値zjを生成する(ステップST101)。この目標観測手段1cにより生成された各観測値zjを示す情報は多目標角度算出部21cおよび加重平均部26cに出力される。
【0049】
次いで、多目標角度算出部21cは、目標観測手段1cにより生成されたN個の観測値zjに基づいて、レーザセンサ位置を基準点とした2つの観測値のなす角度θj,m(j=1,2,・・・,N、m=1,2,・・・,N)を全ての組み合わせについて算出する(ステップST102)。この多目標角度算出部21cにより算出された各組み合わせでの2つの観測値のなす角度θj,mを示す情報は角度基準重心算出部22cに出力される。
【0050】
一方、加重平均部26cは観測値加重平均算出処理を行う(ステップST103)。具体的には、加重平均部26cは、まず、目標観測手段1cにより生成された各観測値zj、および、予測処理部23cにより算出された現観測時刻tkにおける各追尾目標の予測値(予測位置および予測速度)x(ハット)i,k|k−1(i=1,2,・・・,N)に基づいて、追尾目標ごとに、各観測値zjに対する信頼度βi,j(i=1,2,・・・,N j=1,2,・・・,N)を算出する。ここで例えば、現観測時刻tkにおける各追尾目標の予測位置x(ハット)i,k|k−1と各観測値zjとの距離を変数とする関数を用い、式(8)から、追尾目標ごとに各観測値zjに対する信頼度βi,jを算出する。
【0051】
ここで、上式(8)では、追尾目標ごとの各観測値zjに対する信頼度βi,jを算出する際に用いる関数として、各予測位置x(ハット)i,k|k−1と各観測値zjとの距離を変数とする関数を用いているが、これに限定されるものではなく、その他の関数を用いてもよい。
【0052】
次に、加重平均部26cは、目標観測手段1cにより生成された各観測値zj、および、算出した追尾目標ごとの各観測値zjに対する信頼度βi,jに基づいて、式(9)から、追尾目標ごとに、各観測値zjの加重平均値z(バー)i(i=1,2,・・・,N)を算出する。
この加重平均部26cにより算出された追尾目標ごとの加重平均値z(バー)iを示す情報は角度基準重心算出部22cに出力される。
【0053】
次いで、角度基準重心算出部22cは、実施の形態1における角度基準重心算出部22と同様の処理によって、多目標角度算出部21cにより算出された2つの観測値のなす角度θを変数とする関数f(θ)を用いて、この2つの観測値に対する重み係数を全ての組み合わせについて算出する。そして、これらの重み係数および加重平均部26cにより算出された追尾目標ごとの加重平均値z(バー)iに基づいて、式(10)から各観測値zjの信頼度付き重心z(バー)i,θを算出する(ステップST104)。
この角度基準重心算出部22cにより算出された各観測値zjの信頼度付き重心z(バー)i,θを示す情報は平滑処理部24cに出力される。
【0054】
このステップST104の角度基準重心算出処理によって、レーダセンサと各追尾目標との距離が大きく、2つの観測値のなす角度θが小さい場合には、各追尾目標に対応付けられる各観測値zjの信頼度付き重心z(バー)i,θが各加重平均値z(バー)iの中間寄りの位置となる。そのため、追尾目標候補となる位置を安定化させることができ、各追尾目標に対する追尾維持が可能となる。
また、レーダセンサと各追尾目標との距離が小さくなり、2つの観測値のなす角度θが大きくなるに連れて、各追尾目標に対応付けられる各観測値ziの信頼度付き重心z(バー)i,θが各加重平均値z(バー)i側に近づく。そのため、各追尾目標に対する追尾精度を向上させることができる。
【0055】
一方、予測処理部23cは予測処理を行う(ステップST105)。具体的には、予測処理部23cは、まず、1観測周期前の時刻tk−1における各追尾目標の平滑値(平滑位置および平滑速度)x(ハット)i,k−1|k−1(i=1,2,・・・,N)を示す情報を平滑諸元ファイル25cから取得する。そして、この平滑値x(ハット)i,k−1|k−1に基づいて、式(11)から現観測時刻tkにおける各追尾目標の予測値(予測位置および予測速度)x(ハット)i,k|k−1(i=1,2,・・・,N)を算出する。
【0056】
次に、予測処理部23cは、実施の形態1における予測処理部23と同様の処理によって、各追尾目標の予測誤差共分散行列を算出する。
この予測処理部23cにより算出された現観測時刻tkにおける各追尾目標の予測値x(ハット)i,k|k−1および予測誤差共分散行列を示す情報は平滑処理部24cおよび加重平均部26cに出力される。
【0057】
次いで、平滑処理部24cは平滑処理を行う(ステップST106)。具体的には、平滑処理部24cは、まず、角度基準重心算出部22cにより算出された各観測値zjの信頼度付き重心z(バー)i,θを、予測処理部23cより算出された現観測時刻tkにおける各追尾目標の予測値x(ハット)i,k|k−1に対応付け、各追尾目標に対するカルマンゲインKi,k(i=1,2,・・・,N)を算出する。そして、式(12)に示すカルマンフィルタアルゴリズムに従って現観測時刻tkにおける各追尾目標の平滑値x(ハット)i,k|k−1を算出する。
【0058】
次に、平滑処理部24cは、予測処理部23cにより算出された予測誤差共分散行列、および、各追尾目標に対するカルマンゲインKi,kに基づいて、現観測時刻tkにおける各追尾目標の平滑誤差共分散行列を算出する。
この平滑処理部24cにより算出された現観測時刻tkにおける各追尾目標の平滑値x(ハット)i,k|k−1および平滑誤差共分散行列を示す情報は平滑諸元ファイル25cに出力されて記憶される。
【0059】
次いで、平滑諸元ファイル25cは、実施の形態1における平滑諸元ファイル25と同様の処理によって、1観測周期後の時刻tk+1において、記憶している時刻tkにおける各追尾目標の平滑値x(ハット)i,k|k−1および平滑誤差共分散行列を示す情報を予測処理部23cに出力する(ステップST107)。
【0060】
以上のように、この実施の形態3によれば、N個の観測値に基づいて、レーザセンサ位置を基準とした2つの観測値のなす角度を全ての組み合わせに対して算出し、これらの角度から重み係数を算出し、この重み係数に基づいて算出した各観測値の信頼度付き重心を追尾目標に対応付けるように構成したので、レーダセンサと各追尾目標との距離が大きく、2つの観測値のなす角度が小さい場合には、追尾目標候補となる位置を安定化させることで各追尾目標に対する追尾維持が可能となり、レーダセンサと各追尾目標との距離が小さく、2つの観測値のなす角度が大きい場合には、複数得られている各観測値のそれぞれを各追尾目標に対応付けることで、各追尾目標に対する追尾精度を向上させることができる。
【0061】
なお、図9に示す実施の形態3に係る追尾装置は、実施の形態2に係る追尾装置の角度算出部21bを多目標角度算出部21cに変更したものであるが、実施の形態1に係る追尾装置の角度算出部21を多目標角度算出部21cに変更した場合についても同様に適用可能である。
なお、本願発明はその発明の範囲内において、各実施の形態の自由な組み合わせ、あるいは各実施の形態の任意の構成要素の変形、もしくは各実施の形態において任意の構成要素の省略が可能である。
【符号の説明】
【0062】
1,1b,1c 目標観測手段、2,2b,2c 追尾手段、21,21b 角度算出部、21c 多目標角度算出部、22,22b,22c 角度基準重心算出部、23,23b,23c 予測処理部、24,24b,24c 平滑処理部、25,25b,25c 平滑諸元ファイル、26b,26c 加重平均部。
【技術分野】
【0001】
この発明は、近接した複数の追尾目標に対する運動諸元の観測値をセンサを介して取得し、当該追尾目標に対する平滑値および予測値を算出することで追尾を行う追尾装置に関するものである。
【背景技術】
【0002】
従来から、レーダセンサを用いて、近接した複数の追尾目標を追尾する追尾装置が知られている(例えば、非特許文献1参照)。この非特許文献1に開示された追尾装置では、まず、各追尾目標と、追尾ゲート内で複数得られた観測値との対応関係を示す各仮説の信頼度を算出する。この際、各仮説の信頼度として、各追尾目標の予測値(予測位置)と各観測値(観測位置)との距離に基づいて各観測値に対する重み係数を算出する。そして、各信頼度に基づいて各観測値の加重平均値を算出し、それぞれの追尾目標に対応付けることで複数の目標追尾を行っている。
ここで、複数の目標のうち、例えばi番目の目標を追尾する場合、追尾ゲート内での観測値数をNiとし、その観測値をzi,j(j=1,・・・,Ni)とし、これらの観測値がi番目の追尾目標である場合の信頼度をβi,j(j=1,・・・,Ni)とすると、i番目の追尾目標に対応付けられる各観測値の加重平均値は、式(1)で表される。
【先行技術文献】
【非特許文献】
【0003】
【非特許文献1】Fortmann,Bar−Shalom and Scheffe,“Multi−Target Tracking Using Joint Probabilistic Data Association,” Proc.of the IEEE Conf. on Decision and Control,2,pp.807−812,Feb.,1980.
【発明の概要】
【発明が解決しようとする課題】
【0004】
ここで、非特許文献1に開示された従来の追尾装置では、上式(1)のようにNi個の観測値の加重平均値をi番目の追尾目標に対応付けているが、この加重平均値は追尾ゲート内に存在する全観測値の中間寄りに常に位置する。このため、従来の追尾装置では、追尾維持は可能であるが、レーダセンサと目標との距離が小さくなった場合であっても、追尾目標ごとの追尾精度を向上させることができないという課題があった。
【0005】
この発明は、上記のような課題を解決するためになされたもので、近接した複数の追尾目標に対する観測値をセンサを介して取得することで追尾を行う追尾装置において、複数の観測値を検出するセンサと目標との距離が大きい場合には、追尾目標候補となる位置を安定化させることで追尾維持性能を発揮することができ、センサと目標との距離が小さい場合には、センサにより得られた各観測値をそれぞれの追尾目標に対応付けることで、各追尾目標に対する追尾精度を向上させることができる追尾装置を提供することを目的としている。
【課題を解決するための手段】
【0006】
この発明に係る追尾装置は、センサを介して2つの観測値を取得した場合に、当該センサ位置を基準点とした当該2つの観測値のなす角度を算出する角度算出部と、角度算出部により算出された2つの観測値のなす角度に基づいて当該2つの観測値に対する重み係数を算出し、当該重み係数に基づいて当該各観測値の当該各観測値間での重心をそれぞれ算出する角度基準重心算出部と、角度基準重心算出部により算出された各観測値の重心および各追尾目標に対する予測値に基づいて、各追尾目標に対する平滑値を算出する平滑処理部とを備えたものである。
【発明の効果】
【0007】
この発明によれば、上記のように構成したので、センサと目標との距離が大きく、センサにより得られた2つの観測値のなす角度が小さい場合には、追尾目標候補となる位置を安定化させることで各追尾目標に対する追尾維持が可能となり、センサと目標との距離が小さく、センサにより得られた観測値のなす角度が大きい場合には、各観測値をそれぞれの追尾目標に対応付けることで、各追尾目標に対する追尾精度を向上させることができる。
【図面の簡単な説明】
【0008】
【図1】この発明の実施の形態1に係る追尾装置の構成の一例を示すブロック図である。
【図2】この発明の実施の形態1に係る追尾装置による追尾処理の流れを示すフローチャートである。
【図3】この発明の実施の形態1に係る追尾装置が想定する観測状況を示す説明図である。
【図4】この発明の実施の形態1において使用する関数の一例を示した説明図である。
【図5】この発明の実施の形態1に係る追尾装置によって実現される効果を示した概念図である。
【図6】この発明の実施の形態2に係る追尾装置の構成の一例を示すブロック図である。
【図7】この発明の実施の形態2に係る追尾装置による追尾処理の流れを示すフローチャートである。
【図8】この本発明の実施の形態2に係る追尾装置によって実現される効果を示した概念図である。
【図9】この発明の実施の形態3に係る追尾装置の構成の一例を示すブロック図である。
【図10】この発明の実施の形態3に係る追尾装置による追尾処理の流れを示すフローチャートである。
【図11】この発明の実施の形態3に係る追尾装置が想定する観測状況を示す説明図である。
【発明を実施するための形態】
【0009】
以下、この発明の実施の形態について図面を参照しながら詳細に説明する。
実施の形態1.
図1はこの発明の実施の形態1に係る追尾装置の構成の一例を示すブロック図である。
追尾装置は、近接した複数の追尾目標に対する観測値をレーダセンサ(不図示)を介して取得し、この追尾目標に対する平滑値および予測値を算出することで追尾を行うものである。この追尾装置は、図1に示すように、目標観測手段1および追尾手段2から構成されている。なお以下で説明する実施の形態1に係る追尾装置では、図3に示すように、レーダセンサにより2つの追尾目標に対する観測値が検出された状況を想定して説明を行う。
また、レーダセンサの種類は、追尾目標の位置や速度などの観測値が検出可能なレーダセンサであれば何でもよく、例えば測距用のレーダや、パルスドップラ周波数を利用する測速度用のレーダを用いる。また、赤外センサなどの光学センサ、画像レーダなどを用いてもよい。
【0010】
目標観測手段1は、アンテナ・受信系(不図示)を介してレーダセンサから取得した受信信号に対して、公知の方法による信号処理を行い、2つの観測値を生成するものである。この目標観測手段1により生成された各観測値を示す情報は追尾手段2内の角度算出部21に出力される。
【0011】
追尾手段2は、目標観測手段1により生成された各観測値に基づいて、追尾目標に対する追尾処理を行うものである。この追尾手段2は、角度算出部21、角度基準重心算出部22、予測処理部23、平滑処理部24および平滑諸元ファイル25から構成されている。
【0012】
角度算出部21は、目標観測手段1により生成された2つの観測値に基づいて、レーダセンサ位置を基準点とした2つの観測値のなす角度を算出するものである。この角度算出部21により算出された2つの観測値のなす角度を示す情報は角度基準重心算出部22に出力される。
【0013】
角度基準重心算出部22は、角度算出部21により算出された2つの観測値のなす角度に基づいてこの2つの観測値に対する重み係数を算出し、この重み係数に基づいて、各観測値の各観測値間での重心をそれぞれ算出するものである。この角度基準重心算出部22により算出された各観測値の重心を示す情報は平滑処理部24に出力される。
【0014】
予測処理部23は、1観測周期前の観測時刻における各追尾目標の平滑値(平滑位置および平滑速度)を示す情報を平滑諸元ファイル25から取得し、この平滑値に基づいて、現観測時刻における各追尾目標の予測値(予測位置および予測速度)を算出するものである。また、予測処理部23は、1観測周期前の観測時刻における各追尾目標の平滑誤差共分散行列を平滑諸元ファイル25から取得し、この平滑誤差共分散行列に基づいて、現観測時刻における各追尾目標の予測誤差共分散行列を算出する。この予測処理部23により算出された現観測時刻における各追尾目標の予測値および予測誤差共分散行列を示す情報は平滑処理部24に出力される。
【0015】
平滑処理部24は、角度基準重心算出部22により算出された各観測値の重心、および、予測処理部23より算出された現観測時刻における各追尾目標の予測値に基づいて、現観測時刻における各追尾目標の平滑値を算出するものである。また、平滑処理部24は、予測処理部23より算出された現観測時刻における予測誤差共分散行列、および、各追尾目標に対するカルマンゲインに基づいて、現観測時刻における各追尾目標の平滑誤差共分散行列を算出する。この平滑処理部24により算出された現観測時刻における各追尾目標の平滑値および平滑誤差共分散行列を示す情報は平滑諸元ファイル25に出力される。
【0016】
平滑諸元ファイル25は、平滑処理部24により算出された現観測時刻における各追尾目標の平滑値および平滑誤差共分散行列を示す情報を記憶するものである。また、平滑諸元ファイル25は、各観測時刻において、記憶している1観測周期前の観測時刻における各追尾目標の平滑値および平滑誤差共分散行列を示す情報を予測処理部23に出力する。
【0017】
次に、上記のように構成された追尾装置による追尾処理の流れについて説明する。図2はこの発明の実施の形態1に係る追尾装置による追尾処理の流れを示すフローチャートである。なお以下では、図3に示すように、レーダセンサにより2つの追尾目標に対する観測値(観測位置)z1,z2が検出された状況を想定して説明を行う。
【0018】
追尾装置による追尾処理では、図2に示すように、まず、目標観測手段1は、アンテナ・受信系を介してレーダセンサから取得した受信信号に対して、公知の方法による信号処理を行い、2つの観測値z1,z2を生成する(ステップST21)。この目標観測手段1により生成された各観測値z1,z2を示す情報は角度算出部21に出力される。
【0019】
次いで、角度算出部21は、目標観測手段1により生成された各観測値z1,z2に基づいて、レーダセンサ位置を基準点とした2つの観測値z1,z2のなす角度θを算出する(ステップST22)。この角度算出部21により算出された2つの観測値z1,z2のなす角度θを示す情報は角度基準重心算出部22に出力される。
【0020】
次いで、角度基準重心算出部22は、角度算出部21により算出された2つの観測値z1,z2のなす角度θを変数とする関数f(θ)を用いて、この2つの観測値z1,z2に対する重み係数を算出する。そして、この重み係数に基づいて、式(2)から観測値z1,z2に対する各観測値z1,z2間での重心z1θ,z2θをそれぞれ算出する(ステップST23)。
この角度基準重心算出部22により算出された各観測値z1,z2の重心z1θ,z2θを示す情報は平滑処理部24に出力される。
【0021】
ここで、各観測値z1,z2に対する重み係数を算出する際に用いる関数f(θ)としては、例えば図4に示すように、2つの観測値z1,z2のなす角度θに応じて変化する関数を使用する。なお、関数f(θ)は、図4に示す関数に限定されるものではなく、その他の関数を用いてもよい。これは、以降の実施の形態においても同様である。
【0022】
このステップST23の角度基準重心算出処理によって、図5(a)に示すように、レーダセンサと各追尾目標との距離が大きく、2つの観測値z1,z2(図5の×印位置)のなす角度θが小さい場合には、各追尾目標に対応付けられる各観測値z1,z2の重心z1θ,z2θ(図5の□,■印位置)が各観測値z1,z2に対して中間寄りの位置となる。そのため、追尾目標候補となる位置を安定化させることができ、各追尾目標に対する追尾維持が可能となる。
また、図5(b)に示すように、レーダセンサと各追尾目標との距離が小さくなり、2つの観測値z1,z2のなす角度θが大きくなるに連れて、各追尾目標に対応付けられる各観測値z1,z2の重心z1θ,z2θが各観測値z1,z2側に近づく。そのため、各追尾目標に対する追尾精度を向上させることができる。
なお図5の○,●印位置は、現観測時刻tkにおける各追尾目標の予測位置x(ハット)i,k|k−1(i=1,2)である。
【0023】
一方、予測処理部23は予測処理を行う(ステップST24)。具体的には、予測処理部23は、まず、1観測周期前の観測時刻tk−1における各追尾目標の平滑値(平滑位置および平滑速度)x(ハット)i,k−1|k−1(i=1,2)を示す情報を平滑諸元ファイル25から取得する。そして、この平滑値x(ハット)i,k−1|k−1に基づいて、式(3)から現観測時刻tkにおける各追尾目標の予測値(予測位置および予測速度)x(ハット)i,k|k−1を算出する。
【0024】
次に、予測処理部23は、1観測周期前の時刻tk−1における各追尾目標の平滑誤差共分散行列を平滑諸元ファイル25から取得する。そして、この平滑誤差共分散行列に基づいて、現観測時刻tkにおける各追尾目標の予測誤差共分散行列を算出する。
この予測処理部23により算出された現観測時刻tkにおける各追尾目標の予測値x(ハット)i,k|k−1および予測誤差共分散行列を示す情報は平滑処理部24に出力される。
【0025】
次いで、平滑処理部24は平滑処理を行う(ステップST25)。具体的には、平滑処理部24は、まず、角度基準重心算出部22により算出された各観測値z1,z2の重心z1θ,z2θを、予測処理部23より算出された現観測時刻tkにおける各追尾目標の予測値x(ハット)i,k|k−1に対応付け、各追尾目標に対するカルマンゲインKi,k(i=1,2)を算出する。そして、式(4)に示すカルマンフィルタアルゴリズムに従って現観測時刻tkにおける各追尾目標の平滑値x(ハット)i,k|k−1を算出する。
【0026】
次に、平滑処理部24は、予測処理部23により算出された予測誤差共分散行列、および、各追尾目標に対するカルマンゲインKi,kに基づいて、現観測時刻tkにおける各追尾目標の平滑誤差共分散行列を算出する。
この平滑処理部24により算出された現観測時刻tkにおける各追尾目標の平滑値x(ハット)i,k|k−1および平滑誤差共分散行列を示す情報は平滑諸元ファイル25に出力されて記憶される。
【0027】
次いで、平滑諸元ファイル25は、1観測周期後の観測時刻tk+1において、記憶している観測時刻tkにおける各追尾目標の平滑値x(ハット)i,k|kおよび平滑誤差共分散行列を示す情報を予測処理部23に出力する(ステップST26)。
【0028】
以上のように、この実施の形態1によれば、レーザセンサ位置を基準点とした2つの観測値のなす角度に基づいてこの2つの観測値に対する重み係数を算出し、この重み係数に基づいて算出した各観測値の重心を各追尾目標に対応付けるように構成したので、レーダセンサと各追尾目標との距離が大きく、2つの観測値のなす角度が小さい場合には、追尾目標候補となる位置を安定化させることで各追尾目標に対する追尾維持が可能となり、レーダセンサと各追尾目標との距離が小さく、2つの観測値のなす角度が大きい場合には、複数得られている各観測値のそれぞれを各追尾目標に対応付けることで、各追尾目標に対する追尾精度を向上させることができる。
【0029】
実施の形態2.
図6はこの発明の実施の形態2に係る追尾装置の構成の一例を示すブロック図である。図6に示す実施の形態2に係る追尾装置の構成は、図1に示す実施の形態1に係る追尾装置に加重平均部26bを追加したものであり、同様の構成についてはその説明を省略する。なお以下で説明する実施の形態2に係る追尾装置では、図3に示すように、レーダセンサにより2つの追尾目標に対する観測値が検出された状況を想定して説明を行う。
【0030】
加重平均部26bは、目標観測手段1bにより生成された各観測値、および、予測処理部23bにより算出された現観測時刻における各追尾目標の予測値に基づいて、追尾目標ごとに、各観測値に対する信頼度を算出するものである。また、加重平均部26bは、目標観測手段1により生成された各観測値、および、算出した各観測値に対する信頼度に基づいて、追尾目標ごとに、各観測値の加重平均値を算出する。この加重平均部26bにより算出された追尾目標ごとの加重平均値を示す情報は角度基準重心算出部22bに出力される。
【0031】
なお、角度基準重心算出部22bは、実施の形態1における角度基準重心算出部22と同様の処理によって、角度算出部21bにより算出された2つの観測値のなす角度を変数とする関数に基づいて、この2つの観測値に対する重み係数を算出する。さらに、角度基準重心算出部22bは、加重平均部26bにより算出された追尾目標ごとの加重平均値、および、算出した各観測値に対する重み係数に基づいて、各観測値の信頼度付き重心を算出する。この角度基準重心算出部22bにより算出された各観測値の信頼度付き重心を示す情報は平滑処理部24bに出力される。
【0032】
次に、上記のように構成された追尾装置による追尾処理の流れについて説明する。図7はこの発明の実施の形態2に係る追尾装置による追尾処理の流れを示すフローチャートである。なお以下では、図3に示すように、レーダセンサにより2つの追尾目標に対する観測値(観測位置)z1,z2が検出された状況を想定して説明を行う。
【0033】
追尾装置による追尾処理では、図7に示すように、まず、目標観測手段1bは、実施の形態1における目標観測手段1と同様の処理によって、アンテナ・受信系を介してレーダセンサから取得した受信信号から2つの観測値z1,z2を生成する(ステップST71)。この目標観測手段1bにより生成された各観測値z1,z2を示す情報は角度算出部21bおよび加重平均部26bに出力される。
【0034】
次いで、角度算出部21bは、実施の形態1における角度算出部21と同様の処理によって、目標観測手段1bにより生成された各観測値z1,z2に基づいて、レーダセンサ位置を基準点とした2つの観測値z1,z2のなす角度θを算出する(ステップST72)。この角度算出部21bにより算出された2つの観測値z1,z2のなす角度θを示す情報は角度基準重心算出部22bに出力される。
【0035】
一方、加重平均部26bは観測値加重平均算出処理を行う(ステップST73)。具体的には、加重平均部26bは、まず、目標観測手段1bにより生成された各観測値z1,z2、および、予測処理部23bにより算出された現観測時刻tkにおける各追尾目標の予測値(予測位置および予測速度)x(ハット)i,k|k−1(i=1,2)に基づいて、追尾目標ごとに、各観測値z1,z2に対する信頼度βi,j(i=1,2, j=1,2)を算出する。ここで例えば、現観測時刻tkにおける各追尾目標の予測位置x(ハット)i,k|k−1と各観測値z1,z2との距離を変数とする関数を用い、式(5)から、追尾目標ごとに各観測値z1,z2に対する信頼度βi,jを算出する。
【0036】
ここで、上式(5)では、追尾目標ごとの各観測値z1,z2に対する信頼度βi,jを算出する際に用いる関数として、各予測位置x(ハット)i,k|k−1と各観測値z1,z2との距離を変数とする関数を用いているが、これに限定されるものではなく、その他の関数を用いてもよい。
【0037】
次に、加重平均部26bは、目標観測手段1bにより生成された各観測値z1,z2、および、算出した追尾目標ごとの各観測値z1,z2に対する信頼度βi,jに基づいて、式(6)から、追尾目標ごとに、各観測値z1,z2の加重平均値z(バー)1,z(バー)2を算出する。
この加重平均部26bにより算出された追尾目標ごとの加重平均値z(バー)1,z(バー)2を示す情報は角度基準重心算出部22bに出力される。
【0038】
次いで、角度基準重心算出部22bは、実施の形態1における角度基準重心算出部22と同様の処理によって、角度算出部21bにより算出された2つの観測値z1,z2のなす角度θを変数とする関数f(θ)を用いて、各観測値z1,z2に対する重み係数を算出する。そして、この重み係数および加重平均部26bにより算出された追尾目標ごとの加重平均値z(バー)1,z(バー)2に基づいて、式(7)から各観測値z1,z2の信頼度付き重心z(バー)1θ,z(バー)2θを算出する(ステップST74)。
この角度基準重心算出部22bにより算出された各観測値z1,z2の信頼度付き重心z(バー)1θ,z(バー)2θを示す情報は平滑処理部24bに出力される。
【0039】
このステップST74の角度基準重心算出処理によって、図8(a)に示すように、レーダセンサと各追尾目標との距離が大きく、2つの観測値z1,z2(図8の×印位置)のなす角度θが小さい場合には、各追尾目標に対応付けられる各観測値z1,z2の信頼度付き重心z(バー)1θ,z(バー)2θ(図8の□,■印位置)が2つの加重平均値z(バー)1,z(バー)2(図8の△,▲印位置)の中間寄りの位置となる。そのため、追尾目標候補となる位置を安定化させることができ、各追尾目標に対する追尾維持が可能となる。
また、図8(b)に示すように、レーダセンサと各追尾目標との距離が小さくなり、2つの観測値z1,z2のなす角度θが大きくなるに連れて、各追尾目標に対応付けられる各観測値z1,z2の信頼度付き重心z(バー)1θ,z(バー)2θが各加重平均値z(バー)1,z(バー)2側に近づく。そのため、各追尾目標に対する追尾精度を向上させることができる。
なお図8の○,●印位置は、現観測時刻tkにおける各追尾目標の予測位置x(ハット)i,k|k−1(i=1,2)である。
【0040】
一方、予測処理部23bは、実施の形態1における予測処理部23と同様の処理によって予測処理を行い、現観測時刻tkにおける各追尾目標の予測値x(ハット)i,k|k−1および予測誤差共分散行列を算出する(ステップST75)。この予測処理部23bにより算出された現観測時刻tkにおける各追尾目標の予測値x(ハット)i,k|k−1および予測誤差共分散行列を示す情報は平滑処理部24bおよび加重平均部26bに出力される。
【0041】
次いで、平滑処理部24bは、実施の形態1における平滑処理部24と同様の処理によって平滑処理を行い、現観測時刻tkにおける各追尾目標の平滑値x(ハット)i,k|kおよび平滑誤差共分散行列を算出する(ステップST76)。この平滑処理部24bにより算出された現観測時刻tkにおける各追尾目標の平滑値x(ハット)i,k|kおよび平滑誤差共分散行列を示す情報は平滑諸元ファイル25bに出力されて記憶される。
【0042】
次いで、平滑諸元ファイル25bは、実施の形態1における平滑諸元ファイル25と同様の処理によって、1観測周期後の時刻tk+1において、記憶している時刻tkにおける各追尾目標の平滑値x(ハット)i,k|kおよび平滑誤差共分散行列を示す情報を予測処理部23bに出力する(ステップST77)。
【0043】
以上のように、この実施の形態2によれば、追尾目標ごとに各観測値に対する信頼度を算出し、この信頼度に基づいて各観測値の信頼度付き重心を算出して各追尾目標に対応付けるように構成したので、実施の形態1における効果に対して、レーダセンサと各追尾目標との距離が大きく、2つの観測値のなす角度が小さい場合には、追尾目標候補となる位置をより安定化させることができ、レーダセンサと各追尾目標との距離が小さく、2つの観測値のなす角度が大きい場合には、各追尾目標に対する追尾精度をより向上させることができる。
【0044】
実施の形態3.
図9はこの発明の実施の形態3に係る追尾装置の構成の一例を示すブロック図である。図9に示す実施の形態3に係る追尾装置の構成は、図6に示す実施の形態2に係る追尾装置の角度算出部21bを多目標角度算出部21cに変更したものであり、同様の構成についてはその説明を省略する。なお以下で説明する実施の形態3に係る追尾装置では、図11に示すように、レーダセンサによりN個の追尾目標に対する観測値が検出された状況を想定して説明を行う。
【0045】
なお、目標観測手段1cは、アンテナ・受信系(不図示)を介してレーダセンサから取得した受信信号に対して、公知の方法による信号処理を行い、N個の観測値を生成する。この目標観測手段1cにより生成された各観測値を示す情報は多目標角度算出部21cに出力される。
【0046】
多目標角度算出部21cは、目標観測手段1cにより生成されたN個の観測値に基づいて、レーザセンサ位置を基準点とした2つの観測値のなす角度を全ての組み合わせについて算出するものである。この多目標角度算出部21cにより算出された各組み合わせでの2つの観測値のなす角度を示す情報は角度基準重心算出部22cに出力される。
【0047】
次に、上記のように構成された追尾装置による追尾処理の流れについて説明する。図10はこの発明の実施の形態3に係る追尾装置による追尾処理の流れを示すフローチャートである。なお以下では、図11に示すように、レーダセンサによりN個の追尾目標に対する観測値zj(j=1,2,・・・,N)が検出された状況を想定して説明を行う。
【0048】
追尾装置による追尾処理では、図10に示すように、まず、目標観測手段1cは、アンテナ・受信系を介してレーダセンサから取得した受信信号に対して、公知の方法による信号処理を行い、N個の観測値zjを生成する(ステップST101)。この目標観測手段1cにより生成された各観測値zjを示す情報は多目標角度算出部21cおよび加重平均部26cに出力される。
【0049】
次いで、多目標角度算出部21cは、目標観測手段1cにより生成されたN個の観測値zjに基づいて、レーザセンサ位置を基準点とした2つの観測値のなす角度θj,m(j=1,2,・・・,N、m=1,2,・・・,N)を全ての組み合わせについて算出する(ステップST102)。この多目標角度算出部21cにより算出された各組み合わせでの2つの観測値のなす角度θj,mを示す情報は角度基準重心算出部22cに出力される。
【0050】
一方、加重平均部26cは観測値加重平均算出処理を行う(ステップST103)。具体的には、加重平均部26cは、まず、目標観測手段1cにより生成された各観測値zj、および、予測処理部23cにより算出された現観測時刻tkにおける各追尾目標の予測値(予測位置および予測速度)x(ハット)i,k|k−1(i=1,2,・・・,N)に基づいて、追尾目標ごとに、各観測値zjに対する信頼度βi,j(i=1,2,・・・,N j=1,2,・・・,N)を算出する。ここで例えば、現観測時刻tkにおける各追尾目標の予測位置x(ハット)i,k|k−1と各観測値zjとの距離を変数とする関数を用い、式(8)から、追尾目標ごとに各観測値zjに対する信頼度βi,jを算出する。
【0051】
ここで、上式(8)では、追尾目標ごとの各観測値zjに対する信頼度βi,jを算出する際に用いる関数として、各予測位置x(ハット)i,k|k−1と各観測値zjとの距離を変数とする関数を用いているが、これに限定されるものではなく、その他の関数を用いてもよい。
【0052】
次に、加重平均部26cは、目標観測手段1cにより生成された各観測値zj、および、算出した追尾目標ごとの各観測値zjに対する信頼度βi,jに基づいて、式(9)から、追尾目標ごとに、各観測値zjの加重平均値z(バー)i(i=1,2,・・・,N)を算出する。
この加重平均部26cにより算出された追尾目標ごとの加重平均値z(バー)iを示す情報は角度基準重心算出部22cに出力される。
【0053】
次いで、角度基準重心算出部22cは、実施の形態1における角度基準重心算出部22と同様の処理によって、多目標角度算出部21cにより算出された2つの観測値のなす角度θを変数とする関数f(θ)を用いて、この2つの観測値に対する重み係数を全ての組み合わせについて算出する。そして、これらの重み係数および加重平均部26cにより算出された追尾目標ごとの加重平均値z(バー)iに基づいて、式(10)から各観測値zjの信頼度付き重心z(バー)i,θを算出する(ステップST104)。
この角度基準重心算出部22cにより算出された各観測値zjの信頼度付き重心z(バー)i,θを示す情報は平滑処理部24cに出力される。
【0054】
このステップST104の角度基準重心算出処理によって、レーダセンサと各追尾目標との距離が大きく、2つの観測値のなす角度θが小さい場合には、各追尾目標に対応付けられる各観測値zjの信頼度付き重心z(バー)i,θが各加重平均値z(バー)iの中間寄りの位置となる。そのため、追尾目標候補となる位置を安定化させることができ、各追尾目標に対する追尾維持が可能となる。
また、レーダセンサと各追尾目標との距離が小さくなり、2つの観測値のなす角度θが大きくなるに連れて、各追尾目標に対応付けられる各観測値ziの信頼度付き重心z(バー)i,θが各加重平均値z(バー)i側に近づく。そのため、各追尾目標に対する追尾精度を向上させることができる。
【0055】
一方、予測処理部23cは予測処理を行う(ステップST105)。具体的には、予測処理部23cは、まず、1観測周期前の時刻tk−1における各追尾目標の平滑値(平滑位置および平滑速度)x(ハット)i,k−1|k−1(i=1,2,・・・,N)を示す情報を平滑諸元ファイル25cから取得する。そして、この平滑値x(ハット)i,k−1|k−1に基づいて、式(11)から現観測時刻tkにおける各追尾目標の予測値(予測位置および予測速度)x(ハット)i,k|k−1(i=1,2,・・・,N)を算出する。
【0056】
次に、予測処理部23cは、実施の形態1における予測処理部23と同様の処理によって、各追尾目標の予測誤差共分散行列を算出する。
この予測処理部23cにより算出された現観測時刻tkにおける各追尾目標の予測値x(ハット)i,k|k−1および予測誤差共分散行列を示す情報は平滑処理部24cおよび加重平均部26cに出力される。
【0057】
次いで、平滑処理部24cは平滑処理を行う(ステップST106)。具体的には、平滑処理部24cは、まず、角度基準重心算出部22cにより算出された各観測値zjの信頼度付き重心z(バー)i,θを、予測処理部23cより算出された現観測時刻tkにおける各追尾目標の予測値x(ハット)i,k|k−1に対応付け、各追尾目標に対するカルマンゲインKi,k(i=1,2,・・・,N)を算出する。そして、式(12)に示すカルマンフィルタアルゴリズムに従って現観測時刻tkにおける各追尾目標の平滑値x(ハット)i,k|k−1を算出する。
【0058】
次に、平滑処理部24cは、予測処理部23cにより算出された予測誤差共分散行列、および、各追尾目標に対するカルマンゲインKi,kに基づいて、現観測時刻tkにおける各追尾目標の平滑誤差共分散行列を算出する。
この平滑処理部24cにより算出された現観測時刻tkにおける各追尾目標の平滑値x(ハット)i,k|k−1および平滑誤差共分散行列を示す情報は平滑諸元ファイル25cに出力されて記憶される。
【0059】
次いで、平滑諸元ファイル25cは、実施の形態1における平滑諸元ファイル25と同様の処理によって、1観測周期後の時刻tk+1において、記憶している時刻tkにおける各追尾目標の平滑値x(ハット)i,k|k−1および平滑誤差共分散行列を示す情報を予測処理部23cに出力する(ステップST107)。
【0060】
以上のように、この実施の形態3によれば、N個の観測値に基づいて、レーザセンサ位置を基準とした2つの観測値のなす角度を全ての組み合わせに対して算出し、これらの角度から重み係数を算出し、この重み係数に基づいて算出した各観測値の信頼度付き重心を追尾目標に対応付けるように構成したので、レーダセンサと各追尾目標との距離が大きく、2つの観測値のなす角度が小さい場合には、追尾目標候補となる位置を安定化させることで各追尾目標に対する追尾維持が可能となり、レーダセンサと各追尾目標との距離が小さく、2つの観測値のなす角度が大きい場合には、複数得られている各観測値のそれぞれを各追尾目標に対応付けることで、各追尾目標に対する追尾精度を向上させることができる。
【0061】
なお、図9に示す実施の形態3に係る追尾装置は、実施の形態2に係る追尾装置の角度算出部21bを多目標角度算出部21cに変更したものであるが、実施の形態1に係る追尾装置の角度算出部21を多目標角度算出部21cに変更した場合についても同様に適用可能である。
なお、本願発明はその発明の範囲内において、各実施の形態の自由な組み合わせ、あるいは各実施の形態の任意の構成要素の変形、もしくは各実施の形態において任意の構成要素の省略が可能である。
【符号の説明】
【0062】
1,1b,1c 目標観測手段、2,2b,2c 追尾手段、21,21b 角度算出部、21c 多目標角度算出部、22,22b,22c 角度基準重心算出部、23,23b,23c 予測処理部、24,24b,24c 平滑処理部、25,25b,25c 平滑諸元ファイル、26b,26c 加重平均部。
【特許請求の範囲】
【請求項1】
近接した複数の追尾目標に対する観測値をセンサを介して取得し、当該観測値に基づいて当該追尾目標に対する平滑値および予測値を算出することで追尾を行う追尾装置において、
前記センサを介して2つの観測値を取得した場合に、当該センサ位置を基準点とした当該2つの観測値のなす角度を算出する角度算出部と、
前記角度算出部により算出された2つの観測値のなす角度に基づいて当該2つの観測値に対する重み係数を算出し、当該重み係数に基づいて当該各観測値の当該各観測値間での重心をそれぞれ算出する角度基準重心算出部と、
前記角度基準重心算出部により算出された各観測値の重心および前記各追尾目標に対する予測値に基づいて、各追尾目標に対する平滑値を算出する平滑処理部と
を備えた追尾装置。
【請求項2】
前記角度基準重心算出部は、前記2つの観測値のなす角度を変数とする関数を用いて、当該角度が大きくなるにつれて前記各観測値の重心が当該各観測値の中間側から当該各観測値側に近づくような前記重み係数を算出する
ことを特徴とする請求項1記載の追尾装置。
【請求項3】
前記追尾目標ごとに、前記予測値に基づいて前記各観測値に対する信頼度を算出し、当該信頼度に基づいて当該各観測値の加重平均値を算出する加重平均部を備え、
前記角度基準重心算出部は、前記各観測値に対する重み係数および前記加重平均部により算出された追尾目標ごとの加重平均値に基づいて、当該各観測値の信頼度付き重心を算出する
ことを特徴とする請求項1または請求項2記載の追尾装置。
【請求項4】
前記角度算出部は、前記センサを介して3つ以上の観測値を取得した場合に、当該センサ位置を基準点とした2つの観測値のなす角度を全ての組み合わせについて算出し、
前記角度基準重心算出部は、前記角度算出部により算出された2つの観測値のなす角度に基づいて当該2つの観測値に対する重み係数を全ての組み合わせについて算出し、当該各重み係数に基づいて、前記センサを介して取得した各観測値の当該各観測値間での重心をそれぞれ算出する
ことを特徴とする請求項1から請求項3のうちのいずれか1項記載の追尾装置。
【請求項1】
近接した複数の追尾目標に対する観測値をセンサを介して取得し、当該観測値に基づいて当該追尾目標に対する平滑値および予測値を算出することで追尾を行う追尾装置において、
前記センサを介して2つの観測値を取得した場合に、当該センサ位置を基準点とした当該2つの観測値のなす角度を算出する角度算出部と、
前記角度算出部により算出された2つの観測値のなす角度に基づいて当該2つの観測値に対する重み係数を算出し、当該重み係数に基づいて当該各観測値の当該各観測値間での重心をそれぞれ算出する角度基準重心算出部と、
前記角度基準重心算出部により算出された各観測値の重心および前記各追尾目標に対する予測値に基づいて、各追尾目標に対する平滑値を算出する平滑処理部と
を備えた追尾装置。
【請求項2】
前記角度基準重心算出部は、前記2つの観測値のなす角度を変数とする関数を用いて、当該角度が大きくなるにつれて前記各観測値の重心が当該各観測値の中間側から当該各観測値側に近づくような前記重み係数を算出する
ことを特徴とする請求項1記載の追尾装置。
【請求項3】
前記追尾目標ごとに、前記予測値に基づいて前記各観測値に対する信頼度を算出し、当該信頼度に基づいて当該各観測値の加重平均値を算出する加重平均部を備え、
前記角度基準重心算出部は、前記各観測値に対する重み係数および前記加重平均部により算出された追尾目標ごとの加重平均値に基づいて、当該各観測値の信頼度付き重心を算出する
ことを特徴とする請求項1または請求項2記載の追尾装置。
【請求項4】
前記角度算出部は、前記センサを介して3つ以上の観測値を取得した場合に、当該センサ位置を基準点とした2つの観測値のなす角度を全ての組み合わせについて算出し、
前記角度基準重心算出部は、前記角度算出部により算出された2つの観測値のなす角度に基づいて当該2つの観測値に対する重み係数を全ての組み合わせについて算出し、当該各重み係数に基づいて、前記センサを介して取得した各観測値の当該各観測値間での重心をそれぞれ算出する
ことを特徴とする請求項1から請求項3のうちのいずれか1項記載の追尾装置。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【公開番号】特開2012−242121(P2012−242121A)
【公開日】平成24年12月10日(2012.12.10)
【国際特許分類】
【出願番号】特願2011−109522(P2011−109522)
【出願日】平成23年5月16日(2011.5.16)
【出願人】(000006013)三菱電機株式会社 (33,312)
【Fターム(参考)】
【公開日】平成24年12月10日(2012.12.10)
【国際特許分類】
【出願日】平成23年5月16日(2011.5.16)
【出願人】(000006013)三菱電機株式会社 (33,312)
【Fターム(参考)】
[ Back to top ]