JP6516332B2 - 角速度センサを用いてユーザの前方方向を推定する携帯端末、プログラム及び方法 - Google Patents

Description

本発明は、角速度センサを用いて、ユーザの動向を検出する技術に関する。

従来、モーションセンサ（加速度センサ、角速度センサ）を用いて、現在位置を推定する技術がある（例えば特許文献１参照）。この技術によれば、モーションセンサから生成された波形に基づく進行速度と、実際の進行速度との間の誤差を用いて、現在位置を推定する。具体的には、端末姿勢だけでなくジャイロオフセット（ドリフト）も含めて、カルマンフィルタを用いた歩行者測位を実現している。

図１は、従来技術における携帯端末の機能構成図である。

図１によれば、角速度センサによって推定された現在位置を、地図情報にマッピングすることによって補正する技術が表されている。この技術によれば、携帯端末は、進行可能な通路情報をマップデータとして記憶している。その上で、角速度センサの変化量及び積算値に基づいて算出された携帯端末の前方方向の向きから、コーナーを検出する。具体的には、一定時間以内に一定以上向きが変化した際に、コーナーとして検出する。検出されたコーナーは、地図情報に応じた進行方向をマッチングによって推定する。例えば十字通路があった場合、そのコーナーでの進行方向は、直進、９０度、−９０度の選択肢のいずれか１つに特定される。この推定結果に応じて、ユーザの前方方向が推定され、アプリケーションによってその方向に応じたサービスが提供される。

特開２０１４−１６７４６１号公報

「スマートフォン内蔵センサーを用いた複数フロアーデッドレコニング」マルチメディア、分散協調とモバイルシンポジウム2013論文集 2013, 723 - 735, 2013-07-03 「カルマンフィルタ」、[online]、［平成２８年３月１１日検索］、インターネット＜URL:https://ja.wikipedia.org/wiki/%E3%82%AB%E3%83%AB%E3%83%9E%E3%83%B3%E3%83%95%E3%82%A3%E3%83%AB%E3%82%BF%E3%83%BC＞ 「初心者の、初心者による、初心者のためのカルマンフィルタ」、[online]、［平成２８年３月１１日検索］、インターネット＜URL:http://www1.accsnet.ne.jp/~aml00731/kalman.pdf＞ 「クォータニオン便利ノート」、[online]、［平成２８年３月１１日検索］、インターネット＜URL:http://www.mss.co.jp/technology/report/pdf/18-07.pdf＞

しかしながら、前述した従来技術によれば、「人は地図上の通路に沿って移動し、コーナーでのみ方向転換をする」という前提に基づいている。そのために、携帯端末を所持するユーザが、例えば十字通路で、人を避けるなどの想定外の動きをした際に、前方方向を誤判定してしまうという問題があった。角速度センサのみの位置推定によれば、一度でもコーナーを誤判定してしまうと、その後、本来の位置と大幅にずれた位置で測位され続けることとなる。
また、十字通路で進行方向を誤判定しなかったとしても、前方方向は常に地図情報に基づく方向のみを候補として出力する。そのために、例えば直進中に人を避けるといった動作までも検出することは難しい。

これに対し、本願の発明者らは、コーナーにおけるユーザの方向転換が、地図上の通路に沿うことを前提としていることに問題があるのではないか？と考えた。即ち、移動中のユーザにおけるコーナーでの方向転換が検出された際に、時間経過の中で緩やかな方向転換として判定すべきではないか？と考えた。特に、角速度センサにおけるコーナー判定を誤った場合であっても、前方方向の推定性能における大幅な劣化を防ぐことができないか？と考えた。

そこで、本発明は、角速度センサを用いて、地図上の通路に沿った移動中のユーザの方向転換を、時間経過に応じて推定する携帯端末、プログラム及び方法を提供することを目的とする。

本発明によれば、角速度センサ及び加速度センサを有し、ユーザの前方方向を推定する携帯端末であって、
正解方向を予め記憶したマップデータ記憶手段と、
角速度センサによって計測された角速度ベクトルに基づく端末姿勢の回転を表す行列ｑ ^gyro と、時刻t-1における端末姿勢行列ｑ _t-1 とから、時刻tにおける角速度姿勢行列ｑ _t ^gyro を、カルマンフィルタを用いて算出する角速度姿勢行列算出手段と、
加速度センサによって計測された重力ベクトルｇ ^acc と、角速度姿勢行列ｑ _t ^gyro から推定された重力ベクトルｈ(ｑ _t ^gyro )との差分ｅ _t ^acc を用いて、角速度姿勢行列ｑ _t ^gyro から、加速度姿勢行列ｑ _t ^acc を、カルマンフィルタを用いて算出する加速度姿勢行列算出手段と、
角速度姿勢行列ｑ _t ^gyro 又は加速度姿勢行列ｑ _t ^acc から前方方向を推定する前方方向推定手段と、
正解方向と前方方向との差分となる方向誤差Ｅ _t を算出する方向誤差算出手段と、
正解方向の姿勢行列ｑ _t ^correct と、加速度姿勢行列ｑ _t ^acc との間の残差ｅ ^err _t を算出し、加速度姿勢行列算出手段のカルマンフィルタによって算出された共分散行列Ｐ _t ^acc を用いて、最適カルマンゲインＫ _t ^err を算出し、加速度姿勢行列ｑ _t ^acc から、残差ｅ ^err _t 及び最適カルマンゲインＫ _t ^err の積に基づく誤差姿勢行列ｑ _t ^err を算出し、共分散行列Ｐ _t ^acc から、最適カルマンゲインＫ _t ^err に基づく共分散行列Ｐ _t ^err を算出し、更に、Ｐ _t ^err を時刻tにおける最終的な端末姿勢行列の共分散行列Ｐ _t として、時刻t+1における角速度姿勢行列算出手段へフィードバックする誤差姿勢行列算出手段と
を有することを特徴とする。

