姿勢検出装置の補正パラメーター作成方法、姿勢検出装置の補正パラメーター作成用装置及び姿勢検出装置
【課題】センサーの取付角誤差に起因する検出値の誤差を補正するための補正パラメーターをより低コストで作成可能な姿勢検出装置の補正パラメーター作成方法、補正パラメーター作成用装置及び補正機能付きの姿勢検出装置を提供すること。
【解決手段】回転板２３０を上面２３１が水平になるように設置し（Ｓ１０）、立方体治具２１０の面２１１に、Ｘ軸（第１の軸）が面２１２（第２の面）に垂直になり、Ｙ軸（第２の軸）が面２１３（第３の面）に垂直になり、Ｚ軸（第３の軸）が面２１１（第１の面）に垂直になるように、姿勢検出装置１を固定する（Ｓ１２）。そして、立方体治具の面２１２、２１３、２１１と対向する各面を回転板の上面に順次固定し（Ｓ１４、Ｓ２０、Ｓ２６）、回転板を静止又は所定の角速度で回転させて姿勢検出装置の検出値を取得し（Ｓ１６、Ｓ１８、Ｓ２２、Ｓ２４、Ｓ２８、Ｓ３０）、補正パラメーターを作成する（Ｓ３２）。
【解決手段】回転板２３０を上面２３１が水平になるように設置し（Ｓ１０）、立方体治具２１０の面２１１に、Ｘ軸（第１の軸）が面２１２（第２の面）に垂直になり、Ｙ軸（第２の軸）が面２１３（第３の面）に垂直になり、Ｚ軸（第３の軸）が面２１１（第１の面）に垂直になるように、姿勢検出装置１を固定する（Ｓ１２）。そして、立方体治具の面２１２、２１３、２１１と対向する各面を回転板の上面に順次固定し（Ｓ１４、Ｓ２０、Ｓ２６）、回転板を静止又は所定の角速度で回転させて姿勢検出装置の検出値を取得し（Ｓ１６、Ｓ１８、Ｓ２２、Ｓ２４、Ｓ２８、Ｓ３０）、補正パラメーターを作成する（Ｓ３２）。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、３軸の角速度又は加速度を検出するセンサーを含む姿勢検出装置の検出値を所定の直交座標系における検出値に補正するための補正パラメーター作成方法、補正パラメーター作成用装置及び補正機能付き姿勢検出装置に関する。
【背景技術】
【０００２】
近年、角速度センサーや加速度センサーにより物体の姿勢を検出する姿勢検出装置が様々な用途に使用されている。例えば、特許文献１には、ユーザーの頭の姿勢を検出することにより、目前にあるディスプレイに表示された映像が頭の動きに連動して変化し、仮想空間を体験できるヘッドマウントディスプレイが記載されている。ユーザーの頭の姿勢角にあった映像がヘッドマウントディスプレイに映し出される。この姿勢角を検出するために角速度センサーや加速度センサーを備えた姿勢検出装置がヘッドマウントディスプレイの所定の位置に取り付けられる。姿勢検出装置を取り付ける場合、センサーの検出軸が頭の姿勢角を表すための直交座標系の３軸とそれぞれ平行になるように取り付けられていなければ、この取付角の誤差に起因して姿勢検出装置の検出値が誤差を含むことになる。そのため、姿勢検出装置を取り付ける位置や角度が厳密に規定される。
【特許文献１】特開平９−１０６３２２号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
しかし、姿勢検出装置の内部にセンサーが取り付けられる際に取付角にわずかなずれがあると、姿勢検出装置を取り付ける位置や角度が厳密に規定されていたとしても高精度の検出結果を得ることができない。センサーの取付角誤差が無いようにするのはコスト面から現実的ではないため、あらかじめ取付角誤差を算出して姿勢検出装置の検出値を取付角誤差に応じた補正パラメーターで補正することが行われる。式（１）及び式（２）は、それぞれ補正パラメーターを用いた角速度センサー用の補正式及び加速度センサー用の補正式を表している。
【０００４】
【数１】
【０００５】
【数２】
式（１）において、関数行列式（ヤコビアン）ＪｆＧは角速度センサー用の補正パラメーターであり、ｆＧ（ｘ）及びｆＧ（ｐ）はそれぞれ角速度センサーの今回及び前回の補正値（理想値）である。同様に、式（２）において、関数行列式（ヤコビアン）ＪｆＡは加速度センサー用の補正パラメーターであり、ｆＡ（ｘ）及びｆＡ（ｐ）はそれぞれ加速度センサーの今回及び前回の補正値（理想値）である。また、式（１）、式（２）において、ｘ及びｐはそれぞれ角速度センサー又は角速度センサーの今回及び前回の検出値であり、ｏはランダウの記号である。
【０００６】
センサーの取付角誤差は姿勢検出装置毎に異なるため、出荷テスト時等に各姿勢検出装置に対して式（１）、式（２）の補正パラメーター（ＪｆＧ、ＪｆＡ）が作成される。図１３（Ａ）〜図１３（Ｃ）及び図１４（Ａ）〜図１４（Ｃ）は、補正パラメーター（ＪｆＧ、ＪｆＡ）を作成する従来方法を示している。従来方法では、まず、テーブル５１０に取り付けられたソケット５２０に姿勢検出装置をセットし、図１３（Ａ）〜図１３（Ｃ）に示す順に、回転腕５３０をＸ軸回り、Ｙ軸回り、Ｚ軸回りに所定の角速度で回転させて姿勢検出装置の各検出値を取得し、各検出値と各理想値を式（１）に代入して得られる連立方程式を解いて角速度センサー用の補正パラメーターを作成する。さらに、図１４（Ａ）〜図１４（Ｃ）に示す順に、回転腕５３０を操作してＸ軸、Ｙ軸、Ｚ軸の正方向が鉛直上向きになる状態（鉛直下向きに重力加速度が加わる状態）で静止させて姿勢検出装置の各検出値を取得し、各検出値と各理想値を式（２）に代入して得られる連立方程式を解いて加速度センサー用の補正パラメーターを作成する。
【０００７】
図１３（Ａ）〜図１３（Ｃ）及び図１４（Ａ）〜図１４（Ｃ）に示す回転腕５３０の操作において、Ｘ軸、Ｙ軸、Ｚ軸に対してテーブル５１０を所定の角度に正確に固定しなければ、角速度センサーおよび加速度センサーの取付角誤差が正確に反映された検出値を取得することができない。しかし、Ｘ軸、Ｙ軸、Ｚ軸に対してテーブル５１０を所定の角度に正確に固定するためには、テーブル５１０及び回転腕５３０を含む補正パラメーター作成用装置５００が大がかりな装置になる傾向がある。また、Ｘ軸、Ｙ軸、Ｚ軸に対してテーブル５１０を所定の角度に正確に固定するために相当の時間がかかる。角速度センサーと加速度センサーをともに含む姿勢検出装置に対する補正パラメーターを作成するためには、図１３（Ａ）〜図１３（Ｃ）に示す回転腕５３０の操作と図１４（Ａ）〜図１４（Ｃ）に示す回転腕５３０の操作を別々に行う必要があるためさらに時間がかかる。そのため、従来方法では、補正パラメーターの作成にかかるコストが大きいという問題がある。
【０００８】
本発明は、以上のような問題点に鑑みてなされたものであり、本発明のいくつかの態様によれば、センサーの取付角誤差に起因する検出値の誤差を補正するための補正パラメーターをより低コストで作成することができる姿勢検出装置の補正パラメーター作成方法、より低コストで実現可能な補正パラメーター作成用装置及び補正機能付きの姿勢検出装置を提供することができる。
【課題を解決するための手段】
【０００９】
（１）本発明は、検出軸が互いに直交する第１の軸、第２の軸及び第３の軸とそれぞれ略平行になるように取り付けられ角速度又は加速度を検出する第１のセンサー、第２のセンサー及び第３のセンサーを含み、前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの検出信号に基づいて物体の姿勢を検出する姿勢検出装置の検出値を、前記第１の軸、前記第２の軸及び前記第３の軸を座標軸とする直交座標系における検出値に補正する補正式の補正パラメーターを作成する方法であって、上面が水平になるように回転板を設置するステップと、互いに直交する第１の面、第２の面、第３の面を有する直方体形状の治具の前記第１の面に、前記第１の軸が前記第２の面に垂直になり、前記第２の軸が前記第３の面に垂直になり、前記第３の軸が前記第１の面に垂直になるように、前記姿勢検出装置を固定するステップと、前記治具の前記第２の面と対向する面を前記回転板の前記上面に固定し、前記回転板を静止又は所定の角速度で回転させて前記姿勢検出装置の検出値を取得する第１検出値取得ステップと、前記治具の前記第３の面と対向する面を前記回転板の前記上面に固定し、前記回転板を静止又は所定の角速度で回転させて前記姿勢検出装置の検出値を取得する第２検出値取得ステップと、前記治具の前記第１の面と対向する面を前記回転板の前記上面に固定し、前記回転板を静止又は所定の角速度で回転させて前記姿勢検出装置の検出値を取得する第３検出値取得ステップと、取得した検出値に基づいて、前記補正パラメーターを作成する補正パラメーター作成ステップと、を含むことを特徴とする。
【００１０】
Ｘ軸、Ｙ軸、Ｚ軸を座標軸とする直交座標系を考えた場合、第１の軸、第２の軸、第３の軸とＸ軸、Ｙ軸、Ｚ軸の対応関係は特に限定されない。
【００１１】
本発明によれば、直方体形状の治具を用いるので、第１の軸、第２の軸、第３の軸が治具のそれぞれ第１の面、第２の面、第３の面と垂直になるように第１の面に姿勢検出装置を固定することが容易である。そして、上面が水平になるように回転板を設置すれば、治具の第２の面、第３の面、第１の面と対向する面をそれぞれ回転板の上面に固定するだけで、簡単に、第１の軸、第２の軸、第３の軸をそれぞれ鉛直方向と平行にすることができる。さらに、第１の軸、第２の軸、第３の軸をそれぞれ鉛直方向と平行にした状態において、回転板を静止又は回転させることにより、加速度センサー又は角速度センサーの検出値を簡単に短時間で取得することができる。
【００１２】
すなわち、最初に一度だけ、上面が水平になるように回転板を設置すれば回転板の回転方向が固定されるので、第１の軸、第２の軸、第３の軸に関する検出値を取得するためのセッティング時間を大幅に短縮することができる。従って、本発明によれば、センサーの取付角誤差に起因する検出値の誤差を補正するための補正パラメーターをより低コストで作成することができる。
【００１３】
（２）本発明の姿勢検出装置の補正パラメーター作成方法において、前記補正式は、前記補正パラメーターとして前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各検出値を前記直交座標系における各検出値に補正するための第１の補正行列、第２の補正行列及び第３の補正行列を含み、前記第１の補正行列、前記第２の補正行列及び前記第３の補正行列の各々と、前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各検出値がＡ／Ｄ変換されたデジタル値をそれぞれ要素として含む行列の各々と、の積により得られる３つの行列の和として与えられるようにしてもよい。
【００１４】
（３）本発明の姿勢検出装置の補正パラメーター作成方法において、前記第１の補正行列、前記第２の補正行列及び前記第３の補正行列は、前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各検出軸をそれぞれ前記第１の軸、前記第２の軸及び前記第３の軸に変換する回転行列の逆行列であるようにしてもよい。
【００１５】
（４）本発明の姿勢検出装置の補正パラメーター作成方法において、前記補正パラメーター作成ステップは、前記第１検出値取得ステップにおいて取得した前記検出値に基づいて、前記第２のセンサー及び前記第３のセンサーの前記第１の軸回りの各取付角誤差を算出するステップと、前記第２検出値取得ステップにおいて取得した前記検出値に基づいて、前記第１のセンサー及び前記第３のセンサーの前記第２の軸回りの各取付角誤差を算出するステップと、前記第３検出値取得ステップにおいて取得した前記検出値に基づいて、前記第１のセンサー及び前記第２のセンサーの前記第３の軸回りの各取付角誤差を算出するステップと、前記第１のセンサーの前記第２の軸回りの前記取付角誤差及び前記第３の軸回りの前記取付角誤差に基づいて、前記第１の補正行列を作成するステップと、前記第２のセンサーの前記第１の軸回りの前記取付角誤差及び前記第３の軸回りの前記取付角誤差に基づいて、前記第２の補正行列を作成するステップと、前記第３のセンサーの前記第１の軸回りの前記取付角誤差及び前記第２の軸回りの前記取付角誤差に基づいて、前記第３の補正行列を作成するステップと、を含むようにしてもよい。
【００１６】
（５）本発明は、検出軸が互いに直交する第１の軸、第２の軸及び第３の軸とそれぞれ略平行になるように取り付けられ角速度又は加速度を検出する第１のセンサー、第２のセンサー及び第３のセンサーを含み、前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの検出信号に基づいて物体の姿勢を検出する姿勢検出装置の検出値を、前記第１の軸、前記第２の軸及び前記第３の軸を座標軸とする直交座標系における検出値に補正する補正式の補正パラメーターを作成するために使用される補正パラメーター作成用装置であって、互いに直交する第１の面、第２の面、第３の面を有し、前記第１の面に、前記第１の軸が前記第２の面に垂直になり、前記第２の軸が前記第３の面に垂直になり、前記第３の軸が前記第１の面に垂直になるように前記姿勢検出装置を固定可能な直方体形状の治具と、上面に、前記治具の前記第１の面、前記第２の面、前記第２の面とそれぞれ対向する面のいずれかを固定可能な回転板と、前記回転板を所定の角速度で回転させる回転制御部と、を含むことを特徴とする。
【００１７】
本発明によれば、直方体形状の治具と回転板を用いることにより、回転腕を必要としないので、よりコンパクトかつ低コストの補正パラメーター作成用装置を提供することができる。本発明に係る補正パラメーター作成用装置を用いることにより、上記の通り、姿勢検出装置に取り付けられた各センサーの検出値の補正パラメーターを簡単に短時間で取得することができる。
【００１８】
（６）本発明は、検出軸が互いに直交する第１の軸、第２の軸及び第３の軸とそれぞれ略平行になるように取り付けられ角速度又は加速度を検出する第１のセンサー、第２のセンサー及び第３のセンサーと、
前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各検出値を前記第１の軸、前記第２の軸及び前記第３の軸を座標軸とする直交座標系における検出値に補正する補正式の補正パラメーターが記憶された記憶部と、前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各検出信号をデジタル信号に変換する処理を行うＡ／Ｄ変換処理部と、前記デジタル信号の各々と前記補正パラメーターに基づいて前記補正式を計算する処理を行う補正計算処理部と、を含み、前記補正式は、前記補正パラメーターとして前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各検出値を前記直交座標系における各検出値に補正するための第１の補正行列、第２の補正行列及び第３の補正行列を含み、前記第１の補正行列、前記第２の補正行列及び前記第３の補正行列の各々と、前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各検出値がＡ／Ｄ変換されたデジタル値をそれぞれ要素として含む行列の各々と、の積により得られる３つの行列の和として与えられることを特徴とする。
【００１９】
従来の補正式（１）、補正式（２）における関数行列式（ヤコビアン）はセンサーの取付角誤差を直接的に反映する補正パラメーターではなく、また、補正式（１）、補正式（２）では前回の検出値を元に関数行列式（ヤコビアン）を用いて今回の検出値を類推するため、検出値に何らかの写像を施すと補正値が得られるようになっていない。そのため、補正式（１）、補正式（２）では補正精度を上げるのに限界がある。
【００２０】
本発明によれば、補正計算処理部が計算する補正式に含まれる３つの補正行列に各センサーの取付角誤差を直接的に反映させることができる。また、本発明によれば、補正計算処理部が計算する補正式は今回の検出値に対する補正値の計算において前回の検出値を必要としないので、今回の検出値が得られれば直ちに補正値を計算することができる。従って、本発明によれば、補正精度がより高く、かつ、補正計算処理がより速い姿勢検出装置を実現することができる。
【００２１】
（７）本発明の姿勢検出装置において、前記第１の補正行列、前記第２の補正行列及び前記第３の補正行列は、前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各検出軸をそれぞれ前記第１の軸、前記第２の軸及び前記第３の軸に変換する回転行列の逆行列であるようにしてもよい。
【００２２】
（８）本発明の姿勢検出装置は、所定の周期で、前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各前記検出信号のいずれか１つを順次選択する処理を行う信号選択処理部を含み、前記Ａ／Ｄ変換処理部は、前記信号選択処理部が選択した検出値を順次Ａ／Ｄ変換処理するＡ／Ｄ変換回路を含むようにしてもよい。
【発明を実施するための最良の形態】
【００２３】
以下、本発明の好適な実施形態について図面を用いて詳細に説明する。なお、以下に説明する実施の形態は、特許請求の範囲に記載された本発明の内容を不当に限定するものではない。また以下で説明される構成の全てが本発明の必須構成要件であるとは限らない。
【００２４】
なお、以下の説明において、本発明における第１の軸、第２の軸、第３の軸はそれぞれＸ軸、Ｙ軸、Ｚ軸に対応するものとするが、本発明における第１の軸、第２の軸、第３の軸とＸ軸、Ｙ軸、Ｚ軸の対応関係はこれに限らず、任意の対応関係とすることができる。
【００２５】
１．姿勢検出装置
１−１．姿勢検出装置の構成
図１は、本実施形態の補正パラメーター作成方法の対象となる姿勢検出装置の構成の一例を示す図である。
【００２６】
図１に示すように、本実施形態における姿勢検出装置１は、Ｘ軸、Ｙ軸、Ｚ軸回りの角速度を検出する角速度センサーモジュール２と、Ｘ軸、Ｙ軸、Ｚ軸方向の加速度を検出する加速度センサーモジュール３を含んで構成されている。
【００２７】
角速度センサーモジュール２は、それぞれＸ軸、Ｙ軸、Ｚ軸回りの角速度を検出するＸ軸角速度センサー１０ａ、Ｙ軸角速度センサー１０ｂ、Ｚ軸角速度センサー１０ｃを含む。
【００２８】
Ｘ軸角速度センサー１０ａは、振動子１１ａ、振動子１１ａを振動させる駆動回路２０ａ、角速度検出信号３８ａを生成する検出回路３０ａを含み、振動子１１ａの駆動電極１２ａ、１３ａと駆動回路２０ａ、振動子１１ａの検出電極１４ａ、１５ａと検出回路３０ａがそれぞれ接続されている。
【００２９】
同様に、Ｙ軸角速度センサー１０ｂは、振動子１１ｂ、振動子１１ｂを振動させる駆動回路２０ｂ、角速度検出信号３８ｂを生成する検出回路３０ｂを含み、振動子１１ｂの駆動電極１２ｂ、１３ｂと駆動回路２０ｂ、振動子１１ｂの検出電極１４ｂ、１５ｂと検出回路３０ｂがそれぞれ接続されている。
【００３０】
同様に、Ｚ軸角速度センサー１０ｃは、振動子１１ｃ、振動子１１ｃを振動させる駆動する駆動回路２０ｃ、角速度検出信号３８ｃを生成する検出回路３０ｃを含み、振動子１１ｃの駆動電極１２ｃ、１３ｃと駆動回路２０ｃ、振動子１１ｃの検出電極１４ｃ、１５ｃと検出回路３０ｃがそれぞれ接続されている。
【００３１】
加速度センサーモジュール３は、それぞれＸ軸、Ｙ軸、Ｚ軸方向の加速度を検出するＸ軸加速度センサー５０ａ、Ｙ軸加速度センサー５０ｂ、Ｚ軸加速度センサー５０ｃを含む。
【００３２】
Ｘ軸加速度センサー５０ａは、振動子５１ａ、振動子５１ａを振動させる駆動回路６０ａ、加速度検出信号７８ａを生成する検出回路７０ａを含み、振動子５１ａの駆動電極５２ａ、５３ａと駆動回路６０ａ、振動子５１ａの検出電極５４ａ、５５ａと検出回路７０ａがそれぞれ接続されている。
【００３３】
同様に、Ｙ軸加速度センサー５０ｂは、振動子５１ｂ、振動子５１ｂを振動させる駆動回路６０ｂ、加速度検出信号７８ｂを生成する検出回路７０ｂを含み、振動子５１ｂの駆動電極５２ｂ、５３ｂと駆動回路６０ｂ、振動子５１ｂの検出電極５４ｂ、５５ｂと検出回路７０ｂがそれぞれ接続されている。
【００３４】
同様に、Ｚ軸加速度センサー５０ｃは、振動子５１ｃ、振動子５１ｃを振動させる駆動回路６０ｃ、加速度検出信号７８ｃを生成する検出回路７０ｃを含み、振動子５１ｃの駆動電極５２ｃ、５３ｃと駆動回路６０ｃ、振動子５１ｃの検出電極５４ｃ、５５ｃと検出回路７０ｃがそれぞれ接続されている。
【００３５】
なお、角速度センサー１０ａ、１０ｂ、１０ｃは、それぞれ本発明における第１のセンサー、第２のセンサー及び第３のセンサーとして機能する。同様に、加速度センサー５０ａ、５０ｂ、５０ｃは、それぞれ本発明における第１のセンサー、第２のセンサー及び第３のセンサーとして機能する。
【００３６】
図２は、本実施形態における姿勢検出装置の斜視図である。
【００３７】
図２に示すように、姿勢検出装置１において、角速度センサーモジュール２及び加速度センサーモジュール３は、それぞれ立方体（広義には直方体。以下、同じ。）の形状に形成されており、直方体形状のパッケージ４の中に収納されている。
【００３８】
Ｘ軸、Ｙ軸、Ｚ軸は姿勢検出装置１を基準にして決定される。例えば、姿勢検出装置１を構成するパッケージ４が直方体形状である場合は、パッケージ４の直交する３つの面５ａ、５ｂ、５ｃと垂直な軸をそれぞれＸ軸、Ｙ軸、Ｚ軸とすることができる。また、Ｘ軸、Ｙ軸、Ｚ軸の正方向は任意に決定することができ、本実施形態では、図２に示す矢印の先に向かう方向を各軸の正方向とする。
【００３９】
１−２．角速度センサーモジュール
図２に示すように、角速度センサーモジュール２において、角速度センサー１０ａ、１０ｂ、１０ｃは、検出軸がそれぞれＸ軸、Ｙ軸、Ｚ軸とほぼ平行になるように絶縁基板８０の上に取り付けられ、それぞれ振動子１１ａ、１１ｂ、１１ｃがパッケージ８２ａ、８２ｂ、８２ｃ内に収容されている。パッケージ８２ａ、８２ｂ、８２ｃの周囲は樹脂モールド材で覆われている。
【００４０】
パッケージ８２ａ、８２ｂ、８２ｃは、それぞれパッケージ本体８４ａと蓋体８６ａ、パッケージ本体８４ｂと蓋体８６ｂ、パッケージ本体８４ｃと蓋体８６ｃから構成されている。パッケージ本体８４ａ、８４ｂ、８４ｃは、複数のセラミックシートを積層、焼結して直方体の箱状に形成されている。蓋体８６ａ、８６ｂ、８６ｃは、ガラス板や金属板、セラミックシート等で形成されており、金属ロウ材、低融点ガラスなどの接合材を介して、振動子１１ａ、１１ｂ、１１ｃがそれぞれ収容されたパッケージ本体８４ａ、８４ｂ、８４ｃの上面開口部を真空封止している。振動子１１ａ、１１ｂ、１１ｃは、絶縁基板８０に形成された配線パターン（図示せず）によってそれぞれ駆動回路２０ａ、２０ｂ、２０ｃや検出回路３０ａ、３０ｂ、３０ｃと接続されている。
【００４１】
駆動回路２０ａと検出回路３０ａ、駆動回路２０ｂと検出回路３０ｂ、駆動回路２０ｃと検出回路３０ｃは、３つのチップにＩＣ化してそれぞれパッケージ８２ａ、８２ｂ、８２ｃ内にそれぞれ収容されていてもよい。また、駆動回路２０ａ、２０ｂ、２０ｃ、検出回路３０ａ、３０ｂ、３０ｃを１チップにＩＣ化して絶縁基板８０上に配置するようにしてもよい。
【００４２】
なお、図２では図示を省略しているが、検出回路３０ａ、３０ｂ、３０ｃからの各検出信号３８ａ、３８ｂ、３８ｃは外部出力端子（図示せず）を介して姿勢検出装置１の外部に出力されるようになっている。
【００４３】
図３は、角速度センサーに含まれる振動子の一例を示す平面図である。角速度センサー１０ａ、１０ｂ、１０ｃにそれぞれ含まれる振動子１１ａ、１１ｂ、１１ｃはすべて同一の構造であるため、図３では振動子１１ａの構造のみが図示されている。なお、図３におけるＸ軸、Ｙ軸、Ｚ軸は水晶の軸を示し、図２におけるＸ軸、Ｙ軸、Ｚ軸とは無関係である。
【００４４】
振動子１１ａは、水晶などの圧電材料の薄板から形成され、駆動用基部４４ａから駆動振動腕４１ａ（広義には、駆動用振動片）が水晶のＹ軸方向に延出している。駆動振動腕４１ａの側面及び上面にはそれぞれ駆動電極１２ａ及び１３ａが形成されている。図１に示したように、駆動電極１２ａ、１３ａは駆動回路２０ａに接続される。
【００４５】
駆動用基部４４ａは、水晶のＸ軸方向に延びる連結腕４５ａを介して検出用基部４７ａに接続されている。検出振動腕４２ａ（広義には、検出振動片）は、検出用基部４７ａから水晶のＹ軸方向に延出されている。検出振動腕４２ａの上面には検出電極１４ａ及び１５ａが形成されており、検出振動腕４２ａの側面には電極１６ａが形成されている。図１に示したように、検出電極１４ａ、１５ａは駆動回路２０ａに接続される。また、電極１６ａは接地される。
【００４６】
駆動振動腕４１ａの駆動電極１２ａと駆動電極１３ａとの間に交番電圧／交番電流からなる駆動信号が与えられると、図４に示すように、駆動振動腕４１ａは圧電効果によって矢印Ｂのように屈曲振動する。
【００４７】
ここで、図５に示すように振動子１１ａが水晶のＺ軸を回転軸とした回転運動をすると、駆動振動腕４１ａは、矢印Ｂの屈曲振動の方向と水晶のＺ軸の両方に垂直な方向にコリオリの力を得る。その結果、連結腕４５ａは矢印Ｃで示すような振動をする。そして、検出振動腕４２ａは、連結腕４５ａの振動（矢印Ｃ）に連動して、連結腕４５ａと矢印Ｄのような屈曲振動をする。
【００４８】
そして、これらの屈曲振動に基づいて発生する逆圧電効果によって、検出振動腕４２ａの検出電極１４ａ、１５ａと電極１６ａとの間には、それぞれ逆方向の交番電圧／交番電流が発生する。以上のようにして、振動子１１ａは、水晶のＺ軸を検出軸としてコリオリの力に基づく角速度成分を検出し、検出電極１４ａ、１５ａを介して検出信号を出力する。
【００４９】
なお、図３の構成では、振動子１１ａのバランスを良くするために、検出用基部４７ａを中央に配置し、検出用基部４７ａから＋Ｙ軸と−Ｙ軸の両方向に検出振動腕４２ａを延出させている。さらに、検出用基部４７ａから＋Ｘ軸と−Ｘ軸の両方向に連結腕４５ａを延出させ、連結腕４５ａのそれぞれから、＋Ｙ軸と−Ｙ軸の両方向に駆動振動腕４１ａを延出させている。
【００５０】
また、駆動振動腕４１ａの先端を幅広の幅広部４３ａにし、さらに、錘を付けることでコリオリの力を大きくしている。また、錘効果によって、所望の共振周波数を、短い振動腕で得ることができる。同様の理由で、検出振動腕４２ａの先端を幅広の幅広部４６ａにし、さらに、錘を付けている。
【００５１】
なお、振動子１１ａは、上述の構成に限らず、コリオリの力に基づく角速度成分を含む検出信号を出力する振動子であれば良い。例えば、駆動振動腕と検出振動腕とを兼ねる構成であっても良く、また、駆動振動腕や検出振動腕に圧電膜を形成した構成であっても良い。
【００５２】
図６は、角速度センサーに含まれる駆動回路及び検出回路の構成の一例を示す図である。駆動回路２０ａ、２０ｂ、２０ｃはすべて同じ構成であり、検出回路３０ａ、３０ｂ、３０ｃはすべて同じ構成であるので、図３では駆動回路２０ａ及び検出回路３０ａの構成のみを図示している。
【００５３】
図６に示すように、駆動回路２０ａは、電流電圧変換器（Ｉ／Ｖ変換器）２１ａ、ＡＣ増幅器２２ａ、自動利得制御回路（ＡＧＣ）２３ａ、コンパレータ２４ａを含んで構成されている。
【００５４】
振動子１１ａが振動すると、圧電効果に基づく交流電流がフィードバック信号として駆動電極１３ａから出力され、電流電圧変換器（Ｉ／Ｖ変換器）２１ａに入力される。電流電圧変換器（Ｉ／Ｖ変換器）２１ａは、入力された交流電流を振動子１１ａの振動周波数と同一の周波数の交流電圧信号に変換して出力する。
【００５５】
電流電圧変換器（Ｉ／Ｖ変換器）２１ａから出力された交流電圧信号は、ＡＣ増幅器２２ａに入力される。ＡＣ増幅器ａは、入力された交流電圧信号を増幅して出力する。
【００５６】
ＡＣ増幅器２２ａから出力された交流電圧信号は自動利得制御回路（ＡＧＣ）２３ａに入力される。自動利得制御回路（ＡＧＣ）２３ａは、入力された交流電圧信号の振幅を一定値に保持するように利得を制御し、利得制御後の交流電圧信号を振動子１１ａの駆動電極１２ａに出力する。この駆動電極１２ａに入力される交流電圧信号により振動子１１ａが振動する。
【００５７】
ＡＣ増幅器２２ａが増幅した交流電圧信号はコンパレータ２４ａに入力され、交流電圧信号の振幅中心を基準電圧として、交流電圧信号と基準電圧信号との比較結果に応じて出力レベルを切り替える方形波電圧信号を、検出回路３０ａの同期検波回路３５ａに出力する。
【００５８】
図６に示すように、検出回路３０ａは、チャージアンプ３１ａ、３２ａ、差動増幅器３３ａ、ＡＣ増幅器３４ａ、同期検波回路３５ａ、ＤＣ増幅器３６ａ及び積分回路（ＬＰＦ）３７ａを含んで構成されている。
【００５９】
チャージアンプ３１ａ、３２ａには、振動子１１ａにより検出された互いに逆位相の検出信号（交流電流）が検出電極１２ａ、１３ａを介して入力される。そして、チャージアンプ３１ａ、３２ａは、入力された検出信号（交流電流）を基準電圧を中心とする交流電圧信号に変換する。
【００６０】
差動増幅器３３ａはチャージアンプ３１ａの出力信号とチャージアンプ３２ａの出力信号を差動増幅する。差動増幅器３３ａの出力信号は、さらにＡＣ増幅器３４ａで増幅される。
【００６１】
同期検波回路３５ａは、コンパレータ２４ａが出力する方形波電圧信号を基に、ＡＣ増幅器３４ａの出力信号を同期検波することにより角速度成分を抽出する。同期検波回路３５ａは、例えば、方形波電圧信号の電圧レベルが基準電圧よりも高い時はＡＣ増幅器３４ａの出力信号をそのまま出力し、方形波電圧信号の電圧レベルが基準電圧よりも低い時はＡＣ増幅器３４ａの出力信号を基準電圧に対して反転して出力するスイッチ回路として構成することができる。
【００６２】
同期検波回路３５ａで抽出された角速度成分信号は、ＤＣ増幅器３６ａで増幅されて積分回路（ＬＰＦ）３７ａに入力される。
【００６３】
積分回路（ＬＰＦ）３７ａは、ＤＣ増幅器３５ａの出力信号から高周波成分を減衰させて直流成分を抽出することにより角速度検出信号３８ａを生成して外部に出力する。
【００６４】
１−３．加速度センサーモジュール
図２に示すように、加速度センサーモジュール３は、ベース９０、ウエイト１００、３つの加速度センサー５０ａ、５０ｂ、５０ｃを有する。なお、図２では、図１に示した駆動回路６０ａ、６０ｂ、６０ｃ及び検出回路７０ａ、７０ｂ、７０ｃの図示を省略しているがパッケージ４の中の適当な位置に配置され、検出回路７０ａ、７０ｂ、７０ｃからの各検出信号７８ａ、７８ｂ、７８ｃは外部出力端子（図示せず）を介して姿勢検出装置１の外部に出力されるようになっている。
【００６５】
ベース９０は、立方形を作るように３つの正方形の壁部を互いに直交させて形成され、Ｘ軸、Ｙ軸、Ｚ軸方向に互いに直交する３つの取付面９１、９２、９３を有する。ウエイト１００は、所定の質量を有する立方体からなり、互いに直交する３つの接合面１０１、１０２、１０３を有する。ベース９０及びウエイト１００は、例えばアルミニウム合金などの適当な材料を用いて形成される。
【００６６】
本実施形態では、加速度センサー５０ａ、５０ｂ、５０ｃは、それぞれ、水晶などの圧電材料の薄板から形成された双音叉型の振動子５１ａ、５１ｂ、５１ｃを含んで構成されている。
【００６７】
振動子５１ａ、５１ｂ、５１ｃは、検出軸がそれぞれＸ軸、Ｙ軸、Ｚ軸とほぼ平行になるように、一方の基端部５６ａ、５６ｂ、５６ｃがそれぞれベース９０の素子取付面９１、９２、９３に取り付けられ、ベース９０の各壁部に垂直に支持されている。振動子５１ａ、５１ｂ、５１ｃの他方の基端部５７ａ、５７ｂ、５７ｃは、それぞれ素子取付面９１、９２、９３に対応するウエイト１００の素子接合面１０１〜１０３に接合されている。これにより、ウエイト１００が、Ｘ軸、Ｙ軸、Ｚ軸方向から振動子５１ａ、５１ｂ、５１ｃによって浮遊した状態に支持される。
【００６８】
振動子５１ａの２つの駆動振動腕５８ａには、その上下主面及び両側面に駆動電極５２ａ、５３ａが設けられており（図示省略）、駆動回路６０ａにより駆動電極５２ａ、５３ａ間に所定の交流電圧が印加されると、２つの駆動振動腕５８ａは互いに逆向きに即ち近接または離反する向きに所定の周波数で屈曲振動する。
【００６９】
振動子５１ａを所定の周波数で振動させた状態で、加速度センサーモジュール３に外力が作用してウエイト１００にＸ軸方向の加速度が加わると、その大きさ及び向きに対応して、振動子５１ａには長手方向（すなわち、Ｘ軸方向）に圧縮または引張する力が作用する。振動子５１ａの周波数は、圧縮する力が作用すると減少し、引張する力が作用すると増加するように変化する。従って、検出回路７０ａにより振動子５１ａにおける周波数の変化量を検出し、周波数の変化量からＸ軸方向に作用する荷重を算出することにより、ウエイト１００に作用したＸ軸方向の加速度の大きさ及び向きを計算することができる。
【００７０】
振動子５１ｂ、５１ｃの構造も振動子５１ａの構造と同一であり、同様にしてＹ軸及びＺ軸方向の加速度の大きさ及び向きを計算することができる。
【００７１】
なお、駆動回路６０ａ、６０ｂ、６０ｃは図６に示した駆動回路２０ａと同様の構成であり、また、検出回路７０ａ、７０ｂ、７０ｃは周波数の変化量を検出する既知の回路と同様の構成にすることができるので、その説明を省略する。
【００７２】
２．補正パラメーター作成方法
２−１．センサー取付角誤差
角速度センサー１０ａ、１０ｂ、１０ｃは、理想的には、検出軸がそれぞれＸ軸、Ｙ軸、Ｚ軸と厳密に平行になるように取り付けられる。同様に、加速度センサー５０ａ、５０ｂ、５０ｃは、理想的には、検出軸がそれぞれＸ軸、Ｙ軸、Ｚ軸と厳密に平行になるように取り付けられる。しかし、角速度センサー１０ａ、１０ｂ、１０ｃ及び加速度センサー５０ａ、５０ｂ、５０ｃをそのように厳密に取り付けるのはコスト面から難しい。そのため、Ｘ軸角速度センサー１０ａは、実際には、図７（Ａ）に示すように、検出軸がＹ軸回りに微小角Δθ２ｘ、Ｚ軸回りに微小角Δθ３ｘ回ったＸ’軸と平行になるように取り付けられている。同様に、Ｙ軸角速度センサー１０ｂは、実際には、図７（Ｂ）に示すように、検出軸がＺ軸回りに微小角Δθ３ｙ、Ｘ軸回りに微小角Δθ１ｙ回ったＹ’軸と平行になるように取り付けられており、Ｚ軸角速度センサー１０ｃは、実際には、図７（Ｃ）に示すように、検出軸がＸ軸回りに微小角Δθ１ｚ、Ｙ軸回りに微小角Δθ２ｚ回ったＺ’軸と平行になるように取り付けられている。すなわち、Ｘ軸角速度センサー１０ａのＹ軸回りの取付角誤差及びＺ軸回りの取付角誤差はそれぞれΔθ２ｘ、Δθ３ｘであり、Ｙ軸角速度センサー１０ｂのＺ軸回りの取付角誤差及びＸ軸回りの取付角誤差はそれぞれΔθ３ｙ、Δθ１ｙであり、Ｚ軸角速度センサー１０ｃのＸ軸回りの取付角誤差及びＹ軸回りの取付角誤差はそれぞれΔθ３ｚ、Δθ１ｚである。
【００７３】
加速度センサー５０ａ、５０ｂ、５０ｃについても同様に取付角誤差が存在する。そのため、角速度センサー１０ａ、１０ｂ、１０ｃ、加速度センサー５０ａ、５０ｂ、５０ｃの各検出値は理想値とずれている。
【００７４】
２−２．数学的考察
従来の補正式（１）、補正式（２）における関数行列式（ヤコビアン）はセンサーの取付角誤差を直接的に反映する補正パラメーターではなく、また、補正式（１）、補正式（２）では前回の検出値を元に関数行列式（ヤコビアン）を用いて今回の検出値を類推するため、検出値に何らかの写像を施すと補正値が得られるようになっていない。そのため、補正式（１）、補正式（２）では補正精度を上げるのに限界がある。そこで、以下では、より精度の高い補正について数学的に考察する。
【００７５】
３次元ユークリッド空間において、Ｘ軸、Ｙ軸、Ｚ軸の回りにそれぞれ角度θの回転を施す回転行列Ｔ１、Ｔ２、Ｔ３は式（３）によって与えられる。
【００７６】
【数３】
そして、３次元ユークリッド空間における任意の回転は回転行列Ｔ１、Ｔ２、Ｔ３の積の組み合わせにより表すことができる。例えば、Ｚ軸の回りに角度θ３だけ回転させ、Ｙ軸の回りに角度θ２だけ回転させ、Ｘ軸の回りに角度θ１だけ回転させることにより、ＸＹＺ座標系をＸ’Ｙ’Ｚ’座標系に変換する行列Ｔδは式（４）によって与えられる。以下では、Ｔδを「変換行列」ということにする。
【００７７】
【数４】
３つの角速度センサー１０ａ、１０ｂ、１０ｃを、検出軸がそれぞれＸ軸、Ｙ軸、Ｚ軸と平行になるように取り付けたとき、取付角誤差のために実際には検出軸がそれぞれＸ’軸、Ｙ’軸、Ｚ’軸と平行になるように取り付けられていると仮定する。この場合、角速度センサー１０ａ、１０ｂ、１０ｃの各検出値Ｇｘ’、Ｇｙ’、Ｇｚ’と理想値Ｇｘ、Ｇｙ、Ｇｚの間には変換行列Ｔδによる関係式（５）が成立する。
【００７８】
【数５】
従って、次の式（６）により、角速度センサー１０ａ、１０ｂ、１０ｃの各検出値Ｇｘ’、Ｇｙ’、Ｇｚ’から理想値Ｇｘ、Ｇｙ、Ｇｚを計算することができる。
【００７９】
【数６】
すなわち、何らかの方法でＴδ−１を得ることができれば、式（６）を用いて角速度センサー１０ａ、１０ｂ、１０ｃの各検出値を理想値に補正することができる。以下では、Ｔδ−１を「補正行列」ということにする。
【００８０】
角速度センサー１０ａ、１０ｂ、１０ｃの取付角を光学観測できる場合は、直接θ１、θ２、θ３を導出して式（３）を用いて回転行列Ｔ１、Ｔ２、Ｔ３を計算し、その逆行列Ｔ１−１、Ｔ２−１、Ｔ３−１を用いて補正行列Ｔδ−１を得ることができる。
【００８１】
一方、光学観測ができない場合は、例えば、Ｘ軸、Ｙ軸、Ｚ軸を中心に角速度センサー１０ａ、１０ｂ、１０ｃを回転させる等、Ｘ軸、Ｙ軸、Ｚ軸に関する取付角誤差が検出値に反映されるような３通りの入力条件を選択し、その入力条件に対する角速度センサー１０ａ、１０ｂ、１０ｃの検出値Ｇｘ’、Ｇｙ’、Ｇｚ’及び理想値Ｇｘ、Ｇｙ、Ｇｚをそれぞれ式（６）に代入することにより得られる３つの連立方程式を解けばθ１、θ２、θ３を導出することができる。しかし、この連立方程式は非常に複雑であるためθ１、θ２、θ３を簡単に導出することができない。
【００８２】
一方、仮にθが非常に小さい値であれば、次の式（７）が成り立つ。
【００８３】
【数７】
従って、取付角誤差θ１、θ２、θ３が非常に小さい値であれば、変換行列Ｔδは次の式（８）のように表される。
【００８４】
【数８】
従って、次の式（９）に示すように、変換行列Ｔδは３つの基底となる行列Ｊ１、Ｊ２、Ｊ３の線形和で表すことができる。
【００８５】
【数９】
前記のように、Ｘ軸角速度センサー１０ａは、Ｙ軸回りに微小角Δθ２ｘ、Ｚ軸回りに微小角Δθ３ｘ回ったＸ’軸が検出軸になるように取り付けられている。Ｘ軸回りの取付角誤差Δθ１ｘ＝０であるので、式（９）より、変換行列Ｔδｘは次の式（１０）のように表される。
【００８６】
【数１０】
同様に、Ｙ軸角速度センサー１０ｂは、Ｘ軸回りに微小角Δθ１ｙ、Ｚ軸回りに微小角Δθ３ｙ回ったＹ’軸が検出軸になるように取り付けられている。Ｙ軸回りの取付角誤差Δθ２ｙ＝０であるので、式（９）より、変換行列Ｔδｙは次の式（１１）のように表される。
【００８７】
【数１１】
同様に、Ｚ軸角速度センサー１０ｃは、Ｘ軸回りに微小角Δθ１ｚ、Ｙ軸回りに微小角Δθ２ｚ回ったＺ’軸が検出軸になるように取り付けられている。Ｚ軸回りの取付角誤差Δθ３ｚ＝０であるので、式（９）より、変換行列Ｔδｚは次の式（１２）のように表される。
【００８８】
【数１２】
式（１０）、（１１）、（１２）によれば、何らかの方法でΔθ２ｘ、Δθ３ｘ、Δθ１ｙ、Δθ３ｙ、Δθ１ｚ、Δθ２ｚを得ることができれば、変換行列Ｔδｘ、Ｔδｙ、Ｔδｚを計算することができる。そして、変換行列Ｔδｘ、Ｔδｙ、Ｔδｚの逆行列を計算すれば、補正行列Ｔδｘ−１、Ｔδｙ−１、Ｔδｚ−１を作成することができる。そうすると、次の式（１３）より、Ｘ軸角速度センサー１０ａ、Ｙ軸角速度センサー１０ｂ、Ｚ軸角速度センサー１０ｃの各検出値Ｇｘ’、Ｇｙ’、Ｇｚ’をそれぞれ理想値Ｇｘ、Ｇｙ、Ｇｚに補正することができる。
【００８９】
【数１３】
補正式（１３）における補正行列Ｔδｘ−１、Ｔδｙ−１、Ｔδｚ−１は、角速度センサー１０ａ、１０ｂ、１０ｃの各取付角誤差Δθ２ｘ、Δθ３ｘ、Δθ１ｙ、Δθ３ｙ、Δθ１ｚ、Δθ２ｚを直接的に反映している。また、補正式（１３）によれば、前回の検出値を必要とせず、今回の検出値Ｇｘ’、Ｇｙ’、Ｇｚ’が得られれば直ちに補正値（理想値）Ｇｘ、Ｇｙ、Ｇｚを計算することができる。従って、補正式（１３）によれば、補正精度の向上及び補正計算処理の高速化を実現することができる。
【００９０】
なお、補正行列Ｔδｘ−１、Ｔδｙ−１、Ｔδｚ−１は、それぞれ本発明における第１の補正行列、第２の補正行列及び第３の補正行列に相当する。
【００９１】
次に、Δθ２ｘ、Δθ３ｘ、Δθ１ｙ、Δθ３ｙ、Δθ１ｚ、Δθ２ｚを得るための方法について説明する。
【００９２】
Ｘ軸角速度センサー１０ａをＸ軸回りに角度Δθｘｘ、Ｙ軸回りに角度Δθｘｙ、Ｚ軸回りに角度Δθｘｚだけ移動させたときに、Ｘ’軸回りに角度Δθｘｘ’、Ｙ’軸回りに角度Δθｘｙ’、Ｚ’軸回りに角度Δθｘｚ’だけ移動することを示す検出値が得られる関係にあるとすると、式（１０）より次の式（１４）が成立する。
【００９３】
【数１４】
同様に、Ｙ軸角速度センサー１０ｂをＸ軸回りに角度Δθｙｘ、Ｙ軸回りに角度Δθｙｙ、Ｚ軸回りに角度Δθｙｚだけ移動させたときに、Ｘ’軸回りに角度Δθｙｘ’、Ｙ’軸回りに角度Δθｙｙ’、Ｚ’軸回りに角度Δθｙｚ’だけ移動することを示す検出値が得られる関係にあるとすると、式（１１）より次の式（１５）が成立する。
【００９４】
【数１５】
同様に、Ｚ軸角速度センサー１０ｃをＸ軸回りに角度Δθｚｘ、Ｙ軸回りに角度Δθｚｙ、Ｚ軸回りに角度Δθｚｚだけ移動させたときに、Ｘ’軸回りに角度Δθｚｘ’、Ｙ’軸回りに角度Δθｚｙ’、Ｚ’軸回りに角度Δθｚｚ’だけ移動することを示す検出値が得られる関係にあるとすると、式（１２）より次の式（１６）が成立する。
【００９５】
【数１６】
まず、姿勢検出装置１をＹ軸及びＺ軸回りには回転させずにＸ軸回りにのみ角度Δθｘだけ回転させた場合、式（１５）においてΔθｙｘ＝Δθｘ、Δθｙｙ＝０、Δθｙｚ＝０なので、Δθｙｘ’＝Δθｘ、Δθｙｚ’＝０とともに以下の関係式（１７）を得ることができる。
【００９６】
【数１７】
Ｙ軸角速度センサー１０ｂの検出値（Ｙ’軸回りの角速度）に所定時間を掛けることによりΔθｙｙ’を得ることができるので、式（１７）にΔθｙｙ’とΔθｘを代入することにより、Δθ３ｙを得ることができる。
【００９７】
同様に、式（１６）においてΔθｚｘ＝Δθｘ、Δθｚｙ＝０、Δθｚｚ＝０なので、Δθｚｘ’＝Δθｘ、Δθｚｙ’＝０とともに以下の関係式（１８）を得ることができる。
【００９８】
【数１８】
Ｚ軸角速度センサー１０ｃの検出値（Ｚ’軸回りの角速度）に所定時間を掛けることによりΔθｚｚ’を得ることができるので、式（１８）にΔθｚｚ’とΔθｘを代入することにより、Δθ２ｚを得ることができる。
【００９９】
次に、姿勢検出装置１をＸ軸及びＺ軸回りには回転させずにＹ軸回りにのみ角度Δθｙだけ回転させた場合、式（１４）においてΔθｘｘ＝０、Δθｘｙ＝Δθｙ、Δθｘｚ＝０なので、Δθｘｙ’＝Δθｙ、Δθｘｚ’＝０とともに以下の関係式（１９）を得ることができる。
【０１００】
【数１９】
Ｘ軸角速度センサー１０ａの検出値（Ｘ’軸回りの角速度）に所定時間を掛けることによりΔθｘｘ’を得ることができるので、式（１９）にΔθｘｘ’とΔθｙを代入することにより、Δθ３ｘを得ることができる。
【０１０１】
同様に、式（１６）においてΔθｚｘ＝０、Δθｚｙ＝Δθｙ、Δθｚｚ＝０なので、Δθｚｘ’＝０、Δθｚｙ’＝Δθｙとともに以下の関係式（２０）を得ることができる。
【０１０２】
【数２０】
Ｚ軸角速度センサー１０ｃの検出値（Ｚ’軸回りの角速度）に所定時間を掛けることによりΔθｚｚ’を得ることができるので、式（２０）にΔθｚｚ’とΔθｙを代入することにより、Δθ１ｚを得ることができる。
【０１０３】
最後に、姿勢検出装置１をＸ軸及びＹ軸回りには回転させずにＺ軸回りにのみ角度Δθｚだけ回転させた場合、式（１４）においてΔθｘｘ＝０、Δθｘｙ＝０、Δθｘｚ＝Δθｚなので、Δθｘｙ’＝０、Δθｘｚ’＝Δθｚとともに以下の関係式（２１）を得ることができる。
【０１０４】
【数２１】
Ｘ軸角速度センサー１０ａの検出値（Ｘ’軸回りの角速度）に所定時間を掛けることによりΔθｘｘ’を得ることができるので、式（２１）にΔθｘｘ’とΔθｚを代入することにより、Δθ２ｘを得ることができる。
【０１０５】
同様に、式（１５）においてΔθｙｘ＝０、Δθｙｙ＝０、Δθｙｚ＝Δθｚなので、Δθｙｘ’＝０、Δθｙｚ’＝Δθｚとともに以下の関係式（２２）を得ることができる。
【０１０６】
【数２２】
Ｙ軸角速度センサー１０ｂの検出値（Ｙ’軸回りの角速度）に所定時間を掛けることによりΔθｙｙ’を得ることができるので、式（２２）にΔθｙｙ’とΔθｚを代入することにより、Δθ１ｙを得ることができる。
【０１０７】
以上のようにして得られたΔθ２ｘ、Δθ３ｘ、Δθ１ｙ、Δθ３ｙ、Δθ１ｚ、Δθ２ｚから計算される逆行列Ｔδｘ−１、Ｔδｙ−１、Ｔδｚ−１を式（１３）に代入すれば、角速度センサー１０ａ、１０ｂ、１０ｃの各検出値Ｇｘ’、Ｇｙ’、Ｇｚ’をそれぞれ理想値Ｇｘ、Ｇｙ、Ｇｚに補正することができる。
【０１０８】
なお、実際には、式（１３）の補正計算はＣＰＵ又は専用回路によりデジタル値で行われる。そのため、角速度センサー１０ａ、１０ｂ、１０ｃの検出値Ｇｘ’、Ｇｙ’、Ｇｚ’のＡ／Ｄ変換値にＡ／Ｄ変換のサンプル周期Δｔを掛けて得られるＸ’軸、Ｙ’軸、Ｚ’軸回りの微小回転角Δθｘ’、Δθｙ’、Δθｚ’が、次の式（２３）により、Ｘ軸、Ｙ軸、Ｚ軸回りの微小回転角Δθｘ、Δθｙ、Δθｚに補正される。
【０１０９】
【数２３】
Ｘ軸加速度センサー５０ａ、Ｙ軸加速度センサー５０ｂ、Ｚ軸加速度センサー５０ｃの各検出値の補正においても、同様の理論が成り立つ。
【０１１０】
Ｘ軸加速度センサー５０ａは、Ｙ軸回りに微小角Δφ２ｘ、Ｚ軸回りに微小角Δφ３ｘ回ったＸ’軸が検出軸になるように取り付けられており（Δφ２ｘ、Δφ３ｘが取付角誤差になる）、変換行列Ｔγｘは次の式（２４）のように表される。
【０１１１】
【数２４】
同様に、Ｙ軸加速度センサー５０ｂは、Ｘ軸回りに微小角Δφ１ｙ、Ｚ軸回りに微小角Δφ３ｙ回ったＹ’軸が検出軸になるように取り付けられており（Δφ１ｙ、Δφ３ｙが取付角誤差になる）、変換行列Ｔγｙは次の式（２５）のように表される。
【０１１２】
【数２５】
同様に、Ｚ軸加速度センサー５０ｃは、Ｘ軸回りに微小角Δφ１ｚ、Ｙ軸回りに微小角Δφ２ｚ回ったＺ’軸が検出軸になるように取り付けられており（Δφ１ｚ、Δφ２ｚが取付角誤差になる）、変換行列Ｔγｚは次の式（２６）のように表される。
【０１１３】
【数２６】
式（２４）〜式（２６）は角速度センサー１０ａ、１０ｂ、１０ｃにおける式（１０）〜式（１２）と対応している。
【０１１４】
Ｘ軸加速度センサー５０ａをＸ軸方向に速度Δｖｘｘ、Ｙ軸方向に速度Δｖｘｙ、Ｚ軸方向に速度Δｖｘｚで移動させたときに、Ｘ’軸方向に速度Δｖｘｘ’、Ｙ’軸方向に速度Δｖｘｙ’、Ｚ’軸方向に速度Δｖｘｚ’で移動することを示す検出値が得られる関係にあるとすると、次の式（２７）が成立する。
【０１１５】
【数２７】
Ｙ軸加速度センサー５０ｂをＸ軸方向に速度Δｖｙｘ、Ｙ軸方向に速度Δｖｙｙ、Ｚ軸方向に速度Δｖｙｚで移動させたときに、Ｘ’軸方向に速度Δｖｙｘ’、Ｙ’軸方向に速度Δｖｙｙ’、Ｚ’軸方向に速度Δｖｙｚ’で移動することを示す検出値が得られる関係にあるとすると、次の式（２８）が成立する。
【０１１６】
【数２８】
Ｚ軸加速度センサー５０ｃをＸ軸方向に速度Δｖｚｘ、Ｙ軸方向に速度Δｖｚｙ、Ｚ軸方向に速度Δｖｚｚで移動させたときに、Ｘ’軸方向に速度Δｖｚｘ’、Ｙ’軸方向に速度Δｖｚｙ’、Ｚ’軸方向に速度Δｖｚｚ’で移動することを示す検出値が得られる関係にあるとすると、次の式（２９）が成立する。
【０１１７】
【数２９】
式（２７）〜式（２９）は角速度センサー１０ａ、１０ｂ、１０ｃにおける式（１４）〜式（１６）と対応している。そして、角速度センサー１０ａ、１０ｂ、１０ｃにおける式（１７）〜式（２２）の導出と同様の方法により以下の（３０）〜式（３５）が得られる。
【０１１８】
【数３０】
【０１１９】
【数３１】
【０１２０】
【数３２】
【０１２１】
【数３３】
【０１２２】
【数３４】
【０１２３】
【数３５】
そして、Δφ２ｘ、Δφ３ｘ、Δφ１ｙ、Δφ３ｙ、Δφ１ｚ、Δφ２ｚから計算される逆行列Ｔγｘ−１、Ｔγｙ−１、Ｔγｚ−１を次の式（３６）に代入すれば、加速度センサー５０ａ、５０ｂ、５０ｃの各検出値Ａｘ’、Ａｙ’、Ａｚ’をそれぞれ理想値Ａｘ、Ａｙ、Ａｚに補正することができる。なお、式（３６）は角速度センサー１０ａ、１０ｂ、１０ｃにおける式（１３）と対応している。
【０１２４】
【数３６】
なお、実際には、式（３６）の補正計算はＣＰＵ又は専用回路によりデジタル値で行われる。そのため、加速度センサー５０ａ、５０ｂ、５０ｃの検出値Ａｘ’、Ａｙ’、Ａｚ’のＡ／Ｄ変換値にＡ／Ｄ変換のサンプル周期Δｔを掛けて得られるＸ’軸、Ｙ’軸、Ｚ’軸方向の微小速度Δｖｘ’、Δｖｙ’、Δｖｚ’が、次の式（３７）により、Ｘ軸、Ｙ軸、Ｚ軸方向の微小速度Δｖｘ、Δｖｙ、Δｖｚに補正される。
【０１２５】
【数３７】
２−３．補正パラメーター作成用装置
図８は、本実施形態の補正パラメーター作成用装置の構成を示す図である。
【０１２６】
補正パラメーター作成用装置２００は、姿勢検出装置１に含まれる各センサーの取付角誤差に起因する誤差を含む検出値を理想値に補正するための補正パラメーター（補正行列）を作成するために使用される。
【０１２７】
補正パラメーター作成用装置２００は、立方体治具２１０、ソケット２２０、回転板２３０、回転モーター２４０、支持台２５０、ケーブル２６０等により構成される。
【０１２８】
立方体治具２１０は、金属等の材料により立法体形状（直方体形状でもよい）に成型され、３つの面２１１、２１２、２１３が互いに直交するように厳密に面取りがされており、面２１１にはソケット２２０が固着されている。立方体治具２１０の３つの面２１１、２１２、２１３は、それぞれ本発明における治具の第１の面、第２の面及び第３の面に相当する。
【０１２９】
ソケット２２０は、ソケット本体２２２と開閉可能な蓋体２２４で構成されており、ソケット本体２２０は姿勢検出装置１を所定の向きに隙間無く収容できるようになっている。
【０１３０】
立方体治具２１０は、姿勢検出装置１をソケット２２０にセットすることにより、Ｘ軸、Ｙ軸及びＺ軸がそれぞれ面２１２、２１３及び２１１と垂直になるように、姿勢検出装置１を固定可能になっている。また、立方体治具２１０の面２１１、２１２、２１３とそれぞれ対向する３つの面２１４、２１５、２１６にはそれぞれ固定金具（図示せず）が設けられている。
【０１３１】
回転板２３０は、上面２３１の凹凸が無視できるほど小さく、上面２３１には固定金具（図示せず）が設けられており、立方体治具２１０のいずれかの固定金具を回転板２３０の固定金具に接合することにより、立法体治具２１０の面２１４、２１５、２１６のいずれかを上面２３１に固定可能になっている。
【０１３２】
また、回転板２３０は傾きを調整することができるようになっており、データ補正装置２００が設置された状態で回転板２３０の上面２３１が水平になるように厳密に調整される。
【０１３３】
回転モーター２４０は、支持台２５０に取り付けられており、鉛直方向を軸として時計回り又は半時計回りに所定範囲の角速度で回転できるようになっている。
【０１３４】
ケーブル２６０は回転モーター２４０の制御回路（図示せず）に接続されている。ケーブル２６０にはパーソナルコンピュータ等の制御装置（図示せず）が接続され、ＧＰＩＢ（General Purpose Interface Bus）等のインターフェースにより回転モーター２５０の回転速度が調整できるようになっている。
【０１３５】
なお、回転モーター２４０は、本発明における回転制御部として機能する。
【０１３６】
本実施形態によれば、立方体治具２１０と回転板２３０を用いることにより、図１３（Ａ）等に示した回転腕５３０を必要としないので、よりコンパクトかつ低コストの補正パラメーター作成用装置２００を提供することができる。本実施形態に係る補正パラメーター作成用装置２００を用いることにより、後述するように、補正行列Ｔδｘ−１、Ｔδｙ−１、Ｔδｚ−１、Ｔγｘ−１、Ｔγｙ−１、Ｔγｚ−１を簡単に短時間で取得することができる。
【０１３７】
２−４．補正パラメーター作成手順
次に、図８に示した補正パラメーター作成用装置２００を用いて、補正パラメーター（補正行列）を作成する手順の一例について説明する。
【０１３８】
図９は、本実施形態における補正パラメーター作成手順の一例を示すフローチャート図である。
【０１３９】
まず、回転板２３０を上面２３１が水平になるように設置する（ステップＳ１０）。
【０１４０】
次に、立方体冶具２１０の面２１１に取り付けられているソケット２２０に姿勢検出装置１をセットする（ステップＳ１２）。
【０１４１】
次に、立方体治具２１０の面２１２と対向する面２１５を回転板２３０の上面２３１に固定する（ステップＳ１４）。これにより、補正パラメーター作成用装置２００は図１０（Ａ）のようにセッティングされ、Ｘ軸の正方向が鉛直上向きになるように、立方体冶具２１０が回転板２３０に固定される。
【０１４２】
次に、回転板２３０を静止させた状態でＹ軸加速度センサー５０ｂ、Ｚ軸加速度センサー５０ｃの各検出値を取得し、式（３０）、式（３１）よりΔφ３ｙ、Δφ２ｚを計算する（ステップＳ１６）。具体的には、加速度センサー５０ｂ、５０ｃの各検出値Ａｙ’、Ａｚ’をサンプリングしてＡ／Ｄ変換した値にサンプル周期Δｔを掛けて得られる微小速度Δｖｙ’、Δｖｚ’を計算する。ここで、Δｖｙ’及びΔｖｚ’は、それぞれ式（３０）におけるΔｖｙｙ’及び式（３１）におけるΔｖｚｚ’に相当する。また、加速度センサー５０ｂ、５０ｃにはＸ軸の負方向に重力加速度ｇが加わるため、式（３０）、式（３１）におけるΔｖｘ＝−ｇ×Δｔである。従って、式（３０）、式（３１）よりΔφ３ｙ、Δφ２ｚを計算することができる。
【０１４３】
次に、回転板２３０を角速度ωｘで回転させてＹ軸角速度センサー１０ｂ、Ｚ軸角速度センサー１０ｃの各検出値を取得し、式（１７）、式（１８）よりΔθ３ｙ、Δθ２ｚを計算する（ステップＳ１８）。具体的には、角速度センサー１０ｂ、１０ｃの各検出値Ｇｙ’、Ｇｚ’をサンプリングしてＡ／Ｄ変換した値にサンプル周期Δｔを掛けて得られる微小回転角Δθｙ’、Δθｚ’を計算する。ここで、Δθｙ’及びΔθｚ’は、それぞれ式（１７）におけるΔθｙｙ’及び式（１８）におけるΔθｚｚ’に相当する。また、角速度センサー１０ｂ、１０ｃはＸ軸回りに角速度ωｘで回転するため、式（１７）、式（２８）におけるΔθｘ＝ωｘ×Δｔである。従って、式（１７）、式（１８）よりΔθ３ｙ、Δθ２ｚを計算することができる。
【０１４４】
次に、立方体治具２１０の面２１３と対向する面２１６を回転板２３０の上面２３１に固定する（ステップＳ２０）。これにより、補正パラメーター作成用装置２００は図１０（Ｂ）のようにセッティングされ、Ｙ軸の正方向が鉛直上向きになるように、立方体冶具２１０が回転板２３０に固定される。
【０１４５】
次に、回転板２３０を静止させた状態でＸ軸加速度センサー５０ａ、Ｚ軸加速度センサー５０ｃの各検出値を取得し、式（３２）、式（３３）よりΔφ３ｘ、Δφ１ｚを計算する（ステップＳ２２）。ステップＳ２２における具体的な処理は、ステップＳ１６と同様であるため説明を省略する。
【０１４６】
次に、回転板２３０を角速度ωｙで回転させてＸ軸角速度センサー１０ａ、Ｚ軸角速度センサー１０ｃの各検出値を取得し、式（１９）、式（２０）よりΔθ３ｘ、Δθ１ｚを計算する（ステップＳ２４）。ステップＳ２４における具体的な処理は、ステップＳ１８と同様であるため説明を省略する。
【０１４７】
次に、立方体治具２１０の面２１１と対向する面２１４を回転板２３０の上面２３１に固定する（ステップＳ２６）。これにより、補正パラメーター作成用装置２００は図１０（Ｃ）のようにセッティングされ、Ｚ軸の正方向が鉛直上向きになるように、立方体冶具２１０が回転板２３０に固定される。
【０１４８】
次に、回転板２３０を静止させた状態でＸ軸加速度センサー５０ａ、Ｙ軸加速度センサー５０ｂの各検出値を取得し、式（３４）、式（３５）よりΔφ２ｘ、Δφ１ｙを計算する（ステップＳ２８）。ステップＳ２８における具体的な処理は、ステップＳ１６と同様であるため説明を省略する。
【０１４９】
次に、回転板２３０を角速度ωｚで回転させてＸ軸角速度センサー１０ａ、Ｙ軸角速度センサー１０ｂの各検出値を取得し、式（２１）、式（２２）よりΔθ２ｘ、Δθ１ｙを計算する（ステップＳ３０）。ステップＳ３０における具体的な処理は、ステップＳ１８と同様であるため説明を省略する。
【０１５０】
最後に、補正行列Ｔδｘ−１、Ｔδｙ−１、Ｔδｚ−１、Ｔγｘ−１、Ｔγｙ−１、Ｔγｚ−１を作成する（ステップＳ３２）。具体的には、ステップＳ２４、Ｓ３０でそれぞれ計算したΔθ３ｘ、Δθ２ｘを式（１０）に代入して得られる変換行列Ｔδｘの逆行列を計算することにより補正行列Ｔδｘ−１を作成することができる。同様に、ステップＳ１８、Ｓ３０でそれぞれ計算したΔθ３ｙ、Δθ１ｙを式（１１）に代入して得られる変換行列Ｔδｙの逆行列を計算することにより補正行列Ｔδｙ−１を作成することができる。同様に、ステップＳ１８、Ｓ２４でそれぞれ計算したΔθ２ｚ、Δθ１ｚを式（１２）に代入して得られる変換行列Ｔδｚの逆行列を計算することにより補正行列Ｔδｚ−１を作成することができる。同様に、ステップＳ２２、Ｓ２８でそれぞれ計算したΔφ３ｘ、Δφ２ｘを式（２４）に代入して得られる変換行列Ｔγｘの逆行列を計算することにより補正行列Ｔγｘ−１を作成することができる。同様に、ステップＳ１６、Ｓ２８でそれぞれ計算したΔφ３ｙ、Δφ１ｙを式（２５）に代入して得られる変換行列Ｔγｙの逆行列を計算することにより補正行列Ｔγｙ−１を作成することができる。同様に、ステップＳ１６、Ｓ２２でそれぞれ計算したΔφ２ｚ、Δφ１ｚを式（２６）に代入して得られる変換行列Ｔγｚの逆行列を計算することにより補正行列Ｔγｚ−１を作成することができる。
【０１５１】
なお、以上の処理は、補正パラメーター作成用装置２００のケーブル２６０に接続されたパーソナルコンピュータ等により行われる。そして、本実施形態を用いて作成した補正パラメーターは、例えば、姿勢検出装置１の後段に接続されるユーザー側のマイコンに実装される補正計算処理用のタスクにおいて使用される。
【０１５２】
本実施形態によれば、立方体治具２１０を用いることにより、Ｘ軸、Ｙ軸、Ｚ軸が立方体治具２１０のそれぞれ面２１２、面２１３、面２１１と垂直になるように、面２１１に姿勢検出装置１を容易に固定することができる。そして、上面２３１が水平になるように回転板２３０を設置すれば、立方体治具２１０の面２１５、２１６、２１４をそれぞれ回転板２３０の上面２３１に固定するだけで、簡単に、Ｘ軸、Ｙ軸、Ｚ軸をそれぞれ鉛直方向と平行にすることができる。さらに、Ｘ軸、Ｙ軸、Ｚ軸をそれぞれ鉛直方向と平行にした状態において、回転板２３０を静止させることにより加速度センサー５０ａ、５０ｂ、５０ｃの各検出値を、回転板２３０を回転させることにより角速度センサー１０ａ、１０ｂ、１０ｃの各検出値を、それぞれ簡単に短時間で取得することができる。
【０１５３】
すなわち、最初に一度だけ、上面２３１が水平になるように回転板２３０を設置すれば回転板２３０の回転方向が固定されるので、Ｘ軸、Ｙ軸、Ｚ軸に関する検出値を取得するためのセッティング時間を大幅に短縮することができる。従って、本実施形態によれば、補正行列Ｔδｘ−１、Ｔδｙ−１、Ｔδｚ−１、Ｔγｘ−１、Ｔγｙ−１、Ｔγｚ−１をより低コストで作成することができる。
【０１５４】
なお、本実施形態では、例えば、Ｘ軸角速度センサー１０ａの補正行列Ｔδｘ−１は、Ｙ軸及びＺ軸回りの回転時のＸ軸角速度センサー１０ａの検出値に基づいて作成される。そのため、各検出軸に対する取付角誤差と他軸感度誤差の両方が考慮された補正パラメーターが作成される。
【０１５５】
本実施形態を用いて作成した補正パラメーターは、移動体やロボットの姿勢検出、姿勢制御を行う装置、バーチャルリアリティー等に使用されるヘッドマウントディスプレイ、頭の姿勢角度を検出するトラッカー、３Ｄゲームパッド等を使用するゲーム機、デジタルカメラ、携帯電話機、携帯型情報端末、カーナビゲーションシステム等の種々の電子機器に組み込まれる姿勢検出装置の検出値の補正に使用することができる。
【０１５６】
３．補正機能付き姿勢検出装置
図１１は、本実施形態の姿勢検出装置の構成を示す図である。
【０１５７】
補正機能付き姿勢検出装置３００は、角速度センサーモジュール２、加速度センサーモジュール３、アンチエリアスフィルター３１０ａ、３１０ｂ、３１０ｃ、３５０ａ、３５０ｂ、３５０ｃ、Ａ／Ｄ変換回路３２０ａ、３２０ｂ、３２０ｃ、３６０ａ、３６０ｂ、３６０ｃ、補正計算処理部３７０、記憶部３８０を含んで構成されている。
【０１５８】
角速度センサーモジュール２及び加速度センサーモジュール３は図１及び図２と同じ構成であるため、その説明を省略する。
【０１５９】
アンチエリアスフィルター３１０ａ、３１０ｂ、３１０ｃ、３５０ａ、３５０ｂ、３５０ｃは、それぞれＡ／Ｄ変換回路３２０ａ、３２０ｂ、３２０ｃ、３６０ａ、３６０ｂ、３６０ｃの前段に配置され、角速度検出信号３８ａ、３８ｂ、３８ｃ及び加速度検出信号７８ａ、７８ｂ、７８ｃに対して、それぞれＡ／Ｄ変換回路３２０ａ、３２０ｂ、３２０ｃ、３６０ａ、３６０ｂ、３６０ｃのサンプリングによりＤＣ付近の周波数帯域に折り返すノイズをあらかじめ無視できる程度にまで減衰させる。
【０１６０】
なお、アンチエリアスフィルター３１０ａ、３１０ｂ、３１０ｃ、３６０ａ、３６０ｂ、３６０ｃは、例えば、スイッチトキャパシタフィルター（Switched Capacitor Filter（ＳＣＦ））として構成することができる。
【０１６１】
Ａ／Ｄ変換回路３２０ａ、３２０ｂ、３２０ｃ、３６０ａ、３６０ｂ、３６０ｃは、角速度検出信号３８ａ、３８ｂ、３８ｃ及び加速度検出信号７８ａ、７８ｂ、７８ｃがアンチエリアスフィルター３１０ａ、３１０ｂ、３１０ｃ、３５０ａ、３５０ｂ、３５０ｃによってそれぞれフィルター処理された信号をそれぞれ所定ビット数の角速度検出信号３２２ａ、３２２ｂ、３２２ｃ及び加速度検出信号３６２ａ、３６２ｂ、３６２ｃに変換する。Ａ／Ｄ変換回路３２０ａ、３２０ｂ、３２０ｃ、３６０ａ、３６０ｂ、３６０ｃは、本発明におけるＡ／Ｄ変換処理部として機能し、フラッシュ型（並列比較型）、パイプライン型、逐次比較型、デルタシグマ方式等の既知の様々なタイプのＡＤ変換回路により構成することができる。
【０１６２】
記憶部３８０には、角速度センサーの補正パラメーター３８２と加速度センサーの補正用パラメーター３８４が記憶されている。具体的には、補正パラメーター３８２は補正行列Ｔδｘ−１、Ｔδｙ−１、Ｔδｚ−１であり、補正パラメーター３８４は補正行列Ｔγｘ−１、Ｔγｙ−１、Ｔγｚ−１である。
【０１６３】
補正計算処理部３７０は、角速度検出信号３２２ａ、３２２ｂ、３２２ｃと補正パラメーター３８２に基づいて補正式（２３）を計算することにより、角速度センサー１０ａ、１０ｂ、１０ｃの取付角誤差に起因する角速度検出信号３８ａ、３８ｂ、３８ｃの誤差が補正された角速度検出信号３０２ａ、３０２ｂ、３０２ｃを生成する。具体的には、補正計算処理部３７０は、角速度検出信号３２２ａ、３２２ｂ、３２２ｃのデジタル値にＡ／Ｄ変換のサンプル周期Δｔを掛けた値を補正式（２３）の微小回転角Δθｘ’、Δθｙ’、Δθｚ’にそれぞれ代入して微小回転角Δθｘ、Δθｙ、Δθｚを計算し、さらに微小回転角Δθｘ、Δθｙ、ΔθｚをΔｔで割ったデジタル値に対応する角速度検出信号３０２ａ、３０２ｂ、３０２ｃを生成する。
【０１６４】
同様に、補正計算処理部３７０は、加速度検出信号３６２ａ、３６２ｂ、３６２ｃと補正パラメーター３８４に基づいて補正式（３７）を計算することにより、加速度センサー５０ａ、５０ｂ、５０ｃの取付角誤差に起因する加速度検出信号７８ａ、７８ｂ、７８ｃの誤差が補正された角速度検出信号３０４ａ、３０４ｂ、３０４ｃを生成する。具体的には、補正計算処理部３７０は、加速度検出信号３６２ａ、３６２ｂ、３６２ｃのデジタル値にＡ／Ｄ変換のサンプル周期Δｔを掛けた値を補正式（３７）の微小速度Δｖｘ’、Δｖｙ’、Δｖｚ’にそれぞれ代入して微小速度Δｖｘ、Δｖｙ、Δｖｚを計算し、さらに微小速度Δｖｘ、Δｖｙ、ΔｖｚをΔｔで割ったデジタル値に対応する加速度検出信号３０４ａ、３０４ｂ、３０４ｃを生成する。
【０１６５】
補正計算処理部３７０は補正計算処理を行う専用回路として実現することもできるし、ＣＰＵ（Central Processing Unit）が記憶部３８０等に記憶されたプログラムを実行することにより補正計算処理部３７０の機能を実現することもできる。
【０１６６】
図１２は、本実施形態の姿勢検出装置の他の構成を示す図である。
【０１６７】
図１２において、角速度センサーモジュール２、加速度センサーモジュール３、アンチエリアスフィルター３１０ａ、３１０ｂ、３１０ｃ、３５０ａ、３５０ｂ、３５０ｃ、記憶部３８０の構成は図１１と同様であるため、その説明を省略する。
【０１６８】
マルチプレクサ３９０は、角速度検出信号３８ａ、３８ｂ、３８ｃ及び加速度検出信号７８ａ、７８ｂ、７８ｃがアンチエリアスフィルター３１０ａ、３１０ｂ、３１０ｃ、３５０ａ、３５０ｂ、３５０ｃによってそれぞれフィルター処理された信号を所定周期で時分割に順次選択する。
【０１６９】
Ａ／Ｄ変換回路３２０は、マルチプレクサ３９０により選択された信号を所定ビット数の検出信号３２２に変換する。Ａ／Ｄ変換回路３２０及びマルチプレクサ３９０は、それぞれ本発明におけるＡ／Ｄ変換処理部及び信号選択処理部として機能する。
【０１７０】
補正計算処理部３７０は、所定周期毎に検出信号３２２をサンプリングし、検出信号３２２が角速度検出信号３８ａ、３８ｂ、３８ｃに対応する場合は、検出信号３２２と補正パラメーター３８２に基づいて補正式（２３）を計算することにより補正された角速度検出信号をそれぞれ生成し、検出信号３２２として時分割に出力する。
【０１７１】
同様に、補正計算処理部３７０は、検出信号３２２が加速度検出信号７８ａ、７８ｂ、７８ｃに対応する場合は、検出信号３２２と補正パラメーター３８４に基づいて補正式（３７）を計算することにより補正された加速度検出信号をそれぞれ生成し、検出信号３２２として時分割に出力する。
【０１７２】
なお、図１１又、図１２に示した姿勢検出装置３００において、補正計算処理部３７０をバイパスしてＡ／Ｄ変換回路３２０ａ等の出力を外部に出力可能なバイパスモードを設ければ、バイパスモードに設定した状態で本実施形態の補正パラメーター作成方法を用いて補正パラメーター３８２、３８４を作成することができる。すなわち、姿勢検出装置３００も本実施形態の補正パラメーター作成方法の適用対象になり得る。
【０１７３】
本実施形態によれば、補正計算処理部３７０は、前記の通り、補正精度の向上及び補正計算処理の高速化を実現可能な補正式（２３）及び補正式（３７）に従い補正値を計算するので、補正精度がより高く、かつ、補正計算処理がより速い姿勢検出装置を実現することができる。
【０１７４】
また、本実施形態によれば、姿勢検出装置３００の後段に接続されるユーザー側のマイコンに補正計算処理を実装する必要がなくなるため、タスクカプセル化の見地からユーザーに受け入れられやすい。
【０１７５】
また、本実施形態の姿勢検出装置３００はセンサー検出信号をデジタル化して出力するので、姿勢検出装置３００とユーザー側のマイコンの間にＡ／Ｄ変換回路を接続する必要がなくなる。
【０１７６】
本実施形態の姿勢検出装置３００は、移動体やロボットの姿勢検出、姿勢制御を行う装置、バーチャルリアリティー等に使用されるヘッドマウントディスプレイ、頭の姿勢角度を検出するトラッカー、３Ｄゲームパッド等を使用するゲーム機、デジタルカメラ、携帯電話機、携帯型情報端末、カーナビゲーションシステム等の種々の電子機器に組み込むことができる。
【０１７７】
なお、本発明は本実施形態に限定されず、本発明の要旨の範囲内で種々の変形実施が可能である。
【０１７８】
例えば、図１に示した姿勢検出装置は、３つの角速度センサー１０ａ、１０ｂ、１０ｃと３つの加速度センサー５０ａ、５０ｂ、５０ｃを含んでいるが、本発明の補正パラメーター作成方法が適用される姿勢検出装置はこれに限られない。すなわち、本発明の補正パラメーター作成方法が適用される姿勢検出装置は直交する３軸の角速度又は加速度を検出可能な構成であればよく、例えば、角速度センサー１０ａ、１０ｂ、１０ｃのみを含む姿勢検出装置、加速度センサー５０ａ、５０ｂ、５０ｃのみを含む姿勢検出装置、角速度センサー１０ａ、１０ｂと加速度センサー５０ｃのみを含む姿勢検出装置、角速度センサー１０ａと加速度センサー５０ｂ、５０ｃのみを含む姿勢検出装置等も適用対象に含まれる。
【０１７９】
また、例えば、図８に示した補正パラメーター作成用装置２００は、面２１１にソケット２２０が１つだけ取り付けられているが、面２１１に複数のソケット２２０を取り付けるようにしてもよい。そして、複数のソケット２２０の各々に姿勢検出装置１をセットすることにより、各姿勢検出装置１の検出値を同時に取得することができる。
【０１８０】
また、例えば、図９の手順では、Ｘ軸、Ｙ軸、Ｚ軸の順に姿勢検出装置１の検出値を取得するようになっているが、任意の軸の順に姿勢検出装置１の検出値を取得することができる。
【０１８１】
本発明は、実施の形態で説明した構成と実質的に同一の構成（例えば、機能、方法及び結果が同一の構成、あるいは目的及び効果が同一の構成）を含む。また、本発明は、実施の形態で説明した構成の本質的でない部分を置き換えた構成を含む。また、本発明は、実施の形態で説明した構成と同一の作用効果を奏する構成又は同一の目的を達成することができる構成を含む。また、本発明は、実施の形態で説明した構成に公知技術を付加した構成を含む。
【図面の簡単な説明】
【０１８２】
【図１】本実施形態の補正パラメーター作成方法の対象となる姿勢検出装置の構成の一例を示す図。
【図２】本実施形態における姿勢検出装置の斜視図。
【図３】角速度センサーに含まれる振動子の一例を示す平面図。
【図４】角速度センサーに含まれる振動子の動作について説明するための図。
【図５】角速度センサーに含まれる振動子の動作について説明するための図。
【図６】角速度センサーに含まれる駆動回路及び検出回路の構成の一例を示す図。
【図７】センサーの取付角誤差について説明するための図。
【図８】本実施形態の補正パラメーター作成用装置の構成を示す図。
【図９】本実施形態における補正パラメーター作成手順の一例を示すフローチャート図。
【図１０】本実施形態における補正パラメーター作成手順について説明するための図。
【図１１】本実施形態の姿勢検出装置の構成を示す図。
【図１２】本実施形態の姿勢検出装置の他の構成を示す図。
【図１３】従来の補正パラメーター作成方法について説明するための図。
【図１４】従来の補正パラメーター作成方法について説明するための図。
【符号の説明】
【０１８３】
１ 姿勢検出装置、２ 角速度センサーモジュール、３ 加速度センサーモジュール、４ パッケージ、５ａ，５ｂ，５ｃ パッケージの面、１０ａ，１０ｂ，１０ｃ 角速度センサー、１１ａ，１１ｂ，１１ｃ 振動子、１２ａ，１２ｂ，１２ｃ 駆動電極、１３ａ，１３ｂ，１３ｃ 駆動電極、１４ａ，１４ｂ，１４ｃ 検出電極、１５ａ，１５ｂ，１５ｃ 検出電極、２０ａ，２０ｂ，２０ｃ 駆動回路、２１ａ 電流電圧変換器（Ｉ／Ｖ変換器）、２２ａ ＡＣ増幅器、２３ａ 自動利得制御回路（ＡＧＣ） ２４ａ コンパレータ、３０ａ，３０ｂ，３０ｃ 検出回路、３１ａ，３２ａ チャージアンプ、３３ａ 差動増幅器、３４ａ ＡＣ増幅器、３５ａ 同期検波回路、３６ａ ＤＣ増幅器、３７ａ 積分回路（ＬＰＦ）、３８ａ，３８ｂ，３８ｃ 検出信号、４１ａ 駆動振動腕、４２ａ 検出振動腕、４３ａ 幅広部、４４ａ 駆動用基部、４５ａ 連結腕、４６ａ 幅広部、４７ａ 検出用基部、５０ａ，５０ｂ，５０ｃ 加速度センサー、５１ａ，５１ｂ，５１ｃ 振動子、５２ａ，５２ｂ，５２ｃ 駆動電極、５３ａ，５３ｂ，５３ｃ 駆動電極、５４ａ，５４ｂ，５４ｃ 検出電極、５５ａ，５５ｂ，５５ｃ 検出電極、５６ａ，５６ｂ，５６ｃ 基端部、５７ａ，５７ｂ，５７ｃ 基端部、５８ａ，５８ｂ，５８ｃ 駆動振動腕、６０ａ，６０ｂ，６０ｃ 駆動回路、７０ａ，７０ｂ，７０ｃ 検出回路、７８ａ，７８ｂ，７８ｃ 検出信号、８０ 絶縁基板、８２ａ，８２ｂ，８２ｃ パッケージ、８４ａ，８４ｂ，８４ｃ パッケージ本体、８６ａ，８６ｂ，８６ｃ 蓋体、９０ ベース、９１，９２，９３ 素子取付面、１００ ウエイト、１０１，１０２，１０３ 素子接合面、２００ 補正パラメーター作成用装置、２１０ 立方体治具、２１１，２１２，２１３，２１４，２１５，２１６ 立方体治具の面、２２０ ソケット、２２２ ソケット本体、２２４ 蓋体、２３０ 回転板、２３１ 回転板の上面、２４０ 回転モーター、２５０ 支持台、２６０ ケーブル、３００ 姿勢検出装置、３０２，３０２ａ，３０２ｂ，３０２ｃ 検出信号、３０４ａ，３０４ｂ，３０４ｃ 検出信号、３１０ａ，３１０ｂ，３１０ｃ アンチエリアスフィルター、３１０ａ，３１０ｂ，３１０ｃ Ａ／Ｄ変換回路、３２０ Ａ／Ｄ変換回路、３２２ａ，３２２ｂ，３２２ｃ 検出信号、３５０ａ，３５０ｂ，３５０ｃ アンチエリアスフィルター、３６０ａ，３６０ｂ，３６０ｃ Ａ／Ｄ変換回路、３６２ａ，３６２ｂ，３６２ｃ 検出信号、３７０ 補正計算処理部、３８０ 記憶部、３８２ 補正パラメーター、３８４ 補正パラメーター、３９０ マルチプレクサ、５００ 補正パラメーター作成用装置、５１０ テーブル、５２０ ソケット、５３０ 回転腕
【技術分野】
【０００１】
本発明は、３軸の角速度又は加速度を検出するセンサーを含む姿勢検出装置の検出値を所定の直交座標系における検出値に補正するための補正パラメーター作成方法、補正パラメーター作成用装置及び補正機能付き姿勢検出装置に関する。
【背景技術】
【０００２】
近年、角速度センサーや加速度センサーにより物体の姿勢を検出する姿勢検出装置が様々な用途に使用されている。例えば、特許文献１には、ユーザーの頭の姿勢を検出することにより、目前にあるディスプレイに表示された映像が頭の動きに連動して変化し、仮想空間を体験できるヘッドマウントディスプレイが記載されている。ユーザーの頭の姿勢角にあった映像がヘッドマウントディスプレイに映し出される。この姿勢角を検出するために角速度センサーや加速度センサーを備えた姿勢検出装置がヘッドマウントディスプレイの所定の位置に取り付けられる。姿勢検出装置を取り付ける場合、センサーの検出軸が頭の姿勢角を表すための直交座標系の３軸とそれぞれ平行になるように取り付けられていなければ、この取付角の誤差に起因して姿勢検出装置の検出値が誤差を含むことになる。そのため、姿勢検出装置を取り付ける位置や角度が厳密に規定される。
【特許文献１】特開平９−１０６３２２号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
しかし、姿勢検出装置の内部にセンサーが取り付けられる際に取付角にわずかなずれがあると、姿勢検出装置を取り付ける位置や角度が厳密に規定されていたとしても高精度の検出結果を得ることができない。センサーの取付角誤差が無いようにするのはコスト面から現実的ではないため、あらかじめ取付角誤差を算出して姿勢検出装置の検出値を取付角誤差に応じた補正パラメーターで補正することが行われる。式（１）及び式（２）は、それぞれ補正パラメーターを用いた角速度センサー用の補正式及び加速度センサー用の補正式を表している。
【０００４】
【数１】
【０００５】
【数２】
式（１）において、関数行列式（ヤコビアン）ＪｆＧは角速度センサー用の補正パラメーターであり、ｆＧ（ｘ）及びｆＧ（ｐ）はそれぞれ角速度センサーの今回及び前回の補正値（理想値）である。同様に、式（２）において、関数行列式（ヤコビアン）ＪｆＡは加速度センサー用の補正パラメーターであり、ｆＡ（ｘ）及びｆＡ（ｐ）はそれぞれ加速度センサーの今回及び前回の補正値（理想値）である。また、式（１）、式（２）において、ｘ及びｐはそれぞれ角速度センサー又は角速度センサーの今回及び前回の検出値であり、ｏはランダウの記号である。
【０００６】
センサーの取付角誤差は姿勢検出装置毎に異なるため、出荷テスト時等に各姿勢検出装置に対して式（１）、式（２）の補正パラメーター（ＪｆＧ、ＪｆＡ）が作成される。図１３（Ａ）〜図１３（Ｃ）及び図１４（Ａ）〜図１４（Ｃ）は、補正パラメーター（ＪｆＧ、ＪｆＡ）を作成する従来方法を示している。従来方法では、まず、テーブル５１０に取り付けられたソケット５２０に姿勢検出装置をセットし、図１３（Ａ）〜図１３（Ｃ）に示す順に、回転腕５３０をＸ軸回り、Ｙ軸回り、Ｚ軸回りに所定の角速度で回転させて姿勢検出装置の各検出値を取得し、各検出値と各理想値を式（１）に代入して得られる連立方程式を解いて角速度センサー用の補正パラメーターを作成する。さらに、図１４（Ａ）〜図１４（Ｃ）に示す順に、回転腕５３０を操作してＸ軸、Ｙ軸、Ｚ軸の正方向が鉛直上向きになる状態（鉛直下向きに重力加速度が加わる状態）で静止させて姿勢検出装置の各検出値を取得し、各検出値と各理想値を式（２）に代入して得られる連立方程式を解いて加速度センサー用の補正パラメーターを作成する。
【０００７】
図１３（Ａ）〜図１３（Ｃ）及び図１４（Ａ）〜図１４（Ｃ）に示す回転腕５３０の操作において、Ｘ軸、Ｙ軸、Ｚ軸に対してテーブル５１０を所定の角度に正確に固定しなければ、角速度センサーおよび加速度センサーの取付角誤差が正確に反映された検出値を取得することができない。しかし、Ｘ軸、Ｙ軸、Ｚ軸に対してテーブル５１０を所定の角度に正確に固定するためには、テーブル５１０及び回転腕５３０を含む補正パラメーター作成用装置５００が大がかりな装置になる傾向がある。また、Ｘ軸、Ｙ軸、Ｚ軸に対してテーブル５１０を所定の角度に正確に固定するために相当の時間がかかる。角速度センサーと加速度センサーをともに含む姿勢検出装置に対する補正パラメーターを作成するためには、図１３（Ａ）〜図１３（Ｃ）に示す回転腕５３０の操作と図１４（Ａ）〜図１４（Ｃ）に示す回転腕５３０の操作を別々に行う必要があるためさらに時間がかかる。そのため、従来方法では、補正パラメーターの作成にかかるコストが大きいという問題がある。
【０００８】
本発明は、以上のような問題点に鑑みてなされたものであり、本発明のいくつかの態様によれば、センサーの取付角誤差に起因する検出値の誤差を補正するための補正パラメーターをより低コストで作成することができる姿勢検出装置の補正パラメーター作成方法、より低コストで実現可能な補正パラメーター作成用装置及び補正機能付きの姿勢検出装置を提供することができる。
【課題を解決するための手段】
【０００９】
（１）本発明は、検出軸が互いに直交する第１の軸、第２の軸及び第３の軸とそれぞれ略平行になるように取り付けられ角速度又は加速度を検出する第１のセンサー、第２のセンサー及び第３のセンサーを含み、前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの検出信号に基づいて物体の姿勢を検出する姿勢検出装置の検出値を、前記第１の軸、前記第２の軸及び前記第３の軸を座標軸とする直交座標系における検出値に補正する補正式の補正パラメーターを作成する方法であって、上面が水平になるように回転板を設置するステップと、互いに直交する第１の面、第２の面、第３の面を有する直方体形状の治具の前記第１の面に、前記第１の軸が前記第２の面に垂直になり、前記第２の軸が前記第３の面に垂直になり、前記第３の軸が前記第１の面に垂直になるように、前記姿勢検出装置を固定するステップと、前記治具の前記第２の面と対向する面を前記回転板の前記上面に固定し、前記回転板を静止又は所定の角速度で回転させて前記姿勢検出装置の検出値を取得する第１検出値取得ステップと、前記治具の前記第３の面と対向する面を前記回転板の前記上面に固定し、前記回転板を静止又は所定の角速度で回転させて前記姿勢検出装置の検出値を取得する第２検出値取得ステップと、前記治具の前記第１の面と対向する面を前記回転板の前記上面に固定し、前記回転板を静止又は所定の角速度で回転させて前記姿勢検出装置の検出値を取得する第３検出値取得ステップと、取得した検出値に基づいて、前記補正パラメーターを作成する補正パラメーター作成ステップと、を含むことを特徴とする。
【００１０】
Ｘ軸、Ｙ軸、Ｚ軸を座標軸とする直交座標系を考えた場合、第１の軸、第２の軸、第３の軸とＸ軸、Ｙ軸、Ｚ軸の対応関係は特に限定されない。
【００１１】
本発明によれば、直方体形状の治具を用いるので、第１の軸、第２の軸、第３の軸が治具のそれぞれ第１の面、第２の面、第３の面と垂直になるように第１の面に姿勢検出装置を固定することが容易である。そして、上面が水平になるように回転板を設置すれば、治具の第２の面、第３の面、第１の面と対向する面をそれぞれ回転板の上面に固定するだけで、簡単に、第１の軸、第２の軸、第３の軸をそれぞれ鉛直方向と平行にすることができる。さらに、第１の軸、第２の軸、第３の軸をそれぞれ鉛直方向と平行にした状態において、回転板を静止又は回転させることにより、加速度センサー又は角速度センサーの検出値を簡単に短時間で取得することができる。
【００１２】
すなわち、最初に一度だけ、上面が水平になるように回転板を設置すれば回転板の回転方向が固定されるので、第１の軸、第２の軸、第３の軸に関する検出値を取得するためのセッティング時間を大幅に短縮することができる。従って、本発明によれば、センサーの取付角誤差に起因する検出値の誤差を補正するための補正パラメーターをより低コストで作成することができる。
【００１３】
（２）本発明の姿勢検出装置の補正パラメーター作成方法において、前記補正式は、前記補正パラメーターとして前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各検出値を前記直交座標系における各検出値に補正するための第１の補正行列、第２の補正行列及び第３の補正行列を含み、前記第１の補正行列、前記第２の補正行列及び前記第３の補正行列の各々と、前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各検出値がＡ／Ｄ変換されたデジタル値をそれぞれ要素として含む行列の各々と、の積により得られる３つの行列の和として与えられるようにしてもよい。
【００１４】
（３）本発明の姿勢検出装置の補正パラメーター作成方法において、前記第１の補正行列、前記第２の補正行列及び前記第３の補正行列は、前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各検出軸をそれぞれ前記第１の軸、前記第２の軸及び前記第３の軸に変換する回転行列の逆行列であるようにしてもよい。
【００１５】
（４）本発明の姿勢検出装置の補正パラメーター作成方法において、前記補正パラメーター作成ステップは、前記第１検出値取得ステップにおいて取得した前記検出値に基づいて、前記第２のセンサー及び前記第３のセンサーの前記第１の軸回りの各取付角誤差を算出するステップと、前記第２検出値取得ステップにおいて取得した前記検出値に基づいて、前記第１のセンサー及び前記第３のセンサーの前記第２の軸回りの各取付角誤差を算出するステップと、前記第３検出値取得ステップにおいて取得した前記検出値に基づいて、前記第１のセンサー及び前記第２のセンサーの前記第３の軸回りの各取付角誤差を算出するステップと、前記第１のセンサーの前記第２の軸回りの前記取付角誤差及び前記第３の軸回りの前記取付角誤差に基づいて、前記第１の補正行列を作成するステップと、前記第２のセンサーの前記第１の軸回りの前記取付角誤差及び前記第３の軸回りの前記取付角誤差に基づいて、前記第２の補正行列を作成するステップと、前記第３のセンサーの前記第１の軸回りの前記取付角誤差及び前記第２の軸回りの前記取付角誤差に基づいて、前記第３の補正行列を作成するステップと、を含むようにしてもよい。
【００１６】
（５）本発明は、検出軸が互いに直交する第１の軸、第２の軸及び第３の軸とそれぞれ略平行になるように取り付けられ角速度又は加速度を検出する第１のセンサー、第２のセンサー及び第３のセンサーを含み、前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの検出信号に基づいて物体の姿勢を検出する姿勢検出装置の検出値を、前記第１の軸、前記第２の軸及び前記第３の軸を座標軸とする直交座標系における検出値に補正する補正式の補正パラメーターを作成するために使用される補正パラメーター作成用装置であって、互いに直交する第１の面、第２の面、第３の面を有し、前記第１の面に、前記第１の軸が前記第２の面に垂直になり、前記第２の軸が前記第３の面に垂直になり、前記第３の軸が前記第１の面に垂直になるように前記姿勢検出装置を固定可能な直方体形状の治具と、上面に、前記治具の前記第１の面、前記第２の面、前記第２の面とそれぞれ対向する面のいずれかを固定可能な回転板と、前記回転板を所定の角速度で回転させる回転制御部と、を含むことを特徴とする。
【００１７】
本発明によれば、直方体形状の治具と回転板を用いることにより、回転腕を必要としないので、よりコンパクトかつ低コストの補正パラメーター作成用装置を提供することができる。本発明に係る補正パラメーター作成用装置を用いることにより、上記の通り、姿勢検出装置に取り付けられた各センサーの検出値の補正パラメーターを簡単に短時間で取得することができる。
【００１８】
（６）本発明は、検出軸が互いに直交する第１の軸、第２の軸及び第３の軸とそれぞれ略平行になるように取り付けられ角速度又は加速度を検出する第１のセンサー、第２のセンサー及び第３のセンサーと、
前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各検出値を前記第１の軸、前記第２の軸及び前記第３の軸を座標軸とする直交座標系における検出値に補正する補正式の補正パラメーターが記憶された記憶部と、前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各検出信号をデジタル信号に変換する処理を行うＡ／Ｄ変換処理部と、前記デジタル信号の各々と前記補正パラメーターに基づいて前記補正式を計算する処理を行う補正計算処理部と、を含み、前記補正式は、前記補正パラメーターとして前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各検出値を前記直交座標系における各検出値に補正するための第１の補正行列、第２の補正行列及び第３の補正行列を含み、前記第１の補正行列、前記第２の補正行列及び前記第３の補正行列の各々と、前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各検出値がＡ／Ｄ変換されたデジタル値をそれぞれ要素として含む行列の各々と、の積により得られる３つの行列の和として与えられることを特徴とする。
【００１９】
従来の補正式（１）、補正式（２）における関数行列式（ヤコビアン）はセンサーの取付角誤差を直接的に反映する補正パラメーターではなく、また、補正式（１）、補正式（２）では前回の検出値を元に関数行列式（ヤコビアン）を用いて今回の検出値を類推するため、検出値に何らかの写像を施すと補正値が得られるようになっていない。そのため、補正式（１）、補正式（２）では補正精度を上げるのに限界がある。
【００２０】
本発明によれば、補正計算処理部が計算する補正式に含まれる３つの補正行列に各センサーの取付角誤差を直接的に反映させることができる。また、本発明によれば、補正計算処理部が計算する補正式は今回の検出値に対する補正値の計算において前回の検出値を必要としないので、今回の検出値が得られれば直ちに補正値を計算することができる。従って、本発明によれば、補正精度がより高く、かつ、補正計算処理がより速い姿勢検出装置を実現することができる。
【００２１】
（７）本発明の姿勢検出装置において、前記第１の補正行列、前記第２の補正行列及び前記第３の補正行列は、前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各検出軸をそれぞれ前記第１の軸、前記第２の軸及び前記第３の軸に変換する回転行列の逆行列であるようにしてもよい。
【００２２】
（８）本発明の姿勢検出装置は、所定の周期で、前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各前記検出信号のいずれか１つを順次選択する処理を行う信号選択処理部を含み、前記Ａ／Ｄ変換処理部は、前記信号選択処理部が選択した検出値を順次Ａ／Ｄ変換処理するＡ／Ｄ変換回路を含むようにしてもよい。
【発明を実施するための最良の形態】
【００２３】
以下、本発明の好適な実施形態について図面を用いて詳細に説明する。なお、以下に説明する実施の形態は、特許請求の範囲に記載された本発明の内容を不当に限定するものではない。また以下で説明される構成の全てが本発明の必須構成要件であるとは限らない。
【００２４】
なお、以下の説明において、本発明における第１の軸、第２の軸、第３の軸はそれぞれＸ軸、Ｙ軸、Ｚ軸に対応するものとするが、本発明における第１の軸、第２の軸、第３の軸とＸ軸、Ｙ軸、Ｚ軸の対応関係はこれに限らず、任意の対応関係とすることができる。
【００２５】
１．姿勢検出装置
１−１．姿勢検出装置の構成
図１は、本実施形態の補正パラメーター作成方法の対象となる姿勢検出装置の構成の一例を示す図である。
【００２６】
図１に示すように、本実施形態における姿勢検出装置１は、Ｘ軸、Ｙ軸、Ｚ軸回りの角速度を検出する角速度センサーモジュール２と、Ｘ軸、Ｙ軸、Ｚ軸方向の加速度を検出する加速度センサーモジュール３を含んで構成されている。
【００２７】
角速度センサーモジュール２は、それぞれＸ軸、Ｙ軸、Ｚ軸回りの角速度を検出するＸ軸角速度センサー１０ａ、Ｙ軸角速度センサー１０ｂ、Ｚ軸角速度センサー１０ｃを含む。
【００２８】
Ｘ軸角速度センサー１０ａは、振動子１１ａ、振動子１１ａを振動させる駆動回路２０ａ、角速度検出信号３８ａを生成する検出回路３０ａを含み、振動子１１ａの駆動電極１２ａ、１３ａと駆動回路２０ａ、振動子１１ａの検出電極１４ａ、１５ａと検出回路３０ａがそれぞれ接続されている。
【００２９】
同様に、Ｙ軸角速度センサー１０ｂは、振動子１１ｂ、振動子１１ｂを振動させる駆動回路２０ｂ、角速度検出信号３８ｂを生成する検出回路３０ｂを含み、振動子１１ｂの駆動電極１２ｂ、１３ｂと駆動回路２０ｂ、振動子１１ｂの検出電極１４ｂ、１５ｂと検出回路３０ｂがそれぞれ接続されている。
【００３０】
同様に、Ｚ軸角速度センサー１０ｃは、振動子１１ｃ、振動子１１ｃを振動させる駆動する駆動回路２０ｃ、角速度検出信号３８ｃを生成する検出回路３０ｃを含み、振動子１１ｃの駆動電極１２ｃ、１３ｃと駆動回路２０ｃ、振動子１１ｃの検出電極１４ｃ、１５ｃと検出回路３０ｃがそれぞれ接続されている。
【００３１】
加速度センサーモジュール３は、それぞれＸ軸、Ｙ軸、Ｚ軸方向の加速度を検出するＸ軸加速度センサー５０ａ、Ｙ軸加速度センサー５０ｂ、Ｚ軸加速度センサー５０ｃを含む。
【００３２】
Ｘ軸加速度センサー５０ａは、振動子５１ａ、振動子５１ａを振動させる駆動回路６０ａ、加速度検出信号７８ａを生成する検出回路７０ａを含み、振動子５１ａの駆動電極５２ａ、５３ａと駆動回路６０ａ、振動子５１ａの検出電極５４ａ、５５ａと検出回路７０ａがそれぞれ接続されている。
【００３３】
同様に、Ｙ軸加速度センサー５０ｂは、振動子５１ｂ、振動子５１ｂを振動させる駆動回路６０ｂ、加速度検出信号７８ｂを生成する検出回路７０ｂを含み、振動子５１ｂの駆動電極５２ｂ、５３ｂと駆動回路６０ｂ、振動子５１ｂの検出電極５４ｂ、５５ｂと検出回路７０ｂがそれぞれ接続されている。
【００３４】
同様に、Ｚ軸加速度センサー５０ｃは、振動子５１ｃ、振動子５１ｃを振動させる駆動回路６０ｃ、加速度検出信号７８ｃを生成する検出回路７０ｃを含み、振動子５１ｃの駆動電極５２ｃ、５３ｃと駆動回路６０ｃ、振動子５１ｃの検出電極５４ｃ、５５ｃと検出回路７０ｃがそれぞれ接続されている。
【００３５】
なお、角速度センサー１０ａ、１０ｂ、１０ｃは、それぞれ本発明における第１のセンサー、第２のセンサー及び第３のセンサーとして機能する。同様に、加速度センサー５０ａ、５０ｂ、５０ｃは、それぞれ本発明における第１のセンサー、第２のセンサー及び第３のセンサーとして機能する。
【００３６】
図２は、本実施形態における姿勢検出装置の斜視図である。
【００３７】
図２に示すように、姿勢検出装置１において、角速度センサーモジュール２及び加速度センサーモジュール３は、それぞれ立方体（広義には直方体。以下、同じ。）の形状に形成されており、直方体形状のパッケージ４の中に収納されている。
【００３８】
Ｘ軸、Ｙ軸、Ｚ軸は姿勢検出装置１を基準にして決定される。例えば、姿勢検出装置１を構成するパッケージ４が直方体形状である場合は、パッケージ４の直交する３つの面５ａ、５ｂ、５ｃと垂直な軸をそれぞれＸ軸、Ｙ軸、Ｚ軸とすることができる。また、Ｘ軸、Ｙ軸、Ｚ軸の正方向は任意に決定することができ、本実施形態では、図２に示す矢印の先に向かう方向を各軸の正方向とする。
【００３９】
１−２．角速度センサーモジュール
図２に示すように、角速度センサーモジュール２において、角速度センサー１０ａ、１０ｂ、１０ｃは、検出軸がそれぞれＸ軸、Ｙ軸、Ｚ軸とほぼ平行になるように絶縁基板８０の上に取り付けられ、それぞれ振動子１１ａ、１１ｂ、１１ｃがパッケージ８２ａ、８２ｂ、８２ｃ内に収容されている。パッケージ８２ａ、８２ｂ、８２ｃの周囲は樹脂モールド材で覆われている。
【００４０】
パッケージ８２ａ、８２ｂ、８２ｃは、それぞれパッケージ本体８４ａと蓋体８６ａ、パッケージ本体８４ｂと蓋体８６ｂ、パッケージ本体８４ｃと蓋体８６ｃから構成されている。パッケージ本体８４ａ、８４ｂ、８４ｃは、複数のセラミックシートを積層、焼結して直方体の箱状に形成されている。蓋体８６ａ、８６ｂ、８６ｃは、ガラス板や金属板、セラミックシート等で形成されており、金属ロウ材、低融点ガラスなどの接合材を介して、振動子１１ａ、１１ｂ、１１ｃがそれぞれ収容されたパッケージ本体８４ａ、８４ｂ、８４ｃの上面開口部を真空封止している。振動子１１ａ、１１ｂ、１１ｃは、絶縁基板８０に形成された配線パターン（図示せず）によってそれぞれ駆動回路２０ａ、２０ｂ、２０ｃや検出回路３０ａ、３０ｂ、３０ｃと接続されている。
【００４１】
駆動回路２０ａと検出回路３０ａ、駆動回路２０ｂと検出回路３０ｂ、駆動回路２０ｃと検出回路３０ｃは、３つのチップにＩＣ化してそれぞれパッケージ８２ａ、８２ｂ、８２ｃ内にそれぞれ収容されていてもよい。また、駆動回路２０ａ、２０ｂ、２０ｃ、検出回路３０ａ、３０ｂ、３０ｃを１チップにＩＣ化して絶縁基板８０上に配置するようにしてもよい。
【００４２】
なお、図２では図示を省略しているが、検出回路３０ａ、３０ｂ、３０ｃからの各検出信号３８ａ、３８ｂ、３８ｃは外部出力端子（図示せず）を介して姿勢検出装置１の外部に出力されるようになっている。
【００４３】
図３は、角速度センサーに含まれる振動子の一例を示す平面図である。角速度センサー１０ａ、１０ｂ、１０ｃにそれぞれ含まれる振動子１１ａ、１１ｂ、１１ｃはすべて同一の構造であるため、図３では振動子１１ａの構造のみが図示されている。なお、図３におけるＸ軸、Ｙ軸、Ｚ軸は水晶の軸を示し、図２におけるＸ軸、Ｙ軸、Ｚ軸とは無関係である。
【００４４】
振動子１１ａは、水晶などの圧電材料の薄板から形成され、駆動用基部４４ａから駆動振動腕４１ａ（広義には、駆動用振動片）が水晶のＹ軸方向に延出している。駆動振動腕４１ａの側面及び上面にはそれぞれ駆動電極１２ａ及び１３ａが形成されている。図１に示したように、駆動電極１２ａ、１３ａは駆動回路２０ａに接続される。
【００４５】
駆動用基部４４ａは、水晶のＸ軸方向に延びる連結腕４５ａを介して検出用基部４７ａに接続されている。検出振動腕４２ａ（広義には、検出振動片）は、検出用基部４７ａから水晶のＹ軸方向に延出されている。検出振動腕４２ａの上面には検出電極１４ａ及び１５ａが形成されており、検出振動腕４２ａの側面には電極１６ａが形成されている。図１に示したように、検出電極１４ａ、１５ａは駆動回路２０ａに接続される。また、電極１６ａは接地される。
【００４６】
駆動振動腕４１ａの駆動電極１２ａと駆動電極１３ａとの間に交番電圧／交番電流からなる駆動信号が与えられると、図４に示すように、駆動振動腕４１ａは圧電効果によって矢印Ｂのように屈曲振動する。
【００４７】
ここで、図５に示すように振動子１１ａが水晶のＺ軸を回転軸とした回転運動をすると、駆動振動腕４１ａは、矢印Ｂの屈曲振動の方向と水晶のＺ軸の両方に垂直な方向にコリオリの力を得る。その結果、連結腕４５ａは矢印Ｃで示すような振動をする。そして、検出振動腕４２ａは、連結腕４５ａの振動（矢印Ｃ）に連動して、連結腕４５ａと矢印Ｄのような屈曲振動をする。
【００４８】
そして、これらの屈曲振動に基づいて発生する逆圧電効果によって、検出振動腕４２ａの検出電極１４ａ、１５ａと電極１６ａとの間には、それぞれ逆方向の交番電圧／交番電流が発生する。以上のようにして、振動子１１ａは、水晶のＺ軸を検出軸としてコリオリの力に基づく角速度成分を検出し、検出電極１４ａ、１５ａを介して検出信号を出力する。
【００４９】
なお、図３の構成では、振動子１１ａのバランスを良くするために、検出用基部４７ａを中央に配置し、検出用基部４７ａから＋Ｙ軸と−Ｙ軸の両方向に検出振動腕４２ａを延出させている。さらに、検出用基部４７ａから＋Ｘ軸と−Ｘ軸の両方向に連結腕４５ａを延出させ、連結腕４５ａのそれぞれから、＋Ｙ軸と−Ｙ軸の両方向に駆動振動腕４１ａを延出させている。
【００５０】
また、駆動振動腕４１ａの先端を幅広の幅広部４３ａにし、さらに、錘を付けることでコリオリの力を大きくしている。また、錘効果によって、所望の共振周波数を、短い振動腕で得ることができる。同様の理由で、検出振動腕４２ａの先端を幅広の幅広部４６ａにし、さらに、錘を付けている。
【００５１】
なお、振動子１１ａは、上述の構成に限らず、コリオリの力に基づく角速度成分を含む検出信号を出力する振動子であれば良い。例えば、駆動振動腕と検出振動腕とを兼ねる構成であっても良く、また、駆動振動腕や検出振動腕に圧電膜を形成した構成であっても良い。
【００５２】
図６は、角速度センサーに含まれる駆動回路及び検出回路の構成の一例を示す図である。駆動回路２０ａ、２０ｂ、２０ｃはすべて同じ構成であり、検出回路３０ａ、３０ｂ、３０ｃはすべて同じ構成であるので、図３では駆動回路２０ａ及び検出回路３０ａの構成のみを図示している。
【００５３】
図６に示すように、駆動回路２０ａは、電流電圧変換器（Ｉ／Ｖ変換器）２１ａ、ＡＣ増幅器２２ａ、自動利得制御回路（ＡＧＣ）２３ａ、コンパレータ２４ａを含んで構成されている。
【００５４】
振動子１１ａが振動すると、圧電効果に基づく交流電流がフィードバック信号として駆動電極１３ａから出力され、電流電圧変換器（Ｉ／Ｖ変換器）２１ａに入力される。電流電圧変換器（Ｉ／Ｖ変換器）２１ａは、入力された交流電流を振動子１１ａの振動周波数と同一の周波数の交流電圧信号に変換して出力する。
【００５５】
電流電圧変換器（Ｉ／Ｖ変換器）２１ａから出力された交流電圧信号は、ＡＣ増幅器２２ａに入力される。ＡＣ増幅器ａは、入力された交流電圧信号を増幅して出力する。
【００５６】
ＡＣ増幅器２２ａから出力された交流電圧信号は自動利得制御回路（ＡＧＣ）２３ａに入力される。自動利得制御回路（ＡＧＣ）２３ａは、入力された交流電圧信号の振幅を一定値に保持するように利得を制御し、利得制御後の交流電圧信号を振動子１１ａの駆動電極１２ａに出力する。この駆動電極１２ａに入力される交流電圧信号により振動子１１ａが振動する。
【００５７】
ＡＣ増幅器２２ａが増幅した交流電圧信号はコンパレータ２４ａに入力され、交流電圧信号の振幅中心を基準電圧として、交流電圧信号と基準電圧信号との比較結果に応じて出力レベルを切り替える方形波電圧信号を、検出回路３０ａの同期検波回路３５ａに出力する。
【００５８】
図６に示すように、検出回路３０ａは、チャージアンプ３１ａ、３２ａ、差動増幅器３３ａ、ＡＣ増幅器３４ａ、同期検波回路３５ａ、ＤＣ増幅器３６ａ及び積分回路（ＬＰＦ）３７ａを含んで構成されている。
【００５９】
チャージアンプ３１ａ、３２ａには、振動子１１ａにより検出された互いに逆位相の検出信号（交流電流）が検出電極１２ａ、１３ａを介して入力される。そして、チャージアンプ３１ａ、３２ａは、入力された検出信号（交流電流）を基準電圧を中心とする交流電圧信号に変換する。
【００６０】
差動増幅器３３ａはチャージアンプ３１ａの出力信号とチャージアンプ３２ａの出力信号を差動増幅する。差動増幅器３３ａの出力信号は、さらにＡＣ増幅器３４ａで増幅される。
【００６１】
同期検波回路３５ａは、コンパレータ２４ａが出力する方形波電圧信号を基に、ＡＣ増幅器３４ａの出力信号を同期検波することにより角速度成分を抽出する。同期検波回路３５ａは、例えば、方形波電圧信号の電圧レベルが基準電圧よりも高い時はＡＣ増幅器３４ａの出力信号をそのまま出力し、方形波電圧信号の電圧レベルが基準電圧よりも低い時はＡＣ増幅器３４ａの出力信号を基準電圧に対して反転して出力するスイッチ回路として構成することができる。
【００６２】
同期検波回路３５ａで抽出された角速度成分信号は、ＤＣ増幅器３６ａで増幅されて積分回路（ＬＰＦ）３７ａに入力される。
【００６３】
積分回路（ＬＰＦ）３７ａは、ＤＣ増幅器３５ａの出力信号から高周波成分を減衰させて直流成分を抽出することにより角速度検出信号３８ａを生成して外部に出力する。
【００６４】
１−３．加速度センサーモジュール
図２に示すように、加速度センサーモジュール３は、ベース９０、ウエイト１００、３つの加速度センサー５０ａ、５０ｂ、５０ｃを有する。なお、図２では、図１に示した駆動回路６０ａ、６０ｂ、６０ｃ及び検出回路７０ａ、７０ｂ、７０ｃの図示を省略しているがパッケージ４の中の適当な位置に配置され、検出回路７０ａ、７０ｂ、７０ｃからの各検出信号７８ａ、７８ｂ、７８ｃは外部出力端子（図示せず）を介して姿勢検出装置１の外部に出力されるようになっている。
【００６５】
ベース９０は、立方形を作るように３つの正方形の壁部を互いに直交させて形成され、Ｘ軸、Ｙ軸、Ｚ軸方向に互いに直交する３つの取付面９１、９２、９３を有する。ウエイト１００は、所定の質量を有する立方体からなり、互いに直交する３つの接合面１０１、１０２、１０３を有する。ベース９０及びウエイト１００は、例えばアルミニウム合金などの適当な材料を用いて形成される。
【００６６】
本実施形態では、加速度センサー５０ａ、５０ｂ、５０ｃは、それぞれ、水晶などの圧電材料の薄板から形成された双音叉型の振動子５１ａ、５１ｂ、５１ｃを含んで構成されている。
【００６７】
振動子５１ａ、５１ｂ、５１ｃは、検出軸がそれぞれＸ軸、Ｙ軸、Ｚ軸とほぼ平行になるように、一方の基端部５６ａ、５６ｂ、５６ｃがそれぞれベース９０の素子取付面９１、９２、９３に取り付けられ、ベース９０の各壁部に垂直に支持されている。振動子５１ａ、５１ｂ、５１ｃの他方の基端部５７ａ、５７ｂ、５７ｃは、それぞれ素子取付面９１、９２、９３に対応するウエイト１００の素子接合面１０１〜１０３に接合されている。これにより、ウエイト１００が、Ｘ軸、Ｙ軸、Ｚ軸方向から振動子５１ａ、５１ｂ、５１ｃによって浮遊した状態に支持される。
【００６８】
振動子５１ａの２つの駆動振動腕５８ａには、その上下主面及び両側面に駆動電極５２ａ、５３ａが設けられており（図示省略）、駆動回路６０ａにより駆動電極５２ａ、５３ａ間に所定の交流電圧が印加されると、２つの駆動振動腕５８ａは互いに逆向きに即ち近接または離反する向きに所定の周波数で屈曲振動する。
【００６９】
振動子５１ａを所定の周波数で振動させた状態で、加速度センサーモジュール３に外力が作用してウエイト１００にＸ軸方向の加速度が加わると、その大きさ及び向きに対応して、振動子５１ａには長手方向（すなわち、Ｘ軸方向）に圧縮または引張する力が作用する。振動子５１ａの周波数は、圧縮する力が作用すると減少し、引張する力が作用すると増加するように変化する。従って、検出回路７０ａにより振動子５１ａにおける周波数の変化量を検出し、周波数の変化量からＸ軸方向に作用する荷重を算出することにより、ウエイト１００に作用したＸ軸方向の加速度の大きさ及び向きを計算することができる。
【００７０】
振動子５１ｂ、５１ｃの構造も振動子５１ａの構造と同一であり、同様にしてＹ軸及びＺ軸方向の加速度の大きさ及び向きを計算することができる。
【００７１】
なお、駆動回路６０ａ、６０ｂ、６０ｃは図６に示した駆動回路２０ａと同様の構成であり、また、検出回路７０ａ、７０ｂ、７０ｃは周波数の変化量を検出する既知の回路と同様の構成にすることができるので、その説明を省略する。
【００７２】
２．補正パラメーター作成方法
２−１．センサー取付角誤差
角速度センサー１０ａ、１０ｂ、１０ｃは、理想的には、検出軸がそれぞれＸ軸、Ｙ軸、Ｚ軸と厳密に平行になるように取り付けられる。同様に、加速度センサー５０ａ、５０ｂ、５０ｃは、理想的には、検出軸がそれぞれＸ軸、Ｙ軸、Ｚ軸と厳密に平行になるように取り付けられる。しかし、角速度センサー１０ａ、１０ｂ、１０ｃ及び加速度センサー５０ａ、５０ｂ、５０ｃをそのように厳密に取り付けるのはコスト面から難しい。そのため、Ｘ軸角速度センサー１０ａは、実際には、図７（Ａ）に示すように、検出軸がＹ軸回りに微小角Δθ２ｘ、Ｚ軸回りに微小角Δθ３ｘ回ったＸ’軸と平行になるように取り付けられている。同様に、Ｙ軸角速度センサー１０ｂは、実際には、図７（Ｂ）に示すように、検出軸がＺ軸回りに微小角Δθ３ｙ、Ｘ軸回りに微小角Δθ１ｙ回ったＹ’軸と平行になるように取り付けられており、Ｚ軸角速度センサー１０ｃは、実際には、図７（Ｃ）に示すように、検出軸がＸ軸回りに微小角Δθ１ｚ、Ｙ軸回りに微小角Δθ２ｚ回ったＺ’軸と平行になるように取り付けられている。すなわち、Ｘ軸角速度センサー１０ａのＹ軸回りの取付角誤差及びＺ軸回りの取付角誤差はそれぞれΔθ２ｘ、Δθ３ｘであり、Ｙ軸角速度センサー１０ｂのＺ軸回りの取付角誤差及びＸ軸回りの取付角誤差はそれぞれΔθ３ｙ、Δθ１ｙであり、Ｚ軸角速度センサー１０ｃのＸ軸回りの取付角誤差及びＹ軸回りの取付角誤差はそれぞれΔθ３ｚ、Δθ１ｚである。
【００７３】
加速度センサー５０ａ、５０ｂ、５０ｃについても同様に取付角誤差が存在する。そのため、角速度センサー１０ａ、１０ｂ、１０ｃ、加速度センサー５０ａ、５０ｂ、５０ｃの各検出値は理想値とずれている。
【００７４】
２−２．数学的考察
従来の補正式（１）、補正式（２）における関数行列式（ヤコビアン）はセンサーの取付角誤差を直接的に反映する補正パラメーターではなく、また、補正式（１）、補正式（２）では前回の検出値を元に関数行列式（ヤコビアン）を用いて今回の検出値を類推するため、検出値に何らかの写像を施すと補正値が得られるようになっていない。そのため、補正式（１）、補正式（２）では補正精度を上げるのに限界がある。そこで、以下では、より精度の高い補正について数学的に考察する。
【００７５】
３次元ユークリッド空間において、Ｘ軸、Ｙ軸、Ｚ軸の回りにそれぞれ角度θの回転を施す回転行列Ｔ１、Ｔ２、Ｔ３は式（３）によって与えられる。
【００７６】
【数３】
そして、３次元ユークリッド空間における任意の回転は回転行列Ｔ１、Ｔ２、Ｔ３の積の組み合わせにより表すことができる。例えば、Ｚ軸の回りに角度θ３だけ回転させ、Ｙ軸の回りに角度θ２だけ回転させ、Ｘ軸の回りに角度θ１だけ回転させることにより、ＸＹＺ座標系をＸ’Ｙ’Ｚ’座標系に変換する行列Ｔδは式（４）によって与えられる。以下では、Ｔδを「変換行列」ということにする。
【００７７】
【数４】
３つの角速度センサー１０ａ、１０ｂ、１０ｃを、検出軸がそれぞれＸ軸、Ｙ軸、Ｚ軸と平行になるように取り付けたとき、取付角誤差のために実際には検出軸がそれぞれＸ’軸、Ｙ’軸、Ｚ’軸と平行になるように取り付けられていると仮定する。この場合、角速度センサー１０ａ、１０ｂ、１０ｃの各検出値Ｇｘ’、Ｇｙ’、Ｇｚ’と理想値Ｇｘ、Ｇｙ、Ｇｚの間には変換行列Ｔδによる関係式（５）が成立する。
【００７８】
【数５】
従って、次の式（６）により、角速度センサー１０ａ、１０ｂ、１０ｃの各検出値Ｇｘ’、Ｇｙ’、Ｇｚ’から理想値Ｇｘ、Ｇｙ、Ｇｚを計算することができる。
【００７９】
【数６】
すなわち、何らかの方法でＴδ−１を得ることができれば、式（６）を用いて角速度センサー１０ａ、１０ｂ、１０ｃの各検出値を理想値に補正することができる。以下では、Ｔδ−１を「補正行列」ということにする。
【００８０】
角速度センサー１０ａ、１０ｂ、１０ｃの取付角を光学観測できる場合は、直接θ１、θ２、θ３を導出して式（３）を用いて回転行列Ｔ１、Ｔ２、Ｔ３を計算し、その逆行列Ｔ１−１、Ｔ２−１、Ｔ３−１を用いて補正行列Ｔδ−１を得ることができる。
【００８１】
一方、光学観測ができない場合は、例えば、Ｘ軸、Ｙ軸、Ｚ軸を中心に角速度センサー１０ａ、１０ｂ、１０ｃを回転させる等、Ｘ軸、Ｙ軸、Ｚ軸に関する取付角誤差が検出値に反映されるような３通りの入力条件を選択し、その入力条件に対する角速度センサー１０ａ、１０ｂ、１０ｃの検出値Ｇｘ’、Ｇｙ’、Ｇｚ’及び理想値Ｇｘ、Ｇｙ、Ｇｚをそれぞれ式（６）に代入することにより得られる３つの連立方程式を解けばθ１、θ２、θ３を導出することができる。しかし、この連立方程式は非常に複雑であるためθ１、θ２、θ３を簡単に導出することができない。
【００８２】
一方、仮にθが非常に小さい値であれば、次の式（７）が成り立つ。
【００８３】
【数７】
従って、取付角誤差θ１、θ２、θ３が非常に小さい値であれば、変換行列Ｔδは次の式（８）のように表される。
【００８４】
【数８】
従って、次の式（９）に示すように、変換行列Ｔδは３つの基底となる行列Ｊ１、Ｊ２、Ｊ３の線形和で表すことができる。
【００８５】
【数９】
前記のように、Ｘ軸角速度センサー１０ａは、Ｙ軸回りに微小角Δθ２ｘ、Ｚ軸回りに微小角Δθ３ｘ回ったＸ’軸が検出軸になるように取り付けられている。Ｘ軸回りの取付角誤差Δθ１ｘ＝０であるので、式（９）より、変換行列Ｔδｘは次の式（１０）のように表される。
【００８６】
【数１０】
同様に、Ｙ軸角速度センサー１０ｂは、Ｘ軸回りに微小角Δθ１ｙ、Ｚ軸回りに微小角Δθ３ｙ回ったＹ’軸が検出軸になるように取り付けられている。Ｙ軸回りの取付角誤差Δθ２ｙ＝０であるので、式（９）より、変換行列Ｔδｙは次の式（１１）のように表される。
【００８７】
【数１１】
同様に、Ｚ軸角速度センサー１０ｃは、Ｘ軸回りに微小角Δθ１ｚ、Ｙ軸回りに微小角Δθ２ｚ回ったＺ’軸が検出軸になるように取り付けられている。Ｚ軸回りの取付角誤差Δθ３ｚ＝０であるので、式（９）より、変換行列Ｔδｚは次の式（１２）のように表される。
【００８８】
【数１２】
式（１０）、（１１）、（１２）によれば、何らかの方法でΔθ２ｘ、Δθ３ｘ、Δθ１ｙ、Δθ３ｙ、Δθ１ｚ、Δθ２ｚを得ることができれば、変換行列Ｔδｘ、Ｔδｙ、Ｔδｚを計算することができる。そして、変換行列Ｔδｘ、Ｔδｙ、Ｔδｚの逆行列を計算すれば、補正行列Ｔδｘ−１、Ｔδｙ−１、Ｔδｚ−１を作成することができる。そうすると、次の式（１３）より、Ｘ軸角速度センサー１０ａ、Ｙ軸角速度センサー１０ｂ、Ｚ軸角速度センサー１０ｃの各検出値Ｇｘ’、Ｇｙ’、Ｇｚ’をそれぞれ理想値Ｇｘ、Ｇｙ、Ｇｚに補正することができる。
【００８９】
【数１３】
補正式（１３）における補正行列Ｔδｘ−１、Ｔδｙ−１、Ｔδｚ−１は、角速度センサー１０ａ、１０ｂ、１０ｃの各取付角誤差Δθ２ｘ、Δθ３ｘ、Δθ１ｙ、Δθ３ｙ、Δθ１ｚ、Δθ２ｚを直接的に反映している。また、補正式（１３）によれば、前回の検出値を必要とせず、今回の検出値Ｇｘ’、Ｇｙ’、Ｇｚ’が得られれば直ちに補正値（理想値）Ｇｘ、Ｇｙ、Ｇｚを計算することができる。従って、補正式（１３）によれば、補正精度の向上及び補正計算処理の高速化を実現することができる。
【００９０】
なお、補正行列Ｔδｘ−１、Ｔδｙ−１、Ｔδｚ−１は、それぞれ本発明における第１の補正行列、第２の補正行列及び第３の補正行列に相当する。
【００９１】
次に、Δθ２ｘ、Δθ３ｘ、Δθ１ｙ、Δθ３ｙ、Δθ１ｚ、Δθ２ｚを得るための方法について説明する。
【００９２】
Ｘ軸角速度センサー１０ａをＸ軸回りに角度Δθｘｘ、Ｙ軸回りに角度Δθｘｙ、Ｚ軸回りに角度Δθｘｚだけ移動させたときに、Ｘ’軸回りに角度Δθｘｘ’、Ｙ’軸回りに角度Δθｘｙ’、Ｚ’軸回りに角度Δθｘｚ’だけ移動することを示す検出値が得られる関係にあるとすると、式（１０）より次の式（１４）が成立する。
【００９３】
【数１４】
同様に、Ｙ軸角速度センサー１０ｂをＸ軸回りに角度Δθｙｘ、Ｙ軸回りに角度Δθｙｙ、Ｚ軸回りに角度Δθｙｚだけ移動させたときに、Ｘ’軸回りに角度Δθｙｘ’、Ｙ’軸回りに角度Δθｙｙ’、Ｚ’軸回りに角度Δθｙｚ’だけ移動することを示す検出値が得られる関係にあるとすると、式（１１）より次の式（１５）が成立する。
【００９４】
【数１５】
同様に、Ｚ軸角速度センサー１０ｃをＸ軸回りに角度Δθｚｘ、Ｙ軸回りに角度Δθｚｙ、Ｚ軸回りに角度Δθｚｚだけ移動させたときに、Ｘ’軸回りに角度Δθｚｘ’、Ｙ’軸回りに角度Δθｚｙ’、Ｚ’軸回りに角度Δθｚｚ’だけ移動することを示す検出値が得られる関係にあるとすると、式（１２）より次の式（１６）が成立する。
【００９５】
【数１６】
まず、姿勢検出装置１をＹ軸及びＺ軸回りには回転させずにＸ軸回りにのみ角度Δθｘだけ回転させた場合、式（１５）においてΔθｙｘ＝Δθｘ、Δθｙｙ＝０、Δθｙｚ＝０なので、Δθｙｘ’＝Δθｘ、Δθｙｚ’＝０とともに以下の関係式（１７）を得ることができる。
【００９６】
【数１７】
Ｙ軸角速度センサー１０ｂの検出値（Ｙ’軸回りの角速度）に所定時間を掛けることによりΔθｙｙ’を得ることができるので、式（１７）にΔθｙｙ’とΔθｘを代入することにより、Δθ３ｙを得ることができる。
【００９７】
同様に、式（１６）においてΔθｚｘ＝Δθｘ、Δθｚｙ＝０、Δθｚｚ＝０なので、Δθｚｘ’＝Δθｘ、Δθｚｙ’＝０とともに以下の関係式（１８）を得ることができる。
【００９８】
【数１８】
Ｚ軸角速度センサー１０ｃの検出値（Ｚ’軸回りの角速度）に所定時間を掛けることによりΔθｚｚ’を得ることができるので、式（１８）にΔθｚｚ’とΔθｘを代入することにより、Δθ２ｚを得ることができる。
【００９９】
次に、姿勢検出装置１をＸ軸及びＺ軸回りには回転させずにＹ軸回りにのみ角度Δθｙだけ回転させた場合、式（１４）においてΔθｘｘ＝０、Δθｘｙ＝Δθｙ、Δθｘｚ＝０なので、Δθｘｙ’＝Δθｙ、Δθｘｚ’＝０とともに以下の関係式（１９）を得ることができる。
【０１００】
【数１９】
Ｘ軸角速度センサー１０ａの検出値（Ｘ’軸回りの角速度）に所定時間を掛けることによりΔθｘｘ’を得ることができるので、式（１９）にΔθｘｘ’とΔθｙを代入することにより、Δθ３ｘを得ることができる。
【０１０１】
同様に、式（１６）においてΔθｚｘ＝０、Δθｚｙ＝Δθｙ、Δθｚｚ＝０なので、Δθｚｘ’＝０、Δθｚｙ’＝Δθｙとともに以下の関係式（２０）を得ることができる。
【０１０２】
【数２０】
Ｚ軸角速度センサー１０ｃの検出値（Ｚ’軸回りの角速度）に所定時間を掛けることによりΔθｚｚ’を得ることができるので、式（２０）にΔθｚｚ’とΔθｙを代入することにより、Δθ１ｚを得ることができる。
【０１０３】
最後に、姿勢検出装置１をＸ軸及びＹ軸回りには回転させずにＺ軸回りにのみ角度Δθｚだけ回転させた場合、式（１４）においてΔθｘｘ＝０、Δθｘｙ＝０、Δθｘｚ＝Δθｚなので、Δθｘｙ’＝０、Δθｘｚ’＝Δθｚとともに以下の関係式（２１）を得ることができる。
【０１０４】
【数２１】
Ｘ軸角速度センサー１０ａの検出値（Ｘ’軸回りの角速度）に所定時間を掛けることによりΔθｘｘ’を得ることができるので、式（２１）にΔθｘｘ’とΔθｚを代入することにより、Δθ２ｘを得ることができる。
【０１０５】
同様に、式（１５）においてΔθｙｘ＝０、Δθｙｙ＝０、Δθｙｚ＝Δθｚなので、Δθｙｘ’＝０、Δθｙｚ’＝Δθｚとともに以下の関係式（２２）を得ることができる。
【０１０６】
【数２２】
Ｙ軸角速度センサー１０ｂの検出値（Ｙ’軸回りの角速度）に所定時間を掛けることによりΔθｙｙ’を得ることができるので、式（２２）にΔθｙｙ’とΔθｚを代入することにより、Δθ１ｙを得ることができる。
【０１０７】
以上のようにして得られたΔθ２ｘ、Δθ３ｘ、Δθ１ｙ、Δθ３ｙ、Δθ１ｚ、Δθ２ｚから計算される逆行列Ｔδｘ−１、Ｔδｙ−１、Ｔδｚ−１を式（１３）に代入すれば、角速度センサー１０ａ、１０ｂ、１０ｃの各検出値Ｇｘ’、Ｇｙ’、Ｇｚ’をそれぞれ理想値Ｇｘ、Ｇｙ、Ｇｚに補正することができる。
【０１０８】
なお、実際には、式（１３）の補正計算はＣＰＵ又は専用回路によりデジタル値で行われる。そのため、角速度センサー１０ａ、１０ｂ、１０ｃの検出値Ｇｘ’、Ｇｙ’、Ｇｚ’のＡ／Ｄ変換値にＡ／Ｄ変換のサンプル周期Δｔを掛けて得られるＸ’軸、Ｙ’軸、Ｚ’軸回りの微小回転角Δθｘ’、Δθｙ’、Δθｚ’が、次の式（２３）により、Ｘ軸、Ｙ軸、Ｚ軸回りの微小回転角Δθｘ、Δθｙ、Δθｚに補正される。
【０１０９】
【数２３】
Ｘ軸加速度センサー５０ａ、Ｙ軸加速度センサー５０ｂ、Ｚ軸加速度センサー５０ｃの各検出値の補正においても、同様の理論が成り立つ。
【０１１０】
Ｘ軸加速度センサー５０ａは、Ｙ軸回りに微小角Δφ２ｘ、Ｚ軸回りに微小角Δφ３ｘ回ったＸ’軸が検出軸になるように取り付けられており（Δφ２ｘ、Δφ３ｘが取付角誤差になる）、変換行列Ｔγｘは次の式（２４）のように表される。
【０１１１】
【数２４】
同様に、Ｙ軸加速度センサー５０ｂは、Ｘ軸回りに微小角Δφ１ｙ、Ｚ軸回りに微小角Δφ３ｙ回ったＹ’軸が検出軸になるように取り付けられており（Δφ１ｙ、Δφ３ｙが取付角誤差になる）、変換行列Ｔγｙは次の式（２５）のように表される。
【０１１２】
【数２５】
同様に、Ｚ軸加速度センサー５０ｃは、Ｘ軸回りに微小角Δφ１ｚ、Ｙ軸回りに微小角Δφ２ｚ回ったＺ’軸が検出軸になるように取り付けられており（Δφ１ｚ、Δφ２ｚが取付角誤差になる）、変換行列Ｔγｚは次の式（２６）のように表される。
【０１１３】
【数２６】
式（２４）〜式（２６）は角速度センサー１０ａ、１０ｂ、１０ｃにおける式（１０）〜式（１２）と対応している。
【０１１４】
Ｘ軸加速度センサー５０ａをＸ軸方向に速度Δｖｘｘ、Ｙ軸方向に速度Δｖｘｙ、Ｚ軸方向に速度Δｖｘｚで移動させたときに、Ｘ’軸方向に速度Δｖｘｘ’、Ｙ’軸方向に速度Δｖｘｙ’、Ｚ’軸方向に速度Δｖｘｚ’で移動することを示す検出値が得られる関係にあるとすると、次の式（２７）が成立する。
【０１１５】
【数２７】
Ｙ軸加速度センサー５０ｂをＸ軸方向に速度Δｖｙｘ、Ｙ軸方向に速度Δｖｙｙ、Ｚ軸方向に速度Δｖｙｚで移動させたときに、Ｘ’軸方向に速度Δｖｙｘ’、Ｙ’軸方向に速度Δｖｙｙ’、Ｚ’軸方向に速度Δｖｙｚ’で移動することを示す検出値が得られる関係にあるとすると、次の式（２８）が成立する。
【０１１６】
【数２８】
Ｚ軸加速度センサー５０ｃをＸ軸方向に速度Δｖｚｘ、Ｙ軸方向に速度Δｖｚｙ、Ｚ軸方向に速度Δｖｚｚで移動させたときに、Ｘ’軸方向に速度Δｖｚｘ’、Ｙ’軸方向に速度Δｖｚｙ’、Ｚ’軸方向に速度Δｖｚｚ’で移動することを示す検出値が得られる関係にあるとすると、次の式（２９）が成立する。
【０１１７】
【数２９】
式（２７）〜式（２９）は角速度センサー１０ａ、１０ｂ、１０ｃにおける式（１４）〜式（１６）と対応している。そして、角速度センサー１０ａ、１０ｂ、１０ｃにおける式（１７）〜式（２２）の導出と同様の方法により以下の（３０）〜式（３５）が得られる。
【０１１８】
【数３０】
【０１１９】
【数３１】
【０１２０】
【数３２】
【０１２１】
【数３３】
【０１２２】
【数３４】
【０１２３】
【数３５】
そして、Δφ２ｘ、Δφ３ｘ、Δφ１ｙ、Δφ３ｙ、Δφ１ｚ、Δφ２ｚから計算される逆行列Ｔγｘ−１、Ｔγｙ−１、Ｔγｚ−１を次の式（３６）に代入すれば、加速度センサー５０ａ、５０ｂ、５０ｃの各検出値Ａｘ’、Ａｙ’、Ａｚ’をそれぞれ理想値Ａｘ、Ａｙ、Ａｚに補正することができる。なお、式（３６）は角速度センサー１０ａ、１０ｂ、１０ｃにおける式（１３）と対応している。
【０１２４】
【数３６】
なお、実際には、式（３６）の補正計算はＣＰＵ又は専用回路によりデジタル値で行われる。そのため、加速度センサー５０ａ、５０ｂ、５０ｃの検出値Ａｘ’、Ａｙ’、Ａｚ’のＡ／Ｄ変換値にＡ／Ｄ変換のサンプル周期Δｔを掛けて得られるＸ’軸、Ｙ’軸、Ｚ’軸方向の微小速度Δｖｘ’、Δｖｙ’、Δｖｚ’が、次の式（３７）により、Ｘ軸、Ｙ軸、Ｚ軸方向の微小速度Δｖｘ、Δｖｙ、Δｖｚに補正される。
【０１２５】
【数３７】
２−３．補正パラメーター作成用装置
図８は、本実施形態の補正パラメーター作成用装置の構成を示す図である。
【０１２６】
補正パラメーター作成用装置２００は、姿勢検出装置１に含まれる各センサーの取付角誤差に起因する誤差を含む検出値を理想値に補正するための補正パラメーター（補正行列）を作成するために使用される。
【０１２７】
補正パラメーター作成用装置２００は、立方体治具２１０、ソケット２２０、回転板２３０、回転モーター２４０、支持台２５０、ケーブル２６０等により構成される。
【０１２８】
立方体治具２１０は、金属等の材料により立法体形状（直方体形状でもよい）に成型され、３つの面２１１、２１２、２１３が互いに直交するように厳密に面取りがされており、面２１１にはソケット２２０が固着されている。立方体治具２１０の３つの面２１１、２１２、２１３は、それぞれ本発明における治具の第１の面、第２の面及び第３の面に相当する。
【０１２９】
ソケット２２０は、ソケット本体２２２と開閉可能な蓋体２２４で構成されており、ソケット本体２２０は姿勢検出装置１を所定の向きに隙間無く収容できるようになっている。
【０１３０】
立方体治具２１０は、姿勢検出装置１をソケット２２０にセットすることにより、Ｘ軸、Ｙ軸及びＺ軸がそれぞれ面２１２、２１３及び２１１と垂直になるように、姿勢検出装置１を固定可能になっている。また、立方体治具２１０の面２１１、２１２、２１３とそれぞれ対向する３つの面２１４、２１５、２１６にはそれぞれ固定金具（図示せず）が設けられている。
【０１３１】
回転板２３０は、上面２３１の凹凸が無視できるほど小さく、上面２３１には固定金具（図示せず）が設けられており、立方体治具２１０のいずれかの固定金具を回転板２３０の固定金具に接合することにより、立法体治具２１０の面２１４、２１５、２１６のいずれかを上面２３１に固定可能になっている。
【０１３２】
また、回転板２３０は傾きを調整することができるようになっており、データ補正装置２００が設置された状態で回転板２３０の上面２３１が水平になるように厳密に調整される。
【０１３３】
回転モーター２４０は、支持台２５０に取り付けられており、鉛直方向を軸として時計回り又は半時計回りに所定範囲の角速度で回転できるようになっている。
【０１３４】
ケーブル２６０は回転モーター２４０の制御回路（図示せず）に接続されている。ケーブル２６０にはパーソナルコンピュータ等の制御装置（図示せず）が接続され、ＧＰＩＢ（General Purpose Interface Bus）等のインターフェースにより回転モーター２５０の回転速度が調整できるようになっている。
【０１３５】
なお、回転モーター２４０は、本発明における回転制御部として機能する。
【０１３６】
本実施形態によれば、立方体治具２１０と回転板２３０を用いることにより、図１３（Ａ）等に示した回転腕５３０を必要としないので、よりコンパクトかつ低コストの補正パラメーター作成用装置２００を提供することができる。本実施形態に係る補正パラメーター作成用装置２００を用いることにより、後述するように、補正行列Ｔδｘ−１、Ｔδｙ−１、Ｔδｚ−１、Ｔγｘ−１、Ｔγｙ−１、Ｔγｚ−１を簡単に短時間で取得することができる。
【０１３７】
２−４．補正パラメーター作成手順
次に、図８に示した補正パラメーター作成用装置２００を用いて、補正パラメーター（補正行列）を作成する手順の一例について説明する。
【０１３８】
図９は、本実施形態における補正パラメーター作成手順の一例を示すフローチャート図である。
【０１３９】
まず、回転板２３０を上面２３１が水平になるように設置する（ステップＳ１０）。
【０１４０】
次に、立方体冶具２１０の面２１１に取り付けられているソケット２２０に姿勢検出装置１をセットする（ステップＳ１２）。
【０１４１】
次に、立方体治具２１０の面２１２と対向する面２１５を回転板２３０の上面２３１に固定する（ステップＳ１４）。これにより、補正パラメーター作成用装置２００は図１０（Ａ）のようにセッティングされ、Ｘ軸の正方向が鉛直上向きになるように、立方体冶具２１０が回転板２３０に固定される。
【０１４２】
次に、回転板２３０を静止させた状態でＹ軸加速度センサー５０ｂ、Ｚ軸加速度センサー５０ｃの各検出値を取得し、式（３０）、式（３１）よりΔφ３ｙ、Δφ２ｚを計算する（ステップＳ１６）。具体的には、加速度センサー５０ｂ、５０ｃの各検出値Ａｙ’、Ａｚ’をサンプリングしてＡ／Ｄ変換した値にサンプル周期Δｔを掛けて得られる微小速度Δｖｙ’、Δｖｚ’を計算する。ここで、Δｖｙ’及びΔｖｚ’は、それぞれ式（３０）におけるΔｖｙｙ’及び式（３１）におけるΔｖｚｚ’に相当する。また、加速度センサー５０ｂ、５０ｃにはＸ軸の負方向に重力加速度ｇが加わるため、式（３０）、式（３１）におけるΔｖｘ＝−ｇ×Δｔである。従って、式（３０）、式（３１）よりΔφ３ｙ、Δφ２ｚを計算することができる。
【０１４３】
次に、回転板２３０を角速度ωｘで回転させてＹ軸角速度センサー１０ｂ、Ｚ軸角速度センサー１０ｃの各検出値を取得し、式（１７）、式（１８）よりΔθ３ｙ、Δθ２ｚを計算する（ステップＳ１８）。具体的には、角速度センサー１０ｂ、１０ｃの各検出値Ｇｙ’、Ｇｚ’をサンプリングしてＡ／Ｄ変換した値にサンプル周期Δｔを掛けて得られる微小回転角Δθｙ’、Δθｚ’を計算する。ここで、Δθｙ’及びΔθｚ’は、それぞれ式（１７）におけるΔθｙｙ’及び式（１８）におけるΔθｚｚ’に相当する。また、角速度センサー１０ｂ、１０ｃはＸ軸回りに角速度ωｘで回転するため、式（１７）、式（２８）におけるΔθｘ＝ωｘ×Δｔである。従って、式（１７）、式（１８）よりΔθ３ｙ、Δθ２ｚを計算することができる。
【０１４４】
次に、立方体治具２１０の面２１３と対向する面２１６を回転板２３０の上面２３１に固定する（ステップＳ２０）。これにより、補正パラメーター作成用装置２００は図１０（Ｂ）のようにセッティングされ、Ｙ軸の正方向が鉛直上向きになるように、立方体冶具２１０が回転板２３０に固定される。
【０１４５】
次に、回転板２３０を静止させた状態でＸ軸加速度センサー５０ａ、Ｚ軸加速度センサー５０ｃの各検出値を取得し、式（３２）、式（３３）よりΔφ３ｘ、Δφ１ｚを計算する（ステップＳ２２）。ステップＳ２２における具体的な処理は、ステップＳ１６と同様であるため説明を省略する。
【０１４６】
次に、回転板２３０を角速度ωｙで回転させてＸ軸角速度センサー１０ａ、Ｚ軸角速度センサー１０ｃの各検出値を取得し、式（１９）、式（２０）よりΔθ３ｘ、Δθ１ｚを計算する（ステップＳ２４）。ステップＳ２４における具体的な処理は、ステップＳ１８と同様であるため説明を省略する。
【０１４７】
次に、立方体治具２１０の面２１１と対向する面２１４を回転板２３０の上面２３１に固定する（ステップＳ２６）。これにより、補正パラメーター作成用装置２００は図１０（Ｃ）のようにセッティングされ、Ｚ軸の正方向が鉛直上向きになるように、立方体冶具２１０が回転板２３０に固定される。
【０１４８】
次に、回転板２３０を静止させた状態でＸ軸加速度センサー５０ａ、Ｙ軸加速度センサー５０ｂの各検出値を取得し、式（３４）、式（３５）よりΔφ２ｘ、Δφ１ｙを計算する（ステップＳ２８）。ステップＳ２８における具体的な処理は、ステップＳ１６と同様であるため説明を省略する。
【０１４９】
次に、回転板２３０を角速度ωｚで回転させてＸ軸角速度センサー１０ａ、Ｙ軸角速度センサー１０ｂの各検出値を取得し、式（２１）、式（２２）よりΔθ２ｘ、Δθ１ｙを計算する（ステップＳ３０）。ステップＳ３０における具体的な処理は、ステップＳ１８と同様であるため説明を省略する。
【０１５０】
最後に、補正行列Ｔδｘ−１、Ｔδｙ−１、Ｔδｚ−１、Ｔγｘ−１、Ｔγｙ−１、Ｔγｚ−１を作成する（ステップＳ３２）。具体的には、ステップＳ２４、Ｓ３０でそれぞれ計算したΔθ３ｘ、Δθ２ｘを式（１０）に代入して得られる変換行列Ｔδｘの逆行列を計算することにより補正行列Ｔδｘ−１を作成することができる。同様に、ステップＳ１８、Ｓ３０でそれぞれ計算したΔθ３ｙ、Δθ１ｙを式（１１）に代入して得られる変換行列Ｔδｙの逆行列を計算することにより補正行列Ｔδｙ−１を作成することができる。同様に、ステップＳ１８、Ｓ２４でそれぞれ計算したΔθ２ｚ、Δθ１ｚを式（１２）に代入して得られる変換行列Ｔδｚの逆行列を計算することにより補正行列Ｔδｚ−１を作成することができる。同様に、ステップＳ２２、Ｓ２８でそれぞれ計算したΔφ３ｘ、Δφ２ｘを式（２４）に代入して得られる変換行列Ｔγｘの逆行列を計算することにより補正行列Ｔγｘ−１を作成することができる。同様に、ステップＳ１６、Ｓ２８でそれぞれ計算したΔφ３ｙ、Δφ１ｙを式（２５）に代入して得られる変換行列Ｔγｙの逆行列を計算することにより補正行列Ｔγｙ−１を作成することができる。同様に、ステップＳ１６、Ｓ２２でそれぞれ計算したΔφ２ｚ、Δφ１ｚを式（２６）に代入して得られる変換行列Ｔγｚの逆行列を計算することにより補正行列Ｔγｚ−１を作成することができる。
【０１５１】
なお、以上の処理は、補正パラメーター作成用装置２００のケーブル２６０に接続されたパーソナルコンピュータ等により行われる。そして、本実施形態を用いて作成した補正パラメーターは、例えば、姿勢検出装置１の後段に接続されるユーザー側のマイコンに実装される補正計算処理用のタスクにおいて使用される。
【０１５２】
本実施形態によれば、立方体治具２１０を用いることにより、Ｘ軸、Ｙ軸、Ｚ軸が立方体治具２１０のそれぞれ面２１２、面２１３、面２１１と垂直になるように、面２１１に姿勢検出装置１を容易に固定することができる。そして、上面２３１が水平になるように回転板２３０を設置すれば、立方体治具２１０の面２１５、２１６、２１４をそれぞれ回転板２３０の上面２３１に固定するだけで、簡単に、Ｘ軸、Ｙ軸、Ｚ軸をそれぞれ鉛直方向と平行にすることができる。さらに、Ｘ軸、Ｙ軸、Ｚ軸をそれぞれ鉛直方向と平行にした状態において、回転板２３０を静止させることにより加速度センサー５０ａ、５０ｂ、５０ｃの各検出値を、回転板２３０を回転させることにより角速度センサー１０ａ、１０ｂ、１０ｃの各検出値を、それぞれ簡単に短時間で取得することができる。
【０１５３】
すなわち、最初に一度だけ、上面２３１が水平になるように回転板２３０を設置すれば回転板２３０の回転方向が固定されるので、Ｘ軸、Ｙ軸、Ｚ軸に関する検出値を取得するためのセッティング時間を大幅に短縮することができる。従って、本実施形態によれば、補正行列Ｔδｘ−１、Ｔδｙ−１、Ｔδｚ−１、Ｔγｘ−１、Ｔγｙ−１、Ｔγｚ−１をより低コストで作成することができる。
【０１５４】
なお、本実施形態では、例えば、Ｘ軸角速度センサー１０ａの補正行列Ｔδｘ−１は、Ｙ軸及びＺ軸回りの回転時のＸ軸角速度センサー１０ａの検出値に基づいて作成される。そのため、各検出軸に対する取付角誤差と他軸感度誤差の両方が考慮された補正パラメーターが作成される。
【０１５５】
本実施形態を用いて作成した補正パラメーターは、移動体やロボットの姿勢検出、姿勢制御を行う装置、バーチャルリアリティー等に使用されるヘッドマウントディスプレイ、頭の姿勢角度を検出するトラッカー、３Ｄゲームパッド等を使用するゲーム機、デジタルカメラ、携帯電話機、携帯型情報端末、カーナビゲーションシステム等の種々の電子機器に組み込まれる姿勢検出装置の検出値の補正に使用することができる。
【０１５６】
３．補正機能付き姿勢検出装置
図１１は、本実施形態の姿勢検出装置の構成を示す図である。
【０１５７】
補正機能付き姿勢検出装置３００は、角速度センサーモジュール２、加速度センサーモジュール３、アンチエリアスフィルター３１０ａ、３１０ｂ、３１０ｃ、３５０ａ、３５０ｂ、３５０ｃ、Ａ／Ｄ変換回路３２０ａ、３２０ｂ、３２０ｃ、３６０ａ、３６０ｂ、３６０ｃ、補正計算処理部３７０、記憶部３８０を含んで構成されている。
【０１５８】
角速度センサーモジュール２及び加速度センサーモジュール３は図１及び図２と同じ構成であるため、その説明を省略する。
【０１５９】
アンチエリアスフィルター３１０ａ、３１０ｂ、３１０ｃ、３５０ａ、３５０ｂ、３５０ｃは、それぞれＡ／Ｄ変換回路３２０ａ、３２０ｂ、３２０ｃ、３６０ａ、３６０ｂ、３６０ｃの前段に配置され、角速度検出信号３８ａ、３８ｂ、３８ｃ及び加速度検出信号７８ａ、７８ｂ、７８ｃに対して、それぞれＡ／Ｄ変換回路３２０ａ、３２０ｂ、３２０ｃ、３６０ａ、３６０ｂ、３６０ｃのサンプリングによりＤＣ付近の周波数帯域に折り返すノイズをあらかじめ無視できる程度にまで減衰させる。
【０１６０】
なお、アンチエリアスフィルター３１０ａ、３１０ｂ、３１０ｃ、３６０ａ、３６０ｂ、３６０ｃは、例えば、スイッチトキャパシタフィルター（Switched Capacitor Filter（ＳＣＦ））として構成することができる。
【０１６１】
Ａ／Ｄ変換回路３２０ａ、３２０ｂ、３２０ｃ、３６０ａ、３６０ｂ、３６０ｃは、角速度検出信号３８ａ、３８ｂ、３８ｃ及び加速度検出信号７８ａ、７８ｂ、７８ｃがアンチエリアスフィルター３１０ａ、３１０ｂ、３１０ｃ、３５０ａ、３５０ｂ、３５０ｃによってそれぞれフィルター処理された信号をそれぞれ所定ビット数の角速度検出信号３２２ａ、３２２ｂ、３２２ｃ及び加速度検出信号３６２ａ、３６２ｂ、３６２ｃに変換する。Ａ／Ｄ変換回路３２０ａ、３２０ｂ、３２０ｃ、３６０ａ、３６０ｂ、３６０ｃは、本発明におけるＡ／Ｄ変換処理部として機能し、フラッシュ型（並列比較型）、パイプライン型、逐次比較型、デルタシグマ方式等の既知の様々なタイプのＡＤ変換回路により構成することができる。
【０１６２】
記憶部３８０には、角速度センサーの補正パラメーター３８２と加速度センサーの補正用パラメーター３８４が記憶されている。具体的には、補正パラメーター３８２は補正行列Ｔδｘ−１、Ｔδｙ−１、Ｔδｚ−１であり、補正パラメーター３８４は補正行列Ｔγｘ−１、Ｔγｙ−１、Ｔγｚ−１である。
【０１６３】
補正計算処理部３７０は、角速度検出信号３２２ａ、３２２ｂ、３２２ｃと補正パラメーター３８２に基づいて補正式（２３）を計算することにより、角速度センサー１０ａ、１０ｂ、１０ｃの取付角誤差に起因する角速度検出信号３８ａ、３８ｂ、３８ｃの誤差が補正された角速度検出信号３０２ａ、３０２ｂ、３０２ｃを生成する。具体的には、補正計算処理部３７０は、角速度検出信号３２２ａ、３２２ｂ、３２２ｃのデジタル値にＡ／Ｄ変換のサンプル周期Δｔを掛けた値を補正式（２３）の微小回転角Δθｘ’、Δθｙ’、Δθｚ’にそれぞれ代入して微小回転角Δθｘ、Δθｙ、Δθｚを計算し、さらに微小回転角Δθｘ、Δθｙ、ΔθｚをΔｔで割ったデジタル値に対応する角速度検出信号３０２ａ、３０２ｂ、３０２ｃを生成する。
【０１６４】
同様に、補正計算処理部３７０は、加速度検出信号３６２ａ、３６２ｂ、３６２ｃと補正パラメーター３８４に基づいて補正式（３７）を計算することにより、加速度センサー５０ａ、５０ｂ、５０ｃの取付角誤差に起因する加速度検出信号７８ａ、７８ｂ、７８ｃの誤差が補正された角速度検出信号３０４ａ、３０４ｂ、３０４ｃを生成する。具体的には、補正計算処理部３７０は、加速度検出信号３６２ａ、３６２ｂ、３６２ｃのデジタル値にＡ／Ｄ変換のサンプル周期Δｔを掛けた値を補正式（３７）の微小速度Δｖｘ’、Δｖｙ’、Δｖｚ’にそれぞれ代入して微小速度Δｖｘ、Δｖｙ、Δｖｚを計算し、さらに微小速度Δｖｘ、Δｖｙ、ΔｖｚをΔｔで割ったデジタル値に対応する加速度検出信号３０４ａ、３０４ｂ、３０４ｃを生成する。
【０１６５】
補正計算処理部３７０は補正計算処理を行う専用回路として実現することもできるし、ＣＰＵ（Central Processing Unit）が記憶部３８０等に記憶されたプログラムを実行することにより補正計算処理部３７０の機能を実現することもできる。
【０１６６】
図１２は、本実施形態の姿勢検出装置の他の構成を示す図である。
【０１６７】
図１２において、角速度センサーモジュール２、加速度センサーモジュール３、アンチエリアスフィルター３１０ａ、３１０ｂ、３１０ｃ、３５０ａ、３５０ｂ、３５０ｃ、記憶部３８０の構成は図１１と同様であるため、その説明を省略する。
【０１６８】
マルチプレクサ３９０は、角速度検出信号３８ａ、３８ｂ、３８ｃ及び加速度検出信号７８ａ、７８ｂ、７８ｃがアンチエリアスフィルター３１０ａ、３１０ｂ、３１０ｃ、３５０ａ、３５０ｂ、３５０ｃによってそれぞれフィルター処理された信号を所定周期で時分割に順次選択する。
【０１６９】
Ａ／Ｄ変換回路３２０は、マルチプレクサ３９０により選択された信号を所定ビット数の検出信号３２２に変換する。Ａ／Ｄ変換回路３２０及びマルチプレクサ３９０は、それぞれ本発明におけるＡ／Ｄ変換処理部及び信号選択処理部として機能する。
【０１７０】
補正計算処理部３７０は、所定周期毎に検出信号３２２をサンプリングし、検出信号３２２が角速度検出信号３８ａ、３８ｂ、３８ｃに対応する場合は、検出信号３２２と補正パラメーター３８２に基づいて補正式（２３）を計算することにより補正された角速度検出信号をそれぞれ生成し、検出信号３２２として時分割に出力する。
【０１７１】
同様に、補正計算処理部３７０は、検出信号３２２が加速度検出信号７８ａ、７８ｂ、７８ｃに対応する場合は、検出信号３２２と補正パラメーター３８４に基づいて補正式（３７）を計算することにより補正された加速度検出信号をそれぞれ生成し、検出信号３２２として時分割に出力する。
【０１７２】
なお、図１１又、図１２に示した姿勢検出装置３００において、補正計算処理部３７０をバイパスしてＡ／Ｄ変換回路３２０ａ等の出力を外部に出力可能なバイパスモードを設ければ、バイパスモードに設定した状態で本実施形態の補正パラメーター作成方法を用いて補正パラメーター３８２、３８４を作成することができる。すなわち、姿勢検出装置３００も本実施形態の補正パラメーター作成方法の適用対象になり得る。
【０１７３】
本実施形態によれば、補正計算処理部３７０は、前記の通り、補正精度の向上及び補正計算処理の高速化を実現可能な補正式（２３）及び補正式（３７）に従い補正値を計算するので、補正精度がより高く、かつ、補正計算処理がより速い姿勢検出装置を実現することができる。
【０１７４】
また、本実施形態によれば、姿勢検出装置３００の後段に接続されるユーザー側のマイコンに補正計算処理を実装する必要がなくなるため、タスクカプセル化の見地からユーザーに受け入れられやすい。
【０１７５】
また、本実施形態の姿勢検出装置３００はセンサー検出信号をデジタル化して出力するので、姿勢検出装置３００とユーザー側のマイコンの間にＡ／Ｄ変換回路を接続する必要がなくなる。
【０１７６】
本実施形態の姿勢検出装置３００は、移動体やロボットの姿勢検出、姿勢制御を行う装置、バーチャルリアリティー等に使用されるヘッドマウントディスプレイ、頭の姿勢角度を検出するトラッカー、３Ｄゲームパッド等を使用するゲーム機、デジタルカメラ、携帯電話機、携帯型情報端末、カーナビゲーションシステム等の種々の電子機器に組み込むことができる。
【０１７７】
なお、本発明は本実施形態に限定されず、本発明の要旨の範囲内で種々の変形実施が可能である。
【０１７８】
例えば、図１に示した姿勢検出装置は、３つの角速度センサー１０ａ、１０ｂ、１０ｃと３つの加速度センサー５０ａ、５０ｂ、５０ｃを含んでいるが、本発明の補正パラメーター作成方法が適用される姿勢検出装置はこれに限られない。すなわち、本発明の補正パラメーター作成方法が適用される姿勢検出装置は直交する３軸の角速度又は加速度を検出可能な構成であればよく、例えば、角速度センサー１０ａ、１０ｂ、１０ｃのみを含む姿勢検出装置、加速度センサー５０ａ、５０ｂ、５０ｃのみを含む姿勢検出装置、角速度センサー１０ａ、１０ｂと加速度センサー５０ｃのみを含む姿勢検出装置、角速度センサー１０ａと加速度センサー５０ｂ、５０ｃのみを含む姿勢検出装置等も適用対象に含まれる。
【０１７９】
また、例えば、図８に示した補正パラメーター作成用装置２００は、面２１１にソケット２２０が１つだけ取り付けられているが、面２１１に複数のソケット２２０を取り付けるようにしてもよい。そして、複数のソケット２２０の各々に姿勢検出装置１をセットすることにより、各姿勢検出装置１の検出値を同時に取得することができる。
【０１８０】
また、例えば、図９の手順では、Ｘ軸、Ｙ軸、Ｚ軸の順に姿勢検出装置１の検出値を取得するようになっているが、任意の軸の順に姿勢検出装置１の検出値を取得することができる。
【０１８１】
本発明は、実施の形態で説明した構成と実質的に同一の構成（例えば、機能、方法及び結果が同一の構成、あるいは目的及び効果が同一の構成）を含む。また、本発明は、実施の形態で説明した構成の本質的でない部分を置き換えた構成を含む。また、本発明は、実施の形態で説明した構成と同一の作用効果を奏する構成又は同一の目的を達成することができる構成を含む。また、本発明は、実施の形態で説明した構成に公知技術を付加した構成を含む。
【図面の簡単な説明】
【０１８２】
【図１】本実施形態の補正パラメーター作成方法の対象となる姿勢検出装置の構成の一例を示す図。
【図２】本実施形態における姿勢検出装置の斜視図。
【図３】角速度センサーに含まれる振動子の一例を示す平面図。
【図４】角速度センサーに含まれる振動子の動作について説明するための図。
【図５】角速度センサーに含まれる振動子の動作について説明するための図。
【図６】角速度センサーに含まれる駆動回路及び検出回路の構成の一例を示す図。
【図７】センサーの取付角誤差について説明するための図。
【図８】本実施形態の補正パラメーター作成用装置の構成を示す図。
【図９】本実施形態における補正パラメーター作成手順の一例を示すフローチャート図。
【図１０】本実施形態における補正パラメーター作成手順について説明するための図。
【図１１】本実施形態の姿勢検出装置の構成を示す図。
【図１２】本実施形態の姿勢検出装置の他の構成を示す図。
【図１３】従来の補正パラメーター作成方法について説明するための図。
【図１４】従来の補正パラメーター作成方法について説明するための図。
【符号の説明】
【０１８３】
１ 姿勢検出装置、２ 角速度センサーモジュール、３ 加速度センサーモジュール、４ パッケージ、５ａ，５ｂ，５ｃ パッケージの面、１０ａ，１０ｂ，１０ｃ 角速度センサー、１１ａ，１１ｂ，１１ｃ 振動子、１２ａ，１２ｂ，１２ｃ 駆動電極、１３ａ，１３ｂ，１３ｃ 駆動電極、１４ａ，１４ｂ，１４ｃ 検出電極、１５ａ，１５ｂ，１５ｃ 検出電極、２０ａ，２０ｂ，２０ｃ 駆動回路、２１ａ 電流電圧変換器（Ｉ／Ｖ変換器）、２２ａ ＡＣ増幅器、２３ａ 自動利得制御回路（ＡＧＣ） ２４ａ コンパレータ、３０ａ，３０ｂ，３０ｃ 検出回路、３１ａ，３２ａ チャージアンプ、３３ａ 差動増幅器、３４ａ ＡＣ増幅器、３５ａ 同期検波回路、３６ａ ＤＣ増幅器、３７ａ 積分回路（ＬＰＦ）、３８ａ，３８ｂ，３８ｃ 検出信号、４１ａ 駆動振動腕、４２ａ 検出振動腕、４３ａ 幅広部、４４ａ 駆動用基部、４５ａ 連結腕、４６ａ 幅広部、４７ａ 検出用基部、５０ａ，５０ｂ，５０ｃ 加速度センサー、５１ａ，５１ｂ，５１ｃ 振動子、５２ａ，５２ｂ，５２ｃ 駆動電極、５３ａ，５３ｂ，５３ｃ 駆動電極、５４ａ，５４ｂ，５４ｃ 検出電極、５５ａ，５５ｂ，５５ｃ 検出電極、５６ａ，５６ｂ，５６ｃ 基端部、５７ａ，５７ｂ，５７ｃ 基端部、５８ａ，５８ｂ，５８ｃ 駆動振動腕、６０ａ，６０ｂ，６０ｃ 駆動回路、７０ａ，７０ｂ，７０ｃ 検出回路、７８ａ，７８ｂ，７８ｃ 検出信号、８０ 絶縁基板、８２ａ，８２ｂ，８２ｃ パッケージ、８４ａ，８４ｂ，８４ｃ パッケージ本体、８６ａ，８６ｂ，８６ｃ 蓋体、９０ ベース、９１，９２，９３ 素子取付面、１００ ウエイト、１０１，１０２，１０３ 素子接合面、２００ 補正パラメーター作成用装置、２１０ 立方体治具、２１１，２１２，２１３，２１４，２１５，２１６ 立方体治具の面、２２０ ソケット、２２２ ソケット本体、２２４ 蓋体、２３０ 回転板、２３１ 回転板の上面、２４０ 回転モーター、２５０ 支持台、２６０ ケーブル、３００ 姿勢検出装置、３０２，３０２ａ，３０２ｂ，３０２ｃ 検出信号、３０４ａ，３０４ｂ，３０４ｃ 検出信号、３１０ａ，３１０ｂ，３１０ｃ アンチエリアスフィルター、３１０ａ，３１０ｂ，３１０ｃ Ａ／Ｄ変換回路、３２０ Ａ／Ｄ変換回路、３２２ａ，３２２ｂ，３２２ｃ 検出信号、３５０ａ，３５０ｂ，３５０ｃ アンチエリアスフィルター、３６０ａ，３６０ｂ，３６０ｃ Ａ／Ｄ変換回路、３６２ａ，３６２ｂ，３６２ｃ 検出信号、３７０ 補正計算処理部、３８０ 記憶部、３８２ 補正パラメーター、３８４ 補正パラメーター、３９０ マルチプレクサ、５００ 補正パラメーター作成用装置、５１０ テーブル、５２０ ソケット、５３０ 回転腕
【特許請求の範囲】
【請求項１】
検出軸が互いに直交する第１の軸、第２の軸及び第３の軸とそれぞれ略平行になるように取り付けられ角速度又は加速度を検出する第１のセンサー、第２のセンサー及び第３のセンサーを含み、前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの検出信号に基づいて物体の姿勢を検出する姿勢検出装置の検出値を、前記第１の軸、前記第２の軸及び前記第３の軸を座標軸とする直交座標系における検出値に補正する補正式の補正パラメーターを作成する方法であって、
上面が水平になるように回転板を設置するステップと、
互いに直交する第１の面、第２の面、第３の面を有する直方体形状の治具の前記第１の面に、前記第１の軸が前記第２の面に垂直になり、前記第２の軸が前記第３の面に垂直になり、前記第３の軸が前記第１の面に垂直になるように、前記姿勢検出装置を固定するステップと、
前記治具の前記第２の面と対向する面を前記回転板の前記上面に固定し、前記回転板を静止又は所定の角速度で回転させて前記姿勢検出装置の検出値を取得する第１検出値取得ステップと、
前記治具の前記第３の面と対向する面を前記回転板の前記上面に固定し、前記回転板を静止又は所定の角速度で回転させて前記姿勢検出装置の検出値を取得する第２検出値取得ステップと、
前記治具の前記第１の面と対向する面を前記回転板の前記上面に固定し、前記回転板を静止又は所定の角速度で回転させて前記姿勢検出装置の検出値を取得する第３検出値取得ステップと、
取得した検出値に基づいて、前記補正パラメーターを作成する補正パラメーター作成ステップと、を含むことを特徴とする姿勢検出装置の補正パラメーター作成方法。
【請求項２】
請求項１において、
前記補正式は、
前記補正パラメーターとして前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各検出値を前記直交座標系における各検出値に補正するための第１の補正行列、第２の補正行列及び第３の補正行列を含み、前記第１の補正行列、前記第２の補正行列及び前記第３の補正行列の各々と、前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各検出値がＡ／Ｄ変換されたデジタル値をそれぞれ要素として含む行列の各々と、の積により得られる３つの行列の和として与えられることを特徴とする姿勢検出装置の補正パラメーター作成方法。
【請求項３】
請求項２において、
前記第１の補正行列、前記第２の補正行列及び前記第３の補正行列は、前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各検出軸をそれぞれ前記第１の軸、前記第２の軸及び前記第３の軸に変換する回転行列の逆行列であることを特徴とする姿勢検出装置の補正パラメーター作成方法。
【請求項４】
請求項２又は３において、
前記補正パラメーター作成ステップは、
前記第１検出値取得ステップにおいて取得した前記検出値に基づいて、前記第２のセンサー及び前記第３のセンサーの前記第１の軸回りの各取付角誤差を算出するステップと、
前記第２検出値取得ステップにおいて取得した前記検出値に基づいて、前記第１のセンサー及び前記第３のセンサーの前記第２の軸回りの各取付角誤差を算出するステップと、
前記第３検出値取得ステップにおいて取得した前記検出値に基づいて、前記第１のセンサー及び前記第２のセンサーの前記第３の軸回りの各取付角誤差を算出するステップと、
前記第１のセンサーの前記第２の軸回りの前記取付角誤差及び前記第３の軸回りの前記取付角誤差に基づいて、前記第１の補正行列を作成するステップと、
前記第２のセンサーの前記第１の軸回りの前記取付角誤差及び前記第３の軸回りの前記取付角誤差に基づいて、前記第２の補正行列を作成するステップと、
前記第３のセンサーの前記第１の軸回りの前記取付角誤差及び前記第２の軸回りの前記取付角誤差に基づいて、前記第３の補正行列を作成するステップと、を含むことを特徴とする姿勢検出装置の補正パラメーター作成方法。
【請求項５】
検出軸が互いに直交する第１の軸、第２の軸及び第３の軸とそれぞれ略平行になるように取り付けられ角速度又は加速度を検出する第１のセンサー、第２のセンサー及び第３のセンサーを含み、前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの検出信号に基づいて物体の姿勢を検出する姿勢検出装置の検出値を、前記第１の軸、前記第２の軸及び前記第３の軸を座標軸とする直交座標系における検出値に補正する補正式の補正パラメーターを作成するために使用される補正パラメーター作成用装置であって、
互いに直交する第１の面、第２の面、第３の面を有し、前記第１の面に、前記第１の軸が前記第２の面に垂直になり、前記第２の軸が前記第３の面に垂直になり、前記第３の軸が前記第１の面に垂直になるように前記姿勢検出装置を固定可能な直方体形状の治具と、
上面に、前記治具の前記第１の面、前記第２の面、前記第２の面とそれぞれ対向する面のいずれかを固定可能な回転板と、
前記回転板を所定の角速度で回転させる回転制御部と、を含むことを特徴とする姿勢検出装置の補正パラメーター作成用装置。
【請求項６】
検出軸が互いに直交する第１の軸、第２の軸及び第３の軸とそれぞれ略平行になるように取り付けられ角速度又は加速度を検出する第１のセンサー、第２のセンサー及び第３のセンサーと、
前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各検出値を前記第１の軸、前記第２の軸及び前記第３の軸を座標軸とする直交座標系における検出値に補正する補正式の補正パラメーターが記憶された記憶部と、
前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各検出信号をデジタル信号に変換する処理を行うＡ／Ｄ変換処理部と、
前記デジタル信号の各々と前記補正パラメーターに基づいて前記補正式を計算する処理を行う補正計算処理部と、を含み、
前記補正式は、
前記補正パラメーターとして前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各検出値を前記直交座標系における各検出値に補正するための第１の補正行列、第２の補正行列及び第３の補正行列を含み、前記第１の補正行列、前記第２の補正行列及び前記第３の補正行列の各々と、前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各検出値がＡ／Ｄ変換されたデジタル値をそれぞれ要素として含む行列の各々と、の積により得られる３つの行列の和として与えられることを特徴とする姿勢検出装置。
【請求項７】
請求項６において、
前記第１の補正行列、前記第２の補正行列及び前記第３の補正行列は、前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各検出軸をそれぞれ前記第１の軸、前記第２の軸及び前記第３の軸に変換する回転行列の逆行列であることを特徴とする姿勢検出装置。
【請求項８】
請求項６又は７において、
所定の周期で、前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各前記検出信号のいずれか１つを順次選択する処理を行う信号選択処理部を含み、
前記Ａ／Ｄ変換処理部は、
前記信号選択処理部が選択した検出値を順次Ａ／Ｄ変換処理するＡ／Ｄ変換回路を含むことを特徴とする姿勢検出装置。
【請求項１】
検出軸が互いに直交する第１の軸、第２の軸及び第３の軸とそれぞれ略平行になるように取り付けられ角速度又は加速度を検出する第１のセンサー、第２のセンサー及び第３のセンサーを含み、前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの検出信号に基づいて物体の姿勢を検出する姿勢検出装置の検出値を、前記第１の軸、前記第２の軸及び前記第３の軸を座標軸とする直交座標系における検出値に補正する補正式の補正パラメーターを作成する方法であって、
上面が水平になるように回転板を設置するステップと、
互いに直交する第１の面、第２の面、第３の面を有する直方体形状の治具の前記第１の面に、前記第１の軸が前記第２の面に垂直になり、前記第２の軸が前記第３の面に垂直になり、前記第３の軸が前記第１の面に垂直になるように、前記姿勢検出装置を固定するステップと、
前記治具の前記第２の面と対向する面を前記回転板の前記上面に固定し、前記回転板を静止又は所定の角速度で回転させて前記姿勢検出装置の検出値を取得する第１検出値取得ステップと、
前記治具の前記第３の面と対向する面を前記回転板の前記上面に固定し、前記回転板を静止又は所定の角速度で回転させて前記姿勢検出装置の検出値を取得する第２検出値取得ステップと、
前記治具の前記第１の面と対向する面を前記回転板の前記上面に固定し、前記回転板を静止又は所定の角速度で回転させて前記姿勢検出装置の検出値を取得する第３検出値取得ステップと、
取得した検出値に基づいて、前記補正パラメーターを作成する補正パラメーター作成ステップと、を含むことを特徴とする姿勢検出装置の補正パラメーター作成方法。
【請求項２】
請求項１において、
前記補正式は、
前記補正パラメーターとして前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各検出値を前記直交座標系における各検出値に補正するための第１の補正行列、第２の補正行列及び第３の補正行列を含み、前記第１の補正行列、前記第２の補正行列及び前記第３の補正行列の各々と、前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各検出値がＡ／Ｄ変換されたデジタル値をそれぞれ要素として含む行列の各々と、の積により得られる３つの行列の和として与えられることを特徴とする姿勢検出装置の補正パラメーター作成方法。
【請求項３】
請求項２において、
前記第１の補正行列、前記第２の補正行列及び前記第３の補正行列は、前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各検出軸をそれぞれ前記第１の軸、前記第２の軸及び前記第３の軸に変換する回転行列の逆行列であることを特徴とする姿勢検出装置の補正パラメーター作成方法。
【請求項４】
請求項２又は３において、
前記補正パラメーター作成ステップは、
前記第１検出値取得ステップにおいて取得した前記検出値に基づいて、前記第２のセンサー及び前記第３のセンサーの前記第１の軸回りの各取付角誤差を算出するステップと、
前記第２検出値取得ステップにおいて取得した前記検出値に基づいて、前記第１のセンサー及び前記第３のセンサーの前記第２の軸回りの各取付角誤差を算出するステップと、
前記第３検出値取得ステップにおいて取得した前記検出値に基づいて、前記第１のセンサー及び前記第２のセンサーの前記第３の軸回りの各取付角誤差を算出するステップと、
前記第１のセンサーの前記第２の軸回りの前記取付角誤差及び前記第３の軸回りの前記取付角誤差に基づいて、前記第１の補正行列を作成するステップと、
前記第２のセンサーの前記第１の軸回りの前記取付角誤差及び前記第３の軸回りの前記取付角誤差に基づいて、前記第２の補正行列を作成するステップと、
前記第３のセンサーの前記第１の軸回りの前記取付角誤差及び前記第２の軸回りの前記取付角誤差に基づいて、前記第３の補正行列を作成するステップと、を含むことを特徴とする姿勢検出装置の補正パラメーター作成方法。
【請求項５】
検出軸が互いに直交する第１の軸、第２の軸及び第３の軸とそれぞれ略平行になるように取り付けられ角速度又は加速度を検出する第１のセンサー、第２のセンサー及び第３のセンサーを含み、前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの検出信号に基づいて物体の姿勢を検出する姿勢検出装置の検出値を、前記第１の軸、前記第２の軸及び前記第３の軸を座標軸とする直交座標系における検出値に補正する補正式の補正パラメーターを作成するために使用される補正パラメーター作成用装置であって、
互いに直交する第１の面、第２の面、第３の面を有し、前記第１の面に、前記第１の軸が前記第２の面に垂直になり、前記第２の軸が前記第３の面に垂直になり、前記第３の軸が前記第１の面に垂直になるように前記姿勢検出装置を固定可能な直方体形状の治具と、
上面に、前記治具の前記第１の面、前記第２の面、前記第２の面とそれぞれ対向する面のいずれかを固定可能な回転板と、
前記回転板を所定の角速度で回転させる回転制御部と、を含むことを特徴とする姿勢検出装置の補正パラメーター作成用装置。
【請求項６】
検出軸が互いに直交する第１の軸、第２の軸及び第３の軸とそれぞれ略平行になるように取り付けられ角速度又は加速度を検出する第１のセンサー、第２のセンサー及び第３のセンサーと、
前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各検出値を前記第１の軸、前記第２の軸及び前記第３の軸を座標軸とする直交座標系における検出値に補正する補正式の補正パラメーターが記憶された記憶部と、
前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各検出信号をデジタル信号に変換する処理を行うＡ／Ｄ変換処理部と、
前記デジタル信号の各々と前記補正パラメーターに基づいて前記補正式を計算する処理を行う補正計算処理部と、を含み、
前記補正式は、
前記補正パラメーターとして前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各検出値を前記直交座標系における各検出値に補正するための第１の補正行列、第２の補正行列及び第３の補正行列を含み、前記第１の補正行列、前記第２の補正行列及び前記第３の補正行列の各々と、前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各検出値がＡ／Ｄ変換されたデジタル値をそれぞれ要素として含む行列の各々と、の積により得られる３つの行列の和として与えられることを特徴とする姿勢検出装置。
【請求項７】
請求項６において、
前記第１の補正行列、前記第２の補正行列及び前記第３の補正行列は、前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各検出軸をそれぞれ前記第１の軸、前記第２の軸及び前記第３の軸に変換する回転行列の逆行列であることを特徴とする姿勢検出装置。
【請求項８】
請求項６又は７において、
所定の周期で、前記第１のセンサー、前記第２のセンサー及び前記第３のセンサーの各前記検出信号のいずれか１つを順次選択する処理を行う信号選択処理部を含み、
前記Ａ／Ｄ変換処理部は、
前記信号選択処理部が選択した検出値を順次Ａ／Ｄ変換処理するＡ／Ｄ変換回路を含むことを特徴とする姿勢検出装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【公開番号】特開２０１０−１１７２６０（Ｐ２０１０−１１７２６０Ａ）
【公開日】平成２２年５月２７日（２０１０．５．２７）
【国際特許分類】
【出願番号】特願２００８−２９１２１３（Ｐ２００８−２９１２１３）
【出願日】平成２０年１１月１３日（２００８．１１．１３）
【出願人】（０００００３１０４）エプソントヨコム株式会社 (1,528)
【Ｆターム（参考）】
【公開日】平成２２年５月２７日（２０１０．５．２７）
【国際特許分類】
【出願日】平成２０年１１月１３日（２００８．１１．１３）
【出願人】（０００００３１０４）エプソントヨコム株式会社 (1,528)
【Ｆターム（参考）】
[ Back to top ]