JP5649372B2

JP5649372B2 - 信号処理装置

Info

Publication number: JP5649372B2
Application number: JP2010191342A
Authority: JP
Inventors: 英俊古川
Original assignee: Toshiba Corp
Current assignee: Toshiba Corp
Priority date: 2010-08-27
Filing date: 2010-08-27
Publication date: 2015-01-07
Anticipated expiration: 2030-08-27
Also published as: JP2012047642A

Description

本発明の実施形態は、移動可能な二次元角度センサからの情報を入力して、目標の三次元情報を推定する信号処理装置の計算量を低減する技術に関する。

移動可能な二次元角度センサで目標の二次元角度情報を取得し、この二次元角度情報から目標の三次元情報を推定する三次元情報推定システムが知られている。この種の、従来の三次元情報推定システムの一例を図１３に示す。図１３は、従来の三次元情報推定システムの構成の一例を示すブロック図である。従来の三次元情報推定システムは、二次元角度センサ１、アクチュエータ２、センサ位置算出装置３、及び信号処理装置４から構成されている。さらに、信号処理装置４は、三次元情報推定部１１を備えている。

二次元角度センサ１は、目標の二次元角度情報を取得して信号処理装置４に出力する。アクチュエータ２は、二次元角度センサ１の位置を変更する。センサ位置算出装置３は、このアクチュエータ２によって変更された二次元角度センサ１の位置を算出し、二次元角度センサ１の位置情報として信号処理装置４へ出力する。信号処理装置４は、二次元角度センサ１からの目標の二次元角度情報とセンサ位置算出装置３からの二次元角度センサの位置情報を入力して、三次元情報推定部１１により目標の三次元情報の推定を行う。

三次元情報推定部１１における目標の三次元情報の推定処理としては、非特許文献１や非特許文献２に示されているレンジパラメタライズド拡張カルマンフィルタ（ｒａｎｇｅ−ｐａｒａｍｅｔｅｒｉｓｅｄｅｘｔｅｎｄｅｄＫａｌｍａｎｆｉｌｔｅｒ）や、非特許文献３に示されているパーティクルフィルタを用いることができる。以下では、三次元情報推定部１１に、レンジパラメタライズド拡張カルマンフィルタを適用した場合について説明する。

レンジパラメタライズド拡張カルマンフィルタは、二次元角度センサ１からの目標の角度情報に対し、複数の仮説距離を初期値として拡張カルマンフィルタを並列動作させ、それぞれの拡張カルマンフィルタからの三次元情報を重み付け平均することで、目標の三次元情報を推定する。レンジパラメタライズド拡張カルマンフィルタにおける仮説距離の初期値は、目標距離範囲の最小値Ｒ_ｍｉｎと最大値Ｒ_ｍａｘ及び並列フィルタ数Ｎ_ｆから設定される。例えば、Ｒ_ｍｉｎ：２ｋｍ、Ｒ_ｍａｘ：１２８ｋｍ、Ｎ_ｆ：６の場合、それぞれの拡張カルマンフィルタに対する仮説距離の初期値は、３ｋｍ、６ｋｍ、１２ｋｍ、２４ｋｍ、４８ｋｍ、９６ｋｍと設定される。それぞれの拡張カルマンフィルタでは、二次元角度センサ１からの目標の角度情報と仮説距離の初期値の組み合わせ、及び二次元角度センサ１の位置情報に基づいて、三次元情報の推定処理を開始する。それぞれの拡張カルマンフィルタからの三次元情報は、最終的には、重み付け平均されて出力される。上記の初期化処理により一旦三次元情報が確立すると、確立した目標の三次元情報に対して相関のある二次元角度センサからの目標の角度情報を用いて、目標の三次元情報の更新が行われる。

すなわち、図１４のフローチャートに例示したように、取得した二次元角度センサからの目標の角度情報と二次元角度センサの位置情報に対する相関処理等によって既知の目標か否かが判定され（ＳＴ１０２）、その結果新たな目標と判断されると、上記の初期化処理により対象目標の三次元情報が確立される（ＳＴ１０４）。その後はＳＴ１０２のステップにおいて、確立された三次元情報に対して相関を有すると判定された角度情報により、該当する目標の三次元情報が更新されていく（ＳＴ１０３）。

特開２０００−５５５９９号公報（第８ページ、図１０）

N. Peach，"Bearings-only tracking using a set of range-parameterised extended Kalman filters，" IEE Proc. Control Theory Appl., vol.142, no.1, pp.73-80, Jan. 1995. 古川，"RPEKFを用いた複数パッシブセンサによる目標追尾，" 2009信学総大, B-2-43, Mar. 2009. X. Lin, T. Kirubarajan, Y. Bar-Shalom, Simon Maskell,"Comparison of EKF, pseudomeasurement, and particle filters for a bearing-only target tracking problem," Proc. SPIE Vol. 4728, p. 240-250, Signal and Data Processing of Small Targets 2002 S. S. Blackman, Multiple-Target Tracking With Radar Applications,1986

上述のように、目標の三次元情報推定にレンジパラメタライズド拡張カルマンフィルタを適用した、従来の信号処理装置では、複数の仮説距離を初期値として、拡張カルマンフィルタを並列動作させるため、計算量が多いという課題があった。

本実施形態の信号処理装置は、移動可能な二次元角度センサから、目標の角度情報とこの二次元角度センサの位置情報を入力し、前記目標の三次元情報を推定する信号処理装置であって、前記二次元角度センサから前記目標までの距離の仮説を複数の仮説距離として設定し、前記目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に対して、前記複数の仮説距離のそれぞれにおける尤度を算出するとともに、前記複数の仮説距離とこれらに対応して算出した前記尤度を用いて前記目標の概略距離を推定する概略距離推定手段と、前記目標の角度情報と前記二次元角度センサの位置情報、ならびに前記概略距離推定手段からの目標の概略距離に基づいて、前記目標の三次元情報の推定を開始する三次元情報推定手段とを備えたことを特徴とする。

また、移動可能な二次元角度センサから、目標の角度情報とこの二次元角度センサの位置情報を入力し、前記目標の三次元情報を推定する信号処理装置であって、前記目標の時系列の角度情報に基づき、所定の相関範囲内における複数回の観測機会に対してこれら検出された角度情報が有意であるかを判定し、有意であると判定された前記目標の時系列の角度情報をフィッティング処理してフィッティングラインを求め、このフィッティングラインと前記時系列の角度情報との誤差による判定を行って、前記目標の三次元情報の推定を開始するか否かを判定する推定開始判定手段と、前記推定開始判定手段からの三次元情報の推定開始を判定された目標に対し、前記二次元角度センサから前記目標までの距離の仮説を複数の仮説距離として設定し、前記目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に対して、前記複数の仮説距離のそれぞれにおける尤度を算出するとともに、前記複数の仮説距離とこれらに対応して算出した前記尤度を用いて前記目標の概略距離を推定する概略距離推定手段と、前記推定開始判定手段からの三次元情報の推定開始を判定された目標に対し、この目標の角度情報と前記二次元角度センサの位置情報、ならびに前記概略距離推定手段からの目標の概略距離に基づいて、この目標の三次元情報の推定を開始する三次元情報推定手段とを備えたことを特徴とする。

