JP2019074374A

JP2019074374A - 位置標定システム、および位置標定方法

Info

Publication number: JP2019074374A
Application number: JP2017199677A
Authority: JP
Inventors: 泰郎藤島; Yasuo Fujishima; 俊一東; Shunichi Azuma
Original assignee: Nagoya University NUC; Mitsubishi Heavy Industries Ltd
Current assignee: Nagoya University NUC; Mitsubishi Heavy Industries Ltd
Priority date: 2017-10-13
Filing date: 2017-10-13
Publication date: 2019-05-16
Anticipated expiration: 2037-10-13
Also published as: JP7012955B2

Abstract

【課題】簡単な構成及び処理で測角センサが対象物を観測可能な位置に移動可能とする位置標定システム、および位置標定方法を提供すること。【解決手段】位置標定システム１００は、互いに独立して移動可能に配置され、複数の対象物Ｍ１，Ｍ２に対する方位観測値をそれぞれ測定する測角センサＳＯ１〜ＳＯ４と、方位観測値に基づいて、対象物Ｍ１，Ｍ２の位置を推定する位置推定部３３と、方位観測値から測角センサＳＯ１〜ＳＯ４がそれぞれ観測した対象物Ｍ１，Ｍ２の観測数を含む情報を取得するデータ受信部３０と、取得した対象物Ｍ１，Ｍ２の観測数に基づいて、対応する測角センサＳＯ１〜ＳＯ４の位置を移動させるセンサ移動制御部３１とを備えた。【選択図】図１

Description

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

一般に、捜索対象や欠陥などの対象物（目標ともいう）の位置を推定（標定）する場合、対象物に対する方位線の方位角などを測定する測角センサを複数用いて、三角測量の原理に基づき、各測角センサから対象物に延びる方位線の交点から推定することできる。このような三角測量による位置を推定する技術では、複数の測角センサが対象物に対する方位角を常時測定する、すなわち各測角センサが対象物を常時観測することが要求される。

一方で、対象物と測角センサとの間に、例えば障害物が存在し、各測角センサが対象物を観測できない事態が生じることも想定される。このため、従来、センサを備える複数のロボット（移動体）が周囲環境を認識しながら動作する技術が提案されている（例えば、特許文献１参照）。この技術を上述した位置推定に適用することで、すべてのセンサが障害物を回避して、対象物を観測することも可能となる。

特開平０８−３０３２７号公報

しかし、特許文献１に記載の技術は、周囲環境を認識する環境認識機構と、周囲環境から自己運動や周囲環境を変化させる自己運動指令を生成する学習機構とを備るため、自己運動指令を生成するための処理が複雑となり、対象物を観測できるまでに多大な時間を要するといった問題がある。

本発明は、上記の事情に鑑みてなされたものであり、簡単な構成及び処理で測角センサが対象物を観測可能な位置に移動可能とする位置標定システム、および位置標定方法を提供することを目的とする。

上述した課題を解決し、目的を達成するために、本発明に係る位置標定システムは、互いに独立して移動可能に配置され、複数の対象物に対する方位線の方位角を含む方位観測値をそれぞれ測定する少なくとも３つの測角センサと、方位観測値に基づいて、対象物の位置を推定する位置推定部と、方位観測値から測角センサがそれぞれ観測した対象物の観測数を含む情報を取得する情報取得部と、取得した対象物の観測数に基づいて、対応する測角センサの位置を移動させるセンサ移動制御部と、を備えたことを特徴とする。

この構成によれば、方位観測値から測角センサがそれぞれ観測した対象物の観測数を含む情報を取得する情報取得部と、取得した対象物の観測数に基づいて、対応する測角センサの位置を移動させるセンサ移動制御部と、を備えるため、簡単な構成及び処理によって、測角センサが対象物を観測可能な位置に移動させることができる。従って、各測角センサが観測する対象物の観測数を高めることができ、対象物の位置を精度良く推定することができる。

この構成において、センサ移動制御部は、観測数が最大の測角センサを選択し、この観測数が最大の測角センサに接近する方向に、残りの測角センサを移動させる第１の動作を実行してもよい。

また、センサ移動制御部は、第１の動作に続いて、観測数が最大の測角センサを、残りの測角センサから離間する所定方向に移動させる第２の動作を実行してもよい。

また、センサ移動制御部は、残りの測角センサの中から１つの測角センサを選択し、この選択された測角センサが移動中に観測する対象物の観測数が、観測数が最大の測角センサの観測数と同じになるまで第１の動作を実行してもよい。

また、センサ移動制御部は、観測数が最大の測角センサが移動中に観測する対象物の観測数が増加する、もしくは、観測数が最大の測角センサの移動距離が予め設定した所定距離に至るまで第２の動作を実行してもよい。

