センサー装置、電子機器及び角速度センサーのオフセット補正方法
【課題】補正タイミングを自己判定して角速度センサーのオフセット補正を行うことができるセンサー装置、電子機器及び角速度センサーのオフセット補正方法を提供すること。
【解決手段】初期状態検出部210は、加速度センサーの出力に基づいて加速度を算出し、算出した加速度と重力加速度が略一致する状態を初期状態として検出する。角速度算出部220は、初期状態における姿勢情報に基づいて初期状態における各検出軸方向の軸ベクトルを算出し、当該軸ベクトルと各検出軸方向の加速度成分と当該加速度成分の時間変化とに基づいて任意の軸回りに回転する角速度を算出する。角速度判定部230は、角速度算出部220が算出した角速度が略0である場合に、角速度センサーに加わる角速度が0であると判定する。オフセット補正部40は、角速度センサーに加わる角速度が0であると判定された時に、角速度センサーの出力のオフセットを補正する。
【解決手段】初期状態検出部210は、加速度センサーの出力に基づいて加速度を算出し、算出した加速度と重力加速度が略一致する状態を初期状態として検出する。角速度算出部220は、初期状態における姿勢情報に基づいて初期状態における各検出軸方向の軸ベクトルを算出し、当該軸ベクトルと各検出軸方向の加速度成分と当該加速度成分の時間変化とに基づいて任意の軸回りに回転する角速度を算出する。角速度判定部230は、角速度算出部220が算出した角速度が略0である場合に、角速度センサーに加わる角速度が0であると判定する。オフセット補正部40は、角速度センサーに加わる角速度が0であると判定された時に、角速度センサーの出力のオフセットを補正する。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、センサー装置、電子機器及び角速度センサーのオフセット補正方法に関する。
【背景技術】
【0002】
近年、角速度を検出するセンサーモジュールや姿勢角を検知する姿勢角検知ユニットを利用した様々なシステムや電子機器が利用されている。これらのシステムや電子機器では、ジャイロセンサー(角速度センサー)のバイアスオフセット(ゼロ点オフセット)を排除しなければ静止時に静止していることを正しく検出することができないため、性能を向上させる上で、このバイアスオフセットを如何にして排除するかが最も重要な課題になっている。
【0003】
従来より、製品の製造時等にバイアスオフセットを計測し、製品に組み込まれた不揮発性の記憶素子等に補正データを書き込むことでバイアスオフセットを補正する手法や、製品の使用時に静止状態を検出してオフセットのキャリブレーションを行う手法が実現されている。前者の手法では、バイアスオフセットの初期ばらつきを補正することができるが、温度変動等によるユーザー使用時のバイアス変動に対応することができない。一方、後者の手法では、ユーザーの使用環境において生じているバイアスのオフセットを使用環境においてキャリブレーション補正することができる。そのため、製造時の補正からの変動分や、線形関数で補正しきれない温度変動分を補正することができる。
【先行技術文献】
【特許文献】
【0004】
【特許文献1】特開2010−019751号公報
【特許文献2】特許4314566号公報
【特許文献3】特許2857091号公報
【発明の概要】
【発明が解決しようとする課題】
【0005】
しかしながら、使用環境下でバイアスオフセットのキャリブレーションを行う手法は、いずれもアプリケーション機能に連動して行われるものであった。例えば、特許文献1の手法ではプレイヤが弓を引く瞬間、特許文献2の手法では掃除機の充電時や方向転換時、特許文献3の手法ではナビゲーション装置の起動時にキャリブレーションを行うなど、アプリケーション側でキャリブレーションを行うタイミングを決定している。つまり、アプリケーションに組み込まれたキャリブレーション補正アルゴリズムとして実現されており、センサーユニット単体ではキャリブレーションのタイミングを決定する機能を有するものではなかった。そのため、アプリケーションの開発者がキャリブレーション補正の適切なタイミングを熟慮し、補正アルゴリズムを作成する手間が発生していた。
【0006】
本発明は、以上のような問題点に鑑みてなされたものであり、本発明のいくつかの態様によれば、補正タイミングを自己判定して角速度センサーのオフセット補正を行うことができるセンサー装置、電子機器及び角速度センサーのオフセット補正方法を提供することができる。
【課題を解決するための手段】
【0007】
(1)本発明は、角速度センサーと、3つ以上の加速度センサーと、を含むセンサー装置であって、前記加速度センサーの出力に基づいて、前記角速度センサーに加わる角速度が0であるか否かを判定する判定部と、前記判定部により前記角速度が0であると判定された時に、前記角速度センサーの出力のオフセットを補正するオフセット補正部と、を含む、センサー装置である。
【0008】
センサー装置が停止していれば角速度センサーに角速度が加わらないので、この時の角速度センサーの出力電圧が0点電圧になる。そのため、このタイミングであれば0点電圧のオフセットを適切に補正することができる。本発明によれば、加速度センサーの出力に基づいて角速度センサーに角速度が加わらないタイミングを検出することができるので、そのタイミングで自動的に角速度センサーのオフセットを補正することができる。すなわち、本発明によれば、アプリケーション機能によらずセンサー装置単体で角速度センサーのオフセットを補正することができる。そのため、このセンサー装置を使用すれば、従来の手法のようにアプリケーション側に補正アルゴリズムを実装する必要がなく、ユーザーに負荷をかけずに角速度センサーのゼロ点オフセットを補正することができる。
【0009】
また、本発明によれば、使用環境下で角速度センサーのゼロ点の変動を検知して補正することができる。
【0010】
(2)このセンサー装置において、前記判定部は、前記加速度センサーの各々の検出軸方向の加速度成分の時間変化に基づいて、前記角速度センサーに加わる角速度が0であるか否かを判定するようにしてもよい。
【0011】
(3)このセンサー装置において、前記判定部は、前記加速度センサーの出力に基づいて当該センサー装置に加わる加速度を算出し、算出した加速度と重力加速度が略一致する状態を初期状態として検出する初期状態検出部と、前記初期状態における当該センサー装置の姿勢情報に基づいて前記初期状態における各検出軸方向の軸ベクトルを算出し、当該軸ベクトルと各検出軸方向の加速度成分と当該加速度成分の時間変化とに基づいて当該センサー装置が任意の軸回りに回転する角速度を算出する角速度算出部と、前記角速度算出部が算出した角速度が略0である場合に、前記角速度センサーに加わる角速度が0であると判定する角速度判定部と、を含むようにしてもよい。
【0012】
このセンサー装置によれば、加速度センサーが検知できないような小さい遠心力が加わった場合でも、初期状態からの加速度成分の時間変化をモニターすることで、角速度センサーに加わる角速度を検知することができる。そのため、角速度センサーのオフセット補正を行うタイミングの判定精度を高めることができる。
【0013】
(4)このセンサー装置において、前記角速度判定部は、前記角速度算出部が算出した角速度が略0の状態が所定時間継続した場合に、前記角速度センサーに加わる角速度が0であると判定するようにしてもよい。
【0014】
このようにすれば、センサー装置に所定時間継続して角速度が加わっていないのか瞬時的に角速度が0となっただけなのかを区別し、前者の場合だけ角速度センサーのオフセット補正を行うようにすることができる。
【0015】
(5)このセンサー装置において、前記判定部は、前記加速度センサーの出力に基づいて当該センサー装置に加わる加速度を算出する加速度算出部と、前記加速度算出部が算出した加速度と重力加速度が略一致する場合に、前記角速度センサーに加わる角速度が0であると判定する角速度判定部と、を含むようにしてもよい。
【0016】
センサー装置が回転状態にあると、角速度センサーは角速度を検出するので、そのタイミングでは適切なオフセット補正を行うことができない。一方、センサー装置に遠心力が働けば、加速度センサーが遠心力により発生する遠心加速度を検出するため、その検出加速度は重力加速度と一致しない。そのため、このセンサー装置によれば、加速度センサーの検出加速度が重力加速度と一致するか否かを判定する簡便な演算処理により、遠心力が加わっていない適切なタイミングで角速度センサーのオフセットを補正することができる。
【0017】
(6)このセンサー装置において、前記角速度判定部は、前記加速度算出部が算出した加速度と重力加速度が略一致する状態が所定時間継続した場合に、前記角速度センサーに加わる角速度が0であると判定するようにしてもよい。
【0018】
このようにすれば、センサー装置に所定時間継続して遠心力が加わっていないのか瞬時的に遠心力が0となっただけなのかを区別し、前者の場合だけ角速度センサーのオフセット補正を行うようにすることができる。
【0019】
(7)このセンサー装置において、前記判定部は、前記加速度センサーの出力に基づいて当該センサー装置に加わる加速度を算出し、算出した加速度と重力加速度が略一致する状態を初期状態として検出する初期状態検出部と、前記初期状態における当該センサー装置の姿勢情報に基づいて前記初期状態における各検出軸方向の軸ベクトルを算出し、当該軸ベクトルと各検出軸方向の加速度成分と当該加速度成分の時間変化とに基づいて当該センサー装置が任意の軸回りに回転する角速度を算出する角速度算出部と、制御信号に応じて第1の判定モードと第2の判定モードを切り替える判定モード制御部と、前記第1の判定モードでは、前記初期状態検出部が前記初期状態を検出した場合に前記角速度センサーに加わる角速度が0であると判定し、前記第2の判定モードでは、前記角速度算出部が算出した角速度が略0である場合に前記角速度センサーに加わる角速度が0であると判定する角速度判定部と、を含むようにしてもよい。
【0020】
このようにすれば、このセンサー装置を使用するアプリケーションに応じて、第1の判定モードと第2の判定モードのいずれを使用するかで、角速度センサーのオフセット補正タイミングの判定精度を変更することができる。すなわち、第1の判定モードにすれば、遠心力の検出による簡易な判定になり、第2の判定モードにすれば、加速度センサーで検出できないような微小な遠心力が加わった場合でもセンサー装置に加わる角速度が0か否かを判定することができる。
【0021】
(8)このセンサー装置において、前記角速度判定部は、前記第1の判定モードでは、前記初期状態検出部が所定時間継続して前記初期状態を検出した場合に前記角速度センサーに加わる角速度が0であると判定し、前記第2の判定モードでは、前記角速度算出部が算出した角速度が略0の状態が所定時間継続した場合に前記角速度センサーに加わる角速度が0であると判定するようにしてもよい。
【0022】
このようにすれば、第1の判定モードでは、センサー装置に所定時間継続して遠心力が加わっていないのか瞬時的に遠心力が0となっただけなのかを区別し、前者の場合だけ角速度センサーのオフセット補正を行うようにすることができる。また、第2の判定モードでは、センサー装置に所定時間継続して角速度が加わっていないのか瞬時的に角速度が0となっただけなのかを区別し、前者の場合だけ角速度センサーのオフセット補正を行うようにすることができる。
【0023】
(9)本発明は、上記のいずれかのセンサー装置を含む、電子機器である。
【0024】
(10)本発明は、角速度センサーと、3つ以上の加速度センサーと、を含むセンサー装置において、前記角速度センサーの出力のオフセットを補正する角速度センサーのオフセット補正方法であって、前記加速度センサーの出力に基づいて、前記角速度センサーに加わる角速度が0であるか否かを判定する判定ステップと、前記判定ステップで前記角速度が0であると判定された時に、前記角速度センサーの出力のオフセットを補正するオフセット補正ステップと、を含む、角速度センサーのオフセット補正方法である。
【0025】
(11)この角速度センサーのオフセット補正方法において、前記判定ステップは、前記加速度センサーの出力に基づいて前記センサー装置に加わる加速度を算出し、算出した加速度と重力加速度が略一致する状態を初期状態として検出する初期状態検出ステップと、前記初期状態における前記センサー装置の姿勢情報に基づいて前記初期状態における各検出軸方向の軸ベクトルを算出し、当該軸ベクトルと各検出軸方向の加速度成分と当該加速度成分の時間変化とに基づいて前記センサー装置が任意の軸回りに回転する角速度を算出する角速度算出ステップと、前記角速度算出ステップで算出した角速度が略0である場合に、前記角速度センサーに加わる角速度が0であると判定する角速度判定ステップと、を含むようにしてもよい。
【図面の簡単な説明】
【0026】
【図1】第1実施形態のセンサー装置の概略構成図。
【図2】第1実施形態のオフセット補正処理を行うタイミングの判定原理について説明するための図。
【図3】第1実施形態のオフセット補正処理の第1の例を示すフローチャート図。
【図4】第1実施形態のオフセット補正処理の第2の例を示すフローチャート図。
【図5】第2実施形態のセンサー装置の概略構成図。
【図6】Z−X−Z系のオイラー角について説明するための図。
【図7】第2実施形態のオフセット補正処理の第1の例を示すフローチャート図。
【図8】第2実施形態のオフセット補正処理の第2の例を示すフローチャート図。
【図9】第3実施形態のセンサー装置の概略構成図。
【図10】第3実施形態のオフセット補正処理の第1の例を示すフローチャート図。
【図11】第3実施形態のオフセット補正処理の第2の例を示すフローチャート図。
【図12】本実施形態の電子機器の構成例を示す機能ブロック図。
【発明を実施するための形態】
【0027】
以下、本発明の好適な実施形態について図面を用いて詳細に説明する。なお、以下に説明する実施の形態は、特許請求の範囲に記載された本発明の内容を不当に限定するものではない。また以下で説明される構成の全てが本発明の必須構成要件であるとは限らない。
【0028】
1.センサー装置
(1)第1実施形態
[構成]
図1は、第1実施形態のセンサー装置の概略構成図である。
【0029】
図1に示すように、第1実施形態のセンサー装置1Aは、3つの加速度センサー10x、10y、10zを含む。加速度センサー10x、10y、10zは、検出軸が互いに直交するように固定して配置されており、それぞれの検出軸方向に加わる加速度の大きさに応じたDC電圧の検出信号を出力する。
【0030】
また、センサー装置1Aは、3つの角速度センサー(ジャイロセンサー)20x、20y、20zを含む。角速度センサー20x、20y、20zは、検出軸が互いに直交するように固定して配置されており、それぞれの検出軸回りに加わる角速度の大きさに応じたDC電圧の検出信号を出力する。
【0031】
本実施形態では、加速度センサー10xと角速度センサー20xは互いの検出軸が一致するように配置されており、この検出軸をx軸とする。同様に、加速度センサー10yと角速度センサー20yは互いの検出軸が一致するように配置されており、この検出軸をy軸とする。同様に、加速度センサー10zと角速度センサー20zは互いの検出軸が一致するように配置されており、この検出軸をz軸とする。
【0032】
センサー装置1Aは、判定部30を含む。判定部30は、加速度センサー10x、10y、10zの出力に基づいて、角速度センサー20x、20y、20zに加わる角速度が0であるか否かを判定する。
【0033】
本実施形態では、判定部30は、加速度算出部110を含む。加速度算出部110は、加速度センサー10x、10y、10zの出力に基づいてセンサー装置1Aに加わる加速度を算出する。本実施形態では、加速度算出部110は、加速度センサー10x、10y、10zの各出力電圧を所定の周期でサンプリングし、A/D変換して得られるx軸、y軸、z軸方向の加速度成分gx、gy、gzの2乗和gx2+gy2+gz2を計算する。
【0034】
また、本実施形態では、判定部30は、角速度判定部120を含む。角速度判定部120は、加速度算出部110が算出した加速度と重力加速度Gが略一致する場合に、角速度センサー20x、20y、20zに加わる角速度が0であると判定する。本実施形態では、角速度判定部120は、gx2+gy2+gz2=G2であれば、角速度センサー20x、20y、20zに加わる角速度が0であると判定する。
【0035】
例えば、角速度判定部120は、加速度算出部110が算出した加速度と重力加速度Gが略一致する状態が所定時間継続した場合に、角速度センサー20x、20y、20zに加わる角速度が0であると判定するようにしてもよい。
【0036】
センサー装置1Aは、オフセット補正部40を含む。オフセット補正部40は、判定部30により角速度センサー20x、20y、20zに加わる角速度が0であると判定された時に、角速度センサー20x、20y、20zの出力のオフセット(ゼロ点オフセット)を補正する。本実施形態では、角速度センサー20x、20y、20zはゼロ点電圧の調整機能を有しており、オフセット補正部40は、オフセット補正処理のタイミングにおける角速度センサー20x、20y、20zの出力電圧と仕様上のゼロ点電圧との誤差に応じたゼロ点調整信号を角速度センサー20x、20y、20zに供給することで、これらの出力電圧を仕様上のゼロ点電圧と一致させる。
【0037】
このように、本実施形態のセンサー装置1Aは、加速度センサー10x、10y、10zの出力電圧を周期的にサンプリングして順次得られる加速度の2乗和(gx2+gy2+gz2)と重力加速度の2乗(G2)が一致したタイミングで、角速度センサー20x、20y、20zの出力のオフセット(ゼロ点オフセット)を自動的に補正する。
【0038】
[オフセット補正タイミングの判定原理]
次に、本実施形態における、角速度センサー20x、20y、20zのオフセット補正処理を行うタイミングの判定原理について、図2(A)及び図2(B)を用いて説明する。
【0039】
センサー装置1Aを地球上で使用する場合、センサー装置1Aには常に重力が加わる。そのため、センサー装置1Aが停止もしくは等速直線運動をしていれば、図2(A)に示すように、加速度センサー10x、10y、10zは重力加速度のみを検出する。図2(A)において、gx、gy、gzはx軸、y軸、z軸方向の加速度成分を、Gは重力加速度をそれぞれ表している。すなわち、gx、gy、gzのベクトル和はGと一致し、次の式(1)が成立する。
【0040】
【数1】
【0041】
一方、センサー装置1Aが任意の軸回りの回転運動をすれば遠心力が加わるため、図2(B)に示すように、加速度センサー10x、10y、10zは重力加速度と遠心力による遠心加速度とを検出する。図2(B)において、gx、gy、gzはx軸、y軸、z軸方向の加速度成分を、Gは重力加速度を、Rは遠心加速度を、nは回転軸をそれぞれ表している。すなわち、gx、gy、gzのベクトル和はGとRのベクトル和と一致し、次の式(2)が成立する。
【0042】
【数2】
【0043】
ここで、センサー装置1Aが停止もしくは等速直線運動をしていれば(回転運動をしていなければ)、センサー装置1Aには角速度が加わらないので、角速度センサー20x、20y、20zはすべてゼロ点電圧を出力する。そのため、式(1)が成立する時は角速度センサー20x、20y、20zのオフセット補正(ゼロ点補正)を行うことができる。
【0044】
一方、センサー装置1Aが任意の軸回りの回転運動をしていれば、センサー装置1Aにはこの回転運動の角速度が加わり、角速度センサー20x、20y、20zは、この角速度のx軸成分、y軸成分、z軸成分を検出することになる。そのため、角速度センサー20x、20y、20zの少なくとも1つの出力電圧はゼロ点電圧にならないので、式(2)が成立する(式(1)が成立しない)時はオフセット補正(ゼロ点補正)を行うことができない。
【0045】
そこで、角速度判定部120は、式(1)が成立するか否かによってセンサー装置1Aに加わる角速度が0か否かを判定し、角速度センサー20x、20y、20zのオフセット補正処理のタイミングを制御する。ただし、実際には、加速度センサー10x、10y、10zの出力オフセットやセンサー装置1Aの使用環境におけるノイズ等の環境因により、センサー装置1Aが停止若しくは等速運動をしていても加速度センサー10x、10y、10zによって検出される加速度gが重力加速度Gと微妙に一致しないことが想定される。そのため、角速度判定部120は、例えば、gx2+gy2+gz2とG2の差が微小値Δ以下であれば、すなわちG2−Δ≦gx2+gy2+gz2≦G2+Δを満たせば、センサー装置1Aに加わる角速度が0であると判定するようにしてもよい。この微小値Δは、センサー装置1Aを使用するアプリケーション側の仕様等によって適切な値が設定される。
【0046】
[オフセット補正処理]
図3は、本実施形態における、角速度センサー20x、20y、20zのオフセット補正処理の第1の例を示すフローチャート図である。
【0047】
まず、加速度算出部110が、加速度センサー10x、10y、10zの検出信号をサンプリングし、x軸、y軸、z軸方向の加速度成分gx,gy,gzを計算する(ステップS10)。
【0048】
次に、加速度算出部110がgx2+gy2+gz2を計算する(ステップS20)。
【0049】
次に、角速度判定部120がgx2+gy2+gz2とG2が一致するか否かを判定し(ステップS30)、gx2+gy2+gz2≠G2の場合は(ステップS30のN)、ステップS10とS20の処理を再度行う。
【0050】
一方、gx2+gy2+gz2=G2の場合は(ステップS30のY)、オフセット補正部40が角速度センサー20x、20y、20zのオフセット補正を行う(ステップS40)。
【0051】
図4は、本実施形態における、角速度センサー20x、20y、20zのオフセット補正処理の第2の例を示すフローチャート図である。図4において、図3と同じ処理を行うステップには同じ符号を付しており、その説明を省略又は簡略する。
【0052】
まず、角速度判定部120が所定時間T1の計測を開始する(ステップS2)。
【0053】
次に、加速度算出部110がステップS10とS20の処理を行った後、角速度判定部120がgx2+gy2+gz2とG2が一致するか否かを判定し(ステップS30)、gx2+gy2+gz2≠G2の場合は(ステップS30のN)、角速度判定部120が所定時間T1の計測を再度開始し(ステップS2)、加速度算出部110がステップS10とS20の処理を再度行う。
【0054】
一方、gx2+gy2+gz2=G2の場合は(ステップS30のY)、角速度判定部120が、ステップS2で計測を開始してから所定時間T1が経過したか否かを判定し(ステップS32)、所定時間T1が経過していない場合は(ステップS32のN)、所定時間T1が経過するまで、ステップS10、S20、S30の処理を繰り返し行う。ただし、所定時間T1が経過する前に、ステップS30の判定処理でgx2+gy2+gz2≠G2となった場合は、角速度判定部120が所定時間T1の計測を再度開始する。
【0055】
所定時間T1が経過した場合は(ステップS32のY)、オフセット補正部40が角速度センサー20x、20y、20zのオフセット補正を行う(ステップS40)。
【0056】
このようにすれば、センサー装置1Aに所定時間T1以上継続して遠心力が加わっていないのか瞬時的に遠心力が0となっただけなのかを区別し、前者の場合だけ角速度センサー20x、20y、20zのオフセット補正を行うようにすることができる。
【0057】
以上に説明した第1実施形態のセンサー装置によれば、加速度センサーが検出する加速度が重力加速度と一致するか否かを判定する簡便な演算処理により、遠心力が加わっていない適切なタイミングで角速度センサーのオフセットを補正することができる。すなわち、第1実施形態のセンサー装置によれば、アプリケーション機能によらずセンサー装置単体で角速度センサーのオフセットを補正することができる。そのため、このセンサー装置を使用すれば、従来の手法のようにアプリケーション側に補正アルゴリズムを実装する必要がなく、ユーザーに負荷をかけずに角速度センサーのゼロ点オフセットを補正することができる。
【0058】
また、第1実施形態のセンサー装置によれば、使用環境下で角速度センサーのゼロ点の変動を検知して補正することができる。そのため、場合によっては、出荷前に角速度センサーのオフセット調整をs実施したり、温度特性の補正用データを記憶素子に書き込む必要がなくなる。さらに、使用環境に応じて角速度センサーのオフセット補正量を調整することができるので、温度変化にも対応して角速度センサーのオフセット補正を行うことができる。特に、温度特性の補正用データがあったととしても補正が困難な非線形の温度変化にも対応することができる。
【0059】
(2)第2実施形態
[構成]
図5は、第2実施形態のセンサー装置の概略構成図である。図5において、図1と同じ構成要素には同じ符号を付しており、その説明を省略又は簡略する。
【0060】
図5に示すように、第2実施形態のセンサー装置1Bは、3つの加速度センサー10x、10y、10z、3つの角速度センサー(ジャイロセンサー)20x、20y、20zに加えて、3つの地磁気センサー60x、60y、60zを含んで構成されている。地磁気センサー60x、60y、60zは、検出軸(x軸、y軸、z軸)が互いに直交するように固定して配置されており、それぞれの検出軸方向に加わる地磁気の大きさに応じたDC電圧の検出信号を出力する。
【0061】
本実施形態では、加速度センサー10x、角速度センサー20x、地磁気センサー60xは互いの検出軸が一致するように配置されており、この検出軸をx軸とする。同様に、加速度センサー10y、角速度センサー20y、地磁気センサー60yは互いの検出軸が一致するように配置されており、この検出軸をy軸とする。同様に、加速度センサー10z、角速度センサー20z、地磁気センサー60zは互いの検出軸が一致するように配置されており、この検出軸をz軸とする。
【0062】
本実施形態では、このx軸、y軸、z軸を3つの座標軸とする直交座標系をセンサー座標系と呼ぶことにする。一方、空間上に固定された3つの直交する座標軸(X軸、Y軸、Z軸)による直交座標系を絶対座標系と呼ぶことにする。絶対座標系のX軸、Y軸、Z軸は、例えば、X軸の正の方向が北、Y軸の正の方向が西、Z軸の正の方向が重力方向と一致するように定義される。
【0063】
センサー装置1Bは、姿勢情報検出部70を含んで構成されている。姿勢情報検出部70は、加速度センサー10x、10y、10z、角速度センサー20x、20y、20z、地磁気センサー60x、60y、60zの各検出信号に基づいて、センサー装置1Bの現在の姿勢情報を算出する。
【0064】
本実施形態では、絶対座標系(XYZ座標系)とセンサー座標系(xyz座標系)が一致する状態を基準としてセンサー装置1Bの姿勢をZ−X−Z系のオイラー角で表現する。Z−X−Z系のオイラー角は、図6(A)に示すように、絶対座標系(XYZ座標系)をZ軸回りに角度φだけ回転させたX’Y’Z’座標系に変換し、次にX’Y’Z’座標系をX’軸回りに角度θだけ回転させたX’’Y’’Z’’座標系に変換し、最後にX’’Y’’Z’’座標系をZ’’軸回りに角度ψだけ回転させたX’’’Y’’’Z’’’座標系に変換した時、X’’’Y’’’Z’’’座標系がセンサー座標系(xyz座標系)と一致する時の(φ,θ,ψ)である。本実施形態では、Z−X−Z系のオイラー角(φ,θ,ψ)を、姿勢角(φ,θ,ψ)ということにする。
【0065】
つまり、本実施形態では、姿勢情報検出部70は、姿勢情報として姿勢角(φ,θ,ψ)の情報を出力する。
【0066】
センサー装置1Bは、判定部50を含む。判定部50は、加速度センサー10x、10y、10zの出力に基づいて、角速度センサー20x、20y、20zに加わる角速度が0であるか否かを判定する。
【0067】
本実施形態では、判定部50は、初期状態検出部210を含む。初期状態検出部210は、加速度センサー10x、10y、10zの出力に基づいてセンサー装置1Aに加わる加速度を算出し、算出した加速度と重力加速度Gが略一致する状態を初期状態として検出する。本実施形態では、初期状態検出部210は、加速度センサー10x、10y、10zの各出力電圧を所定の周期でサンプリングし、A/D変換して得られるx軸、y軸、z軸方向の加速度成分gx、gy、gzの2乗和gx2+gy2+gz2を計算し、gx2+gy2+gz2=G2の状態を初期状態として検出する。
【0068】
ただし、実際には、加速度センサー10x、10y、10zの出力オフセットやセンサー装置1Bの使用環境におけるノイズ等の環境因により、センサー装置1Bが停止若しくは等速運動をしていても加速度センサー10x、10y、10zによって検出される加速度gが重力加速度Gと微妙に一致しないことが想定される。そのため、初期状態検出部210は、例えば、gx2+gy2+gz2とG2の差が微小値Δ以下であれば、すなわちG2−Δ≦gx2+gy2+gz2≦G2+Δを満たした時に初期状態を検出する。この微小値Δは、センサー装置1Bを使用するアプリケーション側の仕様等によって適切な値が設定される。
【0069】
ここで、図2(B)に示した遠心加速度Rがある程度大きい場合にはgx2+gy2+gz2=G2が成立しないので、初期状態検出部210により初期状態が検出されることはないが、遠心加速度Rが極めて小さい場合には初期状態検出部210により初期状態が検出される場合が想定される。そこで、以下に説明するように、判定部50は、初期状態検出部210により初期状態が検出された場合は、その後の加速度検出成分の時間変化をモニターすることで実際の角速度ωが0か否かを判定する。
【0070】
そのため、本実施形態では、判定部50は、角速度算出部220を含む。角速度算出部220は、初期状態におけるセンサー装置1Bの姿勢情報に基づいて初期状態におけるx軸、y軸、z軸方向の軸ベクトルを算出し、当該軸ベクトルとx軸、y軸、z軸方向の加速度成分と当該加速度成分の時間変化とに基づいてセンサー装置1Bが任意の軸回りに回転する角速度ωを算出する。本実施形態では、角速度算出部220は、後述する式(7)、式(8)、式(9)により、軸ベクトルとしてx軸、y軸、z軸方向の単位ベクトルaxR、ayR、azRを計算する。また、角速度算出部220は、後述する式(24)、式(25)、式(26)により、加速度センサー10x、10y、10zが検出する加速度成分gx、gy、gzを含むD1、D2、D3を計算する。さらに、角速度算出部220は、D1、D2、D3の1階微分(実際には1階差分)と2階微分(実際には2階差分))を計算する。そして、角速度算出部220は、単位ベクトルaxR、ayR、azRと、D1、D2、D3と、D1、D2、D3の1階微分と、D1、D2、D3の2階微分を用いて、後述する式(40)によりω2を計算する。
【0071】
また、本実施形態では、判定部50は、角速度判定部230を含む。角速度判定部230は、角速度算出部220が算出した角速度が略0である場合に、角速度センサー20x、20y、20zに加わる角速度が0であると判定する。本実施形態では、角速度判定部230は、ω2=0であれば、角速度センサー20x、20y、20zに加わる角速度が0であると判定する。
【0072】
例えば、角速度判定部230は、角速度算出部220が算出した角速度ωが略0の状態が所定時間継続した場合に、角速度センサー20x、20y、20zに加わる角速度が0であると判定するようにしてもよい。
【0073】
なお、オフセット補正部40は、第1実施形態と同様であるため、その説明を省略する。
【0074】
このように、本実施形態のセンサー装置1Bは、初期状態におけるx軸、y軸、z軸方向の軸ベクトルと、加速度センサー10x、10y、10zの出力電圧を周期的にサンプリングして順次得られるx軸、y軸、z軸方向の加速度成分と、当該加速度成分の時間変化とに基づいて計算される角速度ωの2乗が0となるタイミングで、角速度センサー20x、20y、20zの出力のオフセット(ゼロ点オフセット)を自動的に補正する。
【0075】
[オフセット補正タイミングの判定原理]
次に、本実施形態における、角速度センサー20x、20y、20zのオフセット補正処理を行うタイミングの判定原理について説明する。
【0076】
本実施形態では、絶対座標系(XYZ座標系)とセンサー座標系(xyz座標系)が一致する状態を基準として、センサー装置1Bの姿勢をZ−X−Z系のオイラー角で表現する。
【0077】
図6(A)に示すように、絶対座標系(XYZ座標系)をZ軸回りに角度φだけ回転させたX’Y’Z’座標系に変換する回転行列R1は次式(3)で与えられる。
【0078】
【数3】
【0079】
さらに、図6(B)に示すように、X’Y’Z’座標系をX’軸回りに角度θだけ回転させたX’’Y’’Z’’座標系に変換する回転行列R2は次式(4)で与えられる。
【0080】
【数4】
【0081】
さらに、図6(C)に示すように、X’’Y’’Z’’座標系をZ’’軸回りに角度ψだけ回転させたX’’’Y’’’Z’’’座標系に変換する回転行列R3は次式(5)で与えられる。
【0082】
【数5】
【0083】
このX’’’Y’’’Z’’’座標系がセンサー座標系(xyz座標系)と一致するなら、センサー装置1Bの姿勢を表現するZ−X−Z系のオイラー角(姿勢角)は(φ,θ,ψ)である。
【0084】
従って、センサー装置1Bの姿勢角を(0,0,0)から(φ,θ,ψ)に変換する回転行列Rは、次式(6)で与えられる。
【0085】
【数6】
【0086】
本実施形態では、センサー装置1Bが基準姿勢の時(姿勢角が(0,0,0)の時)は、絶対座標系(XYZ座標系)とセンサー座標系(xyz座標系)が一致するので、x軸、y軸、z軸方向の3つの単位ベクトルは、それぞれ絶対座標系(XYZ座標系)において(1,0,0)、(0,1,0)、(0,0,1)で表される。
【0087】
本実施形態では、加速度センサー10x、10y、10zの検出軸は常にセンサー座標系のx軸、y軸、z軸とそれぞれ一致するので、センサー装置1Bが基準姿勢の時(姿勢角が(0,0,0)の時)、加速度センサー10x、10y、10zの検出軸方向の単位ベクトルax、ay、azは、それぞれax=(1,0,0)、ay=(0,1,0)、az=(0,0,1)になっている。
【0088】
従って、センサー装置1Bの姿勢角が(φ,θ,ψ)の時のx軸、y軸、z軸方向の単位ベクトルaxR、ayR、azRは、それぞれ次の式(7)、式(8)、式(9)で表される。
【0089】
【数7】
【0090】
【数8】
【0091】
【数9】
【0092】
次に、センサー装置1Bが姿勢角(φ,θ,ψ)で停止又は等速直線運動していると仮定し、この状態からセンサー装置1Bが任意の軸ベクトルn=(nx、ny、nz)の回りに角度δだけ回転する場合を考えると、この回転を表す回転行列Δは、次式(10)で表される。
【0093】
【数10】
【0094】
したがって、軸ベクトルn=(nx、ny、nz)の回りに角度δだけ回転した後のx軸、y軸、z軸方向の単位ベクトルaxRΔ、ayRΔ、azRΔは、それぞれ次の式(11)、式(12)、式(13)で表される。
【0095】
【数11】
【0096】
【数12】
【0097】
【数13】
【0098】
従って、単位ベクトルaxRΔ、ayRΔ、azRΔが、重力方向の単位ベクトルp=(0,0,1)とそれぞれ成す角度γx、γy、γzは、それぞれ次の式(14)、式(15)、式(16)で計算される。
【0099】
【数14】
【0100】
【数15】
【0101】
【数16】
【0102】
ここで、図2(B)で示した遠心加速度Rが極めて小さい場合は、加速度センサー10x、10y、10zが検出する加速度成分gx、gy、gzは、重力加速度Gのx軸、y軸、z軸方向の成分と一致する。すなわち、gx=G・cosγx、gy=G・cosγy、gz=G・cosγzなので、式(14)、式(15)、式(16)より、それぞれ次の式(17)、式(18)、式(19)が導かれる。なお、式(17)、式(18)、式(19)におけるD1、D2、D3はそれぞれ式(20)、式(21)、式(22)で与えられる。
【0103】
【数17】
【0104】
【数18】
【0105】
【数19】
【0106】
【数20】
【0107】
【数21】
【0108】
【数22】
【0109】
式(17)、式(18)、式(19)をまとめると、次の式(23)が得られる。
【0110】
【数23】
【0111】
この式(23)に対してクラメルの公式を適用すると、D1、D2、D3がそれぞれ次の式(24)、式(25)、式(26)により求まる。
【0112】
【数24】
【0113】
【数25】
【0114】
【数26】
【0115】
D1、D2、D3は、それぞれ式(20)、式(21)、式(22)により与えられるので、D1、D2、D3の1階微分はそれぞれ次の式(27)、式(28)、式(29)で計算される。
【0116】
【数27】
【0117】
【数28】
【0118】
【数29】
【0119】
D1、D2、D3の2階微分は、式(27)、式(28)、式(29)をさらに微分して、それぞれ次の式(30)、式(31)、式(32)で計算される。
【0120】
【数30】
【0121】
【数31】
【0122】
【数32】
【0123】
ここで、式(20)、式(21)、式(22)から、次式(33)の関係式が得られる。
【0124】
【数33】
【0125】
また、式(27)、式(28)、式(29)から、次式(34)の関係式が得られる。
【0126】
【数34】
【0127】
また、式(30)、式(31)、式(32)から、次式(35)の関係式が得られる。
【0128】
【数35】
【0129】
式(33)、式(34)、式(35)をまとめると、次の式(36)が得られる。
【0130】
【数36】
【0131】
この式(36)に対してクラメルの公式を適用すると、nx、ny、nzがそれぞれ次の式(37)、式(38)、式(39)により求まる。
【0132】
【数37】
【0133】
【数38】
【0134】
【数39】
【0135】
最後に、式(27)と式(28)からδを消去することで、次の式(40)により、センサー装置1Bに加わる角速度ωの2乗が求まる。
【0136】
【数40】
【0137】
ここで、センサー装置1Bが初期状態のまま停止もしくは等速直線運動をしていれば(回転運動をしていなければ)、センサー装置1Bには角速度が加わらないので、角速度センサー20x、20y、20zはすべてゼロ点電圧を出力する。この時、gx、gy、gzの時間変化は0でありD1、D2、D3は一定値となるため、D1、D2、D3を1階微分した値はすべて0になる。そのため、式(40)により計算されるω2が0になる。
【0138】
一方、センサー装置1Bが任意の軸n=(nx、ny、nz)回りの回転運動をしていれば、センサー装置1Bにはこの回転運動の角速度が加わり、角速度センサー20x、20y、20zは、この角速度のx軸成分、y軸成分、z軸成分を検出することになる。この時、gx、gy、gzが時間変化するため、式(40)により計算されるω2が0にならない。
【0139】
そこで、角速度判定部230は、式(40)で計算されるω2が0になるか否かによってセンサー装置1Bに加わる角速度が0か否かを判定し、角速度センサー20x、20y、20zのオフセット補正処理のタイミングを制御する。ただし、実際には、加速度センサー10x、10y、10zの出力オフセットやセンサー装置1Bの使用環境におけるノイズ等の環境因により、センサー装置1Bが停止若しくは等速運動をしていても加速度センサー10x、10y、10zによって検出される加速度gが重力加速度Gと微妙に一致しないことが想定される。そのため、角速度判定部230は、例えば、式(40)で計算されるω2が微小値Δ以下であれば、すなわちω2≦Δを満たせば、センサー装置1Bに加わる角速度が0であると判定するようにしてもよい。この微小値Δは、センサー装置1Bを使用するアプリケーション側の仕様等によって適切な値が設定される。
【0140】
[オフセット補正処理]
図7は、本実施形態における、角速度センサー20x、20y、20zのオフセット補正処理の第1の例を示すフローチャート図である。
【0141】
まず、初期状態検出部210が加速度センサー10x、10y、10zの検出信号をサンプリングし、x軸、y軸、z軸方向の加速度成分gx,gy,gzを計算する(ステップS110)。
【0142】
次に、初期状態検出部210がg2=gx2+gy2+gz2を計算する(ステップS120)。
【0143】
次に、初期状態検出部210がgx2+gy2+gz2とG2が一致するか否かを判定し(ステップS130)、g2≠G2(初期状態を未検出)の場合は(ステップS130のN)、ステップS110とS120の処理を再度行う。
【0144】
一方、g2=G2の場合は(ステップS130のY)、初期状態検出部210が初期状態を検出し(ステップS140)、角速度算出部220が、式(7)、式(8)、式(9)により、姿勢情報算出部70が算出する姿勢角(φ,θ,ψ)から、初期状態における加速度センサー10x、10y、10zのx軸方向、y軸方向、軸方向の単位ベクトルaxR,ayR,azRを計算する(ステップS150)。
【0145】
次に、角速度算出部220が、ステップS150で得られた単位ベクトルaxR,ayR,azRとステップS110で得られた加速度成分gx,gy,gzから、式(24)、式(25)、式(26)により、D1,D2,D3を計算する(ステップS160)。
【0146】
次に、角速度算出部220が、D1,D2,D3の1階微分を計算する(ステップS170)。具体的には、角速度算出部220が、ステップS160で計算した今回のD1,D2,D3と前回のD1,D2,D3(1サンプル前のgx,gy,gzから計算したD1,D2,D3)との各差分(D1,D2,D3の1階差分)を計算する。
【0147】
次に、角速度算出部220が、式(30)、式(31)、式(32)により、D1,D2,D3の2階微分を計算する(ステップS180)。具体的には、角速度算出部220が、ステップS170で計算した今回のD1,D2,D3の1階差分と前回のD1,D2,D3の1階差分(1サンプル前のgx,gy,gzから計算したD1,D2,D3の1階差分)との各差分(D1,D2,D3の2階差分)を計算する。
【0148】
次に、角速度算出部220が、式(40)により、ω2を計算する(ステップS190)。
【0149】
次に、角速度判定部230がω2=0か否かを判定し(ステップS200)、ω2≠0の場合は(ステップS200のN)、ステップS110〜S190の処理を再度行う。
【0150】
一方、ω2=0の場合は(ステップS200のY)、オフセット補正部40が角速度センサー20x、20y、20zのオフセット補正を行う(ステップS210)。
【0151】
図8は、本実施形態における、角速度センサー20x、20y、20zのオフセット補正処理の第2の例を示すフローチャート図である。図8において、図7と同じ処理を行うステップには同じ符号を付しており、その説明を省略又は簡略する。
【0152】
まず、初期状態検出部210がステップS110〜S140の処理を行う。
【0153】
まず、角速度判定部230が所定時間T2の計測を開始する(ステップS142)。
【0154】
次に、角速度算出部220がステップS150〜S190の処理を行った後、角速度判定部230がω2=0か否かを判定し(ステップS200)、ω2≠0の場合は(ステップS200のN)、初期状態検出部210が初期状態の検出処理(ステップS110〜S140の処理)を再度行う。
【0155】
一方、ω2=0の場合は(ステップS200のY)、角速度判定部230が、ステップS142で計測を開始してから所定時間T2が経過したか否かを判定し(ステップS202)、所定時間T2が経過していない場合は(ステップS202のN)、初期状態検出部210が加速度センサー10x、10y、10zの検出信号をサンプリングしてx軸、y軸、z軸方向の加速度成分gx,gy,gzを計算し(ステップS204)、所定時間T2が経過するまで、ステップS150〜S200の処理を繰り返し行う。ただし、所定時間T2が経過する前に、ステップS200の判定処理でω2≠0となった場合は、初期状態検出部210が初期状態の検出処理(ステップS110〜S140の処理)を再度行う。
【0156】
所定時間T2が経過した場合は(ステップS202のY)、オフセット補正部40が角速度センサー20x、20y、20zのオフセット補正を行う(ステップS210)。
【0157】
このようにすれば、センサー装置1Bに所定時間T2以上継続して角速度が加わっていないのか瞬時的に角速度が0となっただけなのかを区別し、前者の場合だけ角速度センサー20x、20y、20zのオフセット補正を行うようにすることができる。
【0158】
以上に説明した第2実施形態のセンサー装置によれば、第1実施形態のセンサー装置と同様の効果を奏することができる。さらに、第2実施形態のセンサー装置によれば、加速度センサーが検知できないような小さい遠心力が加わった場合でも、初期状態からの加速度成分の時間変化をモニターすることで、角速度センサーに加わる角速度を検知することができる。そのため、角速度センサーのオフセット補正を行うタイミングの判定精度を高めることができる。
【0159】
(3)第3実施形態
[構成]
図9は、第3実施形態のセンサー装置の概略構成図である。図9において、図5と同じ構成要素には同じ符号を付しており、その説明を省略又は簡略する。
【0160】
図9に示すように、第3実施形態のセンサー装置1Cは、図5の判定部40に代えて判定部80を含む。
【0161】
本実施形態では、判定部80は、図5と同じ初期状態検出部210と角速度算出部220を含んで構成されている。
【0162】
また、本実施形態では、判定部80は、判定モード制御部240を含む。判定モード制御部240は、制御信号に応じて第1の判定モードと第2の判定モードを切り替える。この制御信号は、センサー装置1Cの外部から供給されてもよいし、センサー装置1Cの内部で生成してもよい。
【0163】
また、本実施形態では、判定部80は、角速度判定部250を含む。角速度判定部250は、第1の判定モードでは、初期状態検出部210が初期状態を検出した場合に、角速度センサー20x、20y、20zに加わる角速度が0であると判定する。本実施形態では、角速度判定部250は、第1の判定モードでは、gx2+gy2+gz2=G2であれば、角速度センサー20x、20y、20zに加わる角速度が0であると判定する。
【0164】
また、角速度判定部250は、第2の判定モードでは、角速度算出部220が算出した角速度が略0である場合に、角速度センサー20x、20y、20zに加わる角速度が0であると判定する。本実施形態では、角速度判定部250は、第2の判定モードでは、ω2=0であれば、角速度センサー20x、20y、20zに加わる角速度が0であると判定する。
【0165】
例えば、角速度判定部250は、第1の判定モードでは、初期状態検出部210が所定時間継続して初期状態を検出した場合に、角速度センサー20x、20y、20zに加わる角速度が0であると判定するようにしてもよい。また、例えば、角速度判定部250は、第2の判定モードでは、角速度算出部220が算出した角速度ωが略0の状態が所定時間継続した場合に、角速度センサー20x、20y、20zに加わる角速度が0であると判定するようにしてもよい。
【0166】
このように、本実施形態のセンサー装置1Cは、第1の判定モードでは、加速度センサー10x、10y、10zの出力電圧を周期的にサンプリングして順次得られる加速度の2乗和(gx2+gy2+gz2)と重力加速度の2乗(G2)が一致したタイミングで、角速度センサー20x、20y、20zの出力のオフセット(ゼロ点オフセット)を自動的に補正する。また、本実施形態のセンサー装置1Cは、第2の判定モードでは、初期状態におけるx軸、y軸、z軸方向の軸ベクトルと、加速度センサー10x、10y、10zの出力電圧を周期的にサンプリングして順次得られるx軸、y軸、z軸方向の加速度成分と、当該加速度成分の時間変化とに基づいて計算される角速度ωの2乗が0となるタイミングで、角速度センサー20x、20y、20zの出力のオフセット(ゼロ点オフセット)を自動的に補正する。
【0167】
[オフセット補正処理]
図10は、本実施形態における、角速度センサー20x、20y、20zのオフセット補正処理の第1の例を示すフローチャート図である。図10において、図7と同じ処理を行うステップには同じ符号を付しており、その説明を省略又は簡略する。
【0168】
まず、初期状態検出部210が初期状態の検出処理(ステップS110〜S140の処理)を行う。
【0169】
次に、第1の判定モードであれば(ステップS144のY)、オフセット補正部40が角速度センサー20x、20y、20zのオフセット補正を行う(ステップS210)。
【0170】
第1の判定モードでなければ(すなわち第2の判定モードであれば)(ステップS144のN)、角速度算出部220がステップS150〜S190の処理を行った後、角速度判定部250がω2=0か否かを判定し(ステップS200)、ω2≠0の場合は(ステップS200のN)、初期状態検出部210が初期状態の検出処理(ステップS110〜S140の処理)を再度行う。
【0171】
一方、ω2=0の場合は(ステップS200のY)、オフセット補正部40が角速度センサー20x、20y、20zのオフセット補正を行う(ステップS210)。
【0172】
図11は、本実施形態における、角速度センサー20x、20y、20zのオフセット補正処理の第2の例を示すフローチャート図である。図11において、図10と同じ処理を行うステップには同じ符号を付しており、その説明を省略又は簡略する。
【0173】
まず、第1の判定モードであれば(ステップS102のY)、角速度判定部250が所定時間T1の計測を開始する(ステップS104)。
【0174】
次に、初期状態検出部210が初期状態の検出処理(ステップS110〜S140の処理)を行う。
【0175】
次に、第1の判定モードであれば(ステップS144のY)、角速度判定部250が、ステップS104で計測を開始してから所定時間T1が経過したか否かを判定し(ステップS145)、所定時間T1が経過していない場合は(ステップS145のN)、所定時間T1が経過するまで、初期状態検出部210がステップS110〜S140の処理を再度行う。そして、所定時間T1が経過した場合は(ステップS145のY)、オフセット補正部40が角速度センサー20x、20y、20zのオフセット補正を行う(ステップS210)。
【0176】
第1の判定モードでなければ(すなわち第2の判定モードであれば)(ステップS144のN)、角速度判定部250が所定時間T2の計測を開始する(ステップS148)。
【0177】
次に、角速度算出部220がステップS150〜S190の処理を行った後、角速度判定部250がω2=0か否かを判定し(ステップS200)、ω2≠0の場合は(ステップS200のN)、初期状態検出部210が初期状態の検出処理(ステップS110〜S140の処理)を再度行う。
【0178】
一方、ω2=0の場合は(ステップS200のY)、角速度判定部250が、ステップS148で計測を開始してから所定時間T2が経過したか否かを判定し(ステップS202)、所定時間T2が経過していない場合は(ステップS202のN)、初期状態検出部210が加速度センサー10x、10y、10zの検出信号をサンプリングしてx軸、y軸、z軸方向の加速度成分gx,gy,gzを計算し(ステップS204)、所定時間T2が経過するまで、ステップS150〜S200の処理を繰り返し行う。ただし、所定時間T2が経過する前に、ステップS200の判定処理でω2≠0となった場合は、初期状態検出部210が初期状態の検出処理(ステップS110〜S140の処理)を再度行う。
【0179】
所定時間T2が経過した場合は(ステップS202のY)、オフセット補正部40が角速度センサー20x、20y、20zのオフセット補正を行う(ステップS210)。
【0180】
このようにすれば、第1の判定モードでは、センサー装置1Cに所定時間T1以上継続して遠心力が加わっていないのか瞬時的に遠心力が0となっただけなのかを区別し、前者の場合だけ角速度センサー20x、20y、20zのオフセット補正を行うようにすることができる。また、第2の判定モードでは、センサー装置1Cに所定時間T2以上継続して角速度が加わっていないのか瞬時的に角速度が0となっただけなのかを区別し、前者の場合だけ角速度センサー20x、20y、20zのオフセット補正を行うようにすることができる。
【0181】
以上に説明した第3実施形態のセンサー装置によれば、第1実施形態のセンサー装置と同様の効果を奏することができる。さらに、第3実施形態のセンサー装置によれば、当該センサー装置を使用するアプリケーションに応じて、第1の判定モードと第2の判定モードのいずれを使用するかで、角速度センサーのオフセット補正タイミングの判定精度を変更することができる。すなわち、第1の判定モードにすれば、遠心力の検出による簡易な判定になり、第2の判定モードにすれば、加速度センサーで検出できないような微小な遠心力が加わった場合でも当該センサー装置に加わる角速度が0か否かを判定することができる。
【0182】
2.電子機器
図12は、本実施形態のセンサー装置を含む電子機器の構成例を示す機能ブロック図である。本実施形態の電子機器500は、センサー装置400、ホストCPU300、操作部310、表示部320、ROM(Read Only Memory)330、RAM(Random Access Memory)340、通信部350を含んで構成されている。
【0183】
ホストCPU(マイクロコンピューター)300は、ROM330に記憶されているプログラムに従って、各種の計算処理や制御処理を行う。具体的には、ホストCPU300は、センサー装置400に対して各種の制御コマンドを送信してセンサー装置400の動作を制御したり、センサー装置400からデータを受け取って各種の計算処理をする。また、ホストCPU300は、操作部310からの操作信号に応じた各種の処理、表示部320に各種の情報を表示させるための表示信号を送信する処理、外部とデータ通信を行うために通信部350を制御する処理等を行う。
【0184】
操作部310は、操作キーやボタンスイッチ等により構成される入力装置であり、ユーザーによる操作に応じた操作信号をホストCPU300に出力する。
【0185】
表示部320は、LCD(Liquid Crystal Display)等により構成される表示装置であり、ホストCPU300から入力される表示信号に基づいて各種の情報(例えば、ナビゲーション情報等)を表示する。
【0186】
ROM330は、ホストCPU300が各種の計算処理や制御処理を行うためのプログラムや、各種のアプリケーションプログラムやデータ等(例えば、ナビゲーションや姿勢制御を行うためのプログラムやデータ等)を記憶している。
【0187】
RAM340は、ホストCPU300の作業領域として用いられ、ROM330から読み出されたプログラムやデータ、操作部310から入力されたデータ、ホストCPU300が各種プログラムに従って実行した演算結果等を一時的に記憶する。
【0188】
通信部350は、CPU300と外部装置との間のデータ通信を成立させるための各種制御を行う。
【0189】
センサー装置400は、例えば、前述した第1実〜第3実施形態のセンサー装置1A、1B、1C等であり、電子機器500の内部において、センサーのx軸、y軸、z軸が電子機器500を基準として所定の方向に向くように固定して配置されている。そして、センサー装置400は、当該センサー装置400(電子機器500)に加わる角速度が0でるか否かを判定し、角速度が0の時に角速度センサーの0点電圧のオフセットを補正する。このセンサー装置400を電子機器500に組み込むことにより、アプリケーションによらずセンサー装置の角速度センサーのオフセットを自動的に補正することができる。
【0190】
なお、電子機器500としては、ゲームコントローラや3次元マウス等の入力機器、ラジコンヘリ等の玩具、ロボット、航法装置(ナビゲーション装置)、携帯電話機、モバイルパソコン等の種々の電子機器が挙げられる。
【0191】
なお、本発明は本実施形態に限定されず、本発明の要旨の範囲内で種々の変形実施が可能である。
【0192】
例えば、本実施形態のセンサー装置では、3つの角速度センサーを含んでいるが、1つ以上の角速度センサーを含んでいればよい。
【0193】
また、例えば、本実施形態のセンサー装置では、3つの加速度センサーを含んでいるが、3つ以上の加速度センサーを含み、それらの加速度センサーの一部又は全部の出力に基づいて互いに直交する3軸方向の加速度を算出するように変形してもよい。さらに、3つの加速度センサーは、必ずしも3つの検出軸が互いに直交するように配置されている必要はなく、3軸が互いに平行にならないように配置されていればよい。3軸が互いに直交しない場合は、アラインメント補正により、3軸方向の角速度を直交座標系における角速度に変換した後、本実施形態において説明した処理を行うようにすればよい。
【0194】
また、本実施形態のセンサー装置では、角速度センサー20x、20y、20zがゼロ点電圧の調整機能を有し、オフセット補正部40は角速度センサー20x、20y、20zにゼロ点調整信号を供給することで、オフセット補正処理を実現しているが、これに限られない。例えば、角速度センサー20x、20y、20zがゼロ点電圧の調整機能を有していない場合は、オフセット補正部40は、オフセット補正処理のタイミングにおける角速度センサー20x、20y、20zの出力電圧と仕様上のゼロ点電圧との誤差に応じた補正電圧を発生させ、この補正電圧と角速度センサー20x、20y、20zの出力電圧とをそれぞれ電圧加算することで、これらの出力電圧を仕様上のゼロ点電圧と一致させるようにしてもよい。
【0195】
また、第2、第3実施形態のセンサー装置は、姿勢情報算出部70を含む姿勢センサーとして構成されており、角速度算出部220は、姿勢情報算出部70が算出する姿勢角を初期状態における姿勢角(φ,θ,ψ)としてω2の計算を行っているが、これに限られない。センサー装置が姿勢センサーでなければ姿勢情報算出部70を含まず、角速度算出部220は、初期状態における姿勢角(φ,θ,ψ)をセンサー装置の外部から受け取って、ω2の計算を行うようにしてもよい。また、地磁気センサー60x、60y、60zは必須の構成要素ではないので省略してもよい。
【0196】
本発明は、実施の形態で説明した構成と実質的に同一の構成(例えば、機能、方法及び結果が同一の構成、あるいは目的及び効果が同一の構成)を含む。また、本発明は、実施の形態で説明した構成の本質的でない部分を置き換えた構成を含む。また、本発明は、実施の形態で説明した構成と同一の作用効果を奏する構成又は同一の目的を達成することができる構成を含む。また、本発明は、実施の形態で説明した構成に公知技術を付加した構成を含む。
【符号の説明】
【0197】
1A,1B,1C センサー装置、10x,10y,10z 加速度センサー、20x,20y,20z 角速度センサー、30 判定部、40 オフセット補正部、50 判定部、60x,60y,60z 地磁気センサー、70 姿勢情報算出部、110 加速度算出部、120 角速度判定部、210 初期状態検出部、220 角速度算出部、230 角速度判定部、240 判定モード制御部、250 角速度判定部、300 ホストCPU、310 操作部、320 表示部、330 ROM、340 RAM、350 通信部、400 センサー装置、500 電子機器
【技術分野】
【0001】
本発明は、センサー装置、電子機器及び角速度センサーのオフセット補正方法に関する。
【背景技術】
【0002】
近年、角速度を検出するセンサーモジュールや姿勢角を検知する姿勢角検知ユニットを利用した様々なシステムや電子機器が利用されている。これらのシステムや電子機器では、ジャイロセンサー(角速度センサー)のバイアスオフセット(ゼロ点オフセット)を排除しなければ静止時に静止していることを正しく検出することができないため、性能を向上させる上で、このバイアスオフセットを如何にして排除するかが最も重要な課題になっている。
【0003】
従来より、製品の製造時等にバイアスオフセットを計測し、製品に組み込まれた不揮発性の記憶素子等に補正データを書き込むことでバイアスオフセットを補正する手法や、製品の使用時に静止状態を検出してオフセットのキャリブレーションを行う手法が実現されている。前者の手法では、バイアスオフセットの初期ばらつきを補正することができるが、温度変動等によるユーザー使用時のバイアス変動に対応することができない。一方、後者の手法では、ユーザーの使用環境において生じているバイアスのオフセットを使用環境においてキャリブレーション補正することができる。そのため、製造時の補正からの変動分や、線形関数で補正しきれない温度変動分を補正することができる。
【先行技術文献】
【特許文献】
【0004】
【特許文献1】特開2010−019751号公報
【特許文献2】特許4314566号公報
【特許文献3】特許2857091号公報
【発明の概要】
【発明が解決しようとする課題】
【0005】
しかしながら、使用環境下でバイアスオフセットのキャリブレーションを行う手法は、いずれもアプリケーション機能に連動して行われるものであった。例えば、特許文献1の手法ではプレイヤが弓を引く瞬間、特許文献2の手法では掃除機の充電時や方向転換時、特許文献3の手法ではナビゲーション装置の起動時にキャリブレーションを行うなど、アプリケーション側でキャリブレーションを行うタイミングを決定している。つまり、アプリケーションに組み込まれたキャリブレーション補正アルゴリズムとして実現されており、センサーユニット単体ではキャリブレーションのタイミングを決定する機能を有するものではなかった。そのため、アプリケーションの開発者がキャリブレーション補正の適切なタイミングを熟慮し、補正アルゴリズムを作成する手間が発生していた。
【0006】
本発明は、以上のような問題点に鑑みてなされたものであり、本発明のいくつかの態様によれば、補正タイミングを自己判定して角速度センサーのオフセット補正を行うことができるセンサー装置、電子機器及び角速度センサーのオフセット補正方法を提供することができる。
【課題を解決するための手段】
【0007】
(1)本発明は、角速度センサーと、3つ以上の加速度センサーと、を含むセンサー装置であって、前記加速度センサーの出力に基づいて、前記角速度センサーに加わる角速度が0であるか否かを判定する判定部と、前記判定部により前記角速度が0であると判定された時に、前記角速度センサーの出力のオフセットを補正するオフセット補正部と、を含む、センサー装置である。
【0008】
センサー装置が停止していれば角速度センサーに角速度が加わらないので、この時の角速度センサーの出力電圧が0点電圧になる。そのため、このタイミングであれば0点電圧のオフセットを適切に補正することができる。本発明によれば、加速度センサーの出力に基づいて角速度センサーに角速度が加わらないタイミングを検出することができるので、そのタイミングで自動的に角速度センサーのオフセットを補正することができる。すなわち、本発明によれば、アプリケーション機能によらずセンサー装置単体で角速度センサーのオフセットを補正することができる。そのため、このセンサー装置を使用すれば、従来の手法のようにアプリケーション側に補正アルゴリズムを実装する必要がなく、ユーザーに負荷をかけずに角速度センサーのゼロ点オフセットを補正することができる。
【0009】
また、本発明によれば、使用環境下で角速度センサーのゼロ点の変動を検知して補正することができる。
【0010】
(2)このセンサー装置において、前記判定部は、前記加速度センサーの各々の検出軸方向の加速度成分の時間変化に基づいて、前記角速度センサーに加わる角速度が0であるか否かを判定するようにしてもよい。
【0011】
(3)このセンサー装置において、前記判定部は、前記加速度センサーの出力に基づいて当該センサー装置に加わる加速度を算出し、算出した加速度と重力加速度が略一致する状態を初期状態として検出する初期状態検出部と、前記初期状態における当該センサー装置の姿勢情報に基づいて前記初期状態における各検出軸方向の軸ベクトルを算出し、当該軸ベクトルと各検出軸方向の加速度成分と当該加速度成分の時間変化とに基づいて当該センサー装置が任意の軸回りに回転する角速度を算出する角速度算出部と、前記角速度算出部が算出した角速度が略0である場合に、前記角速度センサーに加わる角速度が0であると判定する角速度判定部と、を含むようにしてもよい。
【0012】
このセンサー装置によれば、加速度センサーが検知できないような小さい遠心力が加わった場合でも、初期状態からの加速度成分の時間変化をモニターすることで、角速度センサーに加わる角速度を検知することができる。そのため、角速度センサーのオフセット補正を行うタイミングの判定精度を高めることができる。
【0013】
(4)このセンサー装置において、前記角速度判定部は、前記角速度算出部が算出した角速度が略0の状態が所定時間継続した場合に、前記角速度センサーに加わる角速度が0であると判定するようにしてもよい。
【0014】
このようにすれば、センサー装置に所定時間継続して角速度が加わっていないのか瞬時的に角速度が0となっただけなのかを区別し、前者の場合だけ角速度センサーのオフセット補正を行うようにすることができる。
【0015】
(5)このセンサー装置において、前記判定部は、前記加速度センサーの出力に基づいて当該センサー装置に加わる加速度を算出する加速度算出部と、前記加速度算出部が算出した加速度と重力加速度が略一致する場合に、前記角速度センサーに加わる角速度が0であると判定する角速度判定部と、を含むようにしてもよい。
【0016】
センサー装置が回転状態にあると、角速度センサーは角速度を検出するので、そのタイミングでは適切なオフセット補正を行うことができない。一方、センサー装置に遠心力が働けば、加速度センサーが遠心力により発生する遠心加速度を検出するため、その検出加速度は重力加速度と一致しない。そのため、このセンサー装置によれば、加速度センサーの検出加速度が重力加速度と一致するか否かを判定する簡便な演算処理により、遠心力が加わっていない適切なタイミングで角速度センサーのオフセットを補正することができる。
【0017】
(6)このセンサー装置において、前記角速度判定部は、前記加速度算出部が算出した加速度と重力加速度が略一致する状態が所定時間継続した場合に、前記角速度センサーに加わる角速度が0であると判定するようにしてもよい。
【0018】
このようにすれば、センサー装置に所定時間継続して遠心力が加わっていないのか瞬時的に遠心力が0となっただけなのかを区別し、前者の場合だけ角速度センサーのオフセット補正を行うようにすることができる。
【0019】
(7)このセンサー装置において、前記判定部は、前記加速度センサーの出力に基づいて当該センサー装置に加わる加速度を算出し、算出した加速度と重力加速度が略一致する状態を初期状態として検出する初期状態検出部と、前記初期状態における当該センサー装置の姿勢情報に基づいて前記初期状態における各検出軸方向の軸ベクトルを算出し、当該軸ベクトルと各検出軸方向の加速度成分と当該加速度成分の時間変化とに基づいて当該センサー装置が任意の軸回りに回転する角速度を算出する角速度算出部と、制御信号に応じて第1の判定モードと第2の判定モードを切り替える判定モード制御部と、前記第1の判定モードでは、前記初期状態検出部が前記初期状態を検出した場合に前記角速度センサーに加わる角速度が0であると判定し、前記第2の判定モードでは、前記角速度算出部が算出した角速度が略0である場合に前記角速度センサーに加わる角速度が0であると判定する角速度判定部と、を含むようにしてもよい。
【0020】
このようにすれば、このセンサー装置を使用するアプリケーションに応じて、第1の判定モードと第2の判定モードのいずれを使用するかで、角速度センサーのオフセット補正タイミングの判定精度を変更することができる。すなわち、第1の判定モードにすれば、遠心力の検出による簡易な判定になり、第2の判定モードにすれば、加速度センサーで検出できないような微小な遠心力が加わった場合でもセンサー装置に加わる角速度が0か否かを判定することができる。
【0021】
(8)このセンサー装置において、前記角速度判定部は、前記第1の判定モードでは、前記初期状態検出部が所定時間継続して前記初期状態を検出した場合に前記角速度センサーに加わる角速度が0であると判定し、前記第2の判定モードでは、前記角速度算出部が算出した角速度が略0の状態が所定時間継続した場合に前記角速度センサーに加わる角速度が0であると判定するようにしてもよい。
【0022】
このようにすれば、第1の判定モードでは、センサー装置に所定時間継続して遠心力が加わっていないのか瞬時的に遠心力が0となっただけなのかを区別し、前者の場合だけ角速度センサーのオフセット補正を行うようにすることができる。また、第2の判定モードでは、センサー装置に所定時間継続して角速度が加わっていないのか瞬時的に角速度が0となっただけなのかを区別し、前者の場合だけ角速度センサーのオフセット補正を行うようにすることができる。
【0023】
(9)本発明は、上記のいずれかのセンサー装置を含む、電子機器である。
【0024】
(10)本発明は、角速度センサーと、3つ以上の加速度センサーと、を含むセンサー装置において、前記角速度センサーの出力のオフセットを補正する角速度センサーのオフセット補正方法であって、前記加速度センサーの出力に基づいて、前記角速度センサーに加わる角速度が0であるか否かを判定する判定ステップと、前記判定ステップで前記角速度が0であると判定された時に、前記角速度センサーの出力のオフセットを補正するオフセット補正ステップと、を含む、角速度センサーのオフセット補正方法である。
【0025】
(11)この角速度センサーのオフセット補正方法において、前記判定ステップは、前記加速度センサーの出力に基づいて前記センサー装置に加わる加速度を算出し、算出した加速度と重力加速度が略一致する状態を初期状態として検出する初期状態検出ステップと、前記初期状態における前記センサー装置の姿勢情報に基づいて前記初期状態における各検出軸方向の軸ベクトルを算出し、当該軸ベクトルと各検出軸方向の加速度成分と当該加速度成分の時間変化とに基づいて前記センサー装置が任意の軸回りに回転する角速度を算出する角速度算出ステップと、前記角速度算出ステップで算出した角速度が略0である場合に、前記角速度センサーに加わる角速度が0であると判定する角速度判定ステップと、を含むようにしてもよい。
【図面の簡単な説明】
【0026】
【図1】第1実施形態のセンサー装置の概略構成図。
【図2】第1実施形態のオフセット補正処理を行うタイミングの判定原理について説明するための図。
【図3】第1実施形態のオフセット補正処理の第1の例を示すフローチャート図。
【図4】第1実施形態のオフセット補正処理の第2の例を示すフローチャート図。
【図5】第2実施形態のセンサー装置の概略構成図。
【図6】Z−X−Z系のオイラー角について説明するための図。
【図7】第2実施形態のオフセット補正処理の第1の例を示すフローチャート図。
【図8】第2実施形態のオフセット補正処理の第2の例を示すフローチャート図。
【図9】第3実施形態のセンサー装置の概略構成図。
【図10】第3実施形態のオフセット補正処理の第1の例を示すフローチャート図。
【図11】第3実施形態のオフセット補正処理の第2の例を示すフローチャート図。
【図12】本実施形態の電子機器の構成例を示す機能ブロック図。
【発明を実施するための形態】
【0027】
以下、本発明の好適な実施形態について図面を用いて詳細に説明する。なお、以下に説明する実施の形態は、特許請求の範囲に記載された本発明の内容を不当に限定するものではない。また以下で説明される構成の全てが本発明の必須構成要件であるとは限らない。
【0028】
1.センサー装置
(1)第1実施形態
[構成]
図1は、第1実施形態のセンサー装置の概略構成図である。
【0029】
図1に示すように、第1実施形態のセンサー装置1Aは、3つの加速度センサー10x、10y、10zを含む。加速度センサー10x、10y、10zは、検出軸が互いに直交するように固定して配置されており、それぞれの検出軸方向に加わる加速度の大きさに応じたDC電圧の検出信号を出力する。
【0030】
また、センサー装置1Aは、3つの角速度センサー(ジャイロセンサー)20x、20y、20zを含む。角速度センサー20x、20y、20zは、検出軸が互いに直交するように固定して配置されており、それぞれの検出軸回りに加わる角速度の大きさに応じたDC電圧の検出信号を出力する。
【0031】
本実施形態では、加速度センサー10xと角速度センサー20xは互いの検出軸が一致するように配置されており、この検出軸をx軸とする。同様に、加速度センサー10yと角速度センサー20yは互いの検出軸が一致するように配置されており、この検出軸をy軸とする。同様に、加速度センサー10zと角速度センサー20zは互いの検出軸が一致するように配置されており、この検出軸をz軸とする。
【0032】
センサー装置1Aは、判定部30を含む。判定部30は、加速度センサー10x、10y、10zの出力に基づいて、角速度センサー20x、20y、20zに加わる角速度が0であるか否かを判定する。
【0033】
本実施形態では、判定部30は、加速度算出部110を含む。加速度算出部110は、加速度センサー10x、10y、10zの出力に基づいてセンサー装置1Aに加わる加速度を算出する。本実施形態では、加速度算出部110は、加速度センサー10x、10y、10zの各出力電圧を所定の周期でサンプリングし、A/D変換して得られるx軸、y軸、z軸方向の加速度成分gx、gy、gzの2乗和gx2+gy2+gz2を計算する。
【0034】
また、本実施形態では、判定部30は、角速度判定部120を含む。角速度判定部120は、加速度算出部110が算出した加速度と重力加速度Gが略一致する場合に、角速度センサー20x、20y、20zに加わる角速度が0であると判定する。本実施形態では、角速度判定部120は、gx2+gy2+gz2=G2であれば、角速度センサー20x、20y、20zに加わる角速度が0であると判定する。
【0035】
例えば、角速度判定部120は、加速度算出部110が算出した加速度と重力加速度Gが略一致する状態が所定時間継続した場合に、角速度センサー20x、20y、20zに加わる角速度が0であると判定するようにしてもよい。
【0036】
センサー装置1Aは、オフセット補正部40を含む。オフセット補正部40は、判定部30により角速度センサー20x、20y、20zに加わる角速度が0であると判定された時に、角速度センサー20x、20y、20zの出力のオフセット(ゼロ点オフセット)を補正する。本実施形態では、角速度センサー20x、20y、20zはゼロ点電圧の調整機能を有しており、オフセット補正部40は、オフセット補正処理のタイミングにおける角速度センサー20x、20y、20zの出力電圧と仕様上のゼロ点電圧との誤差に応じたゼロ点調整信号を角速度センサー20x、20y、20zに供給することで、これらの出力電圧を仕様上のゼロ点電圧と一致させる。
【0037】
このように、本実施形態のセンサー装置1Aは、加速度センサー10x、10y、10zの出力電圧を周期的にサンプリングして順次得られる加速度の2乗和(gx2+gy2+gz2)と重力加速度の2乗(G2)が一致したタイミングで、角速度センサー20x、20y、20zの出力のオフセット(ゼロ点オフセット)を自動的に補正する。
【0038】
[オフセット補正タイミングの判定原理]
次に、本実施形態における、角速度センサー20x、20y、20zのオフセット補正処理を行うタイミングの判定原理について、図2(A)及び図2(B)を用いて説明する。
【0039】
センサー装置1Aを地球上で使用する場合、センサー装置1Aには常に重力が加わる。そのため、センサー装置1Aが停止もしくは等速直線運動をしていれば、図2(A)に示すように、加速度センサー10x、10y、10zは重力加速度のみを検出する。図2(A)において、gx、gy、gzはx軸、y軸、z軸方向の加速度成分を、Gは重力加速度をそれぞれ表している。すなわち、gx、gy、gzのベクトル和はGと一致し、次の式(1)が成立する。
【0040】
【数1】
【0041】
一方、センサー装置1Aが任意の軸回りの回転運動をすれば遠心力が加わるため、図2(B)に示すように、加速度センサー10x、10y、10zは重力加速度と遠心力による遠心加速度とを検出する。図2(B)において、gx、gy、gzはx軸、y軸、z軸方向の加速度成分を、Gは重力加速度を、Rは遠心加速度を、nは回転軸をそれぞれ表している。すなわち、gx、gy、gzのベクトル和はGとRのベクトル和と一致し、次の式(2)が成立する。
【0042】
【数2】
【0043】
ここで、センサー装置1Aが停止もしくは等速直線運動をしていれば(回転運動をしていなければ)、センサー装置1Aには角速度が加わらないので、角速度センサー20x、20y、20zはすべてゼロ点電圧を出力する。そのため、式(1)が成立する時は角速度センサー20x、20y、20zのオフセット補正(ゼロ点補正)を行うことができる。
【0044】
一方、センサー装置1Aが任意の軸回りの回転運動をしていれば、センサー装置1Aにはこの回転運動の角速度が加わり、角速度センサー20x、20y、20zは、この角速度のx軸成分、y軸成分、z軸成分を検出することになる。そのため、角速度センサー20x、20y、20zの少なくとも1つの出力電圧はゼロ点電圧にならないので、式(2)が成立する(式(1)が成立しない)時はオフセット補正(ゼロ点補正)を行うことができない。
【0045】
そこで、角速度判定部120は、式(1)が成立するか否かによってセンサー装置1Aに加わる角速度が0か否かを判定し、角速度センサー20x、20y、20zのオフセット補正処理のタイミングを制御する。ただし、実際には、加速度センサー10x、10y、10zの出力オフセットやセンサー装置1Aの使用環境におけるノイズ等の環境因により、センサー装置1Aが停止若しくは等速運動をしていても加速度センサー10x、10y、10zによって検出される加速度gが重力加速度Gと微妙に一致しないことが想定される。そのため、角速度判定部120は、例えば、gx2+gy2+gz2とG2の差が微小値Δ以下であれば、すなわちG2−Δ≦gx2+gy2+gz2≦G2+Δを満たせば、センサー装置1Aに加わる角速度が0であると判定するようにしてもよい。この微小値Δは、センサー装置1Aを使用するアプリケーション側の仕様等によって適切な値が設定される。
【0046】
[オフセット補正処理]
図3は、本実施形態における、角速度センサー20x、20y、20zのオフセット補正処理の第1の例を示すフローチャート図である。
【0047】
まず、加速度算出部110が、加速度センサー10x、10y、10zの検出信号をサンプリングし、x軸、y軸、z軸方向の加速度成分gx,gy,gzを計算する(ステップS10)。
【0048】
次に、加速度算出部110がgx2+gy2+gz2を計算する(ステップS20)。
【0049】
次に、角速度判定部120がgx2+gy2+gz2とG2が一致するか否かを判定し(ステップS30)、gx2+gy2+gz2≠G2の場合は(ステップS30のN)、ステップS10とS20の処理を再度行う。
【0050】
一方、gx2+gy2+gz2=G2の場合は(ステップS30のY)、オフセット補正部40が角速度センサー20x、20y、20zのオフセット補正を行う(ステップS40)。
【0051】
図4は、本実施形態における、角速度センサー20x、20y、20zのオフセット補正処理の第2の例を示すフローチャート図である。図4において、図3と同じ処理を行うステップには同じ符号を付しており、その説明を省略又は簡略する。
【0052】
まず、角速度判定部120が所定時間T1の計測を開始する(ステップS2)。
【0053】
次に、加速度算出部110がステップS10とS20の処理を行った後、角速度判定部120がgx2+gy2+gz2とG2が一致するか否かを判定し(ステップS30)、gx2+gy2+gz2≠G2の場合は(ステップS30のN)、角速度判定部120が所定時間T1の計測を再度開始し(ステップS2)、加速度算出部110がステップS10とS20の処理を再度行う。
【0054】
一方、gx2+gy2+gz2=G2の場合は(ステップS30のY)、角速度判定部120が、ステップS2で計測を開始してから所定時間T1が経過したか否かを判定し(ステップS32)、所定時間T1が経過していない場合は(ステップS32のN)、所定時間T1が経過するまで、ステップS10、S20、S30の処理を繰り返し行う。ただし、所定時間T1が経過する前に、ステップS30の判定処理でgx2+gy2+gz2≠G2となった場合は、角速度判定部120が所定時間T1の計測を再度開始する。
【0055】
所定時間T1が経過した場合は(ステップS32のY)、オフセット補正部40が角速度センサー20x、20y、20zのオフセット補正を行う(ステップS40)。
【0056】
このようにすれば、センサー装置1Aに所定時間T1以上継続して遠心力が加わっていないのか瞬時的に遠心力が0となっただけなのかを区別し、前者の場合だけ角速度センサー20x、20y、20zのオフセット補正を行うようにすることができる。
【0057】
以上に説明した第1実施形態のセンサー装置によれば、加速度センサーが検出する加速度が重力加速度と一致するか否かを判定する簡便な演算処理により、遠心力が加わっていない適切なタイミングで角速度センサーのオフセットを補正することができる。すなわち、第1実施形態のセンサー装置によれば、アプリケーション機能によらずセンサー装置単体で角速度センサーのオフセットを補正することができる。そのため、このセンサー装置を使用すれば、従来の手法のようにアプリケーション側に補正アルゴリズムを実装する必要がなく、ユーザーに負荷をかけずに角速度センサーのゼロ点オフセットを補正することができる。
【0058】
また、第1実施形態のセンサー装置によれば、使用環境下で角速度センサーのゼロ点の変動を検知して補正することができる。そのため、場合によっては、出荷前に角速度センサーのオフセット調整をs実施したり、温度特性の補正用データを記憶素子に書き込む必要がなくなる。さらに、使用環境に応じて角速度センサーのオフセット補正量を調整することができるので、温度変化にも対応して角速度センサーのオフセット補正を行うことができる。特に、温度特性の補正用データがあったととしても補正が困難な非線形の温度変化にも対応することができる。
【0059】
(2)第2実施形態
[構成]
図5は、第2実施形態のセンサー装置の概略構成図である。図5において、図1と同じ構成要素には同じ符号を付しており、その説明を省略又は簡略する。
【0060】
図5に示すように、第2実施形態のセンサー装置1Bは、3つの加速度センサー10x、10y、10z、3つの角速度センサー(ジャイロセンサー)20x、20y、20zに加えて、3つの地磁気センサー60x、60y、60zを含んで構成されている。地磁気センサー60x、60y、60zは、検出軸(x軸、y軸、z軸)が互いに直交するように固定して配置されており、それぞれの検出軸方向に加わる地磁気の大きさに応じたDC電圧の検出信号を出力する。
【0061】
本実施形態では、加速度センサー10x、角速度センサー20x、地磁気センサー60xは互いの検出軸が一致するように配置されており、この検出軸をx軸とする。同様に、加速度センサー10y、角速度センサー20y、地磁気センサー60yは互いの検出軸が一致するように配置されており、この検出軸をy軸とする。同様に、加速度センサー10z、角速度センサー20z、地磁気センサー60zは互いの検出軸が一致するように配置されており、この検出軸をz軸とする。
【0062】
本実施形態では、このx軸、y軸、z軸を3つの座標軸とする直交座標系をセンサー座標系と呼ぶことにする。一方、空間上に固定された3つの直交する座標軸(X軸、Y軸、Z軸)による直交座標系を絶対座標系と呼ぶことにする。絶対座標系のX軸、Y軸、Z軸は、例えば、X軸の正の方向が北、Y軸の正の方向が西、Z軸の正の方向が重力方向と一致するように定義される。
【0063】
センサー装置1Bは、姿勢情報検出部70を含んで構成されている。姿勢情報検出部70は、加速度センサー10x、10y、10z、角速度センサー20x、20y、20z、地磁気センサー60x、60y、60zの各検出信号に基づいて、センサー装置1Bの現在の姿勢情報を算出する。
【0064】
本実施形態では、絶対座標系(XYZ座標系)とセンサー座標系(xyz座標系)が一致する状態を基準としてセンサー装置1Bの姿勢をZ−X−Z系のオイラー角で表現する。Z−X−Z系のオイラー角は、図6(A)に示すように、絶対座標系(XYZ座標系)をZ軸回りに角度φだけ回転させたX’Y’Z’座標系に変換し、次にX’Y’Z’座標系をX’軸回りに角度θだけ回転させたX’’Y’’Z’’座標系に変換し、最後にX’’Y’’Z’’座標系をZ’’軸回りに角度ψだけ回転させたX’’’Y’’’Z’’’座標系に変換した時、X’’’Y’’’Z’’’座標系がセンサー座標系(xyz座標系)と一致する時の(φ,θ,ψ)である。本実施形態では、Z−X−Z系のオイラー角(φ,θ,ψ)を、姿勢角(φ,θ,ψ)ということにする。
【0065】
つまり、本実施形態では、姿勢情報検出部70は、姿勢情報として姿勢角(φ,θ,ψ)の情報を出力する。
【0066】
センサー装置1Bは、判定部50を含む。判定部50は、加速度センサー10x、10y、10zの出力に基づいて、角速度センサー20x、20y、20zに加わる角速度が0であるか否かを判定する。
【0067】
本実施形態では、判定部50は、初期状態検出部210を含む。初期状態検出部210は、加速度センサー10x、10y、10zの出力に基づいてセンサー装置1Aに加わる加速度を算出し、算出した加速度と重力加速度Gが略一致する状態を初期状態として検出する。本実施形態では、初期状態検出部210は、加速度センサー10x、10y、10zの各出力電圧を所定の周期でサンプリングし、A/D変換して得られるx軸、y軸、z軸方向の加速度成分gx、gy、gzの2乗和gx2+gy2+gz2を計算し、gx2+gy2+gz2=G2の状態を初期状態として検出する。
【0068】
ただし、実際には、加速度センサー10x、10y、10zの出力オフセットやセンサー装置1Bの使用環境におけるノイズ等の環境因により、センサー装置1Bが停止若しくは等速運動をしていても加速度センサー10x、10y、10zによって検出される加速度gが重力加速度Gと微妙に一致しないことが想定される。そのため、初期状態検出部210は、例えば、gx2+gy2+gz2とG2の差が微小値Δ以下であれば、すなわちG2−Δ≦gx2+gy2+gz2≦G2+Δを満たした時に初期状態を検出する。この微小値Δは、センサー装置1Bを使用するアプリケーション側の仕様等によって適切な値が設定される。
【0069】
ここで、図2(B)に示した遠心加速度Rがある程度大きい場合にはgx2+gy2+gz2=G2が成立しないので、初期状態検出部210により初期状態が検出されることはないが、遠心加速度Rが極めて小さい場合には初期状態検出部210により初期状態が検出される場合が想定される。そこで、以下に説明するように、判定部50は、初期状態検出部210により初期状態が検出された場合は、その後の加速度検出成分の時間変化をモニターすることで実際の角速度ωが0か否かを判定する。
【0070】
そのため、本実施形態では、判定部50は、角速度算出部220を含む。角速度算出部220は、初期状態におけるセンサー装置1Bの姿勢情報に基づいて初期状態におけるx軸、y軸、z軸方向の軸ベクトルを算出し、当該軸ベクトルとx軸、y軸、z軸方向の加速度成分と当該加速度成分の時間変化とに基づいてセンサー装置1Bが任意の軸回りに回転する角速度ωを算出する。本実施形態では、角速度算出部220は、後述する式(7)、式(8)、式(9)により、軸ベクトルとしてx軸、y軸、z軸方向の単位ベクトルaxR、ayR、azRを計算する。また、角速度算出部220は、後述する式(24)、式(25)、式(26)により、加速度センサー10x、10y、10zが検出する加速度成分gx、gy、gzを含むD1、D2、D3を計算する。さらに、角速度算出部220は、D1、D2、D3の1階微分(実際には1階差分)と2階微分(実際には2階差分))を計算する。そして、角速度算出部220は、単位ベクトルaxR、ayR、azRと、D1、D2、D3と、D1、D2、D3の1階微分と、D1、D2、D3の2階微分を用いて、後述する式(40)によりω2を計算する。
【0071】
また、本実施形態では、判定部50は、角速度判定部230を含む。角速度判定部230は、角速度算出部220が算出した角速度が略0である場合に、角速度センサー20x、20y、20zに加わる角速度が0であると判定する。本実施形態では、角速度判定部230は、ω2=0であれば、角速度センサー20x、20y、20zに加わる角速度が0であると判定する。
【0072】
例えば、角速度判定部230は、角速度算出部220が算出した角速度ωが略0の状態が所定時間継続した場合に、角速度センサー20x、20y、20zに加わる角速度が0であると判定するようにしてもよい。
【0073】
なお、オフセット補正部40は、第1実施形態と同様であるため、その説明を省略する。
【0074】
このように、本実施形態のセンサー装置1Bは、初期状態におけるx軸、y軸、z軸方向の軸ベクトルと、加速度センサー10x、10y、10zの出力電圧を周期的にサンプリングして順次得られるx軸、y軸、z軸方向の加速度成分と、当該加速度成分の時間変化とに基づいて計算される角速度ωの2乗が0となるタイミングで、角速度センサー20x、20y、20zの出力のオフセット(ゼロ点オフセット)を自動的に補正する。
【0075】
[オフセット補正タイミングの判定原理]
次に、本実施形態における、角速度センサー20x、20y、20zのオフセット補正処理を行うタイミングの判定原理について説明する。
【0076】
本実施形態では、絶対座標系(XYZ座標系)とセンサー座標系(xyz座標系)が一致する状態を基準として、センサー装置1Bの姿勢をZ−X−Z系のオイラー角で表現する。
【0077】
図6(A)に示すように、絶対座標系(XYZ座標系)をZ軸回りに角度φだけ回転させたX’Y’Z’座標系に変換する回転行列R1は次式(3)で与えられる。
【0078】
【数3】
【0079】
さらに、図6(B)に示すように、X’Y’Z’座標系をX’軸回りに角度θだけ回転させたX’’Y’’Z’’座標系に変換する回転行列R2は次式(4)で与えられる。
【0080】
【数4】
【0081】
さらに、図6(C)に示すように、X’’Y’’Z’’座標系をZ’’軸回りに角度ψだけ回転させたX’’’Y’’’Z’’’座標系に変換する回転行列R3は次式(5)で与えられる。
【0082】
【数5】
【0083】
このX’’’Y’’’Z’’’座標系がセンサー座標系(xyz座標系)と一致するなら、センサー装置1Bの姿勢を表現するZ−X−Z系のオイラー角(姿勢角)は(φ,θ,ψ)である。
【0084】
従って、センサー装置1Bの姿勢角を(0,0,0)から(φ,θ,ψ)に変換する回転行列Rは、次式(6)で与えられる。
【0085】
【数6】
【0086】
本実施形態では、センサー装置1Bが基準姿勢の時(姿勢角が(0,0,0)の時)は、絶対座標系(XYZ座標系)とセンサー座標系(xyz座標系)が一致するので、x軸、y軸、z軸方向の3つの単位ベクトルは、それぞれ絶対座標系(XYZ座標系)において(1,0,0)、(0,1,0)、(0,0,1)で表される。
【0087】
本実施形態では、加速度センサー10x、10y、10zの検出軸は常にセンサー座標系のx軸、y軸、z軸とそれぞれ一致するので、センサー装置1Bが基準姿勢の時(姿勢角が(0,0,0)の時)、加速度センサー10x、10y、10zの検出軸方向の単位ベクトルax、ay、azは、それぞれax=(1,0,0)、ay=(0,1,0)、az=(0,0,1)になっている。
【0088】
従って、センサー装置1Bの姿勢角が(φ,θ,ψ)の時のx軸、y軸、z軸方向の単位ベクトルaxR、ayR、azRは、それぞれ次の式(7)、式(8)、式(9)で表される。
【0089】
【数7】
【0090】
【数8】
【0091】
【数9】
【0092】
次に、センサー装置1Bが姿勢角(φ,θ,ψ)で停止又は等速直線運動していると仮定し、この状態からセンサー装置1Bが任意の軸ベクトルn=(nx、ny、nz)の回りに角度δだけ回転する場合を考えると、この回転を表す回転行列Δは、次式(10)で表される。
【0093】
【数10】
【0094】
したがって、軸ベクトルn=(nx、ny、nz)の回りに角度δだけ回転した後のx軸、y軸、z軸方向の単位ベクトルaxRΔ、ayRΔ、azRΔは、それぞれ次の式(11)、式(12)、式(13)で表される。
【0095】
【数11】
【0096】
【数12】
【0097】
【数13】
【0098】
従って、単位ベクトルaxRΔ、ayRΔ、azRΔが、重力方向の単位ベクトルp=(0,0,1)とそれぞれ成す角度γx、γy、γzは、それぞれ次の式(14)、式(15)、式(16)で計算される。
【0099】
【数14】
【0100】
【数15】
【0101】
【数16】
【0102】
ここで、図2(B)で示した遠心加速度Rが極めて小さい場合は、加速度センサー10x、10y、10zが検出する加速度成分gx、gy、gzは、重力加速度Gのx軸、y軸、z軸方向の成分と一致する。すなわち、gx=G・cosγx、gy=G・cosγy、gz=G・cosγzなので、式(14)、式(15)、式(16)より、それぞれ次の式(17)、式(18)、式(19)が導かれる。なお、式(17)、式(18)、式(19)におけるD1、D2、D3はそれぞれ式(20)、式(21)、式(22)で与えられる。
【0103】
【数17】
【0104】
【数18】
【0105】
【数19】
【0106】
【数20】
【0107】
【数21】
【0108】
【数22】
【0109】
式(17)、式(18)、式(19)をまとめると、次の式(23)が得られる。
【0110】
【数23】
【0111】
この式(23)に対してクラメルの公式を適用すると、D1、D2、D3がそれぞれ次の式(24)、式(25)、式(26)により求まる。
【0112】
【数24】
【0113】
【数25】
【0114】
【数26】
【0115】
D1、D2、D3は、それぞれ式(20)、式(21)、式(22)により与えられるので、D1、D2、D3の1階微分はそれぞれ次の式(27)、式(28)、式(29)で計算される。
【0116】
【数27】
【0117】
【数28】
【0118】
【数29】
【0119】
D1、D2、D3の2階微分は、式(27)、式(28)、式(29)をさらに微分して、それぞれ次の式(30)、式(31)、式(32)で計算される。
【0120】
【数30】
【0121】
【数31】
【0122】
【数32】
【0123】
ここで、式(20)、式(21)、式(22)から、次式(33)の関係式が得られる。
【0124】
【数33】
【0125】
また、式(27)、式(28)、式(29)から、次式(34)の関係式が得られる。
【0126】
【数34】
【0127】
また、式(30)、式(31)、式(32)から、次式(35)の関係式が得られる。
【0128】
【数35】
【0129】
式(33)、式(34)、式(35)をまとめると、次の式(36)が得られる。
【0130】
【数36】
【0131】
この式(36)に対してクラメルの公式を適用すると、nx、ny、nzがそれぞれ次の式(37)、式(38)、式(39)により求まる。
【0132】
【数37】
【0133】
【数38】
【0134】
【数39】
【0135】
最後に、式(27)と式(28)からδを消去することで、次の式(40)により、センサー装置1Bに加わる角速度ωの2乗が求まる。
【0136】
【数40】
【0137】
ここで、センサー装置1Bが初期状態のまま停止もしくは等速直線運動をしていれば(回転運動をしていなければ)、センサー装置1Bには角速度が加わらないので、角速度センサー20x、20y、20zはすべてゼロ点電圧を出力する。この時、gx、gy、gzの時間変化は0でありD1、D2、D3は一定値となるため、D1、D2、D3を1階微分した値はすべて0になる。そのため、式(40)により計算されるω2が0になる。
【0138】
一方、センサー装置1Bが任意の軸n=(nx、ny、nz)回りの回転運動をしていれば、センサー装置1Bにはこの回転運動の角速度が加わり、角速度センサー20x、20y、20zは、この角速度のx軸成分、y軸成分、z軸成分を検出することになる。この時、gx、gy、gzが時間変化するため、式(40)により計算されるω2が0にならない。
【0139】
そこで、角速度判定部230は、式(40)で計算されるω2が0になるか否かによってセンサー装置1Bに加わる角速度が0か否かを判定し、角速度センサー20x、20y、20zのオフセット補正処理のタイミングを制御する。ただし、実際には、加速度センサー10x、10y、10zの出力オフセットやセンサー装置1Bの使用環境におけるノイズ等の環境因により、センサー装置1Bが停止若しくは等速運動をしていても加速度センサー10x、10y、10zによって検出される加速度gが重力加速度Gと微妙に一致しないことが想定される。そのため、角速度判定部230は、例えば、式(40)で計算されるω2が微小値Δ以下であれば、すなわちω2≦Δを満たせば、センサー装置1Bに加わる角速度が0であると判定するようにしてもよい。この微小値Δは、センサー装置1Bを使用するアプリケーション側の仕様等によって適切な値が設定される。
【0140】
[オフセット補正処理]
図7は、本実施形態における、角速度センサー20x、20y、20zのオフセット補正処理の第1の例を示すフローチャート図である。
【0141】
まず、初期状態検出部210が加速度センサー10x、10y、10zの検出信号をサンプリングし、x軸、y軸、z軸方向の加速度成分gx,gy,gzを計算する(ステップS110)。
【0142】
次に、初期状態検出部210がg2=gx2+gy2+gz2を計算する(ステップS120)。
【0143】
次に、初期状態検出部210がgx2+gy2+gz2とG2が一致するか否かを判定し(ステップS130)、g2≠G2(初期状態を未検出)の場合は(ステップS130のN)、ステップS110とS120の処理を再度行う。
【0144】
一方、g2=G2の場合は(ステップS130のY)、初期状態検出部210が初期状態を検出し(ステップS140)、角速度算出部220が、式(7)、式(8)、式(9)により、姿勢情報算出部70が算出する姿勢角(φ,θ,ψ)から、初期状態における加速度センサー10x、10y、10zのx軸方向、y軸方向、軸方向の単位ベクトルaxR,ayR,azRを計算する(ステップS150)。
【0145】
次に、角速度算出部220が、ステップS150で得られた単位ベクトルaxR,ayR,azRとステップS110で得られた加速度成分gx,gy,gzから、式(24)、式(25)、式(26)により、D1,D2,D3を計算する(ステップS160)。
【0146】
次に、角速度算出部220が、D1,D2,D3の1階微分を計算する(ステップS170)。具体的には、角速度算出部220が、ステップS160で計算した今回のD1,D2,D3と前回のD1,D2,D3(1サンプル前のgx,gy,gzから計算したD1,D2,D3)との各差分(D1,D2,D3の1階差分)を計算する。
【0147】
次に、角速度算出部220が、式(30)、式(31)、式(32)により、D1,D2,D3の2階微分を計算する(ステップS180)。具体的には、角速度算出部220が、ステップS170で計算した今回のD1,D2,D3の1階差分と前回のD1,D2,D3の1階差分(1サンプル前のgx,gy,gzから計算したD1,D2,D3の1階差分)との各差分(D1,D2,D3の2階差分)を計算する。
【0148】
次に、角速度算出部220が、式(40)により、ω2を計算する(ステップS190)。
【0149】
次に、角速度判定部230がω2=0か否かを判定し(ステップS200)、ω2≠0の場合は(ステップS200のN)、ステップS110〜S190の処理を再度行う。
【0150】
一方、ω2=0の場合は(ステップS200のY)、オフセット補正部40が角速度センサー20x、20y、20zのオフセット補正を行う(ステップS210)。
【0151】
図8は、本実施形態における、角速度センサー20x、20y、20zのオフセット補正処理の第2の例を示すフローチャート図である。図8において、図7と同じ処理を行うステップには同じ符号を付しており、その説明を省略又は簡略する。
【0152】
まず、初期状態検出部210がステップS110〜S140の処理を行う。
【0153】
まず、角速度判定部230が所定時間T2の計測を開始する(ステップS142)。
【0154】
次に、角速度算出部220がステップS150〜S190の処理を行った後、角速度判定部230がω2=0か否かを判定し(ステップS200)、ω2≠0の場合は(ステップS200のN)、初期状態検出部210が初期状態の検出処理(ステップS110〜S140の処理)を再度行う。
【0155】
一方、ω2=0の場合は(ステップS200のY)、角速度判定部230が、ステップS142で計測を開始してから所定時間T2が経過したか否かを判定し(ステップS202)、所定時間T2が経過していない場合は(ステップS202のN)、初期状態検出部210が加速度センサー10x、10y、10zの検出信号をサンプリングしてx軸、y軸、z軸方向の加速度成分gx,gy,gzを計算し(ステップS204)、所定時間T2が経過するまで、ステップS150〜S200の処理を繰り返し行う。ただし、所定時間T2が経過する前に、ステップS200の判定処理でω2≠0となった場合は、初期状態検出部210が初期状態の検出処理(ステップS110〜S140の処理)を再度行う。
【0156】
所定時間T2が経過した場合は(ステップS202のY)、オフセット補正部40が角速度センサー20x、20y、20zのオフセット補正を行う(ステップS210)。
【0157】
このようにすれば、センサー装置1Bに所定時間T2以上継続して角速度が加わっていないのか瞬時的に角速度が0となっただけなのかを区別し、前者の場合だけ角速度センサー20x、20y、20zのオフセット補正を行うようにすることができる。
【0158】
以上に説明した第2実施形態のセンサー装置によれば、第1実施形態のセンサー装置と同様の効果を奏することができる。さらに、第2実施形態のセンサー装置によれば、加速度センサーが検知できないような小さい遠心力が加わった場合でも、初期状態からの加速度成分の時間変化をモニターすることで、角速度センサーに加わる角速度を検知することができる。そのため、角速度センサーのオフセット補正を行うタイミングの判定精度を高めることができる。
【0159】
(3)第3実施形態
[構成]
図9は、第3実施形態のセンサー装置の概略構成図である。図9において、図5と同じ構成要素には同じ符号を付しており、その説明を省略又は簡略する。
【0160】
図9に示すように、第3実施形態のセンサー装置1Cは、図5の判定部40に代えて判定部80を含む。
【0161】
本実施形態では、判定部80は、図5と同じ初期状態検出部210と角速度算出部220を含んで構成されている。
【0162】
また、本実施形態では、判定部80は、判定モード制御部240を含む。判定モード制御部240は、制御信号に応じて第1の判定モードと第2の判定モードを切り替える。この制御信号は、センサー装置1Cの外部から供給されてもよいし、センサー装置1Cの内部で生成してもよい。
【0163】
また、本実施形態では、判定部80は、角速度判定部250を含む。角速度判定部250は、第1の判定モードでは、初期状態検出部210が初期状態を検出した場合に、角速度センサー20x、20y、20zに加わる角速度が0であると判定する。本実施形態では、角速度判定部250は、第1の判定モードでは、gx2+gy2+gz2=G2であれば、角速度センサー20x、20y、20zに加わる角速度が0であると判定する。
【0164】
また、角速度判定部250は、第2の判定モードでは、角速度算出部220が算出した角速度が略0である場合に、角速度センサー20x、20y、20zに加わる角速度が0であると判定する。本実施形態では、角速度判定部250は、第2の判定モードでは、ω2=0であれば、角速度センサー20x、20y、20zに加わる角速度が0であると判定する。
【0165】
例えば、角速度判定部250は、第1の判定モードでは、初期状態検出部210が所定時間継続して初期状態を検出した場合に、角速度センサー20x、20y、20zに加わる角速度が0であると判定するようにしてもよい。また、例えば、角速度判定部250は、第2の判定モードでは、角速度算出部220が算出した角速度ωが略0の状態が所定時間継続した場合に、角速度センサー20x、20y、20zに加わる角速度が0であると判定するようにしてもよい。
【0166】
このように、本実施形態のセンサー装置1Cは、第1の判定モードでは、加速度センサー10x、10y、10zの出力電圧を周期的にサンプリングして順次得られる加速度の2乗和(gx2+gy2+gz2)と重力加速度の2乗(G2)が一致したタイミングで、角速度センサー20x、20y、20zの出力のオフセット(ゼロ点オフセット)を自動的に補正する。また、本実施形態のセンサー装置1Cは、第2の判定モードでは、初期状態におけるx軸、y軸、z軸方向の軸ベクトルと、加速度センサー10x、10y、10zの出力電圧を周期的にサンプリングして順次得られるx軸、y軸、z軸方向の加速度成分と、当該加速度成分の時間変化とに基づいて計算される角速度ωの2乗が0となるタイミングで、角速度センサー20x、20y、20zの出力のオフセット(ゼロ点オフセット)を自動的に補正する。
【0167】
[オフセット補正処理]
図10は、本実施形態における、角速度センサー20x、20y、20zのオフセット補正処理の第1の例を示すフローチャート図である。図10において、図7と同じ処理を行うステップには同じ符号を付しており、その説明を省略又は簡略する。
【0168】
まず、初期状態検出部210が初期状態の検出処理(ステップS110〜S140の処理)を行う。
【0169】
次に、第1の判定モードであれば(ステップS144のY)、オフセット補正部40が角速度センサー20x、20y、20zのオフセット補正を行う(ステップS210)。
【0170】
第1の判定モードでなければ(すなわち第2の判定モードであれば)(ステップS144のN)、角速度算出部220がステップS150〜S190の処理を行った後、角速度判定部250がω2=0か否かを判定し(ステップS200)、ω2≠0の場合は(ステップS200のN)、初期状態検出部210が初期状態の検出処理(ステップS110〜S140の処理)を再度行う。
【0171】
一方、ω2=0の場合は(ステップS200のY)、オフセット補正部40が角速度センサー20x、20y、20zのオフセット補正を行う(ステップS210)。
【0172】
図11は、本実施形態における、角速度センサー20x、20y、20zのオフセット補正処理の第2の例を示すフローチャート図である。図11において、図10と同じ処理を行うステップには同じ符号を付しており、その説明を省略又は簡略する。
【0173】
まず、第1の判定モードであれば(ステップS102のY)、角速度判定部250が所定時間T1の計測を開始する(ステップS104)。
【0174】
次に、初期状態検出部210が初期状態の検出処理(ステップS110〜S140の処理)を行う。
【0175】
次に、第1の判定モードであれば(ステップS144のY)、角速度判定部250が、ステップS104で計測を開始してから所定時間T1が経過したか否かを判定し(ステップS145)、所定時間T1が経過していない場合は(ステップS145のN)、所定時間T1が経過するまで、初期状態検出部210がステップS110〜S140の処理を再度行う。そして、所定時間T1が経過した場合は(ステップS145のY)、オフセット補正部40が角速度センサー20x、20y、20zのオフセット補正を行う(ステップS210)。
【0176】
第1の判定モードでなければ(すなわち第2の判定モードであれば)(ステップS144のN)、角速度判定部250が所定時間T2の計測を開始する(ステップS148)。
【0177】
次に、角速度算出部220がステップS150〜S190の処理を行った後、角速度判定部250がω2=0か否かを判定し(ステップS200)、ω2≠0の場合は(ステップS200のN)、初期状態検出部210が初期状態の検出処理(ステップS110〜S140の処理)を再度行う。
【0178】
一方、ω2=0の場合は(ステップS200のY)、角速度判定部250が、ステップS148で計測を開始してから所定時間T2が経過したか否かを判定し(ステップS202)、所定時間T2が経過していない場合は(ステップS202のN)、初期状態検出部210が加速度センサー10x、10y、10zの検出信号をサンプリングしてx軸、y軸、z軸方向の加速度成分gx,gy,gzを計算し(ステップS204)、所定時間T2が経過するまで、ステップS150〜S200の処理を繰り返し行う。ただし、所定時間T2が経過する前に、ステップS200の判定処理でω2≠0となった場合は、初期状態検出部210が初期状態の検出処理(ステップS110〜S140の処理)を再度行う。
【0179】
所定時間T2が経過した場合は(ステップS202のY)、オフセット補正部40が角速度センサー20x、20y、20zのオフセット補正を行う(ステップS210)。
【0180】
このようにすれば、第1の判定モードでは、センサー装置1Cに所定時間T1以上継続して遠心力が加わっていないのか瞬時的に遠心力が0となっただけなのかを区別し、前者の場合だけ角速度センサー20x、20y、20zのオフセット補正を行うようにすることができる。また、第2の判定モードでは、センサー装置1Cに所定時間T2以上継続して角速度が加わっていないのか瞬時的に角速度が0となっただけなのかを区別し、前者の場合だけ角速度センサー20x、20y、20zのオフセット補正を行うようにすることができる。
【0181】
以上に説明した第3実施形態のセンサー装置によれば、第1実施形態のセンサー装置と同様の効果を奏することができる。さらに、第3実施形態のセンサー装置によれば、当該センサー装置を使用するアプリケーションに応じて、第1の判定モードと第2の判定モードのいずれを使用するかで、角速度センサーのオフセット補正タイミングの判定精度を変更することができる。すなわち、第1の判定モードにすれば、遠心力の検出による簡易な判定になり、第2の判定モードにすれば、加速度センサーで検出できないような微小な遠心力が加わった場合でも当該センサー装置に加わる角速度が0か否かを判定することができる。
【0182】
2.電子機器
図12は、本実施形態のセンサー装置を含む電子機器の構成例を示す機能ブロック図である。本実施形態の電子機器500は、センサー装置400、ホストCPU300、操作部310、表示部320、ROM(Read Only Memory)330、RAM(Random Access Memory)340、通信部350を含んで構成されている。
【0183】
ホストCPU(マイクロコンピューター)300は、ROM330に記憶されているプログラムに従って、各種の計算処理や制御処理を行う。具体的には、ホストCPU300は、センサー装置400に対して各種の制御コマンドを送信してセンサー装置400の動作を制御したり、センサー装置400からデータを受け取って各種の計算処理をする。また、ホストCPU300は、操作部310からの操作信号に応じた各種の処理、表示部320に各種の情報を表示させるための表示信号を送信する処理、外部とデータ通信を行うために通信部350を制御する処理等を行う。
【0184】
操作部310は、操作キーやボタンスイッチ等により構成される入力装置であり、ユーザーによる操作に応じた操作信号をホストCPU300に出力する。
【0185】
表示部320は、LCD(Liquid Crystal Display)等により構成される表示装置であり、ホストCPU300から入力される表示信号に基づいて各種の情報(例えば、ナビゲーション情報等)を表示する。
【0186】
ROM330は、ホストCPU300が各種の計算処理や制御処理を行うためのプログラムや、各種のアプリケーションプログラムやデータ等(例えば、ナビゲーションや姿勢制御を行うためのプログラムやデータ等)を記憶している。
【0187】
RAM340は、ホストCPU300の作業領域として用いられ、ROM330から読み出されたプログラムやデータ、操作部310から入力されたデータ、ホストCPU300が各種プログラムに従って実行した演算結果等を一時的に記憶する。
【0188】
通信部350は、CPU300と外部装置との間のデータ通信を成立させるための各種制御を行う。
【0189】
センサー装置400は、例えば、前述した第1実〜第3実施形態のセンサー装置1A、1B、1C等であり、電子機器500の内部において、センサーのx軸、y軸、z軸が電子機器500を基準として所定の方向に向くように固定して配置されている。そして、センサー装置400は、当該センサー装置400(電子機器500)に加わる角速度が0でるか否かを判定し、角速度が0の時に角速度センサーの0点電圧のオフセットを補正する。このセンサー装置400を電子機器500に組み込むことにより、アプリケーションによらずセンサー装置の角速度センサーのオフセットを自動的に補正することができる。
【0190】
なお、電子機器500としては、ゲームコントローラや3次元マウス等の入力機器、ラジコンヘリ等の玩具、ロボット、航法装置(ナビゲーション装置)、携帯電話機、モバイルパソコン等の種々の電子機器が挙げられる。
【0191】
なお、本発明は本実施形態に限定されず、本発明の要旨の範囲内で種々の変形実施が可能である。
【0192】
例えば、本実施形態のセンサー装置では、3つの角速度センサーを含んでいるが、1つ以上の角速度センサーを含んでいればよい。
【0193】
また、例えば、本実施形態のセンサー装置では、3つの加速度センサーを含んでいるが、3つ以上の加速度センサーを含み、それらの加速度センサーの一部又は全部の出力に基づいて互いに直交する3軸方向の加速度を算出するように変形してもよい。さらに、3つの加速度センサーは、必ずしも3つの検出軸が互いに直交するように配置されている必要はなく、3軸が互いに平行にならないように配置されていればよい。3軸が互いに直交しない場合は、アラインメント補正により、3軸方向の角速度を直交座標系における角速度に変換した後、本実施形態において説明した処理を行うようにすればよい。
【0194】
また、本実施形態のセンサー装置では、角速度センサー20x、20y、20zがゼロ点電圧の調整機能を有し、オフセット補正部40は角速度センサー20x、20y、20zにゼロ点調整信号を供給することで、オフセット補正処理を実現しているが、これに限られない。例えば、角速度センサー20x、20y、20zがゼロ点電圧の調整機能を有していない場合は、オフセット補正部40は、オフセット補正処理のタイミングにおける角速度センサー20x、20y、20zの出力電圧と仕様上のゼロ点電圧との誤差に応じた補正電圧を発生させ、この補正電圧と角速度センサー20x、20y、20zの出力電圧とをそれぞれ電圧加算することで、これらの出力電圧を仕様上のゼロ点電圧と一致させるようにしてもよい。
【0195】
また、第2、第3実施形態のセンサー装置は、姿勢情報算出部70を含む姿勢センサーとして構成されており、角速度算出部220は、姿勢情報算出部70が算出する姿勢角を初期状態における姿勢角(φ,θ,ψ)としてω2の計算を行っているが、これに限られない。センサー装置が姿勢センサーでなければ姿勢情報算出部70を含まず、角速度算出部220は、初期状態における姿勢角(φ,θ,ψ)をセンサー装置の外部から受け取って、ω2の計算を行うようにしてもよい。また、地磁気センサー60x、60y、60zは必須の構成要素ではないので省略してもよい。
【0196】
本発明は、実施の形態で説明した構成と実質的に同一の構成(例えば、機能、方法及び結果が同一の構成、あるいは目的及び効果が同一の構成)を含む。また、本発明は、実施の形態で説明した構成の本質的でない部分を置き換えた構成を含む。また、本発明は、実施の形態で説明した構成と同一の作用効果を奏する構成又は同一の目的を達成することができる構成を含む。また、本発明は、実施の形態で説明した構成に公知技術を付加した構成を含む。
【符号の説明】
【0197】
1A,1B,1C センサー装置、10x,10y,10z 加速度センサー、20x,20y,20z 角速度センサー、30 判定部、40 オフセット補正部、50 判定部、60x,60y,60z 地磁気センサー、70 姿勢情報算出部、110 加速度算出部、120 角速度判定部、210 初期状態検出部、220 角速度算出部、230 角速度判定部、240 判定モード制御部、250 角速度判定部、300 ホストCPU、310 操作部、320 表示部、330 ROM、340 RAM、350 通信部、400 センサー装置、500 電子機器
【特許請求の範囲】
【請求項1】
角速度センサーと、3つ以上の加速度センサーと、を含むセンサー装置であって、
前記加速度センサーの出力に基づいて、前記角速度センサーに加わる角速度が0であるか否かを判定する判定部と、
前記判定部により前記角速度が0であると判定された時に、前記角速度センサーの出力のオフセットを補正するオフセット補正部と、を含み、
前記判定部は、
前記加速度センサーの出力に基づいて当該センサー装置に加わる加速度を算出し、算出した加速度と重力加速度が略一致する状態を初期状態として検出する初期状態検出部と、
前記初期状態における当該センサー装置の姿勢情報に基づいて前記初期状態における各検出軸方向の軸ベクトルを算出し、当該軸ベクトルと各検出軸方向の加速度成分と当該加速度成分の時間変化とに基づいて当該センサー装置が任意の軸回りに回転する角速度を算出する角速度算出部と、
前記角速度算出部が算出した角速度が略0である場合に、前記角速度センサーに加わる角速度が0であると判定する角速度判定部と、を含む、センサー装置。
【請求項2】
請求項1において、
前記角速度判定部は、
前記角速度算出部が算出した角速度が略0の状態が所定時間継続した場合に、前記角速度センサーに加わる角速度が0であると判定する、センサー装置。
【請求項3】
請求項1において、
前記判定部は、
制御信号に応じて第1の判定モードと第2の判定モードを切り替える判定モード制御部と、
前記第1の判定モードでは、前記初期状態検出部が前記初期状態を検出した場合に前記角速度センサーに加わる角速度が0であると判定し、前記第2の判定モードでは、前記角速度算出部が算出した角速度が略0である場合に前記角速度センサーに加わる角速度が0であると判定する角速度判定部と、を含む、センサー装置。
【請求項4】
請求項3において、
前記角速度判定部は、
前記第1の判定モードでは、前記初期状態検出部が所定時間継続して前記初期状態を検出した場合に前記角速度センサーに加わる角速度が0であると判定し、前記第2の判定モードでは、前記角速度算出部が算出した角速度が略0の状態が所定時間継続した場合に前記角速度センサーに加わる角速度が0であると判定する、センサー装置。
【請求項5】
請求項1乃至4のいずれかのセンサー装置を含む、電子機器。
【請求項6】
角速度センサーと、3つ以上の加速度センサーと、を含むセンサー装置において、前記角速度センサーの出力のオフセットを補正する角速度センサーのオフセット補正方法であって、
前記加速度センサーの出力に基づいて、前記角速度センサーに加わる角速度が0であるか否かを判定する判定ステップと、
前記判定ステップで前記角速度が0であると判定された時に、前記角速度センサーの出力のオフセットを補正するオフセット補正ステップと、を含み、
前記判定ステップは、
前記加速度センサーの出力に基づいて前記センサー装置に加わる加速度を算出し、算出した加速度と重力加速度が略一致する状態を初期状態として検出する初期状態検出ステップと、
前記初期状態における前記センサー装置の姿勢情報に基づいて前記初期状態における各検出軸方向の軸ベクトルを算出し、当該軸ベクトルと各検出軸方向の加速度成分と当該加速度成分の時間変化とに基づいて前記センサー装置が任意の軸回りに回転する角速度を算出する角速度算出ステップと、
前記角速度算出ステップで算出した角速度が略0である場合に、前記角速度センサーに加わる角速度が0であると判定する角速度判定ステップと、を含む、角速度センサーのオフセット補正方法。
【請求項1】
角速度センサーと、3つ以上の加速度センサーと、を含むセンサー装置であって、
前記加速度センサーの出力に基づいて、前記角速度センサーに加わる角速度が0であるか否かを判定する判定部と、
前記判定部により前記角速度が0であると判定された時に、前記角速度センサーの出力のオフセットを補正するオフセット補正部と、を含み、
前記判定部は、
前記加速度センサーの出力に基づいて当該センサー装置に加わる加速度を算出し、算出した加速度と重力加速度が略一致する状態を初期状態として検出する初期状態検出部と、
前記初期状態における当該センサー装置の姿勢情報に基づいて前記初期状態における各検出軸方向の軸ベクトルを算出し、当該軸ベクトルと各検出軸方向の加速度成分と当該加速度成分の時間変化とに基づいて当該センサー装置が任意の軸回りに回転する角速度を算出する角速度算出部と、
前記角速度算出部が算出した角速度が略0である場合に、前記角速度センサーに加わる角速度が0であると判定する角速度判定部と、を含む、センサー装置。
【請求項2】
請求項1において、
前記角速度判定部は、
前記角速度算出部が算出した角速度が略0の状態が所定時間継続した場合に、前記角速度センサーに加わる角速度が0であると判定する、センサー装置。
【請求項3】
請求項1において、
前記判定部は、
制御信号に応じて第1の判定モードと第2の判定モードを切り替える判定モード制御部と、
前記第1の判定モードでは、前記初期状態検出部が前記初期状態を検出した場合に前記角速度センサーに加わる角速度が0であると判定し、前記第2の判定モードでは、前記角速度算出部が算出した角速度が略0である場合に前記角速度センサーに加わる角速度が0であると判定する角速度判定部と、を含む、センサー装置。
【請求項4】
請求項3において、
前記角速度判定部は、
前記第1の判定モードでは、前記初期状態検出部が所定時間継続して前記初期状態を検出した場合に前記角速度センサーに加わる角速度が0であると判定し、前記第2の判定モードでは、前記角速度算出部が算出した角速度が略0の状態が所定時間継続した場合に前記角速度センサーに加わる角速度が0であると判定する、センサー装置。
【請求項5】
請求項1乃至4のいずれかのセンサー装置を含む、電子機器。
【請求項6】
角速度センサーと、3つ以上の加速度センサーと、を含むセンサー装置において、前記角速度センサーの出力のオフセットを補正する角速度センサーのオフセット補正方法であって、
前記加速度センサーの出力に基づいて、前記角速度センサーに加わる角速度が0であるか否かを判定する判定ステップと、
前記判定ステップで前記角速度が0であると判定された時に、前記角速度センサーの出力のオフセットを補正するオフセット補正ステップと、を含み、
前記判定ステップは、
前記加速度センサーの出力に基づいて前記センサー装置に加わる加速度を算出し、算出した加速度と重力加速度が略一致する状態を初期状態として検出する初期状態検出ステップと、
前記初期状態における前記センサー装置の姿勢情報に基づいて前記初期状態における各検出軸方向の軸ベクトルを算出し、当該軸ベクトルと各検出軸方向の加速度成分と当該加速度成分の時間変化とに基づいて前記センサー装置が任意の軸回りに回転する角速度を算出する角速度算出ステップと、
前記角速度算出ステップで算出した角速度が略0である場合に、前記角速度センサーに加わる角速度が0であると判定する角速度判定ステップと、を含む、角速度センサーのオフセット補正方法。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【公開番号】特開2012−37405(P2012−37405A)
【公開日】平成24年2月23日(2012.2.23)
【国際特許分類】
【出願番号】特願2010−178381(P2010−178381)
【出願日】平成22年8月9日(2010.8.9)
【出願人】(000002369)セイコーエプソン株式会社 (51,324)
【Fターム(参考)】
【公開日】平成24年2月23日(2012.2.23)
【国際特許分類】
【出願日】平成22年8月9日(2010.8.9)
【出願人】(000002369)セイコーエプソン株式会社 (51,324)
【Fターム(参考)】
[ Back to top ]