JP5402751B2 - センサネットワーク、状態推定方法及びプログラム - Google Patents

Description

本発明は、観測対象の状態に関する情報を収集する複数のセンサノードを有するセンサネットワーク、そのようなセンサネットワークにおける状態推定方法、及びコンピュータにそのような状態推定方法の手順を実行させるプログラムに関する。

観測対象の状態に関する情報を収集する複数のセンサノードを有するセンサネットワークは、例えばシステム内の観測対象の状態を推定するのに用いられる。この場合、センサネットワークは、推定器にパーティクルフィルタ（Particle Filter）を使用することが多い。パーティクルフィルタは、観測対象の状態の連続的な事後確率密度分布を離散化して、パーティクルセット（Particle Set）と呼ばれるサンプル集団の各メンバーの状態に対して、センサの観測結果との整合性を逐次的に評価してリサンプリング（Resampling）を行う。これにより、パーティクルでシミュレーションした事後確率密度分布を、真の確率密度分布に収束させる。

図１は、パーティクルフィルタの処理を説明する図である。図１中、（ａ）はパーティクルフィルタの処理を示し、（ｂ）はパーティクルフィルタの処理を説明するための尤度（Likelihood）対状態特性を示し、○印はパーティクル（又は、サンプル）を示す。図１において、ステップＳ１では、観測対象の状態の確率密度分布の初期設定を行う。ステップＳ２では、センサノードにより推定されている前回の（即ち、前時刻での）観測対象の状態の確率密度分布である事前確率密度分布を求める。ステップＳ３では、リサンプリングを行うことで、事前確率密度分布中、良いと判断されるパーティクルを複数個コピーしてパーティクルを生成すると共に、悪いと判断されるパーティクルを消去する。推定している観測対象の状態は、常に時間と共に変化しているため、ステップＳ４では、観測対象の位置の移動量等の状態の変化量を計測することで、システム内部の運動モデルにより初期設定（又は、前回）の観測対象の状態からの状態の遷移を取得する。又、ステップＳ４では、状態の遷移をセンサノードで新たに生成されたパーティクルの状態に反映させ、パーティクルを最新の状態に遷移させる。ステップＳ５では、センサノードが推定した今回（即ち、現時刻）の情報を入力して、遷移させた観測対象の状態と比較することでパーティクルの良さを評価し、評価された確率密度分布により事前確率密度分布を更新し、処理はステップＳ２へ戻る。

１つのシステムの観測対象の状態を推定する複数のセンサノードが夫々１つのパーティクルフィルタを有する場合、推定した観測対象の状態の確率密度分布は複数個求められる。そこで、センサネットワークによる観測対象の状態の推定の信念度を向上するためには、これら複数個の確率密度分布を統合することが望ましい。しかし、複数の確率密度分布の統合の仕方によっては、センサノード間で送受信されるデータの量が大きく異なる。

一般的に、各センサノードが推定した観測対象の状態の確率密度分布をガウス混合モデル（ＧＭＭ：Gaussian Mixture Model）で近似して、ＧＭＭのパラメータのみを他のセンサノードへ転送して共有することで、複数の確率密度分布の統合が可能である。しかし、この場合、ＧＭＭの学習が必要であり、各センサノードで扱うパーティクルの状態をあるセンサノードで集中的に一元管理して各センサノード間で共有するので、パーティクルの多様性が低下する。又、あるセンサノードのパーティクルを他のセンサノードのパーティクル分布に内挿して補間を行う場合、全てのパーティクルを他のセンサノードに転送する必要があり、複数の確率密度分布を統合するための通信時間を含む処理時間が増大する。

特開２００８−１４７４２号公報 特開２００８−３３６９６号公報

従来のセンサネットワークでは、各センサノードが用いるパーティクル数、即ち、各センサノードが行う処理の複雑さによっては、センサノード間で送受信されるデータの量がパーティクル数と比例するために膨大となり、推定の信念度を確保しつつ高速なセンサネットワークを構築することは難しいという問題があった。

そこで、本発明は、各センサノードが用いるパーティクル数にかかわらず各センサノードが送受信するデータの量を少なくすることができ、推定の信念度を確保しつつ比較的高速なセンサネットワーク、状態推定方法及びプログラムを提供することを目的とする。

本発明の一観点によれば、観測対象の状態をパーティクルフィルタを用いて推定し、自身が用いるパーティクルの１つの状態量の区間に対して１つの尤度を付与したパーティクルの状態区間の信念度を算出して通信路に送信する複数のセンサノードと、前記通信路を介して前記複数のセンサノードから受信した信念度を統合して統合結果を各センサノードに送信する１個の統合ノードを備え、前記複数のセンサノードの各々は、自身の信念度を前記統合ノードから受信した統合結果に基づいて修正するセンサネットワークが提供される。

本発明の一観点によれば、観測対象の状態を推定する複数のセンサノードと、前記複数のセンサノードと通信可能な統合ノードを有するセンサネットワークにおける状態推定方法であって、前記複数のセンサノードの各々において、観測対象の状態をパーティクルフィルタを用いて推定し、自身が用いるパーティクルの１つの状態量の区間に対して１つの尤度を付与したパーティクルの状態区間の信念度を算出して通信路に送信し、１個の統合ノードにおいて、前記通信路を介して前記複数のセンサノードから受信した信念度を統合して統合結果を前記通信路を介して各センサノードに送信し、前記複数のセンサノードの各々は、自身の信念度を前記通信路を介して前記統合ノードから受信した統合結果に基づいて修正する状態推定方法が提供される。