また、移動可能な二次元角度センサから、目標の角度情報とこの二次元角度センサの位置情報を入力し、前記目標の三次元情報を推定する信号処理装置であって、前記二次元角度センサから前記目標までの距離の仮説を複数の仮説距離として設定し、前記目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に対して、前記複数の仮説距離のそれぞれにおける尤度を算出するとともに、前記複数の仮説距離とこれらに対応して算出した前記尤度を用いて前記目標の概略距離を推定する概略距離推定手段と、前記概略距離推定手段において概略距離を推定された目標に対し、この目標の時系列の角度情報に基づき、前記目標の概略距離を用いた相関範囲内における複数回の観測機会に対してこれら検出された角度情報が有意であるかを判定し、有意であると判定された前記目標の時系列の角度情報をフィッティング処理してフィッティングラインを求め、このフィッティングラインと前記時系列の角度情報との誤差による判定を行って、前記目標の三次元情報の推定を開始するか否かを判定する推定開始判定手段と、前記推定開始判定手段からの三次元情報の推定開始を判定された目標に対し、この目標の角度情報と前記二次元角度センサの位置情報、ならびに前記概略距離推定手段からの目標の概略距離に基づいて、この目標の三次元情報の推定を開始する三次元情報推定手段とを備えたことを特徴とする。

また、移動可能な二次元角度センサから、目標の角度情報とこの二次元角度センサの位置情報を入力し、前記目標の三次元情報を推定する信号処理装置であって、前記二次元角度センサから前記目標までの距離の仮説を複数の仮説距離として設定し、前記目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に対して、前記複数の仮説距離のそれぞれにおける尤度を算出するとともに、前記複数の仮説距離とこれらに対応して算出した前記尤度を用いて前記目標の概略距離を推定すると共に、前記目標の時系列の角度情報に基づき、所定の相関範囲内及び前記目標の概略距離を用いた相関範囲内における複数回の観測機会に対してこれら検出された角度情報が有意であるかを判定し、有意であると判定された前記目標の時系列の角度情報をフィッティング処理してフィッティングラインを求め、このフィッティングラインと前記時系列の角度情報との誤差による判定を行って、前記目標の三次元情報の推定を開始するか否かを判定する概略距離推定・推定開始判定手段と、前記概略距離推定・推定開始判定手段からの三次元情報の推定開始を判定された目標に対し、この目標の角度情報と二次元角度センサの位置情報、ならびに前記概略距離推定・推定開始判定手段からの目標の概略距離に基づいて、この目標の三次元情報の推定を開始する三次元情報推定手段とを備えたことを特徴とする。

また、移動可能な二次元角度センサから、目標の角度情報とこの二次元角度センサの位置情報を入力し、前記目標の三次元情報を推定する信号処理装置であって、前記目標の時系列の角度情報に基づき、所定の相関範囲内における複数回の観測機会に対してこれら検出された角度情報が有意であるかを判定し、有意であると判定された前記目標の時系列の角度情報をフィッティング処理してフィッティングラインを求め、このフィッティングラインと前記時系列の角度情報との誤差による判定を行って、前記目標の三次元情報の推定を開始するか否かを判定する推定開始判定手段と、前記推定開始判定手段からの三次元情報の推定開始を判定された目標に対し、この目標の角度情報と前記二次元角度センサの位置情報に基づいて、この目標の三次元情報の推定を開始する三次元情報推定手段とを備えたことを特徴とする。

また、前記三次元情報推定手段は、レンジパラメタライズド拡張カルマンフィルタを適用して前記目標の三次元情報を推定することを特徴とする。

第１の実施形態の信号処理装置を含む三次元情報推定システムの構成の一例を示すブロック図。図１の三次元情報推定システムにおける信号処理装置４ａの動作を説明するためのフローチャート。三次元情報推定部１１ａにおける目標の三次元情報推定のシミュレーション結果の一例を示す図。第１の実施形態の三次元情報推定システムの変形例の構成の一例を示すブロック図。第２の実施形態の信号処理装置を含む三次元情報推定システムの構成の一例を示すブロック図。図５の三次元情報推定システムにおける信号処理装置４ｂの動作を説明するためのフローチャート。第３の実施形態の信号処理装置を含む三次元情報推定システムの構成の一例を示すブロック図。図７の三次元情報推定システムにおける信号処理装置４ｃの動作を説明するためのフローチャート。第４の実施形態の信号処理装置を含む三次元情報推定システムの構成の一例を示すブロック図。図９の三次元情報推定システムにおける信号処理装置４ｄの動作を説明するためのフローチャート。第５の実施形態の信号処理装置を含む三次元情報推定システムの構成の一例を示すブロック図。図１０の三次元情報推定システムにおける信号処理装置４ｅの動作を説明するためのフローチャート。従来の三次元情報推定システムの構成の一例を示すブロック図。従来の三次元情報推定システムの信号処理装置における目標の三次元情報の確立の動作を説明するためのフローチャート。

以下に、本実施形態の信号処理装置について、図１乃至図１４を参照して説明する。なお、以下においては、背景技術の欄で説明した構成部分に相当する部分には、背景技術の欄で使用した符号と同じ符号を用いて説明する。

図１は、第１の実施形態の信号処理装置を含む三次元情報推定システムの構成の一例を示すブロック図である。図１に例示したように、この三次元情報推定システムは、二次元角度センサ１、アクチュエータ２、センサ位置算出装置３、及び信号処理装置４ａから構成される。また、信号処理装置４ａは、概略距離推定部１２、及び三次元情報推定部１１ａから構成される。二次元角度センサ１は、目標の二次元角度情報を信号処理装置４ａへ出力する。アクチュエータ２は、二次元角度センサ１の位置を変更する。センサ位置算出装置３は、このアクチュエータ２によって変更された二次元角度センサ１の位置を算出し、二次元角度センサ１の位置情報として信号処理装置４ａへ出力する。

信号処理装置４ａは、二次元角度センサ１からの目標の角度情報とセンサ位置算出装置３からの二次元角度センサの位置情報とを入力して、目標の三次元情報の推定を行なう。ここに、信号処理装置４ａは、概略距離推定部１２、及び三次元情報推定部１１ａから構成されている。

概略距離推定部１２は、二次元角度センサ１からの目標の時系列の二次元角度情報と、センサ位置算出装置３からの二次元角度センサ１の時系列の位置情報とに基づいて、目標の概略距離を推定し、目標の距離範囲を設定する。以下、この概略距離推定部１２について詳述する。

目標の位置の変化量に対して、アクチュエータ２による二次元角度センサ１の位置の変化量が大きな場合、目標の概略距離の推定方法として、例えば、異なる位置で観測された２点以上の目標の時系列の二次元角度情報と時系列の位置情報とに基づいて、交会法より目標の概略距離を算出し推定することができる。交会法そのものは、二次元角度センサ１の位置の変化中における目標の位置の変化には対応していない。このため、目標の真の距離を算出することはできず、目標の位置の変化に起因する誤差を含んでいるが、これを目標の概略距離Ｒ_ｅｓｔとして用いることはできる。そして、算出された目標の概略距離Ｒ_ｅｓｔに基づいて、目標の距離範囲としてその最小値Ｒ_ｍｉｎ及び最大値Ｒ_ｍａｘを設定する。

一方、目標の位置の変化量に対して、アクチュエータ２による二次元角度センサ１の位置の変化量が大きくない場合、２点以上の仮説距離における尤度に基づいて、概略距離を推定することができる。

仮説距離をＲ_ｎ、仮説距離Ｒ_ｎにおける尤度をＬ_ｎとすると、尤度Ｌ_ｎは、以下の（１）式に示す関数で表される。