本発明の携帯端末における他の実施形態によれば、
前方方向推定手段によって推定された前方方向を、アプリケーションへ出力することも好ましい。

本発明の携帯端末における他の実施形態によれば、
誤差姿勢行列算出手段から出力された誤差姿勢行列ｑ_tから、前方方向を推定する付加的前方方向推定手段を更に有し、
付加的前方方向推定手段によって推定された前方方向を、アプリケーションへ出力することも好ましい。

本発明の携帯端末における他の実施形態によれば、
端末姿勢行列であるｑ_t及びｑ_t ^gyro,ｑ_t ^acc,ｑ_t ^errは、スカラー部及びベクトル部によって表現したクォータニオン（四元数）であることも好ましい。

本発明の携帯端末における他の実施形態によれば、
マップデータ記憶手段の正解方向は、当該携帯端末の位置から通路方向に応じた１つ以上の進行可能な方向であることも好ましい。

本発明の携帯端末における他の実施形態によれば、
加速度センサによって計測された加速度ベクトルａを端末座標系における重力方向とみなすことで、初期の角速度姿勢行列ｑ₀を算出し、当該角速度姿勢行列ｑ₀を角速度姿勢行列算出手段へ出力する初期姿勢行列算出手段を
更に有することも好ましい。

角速度センサ及び加速度センサを有し、ユーザの前方方向を推定する装置に搭載されたコンピュータを機能させるプログラムであって、
正解方向を予め記憶したマップデータ記憶手段と、
角速度センサによって計測された角速度ベクトルに基づく端末姿勢の回転を表す行列ｑ ^gyro と、時刻t-1における端末姿勢行列ｑ _t-1 とから、時刻tにおける角速度姿勢行列ｑ _t ^gyro を、カルマンフィルタを用いて算出する角速度姿勢行列算出手段と、
加速度センサによって計測された重力ベクトルｇ ^acc と、角速度姿勢行列ｑ _t ^gyro から推定された重力ベクトルｈ(ｑ _t ^gyro )との差分ｅ _t ^acc を用いて、角速度姿勢行列ｑ _t ^gyro から、加速度姿勢行列ｑ _t ^acc を、カルマンフィルタを用いて算出する加速度姿勢行列算出手段と、
角速度姿勢行列ｑ _t ^gyro 又は加速度姿勢行列ｑ _t ^acc から前方方向を推定する前方方向推定手段と、
正解方向と前方方向との差分となる方向誤差Ｅ _t を算出する方向誤差算出手段と、
正解方向の姿勢行列ｑ _t ^correct と、加速度姿勢行列ｑ _t ^acc との間の残差ｅ ^err _t を算出し、加速度姿勢行列算出手段のカルマンフィルタによって算出された共分散行列Ｐ _t ^acc を用いて、最適カルマンゲインＫ _t ^err を算出し、加速度姿勢行列ｑ _t ^acc から、残差ｅ ^err _t 及び最適カルマンゲインＫ _t ^err の積に基づく誤差姿勢行列ｑ _t ^err を算出し、共分散行列Ｐ _t ^acc から、最適カルマンゲインＫ _t ^err に基づく共分散行列Ｐ _t ^err を算出し、更に、Ｐ _t ^err を時刻tにおける最終的な端末姿勢行列の共分散行列Ｐ _t として、時刻t+1における角速度姿勢行列算出手段へフィードバックする誤差姿勢行列算出手段と
してコンピュータを機能させることを特徴とする。

本発明によれば、角速度センサ及び加速度センサを有する装置の前方方向推定方法であって、
装置は、正解方向を予め記憶したマップデータ記憶部を有し、
装置は、
角速度センサによって計測された角速度ベクトルに基づく端末姿勢の回転を表す行列ｑ ^gyro と、時刻t-1における端末姿勢行列ｑ _t-1 とから、時刻tにおける角速度姿勢行列ｑ _t ^gyro を、カルマンフィルタを用いて算出すると共に、加速度センサによって計測された重力ベクトルｇ ^acc と、角速度姿勢行列ｑ _t ^gyro から推定された重力ベクトルｈ(ｑ _t ^gyro )との差分ｅ _t ^acc を用いて、角速度姿勢行列ｑ _t ^gyro から、加速度姿勢行列ｑ _t ^acc を、カルマンフィルタを用いて算出する第１のステップと、
角速度姿勢行列ｑ _t ^gyro 又は加速度姿勢行列ｑ _t ^acc から前方方向を推定する第２のステップと、
正解方向と前方方向との差分となる方向誤差Ｅ _t を算出する第３のステップと、
正解方向の姿勢行列ｑ _t ^correct と、加速度姿勢行列ｑ _t ^acc との間の残差ｅ ^err _t を算出し、前記加速度姿勢行列ｑ _t ^acc を算出する際に用いたカルマンフィルタによって算出された共分散行列Ｐ _t ^acc を用いて、最適カルマンゲインＫ _t ^err を算出し、加速度姿勢行列ｑ _t ^acc から、残差ｅ ^err _t 及び最適カルマンゲインＫ _t ^err の積に基づく誤差姿勢行列ｑ _t ^err を算出し、共分散行列Ｐ _t ^acc から、最適カルマンゲインＫ _t ^err に基づく共分散行列Ｐ _t ^err を算出し、更に、Ｐ _t ^err を時刻tにおける最終的な端末姿勢行列の共分散行列Ｐ _t として、時刻t+1における第１のステップへフィードバックする第４のステップと
を実行することを特徴とする。