本発明の一観点によれば、コンピュータを観測対象の状態を推定するセンサノードとして機能させるプログラムであって、観測対象の状態をパーティクルフィルタを用いて推定する推定手段と、パーティクルの１つの状態量の区間に対して１つの尤度を付与したパーティクルの状態区間の信念度を算出して出力するデータ生成手段と、前記データ生成手段が算出した信念度を、外部から受信した複数の信念度を統合した統合結果に基づいて修正する統合処理手段として機能させるプログラムが提供される。

開示のセンサネットワーク、状態推定方法及びプログラムによれば、各センサノードが用いるパーティクル数にかかわらず各センサノードが送受信するデータの量を少なくすることができ、推定の信念度を確保しつつ比較的高速なセンサネットワークを構築することが可能となる。

パーティクルフィルタの処理を説明する図である。 本発明の一実施例におけるセンサネットワークの構成の一例を示す図である。 推定確率密度分布の統合を説明する図である。 複数のパーティクルフィルタの処理結果の統合を説明する図である。 ベイジアン理論に従った確率密度信念度の表現形式を説明する図である。 各パーティクルの状態区間の信念度の表現形式を説明する図である。 センサネットワークの一部の構成を示すブロック図である。 コンピュータシステムの一例を示すブロック図である。 センサネットワーク全体の処理を説明するフローチャートである。 センサノードの処理を説明するフローチャートである。 統合ノードの処理を説明するフローチャートである。 ロボットが認識した前方シーンの一例を示す図である。 ＬＲＦの走査領域内で検知した物体を示す図である。 一方のセンサノードのみで推定したロボット位置と姿勢の離散確率密度分布を示す図である。 他方のセンサノードのみで推定したロボット位置と姿勢の離散確率密度分布を示す図である。 両方のセンサノードで推定したロボット位置と姿勢の離散確率密度分布を統合した情報を示す図である。 両方のセンサノードで推定したロボット位置と姿勢の離散確率密度分布を統合した情報を示す図である。 図１４の確率密度分布に基づいてディスプレイに描画されたロボットの推定移動軌跡を示す図である。 図１５の確率密度分布に基づいてディスプレイに描画されたロボットの推定移動軌跡を示す図である。 図１６の確率密度分布に基づいてディスプレイに描画されたロボットの推定移動軌跡を示す図である。 図１７の確率密度分布に基づいてディスプレイに描画されたロボットの推定移動軌跡を示す図である。

開示のセンサネットワークは、複数のセンサノードと１個の統合ノードを有し、例えばシステム内の観測対象の状態を推定するのに用いられる。各センサノードは、パーティクルフィルタを用いて並列的に観測対象の状態を独自に推定する。各センサノードの推定結果は、情報量を圧縮することで少ないデータ量で統合ノードに送信される。統合ノードは、複数のセンサノードからの情報量が圧縮された推定結果を統合し、統合結果を少ないデータ量で各センサノードに返す。各センサノードは、自身の推定結果を統合ノードからの統合結果を用いて修正する。

このように、統合ノードとセンサノード間で送受信される情報のデータ量は少ないので、各センサノードが用いるパーティクル数にかかわらず、各センサノードが送受信するデータの量を少なくすることができ、推定の信念度を確保しつつ比較的高速なセンサネットワークを構築することが可能となる。

以下に、開示のセンサネットワーク、状態推定方法及びプログラムの各実施例を図面と共に説明する。

先ず、本発明の一実施例におけるセンサネットワークのネットワーク構成について説明する。本実施例では、説明の便宜上、観測対象の状態に関する情報を収集する複数のセンサノードと１つの統合ノードで形成されるセンサネットワークは、例えばシステム内の観測対象の状態を推定するのに用いられるものとする。

図２は、本実施例におけるセンサネットワークの構成を示す図である。図２のセンサネットワーク１は、例えば３個のセンサノード２Ａ，２Ｂ，２Ｃと、１個の統合ノード３を有する。各センサノード２Ａ，２Ｂ，２Ｃは、図３に示すように、パーティクルフィルタを用いて並列的に観測対象の状態を独自に推定して観測対象の状態の推定確率密度分布ＰＡ，ＰＢ，ＰＣを求める。図３において、ＰＡ，ＰＢ，ＰＣは、夫々センサノード２Ａ，２Ｂ，２Ｃのパーティクルフィルタにより推定された観測対象の状態の推定確率密度分布を示す。

図３は、推定確率密度分布の統合を説明する図である。センサノード２Ａ，２Ｂ，２Ｃの推定結果である推定確率密度分布ＰＡ，ＰＢ，ＰＣは、後述するように少ないデータ量で統合ノード３に送信される。統合ノード３は、図３に示すように、各センサノード２Ａ，２Ｂ，２Ｃから受信した推定結果である推定確率密度分布ＰＡ，ＰＢ，ＰＣを統合し、観測対象の状態の推定量である統合結果を求める。統合ノード３は、後述するように少ないデータ量で求めた統合結果を各センサノード２Ａ，２Ｂ，２Ｃに送信する。

図４は、複数のパーティクルフィルタの推定結果の統合を説明する図であり、説明の便宜上、２個のセンサノード２Ａ，２Ｂのパーティクルフィルタの推定結果を統合する場合を示す。図４中、図１と実質的に同じステップには同一符号を付し、その説明は省略する。センサノード２Ａのパーティクルフィルタの処理において、ステップＳ１５では、センサノード２Ａ自身で求めた観測対象の状態の確率密度分布のパーティクルの良さを評価し、処理はステップＳ１６へ進む。ステップＳ１６では、統合ノード３から受信した統合結果に基づいて、センサノード２Ａ自身の確率密度分布のパーティクルの良さを再評価して確率密度分布を修正すると共に、修正した確率密度分布により事前確率密度分布を更新し、処理はステップＳ２へ戻る。事前確率密度分布は、センサノード２Ａにより推定されている前回の（即ち、前時刻での）観測対象の状態の確率密度分布である。センサノード２Ｂのパーティクルフィルタの処理は、センサノード２Ａのパーティクル処理と同様である。

