JP2012198066A - 位置検出装置、ウェアラブルセンサシステム及びプログラム - Google Patents

Abstract

【課題】簡単、安価な構成で高精度に位置検出を行う。
【解決手段】３個の受信機までの３つの距離をベクトルとしたデータベースが予め登録された記憶部と、各受信機が出力する送信機と各受信機との間の距離に依存する距離情報に基づいてデータベースを探索して初期位置を用いて連立非線形方程式を解いて移動部分の位置を検出する検知処理部は、距離情報をフィルタリングしてデータベースの探索用距離情報を取得する第１のカルマンフィルタと、動的に調整されて移動部分の位置状態を推定する位置状態推定用の第２のカルマンフィルタを含むように構成する。
【選択図】図１

Description

本発明は、位置検出装置、ウェアラブルセンサシステム（Wearable Sensor System）及びプログラムに関する。

ユーザの手先に装着した発信部を含むウェアラブルセンサシステムを利用してユーザの手先の動きを認識することで、ライフログの記録、キーボード等を用いることなく手先の動きで情報を入力するユーザインタフェース（ＵＩ：User Interface)、手作業の手順確認等の処理を行うことが可能である。ウェアラブルセンサシステムでは、例えば発信部をユーザの手先に装着し、発信部からの信号を例えばユーザの胴体に装着した複数の受信部で受信した結果に基づいて認識部が手先の行動を認識する。例えば、発信部は超音波発信機で形成され、受信部は超音波センサで形成される。超音波発信機は、例えばユーザの手首に装着し、超音波発信機から発信された超音波が各超音波センサに到着する時間を同期して計算することで、超音波発信機、即ち、ユーザの手首と各超音波センサとの相対位置関係を認識することができる。従って、この相対位置関係からユーザの手首の動きを認識することができる。

上記の相対位置関係の認識精度を向上するには、超音波センサの数を増やせば良い。又、超音波発信機と各超音波センサとの相対位置は、最小二乗法等に代表される近似解を求める方法により求めることができる。最小二乗法により位置の近似解を求める方法は、例えば特許文献１等にて提案されている。しかし、超音波センサの数が比較的大きくなると、その分ウェアラブルセンサシステムの構成が複雑になると共に、コストも増加してしまう。又、超音波センサの数が比較的大きくなると、受信部側での信号処理が複雑になってしまう。このように、従来のウェアラブルセンサシステムでは、位置認識精度を向上するにはセンサの数を増加させる必要があるものの、センサの数を増加させると構成及び信号処理が複雑になると共に、コストが増加してしまう。

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

従来のウェアラブルセンサシステムでは、比較的簡単、且つ、安価な構成で比較的高精度に位置検出を行うことは難しいという問題があった。

そこで、本発明は、比較的簡単、且つ、安価な構成で比較的高精度に位置検出を行うことが可能な位置検出装置、ウェアラブルセンサシステム及びプログラムを提供することを目的とする。

本発明の一観点によれば、位置検出対象の移動部分に装着された送信機から発信された信号を前記位置検出対象の参照部分に装着された３個の受信機で受信して前記移動部分の位置を検出する位置検出装置であって、前記移動部分の３次元空間の移動範囲を固定サイズのグリッドで分割し、グリッド中心の３次元座標とグリッド中心から前記３個の受信機までの３つの距離をベクトルとしたＫＤ木のデータベースが予め登録された記憶部と、前記３個の受信機が出力する、前記送信機と各受信機との間の距離に依存する距離情報に基づいて前記記憶部内の前記データベースを探索して反復法ソルバの初期位置を決定し、前記初期位置を用いて連立非線形方程式を解いて前記移動部分の位置を検出するプロセッサを備え、前記プロセッサは、前記距離情報をフィルタリングして前記データベースの探索用距離情報を取得する第１のカルマンフィルタと、前記探索用距離情報を用いて前記データベースから最短隣グリッドを最短隣検索アルゴリズムに基づいて特定し、最短隣グリッドの中心位置を前記反復法ソルバの初期位置として代入して前記連立非線形方程式を解き、前記反復法ソルバが収束するか否かを判定する判定手段と、前記反復法ソルバが収束する場合は、前記第１のカルマンフィルタの推定状態のモデルのノイズレベルに応じて動的に調整されて前記移動部分の位置状態を推定する位置状態推定用の第２のカルマンフィルタを有することを特徴とする位置検出装置が提供される。

本発明の一観点によれば、位置検出対象の移動部分に装着される送信機と、前記位置検出対象の参照部分に装着され、前記送信機から発信された信号を受信する３個の受信機と、前記移動部分の３次元空間の移動範囲を固定サイズのグリッドで分割し、グリッド中心の３次元座標とグリッド中心から前記３個の受信機までの３つの距離をベクトルとしたＫＤ木のデータベースが予め登録された記憶部と、前記３個の受信機が出力する、前記送信機と各受信機との間の距離に依存する距離情報に基づいて前記記憶部内の前記データベースを探索して反復法ソルバの初期位置を決定し、前記初期位置を用いて連立非線形方程式を解いて前記移動部分の位置を検出する検知処理部を備え、前記検知処理部は、前記距離情報をフィルタリングして前記データベースの探索用距離情報を取得する第１のカルマンフィルタと、前記探索用距離情報を用いて前記データベースから最短隣グリッドを最短隣検索アルゴリズムに基づいて特定し、最短隣グリッドの中心位置を前記反復法ソルバの初期位置として代入して前記連立非線形方程式を解き、前記反復法ソルバが収束するか否かを判定する判定手段と、前記反復法ソルバが収束する場合は、前記第１のカルマンフィルタの推定状態のモデルのノイズレベルに応じて動的に調整されて前記移動部分の位置状態を推定する位置状態推定用の第２のカルマンフィルタを有することを特徴とするウェアラブルセンサシステムが提供される。

本発明の一観点によれば、コンピュータに、位置検出対象の移動部分の位置を検出する位置検出処理を実行させるプログラムであって、前記移動部分の３次元空間の移動範囲を固定サイズのグリッドで分割し、グリッド中心の３次元座標とグリッド中心から、前記位置検出対象の参照部分に装着された３個の受信機までの３つの距離をベクトルとしたＫＤ木のデータベースが予め登録された記憶部を用い、前記移動部分に装着された送信機から発信された信号を受信する前記３個の受信機が出力する、前記送信機と各受信機との間の距離に依存する距離情報に基づいて前記記憶部内の前記データベースを探索して反復法ソルバの初期位置を決定し、前記初期位置を用いて連立非線形方程式を解いて前記移動部分の位置を検出する検出手順を前記コンピュータに実行させ、前記検出手順は、前記距離情報を第１のカルマンフィルタによりフィルタリングして前記データベースの探索用距離情報を取得する第１のフィルタリング手順と、前記探索用距離情報を用いて前記データベースから最短隣グリッドを最短隣検索アルゴリズムに基づいて特定し、最短隣グリッドの中心位置を前記反復法ソルバの初期位置として代入して前記連立非線形方程式を解き、前記反復法ソルバが収束するか否かを判定する判定手順と、前記反復法ソルバが収束する場合は、前記第１のカルマンフィルタの推定状態のモデルのノイズレベルに応じて動的に調整されて前記移動部分の位置状態を推定する位置状態推定用の第２のカルマンフィルタによるフィルタリングを行う第２のフィルタリング手順を含むことを特徴とするプログラムが提供される。