本発明の携帯端末、プログラム及び方法によれば、角速度センサを用いて、地図上の通路に沿った移動中のユーザの方向転換を、時間経過に応じて推定することができる。また、角速度センサにおけるコーナー判定を徐々に決定するために、前方方向の推定性能における大幅な劣化を防ぐことができる。

従来技術における携帯端末の機能構成図である。 本発明における携帯端末の第１の機能構成図である。 図２における各機能部の算出式を表す説明図である。 携帯端末に対する端末座標系と世界座標系とを表す説明図である。 本発明における前方方向の推定を表す説明図である。 本発明における携帯端末の第２の機能構成図である。

以下では、図面を用いて、本発明を実施するための形態について詳細に説明する。

図２は、本発明における携帯端末の第１の機能構成図である。
図３は、図２における各機能部の算出式を表す説明図である。

図２によれば、ユーザに所持される携帯端末１は、スマートフォンのような端末であって、ハードウェアとして、角速度センサ１０１と、加速度センサ１０２とを有する。また、携帯端末１は、ソフトウェアとしてユーザの前方方向を推定するものであって、初期姿勢算出部１１と、角速度姿勢行列算出部１２と、加速度姿勢行列算出部１３と、前方方向推定部１４と、方向誤差算出部１５と、マップデータ記憶部１５０と、誤差姿勢行列算出部１６と、付加的前方方向推定部１７とを有する。これら機能構成部は、携帯端末に搭載されたコンピュータを機能させるプログラムを実行することによって実現される。また、これら機能構成部の処理の流れは、装置の前方方向推定方法としても理解できる。

［角速度センサ１０１］
角速度センサ１０１は、基準軸に対する回転運動を検出するセンサであって、角度又は角速度を出力する。携帯端末に搭載可能な角速度センサは、一般に、ＭＥＭＳ(micro electro mechanical systems)で構成されており、物体の運動軸と回転軸との直交方向に働く力を計測することができる。

時刻tにおける角速度センサ１０１から計測される角速度ベクトルは、以下のように表される。
ω_t＝（ωx ωy ωz）
角速度ωについて、角速度センサのサンプリングレートfに対する時刻tにおける端末回転量Δθ_tは、以下のように表される。
Δθ_t＝ω_t／ｆ
この回転は、端末座標系について、Δθ_t／|Δθ_t|を回転の軸として、|Δθ_t|の大きさだけ回転させたものである。

［加速度センサ１０２］
加速度センサ１０２は、端末姿勢を検出するべく、３次元（３軸）の加速度（単位時間当たりの速度の変化）を計測する。
時刻tにおける加速度センサ１０２から計測される加速度ベクトルは、以下のように表される。
ａ_t＝（ｘ_t，ｙ_t，ｚ_t）

［初期姿勢算出部１１］
初期姿勢算出部１１は、携帯端末の初期姿勢を決定する。初期姿勢としては、加速度センサ１０２によって計測された加速度ベクトルａを、端末座標系における重力方向ｇとみなす。端末姿勢は、重力方向ｇを鉛直方向下向きとして決定される。

具体的には、初期姿勢行列（クォータニオン）ｑ₀は、以下のように算出される。
ｇ：重力ベクトル（＝（０，０，−１））
ａ：加速度センサから得られた加速度ベクトル

ｎａ＝ａ／|a| （|x|はベクトルxの大きさを表す）
ｈ＝ｇ＋ｎａ／|ｇ＋ｎａ|
ｑ₀＝（ｎａ・ｈ，ｎａ×ｈ） （・は内積、×は外積）
端末姿勢行列であるｑ_t（以下で記述するｑ_t ^gyro,ｑ_t ^acc,ｑ_t ^errも同様）は、スカラー部及びベクトル部によって表現したクォータニオン（四元数）である（例えば非特許文献４参照）。

そして、計測された初期の角速度姿勢行列ｑ₀を、角速度姿勢行列算出部１２へ出力する。尚、初期姿勢が誤っていても、時間経過とともに正しい姿勢に修正される。そのために、初期姿勢算出部１１は、必須ではなく、固定の初期姿勢を与えてもよい。

［角速度姿勢行列算出部１２］
角速度姿勢行列算出部１２は、角速度センサ１０１によって計測された角速度ベクトルに基づく端末姿勢の回転を表す行列ｑ^gyroと、時刻t-1における端末姿勢行列ｑ_t-1とから、時刻tにおける角速度姿勢行列ｑ_t ^gyroを算出する。
ｑ_t ^gyro＝ｑ^gyro×ｑ_t-1
×：クォータニオンの積の演算子
時刻t-1における端末姿勢行列ｑ_t-1は、誤差姿勢行列算出部１６から入力される。
そして、算出された角速度姿勢行列ｑ_t ^gyroは、加速度姿勢行列算出部１３へ出力される。

［加速度姿勢行列算出部１３］
加速度姿勢行列算出部１３は、加速度センサ１０２によって計測された重力ベクトルｇ^accと、角速度姿勢行列ｑ_t ^gyroから推定された重力ベクトルｈ(ｑ_t ^gyro)との差分ｅ_t ^accを用いて、角速度姿勢行列ｑ_t ^gyroから、加速度姿勢行列ｑ_t ^accを算出する。
ｑ_t ^acc＝ｑ_t ^gyro＋Ｋ_t ^accｅ_t ^acc
算出された加速度姿勢行列ｑ_t ^accは、前方方向推定部１４及び誤差姿勢行列算出部１６へ出力される。ｑ_t ^accは、補正前の端末姿勢に基づく加速度姿勢行列である。

