JP6083279B2 - 移動状況情報算出方法及び移動状況情報算出装置 - Google Patents

Description

本発明は、ユーザーの移動状況情報を算出する方法等に関する。

いわゆるシームレス測位やモーションセンシング、姿勢制御など様々な分野において、センサーの活用が注目されている。センサーとしては、加速度センサーやジャイロセンサー、圧力センサー、地磁気センサーなどが広く知られている。センサーの計測結果を利用して慣性航法演算を行って移動体（例えば自転車や自動車、電車、船、飛行機など）の位置算出を行う技術も考案されている。

慣性航法演算においては、センサーの計測結果に含まれ得る種々の誤差成分に起因して位置算出の正確性が低下するという問題があり、位置算出の正確性を向上させるための様々な技術が考案されている。例えば、特許文献１には、四輪自動車等の移動体を対象として、移動体に対するセンサーの取付姿勢を判定して、移動体の位置や速度を算出する技術が開示されている。

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

特許文献１の技術を人間に適用する場合には問題がある。特許文献１の技術では、移動体は固定形状であって、センサーの姿勢も固定であることを前提としている。すなわち、移動体の移動方向に対して、センサーの姿勢は固定であることを前提としている。しかし、人間は固定形状ではない。移動中は四肢は勿論のこと、胴体も捻れや上下動等の動作を伴う。そのため、取り付けられた部位へのセンサーの姿勢は固定であっても、その部位自体が動く。具体的には、ユーザーの移動方向を基準とした場合の相対的なセンサーの姿勢（全体的に言えば、「ユーザーに対するセンサーの姿勢」とも言える。）は変化し得る。従って、特許文献１の技術をそのまま人間に適用した場合には、ユーザーの位置や速度が適切に算出されないという問題がある。

本発明は上記の課題に鑑みてなされたものであり、その目的とするところは、ユーザーの位置及び速度の何れか（以下、包括して「移動状況情報」という。）を適切に算出するための新たな手法を提案することにある。

以上の課題を解決するための第１の発明は、ユーザーの身体に装着された速度及び加速度の何れかを検出するセンサーの、前記センサーに対応付けられた座標系で表された検出結果の変化が所定の特異条件を満たした時の当該検出結果を用いて、前記ユーザーに対する前記センサーの姿勢を算出することと、前記姿勢を用いて、前記姿勢を算出した時と異なり、かつ、前記特異条件を満たした時の前記検出結果を前記ユーザーに対応付けられた移動体座標系に変換することと、前記変換された検出結果を用いて、前記ユーザーの位置及び速度の何れか（移動状況情報）を算出することと、を含む移動状況情報算出方法である。

また、他の発明として、ユーザーの身体に装着される速度及び加速度の何れかを検出するセンサーと、前記センサーに対応付けられた座標系で表された前記センサーの検出結果の変化が所定の特異条件を満たした時の当該検出結果を用いて、前記ユーザーに対する前記センサーの姿勢を算出する姿勢算出部と、前記姿勢を用いて、前記姿勢を算出した時と異なり、かつ、前記特異条件を満たした時の前記検出結果を前記ユーザーに対応付けられた移動体座標系に変換することに用いる座標変換行列を算出する座標変換行列算出部と、前記座標変換行列と、前記姿勢を算出した時と異なり、かつ、前記特異条件を満たした時の前記検出結果とを用いて、前記ユーザーの移動状況情報を算出する移動状況情報算出部と、を備えた移動状況情報算出装置を構成することとしてもよい。

この第１の発明等によれば、ユーザーの身体に装着された速度及び加速度の何れかを検出するセンサーの検出結果の変化が所定の特異条件を満たした時の当該検出結果を用いて、ユーザーに対するセンサーの姿勢を算出する。そして、算出した姿勢を用いて、姿勢を算出した時と異なり、かつ、特異条件を満たした時の検出結果を、センサー座標系からユーザーに対応付けられた移動体座標系に変換し、変換された検出結果を用いて、ユーザーの移動状況情報を算出する。センサーの検出結果の変化が所定の特異条件を満たした時のセンサーの検出結果を用いて姿勢を算出することで、ユーザーのある特定の動作状態におけるセンサーの姿勢を算出することができる。そして、この姿勢を、姿勢を算出したときと同様の動作状態におけるセンサーの検出結果の座標変換に用いることで、センサー座標系から移動体座標系への座標変換を適切に行うことができる。従って、ユーザーの移動状況情報を適切に算出することができる。

また、第２の発明として、第１の発明の移動状況情報算出方法において、前記変換された検出結果に含まれる前記ユーザーの進行方向成分以外の成分の値を補正すること、を更に含む移動状況情報算出方法を構成することとしてもよい。

この第２の発明によれば、変換された検出結果に含まれるユーザーの進行方向成分以外の成分の値を補正する。姿勢を算出した時と同様の動作状態における検出結果を補正することで、ユーザーの進行方向成分以外の成分の値を適切に補正することができる。

また、第３の発明として、第１又は第２の発明の移動状況情報算出方法における前記移動状況情報を算出することは、前記変換された検出結果を用いて、前記ユーザーの進行方向の加速度の振幅を算出することと、前記振幅に基づいて前記ユーザーの進行方向の速度を算出することと、を含む、移動状況情報算出方法を構成することとしてもよい。

詳細は実施形態で説明するが、ユーザーの進行方向の加速度の振幅とユーザーの進行方向の速度との間には相関関係があることが明らかとなった。そのため、第３の発明のように、変換されたセンサーの検出結果を用いて、ユーザーの進行方向の加速度の振幅を算出し、当該振幅に基づくことで、ユーザーの進行方向の速度を適切に算出することが可能となる。

また、第４の発明として、第１〜第３の何れかの発明の移動状況情報算出方法における前記姿勢を算出することは、前記センサーに対応付けられた前記座標系の前記検出結果から求められる鉛直方向の速度の変化に基づいて前記特異条件を満たしたタイミングを判定することを含む、移動状況情報算出方法を構成することとしてもよい。

この第４の発明によれば、センサーに対応付けられた座標系のセンサーの検出結果から求められる鉛直方向の速度の変化に基づくことで、特異条件を満たしたタイミングを適切に判定することができる。

また、第５の発明として、第４の発明の移動状況情報算出方法における前記タイミングを判定することは、前記鉛直方向の速度が、変化している範囲の中央部の値になり、且つ、当該速度の変化方向が増加方向又は減少方向である場合に、前記特異条件を満たしたと判定することを含む、移動状況情報算出方法を構成することとしてもよい。

この第５の発明によれば、鉛直方向の速度が、変化している範囲の中央部の値になり、且つ、当該速度の変化方向が増加方向又は減少方向である場合に、特異条件を満たしたと判定する。上記の条件を満たす場合とは、例えば、ユーザーが歩行中や走行中に地面に着地したり、足が地面から離れて胴体が最高点に到達した場合であり、これらのタイミングを特異条件を満たすタイミングとして判定することができる。

また、第６の発明として、第１〜第５の何れかの発明の移動状況情報算出方法において、前記センサーは、前記ユーザーの腕以外の所定部位に装着される、移動状況情報算出方法を構成することとしてもよい。

この第６の発明によれば、センサーは、ユーザーの腕以外の所定部位に装着される。このため、上記の発明の移動状況算出方法と相まって、腕以外の所定部位に装着されたセンサーのユーザーに対する姿勢を適切に判定して、ユーザーの移動状況情報を適切に算出することが可能となる。

進行方向速度算出装置の全体システムの構成例を示す図。 進行方向速度算出装置の機能構成の一例を示す図。 処理部の機能ブロックを示す図。 原理の説明図。 原理の説明図。 原理の説明図。 相関式データを説明するための図。 進行方向速度算出処理の流れを示すフローチャート。 一歩検出処理の流れを示すフローチャート。 位置算出装置の機能構成の概要を示す図。 位置算出処理の流れを示すフローチャート。 誤差推定処理の流れを示すフローチャート。 位置算出を行った実験結果の一例を示す図。 位置算出装置の機能構成の概要を示す図。 姿勢算出タイミング及び進行方向速度算出タイミングの変形例の説明図。

以下、図面を参照して、本発明の好適な実施形態の一例について説明する。但し、本発明を適用可能な実施形態が以下説明する実施形態に限定されるわけでないことは勿論である。

