JP6786313B2 - バイアス誤差推定装置、バイアス誤差推定システム、バイアス誤差推定方法及びバイアス誤差推定プログラム - Google Patents

Description

本実施形態は、目標を観測する複数のセンサがネットワークに接続されたセンサネットワークにおいて、各センサのバイアス誤差を推定するバイアス誤差推定装置、バイアス誤差推定システム、バイアス誤差推定方法及びバイアス誤差推定プログラムに関する。

それぞれ目標を観測する複数のセンサがネットワークに接続されたセンサネットワークでは、各センサで得られた観測データを融合するために観測目標の相関処理や統合処理を行う。ところが、各センサにはバイアス誤差があり、このバイアス誤差が観測している目標の相関処理や統合処理の精度に影響を与える。この影響を軽減するため、従来から、センサのバイアス誤差を推定し、補正する方法が提案されている。例えば、非特許文献１には、センサからの観測データを一箇所に集めて、カルマンフィルタによりデータの融合とバイアス誤差の推定を同時に実施する方法が示されている。また、特許文献１には、センサからの観測データと航跡データを一箇所に集めて、カルマンフィルタおよび最小２乗フィルタによりデータの融合とバイアス誤差の推定を同時に実施するセンサバイアス推定装置が示されている。

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

B.A. van Doorn, et al.，"Systematic Error Estimation in Multisensor Fusion Systems"，SPIE，1993 鏡慎吾，石川正俊，"センサフュージョン − センサネットワークの情報処理構造 − "，電子情報通信学会論文誌 A，2005

以上のように、非特許文献１に示されている方法では、目標を観測する複数のセンサからの観測データを一箇所に集めてデータ融合を行う集中化された（centralized）センサネットワーク（非特許文献２参照）において、センサのバイアス誤差を推定し、補正することはできる。しかし、この方法は、センサ毎に目標の追跡処理を行い、それぞれのセンサの観測データや航跡データを一箇所に集めることなく、分散化された（decentralized）形態でデータ融合を実施するセンサネットワーク（非特許文献２参照）には適用できないという課題がある。

本実施形態は上記課題に鑑みなされたもので、目標を観測する複数センサからなり、それぞれのセンサが目標の追跡処理を行い、それぞれのセンサの観測データや航跡データを一箇所に集めることなく、分散化されたセンサネットワークにおいて、センサそれぞれのバイアス誤差を推定することのできるバイアス誤差推定装置、バイアス誤差推定システム、バイアス誤差推定方法及びバイアス誤差推定プログラムを提供することを目的とする。

実施形態によれば、センサネットワークを形成して目標を観測する複数のセンサそれぞれのバイアス誤差を推定するバイアス誤差推定装置は、相互作用値算出手段と、バイアス誤差算出手段と、目標状態値算出手段とを具備する。前記相互作用値算出手段は、前記複数のセンサのうちの誤差推定対象センサの１つ前の時刻ｔ_ｋ−１におけるバイアス誤差を考慮したグローバル座標系の目標状態値と、前記複数のセンサのうちの同一目標を観測する誤差推定対象ではないセンサの１つ前の時刻ｔ_ｋ−１におけるバイアス誤差を考慮したグローバル座標系の目標状態値とを重み付き加算して、前記誤差推定対象センサの時刻ｔ_ｋにおけるグローバル座標系の相互作用値を算出する。前記バイアス誤差算出手段は、前記誤差推定対象センサの１つ前の時刻ｔ_ｋ−１におけるグローバル座標系の目標状態値と、前記相互作用値算出手段によって算出される前記誤差推定対象センサの時刻ｔ_ｋにおけるグローバル座標系の相互作用値とに基づいて、前記誤差推定対象センサの時刻ｔ_ｋにおけるバイアス誤差を算出する。前記目標状態値算出手段は、前記誤差推定対象センサの時刻ｔ_ｋにおけるグローバル座標系の目標状態値と、予め前記センサ毎に設定されるバイアス誤差の初期値または前記バイアス誤差算出手段によって算出される前記誤差推定対象センサの時刻ｔ_ｋにおけるバイアス誤差とに基づいて、前記誤差推定対象センサが観測している目標の時刻ｔ_ｋにおけるバイアス誤差を考慮したグローバル座標系の目標状態値を算出する。

本実施形態に係るバイアス誤差推定装置が適用されるセンサネットワーク全体の構成を示すブロック図。 本実施形態に係るバイアス誤差推定装置の構成を示すブロック図。 第１の実施例に係るバイアス誤差推定装置の処理の具体例を示すフローチャート。 第３の実施例に係るバイアス誤差推定装置が適用されるセンサネットワーク全体の構成を示すブロック図。 第３の実施例に係るバイアス誤差推定装置の処理の具体例を示すタイミングチャート。

以下、本発明に係る実施の形態について、図面を参照して説明する。

以下、図１と図２を参照して、本実施形態について説明する。

図１は、本実施形態に係るバイアス誤差推定装置が適用されるセンサネットワーク全体の構成を示すブロック図である。このセンサネットワークは、それぞれ目標を観測するＮ（図ではＮ＝４）個のセンサ１_１〜１_Ｎ、それぞれセンサ１_１〜１_Ｎに対応するデータ融合装置２_１〜２_Ｎ、それぞれのデータ融合装置に対応するバイアス誤差推定装置３_１〜３_Ｎから構成される。なお、本実施形態において、センサ１_１〜１_Ｎ、データ融合装置２_１〜２_Ｎは、バイアス誤差推定装置３_１〜３_Ｎの外部装置である。データ融合装置２_１〜２_Ｎは、相互に接続され、相互にデータを送受してデータの融合を行う。