また、観測数は、一定時間内に測角センサがそれぞれ対象物を観測した瞬時観測値の平均値であってもよい。

本発明に係る位置標定方法は、互いに独立して移動可能な測角センサを少なくとも３つ用いて、複数の対象物に対する方位線の方位角を含む方位観測値をそれぞれ測定するステップと、方位観測値から測角センサがそれぞれ観測した対象物の観測数を取得するステップと、対象物の観測数に基づいて、対応する測角センサの位置を移動させるステップと、測角センサの位置を移動させた後の方位観測値に基づいて、対象物の位置を推定するステップと、を備えたことを特徴とする。

この構成において、対応する測角センサの位置を移動させるステップでは、観測数が最大の測角センサを選択し、この観測数が最大の測角センサに接近する方向に、残りの測角センサを移動させる第１の動作を実行し、第１の動作に続いて、観測数が最大の測角センサを、残りの測角センサから離間する所定方向に移動させる第２の動作を実行してもよい。

本発明によれば、方位観測値から測角センサがそれぞれ観測した対象物の観測数を含む情報を取得する情報取得部と、取得した対象物の観測数に基づいて、対応する測角センサの位置を移動させるセンサ移動制御部と、を備えるため、簡単な構成及び処理によって、測角センサが対象物を観測可能な位置に移動させることができる。従って、各測角センサが観測する対象物の観測数を高めることができ、対象物の位置を精度良く推定することができる。

図１は、本実施形態に係る位置標定システムの概略構成を示すブロック図である。図２は、方位線の方向ベクトルを規定する方位角と天頂角とを説明するための図である。図３は、位置標定システムの手順の概略を示すフローチャートである。図４は、測角センサの縮小動作の手順を示すフローチャートである。図５は、縮小動作前の測角センサの配置状態の一例を示す概略図である。図６は、縮小動作後の測角センサの配置状態の一例を示す概略図である。図７は、測角センサの拡大動作の手順を示すフローチャートである。図８は、拡大動作前の測角センサの配置状態の一例を示す概略図である。図９は、拡大動作後の測角センサの配置状態の一例を示す概略図である。図１０は、観測数が最大の測角センサが複数の測角センサの端に位置した場合の拡大動作を説明するための図である。図１１は、観測数が最大の測角センサが複数の測角センサの内側に位置した場合の拡大動作を説明するための図である。

以下に、本発明に係る実施形態を図面に基づいて詳細に説明する。なお、この実施形態によりこの発明が限定されるものではない。また、実施形態における構成要素には、当業者が置換可能かつ容易なもの、あるいは実質的に同一のものが含まれる。さらに、以下に記載した構成要素は適宜組み合わせることが可能である。

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

位置標定システム１００は、図１に示すように、４つの測角センサＳ_Ｏ１〜Ｓ_Ｏ４と、これら測角センサＳ_Ｏ１〜Ｓ_Ｏ４の方位観測値に基づき演算処理を行う演算処理部２０とを備える。測角センサＳ_Ｏ１〜Ｓ_Ｏ４は、互いに独立して移動可能に配置された移動体（不図示）に搭載されている。測角センサＳ_Ｏ１〜Ｓ_Ｏ４及び移動体は、演算処理部２０と有線または無線により通信可能に接続されている。測角センサＳ_Ｏ１〜Ｓ_Ｏ４は、対象物Ｍ_１，Ｍ_２からの信号（音波、光波、電波など）を受信して、各対象物Ｍ_１，Ｍ_２に対する方位線ｌ_１１，ｌ_１２，・・・，ｌ_４１，ｌ_４２の方位観測値（例えば、方位角及び天頂角）を計測する。これらの方位観測値は、方位線ｌ_１１，ｌ_１２，・・・，ｌ_４１，ｌ_４２の方向ベクトルを規定する。測角センサとして、例えば、対象物Ｍ_１，Ｍ_２が発する音声信号を受信するマイクを用いることができる。また、測角センサとして、周囲を撮影するカメラを用いて、撮影した画像情報に対象物を示す所定形状が存在するか否かを画像処理などにて識別する構成としてもよい。

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

測角センサＳ_Ｏ１〜Ｓ_Ｏ４は、例えば、ＧＰＳ（Global Positioning System）信号に基づき、自己（移動体）の位置情報を取得可能な構成となっており、自己の位置情報及び姿勢情報と共に、計測した方位角θ_ｍｎ、天頂角φ_ｍｎ（方位観測値）を演算処理部２０に順次送信する。

演算処理部２０は、図１に示すように、データ受信部（情報取得部）３０、センサ移動制御部３１、交差判定部３２、位置推定部３３、表示部３４、および制御部３５とを備える。

