JP2012052904A - 慣性計測装置 - Google Patents

Abstract

【課題】高精度の慣性計測装置を提供する。
【解決手段】慣性計測装置１は、被測定物に働く重力加速度を検出可能な加速度部５１と、被測定物の回転運動を検出可能な複数の角速度センサを有する角速度部５２、５３と、複数の処理機能が組み込まれた演算処理部５４とを備えている。角速度部５２にはＸＹＺ軸のそれぞれの角速度を検出可能な３つの角速度センサ、角速度部５３には３つの角速度センサの検出軸とは異なる軸の角速度を検出可能な１つ以上の角速度センサが含まれている。演算処理部５４では、これら４〜６つの角速度センサから、重力軸と最も平行な軸を検出軸とする角速度センサを除いて、重力軸と最も平行な軸の角速度が算出される。
【選択図】図２

Description

本発明は、被測定物の運動状態を検出する慣性計測装置（ＩＭＵ：Inertial Measurement Unit）に適用して有効な技術に関する。

被測定物に働く加速度を検出可能な加速度部（例えば、加速度センサ）と、被測定物の回転運動（角速度）を検出可能な角速度部（例えば、角速度センサ）とを備えた慣性計測装置が、工業製品（例えば、ロボット、電子機器、自動車）、生体などの被測定物の姿勢や方位などを検出するために用いられている。特開２００８−３０９５９４号公報（特許文献１）および特開２００１−２６４３５１号公報（特許文献２）には、慣性計測装置に関する技術が開示されている。また、特許第４２７７０４８号（特許文献３）には、慣性計測装置をモーションキャプチャに適用する技術が開示されている。

特開２００８−３０９５９４号公報 特開２００１−２６４３５１号公報 特許第４２７７０４８号

例えばロボット、電子機器などの小型の工業製品に組み込まれる慣性計測装置には小型化であることはもちろん、高精度化も要求されている。そこで、本発明者は、このような慣性計測装置を角速度センサのオフセットドリフトを（冗長角度センサとデータ処理によって）低減することで実現しようと試みている。

角速度センサのオフセットドリフトとは、角速度センサの検出値が、静止状態でも角速度が出力され変化してしまう（ゼロではない）ことをいう。このオフセットドリフトが小さいものが、高精度の角速度センサとなる。なお、高精度の角速度センサとして、光路差から角速度を検出するリングレーザ、光ファイバを用いた光学式（光学式ジャイロ）があるが、光学式ジャイロは、その構造により大型化となってしまう。

小型化の角速度センサとしては、ある方向に一次振動させた振動体に回転力が加わると、一次振動と直交した方向にコリオリ力によって二次振動が発生し、この二次振動を計測することにより、角速度を計測する振動式（振動ジャイロ）がある。この振動体は、例えば、水晶振動子やＭＥＭＳ（Micro Electro Mechanical Systems）技術を用いたシリコン振動子から構成される。水晶振動子やシリコン振動子を用いた振動ジャイロは、ロボット、電子機器に用いられる部品としては十分な大きさ（小型化）で構成される。

しかしながら、振動ジャイロでは、振動体の構造、駆動方式により、オフセットドリフトが大きくなってしまう。例えば、振動体の構造としては、音叉型、音片型、リング型があり、また、振動体の駆動方式としては圧電効果、静電引力、電磁力が用いられている。

振動ジャイロのオフセットドリフトの要因としては次のようなものがある。音叉型、音片型では、駆動の構造上、固定支点が必要であり、外部からの振動や衝撃が加わることによる誤出力がある。また、駆動に用いる圧電効果、電磁力は温度による変化があり、また、振動体が帯電してしまうと静電引力による駆動でも変動があり、これらによって誤出力されることもある。また、振動を圧電効果で検出する場合、二次振動を検出する素子の二次振動方向に直交する運動や重力加速度により、剪断力が発生し、二次振動以外の力も検出してしまうこともある。このようにして、振動ジャイロではオフセットドリフトが生じてしまう。

そこで、本発明者は、角速度センサ自体のオフセットドリフトを低減して、角速度センサを高精度化するのではなく、慣性計測装置全体として角速度センサのオフセットドリフトを低減することにより慣性計測装置の高精度化を図ることを検討している。図１は本発明者が検討した慣性計測装置１０１を説明するための図である。

慣性計測装置１０１は、ＸＹＺ軸が互いに直交する三次元直交座標（右手系直交座標）における被測定物（図示せず）の運動状態を検出するものである。慣性計測装置１０１は、Ｘ軸、Ｙ軸およびＺ軸のそれぞれの加速度を検出可能な加速度センサＡ１、Ａ２、Ａ３と、Ｘ軸、Ｙ軸およびＺ軸のそれぞれの角速度を検出可能な角速度センサＧ１、Ｇ２、Ｇ３とを備えている。

このような構成の慣性計測装置１０１は、角速度センサＧ１、Ｇ２、Ｇ３のそれぞれが有するオフセットドリフトΔω１、Δω２、Δω３を除去するためのアルゴリズム（いわゆるカルマンフィルタ）が組み込まれた演算処理部（図示しない）を備えている。カルマンフィルタは、加速度センサＡ１、Ａ２、Ａ３の検出値から算出された重力加速度を参照して、オフセットドリフトΔω１、Δω２、Δω３を算出（推定）するものである。これにより、角速度センサＧ１、Ｇ２、Ｇ３のそれぞれの検出値ω１、ω２、ω３からオフセットドリフトΔω１、Δω２、Δω３を除去することで、被測定物のＸＹＺ軸のそれぞれにおける角速度ωｘ、ωｙ、ωｚを算出することができる。