観測対象の状態の真の確率密度分布関数の推定は比較的難しいが、真の確率密度分布関数の上限と下限は比較的正確に推定することができる。そこで、各センサノード２Ａ，２Ｂ，２Ｃは、各センサノード２Ａ，２Ｂ，２Ｃが用いるパーティクルの状態区間の信念度から、上限確率密度分布関数と下限確率密度分布関数を以下のように算出する。又、各センサノード２Ａ，２Ｂ，２Ｃは、上下限確率密度分布関数の逆関数を離散化して、いくつかの任意の離散区間とその信念度のペアを統合ノード３に送信する。このように、各センサノード２Ａ，２Ｂ，２Ｃは、推定した観測対象の状態に関する全ての情報を統合ノード３に送信するのではなく、情報量を圧縮して統合ノード３に送信する。

又、以下に示すように、統合ノード３は、各センサノード２Ａ，２Ｂ，２Ｃから送信されてきた上下限確率密度分布関数を統合し、新たな上下限確率密度分布関数を生成する。統合ノード３は更に、上下限確率密度関数の逆関数を離散化して、複数の離散区間とその信念度のペアを統合結果として各センサノード２Ａ，２Ｂ，２Ｃに送信する。このようにして、上下限確率密度分布関数の統合と統合後の離散化を行う。

式（４）は離散状態の確率密度分布、式（５）は離散確率密度分布の累積分布、式（６）は区間確率密度分布を示す。区間の累積確率密度分布は、式（７）に示すように下限確率密度分布関数と上限確率密度分布関数で表される。従って、確率密度分布関数と上下限確率密度分布は、式（８）で表すことができる。このようにして、上下限確率密度分布を算出することができる。

各センサノード２Ａ，２Ｂ，２Ｃは、統合ノード３から送信されてきたデータ（統合結果）を、各センサノード２Ａ，２Ｂ，２Ｃ自身が用いるパーティクルの状態区間の信念度と統合して、自身の信念度を修正する。このようにして、統合後の上下限確率密度分布関数に基づいて事後確率密度分布を修正する。又、各センサノード２Ａ，２Ｂ，２Ｃは、修正された信念度に基づいて、パーティクルのリサンプリングを行い、処理を通常のパーティクルの処理に戻す。

離散変数をｊで示すと、センサノード２Ａ，２Ｂ，２Ｃのパーティクルフィルタにより推定した状態区間の上下限確率密度分布関数は、夫々式（９）、式（１０）、式（１１）で表される。

離散変数ｊを連続的変数ｘに置き換えると、式（９）、式（１０）、式（１１）から式（１２）、式（１３）、式（１４）が得られる。

例えばセンサノード２Ａが既に用いているパーティクルの状態区間の信念度は、式（２８）で表すことができる。センサノード２Ａが統合ノード３から受信するデータは、上記式（２７）で表されるので、センサノード２Ａにおける状態区間の信念度を統合ノード３からのデータと統合すると、式（２９）のようになる。このように統合された状態区間の信念度は、式（３０）のように修正することができる。センサノード２Ｂ，２Ｃにおけるデータの統合は、センサノード２Ａにおけるデータの統合と同様に行える。このようにして、各センサノード２Ａ，２Ｂ，２Ｃが用いるパーティクルの評価、即ち、重み付けを修正することができる。

各センサノードと統合ノード間で送受信のデータの量は、センサノードで用いるパーティクルの数にかかわらず常に一定量以下になる。又、各センサノードのパーティクルフィルタが有するパーティクルセットを特定の場所で一元管理する必要がなくなる。更に、各パーティクルフィルタは、独自にパーティクルセットを有するため、推定はバイアス（Bias）しにくく（即ち、偏りにくく）、推定の信念度を確保することができる。各パーティクルフィルタで推定した確率密度分布そのものを共有するのではなく、離散化した状態区間とその信念度のみを共有するため、パーティクルフィルタ間でパーティクルセットの内挿処理は不要となる。

次に、センサネットワークの構成を、図７と共に説明する。図７は、センサネットワーク１の一部の構成を示すブロック図である。図７では、説明の便宜上、２個のセンサノード２Ａ，２Ｂのみを示す。これらの２個のセンサノード２Ａ，２Ｂは、例えば観測対象であるロボット等の移動体の状態を推定するのに用いられる。センサノード２Ｂの構成は、センサノード２Ａの構成と同じであるため、以下の説明ではセンサノード２Ａの構成についてのみ説明する。

図７において、センサノード２Ａは、センサ２１及び推定部（又は、推定器）２２を有する。センサ２１は、観測対象である移動体の状態に関する情報を収集するためのセンサである。センサ２１は、例えば移動体の本体に取り付けてられており移動体の位置情報を計測するエンコーダ等の内的センサ、或いは、移動体の本体以外に取り付けてられているカメラ等の外的センサで形成されている。以下の説明では、便宜上、センサ２１が収集する移動体の状態に関する情報は、移動体の位置情報であるものとする。

