JP6322960B2 - 慣性装置、方法及びプログラム - Google Patents

Description

本発明は、慣性装置、方法及びプログラムに関する。

屋内や地下のように、GPS(Global Positioning System)を用いた測位が行えない環境において、加速度センサや地磁気センサ等が統合された慣性装置の演算により、歩行者の位置を推定する、慣性航法技術（PDR; Pedestrian Dead Reckoning）が知られている。

慣性航法技術を用いた従来の技術には、慣性装置をユーザの体の所定の位置に固定することにより、慣性装置そのものの自由な動きを制限した上で、所定の演算を行い、歩行方向及び歩行位置推定を行うものが存在する（特許文献１乃至４）。

一方、慣性装置を固定しない代わりに、高性能な慣性センサ（加速度センサ、角速度（ジャイロ）センサ、地磁気センサ等）から得られる情報を用いて、高度な演算を行い、歩行推定を行う技術も存在する（特許文献５、６）。このような演算には、進行方向を推定するための、センサから得られる加速度情報や角速度情報を用いた、フーリエ変換や主成分分析による演算が含まれる。

しかしながら、従来の慣性装置は、ユーザ等の測定対象に固定する必要があり、利便性が低かった。また、固定する必要がない慣性装置は、高性能なセンサと、高度な演算能力を備えていなければならず、装置そのものが高コストとなることや、小型化が難しいといった課題があった。

本発明は、このような問題に鑑みてなされたものであり、小型化と低コスト化を実現し、かつ、高い利便性を有する慣性装置を提供することを目的とする。

上述した課題を解決し目的を達成するため、本発明の一実施形態における慣性装置は、
慣性センサの出力に基づいて得られた加速度に含まれる水平成分及び鉛直成分のうち、前記鉛直成分がピークとなるピーク時刻を検出する検出部と、
前記ピーク時刻を中心とする所定の期間内の前記加速度の水平成分を用いて、水平方向の移動速度を表す水平速度を算出する算出部と、
前記水平速度が所定の範囲内である場合に、前記出力が歩行動作によるものであると判定する判定部と、
前記水平速度が、歩行動作によるものと判定された場合に、前記水平速度を用いて、進行方向を推定する推定部と、
を有し、
前記出力が、前記歩行動作とは異なる非歩行動作であるかを認識し、
前記推定部は、前記非歩行動作が認識されている間は前記進行方向を推定しない。

また、本発明の一実施形態における方法は、
コンピュータが、慣性センサの出力に基づいて得られた加速度に含まれる水平成分及び鉛直成分のうち、前記鉛直成分がピークとなるピーク時刻を検出する検出段階と、
コンピュータが、前記ピーク時刻を中心とする所定の期間における、前記加速度の水平成分を用いて、水平方向の移動速度を表す水平速度を算出する算出段階と、
コンピュータが、前記水平速度が所定の範囲内である場合に、前記出力が歩行動作によるものであると判定する判定段階と、
コンピュータが、前記水平速度が、歩行動作によるものと判定された場合に、前記水平速度を用いて、進行方向を推定する推定段階と、
を有し、
前記出力が、前記歩行動作とは異なる非歩行動作であるかを認識し、
前記推定段階では、前記非歩行動作が認識されている間は前記進行方向を推定しない。

また、本発明の一実施形態におけるプログラムは、コンピュータに、上記方法を実行させる。

本発明によれば、小型化と低コスト化を実現し、かつ、高い利便性を有する慣性装置を提供できる。

本発明の一実施形態における慣性装置の概略を表す図。 本発明の一実施形態における慣性装置のハードウェア構成例を表す図。 本発明の一実施形態における慣性装置の機能ブロック図。 姿勢演算処理を説明する図。 時間発展の手続きにおける姿勢演算（ロール・ピッチ・ヨー角）を説明する図。 時間発展の手続きにおける姿勢演算（ロール・ピッチ・ヨー角）を説明する図。 時間発展の手続きにおける姿勢演算（ロール・ピッチ・ヨー角）を説明する図。 第一の観測更新の手続きにおける姿勢演算（ロール・ピッチ角）を説明する図。 第二の観測更新の手続きにおける姿勢演算（ヨー角）を説明する図。 ピーク検出処理を説明する図。 水平成分移動速度特徴情報の算出処理を説明する図。 水平成分移動速度特徴情報の算出処理を説明する図。 判定処理を説明する図。 左右の移動速度ぶれ幅判定処理を説明する図。 進行方向推定処理を説明する図。 ＴＲＩＡＤアルゴリズムにより姿勢を求める処理を説明する図。 拡張カルマンフィルタの一般式を説明する図（従来技術）。 時間更新の手続きにおいて用いられる変数を説明する図（従来技術）。 観測更新の手続きにおいて用いられる変数を説明する図（従来技術）。 歩行動作における鉛直方向の運動特性を説明する図。 歩行動作における水平方向の運動特性を説明する図。 歩行動作と非歩行動作の判別に伴う進行方向推定結果を説明する図。 様々な携行形態での進行方向推定結果を説明する図。 様々な携行形態での進行方向推定結果を説明する図。

以下、本発明の実施形態を図面に基づいて説明する。

１． 概要
２． ハードウェア構成
３． 機能
３．１ 座標系変換
拡張カルマンフィルタの一般式
拡張カルマンフィルタの適用
時間発展の手続き
第一の観測更新の手続き
第二の観測項新の手続き
ＴＲＩＡＤアルゴリズムを用いた姿勢情報の計算
３．２ 進行方向推定
３．３ 進行方向推定結果

≪ １． 概要 ≫
まず、図１を用いて、本発明の一実施形態における慣性装置の概要を説明する。図１は、慣性装置１を所持するユーザが歩行する様子を表している。本明細書では、ユーザの歩行する方向がｘ軸で表され、ｘ軸と垂直であり、かつ、地面に平行な方向がｙ軸で表され、鉛直方向がｚ軸で表される。

慣性装置１は、例えば、携帯電話、スマートフォン、ＰＤＡ（Personal Digital Assistant）又はノートブックＰＣのように、ユーザが持ち運ぶことのできる小型のデバイスである。慣性装置１は、市販されているスマートフォン等に搭載されているような、慣性センサ（加速度センサ、角速度センサ、地磁気センサ）を備えている。慣性装置１は、各センサを利用して、加速度及び角速度の変化並びに装置の向きを検出することができる。

詳しくは後述するが、本発明の一実施形態における慣性装置１は、まず、各センサを利用して、三つの軸方向の加速度（三軸加速度）と、三つの軸方向の角速度（三軸角速度）と、三つの軸で表される地磁気の強さ（三軸地磁気）とを含むセンサ情報を随時取得する。各測定値は、デバイス座標系によって表現されるため、慣性装置１は、各センサ情報を、絶対座標系に変換する。

慣性装置１は、絶対座標空間における移動加速度（ベクトル）を取得する。具体的には、例えば、慣性装置１は、絶対座標系に変換されたセンサ情報から重力成分を除去した、鉛直方向成分と水平成分の加速度（三軸移動加速度）を取得することができる。そして、慣性装置１は、随時取得されて記憶される三軸移動加速度のうち、鉛直方向の移動加速度の変化を表す波形において、歩行中のユーザの一の足（軸足）が着地してから、他方の足が軸足を追い越す時刻を特定する。当該時刻は、鉛直方向の移動加速度の変化を表す波形が、谷のピークとなる時刻に対応ため、本明細書では、当該時刻のことを、ピーク時刻又はピーク位置と呼ぶ。