［前方方向推定部１４］
前方方向推定部１４は、加速度姿勢行列ｑ_t ^accから前方方向を推定する。例えば、ユーザが携帯端末を把持して、そのディスプレイを見ながら歩行している場合を前提とするならば、単純に、世界座標系における端末のｙ軸方向を水平面に投影したものを前方方向とみなす。
前方方向推定部１４によって推定された前方方向は、アプリケーションと方向誤差算出部１５へ出力される。

図４は、携帯端末に対する端末座標系と世界座標系とを表す説明図である。

図４（ａ）によれば、端末座標系が表されている。端末座標系とは、端末のセンサから得られる座標系であって、携帯端末の右方向をｘ軸方向とし、上方向をｙ軸方向とし、表面方向をｚ軸とする。
図４（ｂ）によれば、世界座標系が表されている。世界座標系とは、地表に対するユークリッド座標系で、水平面をｘｙ平面、鉛直方向をｚ軸としたものである。
尚、水平面について、どの方向がｘ軸ｙ軸になるかは不定であって、端末の初期姿勢によって決定される。特に、角速度センサと加速度センサのみを用いる場合、水平面上での絶対的な方向（方位角）を定めることはできない。但し、地磁気センサを用いた場合、絶対的な方向を決定することができる。

ここで、前方方向推定部１４が推定する前方方向は、世界座標系平面に投影した端末座標系のｙ軸方向である。

［マップデータ記憶部１５０］
マップデータ記憶部１５０は、正解方向を予め記憶したものである。「正解方向」は、当該携帯端末の位置から通路方向に応じた１つ以上の進行可能な方向である。例えば、携帯端末が通行可能な地図情報である。正解方向の候補としては、進行可能な通路に応じて、離散化した方向（０度、９０度、１８０度、２７０度）となる。

［方向誤差算出部１５］
方向誤差算出部１５は、前方方向推定部１４から前方方向を入力し、マップデータ記憶部１５０から正解方向を入力する。そして、方向誤差算出部１５は、前方方向に対して最も角度差が小さい正解方向を検索する。そして、本発明によれば、その角度差を打ち消す方向に進行方向をずらすこととなる。
方向誤差算出部１５は、正解方向と前方方向との差分となる方向誤差Ｅ_tを算出する。算出された方向誤差Ｅ_tは、誤差姿勢行列算出部１６へ出力される。

図４（ｂ）によれば、世界座標系水平面に投影した携帯端末の端末座標系のｙ軸と、世界座標系の正解方向の間の誤差が、方向誤差Ｅ_tとなる。例えば、推定された端末座標系のｙ軸（進行方向）が３０度、正解方向の候補は９０度単位であったとする。この場合、３０度に一番近い離散化した方向は０度なので、０度を正解方向とみなし、＋３０度が方向誤差Ｅ_tとなる。
尚、他の実施形態として、マップデータを用いて、推定された現在位置と最も近い通路の方向を正解方向として、方向誤差を計算することもできる。

［誤差姿勢行列算出部１６］
誤差姿勢行列算出部１６は、加速度姿勢行列算出部１３から加速度姿勢行列ｑ_t ^accを入力し、方向誤差算出部１５から方向誤差Ｅ_tを入力する。
誤差姿勢行列算出部１６は、加速度姿勢行列ｑ_t ^accに方向誤差Ｅ_tで重み付けた誤差から誤差姿勢行列ｑ_t ^errを算出する。そして、当該誤差姿勢行列ｑ_t ^errを時刻tにおける最終的な端末姿勢行列ｑ_tとして、時刻t+1における角速度姿勢行列算出部１２へフィードバックして入力する。

［付加的前方方向推定部１７］
付加的前方方向推定部１７は、誤差姿勢行列算出部１６から出力された誤差姿勢行列ｑ_tから、前方方向を推定する。付加的前方方向推定部１７によって推定された前方方向を、アプリケーションへ出力する。
前方方向推定部１４から出力される前方方向と異なって、付加的前方方向推定部１７は、姿勢更新後の姿勢（誤差姿勢行列算出部１６の誤差姿勢行列ｑ_t）に基づいて前方方向を推定する。そのために、現在のタイミングで補正された姿勢に基づく、正確な前方方向を出力することができる。

＜＜誤差姿勢行列を算出する実施形態＞＞
本発明によれば、前述した角速度姿勢行列算出部１２と、加速度姿勢行列算出部１３と、誤差姿勢行列算出部１６とは、以下のような４つの実施形態で実現することができる。
＜第１の実施形態：カルマンフィルタを用いた第１の誤差姿勢行列＞
＜第２の実施形態：カルマンフィルタを用いた第２の誤差姿勢行列＞
＜第３の実施形態：加速度姿勢行列の単位補正角度に基づく誤差姿勢行列＞
＜第４の実施形態：角速度姿勢行列の単位補正角度に基づく誤差姿勢行列＞

＜第１の実施形態：カルマンフィルタを用いた第１の誤差姿勢行列＞
「カルマンフィルタ」とは、離散的な誤差のある観測から、時々刻々と時間変化する量を推定するために用いられる連続時間線形動的システムである（例えば非特許文献２、３参照）。これは、隠れマルコフモデルに類似したものであって、ガウス分布に従う雑音によって未来の状態変数を統計的に出力することができる。

［角速度姿勢行列算出部１２］
角速度姿勢行列算出部１２は、角速度センサ１０１によって絶対座標系で表した回転をΔθ_t ^Gとすると、時刻t-1における端末姿勢ｑ_t-1を用いて、以下のように表すことができる。
Δθ_t ^G＝ｑ_t-1Δθ_t
ｑ：回転を行列で表したクォータニオン（四元数）

