JP2019045178A - 情報処理装置、およびプログラム - Google Patents

Abstract

【課題】移動体の位置をより高精度に補正することが可能な、新規かつ改良された情報処理装置、およびプログラムを提供する。【解決手段】移動体が測距装置２２０に検出されている間に測距装置が算出した移動体の位置から、移動体の第１の軌跡を取得する第１の取得部と、慣性センサが検出する情報に基づき算出された移動体の第２の軌跡を取得する第２の取得部と、第１の軌跡および第２の軌跡に基づき、第２の軌跡の誤差を算出する算出部２７０と、誤差に基づき、移動体の位置に関する情報および角度に関する情報の補正を制御する制御部２８０と、を備える。【選択図】図２

Description

本発明は、情報処理装置、およびプログラムに関する。

近日、屋外における位置情報を取得する技術として、人工衛星から電波を受信することで位置を推定する全地球測位システム（ＧＰＳ：Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）が広く用いられている。ただし、ＧＰＳの電波が届かない屋内においては測位が困難となるため、屋内における測位技術としては、ビーコンから発信される電波を携帯端末等で受信することで測位する方法、無線ＬＡＮの複数のアクセスポイントから受信した電波の情報に基づき測位する方法等が利用されている。また、屋内外でシームレスな測位が可能な技術として、加速度センサ、角速度センサ、磁気センサ等から取得した情報に基づき測位する歩行者自律航法（ＰＤＲ：Ｐｅｄｅｓｔｒｉａｎ Ｄｅａｄ Ｒｅｃｋｏｎｉｎｇ）がある。上述したセンサは、現在市販されているスマートフォンに内蔵されているため、ＰＤＲは今後広く普及する可能性があるとして注目されている。しかしながら、ＰＤＲでは慣性センサによる累積誤差が発生するため、進行方向にずれが生じてしまう。正確な進行方向を推定するためには、累積誤差による進行方向のずれを補正する必要がある。

上述したＰＤＲの累積誤差の補正方法として、例えば下記の特許文献１及び特許文献２には、固定ノード及びビーコンから受信した情報に基づき、位置を補正する方法が開示されている。特許文献３及び４には、ＰＤＲで測位した移動体の位置を、ＧＰＳで取得した位置情報に基づき補正する方法が開示されている。また、特許文献５には、ＰＤＲで測位した移動体の位置を、カメラまたはレーザー装置等を含む測距装置が取得した情報に基づき補正する方法が開示されている。

特開２０１２−１１２６９１号公報 特開２０１７−０４０５０３号公報 特開平０６−１１１１９６号公報 特開２０１２―００７９３７号公報 国際公開第２０１１／０６８１８４号

特許文献１と特許文献２の補正方法は、歩行者が固定ノード、ビーコン送信機、及び測距装置等が設置されている２点の近傍を通過することで、各装置が取得した情報に基づき、歩行者の測位位置を補正する。しかし、屋内における測位の場合、固定ノード、ビーコン送信機、測距装置等を設置する場所には制限があり、装置を高密度で設置できない場合がある。その場合、上述の装置の近傍を歩行者が通過しない状況が継続し、進行方向の誤差が補正されずに蓄積されてしまう可能性がある。また、装置を高密度に設置するにはコストがかかってしまう。特許文献５の補正方法は、装置が設置されている２点の近傍を歩行者が通過する必要はない。しかし、歩行者が携帯する装置、測距装置、各装置が取得した情報を照合する外部装置の少なくとも３つの装置が必要であるため、特許文献１と特許文献２の場合と同様に、装置の設置場所及びコストが問題となる。特許文献３と特許文献４の補正方法は、１つの装置で歩行者の位置を測位することが可能である。しかし、特許文献３と特許文献４の補正方法はＧＰＳを用いた補正方法であるため、ＧＰＳの電波が届きにくい屋内において、ＧＰＳを用いた方法による測位は困難である。

そこで、本発明は、上記問題に鑑みてなされたものであり、本発明の目的とするところは、移動体の位置をより高精度に補正することが可能な、新規かつ改良された情報処理装置、およびプログラムを提供することにある。

上記課題を解決するために、本発明のある観点によれば、移動体が測距装置に検出されている間に前記測距装置が算出した前記移動体の位置から、前記移動体の第１の軌跡を取得する第１の取得部と、慣性センサが検出する情報に基づき算出された前記移動体の第２の軌跡を取得する第２の取得部と、前記第１の軌跡および前記第２の軌跡に基づき、前記第２の軌跡の誤差を算出する算出部と、前記誤差に基づき、前記移動体の位置に関する情報および角度に関する情報の補正を制御する制御部と、を備える、情報処理装置が提供される。

前記算出部は、前記第１の軌跡を回転する際の基準となる第１の基準点、および前記第２の軌跡を回転する際の基準となる第２の基準点を算出し、前記第１の基準点と前記第２の基準点を重ねた上で、基準点を中心に第１の軌跡の時系列データまたは第２の軌跡の時系列データを回転させ、同一時点における、前記第１の基準点と前記第１の軌跡の時系列データの１点を通る直線、及び前記第２の基準点と前記第２の軌跡の時系列データの１点を通る直線が重なる回転角を算出し、前記回転角に基づき補正角度を算出してもよい。

前記算出部は、前記第１の基準点を通る基準軸と、前記第１の基準点と前記第１の軌跡の時系列データの１点を通る直線がなす第１の角度、及び前記第２の基準点を通る前記基準軸と、前記第２の基準点と前記第２の軌跡の時系列データの１点を通る直線がなす第２の角度を取得し、前記第１の軌跡の時系列データと前記第２の軌跡の時系列データの同一時点における前記第１の角度と前記第２の角度の差に基づき、前記回転角を算出してもよい。

前記算出部は、前記第１の軌跡及び前記第２の軌跡の時系列データの数だけ前記回転角を算出し、算出した複数の前記回転角の平均を算出し、算出した平均の前記回転角を前記補正角度としてもよい。

前記制御部は、前記測距装置の検出範囲にて検出された前記移動体の第１の軌跡、および前記算出部が算出した補正角度に基づき、前記移動体の前記位置に関する情報および前記角度に関する情報の補正を制御してもよい。

前記第１の取得部は、前記移動体が前記測距装置の検出範囲に入ってから、前記移動体が前記検出範囲から出るまでの期間の前記移動体の前記第１の軌跡を取得してもよい。

前記第１の取得部は、前記測距装置の位置を示す世界座標系位置データ、および前記測距装置が算出した前記移動体までの距離と角度に基づき、前記移動体の第１の世界座標系位置データを算出してもよい。

前記第１の取得部は、前記第１の世界座標系位置データの時系列データに基づき、前記移動体の前記第１の軌跡を取得してもよい。

前記第２の取得部は、前記慣性センサが検出した情報に基づき算出された前記移動体の第２の世界座標系位置データを取得し、取得した前記第２の世界座標系位置データの時系列データに基づき、前記移動体の前記第２の軌跡を取得してもよい。

前記第２の取得部は、前記移動体から送信される情報が所定の条件を満たす場合、前記移動体の前記第２の世界座標系位置データを取得してもよい。

前記第２の取得部は、前記移動体の前記第１の軌跡に基づき、複数の移動体の複数の前記第２の軌跡を解析することにより、前記移動体の前記第１の軌跡に対応する前記移動体の前記第２の軌跡を取得してもよい。

前記測距装置は、前記測距装置を中心に全方位を検出範囲として設定可能であってもよい。

前記情報処理装置は、ネットワークを介して前記移動体と接続する通信部をさらに備え、前記第２の取得部は、前記通信部を介して、前記移動体の第２の世界座標系位置データを取得してもよい。

また、上記課題を解決するために、本発明の別の観点によれば、移動体が測距装置に検出されている間に前記測距装置が算出した前記移動体の１または２以上の位置を取得する第１の取得部と、前記第１の取得部が取得した前記移動体の１または２以上の位置に基づき、前記移動体の位置に関する情報または角度に関する情報の補正を制御する制御部と、を備える、情報処理装置が提供される。

また、上記課題を解決するために、本発明の別の観点によれば、コンピュータを、移動体が測距装置に検出されている間に前記測距装置が算出した前記移動体の位置から、前記移動体の第１の軌跡を取得する第１の取得部と、慣性センサが検出する情報に基づき算出された前記移動体の第２の軌跡を取得する第２の取得部と、前記第１の軌跡および前記第２の軌跡に基づき、前記第２の軌跡の誤差を算出する算出部と、前記誤差に基づき、前記移動体の位置に関する情報および角度に関する情報の補正を制御する制御部と、として機能させるための、プログラムが提供される。