次に、慣性装置１は、ピーク位置を中心とした所定の期間内の水平成分移動加速度の積算処理を実行する。ここで、算出された値を、水平成分移動速度特徴ベクトルと呼ぶ。慣性装置１は、算出された水平成分移動速度特徴ベクトルと、測定対象の移動に係る動作の周期と、鉛直方向移動加速度のピーク値又はピーク振幅（鉛直方向ピーク加速度）を用いて、得られたデータが、測定対象による実際の歩行又は進行動作によるものであるか判定する。以下、測定対象の移動に係る動作の周期のことを、移動周期と呼ぶ。移動周期の一例は、人の歩行周期である。また、測定対象による実際の歩行又は進行動作を、単に歩行動作と記述する（但し、「歩行」以外の動作も含む）。

次に、慣性装置１は、歩行動作によるものであると判定した場合には、前後の水平成分移動速度特徴ベクトルを合成し、一歩踏み出したときの進行方向ベクトルを、随時算出する。その後、最新の進行方向ベクトルと、直近の進行方向ベクトルの合成方向を、進行方向として推定する。

このように、本発明の一実施形態における慣性装置は、フーリエ変換や主成分分析を用いずに進行方向を推定するため、歩行に伴う加速度のシグナルが低周波数であることに起因する、高速なセンササンプリングが不要となる。また、本発明の一実施形態における慣性装置は、複数歩に対してフーリエ変換を行う必要がないため、一歩あたりの進行方向推定精度及び進行方向推定の応答性が向上する。これにより、本慣性装置は、従来の慣性装置に比べて、コストを低減することができ、さらに、小型化が可能となる。

また、ユーザは、本慣性装置の持ち方に関する制限を受けないため、利便性が向上する。この利点は、本慣性装置が、ユーザに対してナビゲーション機能を提供する場合に、特に有利である。

なお、慣性装置１は、上で例示したデバイス以外のデバイス（例えば、音楽プレーヤ、活動量計、腕時計等）であってもよい。また、慣性装置１は、他のデバイス（例えば、歩行ロボットや、動物に装着する首輪等）に内蔵されて提供されてもよい。これにより、鉛直方向に一定の周期で運動する多様な人又は物体の進行方向を推定することが可能となる。

≪ ２． ハードウェア構成 ≫
次に、図２を用いて、本発明の一実施形態における慣性装置１のハードウェア構成を説明する。図２は、慣性装置１が、スマートフォンのような携帯デバイスである場合の、ハードウェア構成例である。

慣性装置１は、ＣＰＵ１１、ＲＡＭ１２、ＲＯＭ１３、加速度センサ１４、角速度センサ１５、地磁気センサ１６、マイク１７、スピーカ１８、通信モジュール１９、Ｂｌｕｅｔｏｏｔｈ（登録商標）通信モジュール２０、ＧＰＳ受信モジュール２１、ディスプレイ２２、タッチパネル２３、電池２４、気圧センサ２５及びバス２６を有する。

ＣＰＵ１１は、慣性装置１の動作制御を行うプログラムを実行する。ＲＡＭ１２は、ＣＰＵ１１のワークエリア等を構成する。ＲＯＭ１３は、ＣＰＵ１１が実行するプログラムや、プログラムの実行に必要なデータを記憶する。加速度センサ１４は、慣性装置１の、デバイス座標系におけるX'、Y'、Z'軸方向の加速度を検出する。角速度センサ１５（又はジャイロセンサ）は、慣性装置１の、デバイス座標系におけるX'、Y'、Z'軸方向の角速度を検出する。地磁気センサ１６は、磁北を表す三次元のベクトルを出力し、慣性装置１の向きを検出する。気圧センサ２５は、気圧を測定し、慣性装置１の高度を検出する。

マイク１７は、ユーザの声等の音声を電気信号に変換する。スピーカ１８は、電気信号を音声として出力する。通信モジュール１９は、３Ｇネットワーク及び／又は無線ＬＡＮに接続された他の装置と通信するための装置である。Ｂｌｕｅｔｏｏｔｈ（登録商標）通信モジュール２０は、Ｂｌｕｅｔｏｏｔｈを用いて通信するための装置である。ＧＰＳ受信モジュール２１は、ＧＰＳ衛星やＩＭＥＳ（Indoor Messaging Service）によって送信される測位信号を受信するための装置である。

ディスプレイ２２は、ユーザに対して画面を提示するための装置である。タッチパネル２３は、ユーザからの入力を受け付ける装置である。電池２４は、慣性装置１を駆動するための電力を供給する装置である。バス２６は、電池２４を除く各装置を相互に接続する。

なお、マイク１７、スピーカ１８、通信モジュール１９、Ｂｌｕｅｔｏｏｔｈ（登録商標）通信モジュール２０、ＧＰＳ受信モジュール２１、ディスプレイ２２及びタッチパネル２３は、任意の構成要素である。例えば、慣性装置１が、表示画面を有しない活動量計のようなデバイスである場合には、これらの装置を備えていなくてもよい。

また、慣性装置１は、Ｂｌｕｅｔｏｏｔｈ（登録商標）通信モジュール２０の代わりに、他の規格に従って無線通信する装置（例えば、ＺｉｇＢｅｅ（登録商標）通信モジュール）を備えていてもよい。

≪ ３．機能 ≫
次に、図３を用いて、本発明の一実施形態における慣性装置１の機能ブロックを説明する。本節では、慣性装置１の機能を、「座標系変換」機能と、「進行方向推定」機能の、二つに分けて説明する。

≪ ３．１ 座標系変換 ≫
慣性装置１において座標系変換機能を実行する座標系変換部１００は、加速度取得部１０１と、角速度取得部１０２と、地磁気取得部１０３と、姿勢演算部１０４と、地磁気信頼性評価部１０５と、絶対加速度変換部１０７とを含む。座標系変換部１００は、加速度センサ１４より取得された、センサの座標系（デバイス座標系）における三軸の加速度を、絶対座標系における加速度（絶対加速度）に変換する。

なお、絶対座標系は、複数種類のセンサによって観測された座標値を統一的に扱うために使用される統一的な座標系であり、例えば、ＧＰＳで使用されるＷＧＳ８４経緯度座標系や、ＵＴＭ（ユニバーサル横メルカトル図法）など直交座標系がある。言い換えれば、ある空間に固定された唯一の原点を定め、その原点からの距離的関係を用いて、対象となる物体の位置や方位を表現することのできる座標系を、絶対座標系と考えることができる。絶対座標系は、ワールド（世界）座標系とも呼ばれる。また、デバイス座標系は、ボディー座標系とも呼ばれ、慣性装置１上の１点を原点として定義し、この原点で互いに直交する３軸を夫々Ｘ軸、Ｙ軸、Ｚ軸として定義した座標系のことを表す。

加速度取得部１０１は、加速度センサ１４によって検出された三軸加速度の変化を取得する。

角速度取得部１０２は、角速度センサ１５によって検出された三軸角速度の変化を取得する。ここで取得される角速度は、加速度と同様に、デバイス座標系に固定されている。

