特許 6221295 位置算出方法及び位置算出装置

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

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

(11)【特許番号】6221295

(24)【登録日】2017年10月13日

(45)【発行日】2017年11月1日

(54)【発明の名称】位置算出方法及び位置算出装置

(51)【国際特許分類】

   G01C 21/06 20060101AFI20171023BHJP

   G01S 19/19 20100101ALI20171023BHJP

【ＦＩ】

   G01C21/06

   G01S19/19

【請求項の数】12

【全頁数】16

(21)【出願番号】特願2013-67997(P2013-67997)

(22)【出願日】2013年3月28日

(65)【公開番号】特開2014-190900(P2014-190900A)

(43)【公開日】2014年10月6日

【審査請求日】2016年3月24日

(73)【特許権者】

【識別番号】000002369

【氏名又は名称】セイコーエプソン株式会社

(74)【代理人】

【識別番号】100124682

【弁理士】

【氏名又は名称】黒田 泰

(74)【代理人】

【識別番号】100104710

【弁理士】

【氏名又は名称】竹腰 昇

(74)【代理人】

【識別番号】100090479

【弁理士】

【氏名又は名称】井上 一

(72)【発明者】

【氏名】杉谷 大輔

(72)【発明者】

【氏名】内田 周志

【審査官】 岩田 玲彦

(56)【参考文献】

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

【文献】 特開平１１−０９４５７３（ＪＰ，Ａ）

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

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

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

Ｇ０１Ｓ １９／１９

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

【請求項1】

移動体の移動位置を慣性航法演算で算出するステップと、
前記移動体の移動方向を算出するステップと、
前記算出された前記移動方向を用いて、絶対座標系との方位差を取得するステップと、
前記慣性航法演算の算出開始位置及び前記方位差を用いて、前記移動位置を修正するステップと、
を含み、
前記修正するステップの前においては、前記算出された前記移動位置に、
前記修正するステップの後においては、前記修正された前記位置に、
所定のフィルター処理をする、
位置算出方法。

【請求項2】

請求項１において、
前記フィルター処理は、
前記修正するステップの前後で、推定誤差値を継承して誤差推定演算を行うカルマンフィルター処理である、
位置算出方法。

【請求項3】

請求項１又は２において、
前記移動方向を算出するステップは、測位用衛星からの衛星信号に基づいて前記絶対座標系上での移動方向を算出し、
前記方位差を取得するステップは、前記衛星信号に基づく移動方向を用いて前記方位差を取得する、
位置算出方法。

【請求項4】

請求項１又は２において、
前記方位差を取得するステップは、方位計で計測された計測方位を用いて前記方位差を
取得する、
位置算出方法。

【請求項5】

請求項１〜４の何れか一項において、
前記移動方向が安定している安定時期を判定するステップを含み、
前記方位差を取得するステップは、前記安定時期において前記方位差を取得する、
位置算出方法。

【請求項6】

請求項５において、
前記方位差を取得するステップは、
前記安定時期における前記移動方向を平均し、
当該平均した移動方向を用いて前記方位差を取得する、
位置算出方法。

【請求項7】

絶対座標系を仮に設定した絶対仮座標系上での移動体の移動位置を慣性航法演算で算出するステップと、
前記移動体の移動方向を算出するステップと、
前記算出された前記移動方向を用いて、前記絶対座標系との方位差を取得するステップと、
前記慣性航法演算の算出開始位置及び前記方位差を用いて、前記絶対仮座標系上の前記移動位置を前記絶対座標系上の位置に修正するステップと、
を含む位置算出方法。

【請求項8】

請求項１〜６の何れか一項において、
前記慣性航法演算で算出するステップは、前記絶対座標系を仮に設定した絶対仮座標系上での前記移動体の前記移動位置を前記慣性航法演算で算出し、
前記修正するステップは、前記絶対仮座標系上の前記移動位置を前記絶対座標系上の位置に修正する、
位置算出方法。

【請求項9】

移動体の移動位置を慣性航法演算で算出する慣性航法演算部と、
前記移動体の移動方向を算出する移動方向算出部と、
前記算出された前記移動方向を用いて、絶対座標系との方位差を取得する方位差取得部と、
前記慣性航法演算の算出開始位置及び前記方位差を用いて、前記移動位置を修正する修正部と、
を含み、前記修正部による修正の前は前記慣性航法演算部により算出された前記移動位置に、前記修正の後は前記修正部により修正された前記移動位置に、所定のフィルター処理をする、位置算出装置。

【請求項10】