本実施形態において、２つ以上のバイアス誤差推定装置を備える場合、バイアス誤差推定システムと称する。

図２は、本実施形態に係るバイアス誤差推定装置３_ｉ（ｉ∈｛１，．．．，Ｎ｝）の具体的な構成を示すブロック図である。このバイアス誤差推定装置３_ｉは、相互作用値算出部３０１、バイアス誤差算出部３０２、目標状態値算出部３０３を備える。

相互作用値算出部３０１は、１つ前の時刻ｔ_ｋ−１において、それぞれのバイアス誤差推定装置３_ｉがバイアス誤差推定の対象とするセンサ（以下、誤差推定対象センサ、又は自センサと呼ぶ）１_ｉが観測している目標のバイアス誤差を考慮したグローバル座標系の目標状態値（少なくとも位置を含み、位置の時間微分成分である速度等を含んでも良い）と、外部装置から入力される１つ前の時刻ｔ_ｋ−１における他センサ（誤差推定対象ではないセンサ）１_ｊ（ｊ∈｛１，．．．，Ｎ｝，ｊ≠ｉ）が観測している目標のバイアス誤差を考慮したグローバル座標系の目標状態値（少なくとも位置を含み、位置の時間微分成分である速度等を含んでも良い）とを重み付き加算して、時刻ｔ_ｋにおける自センサが観測している目標のグローバル座標系の相互作用値を算出する。

バイアス誤差算出部３０２は、外部装置から入力される１つ前の時刻ｔ_ｋ−１における自センサ１_ｉが観測している目標のローカル座標系の目標状態値（少なくとも位置を含み、位置の時間微分成分である速度等を含んでも良い）と、時刻ｔ_ｋ−１における自センサ１_ｉのグローバル座標系のセンサ状態値（少なくとも位置を含み、位置の時間微分成分である速度等を含んでも良い）と、相互作用値算出部３０１で算出した時刻ｔ_ｋにおける自センサ１_ｉが観測している目標のグローバル座標系の相互作用値とに基づいて、時刻ｔ_ｋにおける自センサ１_ｉのバイアス誤差を算出する。

目標状態値算出部３０３は、外部装置から入力される時刻ｔ_ｋにおける自センサ１_ｉが観測している目標のローカル座標系の目標状態値（少なくとも位置を含み、位置の時間微分成分である速度等を含んでも良い）と、時刻ｔ_ｋにおける自センサ１_ｉのグローバル座標系のセンサ状態値（少なくとも位置を含み、位置の時間微分成分である速度等を含んでも良い）と、バイアス誤差算出部３０２で算出した時刻ｔ_ｋにおける自センサ１_ｉのバイアス誤差とに基づいて、時刻ｔ_ｋにおける自センサ１_ｉが観測している目標のバイアス誤差を考慮したグローバル座標系の目標状態値（少なくとも位置を含み、位置の時間微分成分である速度等を含んでも良い）を算出する。このバイアス誤差を考慮したグローバル座標系の目標状態値は、バイアス誤差を補正（但し、真値補正のほか相対値補正を含む）したグローバル座標系の航跡データに相当する。上記相互作用値算出部３０１、バイアス誤差算出部３０２、目標状態値算出部３０３の処理は、繰り返し実行される。

上記構成によるバイアス誤差推定装置３_ｉの処理動作について、Ｎ個のセンサ１_１，．．．，１_Ｎから構成されるセンサネットワークに適用された場合に、Ｍ個の目標Ｔ_１，．．．，Ｔ_Ｍを観測している状況に想定して説明する。

自センサ１_ｉに対応するバイアス誤差推定装置３_ｉの相互作用値算出部３０１は、自センサ１_ｉが観測している目標Ｔ_ｍ（ｍ∈｛１，．．．，Ｍ｝）に対する１つ前の時刻ｔ_ｋ−１のバイアス誤差を考慮したグローバル座標系の目標状態ベクトルｘ＾_ｉｍ（ｋ−１）（便宜上、数式のｘの上に＾をｘ＾で表す）と他センサ１_ｊの目標Ｔ_ｍに対する１つ前の時刻ｔ_ｋ−１のバイアス誤差を考慮したグローバル座標系の目標状態ベクトルｘ＾_ｊｍ（ｋ−１）を用いて、（１）式のように、目標Ｔ_ｍに対する時刻ｔ_ｋのグローバル座標系の相互作用ベクトルｘ〜_ｉｍ（ｋ）（便宜上、数式のｘの上に〜をｘ〜で表す）を算出する。

ここで、ε_ｍは重みを表す。また、ａ_ｉｊｍは、目標Ｔ_ｍに対して、自センサ１_ｉと他センサ１_ｊが同一目標を観測している場合は１、その他は０とする。

後述するように、目標Ｔ_ｍを観測していない他センサ１_ｊの処理は、（ａ_ｉｊｍ＝０）となるため、省略することができる（実質的には、同一目標の目標状態ベクトルが重み付き加算される）。また、自センサ１_ｉが観測していない目標や、自センサ１_ｉのみが観測している目標については、処理が不要である。

