特許 7059114 位置計測装置および位置計測方法

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2022-04-15

(45)【発行日】2022-04-25

(54)【発明の名称】位置計測装置および位置計測方法

(51)【国際特許分類】

   G01C 21/28 20060101AFI20220418BHJP

   G01C 21/26 20060101ALI20220418BHJP

【ＦＩ】

G01C21/28

G01C21/26 P

【請求項の数】 11

(21)【出願番号】P 2018114745

(22)【出願日】2018-06-15

(65)【公開番号】P2019219187

(43)【公開日】2019-12-26

【審査請求日】2021-02-08

(73)【特許権者】

【識別番号】000003078

【氏名又は名称】株式会社東芝

(73)【特許権者】

【識別番号】317015294

【氏名又は名称】東芝エネルギーシステムズ株式会社

(74)【代理人】

【識別番号】110001380

【氏名又は名称】特許業務法人東京国際特許事務所

(72)【発明者】

【氏名】鮫田 芳富

(72)【発明者】

【氏名】西川 浩行

(72)【発明者】

【氏名】小川 純平

(72)【発明者】

【氏名】黒田 英彦

(72)【発明者】

【氏名】代田 孝広

(72)【発明者】

【氏名】小田 直敬

(72)【発明者】

【氏名】池田 芳朗

【審査官】高島 壮基

(56)【参考文献】

【文献】国際公開第２０１１／０９３４４７（ＷＯ，Ａ１）

【文献】国際公開第２０１７／１０４６４６（ＷＯ，Ａ１）

【文献】国際公開第２０１５／１０７８５３（ＷＯ，Ａ１）

【文献】特開２０１５－１８４１５９（ＪＰ，Ａ）

【文献】特開２０１４－１６７４６１（ＪＰ，Ａ）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｇ０１Ｃ ２１／００－２１／３６

Ｇ０８Ｇ １／００－９９／００

(57)【特許請求の範囲】

【請求項1】

位置計測の対象となる対象者の動作により変化する３軸の角速度をセンシングする角速度センサと、
前記対象者の動作により変化する３軸方向の加速度をセンシングする加速度センサと、
前記３軸の角速度により求められる回転を示す値に基づいて前記３軸方向の加速度をグローバル座標の水平方向および垂直方向に対応するグローバル加速度に変換する座標変換部と、
前記グローバル加速度に基づいて前記グローバル座標の１歩の区間および歩幅を求める歩幅推定部と、
前記１歩の区間における垂直方向の前記グローバル加速度の位相を進めた値と水平方向の前記グローバル加速度の値とに基づいて進行方向を求める進行方向推定部と、
前記歩幅と前記進行方向に基づいて歩行位置を求める位置推定部と、
を備える位置計測装置。

【請求項2】

垂直方向の前記グローバル加速度の値または前記３軸方向の加速度の合計値における変化の１周期を前記１歩の区間とし、この１歩の区間に対応する垂直方向の前記グローバル加速度の値または前記３軸方向の加速度の合計値における最大値および最小値に基づいて前記歩幅を求め、前記１歩の区間における垂直方向の前記グローバル加速度の位相を進めた値と水平方向の前記グローバル加速度の値とを乗算および累積した値を求め、この値の逆正接に基づいて進行方向を求める、ように構成されている、請求項１に記載の位置計測装置。

【請求項3】

３軸方向の磁界ベクトルをセンシングする磁界センサを備え、
前記座標変換部は、前記３軸方向の磁界ベクトルを前記グローバル座標の水平方向および垂直方向に対応するグローバル磁界ベクトルに変換し、前記グローバル磁界ベクトルが使用場所の磁界の方向に対応するように前記回転を示す値を補正する請求項１または請求項２に記載の位置計測装置。

【請求項4】

前記磁界の方向は、地磁気に基づいて求められる請求項３に記載の位置計測装置。

【請求項5】

前記磁界の方向は、前記使用場所にて予め測定された磁界の値に基づいて求められる請求項３に記載の位置計測装置。

【請求項6】

前記座標変換部は、前記グローバル加速度の平均値により前記グローバル座標の垂直方向を求め、この垂直方向に基づいて前記進行方向の演算に関する値を補正する請求項１から請求項５のいずれか１項に記載の位置計測装置。

【請求項7】

前記歩幅推定部は、垂直方向の前記グローバル加速度の値をバンドパスフィルタで処理し、この処理した値がプラスからマイナスになる時点から、次にプラスからマイナスになる時点までを前記１歩の区間として求める請求項１から請求項６のいずれか１項に記載の位置計測装置。

【請求項8】

前記進行方向推定部は、垂直方向の前記グローバル加速度の値をバンドパスフィルタで処理し、この処理した値の時間の差分をとることで、前記グローバル加速度の位相を進めた値を求める請求項１から請求項７のいずれか１項に記載の位置計測装置。

【請求項9】

前記進行方向推定部は、前記１歩の区間における垂直方向の前記グローバル加速度の最小値が走行判定値以下の場合に、前記対象者が走行していると判定し、前記進行方向を１８０°変換する請求項１から請求項８のいずれか１項に記載の位置計測装置。

【請求項10】

前記座標変換部は、一定時間内の前記３軸の角速度の最大値と最小値の差が静止判定値以下の場合に、前記対象者が静止していると判定し、この静止しているときの前記角速度の平均値を求め、この平均値とオフセット値として設定する請求項１から請求項９のいずれか１項に記載の位置計測装置。

【請求項11】

