JP5816879B2 - 磁気式ジャイロ - Google Patents

Description

本発明は、３軸磁気センサを有する磁気式ジャイロに関する。

近年、例えば携帯電話等の携帯機器を傾けたり振り回したりした際の回転角速度を検出し、この携帯機器の動き（回転角速度）をゲーム等のアプリケーションに利用する技術が開発されている。この技術において、携帯機器の回転角速度を検出する手段として、携帯機器内に設けたジャイロが利用されている。

上記ジャイロとして、例えばディレクショナルジャイロ（下記特許文献１参照）や、レートジャイロ（下記特許文献２参照）など、運動力学的な原理を利用したものが従来から知られている。しかし、このようなジャイロは複雑な機構を必要とするため、低コスト化や小型化が難しく、携帯機器に組み込むことは困難だという問題がある。

この問題を解決するため、磁気式ジャイロが開発されている（下記特許文献３、４参照）。磁気式ジャイロは複雑な機械部品を必要としないため、低コスト化や小型化が容易であり、かつ検出精度が高いという利点がある。

磁気式ジャイロは、携帯機器に内蔵した磁気センサを使って地磁気ベクトルを検出し、検出した地磁気ベクトルの時間的変化に基づいて、携帯機器の回転角速度を算出する。例えば下記特許文献３、４に開示されている磁気式ジャイロは、携帯機器の回転運動に伴って地磁気ベクトルの終点が描く軌跡（軌跡円）の中心点および半径を求め、さらに正弦定理を利用することにより、携帯機器の回転角度や回転角速度を算出し、出力する。

米国特許第３１４３８９２号公報 特開平７−１３９９５１号公報 国際公開第２００７／０９９５９９号公報 特許第４５９９５０２号公報

しかしながら前記した特許文献３、４に記載の磁気式ジャイロは、上記軌跡円の中心点や半径を算出し、さらに正弦定理を使って回転角度を算出した後、磁気ベクトルのデータの採取時刻の差から回転角速度を算出するため、演算処理が複雑であり、回転角速度の計算に時間がかかり、実際の被測定体の姿勢変化とジャイロの出力の間に若干の遅れ（時間のずれ）が生じるという問題があった。そのため、高い応答性が要求されるアプリケーションに適用する場合に、上記した出力の時間遅れが問題になる場合があった。
勿論いかなる手段で算出しても回転角速度の算出時間を０とすることは不可能であることから、応答遅れ時間を０とするのは困難である。しかし、計算方法の工夫により、遅れ時間を短縮することは可能と考えられる。

本発明は、かかる背景に鑑みてなされたもので、回転角速度の算出時間を従来公知の上記磁気式ジャイロに比較して短縮化し、上記した時間遅れを大幅に短くすることが可能な磁気式ジャイロを提供しようとするものである。

本発明の一態様は、被測定体に固定された３軸直交座標系における磁気ベクトルとして地磁気を検出する３軸磁気センサと、
該３軸磁気センサによって時系列的に検出される上記磁気ベクトルのデータを検出時の時刻データと共に蓄積するメモリと、
該メモリに蓄積された上記磁気ベクトルのデータに基づいて、上記被測定体の回転運動の基準とする回転軸を決定する回転軸決定手段と、
上記メモリに蓄積された上記磁気ベクトルのデータと上記時刻データとに基づいて、上記回転運動の回転角速度を算出する回転角速度算出手段と、
該回転角速度算出手段によって算出した上記回転角速度を出力する回転角速度出力手段とを有し、
上記回転角速度算出手段は、上記３軸磁気センサによって検出した上記磁気ベクトルＨと、該磁気ベクトルＨから、該磁気ベクトルＨよりも微小時間Δｔ前の時点において測定した磁気ベクトルＨ’を引いたベクトルΔＨ（＝Ｈ−Ｈ’）と、上記回転軸と上記磁気ベクトルＨとの間の角度αを、下記数式
｜ω｜＝｜Ｈ×ΔＨ｜／（｜Ｈ｜^２・ｓｉｎα・Δｔ）
に代入することにより、上記被測定体の回転角速度ベクトルωの絶対値｜ω｜（以下、回転角速度｜ω｜と記す。）を、上記回転角速度として算出するよう構成されていることを特徴とする磁気式ジャイロにある（請求項１）。

上記磁気式ジャイロは、上記数式を用いることにより、被測定体（携帯機器）の回転角速度を算出するが、その算出の際において前記特許文献３、４に記載されている算出方法とは異なり、軌跡円の中心点や半径を算出しなくても求めることができる。そのため、被測定体の回転角速度｜ω｜を算出するための演算処理を簡単にすることができ、回転角速度を短時間で算出することが可能になる。これにより、被測定体の実際の動きに対するジャイロの出力遅れ時間を短縮化することが可能になるため、上記アプリケーション等を使用する際に、被測定体の回転運動をアプリケーション等に素早く反映させることが可能になり、応答性を向上させることができる。

特に、被測定体が一時的であっても非常に高速となる回転運動を含むような場合には、微小時間内にも回転軸、回転角速度が変化しつつ運動することになるため、被測定体の回転運動を精確かつ滑らかにアプリケーション等に反映できるようにするためには、微小時間Δｔを短くし、回転角速度の演算回数を増やす必要がある。上記数式を用いて、回転角速度を演算するためのアルゴリズムを簡素化すれば、演算処理装置（ＣＰＵ）の負担を低減でき、回転角速度の算出速度を向上できるため、演算回数が多い場合に特に有効である。

以上のごとく、本発明によれば、回転角速度を短時間で算出し、応答遅れ時間を短縮できる磁気式ジャイロを提供することができる。

実施例１における、磁気式ジャイロの概念図。 実施例１における、磁気式ジャイロのブロック図。 実施例１における、磁気式ジャイロのプログラムのフローチャート。 実施例１における、回転軸、磁気ベクトル等の説明図。 実施例１における、３軸磁気センサの斜視図。 実施例２における、磁気式ジャイロの概念図。 実施例２における、角度βと回転角速度｜ω｜との関係を表すグラフ。 実施例２における、磁気式ジャイロのプログラムのフローチャート。 実施例３における、回転角速度と時間の関係を表したグラフ。 実施例４における、回転角速度と時間の関係を表したグラフ。

上記磁気式ジャイロにおいて、上記３軸直交座標系における加速度ベクトルとして重力加速度を検出する３軸加速度センサと、該３軸加速度センサによって時系列的に検出される上記加速度ベクトルのデータを検出時の時刻データと共に蓄積する加速度ベクトルデータ蓄積用メモリと、上記磁気ベクトルのデータおよび上記加速度ベクトルのデータの少なくとも一方に基づいて、上記被測定体の回転角速度又は該回転角速度と関連のあるパラメータ値が、基準となる閾値未満である第１回転状態か、又は上記閾値以上である第２回転状態であるかを判断する回転状態判断手段を備え、上記回転角速度出力手段は、上記回転状態判断手段によって、上記被測定体が上記第２回転状態であると判断された場合にのみ、上記回転角速度を出力することが好ましい（請求項２）。
この場合には、測定精度がよい場合のみ、回転角速度を出力することができる。すなわち、回転角速度又は上記パラメータ値が閾値よりも小さい場合（第１回転状態）は、微小時間Δｔ内における磁気ベクトルＨの変化量ΔＨが小さいため、上記数式を用いて回転角速度｜ω｜を算出すると誤差が大きくなりやすい。そのため、第１回転状態において回転角速度を出力すると、誤差の大きい回転角速度を出力することになる。
これに対して、被測定体の回転角速度又は上記パラメータ値が閾値以上である場合（第２回転状態）は、微小時間Δｔ内における磁気ベクトルＨの変化量ΔＨがある程度以上に大きくなるため、回転角速度｜ω｜の誤差が小さくなりやすい。したがって、閾値を適切に設定した上で、第２回転状態の場合のみ回転角速度を出力することにより、測定精度の高い場合のみ、回転角速度を出力することが可能になる。
なお、回転状態判断手段は、回転角速度｜ω｜を使って、回転状態を判断することもできるが、上記パラメータ、例えば磁気ベクトルの単位時間における変化割合や、磁気ベクトルの差分ベクトルの大きさが一定の大きさを越えるまでの時間や、加速度ベクトルの変化割合等のパラメータを採用することもできる。

