JP2018025556A - 位置標定システム、および位置標定方法 - Google Patents

Abstract

【課題】複数の対象物の位置推定精度の向上を図った位置標定システム、および位置標定方法を提供すること。【解決手段】互いに独立して配置され、複数の対象物Ｍ１，Ｍ２に対する方位角をそれぞれ測定する少なくとも３つの測角センサＳ１，Ｓ２，Ｓ３と、３つの測角センサＳ１，Ｓ２，Ｓ３から対象物Ｍ１へそれぞれ延びる３つの方位線ｌ１１，ｌ２１，ｌ３１が交差するか否かを判定する交差判定部１２と、交差判定部１２が３つの方位線ｌ１１，ｌ２１，ｌ３１が交差すると判定した場合に、３つの方位線からみなし交点Ｐ１２３を算出し、このみなし交点Ｐ１２３を対象物Ｍ１の位置と推定する位置推定部１３とを備えた。【選択図】図１

Description

本発明は、測角センサを用いて対象の位置を推定する位置標定システム、および位置標定方法に関する。

一般に、捜索対象や欠陥などの対象物（目標ともいう）の位置を推定する場合、対象物に対する方位角を測定する測角センサを複数用いて、三角測量の原理に基づき、各測角センサから対象物に延びる方位線の交点から推定することできる。一方で、対象物が複数存在する場合には、測角センサから各対象物にそれぞれ延びる方位線は、対象物の個数が求められるため、各方位線の交点が対象物の真の位置以外の箇所（以下、虚像という）にも現れることとなる。このため、この虚像の存在により、実際よりも多くの個数の対象物が存在すると誤認識し、対象物の位置の推定精度が著しく劣化する問題がある。

このような問題を解決するために、従来、各目標の実測データとセンサ位置と実測データがどの目標から得られるかの可能性係数を目標毎に設定した分類データを入力とし、実測値との差を少なくするよう、目標の仮の位置の集まりで表されるクラスタ特性データを出力するクラスタ特性データ最適化手段と、クラスタ特性データと実測データとセンサ位置とを入力とし、実測値との差を少なくするよう、各実測データが各目標から得られる新しい可能性係数を目標毎に再設定し新しい分類データとしてクラスタ特性データ最適化手段と分類評価値算出手段に出力する分類データ最適化手段と、これらを入力とし、分類評価値を算出する分類評価値算出手段を備えた技術が提案されている（例えば、特許文献１参照）。複数の目標に対する状態推定を行う技術として、カルマンフィルタをベースとするＰＨＤフィルタが知られている（例えば、非特許文献１参照）。

特開平０９−３０４５２０号公報

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）信号に基づき、自己の位置情報を取得可能な構成とすれば、測角センサＳ_１〜Ｓ_３を移動体に搭載してもよい。

演算処理部１０は、図１に示すように、データ受信部１１、交差判定部１２、位置推定部１３、補正部１４、表示部１５、および制御部１６とを備える。データ受信部１１は、測角センサＳ_１，Ｓ_２，Ｓ_３から順次送信される方位角θ_１ｐ，θ_２ｑ，θ_３ｒを受信する。交差判定部１２は、受信した方位角θ_１ｐ，θ_２ｑ，θ_３ｒの情報に基づいて、測角センサＳ_１，Ｓ_２，Ｓ_３から対象物Ｍ_１，Ｍ_２に向けてそれぞれ延びる方位線ｌ_１ｐ，ｌ_２ｑ，ｌ_３ｒが交差するか否かを判定する。本実施形態では、交差するとは、方位線ｌ_１ｐ，ｌ_２ｑ，ｌ_３ｒが１点で交差するだけでなく、各方位線が所定領域内に存在する（所定領域内を通過する）場合を含む。位置推定部１３は、交差判定部１２が３本の方位線ｌ_１ｐ，ｌ_２ｑ，ｌ_３ｒが交差すると判定した場合、その交差した位置を対象物Ｍ_１，Ｍ_２と推定する。また、交差判定部１２が３本の方位線ｌ_１ｐ，ｌ_２ｑ，ｌ_３ｒのうち、２本の方位線のみが交差すると判定した場合には、位置推定部１３は、その交点２０，２１，２２は虚像であると推定する。

補正部１４は、所定の運動モデルに基づく状態推定フィルタを用いた状態推定によって、位置推定部１３が推定した対象物Ｍ_１，Ｍ_２の位置を補正する。状態推定フィルタとして、例えば、カルマンフィルタや粒子フィルタを用いることができる。測角センサＳ_１，Ｓ_２，Ｓ_３が計測する方位角は瞬時値であるため、この状態推定により、瞬時値のばらつきを抑圧することができ、測角誤差に起因する位置推定誤差を抑圧できる。

表示部１５は、例えば、ディスプレイ装置であり、推定した対象物Ｍ_１，Ｍ_２の位置情報を表示する。制御部１６は、位置標定システム１の動作を司るものであり、測角センサＳ_１，Ｓ_２，Ｓ_３、データ受信部１１、交差判定部１２、位置推定部１３、補正部１４を制御して、対象物Ｍ_１，Ｍ_２の位置を推定する動作を実行する。次に、位置標定方法の動作手順について説明する。

図２は、位置標定システムが対象物の位置推定を行う手順を示すフローチャートである。制御部１６の制御下、３つの測角センサＳ_１，Ｓ_２，Ｓ_３は、すべての対象物Ｍ_１，Ｍ_２に対する方位角θ_１ｐ，θ_２ｑ，θ_３ｒを計測する（ステップＳ１）。本実施形態では、説明の便宜上、対象物Ｍ_１，Ｍ_２を２つとしているが、実際の計測動作では、対象物の個数は不明である。このため、測角センサＳ_１，Ｓ_２，Ｓ_３がそれぞれ受信した受信信号に基づき、受信信号が発せられた方向に位置する対象物を仮定し、この対象物に対する方位角θ_１ｐ，θ_２ｑ，θ_３ｒを計測する。

次に、各測角センサＳ_１，Ｓ_２，Ｓ_３の計測情報を演算処理部１０に集約する（ステップＳ２）。この計測情報には、測角センサＳ_１，Ｓ_２，Ｓ_３が計測した方位角θ_１ｐ，θ_２ｑ，θ_３ｒ（計測値）のみならず、測角センサＳ_１，Ｓ_２，Ｓ_３の位置情報が含まれる。これらの計測情報により、測角センサＳ_１，Ｓ_２，Ｓ_３から対象物Ｍ_１，Ｍ_２へと延びる方位線ｌ_１ｐ，ｌ_２ｑ，ｌ_３ｒが想定される。

次に、交差判定部１２は、測角センサＳ_１が計測した方位角θ_１ｐから１つの方位角θ_１１を選択する（ステップＳ３）。この場合、方位角θ_１１の選択に伴い、方位角θ_１１に対応する方位線ｌ_１１が選択される。次に、交差判定部１２は、測角センサＳ_２が計測した方位角θ_２ｑから１つの方位角θ_２１を選択する（ステップＳ４）。この場合、方位角θ_２１の選択に伴い、方位角θ_２１に対応する方位線ｌ_２１が選択される。ステップＳ４で選択された方位角θ_２１（方位線ｌ_２１）は、上記した方位角θ_１１（方位線ｌ_１１）と組つけて後述する判定処理を行う。