開示の位置検出装置、ウェアラブルセンサシステム及びプログラムによれば、比較的簡単、且つ、安価な構成で比較的高精度に位置検出を行うことができる。

本発明の第１実施例における位置検出装置の一例を示すブロック図である。 ウェアラブルシステムの一例を示す図である。 グリッド分割及び情報登録処理を説明するフローチャートである。 位置検知における座標の定義を説明する図である。 移動空間のグリッド化を説明する図である。 KD-Treeへ登録するデータを説明する図である。 位置推定処理を説明するフローチャートである。 隣接グリッドを用いて近似解を特定する方法を説明する図である。 本発明の第２実施例における位置検出装置の一例を示すブロック図である。

開示の位置検出装置、ウェアラブルセンサシステム及びプログラムでは、位置検出対象の移動部分に装着された送信機から発信された信号を位置検出対象の参照部分に装着された３個の受信機で受信して前記移動部分の位置を検出する。位置検出装置は、前記移動部分の３次元空間の移動範囲を固定サイズのグリッドで分割し、グリッド中心の３次元座標とグリッド中心から３個の受信機までの３つの距離をベクトルとしたＫＤ木のデータベースが予め登録された記憶部と、３個の受信機が出力する送信機と各受信機との間の距離に依存する距離情報に基づいてデータベースを探索して反復法ソルバの初期位置を決定し、初期位置を用いて連立非線形方程式を解いて移動部分の位置を検出する制御装置を備える。

制御装置は、距離情報をフィルタリングしてデータベースの探索用距離情報を取得する第１のカルマンフィルタと、探索用距離情報を用いてデータベースから最短隣グリッドを最短隣検索アルゴリズムに基づいて特定し、最短隣グリッドの中心位置を反復法ソルバの初期位置として代入して前記連立非線形方程式を解き、前記反復法ソルバが収束するか否かを判定する判定部と、反復法ソルバが収束する場合は第１のカルマンフィルタの推定状態のモデルのノイズレベルに応じて動的に調整されて移動部分の位置状態を推定する位置状態推定用の第２のカルマンフィルタを有する。

全てのグリッドで解が得られない場合は、第２のカルマンフィルタの予測値とグリッド探索の全ての最終点の平均を計算し、第１及び第２のカルマンフィルタの観測及び予測の共分散情報に基づいて第１及び第２のカルマンフィルタによる２つの位置推定を再度重み付けで平均化するようにしても良い。

以下に、開示の位置検出装置、ウェアラブルセンサシステム及びプログラムの各実施例を図面と共に説明する。

（第１実施例）
図１は、本発明の第１実施例における位置検出装置の一例を示すブロック図である。本実施例では、位置検出装置はウェアラブルセンサシステムの一部を形成する。図１に示すウェアラブルセンサシステムは、ウェアラブルシステム１、通信部２、及び制御装置３を有する。

ウェアラブルシステム１は、データ採取部の一例であり、超音波発信部１１、同期信号及びクロック生成部１２、及び超音波受信部１３を有する。超音波発信部１１は発信機の一例であり、超音波受信部１３はセンサ（又は、センシングデバイス）等の受信機の一例である。超音波発信部１１から超音波を発信すると、発信タイミングが同期信号及びクロック生成部１２に供給されて発信タイミングに基づいた同期信号が生成される。又、同期信号及びクロック生成部１２は、クロックを生成する。超音波発信部１１から発信された超音波は、同期信号及びクロック生成部１２からの同期信号に同期して同期信号及びクロック生成部１２からのクロックで決定されるタイミングで超音波受信部１３により受信される。超音波受信部１３は、３個の超音波センサを有し、各超音波センサが同期信号に同期してクロックに基づいて超音波を受信することで、各超音波センサが超音波発信部１１からの距離に応じたタイミングで超音波を受信する。従って、超音波受信部１３からは、各超音波センサの超音波検出出力に基づいて、各超音波センサの超音波発信部１１からの距離に依存する距離情報（又は、距離信号）が出力される。

制御装置３は、例えばパーソナルコンピュータ（ＰＣ：Personal Computer）等の周知の汎用コンピュータ、或いは、スマートフォン（Smartphone）、ＰＤＡ（Personal Digital Assistant）等の携帯端末で形成可能である。この例では、制御装置３はデータベース（ＤＢ：Data-Base）作成部３１、外部ストレージ（ＤＢ）３２、及び検知処理部３３を有する。検知処理部３３は、プロセッサの一例であるＣＰＵ（Central Processing Unit）３３１、記憶部の一例であるメモリ３３２、及び通信部３３３を有する。ＣＰＵ３３１は、メモリ３３２に格納されたプログラムを実行することにより制御装置３全体を制御する。メモリ３３２は、半導体記憶装置、磁気記録媒体、光記録媒体、光磁気記録媒体等で形成可能であり、上記のプログラムや各種データを格納すると共に、ＣＰＵ３３１が実行する演算の中間結果や演算結果等を一時的に格納する一時メモリとしても機能する。

ＣＰＵ３３１は、メモリ３３２に格納されたプログラムを実行することにより、制御装置（コンピュータ）３を位置検出装置として機能させる。プログラムは、ＣＰＵ３３１に通信部２から受信した距離情報に基づいて少なくとも図４と共に後述する位置検出処理の手順を実行させるものであり、メモリ３３２を含む適切なコンピュータ読み取り可能な記憶媒体に格納されていても良い。

ＤＢ作成部３１は、図３と共に後述するグリッド分割及び情報登録処理を実行し、後述する位置推定処理に必要なＫＤ木（KD-Tree）のデータベース（以下、単にKD-Tree DBとも言う）を作成し、外部ストレージ３２に格納する。後述するように、検知処理部３３による位置検出処理は、外部ストレージ３２に格納されたKD-Tree DBを用いて実行される。ＤＢ作成部３１は、例えばＣＰＵにより形成可能であるが、検知処理部３３内のＣＰＵ３３１がＤＢ作成部３１としても機能可能な場合には、ＤＢ作成部３１は省略可能である。

通信部２は、超音波受信部１３からの距離情報を例えばケーブルを介して受信する。又、通信部２は、受信した距離情報を、無線ネットワークを介して制御装置３に送信する。この例では、通信部２と制御装置３との間の通信は、検知処理部３３内の通信部３３３を介して行われる。尚、リアルタイムで位置検出処理を行う必要がない場合には、通信部２はＵＳＢ（Universal Serial Bus）等の接続部（或いは、接続手段）を介して制御装置３（即ち、検知処理部３３内の通信部３３３）に接続可能である。

尚、各超音波センサの超音波発信部１１からの距離に依存する距離情報は、超音波受信部１３内で各超音波センサの出力に基づいて算出しても、制御装置３の検知処理部３３において算出しても良い。検知処理部３３において距離情報を算出する場合は、超音波受信部１３は距離情報として超音波センサの出力をそのまま出力可能である。この場合も、超音波センサの出力（即ち、距離情報）が各超音波センサの超音波発信部１１からの距離に依存することには変わりない。