絶対座標系を仮に設定した絶対仮座標系上での移動体の移動位置を慣性航法演算で算出する慣性航法演算部と、
前記移動体の移動方向を算出する移動方向算出部と、
前記算出された前記移動方向を用いて、前記絶対座標系との方位差を取得する方位差取得部と、
前記慣性航法演算の算出開始位置及び前記方位差を用いて、前記絶対仮座標系上の前記移動位置を前記絶対座標系上の位置に修正する修正部と、
を含む位置算出装置。

【請求項11】

移動体の移動位置を慣性航法演算で算出し、
前記移動体の移動方向を算出し、
前記算出された前記移動方向を用いて、絶対座標系との方位差を取得し、
前記慣性航法演算の算出開始位置及び前記方位差を用いて、前記移動位置を修正し、
前記修正の前は前記算出された前記移動位置に、前記修正の後は前記修正された前記移動位置に、所定のフィルター処理をする、
位置算出装置。

【請求項12】

絶対座標系を仮に設定した絶対仮座標系上での移動体の移動位置を慣性航法演算で算出し、
前記移動体の移動方向を算出し、
前記算出された前記移動方向を用いて、前記絶対座標系との方位差を取得し、
前記慣性航法演算の算出開始位置及び前記方位差を用いて、前記絶対仮座標系上の前記移動位置を前記絶対座標系上の位置に修正する、
位置算出装置。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、位置を算出する方法等に関する。

【背景技術】

【0002】

センサーの計測結果を利用して位置を算出する慣性航法演算においては、センサーの計測結果に含まれ得る種々の誤差に起因して位置算出の正確性が低下するという問題がある。この問題を解決するための技術として、例えば、特許文献１には、移動体のヨー角の初期化が異常であるか否かを判定する技術が開示されている。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開２０１２−４２２８５号公報

【発明の概要】

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

【0004】

慣性航法演算を開始するには、まず、移動方向の方位を取得する必要があるのが一般的である。その方位を基準として慣性航法をスタートするからである。そのため、基準とする方位の情報を取得するまでの時間によって、慣性航法演算の開始が遅れることとなる。
本発明は上記の課題に鑑みて考案されたものであり、その目的とするところは、基準とする方位の情報を取得する前から慣性航法演算を開始するための新たな手法を提案することにある。

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

【0005】

以上の課題を解決するための第１の発明は、絶対座標系を仮に設定した絶対仮座標系上での移動位置を慣性航法演算で算出することと、移動方向を算出することと、前記移動方向を用いて前記絶対座標系と前記絶対仮座標系との方位差を取得することと、前記慣性航法演算の算出開始位置と前記方位差とを用いて、前記移動位置を前記絶対座標系上の位置に修正することと、を含む位置算出方法である。

【0006】

また、他の発明として、絶対座標系を仮に設定した絶対仮座標系上での移動位置を慣性航法演算で算出する慣性航法演算部と、移動方向を算出する移動方向算出部と、前記移動方向を用いて前記絶対座標系と前記絶対仮座標系との方位差を取得する方位差取得部と、前記慣性航法演算の算出開始位置と前記方位差とを用いて、前記移動位置を前記絶対座標系上の位置に修正する修正部と、を備えた位置算出装置を構成することとしてもよい。

【0007】

この第１の発明等によれば、絶対座標系を仮に設定した絶対仮座標系上での移動位置を慣性航法演算で算出し始めることができる。そして、絶対座標系と絶対仮座標系との方位差（例えば、絶対座標系に定義づけていた東西南北の方角（この場合は真の方角とも言える）と、絶対仮座標系に定義づけていた東西南北の方向（この場合は仮の方角とも言える）との差）が取得できれば、その方位差を用いて、移動位置を絶対座標系上の位置に修正することができる。そのため、基準となる方位（この場合は絶対座標系の方位）が不明の状態でも慣性航法演算を開始することができる。また、絶対座標系が不明の状態で算出した絶対仮座標系上の位置は、後から、絶対座標系と絶対仮座標系との方位差を用いて、絶対座標系上の正しい位置に修正することができる。このため、絶対座標系の方位が判明するまで慣性航法演算の開始を待つ必要が無い。

【0008】

また、第２の発明として、第１の発明の位置算出方法において、前記修正の前は前記算出された移動位置に、前記修正の後は前記修正された前記位置に所定のフィルター処理を施すこと、を更に含む位置算出方法を構成することとしてもよい。

【0009】

この第２の発明によれば、位置の修正の前は慣性航法演算によって算出された移動位置に、修正の後は修正後の移動位置に所定のフィルター処理を施すことで、位置の修正の前後に関わらずに位置を適切に算出することが可能となる。