しかしながら、例えばＺ軸が重力加速度の方向（重力軸）と平行となった場合、カルマンフィルタでは、角速度センサＧ３にオフセットドリフトΔω３が存在しても、角速度センサＧ３の検出値ω３が、実際に回転運動して生じた値なのか、オフセットドリフトΔω３による値なのか、分離することができない。このため、慣性計測装置１０１では、高精度に被測定物の運動状態を検出することが困難となってしまう。

なお、特許文献１の明細書段落［００３２］には、角速度センサおよび加速度センサなどの検出値を参照して慣性計測装置の検出値（姿勢角）を算出する技術が開示されている。この技術は、各センサの検出値の参照割合を慣性計測装置の種々の状態に合わせて変化させることによって、外的要因が変化する場合においても、ノイズ、誤差の影響を低減した検出値を得るためのものである（特許文献１の明細書段落［０００３］参照）。すなわち、特許文献１で開示された技術は、外的要因を含めた検出値の補正方法である。したがって、特許文献１の技術は、角速度センサのオフセットドリフトを慣性計測装置全体で低減するものではなく、前述の課題を解決する手段とはならない。

また、特許文献２の請求項９には、ＸＹＺ方向にそれぞれ角速度センサおよび加速度センサを設け、それら方向と交差する方向に他の角速度センサおよび加速度センサを設ける技術が開示されている。この技術は、ＸＹＺ方向にそれぞれ設けた角速度センサおよび加速度センサのいずれかが不良となったときに、他の角速度センサおよび加速度センサを代用し、稼働率を低下させないことにより信頼性の高い慣性計測装置を得るものである（特許文献２の明細書段落［００２３］参照）。すなわち、特許文献２で開示された技術は、単に不良が生じたときの代用品を設けるものである。したがって、特許文献２の技術は、角速度センサのオフセットドリフトを慣性計測装置全体で低減するものではなく、前述の課題を解決する手段とはならない。

本発明の目的は、高精度の慣性計測装置を提供することにある。本発明の前記ならびにその他の目的と新規な特徴は、本明細書の記述および添付図面から明らかになるであろう。

本願において開示される発明のうち、代表的なものの概要を簡単に説明すれば、次のとおりである。本発明の一実施形態における慣性計測装置は、第１、第２および第３軸が互いに直交する三次元直交座標系における被測定物の運動状態を検出するものであって、前記被測定物に働く重力加速度を検出可能な加速度センサと、前記被測定物の回転運動を検出可能な４軸以上の角速度センサと、複数の処理機能が組み込まれた演算処理部とを備えている。前記複数の角速度センサには、前記第１、第２および第３軸のそれぞれの角速度を検出可能な第１、第２および第３角速度センサと、前記第１、第２および第３角速度センサの検出軸とは異なる軸の角速度を検出可能な第４角速度センサとが含まれている。また、前記演算処理部には、前記複数の角速度センサの検出軸から、前記重力加速度の方向と最も平行な軸を決定する第１処理機能と、前記複数の角速度センサの検出軸から、前記第１処理機能で決定された軸を除いた３つの軸を選択する第２処理機能と、前記重力加速度を参照して、前記３つの軸に対応する角速度センサの検出値から、前記第１処理機能で決定された軸の角速度を算出する第３処理機能とが含まれている。

本願において開示される発明のうち、代表的なものによって得られる効果を簡単に説明すれば、次のとおりである。前記一実施形態によれば、高精度の慣性計測装置を提供することができる。

本発明者が検討した慣性計測装置を説明するための図である。 本発明の一実施形態における慣性計測装置の構成を示すブロック図である。 本発明の一実施形態における加速度センサおよび角速度センサの検出軸を説明するため図である。 本発明の一実施形態における慣性計測装置の演算処理部の処理機能を説明するための図である。 重力加速度の方向と最も平行に近い軸の角速度を算出する処理機能の構成を説明するための図である。 本発明の他の実施形態における加速度センサおよび角速度センサの検出軸を説明するための図である。 本発明の他の実施形態における慣性計測装置の演算処理部の処理機能を説明するための図である。 本発明の他の実施形態における加速度センサおよび角速度センサの検出軸を説明するための図である。

以下、本発明の実施形態を図面に基づいて詳細に説明する。なお、実施形態を説明するための全図において、同一の機能を有する部材には同一の符号を付し、その繰り返しの説明は省略する場合がある。

（実施形態１）
図２は本発明の実施形態における慣性計測装置１の構成を示すブロック図である。図３は慣性計測装置１が備える加速度センサＡ１、Ａ２、Ａ３および角速度センサＧ１、Ｇ２、Ｇ３、Ｇ４の検出軸を説明するための図である。図３中のＺ１軸はＺ軸からθｚ１（＝０でもよい）の角度を為すものであり、Ｚ２軸はＺ軸からθｚ２の角度を為すもので、本実施形態では、０≦θｚ１＜θｚ２としている。すなわち、Ｚ２軸は、Ｘ軸、Ｙ軸、Ｚ軸（Ｚ１軸）とは異なる軸としている。

この慣性計測装置１は、ＸＹＺ軸が互いに直交する三次元直交座標系における被測定物の運動状態を検出するものである。例えば、被測定物をロボットの可動部とした場合、慣性計測装置１は、その可動部に組み付けられる（組み込まれる）ものである。本実施形態では、ロボットの可動部に用いられるように、小型化に対応した慣性計測装置１として説明する。

慣性計測装置１は、被測定物に働く加速度を検出可能な加速度部５１と、被測定物の回転運動を検出可能な角速度部５２と、加速度部５２を補助して被測定物の回転運動を検出可能な角速度部５３（補助部５３ともいう）と、複数の処理機能が組み込まれた演算処理部５４とを備えている。加速度部５１には、複数の加速度センサが含まれており、角速度部５２、５３には、複数の角速度センサが含まれている。図２に示すように、慣性計測装置１では、演算処理部５４に、加速度部５１、角速度部５２、５３からの信号が入力される。