自センサ１_ｉに対応するバイアス誤差推定装置３_ｉのバイアス誤差算出部３０２は、自センサ１_ｉのバイアス誤差ベクトルｂ_ｉ（ｋ）を算出するために、グローバル座標系の相互作用ベクトルｘ〜_ｉｍ（ｋ）、１つ前の時刻ｔ_ｋ−１のローカル座標系の目標状態ベクトルｙ_ｉｍ（ｋ−１）（便宜上、数式のｙの上に＾をｙで表す）、１つ前の時刻ｔ_ｋ−１のセンサ状態ベクトルｚ_ｉ（ｋ−１）を用いて、（２）式の逆問題を解く。

ここで、ｆ（・）は、ローカル座標系の目標状態ベクトル、センサ状態ベクトル、バイアス誤差ベクトルから、グローバル座標系の目標状態ベクトルへの変換を行う関数を表す。

自センサ１_ｉに対応するバイアス誤差推定装置３_ｉの目標状態値算出部３０３は、算出したバイアス誤差ベクトルｂ_ｉ（ｋ）、時刻ｔ_ｋのローカル座標系の目標状態ベクトルｙ_ｉｍ（ｋ）、時刻ｔ_ｋのセンサ状態ベクトルｚ_ｉ（ｋ）を用いて、以下の（３）式により、自センサ１_ｉの目標Ｔ_ｍに対する時刻ｔ_ｋのバイアス誤差を考慮したグローバル座標系の目標状態ベクトルｘ＾_ｉｍ（ｋ）を算出する。

自センサ１_ｉに対応するバイアス誤差推定装置３_ｉでは、上記の処理が繰り返し実施される。また、それぞれの自他センサ１_１〜１_Ｎに対応するバイアス誤差推定装置３_１〜３_Ｎにおいて、上記の処理が並行して行われる。

（第１の実施例）
次に、距離と角度にバイアス誤差がある４個の移動しない２次元センサ１_１，．．．，１_４から構成されるセンサネットワークにおいて、４個の固定目標（物標と呼ぶ場合がある）Ｔ_１，．．．，Ｔ_４を観測している状況を例として、第１の実施例に係るバイアス誤差推定装置３Ａ（３Ａ_１，．．．，３Ａ_４）の具体的な処理内容について説明する。

第１の実施例に係るバイアス誤差推定装置は、相互作用値算出部３０１、バイアス誤差算出部３０２、目標状態値算出部３０３を備える。

センサ１_ｉ（ｉ∈｛１，２，３，４｝）のセンサ状態ベクトルは、センサ１_ｉが移動しない（センサの速度は０）ため、時刻パラメータを持たないｚ_ｉとする。センサ状態ベクトルｚ_ｉは、（４）式で表されるものとする。

ここで、ｘ_ｉとｙ_ｉは、それぞれグローバル座標系におけるｘ軸とｙ軸の位置（センサ１_ｉのローカル座標系の原点）を表す。

センサ１_ｉが観測している目標Ｔ_ｍ（ｍ∈｛１，２，３，４｝）に対するローカル座標系の目標状態ベクトルは、物標を目標としているため、時刻パラメータを持たないｙ_ｉｍとする。センサ１_ｉの目標Ｔ_ｍに対する（センサ１_ｉの位置を原点とする）ローカル座標系の目標状態ベクトルｙ_ｉｍは、（５）式で表されるものとする。

ここで、ｒ_ｉｍは、距離のバイアス誤差（未知数）の影響を受けたセンサ１_ｉから目標Ｔ_ｍまでの距離を表し、θ_ｉｍは、角度のバイアス誤差（未知数）の影響を受けたセンサ１_ｉから目標Ｔ_ｍへの角度を表す。

なお、ローカル座標系の目標状態ベクトルｙ_ｉｍの代わりに、（６）式で表されるグローバル座標系の目標状態ベクトルｘ_ｉｍが入力される場合、グローバル座標系の目標状態ベクトルｘ_ｉｍとセンサ状態ベクトルｚ_ｉに基づいて座標変換を行うことにより、ローカル座標系の目標状態ベクトルｙ_ｉｍを算出することができる。

ここで、目標状態ベクトルｘ_ｉｍの要素であるｘ_ｉｍ１とｙ_ｉｍ１は、それぞれグローバル座標系におけるｘ軸とｙ軸の位置を表す。

また、センサ１_ｉの時刻ｔ_ｋのバイアス誤差ベクトルｂ_ｉ（ｋ）は、（７）式で表されるものとする。

ここで、Δｒ_ｉ（ｋ）とΔθ_ｉ（ｋ）は、それぞれ距離と角度のバイアス誤差の推定値を表す。

このとき、センサ１_ｉの目標Ｔ_ｍに対する時刻ｔ_ｋのグローバル座標系の相互作用ベクトルｘ〜_ｉｍ（ｋ）は、（８）式で表される。

ここで、相互作用ベクトルｘ〜_ｉｍ（ｋ）の要素であるｘ_ｉｍ２（ｋ）とｙ_ｉｍ２（ｋ）は、それぞれグローバル座標系におけるｘ軸とｙ軸の位置を表す。

なお、センサ１_ｉの目標Ｔ_ｍに対する時刻ｔ_ｋのバイアス誤差を考慮したグローバル座標系の目標状態ベクトルｘ＾_ｉｍ（ｋ）についても、同様に（９）式で表される。