地磁気取得部１０３は、地磁気センサ１６によって検出された磁北を表す三次元の地磁気ベクトルを取得し、慣性装置１の向きを表す情報を取得する。ここで取得される向きは、加速度と同様に、デバイス座標系に固定されている。

姿勢演算部１０４は、上述した、加速度取得部１０１、角速度取得部１０２及び地磁気取得部１０３が取得したセンサ情報を用いて、慣性装置１の現在の姿勢を演算し、その姿勢情報（回転行列）から、逆行列計算により、逆回転行列１０６を求める。

ここで、地磁気取得部１０３の取得した情報は、屋内環境における環境磁場の影響により、信頼性が低い場合がある。そこで、姿勢演算部１０４は、後述する、地磁気信頼性評価部１０５により信頼できると判断された場合にのみ、地磁気取得部１０３の取得した情報を使用する。

姿勢演算部１０４は、一般的に知られている拡張カルマンフィルタ（非特許文献１、非特許文献２、図１７−１８）を利用して、慣性装置１の姿勢を表す行列を求め、その逆行列を出力する。以下、その処理について詳細に説明する。

≪ 拡張カルマンフィルタの一般式 ≫
図１７は、一般的に知られている拡張カルマンフィルタの一般式を表す。カルマンフィルタでは、時間ステップをひとつ進めるために、時間発展と観測更新の二つの手続きが実行される。時間発展の手続きでは、前の時刻の推定状態から、現在時刻の推定状態が計算される。また、観測更新の手続では、現在時刻の観測を用い、推定値を補正して、より正確な状態が推定される。これらの手続を逐次繰り返すことによって、最適な状態変数が推定される。

図１８は、拡張カルマンフィルタの時間発展の手続きにおいて使用される各変数を説明する図である。図１７の時間発展の項目に含まれる（１）−（３）の式に対応して、各変数の説明が付されている。ここで、ｋは、離散的なステップ時間を表しており、ｋ−１は、今の時間より１ステップ前の時間を表す。

図１９は、拡張カルマンフィルタの観測更新の手続きにおいて使用される各変数を説明する図である。図１７の観測更新の項目に含まれる（１）−（６）の式に対応して、各変数の説明が付されている。

≪ 拡張カルマンフィルタの適用 ≫
姿勢演算部１０４は、上述した拡張カルマンフィルタにおける「時間発展の手続き」を、角速度センサ１５の出力による姿勢情報の更新に用いる（ロール角、ピッチ角、ヨー角）。また、拡張カルマンフィルタにおける「観測更新の手続き」を、加速度センサ１４の出力による姿勢情報の更新に用いる（ロール角、ピッチ角）（以後、「第一の観測更新の手続き」と呼ぶ）。また、「観測更新の手続き」を、さらに、地磁気センサ１６の出力による姿勢情報の更新にも用いる（ヨー角）（以後、「第二の観測更新の手続き」と呼ぶ）。

このように、姿勢演算部１０４は、７状態の拡張カルマンフィルタを構築する。姿勢演算部１０４は、一つの時間発展の手続きと、二つの観測更新の手続きを、並列にループさせ、姿勢とジャイロゼロ点バイアス値の推定を行う。ここで、姿勢は、以下のようなクオータニオン（ベクトル）によって表される。

クオータニオンベクトルとは、４変数からなるベクトルであり、物体の姿勢を表す。ロール・ピッチ・ヨーの姿勢表現ではシンバルロックという特異点問題がある反面、クオータニオンでは、特異点が無く全ての姿勢が表すことができる。また、ジャイロゼロ点バイアス値は、三軸に対応する三変数（ｂｘ_k,ｂｙ_k,ｂｚ_k）によって表される（ｂは定数）。

以下では、上記の三つの手続き（１）−（３）を、それぞれ説明する。

≪ 時間発展の手続き ≫
まず、図５−図７を用いて、拡張カルマンフィルタにおける時間発展の手続きを説明する。姿勢演算部１０４は、当該手続きを実行し、後述する状態推定モデルにおいて、ジャイロ出力値を入力として、拡張カルマンフィルタの時間発展の手続きに従って時間積分を行う。そして、更新されたクオータニオンベクトルｑと、誤差共分散行列Ｐを得る（ロール・ピッチ・ヨー角）。

図５は、本発明において、拡張カルマンフィルタの一般式における、（１）システムの状態推定モデルに含まれる項目を説明する図である。ここでは、現在時刻の状態推定値を、上述したクオータニオンベクトル及びジャイロゼロ点バイアス値により、図５の式（１）−１のように定義する。

また、入力量ｕ_kを、角速度センサの出力値（ω_0xk,ω_0yk,ω_0zk）（rad/sec）を用いて、図５の式（１）−４のように定義する（ｂは定数）。

すなわち、（ω_xk,ω_yk,ω_zk）は、ゼロ点の値を減算し、オフセットがない角速度を表す（rad/sec）。そして、システムの状態推定モデルを、図５の式（１）−５のように表す。ここで、Ｃ₁、Ｃ₂、Ｃ₃は、任意の定数である。

図６は、本発明において、拡張カルマンフィルタの一般式における、（２）時間更新における偏微分行列（ヤコビアン）を説明する図である。図５を用いて説明したように、システムの状態推定モデルは、図５の式（１）−５によって表される。式（１）−５の右辺はｆであるため、右辺の偏微分が、時間発展における偏微分行列となる。

図７は、本発明において、拡張カルマンフィルタの一般式における、（３）誤差の共分散行列推定モデルＰ_k|k-1を説明する図である。プロセスノイズＱ_kは、システム同定によって求められる定数であり、予め算出される。

上記のプロセスノイズＱ_kと、１ステップ前の時刻の誤差共分散行列と、時間発展における偏微分行列（ヤコビアン）Ｆ_k及びその転置行列Ｆ_k ^Tにより、現在時刻の誤差誤差共分散行列Ｐ_k|k-1が求まる（図７の式（３）−５）。なお、現在時刻の誤差誤差共分散行列Ｐ_k|k-1及びＰ_k-1|k-1は、７×７の行列となり、行列内の要素は実数となる。

姿勢演算部１０４は、上記のモデル及び変数の定義を用いて、拡張カルマンフィルタにおける時間発展の手続きを実行し、慣性装置１の、絶対座標系における姿勢を求め、その姿勢を表す行列の逆行列（逆回転行列）を求める。

≪ 第一の観測更新の手続き ≫
次に、図８を用いて、拡張カルマンフィルタにおける第一の観測更新の手続きを説明する。姿勢演算部１０４は、当該手続きを実行し、加速度取得部１０１から得られる水平方向の角度情報と、現在のクオータニオンベクトルの水平角度情報とを比較し、その誤差分を補正する処理を行う（ロール・ピッチ角のみ）。

図８は、拡張カルマンフィルタの一般式における、
（１）観測残差

に含まれる項目を説明する図である。まず、１ステップ前の時間の観測値（ベクトル）ｈは、図８の式（１）−３のように表される。

上記の式に含まれる要素は、三次元の回転行列（４×４）に由来するものであり、予め決められているものとする。また、観測値（ベクトル）ｚ_kは、図８の式（１）−２のように表される。

