JP6906409B2 - 追尾処理装置及び追尾処理方法 - Google Patents

Description

本発明は、目標の追尾処理を行う追尾処理装置及び追尾処理方法に関する。

追尾処理装置は、センサの観測情報を基に、目標の運動諸元を推定しつつ、観測時刻毎に当該運動諸元を逐次更新して行く装置である。目標の運動諸元には、目標位置、目標速度及び目標加速度が含まれる。センサの観測情報には、目標の運動諸元に関する観測値、及び目標の運動諸元を観測したときの観測時刻が含まれる。

下記特許文献１及び特許文献２には、複数のセンサの観測情報を用いて目標の追尾処理を行う際に、センサ間のバイアス誤差を推定して目標の追尾処理に関する推定精度を向上させる技術が開示されている。

特開２０１１−６４４８４号公報 特開２０１２−４７４９０号公報

しかしながら、上記特許文献１及び特許文献２に代表される従来技術では、センサ間のバイアス誤差を推定するためには、一定時間、目標の観測情報を蓄積する必要がある。すなわち、従来技術では、複数のセンサの観測情報を用いて追尾処理を行う際には、一定時間の経過を待たなければ追尾処理の演算を行うことができず、追尾処理が遅延し、追尾精度が低下するという問題があった。

本発明は、上記に鑑みてなされたものであって、バイアス誤差の推定に伴って生じ得る追尾処理の遅延及び追尾精度の低下を抑制することができる追尾処理装置を得ることを目的とする。

上述した課題を解決し、目的を達成するために、本発明は、複数のセンサからの観測情報を基に、目標の追尾処理を行う追尾処理装置である。追尾処理装置は、観測情報を基に、観測誤差共分散行列を演算する第１の演算部、前回の観測時刻における平滑値と、前回の観測時刻における平滑誤差共分散行列と、今回の観測時刻における観測情報と、前回の観測時刻から今回の観測時刻までの経過時間とを基に、目標運動諸元の予測値を演算する第２の演算部、観測情報と、観測誤差共分散行列と、予測値と、予測誤差共分散行列とを基に、平滑値と平滑誤差共分散行列とカルマンゲイン行列とを演算する第３の演算部、及び、観測情報と、平滑値と、予測値と、カルマンゲイン行列とを基に、観測誤差共分散行列を補正して第３の演算部にフィードバックする補正処理部を備える。

本発明によれば、バイアス誤差の推定に伴って生じ得る追尾処理の遅延及び追尾精度の低下を抑制することができるという効果を奏する。

実施の形態に係る追尾処理装置の構成を示すブロック図 第１の演算部の動作説明に供する図 第２の演算部の動作説明に供する図 第３の演算部の動作説明に供する第１の図 第３の演算部の動作説明に供する第２の図 実施の形態に係る追尾処理装置における要部の処理の流れを示すフローチャート 補正処理部における補正処理の詳細説明に供する図 実施の形態の追尾処理装置におけるハードウェア構成の一例を示すブロック図 実施の形態の追尾処理装置におけるハードウェア構成の他の例を示すブロック図

以下に添付図面を参照し、本発明の実施の形態に係る追尾処理装置及び追尾処理方法について詳細に説明する。なお、以下の実施の形態により、本発明が限定されるものではない。

実施の形態．
図１は、実施の形態に係る追尾処理装置１００の構成を示すブロック図である。実施の形態に係る追尾処理装置１００は、複数のセンサからの観測情報を基に、目標の追尾処理を行う装置である。実施の形態に係る追尾処理装置１００は、図１に示すように、第１の演算部２０、第２の演算部３０、第３の演算部４０及び補正処理部５０を備えている。なお、第１の演算部２０、第２の演算部３０及び第３の演算部４０は機能区分であり、機能区分の変更に伴って図１とは異なる構成を採ってもよい。

図１に示すように、追尾処理装置１００には、第１のセンサ１０及び第２のセンサ１１からの観測情報が入力される。第１のセンサ１０及び第２のセンサ１１は複数のセンサの例示である。すなわち、追尾処理装置１００は、３つ以上のセンサからの観測情報が入力される構成であってもよい。前述したように、観測情報には、目標の運動諸元に関する観測値と、目標の運動諸元を観測したときの観測時刻とが含まれる。以下、「目標の運動諸元」を「目標運動諸元」と呼ぶ。なお、観測値とは、センサが観測した観測情報の値である。

追尾処理装置１００の外部には、表示装置１１０が備えられている。表示装置１１０は、表示手段の一例である。追尾処理装置１００の演算結果は、表示装置１１０に出力される。追尾処理装置１００の演算結果は、図１では図示しない記憶装置に出力されてもよい。

次に、追尾処理装置１００を構成する各構成部間の信号又は情報の流れについて説明する。