図２は、ウェアラブルシステムの一例を示す図である。図２に示すように、ウェアラブルシステム１の超音波発信部１１は、第１の装着部１１０を有する。第１の装着部１１０は、超音波発信機１１１と、同期信号及びクロック生成部１２、オン／オフスイッチ（図示せず）等を含む回路部分１１２を有する。第１の装着部１１０は、ユーザの手首への装着を容易にするため、例えば腕時計の如くベルトで装着可能は構造を有する。ユーザの手首は、位置検出対象の移動部分の一例である。

一方、ウェアラブルシステム１の超音波受信部１３は、第２の装着部１３０を有する。第２の装着部１３０は、略Ｖ字形状を有し、例えば互いに等距離の位置に設けられた３個の超音波センサ１３１，１３２，１３３と、紐又はチェーン１３４と、回路部分１３５を有する。第２の装着部１３０は、ユーザへの装着を容易にするため、例えばユーザがネックレスのように首からかけられる構造を有し、装着状態ではユーザの胴体（即ち、胸）の部分に配置される。尚、紐又はチェーン１３４は、第２の装着部１３０をユーザの胴体に装着可能なベルトの如き構造を有しても良いことは言うまでもない。ユーザの胴体は、位置検出対象の参照点を含む参照部分の一例である。回路部分１３５は、各超音波センサ１３１，１３２，１３３の超音波検出出力をそのまま距離情報として出力しても、各超音波センサ１３１，１３２，１３３の超音波検出出力に基づいて各超音波センサ１３１，１３２，１３３の超音波発信機１１１からの距離を示す距離情報を算出して出力する回路を含んでも良い。又、回路部分１３５は、通信部２を含んでも良い。

同期信号及びクロック生成部１２は、第１の装着部１１０の超音波発信機１１１と第２の装着部１３０の各超音波センサ１３１，１３２，１３３とを接続部（或いは、接続手段）を介して接続される。接続部は、有線又は無線で第１及び第２の装着部１１０，１３０を接続する。接続部が第１及び第２の装着部１１０，１３０間を無線接続する場合、。又、この例では、説明の便宜上、通信部２が第２の装着部１３０に設けられているものとするが、通信部２は例えばケーブル（図示せず）を介して第２の装着部１３０に接続されていても良いことは言うまでもない。

ウェアラブルセンサシステムは、比較的簡単、且つ、安価な構成で比較的高精度に位置検出を行うことが望ましい。又、ウェアラブルセンサシステムにおいて、電力消耗、サイズ、重量、デザイン等の要因から、超音波センサ等のセンサの数は最小限に抑えたい。従って、図２に示すように３個の超音波センサ１３１〜１３３を使用する場合、超音波発信機１１１から各超音波センサ１３１〜１３３（即ち、各参照点）の位置までの３つの距離を超音波の到着時間より計測し、計測された距離から超音波発信機１１１の位置、即ち、第１の装着部１１０を装着したユーザの手首（又は、手先）の３次元位置を計測することが考えられる。この場合、次の連立方程式（１）を解くことになる。

しかし、次の理由で、上記連立方程式（１）を解くことは難しい。第１に、強い非線形特性を持つ解析解が存在しないからである。第２に、多変数反復ニュートンラフソン（Multi-variable Iteration Newton-Raphson）法を利用する際、検出対象となる実際の位置付近に初期探索始点を決定する必要があるものの、ユーザの手先の動きを検知するための探索を開始するべきスタート地点が予め分かっていない。第３に、特に超音波センサの場合は、温度、風速、音波の干渉等の影響により超音波センサの出力にノイズが混入し易いので、このノイズにより一般的な非線形方程式のソルバ（Solver）では単一解が得られない場合がある。

そこで、本実施例では、次のような手順ＳＴ１〜ＳＴ６を用いてユーザの手先の位置を推定する。

ＳＴ１： 距離観測用のカルマンフィルタ（Kalman Filter）ＫＦ１を用いて観測モデル（又は、誤差モデル）に基づいた観測距離のフィルタリング（Filtering）を行う。

ＳＴ２： 予めユーザの手先の移動範囲を固定サイズのグリッドで分割し、グリッド中心の３次元座標とグリッド中心から３個の超音波センサ１３１〜１３３までの３つの距離をベクトルとして、KD-treeに登録する。

ＳＴ３： ３つの距離の観測結果を取得する際、KD-treeの最短隣探索で最短隣グリッドを特定し、最短隣グリッドの中心の座標を探索始点とする。

ＳＴ４： 探索始点を初期解とし、Newton-Raphson法で上記連立方程式（１）を解き、解が得られた場合は、別の位置状態推定用の適応カルマンフィルタ（Adaptive Kalman Filter）ＫＦ２を用いて位置状態を推定する。この適応カルマンフィルタＫＦ２のモデルは、カルマンフィルタＫＦ１の推定状態のモデルのノイズレベルに応じて動的に調整される。

ＳＴ５： 手順ＳＴ３において解が得られない場合は、グリッドに隣接する例えば８つの隣接グリッドを特定し、隣接グリッドの中心座標から再度解を探す。

ＳＴ６： 全てのグリッドでの探索が成功しない（即ち、解が得られない）場合は、適応カルマンフィルタＫＦ２の予測値と、グリッド探索の全ての最終点の平均を計算し、カルマンフィルタＫＦ１，ＫＦ２の観測及び予測の共分散情報に基づいてカルマンフィルタＫＦ１，ＫＦ２による２つの位置推定を再度重み付けで平均化する。

これにより、ユーザの手先の移動範囲内において、常に３次元位置を解くための唯一の初期探索始点を特定することができる。又、超音波センサ１３１〜１３３の出力にノイズが混入していても、各方向から探索することで解を探すことが可能となり、解が収束しない場合であっても幾何平均と状態予測で近似解を得ることができる。更に、階層化されたフィルタによる統合で、適応的な状態推定を行うことができ、比較的簡単、且つ、安価な構成で位置測定の精度を向上することが可能となる。

本実施例では、ユーザの手先の位置（以下、単に手先位置とも言う）を高精度に推定するために、オフライン処理とオンライン処理の２つの処理を行う。

図３は、グリッド分割及び情報登録処理を説明するフローチャートであり、ＤＢ作成部３１が実行するオフライン処理の概要を示す。オフライン処理では、オンライン処理が行う位置推定処理に必要なKD-tree DBを作成する。

図３のオフライン処理を説明する前に、手先位置の推定について説明する。

図４は、位置検知における座標の定義を説明する図である。図４に示すように、３次元空間中の３つの超音波センサ１３１〜１３３の参照点RP1, RP2, RP3の座標を定義する。手先位置に装着される超音波発信機１１１の位置を、推定目標となる手先位置ｇ（x, y, z）と定義する。超音波発信機１１１からの超音波が超音波センサ１３１〜１３３に到着する時間に基づいて、超音波発信機１１１からの各超音波センサ１３１〜１３３までの距離を計算することができる。このため、ここでは説明の便宜上、超音波発信機１１１からの超音波が超音波センサ１３１〜１３３に到着する時間ではなく、超音波発信機１１１から各超音波センサ１３１〜１３３までの３つの距離を示す距離情報が観測結果として検知処理部３３に入力されるものとする。尚、図５中、破線は手先位置ｇ（x, y, z）の移動経路の一例を示す。