また、上記課題を解決するために、本発明の別の観点によれば、コンピュータを、移動体が測距装置に検出されている間に前記測距装置が算出した前記移動体の１または２以上の位置を取得する第１の取得部と、前記第１の取得部が取得した前記移動体の１または２以上の位置に基づき、前記移動体の位置に関する情報または角度に関する情報の補正を制御する制御部と、として機能させるための、プログラムが提供される。

以上説明したように本発明によれば、移動体の位置をより高精度に補正することを実現することが可能である。

本発明の一実施形態に係る位置測位システムと歩行者の関係を示す説明図である。 同実施形態に係るアンカーノードの構成例を示すブロック図である。 同実施形態に係るターゲットノードの端末座標を世界座標に変換する方法の例を示す説明図である。 同実施形態に係る第１の軌跡における補正角度の算出方法の例を示す説明図である。 同実施形態に係る第２の軌跡における補正角度の算出方法の例を示す説明図である。 同実施形態に係るターゲットノードの構成例を示すブロック図である。 同実施形態に係る時刻ｔ＝Ｔにおけるターゲットノードの位置及び角度の補正処理の概要を示す説明図である。 同実施形態に係る時刻ｔ＝Ｔ＋１０におけるターゲットノードの位置及び角度の補正処理の概要を示す説明図である。 同実施形態に係る時刻ｔ＝Ｔ＋１５（補正なし）におけるターゲットノードの位置及び角度の補正処理の概要を示す説明図である。 同実施形態に係る時刻ｔ＝Ｔ＋１５（補正あり）におけるターゲットノードの位置及び角度の補正処理の概要を示す説明図である。 同実施形態に係るアンカーノードの動作例を示すフローチャートである。 本発明の第２の実施形態に係るアンカーノードの構成例を示すブロック図である。 同実施形態に係るアンカーノードの動作例を示すフローチャートである。 本発明の一実施形態に係るアンカーノードのハードウェア構成例を示したブロック図である。

以下に添付図面を参照しながら、本発明の実施の形態について詳細に説明する。なお、本明細書および図面において、実質的に同一の機能構成を有する構成要素については、同一の符号を付することにより重複説明を省略する。

また、本明細書および図面において、実質的に同一の機能構成を有する複数の構成要素を、同一の符号の後に異なるアルファベットを付して区別する場合もある。例えば、実質的に同一の機能構成または論理的意義を有する複数の構成を、必要に応じてアンカーノード２０Ａ及び２０Ｂのように区別する。ただし、実質的に同一の機能構成を有する複数の構成要素の各々を特に区別する必要がない場合、複数の構成要素の各々に同一符号のみを付する。例えば、アンカーノード２０Ａおよびアンカーノード２０Ｂを特に区別する必要が無い場合には、各アンカーノードを単にアンカーノード２０と称する。

＜＜１．位置測位システムと歩行者の関係＞＞
図１は、本発明の実施形態に係る位置測位システムと歩行者の関係を示す説明図である。図１は、歩行者１０、アンカーノード２０、及びターゲットノード３０の関係を示している。歩行者１０は、ターゲットノード３０を携帯して歩行する。アンカーノード２０とターゲットノード３０は無線通信により接続されている。なお、ターゲットノード３０は移動体の一例である。ターゲットノード３０が歩行者１０に携帯される場合、ターゲットノード３０および歩行者１０の双方が移動体の概念に包含される。

歩行者１０は、典型的には人である。歩行者１０は、ターゲットノード３０を携帯して歩行することで、アンカーノード２０が備える測距装置により測位される移動体となる。また、歩行者１０は、ターゲットノード３０が備える慣性センサの測位対象でもある。なお、歩行者１０がターゲットノード３０を携帯する方法は任意の方法でよく、特に限定されない。

アンカーノード２０は、固定して設置されたノードであり、ターゲットノード３０の位置の測位、及びターゲットノード３０の位置の補正情報を算出する機能を有する情報処理装置である。アンカーノード２０は、近傍のターゲットノード３０と無線通信ができる通信部２４０、及び近傍の歩行者１０の位置を検出することができる測距装置２２０を備える。なお、本発明の実施形態において、通信部２４０は、汎用的な周波数である２．４ＧＨｚ帯の周波数を利用するが、通信部２４０が利用する周波数帯は限定されず、通信部２４０は任意の周波数帯を用いてもよい。

図１は、アンカーノード２０Ａが、測距装置２２０Ａが検出したターゲットノード３０と、通信部２４０Ａを介して無線通信による情報の送受信を行っている例を示している。また、図１は、アンカーノード２０Ｂが、測距装置２２０Ｂが検出していないターゲットノード３０とは、通信部２４０Ｂを介した無線通信を行っていない例も示している。なお、アンカーノード２０の設置位置は、ターゲットノード３０と水平となる高さに設置されることが望ましい。なぜならば、アンカーノード２０とターゲットノード３０間の距離をより単純な計算式で算出することが可能だからである。ただし、アンカーノード２０の設置場所は限定されるものではなく、例えば天井に設置されてもよいし、屋内及び屋外のいずれに設置されてもよいが、設置する場所によって、アンカーノード２０とターゲットノード３０間の距離を算出する計算式は異なる。

ターゲットノード３０は、アンカーノード２０の測位対象となるノードであり、近傍のアンカーノード２０と無線通信ができる通信部、及び慣性センサを備える。なお、本発明の実施形態において、通信部は、汎用的な周波数である２．４ＧＨｚ帯の周波数を利用するが、通信部が利用する周波数帯は限定されず、通信部は任意の周波数帯を用いてもよい。また、ターゲットノード３０が歩行者１０に携帯される場合、ターゲットノード３０の位置は歩行者１０の位置と等しいため、ターゲットノード３０は歩行者１０の位置を測位することが可能である。また、ターゲットノード３０は、スマートフォン、携帯端末、またはウェアラブル端末等であってもよい。

以上、図１を参照しながら、本発明の実施形態に係る位置測位システムと歩行者の関係を説明した。続いて、本発明の第１の実施形態に係るアンカーノード２０の構成例について説明する。

＜＜２．第１の実施形態＞＞
第１の実施形態では、アンカーノード２０の近傍に１台のターゲットノード３０が存在する場合を例に説明する。なお、アンカーノード２０の近傍に存在するターゲットノード３０の数は１台に限らず、複数存在してもよい。まず、本発明の実施形態に係るアンカーノード２０の構成例について説明する。

＜２−１．アンカーノード２０の構成例＞
図２は、本発明の実施形態に係るアンカーノード２０の構成例を示すブロック図である。図２に示したように、本発明の実施形態に係るアンカーノード２０は、測距装置２２０、ＬＲＦ位置情報取得部２３０、通信部２４０、ＰＤＲ位置情報取得部２５０、記憶部２６０、算出部２７０、制御部２８０を備える。なお、第１の取得部には、測距装置２２０、ＬＲＦ位置情報取得部２３０、記憶部２６０、及び算出部２７０の一部が含まれる。また、第２の取得部には、ＰＤＲ位置情報取得部２５０、記憶部２６０、及び算出部２７０の一部が含まれる。

（１）測距装置２２０
測距装置２２０は、測距装置２２０から測距対象までの距離及び方向を検出する装置である。本発明の実施形態においては、測距装置２２０としてレーザレンジファインダ（ＬＲＦ：Ｌａｓｅｒ Ｒａｎｇｅ Ｆｉｎｄｅｒ）を用い、ターゲットノード３０までの距離と方向を測定する。ただし、測距装置２２０として用いる装置は任意の装置でよく、特に限定されない。

測距装置２２０は、ターゲットノード３０を検出する範囲を設定する。以下では、測距装置２２０がターゲットノード３０を検出する範囲のことを、検出範囲と称する。検出範囲は、ＬＲＦの性能に応じてあらかじめ設定される。例えば、本発明の実施形態のように、正面方向のみを検出可能なＬＲＦを用いる場合、アンカーノード２０は検出範囲をＬＲＦが向いている方向に合わせて設定する。また、例えば、全方位を検出可能なＬＲＦを用いる場合、アンカーノード２０は検出範囲を全方位として設定してもよい。測距装置２２０は、ターゲットノード３０が検出範囲に入ったことを検出した時、ターゲットノード３０までの距離と方向の測定を開始する。その後、測距装置２２０は、通信部２４０がターゲットノード３０のビーコンを受信するごとに、ターゲットノード３０までの距離と方向を測定する。測距装置２２０は、ターゲットノード３０が検出範囲から出たことを検出した時、ターゲットノード３０までの距離と方向の測定を終了する。