データ受信部３０は、測角センサＳ_Ｏ１〜Ｓ_Ｏ４から順次送信される各測角センサＳ_Ｏ１〜Ｓ_Ｏ４の方位観測値（方位角θ_ｍｎ及び天頂角φ_ｍｎ）、位置情報及び姿勢情報を受信する。データ受信部３０は、例えば、各測角センサＳ_Ｏ１〜Ｓ_Ｏ４から送信された方位観測値により規定される方位線ｌ_ｍｎの本数から、各測角センサＳ_Ｏ１〜Ｓ_Ｏ４がそれぞれ観測できた対象物の観測数を取得する。センサ移動制御部３１は、各測角センサＳ_Ｏ１〜Ｓ_Ｏ４を搭載した移動体を動作させることにより、各測角センサＳ_Ｏ１〜Ｓ_Ｏ４を所望の位置に移動する制御を行う。本構成では、センサ移動制御部３１は、各測角センサＳ_Ｏ１〜Ｓ_Ｏ４が観測した対象物の観測数に基づいて、対応する測角センサＳ_Ｏ１〜Ｓ_Ｏ４（移動体）の移動する位置（移動先）を制御する。

交差判定部３２は、受信した方位観測値の情報に基づいて、すべての測角センサＳ_Ｏ１〜Ｓ_Ｏ４から対象物Ｍ_１（一の対象物）に向けてそれぞれ延びるすべての方位線ｌ_ｍｎ（例えば、ｌ_１１，ｌ_２１，ｌ_３１，ｌ_４１）が交差しているか否かを判定する。本実施形態では、交差するとは、観測できた対象物Ｍ_１（一の対象物）に向けて延びるすべての方位線ｌ_１１，ｌ_２１，ｌ_３１，ｌ_４１が１点で交差するだけでなく、各方位線が所定領域内に存在する（所定領域内を通過する）場合を含む。また、交差判定部３２は、すべての方位線ｌ_１１，ｌ_２１，ｌ_３１，ｌ_４１が所定領域内に存在しない（所定領域内を通過しない）場合には、交差していないと判定する。なお、交差判定部３２は、すべての測角センサＳ_Ｏ１〜Ｓ_Ｏ４から対象物Ｍ_２（一の対象物）に向けてそれぞれ延びるすべての方位線ｌ_１２，ｌ_２２，ｌ_３２，ｌ_４２が交差するか否かについても判定する。

位置推定部３３は、交差判定部３２が交差していると判断した点（交点）に基づき、これら交点をそれぞれ対象物Ｍ_１，Ｍ_２の位置と推定する。

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

図３は、位置標定システムの手順の概略を示すフローチャートである。複数の測角センサのＳ_Ｏ１〜Ｓ_Ｏ４を用いて、複数の対象物Ｍ_１，Ｍ_２の位置を標定する場合、各測角センサのＳ_Ｏ１〜Ｓ_Ｏ４が対象物Ｍ_１，Ｍ_２を観測することが重要である。すなわち、各測角センサのＳ_Ｏ１〜Ｓ_Ｏ４が対象物Ｍ_１，Ｍ_２を常時観測できれば、対象物Ｍ_１，Ｍ_２の位置をより正確に標定（推定）することができる。一方で、対象物Ｍ_１，Ｍ_２が移動したり、対象物Ｍ_１，Ｍ_２と測角センサのＳ_Ｏ１〜Ｓ_Ｏ４との間に、例えば障害物が存在する場合には、測角センサのＳ_Ｏ１〜Ｓ_Ｏ４が対象物Ｍ_１，Ｍ_２を観測できない事態が生じることも想定される。このため、本実施形態では、複数の測角センサＳ_Ｏ１〜Ｓ_Ｏ４の位置を観測数に基づいて移動することで、測角センサのＳ_Ｏ１〜Ｓ_Ｏ４の平均観測数を向上させ、対象物Ｍ_１，Ｍ_２の位置の推定精度の向上を図っている。

具体的には、図３に示すように、４つの測角センサＳ_Ｏ１〜Ｓ_Ｏ４が、それぞれ対象物Ｍ_１，Ｍ_２に対する方位観測値（少なくとも方位角θ_ｍｎ）を測定する（ステップＳＴ１）。測定された方位観測値は、各該測角センサＳ_Ｏ１〜Ｓ_Ｏ４の位置情報及び姿勢情報と共にデータ受信部３０に送信して集約される（ステップＳＴ２）。ここで、データ受信部３０は、例えば、各測角センサＳ_Ｏ１〜Ｓ_Ｏ４から送信された方位観測値により規定される方位線ｌ_ｍｎの本数から、各測角センサＳ_Ｏ１〜Ｓ_Ｏ４が観測できた対象物Ｍ_１，Ｍ_２の観測数を取得する。次に、取得した対象物Ｍ_１，Ｍ_２の観測数に基づき、対応する測角センサＳ_Ｏ１〜Ｓ_Ｏ４の位置を協調させて移動して（ステップＳＴ３）、測角センサＳ_Ｏ１〜Ｓ_Ｏ４の平均観測数を向上させる。そして、測角センサＳ_Ｏ１〜Ｓ_Ｏ４を移動させた後に再度測定された方位観測値に基づいて対象物Ｍ_１，Ｍ_２の位置標定を行う（ステップＳＴ４）。本実施形態では、対象物Ｍ_１，Ｍ_２を観測できた測角センサＳ_Ｏ１〜Ｓ_Ｏ４から延びるすべての方位線が、交差したと判別された場合に、位置推定部３３は、その交点を対象物Ｍ_１，Ｍ_２の位置と推定する。これらステップＳＴ１〜ステップＳＴ４の手順は周期的に実行されて対象物Ｍ_１，Ｍ_２の位置標定が行われる。