交差判定部１２は、選択した方位角θ_２１（θ_２ｑ）は既に使用済であるかを判定する（ステップＳ５）。具体的には、選択された方位角θ_２１が既に他の方位角と組みつけられているかを判定する。既に使用済であれば（ステップＳ５；Ｙｅｓ）、ｑに１を加える処置を施した（ステップＳ６）のち、処理をステップＳ４に戻して方位角θ_２ｑから次の方位角θ_２２を選択する。また、使用済でなければ（ステップＳ５；Ｎｏ）、選択された２つの方位線の交点Ｐ_ｐｑ（この例では交点Ｐ_１２）の座標を算出する（ステップＳ７）。

ここで、本実施形態では、測角センサＳ_１，Ｓ_２の位置情報、および測角センサＳ_１，Ｓ_２から対象物Ｍ_１，Ｍ_２へと延びる２本の方位線ｌ_１１，ｌ_２１の方位角θ_１１，θ_２１が判っているため、２本の方位線ｌ_１１，ｌ_２１を示す一次関数式に基づいて、交点Ｐ_ｐｑ（この例では交点Ｐ_１２）の座標（Ｘ_１２，Ｙ_１２）が算出される。

次に、測角センサＳ_３のすべての方位線ｌ_３ｒと交点Ｐ_ｐｑとの距離ｄｒを算出する（ステップＳ８）。この場合、方位線ｌ_３ｒは、測角センサＳ_３の座標を（Ｘ_３，Ｙ_３）^Ｔとすると、式（１）で表すことができる。

このため、方位線ｌ_３ｒと交点Ｐ_ｐｑとの距離ｄｒは、式（２）で表すことができる。

この式（２）に基づき、方位線ｌ_３１，ｌ_３２と、交点Ｐ_１２との各距離ｄ_１，ｄ_２を算出し、この中から最短距離ｄ_ｍｉｎを抽出する（ステップＳ９）。

次に、交差判定部１２は、３本の方位線ｌ_１１，ｌ_２１，ｌ_３１が交差するか否かを判定する。測角センサＳ_１，Ｓ_２，Ｓ_３で測定された方位角は測定誤差があるため、１点で交わることは難しい。このため、本実施形態では、３本の方位線ｌ_１１，ｌ_２１，ｌ_３１が交差するか否かを簡易的に判定している。具体的には、図３に示すように、判定に際し、交点Ｐ_ｐｑ（交点Ｐ_１２）を中心とする円５０の誤差半径Ｒ_ｅｒｒを算出する（ステップＳ１０）。誤差半径Ｒ_ｅｒｒは、測角センサＳ_１，Ｓ_２の測角誤差の標準偏差σに基づいて設定されるものであり、例えば、本実施形態では、標準偏差σの３倍である３σまで考慮した値となっている。この誤差半径Ｒ_ｅｒｒは、適宜変更することができ、例えば、測角センサＳ_１，Ｓ_２の測角誤差が異なる場合には、測角誤差が大きい方の測角センサの測角誤差の標準偏差σを用いるのが好ましい。この構成では、測角誤差に考慮した誤差半径Ｒ_ｅｒｒを設定できる。

次に、交差判定部１２は、式（３）に基づいて、最短距離ｄ_ｍｉｎと誤差半径Ｒ_ｅｒｒとの大きさを比較する。

すなわち、方位線ｌ_３１が誤差半径Ｒ_ｅｒｒの円５０内を通過するか否かを判定する。この判定において、距離ｄ_ｍｉｎ≦誤差半径Ｒ_ｅｒｒでなければ（ステップＳ１１；Ｎｏ）、方位線ｌ_３１が誤差半径Ｒ_ｅｒｒの円５０内を通過しないため、３本の方位線ｌ_１１，ｌ_２１，ｌ_３１は交差しないと判定し、処理をステップＳ６に移行する。

一方、距離ｄ_ｍｉｎ≦誤差半径Ｒ_ｅｒｒであれば（ステップＳ１１；Ｙｅｓ）、３本の方位線ｌ_１１，ｌ_２１，ｌ_３１が交差するみなし交点Ｐ_ｐｑｒ（この例では交点Ｐ_１２３）を算出する（ステップＳ１２）。本実施形態では、みなし交点_ｐｑｒは最小二乗法によって算出される。具体的には、交差すると判定された３本の方位線ｌ_１１，ｌ_２１，ｌ_３１（便宜上、方位線ｌ_１，ｌ_２，ｌ_３と記す）が得られた場合、各方位線は、式（４）で示すことができる。

図３に示すように、方位線ｌ_１１，ｌ_２１は、交差するものの、方位線ｌ_３１も含めた３つの直線の最近傍点は交点Ｐ_ｐｑ（交点Ｐ_１２）と一致せず、改めて計算する必要がある。これらの最近傍点をみなし交点Ｐ_ｐｑｒ（ｘ_ｐｑｒ，ｙ_ｐｑｒ）^Ｔとすると、式（５）が成立する。

この式（５）は、いわゆる優決定系であるため、最小二乗法を用いることにより、みなし交点Ｐ_ｐｑｒ（交点Ｐ_１２３）の座標を算出することができ、このみなし交点Ｐ_１２３に対象物が存在すると推定できる。これにより、誤差の影響で確実に交わらない３本の方位線を仮想的に交わったものとみなすことができ、みなし交点Ｐ_ｐｑｒの位置から対象物の存在する位置を推定できる。また、この構成では、３本の方位線ｌ_１１，ｌ_２１，ｌ_３１が交差するみなし交点Ｐ_ｐｑｒに対象物が存在すると推定するため、対象物の個数が不明な場合であっても、対象物Ｍ_１の位置を精度良く推定することができる。なお、式（３）において、距離ｄ_ｍｉｎが０であった場合には、交点Ｐ_ｐｑ（交点Ｐ_１２）上で３本の方位線ｌ_１１，ｌ_２１，ｌ_３１が交差するため、この交点Ｐ_ｐｑをみなし交点Ｐ_ｐｑｒとすればよい。

さて、上記した計算により、測角センサＳ_１，Ｓ_２，Ｓ_３を用いて対象物の位置を計測することができる。特に、最小二乗法を用いることにより、みなし交点Ｐ_ｐｑｒ（交点Ｐ_１２３）の座標を算出することで、このみなし交点Ｐ_１２３に対象物が存在すると精度良く推定することができる。一方で、上記したみなし交点Ｐ_ｐｑｒ（交点Ｐ_１２３）の値は、測角センサＳ_１，Ｓ_２，Ｓ_３の測角誤差の影響を受けた瞬時値であるため、時系列で評価すると、対象物Ｍ_１，Ｍ_２の位置変化が急峻であるなど、実際の対象物Ｍ_１，Ｍ_２の挙動とは大きく異なっている可能性がある。また、対象物Ｍ_１，Ｍ_２が移動体であり、位置だけでなくその速度や加速度などの運動パラメータを推定しなければならない場合には、差分処理の影響で更にばらつきが大きくなる。