ここで、（ａ_x，ａ_y，ａ_z）は、加速度取得部１０１が取得した、加速度センサ１４の出力値である。上記のｈ及びｚ_kにより、
観測残差

が求まる。

また、拡張カルマンフィルタの一般式における、（２）観測更新における偏微分行列（ヤコビアン）Ｈ_kは、図７の式（１）−３に表される観測値ｈの偏微分を求めることによって求まる。

また、拡張カルマンフィルタの一般式における、（３）残差の共分散Ｓ_kは、以下の観測ノイズ（行列）Ｒ_kと、観測更新における偏微分行列Ｈ_k、その転置行列Ｈ_k ^T及び現在時刻の誤差共分散行列Ｐ_k|k-1を用いて求められる。

ここで、（ｒ₁，ｒ₂，ｒ₃）は、加速度センサ１４のデバイス評価の結果、予め求められた分散値である。

また、拡張カルマンフィルタの一般式における、（４）カルマンゲインＫ_kは、現在時刻の誤差共分散行列Ｐ_k|k-1と、観測更新における偏微分行列の転置行列Ｈ_k ^Tと、残差の共分散の逆行列Ｓ_k ^-1から求まる。なお、Ｋ_kは、７×３の行列であり、実数値を要素に持つ。

同様に、拡張カルマンフィルタの一般式における、（５）更新された状態推定値ｘ_k|k及び（６）更新された誤差共分散行列Ｐ_k|kは、これまでに求められた変数を用いて求めることができる。

姿勢演算部１０４は、上記のモデル及び変数を用いて、拡張カルマンフィルタにおける観測更新の手続きを実行し、水平方向の角度情報と、現在のクオータニオンベクトルの水平角度情報とを比較し、その誤差分を補正する（ロール・ピッチ角のみ）。

≪ 第二の観測更新の手続き ≫
次に、図９を用いて、拡張カルマンフィルタにおける第二の観測更新の手続きを説明する。姿勢演算部１０４は、後述する地磁気信頼性評価部１０５から、地磁気取得部１０３の取得した情報が信頼できるものであるかについての通知を受け取る。そして、地磁気取得部１０３からの情報が信頼できる場合に、ＴＲＩＡＤアルゴリズムに従って取得された姿勢情報から算出されるヨー角の情報を用いて、当該手続きを行い、クオータニオンベクトルのヨー角成分の補正を行う。ＴＲＩＡＤアルゴリズムを用いた演算方法については後述する。

図９は、図８と同様に、拡張カルマンフィルタの一般式における、
（１）観測残差

に含まれる項目を説明する図である。図８の場合と同様に、１ステップ前の時間の観測値（ベクトル）ｈは、図９の式（１）−３のように表される。一方、図８の場合と異なり、観測値（ベクトル）ｚ_kは、図９の式（１）−２のように表される。

上記のベクトルは、ＴＲＩＡＤアルゴリズムによって求められた、ヨー角方向を示すベクトルである。

また、拡張カルマンフィルタの一般式における、（２）観測更新における偏微分行列（ヤコビアン）Ｈ_kは、第一の観測更新の手続きと同様に、１ステップ前の時間の観測地ｈの偏微分を求めることによって、求めることができる。

また、拡張カルマンフィルタの一般式における、（３）残差の共分散Ｓ_kは、以下の観測ノイズ（行列）Ｒ_kと、観測更新における偏微分行列Ｈ_k、その転置行列Ｈ_k ^T及び現在時刻の誤差共分散行列Ｐ_k|k-1を用いて求められる。

ここで、（Ｔ₁，Ｔ₂，Ｔ₃）は、地磁気センサ１６のデバイス評価の結果、予め求められた分散値である。

また、拡張カルマンフィルタの一般式における、（４）カルマンゲインＫ_k、（５）更新された状態推定値ｘ_k|k及び（６）更新された誤差共分散行列Ｐ_k|kは、第一の観測更新の手続きと同様に、求めることができる。

地磁気信頼性評価部１０５は、地磁気取得部１０３を通じて地磁気センサ１６から取得した地磁気ベクトルが信頼できる情報であるかどうかを判定し、その結果を姿勢演算部１０４に伝える。地磁気センサ１６のセンサ情報は、地磁気の乱れや、周辺の環境磁場の影響により、信頼性が低下することが知られている。地磁気信頼性評価部１０５は、これらの影響の有無を評価し、データが信頼できるかどうかの判定を行う。以下に、その判定処理の詳細を述べる。

地磁気信頼性評価部１０５は、まず、上述した手続きにより求められる、最新の姿勢情報である第一の姿勢情報（クオータニオン）を取得する。次に、ＴＲＩＡＤアルゴリズムの枠組みに従って、加速度及び地磁気のリファレンスベクトルと、地磁気取得部１０３から逐次得られる地磁気ベクトルとを元に、第二の姿勢情報（クオータニオン）を取得する。なお、鉛直下向き成分を表す加速度及び地磁気のリファレンスベクトルは、工場出荷時、又はユーザによる設定により設定される。

≪ＴＲＩＡＤアルゴリズムを用いた姿勢情報の計算≫
ここで、図１６を用いて、地磁気信頼性評価部１０５が、既知のＴＲＩＡＤアルゴリズムにより、第二の姿勢情報を計算する例を説明する。

まず、ステップＳ１０において、工場出荷時、あるいはユーザによる設定時に、初期化処理が実行され、加速度の鉛直下向き成分を表すベクトルと、地磁気ベクトルのリファレンスベクトルが、AccRef及びMagRefとして、それぞれ記憶される。ここで、加速度の鉛直下向き成分を表すベクトルは、以下のステップＳ２０で説明されるように、クオータニオンから変換して求められる。また、地磁気ベクトルは、地磁気センサから入力された、磁北を表すベクトルである。当該ステップは、工場出荷時、あるいはユーザによる設定時にのみ実行される。すなわち、リファレンスベクトルは、初期化処理が実行されない限り、同じ値が保持される。

ステップＳ３０において、AccFrameとMagFrameを用いて、３×３の行列であるMagFrameMを計算する。

ステップＳ３２において、AccFrameとMagFrameの外積を求め、その結果を正規化することにより、AccCrossMagを求める。

ステップＳ３４において、AccFrameとAccCrossMagの外積を求め、その結果を正規化することにより、AccCrossAcMを求める。

ステップＳ３６において、AccFrameと、ステップＳ３２で求めたAccCrossMagと、ステップＳ３４で求めたAccCrossAcM（何れも１×３の行列（ベクトル））とを組み合わせて、３ｘ３の行列であるMagFrameMを生成する。

一方、ステップＳ４０において、AccRefとMagRefを用いて、３×３の行列であるMagRefMを計算する
ステップＳ４２において、AccRefとMagRefの外積を求め、その結果を正規化することにより、MagCrossAccを求める。

ステップＳ４４において、AccRefとMagCrossAccの外積を求め、その結果を正規化することにより、MagCrossを求める。

ステップＳ４６において、AccRefと、ステップＳ４２で求めたMagCrossAccと、ステップＳ４４で求めたMagCross（何れも１×３の行列（ベクトル））とを組み合わせて、３ｘ３の行列であるMagRefMを生成する。