本構成では、上記ステップＳＴ３に示す測角センサＳ_Ｏ１〜Ｓ_Ｏ４の位置を対象物Ｍ_１，Ｍ_２の観測数に基づいて移動させる動作に特徴を有する。この動作は、測角センサＳ_Ｏ１〜Ｓ_Ｏ４の被覆領域（ベースライン）が縮小する縮小動作（第１の動作）と、被覆領域が拡大する拡大動作（第２の動作）とに大別されるため、まず、測角センサの縮小動作について詳細に説明する。図４は、測角センサの縮小動作の手順を示すフローチャートである。図５は、縮小動作前の測角センサの配置状態の一例を示す概略図であり、図６は、縮小動作後の測角センサの配置状態の一例を示す概略図である。

縮小動作が実行される前（時刻ｔ）において、測角センサＳ_Ｏ１〜Ｓ_Ｏ４は、図５に示すように、所定の被覆領域Ｑ（S₁(t),S₂(t),S₃(t),S₄(t)）内に分散して配置されている。この被覆領域Ｑは、４つの測角センサＳ_Ｏ１〜Ｓ_Ｏ４により、対象物Ｍ_１，Ｍ_２を観測するために設定された空間であり、その大きさは任意に設定される。図５の例では、測角センサＳ_Ｏ１，Ｓ_Ｏ２と対象物Ｍ_１，Ｍ_２との間に障害物５０が存在するため、測角センサＳ_Ｏ１が観測する対象物Ｍ_１，Ｍ_２の観測数は０、測角センサＳ_Ｏ２が観測する対象物Ｍ_１，Ｍ_２の観測数は１となる。これに対して、測角センサＳ_Ｏ３，Ｓ_Ｏ４が観測する対象物Ｍ_１，Ｍ_２の観測数はそれぞれ２となっている。

縮小動作を実行する場合、まず、センサ移動制御部３１は、対象物Ｍ_１，Ｍ_２の観測数が最大の測角センサを特定する（ステップＳＴ１１）。この図５の例では、測角センサＳ_Ｏ３，Ｓ_Ｏ４の観測数がそれぞれ２で最大となっているため、例えば、測角センサＳ_Ｏ４を観測数が最大の測角センサとして特定する。次に、センサ移動制御部３１は、残りの測角センサを観測数が最大の測角センサに接近する方向に移動させる（ステップＳＴ１２）。具体的には、測角センサＳ_Ｏ１〜Ｓ_Ｏ３を、測角センサＳ_Ｏ４に接近する方向にそれぞれ移動させる。ここで、多くの対象物を観測できている測角センサの位置近辺は、死角が少なく対象物を観測しやすいことが期待できる。このため、残りの測角センサＳ_Ｏ１〜Ｓ_Ｏ３を観測数が最大の測角センサＳ_Ｏ４に接近する方向に移動することにより、測角センサＳ_Ｏ１，Ｓ_Ｏ２の観測数の改善が期待できる。

一方、残りの測角センサＳ_Ｏ１〜Ｓ_Ｏ３を測角センサＳ_Ｏ４の位置まで移動させてしまうと、対象物Ｍ_１，Ｍ_２の観測数が改善するものの、対象物Ｍ_１，Ｍ_２の位置標定の精度は低下することとなる。このため、次に、センサ移動制御部３１は、残りの測角センサの中から１つの測角センサを選択する（ステップＳＴ１３）。選択される測角センサは、例えば、観測数が最大の測角センサから離れた位置にあるものが好ましく、図５の例では、測角センサＳ_Ｏ２が選択される。