また、上記回転角速度算出手段によって算出した上記回転角速度に基づいて、上記微小時間Δｔ内における、上記回転軸を中心とした上記被測定体の回転角度を算出する回転角度算出手段と、算出された上記回転角度を出力する回転角度出力手段とを備えていてもよい。（請求項３）。
この場合には、回転角速度を用いることにより、微小時間Δｔ内における、被測定体の回転角度を算出することが可能となり、回転角度を必要とするアプリケーションに求めた値を利用することができる。上記磁気式ジャイロは回転角速度の算出時間が短いため、被測定体の回転角度の算出及び出力も、実際の被測定体の動きに対する出力遅れ時間を短くすることができる。

また、上記回転角速度算出手段によって算出した上記回転角速度に基づいて、上記微小時間Δｔ内における、上記回転軸を中心とした上記被測定体の回転角度を算出する回転角度算出手段と、算出された上記回転角度を出力する回転角度出力手段とを備えると共に、上記磁気ベクトルのデータと上記加速度ベクトルのデータとに基づいて、上記被測定体の姿勢を算出する第１姿勢算出手段と、上記回転角度算出手段によって算出された上記回転角度に基づいて、上記微小時間Δｔ内における上記被測定体の姿勢変化量を算出し、該姿勢変化量を、上記第１回転状態において上記第１姿勢算出手段が算出した上記姿勢の初期値に積算することにより、上記被測定体の最新の姿勢を算出する第２姿勢算出手段とを備えいてもよい（請求項４）。
この場合には、磁気式ジャイロは第１姿勢算出手段と第２姿勢算出手段との２つの姿勢算出手段を備えるため、これら２つの姿勢算出手段を、被測定体の運動状態に応じてより精度の高い出力ができる方を選択することにより、使い分けることができる。これにより、状況に応じて被測定体の上記姿勢をより正確に算出することが可能となる。すなわち、第１姿勢算出手段は、３軸磁気センサと３軸加速度センサとの２種類のセンサを用いて、被測定体の姿勢を算出する。後述するように、この２種類のセンサを用いると、被測定体が静止している状態では、加速度センサの検出値は重力加速度と等しくなるため、地球座標系に対する被測定体の姿勢を直接、正確に算出することができ、被測定体の回転が静止しているか、比較的遅い場合において、大きな誤差なく姿勢を算出可能になる。その一方で、３軸加速度センサは、被測定体が回転等の運動をすると、その検出値には、被測定体自身の加速度が本来測定したい重力加速度に加算されることになる。それに対し、第２姿勢算出手段は３軸磁気センサのみを用いて姿勢を算出するため、被測定体自身の運動による加速度の影響を排除した算出をすることができる。そのため、これら２つの姿勢算出手段を使い分けることにより、被測定体の姿勢の算出精度をより高めることが可能になる。

また、上記回転状態判断手段によって、上記被測定体の回転運動が上記第１回転状態であると判断された場合は、上記第１姿勢算出手段によって算出した上記姿勢を出力し、上記回転運動が上記第２回転状態であると判断された場合は、上記第２姿勢算出手段によって算出した上記姿勢を出力する姿勢出力手段を備えていてもよい。（請求項５）。
このようにすると、被測定体の、より正確な姿勢を出力することができる。すなわち、第１姿勢算出手段では、３軸加速度センサと３軸磁気センサとの２種類のセンサを用いるため、被測定体が静止しているか、又は比較的遅い速度で回転している場合には、被測定体の姿勢を直接、正確に算出することができる。また、被測定体自身が静止していない場合は、加速度センサの検出値に被測定体自身の加速度が加わり、測定誤差となるが、上記磁気式ジャイロは、被測定体が第１回転状態となり、前記した誤差の影響が無視できる程度に小さくなった場合のみ、３軸加速度センサを利用する第１姿勢算出手段の姿勢算出結果を出力する。そのため、被測定体自身の運動状況に応じ、適切に３軸加速度センサの検出値を被測定体の姿勢計算に利用して、正確な姿勢を出力することができる。

また、被測定体が第２回転状態となった場合には、第２姿勢算出手段によって算出した姿勢、すなわち、第１回転状態であった際に求めた基準となる姿勢（初期姿勢）に、磁気ベクトルのデータにより算出した姿勢変化分を加算していくことにより、求めた最新の姿勢値を出力する。このようにすると、被測定体が第２回転状態となり、３軸加速度センサが被測定体自身の運動による影響で精確な重力加速度ベクトルを求めることができない状況になった場合であっても、３軸加速度センサのデータを利用しない第２姿勢算出手段の姿勢算出結果を出力するため、精度良く最新の姿勢を求めることができる。

また、上記第１姿勢算出手段によって算出した上記被測定体の上記姿勢である第１姿勢と、上記第２姿勢算出手段によって算出した上記被測定体の上記姿勢である第２姿勢との間の中間姿勢を算出する中間姿勢算出手段を備え、
上記中間姿勢は、上記回転角速度がより遅い回転状態になるほど上記第１姿勢に近づき、上記回転角速度がより速い回転状態になるほど上記第２姿勢に近づき、かつ上記第１姿勢と上記第２姿勢との間で連続的に変化し、
上記中間姿勢算出手段によって算出された上記中間姿勢を出力する中間姿勢出力手段を備え、
上記中間姿勢算出手段は、上記回転状態判断手段において用いることが可能な上記パラメータ値を用いて、上記回転状態を定めていることが好ましい（請求項６）。
この場合には、被測定体の回転速度が変化した場合に、上記第１姿勢と上記第２姿勢との間の中間姿勢を出力できるため、姿勢の出力値が、回転角速度の閾値を境に、第１姿勢から第２姿勢に切り替わったり、又は第２姿勢から第１姿勢に切り替わった場合に、出力値が不連続に変化する不具合を防止できる。
また、第１姿勢算出手段は、被測定体の姿勢を直接、算出できるが、被測定体が静止していない場合には、第１回転状態の範囲であっても、３軸加速度センサの出力値は、重力加速度とずれが生じることになり、その影響で姿勢の算出結果に誤差が生じる原因となる。また、第２姿勢算出手段は、３軸加速度センサを用いないため、被測定体自身の運動状態による影響はないものの、姿勢の上記初期値に、微小時間内に算出される姿勢変化量を積算するため、３軸磁気センサ自身の測定誤差が累積しやすい。また、この誤差は、回転速度が遅く磁気ベクトルの変化が小さいほど顕著になる。このように、第１姿勢算出手段は被測定体の回転が遅い場合に比較的誤差が小さくなるという特徴を有し、第２姿勢算出手段は、逆に被測定体の回転が速い場合に比較的誤差が小さくなるという特徴を有しているため、瞬間瞬間の被測定体の状態から最も誤差が小さくなると考えられる第１姿勢と第２姿勢の中間姿勢を算出することにより、被測定体の回転状態がいかなる状況であっても精度の良い姿勢値を算出し、出力することができる。

なお、中間姿勢算出手段は、回転角速度｜ω｜のほか、回転状態判断手段で用いることが可能なパラメータを用いて、上記回転状態を定めることができる。例えば、磁気ベクトルや加速度ベクトルの変化量等を用いることができる。このように、磁気ベクトルの変化割合をパラメータとして用いた場合には、この値が大きいほど、回転角速度が速いということを意味する。
また、ここで用いるパラメータ値は、回転状態判断手段により用いることのできるパラメータ、すなわち、後述の実施例１にて説明しているような回転状態を表現できるパラメータであれば良く、必ずしも回転状態判断手段で用いたパラメータと同一である必要はない。例えば回転状態判断手段では、加速度センサの誤差の問題を考慮して、加速度ベクトルの絶対値の変化量をパラメータ値として用い、中間姿勢を求める際には回転角速度｜ω｜を用いるようにすることもできる。