Ｌ_ｎ＝ｆ（｛ＹＹ，ＸＸ，Ａ｝｜Ｒ_ｎ）・・・（１）
ここで、ＹＹは、二次元角度センサ１からの観測値の時系列データ、ＸＸは、センサ位置算出装置３からのセンサ位置の時系列データ、Ａは、基準値である。

より具体的には、仮説距離Ｒ_ｎを用いて、目標の角度の観測値の時系列データＹＹを三次元ローカル直交座標系の観測値の時系列データＺＺ１に変換し、センサ位置（三次元ワールド直交座標系）の時系列データＸＸを加算することにより、目標の三次元ワールド直交座標系の観測値の時系列データＺＺ２に変換する。更に、変換された目標の三次元ワールド直交座標系の観測値の時系列データＺＺ２と基準値Ａとの間で統計的な処理を実施し、尤度Ｌ_ｎに変換する。

概略距離Ｒ_ｅｓｔは、仮説距離Ｒ_ｉとその仮説距離Ｒ_ｉにおける尤度Ｌ_ｉとを用いて、以下の（２）式で表される。

２点の仮説距離から概略距離を推定する場合の例を以下に示す。近距離側の仮説距離をＲ_１、遠距離側の仮説距離をＲ_２、それぞれの仮説距離における尤度をＬ_１及びＬ_２とする。このとき、概略距離Ｒ_ｅｓｔは、仮説距離Ｒ_１とＲ_２、尤度Ｌ_１とＬ_２から、以下の（３）式により算出する。

なお、二次元角度センサ１から信号強度が得られる場合、目標の概略距離の推定方法として、信号強度に基づいて目標までの距離を算出するように構成しても良い。更に、上記の目標の概略距離の推定方法と併用するように構成しても良い。

このようにして算出された目標の概略距離Ｒ_ｅｓｔに基づいて目標の距離範囲を設定する。目標の距離範囲の最小値Ｒ_ｍｉｎと最大値Ｒ_ｍａｘを背景技術の例に示したようにＲ_ｍｉｎ：２ｋｍ、Ｒ_ｍａｘ：１２８ｋｍとし、並列フィルタ数Ｎ_ｆを３、目標距離範囲として、近距離、遠距離の２種類に分類する場合、近距離範囲のＲ_ｍｉｎとＲ_ｍａｘは、それぞれ２ｋｍと１６ｋｍ、遠距離範囲のＲ_ｍｉｎとＲ_ｍａｘは、それぞれ１６ｋｍと１２８ｋｍとなる。上式で算出された概略距離Ｒ_ｅｓｔが、１６ｋｍ未満の場合、目標距離範囲として、Ｒ_ｍｉｎ：２ｋｍ、Ｒ_ｍａｘ：１６ｋｍと設定され、１６ｋｍ以上の場合、目標距離範囲として、Ｒ_ｍｉｎ：１６ｋｍ、Ｒ_ｍａｘ：１２８ｋｍと設定される。

また、上記のように近距離、遠距離の二者選択であれば、概略距離を算出せず、尤度のみを用いて目標距離範囲を設定することもできる。具体的には、近距離側の尤度Ｌ_１が、遠距離側の尤度Ｌ_２よりも大きい場合、目標距離範囲として、Ｒ_ｍｉｎ：２ｋｍ、Ｒ_ｍａｘ：１６ｋｍと設定され、遠距離側の尤度Ｌ_２が、近距離側の尤度Ｌ_１よりも大きい場合、目標距離範囲として、Ｒ_ｍｉｎ：１６ｋｍ、Ｒ_ｍａｘ：１２８ｋｍと設定される。

三次元情報推定部１１ａは、概略距離推定部１２からの目標の距離範囲に基づいて、二次元角度センサ１からの目標の角度情報、及びセンサ位置算出装置３からの二次元角度センサの位置情報を処理し、目標の三次元情報の推定を行なう。三次元情報推定部１１ａにおける目標の三次元情報の推定処理としては、例えば非特許文献１や非特許文献２に示されているレンジパラメタライズド拡張カルマンフィルタを用いることができる。レンジパラメタライズド拡張カルマンフィルタでは、二次元角度センサ１からの目標の角度情報に対し、複数の仮説距離を初期値として拡張カルマンフィルタを並列動作させ、それぞれの拡張カルマンフィルタからの三次元情報を重み付け平均することで、目標の三次元情報を推定する。

三次元情報推定部１１ａにレンジパラメタライズド拡張カルマンフィルタを適用した場合には、概略距離推定部１２からの目標の距離範囲に基づいて複数の仮説距離を算出し、算出した仮説距離を初期値として拡張カルマンフィルタを並列動作させる。すなわち、従来の三次元情報推定部１１では、予め設定された目標の距離範囲に基づいて、複数の仮説距離を算出し、算出した仮説距離を初期値として、拡張カルマンフィルタを並列動作させたのに対し、三次元情報推定部１１ａでは、概略距離推定部１２からの目標の距離範囲に基づいて、複数の仮説距離を算出し、算出した仮説距離を初期値として、拡張カルマンフィルタを並列動作させるので、目標に応じて距離範囲が変化することが特徴である。このように、目標の概略距離を推定して目標の距離範囲を限定することにより、並列動作させるフィルタ数をより少なくすることが可能となり、推定精度を劣化させることなく計算量を低減できる。

また、三次元情報推定部１１ａにおける目標の三次元情報の推定処理として、例えば非特許文献３に示されているパーティクルフィルタを用いることもできる。パーティクルフィルタは、パーティクルの集合によって、推定を進めて行く手法であり、それぞれのパーティクルの処理としては、拡張カルマンフィルタよりも簡易な処理が行われるが、パーティクル数が多いことが特徴である。例えば非特許文献３のシミュレーションに用いられるパーティクル数は５０００としている。三次元情報推定部１１ａにパーティクルフィルタを適用し、上記の概略距離推定部１２と組み合わせる場合、上記したレンジパラメタライズド拡張カルマンフィルタの並列フィルタ数の代わりに、パーティクル数を低減することにより、計算量を低減することができる。

次に、前出の図１、及び図２のフローチャートを参照して、上述のように構成された第１の実施形態の三次元情報推定システムにおける信号処理装置４ａの動作を説明する。なお、以下においては、三次元情報推定部１１ａにおける目標の三次元情報の推定処理として、レンジパラメタライズド拡張カルマンフィルタを用いた場合をとりあげて説明する。

図２は、第１の実施形態の三次元情報推定システムにおける信号処理装置４ａの動作のうち、背景技術の図１４のフローチャートに例示したＳＴ１０４に対応する三次元情報の初期化処理を説明するためのフローチャートである。

まず、三次元情報推定システムが動作を開始すると、二次元角度センサ１で取得された目標の二次元角度情報が時間経過に沿って信号処理装置４ａに送られてくる（ＳＴ２０１）。これと同時並行して、センサ位置算出装置３で算出された二次元角度センサ１の位置情報も時間経過に沿って信号処理装置４ａに送られてくる（ＳＴ２０２）。信号処理装置４ａでは、これら送られてきた目標の二次元角度情報及び二次元角度センサ１の位置情報が、時系列のデータとして保持される（ＳＴ２０３）。

次いで、信号処理装置４ａ内の概略距離推定部１２において、保持された目標の時系列の二次元角度情報と二次元角度センサ１の時系列の位置情報とに基づいて、目標の概略距離が推定される。目標の概略距離の推定にあたっては、上述したように、例えば目標の位置変化に対して二次元角度センサ１の位置変化が大きな場合は交会法により推定し、二次元角度センサ１の位置変化が大きくない場合は２点以上の仮説距離の尤度に基づき推定する手法を適用する（ＳＴ２０４）。そして、続くステップにおいては、この推定した概略距離に基づいて目標の距離範囲が設定され、その結果が三次元情報推定部１１ａに送出される（ＳＴ２０５）。

