JP2010127650A - 移動体位置推定システムと移動体位置推定方法及び移動体位置推定プログラム - Google Patents

Abstract

【課題】設置センサの位置情報を自動でキャリブレーションしつつ移動体の位置計測も合わせて行う。
【解決手段】測定領域内を移動する移動体Ｍの位置を推定するシステムにおいて、測定領域内に互いに分散して配置され、互い移動体Ｍまでの距離を３個以上の距離センサＴ１〜Ｔｎで計測し、位置推定算出装置Ｐにて、距離センサそれぞれから任意の時刻のセンサ計測値を取得し蓄積し、取得蓄積された各距離センサのセンサ計測値に対して距離に応じた信頼性の度合いを示す距離信頼度を付与し、取得蓄積されたセンサ計測値のうち信頼度の高い計測値を採用して距離センサ及び移動体それぞれの位置を推定するようにし、位置推定の処理は、移動体Ｍの移動前後の位置それぞれの２個以上の距離センサから得られるセンサ計測値を用いて各距離センサの位置のキャリブレーション及び前記移動体の移動位置の推定を行う。
【選択図】 図１

Description

本発明は、測定領域内に互いに分散して配置される多数の距離センサを用いて領域内を移動する移動体の位置を推定する移動体位置推定システムに関する。

従来から移動体の位置を推定する研究は多くなされているが、環境中にセンサを埋め込み、センサ位置を既知とした上で移動体の位置推定を行う方式が主流である。この方法は、一般に（１）計測システムの設置コストが高い、（２）センサあるいは対象移動物体いずれかの位置を既知としなければならない、といった制約がある。

従来の移動体推定技術として、室内にセンサを張り巡らせて人物の位置を取得できるセンシングルームを構築した例がある（非特許文献１参照）。この例は、センシングルーム内での人間の位置を精度良く計測できるようにしたものである。しかしながら、一部屋に５００以上のセンサを設置しており、センサ自体のコストだけでなくキャリブレーションにかける労力も多く、汎用的なシステムとは言えない。

また、他の例として、ＷｉＦｉ（ワイヤレス・フィデリティー）の既存アクセスポイントを利用することで、計測システムの設置コストの低減を図るだけでなく、ユーザが所持する端末の位置情報を利用してアクセスポイントの位置の自動キャリブレーションを実現した「PlaceEngine」と称される技術もある（非特許文献２参照）。しかしながら、この例による方法では、キャリブレーション時にユーザが所持するＲＳＳＩ（受信信号強度値）計測端末の位置を既知としなければならず、上記（１）、（２）の両問題点を克服する位置計測は未だ実現できていない。

森、「生活パターンを覚えて助ける知能住宅」、電子情報通信学会技術研究報告Vol.105 No.224, pp.41-44, 2005。

暦本純一、塩野崎敦、末吉隆彦、味八木崇、「PlaceEngin:実世界集合知に基づくWiFi位置情報基盤」、インターネットコンファレンス2006, pp.95-104, 2006。

従来の移動体位置推定システムでは、環境中にセンサを埋め込み、センサ位置を既知とした上で移動体の位置推定を行う方式が主流であった。しかし、上記方式は一般にキャリブレーションが必要である上に、設置コストが高くなる。こういった問題を克服する位置推定アルゴリズムは依然として実現できていないのが現状である。

本発明は上記の事情に鑑みてなされたもので、設置センサの位置情報を自動でキャリブレーションするだけでなく対象とする移動体の位置計測（トラッキング）も合わせて行うことが可能であり、移動体の位置を容易にかつ高精度に推定することのできる移動体位置推定システム、移動体位置推定方法及び移動体位置推定プログラムを提供することを目的とする。

