特許 6986980 追尾処理装置及び追尾処理方法

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】6986980

(24)【登録日】2021年12月2日

(45)【発行日】2021年12月22日

(54)【発明の名称】追尾処理装置及び追尾処理方法

(51)【国際特許分類】

   G01S 13/66 20060101AFI20211213BHJP

   G01S 7/40 20060101ALI20211213BHJP

【ＦＩ】

   G01S13/66

   G01S7/40 126

   G01S7/40 121

【請求項の数】8

【全頁数】18

(21)【出願番号】特願2018-6677(P2018-6677)

(22)【出願日】2018年1月18日

(65)【公開番号】特開2019-124637(P2019-124637A)

(43)【公開日】2019年7月25日

【審査請求日】2020年5月20日

(73)【特許権者】

【識別番号】000006013

【氏名又は名称】三菱電機株式会社

(74)【代理人】

【識別番号】100118762

【弁理士】

【氏名又は名称】高村 順

(72)【発明者】

【氏名】鈴木 秀治

【審査官】 佐藤 宙子

(56)【参考文献】

【文献】 特開２００２−１７４６８１（ＪＰ，Ａ）

【文献】 特開２０１２−０４７４９０（ＪＰ，Ａ）

【文献】 特開２０１５−０８７３５４（ＪＰ，Ａ）

【文献】 特開２０１１−６４４８４（ＪＰ，Ａ）

【文献】 国際公開第２０１７／１３８１１９（ＷＯ，Ａ１）

【文献】 特開２０１５−１９７４０３（ＪＰ，Ａ）

【文献】 特開２０１１−００２２６６（ＪＰ，Ａ）

【文献】 特開２０１０−２８１７８２（ＪＰ，Ａ）

【文献】 特表２００８−５３４９８３（ＪＰ，Ａ）

【文献】 米国特許第６３５９５８６（ＵＳ，Ｂ１）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｇ０１Ｓ ７／００− ７／６４

Ｇ０１Ｓ １３／００−１７／９５

(57)【特許請求の範囲】

【請求項1】

複数のセンサからの観測情報を基に、目標の追尾処理を行う追尾処理装置であって、
前記観測情報を基に、観測誤差共分散行列を演算する第１の演算部と、
前回の観測時刻における平滑値と、前回の観測時刻における平滑誤差共分散行列と、今回の観測時刻における前記観測情報と、前回の観測時刻から今回の観測時刻までの経過時間とを基に、目標運動諸元の予測値と、今回の観測時刻における予測誤差共分散行列とを演算する第２の演算部と、
前記観測情報と、前記予測値とを基に、前記観測誤差共分散行列及び前記予測誤差共分散行列のうちの何れかを補正する補正処理部と、
前記観測情報、前記平滑値、補正された前記観測誤差共分散行列、及び補正されていない前記予測誤差共分散行列、又は、前記観測情報、前記平滑値、補正されていない前記観測誤差共分散行列、及び補正された前記予測誤差共分散行列に基づいて、前記平滑値及び前記平滑誤差共分散行列を演算する第３の演算部と、
を備え、
前記補正処理部は、複数のセンサ間にバイアス誤差が存在するか否かの判定結果と、前記バイアス誤差の要因が時刻であるか否かの時刻同期誤差の有無の判定結果とに基づいて、前記観測誤差共分散行列及び前記予測誤差共分散行列を補正するか否かを決定することを特徴とする追尾処理装置。

【請求項2】

前記観測情報には、前記目標の運動諸元に関する観測値と、前記運動諸元を観測したときの観測時刻とが含まれることを特徴とする請求項１に記載の追尾処理装置。

【請求項3】

前記補正処理部は、複数のセンサ間に前記時刻同期誤差が存在すると判定した場合には、前記予測誤差共分散行列を補正することを特徴とする請求項１又は２に記載の追尾処理装置。

【請求項4】

前記補正処理部は、前記予測誤差共分散行列を補正する際に、前記予測誤差共分散行列における各要素の中で誤差の広がりが最も小さい要素を補正することを特徴とする請求項３に記載の追尾処理装置。

【請求項5】