位置計測の対象となる対象者の動作により変化する３軸の角速度を角速度センサによりセンシングするステップと、
前記対象者の動作により変化する３軸方向の加速度を加速度センサによりセンシングするステップと、
前記３軸の角速度により求められる回転を示す値に基づいて前記３軸方向の加速度をグローバル座標の水平方向および垂直方向に対応するグローバル加速度に変換するステップと、
前記グローバル加速度に基づいて前記グローバル座標の１歩の区間および歩幅を求めるステップと、
前記１歩の区間における垂直方向の前記グローバル加速度の位相を進めた値と水平方向の前記グローバル加速度の値とに基づいて進行方向を求めるステップと、
前記歩幅と前記進行方向に基づいて歩行位置を求めるステップと、
を含む位置計測方法。

【発明の詳細な説明】

【技術分野】

【0001】

本発明の実施形態は、位置計測装置および位置計測方法に関する。

【背景技術】

【0002】

近年、位置測位サービスの増加に伴い、位置測位技術に注目が集まっている。例えば、ＧＰＳを使用できない屋内において、歩行者が携行する端末の取得した歩行情報に基づいて位置を推定するものがある。しかし、端末の取り付け位置または方向がずれてしまうと正しく進行方向を推定することができない。そこで、垂直方向の加速度成分の変化が極大のときと極小のときとの間で、水平方向の加速度成分の変化が極大を示すときに、この水平方向の加速度成分の方向を進行方向として推定する技術が知られている。

【先行技術文献】

【特許文献】

【0003】

【文献】特許第４１２６３８８号公報

【発明の概要】

【発明が解決しようとする課題】

【0004】

前述の技術では、水平方向の加速度成分のみを用いて進行方向を推定しているので、測定ノイズの影響を受け易く、精度のよい進行方向の推定が難しい。そのため、歩行者の位置の計測精度を向上させることができないという課題がある。

【0005】

本発明の実施形態は、このような事情を考慮してなされたもので、歩行位置の計測精度を向上させることができる位置計測技術を提供することを目的とする。

【課題を解決するための手段】

【0006】

本発明の実施形態に係る位置計測装置は、位置計測の対象となる対象者の動作により変化する３軸の角速度をセンシングする角速度センサと、前記対象者の動作により変化する３軸方向の加速度をセンシングする加速度センサと、前記３軸の角速度により求められる回転を示す値に基づいて前記３軸方向の加速度をグローバル座標の水平方向および垂直方向に対応するグローバル加速度に変換する座標変換部と、前記グローバル加速度に基づいて前記グローバル座標の１歩の区間および歩幅を求める歩幅推定部と、前記１歩の区間における垂直方向の前記グローバル加速度の位相を進めた値と水平方向の前記グローバル加速度の値とに基づいて進行方向を求める進行方向推定部と、前記歩幅と前記進行方向に基づいて歩行位置を求める位置推定部と、を備える。

【発明の効果】

【0007】

本発明の実施形態により、歩行位置の計測精度を向上させることができる位置計測技術が提供される。

【図面の簡単な説明】

【0008】

【図1】本実施形態の位置計測装置としての端末を示す斜視図。

【図2】位置計測装置としての端末のシステム構成を示すブロック図。

【図3】メイン制御処理を示すフローチャート。

【図4】座標変換処理を示すフローチャート。

【図5】歩幅推定処理を示すフローチャート。

【図6】進行方向推定処理を示すフローチャート。

【図7】人の歩行の例を示す模式図。

【図8】人の歩行と加速度の関係を示すグラフ。

【図9】人の歩行の例を示す説明図。

【図10】各加速度の関係を示すグラフ。

【図11】各加速度の関係を示すグラフ。

【図12】本実施形態と従来技術の歩行位置の例を示す説明図。

【図13】変形例１の進行方向推定処理を示すフローチャート。

【図14】人の走りの例を示す模式図。

【図15】人の走りと加速度の関係を示すグラフ。

【図16】変形例２の座標変換処理を示すフローチャート。

【発明を実施するための形態】

【0009】

以下、図面を参照しながら、位置計測装置の実施形態について詳細に説明する。まず、本実施形態の位置計測装置について図１から図１６を用いて説明する。

【0010】

図１の符号１は、本実施形態の位置計測装置としての端末である。この端末１は、例えば、工場またはプラントなどの作業現場において、作業者である人の位置を特定するために用いられる。なお、作業現場のみならず、他の場所で端末１を用いても良い。また、端末１は、人が歩行により移動したときに、その歩幅と進行方向とを推定することで、移動後の位置を推定するようにしている。

【0011】

図１に示すように、端末１は、スマートフォンなどの小型の携帯端末から成る。この端末１は、位置計測の対象者である人が所持し、この人の歩行または走りなどの動作を検出する。また、この端末１は、人が着用している服のポケットに収容されて使用される。そのため、端末１の向きは、人に対して固定的ではなく、端末１の向き（傾き）が常に変化してしまう可能がある。

【0012】

以下の説明において、人が居る空間の絶対的な座標系をグローバル座標Ｃと称する。このグローバル座標Ｃは、Ｘ軸とＹ軸が地面（床面）に沿って延び、Ｚ軸が垂直方向（鉛直方向）に沿って延びる座標系である。つまり、地面に対して固定的な座標系がグローバル座標Ｃである。これに対して端末１に搭載されるセンサで検出されるｕ軸，ｖ軸，ｗ軸の座標系がある。この端末１を中心とした座標系を端末座標Ｃ’と称する。この端末座標Ｃ’は、端末１に対して固定的な座標系である。端末１が回転されると、端末座標Ｃ’の３軸方向がグローバル座標Ｃの３軸方向に対して変化する。