上記目的を達成するため、本発明は以下のような特徴的構成を有する。
（１）測定領域内を移動する移動体の位置を推定する移動体位置推定システムにおいて、前記測定領域内に互いに分散して配置され、前記移動体までの距離を計測する３個以上の距離センサと、前記３個以上の距離センサそれぞれから任意の時刻のセンサ計測値を取得し蓄積する取得蓄積手段と、前記取得蓄積手段で取得蓄積された各距離センサのセンサ計測値に対して距離に応じた信頼性の度合いを示す距離信頼度を付与する信頼度付与手段と、前記取得蓄積手段で取得蓄積されたセンサ計測値のうち前記信頼度の高い計測値を採用して前記距離センサ及び前記移動体それぞれの位置を推定する位置推定手段とを具備し、前記位置推定手段は、前記移動体の移動前後の位置それぞれにおける２時刻分以上の距離センサから得られるセンサ計測値を用いて各距離センサの位置のキャリブレーション及び前記移動体の移動位置の推定を行うことを特徴とする。

（２）（１）の構成において、前記信頼度付与手段は、前記３個以上の距離センサそれぞれについて、予め与えられる距離誤差分布モデルから最新のセンサ計測値に前記距離信頼度を付与し、前記位置推定手段は、前記３個以上の距離センサそれぞれの最新のセンサ計測値を前記距離信頼度が高い順に選択する第１の選択手段、並びに、この第１の選択手段で選択された最新のセンサ計測値に対して空間配置が最も良い計測値を前記取得蓄積手段で蓄積している計測値から選択する第２の選択手段を備えることを特徴とする。

（３）（２）の構成において、前記位置推定手段は、前記第１の選択手段で選択された最新のセンサ計測値と前記第２の選択手段で選択された蓄積計測値を用いて前記距離センサ及び移動体間距離に起因する条件式で前記距離センサ及び移動体それぞれの位置を算出する第１の算出手段を備えることを特徴とする。

（４）（３）の構成において、前記位置推定手段は、前記第１の算出手段で算出された移動体位置xh'(t)、過去に算出したセンサ位置xs(t-1)と分散Σs(t-1)を用いて距離計測予測値z'を算出し、これらの値(xh'(t),xs(t-1),z')とセンサ計測値zから移動体位置確率密度関数PDFを算出し、この移動体位置確率密度関数PDFから前記移動体の平均位置と分散を算出する第２の算出手段を備えることを特徴とする。

（５）（４）の構成において、前記位置推定手段は、前記第２の算出手段で算出された移動体位置確率密度関数PDFから算出された移動体位置xh(t)、過去に算出したセンサ位置xs(t-1)と分散Σs(t-1)、センサ計測予測値z'、そしてセンサ計測値zの情報からセンサ位置PDFを算出し、このセンサ位置PDFから前記距離センサの平均位置と分散を算出する第３の算出手段を備えることを特徴とする。

（６）測定領域内を移動する移動体の位置を推定する移動体位置推定方法において、前記測定領域内に互いに分散して配置され、前記移動体までの距離を３個以上の距離センサで計測し、前記３個以上の距離センサそれぞれから任意の時刻のセンサ計測値を取得し蓄積し、前記取得蓄積された各距離センサのセンサ計測値に対して距離に応じた信頼性の度合いを示す距離信頼度を付与し、前記取得蓄積されたセンサ計測値のうち前記信頼度の高い計測値を採用して前記距離センサ及び前記移動体それぞれの位置を推定するようにし、前記位置推定の処理は、前記移動体の移動前後の位置それぞれの２個以上の距離センサから得られるセンサ計測値を用いて各距離センサの位置のキャリブレーション及び前記移動体の移動位置の推定を行うことを特徴とする。

（７）（６）の構成において、前記信頼度付与の処理は、前記３個以上の距離センサそれぞれについて、予め与えられる距離誤差分布モデルから最新のセンサ計測値に前記距離信頼度を付与し、前記位置推定の処理は、前記３個以上の距離センサそれぞれの最新のセンサ計測値を前記距離信頼度が高い順に選択し、選択された最新のセンサ計測値に対して空間配置が最も良い計測値を前記蓄積している計測値から選択することを特徴とする。