１．第１実施形態
１−１．システム構成
図１は、本実施形態における進行方向速度算出装置１の全体システムの構成例を示す図である。この進行方向速度算出装置１は、例えばユーザーの腰（右腰又は左腰）に装着して利用される小型電子機器であり、ユーザーの進行方向の速度（以下、適宜「進行方向速度」と称す。）を移動状況情報として算出する移動状況情報算出装置の一種である。

進行方向速度算出装置１は、主要な構成として、ユーザーが進行方向速度の算出に係る各種操作を入力するための入力装置である操作ボタン３と、算出された進行方向速度等の情報が表示される液晶ディスプレイ５と、スピーカー７と、加速度センサー１０と、不図示の制御基板とを有して構成される。

本実施形態では、３種類の座標系を定義する。第１の座標系は、進行方向速度算出装置１が具備する加速度センサー１０に対応付けられた三次元直交座標系（センサー座標系）であるローカル座標系である。本明細書では、ローカル座標系のことをＬ（Local）フレームとも言う。本実施形態では、ローカル座標系の３軸をｘ軸、ｙ軸及びｚ軸と表記する。

第２の座標系は、移動体に対応付けられた三次元直交座標系（移動体座標系）である。本明細書では、移動体座標系のことをＶ（Vehicle）フレームとも言う。本実施形態では、ユーザーの前方を正とする前後方向をロール軸（Ｒ軸）、右方を正とする左右方向をピッチ軸（Ｐ軸）、鉛直下方を正とする上下方向をヨー軸（Ｑ軸）とする。

第３の座標系は、移動体の移動空間を定める座標系である三次元直交座標系（絶対座標系）である。本明細書では、絶対座標系のことをＡ（Absolute）フレームとも言う。Ａフレームは、例えば、北東下座標系として知られるＮＥＤ（North East Down）座標系や、地球中心地球固定座標系として知られるＥＣＥＦ（Earth Centered Earth Fixed）座標系として定義される。本実施形態では、絶対座標系の３軸をＸ軸、Ｙ軸及びＺ軸と表記する。

加速度センサー１０は、進行方向速度算出装置１に搭載されるセンサーであり、ユーザーの加速度ベクトルを検出する。加速度センサー１０としては、例えばＭＥＭＳ（Micro Electro Mechanical Systems）センサーが用いられる。加速度センサー１０が計測した値は、ローカル座標系で計測した移動体の加速度となる。

進行方向速度算出装置１は、図１に示すような姿勢でユーザーの腰に装着される。従って、加速度センサー１０のｚ軸が鉛直方向の軸となる。

ユーザーの加速度や速度は、方向及び大きさを持っている。そのため、本明細書では、スカラーとベクトルとを適宜区別して説明する。原則として、加速度や速度と言ったときは加速度や速度の大きさ（スカラー量）を表すものとし、加速度ベクトルや速度ベクトルと言ったときは方向及び大きさを考慮した加速度及び速度を表すものとする。

なお、各座標系において定義される諸量を明確にするため、各諸量を表す文言の先頭に座標系の種類を付して説明する。例えば、ローカル座標系で表した加速度ベクトルのことを「ローカル座標加速度ベクトル」と称し、絶対座標系で表した加速度ベクトルのことを「絶対座標加速度ベクトル」と称する。他の諸量についても同様である。

また、センサーの姿勢は、ロール角、ピッチ角及びヨー角といったオイラー角で表される。絶対座標系で表したセンサーの向きを「絶対姿勢」と表現し、その姿勢角のことを「絶対姿勢角」と称する。また、ユーザーに対するセンサーの相対的な姿勢（相対姿勢）のことを「センサー姿勢」と表現し、その姿勢角（相対姿勢角）のことを「センサー姿勢角」と称する。

また、以下の説明で参照する図面については、各種のセンサーのブロックを二重線で示し、センサーの計測結果を利用して演算処理を行う処理ブロックを一重線で示す。

制御基板は、ＣＰＵ（Central Processing Unit）やＤＳＰ（Digital Signal Processor）、ＡＳＩＣ（Application Specific Integrated Circuit）等を有する処理部１００と、外部機器との間でデータ送受を実現するインターフェイスＩＣや接続端子を有する通信部５００と、ＲＯＭ（Read Only Memory）やフラッシュＲＯＭ、ＲＡＭ（Random Access Memory）等の各種ＩＣメモリーやハードディスクなどの記録媒体等を有する記憶部６００とを有する。

１−２．機能構成
図２は、進行方向速度算出装置１の機能構成の一例を示す図である。
進行方向速度算出装置１は、加速度センサー１０と、処理部１００と、操作部２００と、表示部３００と、音出力部４００と、通信部５００と、記憶部６００とを有して構成される。

処理部１００は、記憶部６００に記憶されているシステムプログラム等の各種プログラムに従って、進行方向速度算出装置１の各部を統括的に制御する。

図３は、処理部１００の機能構成の概略を表わす機能ブロック図である。
処理部１００は、ローカル座標速度ベクトル算出部１２０と、姿勢算出部１３０と、ＬＶ座標変換行列算出部１４０と、移動体座標加速度ベクトル算出部１５０と、進行方向加速度振幅算出部１６０と、進行方向速度算出部１７０と、一歩検出部１８０とを機能部として有する。

ローカル座標速度ベクトル算出部１２０は、加速度センサー１０から入力したローカル座標加速度ベクトルを積分及び継ぎ足し処理して、ローカル座標速度ベクトルを算出する。本実施形態において、「積分」とは、所定時間分の値を累積的に加算することを意味する。また、「継ぎ足す」とは、積分により得られた結果を最後に更新した結果に足し合わせることを意味する。

具体的には、ローカル座標速度ベクトル算出部１２０は、所定時間分のローカル座標加速度ベクトルを積分することで、その所定時間の間におけるローカル座標系で表した速度ベクトルの変化分を算出する。そして、算出した速度ベクトルの変化分を最後に更新したローカル座標速度ベクトルに継ぎ足すことで、ローカル座標速度ベクトルを新たに算出・更新する。

姿勢算出部１３０は、ローカル座標速度ベクトル算出部１２０によって算出されたローカル座標速度ベクトルを用いてセンサー姿勢を判定する。姿勢算出部１３０は、一歩検出部１８０によりユーザーの一歩が検出されたタイミングで、ユーザーに対するセンサーの姿勢を判定する。

ここで言うセンサーの姿勢とは、例えばユーザーの進行方向といった進行に係る基準方向に対する相対的なセンサーの姿勢の意味である。取り付けた部位に対するセンサーの姿勢は変化しなくとも、取り付けた部位自体が移動に伴って動く。すなわち、センサーを取り付けた部位は、進行に係る基準方向に対して相対的に変位する。このため、センサーの姿勢も、進行に係る基準方向に対して相対的に変化することとなる。この変化し得るセンサーの姿勢を判定する。

なお、進行に係る基準方向に対するセンサーの姿勢とは、総体的には、ユーザーに対するセンサーの姿勢と言えるため、本明細書では後者のように表現する。また、姿勢の算出とは、姿勢角を算出することである。

ローカル座標系で表した速度ベクトル（ローカル座標速度ベクトル）と、移動体座標系で表した速度ベクトル（移動体座標速度ベクトル）との間には、次式（１）のような関係がある。

本明細書における数式では、速度ベクトルの成分を小文字の“ｖ”で表す。また、速度ベクトルの表記において、座標系の種類を上付きの大文字の添え字で示す。“Ｌ”はローカル座標系、“Ｖ”は移動体座標系、“Ａ”は絶対座標系をそれぞれ意味する。また、対応する座標系の各軸の成分を下付きの小文字の添え字で示す。例えば、ローカル座標系の３軸であれば“ｘ”、“ｙ”及び“ｚ”の表記を用いる。

また、本明細書では、座標変換行列を大文字の“Ｃ”で表す。各座標変換行列において、下付きの添え字は座標変換前の座標系を示し、上付きの添え字は座標変換後の座標系を示す。例えば、“Ｃ_L ^V”は、ローカル座標系（Ｌフレーム）から移動体座標系（Ｖフレーム）への座標変換行列を示し、“Ｃ_A ^L”は、絶対座標系（Ａフレーム）からローカル座標系（Ｌフレーム）への座標変換行列を示す。

ここで、ユーザーの移動方向に関する制約条件を適用する。具体的には、ユーザーの縦横方向（鉛直方向と左右方向）の速度成分はゼロであると仮定し、移動体座標系（Ｖフレーム）のＱ軸方向及びＰ軸方向の速度成分はゼロであるとする制約条件を設定する。つまり、ユーザーの速度に関して｛ｖ_P ^V＝ｖ_Q ^V＝０｝とする制約条件を設定する。