前記補正処理部は、複数のセンサ間に前記時刻同期誤差が存在しないと判定した場合には、前記観測誤差共分散行列を補正することを特徴とする請求項１又は２に記載の追尾処理装置。

【請求項6】

前記補正処理部は、前記観測誤差共分散行列を補正する際に、前記観測誤差共分散行列における各要素の中で誤差の広がりが最も小さい要素を補正することを特徴とする請求項５に記載の追尾処理装置。

【請求項7】

複数のセンサからの観測情報を基に、目標の追尾処理を行う追尾処理方法であって、
前記観測情報を基に、観測誤差共分散行列を演算する第１のステップと、
前回の観測時刻における平滑値と、今回の観測時刻における前記観測情報と、前回の観測時刻から今回の観測時刻までの経過時間とを基に、目標運動諸元の予測値と、今回の観測時刻における予測誤差共分散行列とを演算する第２のステップと、
前記観測情報と、前記予測値とを基に、前記観測誤差共分散行列を補正する第３のステップと、
前記観測情報と、前記予測値と、前記予測誤差共分散行列と、補正後の前記観測誤差共分散行列とを基に、前記平滑値を演算する第４のステップと、
を含み、
前記第３のステップでは、複数のセンサ間にバイアス誤差が存在するか否かの判定結果と、前記バイアス誤差の要因が時刻であるか否かの判定結果とに基づいて、前記観測誤差共分散行列を補正するか否かが決定される
ことを特徴とする追尾処理方法。

【請求項8】

複数のセンサからの観測情報を基に、目標の追尾処理を行う追尾処理方法であって、
前記観測情報を基に、観測誤差共分散行列を演算する第１のステップと、
前回の観測時刻における平滑値と、今回の観測時刻における前記観測情報と、前回の観測時刻から今回の観測時刻までの経過時間とを基に、目標運動諸元の予測値と、今回の観測時刻における予測誤差共分散行列とを演算する第２のステップと、
前記観測情報と、前記予測値とを基に、前記予測誤差共分散行列を補正する第３のステップと、
前記観測情報と、前記予測値と、前記観測誤差共分散行列と、補正後の前記予測誤差共分散行列とを基に、前記平滑値を演算する第４のステップと、
を含み、
前記第３のステップでは、複数のセンサ間にバイアス誤差が存在するか否かの判定結果と、前記バイアス誤差の要因が時刻であるか否かの判定結果とに基づいて、前記予測誤差共分散行列を補正するか否かが決定される
ことを特徴とする追尾処理方法。

【発明の詳細な説明】

【技術分野】

【0001】

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

【背景技術】

【0002】

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

【0003】

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

【先行技術文献】

【特許文献】

【0004】

【特許文献1】特開２０１１−６４４８４号公報

【特許文献2】特開２０１２−４７４９０号公報

【発明の概要】

【発明が解決しようとする課題】

【0005】

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

【0006】

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

【課題を解決するための手段】

【0007】

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

【発明の効果】

【0008】

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

【図面の簡単な説明】

【0009】

【図1】実施の形態に係る追尾処理装置の構成を示すブロック図

【図2】実施の形態における第１の演算部の動作説明に供する図

【図3】実施の形態における第２の演算部の動作説明に供する図

【図4】実施の形態における第３の演算部の動作説明に供する第１の図

【図5】実施の形態における第３の演算部の動作説明に供する第２の図

【図6】実施の形態に係る追尾処理装置における要部の処理の流れを示すフローチャート

【図7】実施の形態の補正処理部における補正処理の詳細説明に供する第１の図

【図8】実施の形態の補正処理部における補正処理の詳細説明に供する第２の図

【図9】実施の形態の追尾処理装置におけるハードウェア構成の一例を示すブロック図

【図10】実施の形態の追尾処理装置におけるハードウェア構成の他の例を示すブロック図

【発明を実施するための形態】

【0010】

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

【0011】

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

【0012】

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

【0013】

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

【0014】

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

【0015】

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

【0016】

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

【0017】

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

【0018】

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

【0019】

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

【0020】