ここで、目標状態ベクトルｘ＾_ｉｍ（ｋ）の要素であるｘ_ｉｍ３（ｋ）とｙ_ｉｍ３（ｋ）は、それぞれグローバル座標系におけるｘ軸とｙ軸の位置を表す。

ここで、第１のセンサ１_１が第１の目標Ｔ_１と第２の目標Ｔ_２を観測し、第２のセンサ１_２が第２の目標Ｔ_２と第３の目標Ｔ_３を観測し、第３のセンサ１_３が第３の目標Ｔ_３と第４の目標Ｔ_４を観測し、第４のセンサ１_４が第４の目標Ｔ_４と第１の目標Ｔ_１を観測しているとする。

図３は、センサ１_１を誤差推定対象とする第１の実施例に係るバイアス誤差推定装置３Ａ_１の処理の流れの具体例を示すフローチャートで、（ａ）は全体の処理の流れを示し、（ｂ）はバイアス誤差推定処理（相互作用値の算出、バイアス誤差の算出、目標状態値の算出からなる一連の処理）の具体的な流れを示している。同図に沿って、上記バイアス誤差推定装置３Ａ_１の処理の流れを説明する。

バイアス誤差推定装置３Ａ_１は、まず、図３（ａ）に示すように、バイアス誤差算出部３０２において、センサ１_１のバイアス誤差ベクトルとしてｂ_１（０）に初期値（０値、以前のバイアス誤差の推定値、外部装置から設定される値等）を設定する（ステップＳ１０１）。次に、外部装置から自センサ１_１が観測する目標に対するローカル座標系の目標状態ベクトルとして｛ｙ_１１，ｙ_１２｝と、自センサ１_１のグローバル座標系のセンサ状態ベクトルとして｛ｚ_１｝を入力する（ステップＳ１０２）。また、外部装置から他センサ（｛１_２，１_３，１_４｝）が観測する目標に対するバイアス誤差を考慮したグローバル座標系の目標状態ベクトルとして｛ｘ＾_２２，ｘ＾_２３，ｘ＾_３３，ｘ＾_３４，ｘ＾_４４，ｘ＾_４１｝を入力する（ステップＳ１０３）。そして、相互作用値算出部３０１において、図３（ｂ）に示す関数Ａの処理に移行する（ステップＳ１０４）。

関数Ａでは、第１の目標Ｔ_１に対する１つ前の時刻ｔ_ｋ−１のグローバル座標系の相互作用ベクトルｘ〜_１１（ｋ）と第２の目標Ｔ_２に対する１つ前の時刻ｔ_ｋ−１のグローバル座標系の相互作用ベクトルｘ〜_１２（ｋ）を算出する（ステップＳ１００１）。

ここで、自センサ１_１の第１の目標Ｔ_１に対する時刻ｔ_ｋのグローバル座標系の相互作用ベクトルｘ〜_１１（ｋ）は、自センサ１_１の第１の目標Ｔ_１に対する１つ前の時刻ｔ_ｋ−１のバイアス誤差を考慮したグローバル座標系の目標状態ベクトルｘ＾_１１（ｋ−１）と共に、他センサ１_４でも第１の目標Ｔ_１を観測しているため、他センサ１_４に対応するバイアス誤差推定装置３_４の相互作用値算出部３０１からの他センサ１_４の第１の目標Ｔ_１に対する１つ前の時刻ｔ_ｋ−１のバイアス誤差を考慮したグローバル座標系の目標状態ベクトルｘ＾_４１（ｋ−１）を用いて、（１０）式で算出される。

同様に、第２の目標Ｔ_２に対する時刻ｔ_ｋのグローバル座標系の相互作用ベクトルｘ〜_１２（ｋ）は、他センサ１_２でも第２の目標Ｔ_２を観測しているため、（１１）式で算出される。

上記のように、相互作用値算出部３０１による関数Ａの処理において、対象となる全目標の処理が終了しない場合（ステップＳ１００２のＮＯ）、目標パラメータを変更して上記ステップＳ１００１の処理を対象となる全目標について実施する。

次に、バイアス誤差算出部３０２において、算出された相互作用ベクトルｘ〜_１１（ｋ）と相互作用ベクトルｘ〜_１２（ｋ）等を用いた（１２）式の逆問題を解いて、自センサ１_１のバイアス誤差ベクトルｂ_１（ｋ）を算出する（ステップＳ１００３）。

次に、目標状態値算出部３０３において、算出された自センサ１_１のバイアス誤差ベクトルｂ_１（ｋ）等を用いて、自センサ１_１のバイアス誤差を考慮したグローバル座標系の目標状態ベクトルｘ＾_１１（ｋ）と同目標状態ベクトルｘ＾_１２（ｋ）を算出する（ステップＳ１００４）。なお、初回のみ、目標状態値算出部３０３において算出された自センサ１_１のバイアス誤差ベクトルｂ_１（ｋ）の代わりに、上記ステップＳ１０１で設定するバイアス誤差ベクトルの初期値ｂ_１（０）を用いる。

ここで、目標状態値算出部３０３では、対象となる全目標の処理が終了しない場合（ステップＳ１００５のＮＯ）、目標パラメータを変更して上記ステップＳ１００５の処理を対象となる全目標について実施する。全目標について処理が終了した場合（ステップＳ１００５のＹＥＳ）、関数Ａの処理を終了する。