（２）ＬＲＦ位置情報取得部２３０
ＬＲＦ位置情報取得部２３０は、ターゲットノード３０の世界座標系位置を算出する機能を有する。算出方法は、測距装置２２０が取得したターゲットノード３０までの距離と方向、及びアンカーノード２０の世界座標系位置に基づき、ターゲットノード３０の世界座標系位置を算出する方法である。ここで、ターゲットノード３０までの距離はｄ^ＬＲＦ、ターゲットノード３０までの方向を示す角度はθ^ＬＲＦ、アンカーノード２０の世界座標系位置は（Ｘ^ＡＮ，Ｙ^ＡＮ）と定義される。また、ＬＲＦ位置情報取得部２３０が算出したターゲットノード３０の世界座標系位置は（Ｘ^ＬＲＦ、Ｙ^ＬＲＦ）と定義される。なお、以下では、ＬＲＦが取得した情報に基づき算出されたターゲットノード３０の世界座標系位置のことを、ＬＲＦ世界座標系位置（Ｘ^ＬＲＦ、Ｙ^ＬＲＦ）（第１の世界座標系位置データ）と称する。ＬＲＦ位置情報取得部２３０は、算出したＬＲＦ世界座標系位置（Ｘ^ＬＲＦ、Ｙ^ＬＲＦ）を記憶部２６０へ出力する。

図３は、ＬＲＦが検出したターゲットノード３０までの距離ｄ^ＬＲＦ、及びターゲットノード３０までの方向を示す角度θ^ＬＲＦに基づき、ターゲットノード３０のＬＲＦ世界座標系位置（Ｘ^ＬＲＦ、Ｙ^ＬＲＦ）を算出する方法を示している。なお、（Ｘ^ＡＮ，Ｙ^ＡＮ）はアンカーノード２０の世界座標系位置を示しており、世界座標系位置（Ｘ^ＡＮ，Ｙ^ＡＮ）は初期設定された既知の値とする。

ＬＲＦが検出するターゲットノード３０までの距離ｄ^ＬＲＦ、ターゲットノード３０までの方向を示す角度θ^ＬＲＦより、ターゲットノード３０のＬＲＦ世界座標系位置（Ｘ^ＬＲＦ、Ｙ^ＬＲＦ）は、以下の数式１により算出される。

数式１により算出されたＬＲＦ世界座標系位置（Ｘ^ＬＲＦ、Ｙ^ＬＲＦ）は、ターゲットノード３０の検出タイミングと関連付けて記憶部２６０に記憶される。なお、ＬＲＦの検出範囲内に存在するターゲットノード３０は１台とは限らず、複数台存在する場合もある。よって、記憶部２６０は、複数のターゲットノード３０のＬＲＦ世界座標系位置（Ｘ^ＬＲＦ、Ｙ^ＬＲＦ）を記憶してもよい。

（３）通信部２４０
通信部２４０は、アンカーノード２０とターゲットノード３０を接続するインタフェースであり、アンカーノード２０がターゲットノード３０と情報を通信する。通信部２４０は、ターゲットノード３０との通信において、ターゲットノード３０が備える慣性センサが取得した情報に基づき算出された歩行者１０の位置情報を受信する。また、通信部２４０は、制御部２８０から取得した、ターゲットノード３０の位置の補正情報を、ターゲットノード３０へ送信する。なお、位置情報及び補正情報の詳細については後述する。

（４）ＰＤＲ位置情報取得部２５０
ＰＤＲ位置情報取得部２５０は、通信部２４０が受信した情報から、慣性センサによって測位されたターゲットノード３０の位置情報を取得する機能を有する。具体的には、ＰＤＲ位置情報取得部２５０は、通信部２４０が受信したターゲットノード３０のビーコンに含まれるパケット情報から、慣性センサによって測位されたターゲットノード３０の世界座標系位置を取得する。ここで、慣性センサによって測位されたターゲットノード３０の世界座標系位置は（Ｘ^ＰＤＲ、Ｙ^ＰＤＲ）と定義される。なお、以下では、慣性センサによって測位されたターゲットノード３０の世界座標系位置は、ＰＤＲ世界座標系位置（Ｘ^ＰＤＲ、Ｙ^ＰＤＲ）（第２の世界座標系位置データ）と称される。ＰＤＲ位置情報取得部２５０は、取得したＰＤＲ世界座標系位置（Ｘ^ＰＤＲ、Ｙ^ＰＤＲ）を、通信部２４０がビーコンを受信したタイミングと関連付けて記憶部２６０へ出力する。

なお、アンカーノード２０の通信部２４０は、ビーコンの受信条件を設定していない場合、不要なビーコンの情報まで受信してしまう。例えば、ＰＤＲ位置情報取得部２５０は、検出範囲内に存在するターゲットノード３０から送信されるビーコンの情報を取得できればよいが、検出範囲外に存在するターゲットノード３０から送信されるビーコンの情報を取得することも可能である。しかし、検出範囲外に存在するターゲットノード３０のビーコンの情報は、位置情報の算出に使われない情報のため、不要な情報である。そこで、ＰＤＲ位置情報取得部２５０は、通信部２４０がビーコンを受信した際にビーコンの受信電力を測定し、受信電力の強度に基づきビーコンの情報を取得するか否かを判定するように受信条件を設定してもよい。具体的に、受信電力の強度が所定の閾値を超える場合、ターゲットノード３０は検出範囲内に存在するため、ビーコンの情報を取得すると判定する。受信電力の強度が所定の閾値を超えない場合、ターゲットノード３０は検出範囲外に存在するため、ビーコンの情報を取得しないと判定する。

（５）記憶部２６０
記憶部２６０は、ＬＲＦ位置情報取得部２３０から取得したＬＲＦ世界座標系位置（Ｘ^ＬＲＦ、Ｙ^ＬＲＦ）、及びＰＤＲ位置情報取得部２５０から取得したＰＤＲ世界座標系位置（Ｘ^ＰＤＲ、Ｙ^ＰＤＲ）を記憶する。なお、ＬＲＦ世界座標系位置（Ｘ^ＬＲＦ、Ｙ^ＬＲＦ）は、アンカーノード２０がターゲットノード３０からビーコンを受信する度にＬＲＦ位置情報取得部２３０により算出され、記憶部２６０に出力される。また、ＰＤＲ世界座標系位置（Ｘ^ＰＤＲ、Ｙ^ＰＤＲ）も同様に、アンカーノード２０がターゲットノード３０からビーコンを受信する度に、ＰＤＲ位置情報取得部２５０により取得され、記憶部２６０へ出力される。したがって、記憶部２６０は、ＬＲＦ世界座標系位置（Ｘ^ＬＲＦ、Ｙ^ＬＲＦ）とＰＤＲ世界座標系位置（Ｘ^ＰＤＲ、Ｙ^ＰＤＲ）を時系列データとして記憶する。

（６）算出部２７０
算出部２７０は、記憶部２６０から取得したＬＲＦ世界座標系位置（Ｘ^ＬＲＦ、Ｙ^ＬＲＦ）、及びＰＤＲ世界座標系位置（Ｘ^ＰＤＲ、Ｙ^ＰＤＲ）の時系列データに基づき、ターゲットノード３０の位置の補正情報を算出する。ここで、補正情報とは、補正位置と補正角度の総称のことである。また、補正位置とは、慣性センサによる累積誤差を含まない、ターゲットノード３０の正となる位置のことである。補正角度とは、慣性センサによる累積誤差を含むターゲットノード３０の進行方向を、累積誤差を含まない正となる進行方向に補正するために必要な回転角のことである。なお、進行方向は角度によって数値的に示される。よって、進行方向を補正することは、すなわち進行方向を示す角度を補正角度で補正することを意味する。以下では、進行方向を示す角度を補正角度で補正する旨を説明する箇所において、進行方向は進行角度とも称される。

また、本発明の実施形態に係る算出部２７０は、図２に示すように補正位置算出部２７２、及び補正角度算出部２７４を備える。算出部２７０は、補正位置算出部２７２が算出した補正位置、及び補正角度算出部２７４が算出した補正角度を、補正情報として制御部２８０へ出力する。

・補正位置算出部２７２
補正位置算出部２７２は、ＬＲＦ位置情報取得部２３０が算出したターゲットノード３０のＬＲＦ世界座標系位置（Ｘ^ＬＲＦ、Ｙ^ＬＲＦ）の時系列データに基づき、ターゲットノード３０の補正位置（Ｘ^ＲＳＴ、Ｙ^ＲＳＴ）を算出する。具体的に、補正位置算出部２７２は、ＬＲＦの検出範囲内で最後に取得されたターゲットノード３０の情報に基づき算出されたＬＲＦ世界座標系位置（Ｘ^ＬＲＦ、Ｙ^ＬＲＦ）を、補正位置（Ｘ^ＲＳＴ、Ｙ^ＲＳＴ）とする。