（８）（７）の構成において、前記位置推定の処理は、前記距離信頼度が高い順に選択された最新のセンサ計測値と前記蓄積された計測値から選択された空間配置が最も良い蓄積計測値を用いて前記距離センサ及び移動体間距離に起因する条件式で距離センサ及び移動体それぞれの位置を算出することを特徴とする。

（９）（８）の構成において、前記位置推定手段は、前記条件式で算出された移動体位置xh'(t)、過去に算出したセンサ位置xs(t-1)と分散Σs(t-1)を用いて距離計測予測値z'を算出し、これらの値(xh'(t),xs(t-1),z')とセンサ計測値zから移動体位置確率密度関数PDFを算出し、前記移動体位置確率密度関数PDFから前記移動体の平均位置と分散を算出し、前記移動体位置確率密度関数PDFから算出された移動体位置xh(t)、過去に算出したセンサ位置xs(t-1)と分散Σs(t-1)、センサ計測予測値z'、そしてセンサ計測値zの情報からセンサ位置PDFを算出し、前記センサ位置PDFから前記距離センサの平均位置と分散を算出することを特徴とする。

（１０）測定領域内を移動する移動体の位置を推定する計算をコンピュータに実行させるための移動体位置推定プログラムにおいて、前記測定領域内に互いに分散して配置され、前記移動体までの距離を計測する３個以上の距離センサそれぞれから任意の時刻のセンサ計測値を取得し蓄積する処理と、前記取得蓄積された各距離センサのセンサ計測値に対して距離に応じた信頼性の度合いを示す距離信頼度を付与する処理と、前記取得蓄積されたセンサ計測値のうち前記信頼度の高い計測値を採用して前記距離センサ及び前記移動体それぞれの位置を推定する処理とを備え、前記位置推定の処理は、前記移動体の移動前後の位置それぞれの２個以上の距離センサから得られるセンサ計測値を用いて各距離センサの位置のキャリブレーション及び前記移動体の移動位置の推定を行うことを特徴とする。

すなわち、本発明では、（１）計測システムの設置コストが高い、（２）センサあるいは対象物体の位置のいずれかを既知としなければならないといった従来システムの問題点を解決するため、（１）センサをユーザが適当に配置できる、（２）センサと移動体間の距離のみを用いたキャリブレーション法であるため、いずれの位置情報を取得する必要がないといった効果を得ることができる。また、移動体と距離の近い３つのセンサ間とのみ毎時キャリブレーションを行うため、比較的信頼性の高い距離情報を用いることができる上、全計算量を抑えるといった効果も得ることができ、非常に汎用性の高い移動体位置推定システムであるといえる。

以上のように、本発明によれば、設置センサの位置情報を自動でキャリブレーションするだけでなく移動体の位置計測も合わせて行うことが可能であり、移動体の位置を容易にかつ高精度に推定することのできる移動体位置推定システム、移動体位置推定方法及び移動体位置推定プログラムを提供することができる。

以下、図面を参照して本発明の実施の形態を詳細に説明する。
図１は本発明に係る移動体位置推定方法を利用したシステムの概要を説明するための概念図である。図１において、計測領域内には距離センサを搭載した送信機Ｔ０（ＩＤ：０），Ｔ１（ＩＤ：１），Ｔ２（ＩＤ：２），Ｔ３（ＩＤ：３），…，Ｔｎ（ＩＤ：ｎ）が予め任意の地点（相対位置座標（ｘｉ，ｙｉ）：ｉは０〜ｎ）に分散設置される。上記計測領域内を移動する移動体Ｍには受信機Ｒが装着される。

各送信機Ｔ０〜Ｔｎは、それぞれ移動体Ｍをセンシングしてその距離を計測する距離センサ（図示せず）を備え、距離情報として送信ＩＤ、計測時刻、計測距離を定期的に送信する。送信機Ｔ０〜Ｔｎが扱う信号は電波、音波を問わず、距離変換可能なものであればよい。