本実施形態では、加速度部５１は、加速度センサＡ１、Ａ２、Ａ３が含まれて構成されており、それぞれから検出値ａ１、ａ２、ａ３を得ることができる。Ｘ軸の加速度を検出可能な加速度センサＡ１は、検出軸をＸ軸としている。また、Ｙ軸の加速度を検出可能な加速度センサＡ２は、検出軸をＹ軸としている。また、Ｚ軸の加速度を検出可能な加速度センサＡ３は、検出軸をＺ軸としている。なお、実際の加速度センサＡ１〜Ａ３には、ノイズが存在するが、本願においては、説明を明解にするために、加速度センサＡ１〜Ａ３にはノイズが存在していないものとして説明する。

本実施形態では、角速度部５２は、角速度センサＧ１、Ｇ２、Ｇ３が含まれて構成されており、それぞれから検出値ω１、ω２、ω３を得ることができる。また、角速度部５３は、角速度センサＧ４が含まれて構成されており、その検出値ω４を得ることができる。また、Ｘ軸の角速度を検出可能な角速度センサＧ１は、検出軸をＸ軸としている。Ｙ軸の角速度を検出可能な角速度センサＧ２は、検出軸をＹ軸としている。また、Ｚ軸の角速度を検出可能な角速度センサＧ３は、検出軸をＺ１軸（＝Ｚ軸）としている。また、Ｚ軸の角速度を検出可能な角速度センサＧ４は、検出軸をＺ２軸としている。

本実施形態では、角速度センサＧ１〜Ｇ４として、振動式の角速度センサを用いている。このため、前述したように、角速度センサＧ１〜Ｇ４のそれぞれにはオフセットドリフトΔω１、Δω２、Δω３、Δω４が存在していることとなる。

この慣性計測装置１では、加速度センサＡ１の検出軸と角速度センサＧ１の検出軸は同じ軸とし、加速度センサＡ２の検出軸と角速度センサＧ２の検出軸は同じ軸とし、加速度センサＡ３の検出軸と角速度センサＧ３の検出軸とし、角速度センサＧ４の検出軸は、これらの検出軸とは異なる軸としている。

本実施形態における慣性計測装置１を、Ｚ軸に重力加速度（重力ベクトル）が働く被測定物に適用し、この被測定物がＺ軸周りにのみ回転運動している場合について、以下に説明する。まず、角速度センサＧ１、Ｇ２、Ｇ３、Ｇ４のオフセットドリフトΔω１、Δω２、Δω３、Δω４が存在していない場合について説明する。角速度センサＧ１、Ｇ２、Ｇ３、Ｇ４の検出値ω１、ω２、ω３、ω４は、次のようになる。但し、ωｘ、ωｙおよびωｚはそれぞれＸ軸、Ｙ軸およびＺ軸（重力軸）周りの被測定物の角速度である。

ω１＝ωｘ＝０ …… （１）
ω２＝ωｙ＝０ …… （２）
ω３＝ωｚｃｏｓθｚ１ …… （３）
ω４＝ωｚｃｏｓθｚ２ …… （４）

ここで、θｚ１、θｚ２は既知であるから、被測定物のＺ軸周りの角速度ωｚについて次式が得られる。
ωｚ＝ω３／ｃｏｓθｚ１＝ω４／ｃｏｓθｚ２ …… （５）
このように、角速度センサにオフセットドリフトが存在しない場合には、容易に重力軸（Ｚ軸）の角速度を算出することができる。

次に、角速度センサＧ１、Ｇ２、Ｇ３、Ｇ４にオフセットドリフトΔω１、Δω２、Δω３、Δω４が存在する場合について説明する。Ｚ軸周りにのみ回転運動（ωｘ＝０、ωｙ＝０）している被測定物について考えているので、ωｚの算出について説明する。

式（５）に、角速度センサＧ３、Ｇ４のオフセットドリフトΔω３、Δω４を考慮すると、被測定物のＺ軸周りの角速度ωｚについて次式が得られる。
ωｚ＝（ω３−Δω３）／ｃｏｓθｚ１＝（ω４−Δω４）／ｃｏｓθｚ２ …… （６）

式（６）からは、オフセットドリフトΔω３、Δω４を算出（推定）して、角速度センサＧ３、Ｇ４の検出値ω３、ω４から除去すれば、Ｚ軸周りの角速度ωｚを正確に算出することができることがわかる。以下では、オフセットドリフトの算出技術について説明する。

図４は慣性計測装置１の演算処理部５４（図２参照）の複数の処理機能を説明するための図である。演算処理部５４には、加速度センサＡ１、Ａ２、Ａ３の検出値ａ１、ａ２、ａ３および角速度センサＧ１、Ｇ２、Ｇ３、Ｇ４の検出値ω１、ω２、ω３、ω４が入力される。

演算処理部５４の処理機能Ｆ１１では、検出値ａ１、ａ２、ａ３から重力加速度Ｇが算出される。本実施形態では、Ｚ軸を重力軸として重力加速度が働く被測定物に対して、慣性計測装置１を適用しているので、重力加速度Ｇ（重力ベクトルＧ）は大きさが９．８ｍ／ｓ^２で、方向がＺ軸方向として算出される。

演算処理部５４の処理機能Ｆ１２では、重力加速度Ｇを参照して、加速度センサＡ１、Ａ２、Ａ３のそれぞれの検出軸の傾きθ１、θ２、θ３が算出される。本実施形態では、検出軸の傾きθ１、θ２、θ３はそれぞれＸ軸、Ｙ軸、Ｚ軸に対しての角度としている。この加速度センサＡ１、Ａ２、Ａ３の検出軸はそれぞれ角速度センサＧ１、Ｇ２、Ｇ３の検出軸と一致するので、角度θ１、θ２、θ３はそれぞれ角速度センサＧ１、Ｇ２、Ｇ３の検出軸の角度と算出される。