・補正角度算出部２７４
補正角度算出部２７４は、ＬＲＦ位置情報取得部２３０が算出したターゲットノード３０のＬＲＦ世界座標系位置（Ｘ^ＬＲＦ、Ｙ^ＬＲＦ）の時系列データ、及びＰＤＲ位置情報取得部２５０が取得したターゲットノード３０のＰＤＲ世界座標系位置（Ｘ^ＰＤＲ、Ｙ^ＰＤＲ）の時系列データに基づき、ターゲットノード３０の補正角度γを算出する。

図４は、補正角度γの算出方法を示している。算出部２７０の補正角度算出部２７４は、ターゲットノード３０がＬＲＦの検出範囲を出たことをきっかけとして、補正角度γの算出を開始する。補正角度算出部２７４は、ターゲットノード３０がＬＲＦの検出範囲内に存在する間に取得された、「ＬＲＦ世界座標系位置の時系列データ」からなるターゲットノード３０の軌跡、及び「ＰＤＲ世界座標系位置の時系列データ」からなるターゲットノード３０の軌跡に基づき、補正角度γを算出する。なお、以下では、「ＬＲＦ世界座標系位置の時系列データ」からなるターゲットノード３０の軌跡は第１の軌跡、「ＰＤＲ世界座標系位置の時系列データ」からなるターゲットノード３０の軌跡は第２の軌跡と称される。

補正角度算出部２７４は、記憶部２６０から第１の軌跡及び第２の軌跡を取得する。各軌跡を取得後、補正角度算出部２７４は、第１の軌跡を回転させる基準となる第１の基準点、及び第２の軌跡を回転させる基準となる第２の基準点を算出する。補正角度算出部２７４は、第１の基準点と第２の基準点を重ねた上で、第１の軌跡または第２の軌跡を回転させて回転角を算出する。具体的に、同一時点における、第１の基準点と第１の軌跡の時系列データの１点を通る直線、及び第２の基準点と第２の軌跡の時系列データの１点を通る直線が重なるまで回転させた角度を回転角とする。補正角度算出部２７４は、各軌跡を回転させることで求めた回転角に基づき、補正角度を算出する。本発明の実施形態においては、回転角を求める方法として、第１の軌跡の重心と第２の軌跡の重心を求め、重心を基準点として回転角を求める方法を用いる。ただし、回転角を求める方法は任意の方法でよく、特に限定されない。

図４Ａは、ターゲットノード３０の第１の軌跡を示している。第１の軌跡４１Ａは、白丸で表現されたターゲットノード３０の時系列データ４３Ａからなるターゲットノード３０の軌跡である。第１の基準点４５Ａは、第１の軌跡の重心（ｘ_ｃ ^ＬＲＦ、ｙ_ｃ ^ＬＲＦ）である。基準軸４７Ａは、第１の基準点４５Ａを通り、第１の基準点と第２の基準点を重ねる際に基準となる軸である。第１の角度４９Ａは、基準軸４７Ａと、第１の軌跡の時系列データの１点と第１の基準点を通る直線がなす角度τ_ｋ ^ＬＲＦのことである。ここで、ｋは軌跡の時系列データの取得時点を示す値であり、ｋはｋ＝１〜Ｋまでの値を取り得る。ｋ＝１の場合、すなわちτ_１は、アンカーノード２０が検出範囲内でターゲットノード３０を検出してから、アンカーノード２０が最初にビーコンを受信した時点での回転角を示す。また、ｋ＝Ｋの場合、すなわちτ_Ｋは、アンカーノード２０が検出範囲内で最後にビーコンを受信した時点での回転角を示す。

図４Ｂは、ターゲットノード３０の第２の軌跡を示している第２の軌跡４１Ｂは、黒丸で表現されたターゲットノード３０の時系列データ４３Ｂからなるターゲットノード３０の軌跡である。第２の基準点４５Ｂは、第２の軌跡の重心（ｘ_ｃ ^ＰＤＲ、ｙ_ｃ ^ＰＤＲ）である。基準軸４７Ｂは、第２の基準点４５Ｂを通り、第２の基準点と第２の基準点を重ねる際に基準となる軸である。第２の角度４９Ｂは、基準軸４７Ｂと、第２の軌跡の時系列データの１点と第２の基準点を通る直線がなす角度τ_ｃ ^ＰＤＲのことである。

補正角度算出部２７４は、図４Ａの各時系列データにおける角度τ_ｋ ^ＬＲＦ、図４Ｂの各時系列データにおける角度τ_ｋ ^ＰＤＲを求め、回転角γ_ｋをγ_ｋ＝τ_ｋ ^ＬＲＦ−τ_ｋ ^ＰＤＲにより算出する。また、補正角度算出部２７４は、各時系列データにおける回転角γ_ｋの平均γ＝ａｖｅｒａｇｅ（γ_ｋ）を算出し、その平均値を補正角度γとする。

本発明の実施形態において補正角度γは、ターゲットノード３０がアンカーノード２０の検出範囲内に存在する間に検出された時系列データに基づき算出される。しかし、補正角度γは、第１の軌跡と第２の軌跡の時系列データの内、少なくとも１時点の時系列データを用いて算出することが可能である。ただし、時系列データの数が多いほど、アンカーノード２０はより精度の高い補正情報を算出することが可能である。

（７）制御部２８０
制御部２８０は、算出部２７０から取得した補正情報に基づき、ターゲットノード３０が位置を補正する処理を制御する機能を有する。制御部２８０は、算出部２７０の補正位置算出部２７２が算出したターゲットノード３０の補正位置（Ｘ^ＲＳＴ、Ｙ^ＲＳＴ）、及び補正角度算出部２７４が算出したターゲットノード３０の補正角度γを補正情報とし、通信部２４０へ出力する。

以上、図２〜４を参照しながら、本発明の実施形態に係るアンカーノード２０の構成例について説明した。続いて、本発明の実施形態に係るターゲットノード３０の構成例について説明する。

＜２−２．ターゲットノード３０の構成例＞
図５は、本発明の実施形態に係るターゲットノード３０の構成例を示すブロック図である。以下、図５を参照しながら、本発明の実施形態に係るターゲットノード３０の構成例について説明する。

図５に示したように、本発明の実施形態に係るターゲットノード３０は、算出部３２０、制御部３４０、通信部３６０を備える。

（１）算出部３２０
算出部３２０は、ターゲットノード３０が備える慣性センサが取得する情報に基づき、ターゲットノード３０の位置情報を算出する。ここで、位置情報とは、ターゲットノード３０の位置と進行方向の総称のことである。また、位置とは、慣性センサによる累積誤差を含む、ターゲットノード３０の位置のことである。進行方向とは、慣性センサによる累積誤差を含むターゲットノード３０の進行方向のことである。

また、本発明の実施形態に係る算出部３２０は、加速度センサ３２２、角速度センサ３２４、距離算出部３２６、方向算出部３２８、位置算出部３３０を備える。

・加速度センサ３２２
加速度センサ３２２は、物体の加速度を取得する機能を備える慣性センサである。加速度センサ３２２は、一定の時間に物体が移動した際の速度の変化量を求めるための機器である。加速度センサ３２２の種類には、ばねにつながれた錘の位置変化から加速度を得る方式のセンサ、または錘をつけたばねに振動を加えた際の周波数の変化から加速度を得る方式のセンサ等があるが、加速度センサ３２２の種類については特に限定せず、ＭＥＭＳ（Ｍｉｃｒｏ Ｅｌｅｃｔｒｏ Ｍｅｃｈａｎｉｃａｌ Ｓｙｓｔｅｍ）技術を応用してもよい。本発明の実施形態において、例えば、加速度センサ３２２は、歩行者１０の移動に応じて変化する、ターゲットノード３０の移動速度の変化量を計測し、計測した値をターゲットノード３０の端末座標系加速度データとして距離算出部３２６へ出力する。

・角速度センサ３２４
角速度センサ３２４は、物体の角速度を取得する機能を備える慣性センサである。角速度センサ３２４は、一定の時間に物体が回転した際の角度の変化量を求めるための機器である。角速度センサ３２４の種類には、回転する物体に加わる慣性力から角速度を得る機械式のセンサ、または流路中の気体の流れの変化より角速度を得る流体式のセンサ等があるが、角速度センサ３２４の種類については特に限定せず、ＭＥＭＳ技術を応用してもよい。本発明の実施形態において、例えば、上述の角速度センサ３２４は、歩行者１０の移動に応じて変化する、ターゲットノード３０の姿勢の変化量を計測し、計測した値をターゲットノード３０の端末座標系角速度データとして方向算出部３２８へ出力する。

・距離算出部３２６
距離算出部３２６は、加速度センサ３２２が取得したターゲットノード３０の加速度データに基づき、ターゲットノード３０の進行距離Ｄ（ｔ）を算出する機能を有する。また、進行距離Ｄ（ｔ）の算出方法として、本発明の実施形態では、加速度センサが歩行中に受ける振動の強さと歩行距離の関係より算出する方法を用いる。ただし、進行距離の算出方法は任意の方法でよく、特に限定されない。

