信号処理装置
【課題】レンジパラメタライズド拡張カルマンフィルタを適用して目標の二次元角度情報からその三次元情報を推定する信号処理装置の計算量を低減する。
【解決手段】目標の二次元角度情報、及び二次元角度センサの位置情報から目標の三次元情報を推定する際に、その二次元角度情報が真の目標か誤警報等かを判定し、真の目標であると判定された場合に目標の三次元情報の推定処理を開始することにより、真の目標以外に対する、レンジパラメタライズド拡張カルマンフィルタによる不要な演算処理を抑制する。また、目標の概略距離を推定し、推定の対象とする距離範囲を限定した上でレンジパラメタライズド拡張カルマンフィルタによる推定処理を実行することにより、推定精度を劣化させることなく、並列動作させる拡張カルマンフィルタ数を少なくして計算量を低減する。
【解決手段】目標の二次元角度情報、及び二次元角度センサの位置情報から目標の三次元情報を推定する際に、その二次元角度情報が真の目標か誤警報等かを判定し、真の目標であると判定された場合に目標の三次元情報の推定処理を開始することにより、真の目標以外に対する、レンジパラメタライズド拡張カルマンフィルタによる不要な演算処理を抑制する。また、目標の概略距離を推定し、推定の対象とする距離範囲を限定した上でレンジパラメタライズド拡張カルマンフィルタによる推定処理を実行することにより、推定精度を劣化させることなく、並列動作させる拡張カルマンフィルタ数を少なくして計算量を低減する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明の実施形態は、移動可能な二次元角度センサからの情報を入力して、目標の三次元情報を推定する信号処理装置の計算量を低減する技術に関する。
【背景技術】
【０００２】
移動可能な二次元角度センサで目標の二次元角度情報を取得し、この二次元角度情報から目標の三次元情報を推定する三次元情報推定システムが知られている。この種の、従来の三次元情報推定システムの一例を図１３に示す。図１３は、従来の三次元情報推定システムの構成の一例を示すブロック図である。従来の三次元情報推定システムは、二次元角度センサ１、アクチュエータ２、センサ位置算出装置３、及び信号処理装置４から構成されている。さらに、信号処理装置４は、三次元情報推定部１１を備えている。
【０００３】
二次元角度センサ１は、目標の二次元角度情報を取得して信号処理装置４に出力する。アクチュエータ２は、二次元角度センサ１の位置を変更する。センサ位置算出装置３は、このアクチュエータ２によって変更された二次元角度センサ１の位置を算出し、二次元角度センサ１の位置情報として信号処理装置４へ出力する。信号処理装置４は、二次元角度センサ１からの目標の二次元角度情報とセンサ位置算出装置３からの二次元角度センサの位置情報を入力して、三次元情報推定部１１により目標の三次元情報の推定を行う。
【０００４】
三次元情報推定部１１における目標の三次元情報の推定処理としては、非特許文献１や非特許文献２に示されているレンジパラメタライズド拡張カルマンフィルタ（ｒａｎｇｅ−ｐａｒａｍｅｔｅｒｉｓｅｄ ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ ｆｉｌｔｅｒ）や、非特許文献３に示されているパーティクルフィルタを用いることができる。以下では、三次元情報推定部１１に、レンジパラメタライズド拡張カルマンフィルタを適用した場合について説明する。
【０００５】
レンジパラメタライズド拡張カルマンフィルタは、二次元角度センサ１からの目標の角度情報に対し、複数の仮説距離を初期値として拡張カルマンフィルタを並列動作させ、それぞれの拡張カルマンフィルタからの三次元情報を重み付け平均することで、目標の三次元情報を推定する。レンジパラメタライズド拡張カルマンフィルタにおける仮説距離の初期値は、目標距離範囲の最小値Ｒｍｉｎと最大値Ｒｍａｘ及び並列フィルタ数Ｎｆから設定される。例えば、Ｒｍｉｎ：２ｋｍ、Ｒｍａｘ：１２８ｋｍ、Ｎｆ：６の場合、それぞれの拡張カルマンフィルタに対する仮説距離の初期値は、３ｋｍ、６ｋｍ、１２ｋｍ、２４ｋｍ、４８ｋｍ、９６ｋｍと設定される。それぞれの拡張カルマンフィルタでは、二次元角度センサ１からの目標の角度情報と仮説距離の初期値の組み合わせ、及び二次元角度センサ１の位置情報に基づいて、三次元情報の推定処理を開始する。それぞれの拡張カルマンフィルタからの三次元情報は、最終的には、重み付け平均されて出力される。上記の初期化処理により一旦三次元情報が確立すると、確立した目標の三次元情報に対して相関のある二次元角度センサからの目標の角度情報を用いて、目標の三次元情報の更新が行われる。
【０００６】
すなわち、図１４のフローチャートに例示したように、取得した二次元角度センサからの目標の角度情報と二次元角度センサの位置情報に対する相関処理等によって既知の目標か否かが判定され（ＳＴ１０２）、その結果新たな目標と判断されると、上記の初期化処理により対象目標の三次元情報が確立される（ＳＴ１０４）。その後はＳＴ１０２のステップにおいて、確立された三次元情報に対して相関を有すると判定された角度情報により、該当する目標の三次元情報が更新されていく（ＳＴ１０３）。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特開２０００−５５５９９号公報（第８ページ、図１０）
【非特許文献】
【０００８】
【非特許文献１】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
【発明の概要】
【発明が解決しようとする課題】
【０００９】
上述のように、目標の三次元情報推定にレンジパラメタライズド拡張カルマンフィルタを適用した、従来の信号処理装置では、複数の仮説距離を初期値として、拡張カルマンフィルタを並列動作させるため、計算量が多いという課題があった。
【課題を解決するための手段】
【００１０】
本実施形態の信号処理装置は、移動可能な二次元角度センサから、目標の角度情報とこの二次元角度センサの位置情報を入力し、前記目標の三次元情報を推定する信号処理装置であって、前記目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に基づいて、目標の概略距離を推定する概略距離推定手段と、前記目標の角度情報と前記二次元角度センサの位置情報及び前記概略距離推定手段からの目標の概略距離に基づいて、前記目標の三次元情報の推定を開始する三次元情報推定手段とを備えたことを特徴とする。
【００１１】
また、移動可能な二次元角度センサから、目標の角度情報とこの二次元角度センサの位置情報を入力し、前記目標の三次元情報を推定する信号処理装置であって、前記目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に基づいて、前記目標の三次元情報の推定を開始するか否かを判定する推定開始判定手段と、前記推定開始判定手段からの三次元情報の推定開始を判定された目標に対し、この目標の角度情報と前記二次元角度センサの位置情報に基づいて、この目標の三次元情報の推定を開始する三次元情報推定手段とを備えたことを特徴とする。
【００１２】
また、移動可能な二次元角度センサから、目標の角度情報とこの二次元角度センサの位置情報を入力し、前記目標の三次元情報を推定する信号処理装置であって、前記目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に基づいて、前記目標の三次元情報の推定を開始するか否かを判定する推定開始判定手段と、前記推定開始判定手段からの三次元情報の推定開始を判定された目標に対し、この目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に基づいて、目標の概略距離を推定する概略距離推定手段と、前記推定開始判定手段からの三次元情報の推定開始を判定された目標に対し、この目標の角度情報と前記二次元角度センサの位置情報ならびに前記概略距離推定手段からの目標の概略距離に基づいて、この目標の三次元情報の推定を開始する三次元情報推定手段とを備えたことを特徴とする。
【００１３】
また、移動可能な二次元角度センサから、目標の角度情報とこの二次元角度センサの位置情報を入力し、前記目標の三次元情報を推定する信号処理装置であって、前記目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に基づいて、目標の概略距離を推定する概略距離推定手段と、前記概略距離推定手段において概略距離を推定された目標に対し、この目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に基づいて、この目標の三次元情報の推定を開始するか否かを判定する推定開始判定手段と、前記推定開始判定手段からの三次元情報の推定開始を判定された目標に対し、この目標の角度情報と前記二次元角度センサの位置情報及び前記概略距離推定手段からの目標の概略距離に基づいて、この目標の三次元情報の推定を開始する三次元情報推定手段とを備えたことを特徴とする。
【００１４】
また、移動可能な二次元角度センサから、目標の角度情報とこの二次元角度センサの位置情報を入力し、前記目標の三次元情報を推定する信号処理装置であって、前記目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に基づいて、目標の概略距離を推定すると共に、前記目標の三次元情報の推定を開始するか否かを判定する概略距離推定・推定開始判定手段と、前記概略距離推定・推定開始判定手段からの三次元情報の推定開始を判定された目標に対し、この目標の角度情報と二次元角度センサの位置情報ならびに前記概略距離推定・推定開始判定手段からの目標の概略距離に基づいて、この目標の三次元情報の推定を開始する三次元情報推定手段とを備えたことを特徴とする。
【００１５】
また、更に、前記二次元角度センサの姿勢情報を受け取ると共に、この姿勢情報に基づいて前記目標の角度情報を補償する目標角度補償手段を備えたことを特徴とする。
【図面の簡単な説明】
【００１６】
【図１】第１の実施形態の信号処理装置を含む三次元情報推定システムの構成の一例を示すブロック図。
【図２】図１の三次元情報推定システムにおける信号処理装置４ａの動作を説明するためのフローチャート。
【図３】三次元情報推定部１１ａにおける目標の三次元情報推定のシミュレーション結果の一例を示す図。
【図４】第１の実施形態の三次元情報推定システムの変形例の構成の一例を示すブロック図。
【図５】第２の実施形態の信号処理装置を含む三次元情報推定システムの構成の一例を示すブロック図。
【図６】図５の三次元情報推定システムにおける信号処理装置４ｂの動作を説明するためのフローチャート。
【図７】第３の実施形態の信号処理装置を含む三次元情報推定システムの構成の一例を示すブロック図。
【図８】図７の三次元情報推定システムにおける信号処理装置４ｃの動作を説明するためのフローチャート。
【図９】第４の実施形態の信号処理装置を含む三次元情報推定システムの構成の一例を示すブロック図。
【図１０】図９の三次元情報推定システムにおける信号処理装置４ｄの動作を説明するためのフローチャート。
【図１１】第５の実施形態の信号処理装置を含む三次元情報推定システムの構成の一例を示すブロック図。
【図１２】図１０の三次元情報推定システムにおける信号処理装置４ｅの動作を説明するためのフローチャート。
【図１３】従来の三次元情報推定システムの構成の一例を示すブロック図。
【図１４】従来の三次元情報推定システムの信号処理装置における目標の三次元情報の確立の動作を説明するためのフローチャート。
【発明を実施するための形態】
【００１７】
以下に、本実施形態の信号処理装置について、図１乃至図１４を参照して説明する。なお、以下においては、背景技術の欄で説明した構成部分に相当する部分には、背景技術の欄で使用した符号と同じ符号を用いて説明する。
【実施例１】
【００１８】
図１は、第１の実施形態の信号処理装置を含む三次元情報推定システムの構成の一例を示すブロック図である。図１に例示したように、この三次元情報推定システムは、二次元角度センサ１、アクチュエータ２、センサ位置算出装置３、及び信号処理装置４ａから構成される。また、信号処理装置４ａは、概略距離推定部１２、及び三次元情報推定部１１ａから構成される。二次元角度センサ１は、目標の二次元角度情報を信号処理装置４ａへ出力する。アクチュエータ２は、二次元角度センサ１の位置を変更する。センサ位置算出装置３は、このアクチュエータ２によって変更された二次元角度センサ１の位置を算出し、二次元角度センサ１の位置情報として信号処理装置４ａへ出力する。
【００１９】
信号処理装置４ａは、二次元角度センサ１からの目標の角度情報とセンサ位置算出装置３からの二次元角度センサの位置情報とを入力して、目標の三次元情報の推定を行なう。ここに、信号処理装置４ａは、概略距離推定部１２、及び三次元情報推定部１１ａから構成されている。
【００２０】
概略距離推定部１２は、二次元角度センサ１からの目標の時系列の二次元角度情報と、センサ位置算出装置３からの二次元角度センサ１の時系列の位置情報とに基づいて、目標の概略距離を推定し、目標の距離範囲を設定する。以下、この概略距離推定部１２について詳述する。
【００２１】
目標の位置の変化量に対して、アクチュエータ２による二次元角度センサ１の位置の変化量が大きな場合、目標の概略距離の推定方法として、例えば、異なる位置で観測された２点以上の目標の時系列の二次元角度情報と時系列の位置情報とに基づいて、交会法より目標の概略距離を算出し推定することができる。交会法そのものは、二次元角度センサ１の位置の変化中における目標の位置の変化には対応していない。このため、目標の真の距離を算出することはできず、目標の位置の変化に起因する誤差を含んでいるが、これを目標の概略距離Ｒｅｓｔとして用いることはできる。そして、算出された目標の概略距離Ｒｅｓｔに基づいて、目標の距離範囲としてその最小値Ｒｍｉｎ及び最大値Ｒｍａｘを設定する。
【００２２】
一方、目標の位置の変化量に対して、アクチュエータ２による二次元角度センサ１の位置の変化量が大きくない場合、２点以上の仮説距離における尤度に基づいて、概略距離を推定することができる。
【００２３】
仮説距離をＲｎ、仮説距離Ｒｎにおける尤度をＬｎとすると、尤度Ｌｎは、以下の（１）式に示す関数で表される。
【００２４】
Ｌｎ＝ｆ（｛ＹＹ，ＸＸ，Ａ｝｜Ｒｎ） ・・・（１）
ここで、ＹＹは、二次元角度センサ１からの観測値の時系列データ、ＸＸは、センサ位置算出装置３からのセンサ位置の時系列データ、Ａは、基準値である。
【００２５】
より具体的には、仮説距離Ｒｎを用いて、目標の角度の観測値の時系列データＹＹを三次元ローカル直交座標系の観測値の時系列データＺＺ１に変換し、センサ位置（三次元ワールド直交座標系）の時系列データＸＸを加算することにより、目標の三次元ワールド直交座標系の観測値の時系列データＺＺ２に変換する。更に、変換された目標の三次元ワールド直交座標系の観測値の時系列データＺＺ２と基準値Ａとの間で統計的な処理を実施し、尤度Ｌｎに変換する。
【００２６】
概略距離Ｒｅｓｔは、仮説距離Ｒｉとその仮説距離Ｒｉにおける尤度Ｌｉとを用いて、以下の（２）式で表される。
【数１】
【００２７】
２点の仮説距離から概略距離を推定する場合の例を以下に示す。近距離側の仮説距離をＲ１、遠距離側の仮説距離をＲ２、それぞれの仮説距離における尤度をＬ１及びＬ２とする。このとき、概略距離Ｒｅｓｔは、仮説距離Ｒ１とＲ２、尤度Ｌ１とＬ２から、以下の（３）式により算出する。
【数２】
【００２８】
なお、二次元角度センサ１から信号強度が得られる場合、目標の概略距離の推定方法として、信号強度に基づいて目標までの距離を算出するように構成しても良い。更に、上記の目標の概略距離の推定方法と併用するように構成しても良い。
【００２９】
このようにして算出された目標の概略距離Ｒｅｓｔに基づいて目標の距離範囲を設定する。目標の距離範囲の最小値Ｒｍｉｎと最大値Ｒｍａｘを背景技術の例に示したようにＲｍｉｎ：２ｋｍ、Ｒｍａｘ：１２８ｋｍとし、並列フィルタ数Ｎｆを３、目標距離範囲として、近距離、遠距離の２種類に分類する場合、近距離範囲のＲｍｉｎとＲｍａｘは、それぞれ２ｋｍと１６ｋｍ、遠距離範囲のＲｍｉｎとＲｍａｘは、それぞれ１６ｋｍと１２８ｋｍとなる。上式で算出された概略距離Ｒｅｓｔが、１６ｋｍ未満の場合、目標距離範囲として、Ｒｍｉｎ：２ｋｍ、Ｒｍａｘ：１６ｋｍと設定され、１６ｋｍ以上の場合、目標距離範囲として、Ｒｍｉｎ：１６ｋｍ、Ｒｍａｘ：１２８ｋｍと設定される。
【００３０】
また、上記のように近距離、遠距離の二者選択であれば、概略距離を算出せず、尤度のみを用いて目標距離範囲を設定することもできる。具体的には、近距離側の尤度Ｌ１が、遠距離側の尤度Ｌ２よりも大きい場合、目標距離範囲として、Ｒｍｉｎ：２ｋｍ、Ｒｍａｘ：１６ｋｍと設定され、遠距離側の尤度Ｌ２が、近距離側の尤度Ｌ１よりも大きい場合、目標距離範囲として、Ｒｍｉｎ：１６ｋｍ、Ｒｍａｘ：１２８ｋｍと設定される。
【００３１】
三次元情報推定部１１ａは、概略距離推定部１２からの目標の距離範囲に基づいて、二次元角度センサ１からの目標の角度情報、及びセンサ位置算出装置３からの二次元角度センサの位置情報を処理し、目標の三次元情報の推定を行なう。三次元情報推定部１１ａにおける目標の三次元情報の推定処理としては、例えば非特許文献１や非特許文献２に示されているレンジパラメタライズド拡張カルマンフィルタを用いることができる。レンジパラメタライズド拡張カルマンフィルタでは、二次元角度センサ１からの目標の角度情報に対し、複数の仮説距離を初期値として拡張カルマンフィルタを並列動作させ、それぞれの拡張カルマンフィルタからの三次元情報を重み付け平均することで、目標の三次元情報を推定する。
【００３２】
三次元情報推定部１１ａにレンジパラメタライズド拡張カルマンフィルタを適用した場合には、概略距離推定部１２からの目標の距離範囲に基づいて複数の仮説距離を算出し、算出した仮説距離を初期値として拡張カルマンフィルタを並列動作させる。すなわち、従来の三次元情報推定部１１では、予め設定された目標の距離範囲に基づいて、複数の仮説距離を算出し、算出した仮説距離を初期値として、拡張カルマンフィルタを並列動作させたのに対し、三次元情報推定部１１ａでは、概略距離推定部１２からの目標の距離範囲に基づいて、複数の仮説距離を算出し、算出した仮説距離を初期値として、拡張カルマンフィルタを並列動作させるので、目標に応じて距離範囲が変化することが特徴である。このように、目標の概略距離を推定して目標の距離範囲を限定することにより、並列動作させるフィルタ数をより少なくすることが可能となり、推定精度を劣化させることなく計算量を低減できる。
【００３３】
また、三次元情報推定部１１ａにおける目標の三次元情報の推定処理として、例えば非特許文献３に示されているパーティクルフィルタを用いることもできる。パーティクルフィルタは、パーティクルの集合によって、推定を進めて行く手法であり、それぞれのパーティクルの処理としては、拡張カルマンフィルタよりも簡易な処理が行われるが、パーティクル数が多いことが特徴である。例えば非特許文献３のシミュレーションに用いられるパーティクル数は５０００としている。三次元情報推定部１１ａにパーティクルフィルタを適用し、上記の概略距離推定部１２と組み合わせる場合、上記したレンジパラメタライズド拡張カルマンフィルタの並列フィルタ数の代わりに、パーティクル数を低減することにより、計算量を低減することができる。
【００３４】
次に、前出の図１、及び図２のフローチャートを参照して、上述のように構成された第１の実施形態の三次元情報推定システムにおける信号処理装置４ａの動作を説明する。なお、以下においては、三次元情報推定部１１ａにおける目標の三次元情報の推定処理として、レンジパラメタライズド拡張カルマンフィルタを用いた場合をとりあげて説明する。
【００３５】
図２は、第１の実施形態の三次元情報推定システムにおける信号処理装置４ａの動作のうち、背景技術の図１４のフローチャートに例示したＳＴ１０４に対応する三次元情報の初期化処理を説明するためのフローチャートである。
【００３６】
まず、三次元情報推定システムが動作を開始すると、二次元角度センサ１で取得された目標の二次元角度情報が時間経過に沿って信号処理装置４ａに送られてくる（ＳＴ２０１）。これと同時並行して、センサ位置算出装置３で算出された二次元角度センサ１の位置情報も時間経過に沿って信号処理装置４ａに送られてくる（ＳＴ２０２）。信号処理装置４ａでは、これら送られてきた目標の二次元角度情報及び二次元角度センサ１の位置情報が、時系列のデータとして保持される（ＳＴ２０３）。
【００３７】
次いで、信号処理装置４ａ内の概略距離推定部１２において、保持された目標の時系列の二次元角度情報と二次元角度センサ１の時系列の位置情報とに基づいて、目標の概略距離が推定される。目標の概略距離の推定にあたっては、上述したように、例えば目標の位置変化に対して二次元角度センサ１の位置変化が大きな場合は交会法により推定し、二次元角度センサ１の位置変化が大きくない場合は２点以上の仮説距離の尤度に基づき推定する手法を適用する（ＳＴ２０４）。そして、続くステップにおいては、この推定した概略距離に基づいて目標の距離範囲が設定され、その結果が三次元情報推定部１１ａに送出される（ＳＴ２０５）。
【００３８】
次いで、三次元情報推定部１１ａにおいて、概略距離推定部１２からの目標の距離範囲に基づいて、二次元角度センサ１からの目標の角度情報、及びセンサ位置算出装置３からの二次元角度センサの位置情報が処理され、目標の三次元情報が推定される。本実施例においてはレンジパラメタライズド拡張カルマンフィルタを用い、概略距離推定部１２からの目標の距離範囲に基づいて複数の仮説距離を算出し、これらの算出した仮説距離を初期値として拡張カルマンフィルタを並列動作させている（ＳＴ２０６）。そして、この後は動作終了が指示されるまで上述の動作ステップを繰り返す（ＳＴ２０７）。なお、図２に示した初期化処理により一旦三次元情報が確立すると、背景技術の図１４のフローチャートに例示したように、確立した目標の三次元情報に対して相関のある二次元角度センサからの目標の角度情報を用いて、目標の三次元情報の更新が行われる（ＳＴ１０２とＳＴ１０３）。
【００３９】
三次元情報推定部１１ａにおける目標の三次元情報の推定結果の一例を図３に示す。図３は、予め設定された目標の距離範囲に基づき算出した仮説距離を初期値とし、並列フィルタ数を６、または３にした場合と、本実施例のように概略距離推定部１２からの目標の距離範囲に基づき算出した仮説距離を初期値とし、並列フィルタ数を３にした場合とを比較したシミュレーション結果の一例である。この図３のシミュレーション結果においては、並列動作させるフィルタ数を６から３に半減させても、その推定精度を十分に維持できていることが読み取れる。また、フィルタ数が同数であれば、より高い推定精度が得られることも読み取れる。
【００４０】
以上説明したように、本実施例においては、目標の二次元角度情報、及び二次元角度センサの位置情報から目標の三次元情報の推定を開始する際に、目標の概略距離を推定し、推定の対象とする距離範囲を限定した上でレンジパラメタライズド拡張カルマンフィルタによる推定処理を実行している。これにより、図３のシミュレーション結果における概略距離推定を適用せず並列フィルタ６で処理を行った結果と、概略距離推定を適用して距離範囲を限定し並列フィルタ数３で処理を行った結果との比較からも分かるように、推定精度を劣化させることなく、並列動作させる拡張カルマンフィルタ数を少なくすることができるため、信号処理装置の計算量を低減することができる。この結果、処理の高速化、多目標化、信号処理装置の小型化や低価格化等の二次的効果を得ることが出来る。また、図３のシミュレーション結果において並列フィルタ数が同数の３の場合の比較から分かるように、本実施例の手法によれば、同じ計算量でより高い性能（推定精度）を得ることが出来る。このため、同等の計算能力を有する信号処理装置を用いた場合、性能を向上させることも出来る。更に、並列フィルタ数を制御することにより、計算量を低減すると共に、推定性能を向上することもできる。
【００４１】
（変形例）
図４は第１の実施形態の三次元情報推定システムの変形例の構成の一例を示すブロック図である。この変形例は図１の構成に対し、センサ姿勢算出装置５が追加され、信号処理装置４ｆには、角度補償部１５が追加されている。
【００４２】
センサ姿勢算出装置５は、二次元角度センサ１の姿勢を算出し、二次元角度センサの姿勢情報として、信号処理装置４ｆへ出力する。角度補償部１５は、二次元角度センサ１からの角度情報とセンサ姿勢算出装置５からの姿勢情報に基づいて、目標の角度情報を補償する。これら以外は、図１の構成及びその処理と同じである。この変形例においては、例えばヘディング角、ロール角やピッチ角といった二次元角度センサ１の姿勢角が変化する場合にも三次元情報を推定することができる。
【００４３】
なお、図１及び図４に示したアクチュエータ２は、モータ等による駆動装置の他に、自動車や航空機等の二次元角度センサ１の位置を変化させるものであれば良い。
【００４４】
アクチュエータ２として姿勢が変化する航空機等を用いた場合、図１に示す構成では、二次元角度センサ１の姿勢の変化を補償するため、例えばジンバル機構による大型・高価格な安定プラットフォームを必要とするのに対し、図４に示す構成では、例えば小型・低価格なストラップダウン方式を採用することが可能となり、三次元情報推定システムを小型・低価格化できる。
【実施例２】
【００４５】
図５は、第２の実施形態の信号処理装置を含む三次元情報推定システムの構成の一例を示すブロック図である。図５に例示したように、この三次元情報推定システムは、二次元角度センサ１、アクチュエータ２、センサ位置算出装置３、及び信号処理装置４ｂから構成される。また、信号処理装置４ｂは、推定開始判定部１３、及び三次元情報推定部１１から構成される。二次元角度センサ１、アクチュエータ２、センサ位置算出装置３、及び三次元情報推定部１１は既出の構成と同様であるので、詳細な説明は省略する。
【００４６】
推定開始判定部１３は、二次元角度センサ１からの目標の時系列の角度情報とセンサ位置算出装置３からの二次元角度センサの時系列の位置情報を入力して、角度情報が真の目標のものか否かを判定し、三次元情報推定部１１に通知する。三次元情報推定部１１は、この判定結果の通知に基づき目標の三次元情報の推定を行う。判定に際しては、例えば非特許文献４に示されるＭ ｏｕｔ ｏｆ Ｎ（以下ではＭ／Ｎと表す）や、ＳＰＲＴ（ｓｅｑｕｅｎｔｉａｌ ｐｒｏｂａｂｉｌｉｔｙ ｒａｔｉｏ ｔｅｓｔ）の手法を適用し、算出された相関範囲Ｂに基づいて、真の目標であるか否かの判定を行う。
【００４７】
Ｍ／Ｎは、Ｎ回の観測で、Ｍ回以上検出があったものを真の目標と判定する処理である。すなわち、二次元角度センサ１からの目標の角度情報が、例えば熱雑音等による誤警報であった場合、誤警報が複数回検出される確率が低いのに対し、ある程度の探知確率を持った真の目標であった場合、熱雑音等よりも複数回検出される確率が高く、観測機会に対する検出回数の違いを用いて、誤警報か真の目標かの判定を行う。ＳＰＲＴも、複数回の検出データを処理するという意味で、Ｍ／Ｎと似ているが、観測機会に対する検出回数ではなく、検出の有無によって更新される値が、真の目標と判定する閾値以上となって真の目標と判定されるか、誤警報と判定する閾値以下となって誤警報と判定されるまでは、モニタリングが継続されることが特徴である。
【００４８】
また、推定開始判定部１３では、目標の相関範囲Ｂを算出する。目標の相関範囲Ｂについては、一般的に、三次元センサを用いた場合は目標の運動モデルに基づいて設定できるのに対し、二次元角度センサ１からの目標の角度情報には距離情報が含まれていないため、通常の方法では目標の相関範囲を設定することができない。このため、推定開始判定部１３では、三次元情報推定部１１で用いる目標距離範囲の最小値Ｒｍｉｎ、目標の最大速度Ｖｍａｘ及び観測値の時間間隔ｔｓを用いて、相関範囲Ｂを以下の（４）式により算出する。
【数３】
【００４９】
ここで、ｋは単位系の変換及び相関範囲の倍率設定に用いられる係数である。
【００５０】
さらに、三次元情報推定部１１への判定結果の通知に際しては、算出された相関範囲Ｂに基づきＭ／ＮやＳＰＲＴにより最初の判定をした後、真の目標と判定された時系列の二次元角度情報に対しては、例えば低次関数等を用いてフィッティング処理し、フィッティングラインとの誤差に基づき２回目の判定を行った上で、その判定結果を通知する。
【００５１】
次に、前出の図５、及び図６のフローチャートを参照して、上述のように構成された第２の実施形態の三次元情報推定システムにおける信号処理装置４ｂの動作を説明する。図６は、この第２の実施形態の三次元情報推定システムにおける信号処理装置４ｂの動作のうち、背景技術の図１４のフローチャートに例示したＳＴ１０４に対応する三次元情報の初期化処理の動作を説明するためのフローチャートである。
【００５２】
まず、三次元情報推定システムが動作を開始すると、二次元角度センサ１で取得された目標の二次元角度情報が時間経過に沿って信号処理装置４ｂに送られてくる（ＳＴ６０１）。これと同時並行して、センサ位置算出装置３で算出された二次元角度センサ１の位置情報も時間経過に沿って信号処理装置４ｂに送られてくる（ＳＴ６０２）。信号処理装置４ｂでは、これら送られてきた目標の二次元角度情報及び二次元角度センサ１の位置情報が、時系列のデータとして保持される（ＳＴ６０３）。
【００５３】
次いで、推定開始判定部１３では、保持された目標の時系列の二次元角度情報（時間間隔ｔｓ）に基づいて、（４）式により目標の相関範囲Ｂが算出される（ＳＴ６０４）。そして、この算出された相関範囲Ｂ、及び保持された目標の時系列の二次元角度情報と二次元角度センサ１の時系列の位置情報に基づいて、上述したＭ／ＮやＳＰＲＴの手法を用いて、受けとった目標の二次元角度情報が真の目標のものか、あるいは誤警報等かが判定される（ＳＴ６０５）。
【００５４】
真の目標と判定された場合には（ＳＴ６０５のＹ）、対象の時系列の二次元角度情報に対して、例えば一次元や二次元関数といった低次関数を用いてフィッティング処理が行われる（ＳＴ６０６）。そしてフィッティング直線／曲線との誤差に基づき真の目標か否かの２回目の判定が行われる（ＳＴ６０７）。その結果、真の目標であると判定された場合には（ＳＴ６０７のＹ）、三次元情報推定部１１に対して三次元情報の推定開始の指示を通知し（ＳＴ６０８）、この指示通知を受けて三次元情報推定部１１では、目標の三次元情報の推定が開始される。三次元情報の推定には、上述のようにレンジパラメタライズド拡張カルマンフィルタを適用できる（ＳＴ６０９）。
【００５５】
なお、ＳＴ６０５の動作ステップ、及びＳＴ６０７の動作ステップにおいて真の目標ではないと判定された場合には、二次元角度センサ１からの目標の二次元角度情報、及びセンサ位置算出装置３からの二次元角度センサ１の位置情報の受けとりが継続される。そして、この後は動作終了が指示されるまで、上述の動作ステップが繰り返される。（ＳＴ６１０）。
【００５６】
以上説明したように、本実施例においては、目標の二次元角度情報、及び二次元角度センサの位置情報から目標の三次元情報の推定を開始する際に、その二次元角度情報が真の目標によるものか、あるいは誤警報等かを判定し、真の目標であると判定された場合に、目標の三次元情報の推定処理を実行している。これにより、散発的な誤警報等を目標として三次元情報の推定処理が開始され、その後誤警報と判定されてその推定処理を終了するまでの間に発生する、レンジパラメタライズド拡張カルマンフィルタによる不要な演算処理を抑えることができ、信号処理装置の計算量を低減することができる。
【実施例３】
【００５７】
図７は、第３の実施形態の信号処理装置を含む三次元情報推定システムの構成の一例を示すブロック図である。図７に例示したように、この三次元情報推定システムは、二次元角度センサ１、アクチュエータ２、センサ位置算出装置３、及び信号処理装置４ｃから構成される。また、信号処理装置４ｃは、推定開始判定部１３、概略距離推定部１２、及び三次元情報推定部１１ａから構成される。これらの各構成は、上述した実施例１及び実施例２における構成と同様であり、詳細な説明は省略する。
【００５８】
この第３の実施形態においては、第２の実施形態にて詳述した推定開始判定部１３の判定結果を、第１の実施形態にて詳述した概略距離推定部１２に通知するようにし、概略距離推定部１２は、この推定開始部１３からの通知により対象目標の概略距離推定を行うとともに、三次元情報推定部１１ａは、その目標の三次元情報の推定処理を行うように構成されている。以下に、図７、及び図８のフローチャートを参照して、上述のように構成された第３の実施形態の三次元情報推定システムにおける信号処理装置４ｃの動作を説明する。図８は、この第３の実施形態の三次元情報推定システムにおける信号処理装置４ｃの動作のうち、背景技術の図１４のフローチャートに例示したＳＴ１０４に対応する三次元情報の初期化処理の動作を説明するためのフローチャートである。
【００５９】
まず、三次元情報推定システムが動作を開始すると、二次元角度センサ１で取得された目標の二次元角度情報が時間経過に沿って信号処理装置４ｃに送られてくる（ＳＴ８０１）。これと同時並行して、センサ位置算出装置３で算出された二次元角度センサ１の位置情報も時間経過に沿って信号処理装置４ｃに送られてくる（ＳＴ８０２）。信号処理装置４ｃでは、これら送られてきた目標の二次元角度情報及び二次元角度センサ１の位置情報が、時系列のデータとして保持される（ＳＴ８０３）。なお、これらＳＴ８０１〜ＳＴ８０３の動作ステップは、第１の実施形態における図２のＳＴ２０１〜ＳＴ２０３、または第２の実施形態における図６のＳＴ６０１〜ＳＴ６０３の動作ステップに対応する。
【００６０】
次いで、推定開始判定部１３では、保持された目標の時系列の二次元角度情報（時間間隔ｔｓ）に基づいて、（４）式により目標の相関範囲Ｂが算出される（ＳＴ８０４）。そして、この算出された相関範囲Ｂ、及び保持された目標の時系列の二次元角度情報と二次元角度センサ１の時系列の位置情報に基づいて、上述したＭ／ＮやＳＰＲＴの手法を用いて、受けとった目標の二次元角度情報が真の目標か、あるいは誤警報等かが判定される（ＳＴ８０５）。真の目標と判定された場合には（ＳＴ８０５のＹ）、対象の二次元角度情報の時系列のデータに対するフィッティング処理が行われ（ＳＴ８０６）、フィッティング直線／曲線との誤差に基づき真の目標か否かの２回目の判定が行われる（ＳＴ８０７）。その結果、真の目標であると判定された場合には（ＳＴ８０７のＹ）、概略距離推定部１２に対して概略距離の推定開始の指示が通知される（ＳＴ８０８）。なお、これらＳＴ８０４〜ＳＴ８０８の動作ステップは、第２の実施形態における図６のＳＴ６０４〜ＳＴ６０８の動作ステップに対応する。
【００６１】
次いで、信号処理装置４ｃ内の概略距離推定部１２において、時系列に保持された目標の二次元角度情報と二次元角度センサ１の位置情報とに基づいて、目標の概略距離が推定される。目標の概略距離の推定にあたっては、実施例１において詳述したように、例えば目標の位置変化に対して二次元角度センサ１の位置変化が大きな場合は交会法により推定し、二次元角度センサ１の位置変化が大きくない場合は２点以上の仮説距離における尤度に基づき推定する手法を適用する（ＳＴ８０９）。これに続けて、この推定された概略距離に基づいて目標の距離範囲が設定され、その結果が三次元情報推定部１１ａに送出される（ＳＴ８１０）。これらＳＴ８０９〜ＳＴ８１０の動作ステップは、第１の実施形態における図２のＳＴ２０４〜ＳＴ２０５の動作ステップに対応する。
【００６２】
次いで、三次元情報推定部１１ａにおいて、概略距離推定部１２からの目標の距離範囲に基づいて、ＳＴ８０８の動作ステップにて目標と判定された二次元角度センサ１からの目標の二次元角度情報、及びセンサ位置算出装置３からの二次元角度センサの位置情報を処理し、目標の三次元情報が推定される。三次元情報の推定には、実施例１で詳述したようにレンジパラメタライズド拡張カルマンフィルタを適用できる（ＳＴ８１１）。そして、この後は動作終了が指示されるまで、上述の動作ステップを繰り返す。（ＳＴ８１２）。
【００６３】
以上説明したように、本実施例においては、目標の二次元角度情報、及び二次元角度センサの位置情報から目標の三次元情報の推定を開始する際に、その二次元角度情報が真の目標によるものか、あるいは誤警報等かを判定し、真の目標であると判定された場合に、目標の三次元情報の推定処理を実行している。これにより、真の目標以外に対する、レンジパラメタライズド拡張カルマンフィルタによる不要な演算処理を抑制している。
【００６４】
また、目標の概略距離を推定し、推定の対象とする距離範囲を限定した上でレンジパラメタライズド拡張カルマンフィルタによる推定処理を実行している。これにより、推定精度を劣化させることなく、並列動作させる拡張カルマンフィルタ数を少なくすることができる。従って、信号処理装置の計算量を低減することができる。
【実施例４】
【００６５】
図９は、第４の実施形態の信号処理装置を含む三次元情報推定システムの構成の一例を示すブロック図である。この第４の実施形態について、図７に示した第３の実施形態の各部と同一の部分は同一の符号で示し、その説明は省略する。この第４の実施形態が第３の実施形態と異なる点は、二次元角度センサから送られてくる目標の二次元角度情報を信号処理装置で処理する際に、第３の実施形態においては、時系列の二次元角度情報に対してまず真の目標か否かの判定を行い、真の目標であると判定された後に、その時系列の二次元角度情報から概略距離を推定し三次元情報を推定したのに対し、第４の実施形態においては、時系列の二次元角度情報に対してまずその概略距離を推定した後に真の目標か否かの判定を行い、その判定結果により対象目標の三次元情報を推定するようにした点である。以下、前出の図７及び図８、ならびに図９のブロック図及び図１０のフローチャートを参照して、その相違点のみを説明する。
【００６６】
図９に例示したように、この三次元情報推定システムは、二次元角度センサ１、アクチュエータ２、センサ位置算出装置３、信号処理装置４ｄから構成される。また、信号処理装置４ｄは、概略距離推定部１２、推定開始判定部１３ａ、及び三次元情報推定部１１ａから構成される。推定開始判定部１３ａを除く各構成は、上述した第３の実施例における構成と同様に構成される。
【００６７】
推定開始判定部１３ａは、二次元角度センサ１からの目標の時系列の角度情報とセンサ位置算出装置３からの二次元角度センサの時系列の位置情報を入力して、角度情報が真の目標のものか否かを判定し、三次元情報推定部１１ａに通知する。この際に、目標の相関範囲Ｂを算出するが、その算出方法は推定開始判定部１３とは異なる。すなわち、推定開始判定部１３では、三次元情報推定部１１で用いる目標距離範囲の最小値Ｒｍｉｎ、目標の最大速度Ｖｍａｘ及び観測値の時間間隔ｔｓを用いて、（４）式のように算出したが、推定開始判定部１３ａは、目標距離範囲の最小値Ｒｍｉｎの代わりに概略距離推定部１２からの概略距離Ｒｅｓｔを用いて相関範囲Ｂを算出する。そして、Ｍ／ＮやＳＰＲＴを用いて真の目標であるか否かを判定し、真の目標であると判定された場合、三次元情報推定部１１ａに通知し三次元情報の推定開始を指示する。
【００６８】
次に、上述の様に構成された第４の実施形態の三次元情報推定システムにおける信号処理装置４ｄの動作について説明する。図１０は、この第４の実施形態の三次元情報推定システムにおける信号処理装置４ｄの動作のうち、背景技術の図１４のフローチャートに例示したＳＴ１０４に対応する三次元情報の初期化処理の動作を説明するためのフローチャートである。
【００６９】
まず、二次元角度センサ１からの目標の二次元角度情報、及びセンサ位置算出装置３で算出された二次元角度センサ１の位置情報が信号処理装置４ｄに送られ、信号処理装置４ｄ内に時系列のデータとして保持される（ＳＴ８０１〜ＳＴ８０３）。次いで、信号処理装置４ｄ内の概略距離推定部１２において、時系列のデータとして保持された目標の二次元角度情報と二次元角度センサ１の位置情報とに基づいて目標の概略距離Ｒｅｓｔが推定され、推定開始判定部１３ａに送出されるとともに、（ＳＴ８０９）、この推定された概略距離Ｒｅｓｔに基づいて目標の距離範囲が設定され、その結果が三次元情報推定部１１ａに送出される（ＳＴ８１０）。
【００７０】
推定開始判定部１３ａにおいては、上記したようにこの概略距離Ｒｅｓｔに基づき相関範囲Ｂが算出され（ＳＴ８０４ａ）、Ｍ／ＮやＳＰＲＴの手法を用いて、受けとった目標の二次元角度情報が真の目標か、あるいは誤警報等かが判定される（ＳＴ８０５）。真の目標であると判定された場合（ＳＴ８０５のＹ）、対象の二次元角度情報の時系列のデータのフィッティング処理が行われ（ＳＴ８０６）、フィッティング直線／曲線との誤差に基づき真の目標か否かの２回目の判定が行われる（ＳＴ８０７）。その結果、真の目標であると判定された場合には（ＳＴ８０７のＹ）、三次元情報推定部１１ａに通知して三次元情報の推定開始を指示し（ＳＴ８０８）、三次元情報推定部１１ａでは目標の三次元情報の推定が実行される（ＳＴ８１１）。そして、この後は動作終了が指示されるまで、上述の動作ステップを繰り返す。（ＳＴ８１２）。
【００７１】
以上説明したように、本実施例においても、目標の二次元角度情報、及び二次元角度センサの位置情報から目標の三次元情報を推定する際に、時系列の二次元角度情報から目標の概略距離を推定して、後段において推定の対象とする距離範囲を限定するとともに、その二次元角度情報が真の目標によるものか、あるいは誤警報等かを判定し、真の目標であると判定された場合に、推定された概略距離を用いたレンジパラメタライズド拡張カルマンフィルタによる推定処理を実行している。これにより、真の目標以外に対するレンジパラメタライズド拡張カルマンフィルタによる不要な演算処理を抑制できるとともに、推定演算において推定精度を劣化させることなく、並列動作させる拡張カルマンフィルタ数を少なくすることができる。従って、実施例３と同様に、信号処理装置の計算量を低減することができる。
【００７２】
更に、実施例４では、相関範囲Ｂを算出する際に、実施例３で用いる目標距離範囲の最小値Ｒｍｉｎの代わりに、概略距離推定部１２からの概略距離Ｒｅｓｔを用いることにより、遠方に存在する目標に対しては、実施例３よりも狭い相関範囲で判定を行うことが可能となり、誤警報を真の目標と誤判定する頻度を低減することができる。
【実施例５】
【００７３】
図１１は、第５の実施形態の信号処理装置を含む三次元情報推定システムの構成の一例を示すブロック図である。図１１に例示したように、この三次元情報推定システムは、二次元角度センサ１、アクチュエータ２、センサ位置算出装置３、及び信号処理装置４ｅから構成される。また、信号処理装置４ｅは、総合判定部１４、及び三次元情報推定部１１ａから構成される。これらの中で、二次元角度センサ１、アクチュエータ２、センサ位置算出装置３、三次元情報推定部１１ａの各部については、上述した実施例１乃至実施例４における構成と同様であり、詳細な説明は省略する。
【００７４】
総合判定部１４は、二次元角度センサ１からの目標の時系列の角度情報とセンサ位置算出装置３からの二次元角度センサの時系列の位置情報に基づいて、目標の概略距離を推定して目標の距離範囲を設定すると共に、二次元角度センサ１からの目標の角度情報が真の目標によるものか否かにより目標の三次元情報の推定開始を判定する。すなわち、総合判定部１４では、上述した実施例２の推定開始判定部１３、実施例３の概略距離推定部１２及び実施例４の推定開始判定部１３ａに対応する処理が行われる。
【００７５】
具体例としては、実施例２における推定開始判定部１３と同様に、時系列の観測値（二次元角度センサ１からの目標の時系列の角度情報と二次元角度センサ１の時系列の位置情報）に基づいて真の目標であるか否かの判定を行い、真の目標であると判定された場合、実施例３の概略距離推定部１２と同様に、真の目標であると判定された時系列の観測値に基づいて、概略距離Ｒｅｓｔを算出する。概略距離Ｒｅｓｔが算出されると、実施例４の推定開始判定部１３ａと同様に、目標距離範囲の最小値Ｒｍｉｎの代わりに概略距離Ｒｅｓｔを用いて、相関範囲Ｂを算出し、Ｍ／ＮやＳＰＲＴを用いて、真の目標であるか否かの判定を行う。真の目標であると判定された場合、三次元情報推定部１１ａに三次元情報の推定開始を指示する。
【００７６】
次に、上述のように構成された第５の実施形態の三次元情報推定システムにおける信号処理装置４ｅの動作について説明する。図１２は、この第５の実施形態の三次元情報推定システムにおける信号処理装置４ｅの動作のうち、背景技術の図１４のフローチャートに例示したＳＴ１０４に対応する三次元情報の初期化処理の動作を説明するためのフローチャートである。
【００７７】
まず、二次元角度センサ１からの目標の二次元角度情報、及びセンサ位置算出装置３で算出された二次元角度センサ１の位置情報が信号処理装置４ｅに送られ、信号処理装置４ｅ内にそれぞれ時系列のデータとして保持される（ＳＴ１２０１〜ＳＴ１２０３）。次いで、総合判定部１４において、保持された目標の時系列の二次元角度情報（時間間隔ｔｓ）に基づいて、（４）式により目標の相関範囲Ｂが算出され（ＳＴ１２０４）、この相関範囲Ｂ、及び保持された目標の時系列の二次元角度情報と二次元角度センサ１の時系列の位置情報に基づいて、上述したＭ／ＮやＳＰＲＴの手法を用いて、受けとった目標の二次元角度情報が真の目標か、あるいは誤警報等の目標以外かが判定される（ＳＴ１２０５）。
【００７８】
真の目標と判定された場合には（ＳＴ１２０５のＹ）、さらに対象の二次元角度情報の時系列のデータのフィッティング処理が行われ（ＳＴ１２０６）、フィッティング直線／曲線との誤差に基づき真の目標か否かの２回目の判定が行われる（ＳＴ１２０７）。その結果、真の目標であると判定された場合には、目標の概略距離の推定が開始される（ＳＴ１２０７のＹ）。一方、ＳＴ１２０５の動作ステップ、及びＳＴ１２０７の動作ステップにおいて真の目標ではないと判定された場合には（ＳＴ１２０５のＮ、及びＳＴ１２０７のＮ）、二次元角度センサ１からの目標の二次元角度情報、及びセンサ位置算出装置３からの二次元角度センサ１の位置情報の受けとりが継続される。なお、これらＳＴ１２０４〜ＳＴ１２０７の動作ステップは、例えば実施例２における推定開始判定部１３の動作に相当し、図６のＳＴ６０４〜ＳＴ６０７の動作ステップに対応する。
【００７９】
次いで、総合判定部１４においては、時系列に保持された目標の二次元角度情報と二次元角度センサ１の位置情報とに基づいて、目標の概略距離Ｒｅｓｔが推定される。推定にあたっては、例えば交会法や２点以上の仮説距離における尤度に基づく手法を用いることができる（ＳＴ１２０８）。さらに、この推定された概略距離Ｒｅｓｔに基づいて目標の距離範囲が設定される（ＳＴ１２０９）。なお、これらＳＴ１２０８〜１２０９の動作ステップは、例えば実施例３における概略距離推定部１２の動作に相当し、図８のＳＴ８０９〜ＳＴ８１０の動作ステップに対応する。
【００８０】
次いで、総合判定部１４においては、目標の三次元情報の推定を開始するか否かの判定が再度行われる。すなわち、前出のＳＴ１２０８の動作ステップにて算出された概略距離Ｒｅｓｔに基づき目標の相関範囲Ｂが算出され（ＳＴ１２１０）、この相関範囲Ｂに基づいて受けとった二次元角度情報が真の目標か否かが判定される。なお、相関範囲に基づく同様の判定をＳＴ１２０５の動作ステップで行っているが、ＳＴ１２０５の動作ステップでの判定の際には、目標の距離範囲の最小値Ｒｍｉｎに基づき算出された相関範囲を用いているのに対し、この動作ステップでは概略距離Ｒｅｓｔに基づき算出された相関範囲を用いている。従って、より狭い相関範囲で判定を行うので、この動作ステップにより、目標としての確からしさを向上させることができる（ＳＴ１２１１）。
【００８１】
この動作ステップで真の目標であると判定された場合（ＳＴ１２１１のＹ）、対象の二次元角度情報の時系列のデータのフィッティング処理が行われ（ＳＴ１２１２）、さらにフィッティング直線／曲線との誤差に基づき真の目標か否かが判定される（ＳＴ１２１３）。その結果、真の目標であると判定された場合には（ＳＴ１２１３のＹ）、三次元情報推定部１１ａに通知して三次元情報の推定開始を指示し（ＳＴ１２１４）、三次元情報推定部１１ａでは目標の三次元情報の推定が行われる（ＳＴ１２１５）。そして、この後は動作終了が指示されるまで、上述の動作ステップを繰り返す。（ＳＴ１２１６）。なお、これらＳＴ１２１０〜１２１４の動作ステップは、例えば実施例４における推定開始判定部１３ａの動作に相当し、図１０のＳＴ８０４ａ〜ＳＴ８０８の動作ステップに対応する。
【００８２】
なお、実施例２における推定開始判定部１３では、第１のステップで、Ｍ／ＮやＳＰＲＴにより、算出された相関範囲Ｂに基づいて、真の目標であるか否かの判定を行った後、第２のステップで、一次元や二次元関数といった低次関数を用いてフィッティング処理され、フィッティング直線／曲線との誤差に基づいて、真の目標であるか否かの２回目の判定を行うが、総合判定部１４の推定開始判定部１３に相当する処理（ＳＴ１２０４〜ＳＴ１２０７）では、上記第１のステップの処理（Ｍ／ＮやＳＰＲＴによる判定）のみ行い、総合判定部１４の推定開始判定部１３ａに相当する処理（ＳＴ１２１０〜ＳＴ１２１４）においてのみ、上記第２のステップの処理（フィッティング直線／曲線による判定）を実施するように構成することができる。
【００８３】
以上説明したように、本実施例においても、目標の二次元角度情報、及び二次元角度センサの位置情報から目標の三次元情報を推定する際に、時系列の二次元角度情報から目標の概略距離を推定して、後段において推定の対象とする距離範囲を限定するとともに、その二次元角度情報が真の目標によるものか、あるいは誤警報等かを判定し、真の目標であると判定された場合に、推定された概略距離を用いたレンジパラメタライズド拡張カルマンフィルタによる推定処理を実行している。これにより、真の目標以外に対するレンジパラメタライズド拡張カルマンフィルタによる不要な演算処理を抑制できるとともに、推定演算において推定精度を劣化させることなく、並列動作させる拡張カルマンフィルタ数を少なくすることができる。
【００８４】
更に、総合判定部１４では、時系列の観測値に対して総合的な判定を行うように構成したため、実施例３に対しては、真の目標か否かを判定する際に、より狭い相関範囲での判定が可能となり、誤警報を真の目標として誤判定する頻度を低減することができる。また実施例４に対しては、概略距離の算出よりも計算量の少ない目標の判定処理によって、概略距離算出の計算量を低減することができる。
【００８５】
なお、本発明は、上記した実施形態そのままに限定されるものではなく、実施段階ではその要旨を逸脱しない範囲で構成要素を変形して具体化できる。また、上記実施形態に開示されている複数の構成要素の適宜な組み合せにより種々の発明を形成できる。例えば、実施形態に示される全構成要素から幾つかの構成要素を削除してもよい。さらに、異なる実施形態に亘る構成要素を適宜組み合せてもよい。
【符号の説明】
【００８６】
１ 二次元角度センサ
２ アクチュエータ
３ センサ位置算出装置
４、４ａ、４ｂ、４ｃ、４ｄ、４ｅ、４ｆ 信号処理装置
５ センサ姿勢算出装置
１１、１１ａ 三次元情報推定部
１２ 概略距離推定部
１３、１３ａ 推定開始判定部
１４ 総合判定部
１５ 角度補償部
【技術分野】
【０００１】
本発明の実施形態は、移動可能な二次元角度センサからの情報を入力して、目標の三次元情報を推定する信号処理装置の計算量を低減する技術に関する。
【背景技術】
【０００２】
移動可能な二次元角度センサで目標の二次元角度情報を取得し、この二次元角度情報から目標の三次元情報を推定する三次元情報推定システムが知られている。この種の、従来の三次元情報推定システムの一例を図１３に示す。図１３は、従来の三次元情報推定システムの構成の一例を示すブロック図である。従来の三次元情報推定システムは、二次元角度センサ１、アクチュエータ２、センサ位置算出装置３、及び信号処理装置４から構成されている。さらに、信号処理装置４は、三次元情報推定部１１を備えている。
【０００３】
二次元角度センサ１は、目標の二次元角度情報を取得して信号処理装置４に出力する。アクチュエータ２は、二次元角度センサ１の位置を変更する。センサ位置算出装置３は、このアクチュエータ２によって変更された二次元角度センサ１の位置を算出し、二次元角度センサ１の位置情報として信号処理装置４へ出力する。信号処理装置４は、二次元角度センサ１からの目標の二次元角度情報とセンサ位置算出装置３からの二次元角度センサの位置情報を入力して、三次元情報推定部１１により目標の三次元情報の推定を行う。
【０００４】
三次元情報推定部１１における目標の三次元情報の推定処理としては、非特許文献１や非特許文献２に示されているレンジパラメタライズド拡張カルマンフィルタ（ｒａｎｇｅ−ｐａｒａｍｅｔｅｒｉｓｅｄ ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ ｆｉｌｔｅｒ）や、非特許文献３に示されているパーティクルフィルタを用いることができる。以下では、三次元情報推定部１１に、レンジパラメタライズド拡張カルマンフィルタを適用した場合について説明する。
【０００５】
レンジパラメタライズド拡張カルマンフィルタは、二次元角度センサ１からの目標の角度情報に対し、複数の仮説距離を初期値として拡張カルマンフィルタを並列動作させ、それぞれの拡張カルマンフィルタからの三次元情報を重み付け平均することで、目標の三次元情報を推定する。レンジパラメタライズド拡張カルマンフィルタにおける仮説距離の初期値は、目標距離範囲の最小値Ｒｍｉｎと最大値Ｒｍａｘ及び並列フィルタ数Ｎｆから設定される。例えば、Ｒｍｉｎ：２ｋｍ、Ｒｍａｘ：１２８ｋｍ、Ｎｆ：６の場合、それぞれの拡張カルマンフィルタに対する仮説距離の初期値は、３ｋｍ、６ｋｍ、１２ｋｍ、２４ｋｍ、４８ｋｍ、９６ｋｍと設定される。それぞれの拡張カルマンフィルタでは、二次元角度センサ１からの目標の角度情報と仮説距離の初期値の組み合わせ、及び二次元角度センサ１の位置情報に基づいて、三次元情報の推定処理を開始する。それぞれの拡張カルマンフィルタからの三次元情報は、最終的には、重み付け平均されて出力される。上記の初期化処理により一旦三次元情報が確立すると、確立した目標の三次元情報に対して相関のある二次元角度センサからの目標の角度情報を用いて、目標の三次元情報の更新が行われる。
【０００６】
すなわち、図１４のフローチャートに例示したように、取得した二次元角度センサからの目標の角度情報と二次元角度センサの位置情報に対する相関処理等によって既知の目標か否かが判定され（ＳＴ１０２）、その結果新たな目標と判断されると、上記の初期化処理により対象目標の三次元情報が確立される（ＳＴ１０４）。その後はＳＴ１０２のステップにおいて、確立された三次元情報に対して相関を有すると判定された角度情報により、該当する目標の三次元情報が更新されていく（ＳＴ１０３）。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特開２０００−５５５９９号公報（第８ページ、図１０）
【非特許文献】
【０００８】
【非特許文献１】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
【発明の概要】
【発明が解決しようとする課題】
【０００９】
上述のように、目標の三次元情報推定にレンジパラメタライズド拡張カルマンフィルタを適用した、従来の信号処理装置では、複数の仮説距離を初期値として、拡張カルマンフィルタを並列動作させるため、計算量が多いという課題があった。
【課題を解決するための手段】
【００１０】
本実施形態の信号処理装置は、移動可能な二次元角度センサから、目標の角度情報とこの二次元角度センサの位置情報を入力し、前記目標の三次元情報を推定する信号処理装置であって、前記目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に基づいて、目標の概略距離を推定する概略距離推定手段と、前記目標の角度情報と前記二次元角度センサの位置情報及び前記概略距離推定手段からの目標の概略距離に基づいて、前記目標の三次元情報の推定を開始する三次元情報推定手段とを備えたことを特徴とする。
【００１１】
また、移動可能な二次元角度センサから、目標の角度情報とこの二次元角度センサの位置情報を入力し、前記目標の三次元情報を推定する信号処理装置であって、前記目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に基づいて、前記目標の三次元情報の推定を開始するか否かを判定する推定開始判定手段と、前記推定開始判定手段からの三次元情報の推定開始を判定された目標に対し、この目標の角度情報と前記二次元角度センサの位置情報に基づいて、この目標の三次元情報の推定を開始する三次元情報推定手段とを備えたことを特徴とする。
【００１２】
また、移動可能な二次元角度センサから、目標の角度情報とこの二次元角度センサの位置情報を入力し、前記目標の三次元情報を推定する信号処理装置であって、前記目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に基づいて、前記目標の三次元情報の推定を開始するか否かを判定する推定開始判定手段と、前記推定開始判定手段からの三次元情報の推定開始を判定された目標に対し、この目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に基づいて、目標の概略距離を推定する概略距離推定手段と、前記推定開始判定手段からの三次元情報の推定開始を判定された目標に対し、この目標の角度情報と前記二次元角度センサの位置情報ならびに前記概略距離推定手段からの目標の概略距離に基づいて、この目標の三次元情報の推定を開始する三次元情報推定手段とを備えたことを特徴とする。
【００１３】
また、移動可能な二次元角度センサから、目標の角度情報とこの二次元角度センサの位置情報を入力し、前記目標の三次元情報を推定する信号処理装置であって、前記目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に基づいて、目標の概略距離を推定する概略距離推定手段と、前記概略距離推定手段において概略距離を推定された目標に対し、この目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に基づいて、この目標の三次元情報の推定を開始するか否かを判定する推定開始判定手段と、前記推定開始判定手段からの三次元情報の推定開始を判定された目標に対し、この目標の角度情報と前記二次元角度センサの位置情報及び前記概略距離推定手段からの目標の概略距離に基づいて、この目標の三次元情報の推定を開始する三次元情報推定手段とを備えたことを特徴とする。
【００１４】
また、移動可能な二次元角度センサから、目標の角度情報とこの二次元角度センサの位置情報を入力し、前記目標の三次元情報を推定する信号処理装置であって、前記目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に基づいて、目標の概略距離を推定すると共に、前記目標の三次元情報の推定を開始するか否かを判定する概略距離推定・推定開始判定手段と、前記概略距離推定・推定開始判定手段からの三次元情報の推定開始を判定された目標に対し、この目標の角度情報と二次元角度センサの位置情報ならびに前記概略距離推定・推定開始判定手段からの目標の概略距離に基づいて、この目標の三次元情報の推定を開始する三次元情報推定手段とを備えたことを特徴とする。
【００１５】
また、更に、前記二次元角度センサの姿勢情報を受け取ると共に、この姿勢情報に基づいて前記目標の角度情報を補償する目標角度補償手段を備えたことを特徴とする。
【図面の簡単な説明】
【００１６】
【図１】第１の実施形態の信号処理装置を含む三次元情報推定システムの構成の一例を示すブロック図。
【図２】図１の三次元情報推定システムにおける信号処理装置４ａの動作を説明するためのフローチャート。
【図３】三次元情報推定部１１ａにおける目標の三次元情報推定のシミュレーション結果の一例を示す図。
【図４】第１の実施形態の三次元情報推定システムの変形例の構成の一例を示すブロック図。
【図５】第２の実施形態の信号処理装置を含む三次元情報推定システムの構成の一例を示すブロック図。
【図６】図５の三次元情報推定システムにおける信号処理装置４ｂの動作を説明するためのフローチャート。
【図７】第３の実施形態の信号処理装置を含む三次元情報推定システムの構成の一例を示すブロック図。
【図８】図７の三次元情報推定システムにおける信号処理装置４ｃの動作を説明するためのフローチャート。
【図９】第４の実施形態の信号処理装置を含む三次元情報推定システムの構成の一例を示すブロック図。
【図１０】図９の三次元情報推定システムにおける信号処理装置４ｄの動作を説明するためのフローチャート。
【図１１】第５の実施形態の信号処理装置を含む三次元情報推定システムの構成の一例を示すブロック図。
【図１２】図１０の三次元情報推定システムにおける信号処理装置４ｅの動作を説明するためのフローチャート。
【図１３】従来の三次元情報推定システムの構成の一例を示すブロック図。
【図１４】従来の三次元情報推定システムの信号処理装置における目標の三次元情報の確立の動作を説明するためのフローチャート。
【発明を実施するための形態】
【００１７】
以下に、本実施形態の信号処理装置について、図１乃至図１４を参照して説明する。なお、以下においては、背景技術の欄で説明した構成部分に相当する部分には、背景技術の欄で使用した符号と同じ符号を用いて説明する。
【実施例１】
【００１８】
図１は、第１の実施形態の信号処理装置を含む三次元情報推定システムの構成の一例を示すブロック図である。図１に例示したように、この三次元情報推定システムは、二次元角度センサ１、アクチュエータ２、センサ位置算出装置３、及び信号処理装置４ａから構成される。また、信号処理装置４ａは、概略距離推定部１２、及び三次元情報推定部１１ａから構成される。二次元角度センサ１は、目標の二次元角度情報を信号処理装置４ａへ出力する。アクチュエータ２は、二次元角度センサ１の位置を変更する。センサ位置算出装置３は、このアクチュエータ２によって変更された二次元角度センサ１の位置を算出し、二次元角度センサ１の位置情報として信号処理装置４ａへ出力する。
【００１９】
信号処理装置４ａは、二次元角度センサ１からの目標の角度情報とセンサ位置算出装置３からの二次元角度センサの位置情報とを入力して、目標の三次元情報の推定を行なう。ここに、信号処理装置４ａは、概略距離推定部１２、及び三次元情報推定部１１ａから構成されている。
【００２０】
概略距離推定部１２は、二次元角度センサ１からの目標の時系列の二次元角度情報と、センサ位置算出装置３からの二次元角度センサ１の時系列の位置情報とに基づいて、目標の概略距離を推定し、目標の距離範囲を設定する。以下、この概略距離推定部１２について詳述する。
【００２１】
目標の位置の変化量に対して、アクチュエータ２による二次元角度センサ１の位置の変化量が大きな場合、目標の概略距離の推定方法として、例えば、異なる位置で観測された２点以上の目標の時系列の二次元角度情報と時系列の位置情報とに基づいて、交会法より目標の概略距離を算出し推定することができる。交会法そのものは、二次元角度センサ１の位置の変化中における目標の位置の変化には対応していない。このため、目標の真の距離を算出することはできず、目標の位置の変化に起因する誤差を含んでいるが、これを目標の概略距離Ｒｅｓｔとして用いることはできる。そして、算出された目標の概略距離Ｒｅｓｔに基づいて、目標の距離範囲としてその最小値Ｒｍｉｎ及び最大値Ｒｍａｘを設定する。
【００２２】
一方、目標の位置の変化量に対して、アクチュエータ２による二次元角度センサ１の位置の変化量が大きくない場合、２点以上の仮説距離における尤度に基づいて、概略距離を推定することができる。
【００２３】
仮説距離をＲｎ、仮説距離Ｒｎにおける尤度をＬｎとすると、尤度Ｌｎは、以下の（１）式に示す関数で表される。
【００２４】
Ｌｎ＝ｆ（｛ＹＹ，ＸＸ，Ａ｝｜Ｒｎ） ・・・（１）
ここで、ＹＹは、二次元角度センサ１からの観測値の時系列データ、ＸＸは、センサ位置算出装置３からのセンサ位置の時系列データ、Ａは、基準値である。
【００２５】
より具体的には、仮説距離Ｒｎを用いて、目標の角度の観測値の時系列データＹＹを三次元ローカル直交座標系の観測値の時系列データＺＺ１に変換し、センサ位置（三次元ワールド直交座標系）の時系列データＸＸを加算することにより、目標の三次元ワールド直交座標系の観測値の時系列データＺＺ２に変換する。更に、変換された目標の三次元ワールド直交座標系の観測値の時系列データＺＺ２と基準値Ａとの間で統計的な処理を実施し、尤度Ｌｎに変換する。
【００２６】
概略距離Ｒｅｓｔは、仮説距離Ｒｉとその仮説距離Ｒｉにおける尤度Ｌｉとを用いて、以下の（２）式で表される。
【数１】
【００２７】
２点の仮説距離から概略距離を推定する場合の例を以下に示す。近距離側の仮説距離をＲ１、遠距離側の仮説距離をＲ２、それぞれの仮説距離における尤度をＬ１及びＬ２とする。このとき、概略距離Ｒｅｓｔは、仮説距離Ｒ１とＲ２、尤度Ｌ１とＬ２から、以下の（３）式により算出する。
【数２】
【００２８】
なお、二次元角度センサ１から信号強度が得られる場合、目標の概略距離の推定方法として、信号強度に基づいて目標までの距離を算出するように構成しても良い。更に、上記の目標の概略距離の推定方法と併用するように構成しても良い。
【００２９】
このようにして算出された目標の概略距離Ｒｅｓｔに基づいて目標の距離範囲を設定する。目標の距離範囲の最小値Ｒｍｉｎと最大値Ｒｍａｘを背景技術の例に示したようにＲｍｉｎ：２ｋｍ、Ｒｍａｘ：１２８ｋｍとし、並列フィルタ数Ｎｆを３、目標距離範囲として、近距離、遠距離の２種類に分類する場合、近距離範囲のＲｍｉｎとＲｍａｘは、それぞれ２ｋｍと１６ｋｍ、遠距離範囲のＲｍｉｎとＲｍａｘは、それぞれ１６ｋｍと１２８ｋｍとなる。上式で算出された概略距離Ｒｅｓｔが、１６ｋｍ未満の場合、目標距離範囲として、Ｒｍｉｎ：２ｋｍ、Ｒｍａｘ：１６ｋｍと設定され、１６ｋｍ以上の場合、目標距離範囲として、Ｒｍｉｎ：１６ｋｍ、Ｒｍａｘ：１２８ｋｍと設定される。
【００３０】
また、上記のように近距離、遠距離の二者選択であれば、概略距離を算出せず、尤度のみを用いて目標距離範囲を設定することもできる。具体的には、近距離側の尤度Ｌ１が、遠距離側の尤度Ｌ２よりも大きい場合、目標距離範囲として、Ｒｍｉｎ：２ｋｍ、Ｒｍａｘ：１６ｋｍと設定され、遠距離側の尤度Ｌ２が、近距離側の尤度Ｌ１よりも大きい場合、目標距離範囲として、Ｒｍｉｎ：１６ｋｍ、Ｒｍａｘ：１２８ｋｍと設定される。
【００３１】
三次元情報推定部１１ａは、概略距離推定部１２からの目標の距離範囲に基づいて、二次元角度センサ１からの目標の角度情報、及びセンサ位置算出装置３からの二次元角度センサの位置情報を処理し、目標の三次元情報の推定を行なう。三次元情報推定部１１ａにおける目標の三次元情報の推定処理としては、例えば非特許文献１や非特許文献２に示されているレンジパラメタライズド拡張カルマンフィルタを用いることができる。レンジパラメタライズド拡張カルマンフィルタでは、二次元角度センサ１からの目標の角度情報に対し、複数の仮説距離を初期値として拡張カルマンフィルタを並列動作させ、それぞれの拡張カルマンフィルタからの三次元情報を重み付け平均することで、目標の三次元情報を推定する。
【００３２】
三次元情報推定部１１ａにレンジパラメタライズド拡張カルマンフィルタを適用した場合には、概略距離推定部１２からの目標の距離範囲に基づいて複数の仮説距離を算出し、算出した仮説距離を初期値として拡張カルマンフィルタを並列動作させる。すなわち、従来の三次元情報推定部１１では、予め設定された目標の距離範囲に基づいて、複数の仮説距離を算出し、算出した仮説距離を初期値として、拡張カルマンフィルタを並列動作させたのに対し、三次元情報推定部１１ａでは、概略距離推定部１２からの目標の距離範囲に基づいて、複数の仮説距離を算出し、算出した仮説距離を初期値として、拡張カルマンフィルタを並列動作させるので、目標に応じて距離範囲が変化することが特徴である。このように、目標の概略距離を推定して目標の距離範囲を限定することにより、並列動作させるフィルタ数をより少なくすることが可能となり、推定精度を劣化させることなく計算量を低減できる。
【００３３】
また、三次元情報推定部１１ａにおける目標の三次元情報の推定処理として、例えば非特許文献３に示されているパーティクルフィルタを用いることもできる。パーティクルフィルタは、パーティクルの集合によって、推定を進めて行く手法であり、それぞれのパーティクルの処理としては、拡張カルマンフィルタよりも簡易な処理が行われるが、パーティクル数が多いことが特徴である。例えば非特許文献３のシミュレーションに用いられるパーティクル数は５０００としている。三次元情報推定部１１ａにパーティクルフィルタを適用し、上記の概略距離推定部１２と組み合わせる場合、上記したレンジパラメタライズド拡張カルマンフィルタの並列フィルタ数の代わりに、パーティクル数を低減することにより、計算量を低減することができる。
【００３４】
次に、前出の図１、及び図２のフローチャートを参照して、上述のように構成された第１の実施形態の三次元情報推定システムにおける信号処理装置４ａの動作を説明する。なお、以下においては、三次元情報推定部１１ａにおける目標の三次元情報の推定処理として、レンジパラメタライズド拡張カルマンフィルタを用いた場合をとりあげて説明する。
【００３５】
図２は、第１の実施形態の三次元情報推定システムにおける信号処理装置４ａの動作のうち、背景技術の図１４のフローチャートに例示したＳＴ１０４に対応する三次元情報の初期化処理を説明するためのフローチャートである。
【００３６】
まず、三次元情報推定システムが動作を開始すると、二次元角度センサ１で取得された目標の二次元角度情報が時間経過に沿って信号処理装置４ａに送られてくる（ＳＴ２０１）。これと同時並行して、センサ位置算出装置３で算出された二次元角度センサ１の位置情報も時間経過に沿って信号処理装置４ａに送られてくる（ＳＴ２０２）。信号処理装置４ａでは、これら送られてきた目標の二次元角度情報及び二次元角度センサ１の位置情報が、時系列のデータとして保持される（ＳＴ２０３）。
【００３７】
次いで、信号処理装置４ａ内の概略距離推定部１２において、保持された目標の時系列の二次元角度情報と二次元角度センサ１の時系列の位置情報とに基づいて、目標の概略距離が推定される。目標の概略距離の推定にあたっては、上述したように、例えば目標の位置変化に対して二次元角度センサ１の位置変化が大きな場合は交会法により推定し、二次元角度センサ１の位置変化が大きくない場合は２点以上の仮説距離の尤度に基づき推定する手法を適用する（ＳＴ２０４）。そして、続くステップにおいては、この推定した概略距離に基づいて目標の距離範囲が設定され、その結果が三次元情報推定部１１ａに送出される（ＳＴ２０５）。
【００３８】
次いで、三次元情報推定部１１ａにおいて、概略距離推定部１２からの目標の距離範囲に基づいて、二次元角度センサ１からの目標の角度情報、及びセンサ位置算出装置３からの二次元角度センサの位置情報が処理され、目標の三次元情報が推定される。本実施例においてはレンジパラメタライズド拡張カルマンフィルタを用い、概略距離推定部１２からの目標の距離範囲に基づいて複数の仮説距離を算出し、これらの算出した仮説距離を初期値として拡張カルマンフィルタを並列動作させている（ＳＴ２０６）。そして、この後は動作終了が指示されるまで上述の動作ステップを繰り返す（ＳＴ２０７）。なお、図２に示した初期化処理により一旦三次元情報が確立すると、背景技術の図１４のフローチャートに例示したように、確立した目標の三次元情報に対して相関のある二次元角度センサからの目標の角度情報を用いて、目標の三次元情報の更新が行われる（ＳＴ１０２とＳＴ１０３）。
【００３９】
三次元情報推定部１１ａにおける目標の三次元情報の推定結果の一例を図３に示す。図３は、予め設定された目標の距離範囲に基づき算出した仮説距離を初期値とし、並列フィルタ数を６、または３にした場合と、本実施例のように概略距離推定部１２からの目標の距離範囲に基づき算出した仮説距離を初期値とし、並列フィルタ数を３にした場合とを比較したシミュレーション結果の一例である。この図３のシミュレーション結果においては、並列動作させるフィルタ数を６から３に半減させても、その推定精度を十分に維持できていることが読み取れる。また、フィルタ数が同数であれば、より高い推定精度が得られることも読み取れる。
【００４０】
以上説明したように、本実施例においては、目標の二次元角度情報、及び二次元角度センサの位置情報から目標の三次元情報の推定を開始する際に、目標の概略距離を推定し、推定の対象とする距離範囲を限定した上でレンジパラメタライズド拡張カルマンフィルタによる推定処理を実行している。これにより、図３のシミュレーション結果における概略距離推定を適用せず並列フィルタ６で処理を行った結果と、概略距離推定を適用して距離範囲を限定し並列フィルタ数３で処理を行った結果との比較からも分かるように、推定精度を劣化させることなく、並列動作させる拡張カルマンフィルタ数を少なくすることができるため、信号処理装置の計算量を低減することができる。この結果、処理の高速化、多目標化、信号処理装置の小型化や低価格化等の二次的効果を得ることが出来る。また、図３のシミュレーション結果において並列フィルタ数が同数の３の場合の比較から分かるように、本実施例の手法によれば、同じ計算量でより高い性能（推定精度）を得ることが出来る。このため、同等の計算能力を有する信号処理装置を用いた場合、性能を向上させることも出来る。更に、並列フィルタ数を制御することにより、計算量を低減すると共に、推定性能を向上することもできる。
【００４１】
（変形例）
図４は第１の実施形態の三次元情報推定システムの変形例の構成の一例を示すブロック図である。この変形例は図１の構成に対し、センサ姿勢算出装置５が追加され、信号処理装置４ｆには、角度補償部１５が追加されている。
【００４２】
センサ姿勢算出装置５は、二次元角度センサ１の姿勢を算出し、二次元角度センサの姿勢情報として、信号処理装置４ｆへ出力する。角度補償部１５は、二次元角度センサ１からの角度情報とセンサ姿勢算出装置５からの姿勢情報に基づいて、目標の角度情報を補償する。これら以外は、図１の構成及びその処理と同じである。この変形例においては、例えばヘディング角、ロール角やピッチ角といった二次元角度センサ１の姿勢角が変化する場合にも三次元情報を推定することができる。
【００４３】
なお、図１及び図４に示したアクチュエータ２は、モータ等による駆動装置の他に、自動車や航空機等の二次元角度センサ１の位置を変化させるものであれば良い。
【００４４】
アクチュエータ２として姿勢が変化する航空機等を用いた場合、図１に示す構成では、二次元角度センサ１の姿勢の変化を補償するため、例えばジンバル機構による大型・高価格な安定プラットフォームを必要とするのに対し、図４に示す構成では、例えば小型・低価格なストラップダウン方式を採用することが可能となり、三次元情報推定システムを小型・低価格化できる。
【実施例２】
【００４５】
図５は、第２の実施形態の信号処理装置を含む三次元情報推定システムの構成の一例を示すブロック図である。図５に例示したように、この三次元情報推定システムは、二次元角度センサ１、アクチュエータ２、センサ位置算出装置３、及び信号処理装置４ｂから構成される。また、信号処理装置４ｂは、推定開始判定部１３、及び三次元情報推定部１１から構成される。二次元角度センサ１、アクチュエータ２、センサ位置算出装置３、及び三次元情報推定部１１は既出の構成と同様であるので、詳細な説明は省略する。
【００４６】
推定開始判定部１３は、二次元角度センサ１からの目標の時系列の角度情報とセンサ位置算出装置３からの二次元角度センサの時系列の位置情報を入力して、角度情報が真の目標のものか否かを判定し、三次元情報推定部１１に通知する。三次元情報推定部１１は、この判定結果の通知に基づき目標の三次元情報の推定を行う。判定に際しては、例えば非特許文献４に示されるＭ ｏｕｔ ｏｆ Ｎ（以下ではＭ／Ｎと表す）や、ＳＰＲＴ（ｓｅｑｕｅｎｔｉａｌ ｐｒｏｂａｂｉｌｉｔｙ ｒａｔｉｏ ｔｅｓｔ）の手法を適用し、算出された相関範囲Ｂに基づいて、真の目標であるか否かの判定を行う。
【００４７】
Ｍ／Ｎは、Ｎ回の観測で、Ｍ回以上検出があったものを真の目標と判定する処理である。すなわち、二次元角度センサ１からの目標の角度情報が、例えば熱雑音等による誤警報であった場合、誤警報が複数回検出される確率が低いのに対し、ある程度の探知確率を持った真の目標であった場合、熱雑音等よりも複数回検出される確率が高く、観測機会に対する検出回数の違いを用いて、誤警報か真の目標かの判定を行う。ＳＰＲＴも、複数回の検出データを処理するという意味で、Ｍ／Ｎと似ているが、観測機会に対する検出回数ではなく、検出の有無によって更新される値が、真の目標と判定する閾値以上となって真の目標と判定されるか、誤警報と判定する閾値以下となって誤警報と判定されるまでは、モニタリングが継続されることが特徴である。
【００４８】
また、推定開始判定部１３では、目標の相関範囲Ｂを算出する。目標の相関範囲Ｂについては、一般的に、三次元センサを用いた場合は目標の運動モデルに基づいて設定できるのに対し、二次元角度センサ１からの目標の角度情報には距離情報が含まれていないため、通常の方法では目標の相関範囲を設定することができない。このため、推定開始判定部１３では、三次元情報推定部１１で用いる目標距離範囲の最小値Ｒｍｉｎ、目標の最大速度Ｖｍａｘ及び観測値の時間間隔ｔｓを用いて、相関範囲Ｂを以下の（４）式により算出する。
【数３】
【００４９】
ここで、ｋは単位系の変換及び相関範囲の倍率設定に用いられる係数である。
【００５０】
さらに、三次元情報推定部１１への判定結果の通知に際しては、算出された相関範囲Ｂに基づきＭ／ＮやＳＰＲＴにより最初の判定をした後、真の目標と判定された時系列の二次元角度情報に対しては、例えば低次関数等を用いてフィッティング処理し、フィッティングラインとの誤差に基づき２回目の判定を行った上で、その判定結果を通知する。
【００５１】
次に、前出の図５、及び図６のフローチャートを参照して、上述のように構成された第２の実施形態の三次元情報推定システムにおける信号処理装置４ｂの動作を説明する。図６は、この第２の実施形態の三次元情報推定システムにおける信号処理装置４ｂの動作のうち、背景技術の図１４のフローチャートに例示したＳＴ１０４に対応する三次元情報の初期化処理の動作を説明するためのフローチャートである。
【００５２】
まず、三次元情報推定システムが動作を開始すると、二次元角度センサ１で取得された目標の二次元角度情報が時間経過に沿って信号処理装置４ｂに送られてくる（ＳＴ６０１）。これと同時並行して、センサ位置算出装置３で算出された二次元角度センサ１の位置情報も時間経過に沿って信号処理装置４ｂに送られてくる（ＳＴ６０２）。信号処理装置４ｂでは、これら送られてきた目標の二次元角度情報及び二次元角度センサ１の位置情報が、時系列のデータとして保持される（ＳＴ６０３）。
【００５３】
次いで、推定開始判定部１３では、保持された目標の時系列の二次元角度情報（時間間隔ｔｓ）に基づいて、（４）式により目標の相関範囲Ｂが算出される（ＳＴ６０４）。そして、この算出された相関範囲Ｂ、及び保持された目標の時系列の二次元角度情報と二次元角度センサ１の時系列の位置情報に基づいて、上述したＭ／ＮやＳＰＲＴの手法を用いて、受けとった目標の二次元角度情報が真の目標のものか、あるいは誤警報等かが判定される（ＳＴ６０５）。
【００５４】
真の目標と判定された場合には（ＳＴ６０５のＹ）、対象の時系列の二次元角度情報に対して、例えば一次元や二次元関数といった低次関数を用いてフィッティング処理が行われる（ＳＴ６０６）。そしてフィッティング直線／曲線との誤差に基づき真の目標か否かの２回目の判定が行われる（ＳＴ６０７）。その結果、真の目標であると判定された場合には（ＳＴ６０７のＹ）、三次元情報推定部１１に対して三次元情報の推定開始の指示を通知し（ＳＴ６０８）、この指示通知を受けて三次元情報推定部１１では、目標の三次元情報の推定が開始される。三次元情報の推定には、上述のようにレンジパラメタライズド拡張カルマンフィルタを適用できる（ＳＴ６０９）。
【００５５】
なお、ＳＴ６０５の動作ステップ、及びＳＴ６０７の動作ステップにおいて真の目標ではないと判定された場合には、二次元角度センサ１からの目標の二次元角度情報、及びセンサ位置算出装置３からの二次元角度センサ１の位置情報の受けとりが継続される。そして、この後は動作終了が指示されるまで、上述の動作ステップが繰り返される。（ＳＴ６１０）。
【００５６】
以上説明したように、本実施例においては、目標の二次元角度情報、及び二次元角度センサの位置情報から目標の三次元情報の推定を開始する際に、その二次元角度情報が真の目標によるものか、あるいは誤警報等かを判定し、真の目標であると判定された場合に、目標の三次元情報の推定処理を実行している。これにより、散発的な誤警報等を目標として三次元情報の推定処理が開始され、その後誤警報と判定されてその推定処理を終了するまでの間に発生する、レンジパラメタライズド拡張カルマンフィルタによる不要な演算処理を抑えることができ、信号処理装置の計算量を低減することができる。
【実施例３】
【００５７】
図７は、第３の実施形態の信号処理装置を含む三次元情報推定システムの構成の一例を示すブロック図である。図７に例示したように、この三次元情報推定システムは、二次元角度センサ１、アクチュエータ２、センサ位置算出装置３、及び信号処理装置４ｃから構成される。また、信号処理装置４ｃは、推定開始判定部１３、概略距離推定部１２、及び三次元情報推定部１１ａから構成される。これらの各構成は、上述した実施例１及び実施例２における構成と同様であり、詳細な説明は省略する。
【００５８】
この第３の実施形態においては、第２の実施形態にて詳述した推定開始判定部１３の判定結果を、第１の実施形態にて詳述した概略距離推定部１２に通知するようにし、概略距離推定部１２は、この推定開始部１３からの通知により対象目標の概略距離推定を行うとともに、三次元情報推定部１１ａは、その目標の三次元情報の推定処理を行うように構成されている。以下に、図７、及び図８のフローチャートを参照して、上述のように構成された第３の実施形態の三次元情報推定システムにおける信号処理装置４ｃの動作を説明する。図８は、この第３の実施形態の三次元情報推定システムにおける信号処理装置４ｃの動作のうち、背景技術の図１４のフローチャートに例示したＳＴ１０４に対応する三次元情報の初期化処理の動作を説明するためのフローチャートである。
【００５９】
まず、三次元情報推定システムが動作を開始すると、二次元角度センサ１で取得された目標の二次元角度情報が時間経過に沿って信号処理装置４ｃに送られてくる（ＳＴ８０１）。これと同時並行して、センサ位置算出装置３で算出された二次元角度センサ１の位置情報も時間経過に沿って信号処理装置４ｃに送られてくる（ＳＴ８０２）。信号処理装置４ｃでは、これら送られてきた目標の二次元角度情報及び二次元角度センサ１の位置情報が、時系列のデータとして保持される（ＳＴ８０３）。なお、これらＳＴ８０１〜ＳＴ８０３の動作ステップは、第１の実施形態における図２のＳＴ２０１〜ＳＴ２０３、または第２の実施形態における図６のＳＴ６０１〜ＳＴ６０３の動作ステップに対応する。
【００６０】
次いで、推定開始判定部１３では、保持された目標の時系列の二次元角度情報（時間間隔ｔｓ）に基づいて、（４）式により目標の相関範囲Ｂが算出される（ＳＴ８０４）。そして、この算出された相関範囲Ｂ、及び保持された目標の時系列の二次元角度情報と二次元角度センサ１の時系列の位置情報に基づいて、上述したＭ／ＮやＳＰＲＴの手法を用いて、受けとった目標の二次元角度情報が真の目標か、あるいは誤警報等かが判定される（ＳＴ８０５）。真の目標と判定された場合には（ＳＴ８０５のＹ）、対象の二次元角度情報の時系列のデータに対するフィッティング処理が行われ（ＳＴ８０６）、フィッティング直線／曲線との誤差に基づき真の目標か否かの２回目の判定が行われる（ＳＴ８０７）。その結果、真の目標であると判定された場合には（ＳＴ８０７のＹ）、概略距離推定部１２に対して概略距離の推定開始の指示が通知される（ＳＴ８０８）。なお、これらＳＴ８０４〜ＳＴ８０８の動作ステップは、第２の実施形態における図６のＳＴ６０４〜ＳＴ６０８の動作ステップに対応する。
【００６１】
次いで、信号処理装置４ｃ内の概略距離推定部１２において、時系列に保持された目標の二次元角度情報と二次元角度センサ１の位置情報とに基づいて、目標の概略距離が推定される。目標の概略距離の推定にあたっては、実施例１において詳述したように、例えば目標の位置変化に対して二次元角度センサ１の位置変化が大きな場合は交会法により推定し、二次元角度センサ１の位置変化が大きくない場合は２点以上の仮説距離における尤度に基づき推定する手法を適用する（ＳＴ８０９）。これに続けて、この推定された概略距離に基づいて目標の距離範囲が設定され、その結果が三次元情報推定部１１ａに送出される（ＳＴ８１０）。これらＳＴ８０９〜ＳＴ８１０の動作ステップは、第１の実施形態における図２のＳＴ２０４〜ＳＴ２０５の動作ステップに対応する。
【００６２】
次いで、三次元情報推定部１１ａにおいて、概略距離推定部１２からの目標の距離範囲に基づいて、ＳＴ８０８の動作ステップにて目標と判定された二次元角度センサ１からの目標の二次元角度情報、及びセンサ位置算出装置３からの二次元角度センサの位置情報を処理し、目標の三次元情報が推定される。三次元情報の推定には、実施例１で詳述したようにレンジパラメタライズド拡張カルマンフィルタを適用できる（ＳＴ８１１）。そして、この後は動作終了が指示されるまで、上述の動作ステップを繰り返す。（ＳＴ８１２）。
【００６３】
以上説明したように、本実施例においては、目標の二次元角度情報、及び二次元角度センサの位置情報から目標の三次元情報の推定を開始する際に、その二次元角度情報が真の目標によるものか、あるいは誤警報等かを判定し、真の目標であると判定された場合に、目標の三次元情報の推定処理を実行している。これにより、真の目標以外に対する、レンジパラメタライズド拡張カルマンフィルタによる不要な演算処理を抑制している。
【００６４】
また、目標の概略距離を推定し、推定の対象とする距離範囲を限定した上でレンジパラメタライズド拡張カルマンフィルタによる推定処理を実行している。これにより、推定精度を劣化させることなく、並列動作させる拡張カルマンフィルタ数を少なくすることができる。従って、信号処理装置の計算量を低減することができる。
【実施例４】
【００６５】
図９は、第４の実施形態の信号処理装置を含む三次元情報推定システムの構成の一例を示すブロック図である。この第４の実施形態について、図７に示した第３の実施形態の各部と同一の部分は同一の符号で示し、その説明は省略する。この第４の実施形態が第３の実施形態と異なる点は、二次元角度センサから送られてくる目標の二次元角度情報を信号処理装置で処理する際に、第３の実施形態においては、時系列の二次元角度情報に対してまず真の目標か否かの判定を行い、真の目標であると判定された後に、その時系列の二次元角度情報から概略距離を推定し三次元情報を推定したのに対し、第４の実施形態においては、時系列の二次元角度情報に対してまずその概略距離を推定した後に真の目標か否かの判定を行い、その判定結果により対象目標の三次元情報を推定するようにした点である。以下、前出の図７及び図８、ならびに図９のブロック図及び図１０のフローチャートを参照して、その相違点のみを説明する。
【００６６】
図９に例示したように、この三次元情報推定システムは、二次元角度センサ１、アクチュエータ２、センサ位置算出装置３、信号処理装置４ｄから構成される。また、信号処理装置４ｄは、概略距離推定部１２、推定開始判定部１３ａ、及び三次元情報推定部１１ａから構成される。推定開始判定部１３ａを除く各構成は、上述した第３の実施例における構成と同様に構成される。
【００６７】
推定開始判定部１３ａは、二次元角度センサ１からの目標の時系列の角度情報とセンサ位置算出装置３からの二次元角度センサの時系列の位置情報を入力して、角度情報が真の目標のものか否かを判定し、三次元情報推定部１１ａに通知する。この際に、目標の相関範囲Ｂを算出するが、その算出方法は推定開始判定部１３とは異なる。すなわち、推定開始判定部１３では、三次元情報推定部１１で用いる目標距離範囲の最小値Ｒｍｉｎ、目標の最大速度Ｖｍａｘ及び観測値の時間間隔ｔｓを用いて、（４）式のように算出したが、推定開始判定部１３ａは、目標距離範囲の最小値Ｒｍｉｎの代わりに概略距離推定部１２からの概略距離Ｒｅｓｔを用いて相関範囲Ｂを算出する。そして、Ｍ／ＮやＳＰＲＴを用いて真の目標であるか否かを判定し、真の目標であると判定された場合、三次元情報推定部１１ａに通知し三次元情報の推定開始を指示する。
【００６８】
次に、上述の様に構成された第４の実施形態の三次元情報推定システムにおける信号処理装置４ｄの動作について説明する。図１０は、この第４の実施形態の三次元情報推定システムにおける信号処理装置４ｄの動作のうち、背景技術の図１４のフローチャートに例示したＳＴ１０４に対応する三次元情報の初期化処理の動作を説明するためのフローチャートである。
【００６９】
まず、二次元角度センサ１からの目標の二次元角度情報、及びセンサ位置算出装置３で算出された二次元角度センサ１の位置情報が信号処理装置４ｄに送られ、信号処理装置４ｄ内に時系列のデータとして保持される（ＳＴ８０１〜ＳＴ８０３）。次いで、信号処理装置４ｄ内の概略距離推定部１２において、時系列のデータとして保持された目標の二次元角度情報と二次元角度センサ１の位置情報とに基づいて目標の概略距離Ｒｅｓｔが推定され、推定開始判定部１３ａに送出されるとともに、（ＳＴ８０９）、この推定された概略距離Ｒｅｓｔに基づいて目標の距離範囲が設定され、その結果が三次元情報推定部１１ａに送出される（ＳＴ８１０）。
【００７０】
推定開始判定部１３ａにおいては、上記したようにこの概略距離Ｒｅｓｔに基づき相関範囲Ｂが算出され（ＳＴ８０４ａ）、Ｍ／ＮやＳＰＲＴの手法を用いて、受けとった目標の二次元角度情報が真の目標か、あるいは誤警報等かが判定される（ＳＴ８０５）。真の目標であると判定された場合（ＳＴ８０５のＹ）、対象の二次元角度情報の時系列のデータのフィッティング処理が行われ（ＳＴ８０６）、フィッティング直線／曲線との誤差に基づき真の目標か否かの２回目の判定が行われる（ＳＴ８０７）。その結果、真の目標であると判定された場合には（ＳＴ８０７のＹ）、三次元情報推定部１１ａに通知して三次元情報の推定開始を指示し（ＳＴ８０８）、三次元情報推定部１１ａでは目標の三次元情報の推定が実行される（ＳＴ８１１）。そして、この後は動作終了が指示されるまで、上述の動作ステップを繰り返す。（ＳＴ８１２）。
【００７１】
以上説明したように、本実施例においても、目標の二次元角度情報、及び二次元角度センサの位置情報から目標の三次元情報を推定する際に、時系列の二次元角度情報から目標の概略距離を推定して、後段において推定の対象とする距離範囲を限定するとともに、その二次元角度情報が真の目標によるものか、あるいは誤警報等かを判定し、真の目標であると判定された場合に、推定された概略距離を用いたレンジパラメタライズド拡張カルマンフィルタによる推定処理を実行している。これにより、真の目標以外に対するレンジパラメタライズド拡張カルマンフィルタによる不要な演算処理を抑制できるとともに、推定演算において推定精度を劣化させることなく、並列動作させる拡張カルマンフィルタ数を少なくすることができる。従って、実施例３と同様に、信号処理装置の計算量を低減することができる。
【００７２】
更に、実施例４では、相関範囲Ｂを算出する際に、実施例３で用いる目標距離範囲の最小値Ｒｍｉｎの代わりに、概略距離推定部１２からの概略距離Ｒｅｓｔを用いることにより、遠方に存在する目標に対しては、実施例３よりも狭い相関範囲で判定を行うことが可能となり、誤警報を真の目標と誤判定する頻度を低減することができる。
【実施例５】
【００７３】
図１１は、第５の実施形態の信号処理装置を含む三次元情報推定システムの構成の一例を示すブロック図である。図１１に例示したように、この三次元情報推定システムは、二次元角度センサ１、アクチュエータ２、センサ位置算出装置３、及び信号処理装置４ｅから構成される。また、信号処理装置４ｅは、総合判定部１４、及び三次元情報推定部１１ａから構成される。これらの中で、二次元角度センサ１、アクチュエータ２、センサ位置算出装置３、三次元情報推定部１１ａの各部については、上述した実施例１乃至実施例４における構成と同様であり、詳細な説明は省略する。
【００７４】
総合判定部１４は、二次元角度センサ１からの目標の時系列の角度情報とセンサ位置算出装置３からの二次元角度センサの時系列の位置情報に基づいて、目標の概略距離を推定して目標の距離範囲を設定すると共に、二次元角度センサ１からの目標の角度情報が真の目標によるものか否かにより目標の三次元情報の推定開始を判定する。すなわち、総合判定部１４では、上述した実施例２の推定開始判定部１３、実施例３の概略距離推定部１２及び実施例４の推定開始判定部１３ａに対応する処理が行われる。
【００７５】
具体例としては、実施例２における推定開始判定部１３と同様に、時系列の観測値（二次元角度センサ１からの目標の時系列の角度情報と二次元角度センサ１の時系列の位置情報）に基づいて真の目標であるか否かの判定を行い、真の目標であると判定された場合、実施例３の概略距離推定部１２と同様に、真の目標であると判定された時系列の観測値に基づいて、概略距離Ｒｅｓｔを算出する。概略距離Ｒｅｓｔが算出されると、実施例４の推定開始判定部１３ａと同様に、目標距離範囲の最小値Ｒｍｉｎの代わりに概略距離Ｒｅｓｔを用いて、相関範囲Ｂを算出し、Ｍ／ＮやＳＰＲＴを用いて、真の目標であるか否かの判定を行う。真の目標であると判定された場合、三次元情報推定部１１ａに三次元情報の推定開始を指示する。
【００７６】
次に、上述のように構成された第５の実施形態の三次元情報推定システムにおける信号処理装置４ｅの動作について説明する。図１２は、この第５の実施形態の三次元情報推定システムにおける信号処理装置４ｅの動作のうち、背景技術の図１４のフローチャートに例示したＳＴ１０４に対応する三次元情報の初期化処理の動作を説明するためのフローチャートである。
【００７７】
まず、二次元角度センサ１からの目標の二次元角度情報、及びセンサ位置算出装置３で算出された二次元角度センサ１の位置情報が信号処理装置４ｅに送られ、信号処理装置４ｅ内にそれぞれ時系列のデータとして保持される（ＳＴ１２０１〜ＳＴ１２０３）。次いで、総合判定部１４において、保持された目標の時系列の二次元角度情報（時間間隔ｔｓ）に基づいて、（４）式により目標の相関範囲Ｂが算出され（ＳＴ１２０４）、この相関範囲Ｂ、及び保持された目標の時系列の二次元角度情報と二次元角度センサ１の時系列の位置情報に基づいて、上述したＭ／ＮやＳＰＲＴの手法を用いて、受けとった目標の二次元角度情報が真の目標か、あるいは誤警報等の目標以外かが判定される（ＳＴ１２０５）。
【００７８】
真の目標と判定された場合には（ＳＴ１２０５のＹ）、さらに対象の二次元角度情報の時系列のデータのフィッティング処理が行われ（ＳＴ１２０６）、フィッティング直線／曲線との誤差に基づき真の目標か否かの２回目の判定が行われる（ＳＴ１２０７）。その結果、真の目標であると判定された場合には、目標の概略距離の推定が開始される（ＳＴ１２０７のＹ）。一方、ＳＴ１２０５の動作ステップ、及びＳＴ１２０７の動作ステップにおいて真の目標ではないと判定された場合には（ＳＴ１２０５のＮ、及びＳＴ１２０７のＮ）、二次元角度センサ１からの目標の二次元角度情報、及びセンサ位置算出装置３からの二次元角度センサ１の位置情報の受けとりが継続される。なお、これらＳＴ１２０４〜ＳＴ１２０７の動作ステップは、例えば実施例２における推定開始判定部１３の動作に相当し、図６のＳＴ６０４〜ＳＴ６０７の動作ステップに対応する。
【００７９】
次いで、総合判定部１４においては、時系列に保持された目標の二次元角度情報と二次元角度センサ１の位置情報とに基づいて、目標の概略距離Ｒｅｓｔが推定される。推定にあたっては、例えば交会法や２点以上の仮説距離における尤度に基づく手法を用いることができる（ＳＴ１２０８）。さらに、この推定された概略距離Ｒｅｓｔに基づいて目標の距離範囲が設定される（ＳＴ１２０９）。なお、これらＳＴ１２０８〜１２０９の動作ステップは、例えば実施例３における概略距離推定部１２の動作に相当し、図８のＳＴ８０９〜ＳＴ８１０の動作ステップに対応する。
【００８０】
次いで、総合判定部１４においては、目標の三次元情報の推定を開始するか否かの判定が再度行われる。すなわち、前出のＳＴ１２０８の動作ステップにて算出された概略距離Ｒｅｓｔに基づき目標の相関範囲Ｂが算出され（ＳＴ１２１０）、この相関範囲Ｂに基づいて受けとった二次元角度情報が真の目標か否かが判定される。なお、相関範囲に基づく同様の判定をＳＴ１２０５の動作ステップで行っているが、ＳＴ１２０５の動作ステップでの判定の際には、目標の距離範囲の最小値Ｒｍｉｎに基づき算出された相関範囲を用いているのに対し、この動作ステップでは概略距離Ｒｅｓｔに基づき算出された相関範囲を用いている。従って、より狭い相関範囲で判定を行うので、この動作ステップにより、目標としての確からしさを向上させることができる（ＳＴ１２１１）。
【００８１】
この動作ステップで真の目標であると判定された場合（ＳＴ１２１１のＹ）、対象の二次元角度情報の時系列のデータのフィッティング処理が行われ（ＳＴ１２１２）、さらにフィッティング直線／曲線との誤差に基づき真の目標か否かが判定される（ＳＴ１２１３）。その結果、真の目標であると判定された場合には（ＳＴ１２１３のＹ）、三次元情報推定部１１ａに通知して三次元情報の推定開始を指示し（ＳＴ１２１４）、三次元情報推定部１１ａでは目標の三次元情報の推定が行われる（ＳＴ１２１５）。そして、この後は動作終了が指示されるまで、上述の動作ステップを繰り返す。（ＳＴ１２１６）。なお、これらＳＴ１２１０〜１２１４の動作ステップは、例えば実施例４における推定開始判定部１３ａの動作に相当し、図１０のＳＴ８０４ａ〜ＳＴ８０８の動作ステップに対応する。
【００８２】
なお、実施例２における推定開始判定部１３では、第１のステップで、Ｍ／ＮやＳＰＲＴにより、算出された相関範囲Ｂに基づいて、真の目標であるか否かの判定を行った後、第２のステップで、一次元や二次元関数といった低次関数を用いてフィッティング処理され、フィッティング直線／曲線との誤差に基づいて、真の目標であるか否かの２回目の判定を行うが、総合判定部１４の推定開始判定部１３に相当する処理（ＳＴ１２０４〜ＳＴ１２０７）では、上記第１のステップの処理（Ｍ／ＮやＳＰＲＴによる判定）のみ行い、総合判定部１４の推定開始判定部１３ａに相当する処理（ＳＴ１２１０〜ＳＴ１２１４）においてのみ、上記第２のステップの処理（フィッティング直線／曲線による判定）を実施するように構成することができる。
【００８３】
以上説明したように、本実施例においても、目標の二次元角度情報、及び二次元角度センサの位置情報から目標の三次元情報を推定する際に、時系列の二次元角度情報から目標の概略距離を推定して、後段において推定の対象とする距離範囲を限定するとともに、その二次元角度情報が真の目標によるものか、あるいは誤警報等かを判定し、真の目標であると判定された場合に、推定された概略距離を用いたレンジパラメタライズド拡張カルマンフィルタによる推定処理を実行している。これにより、真の目標以外に対するレンジパラメタライズド拡張カルマンフィルタによる不要な演算処理を抑制できるとともに、推定演算において推定精度を劣化させることなく、並列動作させる拡張カルマンフィルタ数を少なくすることができる。
【００８４】
更に、総合判定部１４では、時系列の観測値に対して総合的な判定を行うように構成したため、実施例３に対しては、真の目標か否かを判定する際に、より狭い相関範囲での判定が可能となり、誤警報を真の目標として誤判定する頻度を低減することができる。また実施例４に対しては、概略距離の算出よりも計算量の少ない目標の判定処理によって、概略距離算出の計算量を低減することができる。
【００８５】
なお、本発明は、上記した実施形態そのままに限定されるものではなく、実施段階ではその要旨を逸脱しない範囲で構成要素を変形して具体化できる。また、上記実施形態に開示されている複数の構成要素の適宜な組み合せにより種々の発明を形成できる。例えば、実施形態に示される全構成要素から幾つかの構成要素を削除してもよい。さらに、異なる実施形態に亘る構成要素を適宜組み合せてもよい。
【符号の説明】
【００８６】
１ 二次元角度センサ
２ アクチュエータ
３ センサ位置算出装置
４、４ａ、４ｂ、４ｃ、４ｄ、４ｅ、４ｆ 信号処理装置
５ センサ姿勢算出装置
１１、１１ａ 三次元情報推定部
１２ 概略距離推定部
１３、１３ａ 推定開始判定部
１４ 総合判定部
１５ 角度補償部
【特許請求の範囲】
【請求項１】
移動可能な二次元角度センサから、目標の角度情報とこの二次元角度センサの位置情報を入力し、前記目標の三次元情報を推定する信号処理装置であって、
前記目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に基づいて、目標の概略距離を推定する概略距離推定手段と、
前記目標の角度情報と前記二次元角度センサの位置情報及び前記概略距離推定手段からの目標の概略距離に基づいて、前記目標の三次元情報の推定を開始する三次元情報推定手段と
を備えたことを特徴とする信号処理装置。
【請求項２】
移動可能な二次元角度センサから、目標の角度情報とこの二次元角度センサの位置情報を入力し、前記目標の三次元情報を推定する信号処理装置であって、
前記目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に基づいて、前記目標の三次元情報の推定を開始するか否かを判定する推定開始判定手段と、
前記推定開始判定手段からの三次元情報の推定開始を判定された目標に対し、この目標の角度情報と前記二次元角度センサの位置情報に基づいて、この目標の三次元情報の推定を開始する三次元情報推定手段と
を備えたことを特徴とする信号処理装置。
【請求項３】
移動可能な二次元角度センサから、目標の角度情報とこの二次元角度センサの位置情報を入力し、前記目標の三次元情報を推定する信号処理装置であって、
前記目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に基づいて、前記目標の三次元情報の推定を開始するか否かを判定する推定開始判定手段と、
前記推定開始判定手段からの三次元情報の推定開始を判定された目標に対し、この目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に基づいて、目標の概略距離を推定する概略距離推定手段と、
前記推定開始判定手段からの三次元情報の推定開始を判定された目標に対し、この目標の角度情報と前記二次元角度センサの位置情報ならびに前記概略距離推定手段からの目標の概略距離に基づいて、この目標の三次元情報の推定を開始する三次元情報推定手段と
を備えたことを特徴とする信号処理装置。
【請求項４】
移動可能な二次元角度センサから、目標の角度情報とこの二次元角度センサの位置情報を入力し、前記目標の三次元情報を推定する信号処理装置であって、
前記目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に基づいて、目標の概略距離を推定する概略距離推定手段と、
前記概略距離推定手段において概略距離を推定された目標に対し、この目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に基づいて、この目標の三次元情報の推定を開始するか否かを判定する推定開始判定手段と、
前記推定開始判定手段からの三次元情報の推定開始を判定された目標に対し、この目標の角度情報と前記二次元角度センサの位置情報及び前記概略距離推定手段からの目標の概略距離に基づいて、この目標の三次元情報の推定を開始する三次元情報推定手段と
を備えたことを特徴とする信号処理装置。
【請求項５】
移動可能な二次元角度センサから、目標の角度情報とこの二次元角度センサの位置情報を入力し、前記目標の三次元情報を推定する信号処理装置であって、
前記目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に基づいて、目標の概略距離を推定すると共に、前記目標の三次元情報の推定を開始するか否かを判定する概略距離推定・推定開始判定手段と、
前記概略距離推定・推定開始判定手段からの三次元情報の推定開始を判定された目標に対し、この目標の角度情報と二次元角度センサの位置情報ならびに前記概略距離推定・推定開始判定手段からの目標の概略距離に基づいて、この目標の三次元情報の推定を開始する三次元情報推定手段と
を備えたことを特徴とする信号処理装置。
【請求項６】
更に、前記二次元角度センサの姿勢情報を受け取ると共に、この姿勢情報に基づいて前記目標の角度情報を補償する目標角度補償手段を備えたことを特徴とする請求項１乃至請求項５のいずれか１項に記載の信号処理装置。
【請求項１】
移動可能な二次元角度センサから、目標の角度情報とこの二次元角度センサの位置情報を入力し、前記目標の三次元情報を推定する信号処理装置であって、
前記目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に基づいて、目標の概略距離を推定する概略距離推定手段と、
前記目標の角度情報と前記二次元角度センサの位置情報及び前記概略距離推定手段からの目標の概略距離に基づいて、前記目標の三次元情報の推定を開始する三次元情報推定手段と
を備えたことを特徴とする信号処理装置。
【請求項２】
移動可能な二次元角度センサから、目標の角度情報とこの二次元角度センサの位置情報を入力し、前記目標の三次元情報を推定する信号処理装置であって、
前記目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に基づいて、前記目標の三次元情報の推定を開始するか否かを判定する推定開始判定手段と、
前記推定開始判定手段からの三次元情報の推定開始を判定された目標に対し、この目標の角度情報と前記二次元角度センサの位置情報に基づいて、この目標の三次元情報の推定を開始する三次元情報推定手段と
を備えたことを特徴とする信号処理装置。
【請求項３】
移動可能な二次元角度センサから、目標の角度情報とこの二次元角度センサの位置情報を入力し、前記目標の三次元情報を推定する信号処理装置であって、
前記目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に基づいて、前記目標の三次元情報の推定を開始するか否かを判定する推定開始判定手段と、
前記推定開始判定手段からの三次元情報の推定開始を判定された目標に対し、この目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に基づいて、目標の概略距離を推定する概略距離推定手段と、
前記推定開始判定手段からの三次元情報の推定開始を判定された目標に対し、この目標の角度情報と前記二次元角度センサの位置情報ならびに前記概略距離推定手段からの目標の概略距離に基づいて、この目標の三次元情報の推定を開始する三次元情報推定手段と
を備えたことを特徴とする信号処理装置。
【請求項４】
移動可能な二次元角度センサから、目標の角度情報とこの二次元角度センサの位置情報を入力し、前記目標の三次元情報を推定する信号処理装置であって、
前記目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に基づいて、目標の概略距離を推定する概略距離推定手段と、
前記概略距離推定手段において概略距離を推定された目標に対し、この目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に基づいて、この目標の三次元情報の推定を開始するか否かを判定する推定開始判定手段と、
前記推定開始判定手段からの三次元情報の推定開始を判定された目標に対し、この目標の角度情報と前記二次元角度センサの位置情報及び前記概略距離推定手段からの目標の概略距離に基づいて、この目標の三次元情報の推定を開始する三次元情報推定手段と
を備えたことを特徴とする信号処理装置。
【請求項５】
移動可能な二次元角度センサから、目標の角度情報とこの二次元角度センサの位置情報を入力し、前記目標の三次元情報を推定する信号処理装置であって、
前記目標の時系列の角度情報と前記二次元角度センサの時系列の位置情報に基づいて、目標の概略距離を推定すると共に、前記目標の三次元情報の推定を開始するか否かを判定する概略距離推定・推定開始判定手段と、
前記概略距離推定・推定開始判定手段からの三次元情報の推定開始を判定された目標に対し、この目標の角度情報と二次元角度センサの位置情報ならびに前記概略距離推定・推定開始判定手段からの目標の概略距離に基づいて、この目標の三次元情報の推定を開始する三次元情報推定手段と
を備えたことを特徴とする信号処理装置。
【請求項６】
更に、前記二次元角度センサの姿勢情報を受け取ると共に、この姿勢情報に基づいて前記目標の角度情報を補償する目標角度補償手段を備えたことを特徴とする請求項１乃至請求項５のいずれか１項に記載の信号処理装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【公開番号】特開２０１２−４７６４２（Ｐ２０１２−４７６４２Ａ）
【公開日】平成２４年３月８日（２０１２．３．８）
【国際特許分類】
【出願番号】特願２０１０−１９１３４２（Ｐ２０１０−１９１３４２）
【出願日】平成２２年８月２７日（２０１０．８．２７）
【出願人】（０００００３０７８）株式会社東芝 (54,554)
【Ｆターム（参考）】
【公開日】平成２４年３月８日（２０１２．３．８）
【国際特許分類】
【出願日】平成２２年８月２７日（２０１０．８．２７）
【出願人】（０００００３０７８）株式会社東芝 (54,554)
【Ｆターム（参考）】
[ Back to top ]