３つの参照点RP1, RP2, RP3の座標（x_i, y_i, z_i）と３つの距離情報より（i=1, 2, 3）、理論上次の連立非線形方程式（２）を反復多変数Newton-Raphson法で解くことができ、手先位置を決定することができる。

しかし、手先の動きを検知するための探索を開始するべきスタート地点が予め分かっていないため、反復法ソルバに必要な初期探索始点を特定できない。そこで、本実施例では、次のような方法で初期探索始点を特定する。

次に、反復法ソルバに必要な初期探索始点の特定方法について説明する。

先ず、３次元空間中の任意の場所にグローバル座標系を定義し、３つの参照点RP1, RP2, RP3の座標を定義する。更に、手先の移動空間をサイズＬのグリッドで分割する。分割した全てのグリッドの中心位置の座標Giを取得する。又、グリッドの中心位置の座標Giから３つの超音波センサ１３１〜１３３、即ち、３つの参照点RP1, RP2, RP3までの距離ri1, ri2, ri3を計算する。

図５は、移動空間のグリッド化を説明する図である。説明の便宜上、図５の上部に、分割したグリッドの一部と参照点RP1, RP2, RP3との距離ri1, ri2, ri3を示す。分割したグリッドｉの中心位置（Gxi, Gyi, Gzi）と、この中心位置（Gxi, Gyi, Gzi）から各参照点RP1, RP2, RP3までの距離情報(ri1, ri2, ri3)を利用して、次のような探索用特徴ベクトルViを作成する。ここで、Ｋは常数であり、例えば１以下である。

Vi = [ri1, ri2, ri3, Ｋ*Gxi, Ｋ*Gyi, Ｋ*Gzi]
上記の全てのグリッドで作成した探索用特徴ベクトルViをKD-treeへ登録し、KD-tree DBを作成する。KD-tree自体、及び、KD-treeによる最短隣探索自体は周知であり、その詳細な説明は省略する。

尚、グリッドのサイズＬの設定について、推定目標の近傍から手先の動きを検知するための探索を開始することが望ましいが、グリッドのサイズＬが小さすぎるとKD-Treeのサイズが膨大となり、最短隣探索に時間がかかってしまう。そこで、手先の移動範囲や観測結果に含まれるノイズ（以下、観測ノイズとも言う）の評価も考慮した場合、グリッドのサイズＬは例えば１０ｃｍ〜２０ｃｍに設定可能である。

KD-treeの最短隣探索アルゴリズムを利用して後述するオンライン処理で手先位置を検出する際、３つの距離情報(ri1, ri2, ri3)から手先が存在するグリッドの中心位置を特定することができる。グリッドの中心位置を特定してから、この中心位置の座標（Gix, Giy, Giz）を反復法ソルバの初期探索始点に設定することにより、反復法で解を探索することが可能になる。具体的には、上記KD-treeの探索によるグリッドの中心位置の特定は、最短隣探索用距離の定義に応じて例えば次の２つの方法のいずれかを採用することができる。

第１の方法は、標準ユークリッド距離を定義して用いる。多くのKD-treeの最短隣探索アルゴリズムは、次のD1の如きユークリッドノルム（Euclidean Norm）で探索を行う。

D1 = ||v1 - v2||₂
このような距離D1の定義で、多くのKD-treeのリソースをそのまま利用できると言うメリットがある。ただし、上記の位置推定処理において、後述するオンライン処理での計測情報は３つの距離情報(ri1, ri2, ri3)しかないため、直接KD-treeに登録した探索用特徴ベクトルViとの距離の計算はできない。そこで、直接KD-treeに登録した探索用特徴ベクトルViとの距離の計算ができるように、オンライン処理で３つの距離情報（ri1, ri2, ri3）に基づいて次のようなベクトルｗ_dを作成する。

ｗ_d = [ri1, ri2, ri3, 0, 0, 0]
このベクトルｗ_dでKD-tree DBから最短隣グリッドを探索する際、KD-tree DBの作成時のベクトルのパラメータＫを小さい常数（例えば、１×１０^−６）とする。つまり、最短隣探索のときの実際の距離riが探索の要素となるように、グリッドの中心位置に小さいパラメータＫを乗算し、距離情報に比べて非常に小さくする。この結果、本来であれば直接探索できないグリッドの中心座標を３つの距離情報(ri1, ri2, ri3)の探索を経由することにより間接的に取得することができる。

このような方法で、リアルタイムに取得した任意の３つの距離情報(ri1, ri2, ri3)から、予め用意したKD-tree DBから一番距離が短いグリッドを特定することができるので、特定したグリッドの中心位置に１／Ｋを乗算することで元の中心位置を求めてソルバの初期探索始点に設定する。

第２の方法は、カスタマイズした距離を定義して用いる。上記第１の方法による距離の定義は、汎用的な定義でリソースを流用する上でメリットがあるが、最短隣探索時の距離の計算には、省略可能な情報も含まれている。そこで、距離の計算の効率を更に向上するために、距離の定義を以下の如く変更する。

具体的には、KD-tree DBの登録用ベクトルの定義にあるパラメータＫを１に設定する。

Vi = [ri1, ri2, ri3, Gxi, Gyi, Gzi]
又、オンライン処理での距離の計測結果に基づいて次のように探索用特徴ベクトルViを定義する。

Vi = [rj1, rj2, rj3]
そして、最短隣探索時の距離D1を次のように定義する。

D1 = sqrt( (ri1-rj1)² + (ri2-rj2)²+ (ri3-rj3)² )
このような距離の定義で、取得しようとしているグリッドの中心位置をKD-tree DBに登録するが、最短隣探索に影響しないようにベクトルの距離計算を修正することにより、上記第１の方法と同様にソルバの初期探索始点を設定することができる。

図６は、KD-Treeへ登録するデータを説明する図である。図６では説明の便宜上、参照点座標がRP1(0,0,0), RP2 (0,0.2,0), RP3(0.2,0,0)、手先移動範囲がグリッドの中心から±0.4ｍ、グリッドサイズＬがＬ＝0.1ｍ、パラメータＫがＫ＝1.0の場合を示す。図６が示すKD-Treeへ登録したデータの一例には、手先位置g(x, y, z)と各参照点RP1, RP2, RP3との距離ri1, ri2, ri3、及び分割したグリッドｉの中心位置（Gxi, Gyi, Gzi）が含まれる。

上記のような探索用特徴ベクトルViを作成してKD-Treeへ登録することにより、オンライン処理では３次元観測がないため本来であれば直接探索できない３次元空間における位置を、一旦観測できる距離情報を経由して探索用特徴ベクトルViを求めることができる。このため、この探索用特徴ベクトルViに基づいて３次元空間における３次元位置情報の抽出及び復円を行うことで、３次元空間における位置の間接的な探索を実現できる。