次いで、三次元情報推定部１１ａにおいて、概略距離推定部１２からの目標の距離範囲に基づいて、二次元角度センサ１からの目標の角度情報、及びセンサ位置算出装置３からの二次元角度センサの位置情報が処理され、目標の三次元情報が推定される。本実施例においてはレンジパラメタライズド拡張カルマンフィルタを用い、概略距離推定部１２からの目標の距離範囲に基づいて複数の仮説距離を算出し、これらの算出した仮説距離を初期値として拡張カルマンフィルタを並列動作させている（ＳＴ２０６）。そして、この後は動作終了が指示されるまで上述の動作ステップを繰り返す（ＳＴ２０７）。なお、図２に示した初期化処理により一旦三次元情報が確立すると、背景技術の図１４のフローチャートに例示したように、確立した目標の三次元情報に対して相関のある二次元角度センサからの目標の角度情報を用いて、目標の三次元情報の更新が行われる（ＳＴ１０２とＳＴ１０３）。

三次元情報推定部１１ａにおける目標の三次元情報の推定結果の一例を図３に示す。図３は、予め設定された目標の距離範囲に基づき算出した仮説距離を初期値とし、並列フィルタ数を６、または３にした場合と、本実施例のように概略距離推定部１２からの目標の距離範囲に基づき算出した仮説距離を初期値とし、並列フィルタ数を３にした場合とを比較したシミュレーション結果の一例である。この図３のシミュレーション結果においては、並列動作させるフィルタ数を６から３に半減させても、その推定精度を十分に維持できていることが読み取れる。また、フィルタ数が同数であれば、より高い推定精度が得られることも読み取れる。

以上説明したように、本実施例においては、目標の二次元角度情報、及び二次元角度センサの位置情報から目標の三次元情報の推定を開始する際に、目標の概略距離を推定し、推定の対象とする距離範囲を限定した上でレンジパラメタライズド拡張カルマンフィルタによる推定処理を実行している。これにより、図３のシミュレーション結果における概略距離推定を適用せず並列フィルタ６で処理を行った結果と、概略距離推定を適用して距離範囲を限定し並列フィルタ数３で処理を行った結果との比較からも分かるように、推定精度を劣化させることなく、並列動作させる拡張カルマンフィルタ数を少なくすることができるため、信号処理装置の計算量を低減することができる。この結果、処理の高速化、多目標化、信号処理装置の小型化や低価格化等の二次的効果を得ることが出来る。また、図３のシミュレーション結果において並列フィルタ数が同数の３の場合の比較から分かるように、本実施例の手法によれば、同じ計算量でより高い性能（推定精度）を得ることが出来る。このため、同等の計算能力を有する信号処理装置を用いた場合、性能を向上させることも出来る。更に、並列フィルタ数を制御することにより、計算量を低減すると共に、推定性能を向上することもできる。

（変形例）
図４は第１の実施形態の三次元情報推定システムの変形例の構成の一例を示すブロック図である。この変形例は図１の構成に対し、センサ姿勢算出装置５が追加され、信号処理装置４ｆには、角度補償部１５が追加されている。

センサ姿勢算出装置５は、二次元角度センサ１の姿勢を算出し、二次元角度センサの姿勢情報として、信号処理装置４ｆへ出力する。角度補償部１５は、二次元角度センサ１からの角度情報とセンサ姿勢算出装置５からの姿勢情報に基づいて、目標の角度情報を補償する。これら以外は、図１の構成及びその処理と同じである。この変形例においては、例えばヘディング角、ロール角やピッチ角といった二次元角度センサ１の姿勢角が変化する場合にも三次元情報を推定することができる。

なお、図１及び図４に示したアクチュエータ２は、モータ等による駆動装置の他に、自動車や航空機等の二次元角度センサ１の位置を変化させるものであれば良い。

アクチュエータ２として姿勢が変化する航空機等を用いた場合、図１に示す構成では、二次元角度センサ１の姿勢の変化を補償するため、例えばジンバル機構による大型・高価格な安定プラットフォームを必要とするのに対し、図４に示す構成では、例えば小型・低価格なストラップダウン方式を採用することが可能となり、三次元情報推定システムを小型・低価格化できる。

図５は、第２の実施形態の信号処理装置を含む三次元情報推定システムの構成の一例を示すブロック図である。図５に例示したように、この三次元情報推定システムは、二次元角度センサ１、アクチュエータ２、センサ位置算出装置３、及び信号処理装置４ｂから構成される。また、信号処理装置４ｂは、推定開始判定部１３、及び三次元情報推定部１１から構成される。二次元角度センサ１、アクチュエータ２、センサ位置算出装置３、及び三次元情報推定部１１は既出の構成と同様であるので、詳細な説明は省略する。

推定開始判定部１３は、二次元角度センサ１からの目標の時系列の角度情報とセンサ位置算出装置３からの二次元角度センサの時系列の位置情報を入力して、角度情報が真の目標のものか否かを判定し、三次元情報推定部１１に通知する。三次元情報推定部１１は、この判定結果の通知に基づき目標の三次元情報の推定を行う。判定に際しては、例えば非特許文献４に示されるＭｏｕｔｏｆＮ（以下ではＭ／Ｎと表す）や、ＳＰＲＴ（ｓｅｑｕｅｎｔｉａｌｐｒｏｂａｂｉｌｉｔｙｒａｔｉｏｔｅｓｔ）の手法を適用し、算出された相関範囲Ｂに基づいて、真の目標であるか否かの判定を行う。

Ｍ／Ｎは、Ｎ回の観測で、Ｍ回以上検出があったものを真の目標と判定する処理である。すなわち、二次元角度センサ１からの目標の角度情報が、例えば熱雑音等による誤警報であった場合、誤警報が複数回検出される確率が低いのに対し、ある程度の探知確率を持った真の目標であった場合、熱雑音等よりも複数回検出される確率が高く、観測機会に対する検出回数の違いを用いて、誤警報か真の目標かの判定を行う。ＳＰＲＴも、複数回の検出データを処理するという意味で、Ｍ／Ｎと似ているが、観測機会に対する検出回数ではなく、検出の有無によって更新される値が、真の目標と判定する閾値以上となって真の目標と判定されるか、誤警報と判定する閾値以下となって誤警報と判定されるまでは、モニタリングが継続されることが特徴である。

また、推定開始判定部１３では、目標の相関範囲Ｂを算出する。目標の相関範囲Ｂについては、一般的に、三次元センサを用いた場合は目標の運動モデルに基づいて設定できるのに対し、二次元角度センサ１からの目標の角度情報には距離情報が含まれていないため、通常の方法では目標の相関範囲を設定することができない。このため、推定開始判定部１３では、三次元情報推定部１１で用いる目標距離範囲の最小値Ｒ_ｍｉｎ、目標の最大速度Ｖ_ｍａｘ及び観測値の時間間隔ｔ_ｓを用いて、相関範囲Ｂを以下の（４）式により算出する。

ここで、ｋは単位系の変換及び相関範囲の倍率設定に用いられる係数である。

さらに、三次元情報推定部１１への判定結果の通知に際しては、算出された相関範囲Ｂに基づきＭ／ＮやＳＰＲＴにより最初の判定をした後、真の目標と判定された時系列の二次元角度情報に対しては、例えば低次関数等を用いてフィッティング処理し、フィッティングラインとの誤差に基づき２回目の判定を行った上で、その判定結果を通知する。