また、演算処理部５４の処理機能Ｆ１２では、算出した角度θ１、θ２、θ３、θ４を参照して、角速度センサＧ１、Ｇ２、Ｇ３、Ｇ４のそれぞれの検出軸から、重力加速度の方向（重力軸）と最も平行に近い軸ｉ（重力軸と平行または最も小さい角度で交差する軸ｉ）が決定される。本実施形態では、重力軸をＺ軸としており、このＺ軸に対して最も小さい角度がＺ１軸の角度θｚ１であった場合、角速度センサＧ３の検出軸が軸ｉとなる。

演算処理部５４の処理機能Ｆ１３では、処理機能Ｆ２で決定された軸ｉを除いた３つの軸を選択する。本実施形態では、角速度センサＧ１、Ｇ２、Ｇ３、Ｇ４のそれぞれの検出軸のうち、角速度センサＧ３の検出軸を除いた、角速度センサＧ１、Ｇ２、Ｇ４の３つの検出軸が選択される。

演算処理部５４の処理機能Ｆ１４では、重力加速度Ｇを参照して、Ｘ軸、Ｙ軸、Ｚ軸に対応する角速度ωｊ、ωｋ、ωｌが算出（出力）される。この処理機能Ｆ１４には、重力加速度Ｇを参照して、処理機能Ｆ１３で選択された３つの軸のそれぞれに対応する角速度センサの検出値から、処理機能Ｆ１２で決定された各軸に対応する角速度センサのオフセットドリフトを算出する下位の処理機能（カルマンフィルタなど）が含まれている。処理機能Ｆ１４で算出された角速度ωｊ、ωｋ、ωｌは、処理機能Ｆ１５において、座標変換され、ＸＹＺ軸のそれぞれの角速度ωｘ、ωｙ、ωｚが算出される。

図５は、角速度センサＧ３の検出軸が処理機能Ｆ１２で決定された軸ｉとして、軸ｉ（Ｇ３軸）を除く３軸の角速度ωｊ、ωｋ、ωｌから最終的に軸ｘ、ｙ、ｚに対応する角速度ωｘ、ωｙ、ωｚを算出する処理機能の構成を説明するための図である。この図５では、図４中の処理機能Ｆ１４から先の処理機能が示されていることとなる。

カルマンフィルタＫＦでは、重力加速度Ｇを参照して、角速度センサＧ１、Ｇ２、Ｇ４の検出値ω１、ω２、ω４から、角速度センサＧ１、Ｇ２、Ｇ４のオフセットドリフトΔω１、Δω２、Δω４を算出（推定）する。このカルマンフィルタＫＦでは、ω１、ω２、ω４がいずれも重力軸に対して水平成分を有しているので、オフセットドリフトΔω１、Δω２、Δω４を算出（推定）することができる。なお、このカルマンフィルタＫＦの処理は、被測定物の状態を、現在、過去において推定するために、繰り返して行われる。

また、カルマンフィルタＫＦでは、推定されたω１−Δω１、ω２−Δω２、ω４−Δω４から処理機能Ｆ１２で決定された軸ｉに対応する真の角速度の値ω３’を算出（推定）することができる。逆に、求められたω３’を角速度センサＧ３の検出値ω３から減算してオフセットドリフトΔω３を推定することもできる。

このように４軸の角速度センサ（４つの角速度センサＧ１、Ｇ２、Ｇ３、Ｇ４）の場合は、少なくとも３軸の角速度センサ（３つの角速度センサＧ１、Ｇ２、Ｇ３）が、重力軸と平行でないために、３軸のいずれかの軸が回転しても他の軸の傾斜角に変化を与えることができる。これにより、オフセットドリフトと実回転の分離が可能となり、例えば、角速度センサの検出軸が重力軸と平行した場合であっても、その角速度センサのオフセットドリフトを低減することができる。したがって、本実施形態によれば、高精度の慣性計測装置１を提供することができる。

また、本実施形態によれば、光学式より小型化、低価格の振動式の角速度センサを用いた場合であっても、適切にオフセットドリフトを除去できるため、慣性計測装置１の計測精度を向上することがきる。また、慣性計測装置１を用いれば、高精度角速度、高精度姿勢（傾斜角、方位角）センサ、またセンサによるモーションキャプチャを実現することができる。また、本実施形態では、角速度センサの検出軸数を４軸として説明したが、検出軸数を６軸まで増加できれば、軸の方向を工夫することにより計算処理を容易にすることができる。

なお、本実施形態では、０＜θｚ１＜θｚ２として説明したが、Ｚ軸とＺ１軸を一致（θｚ１＝０）させても前述の技術を適用することができ、高精度の慣性計測装置１を得ることができる。

（実施形態２）
前記実施形態１では、複数の角速度センサの検出軸を４軸とした場合について説明したが、本実施形態では、複数の角速度センサの検出軸を６軸（Ｘ１軸、Ｙ１軸、Ｚ１軸、Ｘ２軸、Ｙ２軸、Ｚ２軸）として説明する。検出軸を多くすることで、より精度の高いオフセットドリフトの推定が可能となる。なお、前記実施形態１と重複する説明は省略する場合がある。

図６は慣性計測装置１が備える加速度センサＡ１、Ａ２、Ａ３および角速度センサＧ１、Ｇ２、Ｇ３、Ｇ４、Ｇ５、Ｇ６の検出軸を説明するための図であり、Ｘ１軸、Ｙ１軸、Ｚ１軸、Ｘ２軸、Ｙ２軸、Ｚ２軸が示されている。なお、図６中のＸ１軸、Ｙ１軸、Ｚ１軸、Ｘ２軸、Ｙ２軸、Ｚ２軸周りの被測定物の角速度をそれぞれωｘ１、ωｙ１、ωｚ１、ωｘ２、ωｙ２、ωｚ２とする。