ここで、上記ステップＳ４０（Ｓ４２−Ｓ４６）は、初期化処理が実行され、AccRef及びMagRefが変更されたときに実行され、以後、再度の初期化処理が実行されるまで、求めたMagRefMの値を保持して利用しても良い。

ステップＳ５０において、MagFrameとMagRefMの内積を求める。求められた行列をmag_triad（３×３）と呼ぶ。行列mag_triadは、デバイス座標系から絶対座標系へ変換するための行列である。ＴＲＩＡＤアルゴリズムにおいて、三つの列を、それぞれ、ＴＲＩＡＤ１、ＴＲＩＡＤ２、ＴＲＩＡＤ３と呼ぶ。

ステップＳ６０において、mag_triadの逆行列（絶対座標系とデバイス座標系間の変換のための行列）を求め、これをクオータニオンに変換する。このようにして得られたクオータニオンが、第二の姿勢情報である。

地磁気信頼性評価部１０５は、第一の姿勢情報と、上述した方法によって求められた第二の姿勢情報の、それぞれの微分値を比較し、それぞれが同じ変化をしているか判定する。地磁気信頼性評価部１０５は、その変化が同じ（すなわち、微分値の差が所定の範囲内）であれば、地磁気センサ１６から取得された地磁気は信頼できるものと判定する。

また、地磁気信頼性評価部１０５は、以下の判断基準を用いて、地磁気ベクトルの信頼性を評価することにより、その評価の精度を高めることができる。
−地磁気ベクトルの絶対値が規定の範囲内か
−第一の姿勢情報と地磁気ベクトルから得られる伏角値が規定の範囲内か
−第二の姿勢情報と地磁気ベクトルから得られる伏角値が規定の範囲内か
ここで、規定の範囲は、国土地理院が発行している、伏角情報と、地磁気ベクトルの大きさの情報を用いて、設定することができる。

そして、地磁気が信頼できるとき、第二の姿勢情報から算出されるヨー角を用いて、第二の観測更新の手続きが実行される。一方で、地磁気が信頼できない場合には、第二の観測更新の手続きは実行されない。

絶対加速度変換部１０７は、加速度取得部１０１が取得した加速度に、姿勢演算部１０４によって求められた逆回転行列１０６を乗算し、絶対座標系での三軸加速度（絶対加速度）を算出する。

≪ ３．２ 進行方向推定 ≫
慣性装置１において進行方向推定機能を実行する進行方向推定部２００は、バンドパスフィルタ２０１と、ピーク検出部２０４と、ピーク位置記憶部２０５と、移動加速度記憶部２０６と、水平成分移動速度特徴情報管理部２０７と、鉛直成分ピーク移動加速度取得部２０８と、水平成分移動速度特徴情報取得部２０９と、周期取得部２１０と、判定部２１１と、進行方向差出部２１２とを含む。進行方向推定部２００は、座標系変換部１００によって得られた、絶対座標系における加速度から、ユーザの一歩ごとの進行方向を求める。

バンドパスフィルタ２０１は、座標系変換部１００より出力された三軸絶対加速度から、重力成分を除去する。通過帯域は、例えば、一般的な歩行周波数である１−３Ｈｚ程度とする。なお、通過帯域は、慣性装置１の測定対象の歩行又は移動に係る周波数に応じて、適宜変更され得る。ここでは、バンドパスフィルタ２０１が出力する、重力成分が除去された絶対加速度を、移動加速度２０２と呼ぶ。移動加速度２０２は、後述する、移動加速度記憶部２０６に記憶される。また、移動加速度の鉛直成分である、鉛直成分移動加速度２０３は、後述する、ピーク検出部２０４に渡される。

ピーク検出部２０４は、バンドパスフィルタ２０１から出力される移動加速度２０２のうち、鉛直成分移動加速度２０３の変化（時間変化）を観測し、その波形の谷ピークの位置（ピーク時刻又はピーク位置）を検出する。検出されたピーク位置は、後述する、ピーク位置記憶部２０５に記憶される。以下では、谷ピークの検出方法について説明する。

図１０は、横軸を時間とした、移動加速度の鉛直成分２０３（Ｚ）と、移動加速度の水平成分（Ｘ、Ｙ）の変化を表す波形である。このように、それぞれの波形は、移動周期（例えば、歩行周期）と整合する周期を持った波形となり、特に、移動加速度の鉛直成分２０３は、±１ｍ／ｓ²程度の、水平成分に比べて大きな振幅波形が出力される。山ピークは、足が地面に着地した際に現れ、谷ピークは、片方の軸足をもう一方の足が追い越す瞬間に現れる。

図２０は、歩行動作における鉛直方向の運動特性を説明する図である。歩行動作は、下肢の動作により、一般に、立脚相と遊脚相とに分類される。立脚相（Stance Phase）は、一側下肢の踵が接地して、同側足先が離れるまでの時期である。また、遊脚相（Swing Phase）は、一側下肢の足趾が地面から離れてから、同側足の踵が地面に接するまでの時期である。さらに、歩行動作は、両脚支持期の有無によって特徴付けられる。一般に、歩行が緩徐になると、歩行の全体に占める両脚支持期の割合が増加し、歩行速度が速くなると少なくなることが知られている。また、両脚支持期は、走行状態になると、消失することも知られている。また、直線歩行をした場合に、体の上下（鉛直方向）移動量および左右（水平面方向）移動量は、立脚中期に最大となることが知られている。

立脚中期の前半は、蹴り出した片方の足が軸足追い抜く（蹴り出した片方の足が体幹直下を通り過ぎる）動作を含む。このとき、体は鉛直上方向に移動し、鉛直上向きの移動加速度が発生する。一方、立脚中期の後半は、蹴り出した片方の足の踵が接地するまでの動作を含む。このとき、体は鉛直下方向に移動し、鉛直下向きの移動加速度が発生する。

図２１は、歩行動作における水平方向の運動特性を説明する図である。立脚中期の前半の移動加速度の水平方向成分は、目標となる地点への体幹の移動のために片足を蹴り出した際の加速度と、体の重心移動に伴う横揺れに起因した加速度の影響を含む。一方、立脚中期の後半の移動加速度の水平方向成分は、蹴り出した足を踵接地地点まで移動させることに伴う体幹の移動と、進行方向に対する体の横揺れで発生する加速度と、踵を接地した際の振動に伴う加速度の影響を含む。つまり、立脚中期の後半には、蹴り出しの際に必要となる純粋な移動加速度を包含していないということになる。

従って、本願では、体幹の移動のために片足を蹴り出した際の加速度が大きく反映される、立脚中期の前半の移動加速度を用いて、進行方向の推定を行う。

そこで、鉛直成分移動加速度２０３に着目し、シグナルの閾値判定により、谷ピークを検出することで、一歩を計測する。なお、谷ピークを用いて一歩を検出する理由は、足が着地する山ピーク位置での水平方向の移動加速度は、足が着地することに伴う振動やノイズを含む可能性があるためである。谷ピーク位置での水平方向の移動加速度は、足の着地に伴う影響を減少させ、歩行に伴う実際の加速度をより的確に反映する。