このため、本実施形態では、みなし交点Ｐ_ｐｑｒに状態推定による平滑化処理を施す（ステップＳ１３）。具体的には、対象物Ｍ_１，Ｍ_２の運動モデルを導入して状態推定を行い、誤差の影響を抑圧している。その運動モデルは、式（６）の状態方程式で示される。

この式は、対象物Ｍ_１，Ｍ_２の現在の位置が直前の位置と強い相関があることを意味しており、対象物Ｍ_１，Ｍ_２の動きをこの式で拘束している。これにより、測角誤差の影響を抑圧し、実際の対象の動き（静止を含む）に即した位置推定が可能となる。また、Ｔｓとｋは、それぞれ図２のフローチャートを１回実施する際の処理時間とサンプリングのインデクスを表している。

また、観測方程式は式（７）で示される。

この場合の観測値は、上記したステップＳ１２で得た位置の瞬時値（ｘ_ｐｑｒ（ｋ），ｙ_ｐｑｒ（ｋ））^Ｔであり、この観測値と上述の運動モデルに基づいて，カルマンフィルタや粒子フィルタ等の状態推定手法を適用することで、測定された方位角の観測誤差を抑圧して対象物Ｍ_１，Ｍ_２の位置を含む運動パラメータを推定できる。このため、対象物Ｍ_１，Ｍ_２の位置をより精度良く推定することができる。なお、この状態推定は、対象物Ｍ_１，Ｍ_２が移動する場合だけでなく、静止している場合にも適用することができる。

次に、制御部１６は、全対象物の位置を推定したか否かを判定する（ステップＳ１４）。具体的には、各測角センサＳ_１，Ｓ_２，Ｓ_３から延びる方位線の数（２本）と同じ数の対象物Ｍ_１，Ｍ_２の位置が推定されたか否かを判定する。この判定において、全対象物の位置が推定されていない場合（ステップＳ１４；Ｎｏ）には、ｐに１を加える処置を施した（ステップＳ１５）のち、処理をステップＳ３に戻して方位角θ_１ｐから次の方位角θ_１２を選択して処理を実行する。一方、全対象物の位置が推定されている場合（ステップＳ１４；Ｙｅｓ）には、処理を終了する。

以上、説明したように、本実施形態によれば、３つの方位線ｌ_１１，ｌ_２１，ｌ_３１からみなし交点Ｐ_１２３を算出し、このみなし交点Ｐ_１２３を対象物Ｍ_１の位置と推定するため、対象物Ｍ_１の位置を精度良く推定することができる。また、３つのｌ_１１，ｌ_２１，ｌ_３１が交差するか否かを判定することで対象物Ｍ_１の位置を推定するため、従来のように、対象物の数の真値を予め入力する必要はなく、対象物の数が不明の場合や変化する場合でも対象物の位置推定ができる。

［第２実施形態］
第１実施形態では、上述のように、少なくとも３つの測角センサＳ_１，Ｓ_２，Ｓ_３から延びる方位線ｌ_１１，ｌ_２１，ｌ_３１からみなし交点Ｐ_１２３を算出し、このみなし交点Ｐ_１２３を対象物Ｍ_１の位置と推定している。この構成では、すべて（例えば３つ）の測角センサＳ_１，Ｓ_２，Ｓ_３が常時、対象物Ｍ_１を検出（観測）する、すなわち検出確率が１００％であることを前提としている。しかしながら、現実には、すべての測角センサＳ_１，Ｓ_２，Ｓ_３が常時、すべて（例えば２つ）の対象物Ｍ_１，Ｍ_２を検出できるとは限らず、センサ処理系の性能に起因した検出確率に応じて、対象物Ｍ_１，Ｍ_２を見失う可能性がある。特に、対象物Ｍ_１，Ｍ_２が測角センサＳ_１，Ｓ_２，Ｓ_３から遠い位置に存在する場合には、相対的に検出確率が低くなるため、各測角センサＳ_１，Ｓ_２，Ｓ_３が検出できる対象物Ｍ_１，Ｍ_２は互いに異なることが想定される。従って、測角センサが対象物を常時検出できない場合には、みなし交点を正確に算出することができず、対象物の位置を推定する精度が悪い可能性があった。このため、第２実施形態では、測角センサが対象物を常時検出できない場合であっても、対象物の位置を精度良く推定できる構成について説明する。

図４は、第２実施形態に係る位置標定システムの概略構成を示すブロック図である。図５は、方向ベクトルを規定する方位角と天頂角とを説明するための図である。位置標定システム１００は、測角センサを用いて、例えば、捜索対象や欠陥などの対象物の位置を推定する。図４には、Ｕ個（一例として４つ）の測角センサＳ_Ｏ１，Ｓ_Ｏ２，Ｓ_Ｏ３，Ｓ_Ｏ４を用いて、Ｎ個（一例として２つ）の対象物Ｍ_１，Ｍ_２の位置を推定する場合が記載されている。測角センサの数は、少なくとも３つ備えていれば数を限るものではなく、３＜Ｕ個であればよい。また、対象物についても、複数であれば数を限るものではなく、２＜Ｎ個であればよい。第２実施形態では、図４に示すように、測角センサから対象物へと向かう所定の一方向をＹ方向とし、このＹ方向に水平方向に直交する方向をＸ方向とし、これらＸ方向及びＹ方向に高さ方向に直交する方向をＺ方向とする。２つの対象物Ｍ_１，Ｍ_２は、３次元空間（ＸＹＺ空間）に存在する物体であり、静止するものであっても、自由に移動するものであってもよい。

位置標定システム１００は、図４に示すように、４つの測角センサＳ_Ｏ１〜Ｓ_Ｏ４と、これら測角センサＳ_Ｏ１〜Ｓ_Ｏ４の計測値に基づき演算処理を行う演算処理部３０とを備える。測角センサＳ_Ｏ１〜Ｓ_Ｏ４は、３次元空間内に所定間隔をあけ、互いに独立して配置されており、演算処理部３０と有線または無線により通信可能に接続されている。測角センサＳ_Ｏ１〜Ｓ_Ｏ４は、対象物Ｍ_１，Ｍ_２からの信号（音波、光波、電波など）を受信して、この信号の発信方向の方位ベクトルを規定する方位角及び天頂角を計測する。

図５には、ｍ番目の測角センサＳ_Ｏｍ（ｍ＝１、２、・・・Ｕ）からｎ番目の対象物Ｍ_ｎに向けて延びる方向ベクトルｖ_ｍｎと、この方向ベクトルｖ_ｍｎに重なる方位線ｌ_ｍｎとが示されている。この図５では、説明の便宜上、測角センサＳ_Ｏｍを原点Ｏに配置している。この場合、方位角θ_ｍｎは、図５に示すように、方向ベクトルｖ_ｍｎをＸＹ平面に投影した線とＸ軸とのなす角として定義される。また、天頂角φ_ｍｎは、方向ベクトルｖ_ｍｎとＺ軸とのなす角として定義される。

測角センサＳ_Ｏ１〜Ｓ_Ｏ４は、それぞれ自己の位置情報を既知の情報として有しており、この位置情報と同時に計測した方位角θ_ｍｎ及び天頂角φ_ｍｎ（計測値）を演算処理部３０に順次送信する。なお、本実施形態では、測角センサＳ_Ｏ１〜Ｓ_Ｏ４は、所定位置に固定設置されたものとするが、例えば、ＧＰＳ信号に基づき、自己の位置情報を取得可能な構成とすれば、測角センサＳ_Ｏ１〜Ｓ_Ｏ４を移動体に搭載してもよい。