本実施形態では、Ｘ２軸が、Ｘ１軸に対してπ／２［rad］、Ｙ１軸およびＺ１軸に対してπ／４［rad］の角度を為しており、Ｙ２軸が、Ｙ１軸に対してπ／２［rad］、Ｚ１軸およびＸ１軸に対してπ／４［rad］の角度を為しており、Ｚ２軸が、Ｚ１軸に対してπ／２［rad］、Ｘ１軸およびＹ１軸に対してπ／４［rad］の角度を為している。

ここで、Ｘ１軸の角速度を検出可能な角速度センサＧ１は、検出軸をＸ１軸としている。また、Ｙ１軸の角速度を検出可能な角速度センサＧ２は、検出軸をＹ１軸としている。また、Ｚ１軸の角速度を検出可能な角速度センサＧ３は、検出軸をＺ１軸としている。また、Ｘ２軸の角速度を検出可能な角速度センサＧ４は、検出軸をＸ２軸としている。また、Ｙ２軸の角速度を検出可能な角速度センサＧ５は、検出軸をＹ２軸としている。また、Ｚ２軸の角速度を検出可能な角速度センサＧ６は、検出軸をＺ２軸としている。

本実施形態では、Ｘ１軸、Ｙ１軸、Ｚ１軸が互いに直交してなる三次元直交座標系（右手系直交座標系）が基準座標系として構成され、また、Ｘ２軸、Ｙ２軸、Ｚ２軸からなる補助座標系が構成されている。言い換えると、角速度センサＧ１、Ｇ２、Ｇ３の検出軸で基準座標系が構成されており、角速度センサＧ４、Ｇ５、Ｇ６の検出軸で補助座標系が構成されている。

また、加速度部５１は、加速度センサＡ１、Ａ２、Ａ３が含まれて構成されており、それぞれから検出値ａ１、ａ２、ａ３を得ることができる。Ｘ１軸の加速度を検出可能な加速度センサＡ１は、検出軸をＸ１軸としている。また、Ｙ１軸の加速度を検出可能な加速度センサＡ２は、検出軸をＹ１軸としている。また、Ｚ１軸の加速度を検出可能な加速度センサＡ３は、検出軸をＺ１軸としている。

本実施形態では、角速度部５２は、角速度センサＧ１、Ｇ２、Ｇ３が含まれて構成されており、それぞれから検出値ω１、ω２、ω３を得ることができる。角速度部５３は、角速度センサＧ４、Ｇ５、Ｇ６が含まれて構成されており、それぞれから検出値ω４、ω５、ω６を得ることができる。また、角速度センサＧ１、Ｇ２、Ｇ３、Ｇ４、Ｇ５、Ｇ６として、振動式の角速度センサを用いている。このため、角速度センサＧ１、Ｇ２、Ｇ３、Ｇ４、Ｇ５、Ｇ６のそれぞれにはオフセットドリフトΔω１、Δω２、Δω３、Δω４、Δω５、Δω６が存在していることとなる。

ここで、角速度センサＧ１、Ｇ２、Ｇ３、Ｇ４、Ｇ５、Ｇ６にオフセットドリフトが存在していない場合について説明する。このとき、各軸の角速度間には以下の関係が成り立っている。

ω４＝（ω２＋ω３）ｃｏｓ（π／４） …… （７）
ω５＝（ω３＋ω１）ｃｏｓ（π／４） …… （８）
ω６＝（ω１＋ω２）ｃｏｓ（π／４） …… （９）

これら式（７）〜（９）により、次式が得られる。
（ω４＋ω５＋ω６）＝２（ω１＋ω２＋ω３）ｃｏｓ（π／４） …… （１０）

式（１０）の関係から、検出値（ω１、ω２、ω３）と、検出値（ω４、ω５、ω６）とは、相互に変換可能であることがわかる。すなわち、次式が得られる。

ω１＝（−ω４＋ω５＋ω６）／（２ｃｏｓ（π／４）） …… （１１）
ω２＝（ω４−ω５＋ω６）／（２ｃｏｓ（π／４）） …… （１２）
ω３＝（ω４＋ω５−ω６）／（２ｃｏｓ（π／４）） …… （１３）

ここで、角速度センサＧ１〜Ｇ６のオフセットドリフトは存在しないとした場合、ωｘ１＝ω１、ωｙ１＝ω２、ωｚ１＝ω３、ωｘ２＝ω４、ωｙ２＝ω５、ωｚ２＝ω６である。これにより、検出値ω１、ω２、ω３の検出軸であるＸ１軸、Ｙ１軸、Ｚ１軸で構成された基準座標系と、検出値ω４、ω５、ω６の検出軸であるＸ２軸、Ｙ２軸、Ｚ２軸で構成された補助座標系とは、相互に座標変換が可能であることがわかる。

次に、角速度センサＧ１〜Ｇ６にオフセットドリフトΔω１〜Δω６が存在する場合について説明する。図７は本実施形態における演算処理部５４（図２参照）の複数の処理機能を説明するための図である。以下では、Ｚ１軸を重力軸として重力加速度が働く被測定物に適用し、この被測定物がＺ１軸周りにのみ回転運動している場合について説明する。本実施形態では、演算処理部５４には、加速度センサＡ１〜Ａ３の検出値ａ１〜ａ３および角速度センサＧ１〜Ｇ６の検出値ω１〜ω６が入力される。

演算処理部５４の処理機能Ｆ２１では、検出値ａ１〜ａ３から重力加速度Ｇが算出される。本実施形態では、Ｚ軸を重力軸として重力加速度が働く被測定物に対して、慣性計測装置１を適用しているので、重力加速度Ｇ（重力ベクトルＧ）は大きさが９．８ｍ／ｓ^２で、方向がＺ軸方向として算出される。