第１のセンサ１０は、目標８０を観測する。第１のセンサ１０による観測値１０ａは、第１の演算部２０、第３の演算部４０及び補正処理部５０に伝送される。観測値１０ａには、第１のセンサ１０が観測した目標運動諸元に関する情報と、第１のセンサ１０が目標８０を観測したときの観測精度に関する情報とが含まれる。第２の演算部３０には、第１のセンサ１０が目標８０を観測したときの観測時刻１０ｂに関する情報が第１のセンサ１０から伝送される。

第２のセンサ１１も第１のセンサ１０と同様に、目標８０を観測する。第１のセンサ１０が観測する目標と第２のセンサ１１が観測する目標とは、同じ目標である場合もあれば、異なる目標である場合もある。第２のセンサ１１による観測値１１ａは、第１の演算部２０、第３の演算部４０及び補正処理部５０に伝送される。観測値１１ａには、第２のセンサ１１が観測した目標運動諸元に関する情報と、第２のセンサ１１が目標８０を観測したときの観測精度に関する情報とが含まれる。第２の演算部３０には、第２のセンサ１１が目標８０を観測したときの観測時刻１１ｂに関する情報が第２のセンサ１１から伝送される。

第１の演算部２０には、上述した観測値１０ａ及び観測値１１ａが入力される。カルマンゲイン行列４０ｃの詳細については後述する。第１の演算部２０は、観測値１０ａ及び観測値１１ａの情報を基に、観測誤差共分散行列２０ａを演算する。観測誤差共分散行列２０ａの詳細については後述する。第１の演算部２０が演算した観測誤差共分散行列２０ａは、第３の演算部４０に伝送される。

第２の演算部３０には、上述した観測時刻１０ｂ及び観測時刻１１ｂに加え、第３の演算部４０が演算した平滑値４０ａ及び平滑誤差共分散行列４０ｂが入力される。平滑値４０ａ及び平滑誤差共分散行列４０ｂは、前回の観測時刻で取得した観測情報を基に演算された値である。平滑値４０ａ及び平滑誤差共分散行列４０ｂの詳細については後述する。なお、演算処理は、観測時刻を基準に行われる。このため、「観測時刻」と「演算時刻」とは同義とし、本明細書では、「観測時刻」という用語を使用する。

第２の演算部３０は、前回の観測時刻において演算された平滑値４０ａ及び平滑誤差共分散行列４０ｂの情報と、観測時刻１０ｂ及び観測時刻１１ｂの情報と、前回の観測時刻から今回の観測時刻までの経過時間とを基に、今回の観測時刻における目標運動諸元の予測値３０ａと、今回の観測時刻における予測誤差共分散行列３０ｂとを演算する。予測値３０ａ及び予測誤差共分散行列３０ｂの詳細については後述する。第２の演算部３０が演算した予測値３０ａ及び予測誤差共分散行列３０ｂは、第３の演算部４０に伝送される。また、予測値３０ａは、補正処理部５０にも伝送される。

第３の演算部４０には、観測値１０ａ及び観測値１１ａと、第１の演算部２０が演算した観測誤差共分散行列２０ａと、第２の演算部３０が演算した予測値３０ａ及び予測誤差共分散行列３０ｂと、補正処理部５０によって補正された観測誤差共分散行列５０ｃとが入力される。すなわち、補正された観測誤差共分散行列５０ｃは、補正処理部５０によって第３の演算部４０にフィードバックされる。

第３の演算部４０は、観測値１０ａ、観測値１１ａ、観測誤差共分散行列２０ａ、予測値３０ａ及び予測誤差共分散行列３０ｂに関する各情報を基に、平滑値４０ａ、平滑誤差共分散行列４０ｂ及びカルマンゲイン行列４０ｃを演算する。また、補正された観測誤差共分散行列５０ｃを基に、平滑値４０ａ、平滑誤差共分散行列４０ｂ及びカルマンゲイン行列４０ｃの再演算が行われる。

補正処理部５０には、観測値１０ａ及び観測値１１ａと、第２の演算部３０が演算した予測値３０ａと、第３の演算部４０が演算した平滑値４０ａ、平滑誤差共分散行列４０ｂ及びカルマンゲイン行列４０ｃとが入力される。補正処理部５０は、平滑値５０ａ及び平滑誤差共分散行列５０ｂを生成して表示装置１１０に出力する。

補正処理部５０による補正処理は、観測値１０ａ、観測値１１ａ、予測値３０ａ、平滑値４０ａ、平滑誤差共分散行列４０ｂ及びカルマンゲイン行列４０ｃに関する各情報を基に行われる。当該補正処理では、まず第３の演算部４０が生成した平滑値４０ａ及び平滑誤差共分散行列４０ｂに対する補正処理の要否が判定される。

当該補正処理が必要と判定された場合、平滑値４０ａ及び平滑誤差共分散行列４０ｂに対する補正処理が実施される。一方、当該補正処理が不要と判定された場合、補正処理は実施されずに、第３の演算部４０によって生成された平滑値４０ａ及び平滑誤差共分散行列４０ｂが、そのまま出力される。補正処理の詳細については後述する。