補正処理部４０には、観測値１０ａ及び観測値１１ａと、第１の演算部２０が演算した観測誤差共分散行列２０ａと、第２の演算部３０が演算した予測値３０ａ及び予測誤差共分散行列３０ｂとが入力される。補正処理部４０による補正処理は、観測値１０ａ、観測値１１ａ、観測誤差共分散行列２０ａ、予測値３０ａ及び予測誤差共分散行列３０ｂに関する各情報を基に行われる。当該補正処理では、第１の演算部２０が演算した観測誤差共分散行列２０ａと、第２の演算部３０が演算した予測誤差共分散行列３０ｂに対する補正処理の要否が判定される。

【0021】

当該補正処理が必要と判定された場合、観測誤差共分散行列２０ａ、または、予測誤差共分散行列３０ｂに対する補正処理が実施される。一方、当該補正処理が不要と判定された場合、補正処理は実施されずに、第１の演算部２０により演算された観測誤差共分散行列２０ａと、第２の演算部３０により演算された予測誤差共分散行列３０ｂが、そのまま出力される。補正処理の詳細については後述する。

【0022】

第３の演算部５０には、観測値１０ａ及び観測値１１ａと、第２の演算部３０が演算した予測値３０ａと、補正処理部４０が演算した補正処理後の観測誤差共分散行列４０ａもしくは補正処理前の観測誤差共分散行列４０ａと、補正処理後の予測誤差共分散行列４０ｂもしくは補正処理前の予測誤差共分散行列４０ｂとが入力される。第３の演算部５０は、これらの入力情報を基に、平滑値５０ａ及び平滑誤差共分散行列５０ｂを演算し、表示装置１１０に出力する。

【0023】

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

【0024】

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

【0025】

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

【0026】

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

【0027】

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

【0028】

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

【0029】

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

【0030】

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

【0031】

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

【0032】

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

【0033】

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

【0034】

【数1】

【0035】

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

【0036】

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

【0037】

【数2】

【0038】

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

【0039】

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

【0040】

【数3】

【0041】

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

【0042】

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

【0043】

【数4】

【0044】

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

【0045】

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

【0046】

【数5】

【0047】

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

【0048】

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

【0049】

【数6】

【0050】

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

【0051】

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

【0052】

【数7】

【0053】

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

【0054】

【数8】

【0055】

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

【0056】

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

【0057】

【数9】

【0058】

補正処理部４０の処理の流れは図６に示す通りである。図６に示す処理は、補正処理部４０によって実施される。ステップＳＴ６−１では、第１のセンサ１０と第２のセンサ１１との間にバイアス誤差が存在するか否かが判定される。前述の通り、バイアス誤差とは、異なるセンサ間の観測値に存在する誤差である。

【0059】

前述した図５の例に示すように、観測値のズレが大きい場合、第１のセンサ１０と第２のセンサ１１との間に、バイアス誤差が存在すると判定される（ステップＳＴ６−１，Ｙｅｓ）。この場合、ステップＳＴ６−２に進む。観測値のズレが大きいか否かは、閾値で判定することができる。

【0060】

一方、ステップＳＴ６−１において、バイアス誤差が存在しないと判定された場合（ステップＳＴ６−１，Ｎｏ）、図６に示す補正処理のフローを終了する。

【0061】

ステップＳＴ６−２では、バイアス誤差の発生要因が時刻であるか否かが判定される。バイアス誤差の発生要因が時刻である場合、第１のセンサ１０の観測情報を基に演算される予測値と、第２のセンサ１１の観測情報を基に演算される予測値との間に、前述した時刻同期誤差が表れる。予測値のズレが大きいか否かは、閾値で判定することができる。

【0062】

第１のセンサ１０と第２のセンサ１１との間に時刻同期誤差があると判定された場合（ステップＳＴ６−２，Ｙｅｓ）、ステップＳＴ６−３に進む。

【0063】

ステップＳＴ６−３では、予測誤差共分散行列が補正される。より詳細には、予測誤差共分散行列の位置に関する広がりが最も小さい要素のみが補正される。ステップＳＴ６−３の処理を終えると、図６のフローを抜ける。

【0064】

予測誤差共分散行列の補正式は、以下の（１０−１）式、（１０−２）式、（１０−３）式、（１０−４）式及び（１０−５）式で表すことができる。

【0065】

【数10】

【0066】