演算処理部５４の処理機能Ｆ２２では、重力加速度Ｇを参照して、Ｘ１軸、Ｙ１軸、Ｚ１軸に対しての角度として、加速度センサＡ１〜Ａ３のそれぞれの検出軸の傾きθ１、θ２、θ３が算出される。この加速度センサＡ１〜Ａ３の検出軸はそれぞれ角速度センサＧ１〜Ｇ３の検出軸と一致するので、角度θ１〜θ３はそれぞれ角速度センサＧ１〜Ｇ３の検出軸の角度と算出される。また、角速度センサＧ１〜Ｇ３の検出軸と角速度センサＧ４〜Ｇ６の検出軸が為す角度が既知であるので、Ｘ２軸、Ｙ２軸、Ｚ２軸に対しての角度として、角速度センサＧ４〜Ｇ６のそれぞれの検出軸の傾きθ４、θ５、θ６が算出される。

また、演算処理部５４の処理機能Ｆ２２では、算出した角度θ１〜θ６を参照して、角速度センサＧ１〜Ｇ６のそれぞれの検出軸から、重力加速度の方向（重力軸）と最も平行に近い軸ｉ（重力軸と平行または最も小さい角度で交差する軸ｉ）が決定される。ここでは、重力軸をＺ１軸としているため、角速度センサＧ３の検出軸（Ｚ１軸）が軸ｉとなる。

演算処理部５４の処理機能Ｆ２３では、（Ｘ１、Ｙ１、Ｚ１）の基準座標系および（Ｘ２、Ｙ２、Ｚ２）の補助座標系のうち、処理機能Ｆ２２で決定された軸ｉが含まれる座標系とは異なる座標系から、３つの軸が選択される。本実施形態では、処理機能Ｆ２２では、軸ｉとしてＺ１軸が決定されているので、補助座標系のＸ２軸、Ｙ２軸、Ｚ２軸が選択される。

演算処理部５４の処理機能Ｆ２５では、処理機能Ｆ２４で座標変換された重力加速度Ｇを参照して、補助座標系のＸ２軸、Ｙ２軸、Ｚ２軸に対応する角速度ωｊ、ωｋ、ωｌが算出（出力）される。この処理機能Ｆ２５には、重力加速度Ｇを参照して、処理機能Ｆ２３で選択された３つの軸のそれぞれに対応する角速度センサＧ４〜Ｇ６の検出値ω４〜ω６から、オフセットドリフトΔω４〜Δω６を算出（推定）し、ωｊ＝ω４−Δω４、ωｋ＝ω５−Δω５、ωｌ＝ω６−Δω６を算出する下位の処理機能（カルマンフィルタなど）が含まれている。

次いで、演算処理部５４の処理機能Ｆ２６では、補助座標系から基準座標系に座標変換して、基準座標系のＸ１軸、Ｙ１軸、Ｚ１軸のそれぞれの角速度ωｘ１、ωｙ１、ωｚ１が算出される。

なお、本実施形態では、Ｚ１軸を重力軸として重力加速度が働く被測定物に適用し、この被測定物がＺ１軸周りにのみ回転運動している場合について説明しているが、Ｚ２軸を重力軸として重力加速度が働く被測定物に適用することもできる。この場合、処理機能Ｆ２２では、軸ｉとしてＺ２軸が決定され、処理機能Ｆ２３では、基準座標系の３つの軸（Ｘ１軸、Ｙ１軸、Ｚ１軸）が選択され、処理機能Ｆ２７では、重力加速度Ｇを参照して、基本座標系のＸ１軸、Ｙ１軸、Ｚ１軸に対応する角速度ωｊ、ωｋ、ωｌが算出（出力）される。

処理機能Ｆ２７には、重力加速度Ｇを参照して、処理機能Ｆ２３で選択された３つの軸のそれぞれに対応する角速度センサＧ１〜Ｇ３の検出値ω１〜ω３から、オフセットドリフトΔω１〜Δω３を算出（推定）し、ωｊ＝ω１−Δω１、ωｋ＝ω２−Δω２、ωｌ＝ω３−Δω３を算出する下位の処理機能（カルマンフィルタなど）が含まれている。そして、この角速度ωｊ、ωｋ、ωｌは、そのまま基本座標系であるＸ１軸、Ｙ１軸、Ｚ１軸の角速度ωｘ１、ωｙ１、ωｚ１として算出（出力）されることとなる。

このように６軸の角速度センサ（６つの角速度センサＧ１〜Ｇ６）を用いて、基本座標系と補助座標系を構成することで、いずれかの座標系の３つの角速度センサの検出軸が重力軸に対して水平成分を持たせることができる。これにより、オフセットドリフトと実回転の分離が可能となり、例えば、角速度センサの検出軸が重力軸と平行した場合であっても、その角速度センサのオフセットドリフトを低減することができる。したがって、本実施形態によれば、高精度の慣性計測装置１を提供することができる。また、前記実施形態１と比較して、座標変換の処理を行うことで、容易に被測定物の運動状態を検出することができる。

（実施形態３）
本実施形態では、前記実施形態２で示した補助座標系とは異なる補助座標系について説明する。なお、前記実施形態１、２と重複する説明は省略する場合がある。

図８は慣性計測装置１が備える加速度センサＡ１、Ａ２、Ａ３および角速度センサＧ１、Ｇ２、Ｇ３、Ｇ４、Ｇ５、Ｇ６の検出軸を説明するための図であり、Ｘ１軸、Ｙ１軸、Ｚ１軸、Ｘ２軸、Ｙ２軸、Ｚ２軸が示されている。なお、図８中のＸ１軸、Ｙ１軸、Ｚ１軸、Ｘ２軸、Ｙ２軸、Ｚ２軸周りの被測定物の角速度をそれぞれωｘ１、ωｙ１、ωｚ１、ωｘ２、ωｙ２、ωｚ２とする。