次に、補正処理部５０を設ける理由について、図１から図５の図面を参照して説明する。図２は、第１の演算部２０の動作説明に供する図である。図３は、第２の演算部３０の動作説明に供する図である。図４は、第３の演算部４０の動作説明に供する第１の図である。図５は、第３の演算部４０の動作説明に供する第２の図である。

図２には、観測値が丸記号「○」で示され、当該観測値の観測誤差の広がりを表す概念を有する観測誤差共分散行列が一点鎖線で示されている。前述したように、観測誤差共分散行列は、第１の演算部２０によって演算される。観測誤差共分散行列は、図２の一点鎖線で示されるように、観測値に対して、破線で示されるセンサ視線方向にセンサの距離精度に応じた広がりと、センサの視線方向に直交する方向にセンサの角度精度に応じた広がりとを持って表される。すなわち、第１の演算部２０は、センサの観測精度である距離精度及び角度精度に応じた観測誤差共分散行列を演算することにより、目標状態の真値が存在すると考えられる範囲を推定している。

図３には、前回の観測時刻ｔ_ｋ−１において演算された平滑値がプラス記号「＋」で示され、当該平滑値の推定誤差の広がりを表す概念を有する平滑誤差共分散行列が実線で示されている。また、図３には、今回の観測時刻ｔ_ｋにおいて演算される予測値が白抜きの三角記号「△」で示され、当該予測値の予測誤差の広がりを表す概念を有する予測誤差共分散行列が破線で示されている。前述したように、予測値及び予測誤差共分散行列は、第２の演算部３０によって演算される。予測値及び予測誤差共分散行列は、前回の観測時刻ｔ_ｋ−１において演算された平滑値及び平滑誤差共分散行列と、前回の観測時刻ｔ_ｋ−１から今回の観測時刻ｔ_ｋまでの経過時間Δｔ＝ｔ_ｋ−ｔ_ｋ−１とを基に演算される。すなわち、予測値は、前回の観測時刻ｔ_ｋ−１で取得した観測情報を基に演算で求めた平滑値を用いて予測される目標運動諸元の予測値である。

図４は、センサ間にバイアス誤差が存在しない場合の図である。ここで、バイアス誤差とは、異なるセンサ間の観測値に存在する誤差である。バイアス誤差には、センサ間の位置誤差、及びセンサ間の角度誤差が含まれる。図４には、プラス記号「＋」で示される平滑値が、丸記号「○」で示される観測値と、白抜きの三角記号「△」で示される予測値とを用いて算出される様子が示されている。すなわち、平滑値は、観測値と予測値とから求められる目標運動諸元の推定値である。また、図４には、図２に示した観測誤差共分散行列が一点鎖線で示され、図３に示した予測誤差共分散行列が破線で示されている。図４を参照すると、平滑値は、観測値と予測値とを内分する値として求められている。すなわち、図４によれば、平滑値は、観測値と予測値とから精度よく求められる状況が理解できる。

図４に対して図５は、センサ間にバイアス誤差が存在する場合の図である。図５において、破線で示される予測誤差共分散行列は、図４に示すものと同一である。一方、図５において、二点鎖線で示される観測誤差共分散行列は、図４に示すものよりも紙面の左上部寄りに変位している。この変位は、バイアス誤差によるズレに起因するものである。バイアス誤差のズレによって、演算される観測値は丸記号で示される位置から太字の丸記号で示される位置に推移する。その結果、演算される平滑値はプラス記号で示される位置から太字のプラス記号で示される位置に推移する。このように、バイアス誤差のズレによって、目標運動諸元の推定精度は悪化してしまうことになる。

ここで、図１に示す構成から補正処理部５０を有さない追尾処理装置を仮定し、この追尾処理装置が複数センサの観測情報を処理する場合について考える。具体的には、第１のセンサ１０による目標８０の観測情報を基に追尾処理演算を実施後、次の追尾処理演算において、第２のセンサ１１による目標８０の観測情報を基に追尾処理演算を実施することを考える。

まず、一般的に、センサ間において、時刻同期が常に行われている訳ではない。このため、各センサは絶対時刻を検知することができない。その結果、第１のセンサ１０から伝送される観測時刻と、第２のセンサ１１から伝送される観測時刻との間には、誤差が存在する。この観測時刻の誤差もバイアス誤差の一つであり、本明細書では「時刻同期誤差」と呼ぶ。よって、センサ間に存在するバイアス誤差には、上記特許文献に示されているセンサ間の位置誤差、及びセンサ間の角度誤差に、ここで定義した時刻同期誤差が加わる。その結果、第１のセンサ１０の観測情報を基に演算される予測値と、第２のセンサ１１の観測情報を基に演算される観測値との間には、センサ間のバイアス誤差が付加される。

よって、第３の演算部４０が、平滑値及び平滑誤差共分散行列を演算する際には、センサ間のバイアス誤差によって、図５に示すように、平滑値の推定精度が悪化してしまう。そこで、図１に示すように、第３の演算部４０の後段に補正処理部５０を設け、補正処理部５０の処理結果をフィードバックするように構成したものが、実施の形態に係る追尾処理装置１００である。