推定部２２は、パーティクルフィルタを用いる推定装置２２１、データ生成装置２２２、送信装置２２３、受信装置２２４、及び統合処理装置２２５を有する。パーティクルフィルタは、時間的逐次処理によって離散的に近似した位置（状態）の確率密度分布を出力する推定器である。推定装置２２１は、上記の図５に示す如き離散確率密度分布をデータ生成装置２２２へ出力する。データ生成装置２２２は、推定装置（即ち、パーティクルフィルタ）２２１が出力する確率密度分布に基づいて、後述する方法で上記式（２１）で表される送信データＤを生成する。送信データＤの生成方法については後述する。送信装置２２３は、データ生成装置２２２が生成した送信データＤを通信路（又は、チャネル）４を介して統合ノード３へ送信する。この例では、通信路４は無線通信路である。受信装置２２４は、統合ノード３が出力する統合結果Ψを受信する。統合処理装置２２５は、推定装置２２１が用いているパーティクルの状態区間の信念度と、受信装置２２４が統合ノード３から受信した統合結果Ψを統合し、統合された状態区間の信念度を上記の式（３０）のように修正する。又、統合処理装置２２５は、修正した統合された状態区間の信念度、即ち、離散確率密度分布を推定装置２２１に返す。推定装置２２２は、統合処理装置２２５から返された修正した確率密度分布を受け取った後、センサ２１が収集した情報を取得して次の処理サイクルに進む。

推定装置２２１、データ生成装置２２２及び統合処理装置２２３は、メモリ等の記憶部及びＣＰＵ等のプロセッサを有する周知のコンピュータ又はコンピュータシステムにより形成可能である。この場合、記憶部には、プロセッサ（コンピュータ）にセンサネットワークにおける状態推定方法の手順を実行させて推定装置２２１、データ生成装置２２２及び統合処理装置２２３等の手段として機能させるプログラムが格納される。このプログラムは、適切なコンピュータ読み取り可能な記憶媒体に格納しても良い。

統合ノード３は、送受信装置３１、統合処理部３２、及び算出部３３を有する。送受信装置３１は、各センサノード２Ａ，２Ｂからの状態区間の信念度を受信すると共に、統合結果Ψを各センサノード２Ａ，２Ｂへ送信する。統合処理部３２は、各センサノード２Ａ，２Ｂからの状態区間の信念度を統合する装置である。統合処理部３２では、各センサノード２Ａ，２Ｂから送信されてきた状態区間の信念度を含むデータＤを統合する。算出部３３は、統合処理部３２におけるデータＤの統合結果から、新たな状態区間の信念度Ψを算出する。算出部３３が算出した新しい状態区間の信念度Ψは、送受信装置３１を介して各センサノード２Ａ，２Ｂに送信（フィードバック）される。

統合処理部３２及び算出部３３は、統合装置を形成しても良く、メモリ等の記憶部及びＣＰＵ等のプロセッサを有する周知のコンピュータ又はコンピュータシステムにより形成可能である。この場合、記憶部には、プロセッサ（コンピュータ）にセンサネットワークにおける状態推定方法の手順を実行させて統合処理部３２及び算出部３３等の手段として機能させるプログラムが格納される。このプログラムは、適切なコンピュータ読み取り可能な記憶媒体に格納しても良い。

図８は、コンピュータシステムの一例を示すブロック図である。図８に示すコンピュータシステム１００は、ＣＰＵ１０１、記憶部１０２、インタフェース（Ｉ／Ｆ）１０３、入力装置１０４、及び出力装置１０５がバス１０６により接続された構成を有する。ＣＰＵ１０１は、記憶部１０２に格納されたプログラムを実行することによりコンピュータシステム１００全体を制御する。記憶部１０２は、半導体記憶装置、磁気記録媒体、光記録媒体、光磁気記録媒体等で形成可能であり、上記のプログラムや各種データを格納すると共に、ＣＰＵ１０１が実行する演算の中間結果や演算結果等を一時的に格納する一時メモリとしても機能する。Ｉ／Ｆ１０３は、この例では後述するセンサ２１が出力するセンサ出力を受信する。入力装置１０４は、キーボード等により形成可能である。出力装置１０５は、ディスプレイ等により形成可能である。入力装置１０４及び出力装置１０５は、タッチパネルのように入力装置と出力装置の両方の機能を有する入出力装置で形成しても良い。

次に、本実施例におけるセンサネットワークの処理を、図９乃至図１１と共に説明する。図９はセンサネットワーク１全体の処理を説明するフローチャート、図１０はセンサノード２Ａの処理を説明するフローチャート、図１１は統合ノード３の処理を説明するフローチャートである。図９では、図を見やすくするために処理の一部の図示を省略してあり、センサノード２Ａの処理の詳細は図１０に示し、統合ノード３の処理の詳細は図１１に示す。又、以下の説明では、便宜上、移動体の位置は平面図（例えば、ＸＹ平面）上の位置と向きを示すものとする。尚、図９において、センサノード２Ａが実行するステップと混同しないように、センサノード２Ｂが実行するステップには参照符号を付けていない。

図１０において破線で囲んで示すように、ステップＳ２１〜Ｓ２９は推定装置２２１により実行され、ステップＳ３０〜Ｓ３３はデータ生成装置２２２により実行され、ステップＳ３４，Ｓ３５は送信装置２２３により実行され、ステップＳ４３，４４は受信装置２２４により実行され、ステップＳ４５は統合処理装置２２５により実行される。

推定装置２２１において、ステップＳ２１は、観測対象の状態の確率密度分布の初期設定、即ち、初期化を行う。ステップＳ２２は、移動体の最初の位置（初期位置）は既知として、センサノード２Ａの起動開始後、移動体の位置の初期確率密度分布（上記の事前確率密度分布に相当）を設定する。初期確率密度分布の設定方法は、例えば移動体の初期位置の近傍に一様の確率密度分布に従ってランダムパーティクル（又は、ランダムサンプル）を生成し、離散的な確率密度分布を生成して設定する方法であっても良い。尚、センサノード２Ａが用いる各パーティクルには同じ尤度（評価値又は重み付け）を付ける。