【0013】

次に、位置計測装置としての端末１のシステム構成を図２に示すブロック図を参照して説明する。本実施形態の位置計測装置は、プロセッサおよびメモリなどのハードウェア資源を有し、ＣＰＵが各種プログラムを実行することで、ソフトウェアによる情報処理がハードウェア資源を用いて実現されるコンピュータで構成される。さらに、本実施形態の位置計測方法は、プログラムをコンピュータに実行させることで実現される。

【0014】

端末１は、３軸角速度センサ２と、３軸加速度センサ３と、３軸磁界センサ４と、座標変換部５と、歩幅推定部６と、進行方向推定部７と、位置推定部８と、情報出力部９とを備える。これらは、メモリまたはＨＤＤに記憶されたプログラムがＣＰＵによって実行されることで実現される。

【0015】

３軸角速度センサ２は、端末座標Ｃ’の３軸の角速度をセンシングする。３軸加速度センサ３は、端末座標Ｃ’の３軸方向の加速度をセンシングする。３軸磁界センサ４は、端末座標Ｃ’の３軸方向の磁界ベクトルをセンシングする。なお、３軸加速度センサ３によりセンシングされる加速度の値には、人の歩行により生じる加速度のみならず、重力加速度も含まれる。

【0016】

座標変換部５は、端末座標Ｃ’の３軸の角速度により求められた端末１の回転状態に基づいて端末座標Ｃ’の３軸方向の加速度をグローバル座標Ｃの水平方向および垂直方向に対応するグローバル加速度に変換する処理を行う。歩幅推定部６は、グローバル加速度に基づいてグローバル座標Ｃの１歩の区間および歩幅を求める処理を行う。

【0017】

進行方向推定部７は、１歩の区間における垂直方向のグローバル加速度の位相を進めた値と水平方向のグローバル加速度の値とに基づいて進行方向を求める処理を行う。位置推定部８は、歩幅と進行方向に基づいて歩行位置を求める処理を行う。なお、歩行位置とは、所定の位置から人が歩行したときに、人が歩行後に居る位置である。

【0018】

情報出力部９は、歩行位置などの所定の情報を出力する。この情報出力部９は、所定の情報を表示するディスプレイで構成される。また、情報出力部９は、音声で所定の情報を通知するスピーカでも良いし、遠隔地に所定の情報を送信する送信装置でも良いし、所定の情報を記憶媒体に記憶させる記憶装置でも良い。

【0019】

次に、具体的な処理内容を以下に説明する。３軸角速度センサ２は、端末１の各軸ｕ，ｖ，ｗ回りの角速度ω_ｕ（ｔ），ω_ｖ（ｔ），ω_ｗ（ｔ）［ｒａｄ／ｓ］を、所定のサンプリング間隔ΔＴ［ｓ］でセンシングする。３軸加速度センサ３は、端末１の各軸ｕ，ｖ，ｗ方向の加速度Ａ_ｕ（ｔ），Ａ_ｖ（ｔ），Ａ_ｗ（ｔ）［ｍ／ｓ^２］を、所定のサンプリング間隔ΔＴ［ｓ］でセンシングする。３軸磁界センサ４は、端末１の各軸ｕ，ｖ，ｗ方向の磁界Ｂ_ｕ（ｔ），Ｂ_ｖ（ｔ），Ｂ_ｗ（ｔ）［μＴ］を、所定のサンプリング間隔ΔＴ［ｓ］でセンシングする。

【0020】

座標変換部５は、角速度センサ２で得られた角速度から端末１の回転を演算する。また、磁界センサ４の値をグローバル座標Ｃに変換する。また、グローバル座標Ｃの磁界ベクトルが、地磁気に基づく北方向、または予め作成した磁界マップに一致するように端末１の回転を示す値を補正する。このようにすれば、端末１の使用場所の磁界の方向に基づいて端末１の回転状態を補正されるので、端末１の位置、つまり歩行位置の計測精度を向上させることができる。

【0021】

また、座標変換部５は、加速度センサ３の値をグローバル座標Ｃに変換し、グローバル座標Ｃの加速度ベクトル平均値が垂直方向になるように端末１の回転を示す値を補正する。つまり、座標変換部５は、グローバル加速度の平均値によりグローバル座標Ｃの垂直方向であるＺ方向を求め、この垂直方向に対応するように端末１の回転状態を補正する。このようにすれば、求められたグローバル座標Ｃの垂直方向の値に誤差がある場合に、その誤差を修正することができる。

【0022】

歩幅推定部６は、座標変換部５で得られた垂直方向の加速度をバンドパスフィルタにかける。そして、垂直方向の加速度の値がプラスからマイナスになる時点から次にプラスからマイナスになる時点までを１歩の区間として検出し、その区間の垂直方向または３軸合成の加速度の最大値および最小値から歩幅を演算する。なお、人が静止した状態から歩行を開始するときには、最初に人の重心が下がり、その後に重心が上がる（図７参照）。そのため、垂直方向の加速度の値がプラスからマイナスになる時点を１歩の最初の時点として処理を行う。このようにすれば、グローバル加速度の値に基づいて１歩の区間を簡素な処理で求めることができる。

【0023】