上記計測領域内を移動する移動体Ｍには受信機Ｒが装着される。この受信機Ｒは各送信機Ｔ０〜Ｔｎから送信される距離情報を選択的に取得して位置推定算出装置Ｐに送る。この位置推定算出装置Ｐは、例えば汎用コンピュータによって実現されるもので、図２に示すように、受信機Ｒで受信された各送信機Ｔ０〜Ｔｎからの位置情報を取り込むインターフェース１１と、予め解析された位置推定アルゴリズムに基づいて移動体Ｍの位置推定処理を実行するためのプログラムを格納するＲＯＭ（Read Only Memory）１２、ＲＯＭ１２に格納されるプログラムに従って受信機Ｒにて得られる距離情報から移動体Ｍの位置を推定するＣＰＵ（Central Processing Unit）１３、ＣＰＵ１３の作業領域として機能し、推定結果を格納するＲＡＭ（Random Access Memory）１４、ＣＰＵ１３に対するプログラムの実行指示やデータの入力を行う入力装置１５、ＣＰＵ１３の推定処理状況をモニタするモニタ装置１６を備える。

尚、以下の説明において、距離センサを搭載した送信機Ｔ０〜Ｔｎをそれぞれセンサ、受信機Ｒが装着された移動体Ｍをユーザと称する。
上記構成において、図３を参照して本発明に係る移動体位置推定システムの全体のアルゴリズムについて説明する。

まず、ユーザをセンシングしているセンサそれぞれの計測値（d0(t),d1(t),d2(t),…,dn(t)）の中から信頼度の高い計測値を選択する（ステップＳ１１）。ここでは、予め作成しておいたセンサ計測値の誤差分布を利用して計測値を選択する。例えば、距離計測値の分散を１０、計測値と理論値との平均偏差を１００［ｍｍ］までの計測値を許容するとすれば、図４（（ａ）は理論値−計測値の分布を示し、（ｂ）は理論値に対する分散の分布を示す）に示すような誤差分布を用いると、フィルタ閾値を４［ｍ］として、４［ｍ］以内の距離計測値を全て選択するということになる。

ステップＳ１１で選択されたｍ個（ｍ≧３）のセンサの距離計測値とそのセンサＩＤを用いてｍ個のセンサＴとユーザＭの位置算出を行っていく。ここで、代表センサは設定済か判断することで、初回計算か否かを把握する(ステップＳ１２)。初回計算時（ＮＯ）には、まず、代表センサを３つ選択し、座標系を設定する。具体的には、設定された代表センサのうち、２つのセンサ位置を局所座標系ｘ軸上に設定し、片方を原点とする。残りの一つのセンサ位置を＋ｙ軸方向に設定する（図５）。（ステップＳ１３）。代表センサの設定後、またはステップＳ１２で代表センサが設定済の場合（ＹＥＳ）には、過去に計測した距離データを選択する（ステップＳ１４）。ステップＳ１４に関しては後述する。

次に、３つの代表センサ位置とユーザの位置をＮ−Ｒ法といった数値計算によって算出する(ステップＳ１５)。数値計算で解くべき条件式の一例としては、以下の（１）式に示すように、過去から現在までに得た計測距離に起因する送受信機間の幾何学的関係を表す条件式Ａ部と送信機の位置不変に起因する条件式Ｂ部からなるものがある。（１）式により、代表センサ初期位置とユーザ位置を算出する。

また、本発明による手法では、各センサ位置算出する際、各センサ位置の初期値が必要となる。新たに計測されたセンサ（Ｎセンサと呼ぶことにする）がｍ個の場合、ｘ軸上に設置された２つの代表センサと１つのＮセンサを用いてステップＳ１５と同様にしてＮセンサの初期位置を算出する。つまり、ｍ個のＮセンサ位置を算出するためにｍ-２［回］Ｎ−Ｒ法を解くことで、ｍ個のセンサ初期位置を算出する。

ここで、ステップＳ１４、ステップＳ１５の説明のために、図５に示すセンサＴ０、センサＴ１、センサＴ２によるユーザＭの位置算出アルゴリズムについて述べる。但し、ここではセンサＴ０とセンサＴ１，Ｔ２を代表センサとする。