この場合、式（１）から、センサー姿勢のピッチ成分であるピッチ角“θ”及びヨー成分であるヨー角“Ψ”を、次式（２）及び（３）のように算出することができる。

ピッチ角“θ”及びヨー角“Ψ”は、それぞれ移動体座標系のＰ軸及びＱ軸回りのセンサーの回転角を示す。つまり、ピッチ角“θ”及びヨー角“Ψ”は、ユーザーに対するセンサーの相対的な姿勢角を表していると言える。

ＬＶ座標変換行列算出部１４０は、姿勢算出部１３０によって判定されたセンサー姿勢角を用いて、ローカル座標系から移動体座標系への座標変換行列（以下、「ＬＶ座標変換行列」と称する。）を算出する。なお、ＬＶ座標変換行列それ自体は公知であるため、数式等を用いた説明は省略する。

移動体座標加速度ベクトル算出部１５０は、加速度センサー１０から入力したローカル座標加速度ベクトルを、ＬＶ座標変換行列算出部１４０によって算出されたＬＶ座標変換行列を用いて座標変換することで、移動体座標加速度ベクトルを算出する。

進行方向加速度振幅算出部１６０は、移動体座標加速度ベクトル算出部１５０によって算出された移動体座標加速度ベクトルに基づいて、ユーザーの進行方向の加速度の振幅（以下、「進行方向加速度振幅」と称す。）を算出する。詳細は後述する。

進行方向速度算出部１７０は、進行方向加速度振幅算出部１６０によって算出された進行方向加速度振幅を用いて、ユーザーの進行方向速度を算出する。進行方向速度算出部１７０は、ユーザーに対するセンサーの姿勢とセンサーの検出結果とを用いて、ユーザーの移動状況情報を算出する移動状況情報算出部に相当する。

一歩検出部１８０は、ローカル座標速度ベクトル算出部１２０によって算出されたローカル座標速度ベクトルに基づいて、ユーザーの一歩を検出する。詳細は後述する。

ここで、姿勢算出部１３０によるセンサー姿勢の算出タイミング及び進行方向速度算出部１７０による進行方向速度の算出タイミングについて説明する。

図４は、進行方向速度算出装置１を右腰に装着したユーザーが歩行動作をする場合のユーザーの移動を俯瞰した図である。ユーザーの右腰に進行方向速度算出装置１が装着された場合を図示・説明する。

ユーザーを基準とする前後方向の軸が進行方向の軸（Ｒ軸）である。ユーザーの歩行動作や走行動作に伴い、ユーザーに対する加速度センサー１０のセンサー姿勢が随時変化する。ユーザーが右足を踏み出した状態では、図中の（２）や（４）に示すように、加速度センサー１０の座標系（ローカル座標系）は進行方向に対して右回りに回転した状態となる。それに対し、ユーザーが左足を踏み出した状態では、図中の（１）や（３）に示すように、加速度センサー１０の座標系（ローカル座標系）は進行方向に対して左回りに回転した状態となる。

つまり、ローカル座標系と移動体座標系との関係は、ユーザーの歩行動作に伴い周期的に変化することになる。そこで、本実施形態では、ある一のタイミングで加速度センサー１０のセンサー姿勢を判定する。そして、その後、歩行動作中の上記のタイミングとユーザーの姿勢が同一になると考えられるタイミングにおいて、先に判定しておいたセンサー姿勢を用いてユーザーの進行方向速度を算出する。

図５は、センサー姿勢の算出タイミング及び進行方向速度の算出タイミングの説明図である。
最初に、ユーザーの右足のタイミング（左足の着地から右足の着地までの間のあるタイミング）で加速度センサー１０のセンサー姿勢を判定する。次に、左足のタイミング（右足の着地から左足の着地までの間の同様のタイミング）では何も行わず、次の右足のタイミングで、先に判定しておいたセンサー姿勢を用いて進行方向の速度を算出する。以後は、右足のタイミングで進行方向の速度を算出していく。

図６は、本実施形態の進行方向速度算出装置１を図１に示す通りに装着した被験者に一定速度で走行してもらう実験を行って求めた、ローカル座標速度ベクトルの各成分の時間変化を示す図である。ｘ軸方向の速度成分の時間変化を破線で、ｙ軸方向の速度成分の時間変化を点線で、ｚ軸方向の速度成分の時間変化を実線でそれぞれ示している。また、下段の図は、進行方向速度算出装置１の位置の高さ方向の成分（高度）を示している。

図６において、周期的なあるタイミングでは、ユーザーに対するセンサーの姿勢は一定であると言える。具体的に説明する。
先ず、時刻ｔ１〜時刻ｔ３は、ユーザーの１歩の変化を示している。ｘ軸方向の速度を減速して時刻ｔ１付近でユーザーの左足が着地し、胴体の上下動速度（ｚ軸方向の速度）が反転して時刻ｔ２付近で胴体の高度が最高点に到達した後、胴体の上下動速度が反転してｘ軸方向の速度が減速して時刻ｔ３付近で逆足の右足が着地した。着地したタイミング（以下、「着地タイミング」と称す。）か、胴体が最高点に到達したタイミング（以下、「最高点到達タイミング」と称す。）かの何れかのタイミングにおいて、ユーザーの鉛直方向の速度（ｚ軸方向の速度）が、その鉛直方向の速度変化のおよそ中央値（図６の太線）を横切ることがわかる。

ここで、時刻ｔ０と時刻ｔ３とに着目すると、時刻ｔ０，ｔ３は共に右足の着地タイミングであり、ｘ軸，ｙ軸，ｚ軸の速度はほぼ一定であると言える。しかし、左足の着地タイミングである時刻ｔ１と、右足の着地タイミングである時刻ｔ３とでは、例えばｙ軸の速度が大きく異なる。このことは、右足なら右足、左足なら左足の着地タイミング同士の間では、ユーザーに対するセンサーの姿勢は一定であることを示していると言える。なお、これは、最高点到達タイミングも同様である。右足の着地から左足の着地までの間の最高点到達タイミング（或いはこの逆）同士では、センサーの姿勢は一定であると言える。

そこで、本実施形態では、予め定めた一方の足の着地から他方の足の着地までの間の最高点到達タイミングを基準タイミングとして、この基準タイミングにおけるセンサー姿勢を一定であると推定する。そして、この基準タイミングが到来する毎に進行方向速度を算出することとする。

最高点到達タイミングは、センサーの検出結果の変化が所定の特異条件を満たした時に相当するタイミングである。この最高点到達タイミングの判定は、鉛直方向速度が変化している範囲の中央部の値になり、且つ、当該速度の変化方向が増加方向となるタイミングである。本実施形態では、更に、最高点到達タイミングが到来する毎にカウントしたカウント値の偶数番目又は奇数番目（すなわち、１つおきの最高点到達タイミング）において、進行方向速度を算出する。こうすることで、右足なら右足の着地から他方の足（左足）の着地までの間の最高点到達タイミングを検出できる。

なお、中央部の値とは、必ずしも厳密に最大値と最小値との中央値を意味するものではなく、略中央値（例えば中央値±１０％の範囲内の値）であればよい意味である。

図２に戻り、操作部２００は、例えばタッチパネルやボタンスイッチ等の入力装置を有して構成され、なされた操作に応じた操作信号を、処理部１００に出力する。この操作部２００の操作により、進行方向速度の算出指示等の各種指示入力がなされる。操作部２００は、図１の操作ボタン３に相当する。

表示部３００は、例えばＬＣＤ（Liquid Crystal Display）等の表示装置を有して構成され、処理部１００から入力される表示信号に基づく各種表示を行う。表示部３００には、算出された進行方向速度等の情報が表示される。表示部３００は、図１の液晶ディスプレイ５に相当する。

音出力部４００は、例えばスピーカーなどの音出力装置を有して構成され、処理部１００から入力される音声信号に基づく各種音出力を行う。音出力部４００は、図１のスピーカー７に相当する。

通信部５００は、装置内部で利用される情報や算出した結果等を、外部の情報制御装置との間で送受するための通信装置である。通信部５００の通信方式としては、所定の通信規格に準拠したケーブルを介して有線接続する形式や、近距離無線通信を利用して無線接続する形式等、種々の方式を適用可能である。通信方式は、有線／無線を問わない。