次に、実施の形態に係る追尾処理装置における演算処理の詳細について、図１及び図６の図面、並びに下記に示す幾つかの数式を参照して説明する。図６は、実施の形態に係る追尾処理装置１００における要部の処理の流れを示すフローチャートである。

まず、各センサから伝送される観測値及び観測精度をベクトル量として、それぞれ「ｚ^→ _ｋ」及び「ｖ^→ _ｋ」で表す。観測値ベクトルｚ^→ _ｋは、観測時刻ｔ_ｋにおいて観測された観測値を表すベクトルである。観測精度ベクトルｖ^→ _ｋは、観測時刻ｔ_ｋにおいて観測された観測値の観測精度を表すベクトルである。ここで、「ｚ^→ _ｋ」という表記は、「ｚ」の上部に矢印記号「→」が配置された文字の代替表記である。本明細書では、イメージで挿入する数式を除き、当該代替表記を使用する。

観測時刻ｔ_ｋにおける観測値ベクトルｚ^→ _ｋ、及び観測精度ベクトルｖ^→ _ｋは、以下の（１−１）式、及び（１−２）式で表すことができる。ここで、観測値ベクトルｚ^→ _ｋは、基準となる直交座標系である追尾基準座標系で定義されたベクトルとする。追尾基準座標系の一例は、北基準直交座標系である。また、観測精度ベクトルｖ^→ _ｋは、センサを基準とする極座標系であるセンサ基準極座標系で定義されたベクトルとする。

上記（１−１）式において、「ｘ_ｏｋ」は、観測時刻ｔ_ｋにおける観測値のｘ軸成分である。「ｙ_ｏｋ」は、観測時刻ｔ_ｋにおける観測値のｙ軸成分である。「ｚ_ｏｋ」は、観測時刻ｔ_ｋにおける観測値のｚ軸成分である。記号「Ｔ」は、転置を表す。この表記は、後述の数式においても同様である。また、上記（１−２）式において、「σ_ｒｋ」は、観測時刻ｔ_ｋにおける観測精度の距離方向の成分である。「σ_ｅｋ」は、観測時刻ｔ_ｋにおける観測精度の仰角方向の成分である。「σ_ｂｙｋ」は、観測時刻ｔ_ｋにおける観測精度の方位角方向の成分である。なお、観測精度の距離方向の成分を「距離精度」と呼び、観測精度の仰角方向の成分を「仰角精度」と呼び、観測精度の方位角方向の成分を「方位角精度」と呼ぶ場合がある。

次いで、第１の演算部２０によって演算される観測誤差共分散行列を「Ｒ_ｋ」で表す。観測誤差共分散行列Ｒ_ｋは、目標運動諸元の真値が存在すると考えられる範囲を表す行列である。観測誤差共分散行列Ｒ_ｋは、以下の（２）式で表すことができる。

上記（２）式に示されるように、観測誤差共分散行列Ｒ_ｋは、上記（１−２）式に示した観測精度ベクトルｖ^→ _ｋの距離精度σ_ｒｋ、仰角精度σ_ｅｋ及び方位角精度σ_ｂｙｋの２乗成分を対角要素とする行列と、座標変換行列Ｇ_ｋとを用いて表すことができる。座標変換行列Ｇ_ｋは、センサ基準極座標系から基準となる直交座標系への変換行列である。

また、第２の演算部３０によって演算される観測時刻ｔ_ｋにおける予測値のベクトルを「ｘ^→ _{ｋ｜ｋ−１}」で表す。観測時刻ｔ_ｋにおける予測値ベクトルｘ^→ _{ｋ｜ｋ−１}は、以下の（３）式で表すことができる。

上記（３）式において、「ｘ_{ｋ｜ｋ−１}」は、観測時刻ｔ_ｋにおける位置予測値のｘ軸成分である。「ｙ_{ｋ｜ｋ−１}」は、観測時刻ｔ_ｋにおける位置予測値のｙ軸成分である。「ｚ_{ｋ｜ｋ−１}」は、観測時刻ｔ_ｋにおける位置予測値のｚ軸成分である。「ｘ^・ _{ｋ｜ｋ−１}」は、観測時刻ｔ_ｋにおける速度予測値のｘ軸成分である。「ｙ^・ _{ｋ｜ｋ−１}」は、観測時刻ｔ_ｋにおける速度予測値のｙ軸成分である。「z^・ _{ｋ｜ｋ−１}」は、観測時刻ｔ_ｋにおける速度予測値のz軸成分である。ここで、「ｘ^・ _{ｋ｜ｋ−１}」という表記は、「ｘ」の上部にドット記号「・」が配置された文字の代替表記である。「ｙ」及び「ｚ」についても同様である。本明細書では、イメージで挿入する数式を除き、当該代替表記を使用する。