関数Ａの処理が終了した場合には、図３（ａ）に示すように、目標状態値算出部３０３で算出された自センサ１_１のバイアス誤差を考慮した目標状態ベクトルｘ＾_１１（ｋ）と目標状態ベクトルｘ＾_１２（ｋ）を、外部装置を経由して、他センサ１_２〜１_４に対応するバイアス誤差推定装置３_２〜３_４に出力する（ステップＳ１０５）。ここで、処理終了の指示等により処理が終了しない場合（ステップＳ１０６のＮＯ）、算出されたバイアス誤差を考慮したグローバル座標系の目標状態ベクトルを用いて、ステップＳ１０３からの処理を繰り返し実施する。また、それぞれのセンサ１_２〜１_４に対応するバイアス誤差推定装置３_２〜３_４において、上記の処理が並行して行われる。

以上のように第１の実施例に係るバイアス誤差推定装置３Ａ_ｉによれば、目標を観測する複数のセンサが目標の追跡処理を行う場合に、それぞれのセンサの観測データや航跡データを一箇所に集めることなく、分散化されたセンサネットワークにおいて、センサそれぞれのバイアス誤差を推定することができる。

なお、上記の実施例では、未知数であるバイアス誤差の真値が時間的に変動せず、外部装置から入力される目標状態ベクトルとセンサ状態ベクトルも共にランダム誤差を含まない例を示したが、未知数であるバイアス誤差の真値が時間的に変動する場合や、外部装置から入力される目標状態ベクトルとセンサ状態ベクトルの少なくとも１つがランダム誤差を含む場合には、図３（ａ）に示したフローチャートの終了判定（ステップＳ１０６）において、処理終了の指示等がなく処理が終了しない場合（ステップＳ１０６のＮＯ）、ステップＳ１０３の代わりにステップＳ１０２からの処理を繰り返し実施するように変更することで、対応することができる。

（第２の実施例）
以下、第２の実施例に係るバイアス誤差推定装置３Ｂ_ｉについて説明する。第２の実施例に係るバイアス誤差推定装置３Ｂ_ｉは、第１の実施例に係るバイアス誤差推定装置３Ａ_ｉと同じく、相互作用値算出部３０１、バイアス誤差算出部３０２、目標状態値算出部３０３を備える。

バイアス誤差推定装置３Ｂ_ｉは、バイアス誤差推定装置３Ａ_ｉが推定するセンサ１_ｉのローカル座標系の距離と角度のバイアス誤差の代わりに、センサ１_ｉのグローバル座標系の位置のバイアス誤差を推定する。

この場合、外部装置から（６）式で表されるグローバル座標系の目標状態ベクトルｘ_ｉｍが入力され、センサ１_ｉのグローバル座標系の位置のバイアス誤差ベクトルｂ_ｉ（ｋ）が（１４）式で表されるものとする。

ここで、Δｘ_ｉ（ｋ）とΔｙ_ｉ（ｋ）は、それぞれグローバル座標系におけるｘ軸とｙ軸のバイアス誤差の推定値を表す。

このとき、第１の実施例の（８）式に対応する式は、以下の（１５）式のように表される（（９）式に対応する式についても同様）。

ここで、相互作用値算出部３０１は、複数のセンサのうちの誤差推定対象センサの１つ前の時刻ｔ_ｋ−１におけるバイアス誤差を考慮したグローバル座標系の目標状態値と、複数のセンサのうちの同一目標を観測する誤差推定対象ではないセンサの１つ前の時刻ｔ_ｋ−１におけるバイアス誤差を考慮したグローバル座標系の目標状態値とを重み付き加算して、誤差推定対象センサの時刻ｔ_ｋにおけるグローバル座標系の相互作用値を算出する。

バイアス誤差算出部３０２は、誤差推定対象センサの１つ前の時刻ｔ_ｋ−１におけるグローバル座標系の目標状態値と、相互作用値算出部３０１で算出される誤差推定対象センサの時刻ｔ_ｋにおけるグローバル座標系の相互作用値とに基づいて、誤差推定対象センサの時刻ｔ_ｋにおけるバイアス誤差を算出する。

目標状態値算出部３０３は、誤差推定対象センサの時刻ｔ_ｋにおけるグローバル座標系の目標状態値と、予めセンサ毎に設定されるバイアス誤差の初期値またはバイアス誤差算出部３０２で算出される誤差推定対象センサの時刻ｔ_ｋにおけるバイアス誤差とに基づいて、誤差推定対象センサが観測している目標の時刻ｔ_ｋにおけるバイアス誤差を考慮したグローバル座標系の目標状態値を算出する。

上記のように、関数ｆ（・）を入力パラメータや推定するバイアス誤差に合わせて変形するだけで、センサ１のグローバル座標系の位置のバイアス誤差を推定するように構成することができる。さらに、センサのローカル座標系の距離と角度のバイアス誤差とセンサのグローバル座標系の位置のバイアス誤差の両方を推定するように構成することもできる。

（第３の実施例）
以下、第３の実施例に係るバイアス誤差推定装置３Ｃ_ｉについて説明する。なお、第３の実施例に係るバイアス誤差推定装置３Ｃ_ｉは、第１の実施例に係るバイアス誤差推定装置３Ａ_ｉと同じく、相互作用値算出部３０１、バイアス誤差算出部３０２、目標状態値算出部３０３を備える。

