(57)【要約】【課題】ユーザーの位置を精度よく算出すること。【解決手段】位置算出装置３は、ユーザーの胴体部に装着された当該ユーザーの運動を検出するための加速度センサー３３１及びジャイロセンサー３３３と、センサー３３１，３３３の検出結果を用いて進行方向軸を設定することと、ユーザーの進行方向の変化を用いて進行方向軸を補正することと、進行方向軸に基づく所定の制約条件を用いてセンサー３３１，３３３の検出結果に基づく位置を算出することとを実行する演算処理部３６０とを備える。【選択図】図３
2016033473.tif 000002
【特許請求の範囲】
【請求項１】
ユーザーに装着された当該ユーザーの運動を検出するセンサーの検出結果を用いて、進行方向軸を設定することと、
前記ユーザーの進行方向の変化を用いて前記進行方向軸を補正することと、
前記進行方向軸に基づく所定の制約条件を用いて、前記センサーの検出結果に基づく位置を算出することと、
を含む位置算出方法。
【請求項２】
前記補正することは、前記設定された進行方向軸からの前記ユーザーの進行方向の変化量を推定することを含む、
請求項１に記載の位置算出方法。
【請求項３】
前記センサーには、加速度センサーとジャイロセンサーとが含まれ、
前記推定することは、前記進行方向の変化量を状態ベクトルの要素とし、前記ジャイロセンサーの検出結果に基づくヨー角変化を観測ベクトルの要素としたカルマンフィルターを用いて補正することを含む、
請求項２に記載の位置算出方法。
【請求項４】
前記位置を算出することは、前記進行方向軸に沿った方向以外への移動を制限することを前記制約条件として用いて位置を補正することを含む、
請求項１〜３の何れか一項に記載の位置算出方法。
【請求項５】
前記位置を補正することは、前記位置変化の誤差を状態ベクトルの要素とし、前記進行方向軸以外の方向への位置変化成分を観測ベクトルの要素としたカルマンフィルターを用いて補正することを含む、
請求項４に記載の位置算出方法。
【請求項６】
ユーザーに装着された当該ユーザーの運動を検出するセンサーと、
前記センサーの検出結果を用いて進行方向軸を設定することと、前記ユーザーの進行方向の変化を用いて前記進行方向軸を補正することと、前記進行方向軸に基づく所定の制約条件を用いて、前記センサーの検出結果に基づく位置を算出することとを実行する演算処理部と、
を備えた位置算出装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ユーザーの位置を算出する位置算出方法等に関する。
【背景技術】
【０００２】
従来から、例えば加速度センサーやジャイロセンサーといったセンサーの計測結果を利用して慣性航法演算を行い、移動体の位置等を算出する技術が知られている。例えば、移動体として自動車を想定したものとして、特許文献１の技術が知られている。特許文献１では、センサーの計測結果から求めた速度ベクトルを用いて移動体に対するセンサーの取付姿勢を判定し、カルマンフィルターを適用して慣性航法演算結果を補正することで移動体の位置算出を行っている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１２−１９４１７５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
ところで、特許文献１における位置算出は、速度ベクトルを移動体の進行方向軸とすることを前提としている。しかし、移動体が人である場合、歩行や走行等の移動に伴い、進行方向に対して体（胴体）の向きが変化する。具体的には、人は、歩行又は走行する際、左右の順で脚を繰り出すために腰を含む胴体部を左右にねじって進行する。つまり、歩行等の間、胴体部の向きは左右に振れ、進行方向に対して周期的に変化することから、速度ベクトルの向きは、実際にユーザーが進んだ進行方向に対してばらついてしまう。そのため、移動体として人を想定し、センサーをユーザーの体（胴体部）に装着して位置算出を行う場合、特許文献１の手法では位置算出を精度よく行うことが困難であった。
【０００５】
そこで、本発明は、ユーザーの胴体部に装着されたセンサーの検出結果から、ユーザーの位置を精度よく算出することを目的とする。
【課題を解決するための手段】
【０００６】
上記の課題を解決するための第１の発明は、ユーザーに装着された当該ユーザーの運動を検出するセンサーの検出結果を用いて、進行方向軸を設定することと、前記ユーザーの進行方向の変化を用いて前記進行方向軸を補正することと、前記進行方向軸に基づく所定の制約条件を用いて、前記センサーの検出結果に基づく位置を算出することと、を含む位置算出方法である。
【０００７】
また、他の発明として、ユーザーに装着された当該ユーザーの運動を検出するセンサーと、前記センサーの検出結果を用いて進行方向軸を設定することと、前記ユーザーの進行方向の変化を用いて前記進行方向軸を補正することと、前記進行方向軸に基づく所定の制約条件を用いて、前記センサーの検出結果に基づく位置を算出することとを実行する演算処理部と、を備えた位置算出装置を構成することとしてもよい。
【０００８】
この第１の発明等によれば、ユーザーに装着されたセンサーの検出結果を用いて進行方向軸を設定し、ユーザーの進行方向の変化を用いて進行方向軸を補正しつつ進行方向軸に基づく制約条件を用いて位置算出を行うことができる。したがって、ユーザーの位置を精度よく算出することができる。
【０００９】
また、第２の発明は、前記補正することは、前記設定された進行方向軸からの前記ユーザーの進行方向の変化量を推定することを含む、第１の発明の位置算出方法である。
【００１０】
この第２の発明によれば、初期設定時の進行方向軸からのユーザーの進行方向の変化量を推定して進行方向軸を補正することができるので、歩行や走行に伴うユーザーの胴体部のヨー角変化に影響されることなく進行方向軸を適正に補正することができる。
【００１１】
また、第３の発明は、前記センサーには、加速度センサーとジャイロセンサーとが含まれ、前記推定することは、前記進行方向の変化量を状態ベクトルの要素とし、前記ジャイロセンサーの検出結果に基づくヨー角変化を観測ベクトルの要素としたカルマンフィルターを用いて補正することを含む、第２の発明の位置算出方法である。
【００１２】
この第３の発明によれば、進行方向の変化量を状態ベクトルの要素とし、ジャイロセンサーの検出結果に基づくヨー角変化を観測ベクトルの要素とするカルマンフィルターを用いて進行方向の変化量を推定することができる。
【００１３】
また、第４の発明は、前記位置を算出することは、前記進行方向軸に沿った方向以外への移動を制限することを前記制約条件として用いて位置を補正することを含む、第１〜第３の何れかの発明の位置算出方法である。
【００１４】
この第４の発明によれば、進行方向軸に沿った方向以外への移動を制限する制約条件を用いて算出位置を補正することができるので、位置算出に際し、歩行や走行に伴うユーザーの胴体部のヨー角変化に起因する誤差を低減することができる。
【００１５】
また、第５の発明は、前記位置を補正することは、前記位置変化の誤差を状態ベクトルの要素とし、前記進行方向軸以外の方向への位置変化成分を観測ベクトルの要素としたカルマンフィルターを用いて補正することを含む、第４の発明の位置算出方法である。
【００１６】
この第５の発明によれば、位置変化の誤差を状態ベクトルの要素とし、進行方向軸以外の方向への位置変化成分を観測ベクトルの要素としたカルマンフィルターを用いて位置を補正することができる。
【図面の簡単な説明】
【００１７】
【図１】位置算出装置を備えた全体システムのシステム構成例を示す模式図。
【図２】移動中のユーザーの絶対座標速度ベクトルの変化の一例を示す図。
【図３】位置算出装置の機能構成の一例を示すブロック図。
【図４】位置算出装置の処理部の機能ブロックを中心に示したブロック図。
【図５】位置算出処理の処理手順を示すフローチャート。
【図６】誤差推定処理の処理手順を示すフローチャート。
【図７】進行方向変化の推定結果を検証した結果を示す図。
【図８】進行方向変化の推定結果を検証した他の結果を示す図。
【図９】進行方向変化の推定結果を検証した他の結果を示す図。
【図１０】変形例における位置算出装置の機能構成例を示すブロック図。
【発明を実施するための形態】
【００１８】
以下、図面を参照して、本発明の位置算出方法及び位置算出装置を実施するための一形態について説明する。なお、以下説明する実施形態によって本発明が限定されるものではなく、本発明を適用可能な形態が以下の実施形態に限定されるものでもない。また、図面の記載において、同一部分には同一の符号を付す。
【００１９】
図１は、本実施形態の位置算出装置３を備えた全体システムのシステム構成例を示す模式図である。位置算出装置３は、別体の表示装置５とともに歩行又は走行するユーザー１に携帯されて使用される小型電子機器である。なお、表示装置５を使用するか否かはユーザー１の任意であり、使用せずに位置算出装置３単体でも本実施形態を実現することができる。位置算出装置３と表示装置５とは無線通信によるデータの送受が可能に構成されており、例えば、図１に示すように、位置算出装置３がユーザー１の胴体部（例えば、右腰又は左腰）に装着され、表示装置５がユーザー１の腕（右腕又は左腕）に装着される。
【００２０】
位置算出装置３は、筺体前面等に配設されたボタンスイッチ３１やディスプレイ３２を適宜備えるとともに、慣性計測ユニットとして知られるＩＭＵ３３を搭載している。ＩＭＵ３３は、加速度センサー３３１と、ジャイロセンサー３３３とを備える。
【００２１】
ここで、本実施形態では、３種類の座標系を定義する。第１の座標系は、ＩＭＵ３３の各センサー３３１，３３３に対応付けられた三次元直交座標系（センサー座標系）であるローカル座標系である。ＩＭＵ３３は、加速度センサー３３１及びジャイロセンサー３３３によって、ローカル座標系の各軸方向の加速度及び各軸回りの角速度を検出する。本明細書では、ローカル座標系の３軸をｘ軸、ｙ軸、及びｚ軸と表記する。
【００２２】
第２の座標系は、移動体であるユーザー１に対応付けられた三次元直交座標系（移動体座標系）である。本明細書では、移動体座標系において、ユーザー１の進行方向（前方）を正とする前後方向をロール軸（Ｒ軸）、右方を正とする左右方向をピッチ軸（Ｐ軸）、鉛直下方を正とする上下方向をヨー軸（Ｑ軸）とする。
【００２３】
第３の座標系は、移動体の移動空間を定める座標系である三次元直交座標系（絶対座標系）である。本明細書では、絶対座標系の３軸をＸ軸、Ｙ軸、及びＺ軸と表記する。絶対座標系は、例えば、北東下座標系として知られるＮＥＤ（North East Down）座標系や、地球中心地球固定座標系として知られるＥＣＥＦ（Earth Centered Earth Fixed）座標系として定義される。
【００２４】
ユーザー１の加速度や速度は、方向及び大きさを持っている。そのため、本明細書では、スカラーとベクトルとを適宜区別して説明する。原則として、加速度や速度と言ったときは加速度や速度の大きさ（スカラー量）を表すものとし、加速度ベクトルや速度ベクトルと言ったときは方向及び大きさを考慮した加速度及び速度を表すものとする。
【００２５】
なお、各座標系において定義される諸量を明確にするため、各諸量を表す文言の先頭に座標系の種類を付して説明する。例えば、ローカル座標系で表した加速度ベクトルのことを「ローカル座標加速度ベクトル」と称し、絶対座標系で表した加速度ベクトルのことを「絶対座標加速度ベクトル」と称する。他の諸量についても同様である。
【００２６】
また、センサー３３１，３３３の姿勢は、ロール角、ピッチ角、及びヨー角といったオイラー角で表される。以下では、絶対座標系で表したセンサー３３１，３３３の向きを「絶対姿勢」と表現し、その姿勢角のことを「絶対姿勢角」と称する。また、ユーザー１に対するセンサー３３１，３３３の相対的な姿勢（相対姿勢）のことを「センサー姿勢」と表現し、その姿勢角（相対姿勢角）のことを「センサー姿勢角」と称する。
【００２７】
ここで、ローカル座標系と移動体座標系との関係について説明する。通常の歩行等を想定して移動中のユーザー１の縦横方向（上下方向と左右方向）の速度成分をゼロと仮定すれば、ユーザー１の進行方向は速度ベクトルを用いて定義することができ、ローカル座標系から移動体座標系への座標変換行列を求めることでセンサー姿勢を把握することができる。しかし、上記したように、歩行等の間は胴体部の向きが左右に振れるため、速度ベクトルを用いて進行方向を捉えるのでは、ヨー角の周期的な変化に伴って進行方向がばらついてしまう。
【００２８】
図２は、移動中のユーザー１の絶対座標系における速度ベクトル（絶対座標速度ベクトル）の変化の一例を示す図である。図２に示すように、地点Ｐ１１から地点Ｐ１３へと移動する間のユーザー１の絶対座標速度ベクトルは、地点Ｐ１１から地点Ｐ１３への位置ベクトルが示す実際のユーザー１の進行方向Ｄ１に対して大きくばらつく。したがって、速度ベクトルを用いてユーザー１の進行方向を適正に定義するためには、ユーザー１の胴体部が正面を向いた時の速度ベクトルを抽出する必要があり、現実的には難しい。
【００２９】
そこで、本実施形態では、ユーザー１が移動を開始した移動開始初期に位置ベクトルを算出して初期進行方向とし、進行方向軸を初期設定する。そして、ユーザー１のヨー角の変化量を用いて、初期設定時の進行方向軸からのユーザー１の進行方向の変化量（初期進行方向からの進行方向の変化量）を推定し、進行方向軸を補正しながら絶対座標位置や移動速度、走行距離等を算出する。そして、位置算出装置３は、算出したユーザー１の絶対座標位置等を表示装置５に送信する。
【００３０】
表示装置５は、位置算出装置３から送信されたユーザー１の絶対座標位置等を受信して表示する腕時計型の電子機器であり、本体ケース５１の前面（ユーザー１が装着したときに外向きになる面）に配設された表示部５３を備える。その他、図示しないが、表示装置５は、本体ケース５１の適所に配設されたボタンスイッチやスピーカー、表示部５３の表示画面と一体形成されたタッチパネル等を適宜備える。
【００３１】
［機能構成］
図３は、位置算出装置３の機能構成の一例を示すブロック図である。また、図４は、位置算出装置３を構成する演算処理部３６０の機能ブロックを中心に示したブロック図である。図３に示すように、位置算出装置３は、操作部３１０と、表示部３２０と、ＩＭＵ３３と、ＧＰＳセンサー３４０と、通信部３５０と、演算処理部３６０と、記憶部３７０とを備える。
【００３２】
操作部３１０は、ボタンスイッチやレバースイッチ、ダイヤルスイッチ等の各種スイッチ、タッチパネル等の入力装置によって実現されるものであり、操作入力に応じた操作信号を演算処理部３６０に出力する。図１では、ボタンスイッチ３１がこれに該当する。
【００３３】
表示部３２０は、ＬＣＤ（Liquid Crystal Display）やＥＬディスプレイ（Electroluminescence display）等の表示装置によって実現されるものであり、演算処理部３６０から入力される表示信号に基づいて各種画面を表示する。図１では、ディスプレイ３２がこれに該当する。
【００３４】
ＩＭＵ３３は、加速度をローカル座標系で計測する加速度センサー３３１と、加速度センサー３３１と同じローカル座標系で角速度を計測するジャイロセンサー３３３とを備える。これらのセンサー３３１，３３３には、例えばＭＥＭＳ（Micro Electro Mechanical Systems）の技術を利用したＭＥＭＳセンサーが用いられる。
【００３５】
ＧＰＳセンサー３４０は、測位用衛星の一種であるＧＰＳ衛星から送信されるＧＰＳ衛星信号を受信し、受信したＧＰＳ衛星信号を利用してユーザー１の位置及び速度ベクトルを算出するセンサーである。このＧＰＳセンサー３４０は、絶対座標系におけるユーザー１の位置及び速度ベクトル（速度及び方位）を算出し、ＧＰＳ算出位置及びＧＰＳ算出速度ベクトルとして誤差推定部３６２に出力する。なお、ＧＰＳを利用して位置や速度ベクトルを算出する方法については従来公知であるため、詳細な説明を省略する。また、ＧＰＳではなく、ＧａｌｉｌｅｏやＧＬＯＮＡＳＳ、北斗などの他の衛星測位システムを用いて位置や速度ベクトルを算出することとしてもよい。
【００３６】
通信部３５０は、無線通信を利用して外部の装置と無線接続するための通信装置である。本実施形態では、この通信部３５０を介してユーザー１の絶対座標位置等が随時表示装置５に送信され、表示部５３（図１を参照）に表示される。これにより、ユーザー１は、自身の位置等を手元で確認しながら走行することができる。
【００３７】
演算処理部３６０は、ＣＰＵ（Central Processing Unit）やＤＳＰ（Digital Signal Processor）等のマイクロプロセッサー、ＡＳＩＣ（Application Specific Integrated Circuit）等の制御装置及び演算装置によって実現されるものであり、図４に示すように、慣性航法演算部３６１と、誤差推定部３６２と、ストライド算出部３６５とを備える。
【００３８】
慣性航法演算部３６１は、加速度センサー３３１から入力されるローカル座標加速度ベクトルと、ジャイロセンサー３３３から入力されるローカル座標角速度とを用いて慣性航法演算処理を行い、絶対座標系におけるユーザー１の位置（絶対座標位置）、速度ベクトル（絶対座標速度ベクトル）、及び姿勢角（絶対姿勢角）を算出する。また、その際、慣性航法演算部３６１は、誤差推定部３６２から入力される加速度バイアス及びジャイロバイアスを用いてローカル座標加速度ベクトル及びローカル座標角速度を補正し、その上で絶対座標位置、絶対座標速度ベクトル、及び絶対姿勢角を算出する。そして、慣性航法演算部３６１は、誤差推定部３６２から入力される位置誤差、速度ベクトル誤差、及び姿勢角誤差を用い、絶対座標位置、絶対座標速度ベクトル、及び絶対姿勢角を補正する。得られた絶対座標位置、絶対座標速度ベクトル、及び絶対姿勢角は、誤差推定部３６２に出力される。
【００３９】
誤差推定部３６２は、初期進行方向誤差及び初期進行方向からの進行方向の変化量（以下、単に「進行方向変化量」とも称す。）と、慣性航法演算部３６１から入力される慣性航法演算結果（絶対座標位置、絶対座標速度ベクトル、及び絶対姿勢角）に含まれる誤差（以下、「慣性航法演算誤差」と称す。）とを所定の誤差推定演算によって推定する処理（誤差推定処理）を行う。誤差推定部３６２は、初期進行方向算出部３６３と、移動体座標系設定部３６４とを備える。
【００４０】
この誤差推定部３６２は、推定した慣性航法演算誤差を慣性航法演算部３６１にフィードバック出力する。本実施形態において、慣性航法演算誤差には、絶対座標位置の誤差（位置誤差）、絶対座標速度ベクトルの誤差（速度ベクトル誤差）、絶対姿勢角の誤差（姿勢角誤差）、加速度バイアス、及びジャイロバイアスが含まれる。
【００４１】
慣性航法演算誤差の推定方法としては種々の手法を適用可能であるが、典型的な手法としては、カルマンフィルター（以下、「ＫＦ（Kalman Filter）」と表記する。）を適用した誤差推定演算（以下、 「ＫＦ誤差推定演算」と称す。）によって実現できる。なお、その他の手法としては、拡張カルマンフィルターやアンセンテッドカルマンフィルター、パーティクルフィルター、Ｈ∞フィルターといったフィルターを適用できる。
【００４２】
ストライド算出部３６５は、誤差推定部３６２が誤差推定処理で用いるストライド、すなわちユーザー１の歩幅を算出する。具体的には、例えば、ローカル座標加速度ベクトルの所定方向（例えば鉛直方向）の振幅値から推定的にストライドを算出する方法を適用できる。
【００４３】
記憶部３７０は、ＲＯＭ（Read Only Memory）やフラッシュＲＯＭ、ＲＡＭ（Random Access Memory）等の各種ＩＣ（Integrated Circuit）メモリーやハードディスク等の記憶媒体により実現されるものである。記憶部３７０には、位置算出装置３を動作させ、この位置算出装置３が備える種々の機能を実現するためのプログラムや、このプログラムの実行中に使用されるデータ等が事前に記憶され、或いは処理の都度一時的に記憶される。
【００４４】
この記憶部３７０には、演算処理部３６０を慣性航法演算部３６１、誤差推定部３６２、及びストライド算出部３６５として機能させ、位置算出処理（図５を参照）を行うための位置算出プログラム３７１が記憶される。
【００４５】
また、記憶部３７０には、ＩＭＵ計測データ３７２と、慣性航法演算データ３７３と、誤差推定データ３７４とが記憶される。その他、座標変換行列のデータ等、位置算出処理の過程で算出され、あるいは用いられるデータが適宜記憶される。
【００４６】
ＩＭＵ計測データ３７２は、ＩＭＵ３３の計測結果のデータであり、加速度センサー３３１により計測されたローカル座標加速度ベクトルと、ジャイロセンサー３３３により計測されたローカル座標角速度とが時系列に記憶される。慣性航法演算データ３７３は、慣性航法演算処理結果のデータであり、絶対座標位置、絶対座標速度ベクトル、及び絶対姿勢角の各値が時系列に記憶される。誤差推定データ３７４は、誤差推定処理結果のデータであり、初期進行方向誤差と、進行方向変化量と、慣性航法演算誤差の各値とが時系列に記憶される。
【００４７】
［処理の流れ］
図４は、位置算出処理の処理手順を示すフローチャートである。この位置算出処理は、演算処理部３６０が記憶部３７０から位置算出プログラム３７１を読み出して実行することで実現できる。
【００４８】
図４に示すように、位置算出処理では、先ず、慣性航法演算部３６１が、慣性航法演算処理を開始する（ステップＳ１）。ここで開始する慣性航法演算処理は従来公知であるため詳細な説明は省略するが、簡単に手順を説明すると、先ず、ローカル座標角速度を積分及び継ぎ足し処理することで、絶対姿勢角を算出する。具体的には、絶対座標系で表した所定の基準姿勢からの姿勢角の変化分を、ローカル座標角速度を積分することで算出する。ここで、「積分」とは、所定の単位時間分の値を累積的に加算することを意味する。また、「継ぎ足す」とは、積分により得られた結果を最後に算出した結果に足し合わせることを意味する。
【００４９】
また、絶対姿勢角を用いてローカル座標系から絶対座標系への座標変換行列（ＬＡ座標変換行列）を算出し、ローカル座標加速度ベクトルを絶対座標加速度ベクトルに座標変換する。そして、絶対座標加速度ベクトルを積分及び継ぎ足し処理し、絶対座標速度ベクトルを算出する。つまり、単位時間分の絶対座標加速度ベクトルを積分することで単位時間の間のユーザー１の速度ベクトルの変化分を算出し、この変化分を最後に更新した絶対座標速度ベクトルに継ぎ足すことで、絶対座標速度ベクトルを新たに算出する。
【００５０】
また、絶対座標速度ベクトルを積分及び継ぎ足し処理し、絶対座標位置を算出する。つまり、単位時間分の絶対座標速度ベクトルを積分することで単位時間の間における位置変化分を算出し、この変化分を最後に更新した絶対座標位置に継ぎ足すことで、絶対座標位置を新たに算出する。
【００５１】
続いて、ストライド算出部３６５が、ストライド算出処理を開始する（ステップＳ３）。ここで開始するストライド算出処理では、ストライド算出部３６５は、例えば、ローカル座標加速度ベクトルをもとに、加速度の振幅と歩幅との関係を定めたモデルを用いて１歩分の移動距離（ストライド；歩幅）を算出する。
【００５２】
そして、誤差推定部３６２が、誤差推定処理を開始する（ステップＳ５）。図６は、誤差推定処理の詳細な処理手順を示すフローチャートである。
【００５３】
図６に示すように、誤差推定処理では、先ず、初期進行方向算出部３６３が、初期進行方向を算出する（ステップＳ５１）。具体的には、初期進行方向算出部３６３は、移動開始時の絶対座標位置から、例えば数歩程度進む時間等として設定される所定時間後の絶対座標位置への位置ベクトルを初期進行方向として算出する。続いて、移動体座標系設定部３６４が、初期進行方向を進行方向軸とし、絶対座標系のＺ軸方向（あるいは、ユーザー１が停止している状態のときに加速度センサー３３１によって検出されたけ重力加速度の方向）を上下方向として、移動体座標系を定義する（ステップＳ５２）。
【００５４】
そして、以上のように進行方向軸を初期設定し、移動体座標系を定義した上で、誤差推定部３６２は、次式（１）に示す状態ベクトル“Ｘ”を設定する（ステップＳ５３）。次式（１）において、ΔＶは速度ベクトル誤差、ΔＡは姿勢角誤差、ｂａは加速度バイアス、ｂｇはジャイロバイアス、ΔΨは初期進行方向誤差、ΔΨＭＶは初期進行方向からの進行方向変化量、ΔＲは位置誤差である。初期進行方向誤差は、移動開始初期におけるＧＰＳセンサー３４０の計測結果を用いて設定することができる。
【数１】
2016033473.tif 000003
【００５５】
また、誤差推定部３６２は、進行方向軸に基づく所定の制約条件を用いて、例えば次式（２）に示す観測ベクトル“Ｚ１”と、次式（３）に示す観測ベクトル“Ｚ２”とを設定する（ステップＳ５４）。
【数２】
2016033473.tif 000004
【００５６】
上記式（２）に示す観測ベクトル“Ｚ１”であるヨー角変化量は、ジャイロセンサー３３３から入力されるローカル座標角速度をもとにセンサー姿勢角の変化分として求める。その際、ローカル座標系から、ステップＳ５２で定義され、ステップＳ５６で再定義される移動体座標系への座標変換行列（ＬＶ座標変換行列）を算出して移動体座標系への座標変換を行う。なお、ＧＰＳセンサー３４０による方位の計測結果をヨー角変化量として用いてもよい。あるいは、方位センサーや地磁気センサーを備えて位置算出装置３を構成し、これら方位センサーや地磁気センサーによる方位の計測結果をヨー角変化量として用いてもよい。
【００５７】
また、上記式（３）に示す観測ベクトル“Ｚ２”において、小文字“ｒ”は位置ベクトルの成分を表し、下付きの添え字によって対応する移動体座標系の軸を示している。このｒＲ，ｒＰ，ｒＱを成分とする位置ベクトルは、１歩前の移動体座標位置から現在の移動体座標位置への位置ベクトルであり、慣性航法演算部３６１から入力される絶対座標位置を移動体座標位置に座標変換した上で求める。ここでの座標変換は、前述のＬＶ座標変換行列と、慣性航法演算部３６１が慣性航法演算処理の過程で求めたＬＡ座標変換行列とを用いて行う。
【００５８】
また、“Stride”は、１歩分の移動距離（ストライド）を表す。このストライドは、図５のステップＳ３で開始されたストライド算出処理においてストライド算出部３６５によって算出される。
【００５９】
この観測ベクトル“Ｚ２”を設定することにより、誤差推定部３６２は、進行方向軸に基づく制約条件として、進行方向軸に沿った方向以外の方向への移動を制限する制約条件を適用する。この制約条件は、歩行等の移動においては、ユーザー１は縦横方向には移動せず、短い時間で求めた位置ベクトルは進行方向と一致するとの仮定に基づく。
【００６０】
以上のように状態ベクトル“Ｘ”及び観測ベクトル“Ｚ１”，“Ｚ２”を設定したならば、誤差推定部３６２は、カルマンフィルターの理論に基づく予測演算（時刻更新）及び補正演算（観測更新）を行い、状態ベクトル“Ｘ”の推定値（状態推定値）を算出する（ステップＳ５５）。なお、観測ベクトル“Ｚ１”，“Ｚ２”それぞれを要素とする１つの観測ベクトル（観測行列）を用いることとしてもよい。
【００６１】
その後は、移動体座標系設定部３６４が、ステップＳ５５で得た進行方向変化量の推定値を用いて進行方向軸を補正して移動体座標系を再定義し（ステップＳ５６）、その上でステップＳ５３に戻って上記した処理を繰り返す。
【００６２】
図５の説明に戻り、続いて、演算処理部３６０が、位置の出力タイミングであるか否かを判定し、出力タイミングでなければ（ステップＳ７：ＮＯ）、ステップＳ１１に移行する。そして、演算処理部３６０は、出力タイミングと判定した場合は（ステップＳ７：ＹＥＳ）、最新の絶対座標位置を表示装置５の表示部５３に表示する制御を行う（ステップＳ９）。ここでの処理は、通信部３５０を介して最新の絶対座標位置を表示装置５に送信し、これを受信した表示装置５がマップマッチング処理等を行って表示部５３に表示させることによって行う。
【００６３】
そして、ステップＳ１１では、演算処理部３６０は、処理を終了するか否かを判定し、処理を継続すると判定した場合は（ステップＳ１１：Ｎｏ）、ステップＳ７に戻る。処理を終了する場合には（ステップＳ１１：ＹＥＳ）、位置算出処理を終える。
【００６４】
ここで、本実施形態を適用して進行方向変化量を推定し、進行方向軸を補正した場合（推定あり）と、進行方向変化量を推定せずに、ローカル座標角速度からセンサー姿勢角の変化分として求めたヨー各変化量を用いて進行方向軸を補正した場合（推定なし）とで移動体座標速度ベクトルを求め、進行方向変化の推定結果を検証した。図７，図８，図９は、検証の結果を示す図である。図７では、推定ありの場合の移動体座標速度ベクトルの進行方向成分（Ｒ軸成分）の時間変化Ｌ２１と、推定なしの場合の移動体座標速度ベクトルの進行方向成分の時間変化Ｌ２３とを、その真値の時間変化Ｌ２５と併せて示している。また、図８は、左右方向成分（Ｐ軸成分）について推定ありの場合Ｌ３１、推定なしの場合Ｌ３３、及び真値Ｌ３５を示し、図９は、上下方向成分（Ｑ軸成分）について推定ありの場合Ｌ４１、推定なしの場合Ｌ４３、及び真値Ｌ４５を示している。
【００６５】
図７に示すように、進行方向変化量を推定した場合の移動体座標速度ベクトルの進行方向成分は真値とよく一致しているのに対し、進行方向変化量を推定せずにヨー角変化量を用いて進行方向軸を補正した場合の移動体座標速度ベクトルの進行方向成分は真値から大きく外れている。図８に示す左右方向成分の場合や、図９に示す上下方向成分の場合も同様である。
【００６６】
以上説明したように、本実施形態によれば、ユーザー１の移動開始初期の位置ベクトルを初期進行方向として進行方向軸を初期設定し、初期進行方向からの進行方向変化量を推定することができる。また、進行方向軸に沿った方向以外の方向への移動を制限する制約条件を適用して慣性航法演算誤差を推定し、推定した慣性航法演算誤差を用いて絶対座標位置、絶対座標速度ベクトル、及び絶対姿勢角を補正することができる。したがって、歩行や走行等の移動に伴うユーザー１の胴体部のヨー角変化に影響されることなく進行方向軸を適正に補正することができる。そして、この胴体部のヨー角変化に起因する誤差を低減して精度よく位置算出を行える。
【００６７】
また、上記した実施形態では、位置算出装置３と表示装置５とを別体としたシステム構成を例示した。これに対し、図１０に示すように、位置算出装置７１ａと計測装置７３ａとで全体システムを構成してもよい。本変形例では、位置算出装置７１ａがユーザー１の腕（右腕又は左腕）に装着され、別体の計測装置７３ａがユーザー１の胴体部（例えば、右腰又は左腰）に装着されて使用される。なお、図１０中、第１実施形態と同様の部分については同一の符号を付している。
【００６８】
計測装置７３ａは、加速度センサー３３１と、ジャイロセンサー３３３とを備えたＩＭＵ３３と、ＧＰＳセンサー３４０とを備えている。この計測装置７３ａは、位置算出装置７１ａとの間で無線通信によるデータの送受が可能に構成されており、ＩＭＵ３３による計測結果やＧＰＳセンサー３４０による計測結果を随時位置算出装置７１ａに送信する。
【００６９】
一方、位置算出装置７１ａは、操作部３１０と、表示部３２０と、通信部３５０と、記憶部３７０と、演算処理部３６０とを備える。記憶部３７０には、上記実施形態の位置算出装置３と同様に、位置算出プログラム３７１と、ＩＭＵ計測データ３７２と、慣性航法演算データ３７３と、誤差推定データ３７４とが記憶される。また、演算処理部３６０は、図４に示した構成と同様に、慣性航法演算部３６１と、誤差推定部３６２と、ストライド算出部３６５とを備える。
【符号の説明】
【００７０】
３，７１ａ  位置算出装置、３１０  操作部、３２０  表示部、３３  ＩＭＵ、３３１  加速度センサー、３３３  ジャイロセンサー、３４０  ＧＰＳセンサー、３５０  通信部、３６０  演算処理部、３６１  慣性航法演算部、３６２  誤差推定部、３６３  初期進行方向算出部、３６４  移動体座標系設定部、３６５  ストライド算出部、３７０  記憶部、３７１  位置算出プログラム、３７２  ＩＭＵ計測データ、３７３  慣性航法演算データ、３７４  誤差推定データ、５  表示装置、５３  表示部、７３ａ  計測装置
【図１】
2016033473.tif 000005
【図２】
2016033473.tif 000006
【図３】
2016033473.tif 000007
【図４】
2016033473.tif 000008
【図５】
2016033473.tif 000009
【図６】
2016033473.tif 000010
【図７】
2016033473.tif 000011
【図８】
2016033473.tif 000012
【図９】
2016033473.tif 000013
【図１０】
2016033473.tif 000014