図３のオフライン処理の説明に戻るに、ステップＳ１は、３次元空間中の任意の場所にグローバル座標系を定義し、３つの参照点RP1, RP2, RP3の座標を定義し、手先の移動空間をサイズＬのグリッドで分割する。ステップＳ２は、分割した全てのグリッドの中心位置の座標Giを取得する。ステップＳ３は、グリッドの中心位置の座標Giから３つの超音波センサ１３１〜１３３、即ち、３つの参照点RP1, RP2, RP3までの距離ri1, ri2, ri3を計算する。ステップＳ４は、分割したグリッドｉの中心位置（Gxi, Gyi, Gzi）と、この中心位置（Gxi, Gyi, Gzi）から各参照点RP1, RP2, RP3までの距離情報(ri1, ri2, ri3)を利用して、上記の如き探索用特徴ベクトルVi = [ri1, ri2, ri3, Ｋ*Gxi, Ｋ*Gyi, Ｋ*Gzi]を作成する。

ステップＳ５は、グリッドｉで作成した探索用特徴ベクトルViをKD-treeへ登録し、KD-tree DB６０１を外部ストレージ３２内に作成する。KD-tree自体、及び、KD-treeによる最短隣探索自体は周知であり、その詳細な説明は省略する。ステップＳ６は、全てのグリッドｉで作成した探索用特徴ベクトルViをKD-treeへ登録したか否かを判定し、判定結果がＮＯであると、処理はステップＳ７へ進む。ステップＳ７は、処理の対象を次に処理するべきグリッドに設定し、処理はステップＳ２へ戻る。ステップＳ６の判定結果がＹＥＳになると、オフライン処理は終了する。

図７は、位置推定処理を説明するフローチャートであり、検知処理部３３（即ち、ＣＰＵ３３１）が実行するオンライン処理の概要を示す。オンライン処理では、超音波センサ１３１〜１３３の出力に基づいて求めた、超音波発信機１１１から超音波センサ１３１〜１３３までの３つの距離情報と、KD-tree DBの最短隣探索から、非線形ソルバの初期位置を特定する。非線形ソルバが収束する場合の解と、収束しない場合の解を求める。更に精度良く手先位置を探索（又は、トラッキング）するために、階層化されたカルマンフィルタの適応調整を行い、調整後のカルマンフィルタの出力を最後の推定結果、即ち、最終の位置出力とする。

図７のオンライン処理を説明する前に、反復法ソルバによる手先位置の特定と、手先位置の推定のためのフィルタリングについて説明する。

反復法ソルバによる手先位置の特定は、以下のように行うことができる。先ず、上記の如く作成したKD-tree DBと３つの距離r1, r2, r3の観測結果より、オンライン処理で手先位置を特定する。具体的には、先ず、オンライン処理で観測した３つの距離情報r1, r2, r3を利用し、KD-tree DBからグリッドGiの中心位置の座標を探索し、探索した結果をX₀とする。上記X₀を反復法ソルバの初期位置として代入し、次のような方法で上記の連立非線形方程式を解く。このような方法で連立非線形方程式を解くこと自体は周知である。

上記反復探索を行う際、関数行列Fが閾値ε以下、又は、δｘがε以下なった場合は、反復探索が収束と判定する。ここで、εは予め設定したパラメータである。反復探索が収束した時点のx_newを、ソルバの新たな解として出力する。ただし、距離観測時のノイズ等の混入により、上記反復探索が収束しない場合もある。そこで、反復探索が収束しないときは、次のような方法で近似解を計算する。

非線形反復法ソルバが収束しない場合は、別の初期位置を用いて探索することになるが、本実施例では次のような規則に従って新探索位置を決定する。

先ず、上記反復法ソルバに必要な初期探索始点の特定方法で特定したグリッドをGiとし、このグリッドGiと隣接する隣接グリッドの中心位置を新探索位置に設定する。例えば、グリッドGiの面と接する６個のグリッドや、グリッドGiの中心から８個の頂点の延長線が通過する８個のグリッド等といった規則（又は、ポリシー）に従い、隣接グリッドを特定する。特定した隣接グリッドの中心位置から、上記反復法ソルバに必要な初期探索始点の特定方法で解を反復探索する。反復探索が収束しない場合は、最後の解が得られた探索位置を終了位置Siとしてメモリ３３２に記録し、次の隣接グリッドの中心位置から再度探索する。この再度の探索の結果、あるグリッドGjでの探索が収束した場合、その解を手先位置とする。全ての隣接グリッドの中心からの探索が収束しない場合は、各探索終了位置の平均位置で次式に従って多グリッド近似解Ｓを求める。

Ｓ = ΣSi／Ｎ
上記多グリッド近似解Ｓをそのまま使用することも可能であるが、位置検出精度を向上させるために、次のように、手先位置の位置状態推定用の適応カルマンフィルタＫＦ２の予測位置Ｚ_ｐと統合するようにしても良い。
Ｐ = (１−ｗ)*Ｓ＋ｗ*Ｚ_ｐ
ここで、Ｓは多グリッド近似解、Ｚ_ｐは位置状態推定用の適応カルマンフィルタＦＫ２の予測位置、Ｐは最後の解が得られた探索位置での位置出力である。重み係数ｗの範囲は例えば[0, 1]であり、位置状態推定用の適応カルマンフィルタＦＫ２は距離観測用のカルマンフィルタＫＦ１の観測予測の共分散と残差による計算を行う。

図８は、隣接グリッドを用いて近似解を特定する方法を説明する図である。説明の便宜上、図８の上部に、隣接グリッドGi, Gj, Gkを示す。又、手先の前回の位置を破線の丸印、手先の予測位置をハッチング付きの丸印、手先の実位置をクロスハッチングで示す。

位置状態推定用の適応カルマンフィルタＦＫ２によるフィルタリングは、以下のように行うことができる。上記の反復法ソルバによる手先位置の特定により得られた結果をそのまま手先位置として利用することは可能であるが、超音波センサ１３１〜１３３の観測ノイズの影響を抑えるために、カルマンフィルタを利用したフィルタリングを行う。更に、本実施例では、超音波センサ１３１〜１３３の距離観測用のカルマンフィルタＫＦ１と位置状態推定用の適応カルマンフィルタＫＦ２を含む階層化されたフィルタリングを行い、距離観測用のカルマンフィルタＫＦ１の内部状態に従って位置状態推定用の適応カルマンフィルタＫＦ２のモデルを適応的に変化させ、位置検出精度の向上を図る。尚、カルマンフィルタ自体、及び、適応カルマンフィルタ自体は周知であるため、その詳細な説明は省略する。

距離観測用のカルマンフィルタＫＦ１は、超音波センサ１３１〜１３３による観測距離rが定速度で変化していくという想定下でプロセスモデルを作成する。３つの観測が別々のフィルタとなる。各フィルタ用のプロセスモデルは、例えば以下の通りである。

距離観測用のカルマンフィルタＫＦ１の観測入力は、超音波センサ１３１〜１３３が出力する生の距離情報である。距離情報をフィルタリングした結果は、KD-tree DBの探索用距離情報riである。

一方、位置状態推定用の適応カルマンフィルタＫＦ２は、手先が定速度移動という想定下で例えば以下の如きプロセスモデルを作成する。勿論、定加速度モデル等を作成しても良い。