Ｎ−Ｒ法計算時には２時刻分以上の３つのセンサとユーザ間の距離データが必要となる（ｎ時刻の距離データを用いた場合：状態変数＝センサ位置変数＋ｎ時刻分のユーザ位置、条件式＝ｎ時刻分の（１）Ａ部＋Ｂ部、すなわち３＋２＊ｎ≦条件式３＊ｎ＋１→２≦ｎ)。今回、最新の距離データを含めて４時刻分の距離データを用いるが、より多くの距離データを用いてもよい。

そこで、過去に計測した距離データの中から信頼度に基づいて最適な距離データを選択する（ステップＳ１４）。この場合、距離データの選択法として、例えばセンサとユーザ間の空間配置に基づく信頼度を用いる。今回、（１）式Ａ部は三角測量に他ならないが、一般的に三角測量は計測物と被計測物の空間配置に精度が大きく依存する。

例えば、ＧＰＳ（全地球測位システム）電波による三角測量の場合、図６に示すように、ユーザＭを単位球の原点に置いたとき、三次元空間中の４つのＧＰＳ衛星Ｓ１〜Ｓ４の位置ベクトルと単位球との交点が作り出す四面体の体積が大きいほど位置算出精度はよくなる。したがって、一箇所に密集した４つの衛星Ｓ１〜Ｓ４によるユーザＭの位置計測は精度が悪化する。

以上のことから、ステップＳ１４で位置算出を行う全３つのセンサから相対位置の大きく異なるユーザ位置に着目し、それらの位置で計測された距離データを蓄積されている過去の距離データから選択することができれば位置算出精度の向上が図れる。つまり、最真値の距離データをd(t)＝（d0(t)，d1(t)，d2(t)）と表したとき、d(t)と非類似度の高い３つの距離データ（d(t1)，d(t2)，d(t3)）を距離のログデータから選択すればよい。非類似度の評価式としては、例えば図６に示す四面体の体積を用いればよい。四面体の体積算出式を以下の（３）式に示す。

新たに計測したセンサの初期位置も同様にして算出する。以上により、ステップＳ１１で選択した３つの代表センサと新たに計測されたＮセンサの計３＋ｍ個のセンサ初期位置とユーザの位置算出が可能となる。
最後にパーティクルフィルタを用いたＳＬＡＭ（Simultaneous Localization And Mapping）理論によって、ステップＳ１５で算出したユーザ位置と、距離センサによる距離データを用いて、局所座標系におけるユーザと各センサの平均位置・分散を算出していく（ステップＳ１６）。

図８は上記ＳＬＡＭ理論によるアルゴリズムの詳細な処理の流れを示すブロック線図である。ここでは、時刻ｔにおける処理として、入力をＹ_t-1 ^[k]：ｔ−１各におけるパーティクル、ｕ：制御入力、ｚ：センサ計測値としたときの、出力Ｙt[k]＝＜ｘ_1,t ^[k]，Ｎ_1,t ^[k]，＜μ_1,t ^[k]，Σ_1,t ^[k]＞，＜μ_2,t ^[k]，Σ_2,t ^[k]＞，…，＜μ_Nt[k],t ^[k]，Σ_Nt[k],t ^[k]＞＞を求める例を示している。ここで、ｘ_1,t ^[k]はユーザの平均位置を表し、Ｎ_1,t ^[k]は計測したセンサ数を表す。

パーティクルは各センサ毎に独立してセンサ位置とユーザ位置を算出する。各パーティクルは、ユーザの平均位置、計測したセンサ数、そして各センサ位置の平均・分散を格納している。まず、時刻ｔにおけるｋ番目のパーティクルがｊ番目のセンサに対してユーザの位置算出を行う場合について述べる。尚、観測モデルはＮ(ｚ_t，ｈ(ｍ_ct,ｘ_t)，Ｑ_t)と定義する。

