(57)【特許請求の範囲】
【請求項１】
角速度センサ及び加速度センサを有し、ユーザの前方方向を推定する携帯端末であって、
正解方向を予め記憶したマップデータ記憶手段と、
前記角速度センサによって計測された角速度ベクトルに基づく端末姿勢の回転を表す行列ｑgyroと、時刻t-1における端末姿勢行列ｑt-1とから、時刻tにおける角速度姿勢行列ｑtgyroを、カルマンフィルタを用いて算出する角速度姿勢行列算出手段と、
前記加速度センサによって計測された重力ベクトルｇaccと、角速度姿勢行列ｑtgyroから推定された重力ベクトルｈ(ｑtgyro)との差分ｅtaccを用いて、角速度姿勢行列ｑtgyroから、加速度姿勢行列ｑtaccを、カルマンフィルタを用いて算出する加速度姿勢行列算出手段と、
前記角速度姿勢行列ｑtgyro又は前記加速度姿勢行列ｑtaccから前方方向を推定する前方方向推定手段と、
前記正解方向と前記前方方向との差分となる方向誤差Ｅtを算出する方向誤差算出手段と、
前記正解方向の姿勢行列ｑtcorrectと、前記加速度姿勢行列ｑtaccとの間の残差ｅerrtを算出し、前記加速度姿勢行列算出手段のカルマンフィルタによって算出された共分散行列Ｐtaccを用いて、最適カルマンゲインＫterrを算出し、前記加速度姿勢行列ｑtaccから、前記残差ｅerrt及び最適カルマンゲインＫterrの積に基づく誤差姿勢行列ｑterrを算出し、前記共分散行列Ｐtaccから、最適カルマンゲインＫterrに基づく共分散行列Ｐterrを算出し、更に、Ｐterrを時刻tにおける最終的な端末姿勢行列の共分散行列Ｐtとして、時刻t+1における前記角速度姿勢行列算出手段へフィードバックする誤差姿勢行列算出手段と
を有することを特徴とする携帯端末。
【請求項２】
前記前方方向推定手段によって推定された前記前方方向を、アプリケーションへ出力する
ことを特徴とする請求項１に記載の携帯端末。
【請求項３】
前記誤差姿勢行列算出手段から出力された前記誤差姿勢行列ｑtから、前方方向を推定する付加的前方方向推定手段を更に有し、
前記付加的前方方向推定手段によって推定された前記前方方向を、アプリケーションへ出力する
ことを特徴とする請求項１に記載の携帯端末。
【請求項４】
端末姿勢行列であるｑt及びｑtgyro,ｑtacc,ｑterrは、スカラー部及びベクトル部によって表現したクォータニオン（四元数）である
ことを特徴とする請求項１から３のいずれか１項に記載の携帯端末。
【請求項５】
前記マップデータ記憶手段の前記正解方向は、当該携帯端末の位置から通路方向に応じた１つ以上の進行可能な方向である
ことを特徴とする請求項１から４のいずれか１項に記載の携帯端末。
【請求項６】
加速度センサによって計測された加速度ベクトルａを端末座標系における重力方向とみなすことで、初期の角速度姿勢行列ｑ0を算出し、当該角速度姿勢行列ｑ0を前記角速度姿勢行列算出手段へ出力する初期姿勢行列算出手段を
更に有することを特徴とする請求項１から５のいずれか１項に記載の携帯端末。
【請求項７】
角速度センサ及び加速度センサを有し、ユーザの前方方向を推定する装置に搭載されたコンピュータを機能させるプログラムであって、
正解方向を予め記憶したマップデータ記憶手段と、
前記角速度センサによって計測された角速度ベクトルに基づく端末姿勢の回転を表す行列ｑgyroと、時刻t-1における端末姿勢行列ｑt-1とから、時刻tにおける角速度姿勢行列ｑtgyroを、カルマンフィルタを用いて算出する角速度姿勢行列算出手段と、
前記加速度センサによって計測された重力ベクトルｇaccと、角速度姿勢行列ｑtgyroから推定された重力ベクトルｈ(ｑtgyro)との差分ｅtaccを用いて、角速度姿勢行列ｑtgyroから、加速度姿勢行列ｑtaccを、カルマンフィルタを用いて算出する加速度姿勢行列算出手段と、
前記角速度姿勢行列ｑtgyro又は前記加速度姿勢行列ｑtaccから前方方向を推定する前方方向推定手段と、
前記正解方向と前記前方方向との差分となる方向誤差Ｅtを算出する方向誤差算出手段と、
前記正解方向の姿勢行列ｑtcorrectと、前記加速度姿勢行列ｑtaccとの間の残差ｅerrtを算出し、前記加速度姿勢行列算出手段のカルマンフィルタによって算出された共分散行列Ｐtaccを用いて、最適カルマンゲインＫterrを算出し、前記加速度姿勢行列ｑtaccから、前記残差ｅerrt及び最適カルマンゲインＫterrの積に基づく誤差姿勢行列ｑterrを算出し、前記共分散行列Ｐtaccから、最適カルマンゲインＫterrに基づく共分散行列Ｐterrを算出し、更に、Ｐterrを時刻tにおける最終的な端末姿勢行列の共分散行列Ｐtとして、時刻t+1における前記角速度姿勢行列算出手段へフィードバックする誤差姿勢行列算出手段と
してコンピュータを機能させることを特徴とするプログラム。
【請求項８】
角速度センサ及び加速度センサを有する装置の前方方向推定方法であって、
前記装置は、正解方向を予め記憶したマップデータ記憶部を有し、
前記装置は、
前記角速度センサによって計測された角速度ベクトルに基づく端末姿勢の回転を表す行列ｑgyroと、時刻t-1における端末姿勢行列ｑt-1とから、時刻tにおける角速度姿勢行列ｑtgyroを、カルマンフィルタを用いて算出すると共に、前記加速度センサによって計測された重力ベクトルｇaccと、角速度姿勢行列ｑtgyroから推定された重力ベクトルｈ(ｑtgyro)との差分ｅtaccを用いて、角速度姿勢行列ｑtgyroから、加速度姿勢行列ｑtaccを、カルマンフィルタを用いて算出する第１のステップと、
前記角速度姿勢行列ｑtgyro又は前記加速度姿勢行列ｑtaccから前方方向を推定する第２のステップと、
前記正解方向と前記前方方向との差分となる方向誤差Ｅtを算出する第３のステップと、
前記正解方向の姿勢行列ｑtcorrectと、前記加速度姿勢行列ｑtaccとの間の残差ｅerrtを算出し、前記加速度姿勢行列ｑtaccを算出する際に用いたカルマンフィルタによって算出された共分散行列Ｐtaccを用いて、最適カルマンゲインＫterrを算出し、前記加速度姿勢行列ｑtaccから、前記残差ｅerrt及び最適カルマンゲインＫterrの積に基づく誤差姿勢行列ｑterrを算出し、前記共分散行列Ｐtaccから、最適カルマンゲインＫterrに基づく共分散行列Ｐterrを算出し、更に、Ｐterrを時刻tにおける最終的な端末姿勢行列の共分散行列Ｐtとして、時刻t+1における第１のステップへフィードバックする第４のステップと
を実行することを特徴とする装置の前方方向推定方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、角速度センサを用いて、ユーザの動向を検出する技術に関する。
【背景技術】
【０００２】
従来、モーションセンサ（加速度センサ、角速度センサ）を用いて、現在位置を推定する技術がある（例えば特許文献１参照）。この技術によれば、モーションセンサから生成された波形に基づく進行速度と、実際の進行速度との間の誤差を用いて、現在位置を推定する。具体的には、端末姿勢だけでなくジャイロオフセット（ドリフト）も含めて、カルマンフィルタを用いた歩行者測位を実現している。
【０００３】
図１は、従来技術における携帯端末の機能構成図である。
【０００４】
図１によれば、角速度センサによって推定された現在位置を、地図情報にマッピングすることによって補正する技術が表されている。この技術によれば、携帯端末は、進行可能な通路情報をマップデータとして記憶している。その上で、角速度センサの変化量及び積算値に基づいて算出された携帯端末の前方方向の向きから、コーナーを検出する。具体的には、一定時間以内に一定以上向きが変化した際に、コーナーとして検出する。検出されたコーナーは、地図情報に応じた進行方向をマッチングによって推定する。例えば十字通路があった場合、そのコーナーでの進行方向は、直進、９０度、−９０度の選択肢のいずれか１つに特定される。この推定結果に応じて、ユーザの前方方向が推定され、アプリケーションによってその方向に応じたサービスが提供される。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２０１４−１６７４６１号公報
【非特許文献】
【０００６】
【非特許文献１】「スマートフォン内蔵センサーを用いた複数フロアーデッドレコニング」マルチメディア、分散協調とモバイルシンポジウム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における角速度姿勢行列ｑtgyroを、カルマンフィルタを用いて算出する角速度姿勢行列算出手段と、
加速度センサによって計測された重力ベクトルｇaccと、角速度姿勢行列ｑtgyroから推定された重力ベクトルｈ(ｑtgyro)との差分ｅtaccを用いて、角速度姿勢行列ｑtgyroから、加速度姿勢行列ｑtaccを、カルマンフィルタを用いて算出する加速度姿勢行列算出手段と、
角速度姿勢行列ｑtgyro又は加速度姿勢行列ｑtaccから前方方向を推定する前方方向推定手段と、
正解方向と前方方向との差分となる方向誤差Ｅtを算出する方向誤差算出手段と、
正解方向の姿勢行列ｑtcorrectと、加速度姿勢行列ｑtaccとの間の残差ｅerrtを算出し、加速度姿勢行列算出手段のカルマンフィルタによって算出された共分散行列Ｐtaccを用いて、最適カルマンゲインＫterrを算出し、加速度姿勢行列ｑtaccから、残差ｅerrt及び最適カルマンゲインＫterrの積に基づく誤差姿勢行列ｑterrを算出し、共分散行列Ｐtaccから、最適カルマンゲインＫterrに基づく共分散行列Ｐterrを算出し、更に、Ｐterrを時刻tにおける最終的な端末姿勢行列の共分散行列Ｐtとして、時刻t+1における角速度姿勢行列算出手段へフィードバックする誤差姿勢行列算出手段と
を有することを特徴とする。
【００１１】
本発明の携帯端末における他の実施形態によれば、
前方方向推定手段によって推定された前方方向を、アプリケーションへ出力することも好ましい。
【００１２】
本発明の携帯端末における他の実施形態によれば、
誤差姿勢行列算出手段から出力された誤差姿勢行列ｑtから、前方方向を推定する付加的前方方向推定手段を更に有し、
付加的前方方向推定手段によって推定された前方方向を、アプリケーションへ出力することも好ましい。
【００１９】
本発明の携帯端末における他の実施形態によれば、
端末姿勢行列であるｑt及びｑtgyro,ｑtacc,ｑterrは、スカラー部及びベクトル部によって表現したクォータニオン（四元数）であることも好ましい。
【００２０】
本発明の携帯端末における他の実施形態によれば、
マップデータ記憶手段の正解方向は、当該携帯端末の位置から通路方向に応じた１つ以上の進行可能な方向であることも好ましい。
【００２１】
本発明の携帯端末における他の実施形態によれば、
加速度センサによって計測された加速度ベクトルａを端末座標系における重力方向とみなすことで、初期の角速度姿勢行列ｑ0を算出し、当該角速度姿勢行列ｑ0を角速度姿勢行列算出手段へ出力する初期姿勢行列算出手段を
更に有することも好ましい。
【００２２】
角速度センサ及び加速度センサを有し、ユーザの前方方向を推定する装置に搭載されたコンピュータを機能させるプログラムであって、
正解方向を予め記憶したマップデータ記憶手段と、
角速度センサによって計測された角速度ベクトルに基づく端末姿勢の回転を表す行列ｑgyroと、時刻t-1における端末姿勢行列ｑt-1とから、時刻tにおける角速度姿勢行列ｑtgyroを、カルマンフィルタを用いて算出する角速度姿勢行列算出手段と、
加速度センサによって計測された重力ベクトルｇaccと、角速度姿勢行列ｑtgyroから推定された重力ベクトルｈ(ｑtgyro)との差分ｅtaccを用いて、角速度姿勢行列ｑtgyroから、加速度姿勢行列ｑtaccを、カルマンフィルタを用いて算出する加速度姿勢行列算出手段と、
角速度姿勢行列ｑtgyro又は加速度姿勢行列ｑtaccから前方方向を推定する前方方向推定手段と、
正解方向と前方方向との差分となる方向誤差Ｅtを算出する方向誤差算出手段と、
正解方向の姿勢行列ｑtcorrectと、加速度姿勢行列ｑtaccとの間の残差ｅerrtを算出し、加速度姿勢行列算出手段のカルマンフィルタによって算出された共分散行列Ｐtaccを用いて、最適カルマンゲインＫterrを算出し、加速度姿勢行列ｑtaccから、残差ｅerrt及び最適カルマンゲインＫterrの積に基づく誤差姿勢行列ｑterrを算出し、共分散行列Ｐtaccから、最適カルマンゲインＫterrに基づく共分散行列Ｐterrを算出し、更に、Ｐterrを時刻tにおける最終的な端末姿勢行列の共分散行列Ｐtとして、時刻t+1における角速度姿勢行列算出手段へフィードバックする誤差姿勢行列算出手段と
してコンピュータを機能させることを特徴とする。
【００２４】
本発明によれば、角速度センサ及び加速度センサを有する装置の前方方向推定方法であって、
装置は、正解方向を予め記憶したマップデータ記憶部を有し、
装置は、
角速度センサによって計測された角速度ベクトルに基づく端末姿勢の回転を表す行列ｑgyroと、時刻t-1における端末姿勢行列ｑt-1とから、時刻tにおける角速度姿勢行列ｑtgyroを、カルマンフィルタを用いて算出すると共に、加速度センサによって計測された重力ベクトルｇaccと、角速度姿勢行列ｑtgyroから推定された重力ベクトルｈ(ｑtgyro)との差分ｅtaccを用いて、角速度姿勢行列ｑtgyroから、加速度姿勢行列ｑtaccを、カルマンフィルタを用いて算出する第１のステップと、
角速度姿勢行列ｑtgyro又は加速度姿勢行列ｑtaccから前方方向を推定する第２のステップと、
正解方向と前方方向との差分となる方向誤差Ｅtを算出する第３のステップと、
正解方向の姿勢行列ｑtcorrectと、加速度姿勢行列ｑtaccとの間の残差ｅerrtを算出し、前記加速度姿勢行列ｑtaccを算出する際に用いたカルマンフィルタによって算出された共分散行列Ｐtaccを用いて、最適カルマンゲインＫterrを算出し、加速度姿勢行列ｑtaccから、残差ｅerrt及び最適カルマンゲインＫterrの積に基づく誤差姿勢行列ｑterrを算出し、共分散行列Ｐtaccから、最適カルマンゲインＫterrに基づく共分散行列Ｐterrを算出し、更に、Ｐterrを時刻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）
【００３４】
［初期姿勢算出部１１］
初期姿勢算出部１１は、携帯端末の初期姿勢を決定する。初期姿勢としては、加速度センサ１０２によって計測された加速度ベクトルａを、端末座標系における重力方向ｇとみなす。端末姿勢は、重力方向ｇを鉛直方向下向きとして決定される。
【００３５】
具体的には、初期姿勢行列（クォータニオン）ｑ0は、以下のように算出される。
ｇ：重力ベクトル（＝（０，０，−１））
ａ：加速度センサから得られた加速度ベクトル
ｎａ＝ａ／|a|  （|x|はベクトルxの大きさを表す）
ｈ＝ｇ＋ｎａ／|ｇ＋ｎａ|
ｑ0＝（ｎａ・ｈ，ｎａ×ｈ）  （・は内積、×は外積）
端末姿勢行列であるｑt（以下で記述するｑtgyro,ｑtacc,ｑterrも同様）は、スカラー部及びベクトル部によって表現したクォータニオン（四元数）である（例えば非特許文献４参照）。
【００３６】
そして、計測された初期の角速度姿勢行列ｑ0を、角速度姿勢行列算出部１２へ出力する。尚、初期姿勢が誤っていても、時間経過とともに正しい姿勢に修正される。そのために、初期姿勢算出部１１は、必須ではなく、固定の初期姿勢を与えてもよい。
【００３７】
［角速度姿勢行列算出部１２］
角速度姿勢行列算出部１２は、角速度センサ１０１によって計測された角速度ベクトルに基づく端末姿勢の回転を表す行列ｑgyroと、時刻t-1における端末姿勢行列ｑt-1とから、時刻tにおける角速度姿勢行列ｑtgyroを算出する。
ｑtgyro＝ｑgyro×ｑt-1
×：クォータニオンの積の演算子
時刻t-1における端末姿勢行列ｑt-1は、誤差姿勢行列算出部１６から入力される。
そして、算出された角速度姿勢行列ｑtgyroは、加速度姿勢行列算出部１３へ出力される。
【００３８】
［加速度姿勢行列算出部１３］
加速度姿勢行列算出部１３は、加速度センサ１０２によって計測された重力ベクトルｇaccと、角速度姿勢行列ｑtgyroから推定された重力ベクトルｈ(ｑtgyro)との差分ｅtaccを用いて、角速度姿勢行列ｑtgyroから、加速度姿勢行列ｑtaccを算出する。
ｑtacc＝ｑtgyro＋Ｋtaccｅtacc
算出された加速度姿勢行列ｑtaccは、前方方向推定部１４及び誤差姿勢行列算出部１６へ出力される。ｑtaccは、補正前の端末姿勢に基づく加速度姿勢行列である。
【００３９】
［前方方向推定部１４］
前方方向推定部１４は、加速度姿勢行列ｑtaccから前方方向を推定する。例えば、ユーザが携帯端末を把持して、そのディスプレイを見ながら歩行している場合を前提とするならば、単純に、世界座標系における端末のｙ軸方向を水平面に投影したものを前方方向とみなす。
前方方向推定部１４によって推定された前方方向は、アプリケーションと方向誤差算出部１５へ出力される。
【００４０】
図４は、携帯端末に対する端末座標系と世界座標系とを表す説明図である。
【００４１】
図４（ａ）によれば、端末座標系が表されている。端末座標系とは、端末のセンサから得られる座標系であって、携帯端末の右方向をｘ軸方向とし、上方向をｙ軸方向とし、表面方向をｚ軸とする。
図４（ｂ）によれば、世界座標系が表されている。世界座標系とは、地表に対するユークリッド座標系で、水平面をｘｙ平面、鉛直方向をｚ軸としたものである。
尚、水平面について、どの方向がｘ軸ｙ軸になるかは不定であって、端末の初期姿勢によって決定される。特に、角速度センサと加速度センサのみを用いる場合、水平面上での絶対的な方向（方位角）を定めることはできない。但し、地磁気センサを用いた場合、絶対的な方向を決定することができる。
【００４２】
ここで、前方方向推定部１４が推定する前方方向は、世界座標系平面に投影した端末座標系のｙ軸方向である。
【００４３】
［マップデータ記憶部１５０］
マップデータ記憶部１５０は、正解方向を予め記憶したものである。「正解方向」は、当該携帯端末の位置から通路方向に応じた１つ以上の進行可能な方向である。例えば、携帯端末が通行可能な地図情報である。正解方向の候補としては、進行可能な通路に応じて、離散化した方向（０度、９０度、１８０度、２７０度）となる。
【００４４】
［方向誤差算出部１５］
方向誤差算出部１５は、前方方向推定部１４から前方方向を入力し、マップデータ記憶部１５０から正解方向を入力する。そして、方向誤差算出部１５は、前方方向に対して最も角度差が小さい正解方向を検索する。そして、本発明によれば、その角度差を打ち消す方向に進行方向をずらすこととなる。
方向誤差算出部１５は、正解方向と前方方向との差分となる方向誤差Ｅtを算出する。算出された方向誤差Ｅtは、誤差姿勢行列算出部１６へ出力される。
【００４５】
図４（ｂ）によれば、世界座標系水平面に投影した携帯端末の端末座標系のｙ軸と、世界座標系の正解方向の間の誤差が、方向誤差Ｅtとなる。例えば、推定された端末座標系のｙ軸（進行方向）が３０度、正解方向の候補は９０度単位であったとする。この場合、３０度に一番近い離散化した方向は０度なので、０度を正解方向とみなし、＋３０度が方向誤差Ｅtとなる。
尚、他の実施形態として、マップデータを用いて、推定された現在位置と最も近い通路の方向を正解方向として、方向誤差を計算することもできる。
【００４６】
［誤差姿勢行列算出部１６］
誤差姿勢行列算出部１６は、加速度姿勢行列算出部１３から加速度姿勢行列ｑtaccを入力し、方向誤差算出部１５から方向誤差Ｅtを入力する。
誤差姿勢行列算出部１６は、加速度姿勢行列ｑtaccに方向誤差Ｅtで重み付けた誤差から誤差姿勢行列ｑterrを算出する。そして、当該誤差姿勢行列ｑterrを時刻tにおける最終的な端末姿勢行列ｑtとして、時刻t+1における角速度姿勢行列算出部１２へフィードバックして入力する。
【００４７】
［付加的前方方向推定部１７］
付加的前方方向推定部１７は、誤差姿勢行列算出部１６から出力された誤差姿勢行列ｑtから、前方方向を推定する。付加的前方方向推定部１７によって推定された前方方向を、アプリケーションへ出力する。
前方方向推定部１４から出力される前方方向と異なって、付加的前方方向推定部１７は、姿勢更新後の姿勢（誤差姿勢行列算出部１６の誤差姿勢行列ｑt）に基づいて前方方向を推定する。そのために、現在のタイミングで補正された姿勢に基づく、正確な前方方向を出力することができる。
【００４８】
＜＜誤差姿勢行列を算出する実施形態＞＞
本発明によれば、前述した角速度姿勢行列算出部１２と、加速度姿勢行列算出部１３と、誤差姿勢行列算出部１６とは、以下のような４つの実施形態で実現することができる。
＜第１の実施形態：カルマンフィルタを用いた第１の誤差姿勢行列＞
＜第２の実施形態：カルマンフィルタを用いた第２の誤差姿勢行列＞
＜第３の実施形態：加速度姿勢行列の単位補正角度に基づく誤差姿勢行列＞
＜第４の実施形態：角速度姿勢行列の単位補正角度に基づく誤差姿勢行列＞
【００４９】
＜第１の実施形態：カルマンフィルタを用いた第１の誤差姿勢行列＞
「カルマンフィルタ」とは、離散的な誤差のある観測から、時々刻々と時間変化する量を推定するために用いられる連続時間線形動的システムである（例えば非特許文献２、３参照）。これは、隠れマルコフモデルに類似したものであって、ガウス分布に従う雑音によって未来の状態変数を統計的に出力することができる。
【００５０】
［角速度姿勢行列算出部１２］
角速度姿勢行列算出部１２は、角速度センサ１０１によって絶対座標系で表した回転をΔθtGとすると、時刻t-1における端末姿勢ｑt-1を用いて、以下のように表すことができる。
ΔθtG＝ｑt-1Δθt
ｑ：回転を行列で表したクォータニオン（四元数）
【００５１】
絶対座標系について、ΔθtG／|ΔθtG|を回転の軸として、|ΔθtG|だけ回転させたものを、以下のように表す。
θt＝|ΔθtG|
ｎt＝ΔθtG／|ΔθtG|
【００５２】
クォータニオンによれば、３次元の回転軸ベクトルｎ（大きさは１）回りにθだけ回転させる場合、４次元ベクトルを用いて、角速度センサ１０１による回転量ｑgyroを、以下のように表すことができる。
ｑgyro＝（cos(θt/2)  ｎtsin(θt/2)）
角速度センサから計算した時刻tにおける端末姿勢である角速度姿勢行列ｑtgyroは、時刻t-1における端末姿勢ｑt-1を、ｑgyroだけ回転させたものとなる。
ｑtgyro＝ｑgyro×ｑt-1
×：クォータニオンの積の演算子
そして、角速度姿勢行列算出部１２は、カルマンフィルタの「予測ステップ」を実行する。
Ｐtgyro＝ＦＰt-1ＦT＋ＧkＱkＧkT
Ｆt＝∂/∂ｑ(ｑgyro・ｑ)|ｑt-1
尚、カルマンフィルタによれば、時変な行列Ｆk、Ｇk、Ｈk、Ｑk、Ｒkによって特徴付けられる線形の状態方程式として表される。
【００５３】
角速度姿勢行列算出部１２は、角速度姿勢行列ｑtgyroとカルマンフィルタの共分散行列Ｐtgyroとを、加速度姿勢行列算出部１３へ出力する。
【００５４】
［加速度姿勢行列算出部１３］
加速度姿勢行列算出部１３は、カルマンフィルタの「更新ステップ」を実行する。
ｇacc：加速度ベクトル（＝重力ベクトル）
ｈ(ｑ)：端末姿勢ｑから推定される重力ベクトル
ｅtacc＝ｇacc−ｈ(ｑtgyro）
Ｈtacc＝∂/∂ｑ(ｈ(ｑ))|ｑtgyro
Ｓtacc＝ＨtaccＰtgyroＨtaccT＋Ｒtacc
Ｋtacc＝ＰtgyroＨtaccTＳacc-1
ｑtacc＝ｑtgyro＋Ｋtaccｅtacc
Ｐtacc＝（Ｉ−ＫtaccＨtacc）Ｐtgyro
加速度姿勢行列算出部１３は、加速度姿勢行列ｑtaccとカルマンフィルタの共分散行列Ｐtaccとを、前方方向推定部１４及び誤差姿勢行列算出部１６へ出力する。
【００５５】
［誤差姿勢行列算出部１６］
誤差姿勢行列算出部１６は、以下のように端末姿勢を更新する。
方向誤差Ｅtが0となる端末姿勢ｑtcoreectは、誤差による補正前の端末姿勢ｑtaccを鉛直軸回りに−Ｅtだけ回転させたものである。鉛直軸回りに−Ｅtの回転は、以下のクォータニオンを用いて表される。
ｑtv＝（cos(−Ｅt/2)  ０  ０  sin(−Ｅt/2)）
これを用いることで、方向誤差Ｅtが0となる端末姿勢ｑtcoreectは下記のように計算できる。
ｑtcoreect＝ｑtv×ｑtacc
（ｑtaccを鉛直方向回りに誤差と同じ大きさで逆方向に回転させた姿勢）
ｑtcoreect：観測値（方向誤差が０となる端末姿勢）
ｑtacc：内部状態の推定値
【００５６】
カルマンフィルタの更新ステップに従って更新することにより、次状態を計算できる。
正解方向の姿勢行列ｑtcorrectと、加速度姿勢行列ｑtaccとの間の（観測）残差ｅerrtを、以下のように算出する。
ｅterr＝ｑtcoreect−ｑtacc
また、観測残差の共分散は、以下のように計算できる。
Ｓterr＝Ｐtacc＋Ｒterr
Ｒterr：カルマンフィルタに外部から与えるパラメータ（固定値でもよい）
Ｒterrのパラメータの値を変えることで、１度のステップでの補正量を調整することが可能である。
加速度姿勢行列算出部１３のカルマンフィルタによって算出された共分散行列Ｐtaccを用いて、最適カルマンゲインＫterrを、以下のように算出する（Ｍ-1は、行列Ｍの逆行列を表す）。
Ｋterr＝ＰtaccＳterr-1
加速度姿勢行列ｑtaccから、残差ｅerrt及び最適カルマンゲインＫterrの積に基づく誤差姿勢行列ｑterr（更新後の姿勢の推定値）を、以下のように算出する。
ｑterr＝ｑtacc＋ＫTerrｅterr
共分散行列Ｐtaccから、最適カルマンゲインＫterrに基づく共分散行列Ｐtを、以下のように算出する。
Ｐterr＝（Ｉ−Ｋterr）Ｐtacc
【００５７】
そして、誤差姿勢行列算出部１６は、時刻tにおける誤差姿勢行列ｑterr及び共分散行列Ｐterrを、時刻tにおける最終的な姿勢行列ｑt及び共分散Ｐtとして時刻t+1における角速度姿勢行列算出部１２へフィードバックする。
【００５８】
＜第２の実施形態：カルマンフィルタを用いた第２の誤差姿勢行列＞
誤差姿勢行列算出部１６は、以下のように端末姿勢を更新する。
クォータニオンｑ＝（ｗ，ｘ，ｙ，ｚ）から、このクォータニオンによる回転と同等の回転を表す行列への変換を表す関数を、以下のように予め定義する。
【数１】
0006516332.tif 000002
方向誤差Ｅtが０となる絶対座標系における端末のＹ軸方向ｙtcoreectは、誤差による補正前の端末のＹ軸方向ｙtaccを鉛直軸回りに、−Ｅtだけ回転させたものである。
誤差による補正前の端末のＹ軸方向ｙtaccは、誤差による補正前の端末姿勢ｑtaccを用いて計算することができる。具体的には、端末座標系でのｙ軸方向を表すベクトル、即ち、(0,1,0)をｑtaccを用いて回転させればよい。即ち、以下のように計算することができる（ＭTは、行列Ｍの転置行列を表す）。
ｙtacc＝Ｒ(ｑtacc)(0,1,0)T
鉛直軸回りに−Ｅtの回転は、以下のクォータニオンを用いて表される。
ｑtv＝（cos(−Ｅt/2)  ０  ０  sin(−Ｅt/2)）
これを用いて、方向誤差Ｅtが０となる端末のＹ軸方向ｙtcoreectは、以下のように算出することができる。
ｙtcoreect＝Ｒ(ｑtv)ｙtacc
（ｙtaccを鉛直方向回りに誤差と同じ大きさで逆方向に回転させたベクトル）
ｙtcoreect：観測値（方向誤差が０となる端末のＹ軸方向）
ｙtacc：内部状態から推定値した端末のＹ軸方向
【００５９】
カルマンフィルタの更新ステップに従って更新することにより、次状態を計算することができる。
正解方向の姿勢行列ｙtcorrectと、加速度姿勢行列ｙtaccとの間の（観測）残差ｅerrtを、以下のように算出する。
ｅterr＝ｙtcoreect−Ｒ(ｑtacc)(0,1,0)T
尚、Ｒ(ｑtacc)(0,1,0)T＝ｙtaccである。
また、観測残差の共分散は、以下のように算出できる。
Ｓterr＝ＨＰtaccＨT＋Ｒterr
Ｒterr：カルマンフィルタに外部から与えるパラメータ（固定値でもよい）
Ｈ：Ｒ(q)(0,1,0)Tのヤコビ行列。
即ち、Ｈ＝∂/∂ｑ(R(ｑ)(0,1,0)T)|ｑtacc
Ｒterrのパラメータの値を変えることで、１度のステップでの補正量を調整することが可能である。
加速度姿勢行列算出部１３のカルマンフィルタによって算出された共分散行列Ｐtaccを用いて、最適カルマンゲインＫterrを、以下のように算出する（Ｍ-1は、行列Ｍの逆行列を表す）。
Ｋterr＝ＰtaccＳterr-1
加速度姿勢行列ｑtaccから、残差ｅerrt及び最適カルマンゲインＫterrの積に基づく誤差姿勢行列ｑt（更新後の姿勢の推定値）を、以下のように算出する。
ｑterr＝ｑtacc＋ＫTerrｅterr
共分散行列Ｐtaccから、最適カルマンゲインＫterrに基づく共分散行列Ｐtを、以下のように算出する。
Ｐterr＝（Ｉ−Ｋterr）Ｐtacc
【００６０】
そして、誤差姿勢行列算出部１６は、時刻tにおける角速度姿勢行列ｑterr及び共分散行列Ｐterrを、時刻tにおける最終的な姿勢行列ｑt及び共分散Ｐtとして時刻t+1における角速度姿勢行列算出部１２へフィードバックする。
【００６１】
＜第３の実施形態：加速度姿勢行列の単位補正角度に基づく誤差姿勢行列＞
誤差姿勢行列算出部１６は、所定の単位補正割合ｒに基づいて、誤差姿勢行列ｑterrを算出する。また、ｑterrを時刻tにおける最終的な姿勢行列ｑtとする。
（１）方向誤差Ｅtに対する所定の単位補正割合ｒの単位補正角度ｒＥtを算出する。
（２）加速度姿勢行列ｑtaccに、単位補正角度ｒＥtだけ鉛直軸回りへの回転を表す姿勢行列を積算して、誤差姿勢行列ｑterrを算出する。
【００６２】
単位補正割合ｒ（０＜ｒ≦１）は、一度に補正する角度における、誤差に対する割合を表す。１回のステップで補正する角度は、ｒＥtとする。
補正後の端末姿勢ｑterrは、誤差による補正前の端末姿勢ｑtaccを、鉛直軸回りに、−ｒＥtだけ回転させたものである。鉛直軸回りに−ｒＥtの回転は、以下のクォータニオンを用いて表される。
ｑtv＝（cos(−ｒＥt/2)  ０  ０  sin(−ｒＥt/2)）
ｑterr＝ｑtv×ｑtacc
【００６３】
＜第４の実施形態：角速度姿勢行列の単位補正角度に基づく誤差姿勢行列＞
図５は、本発明における携帯端末の第２の機能構成図である。
【００６４】
図５（第３の実施形態）によれば、図２（第１の実施形態、第２の実施形態）と比較して、加速度センサ１０２を必要とせず、角速度センサ１０１のみで、前方方向を推定することができる。そのため、図５の機能構成図は、図２の機能構成図の上位概念的なものとなる。
【００６５】
誤差姿勢行列算出部１６は、所定の単位補正割合ｒに基づいて、誤差姿勢行列ｑterrを算出する。また、ｑterrを時刻tにおける最終的な姿勢行列ｑtとする。
（１）方向誤差Ｅtに対する所定の単位補正割合ｒの単位補正角度ｒＥtを算出する。
（２）角速度姿勢行列ｑtgyroに、単位補正角度ｒＥtだけ鉛直軸回りへの回転を表す姿勢行列を積算して、誤差姿勢行列ｑterrを算出する。
【００６６】
尚、図５によれば、前方方向推定部１４は、角速度姿勢行列ｑtaccから前方方向を推定する。
【００６７】
図６は、本発明における前方方向の推定を表す説明図である。
【００６８】
図６（ａ）によれば、従来技術における前方方向の推定を表す（例えば非特許文献１参照）。例えばマップデータに基づく正解方向候補が右方向であった場合、携帯端末の前方方向が右向きとなった場合、コーナーで右折したと判定される。即ち、コーナーが検出されたら、正解方向候補の向きに合わせるという離散的な方向判定となる。
【００６９】
これに対し、図６（ｂ）によれば、本発明における前方方向の推定を表す。例えばマップデータに基づく正解方向候補が右方向であった場合、携帯端末の前方方向が右向きとなった場合であっても、正解方向候補に徐々に近づけている。そのために、直ぐに右折したとまでは判定しない。そのために、正解方向候補以外の方向への方向転換にも、ある程度追従することができる。特に、図６のように、本発明によれば、障害物を避けるなどの歩行時のノイズの影響によって、誤って判定された正解方向に測位されてしまうことを防ぐことができる。
【００７０】
以上、詳細に説明したように、本発明の携帯端末、プログラム及び方法によれば、角速度センサを用いて、地図上の通路に沿った移動中のユーザの方向転換を、時間経過に応じて推定することができる。また、角速度センサにおけるコーナー判定を徐々に決定するために、前方方向の推定性能における大幅な劣化を防ぐことができる。
【００７１】
前述した本発明における種々の実施形態によれば、当業者は、本発明の技術思想及び見地の範囲における種々の変更、修正及び省略を容易に行うことができる。前述の説明はあくまで例であって、何ら制約しようとするものではない。本発明は、特許請求の範囲及びその均等物として限定するものにのみ制約される。
【符号の説明】
【００７２】
１  携帯端末
１０１  角速度センサ
１０２  加速度センサ
１１  初期姿勢算出部
１２  角速度姿勢行列算出部
１３  加速度姿勢行列算出部
１４  前方方向推定部
１５  方向誤差算出部
１５０  マップデータ記憶部
１６  誤差姿勢行列算出部
１７  付加的前方方向推定部
【図１】
0006516332.tif 000003
【図２】
0006516332.tif 000004
【図３】
0006516332.tif 000005
【図４】
0006516332.tif 000006
【図５】
0006516332.tif 000007
【図６】
0006516332.tif 000008