次に、センサ移動制御部３１は、選択された測角センサＳ_Ｏ２が移動中に観測した対象物Ｍ_１，Ｍ_２の観測数が測角センサＳ_Ｏ４の観測数と同一になったか否かを判別する（ステップＳＴ１４）。具体的には、測角センサＳ_Ｏ２が縮小動作前の１から２に変更されたか否かを判別する。この判別において、測角センサＳ_Ｏ２の観測数が測角センサＳ_Ｏ４の観測数と同一にならない場合（ステップＳＴ１４；Ｎｏ）には、処理をステップＳＴ１３に戻し、他の測角センサを新たに選択する。また、測角センサＳ_Ｏ２の観測数が測角センサＳ_Ｏ４の観測数と同一となった場合（ステップＳＴ１４；Ｙｅｓ）には、この測角センサＳ_Ｏ２の移動を停止する（ステップＳＴ１５）。ここで、測角センサＳ_Ｏ２以外の測角センサＳ_Ｏ１，Ｓ_Ｏ３の位置を設定しないと、測角センサＳ_Ｏ１，Ｓ_Ｏ３が、観測数最大の測角センサＳ_Ｏ４に接近しすぎてしまい、対象物Ｍ_１，Ｍ_２の位置標定の精度が低下するおそれがある。このため、センサ移動制御部３１は、観測数最大の測角センサＳ_Ｏ４と選択された測角センサＳ_Ｏ２を除いた測角センサＳ_Ｏ１，Ｓ_Ｏ３（他の測角センサ）の位置を被覆制御により決定する（ステップＳＴ１６）。この場合、観測数最大の測角センサＳ_Ｏ４と選択された測角センサＳ_Ｏ２との位置によって、図６に示すように、縮小動作が実行後の時刻ｔ＋１に対応する新たな被覆領域Ｑ（S₁(t+1),S₂(t+1),S₃(t+1),S₄(t+1)）が設定される。そして、測角センサＳ_Ｏ１，Ｓ_Ｏ３の位置を、この新たな被覆領域内で例えば等間隔となるように分散して配置する。

続いて、センサ移動制御部３１は、すべての測角センサＳ_Ｏ１〜Ｓ_Ｏ４の観測数が同一もしくは、所定の閾値（例えば、最大観測数−１）以内となったか否かを判別する（ステップＳＴ１７）。この判別において、すべての測角センサＳ_Ｏ１〜Ｓ_Ｏ４の観測数が同一もしくは、所定の閾値以内となっていない場合（ステップＳＴ１７；Ｎｏ）には、処理をステップＳＴ１３に戻し、他の測角センサを新たに選択する。また、すべての測角センサＳ_Ｏ１〜Ｓ_Ｏ４の観測数が同一もしくは、所定の閾値以内となった場合（ステップＳＴ１７；Ｙｅｓ）には、測角センサＳ_Ｏ１，Ｓ_Ｏ２の観測数の改善が図られたため、処理を終了する。

次に、縮小動作の原理について説明する。縮小動作において、測角センサ群の総数をＵとし、各測角センサＳ_Ｏｍのダイナミクスを式（１）と規定する。

ここで、ｕ_ｍ（ｋ）は、ｍ番目の測角センサＳ_Ｏｍへの制御入力（例えばＸＹ平面上での速度指令値；２次元ベクトル）であり、ｋは、時間のインデクスを表す。本実施形態の縮小動作では、ステップＳＴ１６に示す被覆制御が同時に実行されるため、このｕ_ｍ（ｋ）を式（２）と記載する。この式（２）の右辺第１項は、被覆制御に係る項であり、第２項は、縮小動作に係る項である。また、ｋ_Ｃとｋ_Ｓは、それらの影響の度合いを規定するゲイン（スカラ）を表す。

被覆制御の制御入力は、例えば『東,永原,石井,林,桜間,畑中,マルチエージェントシステムの制御,2015』に記載の方法に従い、式（３）により求める。

ここで、Ｃ_ｍ（ｋ）は、ｍ番目の測角センサＳ_Ｏｍの位置ｘ_ｍ（ｋ）に対するボロノイ領域の重心を表す。本実施形態では、上記書籍に記載の公知技術とは異なり、縮小動作の影響により、被覆領域が時間変化する。このため、各測角センサＳ_Ｏｍの位置ｘ_ｍ（ｋ）に対するボロノイ領域の重心も同様に時間変化する。このため、本実施形態では、処理周期毎に重心を計算し直している。この制御入力により、上記した測角センサＳ_Ｏ１，Ｓ_Ｏ３は、それぞれボロノイ領域の重心へ移動して、例えば等間隔に測角センサＳ_Ｏ１〜Ｓ_Ｏ４が配置される。なお、等間隔配置は、一例であり、所定の被覆領域内に分散して配置されていればよい。

一方、縮小動作の制御入力の計算方法は公知ではない。このため、本実施形態では、式（４）のように計算している。