上記（１０−２）式において、「Ｄ_ｋ」は、「（Ｈ_ｋ・Ｐ_{ｋ｜ｋ−１}・Ｈ_ｋ^Ｔ）^−１」の固有値を対角要素に持つ対角行列である。「Ｈ_ｋ・Ｐ_{ｋ｜ｋ−１}・Ｈ_ｋ^Ｔ」は、予測誤差共分散行列の位置に関する成分から成る行列である。

【0067】

また、上記（１０−２）式の右辺に現れる「Ｖ_ｋ」は、対角行列Ｄ_ｋに対応する右固有ベクトルを列にもつ行列である。（１０−２）式に示されるように、予測誤差共分散行列Ｐ_{ｋ｜ｋ−１}の位置に関する成分から成る行列「Ｈ_ｋ・Ｐ_{ｋ｜ｋ−１}・Ｈ_ｋ^Ｔ」は、右固有ベクトルを列にもつ行列Ｖ_ｋによって、対角化される。

【0068】

上記（１０−１）式には、時刻Δ_ｉの補正式が示されている。添字「ｉ」は、ｘ，ｙ，ｚのうちの何れか１つを表すという意味で使用される。すなわち、Δ_ｉは、Δ_ｘ，Δ_ｙ，Δ_ｚのうちの何れか１つを表す。δ_ｉについても同様である。

【0069】

上記（１０−３）式に示されるように、対角行列Ｄ_ｋにおける対角要素ｄ_ｘ，ｄ_ｙ，ｄ_ｚのうちの最大要素に対して、δ_ｉが“１”に設定され、最大要素以外の他の２つの要素は“０”に設定される。なお、対角行列Ｄ_ｋにおいて、対角要素ｄ_ｘ，ｄ_ｙ，ｄ_ｚのうちの最大要素は、誤差の広がりが最小の要素を意味する。これにより、対角要素ｄ_ｘ，ｄ_ｙ，ｄ_ｚのうちの最大要素のみが、（１０−１）式を用いて補正される。また、（１０−４）式によって、対角行列Ｄ_ｋが補正される。そして、補正後の対角行列Ｄ_{ｋ，ｎｅｗ}を（１０−５）式に代入して演算することにより、補正後の予測誤差共分散行列Ｐ_{ｋ｜ｋ−１，ｎｅｗ}が生成される。

【0070】

なお、（１０−１）式において、「ｍ」は予測誤差共分散行列の補正量を決定する補正係数である。ここで、ｍ＞１の場合、観測値と予測値とを結ぶ直線方向において、観測誤差共分散行列と予測誤差共分散行列との間で、重なる範囲がある状態を意味している。

【0071】

図７には、補正前の予測誤差共分散行列が破線で示され、補正後の予測誤差共分散行列が太破線で示されている。図７に示すように、補正後の予測誤差共分散行列は、（１０−１）式で表される補正量によって、ある方向に膨らむように制御される。このときに制御される方向が、「対角要素ｄ_ｘ，ｄ_ｙ，ｄ_ｚのうちの最大要素」に対応する方向である。

【0072】

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

【0073】

ステップＳＴ６−２に戻り、第１のセンサ１０と第２のセンサ１１との間に時刻同期誤差がないと判定された場合（ステップＳＴ６−２，Ｎｏ）、ステップＳＴ６−４に進む。ステップＳＴ６−４に進む場合は、第１のセンサ１０と第２のセンサ１１との間において、バイアス誤差があり、且つ時刻同期誤差がない場合である。すなわち、ステップＳＴ６−４は、時刻同期誤差以外のバイアス誤差である、センサ間の位置誤差及びセンサ間の角度誤差により、観測値にズレが生じた場合の補正処理である。

【0074】

ステップＳＴ６−４では、観測誤差共分散行列が補正される。より詳細には、観測誤差共分散行列の位置及び角度に関する広がりが最も小さい要素のみが補正される。ステップＳＴ６−４の処理を終えると、図６のフローを抜ける。

【0075】