演算処理部３０は、図４に示すように、データ受信部３１、交差判定部３２、記憶部３３、交点算出部３４、尤度判定部３５、位置推定部３６、検出確率算出部３７、表示部３８、および制御部３９とを備える。

データ受信部３１は、測角センサＳ_Ｏ１〜Ｓ_Ｏ４から順次送信される方位角θ_ｍｎ及び天頂角φ_ｍｎを受信する。交差判定部３２は、受信した方位角θ_ｍｎ及び天頂角φ_ｍｎの情報に基づいて、すべての測角センサＳ_Ｏ１〜Ｓ_Ｏ４から対象物Ｍ_１（一の対象物）に向けてそれぞれ延びるすべての方位線ｌ_ｍｎ（ｌ_１１，ｌ_２１，ｌ_３１，ｌ_４１）が交差しているか否かを判定する。本実施形態では、交差するとは、すべての方位線ｌ_１１，ｌ_２１，ｌ_３１，ｌ_４１が１点で交差するだけでなく、各方位線が所定領域内に存在する（所定領域内を通過する）場合を含む。また、交差判定部３２は、すべての方位線ｌ_１１，ｌ_２１，ｌ_３１，ｌ_４１が所定領域内に存在しない（所定領域内を通過しない）場合には、交差していないと判定する。なお、交差判定部３２は、すべての測角センサＳ_Ｏ１〜Ｓ_Ｏ４から対象物Ｍ_２（一の対象物）に向けてそれぞれ延びるすべての方位線が交差するか否かについても判定するが、図４では、測角センサＳ_Ｏ１〜Ｓ_Ｏ４から対象物Ｍ_２に向けて延びる方位線の記載を省略している。

記憶部３３は、すべての方位線ｌ_ｍｎ（ｌ_１１，ｌ_２１，ｌ_３１，ｌ_４１）が交差した際に、この交差した全線交差点を記憶する。ここで、本実施形態では、すべての測角センサＳ_Ｏ１〜Ｓ_Ｏ４が、対象物Ｍ_１，Ｍ_２を常時、検出（観察）できない事情を鑑み、現在のみならず、現在から遡った過去の所定期間内における複数の全線交差点を記憶する。

交点算出部３４は、すべての測角センサＳ_Ｏ１〜Ｓ_Ｏ４の中から任意に選択された複数（例えば２本）の測角センサＳ_Ｏ１，Ｓ_Ｏ２から対象物Ｍ_１へそれぞれ延びる方位線ｌ_ｍｎ（ｌ_１１，ｌ_２１）が交差する交点を算出する。選択される測角センサの数Ｕ´は、２以上Ｕ以下であればよい。この場合、交点算出部３４は、測角センサの選択される組み合わせ（パターン）の数だけ、繰り返し複数の交点を算出する。例えば、４つの測角センサＳ_Ｏ１〜Ｓ_Ｏ４の中から任意に２つの測角センサを選択する組み合わせは、６パターンあるため、この６パターン分、方位線の交点を算出する。

尤度判定部３５は、ＰＨＤ（Probabilistic Hypothesis Density）フィルタを用いることにより、記憶部３３に記憶された全線交差点の情報に基づいて、算出された複数の交点の尤度を判定する。ＰＨＤフィルタは、カルマンフィルタのような状態推定技術を複数の対象物に対して同時に行えるように提案された方法論であり、本願では、上記した非特許文献１のようにカルマンフィルタをベースとした実装方法を採用している。一般に、方位線ｌ_ｍｎの数が増えるほど、これら方位線ｌ_ｍｎが交差した交点（例えば、全線交差点）に対象物が存在する確率（尤度）が高く、方位線ｌ_ｍｎの数が少ないほど、これら方位線ｌ_ｍｎが虚像の位置を示す虚像確率が高まることになる。本実施形態では、尤度判定部３５は、全線交差点の情報に基づいて、算出された各交点（観測値）の虚像確率を求めることにより、結果として、これら交点の尤度を判定している。

位置推定部３６は、尤度の大きさに基づいて交点を抽出し、これら交点をそれぞれ対象物Ｍ_１，Ｍ_２の位置と推定する。また、位置推定部３６は、尤度の小さい、すなわち虚像確率の高い交点については、虚像とみなして、対象物の候補から排除する。

検出確率算出部３７は、測角センサＳ_Ｏ１〜Ｓ_Ｏ４がそれぞれ対象物Ｍ_１，Ｍ_２にいずれかを検出する確率（検出確率）を算出する。具体的には、測角センサＳ_Ｏ１〜Ｓ_Ｏ４はそれぞれ、所定の測定周期ごとに、対象物（Ｍ_１であるかＭ_２は問わない）の測定を試みる。この場合、所定の測定回数（例えば１０回）に対する対象物が検出された回数を検出確率とする。算出された検出確率は、定期的に制御部３９に出力される。

表示部３８は、例えば、ディスプレイ装置であり、推定した対象物Ｍ_１，Ｍ_２の位置情報を表示する。制御部３９は、位置標定システム１００の動作を司るものであり、測角センサＳ_Ｏ１〜Ｓ_Ｏ４、データ受信部３１、交差判定部３２、記憶部３３、交点算出部３４、尤度判定部３５、位置推定部３６、検出確率算出部３７を制御して、対象物Ｍ_１，Ｍ_２の位置を推定する動作を実行する。次に、位置標定方法の動作手順について説明する。

図６は、位置標定システムが対象物の位置推定を行う手順を示すフローチャートである。制御部３９の制御下、４つの測角センサＳ_Ｏ１〜Ｓ_Ｏ４は、すべての対象物Ｍ_１，Ｍ_２に対する方位角θ_ｍｎ及び天頂角φ_ｍｎを計測する（ステップＳ２１）。そして、計測した各方位角θ_ｍｎ及び各天頂角φ_ｍｎを用いて、制御部３９は、方向ベクトルｖ_ｍｎと方位線ｌ_ｍｎとを算出する（ステップＳ２２）。

本実施形態では、説明の便宜上、対象物Ｍ_１，Ｍ_２を２つとしているが、実際の計測動作では、対象物の総数の真値Ｎは不明である。ここで、ｍ番目の測角センサＳ_Ｏｍが検出した対象物の数をＮ_ｍ（ｍ＝１、２・・Ｕ）とおく。各測角センサの検出確率が１００％である場合には、各測角センサがそれぞれ検出した対象物の数は同一であり、Ｎ_１＝・・＝Ｎ_ｍ＝・・Ｎ_Ｕ＝Ｎが成立する。しかし、本実施形態では、検出確率が１００％未満である場合を想定しているため、この等式は一般には成立せず、Ｎ_ｍ≦Ｎとなる。ｍ番目の測角センサＳ_Ｏｍが測定した方位角と天頂角とをそれぞれθ_ｍ１、・・・、θ_ｍＮｍおよびφ_ｍ１、・・・、φ_ｍＮｍとすると、方向ベクトルｖ_ｍｎは、式（８）で示される。