絶対座標系について、Δθ_t ^G／|Δθ_t ^G|を回転の軸として、|Δθ_t ^G|だけ回転させたものを、以下のように表す。
θ_t＝|Δθ_t ^G|
ｎ_t＝Δθ_t ^G／|Δθ_t ^G|

クォータニオンによれば、３次元の回転軸ベクトルｎ（大きさは１）回りにθだけ回転させる場合、４次元ベクトルを用いて、角速度センサ１０１による回転量ｑ^gyroを、以下のように表すことができる。
ｑ^gyro＝（cos(θ_t/2) ｎ_tsin(θ_t/2)）
角速度センサから計算した時刻tにおける端末姿勢である角速度姿勢行列ｑ_t ^gyroは、時刻t-1における端末姿勢ｑ_t-1を、ｑ^gyroだけ回転させたものとなる。
ｑ_t ^gyro＝ｑ^gyro×ｑ_t-1
×：クォータニオンの積の演算子
そして、角速度姿勢行列算出部１２は、カルマンフィルタの「予測ステップ」を実行する。
Ｐ_t ^gyro＝ＦＰ_t-1Ｆ^T＋Ｇ_kＱ_kＧ_k ^T
Ｆ_t＝∂/∂ｑ(ｑ^gyro・ｑ)|_ｑt-1
尚、カルマンフィルタによれば、時変な行列Ｆk、Ｇk、Ｈk、Ｑk、Ｒkによって特徴付けられる線形の状態方程式として表される。

角速度姿勢行列算出部１２は、角速度姿勢行列ｑ_t ^gyroとカルマンフィルタの共分散行列Ｐ_t ^gyroとを、加速度姿勢行列算出部１３へ出力する。