プロセスと観測モデルのノイズは、後述するパラメータＱ，Ｒで調整できる。位置状態推定用の適応カルマンフィルタＫＦ２の観測入力は、上記の如き反復法ソルバによる手先位置の特定で説明した、非線形ソルバが見つけた解（収束の場合）、或いは、多グリッドと予測位置の平均位置である。位置状態推定用の適応カルマンフィルタＫＦ２の出力は、位置推定処理による手先位置の最後の推定結果となる。

多グリッドと位置状態推定用の適応カルマンフィルタＫＦ２の予測位置の平均位置を計算するための重みｗは、以下のように計算可能である。先ず、位置状態推定の安定度に応じて重みｗを推定位置に割り当てる。位置状態推定用の適応カルマンフィルタＫＦ２が安定している状態で、推定位置の重みｗを大きくする。位置状態推定用の適応カルマンフィルタＫＦ２の安定性の確認は、例えば次のような予測共分散行列を用いて行うことができる。

上記予測共分散行列において、Ｐ(k+1 | k) が一定の閾値δより小さい場合は、位置状態推定用の適応カルマンフィルタＫＦ２の予測される安定性が高いと判別し、推定位置の重みｗを大きくする。

通常、プロセスノイズｗ(k)を示す共分散Ｑは固定であるが、手先の予測できない動きにより、定速度移動モデルとマッチングが取れない場合がある。特に超音波測位の場合は、手先が素早い、或いは、素早く複雑な動きをする際、反射や周り風速の影響で観測ノイズが発生しやすい。このような場合、位置状態推定用の適応カルマンフィルタＫＦ２のノイズモデルの適応調整において共分散Ｑを大きな値に調整することで、低速度移動モデルとのマッチングの誤差を抑えることが可能となる。このようにマッチングの誤差を抑えるために、上記の距離観測用のカルマンフィルタＫＦ１の観測予測の共分散と残差により、距離観測の安定度を先に評価する。

上記閾値γを利用し、不確定性の性能関数fを作成することができる。例えば、２つの閾値γ₁, γ₂で上記不確定の程度を３つのレベル（大、中、小）で定義することが可能である。尚、一定時間内の移動平均等、 閾値γの値を平均することも可能である。

図７のオンライン処理の説明に戻るに、ステップＳ１１は、超音波受信部１３の超音波センサ１３１〜１３３が出力する生の距離情報をウェアラブルシステム１から取得する。ステップＳ１２は、ステップＳ１１で取得した距離情報（即ち、観測入力）を距離観測用のカルマンフィルタＫＦ１でフィルタリングして、KD-tree DB６０１の探索用距離情報riを取得する。ステップＳ１３は、探索用距離情報riを用いてKD-tree DB６０１から最短隣グリッドGiを周知の最短隣検索アルゴリズムに基づいて特定する。ステップＳ１４は、最短隣グリッドの中心位置を反復法ソルバの初期位置として代入し、上記の如き連立非線形方程式を解き、反復法ソルバが収束するか否かを判定する。

ステップＳ１４の判定結果がＮＯであると、ステップＳ１５は、ステップＳ１３で特定したグリッドGiと隣接する隣接グリッド（上記の例では８つまでの隣接グリッド）を特定し、処理はステップＳ１６へ進む。ステップＳ１６は、隣接グリッドの中心位置を反復法ソルバの更新した初期位置として代入し、上記の如き連立非線形方程式を解き、反復法ソルバが収束するか否かを判定する。ステップＳ１６の判定結果がＮＯであると、ステップＳ１７は、最後の解が得られた探索位置を終了位置Siとしてメモリ３３２に記録し、処理はステップＳ１８へ進む。ステップＳ１８は、未処理の隣接グリッドがあるか否かを判定し、判定結果がＹＥＳであると処理はステップＳ１５へ戻り、判定結果がＮＯであると処理はステップＳ１９へ進む。ステップＳ１９は、解が得られた位置の平均を近似解Ｓとする。即ち、ステップＳ１９は、各探索終了位置の平均位置で上記の式に従って多グリッド近似解Ｓを求める。

一方、ステップＳ１２の後、ステップＳ２２は、距離観測用のカルマンフィルタＫＦ１の観測モデルを作成し、処理はステップＳ２１へ進む。ステップＳ１４又はステップＳ１６の判定結果がＹＥＳ、或いは、ステップＳ１９の後、処理はステップＳ２１へ進む。ステップＳ２１は、適応カルマンフィルタＫＦ２のモデルを、カルマンフィルタＫＦ１の推定状態のモデルのノイズレベルに応じて動的に調整する。上記の如くステップＳ１９で求めた多グリッド近似解Ｓをそのまま使用することも可能であるが、この例では、位置検出精度を向上させるために、上記の式Ｐ = (１−ｗ)*Ｓ＋ｗ*Ｚ_ｐと共に説明したように、手先位置の位置状態推定用の適応カルマンフィルタＫＦ２の予測位置Ｚ_ｐと統合する。全てのグリッドでの探索が成功しない（即ち、解が得られない）場合は、適応カルマンフィルタＫＦ２の予測値と、グリッド探索の全ての最終点の平均を計算し、カルマンフィルタＫＦ１，ＫＦ２の観測及び予測の共分散情報に基づいてカルマンフィルタＫＦ１，ＫＦ２による２つの位置推定を再度重み付けで平均化する。つまり、位置状態推定用の適応カルマンフィルタＫＦ２により、距離観測用のカルマンフィルタＫＦ１の観測予測の共分散と残差による計算を行い、手先が定速度移動という想定下で上記の如きプロセスモデルを作成する。ステップＳ２１の後、次のサイクルの処理を行う場合は処理がステップＳ１１へ戻るが、次のサイクルの処理がない場合は調整後の位置状態推定用の適応カルマンフィルタＫＦ２の出力を最後の推定結果、即ち、最終の位置出力とする。

（第２実施例）
図９は、本発明の第２実施例における位置検出装置の一例を示すブロック図である。図９中、図１と同一部分には同一符号を付し、その説明は省略する。

図９において、ウェアラブルシステム５１は、位置検出装置の一例である。ウェアラブルシステム５１において、超音波受信部１３からの距離情報は、メモリ５２に格納される。つまり、上記第１実施例では、図１に示す検知処理部３３がウェアラブルシステム１とは別体であったのに対し、本実施例では、図９に示すように検知処理部５３がウェアラブルシステム５１の一部を形成する。

図９に示すメモリ５２は、半導体記憶装置、磁気記録媒体、光記録媒体、光磁気記録媒体等で形成可能である。検知処理部５３は、組み込み用マイクロコンピュータ（マイコン）５３１と、ストレージ５３２を有し、マイコン５３１は図１の検知処理部３３と同様にＣＰＵ及びメモリを含む構成を有する。マイコン５３１は、メモリ５２から読み出した距離情報に基づいて位置検出処理を行うことができる。尚、マイコン５３１内のメモリは、メモリ５２又はストレージ５３２を代わりに用いることで省略可能である。ストレージ５３２は、図１の外部ストレージ３２と同様の機能を有する。メモリ５２及びストレージ５３２の一方を他方として機能させることが可能な場合には、他方を省略しても良い。又、ＤＢ作成部６００は、図１のＤＢ作成部３１と同様の機能を有するが、ウェアラブルシステム５１に対して接続可能な外部装置である。