また、第３の演算部４０によって演算される観測時刻ｔ_ｋ−１における平滑値のベクトルを「ｘ^→ _{ｋ−１｜ｋ−１}」で表す。観測時刻ｔ_ｋ−１は、観測時刻ｔ_ｋから見ると、前回の観測時刻である。なお、観測時刻ｔ_ｋを「今回の観測時刻」と呼ぶ場合がある。前回の観測時刻ｔ_ｋ−１における平滑値ベクトルｘ^→ _{ｋ−１｜ｋ−１}は、以下の（４）式で表すことができる。

上記（４）式において、「ｘ_{ｋ−１｜ｋ−１}」は、前回の観測時刻ｔ_ｋ−１における位置平滑値のｘ軸成分である。「ｙ_{ｋ−１｜ｋ−１}」は、前回の観測時刻ｔ_ｋ−１における位置平滑値のｙ軸成分である。「ｚ_{ｋ−１｜ｋ−１}」は、前回の観測時刻ｔ_ｋ−１における位置平滑値のｚ軸成分である。「ｘ^・ _{ｋ−１｜ｋ−１}」は、前回の観測時刻ｔ_ｋ−１における速度平滑値のｘ軸成分である。「ｙ^・ _{ｋ−１｜ｋ−１}」は、前回の観測時刻ｔ_ｋ−１における速度平滑値のｙ軸成分である。「z^・ _{ｋ−１｜ｋ−１}」は、前回の観測時刻ｔ_ｋ−１における速度平滑値のｚ軸成分である。

前回の観測時刻ｔ_ｋ−１における平滑値ベクトルｘ^→ _{ｋ−１｜ｋ−１}と、今回の観測時刻ｔ_ｋにおける予測値ベクトルｘ^→ _{ｋ｜ｋ−１}との関係は、以下の（５）式で表すことができる。

上記（５）式において、Φ（ｔ_ｋ，ｔ_ｋ−１）は、前回の観測時刻ｔ_ｋ−１から今回の観測時刻ｔ_ｋまでの経過時間による目標諸元の変化を表す行列である。以下、この行列を「状態遷移行列」と呼ぶ。

上記（５）式に示した状態遷移行列Φ（ｔ_ｋ，ｔ_ｋ−１）を用いると、第２の演算部３０によって演算される観測時刻ｔ_ｋにおける予測誤差共分散行列Ｐ_{ｋ｜ｋ−１}は、以下の（６）式を用いて表すことができる。

上記（６）式において、「Ｐ_{ｋ−１｜ｋ−１}」は、前回の観測時刻ｔ_ｋ−１における平滑誤差共分散行列であり、「Ｑ_ｋ−１」は、前回の観測時刻ｔ_ｋ−１におけるシステム雑音を表すシステム雑音共分散行列である。

上述したように、第３の演算部４０は、平滑値、平滑誤差共分散行列及びカルマンゲイン行列を演算する。平滑値は、上述した通りである。また、観測時刻ｔ_ｋにおけるカルマンゲイン行列を「Ｋ_ｋ」で表す。カルマンゲイン行列Ｋ_ｋは、以下の（７）式で表すことができる。

上記（７）式において、「Ｐ_{ｋ｜ｋ−１}」は、上記（６）式で表される観測時刻ｔ_ｋにおける予測誤差共分散行列であり、「Ｒ_ｋ」は、上記（２）式で表される観測時刻ｔ_ｋにおける観測誤差共分散行列である。また、「Ｈ_ｋ」は、観測時刻ｔ_ｋにおけるセンサ観測行列である。センサ観測行列Ｈ_ｋは、以下の（８）式で表すことができる。

上記（８）式において、「Ｏ_３×３」は、３行３列の零行列である。センサ観測行列Ｈ_ｋは、予測値ベクトルｘ^→ _{ｋ｜ｋ−１}からｘ軸、ｙ軸及びｚ軸の位置成分を取り出すための行列である。

上記（７）式、及び（８）式を用いると、観測時刻ｔ_ｋにおける平滑値ベクトルｘ^→ _ｋ｜ｋ、及び観測時刻ｔ_ｋにおける平滑誤差共分散行列Ｐ_ｋ｜ｋのそれぞれは、以下の（９−１）式、及び（９−２）式で表すことができる。

補正処理部５０の処理の流れは図６に示す通りである。図６に示す処理は、補正処理部５０によって実施される。ステップＳＴ６−１では、平滑値が観測値及び予測値に対して外れた位置に算出されているか否かが判定される。ステップＳＴ６−１の判定処理は、上記（７）式に示したカルマンゲイン行列Ｋ_ｋの各要素の値に着目することにより行われる。すなわち、補正処理部５０は、カルマンゲイン行列Ｋ_ｋにおける要素を補正することで、観測誤差共分散行列に対する補正を行う。

平滑値が観測値及び予測値に対して外れた位置ではないと判定された場合（ステップＳＴ６−１，Ｎｏ）、図６に示す補正処理のフローを終了する。図４は、平滑値が観測値及び予測値に対して外れた位置ではない場合の例である。

一方、平滑値が観測値及び予測値に対して外れた位置にあると判定された場合（ステップＳＴ６−１，Ｙｅｓ）、ステップＳＴ６−２に進む。図５は、平滑値が観測値及び予測値に対して外れた位置にある場合の例である。