また、上記第２姿勢算出手段を用いて上記姿勢を算出した後、上記被測定体の回転運動が上記第１回転状態となり、再び上記第２回転状態に戻った場合に、上記第２姿勢算出手段によって算出する際の基準となる上記姿勢のデータを、上記第１姿勢算出手段によって算出した、上記第２回転状態に戻る直前に第１姿勢算出手段により求めた上記姿勢のデータと置き換える置換手段を備え、上記第２姿勢算出手段は、置き換え後の基準となる姿勢データに第２姿勢算出手段により求めた姿勢変化量を加算していくことにより、上記姿勢の算出を行うよう構成されていてもよい（請求項７）。
この場合には、上記第２回転状態において、被測定体の姿勢の算出値に誤差が累積する不具合を防止できる。すなわち第２姿勢算出手段は、微小時間Δｔ毎に、３軸磁気センサを用いて検出した姿勢変化量を１ステップ前に算出済みの姿勢算出値に積算していくことにより、最新の姿勢を算出するため、１回の磁気センサの検出により生じた誤差が、姿勢変化量を積算していくことによって累積していくことになる。これに対して第１姿勢算出手段は、後述するように、磁気ベクトルのデータと加速度ベクトルのデータとを用いて、算出に用いた磁気ベクトルと加速度ベクトルの測定時刻における被測定体の姿勢を直接、算出できるため、第２姿勢算出手段のように誤差が累積されることがない。そのため、第２回転状態から第１回転状態に移った場合に、第１姿勢算出手段を用いて正確な姿勢を算出し、再び第２回転状態に移った際に、第２回転状態に移る直前の計算ステップにおいて第１姿勢算出手段を用いて算出した姿勢のデータを基準の姿勢値とし、この姿勢値に最新のステップにおける第２姿勢算出手段で算出した姿勢変化量を積算するようにする。これにより、第２姿勢算出手段において使用する、基準となる姿勢値をリセットでき、姿勢に誤差が累積して算出精度が低下することを防止できる。

（実施例１）
磁気式ジャイロに係る実施例について、図１〜図５を用いて説明する。
図１に示すごとく、本例の磁気式ジャイロ１は、３軸磁気センサ２と、メモリ３と、回転軸決定手段１０と、回転角速度算出手段１１と、回転角速度出力手段１２とを備える。３軸磁気センサ２は、被測定体に固定された３軸直交座標系１００（図５参照）における磁気ベクトルとして地磁気を検出する。メモリ３は、３軸磁気センサ２によって時系列的に検出される磁気ベクトルのデータを検出時の時刻データと共に蓄積する。

また、回転軸決定手段１０は、メモリ３に蓄積された磁気ベクトルのデータに基づいて、被測定体の回転運動の基準とする回転軸を決定する。回転角速度算出手段１１は、メモリ３に蓄積された磁気ベクトルのデータと時刻データとに基づいて、回転運動の回転角速度を算出する。回転角速度出力手段１２は、回転角速度算出手段１１によって算出した回転角速度を出力する。

回転角速度算出手段１１は、３軸磁気センサ２によって検出した磁気ベクトルＨと、該磁気ベクトルＨから、磁気ベクトルＨよりも微小時間Δｔ前の時点において測定した磁気ベクトルＨ’を引いた差分ベクトルΔＨ（＝Ｈ−Ｈ’）と、ｓｉｎα（αは回転軸と磁気ベクトルＨとの間の角度）を、下記数式
｜ω｜＝｜Ｈ×ΔＨ｜／（｜Ｈ｜^２・ｓｉｎα・Δｔ） ・・・（１）
に代入することにより、被測定体の回転角速度｜ω｜を、回転角速度として算出する。

なお、ここで回転軸ベクトルは、最新の３時点の磁気ベクトルＨ，Ｈ’，Ｈ’’の差分ベクトルΔＨ（＝Ｈ−Ｈ’）、ΔＨ’（＝Ｈ’−Ｈ’’）の外積により算出することができる。詳細は特許文献３、４に記載されている。また、最新の磁気ベクトルＨと差分ベクトルΔＨ、ΔＨ’の外積からなるベクトルの内積の式
ｃｏｓα＝（Ｈ，（ΔＨ×ΔＨ’））／｜Ｈ｜｜ΔＨ×ΔＨ’｜（ここで（Ｈ，（ΔＨ×ΔＨ’））は、Ｈと（ΔＨ×ΔＨ’）の内積を意味する。）
によりｃｏｓαを求め、
ｓｉｎα＝√（１−ｃｏｓ^２α）によりｓｉｎαを求め、これを（１）式に代入することにより、｜ω｜を算出できる。

上記被測定体は、例えば携帯電話等の携帯機器である。本例の磁気式ジャイロ１を用いて、例えば、携帯機器を回転させたり振り回したりした際の回転運動（回転角速度）を検出し、その検出値を、携帯機器において実行するゲーム等のアプリケーションに利用するよう構成することができる。

図２に示すごとく、磁気式ジャイロ１は、マイコン６を備える。マイコン６は、ＣＰＵ６０と、ＲＯＭ６１と、ＲＡＭ（メモリ３）と、Ｉ／Ｏ６２と、これらの構成を繋ぐライン６３とを備える。ＲＯＭ６１にはプログラム６１ｐが記憶されている。このプログラム６１ｐをＣＰＵ６０が読み出して実行することにより、本例の回転軸決定手段１０、回転角速度算出手段１１、回転角速度出力手段１２、および後述する回転状態判断手段１３、回転角度算出手段１４、回転角度出力手段１５、第１姿勢算出手段５１、第２姿勢算出手段５２、姿勢出力手段５３、置換手段５６が実現される。

３軸磁気センサ２は、図５に示すごとく、マグネト・インピーダンス・センサ素子２０によって構成してある。即ち、３軸磁気センサ２は、３個のマグネト・インピーダンス・センサ素子２０を、それぞれの感磁方向が互いに直交する３軸方向（Ｘ軸方向、Ｙ軸方向、Ｚ軸方向）となるように配設することにより、形成してある。なお、図５においては、マグネト・インピーダンス・センサ素子２０以外の電子部品や配線は省略してある。

次に図４を用いて、上記数式（１）の説明をする。図４において、Ｈは、３軸磁気センサ２によって測定した磁気ベクトルであり、Ｈ’は、磁気ベクトルＨよりも微小時間Δｔ前に測定した磁気ベクトルである。ΔＨは磁気ベクトルの２つの測定値の差（＝Ｈ−Ｈ’）である。また、Ｃは、被測定体の回転運動に伴って磁気ベクトルＨの終点が描く軌跡（軌跡円）である。ｒは、軌跡円Ｃの中心点Ｏから、磁気ベクトルＨ，Ｈ’の終点に向う半径ベクトルである。θは、微小時間Δｔ内において、半径ベクトルｒが変化する角度である。
また、Ｋは、被測定体の回転運動の中心となる回転軸である。ｎは、上記中心点Ｏを始点とし、回転軸Ｋに平行な単位ベクトルである。αは、回転軸Ｋと磁気ベクトルＨとのなす角度である。この場合において、回転角速度｜ω｜は、前記した通り（１）式で求めることができる。
｜ω｜＝｜Ｈ×ΔＨ｜／（｜Ｈ｜^２・ｓｉｎα・Δｔ） ・・・（１）
なお、回転角速度ベクトルωは、下記の式によって定義することができる。
ω＝｜ω｜・ｎ