次に、前出の図５、及び図６のフローチャートを参照して、上述のように構成された第２の実施形態の三次元情報推定システムにおける信号処理装置４ｂの動作を説明する。図６は、この第２の実施形態の三次元情報推定システムにおける信号処理装置４ｂの動作のうち、背景技術の図１４のフローチャートに例示したＳＴ１０４に対応する三次元情報の初期化処理の動作を説明するためのフローチャートである。

まず、三次元情報推定システムが動作を開始すると、二次元角度センサ１で取得された目標の二次元角度情報が時間経過に沿って信号処理装置４ｂに送られてくる（ＳＴ６０１）。これと同時並行して、センサ位置算出装置３で算出された二次元角度センサ１の位置情報も時間経過に沿って信号処理装置４ｂに送られてくる（ＳＴ６０２）。信号処理装置４ｂでは、これら送られてきた目標の二次元角度情報及び二次元角度センサ１の位置情報が、時系列のデータとして保持される（ＳＴ６０３）。

次いで、推定開始判定部１３では、保持された目標の時系列の二次元角度情報（時間間隔ｔ_ｓ）に基づいて、（４）式により目標の相関範囲Ｂが算出される（ＳＴ６０４）。そして、この算出された相関範囲Ｂ、及び保持された目標の時系列の二次元角度情報と二次元角度センサ１の時系列の位置情報に基づいて、上述したＭ／ＮやＳＰＲＴの手法を用いて、受けとった目標の二次元角度情報が真の目標のものか、あるいは誤警報等かが判定される（ＳＴ６０５）。

真の目標と判定された場合には（ＳＴ６０５のＹ）、対象の時系列の二次元角度情報に対して、例えば一次元や二次元関数といった低次関数を用いてフィッティング処理が行われる（ＳＴ６０６）。そしてフィッティング直線／曲線との誤差に基づき真の目標か否かの２回目の判定が行われる（ＳＴ６０７）。その結果、真の目標であると判定された場合には（ＳＴ６０７のＹ）、三次元情報推定部１１に対して三次元情報の推定開始の指示を通知し（ＳＴ６０８）、この指示通知を受けて三次元情報推定部１１では、目標の三次元情報の推定が開始される。三次元情報の推定には、上述のようにレンジパラメタライズド拡張カルマンフィルタを適用できる（ＳＴ６０９）。

なお、ＳＴ６０５の動作ステップ、及びＳＴ６０７の動作ステップにおいて真の目標ではないと判定された場合には、二次元角度センサ１からの目標の二次元角度情報、及びセンサ位置算出装置３からの二次元角度センサ１の位置情報の受けとりが継続される。そして、この後は動作終了が指示されるまで、上述の動作ステップが繰り返される。（ＳＴ６１０）。

以上説明したように、本実施例においては、目標の二次元角度情報、及び二次元角度センサの位置情報から目標の三次元情報の推定を開始する際に、その二次元角度情報が真の目標によるものか、あるいは誤警報等かを判定し、真の目標であると判定された場合に、目標の三次元情報の推定処理を実行している。これにより、散発的な誤警報等を目標として三次元情報の推定処理が開始され、その後誤警報と判定されてその推定処理を終了するまでの間に発生する、レンジパラメタライズド拡張カルマンフィルタによる不要な演算処理を抑えることができ、信号処理装置の計算量を低減することができる。

図７は、第３の実施形態の信号処理装置を含む三次元情報推定システムの構成の一例を示すブロック図である。図７に例示したように、この三次元情報推定システムは、二次元角度センサ１、アクチュエータ２、センサ位置算出装置３、及び信号処理装置４ｃから構成される。また、信号処理装置４ｃは、推定開始判定部１３、概略距離推定部１２、及び三次元情報推定部１１ａから構成される。これらの各構成は、上述した実施例１及び実施例２における構成と同様であり、詳細な説明は省略する。

この第３の実施形態においては、第２の実施形態にて詳述した推定開始判定部１３の判定結果を、第１の実施形態にて詳述した概略距離推定部１２に通知するようにし、概略距離推定部１２は、この推定開始部１３からの通知により対象目標の概略距離推定を行うとともに、三次元情報推定部１１ａは、その目標の三次元情報の推定処理を行うように構成されている。以下に、図７、及び図８のフローチャートを参照して、上述のように構成された第３の実施形態の三次元情報推定システムにおける信号処理装置４ｃの動作を説明する。図８は、この第３の実施形態の三次元情報推定システムにおける信号処理装置４ｃの動作のうち、背景技術の図１４のフローチャートに例示したＳＴ１０４に対応する三次元情報の初期化処理の動作を説明するためのフローチャートである。

まず、三次元情報推定システムが動作を開始すると、二次元角度センサ１で取得された目標の二次元角度情報が時間経過に沿って信号処理装置４ｃに送られてくる（ＳＴ８０１）。これと同時並行して、センサ位置算出装置３で算出された二次元角度センサ１の位置情報も時間経過に沿って信号処理装置４ｃに送られてくる（ＳＴ８０２）。信号処理装置４ｃでは、これら送られてきた目標の二次元角度情報及び二次元角度センサ１の位置情報が、時系列のデータとして保持される（ＳＴ８０３）。なお、これらＳＴ８０１〜ＳＴ８０３の動作ステップは、第１の実施形態における図２のＳＴ２０１〜ＳＴ２０３、または第２の実施形態における図６のＳＴ６０１〜ＳＴ６０３の動作ステップに対応する。

次いで、推定開始判定部１３では、保持された目標の時系列の二次元角度情報（時間間隔ｔ_ｓ）に基づいて、（４）式により目標の相関範囲Ｂが算出される（ＳＴ８０４）。そして、この算出された相関範囲Ｂ、及び保持された目標の時系列の二次元角度情報と二次元角度センサ１の時系列の位置情報に基づいて、上述したＭ／ＮやＳＰＲＴの手法を用いて、受けとった目標の二次元角度情報が真の目標か、あるいは誤警報等かが判定される（ＳＴ８０５）。真の目標と判定された場合には（ＳＴ８０５のＹ）、対象の二次元角度情報の時系列のデータに対するフィッティング処理が行われ（ＳＴ８０６）、フィッティング直線／曲線との誤差に基づき真の目標か否かの２回目の判定が行われる（ＳＴ８０７）。その結果、真の目標であると判定された場合には（ＳＴ８０７のＹ）、概略距離推定部１２に対して概略距離の推定開始の指示が通知される（ＳＴ８０８）。なお、これらＳＴ８０４〜ＳＴ８０８の動作ステップは、第２の実施形態における図６のＳＴ６０４〜ＳＴ６０８の動作ステップに対応する。

次いで、信号処理装置４ｃ内の概略距離推定部１２において、時系列に保持された目標の二次元角度情報と二次元角度センサ１の位置情報とに基づいて、目標の概略距離が推定される。目標の概略距離の推定にあたっては、実施例１において詳述したように、例えば目標の位置変化に対して二次元角度センサ１の位置変化が大きな場合は交会法により推定し、二次元角度センサ１の位置変化が大きくない場合は２点以上の仮説距離における尤度に基づき推定する手法を適用する（ＳＴ８０９）。これに続けて、この推定された概略距離に基づいて目標の距離範囲が設定され、その結果が三次元情報推定部１１ａに送出される（ＳＴ８１０）。これらＳＴ８０９〜ＳＴ８１０の動作ステップは、第１の実施形態における図２のＳＴ２０４〜ＳＴ２０５の動作ステップに対応する。

