(57)【要約】【課題】算出される進行方向の誤差が抑えられた進行方向算出装置、進行方向算出方法、および進行方向算出方法をコンピューターに実行させるプログラムを提供する。【解決手段】位置算出装置（進行方向算出装置）１は、ユーザーに装着した慣性センサーとしてのＩＭＵ１０から出力信号を受信する受信部１０５と、受信部１０５が受信した出力信号を用いてユーザーの進行方向を算出する進行方向算出部１１５と、を含み、進行方向算出部１１５は、ユーザーの歩行または走行により発生する出力信号に基づく出力周期における、片方の足の動きにより発生する出力周期の振幅の第１のピークと、他方の足の動きにより発生する出力周期の振幅の第２のピークとの中間にある算出タイミングで進行方向を算出する進行方向算出処理を実行することを特徴とする。【選択図】図３
2016200556.tif 000002
【特許請求の範囲】
【請求項１】
ユーザーに装着した慣性センサーから出力信号を受信する受信部と、
前記受信部が受信した前記出力信号を用いて前記ユーザーの進行方向を算出する進行方向算出部と、を含み、
前記進行方向算出部は、前記ユーザーの歩行または走行により発生する前記出力信号に基づく出力周期における、片方の足の動きにより発生する前記出力周期の振幅の第１のピークと、他方の足の動きにより発生する前記出力周期の振幅の第２のピークとの中間にある算出タイミングで前記進行方向を算出することを特徴とする進行方向算出装置。
【請求項２】
請求項１に記載の進行方向算出装置において、
前記算出タイミングは、前記第１のピークと前記第２のピークとの振幅の中間値を示すタイミングであることを特徴とする進行方向算出装置。
【請求項３】
請求項１または請求項２に記載の進行方向算出装置において、
前記出力周期は、重力軸に対して垂直に交わる面に沿った角度の出力周期であることを特徴とする進行方向算出装置。
【請求項４】
請求項３に記載の進行方向算出装置において、
前記算出タイミングは、前記出力周期の出力がゼロに近似な値を示すタイミングであることを特徴とする進行方向算出装置。
【請求項５】
請求項１〜請求項４のいずれか一項に記載の進行方向算出装置において、
前記算出タイミングは、前記第１のピーク寄りの第１の閾値と、前記第２のピーク寄りの第２の閾値との間の所定の範囲を有することを特徴とする進行方向算出装置。
【請求項６】
ユーザーに装着した慣性センサーから出力信号を受信することと、
前記ユーザーの進行方向を算出することと、を含み、
前記進行方向を算出することは、前記ユーザーの歩行または走行により発生する前記出力信号に基づく出力周期における、片方の足の動きにより発生する前記出力周期の振幅の第１のピークと、他方の足の動きにより発生する前記出力周期の振幅の第２のピークとの中間にある算出タイミングで実行することを特徴とする進行方向算出方法。
【請求項７】
請求項６に記載の進行方向算出方法において、
前記算出タイミングは、前記第１のピークと前記第２のピークとの振幅の中間値を示すタイミングであることを特徴とする進行方向算出方法。
【請求項８】
請求項６または請求項７に記載の進行方向算出方法において、
前記出力周期は、重力軸に対して垂直に交わる面に沿った角度の出力周期であることを特徴とする進行方向算出方法。
【請求項９】
請求項８に記載の進行方向算出方法において、
前記算出タイミングは、前記出力周期の出力がゼロに近似な値を示すタイミングであることを特徴とする進行方向算出方法。
【請求項１０】
請求項６〜請求項９のいずれか一項に記載の進行方向算出方法において、
前記算出タイミングは、前記第１のピーク寄りの第１の閾値と、前記第２のピーク寄りの第２の閾値との間の所定の範囲を有することを特徴とする進行方向算出方法。
【請求項１１】
請求項６〜請求項１０のいずれか一項に記載の進行方向算出方法をコンピューターに実行させるプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、進行方向算出装置、進行方向算出方法、および進行方向算出方法をコンピューターに実行させるプログラムに関する。
【背景技術】
【０００２】
従来、センサーの計測結果を利用して位置を算出する慣性航法演算においては、センサーの計測結果に含まれ得る種々の誤差に起因して位置算出の正確性が低下するという問題があった。この問題に対応するための技術として、例えば、特許文献１に記載されているように、歩行または走行するユーザーの位置算出において、ＧＰＳ（Global Positioning System）によって得られたＧＰＳ方位角の平均値と、ユーザーに装着された慣性センサー（例えば加速度センサーや角速度センサー）によって計測されたヨー角度の平均値との差分を算出し、差分が閾値以上であると判断された回数が一定の値以上である場合に、ヨー角の初期化が異常状態であると判定する技術が知られていた。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１２−０４２２８５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、特許文献１に記載の技術では、慣性センサーを体に装着したユーザーが歩行または走行した場合、ユーザーの右足は右斜め前に踏み出され、左足は左斜め前に踏み出されるので、計測されるセンサー方位が１歩ごとに変動する。このため、計測されたヨー角度を平均化する期間として既定の時間の範囲などを設定すると、真の進行方向とは大きくずれたヨー角度も含めて平均化されてしまうことにより、ユーザーの位置および進行方向の算出結果に誤差が生じる虞があるという課題があった。
【課題を解決するための手段】
【０００５】
本発明は、上述の課題の少なくとも一部を解決するためになされたものであり、以下の形態または適用例として実現することが可能である。
【０００６】
［適用例１］  本適用例に係る進行方向算出装置は、ユーザーに装着した慣性センサーから出力信号を受信する受信部と、前記受信部が受信した前記出力信号を用いて前記ユーザーの進行方向を算出する進行方向算出部と、を含み、前記進行方向算出部は、前記ユーザーの歩行または走行により発生する前記出力信号に基づく出力周期における、片方の足の動きにより発生する前記出力周期の振幅の第１のピークと、他方の足の動きにより発生する前記出力周期の振幅の第２のピークとの中間にある算出タイミングで前記進行方向を算出することを特徴とする。
【０００７】
本適用例によれば、ユーザーの歩行または走行により発生する出力信号に基づく出力周期における、片方の足の動きにより発生する振幅の第１のピークと、他方の足の動きにより発生する振幅の第２のピークとの中間にあるタイミングで、ユーザーの進行方向を算出する。これにより、ユーザーの歩行または走行の進行方向の算出において、歩行の周期が考慮されていない既定の期間に慣性センサーで計測されたヨー角度を平均化する従来の算出方法に比して、算出結果の誤差を抑える効果を得ることができる。
したがって、ユーザーの歩行または走行の進行方向をより正確に算出することが可能な進行方向算出装置を提供することができる。
【０００８】
［適用例２］  上記適用例にかかる進行方向算出装置において、前記算出タイミングは、前記第１のピークと前記第２のピークとの振幅の中間値を示すタイミングであることが好ましい。
【０００９】
本適用例によれば、第１のピークと第２のピークとの振幅の中間値を示すタイミングをユーザーの進行方向の算出タイミングとすることにより、歩行または走行によって発生する振動での方位変化の影響を受けにくい。これにより、ユーザーの進行方向の誤差を抑制することができる。
【００１０】
［適用例３］  上記適用例にかかる進行方向算出装置において、前記出力周期は、重力軸に対して垂直に交わる面に沿った角度の出力周期であることが好ましい。
【００１１】
本適用例によれば、外部からの進行方向の入力によって、ユーザーの進行方向の誤差を軽減する場合、外部からの進行方向の入力は、重力軸に対して垂直に交わる平面に沿った方向の角度（方位）であることが多いため、ユーザーの進行方向を前記平面上で求めることで、容易に前記誤差を軽減することができる。
【００１２】
［適用例４］  上記適用例にかかる進行方向算出装置において、前記算出タイミングは、前記出力周期の出力がゼロに近似な値を示すタイミングであることが好ましい。
【００１３】
本適用例によれば、人間（ユーザー）の歩行または走行は、左右の足を交互に出す動作であり、ユーザーの進行方向を中心とした振幅を持つ出力周期を出力するため、その出力周期の出力がゼロに近似な値を示すタイミングでユーザーの進行方向を算出することによって、進行方向をより精度良く算出することが可能となる。
【００１４】
［適用例５］  上記適用例にかかる進行方向算出装置において、前記算出タイミングは、前記第１のピーク寄りの第１の閾値と、前記第２のピーク寄りの第２の閾値との間の所定の範囲を有することを特徴とする。
【００１５】
本適用例によれば、第１の閾値と第２の閾値との間の所定の範囲を算出タイミングとすることにより、進行方向を精度よく算出することが可能な算出タイミングをより容易に取得することができる。
【００１６】
［適用例６］  本適用例にかかる進行方向算出方法は、ユーザーに装着した慣性センサーから出力信号を受信することと、前記ユーザーの進行方向を算出することと、を含み、前記進行方向を算出することは、前記ユーザーの歩行または走行により発生する前記出力信号に基づく出力周期における、片方の足の動きにより発生する前記出力周期の振幅の第１のピークと、他方の足の動きにより発生する前記出力周期の振幅の第２のピークとの中間にある算出タイミングで実行することを特徴とする。
【００１７】
本適用例によれば、ユーザーの歩行または走行により発生する出力信号に基づく出力周期における、片方の足の動きにより発生する振幅の第１のピークと、他方の足の動きにより発生する振幅の第２のピークとの中間にあるタイミングで、ユーザーの進行方向を算出する。これにより、ユーザーの歩行または走行の進行方向の算出において、歩行の周期が考慮されていない既定の期間に慣性センサーで計測されたヨー角度を平均化する従来の算出方法に比して、算出結果の誤差を抑える効果を得ることができる。
したがって、ユーザーの歩行または走行の進行方向をより正確に算出することが可能な進行方向算出方法を提供することができる。
【００１８】
［適用例７］  上記適用例にかかる進行方向算出方法において、前記算出タイミングは、前記第１のピークと前記第２のピークとの振幅の中間値を示すタイミングであることを特徴とする。
【００１９】
本適用例によれば、第１のピークと第２のピークとの振幅の中間値を示すタイミングをユーザーの進行方向の算出タイミングとすることにより、歩行または走行によって発生する振動での方位変化の影響を受けにくいので、ユーザーの進行方向の誤差を抑制することができる。
【００２０】
［適用例８］  上記適用例にかかる進行方向算出方法において、前記出力周期は、重力軸に対して垂直に交わる面に沿った角度の出力周期であることを特徴とする。
【００２１】
本適用例によれば、外部からの進行方向の入力によって、ユーザーの進行方向の誤差を軽減する場合、外部からの進行方向の入力は、重力軸に対して垂直に交わる平面に沿った方向の角度（方位）であることが多いため、ユーザーの進行方向を前記平面上で求めることで、進行方向算出における誤差を比較的容易に軽減することができる。
【００２２】
［適用例９］  上記適用例にかかる進行方向算出方法において、前記算出タイミングは、前記出力周期の出力がゼロに近似な値を示すタイミングであることを特徴とする。
【００２３】
本適用例によれば、人間（ユーザー）の歩行または走行は、左右の足を交互に出す動作であり、ユーザーの進行方向を中心とした振幅を持つ出力周期を出力するため、その出力周期の出力がゼロに近似な値を示すタイミングでユーザーの進行方向を算出することによって、進行方向をより精度良く算出することが可能となる。
【００２４】
［適用例１０］  上記適用例にかかる進行方向算出方法において、前記算出タイミングは、前記第１のピーク寄りの第１の閾値と、前記第２のピーク寄りの第２の閾値との間の所定の範囲を有することを特徴とする。
【００２５】
本適用例によれば、第１の閾値と第２の閾値との間の所定の範囲を算出タイミングとすることにより、進行方向を精度よく算出することが可能な算出タイミングをより容易に取得することができる。
【００２６】
［適用例１１］  本適用例にかかるプログラムは、上記適用例にかかる進行方向算出方法をコンピューターに実行させることを特徴とする。
【００２７】
本適用例によれば、コンピューターに対して、上記適用例にかかる進行方向算出方法を行う制御を実行させることができるので、ユーザーの歩行中または走行中の進行方向を、より正確に算出できるという効果が得られる。
【図面の簡単な説明】
【００２８】
【図１】進行方向算出装置としての機能を含む位置算出装置の全体システムの構成例を示す図。
【図２】位置算出装置の機能構成の一例を示す図。
【図３】処理部の機能ブロックを示す図。
【図４】進行方向算出処理を含む位置算出処理の流れを示すフローチャート。
【図５】実施形態における進行方向の算出タイミングを示す図。
【図６】進行方向の算出タイミングの変形例を示す図。
【発明を実施するための形態】
【００２９】
以下、図面を参照して、本発明の好適な実施形態の一例について説明する。但し、本発明を適用可能な実施形態が以下説明する実施形態に限定されるわけでないことは勿論である。
【００３０】
１．システム構成
図１は、本発明の進行方向算出装置としての機能を含む位置算出装置１の全体システムの構成例を示す図である。この位置算出装置１は、例えばユーザーの腰（右腰又は左腰）に装着して利用される小型電子機器であり、ユーザーの位置を算出して表示する進行方向算出装置の一種である。なお、位置算出装置１は、ユーザーが位置の算出に係る各種操作を入力するための入力装置である操作ボタン３と、算出された位置等の情報が表示される液晶ディスプレイ５と、スピーカー７とを有し、装置内部にＩＭＵ（Inertial Measurement Unit）１０の他、図２の処理部１００の機能を担うＣＰＵ（Central Processing Unit）等のプロセッサー、記憶部６００の機能を担うメモリー、通信部５００の機能を担う通信回路、ＧＰＳ（Global Positioning System）センサー２０などを有する。
【００３１】
本実施形態では、３種類の座標系を定義する。第１の座標系は、位置算出装置１が具備するＩＭＵ１０に対応付けられた三次元直交座標系（センサー座標系）であるローカル座標系である。本明細書では、ローカル座標系のことをＬ（Local）フレームとも言う。本実施形態では、ローカル座標系の３軸をｘ軸、ｙ軸及びｚ軸と表記する。
【００３２】
第２の座標系は、移動体に対応付けられた三次元直交座標系（移動体座標系）である。本明細書では、移動体座標系のことをＶ（Vehicle）フレームとも言う。本実施形態では、ユーザーの前方を正とする前後方向をロール軸（Ｒ軸）、右方を正とする左右方向をピッチ軸（Ｐ軸）、鉛直下方を正とする上下方向をヨー軸（Ｑ軸）とする。
【００３３】
第３の座標系は、移動体の移動空間を定める座標系である三次元直交座標系（絶対座標系）である。本明細書では、絶対座標系のことをＡ（Absolute）フレームとも言う。Ａフレームは、例えば、北東下座標系として知られるＮＥＤ（North East Down）座標系や、地球中心地球固定座標系として知られるＥＣＥＦ（Earth Centered Earth Fixed）座標系として定義される。本実施形態では、絶対座標系の３軸をＸ軸、Ｙ軸及びＺ軸と表記する。
【００３４】
また、本実施形態では、絶対座標系（Ａフレーム）の正確な方位を認識できていない場合でも、かつ、移動体が移動を開始しても、慣性航法演算を開始できる。その絶対座標系の方位を認識できていない間は絶対座標系を仮に設定した「絶対仮座標系」で位置を算出する。移動している移動体の絶対仮座標系での位置が、絶対仮座標系での移動位置に相当する。
【００３５】
ＩＭＵ１０は、位置算出装置１に搭載されるセンサーユニットであり、慣性航法ユニットとして知られるものである。ＩＭＵ１０は、加速度センサー１１と、ジャイロセンサー１３とを有する。図１に示すようにＩＭＵ１０のｚ軸が鉛直方向の軸となる。
【００３６】
加速度センサー１１は、ユーザーの加速度ベクトルを検出する。加速度センサー１１としては、例えばＭＥＭＳ（Micro Electro Mechanical Systems）センサーが用いられる。加速度センサー１１が計測した値は、ローカル座標系で計測した移動体の加速度となる。
【００３７】
本明細書では、スカラーとベクトルとを適宜区別して説明する。原則として、加速度や速度と言ったときは加速度や速度の大きさ（スカラー量）を表し、加速度ベクトルや速度ベクトルと言ったときは方向及び大きさを考慮した加速度及び速度を表すものとする。
【００３８】
ジャイロセンサー１３は、ユーザーの角速度を検出するセンサーであり、位置算出装置１が具備する加速度センサー１１と同じローカル座標系で角速度（以下、「ローカル座標角速度」と称す。）を検出する。
【００３９】
ＧＰＳセンサー２０は、測位用衛星の一種であるＧＰＳ衛星から送信されるＧＰＳ衛星信号を受信し、当該ＧＰＳ衛星信号を利用してユーザーの位置及び速度ベクトルを算出するセンサーである。なお、ＧＰＳを利用して位置や速度ベクトルを算出する方法については従来公知であるため、詳細な説明を省略する。
【００４０】
また、各座標系において定義される諸量を明確にするため、各諸量を表す文言の先頭に座標系の種類を付して説明する。例えば、ローカル座標系で表した加速度ベクトルのことを「ローカル座標加速度ベクトル」と称し、絶対座標系で表した加速度ベクトルのことを「絶対座標加速度ベクトル」と称する。他の諸量についても同様である。また、ユーザーに対するセンサーの相対的な姿勢（相対姿勢）のことを「センサー姿勢」と表現し、その姿勢角（相対姿勢角）のことを「センサー姿勢角」と称す。
【００４１】
また、本明細書における数式では、速度ベクトルの成分を小文字の“ｖ”で表す。また、速度ベクトルの表記において、座標系の種類を上付きの大文字の添え字で示す。“Ｌ”はローカル座標系、“Ｖ”は移動体座標系、“Ａ”は絶対座標系をそれぞれ意味する。また、対応する座標系の各軸の成分を下付きの小文字の添え字で示す。例えば、ローカル座標系の３軸であれば“ｘ”、“ｙ”及び“ｚ”の表記を用いる。
【００４２】
２．機能構成
図２は、位置算出装置１の機能構成の一例を示す図である。
位置算出装置１は、ＩＭＵ１０と、ＧＰＳセンサー２０と、処理部１００と、操作部２００と、表示部３００と、音出力部４００と、通信部５００と、記憶部６００とを有して構成される。
【００４３】
処理部１００は、記憶部６００に記憶されているシステムプログラム等の各種プログラムに従って、位置算出装置１の各部を統括的に制御する。
【００４４】
図３は、処理部１００の機能構成の概略を表わす機能ブロック図である。
処理部１００は、受信部１０５と、慣性航法演算部１１０と、進行方向算出部１１５と、方位差取得部１２０と、修正部１３０と、カルマンフィルター処理部（以下、「ＫＦ（Kalman Filter）処理部」と称す。）１４０とを機能部として有する。
【００４５】
受信部１０５は、ＩＭＵ１０の加速度センサー１１およびジャイロセンサー１３から出力信号を受信する。具体的には、受信部１０５は、加速度センサー１１から出力されるローカル座標加速度ベクトルと、ジャイロセンサー１３から出力されるローカル座標角速度とを受信する。
【００４６】
慣性航法演算部１１０は、加速度センサー１１から受信したローカル座標加速度ベクトルと、ジャイロセンサー１３から受信したローカル座標角速度とを用いて、公知の慣性航法演算処理を行って、絶対座標系におけるユーザーの位置（絶対座標位置）、速度ベクトル（絶対座標速度ベクトル）及び姿勢角（絶対姿勢角）を算出する。慣性航法演算部１１０は、ＫＦ処理部１４０から出力される加速度バイアス及びジャイロバイアスを用いて、ＩＭＵ１０から出力されるローカル座標加速度ベクトル及びローカル座標角速度を補正する。そして、補正後のローカル座標加速度ベクトル及びローカル座標角速度を用いて絶対座標位置、絶対座標速度ベクトル及び絶対姿勢角を算出し、ＫＦ処理部１４０から出力される位置誤差、速度ベクトル誤差及び姿勢角誤差を用いて、絶対座標位置、絶対座標速度ベクトル及び絶対姿勢角を補正する。
【００４７】
進行方向算出部１１５は、慣性航法演算部１１０から出力される慣性航法演算方位の出力信号を用いて、ユーザーの進行方向を算出する算出タイミングを設定し、その算出タイミングでユーザーの進行方向を算出する。ここで、進行方向算出部１１５は、慣性航法演算方位の出力信号の出力周期における振幅の第１のピークと第２のピークとの中間にあるタイミングを算出タイミングとする（詳細は後述する）。なお、以上述べた、ＩＭＵ１０、受信部１０５、慣性航法演算部１１０、および進行方向算出部１１５は、本実施形態の位置算出装置（位置算出システム）において、慣性航法演算によりユーザーの進行方向（移動方向）を算出する進行方向算出装置（移動方向算出装置）としての機能を含む。
【００４８】
方位差取得部１２０は、進行方向算出部１１５により算出された進行方向（慣性航法演算方位）と、ＧＰＳセンサー２０から出力されるＧＰＳ演算方位（絶対座標系上での移動方向：絶対方位）とを用いて、絶対座標系と絶対仮座標系との方位差を取得する。例えば、慣性航法演算方位が絶対仮座標系上で定義され、ＧＰＳ演算方位が絶対座標系上で定義される。しかし、実際には、慣性航法演算方位もＧＰＳ演算方位も同じ方角である。従って、慣性航法演算方位とＧＰＳ演算方位とを同じ方角とすることで、絶対仮座標系の軸方向と、絶対座標系の軸方向とのなす角度（姿勢の違いとも言える）を方位差として取得する。ＧＰＳ演算方位は、衛星信号に基づく移動方向に相当する。また、本実施形態の方位差取得部１２０は、絶対座標系上での移動方向を算出する移動方向算出部の機能を含む。
【００４９】
修正部１３０は、慣性航法演算部１１０による慣性航法演算の結果に基づき、進行方向算出部１１５によって適切な算出タイミングで算出されたユーザーの進行方向（移動方位）と、と、方位差取得部１２０によって取得された方位差とを用いて、移動位置を絶対座標系上の位置に修正する。
【００５０】
ＫＦ処理部１４０は、カルマンフィルター処理を行う機能部である。慣性航法演算部１１０から出力される慣性航法演算結果（絶対座標位置、絶対座標速度ベクトル及び絶対姿勢角）を制御入力として、誤差推定演算（以下、「ＫＦ誤差推定演算」と称す。）を行って、慣性航法演算結果に含まれる誤差（以下、「慣性航法演算誤差」と称す。）を推定する。また、推定した慣性航法演算誤差を慣性航法演算部１１０にフィードバック出力する。ＫＦ処理部１４０は、修正部１３０による座標系の修正の前は慣性航法演算部１１０によって算出された移動位置に、座標系の修正の後は修正後の移動位置に所定のフィルター処理を施すフィルター処理部に相当する。
【００５１】
図２に戻り、操作部２００は、例えばタッチパネルやボタンスイッチ等の入力装置を有して構成され、なされた操作に応じた操作信号を、処理部１００に出力する。この操作部２００の操作により、位置の算出指示等の各種指示入力がなされる。操作部２００は、図１の操作ボタン３に相当する。
【００５２】
表示部３００は、例えばＬＣＤ（Liquid Crystal Display）等の表示装置を有して構成され、処理部１００から入力される表示信号に基づく各種表示を行う。表示部３００には、算出された位置等の情報が表示される。表示部３００は、図１の液晶ディスプレイ５に相当する。
【００５３】
音出力部４００は、例えばスピーカーなどの音出力装置を有して構成され、処理部１００から入力される音声信号に基づく各種音出力を行う。音出力部４００は、図１のスピーカー７に相当する。
通信部５００は、装置内部で利用される情報や算出した結果等を、外部の情報通信装置との間で送受するための通信装置である。
【００５４】
記憶部６００は、例えばＲＯＭ（Read Only Memory）やフラッシュＲＯＭ、ＲＡＭ（Random Access Memory）等の記憶装置で実現され、処理部１００が位置算出装置１を統括的に制御するためのシステムプログラムや、各種アプリケーション処理を実行するための各種プログラムやデータ等を記憶する。
【００５５】
記憶部６００には、位置算出処理（図４参照）として実行される位置算出プログラム６１０が記憶されている。位置算出プログラム６１０は、進行方向算出処理（図４参照）として実行される進行方向算出プログラム６１１、および、ＫＦ処理（図４参照）として実行されるＫＦ処理プログラム６１２をサブルーチンとして含む。また、記憶部６００には、ＩＭＵ検出データ６３０と、ＧＰＳ検出データ６４０と、方位差データ６５０と、慣性航法演算データ６６０と、進行方向算出データ６６５と、慣性航法演算誤差データ６７０とが記憶される。
【００５６】
ＩＭＵ検出データ６３０は、ＩＭＵ１０によって検出されたローカル座標加速度ベクトル及びローカル座標角速度を時系列に記憶する。
ＧＰＳ検出データ６４０は、ＧＰＳセンサー２０によって演算されたＧＰＳ演算結果を時系列に記憶する。
方位差データ６５０は、方位差取得部１２０によって取得された方位差を記憶する。
慣性航法演算データ６６０は、慣性航法演算部１１０が慣性航法演算を行うことで取得されるデータであり、位置や速度ベクトル、姿勢角といった諸量がこれに含まれる。
進行方向算出データ６６５は、慣性航法演算データ６６０の最新の慣性航法演算データに基づいて進行方向算出部１１５が設定した算出タイミングで進行方向算出部１１５が算出することで取得されるユーザーの進行方向（方位）のデータである。
慣性航法演算誤差データ６７０は、ＫＦ処理部１４０がＫＦ処理を行うことで取得されるデータであり、位置誤差や速度ベクトル誤差、姿勢角誤差、加速度バイアス、ジャイロバイアスといった諸量がこれに含まれる。
【００５７】
３．処理の流れ
図４は、位置算出装置１の処理部１００が記憶部６００に記憶されている位置算出プログラム６１０に従って実行する位置算出処理の流れを示すフローチャートである。また、図５は、位置算出処理における進行方向算出処理のサブルーチンにおいて、進行方向の算出タイミングを示す図である。以下、本実施形態の進行方向算出処理のサブルーチンを含む位置算出処理について、これらの図を参照して説明する。
【００５８】
図４に示す本実施形態の位置算出処理において、最初に、処理部１００は、ＩＭＵ１０及びＧＰＳセンサー２０から検出データの取得を開始し、記憶部６００にＩＭＵ検出データ６３０及びＧＰＳ検出データ６４０として記憶させる（ステップＡ１）。その後、慣性航法演算部１１０が、慣性航法演算処理を開始し（ステップＡ３）、慣性航法演算の算出開始位置を含む慣性航法演算データ６６０を記憶部６００に記憶させる（ステップＡ５）。なお、慣性航法演算処理を開始するステップＡ３から、以降説明する進行方向を算出するステップＡ８にかけてが、本実施形態の位置算出処理における進行方向算出処理のサブルーチンとなる。
【００５９】
次いで、処理部１００の進行方向算出部１１５が、記憶部６００に記憶された進行方向算出プログラム６１１に従って進行方向算出処理を行う。進行方向算出処理では、まず、ステップＡ５で取得した慣性航法演算データの出力信号を用いて、ユーザーの進行方向をより高精度に算出することが可能な算出タイミングの設定を行う（ステップＡ７）。この算出タイミングの設定について、図５を用いて詳細に説明する。図５において、縦軸は出力周期の出力としてのＹａｗ角（ヨー角度）であり、横軸は時間ｔである。出力としてのＹａｗ角は、重力軸に対して垂直に交わる面に沿った角度であり、ユーザーの進行方向の角度（方位）であることが多いため、このＹａｗ角の出力周期を進行方向の算出に用いることにより、ユーザーの進行方向算出における誤差を比較的容易に軽減することを可能としている。図５に示すユーザーの歩行または走行により発生する出力信号に基づく出力周期において、ユーザーの片方の足（例えば左足）の動きにより発生する振幅の第１のピークと、他方の足（例えば右足）の動きにより発生する振幅の第２のピークとの中間にあるタイミングを、進行方向の算出タイミングとして設定する。特に、本実施形態では、出力周期の振幅の第１のピークと第２のピークとの中間値を示すタイミングＴｓｙ１を算出タイミングＴｓｙ１として設定する。このように、第１のピークと第２のピークとの振幅の中間値を示すタイミングをユーザーの進行方向の算出タイミングとすることにより、歩行または走行によって発生する振動での方位変化の影響を受けにくくなる。これにより、ユーザーの進行方向の誤差を抑制することが可能な算出タイミングＴｓｙ１を設定することができる。
しかも、本実施形態では、図５に示すように、進行方向の算出タイミングＴｓｙ１は、出力周期の出力（Ｙａｗ角）がゼロに近似な値を示すタイミングとなっている（図５では、算出タイミングＴｓｙ１において出力周期の出力は略ゼロ）。これにより、人間（ユーザー）の歩行または走行は、左右の足を交互に出す動作であり、ユーザーの進行方向を中心とした振幅を持つ出力周期を出力するため、その出力周期の出力がゼロに近似な値を示すタイミング（算出タイミングＴｓｙ１）でユーザーの進行方向を算出することによって、進行方向をより精度良く算出することが可能となる。
【００６０】
次いで、処理部１００の進行方向算出部１１５が、ステップＡ７で設定された進行方向の算出タイミングＴｓｙ１にて進行方向の算出を行う（ステップＡ８）。
【００６１】
次いで、処理部１００は、ステップＡ８の算出タイミングにてＧＰＳセンサー２０からＧＰＳ演算結果を取得できたか否かを判定する（ステップＡ９）。ＧＰＳ演算結果が取得できなかったと判定したならば（ステップＡ９；Ｎｏ）、処理部１００は、ステップＡ７に戻る。一方、ＧＰＳ演算結果が取得できたと判定したならば（ステップＡ９；Ｙｅｓ）、方位差取得部１２０が、方位差取得処理を行う（ステップＡ１１）。具体的には、進行方向算出部１１５によって適切な算出タイミングで算出されたユーザーの進行方向（移動方位）と、ＧＰＳセンサー２０から取得した絶対方位との方位差θを算出し、方位差データ６５０として記憶部６００に記憶させる。
【００６２】
次いで、修正部１３０は、方位差取得部１２０によって取得された方位差を用いて座標系を修正する座標系修正処理を行う（ステップＡ１３）。この際、慣性航法演算の算出開始位置と現在の慣性航法演算位置とを結ぶ位置ベクトルを方位差を用いて回転させることで移動位置を修正する。また、速度ベクトルを方位差を用いて回転させることで速度ベクトルを修正し、変換後の移動位置を用いてユーザーの姿勢角を修正する。なお、これらの修正に係る演算式それ自体は公知である。例えば、Arfken, G. "Mathematical Methods for Physicists, 3rded.” Orland, FL: Academic Press, 1985, p.195 を引用し、本明細書に組み込むことができる。
【００６３】
次いで、ＫＦ処理部１４０が、記憶部６００に記憶されたＫＦ処理プログラム６１２に従ってＫＦ処理を行う。ＫＦ処理では、ＫＦ処理部１４０は、状態ベクトルＸを設定してＫＦ誤差推定演算を行う。
【００６４】
また、ＫＦ誤差推定演算では、状態ベクトルＸの各成分に含まれる誤差の共分散を成分とする誤差共分散行列Ｐを設定して計算を行う。
【００６５】
最初に、ＫＦ処理部１４０は、カルマンフィルターの理論に基づく所定の演算式に従って状態ベクトルＸ及び誤差共分散行列Ｐを予測する予測演算を行う（ステップＡ１５）。
次いで、ＫＦ処理部１４０は、現在のユーザーの移動状況を判定し（ステップＡ１７）、移動状況の判定結果が移動中である場合は（ステップＡ１７；移動中）、ＧＰＳセンサー２０からＧＰＳ演算速度Ｖを取得する（ステップＡ１９）。そして、ＫＦ処理部１４０は、ＧＰＳ演算速度Ｖを用いて、移動時観測ベクトルＺｍｏｖｅを設定する（ステップＡ２１）。
【００６６】
移動時観測ベクトルＺｍｏｖｅは、ユーザーの縦横方向（鉛直方向と左右方向）の速度成分はゼロであるとの仮定に基づき設定される観測ベクトルである。つまり、移動体座標系（Ｖフレーム）のＱ軸方向及びＰ軸方向の速度成分はゼロとなり、ユーザーの前方方向であるＲ軸方向に対しては速度Ｖに相当する速度成分が生ずると仮定する。
【００６７】
この場合、ＫＦ処理部１４０は、ステップＡ２１で設定した移動時観測ベクトルＺｍｏｖｅを用いて、カルマンフィルター処理の補正演算を行って、予測演算で予測された状態ベクトルＸ及び誤差共分散行列Ｐを補正する（ステップＡ２３）。この場合の補正演算は、移動体座標系で行う。なお、絶対座標系から移動体座標系への変換は、ユーザーに対するＩＭＵ１０の姿勢（センサー姿勢）を公知の手法を用いて演算し、ジャイロセンサー１３の検出結果から求められる絶対姿勢角を用いて定められる座標変換行列と、センサー姿勢角を用いて定められる座標変換行列とを用いた座標変換により実現できる。
【００６８】
ステップＡ１７において移動状況が停止中であると判定したならば（ステップＡ１７；停止中）、ＫＦ処理部１４０は、停止時観測ベクトルを設定する（ステップＡ２５）。具体的には、慣性航法演算部１１０が演算した絶対座標速度ベクトルｖＡ＝（ｖＡＸ，ｖＡＹ，ｖＡＺ）Ｔを用いて、停止時観測ベクトルＺｓｔｏｐを設定する。
【００６９】
停止時観測ベクトルＺｓｔｏｐは、ユーザーの速度成分がゼロであるとの仮定に基づき設定される観測ベクトルである。つまり、停止状態では、絶対座標系（Ａフレーム）における各軸方向の速度成分がゼロになると仮定する。この場合、ＫＦ処理部１４０は、ステップＡ２５で設定した停止時観測ベクトルＺｓｔｏｐを適用したカルマンフィルターの理論に基づく補正演算を行って、予測演算で予測された状態ベクトルＸ及び誤差共分散行列Ｐを補正する（ステップＡ２７）。ステップＡ２３又はＡ２７の補正演算で得られた慣性航法演算誤差は、慣性航法演算誤差データ６７０として記憶部６００に記憶させる。そして、ＫＦ処理部１４０は、ＫＦ処理を終了する。
【００７０】
ＫＦ処理の後、慣性航法演算部１１０は、ＫＦ処理で演算された慣性航法演算誤差を用いて慣性航法演算結果を補正する（ステップＡ２９）。次いで、慣性航法演算部１１０は、位置の出力タイミングであるか否かを判定し（ステップＡ３１）、出力タイミングであると判定したならば（ステップＡ３１；Ｙｅｓ）、最新の慣性航法演算結果を出力する（ステップＡ３３）。出力タイミングではないと判定したならば（ステップＡ３１；Ｎｏ）、ステップＡ３５へと移行する。
【００７１】
処理部１００は、処理を終了するか否かを判定し（ステップＡ３５）、処理を継続すると判定したならば（ステップＡ３５；Ｎｏ）、ステップＡ１５に戻る。一方、処理を終了すると判定したならば（ステップＡ３５；Ｙｅｓ）、処理部１００は、位置算出処理を終了する。
【００７２】
４．作用効果
以上述べたように、本実施形態に係る進行方向算出部１１５を含む位置算出装置１、および、それを用いた進行方向算出処理をサブルーチンとして含む位置算出処理によれば、以下の効果を得ることができる。
【００７３】
位置算出装置１において、慣性航法演算部１１０は、絶対座標系を仮に設定した絶対仮座標系上での移動位置を慣性航法演算で算出する。次いで、進行方向算出部１１５が、算出された慣性航法演算データ（絶対仮座標系上での移動位置の算出結果）の出力信号を用いて、ユーザーの進行方向をより高精度に算出することが可能な算出タイミングの設定を行う。具体的には、ユーザーの歩行または走行により発生する出力信号に基づく出力周期において、ユーザーの片方の足の動きにより発生する振幅の第１のピークと、他方の足の動きにより発生する振幅の第２のピークとの中間値を示すタイミングＴｓｙ１を算出タイミングＴｓｙ１として設定する。そして、進行方向算出部１１５は、設定した算出タイミングＴｓｙ１にて進行方向の算出を実行する。このように、第１のピークと第２のピークとの振幅の中間値を示すタイミングをユーザーの進行方向の算出タイミングＴｓｙ１として進行方向を算出することにより、歩行または走行によって発生する振動での方位変化の影響を受けにくくなり、ユーザーの進行方向算出の誤差を抑制することができる。
しかも、本実施形態では、図５に示すように、算出タイミングＴｓｙ１は、出力周期の出力（Ｙａｗ角）がゼロに近似な値を示すタイミングとなっている（図５では、算出タイミングＴｓｙ１において出力周期の出力は略ゼロ）。これにより、人間（ユーザー）の歩行または走行は、左右の足を交互に出す動作であり、ユーザーの進行方向を中心とした振幅を持つ出力周期を出力するため、その出力周期の出力がゼロに近似な値を示すタイミング（算出タイミングＴｓｙ１）でユーザーの進行方向を算出することによって、進行方向をより精度良く算出することが可能となる。
【００７４】
進行方向算出部１１５により設定された算出タイミングにて算出された進行方向（絶対仮座標系の方位）と、ＧＰＳセンサー２０から出力される絶対方位とを用いて、絶対座標系と絶対仮座標系との方位差を取得する。そして、修正部１３０は、慣性航法演算の算出開始位置と方位差とを用いて、移動位置を絶対座標系上の位置に修正する。
従って、ユーザーの進行方向をより精度よく算出し、その進行方向に基づいたユーザーの位置算出を行うことによって、精度の高い位置算出を行うことができる位置算出装置１、および、それを用いた位置算出処理方法を提供することができる。
【００７５】
また、ＫＦ処理部１４０は、修正部１３０による修正の前は慣性航法演算部１１０により算出された移動位置に、修正の後は修正後の移動位置に所定のフィルター処理を施す。より具体的には、ＫＦ処理部１４０は、修正部１３０の修正の前後で推定誤差値を継承して誤差推定演算を行うことを含むカルマンフィルター処理を実行する。これにより、誤差推定値をリセットすることなく、安定的な精度を保持してユーザーの位置を適切に算出することが可能となる。
【００７６】
５．変形例
本発明を適用可能な実施形態は、上記実施形態に限定されず、本発明の趣旨を逸脱しない範囲で適宜変更可能であることは勿論である。以下、変形例について説明する。なお、上記実施形態と同一の構成については同一の符号を付して再度の説明を省略する。
【００７７】
５−１．位置算出処理の算出タイミング
上記の実施形態では、ユーザーの歩行または走行により発生する出力信号に基づく出力周期において、ユーザーの片方の足の動きにより発生する振幅の第１のピークと、他方の足の動きにより発生する振幅の第２のピークとの中間値を示すタイミングＴｓｙ１を算出タイミングＴｓｙ１として設定し、その算出タイミングＴｓｙ１にて進行方向の算出を実行した。これに限らず、所定の範囲を有するタイミングを進行方向の算出タイミングとすることもできる。
図６は、位置算出処理のサブルーチンとしての進行方向算出処理における、進行方向の算出タイミングの変形例を示す図である。図６に示すように、本変形例では、ユーザーの歩行または走行により発生する出力信号に基づく出力周期において、ユーザーの片方の足（例えば右足）の動きにより発生する振幅の第１のピーク寄りに設定した所与の第１の閾値と、他方の足（例えば左足）の動きにより発生する振幅の第２のピーク寄りに設定した所与の第２の閾値との間の所定の範囲のタイミングＴｓｙ２を、進行方向の算出タイミングとして設定する。
このように、第１の閾値と第２の閾値との間の所定の範囲を算出タイミングＴｓｙ２とすることにより、進行方向を精度よく算出することが可能な算出タイミングをより容易に取得することができる。
【００７８】
５−２．センサーの装着部位
上記の実施形態では、センサー（位置算出装置１）のユーザーへの装着部位を腰として説明したが、腰以外の部位に装着することとしてもよい。好適な装着部位はユーザーの体幹（四肢以外の部位）である。しかし、体幹に限らず、腕以外の例えばユーザーの頭や足に装着することとしてもよい。また、位置算出装置１は、人間ではなく、例えば自転車や自動車等に装着することとしてもよい。
【００７９】
５−３．フィルター処理
上記の実施形態では、ユーザーの移動位置に施すフィルター処理をカルマンフィルター処理として説明したが、フィルター処理は何もこれに限られるわけではない。カルマンフィルターの代わりに、例えば、パーティクルフィルターやＨ∞（H Infinity）フィルター等のフィルターを適用したフィルター処理をユーザーの移動位置に施すこととしてもよい。
【００８０】
５−４．方位差の取得
上記の実施形態では、ＧＰＳセンサー２０から取得した絶対方位を用いて方位差を取得することとして説明したが、例えば、ＧＰＳセンサー２０の代わりに、方位計で計測された計測方位を用いて方位差取得部１２０が方位差を取得することとしてもよい。
【００８１】
５−５．衛星測位システム
また、上記の実施形態では、絶対方位を取得するために用いる衛星測位システムをＧＰＳとして説明した。しかし、ＷＡＡＳ（Wide Area Augmentation System）やＧＬＯＮＡＳＳ（Global Navigation Satellite System）、ＧＡＬＩＬＥＯ、Ｂｅｉｄｏｕ等の衛星測位システムとしてもよいことは勿論である。
【符号の説明】
【００８２】
１…位置算出装置、３…操作ボタン、５…液晶ディスプレイ、７…スピーカー、１０…ＩＭＵ、１１…加速度センサー、１３…ジャイロセンサー、２０…ＧＰＳセンサー、１００…処理部、１０５…受信部、１１０…慣性航法演算部、１１５…進行方向算出部、１２０…方位差取得部、１３０…修正部、１４０…ＫＦ処理部、２００…操作部、３００…表示部、４００…音出力部、５００…通信部、６００…記憶部、６１０…位置算出プログラム、６１１…進行方向算出プログラム、６１２…ＫＦ処理プログラム、６３０…ＩＭＵ検出データ、６４０…ＧＰＳ検出データ、６５０…方位差データ、６６０…慣性航法演算データ、６６５…進行方向算出データ、６７０…慣性航法演算誤差データ、Ｔｓｙ１，Ｔｓｙ２…算出タイミング。
【図１】
2016200556.tif 000003
【図２】
2016200556.tif 000004
【図３】
2016200556.tif 000005
【図４】
2016200556.tif 000006
【図５】
2016200556.tif 000007
【図６】
2016200556.tif 000008