本実施形態では、Ｘ１軸、Ｙ１軸、Ｚ１軸で構成される基準座標系［Ｘ１，Ｙ１，Ｚ１］およびＸ２軸、Ｙ２軸、Ｚ２軸で構成される補助座標系［Ｘ２，Ｙ２，Ｚ２］は、右手系直交座標系とし、Ｘ２は［ｋＸ１，ｋＹ１，ｋＺ１］方向の単位ベクトル、Ｙ２は［−ｋＸ１，ｋＹ１，ｋＺ１］方向の単位ベクトル、Ｚ２は［ｋＸ１，−ｋＹ１，ｋＺ１］方向の単位ベクトルとする。ただし、ｋ＝１／√３である。この時、［Ｘ２，Ｙ２，Ｚ２］の各軸は、［ｎＸ１，ｎＹ１，ｎＺ１］（ｎ：整数）の各格子点の立方体中心の方向にある。また、［Ｘ２，Ｙ２，Ｚ２］の各軸は、［Ｘ１，Ｙ１，Ｚ１］の各軸とｃｏｓ^−１（１／√３）の角度（約５４．７°）を為している。

ここで、Ｘ１軸の角速度を検出可能な角速度センサＧ１は、検出軸をＸ１軸としている。また、Ｙ１軸の角速度を検出可能な角速度センサＧ２は、検出軸をＹ１軸としている。また、Ｚ１軸の角速度を検出可能な角速度センサＧ３は、検出軸をＺ１軸としている。また、Ｘ２軸の角速度を検出可能な角速度センサＧ４は、検出軸をＸ２軸としている。また、Ｙ２軸の角速度を検出可能な角速度センサＧ５は、検出軸をＹ２軸としている。また、Ｚ２軸の角速度を検出可能な角速度センサＧ６は、検出軸をＺ２軸としている。

本実施形態では、Ｘ１軸、Ｙ１軸、Ｚ１軸が互いに直交してなる三次元直交座標系（右手系直交座標系）が基準座標系として構成され、また、Ｘ２軸、Ｙ２軸、Ｚ２軸が互いに直交してなる三次元直交座標系（右手系直交座標系）が補助座標系として構成されている。言い換えると、角速度センサＧ１、Ｇ２、Ｇ３の検出軸で基準座標系が構成されており、角速度センサＧ４、Ｇ５、Ｇ６の検出軸で補助座標系が構成されている。

また、加速度部５１は、加速度センサＡ１、Ａ２、Ａ３が含まれて構成されており、それぞれから検出値ａ１、ａ２、ａ３を得ることができる。Ｘ１軸の加速度を検出可能な加速度センサＡ１は、検出軸をＸ１軸としている。また、Ｙ１軸の加速度を検出可能な加速度センサＡ２は、検出軸をＹ１軸としている。また、Ｚ１軸の加速度を検出可能な加速度センサＡ３は、検出軸をＺ１軸としている。

本実施形態では、角速度部５２は、角速度センサＧ１、Ｇ２、Ｇ３が含まれて構成されており、それぞれから検出値ω１、ω２、ω３を得ることができる。角速度部５３は、角速度センサＧ４、Ｇ５、Ｇ６が含まれて構成されており、それぞれから検出値ω４、ω５、ω６を得ることができる。また、角速度センサＧ１、Ｇ２、Ｇ３、Ｇ４、Ｇ５、Ｇ６として、振動式の角速度センサを用いている。このため、角速度センサＧ１、Ｇ２、Ｇ３、Ｇ４、Ｇ５、Ｇ６のそれぞれにはオフセットドリフトΔω１、Δω２、Δω３、Δω４、Δω５、Δω６が存在していることとなる。

ここで、角速度センサＧ１、Ｇ２、Ｇ３、Ｇ４、Ｇ５、Ｇ６にオフセットドリフトが存在していない場合について説明する。このとき、（Ｘ１、Ｙ１、Ｚ１）の基準座標系と、（Ｘ２、Ｙ２、Ｚ２）の補助座標系との間には、次の関係式（変換式）が成り立つ。

ω４＝ｋ（ω１＋ω２＋ω３） …… （１４）
ω５＝ｋ（ω１−ω２＋ω３） …… （１５）
ω６＝ｋ（ω１＋ω２−ω３） …… （１６）

式（１４）〜（１６）から、検出値（ω１、ω２、ω３）と、検出値（ω４、ω５、ω６）とは、相互に変換可能であることがわかる。ここで、角速度センサＧ１〜Ｇ６のオフセットドリフトは存在しないとした場合、ωｘ１＝ω１、ωｙ１＝ω２、ωｚ１＝ω３、ωｘ２＝ω４、ωｙ２＝ω５、ωｚ２＝ω６である。これにより、検出値ω１、ω２、ω３の検出軸であるＸ１軸、Ｙ１軸、Ｚ１軸で構成された基準座標系と、検出値ω４、ω５、ω６の検出軸であるＸ２軸、Ｙ２軸、Ｚ２軸で構成された補助座標系とは、相互に座標変換が可能であることがわかる。

このように、本実施形態においても、基準座標系と、補助座標系とは、相互に座標変換が可能であるため、前記実施形態２において図７を参照して説明したように、オフセットドリフトと実回転の分離が可能となる。すなわち、角速度センサの検出軸が重力軸と平行した場合であっても、その角速度センサのオフセットドリフトを低減することができる。したがって、本実施形態によれば、高精度の慣性計測装置１を提供することができる。また、前記実施形態１と比較して、座標変換の処理を行うことで、容易に被測定物の運動状態を検出することができる。