【0010】

また、第３の発明として、第２の発明の位置算出方法における前記フィルター処理は、前記修正の前後で推定誤差値を継承して誤差推定演算を行うことを含むカルマンフィルター処理である、位置算出方法を構成することとしてもよい。

【0011】

この第３の発明によれば、位置の修正の前後で推定誤差値を継承して誤差推定演算を行うことを含むカルマンフィルター処理を実行するため、位置の修正の前後で誤差推定値をリセットする必要がない。カルマンフィルター処理では、所定のフィルター処理演算を繰り返し実行するが、推定誤差値が安定するまでには一定の時間がかかる。位置の修正の前後で誤差推定値をリセットする必要がないため、位置が修正されたからと言って、カルマンフィルター処理の誤差推定値が原因で最終的な位置がばらつくことはない。

【0012】

また、第４の発明として、第１〜第３の何れかの発明の位置算出方法において、前記移動方向を算出することは、測位用衛星からの衛星信号に基づいて前記絶対座標系上での移動方向を算出することを含み、前記方位差を取得することは、前記衛星信号に基づく移動方向を用いて前記方位差を取得することを含む、位置算出方法を構成することとしてもよい。

【0013】

この第４の発明によれば、測位用衛星からの衛星信号に基づいて絶対座標系上での移動方向を算出し、衛星信号に基づく移動方向を用いることで、絶対座標系と絶対仮座標系との方位差を簡単に取得することができる。

【0014】

また、第５の発明として、第１〜第３の何れかの発明の位置算出方法において、前記方位差を取得することは、方位計で計測された計測方位を用いて前記方位差を取得することを含む、位置算出方法を構成することとしてもよい。

【0015】

この第５の発明によれば、方位計で計測された計測方位を用いることで、絶対座標系と絶対仮座標系との方位差を簡単に取得することができる。

【0016】

また、第６の発明として、第１〜第５の何れかの発明の位置算出方法において、前記移動方向が安定している安定時期を判定することを更に含み、前記方位差を取得することは、前記安定時期において前記方位差を取得することを含む、位置算出方法を構成することとしてもよい。

【0017】

この第６の発明によれば、移動方向が安定している安定時期を判定し、安定時期において方位差を取得することで、ユーザーの移動方向の変化等に起因して誤った方位差が取得される可能性を低減できる。

【0018】

また、第７の発明として、第６の発明の位置算出方法における前記方位差を取得することは、前記安定時期における前記移動方向を平均することと、当該平均した移動方向を用いて前記方位差を取得することと、を含む、位置算出方法を構成することとしてもよい。

【0019】

この第７の発明によれば、安定時期における移動方向を平均し、当該平均した移動方向を用いることで、絶対仮座標系と絶対座標系との方位差を取得することができる。

【図面の簡単な説明】

【0020】

【図1】位置算出装置の全体システムの構成例を示す図。

【図2】位置算出装置の機能構成の一例を示す図。

【図3】処理部の機能ブロックを示す図。

【図4】原理の説明図。

【図5】位置算出処理の流れを示すフローチャート。

【図6】位置算出を行った実験結果の一例を示す図。

【図7】実験結果を示す図。

【図8】変形例における処理部の機能ブロックを示す図。

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

【0021】

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

【0022】

１．システム構成
図１は、本実施形態における位置算出装置１の全体システムの構成例を示す図である。この位置算出装置１は、例えばユーザーの腰（右腰又は左腰）に装着して利用される小型電子機器であり、ユーザーの位置を算出して表示する位置算出装置の一種である。位置算出装置１は、ユーザーが位置の算出に係る各種操作を入力するための入力装置である操作ボタン３と、算出された位置等の情報が表示される液晶ディスプレイ５と、スピーカー７とを有し、装置内部にＩＭＵ(Inertial Measurement Unit)１０の他、図２の処理部１００の機能を担うＣＰＵ（Central Processing Unit）等のプロセッサー、記憶部６００の機能を担うメモリー、通信部５００の機能を担う通信回路、ＧＰＳ（Global Positioning System）センサー２０などを有する。

【0023】

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

【0024】

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

【0025】

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

【0026】

また、本実施形態では、絶対座標系（Ａフレーム）の正確な方位を認識できていない場合でも、かつ、移動体が移動を開始しても、慣性航法演算を開始できる。その絶対座標系の方位を認識できていない間は絶対座標系を仮に設定した「絶対仮座標系」で位置を算出する。移動している移動体の絶対仮座標系での位置が、絶対仮座標系での移動位置に相当する。