図１の第１実施例では、図３及び図７の処理がいずれも位置検出装置内で実行されるのに対し、図９の第２実施例では、図３の処理は位置検出装置内で実行され、図７の処理が位置検出装置に接続される外部装置（即ち、ＤＢ作成部６００）により実行される。

以上の実施例を含む実施形態に関し、更に以下の付記を開示する。
（付記１）
位置検出対象の移動部分に装着された送信機から発信された信号を前記位置検出対象の参照部分に装着された３個の受信機で受信して前記移動部分の位置を検出する位置検出装置であって、
前記移動部分の３次元空間の移動範囲を固定サイズのグリッドで分割し、グリッド中心の３次元座標とグリッド中心から前記３個の受信機までの３つの距離をベクトルとしたＫＤ木のデータベースが予め登録された記憶部と、
前記３個の受信機が出力する、前記送信機と各受信機との間の距離に依存する距離情報に基づいて前記記憶部内の前記データベースを探索して反復法ソルバの初期位置を決定し、前記初期位置を用いて連立非線形方程式を解いて前記移動部分の位置を検出するプロセッサを備え、
前記プロセッサは、
前記距離情報をフィルタリングして前記データベースの探索用距離情報を取得する第１のカルマンフィルタと、
前記探索用距離情報を用いて前記データベースから最短隣グリッドを最短隣検索アルゴリズムに基づいて特定し、最短隣グリッドの中心位置を前記反復法ソルバの初期位置として代入して前記連立非線形方程式を解き、前記反復法ソルバが収束するか否かを判定する判定手段と、
前記反復法ソルバが収束する場合は、前記第１のカルマンフィルタの推定状態のモデルのノイズレベルに応じて動的に調整されて前記移動部分の位置状態を推定する位置状態推定用の第２のカルマンフィルタ
を有することを特徴とする、位置検出装置。
（付記２）
前記プロセッサは、前記反復法ソルバが収束しない場合は、前記最短隣グリッドに隣接する隣接グリッドを特定して各隣接グリッドの中心位置を前記反復法ソルバの初期位置として代入して前記連立非線形方程式を解いて再度解を探す探索手段を更に有することを特徴とする、付記１記載の位置検出装置。
（付記３）
前記プロセッサは、前記探索手段が解を探すことができないと、解が得られた位置の平均から近似解を求めて前記第２のカルマンフィルタに出力する手段を更に有することを特徴とする、付記２記載の位置検出装置。
（付記４）
全てのグリッドで解が得られない場合は、前記第２のカルマンフィルタの予測値とグリッド探索の全ての最終点の平均を計算し、前記第１及び第２のカルマンフィルタの観測及び予測の共分散情報に基づいて前記第１及び第２のカルマンフィルタによる２つの位置推定を再度重み付けで平均化することを特徴とする、付記１乃至３のいずれか１項記載の位置検出装置。
（付記５）
前記送信機及び前記３個の受信機は、前記位置検出対象に装着されるウェアラブルシステムを形成し、
前記位置検出装置は、前記ウェアラブルシステムとは別体であることを特徴とする、付記１乃至４のいずれか１項記載の位置検出装置。
（付記６）
前記送信機、前記３個の受信機、及び前記位置検出装置は、前記位置検出対象に装着されるウェアラブルシステムを形成することを特徴とする、付記１乃至４のいずれか１項記載の位置検出装置。
（付記７）
位置検出対象の移動部分に装着される送信機と、
前記位置検出対象の参照部分に装着され、前記送信機から発信された信号を受信する３個の受信機と、
前記移動部分の３次元空間の移動範囲を固定サイズのグリッドで分割し、グリッド中心の３次元座標とグリッド中心から前記３個の受信機までの３つの距離をベクトルとしたＫＤ木のデータベースが予め登録された記憶部と、
前記３個の受信機が出力する、前記送信機と各受信機との間の距離に依存する距離情報に基づいて前記記憶部内の前記データベースを探索して反復法ソルバの初期位置を決定し、前記初期位置を用いて連立非線形方程式を解いて前記移動部分の位置を検出する検知処理部を備え、
前記検知処理部は、
前記距離情報をフィルタリングして前記データベースの探索用距離情報を取得する第１のカルマンフィルタと、
前記探索用距離情報を用いて前記データベースから最短隣グリッドを最短隣検索アルゴリズムに基づいて特定し、最短隣グリッドの中心位置を前記反復法ソルバの初期位置として代入して前記連立非線形方程式を解き、前記反復法ソルバが収束するか否かを判定する判定手段と、
前記反復法ソルバが収束する場合は、前記第１のカルマンフィルタの推定状態のモデルのノイズレベルに応じて動的に調整されて前記移動部分の位置状態を推定する位置状態推定用の第２のカルマンフィルタ
を有することを特徴とする、ウェアラブルセンサシステム。
（付記８）
前記検知処理部は、前記反復法ソルバが収束しない場合は、前記最短隣グリッドに隣接する隣接グリッドを特定して各隣接グリッドの中心位置を前記反復法ソルバの初期位置として代入して前記連立非線形方程式を解いて再度解を探す探索手段を更に有することを特徴とする、付記７記載のウェアラブルセンサシステム。
（付記９）
前記検知処理部は、前記探索手段が解を探すことができないと、解が得られた位置の平均から近似解を求めて前記第２のカルマンフィルタに出力する手段を更に有することを特徴とする、付記８記載のウェアラブルセンサシステム。
（付記１０）
全てのグリッドで解が得られない場合は、前記第２のカルマンフィルタの予測値とグリッド探索の全ての最終点の平均を計算し、前記第１及び第２のカルマンフィルタの観測及び予測の共分散情報に基づいて前記第１及び第２のカルマンフィルタによる２つの位置推定を再度重み付けで平均化することを特徴とする、付記７乃至９のいずれか１項記載のウェアラブルセンサシステム。
（付記１１）
前記送信機は、前記位置検出対象の手首に装着される第１の装着部に設けられており、
前記３個の受信機は、前記位置検出対象の胴体に装着される第２の装着部に設けられていることを特徴とする、付記７乃至１０のいずれか１項記載のウェアラブルセンサシステム。
（付記１２）
コンピュータに、位置検出対象の移動部分の位置を検出する位置検出処理を実行させるプログラムであって、
前記移動部分の３次元空間の移動範囲を固定サイズのグリッドで分割し、グリッド中心の３次元座標とグリッド中心から、前記位置検出対象の参照部分に装着された３個の受信機までの３つの距離をベクトルとしたＫＤ木のデータベースが予め登録された記憶部を用い、
前記移動部分に装着された送信機から発信された信号を受信する前記３個の受信機が出力する、前記送信機と各受信機との間の距離に依存する距離情報に基づいて前記記憶部内の前記データベースを探索して反復法ソルバの初期位置を決定し、前記初期位置を用いて連立非線形方程式を解いて前記移動部分の位置を検出する検出手順
を前記コンピュータに実行させ、
前記検出手順は、
前記距離情報を第１のカルマンフィルタによりフィルタリングして前記データベースの探索用距離情報を取得する第１のフィルタリング手順と、
前記探索用距離情報を用いて前記データベースから最短隣グリッドを最短隣検索アルゴリズムに基づいて特定し、最短隣グリッドの中心位置を前記反復法ソルバの初期位置として代入して前記連立非線形方程式を解き、前記反復法ソルバが収束するか否かを判定する判定手順と、
前記反復法ソルバが収束する場合は、前記第１のカルマンフィルタの推定状態のモデルのノイズレベルに応じて動的に調整されて前記移動部分の位置状態を推定する位置状態推定用の第２のカルマンフィルタによるフィルタリングを行う第２のフィルタリング手順
を含むことを特徴とする、プログラム。
（付記１３）
前記検出手順は、前記反復法ソルバが収束しない場合は、前記最短隣グリッドに隣接する隣接グリッドを特定して各隣接グリッドの中心位置を前記反復法ソルバの初期位置として代入して前記連立非線形方程式を解いて再度解を探す探索手順を更に含むことを特徴とする、付記１２記載のプログラム。
（付記１４）
前記検出手順は、前記探索手段が解を探すことができないと、解が得られた位置の平均から近似解を求めて前記第２のカルマンフィルタに出力する手順を更に含むことを特徴とする、付記１３記載のプログラム。
（付記１５）
全てのグリッドで解が得られない場合は、前記第２のカルマンフィルタの予測値とグリッド探索の全ての最終点の平均を計算し、前記第１及び第２のカルマンフィルタの観測及び予測の共分散情報に基づいて前記第１及び第２のカルマンフィルタによる２つの位置推定を再度重み付けで平均化することを特徴とする、付記１２乃至１４のいずれか１項記載のプログラム。