この式で回転角速度｜ω｜を求めることにより、軌跡円Ｃの中心点Ｏや半径｜ｒ｜を算出しなくても｜ω｜を求めることができる。なお、本発明の磁気式ジャイロ１は、各時間における瞬間的な回転角速度を求められるようにすることを狙っており、その場合、被測定体が高速回転する場合であって、回転軸や回転角速度が時間とともに変化しながら回転している被測定体であっても、瞬間的な回転角速度を精度よく求めることが可能である必要がある。従って、磁気ベクトルの測定は、高速回転であってもθが小さくなるような微小時間毎に継続して行い、瞬間的な回転角速度が精度良く求められるようにする必要がある。θが大きくなると、そのθの回転の間の｜ω｜や回転軸ベクトルの変化を無視した計算では、精度の良い算出ができなくなるためである。そのため、精度の良い算出を可能とするために、磁気ベクトルの測定は数ｍｓ以下程度の時間間隔で連続して行うことが好ましい。このように微小な時間ごとに測定することにより、高速回転中であっても微小時間内の回転角度を十分に小さく抑えることができ、上記式により、被測定体の瞬間的な回転角速度の算出を精度良く行うことが可能となる。

次に、本例の磁気式ジャイロ１の構成をさらに詳細に説明する。図１に示すごとく、本例の磁気式ジャイロ１は、回転状態判断手段１３を備える。回転状態判断手段１３は、磁気ベクトルＨのデータに基づいて、被測定体の回転角速度｜ω｜又は該回転角速度｜ω｜と関連のあるパラメータ値が、基準となる閾値未満である第１回転状態か、又は閾値以上である第２回転状態であるかを判断する。回転角速度出力手段１２は、回転状態判断手段１３によって、被測定体の回転運動が第２回転状態であると判断された場合にのみ、回転角速度｜ω｜を出力する。
本例の回転状態判断手段は、回転角速度｜ω｜を用いて回転状態を判断している。なお、上述したように、例えば磁気ベクトルＨの単位時間における変化割合や、磁気ベクトルＨの差分ベクトルの大きさが一定の大きさを越えるまでの時間や、加速度ベクトルＡの変化割合等のパラメータを採用することもできる。

また、図１に示すごとく、本例の磁気式ジャイロ１は回転角度算出手段１４を備える。回転角度算出手段１４は、回転角速度算出手段１１によって算出した回転角速度｜ω｜に基づいて、微小時間Δｔ内における、回転軸Ｋを中心とした被測定体の回転角度θ（＝｜ω｜Δｔ）を算出する。また、本例の磁気式ジャイロ１は、算出された回転角度θを出力する回転角度出力手段１５を備える。

また、本例の磁気式ジャイロ１は、３軸加速度センサ４と、第１姿勢算出手段５１と、第２姿勢算出手段５２と、姿勢出力手段５３とを備える。３軸加速度センサ４は、３軸直交座標系１００における加速度ベクトルＡとして重力加速度を検出する。検出した重力加速度のデータは、検出時の時刻データと共にメモリ３に蓄積される。第１姿勢算出手段５１は、磁気ベクトルＨのデータと加速度ベクトルＡのデータとに基づいて、被測定体の姿勢を算出する。
第２姿勢算出手段５２は、回転角度算出手段１４によって算出された回転角度θに基づいて、微小時間Δｔ内における被測定体の姿勢変化量を算出し、該姿勢変化量を、被測定体の回転状態が第１回転状態であった際に第１姿勢算出手段５１により算出した姿勢の初期値に積算することにより、被測定体の最新の姿勢を算出する。
姿勢出力手段５３は、回転状態判断手段１３によって、被測定体の回転運動が第１回転状態であると判断された場合は、第１姿勢算出手段５１によって算出した姿勢を出力し、上記回転運動が第２回転状態であると判断された場合は、第２姿勢算出手段５２によって算出した姿勢を出力する。

次に、第１姿勢算出手段５１によって被測定体の姿勢を算出する方法、すなわち磁気ベクトルと加速度ベクトルのデータを用いて上記姿勢を算出する方法について説明する。まず、例えば下記数式（２）に示すごとく、磁気ベクトルＨ（ｈ_ｘ，ｈ_ｙ，ｈ_ｚ）と加速度ベクトルＡ（ａ_ｘ，ａ_ｙ，ａ_ｚ）との外積を用いて、東方向の単位ベクトルｅ_Ｅを算出する。
また、鉛直方向の単位ベクトルｅ_Uは、加速度ベクトルＡと方向が一致するため、下記数式（４）によって算出できる。
また、北方向の単位ベクトルｅ_Nは、下記数式（３）に示すごとく、東方向の単位ベクトルｅ_Eと鉛直方向の単位ベクトルｅ_Uとの外積に等しい。

これらの単位ベクトルｅ_Ｅ，ｅ_N，ｅ_Uを下記数式（５）のように組み合わせることにより、被測定体の姿勢を表す姿勢行列Ｐが得られる。

なお、姿勢行列Ｐが求まれば、Ｐからピッチ角δ、ロール角φ、ヨー角ηは以下のように求めることができる。
δ＝ｔａｎ^−１（Ｕｙ／Ｕｚ）
φ＝ｓｉｎ^−１（−Ｕｘ）
又はφ＝ｔａｎ^−１（−Ｕｘ／（Ｕｙ・ｓｉｎδ＋Ｕｚ・ｃｏｓδ））
η＝ｔａｎ^−１（Ｎｘ／Ｅｘ）

このように算出したピッチ角δ、ロール角φ、ヨー角ηから、被測定体が、地球座標系においてどの方向を向いているか、すなわち姿勢を判断することができる。

次に、第２姿勢算出手段５２によって被測定体の姿勢を算出する方法について説明する。まず、図４に示す単位ベクトルｎの成分を（ｎ_ｘ，ｎ_ｙ，ｎ_ｚ）とすると、時刻ｔ_ｋにおける被測定体の姿勢行列Ｐ_ｋと、上記時刻ｔ_ｋよりも微小時間Δｔ前における被測定体の姿勢行列Ｐ_ｋ−１との関係は、下記数式（６）、数式（７）のように表される。

Ｐ_ｋ＝Ｐ_ｋ−１・Ｒ_ｋ ・・・（７）
このように、微小時間Δｔ毎に回転行列Ｒ_ｋを算出し、第１姿勢算出手段５１によって算出した、姿勢行列Ｐの初期値Ｐ_０に回転行列Ｒ_ｋを順次掛け続けることにより、被測定体の最新の姿勢を表す姿勢行列Ｐ_ｋを算出することができる。

なお、姿勢行列Ｐの上記初期値Ｐ_０は、磁気式ジャイロ１を起動した後であって、被測定体が静止状態等、第１回転状態にあると判断できる際に、自動的に取得したり、ユーザが所定のボタンを押圧した際に取得したりすることができる。

以上説明したように、被測定体の回転状態が第１回転状態である場合は、磁気ベクトルＨおよび加速度ベクトルＡのデータを用いて、被測定体の姿勢（姿勢行列Ｐ）を算出することができ、被測定体の回転状態が第２回転状態である場合は、磁気ベクトルＨのデータと、姿勢の初期値Ｐ_０とを使って、被測定体の最新の姿勢を表す姿勢行列Ｐ_ｋを算出することができる。

一方、図１に示すごとく、本例の磁気式ジャイロ１は置換手段５６を備える。置換手段５６は、第２姿勢算出手段５２を用いて姿勢（姿勢行列）を算出した後、被測定体の回転運動が第１回転状態となり、再び第２回転状態に戻った場合に、第２回転状態に戻る直前に第１姿勢算出手段５１によって算出した姿勢のデータ（姿勢行列Ｐ_ｋ−１）を、第２姿勢算出手段５２の計算の基準となる姿勢データとして置き換える。そして、その後の第２姿勢算出手段５２は、置き換え後のデータ（姿勢行列Ｐ）に、算出した微小時間Δｔ毎の姿勢変化量を積算していくことにより、最新の姿勢の算出を行う。これにより、前記したとおり、算出誤差が蓄積される不具合を防止することができる。

次に、ＲＯＭ６１（図２参照）に記憶したプログラム６１ｐのフローチャートについて説明する。図３に示すごとく、プログラム６１ｐを開始すると、まずステップＳ１を処理し、磁気ベクトルＨ及び加速度ベクトルＡのデータを取得する。これらのデータは、微小時間Δｔごとにそれぞれの記憶場所として確保されたメモリ３に、取得した時刻データとともに保存される。