ここで、ｍ番目の測角センサＳ_Ｏｍの位置を、Ｓ_Ｏｍ＝［ｘ_Ｏｍ ｙ_Ｏｍ ｚ_Ｏｍ］^Ｔと表せるため、ｍ番目の測角センサＳ_Ｏｍが検出したｎ番目の対象物Ｍ_ｎに対応する方位線ｌ_ｍｎは、媒介変数ｔを用いて、式（９）で示される。

次に、交差判定部３２は、Ｕ個（例えば４つ）すべての測角センサＳ_Ｏ１〜Ｓ_Ｏ４から一の対象物Ｍ_ｎ（例えば対象物Ｍ_１）に向かうすべての方位線ｌ_ｍｎ（ｌ_１１，ｌ_２１，ｌ_３１，ｌ_４１）の最接近点ｑを計算する（ステップＳ２３）。理論上、実像（対象物が実際に存在する位置）ではすべて方位線ｌ_ｍｎが交わるため、すべての方位線ｌ_ｍｎが交わる点（全線交差点）が存在すれば、その点に対象物が存在するとみなすことができる。一方、実際には白色雑音などの測定誤差が生じるため、通常、測定値から算出したすべての方位線ｌ_ｍｎ（ｌ_１１，ｌ_２１，ｌ_３１，ｌ_４１）は交わらない。このため、交差判定部３２は、複数の方位線ｌ_ｍｎの最接近点ｑを計算した上で、その最接近点ｑにおいて各方位線ｌ_ｍｎが交差したか否かを判定する。

複数のｌ_ｍｎの最接近点ｑを、ｑ＝［ｑ_ｘ ｑ_ｙ ｑ_ｚ]^Ｔとすると、この最接近点ｑは、最小二乗法など公知技術を用いて求めることができる。例えば、最小二乗法を用いる場合には、式（１０）に示す連立方程式を解く。

ここで、ｖ_ｍ（ｍ＝１、２・・Ｕ）は、ｍ番目の測角センサＳ_Ｏｍの方向ベクトルの一つである。未知変数を一つのベクトルでまとめると、式（１０）は、式（１１）のように書き表せる。

この連立方程式は、式の数が３Ｕ、変数の数が３＋Ｕであり、Ｕ≧２の条件では式の数が変数の数よりも多い。式（１１）に最小二乗法を適用することにより、最接近点ｑが得られる。すべての測角センサＳ_Ｏｍ（ｍ＝１、２・・Ｕ）からすべての対象物Ｍ_ｎ（ｎ＝１、２・・Ｎ_ｍ）に向かう全ての方位線ｌ_ｍｎの組合せについて求めた最接近点ｑの集合を、ｑ_１、ｑ_２・・・ｑ_Ｌをとする。

次に、交差判定部３２は、最接近点ｑに方位線ｌ_ｍｎが交差したか否かを判定する（ステップＳ２４）。具体的には、図８に示すように、中心をｑ_ｌ（ｌ＝１、２・・・Ｌ）とし、所定半径を有する誤差球（誤差領域）４０を想定し、この誤差球４０の内部を一の対象物Ｍ_ｎ（Ｍ_１）に向けて延びるすべての方位線ｌ_ｍｎ（ｌ_１１，ｌ_２１，ｌ_３１，ｌ_４１）が通過するか否かを判定する。この誤差球４０の半径は、測角センサＳ_Ｏ１〜Ｓ_Ｏ４の測定誤差に起因して設定される。この判定において、誤差球４０の内部をすべての方位線ｌ_ｍｎ（ｌ_１１，ｌ_２１，ｌ_３１，ｌ_４１）が通過すれば、その中心が全線交差点ｑ_ｌであると判定する。一方、誤差球４０の内部をすべての方位線ｌ_ｍｎ（ｌ_１１，ｌ_２１，ｌ_３１，ｌ_４１）が通過しない場合には、この最接近点ｑは虚像であると判定する。なお、誤差球（誤差領域）４０は、必ずしも球形状である必要はなく、たとえば奥行方向と横方向とで三角測量の誤差が異なることを利用し、回転楕円形状を有する誤差楕円体（誤差領域）を採用することもできる。このような手法で選定した全線交差点ｑ_ｌの集合を、ｑ_１、ｑ_２・・・ｑ_Ｌ´とする（Ｌ´＜Ｌ）。

交差判定部３２は、算出された方位線ｌ_ｍｎの情報に基づいて、所定時間ごとに全線交差点ｑ_ｌを算出する。記憶部３３には、現在の全線交差点ｑ_ｌのみならず、過去の所定期間内における複数の全線交差点ｑ_ｌが記憶されている。

次に、交点算出部３４は、すべての測角センサＳ_Ｏｍの中から選択された複数の測角センサＳ_Ｏｍ（ｍ＝２、・・・Ｕ´、Ｕ´＜Ｕ）を用いて、これらの測角センサＳ_Ｏｍ（例えばＳ_Ｏ１，Ｓ_Ｏ２）から一の対象物Ｍ_ｎ（例えばＭ_１）へそれぞれ延びる方位線ｌ_ｍｎ（例えばｌ_１１，ｌ_２１）が交差する交点（観測値）ｚを算出する（ステップＳ２５）。上述したように、すべての方位線ｌ_ｍｎが交わる全線交差点ｑ_ｌが存在すれば、この全線交差点ｑ_ｌに対象物Ｍ_ｎが存在するとみなせるため、全線交差点ｑ_ｌをそのまま観測値として用いることができれば対象物Ｍ_ｎの位置を容易に推定することができる。

ところが、すべての測角センサＳ_Ｏｍがすべての対象物Ｍ_ｎを常時検出できない場合、すなわち測角センサＳ_Ｏｍの検出確率が低い（１００％未満）場合には、そもそも全線交差点ｑ_ｌが得られる確率が低いという問題がある。この問題は、測角センサＳ_Ｏｍの数が増えれば増えるほど顕著となる。従って、全線交差点ｑ_ｌのみを観測値として用いることは現実的ではなく、虚像が多く含まれてしまう可能性はあるものの、選択された数量の測角センサＳ_Ｏｍで交点（観測値）ｚを算出する必要がある。この場合、交点算出部３４は、測角センサの選択される組み合わせ（パターン）の数だけ、繰り返し複数の交点ｚを算出する。

次に、ＰＨＤフィルタを用いた演算処理を実行する（ステップＳ２６）。本実施形態では、ＰＨＤフィルタを用いて、加速度・速度・位置という運動パラメータの推定を行う。このため、状態方程式と観測方程式の組である状態空間表現を導入する。この状態空間表現は、式（１２）及び式（１３）によって示される。

この式（１２）及び式（１３）において、Ｔ_Ｓは測定周期（サンプリング周期）を表す。この例では、簡素化のためＴ_Ｓを固定値としているが、測定周期を変動する構成とすることもできる。また、ｕ（ｋ）とｗ（ｋ）はそれぞれ駆動雑音と観測雑音を表す。

最後に、時間をｋからｋ＋１に更新する（ステップＳ２７）ともに、処理を再びステップＳ２１に戻し、ステップＳ２１からステップＳ２７を繰り返し実行する。