次いで、三次元情報推定部１１ａにおいて、概略距離推定部１２からの目標の距離範囲に基づいて、ＳＴ８０８の動作ステップにて目標と判定された二次元角度センサ１からの目標の二次元角度情報、及びセンサ位置算出装置３からの二次元角度センサの位置情報を処理し、目標の三次元情報が推定される。三次元情報の推定には、実施例１で詳述したようにレンジパラメタライズド拡張カルマンフィルタを適用できる（ＳＴ８１１）。そして、この後は動作終了が指示されるまで、上述の動作ステップを繰り返す。（ＳＴ８１２）。

以上説明したように、本実施例においては、目標の二次元角度情報、及び二次元角度センサの位置情報から目標の三次元情報の推定を開始する際に、その二次元角度情報が真の目標によるものか、あるいは誤警報等かを判定し、真の目標であると判定された場合に、目標の三次元情報の推定処理を実行している。これにより、真の目標以外に対する、レンジパラメタライズド拡張カルマンフィルタによる不要な演算処理を抑制している。

また、目標の概略距離を推定し、推定の対象とする距離範囲を限定した上でレンジパラメタライズド拡張カルマンフィルタによる推定処理を実行している。これにより、推定精度を劣化させることなく、並列動作させる拡張カルマンフィルタ数を少なくすることができる。従って、信号処理装置の計算量を低減することができる。

図９は、第４の実施形態の信号処理装置を含む三次元情報推定システムの構成の一例を示すブロック図である。この第４の実施形態について、図７に示した第３の実施形態の各部と同一の部分は同一の符号で示し、その説明は省略する。この第４の実施形態が第３の実施形態と異なる点は、二次元角度センサから送られてくる目標の二次元角度情報を信号処理装置で処理する際に、第３の実施形態においては、時系列の二次元角度情報に対してまず真の目標か否かの判定を行い、真の目標であると判定された後に、その時系列の二次元角度情報から概略距離を推定し三次元情報を推定したのに対し、第４の実施形態においては、時系列の二次元角度情報に対してまずその概略距離を推定した後に真の目標か否かの判定を行い、その判定結果により対象目標の三次元情報を推定するようにした点である。以下、前出の図７及び図８、ならびに図９のブロック図及び図１０のフローチャートを参照して、その相違点のみを説明する。

図９に例示したように、この三次元情報推定システムは、二次元角度センサ１、アクチュエータ２、センサ位置算出装置３、信号処理装置４ｄから構成される。また、信号処理装置４ｄは、概略距離推定部１２、推定開始判定部１３ａ、及び三次元情報推定部１１ａから構成される。推定開始判定部１３ａを除く各構成は、上述した第３の実施例における構成と同様に構成される。

推定開始判定部１３ａは、二次元角度センサ１からの目標の時系列の角度情報とセンサ位置算出装置３からの二次元角度センサの時系列の位置情報を入力して、角度情報が真の目標のものか否かを判定し、三次元情報推定部１１ａに通知する。この際に、目標の相関範囲Ｂを算出するが、その算出方法は推定開始判定部１３とは異なる。すなわち、推定開始判定部１３では、三次元情報推定部１１で用いる目標距離範囲の最小値Ｒ_ｍｉｎ、目標の最大速度Ｖ_ｍａｘ及び観測値の時間間隔ｔ_ｓを用いて、（４）式のように算出したが、推定開始判定部１３ａは、目標距離範囲の最小値Ｒ_ｍｉｎの代わりに概略距離推定部１２からの概略距離Ｒ_ｅｓｔを用いて相関範囲Ｂを算出する。そして、Ｍ／ＮやＳＰＲＴを用いて真の目標であるか否かを判定し、真の目標であると判定された場合、三次元情報推定部１１ａに通知し三次元情報の推定開始を指示する。

次に、上述の様に構成された第４の実施形態の三次元情報推定システムにおける信号処理装置４ｄの動作について説明する。図１０は、この第４の実施形態の三次元情報推定システムにおける信号処理装置４ｄの動作のうち、背景技術の図１４のフローチャートに例示したＳＴ１０４に対応する三次元情報の初期化処理の動作を説明するためのフローチャートである。

まず、二次元角度センサ１からの目標の二次元角度情報、及びセンサ位置算出装置３で算出された二次元角度センサ１の位置情報が信号処理装置４ｄに送られ、信号処理装置４ｄ内に時系列のデータとして保持される（ＳＴ８０１〜ＳＴ８０３）。次いで、信号処理装置４ｄ内の概略距離推定部１２において、時系列のデータとして保持された目標の二次元角度情報と二次元角度センサ１の位置情報とに基づいて目標の概略距離Ｒ_ｅｓｔが推定され、推定開始判定部１３ａに送出されるとともに、（ＳＴ８０９）、この推定された概略距離Ｒ_ｅｓｔに基づいて目標の距離範囲が設定され、その結果が三次元情報推定部１１ａに送出される（ＳＴ８１０）。

推定開始判定部１３ａにおいては、上記したようにこの概略距離Ｒ_ｅｓｔに基づき相関範囲Ｂが算出され（ＳＴ８０４ａ）、Ｍ／ＮやＳＰＲＴの手法を用いて、受けとった目標の二次元角度情報が真の目標か、あるいは誤警報等かが判定される（ＳＴ８０５）。真の目標であると判定された場合（ＳＴ８０５のＹ）、対象の二次元角度情報の時系列のデータのフィッティング処理が行われ（ＳＴ８０６）、フィッティング直線／曲線との誤差に基づき真の目標か否かの２回目の判定が行われる（ＳＴ８０７）。その結果、真の目標であると判定された場合には（ＳＴ８０７のＹ）、三次元情報推定部１１ａに通知して三次元情報の推定開始を指示し（ＳＴ８０８）、三次元情報推定部１１ａでは目標の三次元情報の推定が実行される（ＳＴ８１１）。そして、この後は動作終了が指示されるまで、上述の動作ステップを繰り返す。（ＳＴ８１２）。

以上説明したように、本実施例においても、目標の二次元角度情報、及び二次元角度センサの位置情報から目標の三次元情報を推定する際に、時系列の二次元角度情報から目標の概略距離を推定して、後段において推定の対象とする距離範囲を限定するとともに、その二次元角度情報が真の目標によるものか、あるいは誤警報等かを判定し、真の目標であると判定された場合に、推定された概略距離を用いたレンジパラメタライズド拡張カルマンフィルタによる推定処理を実行している。これにより、真の目標以外に対するレンジパラメタライズド拡張カルマンフィルタによる不要な演算処理を抑制できるとともに、推定演算において推定精度を劣化させることなく、並列動作させる拡張カルマンフィルタ数を少なくすることができる。従って、実施例３と同様に、信号処理装置の計算量を低減することができる。

更に、実施例４では、相関範囲Ｂを算出する際に、実施例３で用いる目標距離範囲の最小値Ｒ_ｍｉｎの代わりに、概略距離推定部１２からの概略距離Ｒ_ｅｓｔを用いることにより、遠方に存在する目標に対しては、実施例３よりも狭い相関範囲で判定を行うことが可能となり、誤警報を真の目標と誤判定する頻度を低減することができる。