【0027】

ＩＭＵ１０は、位置算出装置１に搭載されるセンサーユニットであり、慣性航法ユニットとして知られるものである。ＩＭＵ１０は、加速度センサー１１と、ジャイロセンサー１３とを有する。図１に示すようにＩＭＵ１０のｚ軸が鉛直方向の軸となる。

【0028】

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

【0029】

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

【0030】

ジャイロセンサー１３は、ユーザーの角速度を検出するセンサーであり、進行方向速度算出装置１が具備する加速度センサー１１と同じローカル座標系で角速度（以下、「ローカル座標角速度」と称す。）を検出する。

【0031】

ＧＰＳセンサー２０は、測位用衛星の一種であるＧＰＳ衛星から送信されるＧＰＳ衛星信号を受信し、当該ＧＰＳ衛星信号を利用してユーザーの位置及び速度ベクトルを算出するセンサーである。なお、ＧＰＳを利用して位置や速度ベクトルを算出する方法については従来公知であるため、詳細な説明を省略する。

【0032】

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

【0033】

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

【0034】

２．機能構成
図２は、位置算出装置１の機能構成の一例を示す図である。
位置算出装置１は、ＩＭＵ１０と、ＧＰＳセンサー２０と、処理部１００と、操作部２００と、表示部３００と、音出力部４００と、通信部５００と、記憶部６００とを有して構成される。

【0035】

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

【0036】

図３は、処理部１００の機能構成の概略を表わす機能ブロック図である。
処理部１００は、慣性航法演算部１１０と、方位差取得部１２０と、修正部１３０と、カルマンフィルター処理部（以下、「ＫＦ（Kalman Filter）処理部」と称す。）１４０とを機能部として有する。

【0037】

慣性航法演算部１１０は、加速度センサー１１から出力されるローカル座標加速度ベクトルと、ジャイロセンサー１３から出力されるローカル座標角速度とを用いて、公知の慣性航法演算処理を行って、絶対座標系におけるユーザーの位置（絶対座標位置）、速度ベクトル（絶対座標速度ベクトル）及び姿勢角（絶対姿勢角）を算出する。慣性航法演算部１１０は、ＫＦ処理部１４０から出力される加速度バイアス及びジャイロバイアスを用いて、ＩＭＵ１０から出力されるローカル座標加速度ベクトル及びローカル座標角速度を補正する。そして、補正後のローカル座標加速度ベクトル及びローカル座標角速度を用いて絶対座標位置、絶対座標速度ベクトル及び絶対姿勢角を算出し、ＫＦ処理部１４０から出力される位置誤差、速度ベクトル誤差及び姿勢角誤差を用いて、絶対座標位置、絶対座標速度ベクトル及び絶対姿勢角を補正する。

【0038】

方位差取得部１２０は、慣性航法演算部１１０から出力される慣性航法演算方位と、ＧＰＳセンサー２０から出力されるＧＰＳ演算方位（絶対座標系上での移動方向：絶対方位）とを用いて、絶対座標系と絶対仮座標系との方位差を取得する。例えば、慣性航法演算方位が絶対仮座標系上で定義され、ＧＰＳ演算方位が絶対座標系上で定義される。しかし、実際には、慣性航法演算方位もＧＰＳ演算方位も同じ方角である。従って、慣性航法演算方位とＧＰＳ演算方位とを同じ方角とすることで、絶対仮座標系の軸方向と、絶対座標系の軸方向とのなす角度（姿勢の違いとも言える）を方位差として取得する。ＧＰＳ演算方位は、衛星信号に基づく移動方向に相当する。また、本実施形態の方位差取得部１２０は、絶対座標系上での移動方向を算出する移動方向算出部の機能を含む。

【0039】

修正部１３０は、慣性航法演算部１１０による慣性航法演算の算出開始位置と、方位差取得部１２０によって取得された方位差とを用いて、移動位置を絶対座標系上の位置に修正する。

【0040】

ＫＦ処理部１４０は、カルマンフィルター処理を行う機能部である。慣性航法演算部１１０から出力される慣性航法演算結果（絶対座標位置、絶対座標速度ベクトル及び絶対姿勢角）を制御入力として、誤差推定演算（以下、「ＫＦ誤差推定演算」と称す。）を行って、慣性航法演算結果に含まれる誤差（以下、「慣性航法演算誤差」と称す。）を推定する。また、推定した慣性航法演算誤差を慣性航法演算部１１０にフィードバック出力する。ＫＦ処理部１４０は、修正部１３０による座標系の修正の前は慣性航法演算部１１０によって算出された移動位置に、座標系の修正の後は修正後の移動位置に所定のフィルター処理を施すフィルター処理部に相当する。