上記データを取得した後、ステップＳ２に移り、回転運動の回転軸Ｋ（図４参照）を算出する。この後、ステップＳ３に移り、上記数式（１）を用いて、被測定体の回転角速度｜ω｜を算出する。

その後、ステップＳ４を処理する。ステップＳ４では、微小時間Δｔ内における被測定体の回転角度θ（図４参照）を算出する。その後、ステップＳ５を処理する。ステップＳ５では、被測定体の回転運動が第１回転状態であるか又は第２回転状態であるかを判断する。ステップＳ５において第１回転状態と判断された場合は、ステップＳ６に移り、第１姿勢算出手段５１による姿勢算出、すなわち、上記数式（２）〜（５）を使って姿勢行列Ｐを算出する処理を行う。そして、算出した姿勢行列Ｐから被測定体の姿勢状態を示すピッチ角δ、ロール角φ、ヨー角ηを求め、姿勢出力手段５３によって算出した姿勢のデータを出力する。なお、最初にステップＳ５を処理する時点において、第２姿勢算出手段５２で基準姿勢となる初期の姿勢行列Ｐ０を、加速度センサの検出値に重力加速度以外の加速度の影響が生じない環境で得られた磁気ベクトルと加速度ベクトルのデータから、算出しておく必要がある。したがって、例えば図３のフローが開始され、最初のステップＳ５の処理段階に限っては、被測定体の画面に、「静止状態を維持して下さい。準備ができたらボタンを押し、ＯＫの表示がされるまで静止状態を維持して下さい。」等の表示をする等の方法で意図的に第１回転状態の環境とし、加速度センサの検出値に重力加速度以外の加速度値の影響が生じない状態で、磁気ベクトルと加速度ベクトルを検出し、ステップＳ６を実行するようにする。なお、ステップＳ５では、後述のステップＳ７の処理で必要となるため、回転状態を判断した後にその結果を保存しておく。

また、ステップＳ５の処理により第２回転状態と判断された場合は、ステップＳ７に移り、１ステップ前の回転状態が、どちらの回転状態であったかをステップＳ５で保存した情報から取得し、直前のステップまで第１回転状態であった場合には、回転状態が変化した直後であると判断し、ステップＳ８に移り、計算の基準となる姿勢データの置換処理、すなわち置換手段５６を実行し、ステップＳ９に移る。また、１ステップ前の回転状態が第２回転状態であった場合は、置換手段を実行することなく、ステップＳ９に移る。そして、置換手段５６が実行された場合は、置換により得られた基準となる姿勢行列に、置換手段５６が実行されない場合は、１ステップ前の第２姿勢算出手段５２にて算出された姿勢行列に、第２姿勢算出手段５２により算出した姿勢変化量を積算して、最新の姿勢行列を求め、姿勢データであるピッチ角δ、ロール角φ、ヨー角ηを求め、姿勢出力手段５３により姿勢のデータを出力する。

ステップＳ６またはステップＳ９を処理した後、ステップＳ１に戻る。そして、ステップＳ１〜ステップＳ９を繰り返し実行する。

なお、ステップＳ８にて実行する置換手段５６は、被測定体が第２回転状態から第１回転状態に移り、再び第２回転状態に戻った場合に、前記した通り、第２姿勢算出手段５２で計算の基準となる姿勢行列Ｐ_ｋ−１を置換する必要がある（Ｙｅｓ）と判断し、その処理を行うものである。具体的には、姿勢行列Ｐ_ｋ−１を、直前のステップ（微小時間Δｔ前のステップ）のステップＳ６において算出した姿勢行列Ｐに置き換える。そして、ステップＳ９で、置き換えた姿勢行列ＰにステップＳ９で新たに算出した回転行列Ｒ_ｋを掛けることにより、最新の姿勢行列Ｐ_ｋを算出する。

また、本例では、ステップＳ５において、被測定体が第１回転状態であるか第２回転状態であるかを、３軸磁気センサ２の検出値に基づいて検出した回転角速度｜ω｜を用いて判断しているが、この判断は、回転状態を判断することができれば、他の方法（他のパラメータ）、具体的には３軸加速度センサ４による加速度ベクトルＡの検出値を利用して判断することもできる。例えば、ある時点ｔ（ｋ）における加速度ベクトルＡ（ｋ）の大きさ｜Ａ（ｋ）｜と、その時点ｔ（ｋ）よりも微小時間Δｔ前における加速度ベクトルＡ（ｋ−１）の大きさ｜Ａ（ｋ−１）｜とを算出し、これらの差｜｜Ａ（ｋ）｜−｜Ａ（ｋ−１）｜｜が所定の閾値未満の場合は第１回転状態と判断し、閾値以上の場合は第２回転状態と判断してもよい。
なお、加速度ベクトルＡ（ｋ）の成分を（ａ_ｘ，ａ_ｙ，ａ_ｚ）とした場合、｜Ａ（ｋ）｜は、下記の式により算出することができる。
｜Ａ（ｋ）｜＝√（ａ_ｘ ^２＋ａ_ｙ ^２＋ａ_ｚ ^２）

回転状態の判断は、検出した磁気ベクトルの検出値を利用した別の方法によっても行うことができる。例えば、磁気ベクトルＨ１とＨ２との差である差分ベクトルｎ１と、磁気ベクトルＨ３とＨ２との差である差分ベクトルｎ２とを算出する。この差分ベクトルｎ１及びｎ２が変化に要する時間Δｔ１及びΔｔ２を算出し、それらを用いて差分の時間変化率ベクトルｖ１及びｖ２を算出する。
ここで得られた差分の時間変化率ベクトルｖ１及びｖ２の外積をＬとおく。

ここでＬは回転軸ベクトルｋと同じ方向を向き、その大きさは回転軸及び回転角度算出に用いた磁気ベクトルの単位時間あたりの変化の割合の自乗に比例する。被測定体の回転速度が速くなるにつれて磁気ベクトルの単位時間あたりの変化の割合は大きくなるので、Ｌが所定の大きさ（例えば５０００（ｍＧ/秒）^２〔ミリガウス毎秒の自乗〕）以上か否かによって、判別することができる。
すなわち、例えばL＜５０００（ｍＧ/秒）^２の場合には、第１回転状態であると判定し、Ｌ≧５０００（ｍＧ/秒）^２の場合には、第２回転状態であると判定する。

あるいは、他の方法として以下の方法もある。
まず、３軸磁気センサ２によって検出した磁気ベクトルデータについて、その変化割合を把握するために、前記した磁気ベクトルデータの取得及びメモリの保存とは別に、検出された磁気ベクトルＨ２と、直前に採取した磁気ベクトルＨ１との差（差分ベクトルｎ１の大きさ）が、所定の大きさ（例えば１００ｍＧ）を超えたときに、次のデータとしてメモリ３に蓄積（採取）する。
そこで、磁気ベクトルのデータの今回の採取時刻ｔ２が、前回の採取時刻ｔ１から、所定時間（例えば５００ｍ秒）以上経過したか否かによって判別することができる。すなわち、例えばｔ２−ｔ１＝Δｔ＜５００ｍ秒の場合には、第２回転状態であると判定し、Δｔ≧５００ｍ秒の場合には、第１回転状態であると判定する。

あるいは、新たに採取した磁気ベクトルＨ２と、前回採取した磁気ベクトルＨ１との差（差分ベクトルｎ１の大きさ）が所定の大きさ以上か否かによって、判別することもできる。すなわち、例えば差分ベクトルｎ１の大きさが所定の大きさ以上の場合には、第２回転状態であると判定し、所定の大きさ未満の場合には、第１回転状態であると判定する。