図１１は、第５の実施形態の信号処理装置を含む三次元情報推定システムの構成の一例を示すブロック図である。図１１に例示したように、この三次元情報推定システムは、二次元角度センサ１、アクチュエータ２、センサ位置算出装置３、及び信号処理装置４ｅから構成される。また、信号処理装置４ｅは、総合判定部１４、及び三次元情報推定部１１ａから構成される。これらの中で、二次元角度センサ１、アクチュエータ２、センサ位置算出装置３、三次元情報推定部１１ａの各部については、上述した実施例１乃至実施例４における構成と同様であり、詳細な説明は省略する。

総合判定部１４は、二次元角度センサ１からの目標の時系列の角度情報とセンサ位置算出装置３からの二次元角度センサの時系列の位置情報に基づいて、目標の概略距離を推定して目標の距離範囲を設定すると共に、二次元角度センサ１からの目標の角度情報が真の目標によるものか否かにより目標の三次元情報の推定開始を判定する。すなわち、総合判定部１４では、上述した実施例２の推定開始判定部１３、実施例３の概略距離推定部１２及び実施例４の推定開始判定部１３ａに対応する処理が行われる。

具体例としては、実施例２における推定開始判定部１３と同様に、時系列の観測値（二次元角度センサ１からの目標の時系列の角度情報と二次元角度センサ１の時系列の位置情報）に基づいて真の目標であるか否かの判定を行い、真の目標であると判定された場合、実施例３の概略距離推定部１２と同様に、真の目標であると判定された時系列の観測値に基づいて、概略距離Ｒ_ｅｓｔを算出する。概略距離Ｒ_ｅｓｔが算出されると、実施例４の推定開始判定部１３ａと同様に、目標距離範囲の最小値Ｒ_ｍｉｎの代わりに概略距離Ｒ_ｅｓｔを用いて、相関範囲Ｂを算出し、Ｍ／ＮやＳＰＲＴを用いて、真の目標であるか否かの判定を行う。真の目標であると判定された場合、三次元情報推定部１１ａに三次元情報の推定開始を指示する。

次に、上述のように構成された第５の実施形態の三次元情報推定システムにおける信号処理装置４ｅの動作について説明する。図１２は、この第５の実施形態の三次元情報推定システムにおける信号処理装置４ｅの動作のうち、背景技術の図１４のフローチャートに例示したＳＴ１０４に対応する三次元情報の初期化処理の動作を説明するためのフローチャートである。

まず、二次元角度センサ１からの目標の二次元角度情報、及びセンサ位置算出装置３で算出された二次元角度センサ１の位置情報が信号処理装置４ｅに送られ、信号処理装置４ｅ内にそれぞれ時系列のデータとして保持される（ＳＴ１２０１〜ＳＴ１２０３）。次いで、総合判定部１４において、保持された目標の時系列の二次元角度情報（時間間隔ｔ_ｓ）に基づいて、（４）式により目標の相関範囲Ｂが算出され（ＳＴ１２０４）、この相関範囲Ｂ、及び保持された目標の時系列の二次元角度情報と二次元角度センサ１の時系列の位置情報に基づいて、上述したＭ／ＮやＳＰＲＴの手法を用いて、受けとった目標の二次元角度情報が真の目標か、あるいは誤警報等の目標以外かが判定される（ＳＴ１２０５）。

真の目標と判定された場合には（ＳＴ１２０５のＹ）、さらに対象の二次元角度情報の時系列のデータのフィッティング処理が行われ（ＳＴ１２０６）、フィッティング直線／曲線との誤差に基づき真の目標か否かの２回目の判定が行われる（ＳＴ１２０７）。その結果、真の目標であると判定された場合には、目標の概略距離の推定が開始される（ＳＴ１２０７のＹ）。一方、ＳＴ１２０５の動作ステップ、及びＳＴ１２０７の動作ステップにおいて真の目標ではないと判定された場合には（ＳＴ１２０５のＮ、及びＳＴ１２０７のＮ）、二次元角度センサ１からの目標の二次元角度情報、及びセンサ位置算出装置３からの二次元角度センサ１の位置情報の受けとりが継続される。なお、これらＳＴ１２０４〜ＳＴ１２０７の動作ステップは、例えば実施例２における推定開始判定部１３の動作に相当し、図６のＳＴ６０４〜ＳＴ６０７の動作ステップに対応する。

次いで、総合判定部１４においては、時系列に保持された目標の二次元角度情報と二次元角度センサ１の位置情報とに基づいて、目標の概略距離Ｒ_ｅｓｔが推定される。推定にあたっては、例えば交会法や２点以上の仮説距離における尤度に基づく手法を用いることができる（ＳＴ１２０８）。さらに、この推定された概略距離Ｒ_ｅｓｔに基づいて目標の距離範囲が設定される（ＳＴ１２０９）。なお、これらＳＴ１２０８〜１２０９の動作ステップは、例えば実施例３における概略距離推定部１２の動作に相当し、図８のＳＴ８０９〜ＳＴ８１０の動作ステップに対応する。

次いで、総合判定部１４においては、目標の三次元情報の推定を開始するか否かの判定が再度行われる。すなわち、前出のＳＴ１２０８の動作ステップにて算出された概略距離Ｒ_ｅｓｔに基づき目標の相関範囲Ｂが算出され（ＳＴ１２１０）、この相関範囲Ｂに基づいて受けとった二次元角度情報が真の目標か否かが判定される。なお、相関範囲に基づく同様の判定をＳＴ１２０５の動作ステップで行っているが、ＳＴ１２０５の動作ステップでの判定の際には、目標の距離範囲の最小値Ｒ_ｍｉｎに基づき算出された相関範囲を用いているのに対し、この動作ステップでは概略距離Ｒ_ｅｓｔに基づき算出された相関範囲を用いている。従って、より狭い相関範囲で判定を行うので、この動作ステップにより、目標としての確からしさを向上させることができる（ＳＴ１２１１）。

この動作ステップで真の目標であると判定された場合（ＳＴ１２１１のＹ）、対象の二次元角度情報の時系列のデータのフィッティング処理が行われ（ＳＴ１２１２）、さらにフィッティング直線／曲線との誤差に基づき真の目標か否かが判定される（ＳＴ１２１３）。その結果、真の目標であると判定された場合には（ＳＴ１２１３のＹ）、三次元情報推定部１１ａに通知して三次元情報の推定開始を指示し（ＳＴ１２１４）、三次元情報推定部１１ａでは目標の三次元情報の推定が行われる（ＳＴ１２１５）。そして、この後は動作終了が指示されるまで、上述の動作ステップを繰り返す。（ＳＴ１２１６）。なお、これらＳＴ１２１０〜１２１４の動作ステップは、例えば実施例４における推定開始判定部１３ａの動作に相当し、図１０のＳＴ８０４ａ〜ＳＴ８０８の動作ステップに対応する。

なお、実施例２における推定開始判定部１３では、第１のステップで、Ｍ／ＮやＳＰＲＴにより、算出された相関範囲Ｂに基づいて、真の目標であるか否かの判定を行った後、第２のステップで、一次元や二次元関数といった低次関数を用いてフィッティング処理され、フィッティング直線／曲線との誤差に基づいて、真の目標であるか否かの２回目の判定を行うが、総合判定部１４の推定開始判定部１３に相当する処理（ＳＴ１２０４〜ＳＴ１２０７）では、上記第１のステップの処理（Ｍ／ＮやＳＰＲＴによる判定）のみ行い、総合判定部１４の推定開始判定部１３ａに相当する処理（ＳＴ１２１０〜ＳＴ１２１４）においてのみ、上記第２のステップの処理（フィッティング直線／曲線による判定）を実施するように構成することができる。