図８において、ユーザの位置予測１００では、Ｎ−Ｒ法によって時刻ｔのユーザ位置の予測値ｘ^_j,t ^[k]を算出し（１１０）、時刻ｔ−１に算出したパーティクルＹ_t-1からセンサ特徴位置の平均μ_j,t-1 ^[k]と分散Σ_j,t-1 ^[k]を抽出し（１２０）、これらの値を用いて観測モデルに基づきセンサ計測予測値ｚ^- _j,t ^[k]を算出し（１３０）、これらの値（ｘ^_j,t ^[k]，μ_j,t-1 ^[k]，Σ_j,t-1 ^[k]）と時刻ｔのセンサ計測値ｚ_tからユーザ位置確率密度関数（ＰＤＦ）を算出し（１４０）、時刻ｔのユーザ位置の平均μ_j,t ^[k]と分散Σ_j,t ^[k]を算出する。ここで、時刻ｔの距離計測予測値ｚ^- _j,t ^[k]を算出する観測モデルは（４）式のようなものがある。

上記ＰＤＦは時刻1からｔ−１までのユーザの位置軌跡ｘ_1:t-1と、時刻１からｔまでの距離データμ_1:t、計測値ｚ_1:t、センサＩＤの対応付けｃ_1:tのそれぞれが与えられたときのユーザ位置ｘ_tの条件付確率と同値である。次に、算出したユーザの平均・分散から作成されるガウス分布に基づいてユーザの位置の推定値ｘ_t,j ^[k]をサンプリングする（１５０）。

センサ位置推定値の更新２００では、サンプリングされたユーザ位置ｘ_t,j ^[k]とセンサ位置の平均μ_j,t-1 ^[k]と分散Σ_j,t-1 ^[k]の値を用いて観測モデルに基づき最適なセンサ計測予測値ｚ^_j,t ^[k]を算出する（２１０）。ここで、特徴とする位置が既知の場合、上記で算出した値（ｚ^_t,j ^[k]，ｚ_t，ｘ_j,t ^[k]，μ_j,t-1 ^[k]，Σ_j,t-1 ^[k]）を用いてＰＤＦを算出し、センサ位置の平均μ_j,t ^[k]と分散Σ_j,t ^[k]を算出する（２２０）。特徴とする位置が未知の場合、ｊ番目のセンサ位置初回計算時には、センサ位置の平均値μ_j,t ^[k]として、図２のステップＳ１５のＮ−Ｒ法で算出したセンサ初期位置を用いる（２３０）。

また、ＳＬＡＭ計算時にｊ番目のセンサが計測されなかった場合は、センサの位置の算出は行わず、過去の算出結果を保存する（２４０）。上記ＰＤＦは、時刻ｔにおけるユーザ位置ｘ_t ^[k]、時刻１からｔまでの計測値ｚ_1:t、センサＩＤの対応付けｃ_1:tが与えられたときのセンサ位置の条件付確率と同値である。このとき算出した尤度（時刻１からｔ−１までのユーザの位置軌跡ｘ_1:t-1、計測値ｚ_1:t-1、センサＩＤの対応付けｃ_1:t-1と、時刻１からｔまでの距離データμ_1:tが与えられたときのセンサ位置ｚ_tの条件付確率）を重みとしてパーティクルに付与する。最後に、重みに比例した確率でパーティクルをリサンプリング（復元抽出として確率の低いものを捨てて確率の高いものを優先的に抽出すること）する（３００）。これにより、重みの小さいパーティクルは消滅し、重みの高いパーティクルが選択されていく。

以上により、代表センサが計測される限り図８の処理を繰り返していくことで、代表センサ以外のセンサが新たに計測された場合でも、各センサ位置を算出していくことが可能となる。
したがって、上記構成による移動体位置推定システムによれば、複数のセンサ(送信機)の位置をキャリブレーションしながらユーザ(受信機)の移動を追尾することが可能となり、移動体の位置を容易にかつ高精度に推定することができる。

