(57)【要約】【課題】簡単な構成及び処理で測角センサが対象物を観測可能な位置に移動可能とする位置標定システム、および位置標定方法を提供すること。【解決手段】位置標定システム１００は、互いに独立して移動可能に配置され、複数の対象物Ｍ１，Ｍ２に対する方位観測値をそれぞれ測定する測角センサＳＯ１〜ＳＯ４と、方位観測値に基づいて、対象物Ｍ１，Ｍ２の位置を推定する位置推定部３３と、方位観測値から測角センサＳＯ１〜ＳＯ４がそれぞれ観測した対象物Ｍ１，Ｍ２の観測数を含む情報を取得するデータ受信部３０と、取得した対象物Ｍ１，Ｍ２の観測数に基づいて、対応する測角センサＳＯ１〜ＳＯ４の位置を移動させるセンサ移動制御部３１とを備えた。【選択図】図１
2019074374.tif 000002
【特許請求の範囲】
【請求項１】
互いに独立して移動可能に配置され、複数の対象物に対する方位線の方位角を含む方位観測値をそれぞれ測定する少なくとも３つの測角センサと、前記方位観測値に基づいて、前記対象物の位置を推定する位置推定部と、
前記方位観測値から前記測角センサがそれぞれ観測した前記対象物の観測数を含む情報を取得する情報取得部と、
取得した前記対象物の観測数に基づいて、対応する前記測角センサの位置を移動させるセンサ移動制御部と、を備えたことを特徴とする位置標定システム。
【請求項２】
前記センサ移動制御部は、観測数が最大の測角センサを選択し、この観測数が最大の測角センサに接近する方向に、残りの測角センサを移動させる第１の動作を実行することを特徴とする請求項１に記載の位置標定システム。
【請求項３】
前記センサ移動制御部は、前記第１の動作に続いて、前記観測数が最大の測角センサを、前記残りの測角センサから離間する所定方向に移動させる第２の動作を実行することを特徴とする請求項２に記載の位置標定システム。
【請求項４】
前記センサ移動制御部は、前記残りの測角センサの中から１つの測角センサを選択し、この選択された測角センサが移動中に観測する前記対象物の観測数が、前記観測数が最大の測角センサの観測数と同じになるまで前記第１の動作を実行することを特徴とする請求項２または３に記載の位置標定システム。
【請求項５】
前記センサ移動制御部は、前記観測数が最大の測角センサが移動中に観測する前記対象物の観測数が増加する、もしくは、前記観測数が最大の測角センサの移動距離が予め設定した所定距離に至るまで前記第２の動作を実行することを特徴とする請求項３に記載の位置標定システム。
【請求項６】
前記観測数は、一定時間内に前記測角センサがそれぞれ前記対象物を観測した瞬時観測値の平均値であることを特徴とする請求項１から５のいずれか一項に記載の位置標定システム。
【請求項７】
互いに独立して移動可能な測角センサを少なくとも３つ用いて、複数の対象物に対する方位線の方位角を含む方位観測値をそれぞれ測定するステップと、
前記方位観測値から前記測角センサがそれぞれ観測した前記対象物の観測数を取得するステップと、
前記対象物の観測数に基づいて、対応する前記測角センサの位置を移動させるステップと、
前記測角センサの位置を移動させた後の前記方位観測値に基づいて、前記対象物の位置を推定するステップと、を備えたことを特徴とする位置標定方法。
【請求項８】
対応する前記測角センサの位置を移動させるステップでは、観測数が最大の測角センサを選択し、この観測数が最大の測角センサに接近する方向に、残りの測角センサを移動させる第１の動作を実行し、前記第１の動作に続いて、前記観測数が最大の測角センサを、前記残りの測角センサから離間する所定方向に移動させる第２の動作を実行することを特徴とする請求項７に記載の位置標定方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、測角センサを用いて対象物の位置を標定する位置標定システム、および位置標定方法に関する。
【背景技術】
【０００２】
一般に、捜索対象や欠陥などの対象物（目標ともいう）の位置を推定（標定）する場合、対象物に対する方位線の方位角などを測定する測角センサを複数用いて、三角測量の原理に基づき、各測角センサから対象物に延びる方位線の交点から推定することできる。このような三角測量による位置を推定する技術では、複数の測角センサが対象物に対する方位角を常時測定する、すなわち各測角センサが対象物を常時観測することが要求される。
【０００３】
一方で、対象物と測角センサとの間に、例えば障害物が存在し、各測角センサが対象物を観測できない事態が生じることも想定される。このため、従来、センサを備える複数のロボット（移動体）が周囲環境を認識しながら動作する技術が提案されている（例えば、特許文献１参照）。この技術を上述した位置推定に適用することで、すべてのセンサが障害物を回避して、対象物を観測することも可能となる。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平０８−３０３２７号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかし、特許文献１に記載の技術は、周囲環境を認識する環境認識機構と、周囲環境から自己運動や周囲環境を変化させる自己運動指令を生成する学習機構とを備るため、自己運動指令を生成するための処理が複雑となり、対象物を観測できるまでに多大な時間を要するといった問題がある。
【０００６】
本発明は、上記の事情に鑑みてなされたものであり、簡単な構成及び処理で測角センサが対象物を観測可能な位置に移動可能とする位置標定システム、および位置標定方法を提供することを目的とする。
【課題を解決するための手段】
【０００７】
上述した課題を解決し、目的を達成するために、本発明に係る位置標定システムは、互いに独立して移動可能に配置され、複数の対象物に対する方位線の方位角を含む方位観測値をそれぞれ測定する少なくとも３つの測角センサと、方位観測値に基づいて、対象物の位置を推定する位置推定部と、方位観測値から測角センサがそれぞれ観測した対象物の観測数を含む情報を取得する情報取得部と、取得した対象物の観測数に基づいて、対応する測角センサの位置を移動させるセンサ移動制御部と、を備えたことを特徴とする。
【０００８】
この構成によれば、方位観測値から測角センサがそれぞれ観測した対象物の観測数を含む情報を取得する情報取得部と、取得した対象物の観測数に基づいて、対応する測角センサの位置を移動させるセンサ移動制御部と、を備えるため、簡単な構成及び処理によって、測角センサが対象物を観測可能な位置に移動させることができる。従って、各測角センサが観測する対象物の観測数を高めることができ、対象物の位置を精度良く推定することができる。
【０００９】
この構成において、センサ移動制御部は、観測数が最大の測角センサを選択し、この観測数が最大の測角センサに接近する方向に、残りの測角センサを移動させる第１の動作を実行してもよい。
【００１０】
また、センサ移動制御部は、第１の動作に続いて、観測数が最大の測角センサを、残りの測角センサから離間する所定方向に移動させる第２の動作を実行してもよい。
【００１１】
また、センサ移動制御部は、残りの測角センサの中から１つの測角センサを選択し、この選択された測角センサが移動中に観測する対象物の観測数が、観測数が最大の測角センサの観測数と同じになるまで第１の動作を実行してもよい。
【００１２】
また、センサ移動制御部は、観測数が最大の測角センサが移動中に観測する対象物の観測数が増加する、もしくは、観測数が最大の測角センサの移動距離が予め設定した所定距離に至るまで第２の動作を実行してもよい。
【００１３】
また、観測数は、一定時間内に測角センサがそれぞれ対象物を観測した瞬時観測値の平均値であってもよい。
【００１４】
本発明に係る位置標定方法は、互いに独立して移動可能な測角センサを少なくとも３つ用いて、複数の対象物に対する方位線の方位角を含む方位観測値をそれぞれ測定するステップと、方位観測値から測角センサがそれぞれ観測した対象物の観測数を取得するステップと、対象物の観測数に基づいて、対応する測角センサの位置を移動させるステップと、測角センサの位置を移動させた後の方位観測値に基づいて、対象物の位置を推定するステップと、を備えたことを特徴とする。
【００１５】
この構成において、対応する測角センサの位置を移動させるステップでは、観測数が最大の測角センサを選択し、この観測数が最大の測角センサに接近する方向に、残りの測角センサを移動させる第１の動作を実行し、第１の動作に続いて、観測数が最大の測角センサを、残りの測角センサから離間する所定方向に移動させる第２の動作を実行してもよい。
【発明の効果】
【００１６】
本発明によれば、方位観測値から測角センサがそれぞれ観測した対象物の観測数を含む情報を取得する情報取得部と、取得した対象物の観測数に基づいて、対応する測角センサの位置を移動させるセンサ移動制御部と、を備えるため、簡単な構成及び処理によって、測角センサが対象物を観測可能な位置に移動させることができる。従って、各測角センサが観測する対象物の観測数を高めることができ、対象物の位置を精度良く推定することができる。
【図面の簡単な説明】
【００１７】
【図１】図１は、本実施形態に係る位置標定システムの概略構成を示すブロック図である。
【図２】図２は、方位線の方向ベクトルを規定する方位角と天頂角とを説明するための図である。
【図３】図３は、位置標定システムの手順の概略を示すフローチャートである。
【図４】図４は、測角センサの縮小動作の手順を示すフローチャートである。
【図５】図５は、縮小動作前の測角センサの配置状態の一例を示す概略図である。
【図６】図６は、縮小動作後の測角センサの配置状態の一例を示す概略図である。
【図７】図７は、測角センサの拡大動作の手順を示すフローチャートである。
【図８】図８は、拡大動作前の測角センサの配置状態の一例を示す概略図である。
【図９】図９は、拡大動作後の測角センサの配置状態の一例を示す概略図である。
【図１０】図１０は、観測数が最大の測角センサが複数の測角センサの端に位置した場合の拡大動作を説明するための図である。
【図１１】図１１は、観測数が最大の測角センサが複数の測角センサの内側に位置した場合の拡大動作を説明するための図である。
【発明を実施するための形態】
【００１８】
以下に、本発明に係る実施形態を図面に基づいて詳細に説明する。なお、この実施形態によりこの発明が限定されるものではない。また、実施形態における構成要素には、当業者が置換可能かつ容易なもの、あるいは実質的に同一のものが含まれる。さらに、以下に記載した構成要素は適宜組み合わせることが可能である。
【００１９】
図１は、本実施形態に係る位置標定システムの概略構成を示すブロック図である。図２は、方位線の方向ベクトルを規定する方位角と天頂角とを説明するための図である。位置標定システム１００は、測角センサを用いて、例えば、捜索対象や欠陥などの対象物の位置を推定する。図１には、４つ（一般にＵ個）の測角センサＳＯ１，ＳＯ２，ＳＯ３，ＳＯ４を用いて、２つ（一般にＮ個）の対象物Ｍ１，Ｍ２の位置を推定する場合が記載されている。本実施形態では、測角センサから対象物へと向かう所定の一方向をＹ方向とし、このＹ方向に水平方向に直交する方向をＸ方向とする。この図１では、説明の便宜上、４つの測角センサＳＯ１〜ＳＯ４及び２つの対象物Ｍ１，Ｍ２を２次元平面（ＸＹ平面）に配置しているが、これに限るものでなく、上記Ｘ方向及びＹ方向と高さ方向に直交する方向をＺ方向とした３次元空間（ＸＹＺ空間）に存在する構成としてもよい。２つの対象物Ｍ１，Ｍ２は、２次元平面（ＸＹ平面）もしくは３次元空間（ＸＹＺ空間）内に静止するものであっても、自由に移動するものであってもよい。測角センサの数は、少なくとも３つ備えていれば数を限るものではなく、３＜Ｕ個であればよい。また、対象物についても、複数であれば数を限るものではなく、２＜Ｎ個であればよい。
【００２０】
位置標定システム１００は、図１に示すように、４つの測角センサＳＯ１〜ＳＯ４と、これら測角センサＳＯ１〜ＳＯ４の方位観測値に基づき演算処理を行う演算処理部２０とを備える。測角センサＳＯ１〜ＳＯ４は、互いに独立して移動可能に配置された移動体（不図示）に搭載されている。測角センサＳＯ１〜ＳＯ４及び移動体は、演算処理部２０と有線または無線により通信可能に接続されている。測角センサＳＯ１〜ＳＯ４は、対象物Ｍ１，Ｍ２からの信号（音波、光波、電波など）を受信して、各対象物Ｍ１，Ｍ２に対する方位線ｌ１１，ｌ１２，・・・，ｌ４１，ｌ４２の方位観測値（例えば、方位角及び天頂角）を計測する。これらの方位観測値は、方位線ｌ１１，ｌ１２，・・・，ｌ４１，ｌ４２の方向ベクトルを規定する。測角センサとして、例えば、対象物Ｍ１，Ｍ２が発する音声信号を受信するマイクを用いることができる。また、測角センサとして、周囲を撮影するカメラを用いて、撮影した画像情報に対象物を示す所定形状が存在するか否かを画像処理などにて識別する構成としてもよい。
【００２１】
図２には、ｍ番目の測角センサＳＯｍ（ｍ＝１、２、・・・Ｕ）からｎ番目の対象物Ｍｎ（ｎ＝１、２、・・・Ｎ）に向けて延びる方向ベクトルｖｍｎと、この方向ベクトルｖｍｎに重なる方位線ｌｍｎとが示されている。この図２では、説明の便宜上、測角センサＳＯｍを原点Ｏに配置している。この場合、方位角θｍｎは、図２に示すように、方向ベクトルｖｍｎをＸＹ平面に投影した線とＸ軸とのなす角として定義される。また、天頂角φｍｎは、方向ベクトルｖｍｎとＺ軸とのなす角として定義される。対象物ＭｎがＸＹ平面上に存在する場合には、方位観測値として方位角θｍｎが計測され、対象物ＭｎがＸＹＺ空間に存在する場合には、方位観測値として方位角θｍｎ及び天頂角φｍｎを計測する。
【００２２】
測角センサＳＯ１〜ＳＯ４は、例えば、ＧＰＳ（Global Positioning System）信号に基づき、自己（移動体）の位置情報を取得可能な構成となっており、自己の位置情報及び姿勢情報と共に、計測した方位角θｍｎ、天頂角φｍｎ（方位観測値）を演算処理部２０に順次送信する。
【００２３】
演算処理部２０は、図１に示すように、データ受信部（情報取得部）３０、センサ移動制御部３１、交差判定部３２、位置推定部３３、表示部３４、および制御部３５とを備える。
【００２４】
データ受信部３０は、測角センサＳＯ１〜ＳＯ４から順次送信される各測角センサＳＯ１〜ＳＯ４の方位観測値（方位角θｍｎ及び天頂角φｍｎ）、位置情報及び姿勢情報を受信する。データ受信部３０は、例えば、各測角センサＳＯ１〜ＳＯ４から送信された方位観測値により規定される方位線ｌｍｎの本数から、各測角センサＳＯ１〜ＳＯ４がそれぞれ観測できた対象物の観測数を取得する。センサ移動制御部３１は、各測角センサＳＯ１〜ＳＯ４を搭載した移動体を動作させることにより、各測角センサＳＯ１〜ＳＯ４を所望の位置に移動する制御を行う。本構成では、センサ移動制御部３１は、各測角センサＳＯ１〜ＳＯ４が観測した対象物の観測数に基づいて、対応する測角センサＳＯ１〜ＳＯ４（移動体）の移動する位置（移動先）を制御する。
【００２５】
交差判定部３２は、受信した方位観測値の情報に基づいて、すべての測角センサＳＯ１〜ＳＯ４から対象物Ｍ１（一の対象物）に向けてそれぞれ延びるすべての方位線ｌｍｎ（例えば、ｌ１１，ｌ２１，ｌ３１，ｌ４１）が交差しているか否かを判定する。本実施形態では、交差するとは、観測できた対象物Ｍ１（一の対象物）に向けて延びるすべての方位線ｌ１１，ｌ２１，ｌ３１，ｌ４１が１点で交差するだけでなく、各方位線が所定領域内に存在する（所定領域内を通過する）場合を含む。また、交差判定部３２は、すべての方位線ｌ１１，ｌ２１，ｌ３１，ｌ４１が所定領域内に存在しない（所定領域内を通過しない）場合には、交差していないと判定する。なお、交差判定部３２は、すべての測角センサＳＯ１〜ＳＯ４から対象物Ｍ２（一の対象物）に向けてそれぞれ延びるすべての方位線ｌ１２，ｌ２２，ｌ３２，ｌ４２が交差するか否かについても判定する。
【００２６】
位置推定部３３は、交差判定部３２が交差していると判断した点（交点）に基づき、これら交点をそれぞれ対象物Ｍ１，Ｍ２の位置と推定する。
【００２７】
表示部３４は、例えば、ディスプレイ装置であり、推定した対象物Ｍ１，Ｍ２の位置情報を表示する。制御部３５は、位置標定システム１００の動作を司るものであり、測角センサＳＯ１〜ＳＯ４、データ受信部３０、センサ移動制御部３１、交差判定部３２、位置推定部３３を制御し、測角センサＳＯ１〜ＳＯ４を効果的な位置に移動させつつ、対象物Ｍ１，Ｍ２の位置を推定する動作を実行する。次に、位置標定方法の動作手順について説明する。
【００２８】
図３は、位置標定システムの手順の概略を示すフローチャートである。複数の測角センサのＳＯ１〜ＳＯ４を用いて、複数の対象物Ｍ１，Ｍ２の位置を標定する場合、各測角センサのＳＯ１〜ＳＯ４が対象物Ｍ１，Ｍ２を観測することが重要である。すなわち、各測角センサのＳＯ１〜ＳＯ４が対象物Ｍ１，Ｍ２を常時観測できれば、対象物Ｍ１，Ｍ２の位置をより正確に標定（推定）することができる。一方で、対象物Ｍ１，Ｍ２が移動したり、対象物Ｍ１，Ｍ２と測角センサのＳＯ１〜ＳＯ４との間に、例えば障害物が存在する場合には、測角センサのＳＯ１〜ＳＯ４が対象物Ｍ１，Ｍ２を観測できない事態が生じることも想定される。このため、本実施形態では、複数の測角センサＳＯ１〜ＳＯ４の位置を観測数に基づいて移動することで、測角センサのＳＯ１〜ＳＯ４の平均観測数を向上させ、対象物Ｍ１，Ｍ２の位置の推定精度の向上を図っている。
【００２９】
具体的には、図３に示すように、４つの測角センサＳＯ１〜ＳＯ４が、それぞれ対象物Ｍ１，Ｍ２に対する方位観測値（少なくとも方位角θｍｎ）を測定する（ステップＳＴ１）。測定された方位観測値は、各該測角センサＳＯ１〜ＳＯ４の位置情報及び姿勢情報と共にデータ受信部３０に送信して集約される（ステップＳＴ２）。ここで、データ受信部３０は、例えば、各測角センサＳＯ１〜ＳＯ４から送信された方位観測値により規定される方位線ｌｍｎの本数から、各測角センサＳＯ１〜ＳＯ４が観測できた対象物Ｍ１，Ｍ２の観測数を取得する。次に、取得した対象物Ｍ１，Ｍ２の観測数に基づき、対応する測角センサＳＯ１〜ＳＯ４の位置を協調させて移動して（ステップＳＴ３）、測角センサＳＯ１〜ＳＯ４の平均観測数を向上させる。そして、測角センサＳＯ１〜ＳＯ４を移動させた後に再度測定された方位観測値に基づいて対象物Ｍ１，Ｍ２の位置標定を行う（ステップＳＴ４）。本実施形態では、対象物Ｍ１，Ｍ２を観測できた測角センサＳＯ１〜ＳＯ４から延びるすべての方位線が、交差したと判別された場合に、位置推定部３３は、その交点を対象物Ｍ１，Ｍ２の位置と推定する。これらステップＳＴ１〜ステップＳＴ４の手順は周期的に実行されて対象物Ｍ１，Ｍ２の位置標定が行われる。
【００３０】
本構成では、上記ステップＳＴ３に示す測角センサＳＯ１〜ＳＯ４の位置を対象物Ｍ１，Ｍ２の観測数に基づいて移動させる動作に特徴を有する。この動作は、測角センサＳＯ１〜ＳＯ４の被覆領域（ベースライン）が縮小する縮小動作（第１の動作）と、被覆領域が拡大する拡大動作（第２の動作）とに大別されるため、まず、測角センサの縮小動作について詳細に説明する。図４は、測角センサの縮小動作の手順を示すフローチャートである。図５は、縮小動作前の測角センサの配置状態の一例を示す概略図であり、図６は、縮小動作後の測角センサの配置状態の一例を示す概略図である。
【００３１】
縮小動作が実行される前（時刻ｔ）において、測角センサＳＯ１〜ＳＯ４は、図５に示すように、所定の被覆領域Ｑ（S1(t),S2(t),S3(t),S4(t)）内に分散して配置されている。この被覆領域Ｑは、４つの測角センサＳＯ１〜ＳＯ４により、対象物Ｍ１，Ｍ２を観測するために設定された空間であり、その大きさは任意に設定される。図５の例では、測角センサＳＯ１，ＳＯ２と対象物Ｍ１，Ｍ２との間に障害物５０が存在するため、測角センサＳＯ１が観測する対象物Ｍ１，Ｍ２の観測数は０、測角センサＳＯ２が観測する対象物Ｍ１，Ｍ２の観測数は１となる。これに対して、測角センサＳＯ３，ＳＯ４が観測する対象物Ｍ１，Ｍ２の観測数はそれぞれ２となっている。
【００３２】
縮小動作を実行する場合、まず、センサ移動制御部３１は、対象物Ｍ１，Ｍ２の観測数が最大の測角センサを特定する（ステップＳＴ１１）。この図５の例では、測角センサＳＯ３，ＳＯ４の観測数がそれぞれ２で最大となっているため、例えば、測角センサＳＯ４を観測数が最大の測角センサとして特定する。次に、センサ移動制御部３１は、残りの測角センサを観測数が最大の測角センサに接近する方向に移動させる（ステップＳＴ１２）。具体的には、測角センサＳＯ１〜ＳＯ３を、測角センサＳＯ４に接近する方向にそれぞれ移動させる。ここで、多くの対象物を観測できている測角センサの位置近辺は、死角が少なく対象物を観測しやすいことが期待できる。このため、残りの測角センサＳＯ１〜ＳＯ３を観測数が最大の測角センサＳＯ４に接近する方向に移動することにより、測角センサＳＯ１，ＳＯ２の観測数の改善が期待できる。
【００３３】
一方、残りの測角センサＳＯ１〜ＳＯ３を測角センサＳＯ４の位置まで移動させてしまうと、対象物Ｍ１，Ｍ２の観測数が改善するものの、対象物Ｍ１，Ｍ２の位置標定の精度は低下することとなる。このため、次に、センサ移動制御部３１は、残りの測角センサの中から１つの測角センサを選択する（ステップＳＴ１３）。選択される測角センサは、例えば、観測数が最大の測角センサから離れた位置にあるものが好ましく、図５の例では、測角センサＳＯ２が選択される。
【００３４】
次に、センサ移動制御部３１は、選択された測角センサＳＯ２が移動中に観測した対象物Ｍ１，Ｍ２の観測数が測角センサＳＯ４の観測数と同一になったか否かを判別する（ステップＳＴ１４）。具体的には、測角センサＳＯ２が縮小動作前の１から２に変更されたか否かを判別する。この判別において、測角センサＳＯ２の観測数が測角センサＳＯ４の観測数と同一にならない場合（ステップＳＴ１４；Ｎｏ）には、処理をステップＳＴ１３に戻し、他の測角センサを新たに選択する。また、測角センサＳＯ２の観測数が測角センサＳＯ４の観測数と同一となった場合（ステップＳＴ１４；Ｙｅｓ）には、この測角センサＳＯ２の移動を停止する（ステップＳＴ１５）。ここで、測角センサＳＯ２以外の測角センサＳＯ１，ＳＯ３の位置を設定しないと、測角センサＳＯ１，ＳＯ３が、観測数最大の測角センサＳＯ４に接近しすぎてしまい、対象物Ｍ１，Ｍ２の位置標定の精度が低下するおそれがある。このため、センサ移動制御部３１は、観測数最大の測角センサＳＯ４と選択された測角センサＳＯ２を除いた測角センサＳＯ１，ＳＯ３（他の測角センサ）の位置を被覆制御により決定する（ステップＳＴ１６）。この場合、観測数最大の測角センサＳＯ４と選択された測角センサＳＯ２との位置によって、図６に示すように、縮小動作が実行後の時刻ｔ＋１に対応する新たな被覆領域Ｑ（S1(t+1),S2(t+1),S3(t+1),S4(t+1)）が設定される。そして、測角センサＳＯ１，ＳＯ３の位置を、この新たな被覆領域内で例えば等間隔となるように分散して配置する。
【００３５】
続いて、センサ移動制御部３１は、すべての測角センサＳＯ１〜ＳＯ４の観測数が同一もしくは、所定の閾値（例えば、最大観測数−１）以内となったか否かを判別する（ステップＳＴ１７）。この判別において、すべての測角センサＳＯ１〜ＳＯ４の観測数が同一もしくは、所定の閾値以内となっていない場合（ステップＳＴ１７；Ｎｏ）には、処理をステップＳＴ１３に戻し、他の測角センサを新たに選択する。また、すべての測角センサＳＯ１〜ＳＯ４の観測数が同一もしくは、所定の閾値以内となった場合（ステップＳＴ１７；Ｙｅｓ）には、測角センサＳＯ１，ＳＯ２の観測数の改善が図られたため、処理を終了する。
【００３６】
次に、縮小動作の原理について説明する。縮小動作において、測角センサ群の総数をＵとし、各測角センサＳＯｍのダイナミクスを式（１）と規定する。
【００３７】
【数１】
2019074374.tif 000003
【００３８】
ここで、ｕｍ（ｋ）は、ｍ番目の測角センサＳＯｍへの制御入力（例えばＸＹ平面上での速度指令値；２次元ベクトル）であり、ｋは、時間のインデクスを表す。本実施形態の縮小動作では、ステップＳＴ１６に示す被覆制御が同時に実行されるため、このｕｍ（ｋ）を式（２）と記載する。この式（２）の右辺第１項は、被覆制御に係る項であり、第２項は、縮小動作に係る項である。また、ｋＣとｋＳは、それらの影響の度合いを規定するゲイン（スカラ）を表す。
【００３９】
【数２】
2019074374.tif 000004
【００４０】
被覆制御の制御入力は、例えば『東,永原,石井,林,桜間,畑中,マルチエージェントシステムの制御,2015』に記載の方法に従い、式（３）により求める。
【００４１】
【数３】
2019074374.tif 000005
【００４２】
ここで、Ｃｍ（ｋ）は、ｍ番目の測角センサＳＯｍの位置ｘｍ（ｋ）に対するボロノイ領域の重心を表す。本実施形態では、上記書籍に記載の公知技術とは異なり、縮小動作の影響により、被覆領域が時間変化する。このため、各測角センサＳＯｍの位置ｘｍ（ｋ）に対するボロノイ領域の重心も同様に時間変化する。このため、本実施形態では、処理周期毎に重心を計算し直している。この制御入力により、上記した測角センサＳＯ１，ＳＯ３は、それぞれボロノイ領域の重心へ移動して、例えば等間隔に測角センサＳＯ１〜ＳＯ４が配置される。なお、等間隔配置は、一例であり、所定の被覆領域内に分散して配置されていればよい。
【００４３】
一方、縮小動作の制御入力の計算方法は公知ではない。このため、本実施形態では、式（４）のように計算している。
【００４４】
【数４】
2019074374.tif 000006
【００４５】
この式（４）において、ｘｍＭＡＸは、観測数が最大の測角センサの位置を表す。また、Ｎｍ（ｋ）は、ｍ番目の測角センサＳＯｍの時刻ｋにおける対象物の観測数を表す。このＮｍ（ｋ）を入力として、各測角センサの縮小動作ゲインＫ（Ｎｍ（ｋ））を、測角センサＳＯｍの観測数が、観測数最大の測角センサの観測数と同一となったか否かで判別する。同一になると、縮小動作ゲインＫ（Ｎｍ（ｋ））が０となるため、縮小動作が停止する。この制御入力により、縮小動作開始時点で観測数が最大の測角センサの位置へ、残りの測角センサが集合する。この制御入力のみであれば、すべての測角センサが同じ位置で重なってしまう事態が生じるが、上記した被覆制御入力と、縮小動作ゲインＫ（Ｎｍ（ｋ））によって、すべての測角センサが同じ位置に重なることなく、被覆領域に分散して配置される。本実施形態では、縮小動作の影響で被覆領域が変化する。そして、この変化する被覆領域の中で可能な限りベースラインを維持しつつ、各測角センサが共通して観測できる対象物の観測数（平均観測数）を増大させることができる。
【００４６】
上記した縮小動作により、理想的にはすべての測角センサＳＯ１〜ＳＯ４の観測数が同一（一致）することができる。一方、対象物の総数は不明であるため、縮小動作によって、すべての測角センサＳＯ１〜ＳＯ４間で一致させた観測数が、対象物の総数と一致するとは限らない。このため、縮小動作に続いて拡大動作を実行する。次に、拡大動作について説明する。図７は、測角センサの拡大動作の手順を示すフローチャートである。図８は、拡大動作前の測角センサの配置状態の一例を示す概略図である。図９は、拡大動作後の測角センサの配置状態の一例を示す概略図である。
【００４７】
拡大動作は、図８に示すように、縮小動作後の時刻ｔ＋１に対応する被覆領域Ｑ（S1(t+1),S2(t+1),S3(t+1),S4(t+1)）対して実行される。この場合、観測空間には、障害物５１により、すべての測角センサＳＯ１〜ＳＯ４が観測できていない対象物Ｍ３が存在しているものとする。
【００４８】
拡大動作を実行する場合、図７に示すように、センサ移動制御部３１は、観測数が最大の測角センサを残りの測角センサから離間する所定方向に移動させる（ステップＳＴ２１）。図８の例では、測角センサＳＯ４を各測角センサの並んだ方向に沿って、残りの測角センサＳＯ１〜ＳＯ３から離間するように移動させる。次に、測角センサが移動中に観測した対象物の観測数が増加したか否かを判別する（ステップＳＴ２２）。この判別において、対象物の観測数が増加した場合（ステップＳＴ２２；Ｙｅｓ）には、図９に示すように、新たな対象物Ｍ３を観測することができたため、測角センサＳＯ４をその場所で停止して拡大動作の処理を終了する。
【００４９】
一方、対象物の観測数が増加しない場合（ステップＳＴ２２；Ｎｏ）には、センサ移動制御部３１は、測角センサＳＯ４が予め定めた所定距離に至るまで移動したか否かを判別する（ステップＳＴ２３）。ここで所定距離とは、移動を許容する限界距離として任意に設定する値であり、観測空間の大きさ等によって、例えば１０ｍ〜１０００ｍの間で変更可能である。この判別において、所定距離に至るまで移動していない（ステップＳＴ２３；Ｎｏ）場合には、処理をステップＳＴ２２に戻し、これらステップＳＴ２２，２３を繰り返し実行する。また、所定距離に至るまで移動した（ステップＳＴ２３；Ｙｅｓ）場合には、上記した所定方向に移動させても対象物の観測数が増加しない可能性が高い。このため、センサ移動制御部３１は、測角センサＳＯ４を一旦、元の位置に戻した後、残りの測角センサＳＯ１〜ＳＯ３から離間する別の所定方向に移動させる（ステップＳＴ２４）。図９の例では、測角センサＳＯ４を各測角センサＳＯ１〜ＳＯ４の並んだ方向に沿って、残りの測角センサＳＯ１〜ＳＯ３から離間するように移動させたが、今度は、例えば、各測角センサＳＯ１〜ＳＯ４の並んだ方向に直交（交差）する方向に沿って、残りの測角センサＳＯ１〜ＳＯ３から離間するように移動させる。これにより、新たな対象物Ｍ３を観測できる可能性が生じる。
【００５０】
次に、測角センサが移動中に観測した対象物の観測数が増加したか否かを判別する（ステップＳＴ２５）。この判別において、対象物の観測数が増加した場合（ステップＳＴ２５；Ｙｅｓ）には、新たな対象物Ｍ３を観測することができたため、測角センサＳＯ４をその場所で停止して拡大動作の処理を終了する。
【００５１】
一方、対象物の観測数が増加しない場合（ステップＳＴ２５；Ｎｏ）には、センサ移動制御部３１は、測角センサＳＯ４が予め定めた所定距離に至るまで移動したか否かを判別する（ステップＳＴ２６）。この判別において、所定距離に至るまで移動していない（ステップＳＴ２６；Ｎｏ）場合には、処理をステップＳＴ２５に戻し、これらステップＳＴ２５，２６を繰り返し実行する。また、所定距離に至るまで移動した（ステップＳＴ２６；Ｙｅｓ）場合には、上記した所定方向に移動させても対象物の観測数が増加しない可能性が高い。このため、センサ移動制御部３１は、測角センサＳＯ４を元の位置に戻し（ステップＳ２７）、処理を終了する。
【００５２】
次に、拡大動作の原理について説明する。図１０は、観測数が最大の測角センサが複数の測角センサの端に位置した場合の拡大動作を説明するための図である。図１１は、観測数が最大の測角センサが複数の測角センサの内側に位置した場合の拡大動作を説明するための図である。この拡大動作では、縮小動作と異なり、観測数が最大の測角センサのみが移動する。この場合、観測数が最大の測角センサが複数の測角センサ（センサ群）の端に位置するか否かで該測角センサの移動方向が異なる。図１０に示すように、観測数が最大の測角センサＳＯ４（ＳＯＵ）が複数の測角センサの端であった場合には、より遠ざかることで観測数が更に増大することを期待し、各測角センサの並んだ線αに沿って、かつ、残りの測角センサＳＯ１〜ＳＯ３から離間する方向（図中矢印α１方向）に移動する。一方、図１１に示すように、観測数が最大の測角センサＳＯ３（ＳＯＲ）が複数の測角センサの端ではなかった場合、各測角センサの並んだ線αと概ね直交（交差）する方向（図中矢印β１、β２方向）へ移動する。いずれの場合も、移動中に観測数が増大した場合は、上述したように、測角センサをその場所で停止する。また、所定距離Ｌだけ移動しても観測数が元の場所から増大しなかった場合は、上述のように、元の位置へ戻して別の方向へ移動するか、その時点で処理を終了する。また、観測数が最大の測角センサが複数ある場合には、観測数が最大の他の測角センサを移動する対象としてもよい。
【００５３】
上記した思想を制御入力に反映すると、図１０のケースでは、式（５）のようになる。
【００５４】
【数５】
2019074374.tif 000007
【００５５】
この式（５）において、ＮＵ（ｋ）は、拡大動作開始時点で観測数が最大であった測角センサＳＯＵの拡大動作中の観測数である。ｘｒｅｆは、観測数が最大であった測角センサの移動目標位置である。ｖは、測角センサＳＯ１の位置ｘ１から測角センサＳＯＵの位置ｘＵへ向かう方向ベクトルを示す。また、図１１のケースでは、制御入力は、式（６）となる。
【００５６】
【数６】
2019074374.tif 000008
【００５７】
拡大動作においては、図９に示すように、上記した縮小動作と異なり、拡大動作中に被覆制御は行わない。このような制御則を採用することにより、観測数が最大の測角センサはＳＯＵ，ＳＯＲは、目標位置ｘｒｅｆへ移動する。その最中に観測数が増大した場合には、式（５）、式（６）の拡大動作ゲインＫ（ＮＵ（ｋ）），Ｋ（ＮＲ（ｋ））が０となるため、拡大動作が停止する。
【００５８】
この拡大動作によれば、複数の測角センサの最大観測数を増大でき、これまで障害物などの死角によって観測できなかった新たな対象物を発見できる可能性がある。拡大動作の終了後、センサ移動制御部３１は、再び、縮小動作を実行することが好ましい。上記した縮小動作と拡大動作とを繰り返し実行することにより、最終的にはすべての対象物ＭＮをすべての測角センサＳＯ１〜ＳＯ４が観測できる配置へ該測角センサＳＯ１〜ＳＯ４を移動させることができ、対象物ＭＮの位置推定精度が向上する。
【００５９】
次に、上記した縮小動作及び拡大動作の別の実施形態について説明する。上記した実施形態では、縮小動作及び拡大動作を実行するためには、測角センサＳＯ１〜ＳＯ４が観測できている対象物の数が必要である。しかし、測角センサＳＯ１〜ＳＯ４の検出確率が１００％でなければ、観測する場所が同じであっても処理周期毎に観測数が異なることとなる。そのため、上記した式（４）において、ｍ番目の測角センサＳＯｍの時刻ｋにおける対象物の観測数を表すＮｍ（ｋ）や、式（５）において、拡大動作開始時点で観測数が最大であった測角センサＳＯＵの拡大動作中の観測数であるＮＵ（ｋ）などの計算に工夫が必要である。
【００６０】
このため、この実施形態では、対象物Ｍｎの位置を精度良く求めるため、観測数の瞬時値を一定時間記憶しておき、それらの平均観測数を採用する。すなわち、式（４）の場合には、瞬時値であるＮｍ（ｋ）ではなく、式（７）にて求められる平均値をｍ番目の測角センサの時刻ｋにおける観測数として採用している。同様に，式（５）の場合であれば、瞬時値であるＮＵ（ｋ）ではなく、式（８）にて求められる平均値を拡大動作開始時点で観測数が最大であった測角センサＳＯＵの拡大動作中の観測数として採用している。
【００６１】
【数７】
2019074374.tif 000009
【００６２】
【数８】
2019074374.tif 000010
【００６３】
この構成によれば、測角センサＳＯ１〜ＳＯ４の検出確率が１００％ではなくとも、測角センサＳＯ１〜ＳＯ４がそれぞれ観測した対象物の観測数を精度良く推定できる。このため、対象物の位置を精度良く推定することができる。
【００６４】
本発明の一実施形態を説明したが、本実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。本実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。本実施形態やその変形は、発明の範囲や要旨に含まれると同様に、特許請求の範囲に記載された発明とその均等の範囲に含まれるものである。
【符号の説明】
【００６５】
１００  位置標定システム
２０  演算処理部
３０  データ受信部（情報取得部）
３１  センサ移動制御部
３２  交差判定部
３３  位置推定部
３４  表示部
３５  制御部
５０，５１  障害物
Ｍ１，Ｍ２，Ｍ３，Ｍｎ  対象物
ＳＯ１，ＳＯ２，ＳＯ３，ＳＯ４，ＳＯｍ，ＳＯＲ，ＳＯＵ  測角センサ
【図１】
2019074374.tif 000011
【図２】
2019074374.tif 000012
【図３】
2019074374.tif 000013
【図４】
2019074374.tif 000014
【図５】
2019074374.tif 000015
【図６】
2019074374.tif 000016
【図７】
2019074374.tif 000017
【図８】
2019074374.tif 000018
【図９】
2019074374.tif 000019
【図１０】
2019074374.tif 000020
【図１１】
2019074374.tif 000021