以上説明したように、本実施例においても、目標の二次元角度情報、及び二次元角度センサの位置情報から目標の三次元情報を推定する際に、時系列の二次元角度情報から目標の概略距離を推定して、後段において推定の対象とする距離範囲を限定するとともに、その二次元角度情報が真の目標によるものか、あるいは誤警報等かを判定し、真の目標であると判定された場合に、推定された概略距離を用いたレンジパラメタライズド拡張カルマンフィルタによる推定処理を実行している。これにより、真の目標以外に対するレンジパラメタライズド拡張カルマンフィルタによる不要な演算処理を抑制できるとともに、推定演算において推定精度を劣化させることなく、並列動作させる拡張カルマンフィルタ数を少なくすることができる。

更に、総合判定部１４では、時系列の観測値に対して総合的な判定を行うように構成したため、実施例３に対しては、真の目標か否かを判定する際に、より狭い相関範囲での判定が可能となり、誤警報を真の目標として誤判定する頻度を低減することができる。また実施例４に対しては、概略距離の算出よりも計算量の少ない目標の判定処理によって、概略距離算出の計算量を低減することができる。

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

１二次元角度センサ
２アクチュエータ
３センサ位置算出装置
４、４ａ、４ｂ、４ｃ、４ｄ、４ｅ、４ｆ信号処理装置
５センサ姿勢算出装置
１１、１１ａ三次元情報推定部
１２概略距離推定部
１３、１３ａ推定開始判定部
１４総合判定部
１５角度補償部

Claims

移動可能な二次元角度センサから、目標の角度情報とこの二次元角度センサの位置情報を入力し、前記目標の三次元情報を推定する信号処理装置であって、
前記二次元角度センサから前記目標までの距離の仮説を複数の仮説距離として設定し、前記目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に対して、前記複数の仮説距離のそれぞれにおける尤度を算出するとともに、前記複数の仮説距離とこれらに対応して算出した前記尤度を用いて前記目標の概略距離を推定する概略距離推定手段と、
前記目標の角度情報と前記二次元角度センサの位置情報、ならびに前記概略距離推定手段からの目標の概略距離に基づいて、前記目標の三次元情報の推定を開始する三次元情報推定手段と
を備えたことを特徴とする信号処理装置。
移動可能な二次元角度センサから、目標の角度情報とこの二次元角度センサの位置情報を入力し、前記目標の三次元情報を推定する信号処理装置であって、
前記目標の時系列の角度情報に基づき、所定の相関範囲内における複数回の観測機会に対してこれら検出された角度情報が有意であるかを判定し、有意であると判定された前記目標の時系列の角度情報をフィッティング処理してフィッティングラインを求め、このフィッティングラインと前記時系列の角度情報との誤差による判定を行って、前記目標の三次元情報の推定を開始するか否かを判定する推定開始判定手段と、
前記推定開始判定手段からの三次元情報の推定開始を判定された目標に対し、前記二次元角度センサから前記目標までの距離の仮説を複数の仮説距離として設定し、前記目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に対して、前記複数の仮説距離のそれぞれにおける尤度を算出するとともに、前記複数の仮説距離とこれらに対応して算出した前記尤度を用いて前記目標の概略距離を推定する概略距離推定手段と、
前記推定開始判定手段からの三次元情報の推定開始を判定された目標に対し、この目標の角度情報と前記二次元角度センサの位置情報、ならびに前記概略距離推定手段からの目標の概略距離に基づいて、この目標の三次元情報の推定を開始する三次元情報推定手段と
を備えたことを特徴とする信号処理装置。
移動可能な二次元角度センサから、目標の角度情報とこの二次元角度センサの位置情報を入力し、前記目標の三次元情報を推定する信号処理装置であって、
前記二次元角度センサから前記目標までの距離の仮説を複数の仮説距離として設定し、前記目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に対して、前記複数の仮説距離のそれぞれにおける尤度を算出するとともに、前記複数の仮説距離とこれらに対応して算出した前記尤度を用いて前記目標の概略距離を推定する概略距離推定手段と、
前記概略距離推定手段において概略距離を推定された目標に対し、この目標の時系列の角度情報に基づき、前記目標の概略距離を用いた相関範囲内における複数回の観測機会に対してこれら検出された角度情報が有意であるかを判定し、有意であると判定された前記目標の時系列の角度情報をフィッティング処理してフィッティングラインを求め、このフィッティングラインと前記時系列の角度情報との誤差による判定を行って、前記目標の三次元情報の推定を開始するか否かを判定する推定開始判定手段と、
前記推定開始判定手段からの三次元情報の推定開始を判定された目標に対し、この目標の角度情報と前記二次元角度センサの位置情報、ならびに前記概略距離推定手段からの目標の概略距離に基づいて、この目標の三次元情報の推定を開始する三次元情報推定手段と
を備えたことを特徴とする信号処理装置。
移動可能な二次元角度センサから、目標の角度情報とこの二次元角度センサの位置情報を入力し、前記目標の三次元情報を推定する信号処理装置であって、
前記二次元角度センサから前記目標までの距離の仮説を複数の仮説距離として設定し、前記目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に対して、前記複数の仮説距離のそれぞれにおける尤度を算出するとともに、前記複数の仮説距離とこれらに対応して算出した前記尤度を用いて前記目標の概略距離を推定すると共に、
前記目標の時系列の角度情報に基づき、所定の相関範囲内及び前記目標の概略距離を用いた相関範囲内における複数回の観測機会に対してこれら検出された角度情報が有意であるかを判定し、有意であると判定された前記目標の時系列の角度情報をフィッティング処理してフィッティングラインを求め、このフィッティングラインと前記時系列の角度情報との誤差による判定を行って、前記目標の三次元情報の推定を開始するか否かを判定する概略距離推定・推定開始判定手段と、
前記概略距離推定・推定開始判定手段からの三次元情報の推定開始を判定された目標に対し、この目標の角度情報と二次元角度センサの位置情報、ならびに前記概略距離推定・推定開始判定手段からの目標の概略距離に基づいて、この目標の三次元情報の推定を開始する三次元情報推定手段と
を備えたことを特徴とする信号処理装置。
移動可能な二次元角度センサから、目標の角度情報とこの二次元角度センサの位置情報を入力し、前記目標の三次元情報を推定する信号処理装置であって、
前記目標の時系列の角度情報に基づき、所定の相関範囲内における複数回の観測機会に対してこれら検出された角度情報が有意であるかを判定し、有意であると判定された前記目標の時系列の角度情報をフィッティング処理してフィッティングラインを求め、このフィッティングラインと前記時系列の角度情報との誤差による判定を行って、前記目標の三次元情報の推定を開始するか否かを判定する推定開始判定手段と、
前記推定開始判定手段からの三次元情報の推定開始を判定された目標に対し、この目標の角度情報と前記二次元角度センサの位置情報に基づいて、この目標の三次元情報の推定を開始する三次元情報推定手段と
を備えたことを特徴とする信号処理装置。
前記三次元情報推定手段は、レンジパラメタライズド拡張カルマンフィルタを適用して前記目標の三次元情報を推定することを特徴とする請求項１乃至請求項５のいずれか１項に記載の信号処理装置。
更に、前記二次元角度センサの姿勢情報を受け取ると共に、この姿勢情報に基づいて前記目標の角度情報を補償する目標角度補償手段を備えたことを特徴とする請求項１乃至請求項６のいずれか１項に記載の信号処理装置。