次に、ＰＨＤフィルタを用いた演算処理（ステップＳ２６）について説明する。図７は、ＰＨＤフィルタを用いた演算処理の手順を示すフローチャートである。本実施形態では、ＰＨＤフィルタにより、記憶部３３に記憶された全線交差点ｑ_ｌの情報に基づいて、状態候補となる複数の交点ｚの虚像確率をそれぞれ算出し、この虚像確率に基づいて尤度を求めている。ここで尤度とは、算出された交点Ｚが対象物Ｍ_ｎである尤もらしさ表す示量であり、交点ｚの虚像確率によって変動する。

尤度判定部３５は、時間更新によって新規に発生する状態候補を生成する（ステップＳ３１）。この場合、測角センサＳ_Ｏｍの測定可能エリア内に出入りする対象物Ｍ_ｎを考慮する。次に、１時刻前（１測定周期前）の状態と相関のある新規状態候補を生成する（ステップＳ３２）。この場合には、例えば、対象物Ｍ_ｎが分裂する状態などを考慮する。これらステップＳ３１，Ｓ３２において、新規の状態候補が発生しなければ、そのまま処理をステップＳ３３に移行する。

次に、尤度判定部３５は、１時刻前（１測定周期前）状態候補を遷移させる（ステップＳ３３）。この場合、上記した式（１２）の状態方程式を用いて状態を遷移させる。ここで、ＰＨＤフィルタでは、状態候補が複数存在するため、これらすべての状態候補に対してそれぞれ状態遷移を行う。

次に、尤度判定部３５は、測角センサＳ_Ｏｍの検出確率に応じて、状態候補の重みを減衰する（ステップＳ３４）。具体的は、検出確率の高い対象物Ｍ_ｎの状態候補については重み付けを大きくし、逆に検出確率の低い対象物Ｍ_ｎの状態候補については小さくする。この場合、例えば、測角センサＳ_Ｏｍが検出できなくなった対象物Ｍ_ｎについても所定時間、状態候補を存続させておく。

次に、尤度判定部３５は、式（１３）の観測方程式を用いて、交点（観測値）ｚの推定値ｙ（ｋ）とカルマンゲインの計算を行う（ステップＳ３５）。そして、尤度判定部３５は、すべての状態候補に対して存在するカルマンゲインＫ（ｋ）∈Ｒ^９×３と交点ｚ（ｋ）を用いることで、式（１４）に示すように、状態の候補を補正する（ステップＳ３６）。この場合、一度、検出できなくなった対象物Ｍ_ｎを再検出することができる。

このような交点（観測値）ｚ（ｋ）と紐付いた状態の第ｊ候補に対して、ＰＨＤフィルタでは尤度ｗ^ｊを式（１５）のように計算して都度更新する（ステップＳ３７）。

この式（１５）において、κ（ｚ（ｋ）,Ｑ（ｋ））は、交点（観測値）ｚ（ｋ）の虚像確率であり、Ｑ（ｋ）は、現在時刻ｋから遡る過去の所定時間（過去数ステップ前から現在時刻ｋまで時間窓幅；所定期間）の全線交差点ｑ_ｌの集合である。この式（１５）に示すように、尤度ｗ^ｊは、虚像確率が高いほど小さくなるように設定されている。このため、算出された虚像確率の大きさに基づいて、尤度ｗ^ｊを求めることができる。

次に、尤度判定部３５は、尤度ｗ^ｊの低い状態候補を棄却したり、類似する状態候補を集約する（ステップＳ３８）。これにより、目標の数と状態をある程度推定することができる。

最後に、位置推定部３６は、尤度ｗ^ｊの大きさに基づいて、複数の交点（観測値）ｚの中から１つを抽出し、この抽出した交点ｚを対象物Ｍ_ｎの位置と推定する（ステップＳ３９）。位置推定部３６は、尤度ｗ^ｊの最も高い状態候補を抽出し、複数の対象物Ｍ_ｎの状態（位置などの運動パラメータ）を推定する。この構成によれば、現在または過去の全線交差点ｑ_ｌに基づいて交点ｚの尤度ｗ^ｊを判定し、この尤度ｗ^ｊの大きさから交点ｚの位置を対象物Ｍ_ｎの位置と推定するため、測角センサＳ_Ｏｍが対象物を常時検出できなくても、対象物Ｍ_ｎの位置を精度良く推定することができる。

本実施形態によれば、現在または過去の全線交差点ｑ_ｌの情報に基づいて交点（観測値）ｚの尤度ｗ^ｊを判定し、この尤度ｗ^ｊの大きさから交点（観測値）ｚの位置を対象物Ｍ_ｎの位置と推定するため、測角センサＳ_Ｏｍが対象物Ｍ_ｎを常時検出できなくても、対象物Ｍ_ｎの位置を精度良く推定することができる。

［第３実施形態］
この第３実施形態では、上記した第２実施形態の式（１５）における虚像確率κ（ｚ（ｋ）,Ｑ（ｋ））を、交点（観測値）ｚと全線交差点Ｑ（ｋ）との距離に基づいて算出する。図９は、全線交差点と各方位線の交点との配置関係の一例を示す図である。全線交差点ｑ_ｌの位置は、対象物の位置と考えられるため、この全線交差点ｑ_ｌに近い交点ｚほど虚像確率が低く、尤度が高いと考えられる。そこで、本実施形態では、選択された少数の測角センサで得た交点ｚと、全線交差点ｑ_ｌとの距離ｄをそれぞれ測定する。全線交差点ｑ_ｌの集合Ｑ（ｋ）はＬ´個の要素からなるため、虚像確率κ（ｚ（ｋ）,Ｑ（ｋ））を、式（１６）で定義することができる。

すなわち、本実施形態では、全線交差点ｑ_ｌに最も近い交点ｚとの距離で虚像確率を定義している。ただし、式（１６）のままでは絶対値によって、虚像確率の値が大きく変化するため、式（１７）により、虚像確率の総和が１となるよう正規化することが好ましい。

本実施形態では、全線交差点ｑ_ｌに最も近い交点ｚとの距離ｄで虚像確率を定義するため、尤度判定部３５は、全線交差点ｑ_ｌとの距離ｄが最も短い交点ｚの尤度ｗ^ｊを最も高く判定するため、測角センサの検出確率が低くても、虚像確率を適切に計算することでき、虚像を排除して対象物Ｍ_ｎの位置を精度良く推定することができる。

［第４実施形態］
第４実施形態は、複数の対象物Ｍ_ｎが所定の範囲内に近接していることが判明している場合に有効な手法である。通常、複数の対象物Ｍ_ｎが所定の範囲内に近接している場合、観測誤差の影響を受けるため、各対象物Ｍ_ｎの分離検出が困難となる。一方、他のセンサもしくは、外部情報によって、複数の対象物Ｍ_ｎが所定の範囲内に近接していることが予め判明している場合には、その情報を位置状態の推定に利用することができる。具体的には、式（１８）に示すように、一の状態候補（交点）の尤度ｗ^ｊを、その状態候補の所定範囲内に存在する他の状態候補（交点）の数ｎ（ｘ^ｊ _Ｔ）に応じて高める補正をする。この式（１８）において、Ｇが補正値であり、Ｇ＞１である。