あるいは、上記２つの条件、すなわち、採取時間の間隔Δｔが、所定時間（例えば５００ｍ秒）未満であり、かつ差分ベクトルｎ１の大きさが所定の大きさ以上である場合にのみ、第２回転状態であると判定し、その他の場合には第１回転状態であると判定することもできる。

本例の作用効果について説明する。本例の磁気式ジャイロ１は、上記数式（１）を用いて、被測定体の回転角速度｜ω｜を算出する。
数式（１）に含まれる磁気ベクトルＨ、ΔＨ、角度αは、軌跡円の中心点や半径を算出しなくても求めることができる。そのため、被測定体の回転角速度｜ω｜を算出するための演算処理を簡単にすることができ、回転角速度を短時間で算出することが可能になる。これにより、アプリケーション等を使用する際に、被測定体の回転運動をアプリケーションに素早く反映させることが可能になり、応答性を向上させることができる。特に、被測定体の動きに対する出力遅れ時間をできるだけ短縮化したいアプリケーションに適用する場合、大きな効果が得られる。

特に、被測定体の回転運動が非常に高速になる可能性がある場合には、極めて微小な時間の間に回転軸、回転速度、回転角速度が変化しつつ運動することになることから、被測定体の動きを精度良くかつ滑らかにアプリケーションに反映できるようにするためには、微小時間Δｔを短くし、回転角速度の演算回数を増やす必要がある。数式（１）を用いて、回転角速度を演算するためのアルゴリズムを簡素化すれば、演算処理装置（ＣＰＵ）の負担を低減でき、回転角速度の算出速度を向上できるため、演算回数が多い場合に特に有効である。

また、本例では、回転軸と回転角速度ベクトルωの絶対値｜ω｜を算出することにより、回転角速度ベクトルωを算出することができ、被測定体の回転方向を容易に認識することができる。

また、図１に示すごとく、本例の磁気式ジャイロ１は回転状態判断手段１３を備える。そして回転角速度出力手段１２は、回転状態判断手段１３によって被測定体の回転運動が第２回転状態であると判断された場合にのみ、回転角速度を出力するよう構成されている。
このようにすると、測定精度がよい場合のみ、回転角速度を出力することができる。すなわち、被測定体の回転運動が低速（静止も含む）である場合（第１回転状態）は、微小時間Δｔ内における磁気ベクトルＨの変化量ΔＨが小さいため、数式（１）を用いて回転角速度｜ω｜を算出すると、誤差が大きくなりやすい。そのため、第１回転状態において回転角速度を出力すると、誤差の大きい回転角速度を出力することになる。
これに対して、被測定体が上記第２回転状態である場合は、閾値を適切に設定することにより、微小時間Δｔ内における磁気ベクトルＨの変化量ΔＨが計算上問題にならない程度に大きい値となるため、回転角速度｜ω｜の誤差が小さくなりやすい。したがって、第２回転状態の場合のみ回転角速度を出力することにより、測定精度の高い場合のみ、回転角速度を出力することが可能になる。なお、回転状態判断手段は、前記した通り、磁気ベクトル及び／又は加速度ベクトルの検出データから得られた、回転角速度や上記パラメータ値を用いて、被測定体が第１回転状態であるか第２回転状態であるかを判断する。

また、図１に示すごとく、本例の磁気式ジャイロ１は、回転角度出力手段１５を備える。
このようにすると、回転角速度｜ω｜を用いることにより、微小時間Δｔ内における、被測定体の回転角度θを算出することが可能となる。回転角度θが求まれば、例えば、地球座標系に対する被測定体の姿勢変化量を算出することができ、この変化量を積算することにより、被測定体の最新姿勢を算出することができる。本例の磁気式ジャイロ１は回転角速度｜ω｜の算出時間が短いため、被測定体の姿勢の算出時間も短くすることができ、被測定体の実際の姿勢変化に対する出力遅れ時間を短縮することができる。

また、図１に示すごとく、本例の磁気式ジャイロ１は、３軸加速度センサ４と、第１姿勢算出手段５１と、第２姿勢算出手段５２とを備える。
このようにすると、磁気式ジャイロ１は第１姿勢算出手段５１と第２姿勢算出手段５２との２つの姿勢算出手段５１，５２を備えるため、これら２つの姿勢算出手段を使い分けることができる。これにより、状況に応じて被測定体の姿勢をより正確に算出することが可能となる。すなわち、第１姿勢算出手段５１は、３軸磁気センサ２と３軸加速度センサ４との２種類のセンサを用いて、被測定体の姿勢を算出する。上記数式（２）〜（５）に示すごとく、この２種類のセンサを用いると、２種類のセンサによって検出したデータの測定時刻における、地球座標系に対する被測定体の姿勢行列を直接、正確に算出することができ、この姿勢行列から、ピッチ角δ、ロール角φ、ヨー角ηを求めることにより、被測定体の姿勢を算出できる。また、３軸加速度センサ４は、被測定体が静止していない場合、その動きによって生じる加速度がセンサの検出値に影響を及ぼすこととなるが、第２姿勢算出手段５２は３軸磁気センサ２のみを用いて姿勢を算出するため、被測定体自身の動きによる加速度の影響を皆無にすることができる。そのため、これら２つの姿勢算出手段５１，５２を使い分けることにより、被測定体の姿勢の算出精度をより高めることが可能になる。

また、本例の磁気式ジャイロ１は、姿勢出力手段５３を備える。
このようにすると、被測定体の、より正確な姿勢を出力することができる。すなわち、第１姿勢算出手段５１では、３軸加速度センサ４と３軸磁気センサ２との２種類のセンサを用いるため、被測定体の姿勢を直接、正確に算出することができる。また、被測定体を速く回転させると遠心力等の被測定体自身の加速度が発生し、加速度センサの検出値に、本来測定したい重力加速度以外の加速度誤差が加わることになるため、姿勢の算出結果に誤差が発生することがあるが、本例の磁気式ジャイロ１は、被測定体が第１回転状態となり、３軸加速度センサ４の検出値に対する被測定体自身の加速度の影響が小さいと判断できる場合のみ、３軸加速度センサ４を利用する第１姿勢算出手段５１の姿勢算出結果を出力する。そのため、被測定体の運動状態に関係なく、３軸加速度センサ４を被測定体の姿勢計算に利用して、正確な姿勢を出力することができる。

また、被測定体が第２回転状態となった場合には、第２姿勢算出手段５２によって算出した微小時間Δｔの間の姿勢変化量を１ステップ毎に算出した姿勢に積算していくことにより、磁気ベクトルのデータのみを用いて算出した姿勢を出力する。このようにすると、被測定体が第２回転状態となり、３軸加速度センサ４が遠心力等の被測定体自体の加速度の影響を受けても、この影響を受けた３軸加速度センサ４のデータを利用しない第２姿勢算出手段５２の姿勢算出結果を出力するため、姿勢の出力結果に誤差が生じにくくなる。

また、本例の磁気式ジャイロ１は、置換手段５６を備える。
このようにすると、第２回転状態において、被測定体の姿勢の算出値に誤差が累積する不具合を防止できる。すなわち、第２姿勢算出手段５２は、姿勢（姿勢行列）の初期値Ｐ_０に、３軸磁気センサ２を用いて算出した、姿勢変化量を表現する回転行列Ｒ_ｋを掛け続けることにより最新の姿勢を算出するため、測定した磁気ベクトルの検出値１つ１つに存在する誤差が姿勢変化量の積算によって累積していくことになる。これに対して第１姿勢算出手段５１は、磁気ベクトルＨのデータと加速度ベクトルＡのデータとを用いて、数式（５）から、２種類のセンサの検出値の測定時刻における被測定体の姿勢を直接、算出できるため、第２姿勢算出手段５２のように誤差が累積されることがない。そのため、第２回転状態から第１回転状態に移った場合に、第１姿勢算出手段５１を用いて正確な姿勢（姿勢行列Ｐ）を算出し、再び第２回転状態に移った際に、第２姿勢算出手段５２の算出において基準となる姿勢行列を、１ステップ前（第２回転状態に移る直前であって、第１回転状態の最後の算出）において、第１姿勢算出手段５１により算出した姿勢行列に置き換え、この姿勢行列にその後の微小時間Δｔ毎に算出した姿勢変化量を意味する回転行列を掛け合わせることにより、最新の姿勢を算出する。これにより、姿勢に誤差が累積することを防止できる。