【0041】

図４は、方位差取得部１２０による方位差の取得及び修正部１３０による座標系修正の原理の説明図である。位置算出装置１は、ユーザーの絶対方位を認識できていない状態で慣性航法演算を開始する。つまり、慣性航法演算部１１０が演算する座標系は仮の絶対座標系（絶対仮座標系）である。慣性航法演算部１１０は、この絶対仮座標系でユーザーの位置、速度ベクトル及び姿勢を演算していく。図４では、時刻ｔ１で慣性航法演算を開始し、時刻ｔ６まで慣性航法演算を行った場合を図示している。

【0042】

時刻ｔ６においてＧＰＳセンサー２０から絶対方位の情報を取得できたとする。この場合、方位差取得部１２０は、時刻ｔ１〜ｔ６までに算出された速度ベクトルから求まる相対的なユーザーの移動方位（移動方向）を平均することで平均移動方位を算出する。そして、平均移動方位とＧＰＳセンサー２０から取得したＧＰＳ演算方位との方位差θを算出する。修正部１３０は、方位差取得部１２０によって取得された方位差θを用いて、慣性航法演算部１１０が演算している絶対仮座標系を絶対座標系に修正する。つまり、絶対仮座標系を方位差θを用いて回転させる。その結果、慣性航法演算部１１０が演算する座標系は絶対座標系となる。これにより、慣性航法演算部１１０は、慣性航法演算の算出開始位置と方位差とを用いて、ユーザーの移動位置を絶対座標系上の位置に修正することができる。

【0043】

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

【0044】

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

【0045】

音出力部４００は、例えばスピーカーなどの音出力装置を有して構成され、処理部１００から入力される音声信号に基づく各種音出力を行う。音出力部４００は、図１のスピーカー７に相当する。
通信部５００は、装置内部で利用される情報や算出した結果等を、外部の情報通信装置との間で送受するための通信装置である。

【0046】

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

【0047】

記憶部６００には、位置算出処理（図５参照）として実行される位置算出プログラム６１０が記憶されている。位置算出プログラム６１０は、ＫＦ処理（図５参照）として実行されるＫＦ処理プログラム６１１をサブルーチンとして含む。また、記憶部６００には、ＩＭＵ検出データ６３０と、ＧＰＳ検出データ６４０と、方位差データ６５０と、慣性航法演算データ６６０と、慣性航法演算誤差データ６７０とが記憶される。

【0048】

ＩＭＵ検出データ６３０は、ＩＭＵ１０によって検出されたローカル座標加速度ベクトル及びローカル座標角速度を時系列に記憶する。
ＧＰＳ検出データ６４０は、ＧＰＳセンサー２０によって演算されたＧＰＳ演算結果を時系列に記憶する。
方位差データ６５０は、方位差取得部１２０によって取得された方位差を記憶する。
慣性航法演算データ６６０は、慣性航法演算部１１０が慣性航法演算を行うことで取得されるデータであり、位置や速度ベクトル、姿勢角といった諸量がこれに含まれる。
慣性航法演算誤差データ６７０は、ＫＦ処理部１４０がＫＦ処理を行うことで取得されるデータであり、位置誤差や速度ベクトル誤差、姿勢角誤差、加速度バイアス、ジャイロバイアスといった諸量がこれに含まれる。

【0049】

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

【0050】

最初に、処理部１００は、ＩＭＵ１０及びＧＰＳセンサー２０から検出データの取得を開始し、記憶部６００にＩＭＵ検出データ６３０及びＧＰＳ検出データ６４０として記憶させる（ステップＡ１）。その後、慣性航法演算部１１０が、慣性航法演算処理を開始し（ステップＡ３）、慣性航法演算の算出開始位置を含む慣性航法演算データ６６０を記憶部６００に記憶させる（ステップＡ５）。

【0051】

次いで、処理部１００は、ユーザーが直進状態にあるか否かを判定する（ステップＡ７）。直進状態にあるか否かの判定は、例えば慣性航法演算部１１０の算出結果から判定できる。例えば、ヨー角が所定の近似範囲（例えば±３度以内）の状態が所定時間（例えば３秒間）継続していることを直進状態判定条件として、この直進状態判定条件を満たせば直進状態にあると判定する。所定時間継続しているか否かの判定は、移動方向が安定しているか否か（同一或いは略同一と言える方向に移動し続けているか）の判定、さらにはその安定時期において方位差を取得することにつながる。移動方向が安定していない場合には、移動方向の変化が誤差となって、後述の方位差を適切に算出することができなくなる。そのため、所定時間継続していることを直進状態判定条件に含めることは重要な点である。