以上、開示の位置検出装置、ウェアラブルセンサシステム及びプログラムを実施例により説明したが、本発明は上記実施例に限定されるものではなく、本発明の範囲内で種々の変形及び改良が可能であることは言うまでもない。

１ データ採取部
２，３３３ 通信部
３ 制御装置
１１ 超音波発信部
１３ 超音波受信部
３３，５３ 検知処理部
５１ ウェアラブルシステム
５２，３３２ メモリ
１１１ 超音波発信機
１３１〜１３３ 超音波センサ
３３１ ＣＰＵ

Claims (5)

1. 位置検出対象の移動部分に装着された送信機から発信された信号を前記位置検出対象の参照部分に装着された３個の受信機で受信して前記移動部分の位置を検出する位置検出装置であって、
   前記移動部分の３次元空間の移動範囲を固定サイズのグリッドで分割し、グリッド中心の３次元座標とグリッド中心から前記３個の受信機までの３つの距離をベクトルとしたＫＤ木のデータベースが予め登録された記憶部と、
   前記３個の受信機が出力する、前記送信機と各受信機との間の距離に依存する距離情報に基づいて前記記憶部内の前記データベースを探索して反復法ソルバの初期位置を決定し、前記初期位置を用いて連立非線形方程式を解いて前記移動部分の位置を検出するプロセッサを備え、
   前記プロセッサは、
   前記距離情報をフィルタリングして前記データベースの探索用距離情報を取得する第１のカルマンフィルタと、
   前記探索用距離情報を用いて前記データベースから最短隣グリッドを最短隣検索アルゴリズムに基づいて特定し、最短隣グリッドの中心位置を前記反復法ソルバの初期位置として代入して前記連立非線形方程式を解き、前記反復法ソルバが収束するか否かを判定する判定手段と、
   前記反復法ソルバが収束する場合は、前記第１のカルマンフィルタの推定状態のモデルのノイズレベルに応じて動的に調整されて前記移動部分の位置状態を推定する位置状態推定用の第２のカルマンフィルタ
   を有することを特徴とする、位置検出装置。
2. 前記プロセッサは、前記反復法ソルバが収束しない場合は、前記最短隣グリッドに隣接する隣接グリッドを特定して各隣接グリッドの中心位置を前記反復法ソルバの初期位置として代入して前記連立非線形方程式を解いて再度解を探す探索手段を更に有することを特徴とする、請求項１記載の位置検出装置。
3. 前記プロセッサは、前記探索手段が解を探すことができないと、解が得られた位置の平均から近似解を求めて前記第２のカルマンフィルタに出力する手段を更に有することを特徴とする、請求項２記載の位置検出装置。
4. 位置検出対象の移動部分に装着される送信機と、
   前記位置検出対象の参照部分に装着され、前記送信機から発信された信号を受信する３個の受信機と、
   前記移動部分の３次元空間の移動範囲を固定サイズのグリッドで分割し、グリッド中心の３次元座標とグリッド中心から前記３個の受信機までの３つの距離をベクトルとしたＫＤ木のデータベースが予め登録された記憶部と、
   前記３個の受信機が出力する、前記送信機と各受信機との間の距離に依存する距離情報に基づいて前記記憶部内の前記データベースを探索して反復法ソルバの初期位置を決定し、前記初期位置を用いて連立非線形方程式を解いて前記移動部分の位置を検出する検知処理部を備え、
   前記検知処理部は、
   前記距離情報をフィルタリングして前記データベースの探索用距離情報を取得する第１のカルマンフィルタと、
   前記探索用距離情報を用いて前記データベースから最短隣グリッドを最短隣検索アルゴリズムに基づいて特定し、最短隣グリッドの中心位置を前記反復法ソルバの初期位置として代入して前記連立非線形方程式を解き、前記反復法ソルバが収束するか否かを判定する判定手段と、
   前記反復法ソルバが収束する場合は、前記第１のカルマンフィルタの推定状態のモデルのノイズレベルに応じて動的に調整されて前記移動部分の位置状態を推定する位置状態推定用の第２のカルマンフィルタ
   を有することを特徴とする、ウェアラブルセンサシステム。
5. コンピュータに、位置検出対象の移動部分の位置を検出する位置検出処理を実行させるプログラムであって、
   前記移動部分の３次元空間の移動範囲を固定サイズのグリッドで分割し、グリッド中心の３次元座標とグリッド中心から、前記位置検出対象の参照部分に装着された３個の受信機までの３つの距離をベクトルとしたＫＤ木のデータベースが予め登録された記憶部を用い、
   前記移動部分に装着された送信機から発信された信号を受信する前記３個の受信機が出力する、前記送信機と各受信機との間の距離に依存する距離情報に基づいて前記記憶部内の前記データベースを探索して反復法ソルバの初期位置を決定し、前記初期位置を用いて連立非線形方程式を解いて前記移動部分の位置を検出する検出手順
   を前記コンピュータに実行させ、
   前記検出手順は、
   前記距離情報を第１のカルマンフィルタによりフィルタリングして前記データベースの探索用距離情報を取得する第１のフィルタリング手順と、
   前記探索用距離情報を用いて前記データベースから最短隣グリッドを最短隣検索アルゴリズムに基づいて特定し、最短隣グリッドの中心位置を前記反復法ソルバの初期位置として代入して前記連立非線形方程式を解き、前記反復法ソルバが収束するか否かを判定する判定手順と、
   前記反復法ソルバが収束する場合は、前記第１のカルマンフィルタの推定状態のモデルのノイズレベルに応じて動的に調整されて前記移動部分の位置状態を推定する位置状態推定用の第２のカルマンフィルタによるフィルタリングを行う第２のフィルタリング手順
   を含むことを特徴とする、プログラム。

Images