ステップＳＴ６−２では、観測誤差共分散の広がりが最も小さい要素について、補正刻み幅分、観測誤差共分散が大きくなるように補正される。図２は、観測誤差共分散の広がりが最も小さい要素が、距離である場合の例である。すなわち、観測誤差共分散行列で表される観測精度の広がりが図２の例の場合、観測誤差共分散行列における距離方向の成分が、設定された補正刻み幅分大きくなるように補正される。

ステップＳＴ６−３では、観測誤差共分散行列の補正量が閾値以下であるかが判定される。補正量が閾値以下である場合（ステップＳＴ６−３，Ｙｅｓ）、ステップＳＴ６−１の処理に戻る。一方、補正量が閾値を超えた場合（ステップＳＴ６−３，Ｎｏ）、ステップＳＴ６−４に進む。ステップＳＴ６−４では、補正量が閾値まで下げられる。すなわち、ステップＳＴ６−３，ＳＴ６−４によって、補正量を閾値までに制限する処理が行われる。ステップＳＴ６−４の処理後、ステップＳＴ６−１の処理に戻る。

以上のように、補正処理部５０は、観測誤差共分散行列を補正し、補正後の観測誤差共分散行列を第３の演算部４０に出力する。なお、第３の演算部４０において、補正後の観測誤差共分散行列を基に、平滑値４０ａ、平滑誤差共分散行列４０ｂ及びカルマンゲイン行列４０ｃの再演算が行われることは、前述の通りである。

次に、補正処理部５０で行われる補正処理の詳細について、図７及び以下に示す数式を参照して説明する。図７は、補正処理部５０における補正処理の詳細説明に供する図である。

以下に示す（１０）式は、上述したステップＳＴ６−１の判定処理に用いる判定式の例である。

上記（１０）式において、「ｍｉｎ（Ｋ_ｋ）」は、カルマンゲイン行列Ｋ_ｋにおける各要素の中の最小値を意味し、「ｍａｘ（Ｋ_ｋ）」は、カルマンゲイン行列Ｋ_ｋにおける各要素の中の最大値を意味する。すなわち、上記（１０）式において、カルマンゲイン行列Ｋ_ｋの各要素の中で、最小値をとる要素が０未満であるか、もしくは、最大値をとる要素が１を超える場合には、平滑値は観測値と予測値に対して外れた位置に算出されていると判定される。

ここで、カルマンゲイン行列Ｋ_ｋにおける各要素の値は、観測時刻ｔ_ｋにおける観測値及び予測値を加重平均して平滑値を算出するために用いられる値である。このため、カルマンゲイン行列Ｋ_ｋにおける各要素の中の最小値が負の場合、もしくは、カルマンゲイン行列Ｋ_ｋにおける各要素の中の最大値が１を超える場合、平滑値が観測値と予測値を外分する形で算出されることを意味する。図４の例に示されるように、平滑値が観測値と予測値を内分する形で算出されれば精度がよく、逆に図５の例に示されるように、平滑値が観測値と予測値を外分する形で算出されれば精度が悪いことが理解できる。

なお、上記（１０）式では、ステップＳＴ６−１における判定の閾値を、０及び１に設定しているが、０又は１以外の値に設定してもよい。すなわち、カルマンゲイン行列Ｋ_ｋの各要素の中で、最小値をとる要素が第１の値未満であるか、もしくは、最大値をとる要素が第１の値よりも大きな第２の値を超える場合に、平滑値が観測値と予測値に対して外れた位置に算出されていると判定してもよい。

また、以下に示す（１１−１）式、及び（１１−２）式は、上述したステップＳＴ６−２の処理に用いる演算式の例である。

上記（１１−１）式において、「σ_ｒｋ」、「σ_ｅｋ」及び「σ_ｂｙｋ」は、上記（２）式に示した観測誤差共分散行列Ｒ_ｋの各要素である、距離精度、仰角精度及び方位角精度である。「ε」は、補正刻み幅である。「σ_{ｒｋ，ｎｅｗ}」、「σ_{ｅｋ，ｎｅｗ}」及び「σ_{ｂｙｋ，ｎｅｗ}」は、補正後の距離精度、仰角精度及び方位角精度である。

ステップＳＴ６−２では、上記（１１−１）式に従って、観測誤差共分散行列Ｒ_ｋにおける各要素の中で誤差の広がりが最も小さい要素が、補正刻み幅εだけ誤差共分散を大きくする処理が実施される。

ステップＳＴ６−２の補正処理により、上記（２）式に示される観測誤差共分散行列Ｒ_ｋは、以下の（１２）式のように変更される。

上記（１２）式に示される「Ｒ_{ｋ，ｎｅｗ}」は、補正処理により更新された観測時刻ｔ_ｋにおける観測誤差共分散行列である。