【0052】

直進状態にないと判定したならば（ステップＡ７；Ｎｏ）、処理部１００は、ステップＡ７に戻る。一方、直進状態にあると判定したならば（ステップＡ７；Ｙｅｓ）、処理部１００は、ステップＡ７の所定時間にＧＰＳセンサー２０からＧＰＳ演算結果を取得できたか否かを判定する（ステップＡ９）。

【0053】

ＧＰＳ演算結果が取得できなかったと判定したならば（ステップＡ９；Ｎｏ）、処理部１００は、ステップＡ７に戻る。一方、ＧＰＳ演算結果が取得できたと判定したならば（ステップＡ９；Ｙｅｓ）、方位差取得部１２０が、方位差取得処理を行う（ステップＡ１１）。具体的には、直進状態にあるときに慣性航法演算部１１０によって演算された移動方位を平均して平均移動方位を算出する。そして、算出した平均移動方位とＧＰＳセンサー２０から取得した絶対方位との方位差θを算出し、方位差データ６５０として記憶部６００に記憶させる。これは、安定時期における移動方向を平均し、当該平均した移動方向を用いて方位差を取得することに相当する。また、本実施形態の慣性航法演算部１１０は、安定時期における移動方向を算出する移動方向算出部の機能を含む。

【0054】

次いで、修正部１３０は、方位差取得部１２０によって取得された方位差を用いて座標系を修正する座標系修正処理を行う（ステップＡ１３）。この際、慣性航法演算の算出開始位置と現在の慣性航法演算位置とを結ぶ位置ベクトルを方位差を用いて回転させることで移動位置を修正する。また、速度ベクトルを方位差を用いて回転させることで速度ベクトルを修正し、変換後の移動位置を用いてユーザーの姿勢角を修正する。なお、これらの修正に係る演算式それ自体は公知である。ここではArfken, G. "Mathematical Methods for Physicists, 3^rded.” Orland, FL: Academic Press, 1985, p.195 を引用し、本明細書に組み込むこととする。

【0055】

次いで、ＫＦ処理部１４０が、記憶部６００に記憶されたＫＦ処理プログラム６１１に従ってＫＦ処理を行う。ＫＦ処理では、ＫＦ処理部１４０は、例えば次式（１）に示すような状態ベクトルＸを設定してＫＦ誤差推定演算を行う。

【数1】

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

【0056】

また、ＫＦ誤差推定演算では、状態ベクトルＸの各成分に含まれる誤差の共分散を成分とする誤差共分散行列Ｐ（数式は省略する。）を設定して計算を行う。

【0057】

最初に、ＫＦ処理部１４０は、カルマンフィルターの理論に基づく所定の演算式に従って状態ベクトルＸ及び誤差共分散行列Ｐを予測する予測演算を行う（ステップＡ１５）。
次いで、ＫＦ処理部１４０は、現在のユーザーの移動状況を判定し（ステップＡ１７）、移動状況の判定結果が移動中である場合は（ステップＡ１７；移動中）、ＧＰＳセンサー２０からＧＰＳ演算速度Ｖを取得する（ステップＡ１９）。そして、ＫＦ処理部１４０は、ＧＰＳ演算速度Ｖを用いて、例えば次式（２）に示すような移動時観測ベクトルＺ_ｍｏｖｅを設定する（ステップＡ２１）。

【数2】

【0058】

移動時観測ベクトルＺ_ｍｏｖｅは、ユーザーの縦横方向（鉛直方向と左右方向）の速度成分はゼロであるとの仮定に基づき設定される観測ベクトルである。つまり、移動体座標系（Ｖフレーム）のＱ軸方向及びＰ軸方向の速度成分はゼロとなり、ユーザーの前方方向であるＲ軸方向に対しては速度Ｖに相当する速度成分が生ずると仮定する。

【0059】

この場合、ＫＦ処理部１４０は、ステップＡ２１で設定した移動時観測ベクトルＺ_ｍｏｖｅを用いて、カルマンフィルター処理の補正演算を行って、予測演算で予測された状態ベクトルＸ及び誤差共分散行列Ｐを補正する（ステップＡ２３）。この場合の補正演算は、移動体座標系で行う。なお、絶対座標系から移動体座標系への変換は、ユーザーに対するＩＭＵ１０の姿勢（センサー姿勢）を公知の手法を用いて演算し、ジャイロセンサー１３の検出結果から求められる絶対姿勢角を用いて定められる座標変換行列と、センサー姿勢角を用いて定められる座標変換行列とを用いた座標変換により実現できる。