記憶部６００は、例えばＲＯＭやフラッシュＲＯＭ、ＲＡＭ等の記憶装置で実現され、処理部１００が進行方向速度算出装置１を統括的に制御するためのシステムプログラムや、各種アプリケーション処理を実行するための各種プログラムやデータ等を記憶する。

記憶部６００には、処理部１００によって読み出され、進行方向速度算出処理（図８参照）として実行される進行方向速度算出プログラム６１０が記憶されている。進行方向速度算出プログラム６１０は、一歩検出処理（図９参照）として実行される一歩検出プログラム６１１をサブルーチンとして含む。

また、記憶部６００には、相関式データ６２０と、センサー検出データ６３０と、ローカル座標速度ベクトルデータ６４０と、姿勢データ６５０と、座標変換行列データ６６０と、移動体座標加速度ベクトルデータ６７０と、進行方向速度データ６８０とが記憶される。

図７は、相関式データ６２０を説明するための図である。相関式データ６２０は、進行方向速度と進行方向加速度振幅との関係を定めたデータであり、関数式のデータであってもよいし、一方から他方を検索可能なテーブル形式のデータであってもよい。

発明者の研究によれば、進行方向速度と進行方向加速度振幅とは、正の相関があることが分かった。図７は、その正の相関を示す図である。図７において、ダイヤ形の各プロットは、実験した進行方向速度における進行方向加速度振幅を示している。これによれば、進行方向速度と進行方向加速度振幅との間には正の相関がある。そこで、この相関特性を示したのが相関式データ６２０である。従来、鉛直方向加速度振幅と進行方向速度との間に正の相関があることが知られていた。しかし、発明者の研究によれば、鉛直方向加速度振幅と進行方向速度とはユーザーが歩行する場合には相関があるが、走行する場合には正の相関が見られない場合があることが分かった（図示せず）。

なお、本実施形態における進行方向加速度振幅とは、進行方向加速度の時間変化のうちの極大値と極小値との差のことを意味する。但し、進行方向加速度の時間変化のうちの極大値と極小値との差の半分の値を進行方向加速度振幅としても略同一の結果が得られるため、何れの定義を採用してもよいことは勿論である。

図２に戻り、センサー検出データ６３０は、加速度センサー１０によって検出されたローカル座標加速度ベクトルが時系列に記憶されたデータである。
ローカル座標速度ベクトルデータ６４０は、ローカル座標速度ベクトル算出部１２０によって算出されたローカル座標速度ベクトルが時系列に記憶されたデータである。

姿勢データ６５０は、姿勢算出部１３０によって算出されたセンサー姿勢角が記憶されたデータである。
座標変換行列データ６６０は、ＬＶ座標変換行列算出部１４０によって算出されたＬＶ座標変換行列が記憶されたデータである。

移動体座標加速度ベクトルデータ６７０は、移動体座標加速度ベクトル算出部１５０によって算出された移動体座標加速度ベクトルが時系列に記憶されたデータである。
進行方向速度データ６８０は、進行方向速度算出部１７０によって算出された進行方向速度が時系列に記憶されたデータである。

１−３．処理の流れ
図８は、処理部１００が記憶部６００に記憶されている進行方向速度算出プログラム６１０に従って実行する進行方向速度算出処理の流れを示すフローチャートである。

最初に、処理部１００は、加速度センサー１０の検出結果を記憶部６００のセンサー検出データ６３０に記憶させる処理を開始する（ステップＡ１）。次いで、ローカル座標速度ベクトル算出部１２０が、加速度センサー１０の検出結果を用いて、ローカル座標速度ベクトルの算出と、算出されたローカル座標速度ベクトルを記憶部６００のローカル座標速度ベクトルデータ６４０に記憶させる処理とを開始する（ステップＡ３）。

その後、一歩検出部１８０が、記憶部６００に記憶されている一歩検出プログラム６１１に従って一歩検出処理を開始する（ステップＡ５）。

図９は、一歩検出処理の流れを示すフローチャートである。
一歩検出部１８０は、サンプルデータの取得のために、所定時間が経過するまでデータを蓄積する（ステップＢ１；Ｎｏ）。つまり、所定時間の間、加速度センサー１０の検出データに基づいてローカル座標速度ベクトルを算出して、データを蓄積する。そして、所定時間が経過したならば（ステップＢ１；Ｙｅｓ）、一歩検出部１８０は、蓄積したデータに基づいて、鉛直方向速度の極大値の平均値を算出する処理と（ステップＢ３）、鉛直方向速度の極小値の平均値を算出する処理とを行う（ステップＢ５）。

次いで、一歩検出部１８０は、閾値を設定する（ステップＢ７）。閾値は、例えば、ステップＢ３で算出した鉛直方向速度の極大値の平均値とステップＢ５で算出した鉛直方向速度の極小値の平均値とを平均した値として設定することができる。

その後、一歩検出部１８０は、歩数のカウントに用いるカウンタ“ｎ”をゼロリセットする（ステップＢ９）。次いで、一歩検出部１８０は、鉛直方向速度が閾値を超えたか否かを判定し（ステップＢ１１）、超えていないと判定したならば（ステップＢ１１；Ｎｏ）、そのまま待機する。一方、超えたと判定したならば（ステップＢ１１；Ｙｅｓ）、一歩検出部１８０は、カウンタ“ｎ”を１だけインクリメントする（ステップＢ１３）。

その後、一歩検出部１８０は、鉛直方向速度が閾値以下となるまで待機する（ステップＢ１５；Ｎｏ）。そして、鉛直方向速度が閾値以下となったならば（ステップＢ１５；Ｙｅｓ）、ステップＢ１１に戻る。

図８の進行方向速度算出処理に戻り、一歩検出処理を開始したならば、姿勢算出部１３０は、センサー姿勢を算出するタイミングが到来したか否かを判定する（ステップＡ７）。例えば、進行方向の速度を算出するタイミングとして、カウンタ“ｎ”を奇数とするならば、カウンタ“ｎ”が奇数となったか否かを判定する。

センサー姿勢を算出するタイミングではないと判定したならば（ステップＡ７；Ｎｏ）、姿勢算出部１３０は、そのまま待機する。一方、センサー姿勢を算出するタイミングであると判定したならば（ステップＡ７；Ｙｅｓ）、姿勢算出部１３０が、センサー姿勢を算出する（ステップＡ９）。具体的には、式（２）及び（３）に従ってセンサー姿勢角（ピッチ角“θ”及びヨー角“Ψ”）を算出し、姿勢データ６５０として記憶部６００に記憶させる。

その後、ＬＶ座標変換行列算出部１４０が、姿勢データ６５０に記憶されたセンサー姿勢角を用いてＬＶ座標変換行列を算出し、記憶部６００の座標変換行列データ６６０に記憶させる（ステップＡ１１）。

次いで、進行方向速度算出部１７０は、進行方向速度を算出するタイミングであるか否かを判定する（ステップＡ１３）。これは、ステップＡ７と同じ条件を満たしたタイミングである。例えば、ステップＡ７がカウンタ“ｎ”が奇数となるタイミングであるならば、ステップＡ１３もカウンタ“ｎ”が奇数となるタイミングである。

進行方向速度を算出するタイミングではないと判定したならば（ステップＡ１３；Ｎｏ）、処理部１００は、ステップＡ２３へと処理を移行する。一方、進行方向速度を算出するタイミングであると判定したならば（ステップＡ１３；Ｙｅｓ）、処理部１００は、対象期間の加速度センサー１０の検出結果（ローカル座標加速度ベクトル）を記憶部６００のローカル座標速度ベクトルデータ６４０から抽出する（ステップＡ１５）。対象期間は、例えば、当該進行方向速度の算出タイミングから遡って直近の鉛直方向速度が閾値を超えたタイミングまでの期間である。これは、片方の足が着地した後に踏み出されて、胴体が最高点に達するまでの期間である。

次いで、移動体座標加速度ベクトル算出部１５０は、移動体座標加速度ベクトル算出処理を行う（ステップＡ１７）。具体的には、ステップＡ１５で抽出された加速度センサー１０の検出結果（ローカル座標加速度ベクトル）を、座標変換行列データ６６０に記憶されたＬＶ座標変換行列を用いて座標変換して移動体座標加速度ベクトルを算出し、記憶部６００の移動体座標加速度ベクトルデータ６７０に記憶させる。