この構成によれば、一の状態候補（交点）の近傍に存在する他の状態候補の数ｎ（ｘ^ｊ _Ｔ）が多ければ多いほど、一の状態候補の尤度ｗ^ｊが上方に補正されるため、複数の対象物Ｍ_ｎが近接して存在していても、各対象物Ｍ_ｎを精度良く位置推定することができる。

［第５実施形態］
第５実施形態では、第２実施形態のステップＳ２５において、交点（観測値）ｚを算出するために選択される測角センサＳ_Ｏｍの数を検出確率に応じて変化させる。図１０は、検出確率と選択される測角センサの数との関係の一例を示すグラフである。

上記した第２実施形態では、測角センサＳ_Ｏｍの検出確率が常時１００％ではないことを想定し、選択された少ない数の測角センサＳ_Ｏｍを用いて方位線ｌ_ｍｎの交点ｚを求めている。しかし、検出確率は、例えば対象物Ｍ_ｎとの距離に応じて変動する。一般に、検出確率が高い場合、使用する測角センサＳ_Ｏｍの数を増やした方が、方位線の数が増えて虚像を排除できるため、対象物Ｍ_ｎの位置を精度良く推定できる。また、検出確率が高いにも関わらず、少数の測角センサＳ_Ｏｍを用いた場合には、交点（観測値）ｚが膨大となり計算負荷が増すことがある。

このため、本実施形態では、交点算出部３４は、測角センサＳ_Ｏｍが対象物Ｍ_ｎを検出する検出確率が高くなるにつれて、選択される測角センサＳ_Ｏｍの数を増やしている。図１０の例では、検出確率が５０％の場合には、半分の数（Ｕ／２）の測角センサＳ_Ｏｍを選択し、検出確率が１００％の場合にすべての測角センサＳ_Ｏｍを選択している。ここで、測角センサＳ_Ｏｍの数を決めるための検出確率は、例えば、すべての測角センサＳ_Ｏｍの検出確率の中で、最も低い測角センサＳ_Ｏｍの検出確率を採用する。また、すべての測角センサＳ_Ｏｍの検出確率の中央値や、平均値を用いても良い。

本実施形態によれば、検出確率に応じて、使用する測角センサＳ_Ｏｍの数が変化し、検出確率が高くなるにつれて、測角センサＳ_Ｏｍの数を増やしているため、検出確率が低い場合は、交点（観測数）ｚを増やせる一方、検出確率が高い場合には、交点（観測数）ｚの増大を避けつつ、尤度の高い交点（観測数）ｚを得ることができる。この結果、検出確率に関わらず精度良く対象物Ｍ_ｎの位置を推定できる。

［第６実施形態］
第６実施形態では、第２実施形態における全線交差点ｑ_ｌの情報を過去に遡って保持する時間窓幅ＴＭ（所定期間）を対象物Ｍ_ｎの状態によって変更する。図１１は、時間窓幅と、測角センサに対する測角センサの相対速度と測角センサの測定周期との積と、の関係を示すグラフである。上述したように、全線交差点ｑ_ｌの集合Ｑ（ｋ）は、現在時刻ｋのみならず過去の時刻ｋ−１、ｋ−２、・・の全線交差点ｑ_ｌも含まれる。これは、検出確率が低い（１００％未満）場合であっても、一定数の全線交差点ｑ_ｌを確保するための処置である。ここで、対象物Ｍ_ｎが測角センサＳ_Ｏｍに対して相対的に移動している場合、過去の全線交差点ｑ_ｌは対象物Ｍ_ｎの位置に対応しない。

本実施形態では、対象物Ｍ_ｎと測角センサＳ_Ｏｍとの相対速度と、測角センサＳ_Ｏｍの測定周期（サンプリング周期）との積が大きくなるにつれて、全線交差点ｑ_ｌの情報を過去に遡って保持する時間窓幅ＴＭを短くしている。この場合、他のセンサもしくは、外部情報によって、対象物Ｍ_ｎの最大速度が予め想定できる場合には、その情報を利用して時間窓幅ＴＭを決定する。この構成によれば、対象物Ｍ_ｎの相対移動速度に応じて、全線交差点ｑ_ｌの情報を保持する時間窓幅ＴＭを適切に調整できるため、対象物Ｍ_ｎが移動しても該対象物Ｍ_ｎの位置を精度良く推定できる。

本発明の一実施形態を説明したが、本実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。本実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。本実施形態やその変形は、発明の範囲や要旨に含まれると同様に、特許請求の範囲に記載された発明とその均等の範囲に含まれるものである。上記した第１実施形態では、任意に選択された２本の方位線ｌ_１１，ｌ_２１の交点Ｐ_１２と、残りの方位線ｌ_３１との距離が所定の誤差半径Ｒ_ｅｒｒよりも小さいか否かで３本の方位線ｌ_１１，ｌ_２１，ｌ_３１が交差しているか否かを判定している。このため、他の２組の方位線と交点についても、距離を算出し、最短距離を示した方位線と交点の組み合わせについて、交差の判定および位置の推定を行うことが好ましい。この構成では、３本の方位線ｌ_１１，ｌ_２１，ｌ_３１の交点をより精度良く算出できるため、対象物Ｍ_１，Ｍ_２の位置をより精度良く推定することができる。

１，１００ 位置標定システム
１０，３０ 演算処理部
１２，３２ 交差判定部
１３，３６ 位置推定部
１４ 補正部
１６，３９ 制御部
３３ 記憶部
３４ 交点算出部
３５ 尤度判定部
Ｐ_１２３，Ｐ_ｐｑｒ みなし交点
Ｐ_１２，Ｐ_ｐｑ 交点
Ｒ_ｅｒｒ 誤差半径
ｄ_ｍｉｎ 距離
Ｍ_１，Ｍ_２，Ｍ_ｎ 対象物
Ｓ_１，Ｓ_２，Ｓ_３，Ｓ_Ｏ１，Ｓ_Ｏ２，Ｓ_Ｏ３，Ｓ_Ｏ４，Ｓ_Ｏｍ 測角センサ

Claims (26)

1. 互いに独立して配置され、複数の対象物に対する方位角をそれぞれ測定する少なくとも３つの測角センサと、
   ３つの測角センサから各対象物へそれぞれ延びる３つの方位線が交差するか否かを判定する交差判定部と、
   前記交差判定部が３つの前記方位線が交差すると判定した場合に、３つの前記方位線からみなし交点を算出し、このみなし交点を前記対象物の位置と推定する位置推定部と、
   を備えたことを特徴とする位置標定システム。