ステップＳ２３は、リサンプリングを行い、初期確率密度分布中、良いと判断されるパーティクルを複数個コピーしてパーティクルを生成すると共に、悪いと判断されるパーティクルを消去することで、観測対象の状態の離散確率密度分布から新たな確率密度分布を生成する。良いと判断されるパーティクルは、尤度がある閾値以上であり、悪いと判断されるパーティクルは、尤度が閾値未満である。本実施例では、センサノード２Ａで用いる各パーティクルの近傍にパーティクルの尤度（評価値又は重み付け）に比例した尤度（評価値又は重み付け）を有する新たなパーティクルを生成する。このリサンプリングにより、尤度の高いパーティクルの近傍にはより多くのパーティクルが生成され、尤度の低いパーティクルの近傍にはより少ないパーティクルが生成されるかパーティクルが消滅する。このように、本実施例では、尤度の高い、或いは、尤度の低いパーティクルの近傍と言う曖昧さの概念をリサンプリングに採用している。

推定している移動体の位置（即ち、状態）は常に時間と共に変化しているため、ステップＳ２４は、例えば加速度センサやエンコーダセンサ等をセンサ２１に用いて、移動体の移動量を計測して移動体の位置の変化を推測することで状態の遷移を取得する。つまり、ステップＳ２４は、移動体のダイナミック状態を取得する。ステップＳ２５は、取得した状態の遷移をセンサノード２Ａで新たに生成されたパーティクルの状態に反映させ、パーティクルを最新の状態に遷移させる。これにより、パーティクルの状態遷移が起こる。

ステップＳ３４は、算出されたデータの中から送信データDを抽出して一時メモリに保存する。一時メモリは、推定部２２に対して外付けされた記憶装置で形成しても、推定部２２（センサノード２Ａ）内の記憶装置で形成しても良い。一時メモリは、データ生成装置２２が図８のコンピュータシステム１００で形成されている場合は記憶部１０２で形成可能である。ステップＳ３５は、送信装置２２３において、一時メモリに保存された送信データＤを通信路４を介して統合ノード３へ送信する。

図１１において破線で囲んで示すように、ステップＳ３５，Ｓ３７，Ｓ４２は送受信装置３１により実行され、ステップＳ３８，Ｓ３９は統合処理部３２により実行され、ステップＳ４０，Ｓ４１は算出部３３により実行される。

送受信装置３１において、図１１に示すステップＳ３６は、センサノード２Ａからの送信データＤを受信する。ステップＳ３７は、受信した送信データＤを一時メモリに保存する。一時メモリは、統合ノード３に対して外付けされた記憶装置で形成しても、統合ノード３内の記憶装置で形成しても良い。一時メモリは、統合処理部３２が図８のコンピュータシステム１００で形成されている場合は記憶部１０２で形成可能である。

送受信装置３１において、ステップＳ４２は、一時メモリに保存された信念度Ψを通信路４を介して各センサノード２Ａ，２Ｂへ送信する。

図１０の説明に戻るに、センサノード２Ａの受信装置２２４において、ステップＳ４３は、統合ノード３からの信念度Ψを受信する。ステップＳ４４は、受信した信念度Ψを一時メモリに保存する。

次に、移動体であるロボットが、周知の検出方法で前方のシーンから視覚ランドマークのＩＤ番号を検出することで、ロボットの三次元位置を計測する場合について説明する。ここでは一例として、ロボットが四角い部屋の中を３周移動するものとする。

図１２は、ロボットが認識した前方シーンの一例を示す図である。図１２中、説明の便宜上、部屋内で固定位置を有するランドマークは十字付きの矩形領域で示され、ＩＤ番号は数字で示されている。ロボットは、カメラの撮像映像等からランドマークを周知の方法認識し、認識したランドマークのＩＤ番号を周知の方法で登録データから取得する。これにより、ロボットは、ＩＤ番号に関連付けて登録してある位置情報から、認識したランドマークの絶対位置を知ることができる。ロボット内の複数のセンサノードのセンサは、例えばレーザレンジファインダ（ＬＲＦ：Laser Range Finder）で形成されており、ＬＲＦが走査した走査領域内で検知した物体との距離を周知の方法で知るころができる。

図１３は、ＬＲＦの走査領域内で検知した物体を示す図である。図１３中、ＲはＬＲＦによるｘｙ平面上の走査領域を示し、黒い領域は検知された物体を示す。例えば、走査領域Ｒの半径は２ｍである。尚、ＬＲＦの走査領域Ｒは、ＬＲＦ（又は、ロボット）のチルト角に応じてｘｙ平面に対して角度θ（例えば、θ＝０°〜９０°）だけ傾斜した平面上にあるものとする。

図１４は、ＬＲＦがセンサノード２Ａのセンサ２１の場合に、ある時刻ｔにおいてセンサノード２Ａのみで推定したロボット位置（ｘ，ｙ）と姿勢θの離散確率密度分布を示しており、図１０のステップ２９で得られる算出結果を示す。図１４及び後述する図１５乃至図１７中、黒い点はパーティクルを示す。図１４において、（ａ）はｘ軸に対する離散確率密度、（ｂ）はｙ軸に対する離散確率密度、（ｃ）は姿勢（又は、チルト）θに対する離散確率密度を任意単位で示す。

図１５は、ＬＲＦがセンサノード２Ｂのセンサ２１の場合に、ある時刻ｔにおいてセンサノード２Ｂのみで推定したロボット位置（ｘ，ｙ）と姿勢θの離散確率密度分布を示しており、図１０のステップ２９に相当するステップで得られる算出結果を示す。図１５において、（ａ）はｘ軸に対する離散確率密度、（ｂ）はｙ軸に対する離散確率密度、（ｃ）は姿勢（又は、チルト）θに対する離散確率密度を任意単位で示す。