以上のごとく、本例によれば、被測定体の回転角速度、回転角度、姿勢の算出時間を短縮し、被測定体の実際の動きに対する出力遅れ時間を短縮できる磁気式ジャイロを提供することができる。

（実施例２）
本例は、図６に示すごとく、第１姿勢算出手段５１によって算出した被測定体の姿勢である第１姿勢と、第２姿勢算出手段５２によって算出した被測定体の姿勢である第２姿勢との間の中間姿勢を算出する中間姿勢算出手段５４を設けた例である。中間姿勢は、回転角速度や上記パラメータ値が、回転が遅いことを示す値となるほど第１姿勢に近づき、回転角速度や上記パラメータ値が、回転が速いことを示す値となるほど第２姿勢に近づき、かつ第１姿勢と第２姿勢との間で連続的に変化する。また、本例の磁気式ジャイロ１は、中間姿勢算出手段５４によって算出された中間姿勢を出力する中間姿勢出力手段５５を備える。なお、本例の中間姿勢算出手段５４は、上記中間姿勢の算出に、回転角速度｜ω｜を用いている。

中間姿勢の算出方法について説明する。まず、上記第１姿勢を表す姿勢行列Ｐ_１ｋと、上記第２姿勢を表す姿勢行列Ｐ_２ｋとの間で、以下の数式を満たす回転差分行列Ｑ_ｋを定義する。
Ｐ_２ｋＱ_ｋ＝Ｐ_１ｋ

回転差分行列Ｑ_ｋを上記数式（８）のように表した場合、第２姿勢から第１姿勢への回転軸ベクトルｍ’と、第２姿勢から第１姿勢への回転軸ベクトルｍ’に対する回転角度αとは、下記数式のように表すことができる。

ｍ＝ｍ’／｜ｍ’｜
ｔａｎα＝｜ｍ’｜／（ｑ_１１＋ｑ_２２＋ｑ_３３−１） （但し０°≦α≦１８０°）
α＝ｔａｎ^−１（｜ｍ’｜／（ｑ_１１＋ｑ_２２＋ｑ_３３−１））

次に、第２姿勢と第１姿勢の間の中間姿勢を求めるための回転角度βを定義する。この角度βを定義するにあたって、被測定体の回転角速度｜ω｜の値によって０〜１の間の値をとる単調減少関数ｆ（｜ω｜）を、予め決定しておく。ｆ（｜ω｜）は以下の条件を満たし、かつ連続的に変化する関数である。
ｆ（０）＝１
ｆ（∞）＝０

そして、β＝αｆ（｜ω｜）と定義する。ｆ（｜ω｜）は１〜０の間の値をとる単調減少関数なので、βは図７に示すごとく、回転角速度｜ω｜が増えるほど連続的に０に近づき、回転角速度｜ω｜が減るほど連続的にαに近づく。このように、回転角速度｜ω｜の変化に伴って滑らかに変化する角度βを使って、上記中間姿勢を表す中間姿勢行列Ｐ’_ｋを算出する。
すなわち、回転軸単位ベクトルｍ＝（ｍ_ｘ，ｍ_ｙ，ｍ_ｚ）としたとき、上記第２姿勢を中間姿勢に変換するための回転行列Ｒ’_ｋは、下記数式により表される。

また、下記数式（９）に示すごとく、第２姿勢を表す姿勢行列Ｐ_２ｋに回転行列Ｒ’_ｋを掛けることにより、上記中間姿勢を表す中間姿勢行列Ｐ’_ｋを求めることができる。
Ｐ’_ｋ＝Ｐ_２ｋＲ’_ｋ ・・・（９）
上述したように、角度βは、回転角速度｜ω｜が大きくなるほど連続的に０に近づき、逆に回転角速度｜ω｜が小さくなるほど連続的にαに近づく（図７参照）。そのため、角度βを使って中間姿勢行列Ｐ’_ｋを求めると、この中間姿勢行列Ｐ’_ｋは、回転角速度｜ω｜が大きくなるほど第２姿勢行列Ｐ_２ｋに近づき、回転角速度｜ω｜が小さくなるほど第１姿勢行列Ｐ_１ｋに近づく行列となる。
なお、本例では、関数ｆを回転角速度｜ω｜の関数として表現したが、このパラメータは、回転角速度以外のパラメータであっても、回転状態と関連するパラメータであれば構わない点は前記した通りである。

次に、図８を用いて、本例におけるプログラム６１ｐのフローチャートの説明をする。本例では、ステップＳ１〜ステップＳ４は実施例１と同様である。本例では、回転状態に関係なく、第１姿勢と、第２姿勢の両方を求めるため、回転状態の判断を行わずに算出することも可能であるが、置換手段５６を全く実施することなく第２姿勢の算出を継続すると、誤差の蓄積が大きくなってしまうため、置換手段５６を適切な時期に行うために、回転状態の判断を実施例と１と同様に行う。具体的に説明すると、ステップＳ１０にてステップＳ５と同様に回転状態を判断し、その判断結果を保存しておく。そして、ステップＳ１１に移り、一つ前のステップ（時刻が微小時間Δｔ前の計算）で保存した回転状態の判断情報を保存場所から取り出し、その結果が、第１回転状態であった場合には、ステップＳ１２で第２姿勢算出手段５２の計算の基礎となる姿勢データの置換処理を前記した説明通りに実施し、ステップＳ１３に移る。一方、ステップＳ１１において、一つ前のステップが、第２回転状態であった場合には、置換処理をすることなくステップＳ１３に移る。そして、ステップＳ１３にて第１姿勢と第２姿勢の両方の姿勢値を算出する。また、ステップＳ１３を実行した後、ステップＳ１４を実行する。ステップＳ１４では、第１姿勢と第２姿勢との間の中間姿勢を表す中間姿勢行列Ｐ’_ｋを、上記数式（９）を使って求め、実施例１と同様にピッチ角δ、ロール角φ、ヨー角ηを求める。その後、ステップＳ１５において、算出した姿勢におけるピッチ角δ、ロール角φ、ヨー角ηを出力する。
その他、実施例１と同様の構成を備える。

本例の作用効果について説明する。本例の磁気式ジャイロ１は、第１姿勢と第２姿勢との間の中間姿勢を出力できるため、第１回転状態から第２回転状態に切り替わった場合、又は第２回転状態から第１回転状態に切り替わった場合に、姿勢の出力値が不連続に変化する可能性があることを防止することができる。また、中間姿勢を出力しない磁気式ジャイロの場合、明らかに第１姿勢算出手段５１が適している回転状態であるかそれに近い状態と、明らかに第２姿勢算出手段５２による算出が適していると考えられる高速回転状態では、算出誤差は比較的小さいと考えられるが、中間の回転速度領域で算出精度に問題が生じる可能性があった。本例は、その問題を解決し、閾値の適切な設定によって、中間速度領域での算出精度を向上できる効果を有する。
その他、実施例１と同様の作用効果を有する。

（実施例３）
本例は、回転角速度｜ω｜が所定の範囲内にある場合のみ、中間姿勢を出力するようにした例である。本例では図９に示すごとく、回転角速度｜ω｜に、第１閾値Ｋ_１と第２閾値Ｋ_２との２つの閾値を設定した。そして、回転角速度｜ω｜が０〜第１閾値Ｋ_１の範囲にある場合は、第１姿勢算出手段５１によって算出した第１姿勢を出力し、回転角速度｜ω｜が第１閾値Ｋ_１〜第２閾値Ｋ_２の範囲にある場合は、中間姿勢算出手段５４によって算出した中間姿勢を出力するようにした。さらに、回転角速度｜ω｜が第２閾値Ｋ_２以上の場合は、第２姿勢算出手段５２によって算出した第２姿勢を出力するようにした。
その他、実施例２と同様の構成を備える。