2. 前記交差判定部は、任意に選択された２つの測角センサから各対象物へそれぞれ延びる２つの方位線の交点を算出し、この交点を中心とする所定の誤差半径の円の内側を、残りの測角センサから該対象物へ延びる方位線が通過する場合に、３つの前記方位線は交差すると判定することを特徴とする請求項１に記載の位置標定システム。
3. 前記交差判定部は、任意に選択された２つの測角センサから各対象物へそれぞれ延びる２つの方位線の交点と、残りの測角センサから該対象物へ延びる方位線との距離を算出し、この距離が前記交点を中心とする円の所定の誤差半径よりも小さい場合に、３つの前記方位線は交差すると判定することを特徴とする請求項１に記載の位置標定システム。
4. 前記誤差半径は、前記測角センサの測角誤差の標準偏差の値に基づいて設定されることを特徴とする請求項２または３に記載の位置標定システム。
5. 前記誤差半径は、任意に選択された２つの測角センサのうち、測角誤差の大きい測角センサにおける測角誤差の標準偏差の値に基づいて設定されることを特徴とする請求項２または３に記載の位置標定システム。
6. 前記位置推定部は、３つの前記方位線から最小二乗法を用いて前記みなし交点を算出することを特徴とする請求項１から５のいずれか一項に記載の位置標定システム。
7. 所定の運動モデルに基づく状態推定フィルタを用いた前記対象物の状態推定によって、前記みなし交点の値を補正する補正部を備えたことを特徴とする請求項１から６のいずれか一項に記載の位置標定システム。
8. 互いに独立して配置され、複数の対象物に対する方位線の方向ベクトルを規定する方位角及び天頂角をそれぞれ測定する少なくとも３つの測角センサと、
   現在または過去の所定期間内に、すべての前記測角センサから一の対象物へそれぞれ延びるすべての前記方位線が交差した全線交差点を記憶する記憶部と、
   任意に選択された複数の前記測角センサから該対象物へそれぞれ延びる前記方位線が交差する複数の交点を算出する交点算出部と、
   前記全線交差点に基づき、複数の前記交点の尤度をそれぞれ判定する尤度判定部と、
   尤度の大きさに基づいて一の交点を抽出し、該交点を一の前記対象物の位置と推定する位置推定部と、
   を備えたことを特徴とする位置標定システム。
9. 前記尤度判定部は、前記全線交差点との距離が最も短い前記交点の尤度を最も高く判定することを特徴とする請求項８に記載の位置標定システム。
10. 予め複数の対象物が近接して存在することが判明している場合、
    前記尤度判定部は、一の交点の尤度を、該交点の所定範囲内に存在する他の交点の数に応じて高める補正をすることを特徴とする請求項８または９に記載の位置標定システム。
11. 前記交点算出部は、前記測角センサが前記対象物を検出する検出確率が高くなるにつれて、選択される前記測角センサの数を増やすことを特徴とする請求項８から１０のいずれか一項に記載の位置標定システム。
12. 前記記憶部は、前記対象物と前記測角センサとの相対速度と、前記測角センサの測定周期との積が大きくなるにつれて、前記所定期間を短くすることを特徴とする請求項８から１１のいずれか一項に記載の位置標定システム。
13. すべての前記測角センサから一の前記対象物へそれぞれ延びるすべての前記方位線の最接近点を算出すると共に、この最接近点を中心とした所定の誤差領域内を、すべての方位線が通過する場合に、該最接近点を前記全線交差点と判定する交差判定部を備えることを特徴とする請求項８から１２のいずれか一項に記載の位置標定システム。
14. 少なくとも３つの測角センサを用いて、複数の対象物に対する方位角をそれぞれ測定する測角ステップと、
    ３つの測角センサから各対象物へそれぞれ延びる３つの方位線が交差するか否かを判定する交差判定ステップと、
    ３つの前記方位線が交差すると判定された場合に、３つの前記方位線からみなし交点を算出し、このみなし交点を前記対象物の位置と推定する位置推定ステップと、
    を備えたことを特徴とする位置標定方法。
15. 前記交差判定ステップは、任意に選択された２つの測角センサから各対象物へそれぞれ延びる２つの方位線の交点を算出し、この交点を中心とする所定の誤差半径の円の内側を、残りの測角センサから該対象物へ延びる方位線が通過する場合に、３つの前記方位線は交差すると判定することを特徴とする請求項１４に記載の位置標定方法。
16. 前記交差判定ステップは、任意に選択された２つの測角センサから各対象物へそれぞれ延びる２つの方位線の交点と、残りの測角センサから該対象物へ延びる方位線との距離を算出し、この距離が前記交点を中心とする円の所定の誤差半径よりも小さい場合に、３つの前記方位線は交差すると判定することを特徴とする請求項１４に記載の位置標定方法。
17. 前記交差判定ステップにおいて、前記誤差半径は、前記測角センサの測角誤差の標準偏差の値に基づいて設定されることを特徴とする請求項１５または１６に記載の位置標定方法。
18. 前記交差判定ステップにおいて、前記誤差半径は、任意に選択された２つの測角センサのうち、測角誤差の大きい測角センサにおける測角誤差の標準偏差の値に基づいて設定されることを特徴とする請求項１５または１６に記載の位置標定方法。
19. 前記位置推定ステップは、３つの前記方位線から最小二乗法を用いて前記みなし交点を算出することを特徴とする請求項１４から１８のいずれか一項に記載の位置標定方法。
20. 所定の運動モデルに基づく状態推定フィルタを用いた前記対象物の状態推定によって、前記みなし交点の値を補正する補正ステップを備えたことを特徴とする請求項１４から１９のいずれか一項に記載の位置標定方法。
21. 少なくとも３つの測角センサを用いて、複数の対象物に対する方位線の方向ベクトルを規定する方位角及び天頂角をそれぞれ測定する測角ステップと、
    現在または過去の所定期間内に、すべての前記測角センサから一の対象物へそれぞれ延びるすべての前記方位線が交差した全線交差点を算出して記憶する全線交差点記憶ステップと、
    任意に選択された複数の前記測角センサから該対象物へそれぞれ延びる前記方位線が交差する複数の交点を算出する交点算出ステップと、
    前記全線交差点に基づき、複数の前記交点の尤度をそれぞれ判定する尤度判定ステップと、
    尤度の大きさに基づいて一の交点を抽出し、該交点を一の前記対象物の位置と推定する位置推定ステップと、
    を備えたことを特徴とする位置標定方法。
22. 前記尤度判定ステップにおいて、前記全線交差点との距離が最も短い前記交点の尤度を最も高く判定することを特徴とする請求項２１に記載の位置標定方法。
23. 予め複数の対象物が近接して存在することが判明している場合、
    前記尤度判定ステップにおいて、一の交点の尤度を、該交点の所定範囲内に存在する他の交点の数に応じて高める補正をすることを特徴とする請求項２１または２２に記載の位置標定方法。
24. 前記交点算出ステップにおいて、前記測角センサが前記対象物を検出する検出確率が高くなるにつれて、選択される前記測角センサの数が増えることを特徴とする請求項２１から２３のいずれか一項に記載の位置標定方法。
25. 前記全線交差点記憶ステップは、前記対象物と前記測角センサとの相対速度と、前記測角センサの測定周期との積が大きくなるにつれて、前記所定期間が短く設定されることを特徴とする請求項２１から２４のいずれか一項に記載の位置標定方法。
26. すべての前記測角センサから一の前記対象物へそれぞれ延びるすべての前記方位線の最接近点を算出すると共に、この最接近点を中心とした所定の誤差領域内を、すべての方位線が通過する場合に、該最接近点を前記全線交差点と判定する交差判定ステップを備えることを特徴とする請求項２１から２５のいずれか一項に記載の位置標定方法。

Images