この式（４）において、ｘ_ｍＭＡＸは、観測数が最大の測角センサの位置を表す。また、Ｎ_ｍ（ｋ）は、ｍ番目の測角センサＳ_Ｏｍの時刻ｋにおける対象物の観測数を表す。このＮ_ｍ（ｋ）を入力として、各測角センサの縮小動作ゲインＫ（Ｎ_ｍ（ｋ））を、測角センサＳ_Ｏｍの観測数が、観測数最大の測角センサの観測数と同一となったか否かで判別する。同一になると、縮小動作ゲインＫ（Ｎ_ｍ（ｋ））が０となるため、縮小動作が停止する。この制御入力により、縮小動作開始時点で観測数が最大の測角センサの位置へ、残りの測角センサが集合する。この制御入力のみであれば、すべての測角センサが同じ位置で重なってしまう事態が生じるが、上記した被覆制御入力と、縮小動作ゲインＫ（Ｎ_ｍ（ｋ））によって、すべての測角センサが同じ位置に重なることなく、被覆領域に分散して配置される。本実施形態では、縮小動作の影響で被覆領域が変化する。そして、この変化する被覆領域の中で可能な限りベースラインを維持しつつ、各測角センサが共通して観測できる対象物の観測数（平均観測数）を増大させることができる。

上記した縮小動作により、理想的にはすべての測角センサＳ_Ｏ１〜Ｓ_Ｏ４の観測数が同一（一致）することができる。一方、対象物の総数は不明であるため、縮小動作によって、すべての測角センサＳ_Ｏ１〜Ｓ_Ｏ４間で一致させた観測数が、対象物の総数と一致するとは限らない。このため、縮小動作に続いて拡大動作を実行する。次に、拡大動作について説明する。図７は、測角センサの拡大動作の手順を示すフローチャートである。図８は、拡大動作前の測角センサの配置状態の一例を示す概略図である。図９は、拡大動作後の測角センサの配置状態の一例を示す概略図である。

拡大動作は、図８に示すように、縮小動作後の時刻ｔ＋１に対応する被覆領域Ｑ（S₁(t+1),S₂(t+1),S₃(t+1),S₄(t+1)）対して実行される。この場合、観測空間には、障害物５１により、すべての測角センサＳ_Ｏ１〜Ｓ_Ｏ４が観測できていない対象物Ｍ_３が存在しているものとする。

拡大動作を実行する場合、図７に示すように、センサ移動制御部３１は、観測数が最大の測角センサを残りの測角センサから離間する所定方向に移動させる（ステップＳＴ２１）。図８の例では、測角センサＳ_Ｏ４を各測角センサの並んだ方向に沿って、残りの測角センサＳ_Ｏ１〜Ｓ_Ｏ３から離間するように移動させる。次に、測角センサが移動中に観測した対象物の観測数が増加したか否かを判別する（ステップＳＴ２２）。この判別において、対象物の観測数が増加した場合（ステップＳＴ２２；Ｙｅｓ）には、図９に示すように、新たな対象物Ｍ_３を観測することができたため、測角センサＳ_Ｏ４をその場所で停止して拡大動作の処理を終了する。

一方、対象物の観測数が増加しない場合（ステップＳＴ２２；Ｎｏ）には、センサ移動制御部３１は、測角センサＳ_Ｏ４が予め定めた所定距離に至るまで移動したか否かを判別する（ステップＳＴ２３）。ここで所定距離とは、移動を許容する限界距離として任意に設定する値であり、観測空間の大きさ等によって、例えば１０ｍ〜１０００ｍの間で変更可能である。この判別において、所定距離に至るまで移動していない（ステップＳＴ２３；Ｎｏ）場合には、処理をステップＳＴ２２に戻し、これらステップＳＴ２２，２３を繰り返し実行する。また、所定距離に至るまで移動した（ステップＳＴ２３；Ｙｅｓ）場合には、上記した所定方向に移動させても対象物の観測数が増加しない可能性が高い。このため、センサ移動制御部３１は、測角センサＳ_Ｏ４を一旦、元の位置に戻した後、残りの測角センサＳ_Ｏ１〜Ｓ_Ｏ３から離間する別の所定方向に移動させる（ステップＳＴ２４）。図９の例では、測角センサＳ_Ｏ４を各測角センサＳ_Ｏ１〜Ｓ_Ｏ４の並んだ方向に沿って、残りの測角センサＳ_Ｏ１〜Ｓ_Ｏ３から離間するように移動させたが、今度は、例えば、各測角センサＳ_Ｏ１〜Ｓ_Ｏ４の並んだ方向に直交（交差）する方向に沿って、残りの測角センサＳ_Ｏ１〜Ｓ_Ｏ３から離間するように移動させる。これにより、新たな対象物Ｍ_３を観測できる可能性が生じる。