図１６は、ＬＲＦがセンサノード２Ａのセンサ２１の場合に、ある時刻ｔにおいて各センサノード２Ａ，２Ｂで推定したロボット位置（ｘ，ｙ）と姿勢θの離散確率密度分布を統合した情報を統合ノード３から受信し、この統合した情報に基づいて図１０のステップ４５で得られる算出結果、即ち、修正された離散確率密度分布を示す。図１６において、（ａ）はｘ軸に対する離散確率密度、（ｂ）はｙ軸に対する離散確率密度、（ｃ）は姿勢（又は、チルト）θに対する離散確率密度を任意単位で示す。

図１７は、ＬＲＦがセンサノード２Ｂのセンサ２１の場合に、ある時刻ｔにおいて各センサノード２Ａ，２Ｂで推定したロボット位置（ｘ，ｙ）と姿勢θの離散確率密度分布を統合した情報を統合ノード３から受信し、この統合した情報に基づいて図１０のステップ４５に相当するステップで得られる算出結果、即ち、修正された離散確率密度分布を示す。図１７において、（ａ）はｘ軸に対する離散確率密度、（ｂ）はｙ軸に対する離散確率密度、（ｃ）は姿勢（又は、チルト）θに対する離散確率密度を任意単位で示す。

図１８は、センサノード２Ａにおいて図９のステップＳ４６で一時メモリに保持された、時刻ｔにおいてセンサノード２Ａのみで推定したロボット位置（ｘ，ｙ）と姿勢θの離散確率密度分布（図１４）に基づいてディスプレイに描画されたロボットの推定移動軌跡を示す図である。ただし、この場合に一時メモリに保持されている離散確率密度分布（図１４）は、ステップＳ４５を経ずにステップＳ２９の算出結果から直接得られる期待値である。即ち、図１８は、各センサノード２Ａ，２Ｂの統合結果を利用していない場合の推定移動軌跡を示す。

図１９は、センサノード２Ｂにおいて図９のステップＳ４６に相当するステップで一時メモリに保持された、時刻ｔにおいてセンサノード２Ｂのみで推定したロボット位置（ｘ，ｙ）と姿勢θの離散確率密度分布（図１５）に基づいてディスプレイに描画されたロボットの推定移動軌跡を示す図である。ただし、この場合に一時メモリに保持されている離散確率密度分布（図１５）は、ステップＳ４５に相当するステップを経ずにステップＳ２９に相当するステップの算出結果から直接得られる期待値である。即ち、図１９は、各センサノード２Ａ，２Ｂの統合結果を利用していない場合の推定移動軌跡を示す。

図２０は、センサノード２Ａにおいて図９のステップＳ４６で一時メモリに保持された、時刻ｔにおいて各センサノード２Ａ，２Ｂで推定したロボット位置（ｘ，ｙ）と姿勢θの離散確率密度分布を統合した情報（図１６）に基づいてディスプレイに描画されたロボットの推定移動軌跡を示す図である。この場合に一時メモリに保持されている離散確率密度分布（図１６）は、ステップＳ４５を経てステップＳ２９の算出結果を修正して得られる期待値である。即ち、図２０は、各センサノード２Ａ，２Ｂの統合結果を利用した場合の推定移動軌跡を示す。

図２１は、センサノード２Ｂにおいて図９のステップＳ４６に相当するステップで一時メモリに保持された、時刻ｔにおいて各センサノード２Ａ，２Ｂで推定したロボット位置（ｘ，ｙ）と姿勢θの離散確率密度分布を統合した情報（図１７）に基づいてディスプレイに描画されたロボットの推定移動軌跡を示す図である。この場合に一時メモリに保持されている離散確率密度分布（図１７）は、ステップＳ４５に相当するステップを経てステップＳ２９に相当するステップの算出結果を修正して得られる期待値である。即ち、図２１は、各センサノード２Ａ，２Ｂの統合結果を利用した場合の推定移動軌跡を示す。

図９乃至図１１と共に説明したように、各センサノードが用いるパーティクル数にかかわらず、即ち、各センサノードが行う処理の複雑さにかかわらず、各センサノードが送受信するデータの量を少なく、且つ、常に一定量以下にすることができ、比較的高速なセンサネットワークを構築可能であることが確認された。又、図２０及び図２１からもわかるように、各センサノードの離散確率密度分布を統合した情報に基づいて求めたロボットの推定移動軌跡は略同じになり、推定の信念度を確保可能であることが確認された。

このように、本実施例によれば、各パーティクルフィルタが持つパーティクルセットを特定のノード（即ち、装置）で一元化する必要性はなく、各パーティクルフィルタが独自にパーティクルセットを持つことができるので、状態空間をより精密にサンプリングすることができる。例えば、Ｎ個のパーティクルを収集して特定のノードで管理し、各パーティクルフィルタがこれらＮ個のパーティクルをコピーして使用する場合、状態空間の大きさをＶで表すと、状態空間のサンプリング率はＶ／Ｎとなる。これに対し、本実施例のようにパーティクルフィルタが互いに独立してパーティクルを持つ場合、例えばＭ個のパーティクルフィルタがあれば、状態空間のサンプリング率はＶ／（Ｍ＊Ｎ）となる。

又、本実施例によれば、あるセンサノードのパーティクルを他のセンサノードのパーティクル分布に内挿して補間する処理は不要であるため、そのような補間処理を行う場合と比較するとセンサノードで行う処理の負荷を軽減することができる。