なお、本発明は、上記実施形態例そのままに限定されるものではなく、実施段階ではその要旨を逸脱しない範囲で構成要素を変形して具体化できる。また、上記実施形態例に開示されている複数の構成要素の適宜な組合せにより種種の発明を形成できる。例えば、実施形態例に示される全構成要素からいくつかの構成要素を削除しても良い。更に、異なる実施形態例に亘る構成要素を適宜組み合わせても良い。

本発明の一実施形態に係る移動体位置推定システムの概略構成を示す概念図。 上記実施形態のシステムに用いられる位置推定算出装置の具体的な構成を示す概略構成図。 上記実施形態の移動体位置推定システムの全体のアルゴリズムを示すフローチャート。 上記実施形態のシステムに用いられるセンサの計測値の誤差分布例を示す特性図。 本発明の実施形態のシステムにおいて、代表点の設定例を示す概念図。 上記実施形態のシステムに用いられる三角測量の精度を説明するための概念図。 上記実施形態のシステムのセンサ選択の具体的な方法を説明するための概念図。 上記実施形態のシステムに用いられるＳＬＡＭ理論によるアルゴリズムの詳細な処理の流れを示すブロック線図。

符号の説明

Ｔ０〜Ｔｎ…送信機（距離センサ搭載）、Ｍ…移動体、Ｒ…受信機、Ｐ…位置推定算出装置、１１…インターフェース、１２…ＲＯＭ、１３…ＣＰＵ、１４…ＲＡＭ（Random Access Memory）、１５…入力装置、１３…ＣＰＵ、１６…モニタ装置。

Claims (10)

1. 測定領域内を移動する移動体の位置を推定する移動体位置推定システムにおいて、
   前記測定領域内に互いに分散して配置され、前記移動体までの距離を計測する３個以上の距離センサと、
   前記３個以上の距離センサそれぞれから任意の時刻のセンサ計測値を取得し蓄積する取得蓄積手段と、
   前記取得蓄積手段で取得蓄積された各距離センサのセンサ計測値に対して距離に応じた信頼性の度合いを示す距離信頼度を付与する信頼度付与手段と、
   前記取得蓄積手段で取得蓄積されたセンサ計測値のうち前記信頼度の高い計測値を採用して前記距離センサ及び前記移動体それぞれの位置を推定する位置推定手段と
   を具備し、
   前記位置推定手段は、前記移動体の移動前後の位置それぞれにおける２時刻分以上の距離センサから得られるセンサ計測値を用いて各距離センサの位置のキャリブレーション及び前記移動体の移動位置の推定を行うことを特徴とする移動体位置推定システム。
2. 前記信頼度付与手段は、前記３個以上の距離センサそれぞれについて、予め与えられる距離誤差分布モデルから最新のセンサ計測値に前記距離信頼度を付与し、
   前記位置推定手段は、前記３個以上の距離センサそれぞれの最新のセンサ計測値を前記距離信頼度が高い順に選択する第１の選択手段、並びに、この第１の選択手段で選択された最新のセンサ計測値に対して空間配置が最も良い計測値を前記取得蓄積手段で蓄積している計測値から選択する第２の選択手段を備えることを特徴とする請求項１記載の移動体位置推定システム。