次に、測角センサが移動中に観測した対象物の観測数が増加したか否かを判別する（ステップＳＴ２５）。この判別において、対象物の観測数が増加した場合（ステップＳＴ２５；Ｙｅｓ）には、新たな対象物Ｍ_３を観測することができたため、測角センサＳ_Ｏ４をその場所で停止して拡大動作の処理を終了する。

一方、対象物の観測数が増加しない場合（ステップＳＴ２５；Ｎｏ）には、センサ移動制御部３１は、測角センサＳ_Ｏ４が予め定めた所定距離に至るまで移動したか否かを判別する（ステップＳＴ２６）。この判別において、所定距離に至るまで移動していない（ステップＳＴ２６；Ｎｏ）場合には、処理をステップＳＴ２５に戻し、これらステップＳＴ２５，２６を繰り返し実行する。また、所定距離に至るまで移動した（ステップＳＴ２６；Ｙｅｓ）場合には、上記した所定方向に移動させても対象物の観測数が増加しない可能性が高い。このため、センサ移動制御部３１は、測角センサＳ_Ｏ４を元の位置に戻し（ステップＳ２７）、処理を終了する。

次に、拡大動作の原理について説明する。図１０は、観測数が最大の測角センサが複数の測角センサの端に位置した場合の拡大動作を説明するための図である。図１１は、観測数が最大の測角センサが複数の測角センサの内側に位置した場合の拡大動作を説明するための図である。この拡大動作では、縮小動作と異なり、観測数が最大の測角センサのみが移動する。この場合、観測数が最大の測角センサが複数の測角センサ（センサ群）の端に位置するか否かで該測角センサの移動方向が異なる。図１０に示すように、観測数が最大の測角センサＳ_Ｏ４（Ｓ_ＯＵ）が複数の測角センサの端であった場合には、より遠ざかることで観測数が更に増大することを期待し、各測角センサの並んだ線αに沿って、かつ、残りの測角センサＳ_Ｏ１〜Ｓ_Ｏ３から離間する方向（図中矢印α１方向）に移動する。一方、図１１に示すように、観測数が最大の測角センサＳ_Ｏ３（Ｓ_ＯＲ）が複数の測角センサの端ではなかった場合、各測角センサの並んだ線αと概ね直交（交差）する方向（図中矢印β１、β２方向）へ移動する。いずれの場合も、移動中に観測数が増大した場合は、上述したように、測角センサをその場所で停止する。また、所定距離Ｌだけ移動しても観測数が元の場所から増大しなかった場合は、上述のように、元の位置へ戻して別の方向へ移動するか、その時点で処理を終了する。また、観測数が最大の測角センサが複数ある場合には、観測数が最大の他の測角センサを移動する対象としてもよい。

上記した思想を制御入力に反映すると、図１０のケースでは、式（５）のようになる。

この式（５）において、Ｎ_Ｕ（ｋ）は、拡大動作開始時点で観測数が最大であった測角センサＳ_ＯＵの拡大動作中の観測数である。ｘ_ｒｅｆは、観測数が最大であった測角センサの移動目標位置である。ｖは、測角センサＳ_Ｏ１の位置ｘ_１から測角センサＳ_ＯＵの位置ｘ_Ｕへ向かう方向ベクトルを示す。また、図１１のケースでは、制御入力は、式（６）となる。

拡大動作においては、図９に示すように、上記した縮小動作と異なり、拡大動作中に被覆制御は行わない。このような制御則を採用することにより、観測数が最大の測角センサはＳ_ＯＵ，Ｓ_ＯＲは、目標位置ｘ_ｒｅｆへ移動する。その最中に観測数が増大した場合には、式（５）、式（６）の拡大動作ゲインＫ（Ｎ_Ｕ（ｋ）），Ｋ（Ｎ_Ｒ（ｋ））が０となるため、拡大動作が停止する。

この拡大動作によれば、複数の測角センサの最大観測数を増大でき、これまで障害物などの死角によって観測できなかった新たな対象物を発見できる可能性がある。拡大動作の終了後、センサ移動制御部３１は、再び、縮小動作を実行することが好ましい。上記した縮小動作と拡大動作とを繰り返し実行することにより、最終的にはすべての対象物Ｍ_Ｎをすべての測角センサＳ_Ｏ１〜Ｓ_Ｏ４が観測できる配置へ該測角センサＳ_Ｏ１〜Ｓ_Ｏ４を移動させることができ、対象物Ｍ_Ｎの位置推定精度が向上する。

次に、上記した縮小動作及び拡大動作の別の実施形態について説明する。上記した実施形態では、縮小動作及び拡大動作を実行するためには、測角センサＳ_Ｏ１〜Ｓ_Ｏ４が観測できている対象物の数が必要である。しかし、測角センサＳ_Ｏ１〜Ｓ_Ｏ４の検出確率が１００％でなければ、観測する場所が同じであっても処理周期毎に観測数が異なることとなる。そのため、上記した式（４）において、ｍ番目の測角センサＳ_Ｏｍの時刻ｋにおける対象物の観測数を表すＮ_ｍ（ｋ）や、式（５）において、拡大動作開始時点で観測数が最大であった測角センサＳ_ＯＵの拡大動作中の観測数であるＮ_Ｕ（ｋ）などの計算に工夫が必要である。