以上の実施例を含む実施形態に関し、更に以下の付記を開示する。
（付記１）
観測対象の状態をパーティクルフィルタを用いて推定し、自身が用いるパーティクルの１つの状態量の区間に対して１つの尤度を付与したパーティクルの状態区間の信念度を算出して通信路に送信する複数のセンサノードと、
前記通信路を介して前記複数のセンサノードから受信した信念度を統合して統合結果を各センサノードに送信する１個の統合ノードを備え、
前記複数のセンサノードの各々は、自身の信念度を前記統合ノードから受信した統合結果に基づいて修正する、センサネットワーク。
（付記２）
前記複数のセンサノードの各々は、
前記パーティクルフィルタが出力する確率密度分布関数の上限と下限に相当する上下限確率密度分布関数を自身の信念度に基づいて算出し、前記上下限確率密度分布関数の逆関数を離散化して離散確率密度分布、任意の離散区間とその信念度のペアを求めるデータ生成装置と、
前記ペアを前記統合ノードに送信する送信装置を備え、
前記統合ノードは、
前記複数のセンサノードの送信装置から前記ペアを受信する送受信装置と、
前記送受信装置が受信した前記ペアから上下限確率密度分布を算出し、上下限確率密度分布を統合すると共に上下限確率密度分布の逆関数を算出して離散化することで前記統合結果を算出する統合装置を備え、
前記送受信装置は前記統合結果を各センサノードに送信する、付記１記載のセンサネットワーク。
（付記３）
前記複数のセンサノードの各々は、
前記データ生成装置が推定した離散確率密度分布を前記統合結果に基づいて修正し、修正後の離散確率密度分布を次の処理サイクルで前回の観測対象の状態の確率密度分布である事前確率密度分布として設定する統合処理装置を備える、付記２記載のセンサネットワーク。
（付記４）
前記統合処理装置は、前記統合処理装置が属するセンサノードのパーティクルフィルタが推定したパーティクル数分の状態区間の信念度を修正する、付記３記載のセンサネットワーク。
（付記５）
前記複数のセンサノードは、並列的に前記観測対象の状態を推定する、付記１乃至４のいずれか１項記載のセンサネットワーク。
（付記６）
観測対象の状態を推定する複数のセンサノードと、前記複数のセンサノードと通信可能な統合ノードを有するセンサネットワークにおける状態推定方法であって、
前記複数のセンサノードの各々において、
観測対象の状態をパーティクルフィルタを用いて推定し、
自身が用いるパーティクルの１つの状態量の区間に対して１つの尤度を付与したパーティクルの状態区間の信念度を算出して通信路に送信し、
１個の統合ノードにおいて、
前記通信路を介して前記複数のセンサノードから受信した信念度を統合して統合結果を前記通信路を介して各センサノードに送信し、
前記複数のセンサノードの各々は、自身の信念度を前記通信路を介して前記統合ノードから受信した統合結果に基づいて修正する、状態推定方法。
（付記７）
前記複数のセンサノードの各々において、
前記パーティクルフィルタが出力する確率密度分布関数の上限と下限に相当する上下限確率密度分布関数を自身の信念度に基づいて算出し、
前記上下限確率密度分布関数の逆関数を離散化して離散確率密度分布を算出し、任意の離散区間とその信念度のペアを求め、
前記ペアを前記統合ノードに送信し、
前記統合ノードにおいて、
前記複数のセンサノードの送信装置から前記ペアを受信し、
受信した前記ペアから上下限確率密度分布を算出し、
上下限確率密度分布を統合すると共に上下限確率密度分布の逆関数を算出して離散化することで前記統合結果を算出し、
前記統合結果を各センサノードに送信する、付記６記載の状態推定方法。
（付記８）
前記複数のセンサノードの各々において、
推定した離散確率密度分布を前記統合結果に基づいて修正し、修正後の離散確率密度分布を次の処理サイクルで前回の観測対象の状態の確率密度分布である事前確率密度分布として設定する、付記７記載の状態推定方法。
（付記９）
前記センサノードの各々において、
パーティクルフィルタが推定したパーティクル数分の状態区間の信念度を修正する、付記８記載の状態推定方法。
（付記１０）
前記複数のセンサノードは並列的に前記観測対象の状態を推定する、付記６乃至９のいずれか１項記載の状態推定方法。
（付記１１）
コンピュータを観測対象の状態を推定するセンサノードとして機能させるプログラムであって、
観測対象の状態をパーティクルフィルタを用いて推定する推定手段と、
パーティクルの１つの状態量の区間に対して１つの尤度を付与したパーティクルの状態区間の信念度を算出して出力するデータ生成手段と、
前記データ生成手段が算出した信念度を、外部から受信した複数の信念度を統合した統合結果に基づいて修正する統合処理手段として機能させる、プログラム。
（付記１２）
前記データ生成手段は、
前記パーティクルフィルタが出力する確率密度分布関数の上限と下限に相当する上下限確率密度分布関数を、前記データ生成手段が算出した前記信念度に基づいて算出し、前記上下限確率密度分布関数の逆関数を離散化して離散確率密度分布を算出し、任意の離散区間とその信念度のペアを求めて出力する、付記１１記載のプログラム。
（付記１３）
前記統合処理手段は、
前記データ生成手段が推定した離散確率密度分布を前記統合結果に基づいて修正し、修正後の離散確率密度分布を次の処理サイクルで前回の観測対象の状態の確率密度分布である事前確率密度分布として設定する、付記１２記載のプログラム。
（付記１４）
コンピュータをセンサネットワークにおいて複数のセンサノードと通信可能な統合ノードとして機能させるプログラムであって、
複数のセンサノードの各々は、パーティクルフィルタを用いてパーティクルの１つの状態量の区間に対して１つの尤度を付与したパーティクルの状態区間の信念度を算出すると共に、前記パーティクルフィルタが出力する確率密度分布関数の上限と下限に相当する上下限確率密度分布関数の逆関数を離散化した離散確率密度分布を算出し、任意の離散区間とその信念度のペアを前記統合ノードに送信し、
前記プログラムは、
前記複数のセンサノードから受信した前記ペアから上下限確率密度分布を算出し、前記上下限確率密度分布を統合すると共に統合した上下限確率密度分布の逆関数を算出する統合処理手段と、
前記統合した上下限確率密度分布の逆関数を離散化して各センサノードに送信するべき統合結果を算出する算出手段として機能させる、プログラム。
（付記１５）
付記１１乃至１４のいずれか１項記載のプログラムを格納した、コンピュータ読み取り可能な記憶媒体。
（付記１６）
観測対象の状態をパーティクルフィルタを用いて推定する推定装置と、
パーティクルの１つの状態量の区間に対して１つの尤度を付与したパーティクルの状態区間の信念度を算出するデータ生成装置と、
前記データ生成装置が算出した信念度を出力する送信装置と、
複数の信念度を統合した統合結果を外部から受信する受信装置と、
前記データ生成装置が算出した信念度を前記受信装置が受信した統合結果に基づいて修正する統合処理装置を備えた、センサノード。
（付記１７）
センサネットワークにおいて複数のセンサノードと通信可能な統合ノードであって、
前記複数のセンサノードで算出されたパーティクルの１つの状態量の区間に対して１つの尤度を付与したパーティクルの状態区間の信念度を受信する送受信装置と、
前記送受信装置が受信した前記複数のセンサノードからの信念度を統合して統合結果を算出する統合装置を備え、
前記送受信装置は、前記統合結果を前記複数のセンサノードへ送信する、統合ノード。