ピーク検出部２０４は、移動加速度の鉛直成分２０３が、所定の閾値Ｔｈを下回り、再度、閾値Ｔｈを越える瞬間を捉えることにより、ピーク検出を行なう。ここで、所定の閾値Ｔｈを下回ったときの時刻ｔ_aと、閾値Ｔｈを越えたときの時刻ｔ_bとの中間の時刻を、ピーク位置とすることができる。例えば、閾値Ｔｈは、実際に歩行した際の、鉛直方向の移動加速度の半分程度の値を設定するのが望ましい。なお、ピーク検出のために、上記以外の任意の方法を用いても良い。

また、過去のピーク位置を記録しておくことにより、現在のピーク位置と、過去のピーク位置との間の時間を表すピーク間隔を求めることができる。

ピーク位置記憶部２０５は、ピーク検出部２０４の検出したピーク位置を記憶する。ピーク位置記憶部２０５は、例えばリングバッファにより、ピーク位置（時間）を、最新のものから、過去の時間領域に相当するものまでを記憶する。ピーク位置記憶部２０５は、少なくとも、最新のピーク位置と一つ過去のピーク位置を記憶し、後に得られたピーク位置によって随時更新される。記憶されるピーク位置の数は、慣性装置１の記憶容量に応じて適宜変更されてもよい。

移動加速度記憶部２０６は、バンドパスフィルタ２０１から出力された移動加速度２０２を、観測された時刻情報を付加し時系列データとして記憶する。

水平成分移動速度特徴情報管理部２０７は、ピーク検出部２０４によるピーク位置の検出に応じて、そのピーク位置を中心とする所定の期間(τ)内の、水平成分の移動加速度を、各成分（Ｘ、Ｙ）ごとに積算処理し、水平速度を算出する。この水平速度を、水平成分移動速度特徴情報と呼ぶ。水平成分移動速度特徴情報は、速さの方向と大きさの相対値を示すベクトルである。水平成分移動速度特徴情報管理部２０７は、水平成分速度特徴情報を、時刻情報tと共に記憶する。すなわち、水平成分移動速度特徴情報管理部２０７は、水平成分移動速度特徴情報を算出する算出部としての機能と、水平成分移動速度特徴情報を記憶する記憶部としての機能を有する。

図１１は、図１０に対応する、移動加速度の水平成分と、鉛直成分の、時間に対する変化を表す波形を示す。この例では、移動加速度の鉛直成分の波形から求められるピーク位置ｔ₁、ｔ₂、ｔ₃を中心とする、τの期間の、水平成分の移動加速度が、時間積分され、水平成分移動速度特徴情報Ｖ₁、Ｖ₂、Ｖ₃が算出される。

期間(τ)は、例えば、（ｔ_b−t_a）時間以内の期間であることが望ましい。これは、全時間領域において積算を実行すると、進行方向に対する体の横揺れで発生する加速度と、踵を接地した際の振動に伴う加速度の影響を大きく含んだまま積算してしまい、進行方向の推定が正しく行なえない可能性があるためである。

水平成分移動速度特徴情報は、前述のピーク検出処理と、その後の一連の処理によって、一歩足を踏み出し、片足が軸足を追い越す瞬間に生成される。生成される特徴情報は、大きさと方向を特徴として持った水平成分移動速度特徴ベクトルVである。図１２に例示されるように、水平成分移動速度特徴ベクトルは、片足が軸足を追い越す瞬間に歩行者の体が移動している方向（進行方向）と強度を表している。

鉛直成分ピーク移動加速度取得部２０８は、時刻tにおけるピーク位置（時間）に対応する、移動加速度の鉛直成分２０３における移動加速度（鉛直成分ピーク移動加速度）を取得し、この加速度を、後述する、判定部２１１に渡す。

水平成分移動速度特徴情報取得部２０９は、水平成分移動速度特徴情報管理部２０７から、最新の水平成分移動速度特徴情報及び過去の水平成分移動速度特徴情報を取得し、取得した水平成分移動速度特徴情報を、後述する、判定部２１１に渡す。

周期取得部２１０は、ピーク位置記憶部２０５から複数のピーク位置を取得し、測定対象の移動周期（例えば、歩行周期）への換算処理を実行する。また、周期取得部２１０は、複数のピーク位置の差分を順次計算することによって、最新の移動周期および過去の移動周期を取得することができる。周期取得部２１０は、取得した複数の移動周期を、後述する、判定部２１１に渡す。

判定部２１１は、図１３に示される手順を実行することにより、これまでに検出された各種情報が歩行動作に由来して取得されたデータであるかどうかを判定する。歩行動作は、歩行および走行を含んだ、測定対象による移動動作である。一方、歩行動作でない非歩行動作は、慣性装置を無作為又は意図的に振る動作や、外部環境から加速度を受けた場合（例えば、外部の移動物体により移動させられている場合）のように、測定対象による移動動作のみに起因しない動作である。以下、図１３に示されるステップに沿って説明する。

まず、ステップＳ１００において、鉛直成分ピーク移動加速度取得部２０８から取得した、鉛直成分ピーク移動加速度が、所定の範囲内である場合に、ステップＳ２００に進む。そうでない場合には、ステップＳ６００に進み、判定部２１１は、非歩行動作に起因すると判定する。なお、鉛直成分ピーク移動加速度についての所定の範囲は、慣性装置１の測位対象の特性（例えば、歩行者の歩行特性）に応じて、慣性装置１の製造者又はユーザにより、予め設定される。

次に、ステップＳ２００において、水平成分移動速度特徴情報管理部２０７から取得した、水平成分移動速度特徴情報（ベクトル）の大きさが、所定の範囲内である場合に、ステップＳ３００に進む。そうでない場合には、ステップＳ６００に進み、判定部２１１は、非歩行動作に起因すると判定する。なお、水平成分移動速度特徴情報（ベクトル）の大きさについての所定の範囲は、慣性装置１の測位対象の特性（例えば、歩行者の歩行特性）に応じて、慣性装置１の製造者又はユーザにより、予め設定される。

次に、ステップＳ３００において、周期取得部２１０から取得した、移動周期が、所定の範囲内である場合に、ステップＳ４００に進む。そうでない場合には、ステップＳ６００に進み、判定部２１１は、非歩行動作に起因すると判定する。なお、移動周期についての所定の範囲は、慣性装置１の測位対象の特性（例えば、歩行者の歩行特性）に応じて、慣性装置１の製造者又はユーザにより、予め設定される。

次に、ステップＳ４００において、左右の移動速度のぶれ幅が、所定の範囲内である場合に、ステップＳ５００に進み、判定部２１１は、歩行動作に起因すると判定する。そうでない場合には、ステップＳ６００に進み、判定部２１１は、非歩行動作に起因すると判定する。

ここで、左右の移動速度のぶれ幅について、図１４を用いて説明する。人の歩行では、右足を踏み出すと、右方向に移動速度ベクトルが発生し、左足を踏み出すと、左方向に移動速度ベクトルが発生する。本判定を行うために、水平成分移動速度特徴ベクトルがこの特性に適合するかどうか判定する。

判定部２１１は、まず、水平成分移動速度特徴ベクトルの始点と終点を、図１４（ｂ）のように結合する。次に、判定部２１１は、各Ｖ_nベクトルの中点を結ぶ直線と、各Ｖ_nベクトルの終点との距離ｄ_nを算出する。そして、判定部２１１は、ｄ_nが所定の範囲内であるか判定を行い、範囲内であれば、歩行動作に起因すると判定する。なお、ｄ_nについての所定の範囲は、慣性装置１の測位対象の特性（例えば、歩行者の歩行特性）に応じて、慣性装置１の製造者又はユーザにより、予め設定される。

