(57)【要約】【課題】位置同定するデバイス（例えば移動ロボット）の数が増えても、計算負荷及び通信負荷の増加を大幅に抑制することができるデバイスの状態同定方法と装置を提供する。【解決手段】デバイスＡ、Ｂ、Ｃは、他のデバイスの方向又は距離を観測する外界センサ１６と、それ自身の状態量を推定し管理する状態推定部３０とを有する。各状態推定部３０により、（Ｓ１）自身の状態量とその分散及び共分散とを管理し、（Ｓ２）他のデバイスの状態量と自身の状態量との相関が高い相関情報に限定して管理し、（Ｓ５）他のデバイスとの間で通信し、外界センサ１６による観測値が得られた観測時刻における自身及び他のデバイスの状態量、その分散及び共分散、及び両者間の共分散を予測し、（Ｓ６）予測された値と外界センサ１６の観測値から自身及び他のデバイスの状態量、その分散及び共分散、及び両者間の共分散を修正し、（Ｓ７）修正した結果を自身及び他のデバイスで共有する。【選択図】図４
2014066679.tif 000002
【特許請求の範囲】
【請求項１】
位置、姿勢、速度、角速度又は加速度を状態量とする複数のデバイスを備え、
各デバイスは、他のデバイスの方向又は距離を観測する外界センサと、それ自身の状態量を推定し管理する状態推定部とを有し、各状態推定部により、
（Ａ）自身の状態量とその分散及び共分散とを管理し、
（Ｂ）他のデバイスの状態量と自身の状態量との相関が高い相関情報に限定して管理し、
（Ｃ１）他のデバイスとの間で通信し、外界センサによる観測値が得られた観測時刻における自身及び他のデバイスの状態量、その分散及び共分散、及び両者間の共分散を予測し、
（Ｃ２）予測された値と外界センサの観測値から自身及び他のデバイスの状態量、その分散及び共分散、及び両者間の共分散を修正し、
（Ｃ３）修正した結果を自身及び他のデバイスで共有する、ことを特徴とするデバイスの状態同定方法。
【請求項２】
前記デバイスは、移動可能な移動デバイスと、固定又は可搬可能な固定デバイスとからなり、
移動デバイス又は固定デバイスは、それ自身の位置、速度又は加速度を観測する内界センサを有し、各状態推定部により、
（Ｄ１）内界センサによる観測値が得られた観測時刻における自身の内部状態と分散及び共分散を予測し、
（Ｄ２）予測した予測値と内界センサの観測値から自身の状態量とその分散及び共分散を修正する、ことを特徴とする請求項１に記載のデバイスの状態同定方法。
【請求項３】
前記相関が高い相関情報は、相関係数の絶対値の高い複数の相関情報である、ことを特徴とする請求項１に記載のデバイスの状態同定方法。
【請求項４】
各状態推定部は、相関情報を記憶する複数のメモリを有しており、
前記相関情報の相関係数を絶対値の高い順位の順で前記メモリに記憶し、
前記（Ｂ）において、前記順位の高い１又は複数のメモリ内の相関情報のみを管理する、ことを特徴とする請求項３に記載のデバイスの状態同定方法。
【請求項５】
前記相関が高い相関情報は、
（Ａ）外界センサによって観測したことのある他のデバイスとの相関情報、又は、
（Ｂ）外界センサによって自身を観測したことのある他のデバイスとの相関情報を含む、ことを特徴とする請求項１に記載のデバイスの状態同定方法。
【請求項６】
前記相関が高い相関情報は、
（Ａ）絶対値の小さい共分散、
（Ｂ）一定時間、外界センサ１６によって計測していないデバイスのうち、相手の外界センサ１６によって自身が計測されていないデバイスとの共分散、
（Ｃ）自身の内部状態を修正した時、同時に修正しなかったデバイスとの共分散、又は、
（Ｄ）通信不能、或いはアドホック通信の場合に通信ホップ数が予め設定した数より多いデバイスとの共分散、を除外する、ことを特徴とする請求項１に記載のデバイスの状態同定方法。
【請求項７】
位置、姿勢、速度、角速度又は加速度を状態量とする複数のデバイスを備え、
各デバイスは、他のデバイスの方向又は距離を観測する外界センサと、それ自身の状態量を推定し管理する状態推定部とを有し、各状態推定部により、
（Ａ）自身の状態量とその分散及び共分散とを管理し、
（Ｂ）他のデバイスの状態量と自身の状態量との相関が高い相関情報に限定して管理し、
（Ｃ１）外界センサによる観測値に基づき、他のデバイスとの間で通信し、観測時刻における自身及び他のデバイスの状態量、その分散及び共分散、及び両者間の共分散を予測し、
（Ｃ２）予測された値と外界センサの観測値から自身及び他のデバイスの状態量、その分散及び共分散、及び両者間の共分散を修正し、
（Ｃ３）修正した結果を自身及び他のデバイスで共有する、ことを特徴とするデバイスの状態同定装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、複数のデバイス（例えば移動ロボット）の状態を同定するデバイスの状態同定方法と装置に関する。
【背景技術】
【０００２】
複数の移動デバイス（例えば移動ロボット）を制御したり、人を誘導したりするために、移動デバイスや目標物の位置や姿勢などの状態を同定する技術が知られている。一般的な位置同定手段では、以下のように、移動デバイスの位置や姿勢に関する様々な情報を計測する。
【０００３】
手段Ａ：移動デバイス上に設置したＧＰＳ、エンコーダ、ジャイロセンサによって、移動デバイスの位置、速度、加速度などを計測する。
手段Ｂ：移動デバイス上の外界センサ（カメラやレーザセンサなど）によって、周囲の物体の方向や距離を計測する。或いは、移動デバイスの外部にある外界センサによって移動デバイスの方向や距離を計測する。
これらの情報を、ベイズフィルタ（カルマンフィルタ、パーティクルフィルタなど）を用いて統合し移動デバイスの位置や姿勢を同定する。
【０００４】
また、手段Ｂは、計測側から見た計測対象の方向や距離といった相対関係を計測する。そのため、手段Ｂにおいて移動デバイスの位置を同定するには、周囲の物体の位置や、外部センサの位置や姿勢といった情報も必要となる。そこで、ベイズフィルタによって、これらの情報も含めて同定する同定手段１〜３がある。
【０００５】
同定手段１は、移動デバイス上の外界センサを使って、周囲の物体（抽出したランドマーク、マーカーなど）の位置と、移動デバイスの位置を同時に推定するＳＬＡＭ技術であり、例えば特許文献１に開示されている。
同定手段２は、同定手段１のＳＬＡＭ技術を複数の移動デバイスで同時に行い、ランドマークの位置情報（マップ情報）を移動デバイス間で共有するものである。
同定手段３は、複数の移動デバイスが、互いに相対関係を計測し合い、双方の位置や姿勢を推定するものである。
【０００６】
ここで、同定手段２や同定手段３では、エリア内に複数の移動センサや固定センサを配置する。このように、分散配置されたセンサの情報を、ベイズフィルタを用いて統合する場合、以下のような統合手段１〜３が考えられる。
【０００７】
統合手段１は、各センサの情報を、１ヶ所の処理装置に集約して処理するものである。
統合手段２は、各センサがベイズフィルタによって処理し、処理した結果（推定した確率分布）を上位の統合ノードに送信する。統合ノードでは、確率分布の重ね合わせによって推定結果を統合するものである。この統合手段は、例えば特許文献２に開示されている。
統合手段３は、各センサ上で、情報型のカルマンフィルタ（情報フィルタ）による処理を行い、処理の際に算出した修正量を周囲のデバイス間で通信して統合するものである。この統合手段における情報フィルタは、ベイズフィルタにおける予測処理、修正処理のうち、修正処理が簡易になる手段である。すなわち各センサ上で算出した修正量を加算していくだけで、修正処理できる特徴がある。この統合手段は、例えば非特許文献１に開示されている。
【先行技術文献】
【特許文献】
【０００８】
【特許文献１】特開２００８−３０４２６８号公報
【特許文献２】特開２０１１−１９７９６４号公報
【非特許文献】
【０００９】
【非特許文献１】Ｈ．  Ｄｕｒｒａｎｔ−Ｗｈｙｔｅ、Ｍ．  Ｓｔｅｖｅｎｓ．“Ｄａｔａ  Ｆｕｓｉｏｎ  ｉｎ  Ｄｅｃｅｎｔｒａｌｉｚｅｄ  Ｓｅｎｓｉｎｇ  Ｎｅｔｗｏｒｋｓ”  Ｐｒｏｃｅｅｄｉｎｇｓ  ｏｆ  ｔｈｅ  Ｉｎｔｅｒｎａｔｉｏｎａｌ  Ｃｏｎｆｅｒｅｎｃｅ  ｏｎ  Ｉｎｆｏｒｍａｔｉｏｎ  Ｆｕｓｉｏｎ、２００１、ｐｐ．ＷｅＡ３−１９−２４．
【発明の概要】
【発明が解決しようとする課題】
【００１０】
統合手段１のように、センサの情報を１ヶ所に集約して統合処理する場合、以下のような問題がある。
（１）位置同定する移動デバイスの数が増えると、ベイズフィルタ処理における、１回あたりの観測更新の計算量が増加する。
（２）移動デバイスの数が増えると、統合する観測値が増える。そのため、観測更新の回数が増えて、計算負荷が高くなる。
（３）移動デバイスの数が増えると、計測結果を集約するための通信負荷が大きくなる。特に無線通信の場合、電波が直接届かないデバイス間で通信するには、中継機による転送処理（アドホック通信）が必要となり、通信負荷がさらに増える。
【００１１】
統合手段２では、各センサがベイズフィルタ処理をする分、統合ノードにおける計算量は軽減される。しかし、（２）に挙げた観測更新の回数については、解決されない。また、特許文献２では、統合ノードに情報を集約する通信が必要となる。従って、センサ数が多い場合や、センサと統合ノード間の通信に転送処理が必要な場合は、（３）の通信負荷が問題となる可能性がある。
【００１２】
統合手段３では、各センサ上でベイズフィルタ処理し、付近のデバイスとの通信（転送処理の要らない通信）のみで情報を統合する。しかし、情報フィルタの場合、予測処理が複雑となる。同定手段２のように、マップ情報のみ共有する場合は、各センサが予測するのは自身の状態（位置、速度など）のみで良い。一方、同定手段３では、他のセンサの状態も予測する必要がある。従って、移動デバイスが増えるにつれて、（１）に挙げた計算量が増加する。
【００１３】
以上のように、同定手段３のような事例に対しては、全センサの状態を同時に予測、修正するようなベイズフィルタの枠組みしかなく、計算量や通信量の点で、デバイス（例えば移動ロボット）の増加に対応できない問題点があった。
【００１４】
本発明は上述した問題点を解決するために創案されたものである。すなわち本発明の目的は、位置同定するデバイス（例えば移動ロボット）の数が増えても、計算負荷及び通信負荷の増加を大幅に抑制することができるデバイスの状態同定方法と装置を提供することにある。
【課題を解決するための手段】
【００１５】
本発明によれば、位置、姿勢、速度、角速度又は加速度を状態量とする複数のデバイスを備え、
各デバイスは、他のデバイスの方向又は距離を観測する外界センサと、それ自身の状態量を推定し管理する状態推定部とを有し、各状態推定部により、
（Ａ）自身の状態量とその分散及び共分散とを管理し、
（Ｂ）他のデバイスの状態量と自身の状態量との相関が高い相関情報に限定して管理し、
（Ｃ１）外界センサによる観測値に基づき、他のデバイスとの間で通信し、観測時刻における自身及び他のデバイスの状態量、その分散及び共分散、及び両者間の共分散を予測し、
（Ｃ２）予測された値と外界センサの観測値から自身及び他のデバイスの状態量、その分散及び共分散、及び両者間の共分散を修正し、
（Ｃ３）修正した結果を自身及び他のデバイスで共有する、ことを特徴とするデバイスの状態同定方法が提供される。
【００１６】
本発明の実施形態によれば、前記デバイスは、移動可能な移動デバイスと、固定又は可搬可能な固定デバイスとからなり、
移動デバイス又は固定デバイスは、それ自身の位置、速度又は加速度を観測する内界センサを有し、各状態推定部により、
（Ｄ１）内界センサによる観測値が得られた観測時刻における自身の内部状態と分散及び共分散を予測し、
（Ｄ２）予測した予測値と内界センサの観測値から自身の状態量とその分散及び共分散を修正する。
【００１７】
また、前記相関が高い相関情報は、相関係数の絶対値の高い複数の相関情報である。
【００１８】
また、各状態推定部は、相関情報を記憶する複数のメモリを有しており、
前記相関情報の相関係数を絶対値の高い順位の順で前記メモリに記憶し、
前記（Ｂ）において、前記順位の高い１又は複数のメモリ内の相関情報のみを管理する。
【００１９】
また、前記相関が高い相関情報は、
（Ａ）外界センサによって観測したことのある他のデバイスとの相関情報、又は、
（Ｂ）外界センサによって自身を観測したことのある他のデバイスとの相関情報を含む。
【００２０】
また、前記相関が高い相関情報は、
（Ａ）絶対値の小さい共分散、
（Ｂ）一定時間、外界センサ１６によって計測していないデバイスのうち、相手の外界センサ１６によって自身が計測されていないデバイスとの共分散、
（Ｃ）自身の内部状態を修正した時、同時に修正しなかったデバイスとの共分散、又は、
（Ｄ）通信不能、或いはアドホック通信の場合に通信ホップ数が予め設定した数より多いデバイスとの共分散、を除外する。
【００２１】
また本発明によれば、位置、姿勢、速度、角速度又は加速度を状態量とする複数のデバイスを備え、
各デバイスは、他のデバイスの方向又は距離を観測する外界センサと、それ自身の状態量を推定し管理する状態推定部とを有し、各状態推定部により、
（Ａ）自身の状態量とその分散及び共分散とを管理し、
（Ｂ）他のデバイスの状態量と自身の状態量との相関が高い相関情報に限定して管理し、
（Ｃ１）外界センサによる観測値に基づき、他のデバイスとの間で通信し、観測時刻における自身及び他のデバイスの状態量、その分散及び共分散、及び両者間の共分散を予測し、
（Ｃ２）予測された値と外界センサの観測値から自身及び他のデバイスの状態量、その分散及び共分散、及び両者間の共分散を修正し、
（Ｃ３）修正した結果を自身及び他のデバイスで共有する、ことを特徴とするデバイスの状態同定装置が提供される。
【発明の効果】
【００２２】
上記本発明の方法と装置によれば、（Ｃ１）（Ｃ２）の更新処理の際、観測に関与した（自身及び他の）デバイスの状態のみ予測し修正するため、デバイス（例えば移動ロボット）の総数が増えても、計算量が増加しない。
【００２３】
また外界センサで計測したデバイスがそれぞれ更新処理するため、システム全体で観測値が得られる頻度が増えても、各デバイスが更新処理する頻度は増加しない。
【００２４】
また（Ｃ１）において観測に関与した（自身及び他の）デバイスとの間でのみ通信するため、通信負荷が１ヶ所に集中しない。また、外界センサで観測したデバイスと、観測されたデバイスとの間は、物理的な距離が近く、間に遮蔽物がない状況にある。そのため、両者は直接電波が届く可能性が高く、転送処理の要らない通信になりやすい。従って、アドホック通信の場合は、通信負荷が少ない。
【００２５】
また各デバイスは、自身の内部状態推定結果を、通信を介することなく参照できる。例えば、デバイスとして移動ロボットの制御に用いる場合、通信遅れのない最新の推定結果を使って制御できるため、制御の性能が上がる。
【００２６】
また各デバイスが個別に予測処理するため、他のデバイスの運動モデルや内部状態を把握する必要がない。従って、それぞれのデバイスごとに、運動モデルや、推定したい状態量を自由に設定できる。
【図面の簡単な説明】
【００２７】
【図１】本発明の使用例を示す図である。
【図２】移動デバイスの構成例を示す図である。
【図３】固定デバイスの構成例を示す図である。
【図４】状態推定部による状態同定方法を示すフロー図である。
【発明を実施するための形態】
【００２８】
本発明の好ましい実施形態を図面に基づいて説明する。なお、各図において共通する部分には同一の符号を付し、重複した説明を省略する。
【００２９】
図１は、本発明の使用例を示す図である。
この図に示すように、本発明の状態同定装置は、位置、姿勢、速度、又は加速度を状態量とする複数のデバイスＡ、Ｂ、Ｃを備える。
【００３０】
この例では、デバイスＡ、Ｂは、予め設定したエリア内で移動可能なデバイス（例えば移動ロボット）である。また、デバイスＣは、予め設定したエリア内に固定又は可搬可能に位置するデバイス（例えば１以上の固定デバイス）である。
以下、デバイスの区別が必要な場合、移動可能なデバイスを「移動デバイス」、固定又は可搬可能なデバイスを「固定デバイス」と呼ぶ。
【００３１】
本発明の状態同定装置は、これらのデバイスＡ、Ｂ、Ｃの状態量（位置、姿勢、速度、又は加速度）を求める。得られた状態量の推定結果は、上位の制御装置１を介して、デバイスＡ、Ｂ、Ｃの制御や、エリアの地図作成、監視などに用いる。
なお、上位の制御装置１と各デバイスＡ、Ｂ、Ｃとの通信は、後述する内界センサ１０及び外界センサ１６の計測周期より長く、通信負荷の小さい頻度（例えば１〜１０分間隔）で実施するのがよい。
【００３２】
図２は、移動デバイスＡ、Ｂの構成例を示す図である。
この図において、各移動デバイスＡ、Ｂ（例えば移動ロボット）は、内界センサ１０及び外界センサ１６を備える。
【００３３】
内界センサ１０は、この例では移動デバイスＡ、Ｂの角速度又は加速度を計測するジャイロセンサ１１、移動デバイスＡ、Ｂの位置と姿勢を計測するＧＰＳコンパス１２、各移動デバイスＡ、Ｂに設けられた車輪の回転量を計測する車輪用エンコーダ１３、及び各移動デバイスＡ、Ｂに設けられた車輪のステア角計測用のステア角エンコーダ１４からなり、それ自身（すなわち移動デバイスＡ、Ｂ）の位置、速度又は加速度などを観測する。
【００３４】
外界センサ１６は、この例ではカメラ１７及び周囲の形状を計測するレーザセンサ１８からなり、他のデバイスＡ、Ｂ、又はＣの方向又は距離を観測する。
【００３５】
図２において、各移動デバイスＡ、Ｂはさらに、移動のためのロボット駆動部２１、ロボット駆動部２１を制御する車両制御部２２、各移動デバイスＡ、Ｂの目標軌道を算出する行動計画部２３、内界センサ１０及び外界センサ１６のセンサ処理を行う計測部２４を備える。
【００３６】
行動計画部２３は、一定の制御周期で動作するリアルタイム処理系であり、ロボット上予測部２３ａを使って、各移動デバイスＡ、Ｂの現在又は未来の位置や姿勢を予測し、軌道生成部２３ｂによる各移動デバイスＡ、Ｂの目標軌道の算出に用いる。また、衝突回避の場合など、必要に応じて、各移動デバイスＡ、Ｂの現在又は未来の位置や姿勢を予測する。
【００３７】
ロボット上予測部２３ａは、状態推定部３０（後述する）に記録されている内部状態Ｘのうち、デバイス制御に関連する状態量を、状態遷移方程式ｆ（Ｘ）を使って、指定された時刻まで遷移させる計算を行う。ロボット上予測部２３ａは、リアルタイム制御の時間制約下で予測処理できるように設計される。
【００３８】
車両制御部２２は、一定の制御周期で動作するリアルタイム処理系であり、移動デバイスＡ、Ｂが目標軌道に沿って動くように、指令値算出部２２ａで指令値を算出し、ロボット駆動部２１を制御する。
【００３９】
通信部２５は、他のデバイスＡ、Ｂ、又はＣとの間のデータ通信機能を有する。具体的には、イーサネット（登録商標）やＵＳＢなどの有線通信や、Ｂｌｕｅｔｏｏｔｈ(登録商標)、無線ＬＡＮ、などの無線通信を行うデバイスを用いる。
【００４０】
図３は、固定デバイスＣの構成例を示す図である。
この図において、固定デバイスＣは、移動デバイスＡ、Ｂと同様に、内界センサ１０及び外界センサ１６を備える。
この例における内界センサ１０は、ＧＰＳコンパス１２である。また、外界センサ１６は、この例ではカメラ１７及びレーザセンサ１８からなり、他のデバイスＡ、Ｂ、又はＣの方向又は距離を観測する。
【００４１】
また、固定デバイスＣは、予測部２６ａとディスプレイ２６ｂからなる表示部２６を備える。
予測部２６ａは、状態推定部３０（後述する）に記録されている内部状態Ｘのうち、デバイス制御に関連する状態量を、状態遷移方程式ｆ（Ｘ）を使って、指定された時刻まで遷移させる計算を行う。予測部２６ａはロボット上予測部２３ａと同様に、リアルタイム制御の時間制約下で予測処理できるように設計される。
ディスプレイ２６ｂは、予測部２６ａで予測した状態量を表示する。
その他の構成は図２と同様である。
【００４２】
図２、図３において、デバイスＡ、Ｂ、Ｃは、さらに、それ自身の状態量を推定し管理する状態推定部３０を備える。
【００４３】
図４は、状態推定部３０による状態同定方法を示すフロー図である。
この図に示すように、状態推定部３０による状態同定方法は、（Ｓ１）〜（Ｓ７）の各ステップ（工程）からなる。
以下、デバイスＡの状態推定部３０について説明する。この場合、「自身」とはデバイスＡを意味する。
なお、デバイスＢ、Ｃの場合も同様である。
【００４４】
（Ｓ１）では、デバイスＡは、自身（デバイスＡ）の状態量とその分散及び共分散とを管理する。
すなわち、デバイスＡは、自身の位置、姿勢、速度、加速度などの状態量とその分散及び共分散を、確率分布として管理する。
【００４５】
（Ｓ２）では、他のデバイスＢ又はＣの状態量と自身（デバイスＡ）の状態量との相関が高い相関情報に限定して管理する。
すなわち、デバイスＡは、他のデバイスＢ又はＣの状態量と自身の状態量との相関を示す情報（例えば共分散）を、必要なものだけ管理する。なお、他のデバイスＢ又はＣは複数であってもよい。
【００４６】
（Ｓ２）における「相関が高い相関情報」は、例えば相関係数の絶対値の高い複数の相関情報である。
すなわち、デバイスＡの各状態推定部３０は、相関情報を記憶する複数のメモリ（図示せず）を有しており、他のデバイスＢ又はＣとの相関情報の相関係数を絶対値の高い順位の順でメモリに記憶する。
また、（Ｓ２）において、記憶した順位の高い１又は複数のメモリ内の相関情報のみを管理する。
【００４７】
（Ｓ２）における「相関が高い相関情報」は、下記の（Ａ）又は（Ｂ）を含むことが好ましい。
（Ａ）外界センサによって観測したことのある他のデバイスとの相関情報、
（Ｂ）外界センサによって自身（デバイスＡ）を観測したことのある他のデバイスとの相関情報。
【００４８】
また、（Ｓ２）における「相関が高い相関情報」は、下記の（Ａ）（Ｂ）（Ｃ）又は（Ｄ）を除外することが好ましい。
（Ａ）絶対値の小さい共分散。これは例えば、共分散行列の各成分の絶対値の総和や平均などを比較して判定することができる。
（Ｂ）一定時間、外界センサ１６によって計測していないデバイスのうち、相手の外界センサ１６によって自身が計測されていないデバイスとの共分散。
（Ｃ）自身の内部状態を修正した時、同時に修正しなかったデバイスとの共分散。例えば、デバイスＡがデバイスＢを観測した場合、デバイスＢ以外との共分散を全て管理対象から除外する。
（Ｄ）通信不能、或いはアドホック通信の場合に通信ホップ数が予め設定した数より多いデバイスとの共分散。
【００４９】
上述した（Ｓ１）と（Ｓ２）は、この順で実施するのが好ましいが、並行して実施してもよい。
また、（Ｓ１）と（Ｓ２）は、常時、リアルタイムで実施するのがよい。
【００５０】
（Ｓ３）では、内界センサ１０による観測値が得られた観測時刻における自身（デバイスＡ）の内部状態と分散及び共分散を予測する。
（Ｓ４）では、予測した予測値と内界センサ１０の観測値から自身（デバイスＡ）の状態量とその分散及び共分散を修正する。
（Ｓ３）（Ｓ４）は、内界センサ１０による相対的に短い観測サイクル（例えば１〜１０ｍｓｅｃ）毎に繰り返し実施する。
【００５１】
（Ｓ５）では、他のデバイスＡ、Ｂ、又はＣとの間で通信し、外界センサ１６による観測値が得られた観測時刻における自身（デバイスＡ）及び他のデバイスＡ、Ｂ、又はＣの状態量、その分散及び共分散、及び両者間の共分散を予測する。
【００５２】
例えば、「観測した相手（Ｂ、又はＣのうちの１つ）のみ」と通信して、観測した対象のデバイスの状態量、分散・共分散、両者間の共分散を予測する。
さらに、自身との相関が高い他のデバイスとも通信して、デバイスの状態量、分散・共分散、両者間の共分散を予測するのがよい。
次いで、以下（Ｓ６）（Ｓ７）の通り、予測値を修正して、共有する。
【００５３】
通信するデータは、例えば、以下の（１）（２）又は（３）である。
（１）他のデバイス上で予測した、他のデバイスの状態量。
（２）他のデバイス上で予測した、他のデバイスの分散及び共分散。
（３）他のデバイス上で予測した、両者間の共分散情報。
【００５４】
上記の（３）の「両者間の共分散情報」は、正確には、以下のように共分散×状態遷移行列になる。
（３−１）デバイスＡ、Ｂ間の共分散を共分散行列ＣｏｖＡＢ，ｔで表す。
（３−２）Δｔ後の共分散行列の予測値ＣｏｖＡＢ，ｔ＋Δｔは以下の式（１）で表される。
ＣｏｖＡＢ，ｔ＋Δｔ＝ＡＡｔ・ＣｏｖＡＢ，ｔ・ＡＢｔＴ・・・（１）
ここで、ＡＡｔはデバイスＡの内部状態の状態遷移行列（デバイスＡが管理）、ＡＢｔＴは他デバイスＢの内部状態の状態遷移行列（デバイスＢが管理）である。
本発明では，他のデバイスから式（１）のＣｏｖＡＢ，ｔ・ＡＢｔＴを通信で取得し、両者間の共分散を予測する。
【００５５】
（Ｓ６）では、予測された値と外界センサ１６の観測値から自身及び他のデバイスＡ、Ｂ、又はＣの状態量、その分散及び共分散、及び両者間の共分散を修正する。
（Ｓ７）では、修正した結果を自身及び他のデバイスＡ、Ｂ、又はＣで共有する。
（Ｓ５）〜（Ｓ７）は、外界センサ１６による相対的に長い観測サイクル（例えば０．０５〜１．０ｓｅｃ）毎に繰り返し実施する。
【００５６】
以下、上述した本発明の状態同定装置の作用を説明する。
各デバイスＡ、Ｂ、Ｃは、内界センサ１０及び外界センサ１６による観測値を得るごとに、以下の手順で更新処理する。観測値を得るタイミングは、内界センサ１０及び外界センサ１６の計測周期等によって決まる。
【００５７】
（１）観測に関する情報（観測時刻、観測値、観測ノイズ）を取得する。観測値は、内界センサ１０及び外界センサ１６の種類ごとに定義する。観測ノイズは、内界センサ１０及び外界センサ１６で計測するときに予想されるノイズの大きさであり、分散行列の形式で定義する。
（２）観測時刻における、観測に関与したデバイスＡ、Ｂ、Ｃの内部状態と分散及び共分散を予測する。ここで、デバイスＡ、Ｂ、Ｃ間の相対関係を計測した場合は、観測対象のデバイスＡ、Ｂ、Ｃと通信して予測値を取得する。
（３）算出した内部状態の予測値と、内界センサ１０及び外界センサ１６ごとに定義した観測方程式から、観測値の予測値を算出する。
（４）観測値の予測値と、実際の観測値を比較して、内部状態を修正する。
【００５８】
上述した本発明の方法と装置によれば、（Ｓ５）（Ｓ６）の更新処理の際、観測に関与した（自身及び他の）デバイスＡ、Ｂ、又はＣの状態のみ予測し修正するため、デバイスＡ、Ｂ（例えば移動ロボット）の総数が増えても、計算量が増加しない。
【００５９】
また外界センサ１６で計測したデバイスＡ、Ｂ、Ｃがそれぞれ更新処理するため、システム全体で観測値が得られる頻度が増えても、各デバイスＡ、Ｂ、Ｃが更新処理する頻度は増加しない。
【００６０】
また（Ｓ５）において観測に関与した（自身及び他の）デバイスＡ、Ｂ、又はＣとの間でのみ通信するため、通信負荷が１ヶ所に集中しない。また、外界センサ１６で観測したデバイス（例えばＡ）と、観測されたデバイス（例えばＢ）との間は、物理的な距離が近く、間に遮蔽物がない状況にある。そのため、両者は直接電波が届く可能性が高く、転送処理の要らない通信になりやすい。従って、アドホック通信の場合は、通信負荷が少ない。
【００６１】
また各デバイスＡ、Ｂ、Ｃは、自身の内部状態推定結果を、通信を介することなく参照できる。例えば、デバイスＡ、Ｂとして移動ロボットの制御に用いる場合、通信遅れのない最新の推定結果を使って制御できるため、制御の性能が上がる。
【００６２】
また各デバイスＡ、Ｂ、Ｃが個別に予測処理するため、他のデバイスＡ、Ｂ、又はＣの運動モデルや内部状態を把握する必要がない。従って、それぞれのデバイスＡ、Ｂ、Ｃごとに、運動モデルや、推定したい状態量を自由に設定できる。
【００６３】
上述した例では、「観測したデバイスＡ」が更新処理するが、「観測されたデバイスＢ」側に予測値を集約して更新処理しても良い。ただしこの場合は、観測に関する情報（観測値、観測ノイズ、観測方程式などの情報）を観測されたデバイスＢに送信する必要がある。
【００６４】
全センサ１０、１６の情報を１ヶ所に集約する従来方式でも、観測したデバイスＡ、観測されたデバイスＢの内部状態のみ予測し修正することで、計算量を減らすことができる。
【００６５】
本発明は、ベイズフィルタの各手法のうち、以下の条件を満たすものに適用できる。
条件１：各デバイスに関する状態量の時間遷移（予測処理）を、デバイスごとに分けて予測できること。
条件２：２つのデバイス間の相関情報の時間遷移を、２つのデバイスに関する情報から予測できること。
条件１、２を満たすものとして、カルマンフィルタ、拡張カルマンフィルタ、アンセンテッドカルマンフィルタなどが挙げられる。いずれも、確率分布を平均、分散及び共分散で表す手法である。
【００６６】
ただし、アンセンテッドカルマンフィルタの場合は、予測処理が上述した例と以下のように一部異なる。
（１）観測に関与したデバイスに関する現時点での推定結果（推定値、分散及び共分散）を集約する。
（２）（１）の推定値、分散及び共分散から確率分布を代表する点（シグマ点）を数点算出する。
（３）観測時刻におけるシグマ点を予測する。両デバイスの状態遷移モデルを用いる。
（４）予測したシグマ点から、観測時刻における、両デバイスの内部状態、分散及び共分散、両デバイス間の共分散を求める。
【００６７】
一方、上述した例と相違し、以下のようなベイズフィルタの場合、本発明とは異なる処理が必要となる。
パーティクルフィルタは、確率分布を点群で表し、分散及び共分散を管理しないため、処理が異なる。
情報フィルタは、確率分布を情報ベクトル、情報行列（分散行列の逆行列）によって表す。予測処理では、情報行列の逆行列計算が必要なため、各デバイスが独立して分散処理できない。従って、全デバイスの内部状態を同時に予測処理する必要がある。
【００６８】
本発明は上述した実施形態に限定されず、本発明の要旨を逸脱しない範囲で種々変更を加え得ることは勿論である。
【符号の説明】
【００６９】
Ａ、Ｂ  デバイス（移動デバイス、移動ロボット）、
Ｃ  デバイス（固定デバイス）、
１  上位の制御装置
１０  内界センサ、１１  ジャイロセンサ、１２  ＧＰＳコンパス、
１３  車輪用エンコーダ、１４  ステア角エンコーダ、
１６  外界センサ、１７  カメラ、１８  レーザセンサ、
２１  ロボット駆動部、２２  車両制御部、２２ａ  指令値算出部、
２３  行動計画部、２３ａ  ロボット上予測部、２３ｂ  軌道生成部、
２４  計測部、２５  通信部、２６  表示部、
２６ａ  予測部、２６ｂ  ディスプレイ、
３０  状態推定部
【図１】
2014066679.tif 000003
【図２】
2014066679.tif 000004
【図３】
2014066679.tif 000005
【図４】
2014066679.tif 000006