進行方向推定部７は、グローバル座標Ｃの垂直方向の加速度の位相を進めた値と、グローバル座標Ｃの１歩の区間のＸ方向、Ｙ方向を乗算および累積して、逆正接（arctan）から進行方向を演算する。このようにすれば、歩幅と進行方向の計測精度を向上させることができる。

【0024】

位置推定部８は、歩幅推定部６が求めた歩幅と、進行方向推定部７が求めた進行方向から人の歩行位置を求める。情報出力部９は、位置推定部８が求めた歩行位置を示す情報を出力する。

【0025】

次に、位置計測装置としての端末１が実行するメイン制御処理について図３のフローチャートを用いて説明する。なお、図２に示すブロック図を適宜参照する。この処理は、一定時間毎に繰り返される処理である。

【0026】

図３に示すように、まず、ステップＳ１において、３軸角速度センサ２は、端末座標Ｃ’の３軸の角速度をセンシングする。次のステップＳ２において、３軸加速度センサ３は、端末座標Ｃ’の３軸の加速度をセンシングする。次のステップＳ３において、３軸磁界センサ４は、端末座標Ｃ’の３軸の磁界をセンシングする。

【0027】

次のステップＳ４において、端末１は、座標変換処理（図４参照）を実行する。次のステップＳ５において、端末１は、歩幅推定処理（図５参照）を実行する。次のステップＳ６において、端末１は、進行方向推定処理（図６参照）を実行する。

【0028】

次のステップＳ７において、端末１は、位置推定処理を実行する。次のステップＳ８において、端末１は、情報出力処理を実行する。そして、処理を終了する。

【0029】

次に、位置計測装置としての端末１が実行する座標変換処理について図４のフローチャートを用いて説明する。なお、図２に示すブロック図を適宜参照する。この処理は、一定時間毎に繰り返される処理である。

【0030】

図４に示すように、まず、ステップＳ１１において、座標変換部５は、端末１の回転演算を行う。座標変換部５は、端末座標Ｃ’の角速度ω_ｕ（ｔ），ω_ｖ（ｔ），ω_ｗ（ｔ）を用いて、端末１の回転ベクトルｑ（ｔ）＝｛ｑ_０（ｔ），ｑ_１（ｔ），ｑ_２（ｔ），ｑ_３（ｔ）｝を求める。なお、回転ベクトルはクォータニオンである。ここで、端末１の各軸のベクトルをｕ，ｖ，ｗとすると、以下の数式（１）～（３）により、端末１の回転ベクトルｑ（ｔ）を求めることができる。

【0031】

n=uω_u(t)+vω_v(t)+wω_w(t) （１）
θ=ΔT√(ω² _u(t)+ω² _v(t)+ω² _w(t)) （２）
q(t)=q(t-ΔT)(cos(θ/2)+n sin(θ/2)) （３）

【0032】

次のステップＳ１２において、座標変換部５は、磁界の座標変換を行う。座標変換部５は、端末１の各軸ｕ，ｖ，ｗ方向の磁界Ｂ_ｕ（ｔ），Ｂ_ｖ（ｔ），Ｂ_ｗ（ｔ）をグローバル座標ＣのＸ，Ｙ，Ｚへ座標変換する。端末１の回転ベクトルｑ（ｔ）の共役クォータニオンをｑ^＊（ｔ）とすれば、以下の数式（４）により、グローバル座標Ｃの磁界Ｂ_ｘ（ｔ），Ｂ_ｙ（ｔ），Ｂ_ｚ（ｔ）を求めることができる。

【0033】

{B_x(t)，B_y(t)，B_z(t)}=q(t){B_u(t)，B_v(t)，B_w(t)}q^*(t) （４）

【0034】

次のステップＳ１３において、座標変換部５は、磁界に基づいて端末１の回転を示す値の補正を行う。座標変換部５は、地磁気に基づいて、グローバル座標Ｃの磁界の向きが北方向になるように、端末１の回転を示す値を補正する。正確には、磁界ベクトルは、その使用場所での偏角となるはずである。例えば、東京周辺の偏角は西偏約７度である。そこで、グローバル座標ＣのＸ方向を北とするならば、以下の数式（５）となるように、端末１の回転ベクトルｑ（ｔ）を補正する。このようにすれば、地磁気により端末１の回転状態を補正することができる。

【0035】

arctan(B_y(t)/B_x(t))=偏角 （５）

【0036】

鉄筋鉄骨コンクリート製の建物の内部では、地磁気が遮蔽され、室内の磁界の方向が北とならない場合がある。このような場合は、測位するエリアの磁界を、磁界測定装置を用いて予め測定して磁界マップを作成しておく。そして、前述の数式（４）で求めた磁界の方向が、事前に作成した磁界マップと一致するように端末１の回転ベクトルｑ（ｔ）を補正する。このようにすれば、地磁気の影響を受けない建物内であっても端末１の回転状態を補正することができる。

【0037】

次のステップＳ１４において、座標変換部５は、加速度の座標変換を行う。座標変換部５は、端末１の各軸ｕ，ｖ，ｗ方向の加速度Ａ_ｕ（ｔ），Ａ_ｖ（ｔ），Ａ_ｗ（ｔ）をグローバル座標ＣのＸ，Ｙ，Ｚへ座標変換する。端末１の回転ベクトルｑ（ｔ）の共役クォータニオンをｑ^＊（ｔ）とすれば、以下の数式（６）により、グローバル座標Ｃの加速度Ａ_ｘ（ｔ），Ａ_ｙ（ｔ），Ａ_ｚ（ｔ）を求めることができる。