図７には、補正前の観測誤差共分散行列Ｒ_ｋが破線で示され、補正後の観測誤差共分散行列Ｒ_{ｋ，ｎｅｗ}が太破線で示されている。図２と図７とを比較すれば、補正後の観測誤差共分散行列Ｒ_{ｋ，ｎｅｗ}は、距離方向に膨らんでおり、距離方向に補正されていることが理解できる。距離方向は、ステップＳＴ６−２の処理で言うところの「観測誤差共分散の広がりが最も小さい要素」である。

また、図７には、センサ間にバイアス誤差が存在しない場合の平滑値が細字のプラス記号で示され、センサ間にバイアス誤差が存在する場合の補正前の平滑値が中太字のプラス記号で示され、センサ間にバイアス誤差が存在する場合の補正後の平滑値が太字のプラス記号で示されている。ステップＳＴ６−２による補正処理によって、中太字のプラス記号で示される補正前の平滑値は、太字のプラス記号で示される補正後の平滑値に変更される。これにより、補正後の平滑値は、センサ間にバイアス誤差が存在しない場合の平滑値に近い位置となる。すなわち、図６に示す補正処理によって、平滑値がバイアス誤差の無い条件の平滑値に近い値となるように補正されるので、平滑値の推定精度を高めることができる。

なお、観測誤差共分散行列Ｒ_ｋを広げる補正刻み幅は、固定値として設定してもよいし、各センサの配置状況、各センサの観測精度、又は目標との相対距離に応じてテーブルとして設定してもよい。また、当該補正刻み幅は、観測誤差共分散行列の広がりの大きさに対して、固定の上限値を設定してもよいし、各センサの配置状況、各センサの観測精度、又は目標との相対距離に応じてテーブルとして設定してもよい。

以上のように、実施の形態に係る追尾処理装置及び追尾処理方法は、観測情報、平滑値及びカルマンゲイン行列を基に、観測誤差共分散行列を補正し、補正後の観測誤差共分散行列を基に、平滑値及びカルマンゲイン行列の再演算を行う。これにより、平滑値の推定精度を高めることができ、バイアス誤差の推定に伴って生じ得る追尾処理の遅延及び追尾精度の低下を抑制することが可能となる。

次に、実施の形態に係る追尾処理方法について説明する。実施の形態に係る追尾処理装置を用いて行う追尾処理方法の特徴は、以下の通りである。

（１）第１のステップでは、観測情報を基に、観測誤差共分散行列が演算される。
（２）第２のステップでは、前回の観測時刻における平滑値と、今回の観測時刻における観測情報と、前回の観測時刻から今回の観測時刻までの経過時間とを基に、目標運動諸元の予測値が演算される。
（３）第３のステップでは、観測誤差共分散行列と、予測値とを基に、平滑値及びカルマンゲイン行列が演算される。
（４）第４のステップでは、観測情報、平滑値及びカルマンゲイン行列を基に、観測誤差共分散行列が補正される。
（５）第５のステップでは、補正後の観測誤差共分散行列を基に、平滑値及びカルマンゲイン行列の再演算が行われる。

上記したように、第４のステップでは、観測情報、平滑値及びカルマンゲイン行列を基に観測誤差共分散行列が補正される。また、第５のステップでは、補正後の観測誤差共分散行列を基に平滑値及びカルマンゲイン行列の再演算が行われる。これにより、平滑値の推定精度を高めることができ、バイアス誤差の推定に伴って生じ得る追尾処理の遅延及び追尾精度の低下を抑制することが可能となる。

最後に、実施の形態における追尾処理装置１００の機能を実現するためのハードウェア構成について、図８及び図９の図面を参照して説明する。図８は、実施の形態の追尾処理装置１００におけるハードウェア構成の一例を示すブロック図である。図９は、実施の形態の追尾処理装置１００におけるハードウェア構成の他の例を示すブロック図である。

実施の形態における追尾処理装置１００の機能を実現する場合には、図８に示すように、演算を行うプロセッサ２００、プロセッサ２００によって読みとられるプログラムが保存されるメモリ２０２、及び信号の入出力を行うインタフェース２０４を含む構成とすることができる。

プロセッサ２００は、演算装置、マイクロプロセッサ、マイクロコンピュータ、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）、又はＤＳＰ（Ｄｉｇｉｔａｌ Ｓｉｇｎａｌ Ｐｒｏｃｅｓｓｏｒ）といった演算手段であってもよい。また、メモリ２０２には、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、フラッシュメモリ、ＥＰＲＯＭ（Ｅｒａｓａｂｌｅ Ｐｒｏｇｒａｍｍａｂｌｅ ＲＯＭ）、ＥＥＰＲＯＭ（Ｅｌｅｃｔｒｉｃａｌｌｙ ＥＰＲＯＭ）といった不揮発性又は揮発性の半導体メモリ、磁気ディスク、フレキシブルディスク、光ディスク、コンパクトディスク、ミニディスク、ＤＶＤ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｃ）を例示することができる。