その後、進行方向加速度振幅算出部１６０が、進行方向加速度振幅を算出する（ステップＡ１９）。具体的には、ステップＡ１７で算出された移動体座標速度ベクトルに含まれる進行方向の加速度の時間変化の中から直近の進行方向加速度の極大値と極小値とを抽出し、その差を進行方向加速度振幅として算出する。

次いで、進行方向速度算出部１７０は、進行方向速度を算出する（ステップＡ２１）。具体的には、記憶部６００の相関式データ６２０に記憶された相関式に、ステップＡ１９で算出した進行方向加速度振幅を代入することで進行方向速度を算出し、記憶部６００の進行方向速度データ６８０に記憶させる。

その後、処理部１００は、処理を終了するか否かを判定し（ステップＡ２３）、処理を継続すると判定したならば（ステップＡ２３；Ｎｏ）、ステップＡ１３に戻る。また、処理を終了すると判定したならば（ステップＡ２３；Ｙｅｓ）、進行方向速度算出処理を終了する。

１−４．作用効果
ユーザーの身体に装着される進行方向速度算出装置１において、加速度を検出する加速度センサー１０に対応付けられたローカル座標系で表された検出結果の変化が所定の特異条件を満たした時の当該検出結果を用いて、ユーザーに対する加速度センサー１０の姿勢を算出する。そして、算出した姿勢を用いて、姿勢を算出した時と異なり、かつ、特異条件を満たした時の加速度センサー１０の検出結果をユーザーに対応付けられた移動体座標系に変換する。そして、変換された加速度センサー１０の検出結果を用いて、ユーザーの進行方向の速度を算出する。

具体的には、加速度センサー１０の検出結果から求められる鉛直方向の速度の変化に基づき、鉛直方向の速度が、変化している範囲の中央部の値になり、且つ、当該速度の変化方向が増加方向である場合（最高点到達タイミング）に、特異条件を満たしたと判定する。そして、最高点到達タイミングにおける加速度センサー１０の検出結果を用いて、加速度センサー１０のセンサー姿勢角を算出する。そして、算出したセンサー姿勢角を用いてＬＶ座標変換行列を算出し、これを用いて加速度センサー１０の検出結果を移動体座標に座標変換して、ユーザーの進行方向加速度振幅を算出する。そして、算出した進行方向加速度振幅と進行方向速度との所与の相関関係に基づいて、ユーザーの進行方向の速度を算出する。

加速度センサー１０のユーザーに対する姿勢が同じになると考えられるタイミングに合わせて進行方向速度を算出するようにしたことで、ユーザーの歩行動作や走行動作に関わらず、ユーザーの進行方向速度を適切に算出することができる。また、進行方向加速度振幅と進行方向速度との所与の相関関係に基づくことで、従来と比べてユーザーの進行方向速度をより正しく算出することが可能となる。

特異条件を満たすタイミングの判定は、加速度センサー１０の検出結果に基づいて算出した鉛直方向の速度が、変化している範囲の中央部の値になり、且つ、当該速度の変化方向が増加方向であることを検出することにより行う。これは、胴体が最高到達点に達したタイミング（最高点到達タイミング）である。この最高点到達タイミングを特異条件を満たすタイミングとしてセンサー姿勢を算出し、その後は、センサー姿勢が同じになると考えられるタイミングで間欠的に進行方向速度を算出するため、高い正確性でユーザーの進行方向速度を算出することが可能となる。

２．第２実施形態
２−１．機能構成
図１０は、第２実施形態における位置算出装置２Ａの機能構成の概要を示す図である。位置算出装置２Ａは、第１実施形態の進行方向速度算出装置１の機能に付加して構成される装置である。説明を分かり易くするために、図１０では、位置算出装置２Ａは進行方向速度算出装置１を具備することとして図示しているが、実際には、進行方向速度算出装置１の処理部１００に図１０の処理部１１０の機能を付加し、更に、ジャイロセンサー２０及びＧＰＳ（Global Positioning System）センサー３０を進行方向速度算出装置１に付加して構成することができる。

なお、位置算出装置２Ａは、進行方向速度算出装置１と同様に、例えばユーザーの腰（右腰又は左腰）に装着して利用され、ユーザーの進行方向の速度の他、位置を移動状況情報に含めて算出する移動状況情報算出装置の一種である。

ジャイロセンサー２０は、ユーザーの角速度を検出するセンサーであり、進行方向速度算出装置１が具備する加速度センサー１０と同じローカル座標系で角速度（以下、「ローカル座標角速度」と称す。）を検出する。そして、検出したローカル座標角速度を慣性航法演算部４０に出力する。

ＧＰＳセンサー３０は、測位用衛星の一種であるＧＰＳ衛星から送信されるＧＰＳ衛星信号を受信し、当該ＧＰＳ衛星信号を利用してユーザーの位置及び速度ベクトルを算出するセンサーである。ＧＰＳセンサー３０は、所与の絶対座標系におけるユーザーの位置及び速度ベクトル（速度及び方位）を算出し、ＧＰＳ算出位置及びＧＰＳ算出速度ベクトルとして誤差推定部５０に出力する。なお、ＧＰＳを利用して位置や速度ベクトルを算出する方法については従来公知であるため、詳細な説明を省略する。

処理部１１０は、慣性航法演算部４０と、誤差推定部５０と、移動状況判定部６０とを有して構成される。上述した通り、進行方向速度算出装置１の処理部１００に、これらの機能部を付加して構成することとしてもよいし、別のプロセッサー等として構成してもよい。

慣性航法演算部４０は、進行方向速度算出装置１から出力されるローカル座標加速度ベクトルと、ジャイロセンサー２０から出力されるローカル座標角速度とを用いて、公知の慣性航法演算処理を行って、絶対座標系におけるユーザーの位置（絶対座標位置）、速度ベクトル（絶対座標速度ベクトル）及び姿勢角（絶対姿勢角）を算出する。

慣性航法演算部４０は、誤差推定部５０から出力される加速度バイアス及びジャイロバイアスを用いて、進行方向速度算出装置１の加速度センサー１０から出力されるローカル座標加速度ベクトル及びジャイロセンサー２０から出力されるローカル座標角速度を補正する。そして、補正後のローカル座標加速度ベクトル及びローカル座標角速度を用いて絶対座標位置、絶対座標速度ベクトル及び絶対姿勢角を算出し、誤差推定部５０から出力される位置誤差、速度ベクトル誤差及び姿勢角誤差を用いて、絶対座標位置、絶対座標速度ベクトル及び絶対姿勢角を補正する。

誤差推定部５０は、慣性航法演算部４０から出力される慣性航法演算結果（絶対座標位置、絶対座標速度ベクトル及び絶対姿勢角）を制御入力として、例えばカルマンフィルター（以下、「ＫＦ（Kalman Filter）」と表記する。）を利用した誤差推定演算（以下、「ＫＦ誤差推定演算」と称す。）を行って、慣性航法演算結果に含まれる誤差（以下、「慣性航法演算誤差」と称す。）を推定する。そして、推定した慣性航法演算誤差を慣性航法演算部４０にフィードバック出力する。本実施形態において、慣性航法演算誤差には、位置誤差、速度ベクトル誤差、姿勢角誤差、加速度バイアス及びジャイロバイアスが含まれる。

慣性航法演算部４０及び誤差推定部５０は、ユーザーに対するセンサーの姿勢とセンサーの検出結果とを用いて、ユーザーの移動状況情報を算出する移動状況情報算出部に相当する。

移動状況判定部６０は、加速度センサー１０から出力されるローカル座標加速度ベクトル及びジャイロセンサー２０から出力されるローカル座標角速度に基づいて、ユーザーの移動状況を判定する。本実施形態では、ユーザーの移動状況として、ユーザーが停止している状態（以下、「停止状態」と称す。）であるか、移動している状態（以下、「移動状態」と称す。）であるかを判定する。

慣性航法演算部４０と、誤差推定部５０と、移動状況判定部６０とは、ＣＰＵやＤＳＰ等のプロセッサーを有する処理部１１０として構成することができる。

２−２．処理の流れ
図１１は、位置算出装置２Ａの処理部１１０が実行する位置算出処理の流れを示すフローチャートである。

最初に、慣性航法演算部４０は、進行方向速度算出装置１から進行方向速度の取得を開始する（ステップＣ１）。その後、慣性航法演算部４０が、慣性航法演算処理を開始する（ステップＣ３）。そして、誤差推定部５０が、後述する誤差推定処理を開始する（ステップＣ５）。