上記の実施例では、センサ、データ融合装置、バイアス誤差推定装置が、それぞれ１対１に対応する例を示したが、第３の実施例に係るバイアス誤差推定装置３Ｃ_ｉは、図４（ａ）または（ｂ）に示すセンサネットワーク全体の構成のように、１つのデータ融合装置が複数のセンサに対応し、複数のセンサに対応したデータ融合装置とバイアス誤差推定装置が１対１に対応する構成（図４（ａ））や、１つのバイアス誤差推定装置が複数のデータ融合装置に対応する構成（図４（ｂ））とすることができる。即ち、１つのバイアス誤差推定装置が複数のセンサに対応するように構成することができる。

第３の実施例に係るバイアス誤差推定装置３Ｃ_ｉによれば、複数のセンサに対応することができるため、バイアス誤差推定装置３Ｃの総数を減らすことや、バイアス誤差推定装置３Ｃの個々の故障等に対するロバスト性を向上させることができる。

また、第３の実施例に係るバイアス誤差推定装置３Ｃ_ｉは、装置内でバイアス誤差推定処理のいずれかを並列に実施するように構成することができる。

図５は、第３の実施例に係るバイアス誤差推定装置３Ｃ_ｉの処理の具体例を示すタイミングチャートである。図５（ａ）は、図３（ｂ）に示したバイアス誤差推定処理の相互作用値の算出、バイアス誤差の算出、目標状態値の算出の全てを並列に実施する例であり、それぞれセンサ１_１〜１_２のデータ毎に順次相互作用値、バイアス誤差、目標状態値の算出を並列に実施する。また、図５（ｂ）は、図３（ｂ）に示したバイアス誤差推定処理の内、相互作用値、目標状態値の算出については、それぞれセンサ１_１〜１_２のデータについて順に実施し、バイアス誤差の算出のみセンサ１_１〜１_２のデータ毎に並列に実施する。

第３の実施例に係り、並列処理を行うバイアス誤差推定装置３Ｃ_ｉによれば、複数のセンサに対応するバイアス誤差推定処理においても、短時間で実施することができる。

また、第１または第２の実施例において、誤差推定対象センサの目標毎に行う処理を並列に実施するように構成することもできる。

（変形例）
なお、上記の実施例では、二次元センサを用いる例を示したが、三次元センサを用いるように構成することもできる。

また、目標状態値算出部３０３は、算出したバイアス誤差を考慮したグローバル座標系の目標状態値を、バイアス誤差を補正（但し、真値補正のほか相対値補正を含む）したグローバル座標系の航跡データとして外部装置に出力するように構成することができるのはもちろんのこと、バイアス誤差算出部３０２は、算出したバイアス誤差を外部装置に出力するように構成することができる。

なお、上記実施例に係るバイアス誤差推定装置３Ａ，３Ｂ，３Ｃは、相互作用値算出部３０１、バイアス誤差算出部３０２、目標状態値算出部３０３それぞれの処理機能をコンピュータに実行させるプログラムとして構成することができる。

上記実施形態は、いずれもレーダ装置、ソナー装置等の目標を観測する複数センサからなるセンサネットワークのバイアス誤差推定装置に適用可能である。

その他、本実施形態は上記実施形態そのままに限定されるものではなく、実施段階ではその要旨を逸脱しない範囲で構成要素を変形して具体化できる。また、上記実施形態に開示されている複数の構成要素の適宜な組み合わせにより、種々の発明を形成できる。例えば、実施形態に示される全構成要素から幾つかの構成要素を削除してもよい。さらに、異なる実施形態にわたる構成要素を適宜組み合わせてもよい。

１_１〜１_Ｎ…センサ、
２_１〜２_Ｎ…データ融合装置、
３_１〜３_Ｎ，３Ａ，３Ｂ，３Ｃ…バイアス誤差推定装置、
３０１…相互作用値算出部、
３０２…バイアス誤差算出部、
３０３…目標状態値算出部。

Claims (11)

1. センサネットワークを形成して目標を観測する複数のセンサそれぞれのバイアス誤差を推定するバイアス誤差推定装置であって、
   前記複数のセンサのうちの誤差推定対象センサの１つ前の時刻ｔ_ｋ−１におけるバイアス誤差を考慮したグローバル座標系の目標状態値と、前記複数のセンサのうちの同一目標を観測する誤差推定対象ではないセンサの１つ前の時刻ｔ_ｋ−１におけるバイアス誤差を考慮したグローバル座標系の目標状態値とを重み付き加算して、前記誤差推定対象センサの時刻ｔ_ｋにおけるグローバル座標系の相互作用値を算出する相互作用値算出手段と、
   前記誤差推定対象センサの１つ前の時刻ｔ_ｋ−１におけるグローバル座標系の目標状態値と、前記相互作用値算出手段によって算出される前記誤差推定対象センサの時刻ｔ_ｋにおけるグローバル座標系の相互作用値とに基づいて、前記誤差推定対象センサの時刻ｔ_ｋにおけるバイアス誤差を算出するバイアス誤差算出手段と、
   前記誤差推定対象センサの時刻ｔ_ｋにおけるグローバル座標系の目標状態値と、予め前記センサ毎に設定されるバイアス誤差の初期値または前記バイアス誤差算出手段によって算出される前記誤差推定対象センサの時刻ｔ_ｋにおけるバイアス誤差とに基づいて、前記誤差推定対象センサが観測している目標の時刻ｔ_ｋにおけるバイアス誤差を考慮したグローバル座標系の目標状態値を算出する目標状態値算出手段と
   を具備するバイアス誤差推定装置。