3. 前記位置推定手段は、前記第１の選択手段で選択された最新のセンサ計測値と前記第２の選択手段で選択された蓄積計測値を用いて前記距離センサ及び移動体間距離に起因する条件式で前記距離センサ及び移動体それぞれの位置を算出する第１の算出手段を備えることを特徴とする請求項２記載の移動体位置推定システム。
4. 前記位置推定手段は、前記第１の算出手段で算出された移動体位置xh'(t)、過去に算出したセンサ位置xs(t-1)と分散Σs(t-1)を用いて距離計測予測値z'を算出し、これらの値(xh'(t),xs(t-1),z')とセンサ計測値zから移動体位置確率密度関数PDFを算出し、この移動体位置確率密度関数PDFから前記移動体の平均位置と分散を算出する第２の算出手段を備えることを特徴とする請求項３記載の移動体位置推定システム。
5. 前記位置推定手段は、前記第２の算出手段で算出された移動体位置確率密度関数PDFから算出された移動体位置xh(t)、過去に算出したセンサ位置xs(t-1)と分散Σs(t-1)、センサ計測予測値z'、そしてセンサ計測値zの情報からセンサ位置PDFを算出し、このセンサ位置PDFから前記距離センサの平均位置と分散を算出する第３の算出手段を備えることを特徴とする請求項４記載の移動体位置推定システム。
6. 測定領域内を移動する移動体の位置を推定する移動体位置推定方法において、
   前記測定領域内に互いに分散して配置される３個以上の距離センサで前記移動体までの距離を計測し、
   前記３個以上の距離センサそれぞれから任意の時刻のセンサ計測値を取得し蓄積し、
   前記取得蓄積された各距離センサのセンサ計測値に対して距離に応じた信頼性の度合いを示す距離信頼度を付与し、
   前記取得蓄積されたセンサ計測値のうち前記信頼度の高い計測値を採用して前記距離センサ及び前記移動体それぞれの位置を推定するようにし、
   前記位置推定の処理は、前記移動体の移動前後の位置それぞれにおける２時刻分以上の距離センサから得られるセンサ計測値を用いて各距離センサの位置のキャリブレーション及び前記移動体の移動位置の推定を行うことを特徴とする移動体位置推定方法。
7. 前記信頼度付与の処理は、前記３個以上の距離センサそれぞれについて、予め与えられる距離誤差分布モデルから最新のセンサ計測値に前記距離信頼度を付与し、
   前記位置推定の処理は、前記３個以上の距離センサそれぞれの最新のセンサ計測値を前記距離信頼度が高い順に選択し、選択された最新のセンサ計測値に対して空間配置が最も良い計測値を前記蓄積している計測値から選択することを特徴とする請求項６記載の移動体位置推定方法。
8. 前記位置推定の処理は、前記距離信頼度が高い順に選択された最新のセンサ計測値と前記蓄積された計測値から選択された空間配置が最も良い蓄積計測値を用いて前記距離センサ及び移動体間距離に起因する条件式で距離センサ及び移動体それぞれの位置を算出することを特徴とする請求項７記載の移動体位置推定方法。
9. 前記位置推定手段は、
   前記条件式で算出された移動体位置xh'(t)、過去に算出したセンサ位置xs(t-1)と分散Σs(t-1)を用いて距離計測予測値z'を算出し、
   これらの値(xh'(t),xs(t-1),z')とセンサ計測値zから移動体位置確率密度関数PDFを算出し、
   前記移動体位置確率密度関数PDFから前記移動体の平均位置と分散を算出し、
   前記移動体位置確率密度関数PDFから算出された移動体位置xh(t)、過去に算出したセンサ位置xs(t-1)と分散Σs(t-1)、センサ計測予測値z'、そしてセンサ計測値zの情報からセンサ位置PDFを算出し、
   前記センサ位置PDFから前記距離センサの平均位置と分散を算出することを特徴とする請求項８記載の移動体位置推定方法。
10. 測定領域内を移動する移動体の位置を推定する計算をコンピュータに実行させるための移動体位置推定プログラムにおいて、
    前記測定領域内に互いに分散して配置され、前記移動体までの距離を計測する３個以上の距離センサそれぞれから任意の時刻のセンサ計測値を取得し蓄積する処理と、
    前記取得蓄積された各距離センサのセンサ計測値に対して距離に応じた信頼性の度合いを示す距離信頼度を付与する処理と、
    前記取得蓄積されたセンサ計測値のうち前記信頼度の高い計測値を採用して前記距離センサ及び前記移動体それぞれの位置を推定する処理とを備え、
    前記位置推定の処理は、前記移動体の移動前後の位置それぞれの２個以上の距離センサから得られるセンサ計測値を用いて各距離センサの位置のキャリブレーション及び前記移動体の移動位置の推定を行うことを特徴とする移動体位置推定プログラム。

Images