［加速度姿勢行列算出部１３］
加速度姿勢行列算出部１３は、カルマンフィルタの「更新ステップ」を実行する。
ｇ^acc：加速度ベクトル（＝重力ベクトル）
ｈ(ｑ)：端末姿勢ｑから推定される重力ベクトル
ｅ_t ^acc＝ｇ^acc−ｈ(ｑ_t ^gyro）
Ｈ_t ^acc＝∂/∂ｑ(ｈ(ｑ))|_ｑt ^gyro
Ｓ_t ^acc＝Ｈ_t ^accＰ_t ^gyroＨ_t ^accT＋Ｒ_t ^acc
Ｋ_t ^acc＝Ｐ_t ^gyroＨ_t ^accTＳ^acc-1
ｑ_t ^acc＝ｑ_t ^gyro＋Ｋ_t ^accｅ_t ^acc
Ｐ_t ^acc＝（Ｉ−Ｋ_t ^accＨ_t ^acc）Ｐ_t ^gyro
加速度姿勢行列算出部１３は、加速度姿勢行列ｑ_t ^accとカルマンフィルタの共分散行列Ｐ_t ^accとを、前方方向推定部１４及び誤差姿勢行列算出部１６へ出力する。

［誤差姿勢行列算出部１６］
誤差姿勢行列算出部１６は、以下のように端末姿勢を更新する。
方向誤差Ｅ_tが0となる端末姿勢ｑ_t ^coreectは、誤差による補正前の端末姿勢ｑ_t ^accを鉛直軸回りに−Ｅ_tだけ回転させたものである。鉛直軸回りに−Ｅ_tの回転は、以下のクォータニオンを用いて表される。
ｑ_t ^v＝（cos(−Ｅ_t/2) ０ ０ sin(−Ｅ_t/2)）
これを用いることで、方向誤差Ｅ_tが0となる端末姿勢ｑ_t ^coreectは下記のように計算できる。
ｑ_t ^coreect＝ｑ_t ^v×ｑ_t ^acc
（ｑ_t ^accを鉛直方向回りに誤差と同じ大きさで逆方向に回転させた姿勢）
ｑ_t ^coreect：観測値（方向誤差が０となる端末姿勢）
ｑ_t ^acc：内部状態の推定値

カルマンフィルタの更新ステップに従って更新することにより、次状態を計算できる。
正解方向の姿勢行列ｑ_t ^correctと、加速度姿勢行列ｑ_t ^accとの間の（観測）残差ｅ^err _tを、以下のように算出する。
ｅ_t ^err＝ｑ_t ^coreect−ｑ_t ^acc
また、観測残差の共分散は、以下のように計算できる。
Ｓ_t ^err＝Ｐ_t ^acc＋Ｒ_t ^err
Ｒ_t ^err：カルマンフィルタに外部から与えるパラメータ（固定値でもよい）
Ｒ_t ^errのパラメータの値を変えることで、１度のステップでの補正量を調整することが可能である。
加速度姿勢行列算出部１３のカルマンフィルタによって算出された共分散行列Ｐ_t ^accを用いて、最適カルマンゲインＫ_t ^errを、以下のように算出する（Ｍ^-1は、行列Ｍの逆行列を表す）。
Ｋ_t ^err＝Ｐ_t ^accＳ_t ^err-1
加速度姿勢行列ｑ_t ^accから、残差ｅ^err _t及び最適カルマンゲインＫ_t ^errの積に基づく誤差姿勢行列ｑ_t ^err（更新後の姿勢の推定値）を、以下のように算出する。
ｑ_t ^err＝ｑ_t ^acc＋Ｋ_T ^errｅ_t ^err
共分散行列Ｐ_t ^accから、最適カルマンゲインＫ_t ^errに基づく共分散行列Ｐ_tを、以下のように算出する。
Ｐ_t ^err＝（Ｉ−Ｋ_t ^err）Ｐ_t ^acc

そして、誤差姿勢行列算出部１６は、時刻tにおける誤差姿勢行列ｑ_t ^err及び共分散行列Ｐ_t ^errを、時刻tにおける最終的な姿勢行列ｑ_t及び共分散Ｐ_tとして時刻t+1における角速度姿勢行列算出部１２へフィードバックする。

＜第２の実施形態：カルマンフィルタを用いた第２の誤差姿勢行列＞
誤差姿勢行列算出部１６は、以下のように端末姿勢を更新する。
クォータニオンｑ＝（ｗ，ｘ，ｙ，ｚ）から、このクォータニオンによる回転と同等の回転を表す行列への変換を表す関数を、以下のように予め定義する。

方向誤差Ｅ_tが０となる絶対座標系における端末のＹ軸方向ｙ_t ^coreectは、誤差による補正前の端末のＹ軸方向ｙ_t ^accを鉛直軸回りに、−Ｅ_tだけ回転させたものである。
誤差による補正前の端末のＹ軸方向ｙ_t ^accは、誤差による補正前の端末姿勢ｑ_t ^accを用いて計算することができる。具体的には、端末座標系でのｙ軸方向を表すベクトル、即ち、(0,1,0)をｑ_t ^accを用いて回転させればよい。即ち、以下のように計算することができる（Ｍ^Tは、行列Ｍの転置行列を表す）。
ｙ_t ^acc＝Ｒ(ｑ_t ^acc)(0,1,0)^T
鉛直軸回りに−Ｅ_tの回転は、以下のクォータニオンを用いて表される。
ｑ_t ^v＝（cos(−Ｅ_t/2) ０ ０ sin(−Ｅ_t/2)）
これを用いて、方向誤差Ｅ_tが０となる端末のＹ軸方向ｙ_t ^coreectは、以下のように算出することができる。
ｙ_t ^coreect＝Ｒ(ｑ_t ^v)ｙ_t ^acc
（ｙ_t ^accを鉛直方向回りに誤差と同じ大きさで逆方向に回転させたベクトル）
ｙ_t ^coreect：観測値（方向誤差が０となる端末のＹ軸方向）
ｙ_t ^acc：内部状態から推定値した端末のＹ軸方向

カルマンフィルタの更新ステップに従って更新することにより、次状態を計算することができる。
正解方向の姿勢行列ｙ_t ^correctと、加速度姿勢行列ｙ_t ^accとの間の（観測）残差ｅ^err _tを、以下のように算出する。
ｅ_t ^err＝ｙ_t ^coreect−Ｒ(ｑ_t ^acc)(0,1,0)^T
尚、Ｒ(ｑ_t ^acc)(0,1,0)^T＝ｙ_t ^accである。
また、観測残差の共分散は、以下のように算出できる。
Ｓ_t ^err＝ＨＰ_t ^accＨ^T＋Ｒ_t ^err
Ｒ_t ^err：カルマンフィルタに外部から与えるパラメータ（固定値でもよい）
Ｈ：Ｒ(q)(0,1,0)^Tのヤコビ行列。
即ち、Ｈ＝∂/∂ｑ(R(ｑ)(0,1,0)^T)|_ｑt ^acc
Ｒ_t ^errのパラメータの値を変えることで、１度のステップでの補正量を調整することが可能である。
加速度姿勢行列算出部１３のカルマンフィルタによって算出された共分散行列Ｐ_t ^accを用いて、最適カルマンゲインＫ_t ^errを、以下のように算出する（Ｍ^-1は、行列Ｍの逆行列を表す）。
Ｋ_t ^err＝Ｐ_t ^accＳ_t ^err-1
加速度姿勢行列ｑ_t ^accから、残差ｅ^err _t及び最適カルマンゲインＫ_t ^errの積に基づく誤差姿勢行列ｑ_t（更新後の姿勢の推定値）を、以下のように算出する。
ｑ_t ^err＝ｑ_t ^acc＋Ｋ_T ^errｅ_t ^err
共分散行列Ｐ_t ^accから、最適カルマンゲインＫ_t ^errに基づく共分散行列Ｐ_tを、以下のように算出する。
Ｐ_t ^err＝（Ｉ−Ｋ_t ^err）Ｐ_t ^acc

そして、誤差姿勢行列算出部１６は、時刻tにおける角速度姿勢行列ｑ_t ^err及び共分散行列Ｐ_t ^errを、時刻tにおける最終的な姿勢行列ｑ_t及び共分散Ｐ_tとして時刻t+1における角速度姿勢行列算出部１２へフィードバックする。

＜第３の実施形態：加速度姿勢行列の単位補正角度に基づく誤差姿勢行列＞
誤差姿勢行列算出部１６は、所定の単位補正割合ｒに基づいて、誤差姿勢行列ｑ_t ^errを算出する。また、ｑ_t ^errを時刻tにおける最終的な姿勢行列ｑ_tとする。
（１）方向誤差Ｅ_tに対する所定の単位補正割合ｒの単位補正角度ｒＥ_tを算出する。
（２）加速度姿勢行列ｑ_t ^accに、単位補正角度ｒＥ_tだけ鉛直軸回りへの回転を表す姿勢行列を積算して、誤差姿勢行列ｑ_t ^errを算出する。

単位補正割合ｒ（０＜ｒ≦１）は、一度に補正する角度における、誤差に対する割合を表す。１回のステップで補正する角度は、ｒＥ_tとする。
補正後の端末姿勢ｑ_t ^errは、誤差による補正前の端末姿勢ｑ_t ^accを、鉛直軸回りに、−ｒＥ_tだけ回転させたものである。鉛直軸回りに−ｒＥ_tの回転は、以下のクォータニオンを用いて表される。
ｑ_t ^v＝（cos(−ｒＥ_t/2) ０ ０ sin(−ｒＥ_t/2)）
ｑ_t ^err＝ｑ_t ^v×ｑ_t ^acc

＜第４の実施形態：角速度姿勢行列の単位補正角度に基づく誤差姿勢行列＞
図５は、本発明における携帯端末の第２の機能構成図である。

図５（第３の実施形態）によれば、図２（第１の実施形態、第２の実施形態）と比較して、加速度センサ１０２を必要とせず、角速度センサ１０１のみで、前方方向を推定することができる。そのため、図５の機能構成図は、図２の機能構成図の上位概念的なものとなる。

誤差姿勢行列算出部１６は、所定の単位補正割合ｒに基づいて、誤差姿勢行列ｑ_t ^errを算出する。また、ｑ_t ^errを時刻tにおける最終的な姿勢行列ｑ_tとする。
（１）方向誤差Ｅ_tに対する所定の単位補正割合ｒの単位補正角度ｒＥ_tを算出する。
（２）角速度姿勢行列ｑ_t ^gyroに、単位補正角度ｒＥ_tだけ鉛直軸回りへの回転を表す姿勢行列を積算して、誤差姿勢行列ｑ_t ^errを算出する。

尚、図５によれば、前方方向推定部１４は、角速度姿勢行列ｑ_t ^accから前方方向を推定する。

図６は、本発明における前方方向の推定を表す説明図である。

図６（ａ）によれば、従来技術における前方方向の推定を表す（例えば非特許文献１参照）。例えばマップデータに基づく正解方向候補が右方向であった場合、携帯端末の前方方向が右向きとなった場合、コーナーで右折したと判定される。即ち、コーナーが検出されたら、正解方向候補の向きに合わせるという離散的な方向判定となる。

これに対し、図６（ｂ）によれば、本発明における前方方向の推定を表す。例えばマップデータに基づく正解方向候補が右方向であった場合、携帯端末の前方方向が右向きとなった場合であっても、正解方向候補に徐々に近づけている。そのために、直ぐに右折したとまでは判定しない。そのために、正解方向候補以外の方向への方向転換にも、ある程度追従することができる。特に、図６のように、本発明によれば、障害物を避けるなどの歩行時のノイズの影響によって、誤って判定された正解方向に測位されてしまうことを防ぐことができる。

以上、詳細に説明したように、本発明の携帯端末、プログラム及び方法によれば、角速度センサを用いて、地図上の通路に沿った移動中のユーザの方向転換を、時間経過に応じて推定することができる。また、角速度センサにおけるコーナー判定を徐々に決定するために、前方方向の推定性能における大幅な劣化を防ぐことができる。

前述した本発明における種々の実施形態によれば、当業者は、本発明の技術思想及び見地の範囲における種々の変更、修正及び省略を容易に行うことができる。前述の説明はあくまで例であって、何ら制約しようとするものではない。本発明は、特許請求の範囲及びその均等物として限定するものにのみ制約される。

１ 携帯端末
１０１ 角速度センサ
１０２ 加速度センサ
１１ 初期姿勢算出部
１２ 角速度姿勢行列算出部
１３ 加速度姿勢行列算出部
１４ 前方方向推定部
１５ 方向誤差算出部
１５０ マップデータ記憶部
１６ 誤差姿勢行列算出部
１７ 付加的前方方向推定部

Claims (8)

1. 角速度センサ及び加速度センサを有し、ユーザの前方方向を推定する携帯端末であって、
   正解方向を予め記憶したマップデータ記憶手段と、
   前記角速度センサによって計測された角速度ベクトルに基づく端末姿勢の回転を表す行列ｑ ^gyro と、時刻t-1における端末姿勢行列ｑ _t-1 とから、時刻tにおける角速度姿勢行列ｑ _t ^gyro を、カルマンフィルタを用いて算出する角速度姿勢行列算出手段と、
   前記加速度センサによって計測された重力ベクトルｇ ^acc と、角速度姿勢行列ｑ _t ^gyro から推定された重力ベクトルｈ(ｑ _t ^gyro )との差分ｅ _t ^acc を用いて、角速度姿勢行列ｑ _t ^gyro から、加速度姿勢行列ｑ _t ^acc を、カルマンフィルタを用いて算出する加速度姿勢行列算出手段と、
   前記角速度姿勢行列ｑ _t ^gyro 又は前記加速度姿勢行列ｑ _t ^acc から前方方向を推定する前方方向推定手段と、
   前記正解方向と前記前方方向との差分となる方向誤差Ｅ _t を算出する方向誤差算出手段と、
   前記正解方向の姿勢行列ｑ _t ^correct と、前記加速度姿勢行列ｑ _t ^acc との間の残差ｅ ^err _t を算出し、前記加速度姿勢行列算出手段のカルマンフィルタによって算出された共分散行列Ｐ _t ^acc を用いて、最適カルマンゲインＫ _t ^err を算出し、前記加速度姿勢行列ｑ _t ^acc から、前記残差ｅ ^err _t 及び最適カルマンゲインＫ _t ^err の積に基づく誤差姿勢行列ｑ _t ^err を算出し、前記共分散行列Ｐ _t ^acc から、最適カルマンゲインＫ _t ^err に基づく共分散行列Ｐ _t ^err を算出し、更に、Ｐ _t ^err を時刻tにおける最終的な端末姿勢行列の共分散行列Ｐ _t として、時刻t+1における前記角速度姿勢行列算出手段へフィードバックする誤差姿勢行列算出手段と
   を有することを特徴とする携帯端末。
2. 前記前方方向推定手段によって推定された前記前方方向を、アプリケーションへ出力する
   ことを特徴とする請求項１に記載の携帯端末。
3. 前記誤差姿勢行列算出手段から出力された前記誤差姿勢行列ｑ_tから、前方方向を推定する付加的前方方向推定手段を更に有し、
   前記付加的前方方向推定手段によって推定された前記前方方向を、アプリケーションへ出力する
   ことを特徴とする請求項１に記載の携帯端末。
4. 端末姿勢行列であるｑ_t及びｑ_t ^gyro,ｑ_t ^acc,ｑ_t ^errは、スカラー部及びベクトル部によって表現したクォータニオン（四元数）である
   ことを特徴とする請求項１から３のいずれか１項に記載の携帯端末。
5. 前記マップデータ記憶手段の前記正解方向は、当該携帯端末の位置から通路方向に応じた１つ以上の進行可能な方向である
   ことを特徴とする請求項１から４のいずれか１項に記載の携帯端末。
6. 加速度センサによって計測された加速度ベクトルａを端末座標系における重力方向とみなすことで、初期の角速度姿勢行列ｑ₀を算出し、当該角速度姿勢行列ｑ₀を前記角速度姿勢行列算出手段へ出力する初期姿勢行列算出手段を
   更に有することを特徴とする請求項１から５のいずれか１項に記載の携帯端末。
7. 角速度センサ及び加速度センサを有し、ユーザの前方方向を推定する装置に搭載されたコンピュータを機能させるプログラムであって、
   正解方向を予め記憶したマップデータ記憶手段と、
   前記角速度センサによって計測された角速度ベクトルに基づく端末姿勢の回転を表す行列ｑ ^gyro と、時刻t-1における端末姿勢行列ｑ _t-1 とから、時刻tにおける角速度姿勢行列ｑ _t ^gyro を、カルマンフィルタを用いて算出する角速度姿勢行列算出手段と、
   前記加速度センサによって計測された重力ベクトルｇ ^acc と、角速度姿勢行列ｑ _t ^gyro から推定された重力ベクトルｈ(ｑ _t ^gyro )との差分ｅ _t ^acc を用いて、角速度姿勢行列ｑ _t ^gyro から、加速度姿勢行列ｑ _t ^acc を、カルマンフィルタを用いて算出する加速度姿勢行列算出手段と、
   前記角速度姿勢行列ｑ _t ^gyro 又は前記加速度姿勢行列ｑ _t ^acc から前方方向を推定する前方方向推定手段と、
   前記正解方向と前記前方方向との差分となる方向誤差Ｅ _t を算出する方向誤差算出手段と、
   前記正解方向の姿勢行列ｑ _t ^correct と、前記加速度姿勢行列ｑ _t ^acc との間の残差ｅ ^err _t を算出し、前記加速度姿勢行列算出手段のカルマンフィルタによって算出された共分散行列Ｐ _t ^acc を用いて、最適カルマンゲインＫ _t ^err を算出し、前記加速度姿勢行列ｑ _t ^acc から、前記残差ｅ ^err _t 及び最適カルマンゲインＫ _t ^err の積に基づく誤差姿勢行列ｑ _t ^err を算出し、前記共分散行列Ｐ _t ^acc から、最適カルマンゲインＫ _t ^err に基づく共分散行列Ｐ _t ^err を算出し、更に、Ｐ _t ^err を時刻tにおける最終的な端末姿勢行列の共分散行列Ｐ _t として、時刻t+1における前記角速度姿勢行列算出手段へフィードバックする誤差姿勢行列算出手段と
   してコンピュータを機能させることを特徴とするプログラム。
8. 角速度センサ及び加速度センサを有する装置の前方方向推定方法であって、
   前記装置は、正解方向を予め記憶したマップデータ記憶部を有し、
   前記装置は、
   前記角速度センサによって計測された角速度ベクトルに基づく端末姿勢の回転を表す行列ｑ ^gyro と、時刻t-1における端末姿勢行列ｑ _t-1 とから、時刻tにおける角速度姿勢行列ｑ _t ^gyro を、カルマンフィルタを用いて算出すると共に、前記加速度センサによって計測された重力ベクトルｇ ^acc と、角速度姿勢行列ｑ _t ^gyro から推定された重力ベクトルｈ(ｑ _t ^gyro )との差分ｅ _t ^acc を用いて、角速度姿勢行列ｑ _t ^gyro から、加速度姿勢行列ｑ _t ^acc を、カルマンフィルタを用いて算出する第１のステップと、
   前記角速度姿勢行列ｑ _t ^gyro 又は前記加速度姿勢行列ｑ _t ^acc から前方方向を推定する第２のステップと、
   前記正解方向と前記前方方向との差分となる方向誤差Ｅ _t を算出する第３のステップと、
   前記正解方向の姿勢行列ｑ _t ^correct と、前記加速度姿勢行列ｑ _t ^acc との間の残差ｅ ^err _t を算出し、前記加速度姿勢行列ｑ _t ^acc を算出する際に用いたカルマンフィルタによって算出された共分散行列Ｐ _t ^acc を用いて、最適カルマンゲインＫ _t ^err を算出し、前記加速度姿勢行列ｑ _t ^acc から、前記残差ｅ ^err _t 及び最適カルマンゲインＫ _t ^err の積に基づく誤差姿勢行列ｑ _t ^err を算出し、前記共分散行列Ｐ _t ^acc から、最適カルマンゲインＫ _t ^err に基づく共分散行列Ｐ _t ^err を算出し、更に、Ｐ _t ^err を時刻tにおける最終的な端末姿勢行列の共分散行列Ｐ _t として、時刻t+1における第１のステップへフィードバックする第４のステップと
   を実行することを特徴とする装置の前方方向推定方法。

Images