進行距離Ｄ（ｔ）の算出にあたり、加速度データを（ａ_ｘ，ａ_ｙ，ａ_ｚ）、振動の強さをｚ（ｔ）、重力加速度をｇとすると、振動の強さｚ（ｔ）は以下の数式２により算出される。

歩行者１０の歩行速度をｖ（ｔ）とすると、歩行速度ｖ（ｔ）は、以下の数式３により算出される。なお、α、βは実験的に求められる定数であり、歩行速度ｖ（ｔ）は、以下の数式３のように線形の式で近似できることが実験的に求められている。

サンプリング周期をＴとすると、進行距離Ｄ（ｔ）は以下の数式４により算出される。

・方向算出部３２８、
方向算出部３２８は、角速度センサ３２４が取得したターゲットノード３０の角速度データに基づき、ターゲットノード３０の進行角度Ψ（進行方向）を算出する機能を有する。

進行角度Ψは、本発明の実施形態では、ターゲットノード３０の端末姿勢（Ψ、θ、φ）を算出することで取得される。ターゲットノード３０の端末姿勢（Ψ、θ、φ）は、世界座標系から端末座標系へＺ軸、Ｙ軸、Ｘ軸の順に回転させた角度と定義し、それぞれの角度をヨー角Ψ、ピッチ角θ、ロール角φと称する。ヨー角Ψは、北極を基準とした世界座標系における進行角度Ψである。端末姿勢（Ψ、θ、φ）は、角速度センサ３２４が検出する角速度データ（ω_ｘ、ω_ｙ、ω_ｚ）から連続的に算出される。

さらに、端末姿勢（Ψ、θ、φ）は、計算過程で特異点の影響を受けないクォータニオンを用いた方法により算出される。ただし、上述の進行角度Ψ及び端末姿勢（Ψ、θ、φ）の算出方法は任意の方法でよく、特に限定されない。以下では、クォータニオンを用いた方法による具体的な算出方法を説明する。

まず、クォータニオンをｑ＝（ｑ_０、ｑ_１、ｑ_２、ｑ_３）と定義すると、クォータニオンｑ＝（ｑ_０、ｑ_１、ｑ_２、ｑ_３）は端末姿勢（Ψ、θ、φ）を用いて以下の数式５と示される。

ただし、座標変換行列Ｒ（Ψ、θ、φ）は、以下の数式６とし、座標変換行列Ｒ（Ψ、θ、φ）の添え字は行列の各成分とする。

ここで、クォータニオンの基準値をｑ_０＝（ｑ_００、ｑ_１０、ｑ_２０、ｑ_３０）と定義すると、動作開始時のｑ_０は、初期端末姿勢（Ψ_０、θ_０、φ_０）を与えることで、数式５及び数式６から算出される。また、補正処理が実行される度にクォータニオンの基準値ｑ_０を再計算することで、補正処理後の新たな基準値ｑ_０ ^’を算出し、リセット処理以降の処理では新たな基準値ｑ_０ ^’を用いて計算を行う。

クォータニオンｑの時間微分値は、角速度データ（ω_ｘ、ω_ｙ、ω_ｚ）を用いて、以下の数式７と示される。

また、現在のクォータニオン値は、クォータニオンの基準値ｑ_０を基準として積分することで、以下の数式８により算出される。

さらに、端末姿勢（Ψ、θ、φ）は、数式８に示す現在のクォータニオン値に基づき、以下の数式９により算出される。

・位置算出部３３０
位置算出部３３０は、慣性センサが取得した情報に基づくターゲットノード３０のＰＤＲ世界座標系位置（Ｘ^ＰＤＲ、Ｙ^ＰＤＲ）を算出する機能を有する。位置算出部３３０は、距離算出部３２６が算出したターゲットノード３０の進行距離Ｄ（ｔ）、及び方向算出部が算出したターゲットノード３０の進行角度Ψに基づき、ターゲットノード３０のＰＤＲ世界座標系位置（Ｘ^ＰＤＲ、Ｙ^ＰＤＲ）を算出する。

ターゲットノード３０のＰＤＲ世界座標系位置（Ｘ^ＰＤＲ、Ｙ^ＰＤＲ）は、ターゲットノード３０の進行距離Ｄ（ｔ）、及び進行角度Ψを用いて、以下の数式１０により算出される。

（２）制御部３４０
制御部３４０は、通信部３６０が受信した補正情報に基づき、ターゲットノード３０の位置の補正処理を制御する機能を有する。制御部３４０は、算出部３２０より、ターゲットノード３０の位置情報として、ＰＤＲ世界座標系位置（Ｘ^ＰＤＲ、Ｙ^ＰＤＲ）、及び進行角度Ψを取得する。また、制御部３４０は、通信部３６０からターゲットノード３０の位置の補正情報として、補正位置（Ｘ^ＲＳＴ、Ｙ^ＲＳＴ）、及び補正角度γを取得する。制御部３４０は、位置（Ｘ^ＰＤＲ、Ｙ^ＰＤＲ）を補正位置（Ｘ^ＲＳＴ、Ｙ^ＲＳＴ）に置き換え、進行角度Ψに補正角度γを加えることで、ターゲットノード３０の位置情報を補正する。

図６は、ターゲットノード３０の位置と角度の補正処理の概要を示している。図６はアンカーノード２０、及びターゲットノード３０の位置関係を上から見た構図であり、アンカーノード２０、ＬＲＦの検出範囲２２、ＬＲＦ検出位置３２、ＰＤＲ検出位置３４から構成されている。なお、ＬＲＦ検出位置３２は、ＬＲＦによるターゲットノード３０の検出位置のことである。また、ＰＤＲ検出位置３４は、ＰＤＲによるターゲットノード３０の検出位置のことである。

図６Ａ〜図６Ｄは、ターゲットノード３０の検出位置の時系列変化を示している。図６Ａ〜図６Ｄにおいて時刻ｔは、アンカーノード２０がターゲットノード３０からビーコンを受信する度に１増える。例えば、時刻ｔ＝Ｔから、ビーコンを１回受信すると時刻ｔはｔ＝Ｔ＋１となり、ビーコンを１０回受信すると時刻ｔはｔ＝Ｔ＋１０となる。図６Ａは時刻ｔ＝Ｔ、図６Ｂは時刻ｔ＝Ｔ＋１０、図６Ｃ及び図６Ｄは時刻ｔ＝Ｔ＋１５におけるターゲットノード３０の位置を示している。なお、図６Ｃは位置情報を補正しなかった場合の時刻ｔ＝Ｔ＋１５におけるターゲットノード３０の位置、図６Ｄは位置情報を補正した場合のｔ＝Ｔ＋１５におけるターゲットノード３０の位置を示している。

図６Ａは、ターゲットノード３０がＬＲＦに検出された時刻Ｔにおけるターゲットノード３０の位置を示している。この時、ＬＲＦにより算出されたターゲットノード３０の位置はＬＲＦ検出位置３２Ａであり、ＰＤＲにより算出されたターゲットノード３０の位置はＰＤＲ検出位置３４Ａである。

図６Ｂは、ターゲットノード３０がＬＲＦの検出範囲内で最後に検出された時刻Ｔ＋１０におけるターゲットノード３０の位置を示している。この時、ＬＲＦにより算出されたターゲットノード３０の位置はＬＲＦ検出位置３２Ｂであり、ＰＤＲにより算出されたターゲットノード３０の位置はＰＤＲ検出位置３４Ｂである。検出範囲２２内の白丸は、時刻Ｔから時刻Ｔ＋１０の間にＬＲＦに検出されたターゲットノード３０の位置の時系列変化（第１の軌跡）を示している。また、黒丸は、時刻Ｔから時刻Ｔ＋１０の間にＰＤＲに検出されたターゲットノード３０の位置の時系列変化（第２の軌跡）を示している。なお、アンカーノード２０は、ターゲットノード３０がＬＲＦの検出範囲を出たことをきっかけとして、補正情報を算出する。

図６Ｃは、ターゲットノード３０の位置情報が補正されなかった場合の時刻Ｔ＋１５におけるターゲットノード３０の位置を示している。この時、ＰＤＲにより算出されたターゲットノード３０の位置はＰＤＲ検出位置３４Ｃである。図６Ｃでは、ターゲットノード３０の位置が補正されていないため、ＰＤＲ検出位置３４Ｃは累積誤差を含むターゲットノード３０の位置を示している。