2. センサネットワークを形成して目標を観測する複数のセンサそれぞれのバイアス誤差を推定するバイアス誤差推定装置であって、
   前記複数のセンサのうちの誤差推定対象センサの１つ前の時刻ｔ_ｋ−１におけるバイアス誤差を考慮したグローバル座標系の目標状態値と、前記複数のセンサのうちの同一目標を観測する誤差推定対象ではないセンサの１つ前の時刻ｔ_ｋ−１におけるバイアス誤差を考慮したグローバル座標系の目標状態値とを重み付き加算して、前記誤差推定対象センサの時刻ｔ_ｋにおけるグローバル座標系の相互作用値を算出する相互作用値算出手段と、
   前記誤差推定対象センサの１つ前の時刻ｔ_ｋ−１におけるローカル座標系またはグローバル座標系の目標状態値と、前記誤差推定対象センサの１つ前の時刻ｔ_ｋ−１におけるグローバル座標系のセンサ状態値と、前記相互作用値算出手段によって算出される前記誤差推定対象センサの時刻ｔ_ｋにおけるグローバル座標系の相互作用値とに基づいて、前記誤差推定対象センサの時刻ｔ_ｋにおけるバイアス誤差を算出するバイアス誤差算出手段と、
   前記誤差推定対象センサの時刻ｔ_ｋにおけるローカル座標系またはグローバル座標系の目標状態値と、前記誤差推定対象センサの時刻ｔ_ｋにおけるグローバル座標系のセンサ状態値と、予め前記センサ毎に設定されるバイアス誤差の初期値または前記バイアス誤差算出手段によって算出される前記誤差推定対象センサの時刻ｔ_ｋにおけるバイアス誤差とに基づいて、前記誤差推定対象センサが観測している目標の時刻ｔ_ｋにおけるバイアス誤差を考慮したグローバル座標系の目標状態値を算出する目標状態値算出手段と
   を具備するバイアス誤差推定装置。
3. 前記バイアス誤差推定装置は、目標毎に行われる前記相互作用値算出手段、前記バイアス誤差算出手段、前記目標状態値算出手段の処理のいずれかを並列に実施して、前記バイアス誤差を算出する請求項１または２に記載のバイアス誤差推定装置。
4. 前記バイアス誤差推定装置は、複数のセンサを前記誤差推定対象として、それぞれのバイアス誤差を推定する請求項１または２に記載のバイアス誤差推定装置。
5. 前記バイアス誤差推定装置は、目標及びセンサ毎に行われる前記相互作用値算出手段、前記バイアス誤差算出手段、前記目標状態値算出手段の処理のいずれかを並列に実施して、前記バイアス誤差を算出する請求項４に記載のバイアス誤差推定装置。
6. 前記バイアス誤差推定装置は、算出されたバイアス誤差、または前記バイアス誤差を考慮したグローバル座標系の目標状態値の少なくとも一方を出力する請求項１乃至５のいずれか１項に記載のバイアス誤差推定装置。
7. 請求項１または２に記載のバイアス誤差推定装置を２つ以上備えるバイアス誤差推定システム。
8. センサネットワークを形成して目標を観測する複数のセンサそれぞれのバイアス誤差を推定するバイアス誤差推定方法であって、
   前記複数のセンサのうちの誤差推定対象センサの１つ前の時刻ｔ_ｋ−１におけるバイアス誤差を考慮したグローバル座標系の目標状態値と、前記複数のセンサのうちの同一目標を観測する誤差推定対象ではないセンサの１つ前の時刻ｔ_ｋ−１におけるバイアス誤差を考慮したグローバル座標系の目標状態値とを重み付き加算して、前記誤差推定対象センサの時刻ｔ_ｋにおけるグローバル座標系の相互作用値を算出し、
   前記誤差推定対象センサの１つ前の時刻ｔ_ｋ−１におけるグローバル座標系の目標状態値と、前記相互作用値の算出処理によって算出される前記誤差推定対象センサの時刻ｔ_ｋにおけるグローバル座標系の相互作用値とに基づいて、前記誤差推定対象センサの時刻ｔ_ｋにおけるバイアス誤差を算出し、
   前記誤差推定対象センサの時刻ｔ_ｋにおけるグローバル座標系の目標状態値と、予め前記センサ毎に設定されるバイアス誤差の初期値または前記バイアス誤差の算出処理によって算出される前記誤差推定対象センサの時刻ｔ_ｋにおけるバイアス誤差とに基づいて、前記誤差推定対象センサが観測している目標の時刻ｔ_ｋにおけるバイアス誤差を考慮したグローバル座標系の目標状態値を算出するバイアス誤差推定方法。