ステップＳ５００では、判定部２１１は、歩行動作に起因すると判定する。

ステップＳ６００では、判定部２１１は、非歩行動作に起因すると判定する。

なお、上述したステップＳ１００−Ｓ４００における判断のうち、一部の判断が省略されてもよい。しかしながら、全ての判断がなされることで、より精度の高い進行方向の推定が可能となる。

進行方向差出部２１２は、上述した判定部２１１が、「歩行動作に起因する」と判定した場合に、一歩ごとの進行方向２１３を求めるために、以下の処理を行う。

進行方向差出部２１２は、ユーザが、ゼロ歩目から一歩目に移るとき、水平成分移動速度特徴情報取得部２０９より、水平成分移動速度特徴ベクトルＶ₀を取得する（図１５（ａ））。また、進行方向差出部２１２は、ユーザが、一歩目から二歩目に移るとき、水平成分移動速度特徴情報取得部２０９より、水平成分移動速度特徴ベクトルＶ₁を取得する（図１５（ａ））。

ここで、進行方向差出部２１２は、ベクトルＶ₀とベクトルＶ₁とを規格化し、ベクトルＶ₀'とベクトルＶ₁'を得る（図１５（ｂ））。そして、進行方向差出部２１２は、それらの合成ベクトル（Ｖ₀'＋Ｖ₁'）を求め、合成ベクトルの向きを、一歩ごとの進行方向２１３と推定する（（図１５（ｃ）））。上記処理は、ユーザが一歩踏み出すごとに、逐次実行される。

以上のように、測定対象の位置を測定する外部のアプリケーションは、進行方向推定部２００の出力する、一歩ごとの進行方向２１３を用いて、測定対象の現在位置を推定することが可能となる。アプリケーションは、例えば、前回の慣性装置１の位置から、進行方向差出部２１２の算出した合成ベクトル（Ｖ₀'＋Ｖ₁'）の向きに、所定の時間歩行したものとして、現在位置の推定を行うことができる。

このように、本発明の一実施形態における慣性装置１は、通常スマートフォン等で使用される慣性センサにより取得したセンサデータを用いて、慣性装置１の測定対象の一歩ごとの進行方向を推定することができる。よって、従来のＰＤＲ技術で用いられているような、フーリエ変換や主成分分析を行わないため、高い計算能力は不要となる。これにより、高速なサンプリングに耐えうる高性能な慣性センサが不要となり、慣性装置のコストダウンに寄与する。

また、慣性装置１は、一歩ごとの進行方向を推定するために、絶対座標系における加速度の、鉛直方向の加速度の谷ピーク位置を基点とし、その前後の一定の時間枠を用いて算出水平方向の速度ベクトルを用いる。これにより、歩行者の目標となる地点への体幹の移動のために片足を蹴り出した際の加速度を優位に捉えることで、進行方向に対する体の横揺れで発生する加速度や、踵を接地した際の振動に伴う加速度の影響を少なくすることができ、進行方向の推定の精度が向上する。

さらに、慣性装置１は、地磁気センサによるセンサ情報が信頼できるかどうかを評価し、センサ情報が信頼できる場合に、そのセンサ情報を、慣性装置１の姿勢を表すベクトルを補正（ヨー角成分）するために使用する。これにより、姿勢を表すベクトルを、地地磁気センサ情報を用いて、より高い精度で補正することが可能となる。

本発明の一実施形態における慣性装置１の出力する、一歩ごとの進行方向を用いることで、例えばナビゲーションを行うアプリケーションは、ＧＰＳ等の外部のシステムから絶対位置を測定できない場合に、精度の高いＰＤＲを実現することができる。

≪ ３．３ 進行方向推定結果 ≫
次に、図２２を用いて、歩行動作と非歩行動作の判定に伴う進行方向推定結果を説明する。図２２は、本発明の一実施形態における慣性装置１を右手に所持した人が、歩行動作と非歩行動作を行なった場合の、進行方向推定結果を示している。

まず、人の動作について説明する。慣性装置１を所持した人は、直線歩行をした後、静止し、慣性装置１を上下方向に振る動作（非歩行動作）を１０回実施する。そして、約１０秒間の静止の後に、直線歩行した方位角と同じ方位角に、直線歩行を行う。

図２２（ａ）は、慣性装置１（判定部２１１）が判定した結果を示す。慣性装置１は、検出された移動加速度のピークが歩行動作に起因すると判定した場合には、WALK_DETECTフラグを出力し、非歩行動作に起因すると判定した場合にはN_WALK_DETECTフラグを出力するものとする。図２２（ａ）の結果から、実際の人の動作の通りの認識結果が得られていることがわかる。

なお、慣性装置１は、実際の非歩行動作（慣性装置１を１０回上下方向に振る動作）に対して、１１回の非歩行動作を検出している。これは、慣性装置１が、実際の非歩行動作に起因して発生したノイズ（外乱）を、非歩行動作として判定していることを意味する。つまり、慣性装置１は、人が無作為又は意図的に外乱を与える動作そのもの、および、それに伴って発生した二次的なノイズを、非歩行動作と判定する。

図２２（ｂ）は、慣性装置１（進行方向算出部２１２）が、図２２（ａ）に示される判定結果に基づき、進行方向を推定した結果を表す。図２２（ｂ）からわかるように、歩行動作が認識された場合にのみ、進行方向の推定結果が得られており、非歩行動作が認識されている間は、進行方向の推定結果は出力されていない。

このように、本発明の一実施形態における慣性装置１を用いることにより、誤った進行方向推定結果の取得を防止することができる。その結果、推定した進行方向を利用する、例えば歩行者ナビゲーションのようなアプリケーションにおいて、誤った現在位置を表示する可能性が低減される。

次に、図２３、図２４を用いて、本発明の一実施形態における慣性装置１による、様々な携行形態での進行方向推定結果を説明する。図２３は、慣性装置１を所持した人が歩行する歩行経路を表す。図２４（ａ）−（ｅ）は、この条件の元で、人が５つのパターンの携行形態をとったときの、慣性装置１による進行方向推定結果を表している。

図２４（ａ）は、人が慣性装置１を片手に持った状態で、図２３に示す歩行経路を歩行した場合に、慣性装置１が取得した進行方向推定結果である。

図２４（ｂ）は、人が慣性装置１を腹部に固定した状態で、図２３に示す歩行経路を歩行した場合に、慣性装置１が取得した進行方向推定結果である。

図２４（ｃ）は、人が慣性装置１をズボンのポケットに挿入した状態で、図２３に示す歩行経路を歩行した場合に、慣性装置１が取得した進行方向推定結果である。

図２４（ｄ）は、人が慣性装置１を胸ポケットに挿入した状態で、図２３に示す歩行経路を歩行した場合に、慣性装置１が取得した進行方向推定結果である。

図２４（ｅ）は、人が慣性装置１を耳に当てた状態（通話中の動作を模した状態）で、図２３に示す歩行経路を歩行した場合に、慣性装置１が取得した進行方向推定結果である。