次いで、移動状況判定部６０が、移動状況判定処理を行う（ステップＣ７）。その後、処理部１１０は、移動状況が停止状態から移動状態に変化したか否かを判定し（ステップＣ９）、その判定結果が肯定判定であったならば（ステップＣ９；Ｙｅｓ）、フラグをＯＮに設定する（ステップＣ１１）。

一方、ステップＣ９の判定結果が否定判定であったならば（ステップＣ９；Ｎｏ）、処理部１１０は、移動状況が移動状態から停止状態に変化したか否かを判定する（ステップＣ１３）。そして、その判定結果が肯定判定であるならば（ステップＣ１３；Ｙｅｓ）、処理部１１０は、フラグをＯＦＦに設定する（ステップＣ１５）。

ステップＣ１１又はＣ１５の後、或いは、ステップＣ１３の判定結果が否定判定であったならば（ステップＣ１３；Ｎｏ）、慣性航法演算部４０は、誤差推定部５０により推定された最新の慣性航法演算誤差を用いて慣性航法演算結果を補正する（ステップＣ１７）。

その後、慣性航法演算部４０は、位置の出力タイミングであるか否かを判定し（ステップＣ１９）、出力タイミングであると判定したならば（ステップＣ１９；Ｙｅｓ）、最新の絶対座標位置を出力する（ステップＣ２１）。出力タイミングではないと判定したならば（ステップＣ１９；Ｎｏ）、ステップＣ２３へと移行する。

次いで、処理部１１０は、処理を終了するか否かを判定し（ステップＣ２３）、処理を継続すると判定したならば（ステップＣ２３；Ｎｏ）、ステップＣ７に戻る。一方、処理を終了すると判定したならば（ステップＣ２３；Ｙｅｓ）、処理部１１０は、位置算出処理を終了する。

図１２は、誤差推定処理の流れを示すフローチャートである。
誤差推定処理では、誤差推定部５０は、次式（４）に示すような状態ベクトルＸを設定してＫＦ誤差推定演算を行う。

但し、「δＰ」、「δＶ」及び「δＡ」は、それぞれ慣性航法演算部４０の演算結果に含まれる位置誤差、速度ベクトル誤差及び姿勢角誤差である。また、「ｂ_ａ」及び「ｂ_ｗ」は、それぞれ加速度バイアス及びジャイロバイアスである。

最初に、誤差推定部５０は、フラグがＯＮに設定されているか否かを判定する（ステップＤ１）。そして、フラグがＯＮに設定されていると判定したならば（ステップＤ１；Ｙｅｓ）、移動時制約条件の適用タイミングであるか否かを判定する（ステップＤ３）。移動時制約条件の適用タイミングは、進行方向速度算出装置１による進行方向速度の算出タイミング（図８のステップＡ１３）と同じタイミングである。つまり、１つおきの最高点到達タイミングが到来したタイミングである。

移動時制約条件の適用タイミングであると判定したならば（ステップＤ３；Ｙｅｓ）、誤差推定部５０は、進行方向速度算出装置１から出力される進行方向速度を用いて移動時観測ベクトルを設定する（ステップＤ５）。

具体的には、ジャイロセンサー２０から出力される角速度を用いて姿勢角を算出し、これを用いて絶対座標系からローカル座標系への座標変換行列（以下、「ＡＬ座標変換行列」と称す。）を算出する。そして、算出したＡＬ座標変換行列と進行方向速度算出装置１から出力されるＬＶ座標変換行列とを用いて、慣性航法演算部４０によって演算された絶対座標速度ベクトルを移動体座標速度ベクトルに変換する。そして、進行方向速度算出装置１から出力される進行方向速度Ｖと、算出した移動体座標速度ベクトルｖ^Ｖ＝（ｖ^Ｖ _Ｒ，ｖ^Ｖ _Ｐ，ｖ^Ｖ _Ｑ）^Ｔとを用いて、例えば次式（５）に示すような移動時観測ベクトルＺ_ｍｏｖｅを設定する。

移動時観測ベクトルＺ_ｍｏｖｅは、ユーザーの縦横方向（鉛直方向と左右方向）の速度成分はゼロであるとの仮定に基づき設定される観測ベクトルである。つまり、最高点到達タイミングが到来したタイミングでは、移動体座標系（Ｖフレーム）のＱ軸方向及びＰ軸方向の速度成分はゼロとなり、進行方向であるＲ軸方向に対しては進行方向速度Ｖに相当する速度成分が生ずると仮定して、上記のような移動時観測ベクトルＺ_ｍｏｖｅを設定する。

この場合、誤差推定部５０は、ステップＤ５で設定した移動時観測ベクトルＺ_ｍｏｖｅを適用したＫＦ誤差推定演算を行う（ステップＤ７）。すなわち、ステップＤ５で設定した移動時観測ベクトルＺ_ｍｏｖｅを観測情報として、所定の予測演算及び補正演算を行って慣性航法演算誤差を推定する。

移動時観測ベクトルＺ_ｍｏｖｅは、進行方向の加速度の振幅に基づいて算出される進行方向速度を用いて設定される。また、誤差推定部５０が推定する慣性航法演算誤差には、ユーザーの速度ベクトル誤差が含まれ、慣性航法演算部４０が演算する絶対座標速度ベクトルは、加速度センサー１０の検出結果を用いて算出される。従って、移動時観測ベクトルＺ_ｍｏｖｅを用いて慣性航法演算誤差を推定して慣性航法演算結果を補正することは、進行方向の加速度の振幅に基づいて算出された進行方向の速度を用いて、センサーの検出結果が示す速度を補正して、ユーザーの移動状況情報を算出することと等価である。

ステップＤ３において移動時制約条件適用タイミングではないと判定したならば（ステップＤ３；Ｎｏ）、誤差推定部５０は、ＧＰＳセンサー３０から出力されるＧＰＳ演算結果を用いてＧＰＳ観測ベクトルを設定する（ステップＤ９）。つまり、ＧＰＳ算出位置及びＧＰＳ算出速度ベクトルを成分とするＧＰＳ観測ベクトルを設定する。

次いで、誤差推定部５０は、設定したＧＰＳ観測ベクトルを適用したＫＦ誤差推定演算を行う（ステップＤ１１）。すなわち、ステップＤ９で設定したＧＰＳ観測ベクトルを観測情報として、所定の予測演算及び補正演算を行って慣性航法演算誤差を推定する。

ステップＤ１においてフラグがＯＦＦに設定されていると判定したならば（ステップＤ１；Ｎｏ）、誤差推定部５０は、停止時観測ベクトルを設定する（ステップＤ１３）。具体的には、慣性航法演算部４０が演算した絶対座標速度ベクトルｖ^Ａ＝（ｖ^Ａ _Ｘ，ｖ^Ａ _Ｙ，ｖ^Ａ _Ｚ）^Ｔを用いて、例えば次式（６）に示すような停止時観測ベクトルＺ_ｓｔｏｐを設定する。

停止時観測ベクトルＺ_ｓｔｏｐは、ユーザーの速度成分がゼロであるとの仮定に基づき設定される観測ベクトルである。つまり、停止状態では、絶対座標系（Ａフレーム）における各軸方向の速度成分がゼロになると仮定して、上記のような停止時観測ベクトルＺ_ｓｔｏｐを設定する。

この場合、誤差推定部５０は、ステップＤ１３で設定した停止時観測ベクトルＺ_ｓｔｏｐを適用したＫＦ誤差推定演算を行う（ステップＤ１５）。すなわち、ステップＤ１５で設定した停止時観測ベクトルＺ_ｓｔｏｐを観測情報として、所定の予測演算及び補正演算を行って慣性航法演算誤差を推定する。

ステップＤ７，Ｄ１１又はＤ１５の後、誤差推定部５０は、ＫＦ誤差推定演算で推定した慣性航法演算誤差を記憶更新する（ステップＤ１７）。そして、誤差推定部５０は、ステップＤ１に戻る。

２−３．実験結果
図１３は、位置算出装置２Ａで位置算出を行った実験結果を示す図である。位置算出装置２Ａを装着した被験者を所定の経路に沿って走行させて位置を算出する実験を行った。図１３において、横軸は経度、縦軸は緯度をそれぞれ示している。また、真位置の軌跡を細破線で、従来の手法を用いて位置算出を行った場合の算出位置の軌跡を太破線で、本実施形態の手法を用いて位置算出を行った場合の算出位置の軌跡を実線でそれぞれ示している。なお、従来の手法は、特異条件を満たすタイミングとは無関係なあるタイミングで算出されたユーザーに対するセンサーの姿勢を、以後の任意のタイミングにおける進行方向速度算出に用いる方法とした。