本例の作用効果について説明する。本例では、（１）式で｜ω｜を計算することにより計算時間の短縮を図っているが、中間姿勢を求める場合には、求めない場合と比較して計算量が増加する。一方で、ある程度以下に遅い回転状態である場合と、ある程度以上に速い回転状態である場合には、中間姿勢を求めなくても、前者は第１姿勢を、後者は第２姿勢をそのまま出力すれば問題ないと考えられる回転状態の範囲が存在すると考えられる。そこで、無条件に中間姿勢を求める構成とするのではなく、中間の速度領域のみ、中間姿勢を求めることにより、計算による負担をできるだけ小さく抑えつつ、全ての速度領域で、精度の高い姿勢計算をすることを可能にするのが実施例３の特徴である。
その他、実施例２と同様の作用効果を有する。

（実施例４）
本例は、中間姿勢を出力する時間を変更した例である。図１０に示すごとく、本例では、回転角速度｜ω｜に１つの閾値Ｋを設定した。そして、回転角速度｜ω｜が閾値Ｋ未満の場合は、第１姿勢算出手段５１によって算出した第１姿勢を出力し、閾値Ｋ以上の時は、第２姿勢算出手段５２によって算出した第２姿勢を出力するようにした。また、回転角速度｜ω｜が次第に速くなって閾値Ｋを超えた後の所定時間ｗ１内と、回転角速度｜ω｜が次第に遅くなって閾値Ｋよりも遅くなった後の所定時間ｗ２内は、中間姿勢を出力するようにした。
すなわち、閾値Ｋを超えたり遅くなった直後においては、実施例３で言う中間速度領域である確率が高くなると考えられる。したがって、計算量が多くなる中間姿勢の計算を、この時間内に限定して行うことにより、計算量の増加を最低限に抑えることができ、出力の時間遅れを短縮することができる。
その他、実施例３と同様の構成および作用効果を有する。

１ 磁気式ジャイロ
１０ 回転軸決定手段
１１ 回転角速度算出手段
１２ 回転角速度出力手段
２ ３軸磁気センサ
３ メモリ
４ ３軸加速度センサ
ω 回転角速度ベクトル
Ｈ 磁気ベクトル
α 回転軸と磁気ベクトルＨのなす角度

Claims (7)

1. 被測定体に固定された３軸直交座標系における磁気ベクトルとして地磁気を検出する３軸磁気センサと、
   該３軸磁気センサによって時系列的に検出される上記磁気ベクトルのデータを検出時の時刻データと共に蓄積するメモリと、
   該メモリに蓄積された上記磁気ベクトルのデータに基づいて、上記被測定体の回転運動の基準とする回転軸を決定する回転軸決定手段と、
   上記メモリに蓄積された上記磁気ベクトルのデータと上記時刻データとに基づいて、上記回転運動の回転角速度を算出する回転角速度算出手段と、
   該回転角速度算出手段によって算出した上記回転角速度を出力する回転角速度出力手段とを有し、
   上記回転角速度算出手段は、上記３軸磁気センサによって検出した上記磁気ベクトルＨと、該磁気ベクトルＨから、該磁気ベクトルＨよりも微小時間Δｔ前の時点において測定した磁気ベクトルＨ’を引いたベクトルΔＨ（＝Ｈ−Ｈ’）と、上記回転軸と上記磁気ベクトルＨとの間の角度αを、下記数式
   ｜ω｜＝｜Ｈ×ΔＨ｜／（｜Ｈ｜^２・ｓｉｎα・Δｔ）
   に代入することにより、上記被測定体の回転角速度ベクトルωの絶対値｜ω｜を、上記回転角速度として算出するよう構成されていることを特徴とする磁気式ジャイロ。
2. 請求項１に記載の磁気式ジャイロにおいて、上記３軸直交座標系における加速度ベクトルとして重力加速度を検出する３軸加速度センサと、該３軸加速度センサによって時系列的に検出される上記加速度ベクトルのデータを検出時の時刻データと共に蓄積する加速度ベクトルデータ蓄積用メモリと、上記磁気ベクトルのデータおよび上記加速度ベクトルのデータの少なくとも一方に基づいて、上記被測定体の回転角速度又は該回転角速度と関連のあるパラメータ値が、基準となる閾値未満である第１回転状態か、又は上記閾値以上である第２回転状態であるかを判断する回転状態判断手段を備え、上記回転角速度出力手段は、上記回転状態判断手段によって、上記被測定体が上記第２回転状態であると判断された場合にのみ、上記回転角速度を出力する磁気式ジャイロ。
3. 請求項１または請求項２に記載の磁気式ジャイロにおいて、上記回転角速度算出手段によって算出した上記回転角速度に基づいて、上記微小時間Δｔ内における、上記回転軸を中心とした上記被測定体の回転角度を算出する回転角度算出手段と、算出された上記回転角度を出力する回転角度出力手段とを備えることを特徴とする磁気式ジャイロ。
4. 請求項２に記載の磁気式ジャイロにおいて、上記回転角速度算出手段によって算出した上記回転角速度に基づいて、上記微小時間Δｔ内における、上記回転軸を中心とした上記被測定体の回転角度を算出する回転角度算出手段と、算出された上記回転角度を出力する回転角度出力手段とを備えると共に、上記磁気ベクトルのデータと上記加速度ベクトルのデータとに基づいて、上記被測定体の姿勢を算出する第１姿勢算出手段と、上記回転角度算出手段によって算出された上記回転角度に基づいて、上記微小時間Δｔ内における上記被測定体の姿勢変化量を算出し、該姿勢変化量を、上記第１回転状態において上記第１姿勢算出手段が算出した上記姿勢の初期値に積算することにより、上記被測定体の最新の姿勢を算出する第２姿勢算出手段とを備えることを特徴とする磁気式ジャイロ。
5. 請求項４に記載の磁気式ジャイロにおいて、上記回転状態判断手段によって、上記被測定体の回転運動が上記第１回転状態であると判断された場合は、上記第１姿勢算出手段によって算出した上記姿勢を出力し、上記回転運動が上記第２回転状態であると判断された場合は、上記第２姿勢算出手段によって算出した上記姿勢を出力する姿勢出力手段を備えることを特徴とする磁気式ジャイロ。
6. 請求項４に記載の磁気式ジャイロにおいて、上記第１姿勢算出手段によって算出した上記被測定体の上記姿勢である第１姿勢と、上記第２姿勢算出手段によって算出した上記被測定体の上記姿勢である第２姿勢との間の中間姿勢を算出する中間姿勢算出手段を備え、
   上記中間姿勢は、上記回転角速度がより遅い回転状態になるほど上記第１姿勢に近づき、上記回転角速度がより速い回転状態になるほど上記第２姿勢に近づき、かつ上記第１姿勢と上記第２姿勢との間で連続的に変化し、
   上記中間姿勢算出手段によって算出された上記中間姿勢を出力する中間姿勢出力手段を備え、
   上記中間姿勢算出手段は、上記回転状態判断手段において用いることが可能な上記パラメータ値を用いて、上記回転状態を定めていることを特徴とする磁気式ジャイロ。
7. 請求項４〜請求項６のいずれか１項に記載の磁気式ジャイロにおいて、上記第２姿勢算出手段を用いて上記姿勢を算出した後、上記被測定体の回転運動が上記第１回転状態となり、再び上記第２回転状態に戻った場合に、上記第２姿勢算出手段によって算出する際の基準となる上記姿勢のデータを、上記第１姿勢算出手段によって算出した、上記第２回転状態に戻る直前に第１姿勢算出手段により求めた上記姿勢のデータと置き換える置換手段を備え、上記第２姿勢算出手段は、置き換え後の基準となる姿勢データに第２姿勢算出手段により求めた姿勢変化量を加算していくことにより、上記姿勢の算出を行うよう構成されていることを特徴とする磁気式ジャイロ。

Images