【0060】

ステップＡ１７において移動状況が停止中であると判定したならば（ステップ１７；停止中）、ＫＦ処理部１４０は、停止時観測ベクトルを設定する（ステップＡ２５）。具体的には、慣性航法演算部１１０が演算した絶対座標速度ベクトルｖ^Ａ＝（ｖ^Ａ_Ｘ，ｖ^Ａ_Ｙ，ｖ^Ａ_Ｚ）^Ｔを用いて、例えば次式（３）に示すような停止時観測ベクトルＺ_ｓｔｏｐを設定する。

【数3】

【0061】

停止時観測ベクトルＺ_ｓｔｏｐは、ユーザーの速度成分がゼロであるとの仮定に基づき設定される観測ベクトルである。つまり、停止状態では、絶対座標系（Ａフレーム）における各軸方向の速度成分がゼロになると仮定する。この場合、ＫＦ処理部１４０は、ステップＡ２５で設定した停止時観測ベクトルＺ_ｓｔｏｐを適用したカルマンフィルターの理論に基づく補正演算を行って、予測演算で予測された状態ベクトルＸ及び誤差共分散行列Ｐを補正する（ステップＡ２７）。ステップＡ２３又はＡ２７の補正演算で得られた慣性航法演算誤差は、慣性航法演算誤差データ６７０として記憶部６００に記憶させる。そして、ＫＦ処理部１４０は、ＫＦ処理を終了する。

【0062】

ＫＦ処理の後、慣性航法演算部１１０は、ＫＦ処理で演算された慣性航法演算誤差を用いて慣性航法演算結果を補正する（ステップＡ２９）。次いで、慣性航法演算部１１０は、位置の出力タイミングであるか否かを判定し（ステップＡ３１）、出力タイミングであると判定したならば（ステップＡ３１；Ｙｅｓ）、最新の慣性航法演算結果を出力する（ステップＡ３３）。出力タイミングではないと判定したならば（ステップＡ３１；Ｎｏ）、ステップＡ３５へと移行する。

【0063】

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

【0064】

４．実験結果
図６は、位置算出装置１を装着した被験者が所定の経路（図６のスタート地点ＳＰから、右上の方向）に沿って移動したときの位置を算出する実験の結果を示す図である。図６において、横軸は経度、縦軸は緯度をそれぞれ示している。スタート地点ＳＰから右上方向に移動したにも関わらず、本実施形態の位置は、スタート地点ＳＰから一時、下方向に移動した軌跡となっている。これは、ＧＰＳ演算方位を取得するまでの絶対仮座標系で算出していることによる。ＧＰＳ演算方位が取得できると、正しい位置に修正され、以降は右上方向に移動した適切な位置が算出できている。なお、ＧＰＳ演算方位を取得するまでに算出した全ての位置について、位置の修正を施せば、正しい位置に修正することができるのは勿論である。

【0065】

図７は、上記の実験においてＫＦ処理を行った場合の位置誤差及び速度誤差の誤差共分散として表される誤差推定値（以下、「推定誤差値」と称す。）の時間変化を示す図である。図７（１）は、従来の手法を用いた場合の推定誤差値の時間変化を示し、図７（２）は、本実施形態の手法を用いた場合の推定誤差値の時間変化を示している。各図において、横軸は時間であり、縦軸は推定誤差値である。時刻ｔは、ＧＰＳセンサー２０からＧＰＳ演算方位（絶対方位）を取得した時刻である。

【0066】

従来の手法では、時刻ｔで座標系の修正が施されるために推定誤差値をリセット（初期値に戻す）ことをしている。このため、折角安定してきたＫＦ処理が初期化されてしまい、精度が不安定な状態に戻り、再びＫＦ処理が安定するまでに時間を要してしまう。

【0067】

それに対し、本実施形態の手法を用いた場合は、時刻ｔにおいても推定誤差値がリセットされずに、漸次ゼロに収束していくことがわかる。座標系の修正を行う前のＫＦ処理の結果をそのまま継承してＫＦ処理を行うことができるためである。これは、本実施形態では、座標系が絶対仮座標系であっても、絶対座標系であっても、算出される位置は、その座標系の基準点からの相対位置に過ぎないため、座標系が修正されたとしてもＫＦ処理は継続できることを原理とする。本実施形態によれば、座標系の修正を行う前の精度を保ったままＫＦ処理を継続することができる。従って、本実施形態のフィルター処理によれば、座標系の修正の前後で推定誤差値を継承して誤差推定演算を行うことができる。