この結果を見ると、従来の手法を用いて位置算出を行った場合の算出位置の軌跡は、真位置の軌跡に対して全体的に縮小されたような軌跡となっており、位置が大きくずれていることがわかる。それに対し、本実施形態の手法を用いて位置算出を行った場合の算出位置の軌跡は、真の軌跡に近い軌跡となっており、従来の手法を用いる場合と比べて位置算出の正確性が大きく向上していることが確認できる。

２−４．作用効果
位置算出装置２Ａにおいて、慣性航法演算部４０は、進行方向速度算出装置１から出力されるローカル座標加速度ベクトルと、ジャイロセンサー２０から出力されるローカル座標角速度とを用いて、慣性航法演算処理を行って、絶対座標系におけるユーザーの位置、速度ベクトル及び姿勢角を算出する。誤差推定部５０は、慣性航法演算部４０によって算出された慣性航法演算結果に含まれる誤差を推定する。そして、慣性航法演算部４０は、誤差推定部５０によって推定された慣性航法演算誤差を用いて、慣性航法演算結果を補正する。

誤差推定部５０は、進行方向速度算出装置１から出力される進行方向速度と、移動体座標速度ベクトルとを用いて、移動時観測ベクトルを設定する。そして、進行方向速度の算出タイミングと同期するようにして移動時観測ベクトルを適用したＫＦ誤差推定演算を行うことで、ユーザーの歩行動作や走行動作に伴うセンサー姿勢の変化の影響を受けずに、慣性航法演算誤差を適切に推定することができる。その結果、慣性航法演算誤差を用いて慣性航法演算結果を補正することで、ユーザーの位置や速度といった移動状況情報を適切に算出することが可能となる。

また、誤差推定部５０は、ＫＦ誤差推定演算において、位置や速度ベクトル、姿勢角に含まれる誤差に加えて、加速度センサー１０の検出結果に含まれるバイアス（加速度バイアス）や、ジャイロセンサー２０の検出結果に含まれるバイアス（ジャイロバイアス）を推定する。センサーのバイアスは、慣性航法演算を行う際の大きな誤差の要因となる。しかし、誤差推定部５０が推定した加速度バイアスやジャイロバイアスを用いて慣性航法演算部４０が加速度センサー１０の検出結果やジャイロセンサー２０の検出結果を補正し、これを用いて慣性航法演算処理を行うことで、高い正確性でユーザーの移動状況情報を算出することが可能となる。

３．第３実施形態
図１４は、第３実施形態における位置算出装置２Ｂの概要を示す図である。
位置算出装置２Ｂは、第１実施形態の進行方向速度算出装置１の機能に付加して構成される装置である。説明を分かり易くするために、図１４では、位置算出装置２Ｂは進行方向速度算出装置１を具備することとして図示しているが、実際には、進行方向速度算出装置１の処理部１００に図１４の慣性航法演算部８０の機能を付加し、更に、方位計７０を進行方向速度算出装置１に付加して構成することができる。

なお、位置算出装置２Ｂは、進行方向速度算出装置１と同様に、例えばユーザーの腰（右腰又は左腰）に装着して利用され、ユーザーの進行方向の速度の他、位置を移動状況情報に含めて算出する移動状況情報算出装置の一種である。

進行方向速度算出装置１は、第１実施形態の進行方向速度算出装置１であり、ユーザーの進行方向速度を算出して、慣性航法演算部８０に出力する。

方位計７０は、ユーザーの進行方向を検出する方位計測装置であり、磁気センサー等の方位センサーを有して構成される。方位計７０は、計測したユーザーの進行方向（方位）を慣性航法演算部８０に出力する。

慣性航法演算部８０は、進行方向速度算出装置１から出力される進行方向速度と方位計７０から出力される進行方向とを用いた公知の慣性航法演算処理を行って、ユーザーの位置を算出する。本実施形態において、慣性航法演算部８０は、ユーザーに対するセンサーの姿勢とセンサーの検出結果とを用いて、ユーザーの移動状況情報を算出する移動状況情報算出部に相当する。

４．変形例
本発明を適用可能な実施形態は、上記の第１〜第３の実施形態に限定されることなく、本発明の趣旨を逸脱しない範囲で適宜変更可能であることは勿論である。以下、変形例について説明する。なお、上記実施形態と同一の構成については同一の符号を付して再度の説明を省略する。

４−１．センサーの種類
上記の実施形態では、ユーザーの身体に装着されるセンサーを加速度センサー１０として説明したが、加速度センサー１０ではなく、ユーザーの速度を検出する速度センサーをユーザーの身体に装着することとしてもよい。この場合は、速度センサーを具備する進行方向速度算出装置１や位置算出装置２Ａ，２Ｂを構成して、ユーザーの身体の所定部位に装着するようにすればよい。

４−２．センサーの装着部位
上記の実施形態では、センサー（進行方向速度算出装置１や位置算出装置２Ａ，２Ｂ）のユーザーへの装着部位を腰として説明したが、腰以外の部位に装着することとしてもよい。好適な装着部位はユーザーの体幹（四肢以外の部位）である。しかし、体幹に限らず、腕以外の例えばユーザーの頭や足に装着することとしてもよい。

４−３．姿勢算出タイミング及び進行方向速度算出タイミング
上記の実施形態では、進行方向速度算出タイミングを、１つおきの最高点到達タイミングとして説明したが、最高点到達タイミングそれぞれにおいて進行方向の速度の算出を行うこととしてもよい。

図１５は、この場合における進行方向の速度の算出タイミングを説明するための図である。
右足に係る最高点到達タイミング（右足の着地から左足の着地までの最高点到達タイミング）を第１の姿勢算出タイミングとして第１のセンサー姿勢を算出する。そして、次の左足に係る最高点到達タイミング（左足の着地から右足の着地までの最高点到達タイミング）を第２の姿勢算出タイミングとして第２のセンサー姿勢を算出する。その後は、右足に係る最高点到達タイミングが到来する毎に第１のセンサー姿勢を用いて第１の進行方向速度を算出し、左足に係る最高点到達タイミングが到来する毎に第２のセンサー姿勢を用いて第２の進行方向速度を算出する。なお、センサー姿勢の算出方法や進行方向速度の算出方法は、上記の実施形態と同じである。

この変形例を第２実施形態で説明した位置算出装置２Ａに適用する場合は次のようになる。誤差推定部５０が、右足に係る最高点到達タイミングと同期するように、第１の進行方向速度を用いて設定した第１の移動時観測ベクトルを観測情報とするＫＦ誤差推定演算を行って、慣性航法演算誤差を推定する。また、左足に係る最高点到達タイミングに同期するように、第２の進行方向速度を用いて設定した第２の移動時観測ベクトルを観測情報とするＫＦ誤差推定演算を行って、慣性航法演算誤差を推定する。

また、上記の実施形態や上記の変形例のように、最高点到達タイミングを姿勢算出タイミングや進行方向速度算出タイミングとするのではなく、一方の足又はそれぞれの足の着地タイミングを姿勢算出タイミングや進行方向速度算出タイミングとしてもよい。着地タイミングの判定は、鉛直方向速度が変化している範囲の中央部の値になり、且つ、当該速度の変化方向が減少方向となるタイミングを判定することで実現することができる。

また、加速度センサー１０の各サンプルタイミングにおける検出結果に基づいてセンサー姿勢を逐次算出することとし、一歩が検出される毎に、加速度センサー１０の対応するサンプルタイミングを進行方向速度の算出タイミングとして進行方向速度を算出するようにしてもよい。

また、算出したセンサー姿勢を、当該センサー姿勢を算出したタイミングよりも前のタイミングにおける進行方向速度の算出に用いてもよい。例えば、記憶部６００のセンサー検出データ６３０に記憶された、姿勢算出処理（ステップＡ９）が行われた時点以前の加速度センサー１０の検出結果に対して一歩検出処理を行い、進行方向速度算出タイミングを判定して進行方向速度を算出してもよい。

また、センサー姿勢は、上記の実施形態のようにある一のタイミングにおける加速度センサー１０の検出結果に基づいて算出しなければならないわけではない。例えば、一歩分の加速度センサー１０の検出結果の平均値を用いてセンサー姿勢を算出することとしてもよい。また、加速度センサー１０の各サンプルタイミングにおける検出結果に基づいてセンサー姿勢を逐次算出し、一歩分のセンサー姿勢を平均した値を当該一歩におけるセンサー姿勢としてもよい。これらの場合においては、右足が踏み出される場合と左足が踏み出される場合とで、それぞれセンサー姿勢を算出するようにすると好適である。