図６Ｄは、ターゲットノード３０の位置情報が補正された場合の時刻Ｔ＋１５におけるターゲットノード３０の位置を示している。この時、ＰＤＲにより算出されたターゲットノード３０の位置はＰＤＲ検出位置３４Ｄである。図６Ｄでは、ターゲットノード３０の位置が補正されているため、ＰＤＲ検出位置３４Ｄは累積誤差がリセットされたターゲットノード３０の位置を示している。

（３）通信部３６０
通信部３６０は、ターゲットノード３０とアンカーノード２０を接続するインタフェースであり、ターゲットノード３０がアンカーノード２０と情報を通信する。通信部３６０は、アンカーノード２０との通信において、アンカーノード２０が算出したターゲットノード３０の位置情報の補正情報を受信する。また、通信部３６０は、算出部３２０が算出したターゲットノード３０の位置情報を、ターゲットノード３０へ送信する。

以上、図５、図６を参照しながら、本発明の実施形態に係るターゲットノード３０の構成について説明した。続いて、本発明の実施形態に係るアンカーノード２０、及びターゲットノード３０の動作例について説明する。

＜２−３．動作例＞
図７は、本発明の実施形態に係るアンカーノード２０の動作例を示すフローチャートである。まず、測距装置２２０であるＬＲＦが、検出範囲にてターゲットノード３０を検出する（ステップＳ１０１）。ステップＳ１０１以降、ステップＳ１０３〜ステップＳ１０７の処理とステップＳ１０９〜ステップＳ１１３までの処理は並列処理が可能である。

１つ目の並列処理として、まず、測距装置２２０であるＬＲＦが、アンカーノード２０の世界座標位置（Ｘ^ＡＮ，Ｙ^ＡＮ）からターゲットノード３０までの距離ｄ^ＬＲＦと角度θ^ＬＲＦを検出する（ステップＳ１０３）。ＬＲＦ位置情報取得部２３０は、アンカーノード２０の世界座標位置（Ｘ^ＡＮ，Ｙ^ＡＮ）、及びＬＲＦが検出した（ｄ^ＬＲＦ、θ^ＬＲＦ）に基づき、ターゲットノード３０の世界座標系位置（Ｘ_ｋ ^ＬＲＦ、Ｙ_ｋ ^ＬＲＦ）を算出し、記憶部２６０へ出力する（ステップＳ１０５）。記憶部２６０は、ＬＲＦ位置情報取得部２３０から取得した世界座標系位置（Ｘ_ｋ ^ＬＲＦ、Ｙ_ｋ ^ＬＲＦ）を記憶する（ステップＳ１０７）。

２つ目の並列処理として、通信部２４０はターゲットノード３０から受信したビーコンからパケット情報を取得する（ステップＳ１０９）。ＰＤＲ位置情報取得部２５０は、パケット情報からターゲットノード３０の世界座標系位置（Ｘ^ＰＤＲ，Ｙ^ＰＤＲ）を取得し、記憶部２６０へ出力する（ステップＳ１１１）。記憶部２６０は、ＰＤＲ位置情報取得部２５０から取得した世界座標系位置（Ｘ^ＰＤＲ，Ｙ^ＰＤＲ）を記憶する（ステップＳ１１３）。以上で、アンカーノード２０は並列処理を終了する。

並列処理終了後、アンカーノード２０は、ターゲットノード３０がＬＲＦの検出範囲から出たかどうかを判定する（ステップＳ１１５）。ステップＳ１１５にて、ターゲットノード３０がＬＲＦの検出範囲から出ていないと判定された場合、アンカーノード２０はステップＳ１０３〜ステップＳ１１３の処理を繰り返す。ステップＳ１１５にて、ターゲットノード３０がＬＲＦの検出範囲から出たと判定された場合、ターゲットノード３０の世界座標系位置（Ｘ^ＰＤＲ，Ｙ^ＰＤＲ）、及び世界座標系位置（Ｘ^ＰＤＲ，Ｙ^ＰＤＲ）の時系列データに基づき、算出部２７０はターゲットノード３０の補正情報を算出する（ステップＳ１１７）。補正情報には、補正位置算出部２７２が算出する補正位置、及び補正角度算出部２７４が算出する補正角度γが含まれる。制御部２８０は、算出部２７０が算出した補正情報を、通信部２４０へ出力する。そして、通信部２４０は算出部２７０から取得した補正情報をターゲットノード３０へ送信する（ステップＳ１１９）。以上で、図７に示す、アンカーノード２０が実施する処理を終了する。

以上、図７を参照しながら、本発明の実施形態に係るアンカーノード２０の動作例を示すフローチャートについて説明した。

＜２−４．効果＞
上述の説明より、アンカーノード２０は、ターゲットノード３０が検出範囲内に存在する間に取得した第１の軌跡と第２の軌跡に基づき、補正情報を算出することが可能である。また、ターゲットノード３０は、アンカーノード２０が算出した補正情報を取得し、位置及び方向を補正することが可能である。

以上、図２〜７を参照しながら、本発明の第１の実施形態について説明した。続いて、本発明の第２の実施形態について説明する。

＜＜３．第２の実施形態＞＞
アンカーノード２０の検出範囲内に存在するターゲットノード３０は１台に限らず、複数台存在する場合もある。複数のターゲットノード３０がアンカーノード２０の検出範囲内に存在する場合、アンカーノード２０はどのターゲットノード３０の位置を検出しているかを判別できない。そこで、記憶部２６０に記憶された複数のターゲットノード３０の情報と、アンカーノード２０が検出したターゲットノード３０の情報を紐付けることで、アンカーノード２０が検出したターゲットノード３０を判別する。

＜３−１．アンカーノード２０の構成例＞
図８は、本発明の第２の実施形態に係るアンカーノード２０の構成例を示すブロック図である。第１の実施形態にて説明した図２のアンカーノード２０の構成例と比較し、算出部２７０の構成に相関演算部２７６を追加した点が異なる。なお、その他の構成、及び機能については、＜２−１．アンカーノード２０の構成例＞にて説明した内容と同一のため、本章での説明を省略する。

（１）算出部２７０
・相関演算部２７６
相関演算部２７６は、相関演算の処理を行う機能を有する。相関演算部２７６は、第１の軌跡、及び複数のターゲットノード３０の複数の第２の軌跡に対して相関演算を行う。相関演算の結果、相関演算部２７６は複数の第２の軌跡から、第１の軌跡に対応する１つの第２の軌跡を特定する。なお、本発明の第２の実施形態においては、補正角度算出部２７４が算出した第１の軌跡の回転角と、複数の第２の軌跡の回転角に対して相関演算を行うが、相関演算の方法は任意の方法でよく、特に限定されない。

相関演算部２７６は、アンカーノード２０の検出範囲内に存在する複数のターゲットノード３０の中の１台のターゲットノード３０が、検出範囲の外に出たことを検出したことをきっかけに、相関演算処理を開始する。まず、相関演算部２７６は、記憶部２６０から、検出範囲の外に出たターゲットノード３０の第１の軌跡と、複数のターゲットノード３０の複数の第２の軌跡を取得する。相関演算部２７６は、第１の実施形態と同様の方法で、第１の軌跡の回転角τ_ｋ ^ＬＲＦと複数の第２の軌跡の複数の回転角τ_ｋ ^１：ｎを算出する。ここで、ｎはアンカーノード２０の検出範囲内に存在するターゲットノード３０の数である。相関演算部２７６は、算出したτ_ｋ ^ＬＲＦとτ_ｋ ^１：ｎに対して相関演算を行い、算出された相関値が所定の閾値以上となる第２の軌跡を取得する。相関演算部２７６が第２の軌跡を１つに特定した場合、第１の軌跡及び特定した第２の軌跡の時系列データに基づき、補正位置算出部２７２が補正位置（Ｘ^ＲＳＴ，Ｙ^ＲＳＴ）を、補正角度算出部２７４が補正角度γを算出する。

以上、図８を参照しながら、本発明の第２の実施形態に係るアンカーノード２０の構成例について説明した。続いて、本発明の第２の実施形態に係るターゲットノード３０の構成例について説明する。

＜３−２．ターゲットノード３０の構成例＞
本発明の第２の実施形態に係るターゲットノード３０の構成例は、図５に示した第１の実施形態に係るターゲットノード３０の構成例と同一であり、その詳細は＜２−２．ターゲットノード３０の構成例＞にて説明した内容と同一のため、本章での説明を省略する。

＜３−３．動作例＞
図９は第２の実施形態に係るアンカーノード２０の動作例を示すフローチャートである。第１の実施形態にて説明した、図７のフローチャートと比較し、ステップＳ１１５にてＹｅｓの判定がされて以降の処理に、相関演算の処理が追加されている点が異なる。なお、図９のステップＳ１１５までの処理については、＜２−３．動作例＞にて説明した内容と同一のため、本章での説明を省略する。