【0068】

５．作用効果
位置算出装置１において、慣性航法演算部１１０は、絶対座標系を仮に設定した絶対仮座標系上での移動位置を慣性航法演算で算出する。そして、方位差取得部１２０は、慣性航法演算部１１０によって算出される慣性航法演算方位とＧＰＳセンサー２０から出力される絶対方位とを用いて、絶対座標系と絶対仮座標系との方位差を取得する。そして、修正部１３０は、慣性航法演算の算出開始位置と方位差とを用いて、移動位置を絶対座標系上の位置に修正する。従って、絶対座標系の方位が不明の状態でも慣性航法演算を開始することができる。また、絶対座標系が不明の状態で算出した絶対仮座標系上の位置は、後から、絶対座標系と絶対仮座標系との方位差を用いて、絶対座標系上の正しい位置に修正される。このため、絶対座標系の方位が判明するまで慣性航法演算の開始を待つ必要が無い。

【0069】

また、ＫＦ処理部１４０は、修正部１３０による修正の前は慣性航法演算部１１０により算出された移動位置に、修正の後は修正後の移動位置に所定のフィルター処理を施す。より具体的には、ＫＦ処理部１４０は、修正部１３０の修正の前後で推定誤差値を継承して誤差推定演算を行うことを含むカルマンフィルター処理を実行する。これにより、誤差推定値をリセットすることなく、安定的な精度を保持してユーザーの位置を適切に算出することが可能となる。

【0070】

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

【0071】

６−１．センサーの装着部位
上記の実施形態では、センサー（位置算出装置１）のユーザーへの装着部位を腰として説明したが、腰以外の部位に装着することとしてもよい。好適な装着部位はユーザーの体幹（四肢以外の部位）である。しかし、体幹に限らず、腕以外の例えばユーザーの頭や足に装着することとしてもよい。また、位置算出装置１は、人間ではなく、例えば自転車や自動車等に装着することとしてもよい。

【0072】

６−２．フィルター処理
上記の実施形態では、ユーザーの移動位置に施すフィルター処理をカルマンフィルター処理として説明したが、フィルター処理は何もこれに限られるわけではない。カルマンフィルターの代わりに、例えば、パーティクルフィルターやＨ∞(H Infinity)フィルター等のフィルターを適用したフィルター処理をユーザーの移動位置に施すこととしてもよい。

【0073】

６−３．方位差の取得
上記の実施形態では、ＧＰＳセンサー２０から取得した絶対方位を用いて方位差を取得することとして説明したが、例えば図８に示すように、ＧＰＳセンサー２０の代わりに、方位計３０で計測された計測方位を用いて方位差取得部１２０が方位差を取得することとしてもよい。

【0074】

６−４．衛星測位システム
また、上記の実施形態では、絶対方位を取得するために用いる衛星測位システムをＧＰＳとして説明した。しかし、ＷＡＡＳ（Wide Area Augmentation System）やＧＬＯＮＡＳＳ（GLObal NAvigation Satellite System）、ＧＡＬＩＬＥＯ、Ｂｅｉｄｏｕ等の衛星測位システムとしてもよいことは勿論である。

【0075】

６−５．直進状態判定
上記の実施形態では、ヨー角が所定の近似範囲の状態が所定時間継続していることを直進状態判定条件として説明したが、直進状態判定条件はこれに限られるわけではない。ヨー角に代えて、ＧＰＳ演算方位の変化や、ＧＰＳセンサーが算出する位置から求められる移動方向の変化を用いてもよい。直進状態判定にＧＰＳセンサーを利用する場合には、ＧＰＳ演算結果を取得できたか否かの判定は省略してもよい。また、直進状態判定は上記以外の公知の手法を用いてもよい。

【0076】

６−６．移動方向の平均
上記の実施形態では、慣性航法演算部１１０が演算した速度ベクトルから求まる相対的なユーザーの移動方位を平均して平均移動方位を算出したが、ヨー角を平均してもよい。

【符号の説明】

【0077】

１ 位置算出装置、 ３ 操作ボタン、 ５ 液晶ディスプレイ、 ７ スピーカー、 １０ ＩＭＵ、 １１ 加速度センサー、 １３ ジャイロセンサー、 ２０ ＧＰＳセンサー、 ３０ 方位計、 １００ 処理部、 ２００ 操作部、 ３００ 表示部、 ４００ 音出力部、 ５００ 通信部、 ６００ 記憶部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】