４−４．センサー姿勢の算出方法
第２実施形態において、慣性航法演算部４０が演算した慣性航法演算結果に含まれる絶対座標速度ベクトルを用いてユーザーに対するセンサーの姿勢を算出するようにしてもよい。具体的には、慣性航法演算部４０が演算した絶対座標速度ベクトルを絶対姿勢角を用いてローカル座標速度ベクトルに座標変換する。そして、当該ローカル座標速度ベクトルを用いて、式（２）及び（３）に従ってセンサー姿勢を算出する。

４−５．誤差推定
上記の実施形態では、姿勢算出タイミングにおいてセンサー姿勢を一度算出した後は、進行方向速度算出タイミングにおいて、上記のセンサー姿勢を用いて進行方向速度の算出を行うこととして説明した。しかし、ユーザーが歩行中又は走行中に同じ姿勢になると考えられるタイミングにおいても、ユーザーに対するセンサーの姿勢は僅かではあるが変化するものと考えられる。そこで、次のようにしてもよい。

誤差推定部５０がＫＦ誤差推定演算で用いる状態ベクトルＸに、センサー姿勢の変化、すなわち設置状態の変化による誤差角度の成分を追加し、上記の実施形態と同様のＫＦ誤差推定演算を行って、設置状態の誤差角度を推定する。そして、設置状態の誤差角度を用いてセンサー姿勢を更新し、進行方向速度算出タイミングにおいて、更新したセンサー姿勢を用いて進行方向速度を算出するようにしてもよい。

４−６．センサーの検出結果の補正
センサーの検出結果に含まれるユーザーの進行方向成分以外の成分の値を補正することとしてもよい。具体的には、上記の実施形態の進行方向速度算出装置１において、センサー姿勢を算出した後、当該センサー姿勢を用いてＬＶ座標変換行列を算出する。そして、このＬＶ座標変換行列を用いて、加速度センサー１０のローカル座標加速度ベクトルを移動体座標加速度ベクトルに座標変換する。そして、この移動体座標加速度ベクトルを積分及び継ぎ足し処理して移動体座標速度ベクトルを算出し、当該移動体座標速度ベクトルの進行方向成分以外の値をゼロに補正する。これにより、第１実施形態で説明した進行方向速度の算出方法とは異なる手法でユーザーの進行方向速度を算出することができる。

移動体座標速度ベクトルは移動体座標加速度ベクトルに基づいて算出され、移動体座標加速度ベクトルはローカル座標加速度ベクトルに基づいて算出される。従って、移動体座標速度ベクトルの進行方向成分以外の値をゼロに補正することは、センサーの検出結果に含まれるユーザーの進行方向成分以外の成分の値を補正することと等価である。

４−７．相関特性
上記の実施形態では、進行方向加速度振幅と進行方向速度との相関関係を表す相関式を予め定められた固定的な相関式として説明した。しかし、歩幅や歩行速度はユーザー毎に異なる。このため、ユーザー毎に相関式を作成することとしてもよい。

この場合は、例えば、上記の「４−６．センサーの検出結果の補正」で説明した方法を用いて一定期間分のユーザーの進行方向速度を算出し、ユーザーの進行方向加速度振幅と対応付けたサンプルデータを蓄積する。そして、それらのサンプルデータを用いて最小二乗法等の手法を用いて相関式を算出し、記憶部６００の相関式データ６２０に記憶させることとしてもよい。

４−８．姿勢判定の成分
上記の実施形態では、ユーザーに対するセンサーの姿勢のピッチ成分及びヨー成分の両方を判定するものとして説明したが、ピッチ成分及びヨー成分の何れか一方のみを判定することとしてもよいのは勿論である。

４−９．一歩検出処理の方法
上記の実施形態では、鉛直方向速度が、変化している範囲の中央部の値になり、かつ、当該速度の変化方向が増加方向または減少方向となるタイミングを判定することで一歩検出を行うものとして説明した。しかし、他の公知の一歩検出処理を行ってセンサー姿勢を算出するタイミングを判定してもよい。例えば、鉛直方向速度または鉛直方向加速度が極小値または極大値となるタイミングを判定してもよい。また、センサーの検出結果から高周波ノイズを除去するフィルター処理を行ったデータを用いてもよい。

１ 進行方向速度算出装置、 ２Ａ，２Ｂ 位置算出装置、 ３ 操作ボタン、 ５ 液晶ディスプレイ、 ７ スピーカー、 １０ 加速度センサー、 ２０ ジャイロセンサー、 ３０ ＧＰＳセンサー、 ４０ 慣性航法演算部、 ５０ 誤差推定部、 ６０ 移動状況判定部、 ７０ 方位計、 ８０ 慣性航法演算部、 １００，１１０ 処理部、 ２００ 操作部、 ３００ 表示部、 ４００ 音出力部、 ５００ 通信部、 ６００ 記憶部

Claims (8)

1. ユーザーの身体に装着され、速度及び加速度の何れかを検出するセンサーの、前記センサーに対応付けられている座標系で表された検出結果の変化が所定の特異条件を満たした時の当該検出結果を用いて、前記ユーザーに対する前記センサーの姿勢を算出するステップと、
   前記算出された前記姿勢を用いて、前記姿勢を算出した時と異なり、かつ、前記特異条件を満たした時の検出結果を前記ユーザーに対応付けられている移動体座標系で表された移動体検出結果に変換するステップと、
   前記変換された前記移動体検出結果を用いて、前記ユーザーの位置及び速度の何れかを含む移動状況情報を算出するステップと、
   を含む移動状況情報算出方法。
2. 請求項１において、
   前記移動状況情報を算出するステップの後に行われるステップであって、前記移動体検出結果に含まれ、前記ユーザーの進行方向成分以外の成分を補正するステップ、
   を含む移動状況情報算出方法。
3. 請求項１又は２において、
   前記移動状況情報を算出するステップは、
   前記移動体検出結果を用いて、前記ユーザーの進行方向の加速度の振幅を算出するステップと、
   前記振幅に基づいて前記ユーザーの進行方向の速度を算出するステップと、
   を含む、
   移動状況情報算出方法。
4. 請求項１〜３の何れか一項において、
   前記姿勢を算出するステップは、
   前記センサーに対応付けられている前記座標系で表された前記検出結果から求められる鉛直方向の速度の変化に基づいて、前記特異条件を満たすタイミングを判定するステップを含む、
   移動状況情報算出方法。
5. 請求項４において、
   前記特異条件は、
   前記鉛直方向の前記速度が、変化している範囲の中央部の値になり、
   且つ、当該速度の変化する方向が増加方向、又は減少方向である場合である、
   移動状況情報算出方法。
6. 請求項１〜５の何れか一項において、
   前記センサーは、前記ユーザーの腕以外の所定部位に装着される、
   移動状況情報算出方法。
7. ユーザーの身体に装着され、速度及び加速度の何れかを検出するセンサーと、
   前記センサーに対応付けられている座標系で表された前記センサーの検出結果の変化が所定の特異条件を満たした時の当該検出結果を用いて、前記ユーザーに対する前記センサーの姿勢を算出する姿勢算出部と、
   前記算出された前記姿勢を用いて、前記姿勢を算出した時と異なり、かつ、前記特異条件を満たした時の検出結果を前記ユーザーに対応付けている移動体座標系で表された移動体検出結果に変換する座標変換算出部と、
   前記変換された前記移動体検出結果を用いて、前記ユーザーの位置及び速度の何れかを含む移動状況情報を算出する移動状況情報算出部と、
   を含む移動状況情報算出装置。
8. ユーザーの身体に装着され、速度及び加速度の何れかを検出するセンサーの、前記センサーに対応付けられている座標系で表された検出結果の変化が所定の特異条件を満たした時の当該検出結果を用いて、前記ユーザーに対する前記センサーの姿勢を算出し、
   前記算出された前記姿勢を用いて、前記姿勢を算出した時と異なり、かつ、前記特異条件を満たした時の検出結果を前記ユーザーに対応付けられている移動体座標系で表された移動体検出結果に変換し、
   前記変換された前記移動体検出結果を用いて、前記ユーザーの位置及び速度の何れかを含む移動状況情報を算出する、
   移動状況情報算出装置。

Images