(57)【要約】 【課題】磁気型ジャイロスコープの測定遅れおよびドリフト問題を解消できる回転情報演算方法を提供する。 【解決手段】本発明の回転情報演算方法では、磁気センサと加速度センサを用いて静止状態の姿勢クオータニオン、準静止状態の角速度ベクトルを高い精度で求める。つぎに回転中の姿勢クオータニオンと角速度ベクトルについては、回転中の３点の磁気ベクトルから回転クオータニオンを求め、それと既知の姿勢クオータニオンと回転演算を行い、回転中の姿勢クオータニオンを求め、さらにそれを使って角速度ベクトルを計算する。初期の姿勢クオータニオンと回転クオータニオンは非常に精度よく求めることができるので、両者を回転演算により組み合わせた本発明は、リアルタイムで高精度の３次元方位と角速度ベクトルを提供することができる。【選択図】図２
2015148497.tif 000002
【特許請求の範囲】
【請求項１】
地磁気などのような向きと大きさが安定な基準磁気を連続的に測定して三次元の磁気成分からなる磁気ベクトルを微小時間Δｔ毎に取得する磁気ベクトル取得ステッ プと、第一時刻ｔｉ（ｉはゼロ以上の正の整数）で取得された第一磁気ベクトルＨｉと、該第一時刻よりも所定の時間前の第二時刻ｔｉ―１、第三時刻ｔｉ−２で取得された第二磁気ベクトルＨ２と第三磁気ベクトルＨ３を使って、第一と第二磁気ベクトルの差ΔＨ１と第二と第三磁気ベクトルの差ΔＨ２を求め、その外積から回転ベクトルつまり回転軸と回転量を計算して、回転クオータニオンＱｒｉを求めるステップと、該第三磁気ベクトルに対応する姿勢クオータニオンＰ３と回転クオータニオンＱｒｉから第一時刻ｔｉに対応する姿勢クオータニオンＰｉを計算するステップと、３軸の加速度センサで測定した重力の値と上記磁気測定値を使って初期の静止状態あるいは準静止状態における静止姿勢クオータニオンＰ０と初期回転クオータニオンＱｒ１および回転初期姿勢クオータニオンＰ１を計算する初期姿勢計算ステップ、およびＰｉとＰｉ−Δｔから角速度ωｉを計算する角速度演算ステップとを備え、移動体に関するリアルタイムの三次元方位および角速度などの回転情報を求めることを特徴とする回転情報演算方法。 
【請求項２】
請求項１において、回転ベクトルを回転所要時間で除して角速度ベクトルωｉとして、角速度クオータニオンωｉを求めて、姿勢クオータニオンＰｉ−１に回転操作を行って姿勢クオータニオンＰｉを求めることを特徴とする回転情報演算方法。 
【請求項３】
請求項１と２の初期姿勢計算ステップと回転中の第一時刻における姿勢計算ステップとの時刻ｔｉにおける姿勢および角速度とを連続的に接続するために、準静止から回転初期の期間の間、両方の方程式による計算を継続するステップと、両者の値を重み付き平均で結合するステップと、動加速度と回転速度の大きさをパラメータとする重み係数を決定するステップとからなり動加速度による誤差を低減したことを特徴とする回転情報演算方法。 
【請求項４】
請求項１から請求項３の磁気ベクトルおよび加速度ベクトルの測定値に対して、ノイズの周波数特性に応じたノイズフィルター、回転速度に応じた重み付平均化処理、動加速度の大きさに応じた重み付平均化処理および移動差平均化処理を組み合わせたノイズ対策を請求項１から請求項３の回転情報演算方法に付加することを特徴する回転情報演算方法。
【請求項５】
メモリーへの格納若しくは媒体への記録または通信がなされ得ると共に、請求項１から請求項４のいずれかに記載の回転情報演算方法をコンピューターで実行可能なことを特徴とする回転情報演算プログラム。
【請求項６】
向きと大きさが安定な基準磁気を三次元的に測定可能な磁気センサーと、請求項５に記載した回転情報演算プログラムを格納したメモリーと、該回転情報演算プログラムを実行可能な演算装置とを備え、移動体の姿勢変化に関する回転情報を出力し得ることを特徴とする磁気型ジャイロスコープ。 
【請求項７】
請求項６に記載の磁気型ジャイロスコープを備えたことを特徴とする移動体。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、地磁気および重力という地球が持つ二つのベクトル量を基準に利用して、回転体の角速度、三次元方位姿勢などの回転情報を求める方法に関するものである。 
【背景技術】
【０００２】
航空機、船舶、自動車等のみならず、携帯電話、スマートホン、タブレット端末などの携帯情報端末、カーナビゲーションなどの位置情報端末、デジタルカメラ等の電子器機などでも、自身の姿勢（方位、向き、傾き等）やその変化を精確にかつリアルタイムに把握することが求められる。このため、回転体の特に角速度や三次元方位などの回転情報を的確かつ迅速に検出または測定することが不可欠となっている。 
【０００３】
角速度を検出できるセンサとして、振動型ジャイロスコープが知られており、これを携帯端末等に組み込めるサイズに小型化したＭＥＭＳ（Micro Electro Mechanical Systems ）ジャイロセンサーが多用されている。このジャイロセンサーは、例えば、シリコン基板を歯状に微細加工した振動子（質量ｍ）からなる。電気的に共振（振動速度ｖ）している振動子へ角速度（ω）が印加されると、振動子の振動方向と回転軸方向の両方に直交する方向へコリオリ力（２ｍｖω）が作用し、振動子の歯間の静電容量が変化する。この静電容量の変化を検出することにより、角速度がリアルタイムで検出される。この検出された角速度を使って微小時間内の微小回転量を積分し続ければ、所定時間内の回転量が求まり、移動体の姿勢変動を把握することが可能となる。 
【０００４】
ところが、このようなジャイロセンサーは、携帯端末等に同時に組み込まれる磁気センサーや加速度センサーに比べて高価である。また、角速度の検出にはその振動子を電気的に常時振動させておく必要があり、消費電力が大きくなる。積分ジャイロの最大の欠点はドリフト特性で、その改善が求められている。さらに、その振動子は、回転運動以外の力学的な振動や衝撃等にも反応するため、ＭＥＭＳジャイロセンサーだけでは必ずしも角速度を精確に検出できるものではなかった。
【０００５】
そこで、ＭＥＭＳジャイロセンサー等による角速度等の検出を、磁気センサーで代替することにより、携帯端末等の小型化、省電力化、低コスト化さらには姿勢情報の検出精度向上等を図ることが考えられている。これに関連する記載が、例えば、下記の特許文献にある。しかし回転初期において微小な回転量とセンサノイズが干渉して回転軸の精度が低下し、それを積分し続けると方位の誤差（ドリフト）となってくる。一方精度確保のためには十分な回転量を取って計算すると、計算遅れにつながりリアルタイム性を損なうことになる。精度とリアルタイム性のトレードオフの関係を克服して、MEMSジャイロセンサを凌ぐ性能を有する磁気式ジャイロスコープおよびそれを使ったリアルタイム性に優れた三次元方位計の開発が求められている。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】ＷＯ２００７／９９５９９号公報
【特許文献２】ＰＣＴ／ＪＰ２０１４／５１５６２
【発明の概要】
【発明が解決しようとする課題】
【０００７】
特許文献１は、被測定体に固定された３軸直交座標系で地磁気を検出して得られた３時点以上の磁気ベクトルに基づいて、回転軸とその回転軸を中心とした回転角度とその回転角度から求まる回転角速度を算出できる磁気式ジャイロを提案している。この磁気式ジャイロを用いれば、ＭＥＭＳジャイロセンサー等を省略しても回転情報を取得することが可能となる。 
【０００８】
もっとも、特許文献１では、ある時点の回転情報を取得するために、それ以前に取得した３時点の磁気ベクトルを必要としており、得られる回転情報はいわば３時点における回転状態を平均化したものに過ぎない。また、その算出精度を向上させるためには、それら３時点で形成される扇形の面積を十分に大きく確保する必要がある。換言するなら磁気ベクトルのサンプリング間隔を大きくする必要がある。これでは算出時間が長くなり、移動体の姿勢変化に追従した迅速な回転情報の取得が困難となる。具体的にいうと、特許文献１にあるような磁気式ジャイロを用いた場合、ＭＥＭＳジャイロセンサーを用いた場合と比較して、１００〜２００ｍｓ程度の測定遅れを生じ得る。一般に端末のスクリーン画面は１６ｍｓ程度で変化するため、１００ｍｓ以上の遅れは操作者にとって無視できない遅れとなる。特に回転軸そのものが自由に変動する自由回転モードの場合、正しく角速度ベクトルを計算することができない。従って、磁気型ジャイロスコープの実用化を図るには、その遅れを１６ｍｓ以下に抑制することが重要となる。さらに回転速度が遅い場合、回転量が小さくなり精度が低下するという問題があった。 
【０００９】
特許文献２は、回転軸が変動する自由回転に対応した磁気ジャイロを提供するものである。この中である時刻の回転軸と磁界変化量ベクトルΔＨが直交する関係に注目して時刻ごとに回転軸の変化をフィードバック補正して対応している。しかし微小磁界変化量が微小の場合、測定ノイズと干渉し誤差が増加する。積分ジャイロの場合にはそれを積分し続けた場合には方位ドリフトとなってしまう欠点があった。
さらに特許文献２では、３軸の磁気センサと３軸の加速度センサの値から求まる三次元方位姿勢に着目して、先に３次元方位姿勢ベクトルを求めて、つぎにその時間変化量から角速度ベクトルを求める姿勢ジャイロについて記載している。動加速度が大きくなると、その角速度は大きな誤差を持つことになり使用できない。
【００１０】
本発明は、これまでの地磁気を活用した磁気式ジャイロあるいは姿勢ジャイロの欠点を解消したもので、地磁気および重力情報に基づいて、各種の移動体の回転情報を素早く取得できる回転情報演算方法と、その方法を実行できる回転情報演算プログラム、そのプログラムを備えた磁気型ジャイロスコープおよびそれを備えた移動体を提供することを目的とする。 
【課題を解決するための手段】
【００１１】
本発明者はこの課題を解決すべく鋭意研究し、試行錯誤を重ねた結果、積分ジャイロ方式で方位姿勢を求める従来の方式に代わる方式、つまり磁気回転量から算出する回転ベクトル活用して直接に現時刻の姿勢クオータニオンを計算し、現時刻の角速度ベクトルおよび3次元方位（回転体の姿勢）をほぼ瞬間的に算出できる新たなアルゴリズム（回転情報演算方法）を考案した。
【００１２】
新たなアルゴリズムを説明する前に、まず、姿勢計算に関する一般的な方法について説明する。静止状態にある物体の三次元姿勢は、物体上に設置されたセンサ座標系の地球座標系に対する姿勢関係、つまり固定した地球座標系と回転するセンサ座標系が一致した状態において方位単位ベクトルＥＰ（例えばｚ軸向きの単位ベクトル）を定義して、センサ座標系が地球座標系に対してＺ軸、Ｘ軸、Ｙ軸回りに角度θ、η、φ回転した時の方位ベクトルの向きで物体の姿勢を定義することができる。
地球座標系は、垂直向きの重力ベクトルと北向きの地磁気ベクトルおよびその外積で３つの直交軸を定義することができる。センサ系において３軸の加速度センサおよび３軸の磁気センサで両者を測定し、センサ系から見た地球座標系を決定できる。両者の方位関係はθ、η、φをパラメータとする回転マトリックスで表現できる。センサ系と定義した方位ベクトルに、この回転マトリックス操作、つまりＺ軸、Ｘ軸、Ｙ軸のθ、η、φ回転させると、方位ベクトルＥＰの向きでセンサ系の三次元姿勢は示される。四元数クオータニオンで表現するならば姿勢クオータニオンＰは、（０；ベクトルＥＰ）と４次元空間の座標点で表現される。回転速度が比較的遅い場合（準静止状態）、この方法が適用できるが、回転中には動加速度が発生して正確な重力を求めることができないため適用できない。
【００１３】
回転中の物体の姿勢は、第一の時刻ｔｉにおける角速度ベクトルωｉを測定し、その値を使って、姿勢クオータニオンに対してジャイロ積分を続けていくことによって求めることができる。角速度ベクトルωｉを求めるために、ＭＥＭＳジャイロ、磁気式ジャイロが提案されているが、積分ドリフトが大きな問題となっている。
【００１４】
そこで、発明者は、回転中の異なる３つの時刻におけるに磁気ベクトルに着目して、回転ベクトルＲｉあるいは回転クオータニオンＱｒｉを求め、既知の方位ベクトルあるいは姿勢クオータニオンＰｉに対して回転演算を行うことによって、第一時刻の姿勢を動加速度による影響を受けることなくリアルタイムに求めることを可能にしたものである。この姿勢クオータニオンＰｉとΔｔ時間前のそれとの変化量から角速度を導出することができる。
この考えを発展させることにより、以降に述べる本発明を完成するに至った。 
【００１５】
《回転情報演算方法》 
本発明の回転情報演算方法は、地磁気などのような向きと大きさが安定な基準磁気を連続的に測定して三次元の磁気成分からなる磁気ベクトルを微小時間Δｔ毎に取得する磁気ベクトル取得ステッ プと、第一時刻ｔｉ（ｉはゼロ以上の正の整数）で取得された第一磁気ベクトルＨｉと、該第一時刻よりも所定の時間前の第二時刻ｔｉ―１、第三時刻ｔｉ−２で取得された第二磁気ベクトルＨ２と第三磁気ベクトルＨ３を使って、第一と第二磁気ベクトルの差ΔＨ１と第二と第三磁気ベクトルの差ΔＨ２を求め、その外積から回転ベクトルつまり回転軸と回転量を計算して、回転クオータニオンＱｒｉを求めるステップと、該第三磁気ベクトルに対応する姿勢クオータニオンＰ３と回転クオータニオンＱｒｉから第一時刻ｔｉに対応する姿勢クオータニオンＰｉを計算するステップと、３軸の加速度センサで測定した重力の値と上記磁気測定値を使って初期の静止状態あるいは準静止状態における静止姿勢クオータニオンＰ０と初期回転クオータニオンＱｒ１および回転初期姿勢クオータニオンＰ１を計算する初期姿勢計算ステップ、およびＰｉとＰｉ−Δｔから角速度ωｉを計算する角速度演算ステップとを備え、移動体に関するリアルタイムの三次元方位および角速度などの回転情報を求めることを特徴とする。 
【００１６】
本発明の回転情報演算方法では、まず静止あるいは準静止状態においては、重力ベクトルＧｓと地磁気ベクトルＨｓの二つのベクトル量の測定値から地球座標系に対する回転体の姿勢を計算して静止姿勢クオータニオンＰｓ０を求める。ある時間経過後の姿勢クオータニオンＰｓ＋１を求めて、その方位差分を経過時間で除して角速度ベクトルωｓを求める。回転速度の増加に伴い動加速度が増加しその影響で姿勢クオータニオンＰｓおよび角速度ベクトルωｓは大きな誤差を持つことになる。
そこで回転状態以前の準静止状態においての磁気ベクトルＨｓをＨ３として、少し回転した近接の磁気ベクトルをＨ２とし、さらに回転開始時期における磁気ベクトルＨｉの３つの磁気ベクトルを使って、磁気ベクトルＨ３からＨｉまでの回転ベクトルを計算し、回転クオータニオンＱｒｉを求める。磁気ベクトルＨｓに対応した姿勢クオータニオンＰｓ０に回転クオータニオンＱｒｉを作用させて回転演算を行い第一時刻ｔｉに対応する姿勢クオータニオンＰｈｉを計算する。つまり初期の姿勢クオータニオンＰｓ０から回転中の任意の時刻ｔｉの姿勢クオータニオンＰｈｉが求まることになる。  
ここで、ｓ記号は、加速度センサと磁気センサの二つの値を利用して計算した値、ｈ記号は３つの磁気ベクトルの値を利用して計算した値、さらに特に両者を区別する必要がない時にはｓとｈ記号は省略した。
【００１７】
磁気ベクトルと重力ベクトルの測定値から求める静止状態における静止姿勢クオータニオンＰｓ０ と磁気ベクトルＨｓおよび準静止状態から回転初期における磁気ベクトルＨ２とＨｉの３点の磁気ベクトルの測定値から計算した回転クオータニオンＱｒ１を使って回転状態における姿勢クオータニオンＰｈ１を求めて、以後回転の継続によってＰｈ２，Ｐｈ３・・・と順次Ｐｉを計算する。
【００１８】
回転状態が継続した場合、第一時刻の磁気ベクトルとＨｉとＨＳとの間隔が拡大していくが、その中間の磁気ベクトルをＨ２として採用し、回転クオータニオンＱｒｉを計算する。しかし第一時刻ｔｉの磁気ベクトルＨｉと回転初期の磁気ベクトルＨｓとの差Δ＝｜Ｈｉ−Ｈｓ｜が大きくなって、つまりθ＝Δ／｜Ｈｉ｜が０．２５ラジアン（１５度）以上になるとｓｉｎθ＝θの近似式の誤差が大きくなってしまう。そこで回転クオータニオンＱｒｉを計算するための基準磁気ベクトルＨ２とＨ３を回転初期の磁気ベクトルＨｓ（＝Ｈ３）とＨ２から該第一時刻よりも所定の時間前の第二時刻ｔｉ―１、第三時刻ｔｉ−２の磁気ベクトルＨ２とＨ３に移動させて、回転クオータニオンＱｒｉを求め、それを磁界ベクトルＨ３に対応した姿勢クオータニオンＰ３に作用させて回転演算を行い、姿勢クオータニオンＰｉを求める。角速度は姿勢クオータニオンＰｉと共役姿勢クオータニオンＰ＊ｉ−Δｔとの積から計算することができる。
これにより、ジャイロ積分に伴う姿勢ドリフトの問題および磁気式ジャイロ計算の微小回転角に起因したノイズによる誤差の問題を解決することができ、非常に大きな精度改善が期待できる。
【００１９】
磁界ベクトルと重力ベクトルから算出した静止姿勢クオータニオンと角速度ベクトルに対して、回転中の磁界ベクトルのみで計算した回転中の姿勢クオータニオンと角速度ベクトルとは、ある回転初期の期間では両者の計算を行い、それから計算された姿勢クオータニオンと角速度ベクトルとを滑らかに接続する必要がある。計算式が異なる二つの値は、動加速度の大きさおよび角速度の大きさをパラメータにした関数で接続するが、重み付平均で接続することが望ましい。
【００２０】
静止姿勢クオータニオンの計算では、回転が激しくなって動加速度が増加すると誤差が大きくなる。一方回転中の姿勢クオータニオンの計算では、回転速度が小さく回転クオータニオンＱｒ１を計算する時の回転量が少ないと誤差が増加する。言い換えれば、回転速度が小さい時には静止姿勢クオータニオンの精度が優れ、回転速度が大きくなると回転中の姿勢クオータニオンの精度が良好となる。したがって、中間の回転速度において両者を重み付平均で結合することが必要である。
【００２１】
《磁気型ジャイロスコープ等》 
本発明は上述した回転情報演算方法としてのみならず、メモリーへの格納若しくは媒体への記録または通信がなされ得ると共に、上述した回転情報演算方法をコンピューターで実行可能なことを特徴とする回転情報演算プログラムとしても把握できる。 
【００２２】
なお、プログラム自体が物の発明として認められない場合は、上記の回転情報演算プロ 
グラムを記録した記録媒体、その回転情報演算プログラムを格納したメモリーまたはその 
メモリーを備えたコンピューター等として本発明は把握される。なお、本発明でいうメモリーは、いわゆるＲＯＭ（Read Only Memory）またはＲＡＭ（Random Access Memory）などのコンピューターに装備される記憶装置である。 
【００２３】
また本発明は、向きと大きさが安定な基準磁気を三次元的に測定可能な磁気センサーと、上述した回転情報演算プログラムを格納したメモリーと、該回転情報演算プログラムを実行可能な演算装置とを備え、移動体の姿勢変化に関する回転情報を出力し得ることを特徴とする磁気型ジャイロスコープとしても把握できる。なお、ここでいうメモリーや 演算装置は、磁気型ジャイロスコープに専用なものでも、移動体に元々備わるメモリーや演算装置を兼用したものでもよい。 
【００２４】
さらに本発明は、上述した磁気型ジャイロスコープを備えたことを特徴とする移動体としても把握できる。移動体は、例えば、方位、向き、姿勢などを測定できる測定モジュールや測定器機等でも良いし、それらを組み込んだ各種の電子器機（例えば携帯端末）等でもよい。 
【００２５】
《その他》 
（１）本発明の構成要素である各「ステップ」は、適宜、「手段」と読み替えられる。これにより、例えば、上述した本発明の回転情報演算方法の各構成要素は、実質的に本発明の回転情報演算プログラムの各構成要素となり得る。 
【００２６】
本発明でいう「回転情報」は、移動体の回転運動に伴う状態または結果を指標する情報であり、角速度ベクトルおよび姿勢ベクトルに基づくものであれば足る。回転情報は、例えば、移動体の角速度の他、その角速度を積分して得られる回転量（回転角）、回転軸の特定情報等でもよい。また本発明に係る各ベクトルは、当然に大きさと向きを有するが、本発明に係る回転情報はそれらの一方のみを利用したものでもよい。 
【００２７】
（２）本明細書でいう「基準磁気」は、移動体の回転運動、したがってそれに取り付けられたセンサの回転に伴って、回転軸と回転量が同じで回転の向きが反対であるように磁気ベクトルが安定的に変化するものであれば足り、地磁気が代表的である。また本明細書でいう「第一時刻」、「第二時刻」等は便宜的な記載であり、特定の時刻やベクトルを意味するものではない。本明細書でいう微小時間（Δｔ）は、小さいほど高精度化を図れるが、使用する磁気センサーの精度、演算装置への負荷等を考慮して、例えば、３〜８ｍｓ程度とすると好ましい。 
【発明の効果】
【００２８】
静止状態の姿勢クオータニオンと広い扇型状の配置された３点の磁気ベクトルから計算した回転クオータニオンは非常に優れた精度で求めることに着目して、両者の演算処理で回転中の任意の時刻の姿勢クオータニオンＰｉと角速度ベクトルを求める本発明の演算方法は、従来のジャイロ積分ドリフト問題、精度と応答性のトレードオフの問題を解決して、リアルタイムで制度の高い方位姿勢および角速度を提供する。同時にＭＥＭＳジャイロの価格、サイズ、消費電流およびドリフトなどの問題を解消して、それに代わって広く使用されることが期待される。
【図面の簡単な説明】
【００２９】
【図１】３時点の磁気ベクトルの終点（Ｈ１,Ｈ２,Ｈ３ ）と回転運動の様子を示す模式図である。
【図２】本発明の回転情報演算方法に係る一実施例を示すフローチャートである。
【図３】本発明に係る一軸回転モードで算出した角速度ベクトルωｚの一例を、従来のジャイロセンサーによる測定値と比較したグラフである。
【符号の説明】
【００３０】
１：磁気ベクトル測定値の球面    ２：磁気ベクトルＨｉ    
３：磁気ベクトルの回転面   ４：回転軸    ５：回転角
６：磁気ベクトルの変化量
７：実施例の測定値    ８：比較例の測定値 
【発明を実施するための形態】
【００３１】
本明細書で説明する内容は、本発明の回転情報演算方法のみならず、回転情報演算プログラム、磁気型ジャイロスコープおよび回転体にも適宜該当し得る。方法に関する構成要素は、「ステップ」を「手段」として理解すれば物に関する構成要素ともなり得る。上述した本発明の構成要素に、本明細書中から任意に選択した一つまたは二つ以上の構成要素 を付加し得る。いずれの実施形態が最良であるか否かは、対象、要求性能等によって異なる。 
【００３２】
《アルゴリズム》
第一発明の実施形態のアルゴリズムは、地磁気などのような向きと大きさが安定な基準磁気を連続的に測定して三次元の磁気成分からなる磁気ベクトルを微小時間Δｔ毎に取得する磁気ベクトル取得ステッ プと、第一時刻ｔｉ（ｉは正の整数）で取得された第一磁気ベクトルＨｉと、該第一時刻よりも所定の時間前の第二時刻ｔｉ―１、第三時刻ｔｉ−２で取得された第二磁気ベクトルＨ２と第三磁気ベクトルＨ３を使って、第一と第二磁気ベクトルの差ΔＨ１と第二と第三磁気ベクトルの差ΔＨ２を求め、その外積から回転ベクトルつまり回転軸と回転量を計算して、回転クオータニオンＱｒｉを求めるステップと、該第三磁気ベクトルに対応する姿勢クオータニオンＰ３と回転クオータニオンＱｒｉから第一時刻ｔｉに対応する姿勢クオータニオンＰｈｉを計算するステップと、３軸の加速度センサで測定した重力の値と上記磁気測定値を使って初期の静止状態あるいは準静止状態における静止姿勢クオータニオンＰｓ０と初期回転クオータニオンＱｒ１および回転初期の姿勢クオータニオンＰｈ１を計算する初期姿勢計算ステップ、およびＰｉとＰｉ−Δｔから角速度ωｉを計算する角速度演算ステップからなっている。 
【００３３】
磁気ベクトル取得ステッ プは、Δｔ時間ごとに磁気ベクトルおよび３軸の加速度センサの値をＦＩＦＯメモリなどに保存する。メモリ容量は測定時間間隔、使用するセンサのＳＮ比、測定対象の回転数などに依存するが、回転クオータニオンＱｒｉの計算に必要な磁気変化量を持つ３点の磁界ベクトルをメモリの中から取得できるように保存能力を持つべきである。なお測定時刻を指定できればΔｔは一定である必要はない。
【００３４】
回転クオータニオンＱｒｉを求めるステップは、第三時刻ｔｉ−２で取得された第三磁気ベクトルＨ３において姿勢クオータニオンＰ３は既知であるとして、磁気ベクトルＨｉ，Ｈ２，Ｈ３の磁気ベクトルの差分ΔＨ１とΔＨ２からＨｉからＨ３への回転ベクトル、つまり回転軸ｎと回転量θを計算し、回転クオータニオンＱｒｉを求める。ここで回転速度が遅い場合には、時間間隔を延ばして回転軸計算に必要な十分なΔＨの大きさを確保する。センサのノイズレベルに依存するが、ΔＨの大きさは２０ｍＧ程度以上、回転量に換算すると６度以上にするのが望ましい。回転速度が速い場合には、ΔＨが７０ｍＧ以上と大きくなりすぎて、ｓｉｎθ＝θの近似式の誤差が無視できなくなるので、Δｔ前のもっとも近い時刻の測定値を使用することが望ましい。
【００３５】
姿勢クオータニオンＰｉを計算するステップは、３点の磁界ベクトルの差分ベクトルΔＨ１とΔＨ２を式（１）に代入して回転ベクトルＲｉを求め、式（２）から、回転量θｉと回転軸ベクトルｎｉを求める。次に２θｉとｎｉを回転クオータニオンＱｒｉに代入して式（３）を求める。Ｑｒｉの共役回転クオータニオンＱｒｉ＊を式（４）から求める。姿勢クオータニオンＰｉは式（５）で定義される。姿勢クオータニオンＰ３に式（６）に示す回転演算を行うことによって、姿勢クオータニオンＰｉを求めることができる。
【００３６】
回転初期姿勢計算ステップは、静止あるいは準静止状態においては、重力ベクトルＧｓと地磁気ベクトルＨｓの二つのベクトル量を測定して、その値から地球座標系に対する回転体の姿勢を計算して静止姿勢クオータニオンＰｓを求めることができる。回転開始状態の姿勢クオータニオンＰｈ１は、静止状態の磁気ベクトルＨＳ（＝Ｈ３）、準静止状態の磁気ベクトルＨ２および回転初期の磁気ベクトルＨ１の３点の磁気ベクトルの測定値から計算した回転クオータニオンＱｒ１を使って回転初期状態における姿勢クオータニオンＰｈ１を求めて、以後回転の継続によって順次Ｐｈｉを計算していく。姿勢クオータニオンＰｈｉを計算する前提に、第三磁気ベクトルＨ３において姿勢クオータニオンＰｈ０＝Ｐｓは既知であるとしたが、この回転初期姿勢計算ステップによってその値を与えることができる。
【００３７】
角速度演算ステップは、第一時刻の姿勢クオータニオンＰｉとΔｔ時間前のＰｉ−Δｔを式（７）に代入すると、角速度ベクトルωｉはその虚数部から求めることができる。
これにより、ジャイロ積分に伴う姿勢ドリフトの問題および磁気式ジャイロ計算の微小回転角に起因したノイズによる誤差の問題を解決することができ、非常に大きな精度改善が期待できる。
【００３８】
第二発明の実施形態のアルゴリズムは、第一発明において、回転クオータニオンＱｒｉを求めるステップの際に求める回転ベクトルを回転所要時間で除して平均角速度ベクトルωｉとして、角速度クオータニオンωｉを求めて、姿勢クオータニオンＰｉ−１に回転操作を行って姿勢クオータニオンＰｉを求めることを特徴とする。回転軸が比較的安定している場合、平均角速度ベクトルωｉを用いることによりアルゴリズムを簡素化できる。
【００３９】
第三発明の実施形態のアルゴリズムは、請求項１の初期姿勢計算ステップと回転中の第一時刻ｔｉにおける姿勢計算ステップから求めた両方の姿勢および両方の角速度とを時刻ｔｉにおいて連続的に接続するためのものである。準静止から回転初期の期間の間、両方の方程式による計算を継続して、両者の値を重み付き平均で結合する。両者の重み係数は、動加速度と回転速度の大きさをパラメータとして決定する。 
【００４０】
両方の計算は、全回転運動中にわたって重力ベクトルＧと地磁気ベクトルＨの二つのベクトル量を測定して動加速度による誤差を無視して姿勢クオータニオンＰｓｉと角速度ベクトルωｓｉをその計算式から求める。動加速度が大きくなると誤差が大きくなる。同時に回転クオータニオンＱｒｉから計算した姿勢クオータニオンＰｈｉと角速度ベクトルωｈｉを求める。回転角度が小さな回転初期においてノイズの影響を受けて誤差が大きくなる。
【００４１】
重み付き平均で結合する場合は、遅い回転速度域ではＰｓｉ、ωｓｉを優勢に、逆に早い速度域ではＰｈｉ、ωｈｉを優勢にした重みを付けて結合する。ωｉは、式（８）を使って連結する。Ｐｉについては、その虚数部をＰａｉとして、式（９）を使って連結し、その結果を式（１０）で単位ベクトル化する。両者の重み係数をＡとＢとして、式（８）と式（９）で示すように滑らかに連結する。Ａは、動加速度の大きさ（Δ｜Ｇｉ｜＝｜Ｇｉ｜−１０００）が小さく、かつ回転速度が小さいほど（｜ΔＨ｜に比例）大きくなり、Ｂが小さくなる。逆に動加速度の大きさ（Δ｜Ｇｉ｜＝｜Ｇｉ｜−１０００）が大きく、かつ回転速度が大きいほど（｜ΔＨ｜に比例）Ａは小さくなり、Ｂが大きくなる。
【００４２】
ＡとＢを求める式を式（１１）から式（１４）に示す。式中のαｇ、αｈ、βｇ、βｈなどのパラメータについては、αｇはＰｓｉに及ぼす動加速度の影響を無視できる範囲をきめるもので、方位誤差２％以下ならば２０ｍｇ程度である。βｇは、いくらの動加速度までなら許容しうるかを示すもので、２０ｍｇ程度が望ましい。αｈは、回転速度が遅くてＰｈｉを適用するには望ましくない範囲を決めるもので、方位誤差２％以下なら５ｍＧ程度である。これはセンサのノイズ特性に強く依存し、ノイズが低いほど小さくできる。βｈは、大きな回転速度になってＰｈｉのみを使用すべき範囲を指定するもので、センサノイズがσ＝１ｍＧの場合に２ｍＧ程度が望ましい。
【００４３】
第四発明の実施形態のアルゴリズムは、第一発明から第三発明において、磁気ベクトルおよび加速度ベクトルの測定値に対して、ノイズの周波数特性に応じたノイズフィルター、回転速度に応じた重み付平均化処理、動加速度の大きさに応じた重み付平均化処理および移動差平均化処理を組み合わせたノイズ対策を請求項１−３の回転情報演算方法に付加して、姿勢計算および角速度ベクトルの計算精度を著しく改善するものである。
【００４４】
磁気ベクトルに関する回転速度に応じた重み付平均化処理の一例を式（１５）から式（１８）に示す。ここでＮは、｜ΔＨ｜がα以下の場合ａ＝１となって、その時の平均回数である。αは、回転以前の状態と判定するパラメータで、センサのノイズ特性に依存する。βは、回転速度が増加すると平均をほとんど行わなくするためのパラメータである。実施例の磁気センサを使用した場合、Ｎ＝４，α＝１、β＝８を一例として採用した。
動加速度の大きさに応じた重み付平均化処理の一例を式（１９）から式（２２）に示す。ここでＮは、Δ｜Ｇ｜がα以下の場合ｂ＝１となって、その時の平均回数である。αは、動加速度がこれ以下においてほとんど平均化を行う必要がないことを示すパラメータである。βは、動加速度が増加すると平均回数を増加させて、動加速度の影響を小さくするためのパラメータである。実施例の加速度センサを使用した場合、Ｎ＝４，α＝２０、β＝２０を一例として採用した。
【００４５】
《磁気センサー》 
本発明に係る磁気センサーは、特定の三次元座標系に沿って地磁気等の基準磁気を測定した際に三次元の磁気成分を検出できるものであれば足り、その種類等は問わない。例えば、磁気センサーとして、マグネト・インピーダンス素子（ＭＩ素子）、ホール素子、磁気抵抗素子等を利用することが考えられる。もっとも、応答性、感度、精度等のいずれにも優れ、小型化を図れるＭＩセンサーが磁気センサーとして好ましい。 
【００４６】
ＭＩセンサーは、アモルファスワイヤからなる感磁体とその周囲に巻回した検出コイルとからなるＭＩ素子により構成される。ＭＩセンサーは、高周波電流が流れる感磁体のインピーダンスが周辺磁界に応じて変化するＭＩ効果を利用したものであり、そのインピーダンスの変化を検出コイルで検出することにより周辺磁界が測定される。このＭＩセンサーに関する出願は多数されており、例えば、ＷＯ２００５／１９８５１号公報、ＷＯ２００９／１１９０８１号公報などに詳しく記載されている。 
【００４７】
なお、ＭＩ素子は、磁気ベクトルの検出方向（座標軸方向）毎に１つ設けられているとよい。例えば、三軸直交座標系で磁気ベクトルを検出する場合なら、ｘ軸方向、ｙ軸方向およびｚ軸方向のそれぞれに対応してアモルファスワイヤを平行にした３つのＭＩ素子を配置してＭＩセンサー（３軸磁気センサー）を構成するとよい。もっとも、ＷＯ２００５／１９８５１号公報にあるように、ＭＩ素子の１つを省略して２つのＭＩ素子で三軸方向の磁気成分を検出できるように構成したＭＩセンサーを用いてもよい。 
【００４８】
《移動体》 
本発明に係る移動体は、本発明の磁気型ジャイロスコープを備えた測定モジュール、測定器機、それらを組み込んだ各種の電子器機等である。本発明の磁気型ジャイロスコープを用いると、方位や姿勢把握等に従来必要とされていた３軸ジャイロセンサーを省略できる。従って本発明の移動体は、携帯電話、スマートホン、タブレット端末、カーナビゲーション、デジタルカメラ等のように、小型化や低コスト化等が厳しく要請される電子器機であると好適である。ちなみに本発明の移動体は、回転情報を手ぶれ補正などに利用するデジタルカメラでもよい。 
【００４９】
なお、本発明の移動体は、磁気センサーの他、加速度センサーおよびそれらによる検出データを処理して方位等を算出する各種のプログラム等を備えると好ましい。また本発明の移動体は、上述した磁気型ジャイロスコープを備えれば足り、従来型のジャイロセンサーまたはジャイロスコープが併設されるものでもよい。 
【実施例】
【００５０】
《フローチャート》 
本発明の回転情報演算方法に係る一実施例であるフローチャートを図３に示した。
先ず、ステップＳ１は測定値の取得ステップである。ステップＳ１―１で、微小時間Δｔ毎に、地磁気（基準磁気）および３軸の加速度センサによる重力を測定する。地磁気の測定は、３軸磁気センサーを介してなされる。こうして３軸直交座標系に沿った３つの磁気成分からなる磁気ベクトルＨｉと加速度ベクトルＧｉが順次取得された各Ｎ個の測定値がセンサ回路のデジタル部に備えられたメモリーに一時的に格納される。 Ｎ個の測定値は、２個から６４個程度が望ましい。
【００５１】
ステップＳ１―２で、計算に必要なデータを呼び出す前に所定のノイズフィルターにかける。
【００５２】
次に、ステップＳ２は、静止または準静止状態（遅い回転状態）における姿勢クオータニオンＰｓｉと角速度ベクトルωｓｉを計算するステップである。
【００５３】
ステップＳ２−１で、時刻ｔｉ（ｔｉ＝ｔ０＋Δｔ・ｉ）で取得された磁気ベクトルＨｉと加速度ベクトルＧｉとを用いて、センサー系の姿勢クオータニオンＰｓｉを計算する。
【００５４】
ステップＳ２−２で、姿勢クオータニオンＰｓｉから角速度ベクトルを計算する。ただし、回転量が小さい場合には時間間隔を３２Δｔまで拡大してその間の平均角速度を計算してωｓｉとする。 
【００５５】
ステップＳ３は、回転中の姿勢クオータニオンＰｈｉと角速度ベクトルωｈｉを計算するステップである。
ステップＳ３−１で、初期回転状態の開始を判定し、その時の時刻ｔｉをｔ１として磁気ベクトルＨ１を取得して、保存した磁気ベクトルＨｉからＨｉ−３２のデータから所定の手続きで時刻ｔ２とｔ３を決めて、Ｈ２，Ｈ３を取得する。ΔＨ１とΔＨ２の値が所定の値、つまり使用している磁気センサのノイズ（例えば標準偏差σ＝０．５ｍＧ）より十分大きな値（例えば１０倍の５ｍＧ）を超えた場合には、Ｈ２，Ｈ３を特定するが、多くの測定値がこの条件を満たす場合にはできるだけＨ１取得時刻に近い方を取る。条件を満たさないときにはＨ３＝Ｈｉ−３２，Ｈ２＝Ｈｉ−１６とする。
【００５６】
ステップＳ３−２で、回転状態において、時刻ｔｉにおいて磁気ベクトルＨｉを取得し、保存した磁気ベクトルＨｉからＨｉ−３２の３２個のデータから所定の手続きで時刻ｔ２とｔ３において、Ｈ２，Ｈ３を取得する。ΔＨ１とΔＨ２の値が所定の値、つまり使用している磁気センサのノイズ（例えば標準偏差σ＝０．５ｍＧ）より１０倍大きな値を超えるように取る。
【００５７】
ステップＳ３−３で、３つの磁気ベクトルＨｉ，Ｈ２，Ｈ３を用いて回転軸ｎｉ、回転量θｉ、回転ベクトルＲｉ，回転クオータニオンＱｒｉを求める。
【００５８】
ステップＳ３−４で、初期回転状態の判定ステップでＨ３を取得した時刻ｔ３の静止状態の姿勢クオータニオンＰｓ３をＰｈ０として、回転初期の姿勢クオータニオンＰｈ１を計算する。ここで最初の回転クオータニオンＱｒ１は、回転初期判定時に取得したＨ１，Ｈ２，Ｈ３を用いて計算する。
【００５９】
ステップＳ３−５で、回転中の姿勢クオータニオンＰｈｉを計算する。ステップＳ３−４で順次計算して求めるＰｓ３（＝Ｐｈ０），Ｐｈ１、Ｐｈ２・・・Ｐｈｉ−１に対してステップＳ３−３で計算した回転クオータニオンＱｒｉによる回転演算を行い求めうる。
【００６０】
ステップＳ３−６で、姿勢クオータニオンＰｈｉとＰｈｉ−Δｔから角速度ベクトルを計算する。
【００６１】
ステップＳ４は、時刻ｔｉにおいて、計算された二つの姿勢クオータニオンＰｓｉとＰｈｉ、角速度ベクトルωｓｉとωｈｉとを結合する。ステップＳ４−１では、角速度の大きさを考慮して重み付係数ＡとＢを計算する。
【００６２】
ステップＳ４−２で、姿勢クオータニオン虚数部をＥＰｉ＝Ａ×ＥＰｓｉ＋Ｂ×ＥＰｈｉで結合し、さらにそれを単位ベクトル化して、連結姿勢クオータニオンＰｉを求める。
【００６３】
ステップＳ４−３で、角速度ベクトルをωｉ＝Ａωｓｉ＋Ｂωｈｉで結合して、連結角速度ベクトルωｉを求める。
【００６４】
ステップＳ５は、姿勢クオータニオンＰｉと角速度ベクトルωｉを出力するステップである。ここでは、必要に応じて平均化処理をして出力してもよい。
【００６５】
《実験》 
上述したフローチャートを実行できる回転情報演算プログラムを組み込んだ磁気型ジャイロスコープと、従来の振動型ジャイロスコープ（ＭＥＭＳジャイロセンサー）とを用いて、移動体の角速度を測定した結果を図３に示した。図３は一軸回転モード（ｚ軸が回転軸）の場合である。本実施例に係る磁気型ジャイロスコープを用いたときの測定結果と、ＭＥＭＳジャイロセンサーを用いたときの測定結果とはよく一致していた。 
【００６６】
ちなみに、図３による試験は、アイチ・マイクロ・インテリジェント株式会社製ＡＭＩ３０６（磁気センサー）およびInvensense社製ＭＰＵ３０５０（ＭＥＭＳジャイロ センサー）を用いて、測定間隔Δｔ：８ｍｓとして行った。 
【００６７】
以下に計算式をまとめ示す。
2015148497.tif 000003
(1)
2015148497.tif 000004
(2)
2015148497.tif 000005
(3)
2015148497.tif 000006
(4)
2015148497.tif 000007
(5)
2015148497.tif 000008
(6)
2015148497.tif 000009
(7)
2015148497.tif 000010
(8)
2015148497.tif 000011
(9)
2015148497.tif 000012
(10)
2015148497.tif 000013
(11)
2015148497.tif 000014
(12)
2015148497.tif 000015
(13)
2015148497.tif 000016
(14)
2015148497.tif 000017
(15)
2015148497.tif 000018
(16)
2015148497.tif 000019
(17)
2015148497.tif 000020
(18)
2015148497.tif 000021
(19)
2015148497.tif 000022
(20)
2015148497.tif 000023
(21)
2015148497.tif 000024
(22)
【図１】
2015148497.tif 000025
【図２】
2015148497.tif 000026
【図３】
2015148497.tif 000027