図２４（ａ）−（ｅ）において、±180°は、同一の方位角を示す。図２４（ａ）−（ｅ）が示すように、本発明による進行方向推定を行うことにより、様々な携行形態においても、正しい進行方向を推定することができる。その結果、例えば歩行者ナビゲーションのようなアプリケーションにおいて、ナビゲーションの精度を高めることができる。

１ 慣性装置
１１ ＣＰＵ
１２ ＲＡＭ
１３ ＲＯＭ
１４ 加速度センサ
１５ 角速度センサ（ジャイロセンサ）
１６ 地磁気センサ
１７ マイク
１８ スピーカ
１９ 通信モジュール
２０ Ｂｌｕｅｔｏｏｔｈ通信モジュール
２１ ＧＰＳ通信モジュール
２２ ディスプレイ
２３ タッチパネル
２４ 電池
２５ 気圧センサ
１００ 座標系変換部
１０１ 加速度取得部
１０２ 角速度取得部
１０３ 地磁気取得部
１０４ 姿勢演算部
１０５ 地磁気信頼性評価部
１０７ 絶対加速度変換部
２０１ バンドパスフィルタ
２０４ ピーク検出部
２０５ ピーク位置記憶部
２０６ 移動加速度記憶部
２０７ 水平成分移動速度特徴情報管理部
２０８ 鉛直成分ピーク移動加速度取得部
２０９ 水平成分移動速度特徴情報取得部
２１０ 周期取得部
２１１ 判定部
２１２ 進行方向推定部

特開２００３−３０２４１９号公報 特開２０１１−２３７４５２号公報 特開２０００−９７７２２号公報 特開２００２−１３９３４０号公報 特開２０１２−１４５４５７号公報 特許第４１２６３８８号公報

Greg Welch and Gary Bishop、"An Introduction to the Kalman Filter"、Department of Computer Science, University of North Carolina at Chapel Hill、2006年7月24日 片山徹、「非線形カルマンフィルタ」、朝倉書店、2011年11月25日 Wei Tech Ang et al.、" Kalman filtering for real-time orientation tracking of handheld microsurgical instrument"、Intelligent Robots and Systems, 2004. (IROS 2004). Proceedings. 2004 IEEE/RSJ International Conference、2004年9月28日、Volume 3、p.2574-2580 vol.3 Malcolm D. Shuster、"Deterministic Three-Axis Attitude Determination"、The Journal of the Astronautical Sciences、2004年7月、Volume 52、No. 3、p.405-419 "TRIAD Algorithm"、[online]、2011年5月25日、Wikipedia、インターネット<URL:http://en.wikipedia.org/wiki/User:Snietfeld/TRIAD_Algorithm>

Claims (13)

1. 慣性センサの出力に基づいて得られた加速度に含まれる水平成分及び鉛直成分のうち、前記鉛直成分がピークとなるピーク時刻を検出する検出部と、
   前記ピーク時刻を中心とする所定の期間内の前記加速度の水平成分を用いて、水平方向の移動速度を表す水平速度を算出する算出部と、
   前記水平速度が所定の範囲内である場合に、前記出力が歩行動作によるものであると判定する判定部と、
   前記水平速度が、歩行動作によるものと判定された場合に、前記水平速度を用いて、進行方向を推定する推定部と、
   を有し、
   前記出力が、前記歩行動作とは異なる非歩行動作であるかを認識し、
   前記推定部は、前記非歩行動作が認識されている間は前記進行方向を推定しない
   慣性装置。
2. 前記慣性センサは、加速度センサ、角速度センサ及び地磁気センサを含み、
   前記加速度センサ及び前記角速度センサの出力を用いて、当該慣性装置の姿勢を表す姿勢情報を求める演算部と、
   前記地磁気センサの出力の信頼性を評価する評価部と、
   を有し、
   前記演算部は、前記地磁気センサの出力が信頼できる場合に、前記地磁気センサの出力をさらに用いて、前記姿勢情報を求める
   請求項１に記載の慣性装置。
3. 前記評価部は、
   前記演算部が、前記加速度センサ及び前記角速度センサの出力を用いて求めた姿勢情報である第一の姿勢情報と、
   該第一の姿勢情報、予め取得された、リファレンスとなる前記加速度センサ及び前記角速度センサの出力、並びに前記地磁気センサの出力に基づいて求めた第二の姿勢情報と、
   を比較することにより、前記地磁気センサの出力の信頼性を評価する、
   請求項２に記載の慣性装置。
4. 前記第一の姿勢情報及び前記第二の姿勢情報は、クオータニオンベクトルによって表現される、
   請求項３に記載の慣性装置。
5. 前記評価部は、前記第一の姿勢情報及び前記第二の姿勢情報の微分値の差が所定の範囲内である場合に、前記地磁気センサの出力が信頼できると判定する、
   請求項４に記載の慣性装置。
6. 前記演算部は、
   前記角速度センサの出力を用いて、ロール角、ピッチ角及びヨー角についての前記姿勢情報を求める第一の手続きと、
   前記加速度センサの出力を用いて、ロール角及びピッチ角についての前記姿勢情報を求める第二の手続きと、
   前記地磁気センサの出力を用いて、ヨー角についての前記姿勢情報を求める第三の手続きと、
   を実行する、請求項２に記載の慣性装置。
7. 前記ピークは、前記加速度の鉛直成分が負となる場合のピークである、
   請求項１乃至６何れか一項に記載の慣性装置。
8. 前記判定部は、さらに、前記ピーク時刻における前記加速度の鉛直成分が所定の範囲内である場合に、前記出力が歩行動作によるものであると判定する、
   請求項７に記載の慣性装置。
9. 前記ピーク時刻に基づいて移動周期を取得する周期取得部
   をさらに有し、
   前記判定部は、さらに、前記移動周期が所定の範囲内である場合に、前記出力が移動動作によるものであると判定する、
   請求項１乃至８何れか一項に記載の慣性装置。
10. 前記判定部は、さらに、前記水平速度を用いて求めた、当該慣性装置の進行方向に対する左右のぶれ幅が、所定の範囲内である場合に、前記出力が歩行動作によるものであると判定する、
    請求項１乃至９何れか一項に記載の慣性装置。
11. 前記水平速度は、二つのベクトルによって表され、
    前記推定部は、二つのベクトルの合成ベクトルの向きを進行方向と推定する、
    請求項１乃至１０何れか一項に記載の慣性装置。
12. コンピュータが、慣性センサの出力に基づいて得られた加速度に含まれる水平成分及び鉛直成分のうち、前記鉛直成分がピークとなるピーク時刻を検出する検出段階と、
    コンピュータが、前記ピーク時刻を中心とする所定の期間における、前記加速度の水平成分を用いて、水平方向の移動速度を表す水平速度を算出する算出段階と、
    コンピュータが、前記水平速度が所定の範囲内である場合に、前記出力が歩行動作によるものであると判定する判定段階と、
    コンピュータが、前記水平速度が、歩行動作によるものと判定された場合に、前記水平速度を用いて、進行方向を推定する推定段階と、
    を有し、
    前記出力が、前記歩行動作とは異なる非歩行動作であるかを認識し、
    前記推定段階では、前記非歩行動作が認識されている間は前記進行方向を推定しない
    方法。
13. コンピュータに、請求項１２に記載の方法を実行させるプログラム。

Images