このため、この実施形態では、対象物Ｍ_ｎの位置を精度良く求めるため、観測数の瞬時値を一定時間記憶しておき、それらの平均観測数を採用する。すなわち、式（４）の場合には、瞬時値であるＮ_ｍ（ｋ）ではなく、式（７）にて求められる平均値をｍ番目の測角センサの時刻ｋにおける観測数として採用している。同様に，式（５）の場合であれば、瞬時値であるＮ_Ｕ（ｋ）ではなく、式（８）にて求められる平均値を拡大動作開始時点で観測数が最大であった測角センサＳ_ＯＵの拡大動作中の観測数として採用している。

この構成によれば、測角センサＳ_Ｏ１〜Ｓ_Ｏ４の検出確率が１００％ではなくとも、測角センサＳ_Ｏ１〜Ｓ_Ｏ４がそれぞれ観測した対象物の観測数を精度良く推定できる。このため、対象物の位置を精度良く推定することができる。

本発明の一実施形態を説明したが、本実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。本実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。本実施形態やその変形は、発明の範囲や要旨に含まれると同様に、特許請求の範囲に記載された発明とその均等の範囲に含まれるものである。

１００位置標定システム
２０演算処理部
３０データ受信部（情報取得部）
３１センサ移動制御部
３２交差判定部
３３位置推定部
３４表示部
３５制御部
５０，５１障害物
Ｍ_１，Ｍ_２，Ｍ_３，Ｍ_ｎ対象物
Ｓ_Ｏ１，Ｓ_Ｏ２，Ｓ_Ｏ３，Ｓ_Ｏ４，Ｓ_Ｏｍ，Ｓ_ＯＲ，Ｓ_ＯＵ測角センサ

Claims

互いに独立して移動可能に配置され、複数の対象物に対する方位線の方位角を含む方位観測値をそれぞれ測定する少なくとも３つの測角センサと、前記方位観測値に基づいて、前記対象物の位置を推定する位置推定部と、
前記方位観測値から前記測角センサがそれぞれ観測した前記対象物の観測数を含む情報を取得する情報取得部と、
取得した前記対象物の観測数に基づいて、対応する前記測角センサの位置を移動させるセンサ移動制御部と、を備えたことを特徴とする位置標定システム。
前記センサ移動制御部は、観測数が最大の測角センサを選択し、この観測数が最大の測角センサに接近する方向に、残りの測角センサを移動させる第１の動作を実行することを特徴とする請求項１に記載の位置標定システム。
前記センサ移動制御部は、前記第１の動作に続いて、前記観測数が最大の測角センサを、前記残りの測角センサから離間する所定方向に移動させる第２の動作を実行することを特徴とする請求項２に記載の位置標定システム。
前記センサ移動制御部は、前記残りの測角センサの中から１つの測角センサを選択し、この選択された測角センサが移動中に観測する前記対象物の観測数が、前記観測数が最大の測角センサの観測数と同じになるまで前記第１の動作を実行することを特徴とする請求項２または３に記載の位置標定システム。
前記センサ移動制御部は、前記観測数が最大の測角センサが移動中に観測する前記対象物の観測数が増加する、もしくは、前記観測数が最大の測角センサの移動距離が予め設定した所定距離に至るまで前記第２の動作を実行することを特徴とする請求項３に記載の位置標定システム。
前記観測数は、一定時間内に前記測角センサがそれぞれ前記対象物を観測した瞬時観測値の平均値であることを特徴とする請求項１から５のいずれか一項に記載の位置標定システム。
互いに独立して移動可能な測角センサを少なくとも３つ用いて、複数の対象物に対する方位線の方位角を含む方位観測値をそれぞれ測定するステップと、
前記方位観測値から前記測角センサがそれぞれ観測した前記対象物の観測数を取得するステップと、
前記対象物の観測数に基づいて、対応する前記測角センサの位置を移動させるステップと、
前記測角センサの位置を移動させた後の前記方位観測値に基づいて、前記対象物の位置を推定するステップと、を備えたことを特徴とする位置標定方法。
対応する前記測角センサの位置を移動させるステップでは、観測数が最大の測角センサを選択し、この観測数が最大の測角センサに接近する方向に、残りの測角センサを移動させる第１の動作を実行し、前記第１の動作に続いて、前記観測数が最大の測角センサを、前記残りの測角センサから離間する所定方向に移動させる第２の動作を実行することを特徴とする請求項７に記載の位置標定方法。