なお、図６のフローでは、バイアス誤差の発生要因が時刻であるか否かを判定するステップＳＴ６−２が設けられているが、このステップＳＴ６−２の判定処理は省略してもよい。ステップＳＴ６−２の判定処理を省略した場合、予測誤差共分散行列及び観測誤差共分散行列のうちの何れの誤差共分散行列を補正するか、予め決めておけばよい。予測誤差共分散行列及び観測誤差共分散行列のうちの何れかの誤差共分散行列の補正を実施した場合、平滑値の精度は、補正前よりも改善されるため、ステップＳＴ６−１の判定処理をクリアすることが可能となる。なお、一般的に、バイアス誤差による平滑値への影響が大きいのは方位角成分である。このため、何れか一方のみを補正するのであれば、観測誤差共分散行列を補正する方が平滑値の精度を改善するのに適している。

【0076】

観測誤差共分散行列の補正式は、以下の（１１−１）式、（１１−２）式、及び（１１−３）式で表すことができる。

【0077】

【数11】

【0078】

上記（１１−１）式には、観測誤差共分散行列Ｒ_ｋにおける補正後の各要素であるσ_{ｊ，ｎｅｗ}の補正式が示されている。添字「ｊ」は、「ｒｋ」，「ｅｋ」，「ｂｙｋ」のうちの何れか１つを表すという意味で使用される。上記（１１−２）式に示されるδ_ｊについても同様である。すなわち、σ_{ｊ，ｎｅｗ}は、σ_{ｒｋ，ｎｅｗ}，σ_{ｅｋ，ｎｅｗ}，σ_{ｂｙｋ，ｎｅｗ}のうちの何れか１つを表す。

【0079】

（１１−２）式に示されるように、観測誤差共分散行列Ｒ_ｋにおける補正前の各要素であるσ_ｒｋ，σ_ｅｋ，σ_ｂｙｋのうちの最小要素に対して、δ_ｊが“１”に設定され、最小要素以外の他の２つの要素は“０”に設定される。これらの値が（１１−１）式に代入され、（１１−１）式に従って補正量が演算される。また、（１１−３）式によって、補正後の観測誤差共分散行列Ｒ_{ｋ，ｎｅｗ}が演算される。

【0080】

なお、（１１−１）式において、「ｎ」は観測誤差共分散行列の補正量を決定する補正係数である。ここで、ｎ＞１の場合、観測値と予測値を結ぶ直線方向において、観測誤差共分散行列と予測誤差共分散行列が重なる範囲がある状態を意味している。

【0081】

図８には、補正前の観測誤差共分散行列が破線で示され、補正後の観測誤差共分散行列が太破線で示されている。図８に示すように、補正後の観測誤差共分散行列は、（１１−１）式で表される補正量によって、ある方向に膨らむように制御される。このときに制御される方向が、「σ_ｒｋ，σ_ｅｋ，σ_ｂｙｋのうちの最小要素」に対応する方向である。

【0082】

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

【0083】

以上のように、補正処理部４０は、第１のセンサ１０と第２のセンサ１１との間にバイアス誤差が存在すると判定された場合に、観測誤差共分散行列及び予測誤差共分散行列のうちの何れかを補正し、補正後の観測誤差共分散行列又は補正後の予測誤差共分散行列を第３の演算部５０に出力する。第３の演算部５０は、補正後の観測誤差共分散行列又は補正後の予測誤差共分散行列を基に、平滑値５０ａ及び平滑誤差共分散行列５０ｂの演算を行う。これにより、バイアス誤差の推定に伴って生じ得る追尾処理の遅延及び追尾精度の低下を抑制することができる。

【0084】

また、補正処理部４０は、バイアス誤差の要因が時刻であるか否かの判定処理によって、バイアス誤差の要因の切り分けを行い、切り分けの結果に基づいて、観測誤差共分散行列及び予測誤差共分散行列のうちの何れかを補正する。これにより、精度を一時的に下げる制御が、観測誤差共分散行列及び予測誤差共分散行列のうちの何れか一方に限定されるので、補正処理によって平滑値の推定精度が悪化することを防止することができる。また、観測誤差共分散行列及び予測誤差共分散行列のうちの何れかを補正するので、平滑値５０ａ及び平滑誤差共分散行列５０ｂの出力が遅延するのを抑制することができる。

【0085】

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

【0086】

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

【0087】

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

【0088】

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

【0089】

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

【0090】

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

【0091】

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

【0092】

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

【0093】

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

【符号の説明】

【0094】

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

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】