(57)【要約】【課題】複数の対象物の位置推定精度の向上を図った位置標定システム、および位置標定方法を提供すること。【解決手段】互いに独立して配置され、複数の対象物Ｍ１，Ｍ２に対する方位角をそれぞれ測定する少なくとも３つの測角センサＳ１，Ｓ２，Ｓ３と、３つの測角センサＳ１，Ｓ２，Ｓ３から対象物Ｍ１へそれぞれ延びる３つの方位線ｌ１１，ｌ２１，ｌ３１が交差するか否かを判定する交差判定部１２と、交差判定部１２が３つの方位線ｌ１１，ｌ２１，ｌ３１が交差すると判定した場合に、３つの方位線からみなし交点Ｐ１２３を算出し、このみなし交点Ｐ１２３を対象物Ｍ１の位置と推定する位置推定部１３とを備えた。【選択図】図１
2018025556.tif 000002
【特許請求の範囲】
【請求項１】
互いに独立して配置され、複数の対象物に対する方位角をそれぞれ測定する少なくとも３つの測角センサと、
３つの測角センサから各対象物へそれぞれ延びる３つの方位線が交差するか否かを判定する交差判定部と、
前記交差判定部が３つの前記方位線が交差すると判定した場合に、３つの前記方位線からみなし交点を算出し、このみなし交点を前記対象物の位置と推定する位置推定部と、
を備えたことを特徴とする位置標定システム。
【請求項２】
前記交差判定部は、任意に選択された２つの測角センサから各対象物へそれぞれ延びる２つの方位線の交点を算出し、この交点を中心とする所定の誤差半径の円の内側を、残りの測角センサから該対象物へ延びる方位線が通過する場合に、３つの前記方位線は交差すると判定することを特徴とする請求項１に記載の位置標定システム。
【請求項３】
前記交差判定部は、任意に選択された２つの測角センサから各対象物へそれぞれ延びる２つの方位線の交点と、残りの測角センサから該対象物へ延びる方位線との距離を算出し、この距離が前記交点を中心とする円の所定の誤差半径よりも小さい場合に、３つの前記方位線は交差すると判定することを特徴とする請求項１に記載の位置標定システム。
【請求項４】
前記誤差半径は、前記測角センサの測角誤差の標準偏差の値に基づいて設定されることを特徴とする請求項２または３に記載の位置標定システム。
【請求項５】
前記誤差半径は、任意に選択された２つの測角センサのうち、測角誤差の大きい測角センサにおける測角誤差の標準偏差の値に基づいて設定されることを特徴とする請求項２または３に記載の位置標定システム。
【請求項６】
前記位置推定部は、３つの前記方位線から最小二乗法を用いて前記みなし交点を算出することを特徴とする請求項１から５のいずれか一項に記載の位置標定システム。
【請求項７】
所定の運動モデルに基づく状態推定フィルタを用いた前記対象物の状態推定によって、前記みなし交点の値を補正する補正部を備えたことを特徴とする請求項１から６のいずれか一項に記載の位置標定システム。
【請求項８】
互いに独立して配置され、複数の対象物に対する方位線の方向ベクトルを規定する方位角及び天頂角をそれぞれ測定する少なくとも３つの測角センサと、
現在または過去の所定期間内に、すべての前記測角センサから一の対象物へそれぞれ延びるすべての前記方位線が交差した全線交差点を記憶する記憶部と、
任意に選択された複数の前記測角センサから該対象物へそれぞれ延びる前記方位線が交差する複数の交点を算出する交点算出部と、
前記全線交差点に基づき、複数の前記交点の尤度をそれぞれ判定する尤度判定部と、
尤度の大きさに基づいて一の交点を抽出し、該交点を一の前記対象物の位置と推定する位置推定部と、
を備えたことを特徴とする位置標定システム。
【請求項９】
前記尤度判定部は、前記全線交差点との距離が最も短い前記交点の尤度を最も高く判定することを特徴とする請求項８に記載の位置標定システム。
【請求項１０】
予め複数の対象物が近接して存在することが判明している場合、
前記尤度判定部は、一の交点の尤度を、該交点の所定範囲内に存在する他の交点の数に応じて高める補正をすることを特徴とする請求項８または９に記載の位置標定システム。
【請求項１１】
前記交点算出部は、前記測角センサが前記対象物を検出する検出確率が高くなるにつれて、選択される前記測角センサの数を増やすことを特徴とする請求項８から１０のいずれか一項に記載の位置標定システム。
【請求項１２】
前記記憶部は、前記対象物と前記測角センサとの相対速度と、前記測角センサの測定周期との積が大きくなるにつれて、前記所定期間を短くすることを特徴とする請求項８から１１のいずれか一項に記載の位置標定システム。
【請求項１３】
すべての前記測角センサから一の前記対象物へそれぞれ延びるすべての前記方位線の最接近点を算出すると共に、この最接近点を中心とした所定の誤差領域内を、すべての方位線が通過する場合に、該最接近点を前記全線交差点と判定する交差判定部を備えることを特徴とする請求項８から１２のいずれか一項に記載の位置標定システム。
【請求項１４】
少なくとも３つの測角センサを用いて、複数の対象物に対する方位角をそれぞれ測定する測角ステップと、
３つの測角センサから各対象物へそれぞれ延びる３つの方位線が交差するか否かを判定する交差判定ステップと、
３つの前記方位線が交差すると判定された場合に、３つの前記方位線からみなし交点を算出し、このみなし交点を前記対象物の位置と推定する位置推定ステップと、
を備えたことを特徴とする位置標定方法。
【請求項１５】
前記交差判定ステップは、任意に選択された２つの測角センサから各対象物へそれぞれ延びる２つの方位線の交点を算出し、この交点を中心とする所定の誤差半径の円の内側を、残りの測角センサから該対象物へ延びる方位線が通過する場合に、３つの前記方位線は交差すると判定することを特徴とする請求項１４に記載の位置標定方法。
【請求項１６】
前記交差判定ステップは、任意に選択された２つの測角センサから各対象物へそれぞれ延びる２つの方位線の交点と、残りの測角センサから該対象物へ延びる方位線との距離を算出し、この距離が前記交点を中心とする円の所定の誤差半径よりも小さい場合に、３つの前記方位線は交差すると判定することを特徴とする請求項１４に記載の位置標定方法。
【請求項１７】
前記交差判定ステップにおいて、前記誤差半径は、前記測角センサの測角誤差の標準偏差の値に基づいて設定されることを特徴とする請求項１５または１６に記載の位置標定方法。
【請求項１８】
前記交差判定ステップにおいて、前記誤差半径は、任意に選択された２つの測角センサのうち、測角誤差の大きい測角センサにおける測角誤差の標準偏差の値に基づいて設定されることを特徴とする請求項１５または１６に記載の位置標定方法。
【請求項１９】
前記位置推定ステップは、３つの前記方位線から最小二乗法を用いて前記みなし交点を算出することを特徴とする請求項１４から１８のいずれか一項に記載の位置標定方法。
【請求項２０】
所定の運動モデルに基づく状態推定フィルタを用いた前記対象物の状態推定によって、前記みなし交点の値を補正する補正ステップを備えたことを特徴とする請求項１４から１９のいずれか一項に記載の位置標定方法。
【請求項２１】
少なくとも３つの測角センサを用いて、複数の対象物に対する方位線の方向ベクトルを規定する方位角及び天頂角をそれぞれ測定する測角ステップと、
現在または過去の所定期間内に、すべての前記測角センサから一の対象物へそれぞれ延びるすべての前記方位線が交差した全線交差点を算出して記憶する全線交差点記憶ステップと、
任意に選択された複数の前記測角センサから該対象物へそれぞれ延びる前記方位線が交差する複数の交点を算出する交点算出ステップと、
前記全線交差点に基づき、複数の前記交点の尤度をそれぞれ判定する尤度判定ステップと、
尤度の大きさに基づいて一の交点を抽出し、該交点を一の前記対象物の位置と推定する位置推定ステップと、
を備えたことを特徴とする位置標定方法。
【請求項２２】
前記尤度判定ステップにおいて、前記全線交差点との距離が最も短い前記交点の尤度を最も高く判定することを特徴とする請求項２１に記載の位置標定方法。
【請求項２３】
予め複数の対象物が近接して存在することが判明している場合、
前記尤度判定ステップにおいて、一の交点の尤度を、該交点の所定範囲内に存在する他の交点の数に応じて高める補正をすることを特徴とする請求項２１または２２に記載の位置標定方法。
【請求項２４】
前記交点算出ステップにおいて、前記測角センサが前記対象物を検出する検出確率が高くなるにつれて、選択される前記測角センサの数が増えることを特徴とする請求項２１から２３のいずれか一項に記載の位置標定方法。
【請求項２５】
前記全線交差点記憶ステップは、前記対象物と前記測角センサとの相対速度と、前記測角センサの測定周期との積が大きくなるにつれて、前記所定期間が短く設定されることを特徴とする請求項２１から２４のいずれか一項に記載の位置標定方法。
【請求項２６】
すべての前記測角センサから一の前記対象物へそれぞれ延びるすべての前記方位線の最接近点を算出すると共に、この最接近点を中心とした所定の誤差領域内を、すべての方位線が通過する場合に、該最接近点を前記全線交差点と判定する交差判定ステップを備えることを特徴とする請求項２１から２５のいずれか一項に記載の位置標定方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、測角センサを用いて対象の位置を推定する位置標定システム、および位置標定方法に関する。
【背景技術】
【０００２】
一般に、捜索対象や欠陥などの対象物（目標ともいう）の位置を推定する場合、対象物に対する方位角を測定する測角センサを複数用いて、三角測量の原理に基づき、各測角センサから対象物に延びる方位線の交点から推定することできる。一方で、対象物が複数存在する場合には、測角センサから各対象物にそれぞれ延びる方位線は、対象物の個数が求められるため、各方位線の交点が対象物の真の位置以外の箇所（以下、虚像という）にも現れることとなる。このため、この虚像の存在により、実際よりも多くの個数の対象物が存在すると誤認識し、対象物の位置の推定精度が著しく劣化する問題がある。
【０００３】
このような問題を解決するために、従来、各目標の実測データとセンサ位置と実測データがどの目標から得られるかの可能性係数を目標毎に設定した分類データを入力とし、実測値との差を少なくするよう、目標の仮の位置の集まりで表されるクラスタ特性データを出力するクラスタ特性データ最適化手段と、クラスタ特性データと実測データとセンサ位置とを入力とし、実測値との差を少なくするよう、各実測データが各目標から得られる新しい可能性係数を目標毎に再設定し新しい分類データとしてクラスタ特性データ最適化手段と分類評価値算出手段に出力する分類データ最適化手段と、これらを入力とし、分類評価値を算出する分類評価値算出手段を備えた技術が提案されている（例えば、特許文献１参照）。複数の目標に対する状態推定を行う技術として、カルマンフィルタをベースとするＰＨＤフィルタが知られている（例えば、非特許文献１参照）。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平０９−３０４５２０号公報
【非特許文献】
【０００５】
【非特許文献１】B-N. Vo and W-K. Ma, “The Gaussian Mixture Probability Hypothesis Density Filter,” IEEE Trans. Signal Processing, vol.54, no.11, pp.4091-4104, Nov., 2006.
【発明の概要】
【発明が解決しようとする課題】
【０００６】
特許文献１に記載の技術は、同一対象を指し示すと推定した方位線の組み合わせを出力し、虚像を排除することを目的としている。しかしながら、現実には、測角センサが計測した方位角の実測値には観測誤差が存在するため、仮に虚像をすべて排除できたとしても、対象物の位置推定には誤差が含まれ、推定精度の低下が懸念される。さらに、上記特許文献１に記載の技術は、対象物の位置推定にあたり、対象物の数の真値を予め入力する必要があり、対象物の数が不明の場合や変化する場合には、対象物の位置推定が困難であった。
【０００７】
本発明は、上記の事情に鑑みてなされたものであり、複数の対象物の位置推定精度の向上を図った位置標定システム、および位置標定方法を提供することを目的とする。
【課題を解決するための手段】
【０００８】
上述した課題を解決し、目的を達成するために、本発明に係る位置標定システムは、互いに独立して配置され、複数の対象物に対する方位角をそれぞれ測定する少なくとも３つの測角センサと、３つの測角センサから各対象物へそれぞれ延びる３つの方位線が交差するか否かを判定する交差判定部と、交差判定部が３つの方位線が交差すると判定した場合に、３つの方位線からみなし交点を算出し、このみなし交点を対象物の位置と推定する位置推定部と、を備えたことを特徴とする。
【０００９】
この構成によれば、３つの方位線からみなし交点を算出し、このみなし交点を対象物の位置と推定するため、対象物の位置を精度良く推定することができる。また、３つの方位線が交差するか否かを判定することで対象物の位置を推定するため、従来のように、対象物の数の真値を予め入力する必要はなく、対象物の数が不明の場合や変化する場合でも容易に対象物の位置推定ができる。
【００１０】
この構成において、交差判定部は、任意に選択された２つの測角センサから各対象物へそれぞれ延びる２つの方位線の交点を算出し、この交点を中心とする所定の誤差半径の円の内側を、残りの測角センサから該対象物へ延びる方位線が通過する場合に、３つの方位線は交差すると判定してもよい。
【００１１】
また、交差判定部は、任意に選択された２つの測角センサから各対象物へそれぞれ延びる２つの方位線の交点と、残りの測角センサから該対象物へ延びる方位線との距離を算出し、この距離が交点を中心とする円の所定の誤差半径よりも小さい場合に、３つの方位線は交差すると判定してもよい。
【００１２】
また、誤差半径は、測角センサの測角誤差の標準偏差の値に基づいて設定されてもよい。また、誤差半径は、任意に選択された２つの測角センサのうち、測角誤差の大きい測角センサにおける測角誤差の標準偏差の値に基づいて設定されてもよい。
【００１３】
また、位置推定部は、３つの方位線から最小二乗法を用いてみなし交点を算出してもよい。また、所定の運動モデルに基づく状態推定フィルタを用いた対象物の状態推定によって、みなし交点の値を補正する補正部を備えてもよい。
【００１４】
また、本発明に係る位置標定システムは、互いに独立して配置され、複数の対象物に対する方位線の方向ベクトルを規定する方位角及び天頂角をそれぞれ測定する少なくとも３つの測角センサと、現在または過去の所定期間内に、すべての測角センサから一の対象物へそれぞれ延びるすべての方位線が交差した全線交差点を記憶する記憶部と、任意に選択された複数の測角センサから該対象物へそれぞれ延びる方位線が交差する複数の交点を算出する交点算出部と、全線交差点に基づき、複数の交点の尤度をそれぞれ判定する尤度判定部と、尤度の大きさに基づいて一の交点を抽出し、該交点を一の対象物の位置と推定する位置推定部と、を備えたことを特徴とする。
【００１５】
この構成によれば、現在または過去の全線交差点に基づいて交点の尤度を判定し、この尤度の大きさから交点の位置を対象物の位置と推定するため、測角センサが対象物を常時検出できなくても、対象物の位置を精度良く推定することができる。
【００１６】
また、尤度判定部は、全線交差点との距離が最も短い交点の尤度を最も高く判定する構成としてもよい。
【００１７】
また、予め複数の対象物が近接して存在することが判明している場合、尤度判定部は、一の交点の尤度を、該交点の所定範囲内に存在する他の交点の数に応じて高める補正をしてもよい。
【００１８】
また、交点算出部は、測角センサが対象物を検出する検出確率が高くなるにつれて、選択される測角センサの数を増やしてもよい。
【００１９】
また、記憶部は、対象物と測角センサとの相対速度と、測角センサの測定周期との積が大きくなるにつれて、所定期間を短くしてもよい。
【００２０】
また、すべての測角センサから一の対象物へそれぞれ延びるすべての方位線の最接近点を算出すると共に、この最接近点を中心とした所定の誤差領域内を、すべての方位線が通過する場合に、該最接近点を全線交差点と判定する交差判定部を備えてもよい。
【００２１】
本発明に係る位置標定方法は、少なくとも３つの測角センサを用いて、複数の対象物に対する方位角をそれぞれ測定する測角ステップと、３つの測角センサから各対象物へそれぞれ延びる３つの方位線が交差するか否かを判定する交差判定ステップと、３つの方位線が交差すると判定された場合に、３つの方位線からみなし交点を算出し、このみなし交点を対象物の位置と推定する位置推定ステップと、を備えたことを特徴とする。
【００２２】
この構成において、交差判定ステップは、任意に選択された２つの測角センサから各対象物へそれぞれ延びる２つの方位線の交点を算出し、この交点を中心とする所定の誤差半径の円の内側を、残りの測角センサから該対象物へ延びる方位線が通過する場合に、３つの方位線は交差すると判定してもよい。
【００２３】
また、交差判定ステップは、任意に選択された２つの測角センサから各対象物へそれぞれ延びる２つの方位線の交点と、残りの測角センサから該対象物へ延びる方位線との距離を算出し、この距離が交点を中心とする円の所定の誤差半径よりも小さい場合に、３つの方位線は交差すると判定してもよい。
【００２４】
また、交差判定ステップにおいて、誤差半径は、測角センサの測角誤差の標準偏差の値に基づいて設定されてもよい。また、交差判定ステップにおいて、誤差半径は、任意に選択された２つの測角センサのうち、測角誤差の大きい測角センサにおける測角誤差の標準偏差の値に基づいて設定されてもよい。
【００２５】
また、位置推定ステップは、３つの方位線から最小二乗法を用いてみなし交点を算出してもよい。また、所定の運動モデルに基づく状態推定フィルタを用いた対象物の状態推定によって、みなし交点の値を補正する補正ステップを備えてもよい。
【００２６】
本発明に係る位置標定方法は、少なくとも３つの測角センサを用いて、複数の対象物に対する方位線の方向ベクトルを規定する方位角及び天頂角をそれぞれ測定する測角ステップと、現在または過去の所定期間内に、すべての測角センサから一の対象物へそれぞれ延びるすべての方位線が交差した全線交差点を算出して記憶する全線交差点記憶ステップと、任意に選択された複数の測角センサから該対象物へそれぞれ延びる方位線が交差する複数の交点を算出する交点算出ステップと、全線交差点に基づき、複数の交点の尤度をそれぞれ判定する尤度判定ステップと、尤度の大きさに基づいて一の交点を抽出し、該交点を一の対象物の位置と推定する位置推定ステップと、を備えたことを特徴とする。
【００２７】
また、尤度判定ステップにおいて、全線交差点との距離が最も短い交点の尤度を最も高く判定してもよい。
【００２８】
また、予め複数の対象物が近接して存在することが判明している場合、尤度判定ステップにおいて、一の交点の尤度を、該交点の所定範囲内に存在する他の交点の数に応じて高める補正をしてもよい。
【００２９】
また、交点算出ステップにおいて、測角センサが対象物を検出する検出確率が高くなるにつれて、選択される測角センサの数を増やしてもよい。
【００３０】
また、全線交差点記憶ステップは、対象物と測角センサとの相対速度と、測角センサの測定周期との積が大きくなるにつれて、所定期間が短く設定されてもよい。
【００３１】
すべての測角センサから一の対象物へそれぞれ延びるすべての方位線の最接近点を算出すると共に、この最接近点を中心とした所定の誤差領域内を、すべての方位線が通過する場合に、該最接近点を全線交差点と判定する交差判定ステップを備えてもよい。
【発明の効果】
【００３２】
本発明によれば、３つの方位線からみなし交点を算出し、このみなし交点を対象物の位置と推定するため、対象物の位置を精度良く推定することができる。また、３つの方位線が交差するか否かを判定することで対象物の位置を推定するため、従来のように、対象物の数の真値を予め入力する必要はなく、対象物の数が不明の場合や変化する場合でも容易に対象物の位置推定ができる。
【００３３】
また、本発明によれば、現在または過去の全線交差点に基づいて交点の尤度を判定し、この尤度の大きさから交点の位置を対象物の位置と推定するため、測角センサが対象物を常時検出できなくても、対象物の位置を精度良く推定することができる。
【図面の簡単な説明】
【００３４】
【図１】図１は、第１実施形態に係る位置標定システムの概略構成を示すブロック図である。
【図２】図２は、位置標定システムが対象物の位置推定を行う手順を示すフローチャートである。
【図３】図３は、交差判定ステップの判定手順を説明する図である。
【図４】図４は、第２実施形態に係る位置標定システムの概略構成を示すブロック図である。
【図５】図５は、方向ベクトルを規定する方位角と天頂角とを説明するための図である。
【図６】図６は、位置標定システムが対象物の位置推定を行う手順を示すフローチャートである。
【図７】図７は、ＰＨＤフィルタを用いた演算処理の手順を示すフローチャートである。
【図８】図８は、交差判定ステップの判定手順を説明する図である。
【図９】図９は、全線交差点と各方位線の交点との配置関係の一例を示す図である。
【図１０】図１０は、検出確率と選択される測角センサの数との関係の一例を示すグラフである。
【図１１】図１１は、時間窓幅と、測角センサに対する測角センサの相対速度と測角センサの測定周期との積と、の関係を示すグラフである。
【発明を実施するための形態】
【００３５】
以下に、本発明に係る実施形態を図面に基づいて詳細に説明する。なお、この実施形態によりこの発明が限定されるものではない。また、実施形態における構成要素には、当業者が置換可能かつ容易なもの、あるいは実質的に同一のものが含まれる。さらに、以下に記載した構成要素は適宜組み合わせることが可能である。
【００３６】
［第１実施形態］
図１は、第１実施形態に係る位置標定システムの概略構成を示すブロック図である。位置標定システム１は、測角センサを用いて、例えば、捜索対象や欠陥などの対象物の位置を推定する。図１には、一例として、３つの測角センサＳ１，Ｓ２，Ｓ３を用いて、２つの対象物Ｍ１，Ｍ２の位置を推定する場合について説明する。本実施形態では、測角センサから対象物へと向かう所定の一方向をＸ方向とし、このＸ方向に水平方向に直交する方向をＹ方向とする。測角センサは、少なくとも３つ備えていれば数を限るものではなく、また、対象物についても、複数であれば数を限るものではない。ここで、２つの対象物Ｍ１，Ｍ２は、例えば、地表面のような同一平面（ＸＹ平面）上に存在する物体であり、静止するものであっても、自由に移動するものであってもよい。
【００３７】
位置標定システム１は、図１に示すように、３つの測角センサＳ１，Ｓ２，Ｓ３と、これら測角センサＳ１，Ｓ２，Ｓ３の計測値に基づき演算処理を行う演算処理部１０とを備える。測角センサＳ１〜Ｓ３は、Ｙ方向に所定間隔をあけ、互いに独立して配置されており、演算処理部１０と有線または無線により通信可能に接続されている。測角センサＳ１，Ｓ２，Ｓ３は、対象物Ｍ１，Ｍ２からの信号（音波、光波、電波など）を受信して、この信号の発信方向を方位角として計測する。例えば、測角センサとして、対象物Ｍ１，Ｍ２が発する音声信号を受信するマイクを用いることができる。また、測角センサとして、周囲を撮影するカメラを用いて、撮影した画像情報に対象物を示す所定形状が存在するか否かを画像処理などにて識別する構成としてもよい。
【００３８】
方位角は、図１に示すＸ方向に延びる基準線５となす角として定義され、例えば、測角センサＳ１から対象物Ｍ１，Ｍ２にそれぞれ延びる方位線ｌ１１，ｌ１２（ｌ１ｐ，ｐ＝１、２）の方位角は、方位角θ１１，θ１２（θ１ｐ，ｐ＝１、２）として表される。同様に、測角センサＳ２から対象物Ｍ１，Ｍ２にそれぞれ延びる方位線ｌ２１，ｌ２２（ｌ２ｑ，ｑ＝１、２）の方位角は、方位角θ２１，θ２２（θ２ｑ，ｑ＝１、２）として表され、測角センサＳ３から対象物Ｍ１，Ｍ２にそれぞれ延びる方位線ｌ３１，ｌ３２（ｌ３ｒ，ｒ＝１、２）の方位角は、方位角θ３１，θ３２（θ３ｒ，ｒ＝１、２）として表される。測角センサＳ１〜Ｓ３は、それぞれ自己の位置情報を既知の情報として有しており、この位置情報と同時に計測した方位角（計測値）θ１ｐ，θ２ｑ，θ３ｒとを演算処理部１０に順次送信する。なお、本実施形態では、測角センサＳ１〜Ｓ３は、所定位置に固定設置されたものとするが、例えば、ＧＰＳ（Global  Positioning  System）信号に基づき、自己の位置情報を取得可能な構成とすれば、測角センサＳ１〜Ｓ３を移動体に搭載してもよい。
【００３９】
演算処理部１０は、図１に示すように、データ受信部１１、交差判定部１２、位置推定部１３、補正部１４、表示部１５、および制御部１６とを備える。データ受信部１１は、測角センサＳ１，Ｓ２，Ｓ３から順次送信される方位角θ１ｐ，θ２ｑ，θ３ｒを受信する。交差判定部１２は、受信した方位角θ１ｐ，θ２ｑ，θ３ｒの情報に基づいて、測角センサＳ１，Ｓ２，Ｓ３から対象物Ｍ１，Ｍ２に向けてそれぞれ延びる方位線ｌ１ｐ，ｌ２ｑ，ｌ３ｒが交差するか否かを判定する。本実施形態では、交差するとは、方位線ｌ１ｐ，ｌ２ｑ，ｌ３ｒが１点で交差するだけでなく、各方位線が所定領域内に存在する（所定領域内を通過する）場合を含む。位置推定部１３は、交差判定部１２が３本の方位線ｌ１ｐ，ｌ２ｑ，ｌ３ｒが交差すると判定した場合、その交差した位置を対象物Ｍ１，Ｍ２と推定する。また、交差判定部１２が３本の方位線ｌ１ｐ，ｌ２ｑ，ｌ３ｒのうち、２本の方位線のみが交差すると判定した場合には、位置推定部１３は、その交点２０，２１，２２は虚像であると推定する。
【００４０】
補正部１４は、所定の運動モデルに基づく状態推定フィルタを用いた状態推定によって、位置推定部１３が推定した対象物Ｍ１，Ｍ２の位置を補正する。状態推定フィルタとして、例えば、カルマンフィルタや粒子フィルタを用いることができる。測角センサＳ１，Ｓ２，Ｓ３が計測する方位角は瞬時値であるため、この状態推定により、瞬時値のばらつきを抑圧することができ、測角誤差に起因する位置推定誤差を抑圧できる。
【００４１】
表示部１５は、例えば、ディスプレイ装置であり、推定した対象物Ｍ１，Ｍ２の位置情報を表示する。制御部１６は、位置標定システム１の動作を司るものであり、測角センサＳ１，Ｓ２，Ｓ３、データ受信部１１、交差判定部１２、位置推定部１３、補正部１４を制御して、対象物Ｍ１，Ｍ２の位置を推定する動作を実行する。次に、位置標定方法の動作手順について説明する。
【００４２】
図２は、位置標定システムが対象物の位置推定を行う手順を示すフローチャートである。制御部１６の制御下、３つの測角センサＳ１，Ｓ２，Ｓ３は、すべての対象物Ｍ１，Ｍ２に対する方位角θ１ｐ，θ２ｑ，θ３ｒを計測する（ステップＳ１）。本実施形態では、説明の便宜上、対象物Ｍ１，Ｍ２を２つとしているが、実際の計測動作では、対象物の個数は不明である。このため、測角センサＳ１，Ｓ２，Ｓ３がそれぞれ受信した受信信号に基づき、受信信号が発せられた方向に位置する対象物を仮定し、この対象物に対する方位角θ１ｐ，θ２ｑ，θ３ｒを計測する。
【００４３】
次に、各測角センサＳ１，Ｓ２，Ｓ３の計測情報を演算処理部１０に集約する（ステップＳ２）。この計測情報には、測角センサＳ１，Ｓ２，Ｓ３が計測した方位角θ１ｐ，θ２ｑ，θ３ｒ（計測値）のみならず、測角センサＳ１，Ｓ２，Ｓ３の位置情報が含まれる。これらの計測情報により、測角センサＳ１，Ｓ２，Ｓ３から対象物Ｍ１，Ｍ２へと延びる方位線ｌ１ｐ，ｌ２ｑ，ｌ３ｒが想定される。
【００４４】
次に、交差判定部１２は、測角センサＳ１が計測した方位角θ１ｐから１つの方位角θ１１を選択する（ステップＳ３）。この場合、方位角θ１１の選択に伴い、方位角θ１１に対応する方位線ｌ１１が選択される。次に、交差判定部１２は、測角センサＳ２が計測した方位角θ２ｑから１つの方位角θ２１を選択する（ステップＳ４）。この場合、方位角θ２１の選択に伴い、方位角θ２１に対応する方位線ｌ２１が選択される。ステップＳ４で選択された方位角θ２１（方位線ｌ２１）は、上記した方位角θ１１（方位線ｌ１１）と組つけて後述する判定処理を行う。
【００４５】
交差判定部１２は、選択した方位角θ２１（θ２ｑ）は既に使用済であるかを判定する（ステップＳ５）。具体的には、選択された方位角θ２１が既に他の方位角と組みつけられているかを判定する。既に使用済であれば（ステップＳ５；Ｙｅｓ）、ｑに１を加える処置を施した（ステップＳ６）のち、処理をステップＳ４に戻して方位角θ２ｑから次の方位角θ２２を選択する。また、使用済でなければ（ステップＳ５；Ｎｏ）、選択された２つの方位線の交点Ｐｐｑ（この例では交点Ｐ１２）の座標を算出する（ステップＳ７）。
【００４６】
ここで、本実施形態では、測角センサＳ１，Ｓ２の位置情報、および測角センサＳ１，Ｓ２から対象物Ｍ１，Ｍ２へと延びる２本の方位線ｌ１１，ｌ２１の方位角θ１１，θ２１が判っているため、２本の方位線ｌ１１，ｌ２１を示す一次関数式に基づいて、交点Ｐｐｑ（この例では交点Ｐ１２）の座標（Ｘ１２，Ｙ１２）が算出される。
【００４７】
次に、測角センサＳ３のすべての方位線ｌ３ｒと交点Ｐｐｑとの距離ｄｒを算出する（ステップＳ８）。この場合、方位線ｌ３ｒは、測角センサＳ３の座標を（Ｘ３，Ｙ３）Ｔとすると、式（１）で表すことができる。
【００４８】
【数１】
2018025556.tif 000003
【００４９】
このため、方位線ｌ３ｒと交点Ｐｐｑとの距離ｄｒは、式（２）で表すことができる。
【数２】
2018025556.tif 000004
【００５０】
この式（２）に基づき、方位線ｌ３１，ｌ３２と、交点Ｐ１２との各距離ｄ１，ｄ２を算出し、この中から最短距離ｄｍｉｎを抽出する（ステップＳ９）。
【００５１】
次に、交差判定部１２は、３本の方位線ｌ１１，ｌ２１，ｌ３１が交差するか否かを判定する。測角センサＳ１，Ｓ２，Ｓ３で測定された方位角は測定誤差があるため、１点で交わることは難しい。このため、本実施形態では、３本の方位線ｌ１１，ｌ２１，ｌ３１が交差するか否かを簡易的に判定している。具体的には、図３に示すように、判定に際し、交点Ｐｐｑ（交点Ｐ１２）を中心とする円５０の誤差半径Ｒｅｒｒを算出する（ステップＳ１０）。誤差半径Ｒｅｒｒは、測角センサＳ１，Ｓ２の測角誤差の標準偏差σに基づいて設定されるものであり、例えば、本実施形態では、標準偏差σの３倍である３σまで考慮した値となっている。この誤差半径Ｒｅｒｒは、適宜変更することができ、例えば、測角センサＳ１，Ｓ２の測角誤差が異なる場合には、測角誤差が大きい方の測角センサの測角誤差の標準偏差σを用いるのが好ましい。この構成では、測角誤差に考慮した誤差半径Ｒｅｒｒを設定できる。
【００５２】
次に、交差判定部１２は、式（３）に基づいて、最短距離ｄｍｉｎと誤差半径Ｒｅｒｒとの大きさを比較する。
【００５３】
【数３】
2018025556.tif 000005
【００５４】
すなわち、方位線ｌ３１が誤差半径Ｒｅｒｒの円５０内を通過するか否かを判定する。この判定において、距離ｄｍｉｎ≦誤差半径Ｒｅｒｒでなければ（ステップＳ１１；Ｎｏ）、方位線ｌ３１が誤差半径Ｒｅｒｒの円５０内を通過しないため、３本の方位線ｌ１１，ｌ２１，ｌ３１は交差しないと判定し、処理をステップＳ６に移行する。
【００５５】
一方、距離ｄｍｉｎ≦誤差半径Ｒｅｒｒであれば（ステップＳ１１；Ｙｅｓ）、３本の方位線ｌ１１，ｌ２１，ｌ３１が交差するみなし交点Ｐｐｑｒ（この例では交点Ｐ１２３）を算出する（ステップＳ１２）。本実施形態では、みなし交点ｐｑｒは最小二乗法によって算出される。具体的には、交差すると判定された３本の方位線ｌ１１，ｌ２１，ｌ３１（便宜上、方位線ｌ１，ｌ２，ｌ３と記す）が得られた場合、各方位線は、式（４）で示すことができる。
【００５６】
【数４】
2018025556.tif 000006
【００５７】
図３に示すように、方位線ｌ１１，ｌ２１は、交差するものの、方位線ｌ３１も含めた３つの直線の最近傍点は交点Ｐｐｑ（交点Ｐ１２）と一致せず、改めて計算する必要がある。これらの最近傍点をみなし交点Ｐｐｑｒ（ｘｐｑｒ，ｙｐｑｒ）Ｔとすると、式（５）が成立する。
【００５８】
【数５】
2018025556.tif 000007
【００５９】
この式（５）は、いわゆる優決定系であるため、最小二乗法を用いることにより、みなし交点Ｐｐｑｒ（交点Ｐ１２３）の座標を算出することができ、このみなし交点Ｐ１２３に対象物が存在すると推定できる。これにより、誤差の影響で確実に交わらない３本の方位線を仮想的に交わったものとみなすことができ、みなし交点Ｐｐｑｒの位置から対象物の存在する位置を推定できる。また、この構成では、３本の方位線ｌ１１，ｌ２１，ｌ３１が交差するみなし交点Ｐｐｑｒに対象物が存在すると推定するため、対象物の個数が不明な場合であっても、対象物Ｍ１の位置を精度良く推定することができる。なお、式（３）において、距離ｄｍｉｎが０であった場合には、交点Ｐｐｑ（交点Ｐ１２）上で３本の方位線ｌ１１，ｌ２１，ｌ３１が交差するため、この交点Ｐｐｑをみなし交点Ｐｐｑｒとすればよい。
【００６０】
さて、上記した計算により、測角センサＳ１，Ｓ２，Ｓ３を用いて対象物の位置を計測することができる。特に、最小二乗法を用いることにより、みなし交点Ｐｐｑｒ（交点Ｐ１２３）の座標を算出することで、このみなし交点Ｐ１２３に対象物が存在すると精度良く推定することができる。一方で、上記したみなし交点Ｐｐｑｒ（交点Ｐ１２３）の値は、測角センサＳ１，Ｓ２，Ｓ３の測角誤差の影響を受けた瞬時値であるため、時系列で評価すると、対象物Ｍ１，Ｍ２の位置変化が急峻であるなど、実際の対象物Ｍ１，Ｍ２の挙動とは大きく異なっている可能性がある。また、対象物Ｍ１，Ｍ２が移動体であり、位置だけでなくその速度や加速度などの運動パラメータを推定しなければならない場合には、差分処理の影響で更にばらつきが大きくなる。
【００６１】
このため、本実施形態では、みなし交点Ｐｐｑｒに状態推定による平滑化処理を施す（ステップＳ１３）。具体的には、対象物Ｍ１，Ｍ２の運動モデルを導入して状態推定を行い、誤差の影響を抑圧している。その運動モデルは、式（６）の状態方程式で示される。
【００６２】
【数６】
2018025556.tif 000008
【００６３】
この式は、対象物Ｍ１，Ｍ２の現在の位置が直前の位置と強い相関があることを意味しており、対象物Ｍ１，Ｍ２の動きをこの式で拘束している。これにより、測角誤差の影響を抑圧し、実際の対象の動き（静止を含む）に即した位置推定が可能となる。また、Ｔｓとｋは、それぞれ図２のフローチャートを１回実施する際の処理時間とサンプリングのインデクスを表している。
【００６４】
また、観測方程式は式（７）で示される。
【００６５】
【数７】
2018025556.tif 000009
【００６６】
この場合の観測値は、上記したステップＳ１２で得た位置の瞬時値（ｘｐｑｒ（ｋ），ｙｐｑｒ（ｋ））Ｔであり、この観測値と上述の運動モデルに基づいて，カルマンフィルタや粒子フィルタ等の状態推定手法を適用することで、測定された方位角の観測誤差を抑圧して対象物Ｍ１，Ｍ２の位置を含む運動パラメータを推定できる。このため、対象物Ｍ１，Ｍ２の位置をより精度良く推定することができる。なお、この状態推定は、対象物Ｍ１，Ｍ２が移動する場合だけでなく、静止している場合にも適用することができる。
【００６７】
次に、制御部１６は、全対象物の位置を推定したか否かを判定する（ステップＳ１４）。具体的には、各測角センサＳ１，Ｓ２，Ｓ３から延びる方位線の数（２本）と同じ数の対象物Ｍ１，Ｍ２の位置が推定されたか否かを判定する。この判定において、全対象物の位置が推定されていない場合（ステップＳ１４；Ｎｏ）には、ｐに１を加える処置を施した（ステップＳ１５）のち、処理をステップＳ３に戻して方位角θ１ｐから次の方位角θ１２を選択して処理を実行する。一方、全対象物の位置が推定されている場合（ステップＳ１４；Ｙｅｓ）には、処理を終了する。
【００６８】
以上、説明したように、本実施形態によれば、３つの方位線ｌ１１，ｌ２１，ｌ３１からみなし交点Ｐ１２３を算出し、このみなし交点Ｐ１２３を対象物Ｍ１の位置と推定するため、対象物Ｍ１の位置を精度良く推定することができる。また、３つのｌ１１，ｌ２１，ｌ３１が交差するか否かを判定することで対象物Ｍ１の位置を推定するため、従来のように、対象物の数の真値を予め入力する必要はなく、対象物の数が不明の場合や変化する場合でも対象物の位置推定ができる。
【００６９】
［第２実施形態］
第１実施形態では、上述のように、少なくとも３つの測角センサＳ１，Ｓ２，Ｓ３から延びる方位線ｌ１１，ｌ２１，ｌ３１からみなし交点Ｐ１２３を算出し、このみなし交点Ｐ１２３を対象物Ｍ１の位置と推定している。この構成では、すべて（例えば３つ）の測角センサＳ１，Ｓ２，Ｓ３が常時、対象物Ｍ１を検出（観測）する、すなわち検出確率が１００％であることを前提としている。しかしながら、現実には、すべての測角センサＳ１，Ｓ２，Ｓ３が常時、すべて（例えば２つ）の対象物Ｍ１，Ｍ２を検出できるとは限らず、センサ処理系の性能に起因した検出確率に応じて、対象物Ｍ１，Ｍ２を見失う可能性がある。特に、対象物Ｍ１，Ｍ２が測角センサＳ１，Ｓ２，Ｓ３から遠い位置に存在する場合には、相対的に検出確率が低くなるため、各測角センサＳ１，Ｓ２，Ｓ３が検出できる対象物Ｍ１，Ｍ２は互いに異なることが想定される。従って、測角センサが対象物を常時検出できない場合には、みなし交点を正確に算出することができず、対象物の位置を推定する精度が悪い可能性があった。このため、第２実施形態では、測角センサが対象物を常時検出できない場合であっても、対象物の位置を精度良く推定できる構成について説明する。
【００７０】
図４は、第２実施形態に係る位置標定システムの概略構成を示すブロック図である。図５は、方向ベクトルを規定する方位角と天頂角とを説明するための図である。位置標定システム１００は、測角センサを用いて、例えば、捜索対象や欠陥などの対象物の位置を推定する。図４には、Ｕ個（一例として４つ）の測角センサＳＯ１，ＳＯ２，ＳＯ３，ＳＯ４を用いて、Ｎ個（一例として２つ）の対象物Ｍ１，Ｍ２の位置を推定する場合が記載されている。測角センサの数は、少なくとも３つ備えていれば数を限るものではなく、３＜Ｕ個であればよい。また、対象物についても、複数であれば数を限るものではなく、２＜Ｎ個であればよい。第２実施形態では、図４に示すように、測角センサから対象物へと向かう所定の一方向をＹ方向とし、このＹ方向に水平方向に直交する方向をＸ方向とし、これらＸ方向及びＹ方向に高さ方向に直交する方向をＺ方向とする。２つの対象物Ｍ１，Ｍ２は、３次元空間（ＸＹＺ空間）に存在する物体であり、静止するものであっても、自由に移動するものであってもよい。
【００７１】
位置標定システム１００は、図４に示すように、４つの測角センサＳＯ１〜ＳＯ４と、これら測角センサＳＯ１〜ＳＯ４の計測値に基づき演算処理を行う演算処理部３０とを備える。測角センサＳＯ１〜ＳＯ４は、３次元空間内に所定間隔をあけ、互いに独立して配置されており、演算処理部３０と有線または無線により通信可能に接続されている。測角センサＳＯ１〜ＳＯ４は、対象物Ｍ１，Ｍ２からの信号（音波、光波、電波など）を受信して、この信号の発信方向の方位ベクトルを規定する方位角及び天頂角を計測する。
【００７２】
図５には、ｍ番目の測角センサＳＯｍ（ｍ＝１、２、・・・Ｕ）からｎ番目の対象物Ｍｎに向けて延びる方向ベクトルｖｍｎと、この方向ベクトルｖｍｎに重なる方位線ｌｍｎとが示されている。この図５では、説明の便宜上、測角センサＳＯｍを原点Ｏに配置している。この場合、方位角θｍｎは、図５に示すように、方向ベクトルｖｍｎをＸＹ平面に投影した線とＸ軸とのなす角として定義される。また、天頂角φｍｎは、方向ベクトルｖｍｎとＺ軸とのなす角として定義される。
【００７３】
測角センサＳＯ１〜ＳＯ４は、それぞれ自己の位置情報を既知の情報として有しており、この位置情報と同時に計測した方位角θｍｎ及び天頂角φｍｎ（計測値）を演算処理部３０に順次送信する。なお、本実施形態では、測角センサＳＯ１〜ＳＯ４は、所定位置に固定設置されたものとするが、例えば、ＧＰＳ信号に基づき、自己の位置情報を取得可能な構成とすれば、測角センサＳＯ１〜ＳＯ４を移動体に搭載してもよい。
【００７４】
演算処理部３０は、図４に示すように、データ受信部３１、交差判定部３２、記憶部３３、交点算出部３４、尤度判定部３５、位置推定部３６、検出確率算出部３７、表示部３８、および制御部３９とを備える。
【００７５】
データ受信部３１は、測角センサＳＯ１〜ＳＯ４から順次送信される方位角θｍｎ及び天頂角φｍｎを受信する。交差判定部３２は、受信した方位角θｍｎ及び天頂角φｍｎの情報に基づいて、すべての測角センサＳＯ１〜ＳＯ４から対象物Ｍ１（一の対象物）に向けてそれぞれ延びるすべての方位線ｌｍｎ（ｌ１１，ｌ２１，ｌ３１，ｌ４１）が交差しているか否かを判定する。本実施形態では、交差するとは、すべての方位線ｌ１１，ｌ２１，ｌ３１，ｌ４１が１点で交差するだけでなく、各方位線が所定領域内に存在する（所定領域内を通過する）場合を含む。また、交差判定部３２は、すべての方位線ｌ１１，ｌ２１，ｌ３１，ｌ４１が所定領域内に存在しない（所定領域内を通過しない）場合には、交差していないと判定する。なお、交差判定部３２は、すべての測角センサＳＯ１〜ＳＯ４から対象物Ｍ２（一の対象物）に向けてそれぞれ延びるすべての方位線が交差するか否かについても判定するが、図４では、測角センサＳＯ１〜ＳＯ４から対象物Ｍ２に向けて延びる方位線の記載を省略している。
【００７６】
記憶部３３は、すべての方位線ｌｍｎ（ｌ１１，ｌ２１，ｌ３１，ｌ４１）が交差した際に、この交差した全線交差点を記憶する。ここで、本実施形態では、すべての測角センサＳＯ１〜ＳＯ４が、対象物Ｍ１，Ｍ２を常時、検出（観察）できない事情を鑑み、現在のみならず、現在から遡った過去の所定期間内における複数の全線交差点を記憶する。
【００７７】
交点算出部３４は、すべての測角センサＳＯ１〜ＳＯ４の中から任意に選択された複数（例えば２本）の測角センサＳＯ１，ＳＯ２から対象物Ｍ１へそれぞれ延びる方位線ｌｍｎ（ｌ１１，ｌ２１）が交差する交点を算出する。選択される測角センサの数Ｕ´は、２以上Ｕ以下であればよい。この場合、交点算出部３４は、測角センサの選択される組み合わせ（パターン）の数だけ、繰り返し複数の交点を算出する。例えば、４つの測角センサＳＯ１〜ＳＯ４の中から任意に２つの測角センサを選択する組み合わせは、６パターンあるため、この６パターン分、方位線の交点を算出する。
【００７８】
尤度判定部３５は、ＰＨＤ（Probabilistic Hypothesis Density）フィルタを用いることにより、記憶部３３に記憶された全線交差点の情報に基づいて、算出された複数の交点の尤度を判定する。ＰＨＤフィルタは、カルマンフィルタのような状態推定技術を複数の対象物に対して同時に行えるように提案された方法論であり、本願では、上記した非特許文献１のようにカルマンフィルタをベースとした実装方法を採用している。一般に、方位線ｌｍｎの数が増えるほど、これら方位線ｌｍｎが交差した交点（例えば、全線交差点）に対象物が存在する確率（尤度）が高く、方位線ｌｍｎの数が少ないほど、これら方位線ｌｍｎが虚像の位置を示す虚像確率が高まることになる。本実施形態では、尤度判定部３５は、全線交差点の情報に基づいて、算出された各交点（観測値）の虚像確率を求めることにより、結果として、これら交点の尤度を判定している。
【００７９】
位置推定部３６は、尤度の大きさに基づいて交点を抽出し、これら交点をそれぞれ対象物Ｍ１，Ｍ２の位置と推定する。また、位置推定部３６は、尤度の小さい、すなわち虚像確率の高い交点については、虚像とみなして、対象物の候補から排除する。
【００８０】
検出確率算出部３７は、測角センサＳＯ１〜ＳＯ４がそれぞれ対象物Ｍ１，Ｍ２にいずれかを検出する確率（検出確率）を算出する。具体的には、測角センサＳＯ１〜ＳＯ４はそれぞれ、所定の測定周期ごとに、対象物（Ｍ１であるかＭ２は問わない）の測定を試みる。この場合、所定の測定回数（例えば１０回）に対する対象物が検出された回数を検出確率とする。算出された検出確率は、定期的に制御部３９に出力される。
【００８１】
表示部３８は、例えば、ディスプレイ装置であり、推定した対象物Ｍ１，Ｍ２の位置情報を表示する。制御部３９は、位置標定システム１００の動作を司るものであり、測角センサＳＯ１〜ＳＯ４、データ受信部３１、交差判定部３２、記憶部３３、交点算出部３４、尤度判定部３５、位置推定部３６、検出確率算出部３７を制御して、対象物Ｍ１，Ｍ２の位置を推定する動作を実行する。次に、位置標定方法の動作手順について説明する。
【００８２】
図６は、位置標定システムが対象物の位置推定を行う手順を示すフローチャートである。制御部３９の制御下、４つの測角センサＳＯ１〜ＳＯ４は、すべての対象物Ｍ１，Ｍ２に対する方位角θｍｎ及び天頂角φｍｎを計測する（ステップＳ２１）。そして、計測した各方位角θｍｎ及び各天頂角φｍｎを用いて、制御部３９は、方向ベクトルｖｍｎと方位線ｌｍｎとを算出する（ステップＳ２２）。
【００８３】
本実施形態では、説明の便宜上、対象物Ｍ１，Ｍ２を２つとしているが、実際の計測動作では、対象物の総数の真値Ｎは不明である。ここで、ｍ番目の測角センサＳＯｍが検出した対象物の数をＮｍ（ｍ＝１、２・・Ｕ）とおく。各測角センサの検出確率が１００％である場合には、各測角センサがそれぞれ検出した対象物の数は同一であり、Ｎ１＝・・＝Ｎｍ＝・・ＮＵ＝Ｎが成立する。しかし、本実施形態では、検出確率が１００％未満である場合を想定しているため、この等式は一般には成立せず、Ｎｍ≦Ｎとなる。ｍ番目の測角センサＳＯｍが測定した方位角と天頂角とをそれぞれθｍ１、・・・、θｍＮｍおよびφｍ１、・・・、φｍＮｍとすると、方向ベクトルｖｍｎは、式（８）で示される。
【００８４】
【数８】
2018025556.tif 000010
【００８５】
ここで、ｍ番目の測角センサＳＯｍの位置を、ＳＯｍ＝［ｘＯｍ  ｙＯｍ  ｚＯｍ］Ｔと表せるため、ｍ番目の測角センサＳＯｍが検出したｎ番目の対象物Ｍｎに対応する方位線ｌｍｎは、媒介変数ｔを用いて、式（９）で示される。
【００８６】
【数９】
2018025556.tif 000011
【００８７】
次に、交差判定部３２は、Ｕ個（例えば４つ）すべての測角センサＳＯ１〜ＳＯ４から一の対象物Ｍｎ（例えば対象物Ｍ１）に向かうすべての方位線ｌｍｎ（ｌ１１，ｌ２１，ｌ３１，ｌ４１）の最接近点ｑを計算する（ステップＳ２３）。理論上、実像（対象物が実際に存在する位置）ではすべて方位線ｌｍｎが交わるため、すべての方位線ｌｍｎが交わる点（全線交差点）が存在すれば、その点に対象物が存在するとみなすことができる。一方、実際には白色雑音などの測定誤差が生じるため、通常、測定値から算出したすべての方位線ｌｍｎ（ｌ１１，ｌ２１，ｌ３１，ｌ４１）は交わらない。このため、交差判定部３２は、複数の方位線ｌｍｎの最接近点ｑを計算した上で、その最接近点ｑにおいて各方位線ｌｍｎが交差したか否かを判定する。
【００８８】
複数のｌｍｎの最接近点ｑを、ｑ＝［ｑｘ  ｑｙ  ｑｚ]Ｔとすると、この最接近点ｑは、最小二乗法など公知技術を用いて求めることができる。例えば、最小二乗法を用いる場合には、式（１０）に示す連立方程式を解く。
【００８９】
【数１０】
2018025556.tif 000012
【００９０】
ここで、ｖｍ（ｍ＝１、２・・Ｕ）は、ｍ番目の測角センサＳＯｍの方向ベクトルの一つである。未知変数を一つのベクトルでまとめると、式（１０）は、式（１１）のように書き表せる。
【００９１】
【数１１】
2018025556.tif 000013
【００９２】
この連立方程式は、式の数が３Ｕ、変数の数が３＋Ｕであり、Ｕ≧２の条件では式の数が変数の数よりも多い。式（１１）に最小二乗法を適用することにより、最接近点ｑが得られる。すべての測角センサＳＯｍ（ｍ＝１、２・・Ｕ）からすべての対象物Ｍｎ（ｎ＝１、２・・Ｎｍ）に向かう全ての方位線ｌｍｎの組合せについて求めた最接近点ｑの集合を、ｑ１、ｑ２・・・ｑＬをとする。
【００９３】
次に、交差判定部３２は、最接近点ｑに方位線ｌｍｎが交差したか否かを判定する（ステップＳ２４）。具体的には、図８に示すように、中心をｑｌ（ｌ＝１、２・・・Ｌ）とし、所定半径を有する誤差球（誤差領域）４０を想定し、この誤差球４０の内部を一の対象物Ｍｎ（Ｍ１）に向けて延びるすべての方位線ｌｍｎ（ｌ１１，ｌ２１，ｌ３１，ｌ４１）が通過するか否かを判定する。この誤差球４０の半径は、測角センサＳＯ１〜ＳＯ４の測定誤差に起因して設定される。この判定において、誤差球４０の内部をすべての方位線ｌｍｎ（ｌ１１，ｌ２１，ｌ３１，ｌ４１）が通過すれば、その中心が全線交差点ｑｌであると判定する。一方、誤差球４０の内部をすべての方位線ｌｍｎ（ｌ１１，ｌ２１，ｌ３１，ｌ４１）が通過しない場合には、この最接近点ｑは虚像であると判定する。なお、誤差球（誤差領域）４０は、必ずしも球形状である必要はなく、たとえば奥行方向と横方向とで三角測量の誤差が異なることを利用し、回転楕円形状を有する誤差楕円体（誤差領域）を採用することもできる。このような手法で選定した全線交差点ｑｌの集合を、ｑ１、ｑ２・・・ｑＬ´とする（Ｌ´＜Ｌ）。
【００９４】
交差判定部３２は、算出された方位線ｌｍｎの情報に基づいて、所定時間ごとに全線交差点ｑｌを算出する。記憶部３３には、現在の全線交差点ｑｌのみならず、過去の所定期間内における複数の全線交差点ｑｌが記憶されている。
【００９５】
次に、交点算出部３４は、すべての測角センサＳＯｍの中から選択された複数の測角センサＳＯｍ（ｍ＝２、・・・Ｕ´、Ｕ´＜Ｕ）を用いて、これらの測角センサＳＯｍ（例えばＳＯ１，ＳＯ２）から一の対象物Ｍｎ（例えばＭ１）へそれぞれ延びる方位線ｌｍｎ（例えばｌ１１，ｌ２１）が交差する交点（観測値）ｚを算出する（ステップＳ２５）。上述したように、すべての方位線ｌｍｎが交わる全線交差点ｑｌが存在すれば、この全線交差点ｑｌに対象物Ｍｎが存在するとみなせるため、全線交差点ｑｌをそのまま観測値として用いることができれば対象物Ｍｎの位置を容易に推定することができる。
【００９６】
ところが、すべての測角センサＳＯｍがすべての対象物Ｍｎを常時検出できない場合、すなわち測角センサＳＯｍの検出確率が低い（１００％未満）場合には、そもそも全線交差点ｑｌが得られる確率が低いという問題がある。この問題は、測角センサＳＯｍの数が増えれば増えるほど顕著となる。従って、全線交差点ｑｌのみを観測値として用いることは現実的ではなく、虚像が多く含まれてしまう可能性はあるものの、選択された数量の測角センサＳＯｍで交点（観測値）ｚを算出する必要がある。この場合、交点算出部３４は、測角センサの選択される組み合わせ（パターン）の数だけ、繰り返し複数の交点ｚを算出する。
【００９７】
次に、ＰＨＤフィルタを用いた演算処理を実行する（ステップＳ２６）。本実施形態では、ＰＨＤフィルタを用いて、加速度・速度・位置という運動パラメータの推定を行う。このため、状態方程式と観測方程式の組である状態空間表現を導入する。この状態空間表現は、式（１２）及び式（１３）によって示される。
【００９８】
【数１２】
2018025556.tif 000014
【００９９】
【数１３】
2018025556.tif 000015
【０１００】
この式（１２）及び式（１３）において、ＴＳは測定周期（サンプリング周期）を表す。この例では、簡素化のためＴＳを固定値としているが、測定周期を変動する構成とすることもできる。また、ｕ（ｋ）とｗ（ｋ）はそれぞれ駆動雑音と観測雑音を表す。
【０１０１】
最後に、時間をｋからｋ＋１に更新する（ステップＳ２７）ともに、処理を再びステップＳ２１に戻し、ステップＳ２１からステップＳ２７を繰り返し実行する。
【０１０２】
次に、ＰＨＤフィルタを用いた演算処理（ステップＳ２６）について説明する。図７は、ＰＨＤフィルタを用いた演算処理の手順を示すフローチャートである。本実施形態では、ＰＨＤフィルタにより、記憶部３３に記憶された全線交差点ｑｌの情報に基づいて、状態候補となる複数の交点ｚの虚像確率をそれぞれ算出し、この虚像確率に基づいて尤度を求めている。ここで尤度とは、算出された交点Ｚが対象物Ｍｎである尤もらしさ表す示量であり、交点ｚの虚像確率によって変動する。
【０１０３】
尤度判定部３５は、時間更新によって新規に発生する状態候補を生成する（ステップＳ３１）。この場合、測角センサＳＯｍの測定可能エリア内に出入りする対象物Ｍｎを考慮する。次に、１時刻前（１測定周期前）の状態と相関のある新規状態候補を生成する（ステップＳ３２）。この場合には、例えば、対象物Ｍｎが分裂する状態などを考慮する。これらステップＳ３１，Ｓ３２において、新規の状態候補が発生しなければ、そのまま処理をステップＳ３３に移行する。
【０１０４】
次に、尤度判定部３５は、１時刻前（１測定周期前）状態候補を遷移させる（ステップＳ３３）。この場合、上記した式（１２）の状態方程式を用いて状態を遷移させる。ここで、ＰＨＤフィルタでは、状態候補が複数存在するため、これらすべての状態候補に対してそれぞれ状態遷移を行う。
【０１０５】
次に、尤度判定部３５は、測角センサＳＯｍの検出確率に応じて、状態候補の重みを減衰する（ステップＳ３４）。具体的は、検出確率の高い対象物Ｍｎの状態候補については重み付けを大きくし、逆に検出確率の低い対象物Ｍｎの状態候補については小さくする。この場合、例えば、測角センサＳＯｍが検出できなくなった対象物Ｍｎについても所定時間、状態候補を存続させておく。
【０１０６】
次に、尤度判定部３５は、式（１３）の観測方程式を用いて、交点（観測値）ｚの推定値ｙ（ｋ）とカルマンゲインの計算を行う（ステップＳ３５）。そして、尤度判定部３５は、すべての状態候補に対して存在するカルマンゲインＫ（ｋ）∈Ｒ９×３と交点ｚ（ｋ）を用いることで、式（１４）に示すように、状態の候補を補正する（ステップＳ３６）。この場合、一度、検出できなくなった対象物Ｍｎを再検出することができる。
【０１０７】
【数１４】
2018025556.tif 000016
【０１０８】
このような交点（観測値）ｚ（ｋ）と紐付いた状態の第ｊ候補に対して、ＰＨＤフィルタでは尤度ｗｊを式（１５）のように計算して都度更新する（ステップＳ３７）。
【０１０９】
【数１５】
2018025556.tif 000017
【０１１０】
この式（１５）において、κ（ｚ（ｋ）,Ｑ（ｋ））は、交点（観測値）ｚ（ｋ）の虚像確率であり、Ｑ（ｋ）は、現在時刻ｋから遡る過去の所定時間（過去数ステップ前から現在時刻ｋまで時間窓幅；所定期間）の全線交差点ｑｌの集合である。この式（１５）に示すように、尤度ｗｊは、虚像確率が高いほど小さくなるように設定されている。このため、算出された虚像確率の大きさに基づいて、尤度ｗｊを求めることができる。
【０１１１】
次に、尤度判定部３５は、尤度ｗｊの低い状態候補を棄却したり、類似する状態候補を集約する（ステップＳ３８）。これにより、目標の数と状態をある程度推定することができる。
【０１１２】
最後に、位置推定部３６は、尤度ｗｊの大きさに基づいて、複数の交点（観測値）ｚの中から１つを抽出し、この抽出した交点ｚを対象物Ｍｎの位置と推定する（ステップＳ３９）。位置推定部３６は、尤度ｗｊの最も高い状態候補を抽出し、複数の対象物Ｍｎの状態（位置などの運動パラメータ）を推定する。この構成によれば、現在または過去の全線交差点ｑｌに基づいて交点ｚの尤度ｗｊを判定し、この尤度ｗｊの大きさから交点ｚの位置を対象物Ｍｎの位置と推定するため、測角センサＳＯｍが対象物を常時検出できなくても、対象物Ｍｎの位置を精度良く推定することができる。
【０１１３】
本実施形態によれば、現在または過去の全線交差点ｑｌの情報に基づいて交点（観測値）ｚの尤度ｗｊを判定し、この尤度ｗｊの大きさから交点（観測値）ｚの位置を対象物Ｍｎの位置と推定するため、測角センサＳＯｍが対象物Ｍｎを常時検出できなくても、対象物Ｍｎの位置を精度良く推定することができる。
【０１１４】
［第３実施形態］
この第３実施形態では、上記した第２実施形態の式（１５）における虚像確率κ（ｚ（ｋ）,Ｑ（ｋ））を、交点（観測値）ｚと全線交差点Ｑ（ｋ）との距離に基づいて算出する。図９は、全線交差点と各方位線の交点との配置関係の一例を示す図である。全線交差点ｑｌの位置は、対象物の位置と考えられるため、この全線交差点ｑｌに近い交点ｚほど虚像確率が低く、尤度が高いと考えられる。そこで、本実施形態では、選択された少数の測角センサで得た交点ｚと、全線交差点ｑｌとの距離ｄをそれぞれ測定する。全線交差点ｑｌの集合Ｑ（ｋ）はＬ´個の要素からなるため、虚像確率κ（ｚ（ｋ）,Ｑ（ｋ））を、式（１６）で定義することができる。
【０１１５】
【数１６】
2018025556.tif 000018
【０１１６】
すなわち、本実施形態では、全線交差点ｑｌに最も近い交点ｚとの距離で虚像確率を定義している。ただし、式（１６）のままでは絶対値によって、虚像確率の値が大きく変化するため、式（１７）により、虚像確率の総和が１となるよう正規化することが好ましい。
【０１１７】
【数１７】
2018025556.tif 000019
【０１１８】
本実施形態では、全線交差点ｑｌに最も近い交点ｚとの距離ｄで虚像確率を定義するため、尤度判定部３５は、全線交差点ｑｌとの距離ｄが最も短い交点ｚの尤度ｗｊを最も高く判定するため、測角センサの検出確率が低くても、虚像確率を適切に計算することでき、虚像を排除して対象物Ｍｎの位置を精度良く推定することができる。
【０１１９】
［第４実施形態］
第４実施形態は、複数の対象物Ｍｎが所定の範囲内に近接していることが判明している場合に有効な手法である。通常、複数の対象物Ｍｎが所定の範囲内に近接している場合、観測誤差の影響を受けるため、各対象物Ｍｎの分離検出が困難となる。一方、他のセンサもしくは、外部情報によって、複数の対象物Ｍｎが所定の範囲内に近接していることが予め判明している場合には、その情報を位置状態の推定に利用することができる。具体的には、式（１８）に示すように、一の状態候補（交点）の尤度ｗｊを、その状態候補の所定範囲内に存在する他の状態候補（交点）の数ｎ（ｘｊＴ）に応じて高める補正をする。この式（１８）において、Ｇが補正値であり、Ｇ＞１である。
【０１２０】
【数１８】
2018025556.tif 000020
【０１２１】
この構成によれば、一の状態候補（交点）の近傍に存在する他の状態候補の数ｎ（ｘｊＴ）が多ければ多いほど、一の状態候補の尤度ｗｊが上方に補正されるため、複数の対象物Ｍｎが近接して存在していても、各対象物Ｍｎを精度良く位置推定することができる。
【０１２２】
［第５実施形態］
第５実施形態では、第２実施形態のステップＳ２５において、交点（観測値）ｚを算出するために選択される測角センサＳＯｍの数を検出確率に応じて変化させる。図１０は、検出確率と選択される測角センサの数との関係の一例を示すグラフである。
【０１２３】
上記した第２実施形態では、測角センサＳＯｍの検出確率が常時１００％ではないことを想定し、選択された少ない数の測角センサＳＯｍを用いて方位線ｌｍｎの交点ｚを求めている。しかし、検出確率は、例えば対象物Ｍｎとの距離に応じて変動する。一般に、検出確率が高い場合、使用する測角センサＳＯｍの数を増やした方が、方位線の数が増えて虚像を排除できるため、対象物Ｍｎの位置を精度良く推定できる。また、検出確率が高いにも関わらず、少数の測角センサＳＯｍを用いた場合には、交点（観測値）ｚが膨大となり計算負荷が増すことがある。
【０１２４】
このため、本実施形態では、交点算出部３４は、測角センサＳＯｍが対象物Ｍｎを検出する検出確率が高くなるにつれて、選択される測角センサＳＯｍの数を増やしている。図１０の例では、検出確率が５０％の場合には、半分の数（Ｕ／２）の測角センサＳＯｍを選択し、検出確率が１００％の場合にすべての測角センサＳＯｍを選択している。ここで、測角センサＳＯｍの数を決めるための検出確率は、例えば、すべての測角センサＳＯｍの検出確率の中で、最も低い測角センサＳＯｍの検出確率を採用する。また、すべての測角センサＳＯｍの検出確率の中央値や、平均値を用いても良い。
【０１２５】
本実施形態によれば、検出確率に応じて、使用する測角センサＳＯｍの数が変化し、検出確率が高くなるにつれて、測角センサＳＯｍの数を増やしているため、検出確率が低い場合は、交点（観測数）ｚを増やせる一方、検出確率が高い場合には、交点（観測数）ｚの増大を避けつつ、尤度の高い交点（観測数）ｚを得ることができる。この結果、検出確率に関わらず精度良く対象物Ｍｎの位置を推定できる。
【０１２６】
［第６実施形態］
第６実施形態では、第２実施形態における全線交差点ｑｌの情報を過去に遡って保持する時間窓幅ＴＭ（所定期間）を対象物Ｍｎの状態によって変更する。図１１は、時間窓幅と、測角センサに対する測角センサの相対速度と測角センサの測定周期との積と、の関係を示すグラフである。上述したように、全線交差点ｑｌの集合Ｑ（ｋ）は、現在時刻ｋのみならず過去の時刻ｋ−１、ｋ−２、・・の全線交差点ｑｌも含まれる。これは、検出確率が低い（１００％未満）場合であっても、一定数の全線交差点ｑｌを確保するための処置である。ここで、対象物Ｍｎが測角センサＳＯｍに対して相対的に移動している場合、過去の全線交差点ｑｌは対象物Ｍｎの位置に対応しない。
【０１２７】
本実施形態では、対象物Ｍｎと測角センサＳＯｍとの相対速度と、測角センサＳＯｍの測定周期（サンプリング周期）との積が大きくなるにつれて、全線交差点ｑｌの情報を過去に遡って保持する時間窓幅ＴＭを短くしている。この場合、他のセンサもしくは、外部情報によって、対象物Ｍｎの最大速度が予め想定できる場合には、その情報を利用して時間窓幅ＴＭを決定する。この構成によれば、対象物Ｍｎの相対移動速度に応じて、全線交差点ｑｌの情報を保持する時間窓幅ＴＭを適切に調整できるため、対象物Ｍｎが移動しても該対象物Ｍｎの位置を精度良く推定できる。
【０１２８】
本発明の一実施形態を説明したが、本実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。本実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。本実施形態やその変形は、発明の範囲や要旨に含まれると同様に、特許請求の範囲に記載された発明とその均等の範囲に含まれるものである。上記した第１実施形態では、任意に選択された２本の方位線ｌ１１，ｌ２１の交点Ｐ１２と、残りの方位線ｌ３１との距離が所定の誤差半径Ｒｅｒｒよりも小さいか否かで３本の方位線ｌ１１，ｌ２１，ｌ３１が交差しているか否かを判定している。このため、他の２組の方位線と交点についても、距離を算出し、最短距離を示した方位線と交点の組み合わせについて、交差の判定および位置の推定を行うことが好ましい。この構成では、３本の方位線ｌ１１，ｌ２１，ｌ３１の交点をより精度良く算出できるため、対象物Ｍ１，Ｍ２の位置をより精度良く推定することができる。
【符号の説明】
【０１２９】
１，１００  位置標定システム
１０，３０  演算処理部
１２，３２  交差判定部
１３，３６  位置推定部
１４  補正部
１６，３９  制御部
３３  記憶部
３４  交点算出部
３５  尤度判定部
Ｐ１２３，Ｐｐｑｒ  みなし交点
Ｐ１２，Ｐｐｑ  交点
Ｒｅｒｒ  誤差半径
ｄｍｉｎ  距離
Ｍ１，Ｍ２，Ｍｎ  対象物
Ｓ１，Ｓ２，Ｓ３，ＳＯ１，ＳＯ２，ＳＯ３，ＳＯ４，ＳＯｍ  測角センサ
【図１】
2018025556.tif 000021
【図２】
2018025556.tif 000022
【図３】
2018025556.tif 000023
【図４】
2018025556.tif 000024
【図５】
2018025556.tif 000025
【図６】
2018025556.tif 000026
【図７】
2018025556.tif 000027
【図８】
2018025556.tif 000028
【図９】
2018025556.tif 000029
【図１０】
2018025556.tif 000030
【図１１】
2018025556.tif 000031