図９に示すように、ステップＳ１１５にてターゲットノード３０がＬＲＦの検出範囲から出たことを検出された場合、相関演算部２７６は、記憶部２６０から複数のターゲットノード３０の複数の軌跡を取得する（ステップＳ１２１）。相関演算部２７６は、ターゲットノード３０が検出範囲に入ってから検出範囲を出るまでに取得された、時系列データの各時点における回転角τ_ｋ ^ＬＲＦとτ_ｋ ^１：ｎを算出する。相関演算部２７６は、算出した回転角τ_ｋ ^ＬＲＦとτ_ｋ ^１：ｎに対して相関演算を行う（ステップＳ１２３）。相関演算の結果、相関値が閾値を超えるターゲットノード３０を１台に特定できたかどうかを判定する（ステップＳ１２５）。ステップＳ１２５にて、相関演算部２７６がターゲットノード３０を１台に特定できた場合、そのターゲットノード３０の時系列データに基づき、補正位置算出部２７２は補正位置を、補正角度算出部２７４は補正角度γを算出する（ステップＳ１２７）。通信部２４０は、補正位置及び補正角度γをターゲットノード３０へ送信する（ステップＳ１２９）。なおステップＳ１２５にて、相関演算部２７６がターゲットノード３０を１台に特定できない場合、ターゲットノード３０の位置情報を補正せずに処理を終了する。以上で、図９に示すアンカーノード２０が実施する処理を終了する。

なお本発明の第２の実施形態においては、記憶部２６０に記憶された複数のターゲットノード３０の情報と、アンカーノード２０が検出したターゲットノード３０の情報を相関演算により紐付けしているが、情報の紐付ける方法は任意の方法でよく、特に限定されない。

以上、図９を参照しながら、第２の実施形態に係るアンカーノード２０の動作例について説明した。

＜３−４．効果＞
上述の説明より、アンカーノード２０の検出範囲に複数のターゲットノード３０が存在した場合でも、第２の実施形態に係るアンカーノード２０の構成例により、相関演算処理を行うことで、アンカーノード２０は、複数のターゲットノード３０の中から検出したターゲットノード３０を特定することが可能である。

以上、図８、図９を参照しながら、本発明の第２の実施形態について説明した。続いて、本発明の実施形態に係る変形例について説明する。

＜＜４．変形例＞＞
以下では、本発明の実施形態の幾つかの変形例を説明する。なお、以下に説明する各変形例は、単独で本発明の実施形態に適用されてもよいし、組み合わせで本発明の実施形態に適用されてもよい。また、各変形例は、本発明の実施形態で説明した構成に代えて適用されてもよいし、本発明の実施形態で説明した構成に対して追加的に適用されてもよい。

（第１の変形例）
上述の実施形態では、アンカーノード２０がターゲットノード３０の補正情報を算出し、ターゲットノード３０は補正情報に基づき補正後の角度を算出し、実際に位置の補正を行う例を説明したが、アンカーノード２０がターゲットノード３０の補正後の角度まで算出し、ターゲットノード３０は実際に位置を補正する処理だけを行うようにしてもよい。上述の実施形態では、例えば、アンカーノード２０は算出部２７０が算出した補正情報を通信部２４０からターゲットノード３０へ送信し、ターゲットノード３０の制御部３４０は受信した補正情報に基づきターゲットノード３０の補正後の角度を算出し、位置の補正を行う。上述の処理に変形例を適用した場合、アンカーノード２０の算出部２７０にてターゲットノード３０の補正後の角度まで算出し、通信部２４０がターゲットノード３０へ補正情報を送信し、ターゲットノード３０の制御部３４０は受信した補正情報に基づき、位置の補正を行うだけでよくなる。

（第２の変形例）
上述の実施形態では、ターゲットノード３０はアンカーノード２０から取得した補正情報に基づき、位置と角度の両方を補正する例を説明したが、ターゲットノード３０は、位置または角度のどちらか一方のみを修正してもよい。例えば、アンカーノード２０がターゲットノード３０の位置を測位している時に、測距装置２２０が一時的に遮られ、測距装置２２０が取得する位置情報の信頼性が低下したとする。位置情報の信頼性が低下したことにより、補正情報の算出結果も影響を受けて信頼性が低下する可能性がある。しかし、補正情報の補正位置は影響を受けて信頼性が低下しても、補正角度は影響を受けずに信頼性が担保されている場合、ターゲットノード３０は位置を補正せず、角度のみを補正してもよい。

（第３の変形例）
上述の実施形態では、アンカーノード２０は、ＬＲＦにより検出される第１の軌跡、及びＰＤＲにより検出される第２の軌跡に基づき補正情報を算出する例を説明したが、第２の軌跡を用いずに第１の軌跡のみから補正情報を算出してもよい。例えば、アンカーノード２０は、第１の軌跡の時系列データの任意の時点の位置を補正位置としてよい。その場合、第１の軌跡の時系列データの内少なくとも１点あれば、アンカーノード２０はターゲットノード３０の補正位置を算出することができる。また、例えば、アンカーノード２０は、第１の軌跡の時系列データの任意の２点から補正角度を求めてよい。その場合、第１の軌跡の時系列データの内少なくとも２点あれば、アンカーノード２０はターゲットノード３０の補正角度を算出することができる。ただし、上述の変形例が適用できるのは、アンカーノード２０の検出範囲にターゲットノード３０が１台のみ存在する場合である。アンカーノード２０の検出範囲に複数のターゲットノード３０が存在する場合、第２の実施形態に係る相関演算の際に第２の軌跡を用いるからである。

以上、本発明の実施形態に係る変形例ついて説明した。続いて、本発明の実施形態に係るハードウェア構成について説明する。

＜＜５．ハードウェア構成＞＞
上述した位置測位システムに関する情報処理は、ソフトウェアと、以下に説明するアンカーノード２０のハードウェアとの協働により実現される。

図１０は、アンカーノード２０のハードウェア構成例を示したブロック図である。アンカーノード２０は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）２０２と、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）２０４と、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）２０６と、を備える。また、アンカーノード２０は、入力部２０８と、出力部２１０と、ストレージ装置２１２と、ネットワークインタフェース２１４とを備える。

ＣＰＵ２０２は、演算処理装置および制御装置として機能し、各種プログラムに従ってアンカーノード２０内の動作全般を制御する。また、ＣＰＵ２０２は、マイクロプロセッサであってもよい。ＲＯＭ２０４は、ＣＰＵ２０２が使用するプログラムや演算パラメータ等を記憶する。ＲＡＭ２０６は、ＣＰＵ２０２の実行において使用するプログラムや、その実行において適宜変化するパラメータ等を一時記憶する。これらはＣＰＵバスなどから構成されるホストバスにより相互に接続されている。ＣＰＵ２０２、ＲＯＭ２０４およびＲＡＭ２０６は、ソフトウェアとの協働により図２を参照して説明した制御部２８０の機能を実現し得る。

入力部２０８は、マウス、キーボード、タッチパネル、ボタン、マイクロフォン、センサ、スイッチおよびレバーなどユーザが情報を入力するための入力手段と、ユーザによる入力に基づいて入力信号を生成し、ＣＰＵ２０２に出力する入力制御回路などから構成されている。アンカーノード２０のユーザは、該入力部２０８を操作することにより、アンカーノード２０に対して各種のデータを入力したり処理動作を指示したりすることができる。

出力部２１０は、例えば、ＣＲＴ（Ｃａｔｈｏｄｅ Ｒａｙ Ｔｕｂｅ）ディスプレイ装置、液晶ディスプレイ（ＬＣＤ）装置、プロジェクター装置、ＯＬＥＤ（Ｏｒｇａｎｉｃ Ｌｉｇｈｔ Ｅｍｉｔｔｉｎｇ Ｄｉｏｄｅ）装置およびランプなどの表示装置を含む。また、出力部２１０は、スピーカおよびヘッドホンなどの音声出力装置を含む。

ストレージ装置２１２は、データ格納用の装置である。ストレージ装置２１２は、記憶媒体、記憶媒体にデータを記録する記録装置、記憶媒体からデータを読み出す読出し装置および記憶媒体に記録されたデータを削除する削除装置などを含んでもよい。ストレージ装置２１２は、例えば、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）またはＳＳＤ（Ｓｏｌｉｄ Ｓｔｒａｇｅ Ｄｒｉｖｅ）、あるいは同等の機能を有するメモリ等で構成される。このストレージ装置２１２は、ストレージを駆動し、ＣＰＵ２０２が実行するプログラムや各種データを格納する。