【0038】

{A_x(t)，A_y(t)，A_z(t)}=q(t){A_u(t)，A_v(t)，A_w(t)}q^*(t) （６）

【0039】

次のステップＳ１５において、座標変換部５は、加速度による回転の補正を行う。ここで、グローバル座標Ｃの加速度平均値は、垂直方向になるはずなので、以下の数式（７）～（９）となるように、端末１の回転ベクトルｑ（ｔ）を補正する。ただし、Ａｖｅｒａｇｅ（ｘ）は、例えば、３秒間の平均値を示す。そして、処理を終了する。

【0040】

Average(A_x(t))=0 （７）
Average(A_y(t))=0 （８）
Average(A_z(t))=G （９）

【0041】

次に、位置計測装置としての端末１が実行する歩幅推定処理について図５のフローチャートを用いて説明する。なお、図２に示すブロック図を適宜参照する。この処理は、一定時間毎に繰り返される処理である。

【0042】

図５に示すように、まず、ステップＳ２１において、歩幅推定部６は、グローバル座標Ｃの加速度Ａ_ｘ（ｔ），Ａ_ｙ（ｔ），Ａ_ｚ（ｔ）をバンドパスフィルタにかけて、Ａｂ_ｘ（ｔ），Ａｂ_ｙ（ｔ），Ａｂ_ｚ（ｔ）を求める。バンドパスフィルタは、例えば、１～５Ｈｚ程度を通過帯域とする。

【0043】

次のステップＳ２２において、歩幅推定部６は、１歩区間の検出を行う。例えば、加速度の値がプラスからマイナスになる時点から、次にプラスからマイナスになる時点までを１歩の区間として検出する。

【0044】

次のステップＳ２３において、歩幅推定部６は、１歩の区間のバンドパスフィルタした垂直方向の加速度Ａｂ_ｚ（ｔ）の最大値および最小値を求める。

【0045】

次のステップＳ２４において、歩幅推定部６は、以下の数式（１０）により、歩幅を求める。ただし、例えば、Ｎ＝１／４ とする。そして、処理を終了する。

【0046】

歩幅=係数×(Ab_z(t)の最大値-Ab_z(t)の最小値)^N （１０）

【0047】

次に、位置計測装置としての端末１が実行する進行方向推定処理について図６のフローチャートを用いて説明する。なお、図２に示すブロック図を適宜参照する。この処理は、一定時間毎に繰り返される処理である。

【0048】

図６に示すように、まず、ステップＳ３１において、進行方向推定部７は、グローバル座標Ｃの加速度Ａ_ｘ（ｔ），Ａ_ｙ（ｔ），Ａ_ｚ（ｔ）をバンドパスフィルタにかけて、Ａｂ_ｘ（ｔ），Ａｂ_ｙ（ｔ），Ａｂ_ｚ（ｔ）を求める。バンドパスフィルタは、例えば、１～５Ｈｚ程度を通過帯域とする。

【0049】

次のステップＳ３２において、進行方向推定部７は、垂直方向加速度の位相を進める処理を行う。ここで、位相を進めた垂直方向加速度Ａｄ_ｚ（ｔ）は、例えば、以下の数式（１１）によって求める。ここで、Δ₁，Δ₂は差分の時間である。

【0050】

Ad_z(t)=Ab_z(t+Δ₁)-Ab_z(t－Δ₂) （１１）

【0051】

または、単に、Ａｄ_ｚ（ｔ）＝Ａｂ_ｚ（ｔ＋Δ）として求めてもよい。この場合は、例えば、Δ＝１歩の時間／４ とする。

【0052】

次のステップＳ３３において、進行方向推定部７は、１歩の区間のＸ方向加速度、Ｙ方向加速度と、位相を進めた垂直方向加速度を乗算および累積する処理を行う。ここで、以下の数式（１２）および（１３）によって、１歩の区間のＸ方向、Ｙ方向の乗算および累積を求める。

【0053】

S_x=Σ{Ab_x(t)Ad_z(t)} （１２）
S_y=Σ{Ab_y(t)Ad_z(t)} （１３）

【0054】

次のステップＳ３４において、進行方向推定部７は、逆正接演算を行い、進行方向を求める処理を行う。ここで、以下の数式（１４）によって、進行方向を求める。そして、処理を終了する。

【0055】

進行方向=arctan(S_y/S_x) （１４）

【0056】

本実施形態では、進行方向推定部７は、垂直方向のグローバル加速度の値をバンドパスフィルタで処理し、この処理した値の時間の差分をとることで、グローバル加速度の位相を進めた値を求める。このようにすれば、時間の差分を用いることで、グローバル加速度の位相を進めた値を求める処理を容易に行うことができる。

【0057】

なお、本実施形態では、ステップＳ３２において、進行方向推定部７が、垂直方向加速度の位相を進める処理を行っているが、その他の態様であっても良い。例えば、進行方向推定部７が、端末座標Ｃ’の３軸方向の加速度の合計値の位相を進める処理を行っても良い。つまり、位相を進める対象となる値は、少なくとも垂直方向加速度の成分が含まれている値であれば良い。

【0058】

図３に示すように、メイン制御処理において、前述の座標変換処理、歩幅推定処理、座標変換処理の後、ステップＳ７の位置推定処理において、位置推定部８は、歩幅推定部６で求めたｉ歩目の歩幅［ｉ］と、進行方向推定部７で求めたｉ歩目の進行方向［ｉ］から、ｉ歩目の歩行位置ｘ［ｉ］，ｙ［ｉ］を以下の数式（１５）および（１６）によって求める。