以上、本発明者によってなされた発明を実施形態に基づき具体的に説明したが、本発明は前記実施形態に限定されるものではなく、その要旨を逸脱しない範囲で種々変更可能であることはいうまでもない。

例えば、前記実施形態では、慣性計測装置が備える角度センサとして、振動式の角速度センサを適用した場合について説明したが、オフセットドリフトが存在する角度センサであれば適用することができる。

（慣性計測装置の用途について）
前記実施形態１〜３で説明した重力軸（鉛直軸）周りの角度（ヨー角）のドリフトが除去された慣性計測装置１を、人体動作計測や自動車などの乗り物の計測に用いることができる。以下、慣性計測装置１の用途について説明する。

（人体動作計測）
従来では計測が難しい人体の動き（例えば、体のひねり、歩行計測時の脚の回旋、関節の滑りなど）の角度を、ヨー角のドリフトが除去できる慣性計測装置１を用いることによって、計測することができる。

体のひねりの計測に慣性計測装置１を用いる。ヨー角のドリフト除去により、従来では計測が難しい、立位での腰・肩・頭部(首)のひねりが計測できる。例えば、立っている状態で体をひねった時の、腰・肩・頭部のひねり角度量の違いを計測できる。また、首を振る運動の角度を計測できる。神経損傷や脳損傷などの、運動機能障害の計測に有用である。なお、これらの計測は、従来では、医師・理学療法士の目視による観測に頼っているものである。

歩行計測時の脚の回旋の計測に慣性計測装置１を用いる。歩行の計測は、現在、高価な画像解析システムを用いて行っているが、画像の解像度に精度が左右され、細かい動きの検出が難しい。ヨー角のドリフト除去により、歩行時の脚の動きの軌跡が計測でき、また、脚の回旋も計測できるため、歩行障害のある人が、どのように脚を回し、障害を補っているかを計測できる。

関節の滑りの計測に慣性計測装置１を用いる。膝関節の上下などで、計測することにより、回転と直線移動双方の関節の滑りを計測することができるようになる。関節の滑りは、従来は計測が困難なものである。関節が滑って動いていることは、確認されているが、定量的に計測することは難しい。リハビリ用途だけでなく、スポーツ用途でも関節の滑りが計測できると、けがの予防効果などが期待できる。

（自動車など乗り物の計測）
従来では計測が難しい自動車などの乗り物の挙動を、ヨー角のドリフトが除去できる慣性計測装置１を用いることによって行うことができる。自動車やロボットなどの動きの軌跡は、センサのドリフトにより、加速度・角速度センサのみでは、きれいに描けず、従来では、磁気センサにより方位角の補正や、ＧＰＳにより位置の補正などを行い、加速度・角速度センサ以外のセンサなどを用いて精度を上げている。ヨー角のドリフト除去により、加速度・角速度センサのみで姿勢と位置を計測できるようになる。

１、１０１ 慣性計測装置
５１ 加速度部
５２ 角速度部
５３ 補助部（角速度部）
５４ 演算処理部
Ａ１、Ａ２、Ａ３ 加速度センサ
Ｇ１、Ｇ２、Ｇ３、Ｇ４、Ｇ５、Ｇ６ 角速度センサ

Claims (4)

1. 第１、第２および第３軸が互いに直交する三次元直交座標系における被測定物の運動状態を検出する慣性計測装置であって、
   前記被測定物に働く重力加速度を検出可能な加速度センサと、
   前記被測定物の回転運動を検出可能な複数の角速度センサと、
   複数の処理機能が組み込まれた演算処理部とを備えており、
   前記複数の角速度センサには、前記第１、第２および第３軸のそれぞれの角速度を検出可能な第１、第２および第３角速度センサと、前記第１、第２および第３角速度センサの検出軸とは異なる軸の角速度を検出可能な第４角速度センサとが含まれており、
   前記演算処理部には、前記複数の角速度センサの検出軸から、前記重力加速度の方向と最も平行な軸を決定する第１処理機能と、前記複数の角速度センサの検出軸から、前記第１処理機能で決定された軸を除いた３つの軸を選択する第２処理機能と、前記重力加速度を参照して、前記３つの軸に対応する角速度センサの検出値から、前記第１処理機能で決定された軸の角速度を算出する第３処理機能とが含まれていることを特徴とする慣性計測装置。
2. 請求項１記載の慣性計測装置において、
   前記複数の角速度センサには、第５および第６角速度センサが含まれており、
   前記第１、第２および第３角速度センサの検出軸で第１座標系が構成されており、
   前記第４、第５および第６角速度センサの検出軸で第２座標系が構成されており、
   前記第１座標系と前記第２座標系とは互いに座標変換が可能であり、
   前記第２処理機能では、前記第１および第２座標系のうち、前記第１処理機能で決定された軸が含まれた座標系とは異なる座標系から前記３つの軸が選択されることを特徴とする慣性計測装置。
3. 請求項２記載の慣性計測装置において、
   前記第４角速度センサの検出軸が、前記第１角速度センサの検出軸に対してπ／２［rad］、前記第２および第３角速度センサのそれぞれの検出軸に対してπ／４［rad］の角度を為しており、
   前記第５角速度センサの検出軸が、前記第２角速度センサの検出軸に対してπ／２［rad］、前記第３および第１角速度センサのそれぞれの検出軸に対してπ／４［rad］の角度を為しており、
   前記第６角速度センサの検出軸が、前記第３角速度センサの検出軸に対してπ／２［rad］、前記第１および第２角速度センサの検出軸に対してπ／４［rad］の角度を為していることを特徴とする慣性計測装置。
4. 請求項２記載の慣性計測装置において、
   前記第４、第５および第６角速度センサの検出軸のそれぞれが、前記第１、第２および第３角速度センサの検出軸のそれぞれに対してｃｏｓ^−１（１／√３）［rad］の角度を為していることを特徴とする慣性計測装置。

Images