メモリ２０２には、追尾処理装置１００の機能を実行するプログラム及びプロセッサ２００によって参照されるテーブルが格納されている。プロセッサ２００は、インタフェース２０４を介して必要な情報を授受し、メモリ２０２に格納されたプログラムをプロセッサ２００が実行し、メモリ２０２に格納されたテーブルをプロセッサ２００が参照することにより、上述した第１の演算部２０、第２の演算部３０、第３の演算部４０及び補正処理部５０の演算処理を行うことができる。プロセッサ２００による演算結果は、インタフェース２０４を介して表示装置１１０に出力することができる。プロセッサ２００による演算結果は、表示装置１１０への出力と共に、メモリ２０２に記憶してもよい。

図８に示すプロセッサ２００及びメモリ２０２は、図９のように処理回路２０３に置き換えてもよい。処理回路２０３は、単一回路、複合回路、ＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ Ｓｐｅｃｉｆｉｃ Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ）、ＦＰＧＡ（Ｆｉｅｌｄ−Ｐｒｏｇｒａｍｍａｂｌｅ Ｇａｔｅ Ａｒｒａｙ）、又は、これらを組み合わせたものが該当する。なお、第１の演算部２０、第２の演算部３０、第３の演算部４０及び補正処理部５０における一部の処理を処理回路２０３で実施し、処理回路２０３で実施しない処理をプロセッサ２００及びメモリ２０２で実施してもよい。

なお、以上の実施の形態に示した構成は、本発明の内容の一例を示すものであり、別の公知の技術と組み合わせることも可能であるし、本発明の要旨を逸脱しない範囲で、構成の一部を省略、変更することも可能である。

１０ 第１のセンサ、１０ａ，１１ａ 観測値、１０ｂ，１１ｂ 観測時刻、１１ 第２のセンサ、２０ 第１の演算部、２０ａ，５０ｃ 観測誤差共分散行列、３０ 第２の演算部、３０ａ 予測値、３０ｂ 予測誤差共分散行列、４０ 第３の演算部、４０ａ，５０ａ 平滑値、４０ｂ，５０ｂ 平滑誤差共分散行列、４０ｃ カルマンゲイン行列、５０ 補正処理部、８０ 目標、１００ 追尾処理装置、１１０ 表示装置、２００ プロセッサ、２０２ メモリ、２０３ 処理回路、２０４ インタフェース。

Claims (6)

1. 複数のセンサからの観測情報を基に、目標の追尾処理を行う追尾処理装置であって、
   前記観測情報を基に、観測誤差共分散行列を演算する第１の演算部と、
   前回の観測時刻における平滑値と、前回の観測時刻における平滑誤差共分散行列と、今回の観測時刻における前記観測情報と、前回の観測時刻から今回の観測時刻までの経過時間とを基に、目標運動諸元の予測値を演算する第２の演算部と、
   前記観測情報と、前記観測誤差共分散行列と、前記予測値と、予測誤差共分散行列とを基に、前記平滑値と前記平滑誤差共分散行列とカルマンゲイン行列とを演算する第３の演算部と、
   前記観測情報と、前記平滑値と、前記予測値と、前記カルマンゲイン行列とを基に、前記観測誤差共分散行列を補正して前記第３の演算部にフィードバックする補正処理部と、
   備えたことを特徴とする追尾処理装置。
2. 前記観測情報には、前記目標の運動諸元に関する観測値と、前記運動諸元を観測したときの観測時刻とが含まれることを特徴とする請求項１に記載の追尾処理装置。
3. 前記補正処理部は、前記カルマンゲイン行列の各要素の中で、最小値をとる要素が第１の値未満であるか、もしくは、最大値をとる要素が前記第１の値よりも大きな第２の値を超える場合に、前記観測誤差共分散行列の補正を行うことを特徴とする請求項１又は２に記載の追尾処理装置。
4. 前記第１の値は０であり、前記第２の値は１であることを特徴とする請求項３に記載の追尾処理装置。
5. 前記補正処理部は、前記観測誤差共分散行列を補正する際に、前記観測誤差共分散行列における各要素の中で誤差の広がりが最も小さい要素を補正刻み幅だけ補正することを特徴とする請求項１から４の何れか１項に記載の追尾処理装置。
6. 複数のセンサからの観測情報を基に、目標の追尾処理を行う追尾処理方法であって、
   前記観測情報を基に、観測誤差共分散行列を演算する第１のステップと、
   前回の観測時刻における平滑値と、今回の観測時刻における前記観測情報と、前回の観測時刻から今回の観測時刻までの経過時間とを基に、目標運動諸元の予測値を演算する第２のステップと、
   前記観測情報と、前記観測誤差共分散行列と、前記予測値と、予測誤差共分散行列とを基に、前記平滑値及びカルマンゲイン行列を演算する第３のステップと、
   前記観測情報、前記平滑値、前記予測値及び前記カルマンゲイン行列を基に、前記観測誤差共分散行列を補正する第４のステップと、
   補正後の前記観測誤差共分散行列を基に、前記平滑値及び前記カルマンゲイン行列の再演算を行う第５のステップと、
   を含むことを特徴とする追尾処理方法。

Images