【0059】

歩行位置x[i]=x[i-1]+歩幅[i]cos(進行方向[i]) （１５）
歩行位置y[i]=y[i-1]+歩幅[i]sin(進行方向[i]) （１６）

【0060】

そして、ステップＳ８の情報出力処理において、情報出力部９は、位置推定処理で求められた歩行位置を出力する。

【0061】

図７は、人Ｐの歩行の例を示す模式図である。歩行に伴って、端末１を取り付けた人Ｐの上半身（重心位置）は上下する。ここで、人Ｐが通常の省エネルギーな歩行を行う場合には、位置エネルギーと運動エネルギーは相互に変換される。そのため、端末１の位置が下がるときに進行方向に向かって加速され、端末１の位置が上がるときに進行方向に減速される。

【0062】

図８は、人の歩行と加速度の関係を示すグラフである。これらのグラフの横軸は時間を表し、縦軸は変化量を表す。図８において、一点鎖線のグラフＬ１は、位置ｈ（ｔ）を示す。点線のグラフＬ２は、垂直方向の加速度ｈ’’（ｔ）を示す。実線のグラフＬ３は、位相を進めた垂直方向の加速度ｈ’’（ｔ＋π／２）を示す。二点鎖線のグラフＬ４は、進行方向の加速度ｖ’（ｔ）を示す。

【0063】

図８に示すように、人Ｐが複数歩の歩行を行っているとき、端末１の位置と垂直方向の加速度は、逆位相になる。さらに、位相を進めた垂直方向の加速度と進行方向の加速度は、ほぼ同位相になる。なお、位相を進めるとは、グラフの波形が時間的に先に変化することを示す。グラフＬ２の位相を進めることで、グラフＬ３になっている。

【0064】

端末の位置ｈ（ｔ）と速度ｖ（ｔ）の間でエネルギーが保存されるとすれば、以下の数式（１７）の関係がある。ただし、ｍは人の質量、Ｅは保持するエネルギーである。

【0065】

m h(t)+(1/2)m v²(t)=E （１７）

【0066】

また、端末の位置ｈ（ｔ）を以下の数式（１８）のようにモデル化する。ただし、Ｃ＝２π／Ｔ、Ｔは１歩の周期である。

【0067】

h(t)=cos(Ct) （１８）

【0068】

ここで、垂直方向の加速度ｈ’’（ｔ）と水平方向の加速度ｖ_ｘ’（ｔ），ｖ_ｙ’（ｔ）は、以下の数式（１９）～（２１）となる。だたし、θは進行方向である。

【0069】

h''(t)=-C²cos(Ct) （１９）
v_x'(t)=(C√2sin(Ct))/(2(E/m-cos(Ct))^1/2)cos(θ) （２０）
v_y'(t)=(C√2sin(Ct))/(2(E/m-cos(Ct))^1/2)sin(θ) （２１）

【0070】

例えば、ｈ’’（ｔ）の位相をπ／２進めると以下の数式（２２）となる。

【0071】

h''(t+π/2)=C² sin(Ct) （２２）

【0072】

１歩の区間の加速度ｖ_ｘ’（ｔ），ｖ_ｙ’（ｔ）と、位相を進めた垂直方向加速度ｈ’’（ｔ＋π／２）を乗算および累積すれば、以下の数式（２３）および（２４）となる。

【0073】