9. センサネットワークを形成して目標を観測する複数のセンサそれぞれのバイアス誤差を推定するバイアス誤差推定方法であって、
   前記複数のセンサのうちの誤差推定対象センサの１つ前の時刻ｔ_ｋ−１におけるバイアス誤差を考慮したグローバル座標系の目標状態値と、前記複数のセンサのうちの同一目標を観測する誤差推定対象ではないセンサの１つ前の時刻ｔ_ｋ−１におけるバイアス誤差を考慮したグローバル座標系の目標状態値とを重み付き加算して、前記誤差推定対象センサの時刻ｔ_ｋにおけるグローバル座標系の相互作用値を算出し、
   前記誤差推定対象センサの１つ前の時刻ｔ_ｋ−１におけるローカル座標系またはグローバル座標系の目標状態値と、前記誤差推定対象センサの１つ前の時刻ｔ_ｋ−１におけるグローバル座標系のセンサ状態値と、前記相互作用値の算出処理によって算出される前記誤差推定対象センサの時刻ｔ_ｋにおけるグローバル座標系の相互作用値とに基づいて、前記誤差推定対象センサの時刻ｔ_ｋにおけるバイアス誤差を算出し、
   前記誤差推定対象センサの時刻ｔ_ｋにおけるローカル座標系またはグローバル座標系の目標状態値と、前記誤差推定対象センサの時刻ｔ_ｋにおけるグローバル座標系のセンサ状態値と、予め前記センサ毎に設定されるバイアス誤差の初期値または前記バイアス誤差の算出処理によって算出される前記誤差推定対象センサの時刻ｔ_ｋにおけるバイアス誤差とに基づいて、前記誤差推定対象センサが観測している目標の時刻ｔ_ｋにおけるバイアス誤差を考慮したグローバル座標系の目標状態値を算出するバイアス誤差推定方法。
10. センサネットワークを形成して目標を観測する複数のセンサそれぞれのバイアス誤差を推定する処理をコンピュータに実行させるためのバイアス誤差推定プログラムであって、
    前記複数のセンサのうちの誤差推定対象センサの１つ前の時刻ｔ_ｋ−１におけるバイアス誤差を考慮したグローバル座標系の目標状態値と、前記複数のセンサのうちの同一目標を観測する誤差推定対象ではないセンサの１つ前の時刻ｔ_ｋ−１におけるバイアス誤差を考慮したグローバル座標系の目標状態値とを重み付き加算して、前記誤差推定対象センサの時刻ｔ_ｋにおけるグローバル座標系の相互作用値を算出する相互作用値算出ステップと、
    前記誤差推定対象センサの１つ前の時刻ｔ_ｋ−１におけるグローバル座標系の目標状態値と、前記相互作用値算出ステップの算出処理によって算出される前記誤差推定対象センサの時刻ｔ_ｋにおけるグローバル座標系の相互作用値とに基づいて、前記誤差推定対象センサの時刻ｔ_ｋにおけるバイアス誤差を算出するバイアス誤差算出ステップと、
    前記誤差推定対象センサの時刻ｔ_ｋにおけるグローバル座標系の目標状態値と、予め前記センサ毎に設定されるバイアス誤差の初期値または前記バイアス誤差算出ステップの算出処理によって算出される前記誤差推定対象センサの時刻ｔ_ｋにおけるバイアス誤差とに基づいて、前記誤差推定対象センサが観測している目標の時刻ｔ_ｋにおけるバイアス誤差を考慮したグローバル座標系の目標状態値を算出する目標状態値算出ステップと
    を具備するバイアス誤差推定プログラム。
11. センサネットワークを形成して目標を観測する複数のセンサそれぞれのバイアス誤差を推定する処理をコンピュータに実行させるためのバイアス誤差推定プログラムであって、
    前記複数のセンサのうちの誤差推定対象センサの１つ前の時刻ｔ_ｋ−１におけるバイアス誤差を考慮したグローバル座標系の目標状態値と、前記複数のセンサのうちの同一目標を観測する誤差推定対象ではないセンサの１つ前の時刻ｔ_ｋ−１におけるバイアス誤差を考慮したグローバル座標系の目標状態値とを重み付き加算して、前記誤差推定対象センサの時刻ｔ_ｋにおけるグローバル座標系の相互作用値を算出する相互作用値算出ステップと、
    前記誤差推定対象センサの１つ前の時刻ｔ_ｋ−１におけるローカル座標系またはグローバル座標系の目標状態値と、前記誤差推定対象センサの１つ前の時刻ｔ_ｋ−１におけるグローバル座標系のセンサ状態値と、前記相互作用値算出ステップの算出処理によって算出される前記誤差推定対象センサの時刻ｔ_ｋにおけるグローバル座標系の相互作用値とに基づいて、前記誤差推定対象センサの時刻ｔ_ｋにおけるバイアス誤差を算出するバイアス誤差算出ステップと、
    前記誤差推定対象センサの時刻ｔ_ｋにおけるローカル座標系またはグローバル座標系の目標状態値と、前記誤差推定対象センサの時刻ｔ_ｋにおけるグローバル座標系のセンサ状態値と、予め前記センサ毎に設定されるバイアス誤差の初期値または前記バイアス誤差算出ステップの算出処理によって算出される前記誤差推定対象センサの時刻ｔ_ｋにおけるバイアス誤差とに基づいて、前記誤差推定対象センサが観測している目標の時刻ｔ_ｋにおけるバイアス誤差を考慮したグローバル座標系の目標状態値を算出する目標状態値算出ステップと
    を具備するバイアス誤差推定プログラム。

Images