ネットワークインタフェース２１４は、例えば、ネットワークに接続するための通信デバイス等で構成された通信インタフェースである。かかる通信インタフェースは、例えば、Ｂｌｕｅｔｏｏｔｈ（登録商標）またはＺｉｇＢｅｅ（登録商標）等の近距離無線通信インタフェースや、無線ＬＡＮ（Ｌｏｃａｌ Ａｒｅａ Ｎｅｔｗｏｒｋ）、Ｗｉ−Ｆｉ（登録商標）、または携帯通信網（ＬＴＥ、３Ｇ）等の通信インタフェースである。また、ネットワークインタフェース２１４は、有線による通信を行う有線通信装置であってもよい。

以上、図１０を参照しながら、アンカーノード２０のハードウェア構成例について説明した。

＜＜６．むすび＞＞
以上説明したように、本発明の実施形態によれば、アンカーノード２０は、歩行者１０に携帯されたターゲットノード３０が検出範囲内に存在する間、測距装置２２０が検出したターゲットノード３０の位置を、時系列データ（第１の軌跡）として取得することが可能である。また、アンカーノード２０は、ターゲットノード３０が備える慣性センサが検出したターゲットノード３０の位置を、時系列データ（第２の軌跡）として取得することも可能である。さらに、アンカーノード２０は、第１の軌跡と第２の軌跡に基づき、第２の軌跡の誤差を算出し、誤差から補正情報を算出することが可能である。なお、アンカーノード２０は、検出範囲内にターゲットノード３０が複数存在しても、検出範囲から出た１台のターゲットノード３０を特定することが可能である。

ターゲットノード３０は、アンカーノード２０が算出した補正情報に基づき、ターゲットノード３０の位置及び方向を補正することが可能である。

以上より、移動体の位置をより高精度に補正することが可能な、新規かつ改良された情報処理装置、およびプログラムを提供することが可能である。

＜＜７．補足＞＞
なお、添付図面を参照しながら本発明の好適な実施形態について詳細に説明したが、本発明はかかる例に限定されない。本発明の属する技術の分野における通常の知識を有する者であれば、特許請求の範囲に記載された技術的思想の範疇内において、各種の変更例または修正例に想到し得ることは明らかであり、これらについても、当然に本発明の技術的範囲に属するものと了解される。

例えば、本明細書の位置測位システムまたはアンカーノード２０の処理における各ステップは、必ずしもシーケンス図またはフローチャートとして記載された順序に沿って時系列に処理する必要はない。例えば、アンカーノード２０の処理における各ステップは、フローチャートとして記載した順序と異なる順序で処理されても、並列的に処理されてもよい。

また、位置測位システム、アンカーノード２０およびターゲットノード３０に内蔵されるＣＰＵ、ＲＯＭおよびＲＡＭなどのハードウェアに、上述した位置測位システム、アンカーノード２０およびターゲットノード３０の各構成と同等の機能を発揮させるためのコンピュータプログラムも作成可能である。また、該コンピュータプログラムを記憶させた記憶媒体も提供される。

１０ 歩行者
２０ アンカーノード
３０ ターゲットノード
２２０ 測距装置
２３０ ＬＲＦ位置情報取得部
２４０ 通信部
２５０ ＰＤＲ位置情報取得部
２６０ 記憶部
２７０ 算出部
２７２ 補正位置算出部
２７４ 補正角度算出部
２７６ 相関演算部
２８０ 制御部
３２０ 算出部
３２２ 加速度センサ
３２４ 角速度センサ
３２６ 距離算出部
３２８ 方向算出部
３３０ 位置算出部
３４０ 制御部
３６０ 通信部

Claims (16)

1. 移動体が測距装置に検出されている間に前記測距装置が算出した前記移動体の位置から、前記移動体の第１の軌跡を取得する第１の取得部と、
   慣性センサが検出する情報に基づき算出された前記移動体の第２の軌跡を取得する第２の取得部と、
   前記第１の軌跡および前記第２の軌跡に基づき、前記第２の軌跡の誤差を算出する算出部と、
   前記誤差に基づき、前記移動体の位置に関する情報および角度に関する情報の補正を制御する制御部と、
   を備える、情報処理装置。
2. 前記算出部は、前記第１の軌跡を回転する際の基準となる第１の基準点、および前記第２の軌跡を回転する際の基準となる第２の基準点を算出し、前記第１の基準点と前記第２の基準点を重ねた上で、基準点を中心に第１の軌跡の時系列データまたは第２の軌跡の時系列データを回転させ、同一時点における、前記第１の基準点と前記第１の軌跡の時系列データの１点を通る直線、及び前記第２の基準点と前記第２の軌跡の時系列データの１点を通る直線が重なる回転角を算出し、前記回転角に基づき補正角度を算出する、請求項１に記載の情報処理装置。
3. 前記算出部は、前記第１の基準点を通る基準軸と、前記第１の基準点と前記第１の軌跡の時系列データの１点を通る直線がなす第１の角度、及び前記第２の基準点を通る前記基準軸と、前記第２の基準点と前記第２の軌跡の時系列データの１点を通る直線がなす第２の角度を取得し、前記第１の軌跡の時系列データと前記第２の軌跡の時系列データの同一時点における前記第１の角度と前記第２の角度の差に基づき、前記回転角を算出する、請求項２に記載の情報処理装置。
4. 前記算出部は、前記第１の軌跡及び前記第２の軌跡の時系列データの数だけ前記回転角を算出し、算出した複数の前記回転角の平均を算出し、算出した平均の前記回転角を前記補正角度とする、請求項３に記載の情報処理装置。
5. 前記制御部は、前記測距装置の検出範囲にて検出された前記移動体の第１の軌跡、および前記算出部が算出した補正角度に基づき、前記移動体の前記位置に関する情報および前記角度に関する情報の補正を制御する、請求項１〜４のいずれか一項に記載の情報処理装置。
6. 前記第１の取得部は、前記移動体が前記測距装置の検出範囲に入ってから、前記移動体が前記検出範囲から出るまでの期間の前記移動体の前記第１の軌跡を取得する、請求項１〜５のいずれか一項に記載の情報処理装置。
7. 前記第１の取得部は、前記測距装置の位置を示す世界座標系位置データ、および前記測距装置が算出した前記移動体までの距離と角度に基づき、前記移動体の第１の世界座標系位置データを算出する、請求項６に記載の情報処理装置。
8. 前記第１の取得部は、前記第１の世界座標系位置データの時系列データに基づき、前記移動体の前記第１の軌跡を取得する、請求項７に記載の情報処理装置。
9. 前記第２の取得部は、前記慣性センサが検出した情報に基づき算出された前記移動体の第２の世界座標系位置データを取得し、取得した前記第２の世界座標系位置データの時系列データに基づき、前記移動体の前記第２の軌跡を取得する、請求項１〜８のいずれか一項に記載の情報処理装置。
10. 前記第２の取得部は、前記移動体から送信される情報が所定の条件を満たす場合、前記移動体の前記第２の世界座標系位置データを取得する、請求項９に記載の情報処理装置。
11. 前記第２の取得部は、前記移動体の前記第１の軌跡に基づき、複数の移動体の複数の前記第２の軌跡を解析することにより、前記移動体の前記第１の軌跡に対応する前記移動体の前記第２の軌跡を取得する、請求項１〜１０のいずれか一項に記載の情報処理装置。
12. 前記測距装置は、前記測距装置を中心に全方位を検出範囲として設定可能である、請求項１〜１１のいずれか一項に記載の情報処理装置。
13. 前記情報処理装置は、ネットワークを介して前記移動体と接続する通信部をさらに備え、
    前記第２の取得部は、前記通信部を介して、前記移動体の第２の世界座標系位置データを取得する、請求項１〜１２のいずれか一項に記載の情報処理装置。
14. 移動体が測距装置に検出されている間に前記測距装置が算出した前記移動体の１または２以上の位置を取得する第１の取得部と、
    前記第１の取得部が取得した前記移動体の１または２以上の位置に基づき、前記移動体の位置に関する情報または角度に関する情報の補正を制御する制御部と、
    を備える、情報処理装置。
15. コンピュータを、
    移動体が測距装置に検出されている間に前記測距装置が算出した前記移動体の位置から、前記移動体の第１の軌跡を取得する第１の取得部と、
    慣性センサが検出する情報に基づき算出された前記移動体の第２の軌跡を取得する第２の取得部と、
    前記第１の軌跡および前記第２の軌跡に基づき、前記第２の軌跡の誤差を算出する算出部と、
    前記誤差に基づき、前記移動体の位置に関する情報および角度に関する情報の補正を制御する制御部と、
    として機能させるための、プログラム。
16. コンピュータを、
    移動体が測距装置に検出されている間に前記測距装置が算出した前記移動体の１または２以上の位置を取得する第１の取得部と、
    前記第１の取得部が取得した前記移動体の１または２以上の位置に基づき、前記移動体の位置に関する情報または角度に関する情報の補正を制御する制御部と、
    として機能させるための、プログラム。

Images