S_x=Σ{v'_x(t)h''(t+π/2)}
=Σ{(C√2 sin(Ct))/(2(E/m-cos(Ct))^1/2)cos(θ) C² sin(Ct)}
=C³√2 Σ{sin²(Ct)/(2(E/m-cos(Ct))^1/2)}cos(θ) （２３）

【0074】

S_y=Σ{v'_y(t)h''(t+π/2)}
=Σ{(C√2sin(Ct))/(2(E/m-cos(Ct))^1/2)sin(θ) C²sin(Ct)}
=C³√2 Σ{sin²(Ct)/(2(E/m-cos(Ct))^1/2)}sin(θ) （２４）

【0075】

ここで、逆正接を求めれば、以下の数式（２５）となり、進行方向が求められる。

【0076】

進行方向=arctan(S_y/S_x)
=｛C³√2 Σ{sin²(Ct)/(2(E/m-cos(Ct))^1/2)}sin(θ)｝
/｛C³√2 Σ{sin²(Ct)/(2(E/m-cos(Ct))^1/2)}cos(θ)｝
=sin(θ)/cos(θ)
=θ （２５）

【0077】

図９は、人の歩行の例を示す説明図である。人Ｐが歩くときに、１～４歩目はＸ方向に歩き、５～８歩目は概ねＹ方向に横歩きした例を示している。このときの加速度波形の例を図１０および図１１に示す。図１０において、実線のグラフＬ５は、垂直方向加速度Ａ_ｚを示す。点線のグラフＬ６は、バンドパスフィルタした垂直方向加速度Ａｂ_ｚを示す。図１１において、実線のグラフＬ７は、バンドパスフィルタして位相進めた垂直方向加速度Ａｄ_ｚを示す。点線のグラフＬ８は、バンドパスフィルタしたＸ方向加速度Ａｂ_ｘを示す。一点鎖線のグラフＬ９は、バンドパスフィルタしたＹ方向加速度Ａｂ_ｙを示す。

【0078】

垂直方向加速度Ａ_ｚをバンドパスフィルタにかけて、バンドパスフィルタした垂直方向加速度Ａｂ_ｚを求める。バンドパスフィルタした垂直方向加速度Ａｂ_ｚの値がプラスからマイナスになる時点から、次にプラスからマイナスになる時点までを１歩の区間として検出する。

【0079】

１～４歩目では、バンドパスフィルタして位相進めた垂直方向加速度Ａｄ_ｚとバンドパスフィルタしたＸ方向加速度Ａｂ_ｘとがほぼ同位相である。４～７歩目では、バンドパスフィルタして位相進めた垂直方向加速度Ａｄ_ｚとバンドパスフィルタしたＹ方向加速度Ａｂ_ｙとがほぼ同位相である。

【0080】

図１２は、本実施形態と従来技術の歩行位置の例を示す説明図である。人Ｐが歩くときに、１～４歩目はＸ方向に歩き、５～８歩目は概ねＹ方向に横歩きした例を示している。図１２において、実線のグラフＪ１は、本実施形態の端末１により求めた歩行位置を示す。点線のグラフＪ２は、従来技術の端末により求めた歩行位置を示す。

【0081】

従来技術の端末により進行方向を求めた歩行位置では、５～８歩目においても端末（身体）が概ねＸ方向を向いているため、Ｘ方向に進んでいるような値が算出される。これに対して、本実施形態の端末１により進行方向を求めた歩行位置では、Ｙ方向に進んでいる値が算出されることが分かる。このように、本実施形態では、歩行位置の計測精度を向上させることができる。

【0082】

図１３は、変形例１の進行方向推定処理を示すフローチャートである。なお、図２に示すブロック図を適宜参照する。この処理は、一定時間毎に繰り返される処理である。この変形例１の進行方向推定処理は、ステップＳ３１からステップＳ３４が前述の進行方向推定処理（図６）と同様のステップであるので、説明を省略する。

【0083】

図１３に示すように、ステップＳ３４の次に進むステップＳ３５において、進行方向推定部７は、１歩の区間の垂直方向の加速度Ａ_ｚ（ｔ）の最小値を求める。

【0084】

次のステップＳ３６において、進行方向推定部７は、垂直方向の加速度Ａ_ｚ（ｔ）の最小値が、走行判定値である一定値以下であるか否かを判定する。ここで、垂直方向の加速度Ａ_ｚ（ｔ）の最小値が一定値以下の場合（ステップＳ３６がＹＥＳ）は、人が走っていると判定して、ステップＳ３７に進む。一方、垂直方向の加速度Ａ_ｚ（ｔ）の最小値が一定値を超えている場合（ステップＳ３６がＮＯ）は、処理を終了する。

【0085】

本実施形態では、最小値が、走行判定値である一定値以下の場合、例えば、垂直方向の加速度Ａ_ｚ（ｔ）の最小値がゼロ以下の場合に走りと判定する。人が走り始めると足が地面から離れる瞬間があるので、垂直方向の加速度Ａ_ｚ（ｔ）の最小値がゼロ以下になる瞬間が生じる。このときの加速度を判定に用いる。

【0086】

次のステップＳ３７において、進行方向推定部７は、走りと判定された場合に、進行方向を１８０°変換する。これにより、走りの場合でも正しく進行方向を推定できる。そして、処理を終了する。

【0087】

図１４は、人の走りの例を示す模式図である。走りに伴って、端末１を取り付けた人Ｐの上半身は上下する。人Ｐが跳躍するとき、即ち端末位置が上がるときに進行方向に加速され、人Ｐが着地するとき、即ち端末位置が下がるときに進行方向に減速される。

【0088】

図１５は、人の走りと加速度の関係を示すグラフである。これらのグラフの横軸は時間を表し、縦軸は変化量を表す。図１５において、一点鎖線のグラフＬ１１は、位置ｈ（ｔ）を示す。点線のグラフＬ１２は、垂直方向の加速度ｈ’’（ｔ）を示す。実線のグラフＬ１３は、位相を進めた垂直方向の加速度ｈ’’（ｔ＋π／２）を示す。二点鎖線のグラフＬ１４は、進行方向の加速度ｖ’（ｔ）を示す。

【0089】

これらのグラフに示すように、端末１の位置と垂直方向の加速度は逆位相になる。さらに、位相を進めた垂直方向の加速度と進行方向の加速度は逆位相になる。

【0090】

変形例１において、進行方向推定部７は、１歩の区間における垂直方向のグローバル加速度の最小値が走行判定値以下の場合に、対象者である人が走行していると判定し、進行方向を１８０°変換する。このようにすれば、対象者である人が走行しても進行方向を求めることができる。

【0091】

図１６は、変形例２の座標変換処理を示すフローチャートである。なお、図２に示すブロック図を適宜参照する。この処理は、一定時間毎に繰り返される処理である。この変形例２の座標変換処理は、ステップＳ１１からステップＳ１５が前述の座標変換処理（図４）と同様のステップであるので、説明を省略する。

【0092】

図１６に示すように、ステップＳ１５の次に進むステップＳ１６において、座標変換部５は、一定時間内の角速度の最大－最小の値を求める４６は、例えば、１秒間の角速度ω_u(t)，ω_v(t)，ω_w(t)の各軸の最大値と最小値を求め、以下の数式（２６）により、その最大－最小の値を求める。だたし、ｍａｘ（ｘ），ｍｉｎ（ｘ）は、それぞれ１秒間の最大値と最小値を求める関数である。

【0093】

最大-最小=max(ω_u(t))-min(ω_u(t))，
max(ω_v(t))-min(ω_v(t))，
max(ω_w(t))-min(ω_w(t)) の最大値 （２６）

【0094】

次のステップＳ１７において、座標変換部５は、一定時間内の角速度の最大－最小の値が、静止判定値である一定値以下であるか否かを判定する。つまり、人が歩行または走りをせずに端末１が静止しているか否かを判定する。ここで、最大－最小の値が一定値以下である場合（ステップＳ１７がＹＥＳ）は、ステップＳ１８に進む。一方、最大－最小の値が一定値を超えている場合（ステップＳ１７がＮＯ）は、処理を終了する。

【0095】

本実施形態では、一定時間内の角速度の最大－最小の値が、静止判定値である一定値以下の場合、例えば、前述の数式（２６）の値が、０.０１［ｒａｄ/ｓ］の場合に、端末１は静止していると判定する。

【0096】

次のステップＳ１８において、座標変換部５は、一定時間内の角速度の平均をオフセットとする。例えば、１秒間の角速度ω_u(t)，ω_v(t)，ω_w(t)の各軸の平均値を求め、それをオフセットとする。そして、以後のステップＳ１１の端末１の回転演算では、センシングした値からオフセットを減算して用いる。これにより、角速度センサ２にオフセットがある場合でも、角速度センサ値を補正できる。そして、処理を終了する。

【0097】

変形例２において、座標変換部５は、一定時間内の３軸の角速度の最大値と最小値の差が静止判定値以下の場合に、対象者である人が静止していると判定し、この静止しているときの角速度の平均値を求め、この平均値とオフセット値として設定する。このようにすれば、角速度センサ２が角速度をセンシングしたときに生じる誤差をキャンセルすることができる。

【0098】

なお、本実施形態において、基準値（判定値）を用いた任意の値の判定は、「任意の値が基準値以上か否か」の判定でも良いし、「任意の値が基準値を超えているか否か」の判定でも良い。或いは、「任意の値が基準値以下か否か」の判定でも良いし、「任意の値が基準値未満か否か」の判定でも良い。また、基準値が固定されるものでなく、変化するものであっても良い。従って、基準値の代わりに所定範囲の値を用い、任意の値が所定範囲に収まるか否かの判定を行っても良い。また、予め装置に生じる誤差を解析し、基準値を中心として誤差範囲を含めた所定範囲を判定に用いても良い。

【0099】

なお、本実施形態のフローチャートにおいて、各ステップが直列に実行される形態を例示しているが、必ずしも各ステップの前後関係が固定されるものでなく、一部のステップの前後関係が入れ替わっても良い。また、一部のステップが他のステップと並列に実行されても良い。

【0100】

なお、本実施形態の位置計測装置で実行されるプログラムは、ＲＯＭなどに予め組み込んで提供される。もしくは、このプログラムは、インストール可能な形式または実行可能な形式のファイルでＣＤ－ＲＯＭ、ＣＤ－Ｒ、メモリカード、ＤＶＤ、フレキシブルディスク（ＦＤ）などのコンピュータで読み取り可能な非一過性の記憶媒体に記憶されて提供するようにしても良い。

【0101】

また、この位置計測装置で実行されるプログラムは、インターネットなどのネットワークに接続されたコンピュータ上に格納し、ネットワーク経由でダウンロードさせて提供するようにしても良い。また、この位置計測装置は、構成要素の各機能を独立して発揮する別々のモジュールを、ネットワークまたは専用線で相互に接続し、組み合わせて構成することもできる。

【0102】

なお、本実施形態では、進行方向推定部７が、１歩の区間における垂直方向のグローバル加速度の位相を進めた値と水平方向のグローバル加速度の値とに基づいて進行方向を求める処理を行っているが、その他の態様でも良い。例えば、進行方向推定部７が、１歩の区間における端末座標Ｃ’の３軸方向の加速度の合計値の位相を進めた値と水平方向のグローバル加速度の値とに基づいて進行方向を求める処理を行っても良い。

【0103】

なお、本実施形態では、端末１に、座標変換部５と歩幅推定部６と進行方向推定部７と位置推定部８と情報出力部９とが設けられているが、他の実施形態であっても良い。例えば、座標変換部５と歩幅推定部６と進行方向推定部７と位置推定部８と情報出力部９が、端末１から離れた位置に設けられたコンピュータなどの処理装置に搭載されていても良い。そして、このコンピュータと端末１とが無線通信回線で接続され、端末１の各センサで検出した情報を処理装置に送信して処理を行っても良い。

【0104】

以上説明した実施形態によれば、１歩の区間における垂直方向のグローバル加速度の位相を進めた値と水平方向のグローバル加速度の値とに基づいて進行方向を求める進行方向推定部を備えることにより、歩行位置の計測精度を向上させることができる。

【0105】

本発明のいくつかの実施形態を説明したが、これらの実施形態は、例として提示したものであり、発明の範囲を限定することは意図していない。これら実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更、組み合わせを行うことができる。これら実施形態やその変形は、発明の範囲や要旨に含まれると同様に、特許請求の範囲に記載された発明とその均等の範囲に含まれるものである。

【符号の説明】

【0106】

１…端末、２…角速度センサ、３…加速度センサ、４…磁界センサ、５…座標変換部、６…歩幅推定部、７…進行方向推定部、８…位置推定部、９…情報出力部、Ｃ…グローバル座標、Ｃ’…端末座標、Ｐ…人。

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】

【図16】