以上、開示のセンサネットワーク、状態推定方法及びプログラムを実施例により説明したが、本発明は上記実施例に限定されるものではなく、本発明の範囲内で種々の変形及び改良が可能であることは言うまでもない。

１ センサネットワーク
２Ａ，２Ｂ，２Ｃ センサノード
３ 統合ノード
４ 通信路
２１ センサ
３１ 送受信装置
３２ 統合処理部
３３ 算出部
２２１ 推定装置
２２２ データ生成装置
２２３ 送信装置
２２４ 受信装置
２２５ 統合処理装置

Claims (5)

1. 観測対象の状態をパーティクルフィルタを用いて推定し、自身が用いるパーティクルの１つの状態量の区間に対して１つの尤度を付与したパーティクルの状態区間の信念度を算出して通信路に送信する複数のセンサノードと、
   前記通信路を介して前記複数のセンサノードから受信した信念度を統合して統合結果を各センサノードに送信する１個の統合ノードを備え、
   前記複数のセンサノードの各々は、自身の信念度を前記統合ノードから受信した統合結果に基づいて修正する、センサネットワーク。
2. 前記複数のセンサノードの各々は、
   前記パーティクルフィルタが出力する確率密度分布関数の上限と下限に相当する上下限確率密度分布関数を自身の信念度に基づいて算出し、前記上下限確率密度分布関数の逆関数を離散化して離散確率密度分布、任意の離散区間とその信念度のペアを求めるデータ生成装置と、
   前記ペアを前記統合ノードに送信する送信装置を備え、
   前記統合ノードは、
   前記複数のセンサノードの送信装置から前記ペアを受信する送受信装置と、
   前記送受信装置が受信した前記ペアから上下限確率密度分布を算出し、上下限確率密度分布を統合すると共に上下限確率密度分布の逆関数を算出して離散化することで前記統合結果を算出する統合装置を備え、
   前記送受信装置は前記統合結果を各センサノードに送信する、請求項１記載のセンサネットワーク。
3. 観測対象の状態を推定する複数のセンサノードと、前記複数のセンサノードと通信可能な統合ノードを有するセンサネットワークにおける状態推定方法であって、
   前記複数のセンサノードの各々において、観測対象の状態をパーティクルフィルタを用いて推定し、自身が用いるパーティクルの１つの状態量の区間に対して１つの尤度を付与したパーティクルの状態区間の信念度を算出して通信路に送信し、
   １個の統合ノードにおいて、前記通信路を介して前記複数のセンサノードから受信した信念度を統合して統合結果を前記通信路を介して各センサノードに送信し、
   前記複数のセンサノードの各々は、自身の信念度を前記通信路を介して前記統合ノードから受信した統合結果に基づいて修正する、状態推定方法。
4. 前記複数のセンサノードの各々において、
   前記パーティクルフィルタが出力する確率密度分布関数の上限と下限に相当する上下限確率密度分布関数を自身の信念度に基づいて算出し、
   前記上下限確率密度分布関数の逆関数を離散化して離散確率密度分布を算出し、任意の離散区間とその信念度のペアを求め、
   前記ペアを前記統合ノードに送信し、
   前記統合ノードにおいて、
   前記複数のセンサノードの送信装置から前記ペアを受信し、
   受信した前記ペアから上下限確率密度分布を算出し、
   上下限確率密度分布を統合すると共に上下限確率密度分布の逆関数を算出して離散化することで前記統合結果を算出し、
   前記統合結果を各センサノードに送信する、請求項３記載の状態推定方法。
5. コンピュータを観測対象の状態を推定するセンサノードとして機能させるプログラムであって、
   観測対象の状態をパーティクルフィルタを用いて推定する推定手段と、
   パーティクルの１つの状態量の区間に対して１つの尤度を付与したパーティクルの状態区間の信念度を算出して出力するデータ生成手段と、
   前記データ生成手段が算出した信念度を、外部から受信した複数の信念度を統合した統合結果に基づいて修正する統合処理手段として機能させる、プログラム。