移動体位置推定方法及び移動体位置推定システム
【課題】設置センサ、移動体の位置情報を自動でキャリブレーションするだけでなく移動体のトラッキングも同時に行う。
【解決手段】測定領域内を移動する移動体Ｍの位置を推定する場合に、測定領域内のｎ箇所以上の地点の送信機Ｔ１〜Ｔｎから移動体Ｍ（受信機Ｒ）までの距離を互いに時刻同期した状態で測定し、各測定地点から移動体Ｍまでの任意の測定時刻の距離データを受信機Ｒで取得し、位置推定装置Ｐにおいて、取得された任意の測定時刻の距離データから個々の測定地点及び移動体それぞれの位置を推定する。その際、移動体Ｍの移動前後の位置それぞれの３箇所以上の測定地点から得られる距離データを用いて、各測定地点の位置のキャリブレーション及び移動体Ｍの移動位置の推定を同時に行う。
【解決手段】測定領域内を移動する移動体Ｍの位置を推定する場合に、測定領域内のｎ箇所以上の地点の送信機Ｔ１〜Ｔｎから移動体Ｍ（受信機Ｒ）までの距離を互いに時刻同期した状態で測定し、各測定地点から移動体Ｍまでの任意の測定時刻の距離データを受信機Ｒで取得し、位置推定装置Ｐにおいて、取得された任意の測定時刻の距離データから個々の測定地点及び移動体それぞれの位置を推定する。その際、移動体Ｍの移動前後の位置それぞれの３箇所以上の測定地点から得られる距離データを用いて、各測定地点の位置のキャリブレーション及び移動体Ｍの移動位置の推定を同時に行う。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体の位置を推定する移動体位置推定方法及び移動体位置推定システムに関する。
【背景技術】
【０００２】
従来から移動体の位置を推定する研究は多くなされているが、環境中にセンサを埋め込み、センサ位置を既知とした上で移動体の位置推定を行う方式が主流である。この方法は、一般に（１）計測システムの設置コストが高い、（２）センサあるいは対象移動物体いずれかの位置を既知としなければならない、といった制約がある。
【０００３】
従来の移動体推定技術として、室内にセンサを張り巡らせて人物の位置を取得できるセンシングルームを構築した例がある（非特許文献１参照）。この例は、センシングルーム内での人間の位置を精度良く計測できるようにしたものである。しかしながら、一部屋に５００以上のセンサを設置しており、センサ自体のコストだけでなくキャリブレーションにかける労力も多く、汎用的なシステムとは言えない。
【０００４】
また、他の例として、ＷｉＦｉ（ワイヤレス・フィデリティー）の既存アクセスポイントを利用することで、計測システムの設置コストの低減を図るだけでなく、ユーザが所持する端末の位置情報を利用してアクセスポイントの位置の自動キャリブレーションを実現した「PlaceEngine」と称される技術もある（非特許文献２参照）。しかしながら、この例による方法では、キャリブレーション時にユーザが所持するＲＳＳＩ（受信信号強度値）計測端末の位置を既知としなければならず、上記（１）、（２）の両問題点を克服する位置計測は未だ実現できていない。
【０００５】
【非特許文献１】森、「生活パターンを覚えて助ける知能住宅」、電子情報通信学会技術研究報告Vol.105 No.224, pp.41-44, 2005。
【０００６】
【非特許文献２】暦本純一、塩野崎敦、末吉隆彦、味八木崇、「PlaceEngin:実世界集合知に基づくWiFi位置情報基盤」、インターネットコンファレンス2006, pp.95-104, 2006。
【発明の開示】
【発明が解決しようとする課題】
【０００７】
従来の移動体位置推定システムでは、環境中にセンサを埋め込み、センサ位置を既知とした上で移動体の位置推定を行う方式が主流であった。しかし、上記方式は一般にキャリブレーションが必要である上に、設置コストが高くなる。こういった問題を克服する位置推定アルゴリズムは依然として実現できていないのが現状である。
【０００８】
本発明は上記の事情に鑑みてなされたもので、設置センサの位置情報を自動でキャリブレーションするだけでなく対象とする移動体のトラッキングも同時に行うことが可能であり、移動体の位置を容易にかつ高精度に推定することのできる移動体位置推定方法及び移動体位置推定装置を提供することを目的とする。
【課題を解決するための手段】
【０００９】
上記目的を達成するため、本発明は以下のような特徴的構成を有する。
（１）測定領域内を移動する移動体の位置を推定する移動体位置推定方法において、前記測定領域内の３箇所以上の測定地点から前記移動体までの距離を互いに時刻同期した状態で測定し、前記３箇所以上の測定地点から前記移動体までの任意の測定時刻の距離データを取得し、前記取得された任意の測定時刻の距離データから個々の測定地点及び前記移動体それぞれの位置を推定するものであり、前記移動体の移動前後の位置それぞれの前記３箇所以上の測定地点から得られる距離データを用いて各測定地点の位置のキャリブレーション及び前記移動体の移動位置の推定を同時に行うことを特徴とする。
【００１０】
（２）（１）において、前記推定は、前記測定地点の位置が固定であることに起因する条件式、個々の計測距離に起因する拘束条件を踏まえて推定を行うことを特徴とする。
（３）（１）において、前記測定地点が４箇所以上のとき、前記推定は、前記移動体との距離が近い順に３つの測定地点を特定してそれぞれの位置のキャリブレーションを行い、前記移動体の移動に伴って順次特定される３つの測定地点により順次形成される三角形を最終的に連結させていくことで、全距離センサと移動体との位置を絶対座標系に示すことを特徴とする。
【００１１】
（４）（１）において、前記推定は、前記３個以上の測定地点のいずれかを基準位置とし、この基準位置を通る直線を座標軸として、各測定地点の位置を相対的に推定することを特徴とする。
（５）測定領域内を移動する移動体の位置を推定する移動体位置推定システムにおいて、前記測定領域内に配置され、互いに時刻同期した状態で前記移動体までの距離を測定する３個以上の距離センサと、前記３個以上の距離センサから前記移動体までの任意の測定時刻の距離データを取得する距離データ取得手段と、前記距離データ取得手段で取得された任意の測定時刻の距離データから個々の距離センサ及び前記移動体それぞれの位置を推定する推定手段とを具備し、前記推定手段は、前記移動体の移動前後の位置それぞれの前記３個以上の距離センサから得られる距離データを用いて各距離センサの位置のキャリブレーション及び前記移動体の移動位置の推定を同時に行うことを特徴とする。
【００１２】
（６）（５）の構成において、前記推定手段は、前記距離センサの位置が固定であることに起因する条件式、個々の計測距離に起因する拘束条件を踏まえて推定を行うことを特徴とする。
（７）（５）の構成において、前記距離センサが４個以上のとき、前記推定手段は、前記移動体との距離が近い順に３つの距離センサを特定してそれぞれの位置のキャリブレーションを行い、前記移動体の移動に伴って順次特定される３つの距離センサにより順次形成される三角形を最終的に連結させていくことで、全距離センサと移動体との位置を絶対座標系に示すことを特徴とする。
【００１３】
（８）（５）の構成において、前記推定手段は、前記３個以上の距離センサのいずれかを基準位置とし、この基準位置を通る直線を座標軸として、各距離センサの位置を相対的に推定することを特徴とする。
【発明の効果】
【００１４】
すなわち、本発明では、（１）計測システムの設置コストが高い、（２）センサあるいは対象物体の位置のいずれかを既知としなければならないといった従来システムの問題点を解決するため、（１）センサをユーザが適当に配置できる、（２）センサと移動体間の距離のみを用いたキャリブレーション法であるため、いずれの位置情報を取得する必要がないといった効果を得ることができる。また、移動体と距離の近い３つのセンサ間とのみ毎時キャリブレーションを行うため、比較的信頼性の高い距離情報を用いることができる上、全計算量を抑えるといった効果も得ることができ、非常に汎用性の高い移動体位置推定システムであるといえる。
【００１５】
以上のように、本発明によれば、設置センサの位置情報を自動でキャリブレーションするだけでなく移動体のトラッキングも同時に行うことが可能であり、移動体の位置を容易にかつ高精度に推定することのできる移動体位置推定方法及び移動体位置推定装置を提供することができる。
【発明を実施するための最良の形態】
【００１６】
以下、図面を参照して本発明の実施の形態を詳細に説明する。
図１は本発明に係る移動体位置推定方法を利用したシステムの概要を説明するための概念図である。図１において、計測領域内には距離センサを搭載した送信機Ｔ０（ＩＤ：０），Ｔ１（ＩＤ：１），Ｔ２（ＩＤ：２），Ｔ３（ＩＤ：３），…，Ｔｎ（ＩＤ：ｎ）が予め任意の地点（相対位置座標（ｘｉ，ｙｉ）：ｉは０〜ｎ）に分散設置される。上記計測領域内を移動する移動体Ｍには受信機Ｒが装着される。
【００１７】
各送信機Ｔ０〜Ｔｎは、それぞれ移動体との距離を計測する距離センサ（図示せず）を備え、距離情報として送信ＩＤ、計測時刻、計測距離を定期的に送信する。送信機Ｔ０〜Ｔｎが扱う信号は電波、音波を問わず、距離変換可能なものであればよい。
上記計測領域内を移動する移動体Ｍには受信機Ｒが装着される。この受信機Ｒは各送信機Ｔ０〜Ｔｎから送信される距離情報を選択的に取得して位置推定算出装置Ｐに送る。この位置推定算出装置Ｐは、例えば汎用コンピュータによって実現されるもので、図２に示すように、受信機Ｒで受信された各送信機Ｔ０〜Ｔｎからの位置情報を取り込むインターフェース１１と、予め解析された位置推定アルゴリズムに基づいて移動体Ｍの位置推定処理を実行するためのプログラムを格納するＲＯＭ（Read Only Memory）１２、ＲＯＭ１２に格納されるプログラムに従って受信機Ｒにて得られる距離情報から移動体Ｍの位置を推定するＣＰＵ（Central Processing Unit）１３、ＣＰＵ１３の作業領域として機能し、推定結果を格納するＲＡＭ（Random Access Memory）１４、ＣＰＵ１３に対するプログラムの実行指示やデータの入力を行う入力装置１５、ＣＰＵ１３の推定処理状況をモニタするモニタ装置１６を備える。
【００１８】
上記構成において、図３を参照して本発明に係る移動体位置推定システムの全体のアルゴリズムについて説明する。
まず、移動体Ｍが所持する受信機Ｒと距離が近い３つの送信機Ｔを選定し（Ｓ１）、３つの送信機Ｔとの自動キャリブレーション処理を実行する（Ｓ２）。ここで、キャリブレーションによって得られた各送信機Ｔで求まる距離推定値の評価を行い、所定の条件を満足するか否かを判断して、全ての距離推定値が所定の条件を満足するまでステップＳ１，Ｓ２を繰り返し実行する（Ｓ３）。次に、任意の送信機の位置を基準として各送信機の位置座標を変換し（Ｓ４）、再度所定の条件に基づく推定値の評価を行い、全ての距離推定値が所定の条件を満足するまでステップＳ１〜Ｓ４を繰り返し実行する（Ｓ５）。続いて位置推定領域を導出することで（Ｓ６）、送信機キャリブレーション処理を完了する。
【００１９】
キャリブレーション処理後は、受信機のトラッキングを行い（Ｓ７）、再び所定の条件に基づく推定値の評価を行い、全ての距離推定値が所定の条件を満足するまでステップＳ１〜Ｓ７を繰り返し実行する（Ｓ５）。条件を満足する場合には、ステップＳ７に戻り、受信機のトラッキング処理を継続する。
【００２０】
ここで、上記ステップＳ２の３つの送信機との自動キャリブレーション処理の概要を図４に示して説明する。
図４において、選定された３つの送信機から移動体側の受信機との間で行われた距離計測結果が得られると（Ｓ２１）、各送信機及び受信機それぞれの位置推定処理を行う（Ｓ２２）。この位置推定処理は所定の条件に基づく推定値の評価結果が規定値以上になるまで繰り返し処理される（Ｓ２３）。各送受信機間の位置推定が完了した場合には、推定誤差を低減する処理を行い（Ｓ２４）、最終的に位置推定領域を導出する（Ｓ２５）。
【００２１】
上記ステップＳ２２の位置推定では、式（１）に示す条件式を解析的に解くことによって送受信機それぞれの位置推定を行う。
【００２２】
【数１】
上記条件式の一例としては、過去から現在までに得た計測距離に起因する送受信機間の幾何学的関係を表す条件式Ａ部と、送信機の位置不変に起因する条件式Ｂ部からなる。図５に上記送受信機間の幾何学的関係を示す。図５において、送信機Ｔ０（ＩＤ：０），Ｔ１（ＩＤ：１），Ｔ２（ＩＤ：２）に対し、時刻ｔ−２，ｔ−１，ｔの順に受信機Ｒが装着された移動体Ｍの位置が移動したとする。ここで、時刻ｔにおけるＩＤ：ｉの送信機Ｔｉと移動体Ｍとの間の距離をｄi,tとし、ＩＤ：ｉの送信機Ｔｉの位置座標を（ｘｉ，ｙｉ）、時刻ｔにおける移動体の位置座標を（ｘh,t，ｙh,t）とすると、時刻ｔ−２における移動体Ｍの送信機Ｔ０〜Ｔ２それぞれからの距離はｄ0,t-2，ｄ1,t-2，ｄ2,t-2となり、時刻ｔ−１における移動体Ｍの送信機Ｔ０〜Ｔ２それぞれからの距離はｄ0,t-1，ｄ1,t-1，ｄ2,t-1となり、時刻ｔにおける移動体Ｍの送信機Ｔ０〜Ｔ２それぞれからの距離はｄ0,t，ｄ1,t，ｄ2,tとなる。
【００２３】
この場合、以下に説明する３辺キャリブレーションの原理に基づいて処理される。すなわち、３個の位置固定された送信機が用意されたとすると、単純には変数が６個（各送信機のｘ，ｙ座標）となる。続いて、移動体の位置として変数が２個（受信機のｘ，ｙ座標）追加される。この場合、位置と距離の関係式が３つ組みあがるため、３辺の距離を測定することができる。単純には、６つの位置で測定すれば変数が１８個となり、関係式も１８個となって、連立方程式を解くことで、３つの送信機と移動体の各時点の位置を算出することができる。
【００２４】
ただし、互いに相対的な位置を求めることなので、上記位置推定処理（Ｓ２２）において、図５に示すように、局所座標系の原点（０，０）と軸方向（Ｘ軸）を定め、さらに、二つの送信機を局所座標系のＸ軸上に配置することで、３つの送信機の位置を表す変数を３個に軽減することができる。したがって、３つの位置での距離を測定すれば、送信機と移動体の位置を求めることができる（２辺キャリブレーションの原理）。すなわち、３個の位置固定された送信機が用意され、２つの送信機の位置が既知であるとする。この場合、送信機の位置を表す変数が２個（位置が不明な送信機のｘ，ｙ座標）となり、移動体の位置として変数が２個（受信機のｘ，ｙ座標）追加される。この場合も、位置と距離の関係式が３つ組みあがるため、３パスの距離を測定することができる。２つの位置で測定すれば、変数が６個。関係式も６個となって、連立方程式を解くことができる。
【００２５】
尚、推定誤差低減処理（Ｓ２４）において、位置推定値をカルマンフィルタのようなものでフィルタ処理を行うことにより、推定精度を向上できるのは言うまでもない。以上のアルゴリズムにより、３つの送信機によって形成される三角形が送信機の数に応じて作られる。
【００２６】
次に、上記で形成される各三角形の連結を利用した全送信機の絶対座標系記述方式について述べる。処理概要を図６に示す。図６は、図３に示したフローチャートの送受信機キャリブレーション処理部分を具体的に示したフローチャートである。まず、ステップＳ１で３つの送信機を選定する際、その組み合わせによって作成された各三角形に順次ＩＤを割り当てる（Ｔ０，Ｔ１，…，Ｔｎ）。続いて、ステップＳ２において、Ｔ０の初回キャリブレーション時は全辺のキャリブレーションを行うが、以降は移動前後の隣接辺の長さを固定した状態で残り２辺のキャリブレーションを行う。例えば、図７に示すように、Ｔ０（送信機Ｔ０，Ｔ１，Ｔ２による三角形）からＴ１（送信機Ｔ１，Ｔ２，Ｔ４による三角形）に移動した場合、隣接辺である送信機Ｔ１，Ｔ２間の距離を固定して送信機Ｔ４の位置推定を行う。
【００２７】
以上を繰り返すことにより、全送信機の位置推定の精度を向上させることができ、結果として全送信機間の距離を算出することができる。最後に、ある送信機を基準（原点）として各三角形を連結させれば、全送信機の絶対座標系記述が可能となる。但し、各三角形連結時に、図８のような鏡像問題が生じるが、送信機Ｔ２，Ｔ４間の推定距離を用いることで回避できる。
【００２８】
各送信機の位置が定まった後は、受信機のトラッキング専用システムとして機能する。トラッキング時に不具合が生じた場合は、送受信機キャリブレーション処理に戻り、再度図３に示す各送受信機のキャリブレーションを行う。以上により、送信機（センサ）と受信機（移動体）のキャリブレーションならびに受信機（移動体）トラッキングが可能となる。
【００２９】
なお、本発明は、上記実施形態例そのままに限定されるものではなく、実施段階ではその要旨を逸脱しない範囲で構成要素を変形して具体化できる。また、上記実施形態例に開示されている複数の構成要素の適宜な組合せにより種種の発明を形成できる。例えば、実施形態例に示される全構成要素からいくつかの構成要素を削除しても良い。更に、異なる実施形態例に亘る構成要素を適宜組み合わせても良い。
【図面の簡単な説明】
【００３０】
【図１】本発明に係る移動体位置推定方法を利用したシステムの一実施形態の概要を説明するための概念図。
【図２】図１に示すシステムに用いられる位置推定算出装置の具体的な構成を示す概略構成図。
【図３】本発明に係る移動体位置推定システムの全体のアルゴリズムを示すフローチャート。
【図４】図１に示すシステムにおいて、３つの送信機との自動キャリブレーション処理の概要を示すフローチャート。
【図５】図１に示すシステムにおいて、各計測時の幾何学的関係を示す概念図。
【図６】図３に示す送受信機キャリブレーション処理の概要を示すフローチャート。
【図７】本実施形態において、選定三角形の移動例を示す図。
【図８】本実施形態において、三角形連結時に問題となる鏡像の一例を示す図。
【符号の説明】
【００３１】
Ｔ０〜Ｔｎ…送信機、Ｍ…移動体、Ｒ…受信機、Ｐ…位置推定算出装置、１１…位置情報インターフェースと、１２…ＲＯＭ（Read Only Memory）、１３…ＣＰＵ（Central Processing Unit）、１４…ＲＡＭ（Random Access Memory）、１５…入力装置、１６…モニタ装置。
【技術分野】
【０００１】
本発明は、移動体の位置を推定する移動体位置推定方法及び移動体位置推定システムに関する。
【背景技術】
【０００２】
従来から移動体の位置を推定する研究は多くなされているが、環境中にセンサを埋め込み、センサ位置を既知とした上で移動体の位置推定を行う方式が主流である。この方法は、一般に（１）計測システムの設置コストが高い、（２）センサあるいは対象移動物体いずれかの位置を既知としなければならない、といった制約がある。
【０００３】
従来の移動体推定技術として、室内にセンサを張り巡らせて人物の位置を取得できるセンシングルームを構築した例がある（非特許文献１参照）。この例は、センシングルーム内での人間の位置を精度良く計測できるようにしたものである。しかしながら、一部屋に５００以上のセンサを設置しており、センサ自体のコストだけでなくキャリブレーションにかける労力も多く、汎用的なシステムとは言えない。
【０００４】
また、他の例として、ＷｉＦｉ（ワイヤレス・フィデリティー）の既存アクセスポイントを利用することで、計測システムの設置コストの低減を図るだけでなく、ユーザが所持する端末の位置情報を利用してアクセスポイントの位置の自動キャリブレーションを実現した「PlaceEngine」と称される技術もある（非特許文献２参照）。しかしながら、この例による方法では、キャリブレーション時にユーザが所持するＲＳＳＩ（受信信号強度値）計測端末の位置を既知としなければならず、上記（１）、（２）の両問題点を克服する位置計測は未だ実現できていない。
【０００５】
【非特許文献１】森、「生活パターンを覚えて助ける知能住宅」、電子情報通信学会技術研究報告Vol.105 No.224, pp.41-44, 2005。
【０００６】
【非特許文献２】暦本純一、塩野崎敦、末吉隆彦、味八木崇、「PlaceEngin:実世界集合知に基づくWiFi位置情報基盤」、インターネットコンファレンス2006, pp.95-104, 2006。
【発明の開示】
【発明が解決しようとする課題】
【０００７】
従来の移動体位置推定システムでは、環境中にセンサを埋め込み、センサ位置を既知とした上で移動体の位置推定を行う方式が主流であった。しかし、上記方式は一般にキャリブレーションが必要である上に、設置コストが高くなる。こういった問題を克服する位置推定アルゴリズムは依然として実現できていないのが現状である。
【０００８】
本発明は上記の事情に鑑みてなされたもので、設置センサの位置情報を自動でキャリブレーションするだけでなく対象とする移動体のトラッキングも同時に行うことが可能であり、移動体の位置を容易にかつ高精度に推定することのできる移動体位置推定方法及び移動体位置推定装置を提供することを目的とする。
【課題を解決するための手段】
【０００９】
上記目的を達成するため、本発明は以下のような特徴的構成を有する。
（１）測定領域内を移動する移動体の位置を推定する移動体位置推定方法において、前記測定領域内の３箇所以上の測定地点から前記移動体までの距離を互いに時刻同期した状態で測定し、前記３箇所以上の測定地点から前記移動体までの任意の測定時刻の距離データを取得し、前記取得された任意の測定時刻の距離データから個々の測定地点及び前記移動体それぞれの位置を推定するものであり、前記移動体の移動前後の位置それぞれの前記３箇所以上の測定地点から得られる距離データを用いて各測定地点の位置のキャリブレーション及び前記移動体の移動位置の推定を同時に行うことを特徴とする。
【００１０】
（２）（１）において、前記推定は、前記測定地点の位置が固定であることに起因する条件式、個々の計測距離に起因する拘束条件を踏まえて推定を行うことを特徴とする。
（３）（１）において、前記測定地点が４箇所以上のとき、前記推定は、前記移動体との距離が近い順に３つの測定地点を特定してそれぞれの位置のキャリブレーションを行い、前記移動体の移動に伴って順次特定される３つの測定地点により順次形成される三角形を最終的に連結させていくことで、全距離センサと移動体との位置を絶対座標系に示すことを特徴とする。
【００１１】
（４）（１）において、前記推定は、前記３個以上の測定地点のいずれかを基準位置とし、この基準位置を通る直線を座標軸として、各測定地点の位置を相対的に推定することを特徴とする。
（５）測定領域内を移動する移動体の位置を推定する移動体位置推定システムにおいて、前記測定領域内に配置され、互いに時刻同期した状態で前記移動体までの距離を測定する３個以上の距離センサと、前記３個以上の距離センサから前記移動体までの任意の測定時刻の距離データを取得する距離データ取得手段と、前記距離データ取得手段で取得された任意の測定時刻の距離データから個々の距離センサ及び前記移動体それぞれの位置を推定する推定手段とを具備し、前記推定手段は、前記移動体の移動前後の位置それぞれの前記３個以上の距離センサから得られる距離データを用いて各距離センサの位置のキャリブレーション及び前記移動体の移動位置の推定を同時に行うことを特徴とする。
【００１２】
（６）（５）の構成において、前記推定手段は、前記距離センサの位置が固定であることに起因する条件式、個々の計測距離に起因する拘束条件を踏まえて推定を行うことを特徴とする。
（７）（５）の構成において、前記距離センサが４個以上のとき、前記推定手段は、前記移動体との距離が近い順に３つの距離センサを特定してそれぞれの位置のキャリブレーションを行い、前記移動体の移動に伴って順次特定される３つの距離センサにより順次形成される三角形を最終的に連結させていくことで、全距離センサと移動体との位置を絶対座標系に示すことを特徴とする。
【００１３】
（８）（５）の構成において、前記推定手段は、前記３個以上の距離センサのいずれかを基準位置とし、この基準位置を通る直線を座標軸として、各距離センサの位置を相対的に推定することを特徴とする。
【発明の効果】
【００１４】
すなわち、本発明では、（１）計測システムの設置コストが高い、（２）センサあるいは対象物体の位置のいずれかを既知としなければならないといった従来システムの問題点を解決するため、（１）センサをユーザが適当に配置できる、（２）センサと移動体間の距離のみを用いたキャリブレーション法であるため、いずれの位置情報を取得する必要がないといった効果を得ることができる。また、移動体と距離の近い３つのセンサ間とのみ毎時キャリブレーションを行うため、比較的信頼性の高い距離情報を用いることができる上、全計算量を抑えるといった効果も得ることができ、非常に汎用性の高い移動体位置推定システムであるといえる。
【００１５】
以上のように、本発明によれば、設置センサの位置情報を自動でキャリブレーションするだけでなく移動体のトラッキングも同時に行うことが可能であり、移動体の位置を容易にかつ高精度に推定することのできる移動体位置推定方法及び移動体位置推定装置を提供することができる。
【発明を実施するための最良の形態】
【００１６】
以下、図面を参照して本発明の実施の形態を詳細に説明する。
図１は本発明に係る移動体位置推定方法を利用したシステムの概要を説明するための概念図である。図１において、計測領域内には距離センサを搭載した送信機Ｔ０（ＩＤ：０），Ｔ１（ＩＤ：１），Ｔ２（ＩＤ：２），Ｔ３（ＩＤ：３），…，Ｔｎ（ＩＤ：ｎ）が予め任意の地点（相対位置座標（ｘｉ，ｙｉ）：ｉは０〜ｎ）に分散設置される。上記計測領域内を移動する移動体Ｍには受信機Ｒが装着される。
【００１７】
各送信機Ｔ０〜Ｔｎは、それぞれ移動体との距離を計測する距離センサ（図示せず）を備え、距離情報として送信ＩＤ、計測時刻、計測距離を定期的に送信する。送信機Ｔ０〜Ｔｎが扱う信号は電波、音波を問わず、距離変換可能なものであればよい。
上記計測領域内を移動する移動体Ｍには受信機Ｒが装着される。この受信機Ｒは各送信機Ｔ０〜Ｔｎから送信される距離情報を選択的に取得して位置推定算出装置Ｐに送る。この位置推定算出装置Ｐは、例えば汎用コンピュータによって実現されるもので、図２に示すように、受信機Ｒで受信された各送信機Ｔ０〜Ｔｎからの位置情報を取り込むインターフェース１１と、予め解析された位置推定アルゴリズムに基づいて移動体Ｍの位置推定処理を実行するためのプログラムを格納するＲＯＭ（Read Only Memory）１２、ＲＯＭ１２に格納されるプログラムに従って受信機Ｒにて得られる距離情報から移動体Ｍの位置を推定するＣＰＵ（Central Processing Unit）１３、ＣＰＵ１３の作業領域として機能し、推定結果を格納するＲＡＭ（Random Access Memory）１４、ＣＰＵ１３に対するプログラムの実行指示やデータの入力を行う入力装置１５、ＣＰＵ１３の推定処理状況をモニタするモニタ装置１６を備える。
【００１８】
上記構成において、図３を参照して本発明に係る移動体位置推定システムの全体のアルゴリズムについて説明する。
まず、移動体Ｍが所持する受信機Ｒと距離が近い３つの送信機Ｔを選定し（Ｓ１）、３つの送信機Ｔとの自動キャリブレーション処理を実行する（Ｓ２）。ここで、キャリブレーションによって得られた各送信機Ｔで求まる距離推定値の評価を行い、所定の条件を満足するか否かを判断して、全ての距離推定値が所定の条件を満足するまでステップＳ１，Ｓ２を繰り返し実行する（Ｓ３）。次に、任意の送信機の位置を基準として各送信機の位置座標を変換し（Ｓ４）、再度所定の条件に基づく推定値の評価を行い、全ての距離推定値が所定の条件を満足するまでステップＳ１〜Ｓ４を繰り返し実行する（Ｓ５）。続いて位置推定領域を導出することで（Ｓ６）、送信機キャリブレーション処理を完了する。
【００１９】
キャリブレーション処理後は、受信機のトラッキングを行い（Ｓ７）、再び所定の条件に基づく推定値の評価を行い、全ての距離推定値が所定の条件を満足するまでステップＳ１〜Ｓ７を繰り返し実行する（Ｓ５）。条件を満足する場合には、ステップＳ７に戻り、受信機のトラッキング処理を継続する。
【００２０】
ここで、上記ステップＳ２の３つの送信機との自動キャリブレーション処理の概要を図４に示して説明する。
図４において、選定された３つの送信機から移動体側の受信機との間で行われた距離計測結果が得られると（Ｓ２１）、各送信機及び受信機それぞれの位置推定処理を行う（Ｓ２２）。この位置推定処理は所定の条件に基づく推定値の評価結果が規定値以上になるまで繰り返し処理される（Ｓ２３）。各送受信機間の位置推定が完了した場合には、推定誤差を低減する処理を行い（Ｓ２４）、最終的に位置推定領域を導出する（Ｓ２５）。
【００２１】
上記ステップＳ２２の位置推定では、式（１）に示す条件式を解析的に解くことによって送受信機それぞれの位置推定を行う。
【００２２】
【数１】
上記条件式の一例としては、過去から現在までに得た計測距離に起因する送受信機間の幾何学的関係を表す条件式Ａ部と、送信機の位置不変に起因する条件式Ｂ部からなる。図５に上記送受信機間の幾何学的関係を示す。図５において、送信機Ｔ０（ＩＤ：０），Ｔ１（ＩＤ：１），Ｔ２（ＩＤ：２）に対し、時刻ｔ−２，ｔ−１，ｔの順に受信機Ｒが装着された移動体Ｍの位置が移動したとする。ここで、時刻ｔにおけるＩＤ：ｉの送信機Ｔｉと移動体Ｍとの間の距離をｄi,tとし、ＩＤ：ｉの送信機Ｔｉの位置座標を（ｘｉ，ｙｉ）、時刻ｔにおける移動体の位置座標を（ｘh,t，ｙh,t）とすると、時刻ｔ−２における移動体Ｍの送信機Ｔ０〜Ｔ２それぞれからの距離はｄ0,t-2，ｄ1,t-2，ｄ2,t-2となり、時刻ｔ−１における移動体Ｍの送信機Ｔ０〜Ｔ２それぞれからの距離はｄ0,t-1，ｄ1,t-1，ｄ2,t-1となり、時刻ｔにおける移動体Ｍの送信機Ｔ０〜Ｔ２それぞれからの距離はｄ0,t，ｄ1,t，ｄ2,tとなる。
【００２３】
この場合、以下に説明する３辺キャリブレーションの原理に基づいて処理される。すなわち、３個の位置固定された送信機が用意されたとすると、単純には変数が６個（各送信機のｘ，ｙ座標）となる。続いて、移動体の位置として変数が２個（受信機のｘ，ｙ座標）追加される。この場合、位置と距離の関係式が３つ組みあがるため、３辺の距離を測定することができる。単純には、６つの位置で測定すれば変数が１８個となり、関係式も１８個となって、連立方程式を解くことで、３つの送信機と移動体の各時点の位置を算出することができる。
【００２４】
ただし、互いに相対的な位置を求めることなので、上記位置推定処理（Ｓ２２）において、図５に示すように、局所座標系の原点（０，０）と軸方向（Ｘ軸）を定め、さらに、二つの送信機を局所座標系のＸ軸上に配置することで、３つの送信機の位置を表す変数を３個に軽減することができる。したがって、３つの位置での距離を測定すれば、送信機と移動体の位置を求めることができる（２辺キャリブレーションの原理）。すなわち、３個の位置固定された送信機が用意され、２つの送信機の位置が既知であるとする。この場合、送信機の位置を表す変数が２個（位置が不明な送信機のｘ，ｙ座標）となり、移動体の位置として変数が２個（受信機のｘ，ｙ座標）追加される。この場合も、位置と距離の関係式が３つ組みあがるため、３パスの距離を測定することができる。２つの位置で測定すれば、変数が６個。関係式も６個となって、連立方程式を解くことができる。
【００２５】
尚、推定誤差低減処理（Ｓ２４）において、位置推定値をカルマンフィルタのようなものでフィルタ処理を行うことにより、推定精度を向上できるのは言うまでもない。以上のアルゴリズムにより、３つの送信機によって形成される三角形が送信機の数に応じて作られる。
【００２６】
次に、上記で形成される各三角形の連結を利用した全送信機の絶対座標系記述方式について述べる。処理概要を図６に示す。図６は、図３に示したフローチャートの送受信機キャリブレーション処理部分を具体的に示したフローチャートである。まず、ステップＳ１で３つの送信機を選定する際、その組み合わせによって作成された各三角形に順次ＩＤを割り当てる（Ｔ０，Ｔ１，…，Ｔｎ）。続いて、ステップＳ２において、Ｔ０の初回キャリブレーション時は全辺のキャリブレーションを行うが、以降は移動前後の隣接辺の長さを固定した状態で残り２辺のキャリブレーションを行う。例えば、図７に示すように、Ｔ０（送信機Ｔ０，Ｔ１，Ｔ２による三角形）からＴ１（送信機Ｔ１，Ｔ２，Ｔ４による三角形）に移動した場合、隣接辺である送信機Ｔ１，Ｔ２間の距離を固定して送信機Ｔ４の位置推定を行う。
【００２７】
以上を繰り返すことにより、全送信機の位置推定の精度を向上させることができ、結果として全送信機間の距離を算出することができる。最後に、ある送信機を基準（原点）として各三角形を連結させれば、全送信機の絶対座標系記述が可能となる。但し、各三角形連結時に、図８のような鏡像問題が生じるが、送信機Ｔ２，Ｔ４間の推定距離を用いることで回避できる。
【００２８】
各送信機の位置が定まった後は、受信機のトラッキング専用システムとして機能する。トラッキング時に不具合が生じた場合は、送受信機キャリブレーション処理に戻り、再度図３に示す各送受信機のキャリブレーションを行う。以上により、送信機（センサ）と受信機（移動体）のキャリブレーションならびに受信機（移動体）トラッキングが可能となる。
【００２９】
なお、本発明は、上記実施形態例そのままに限定されるものではなく、実施段階ではその要旨を逸脱しない範囲で構成要素を変形して具体化できる。また、上記実施形態例に開示されている複数の構成要素の適宜な組合せにより種種の発明を形成できる。例えば、実施形態例に示される全構成要素からいくつかの構成要素を削除しても良い。更に、異なる実施形態例に亘る構成要素を適宜組み合わせても良い。
【図面の簡単な説明】
【００３０】
【図１】本発明に係る移動体位置推定方法を利用したシステムの一実施形態の概要を説明するための概念図。
【図２】図１に示すシステムに用いられる位置推定算出装置の具体的な構成を示す概略構成図。
【図３】本発明に係る移動体位置推定システムの全体のアルゴリズムを示すフローチャート。
【図４】図１に示すシステムにおいて、３つの送信機との自動キャリブレーション処理の概要を示すフローチャート。
【図５】図１に示すシステムにおいて、各計測時の幾何学的関係を示す概念図。
【図６】図３に示す送受信機キャリブレーション処理の概要を示すフローチャート。
【図７】本実施形態において、選定三角形の移動例を示す図。
【図８】本実施形態において、三角形連結時に問題となる鏡像の一例を示す図。
【符号の説明】
【００３１】
Ｔ０〜Ｔｎ…送信機、Ｍ…移動体、Ｒ…受信機、Ｐ…位置推定算出装置、１１…位置情報インターフェースと、１２…ＲＯＭ（Read Only Memory）、１３…ＣＰＵ（Central Processing Unit）、１４…ＲＡＭ（Random Access Memory）、１５…入力装置、１６…モニタ装置。
【特許請求の範囲】
【請求項１】
測定領域内を移動する移動体の位置を推定する移動体位置推定方法において、
前記測定領域内の３箇所以上の測定地点から前記移動体までの距離を互いに時刻同期した状態で測定し、
前記３箇所以上の測定地点から前記移動体までの任意の測定時刻の距離データを取得し、
前記取得された任意の測定時刻の距離データから個々の測定地点及び前記移動体それぞれの位置を推定するものであり、
前記移動体の移動前後の位置それぞれの前記３箇所以上の測定地点から得られる距離データを用いて各測定地点の位置のキャリブレーション及び前記移動体の移動位置の推定を同時に行うことを特徴とする移動体位置推定方法。
【請求項２】
前記推定は、前記測定地点の位置が固定であることに起因する条件式、個々の計測距離に起因する拘束条件を踏まえて推定を行うことを特徴とする請求項１記載の移動体位置推定方法。
【請求項３】
前記測定地点が４箇所以上のとき、
前記推定は、前記移動体との距離が近い順に３つの測定地点を特定してそれぞれの位置のキャリブレーションを行い、前記移動体の移動に伴って順次特定される３つの測定地点により順次形成される三角形を最終的に連結させていくことで、全距離センサと移動体との位置を絶対座標系に示すことを特徴とする請求項１記載の位置推定算出方法。
【請求項４】
前記推定は、前記３個以上の測定地点のいずれかを基準位置とし、この基準位置を通る直線を座標軸として、各測定地点の位置を相対的に推定することを特徴とする請求項１記載の位置推定算出方法。
【請求項５】
測定領域内を移動する移動体の位置を推定する移動体位置推定システムにおいて、
前記測定領域内に配置され、互いに時刻同期した状態で前記移動体までの距離を測定する３個以上の距離センサと、
前記３個以上の距離センサから前記移動体までの任意の測定時刻の距離データを取得する距離データ取得手段と、
前記距離データ取得手段で取得された任意の測定時刻の距離データから個々の距離センサ及び前記移動体それぞれの位置を推定する推定手段とを具備し、
前記推定手段は、前記移動体の移動前後の位置それぞれの前記３個以上の距離センサから得られる距離データを用いて各距離センサの位置のキャリブレーション及び前記移動体の移動位置の推定を同時に行うことを特徴とする移動体位置推定システム。
【請求項６】
前記推定手段は、前記距離センサの位置が固定であることに起因する条件式、個々の計測距離に起因する拘束条件を踏まえて推定を行うことを特徴とする請求項５記載の移動体位置推定システム。
【請求項７】
前記距離センサが４個以上のとき、
前記推定手段は、前記移動体との距離が近い順に３つの距離センサを特定してそれぞれの位置のキャリブレーションを行い、前記移動体の移動に伴って順次特定される３つの距離センサにより順次形成される三角形を最終的に連結させていくことで、全距離センサと移動体との位置を絶対座標系に示すことを特徴とする請求項５記載の位置推定算出システム。
【請求項８】
前記推定手段は、前記３個以上の距離センサのいずれかを基準位置とし、この基準位置を通る直線を座標軸として、各距離センサの位置を相対的に推定することを特徴とする請求項５記載の位置推定算出システム。
【請求項１】
測定領域内を移動する移動体の位置を推定する移動体位置推定方法において、
前記測定領域内の３箇所以上の測定地点から前記移動体までの距離を互いに時刻同期した状態で測定し、
前記３箇所以上の測定地点から前記移動体までの任意の測定時刻の距離データを取得し、
前記取得された任意の測定時刻の距離データから個々の測定地点及び前記移動体それぞれの位置を推定するものであり、
前記移動体の移動前後の位置それぞれの前記３箇所以上の測定地点から得られる距離データを用いて各測定地点の位置のキャリブレーション及び前記移動体の移動位置の推定を同時に行うことを特徴とする移動体位置推定方法。
【請求項２】
前記推定は、前記測定地点の位置が固定であることに起因する条件式、個々の計測距離に起因する拘束条件を踏まえて推定を行うことを特徴とする請求項１記載の移動体位置推定方法。
【請求項３】
前記測定地点が４箇所以上のとき、
前記推定は、前記移動体との距離が近い順に３つの測定地点を特定してそれぞれの位置のキャリブレーションを行い、前記移動体の移動に伴って順次特定される３つの測定地点により順次形成される三角形を最終的に連結させていくことで、全距離センサと移動体との位置を絶対座標系に示すことを特徴とする請求項１記載の位置推定算出方法。
【請求項４】
前記推定は、前記３個以上の測定地点のいずれかを基準位置とし、この基準位置を通る直線を座標軸として、各測定地点の位置を相対的に推定することを特徴とする請求項１記載の位置推定算出方法。
【請求項５】
測定領域内を移動する移動体の位置を推定する移動体位置推定システムにおいて、
前記測定領域内に配置され、互いに時刻同期した状態で前記移動体までの距離を測定する３個以上の距離センサと、
前記３個以上の距離センサから前記移動体までの任意の測定時刻の距離データを取得する距離データ取得手段と、
前記距離データ取得手段で取得された任意の測定時刻の距離データから個々の距離センサ及び前記移動体それぞれの位置を推定する推定手段とを具備し、
前記推定手段は、前記移動体の移動前後の位置それぞれの前記３個以上の距離センサから得られる距離データを用いて各距離センサの位置のキャリブレーション及び前記移動体の移動位置の推定を同時に行うことを特徴とする移動体位置推定システム。
【請求項６】
前記推定手段は、前記距離センサの位置が固定であることに起因する条件式、個々の計測距離に起因する拘束条件を踏まえて推定を行うことを特徴とする請求項５記載の移動体位置推定システム。
【請求項７】
前記距離センサが４個以上のとき、
前記推定手段は、前記移動体との距離が近い順に３つの距離センサを特定してそれぞれの位置のキャリブレーションを行い、前記移動体の移動に伴って順次特定される３つの距離センサにより順次形成される三角形を最終的に連結させていくことで、全距離センサと移動体との位置を絶対座標系に示すことを特徴とする請求項５記載の位置推定算出システム。
【請求項８】
前記推定手段は、前記３個以上の距離センサのいずれかを基準位置とし、この基準位置を通る直線を座標軸として、各距離センサの位置を相対的に推定することを特徴とする請求項５記載の位置推定算出システム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【公開番号】特開２００９−２８１９９４（Ｐ２００９−２８１９９４Ａ）
【公開日】平成２１年１２月３日（２００９．１２．３）
【国際特許分類】
【出願番号】特願２００８−１３７２８７（Ｐ２００８−１３７２８７）
【出願日】平成２０年５月２６日（２００８．５．２６）
【出願人】（０００００４２２６）日本電信電話株式会社 (13,992)
【Ｆターム（参考）】
【公開日】平成２１年１２月３日（２００９．１２．３）
【国際特許分類】
【出願日】平成２０年５月２６日（２００８．５．２６）
【出願人】（０００００４２２６）日本電信電話株式会社 (13,992)
【Ｆターム（参考）】
[ Back to top ]