(57)【特許請求の範囲】
【請求項１】
水平面内で直交する軸をＸ軸、Ｙ軸とし、そのＸ軸、Ｙ軸に直交する軸をＺ軸とするとき、これら互いに直交するＸ軸、Ｙ軸、Ｚ軸が形成する基準座標系において、センサが搭載された被測定物の前記互いに直交する３軸の回りの回転角を検出する姿勢角検出装置において、互いに直交するｘ軸、ｙ軸、ｚ軸が形成するセンサ座標系のｘ軸、ｙ軸及びｚ軸の回りの角速度を検出する３個のジャイロスコープと、該ジャイロスコープの角速度に応じた出力に基づいて単位時間に移動した角度を演算する運動角演算装置と、ｘ−ｙ平面で互いに直交する２軸の加速度を検出するように配置された加速度センサと、ｘ−ｙ平面で互いに直交する２軸の地磁気を検出するように配置される地磁気センサと、該加速度センサ及び地磁気センサの出力に基づいて、Ｘ軸、Ｙ軸、Ｚ軸回りの回転角を演算する静止角演算装置と、静止角演算装置による演算結果の真偽を判別する判別装置と、該判別装置の演算結果に応じて、運動角演算装置の演算結果と静止角演算装置の演算結果から出力するべき姿勢角を演算する姿勢角演算装置とから構成されることを特徴とする姿勢角検出装置。
【請求項２】
請求項１記載の姿勢角検出装置のジャイロスコープが、圧電振動ジャイロ用振動子と、その振動子をジャイロスコープとして動作させるための駆動検出回路からなり、該振動子が、非磁性材料で構成されることを特徴とする姿勢角検出装置。
【請求項３】
請求項１または２記載の姿勢角検出装置のジァイロスコープの出力部に、ハイパスフィルタが接続され、ハイパスフィルタの出力部は演算装置に入力され、ハイパスフィルタの出力を用いて運動角を演算することを特徴とする姿勢角検出装置。
【請求項４】
請求項３記載のハイパスフィルタは、カットオフ周波数可変手段を有することを特徴とする姿勢角検出装置。
【請求項５】
請求項１乃至請求項４のいずれかに記載の姿勢角検出装置において、２軸の加速度センサと、加速度センサの出力Ａｘ（ｎ）、Ａｙ（ｎ）から仮のロール角Ｒ（ｎ）、仮のピッチ角Ｐ（ｎ）を演算するロール角演算手段、ピッチ角演算手段と、地磁気のｘ方向成分Ｍｘ（ｎ）、ｙ方向成分Ｍｙ（ｎ）を計測する２軸の地磁気センサと、演算する単位時間で１単位時間過去の方位角演算結果Φ（ｎ−１）を記憶しておくためのバックアップメモリーと、加速度センサの出力Ａｘ（ｎ）、Ａｙ（ｎ）及びバックアップメモリーの内容Φ（ｎ−１）より、傾斜角度の北方向成分θ2を演算する北方向成分傾斜角演算手段と、地磁気のＺ方向成分Ｍｚの符号を判定する地磁気Ｍｚ符号判別手段と、地磁気のＺ方向成分地磁気Ｍｚ絶対値演算手段と、前記地磁気Ｍｚ符号判別手段と、地磁気Ｍｚ絶対値演算手段よりＺ軸方向成分の地磁気Ｍｚを計算する地磁気Ｍｚ演算手段と、前記地磁気Ｍｘ、Ｍｙ、Ｍｚより、基準座標系での地磁気のＸ，Ｙ，Ｚ成分ＨＸ，ＨＹ，ＨＺを演算する座標変換演算手段と、基準座標系における前記のＨＸ、ＨＹにより方位角Φを演算する方位演算手段から構成され、地磁気Ｍｚ演算手段により仮想的に地磁気Ｍｚを求めることにより地磁気センサ２軸により方位角Φ（ｎ）を計測することを特徴とする姿勢角検出装置。
【請求項６】
請求項５記載の姿勢角検出装置において、前記北方向成分傾斜角演算装置及び地磁気Ｍｚ符号判別手段により、北方向成分傾斜角θ2、原点時の地磁気の伏角θ0、バックアップメモリの出力をΦ（ｎ−１）、加速度センサ２軸の出力をそれぞれＡｘ（ｎ）、Ａｙ（ｎ）とする時、北方向成分傾斜角θ2は、θ2＝ｓｉｎ-1［Ａｘ（ｎ）・ｃｏｓ（−Φ（ｎ−１））＋Ａｙ（ｎ）・ｓｉｎ（−Φ（ｎ−１））］として求め、−９０度≦θ2≦θ0であれば、地磁気Ｍｚ符号はマイナス、θ0＜θ2≦９０度であれば、地磁気Ｍｚ符号はプラスと判別することを特徴とする姿勢角検出装置。
【請求項７】
請求項１乃至６の姿勢角検出装置において、前記判別装置により、２軸の加速度センサおよび、２軸の地磁気センサからなる静止角検出手段から求められる仮のヨー角Φ・仮のピッチ角Ｐ・仮のロール角Ｒの検出結果が、正しいかどうかの判別手段として、加速度センサの現在の出力Ａｘ（ｎ）から演算して得られた仮のロール角をＲ（ｎ）、演算する単位時間で１単位時間過去の出力Ａｘ（ｎ−１）から得られた仮のロール角演算結果をＲ（ｎ−１）とし、同様に加速度センサの現在の出力Ａｙ（ｎ）から演算して得られた仮のピッチ角をＰ（ｎ）、演算する単位時間で１単位時間過去の出力Ａｙ（ｎ−１）から得られた仮のピッチ角をＰ（ｎ−１）とすると、数１あるいは数２のうち、少なくとも一つの値が、ある値以内にあるとき、静止角検出手段からの姿勢角情報は正しいと判別することを特徴とする姿勢角検出装置。
【数１】
0003763435.tif 000004
【数２】
0003763435.tif 000005
【請求項８】
請求項１乃至請求項７の姿勢角検出装置において、求めるべき出力をロール角α（ｎ）、ピッチ角β（ｎ）、ヨー角γ（ｎ）、運動角検出手段から求められる運動角を、仮のロール角変化△Ｘ（ｎ）、仮のピッチ角変化△Ｙ（ｎ）、仮のヨー角変化△Ｚ（ｎ）、静止角検出手段から求められる静止角を、仮のロール角Ｒ（ｎ）、仮のピッチ角Ｐ（ｎ）、仮のヨー角Φ（ｎ）、誤差より大きくない一定定数をＣ1，Ｃ2，Ｃ3とすると、判別装置の判別結果により、静止角検出手段からの姿勢角情報は正しいと判別する場合、α（ｎ）＝α（ｎ−１）＋△Ｘ（ｎ）−Ｃ1、β（ｎ）＝β（ｎ−１）＋△Ｙ（ｎ）−Ｃ2、γ（ｎ）＝γ（ｎ−１）＋△Ｚ（ｎ）−Ｃ3により、また、静止角検出手段からの姿勢角情報は誤りと判別する場合は、α（ｎ）＝α（ｎ−１）＋△Ｘ（ｎ）、β（ｎ）＝β（ｎ−１）＋△Ｙ（ｎ）、γ（ｎ）＝γ（ｎ−１）＋△Ｚ（ｎ）により、姿勢角を演算することを特徴とする姿勢角検出装置。
【請求項９】
請求項１乃至請求項８の姿勢角検出装置において、求めるべき出力をロール角α（ｎ）、ピッチ角β（ｎ）、ヨー角γ（ｎ）、動的な運動角検出手段から求められる運動角を、仮のロール角変化△Ｘ（ｎ）、仮のピッチ角変化△Ｙ（ｎ）、仮のヨー角変化△Ｚ（ｎ）、静的な静止角検出手段から求められる姿勢角を、仮のロール角Ｒ（ｎ）、仮のピッチ角Ｐ（ｎ）、仮のヨー角Φ（ｎ）、１以下の比例定数をｋ1,ｋ2,ｋ3とすると、判別装置の判別結果により、静止角検出手段からの姿勢角情報は正しいと判別する場合は、α（ｎ）＝α（ｎ−１）＋△Ｘ（ｎ）−ｋ1［α（ｎ−１）＋△Ｘ（ｎ）−Ｒ（ｎ）］、β（ｎ）＝β（ｎ−１）＋△Ｙ（ｎ）−ｋ2［β（ｎ−１）＋△Ｙ（ｎ）−Ｐ（ｎ）］、 γ（ｎ）＝γ（ｎ−１）＋△Ｚ（ｎ）−ｋ3［γ（ｎ−１）＋△Ｚ（ｎ）−Φ（ｎ）］により、また、静止角検出手段からの姿勢角情報は誤りと判別する場合は、α（ｎ）＝α（ｎ−１）＋△Ｘ（ｎ）、β（ｎ）＝β（ｎ−１）＋△Ｙ（ｎ）、γ（ｎ）＝γ（ｎ−１）＋△Ｚ（ｎ）により、姿勢角を演算することを特徴とする姿勢角検出装置。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、移動体の姿勢検出や姿勢制御・バーチャルリアリティー等に使用されるヘッドマウントディスプレーのうち、頭の姿勢角度を検出するトラッカー、３Ｄゲームパッド等、３次元空間中で被測定対象物の姿勢角度（３軸の回転角度）を検出するのに特に適した姿勢角検出装置に関する。
【０００２】
【従来の技術】
従来、バーチャルリアリティーに使用される、頭の動きを検出する姿勢角検出方法として、外部に設置された交流磁場発生源から微弱交流磁場を発生し、ヘッドマウントディスプレイ（以下、ＨＭＤという）に配置したセンサ部で検知し、制御演算部で頭の動きを検出する交流磁場を利用する方法や、外部に設置した超音波発生源からの超音波信号をＨＭＤに配置したセンサ部で検知し、制御演算部で頭の動きを検出する超音波を利用する方法がある。
【０００３】
【発明が解決しようとする課題】
しかしながら、前者の交流磁場を利用した方法では、信号発生源が微弱交流磁場のため、ノイズをキャンセルする為にフィルタを多用することにより応答性が低下し、頭の動きに比べて、ＨＭＤの映像の動きが遅くなり、気分を悪くしたり、酔いを発生する場合がある。
【０００４】
また、後者の超音波を利用した方法では、他の様々な超音波信号の影響を受けての誤動作や、信号発生源とセンサとの間の遮蔽物により信号を検出できなくなる可能性があり、センサの前に腕や髪の毛があるだけで受信不能になるので、このための誤動作を防止するための対策を必要とする等の問題があった。
【０００５】
本発明は、上記問題点を解消し、使用すると場所や環境の制限を緩和し、幅広く使用できる移動体の姿勢制御・検出に用いる姿勢角検出装置を提供するものである。
【０００６】
【課題を解決するための手段】
前記課題を解決するため、本発明に係る姿勢角検出装置は、互いに直交する３軸の回りの角速度を検出する第１、第２及び第３のジャイロスコープと、互いに直交する２軸の加速度を検出する第１及び第２の加速度センサと、互いに直交する２軸の地磁気を検出する第１及び第２の地磁気センサを配置し、前記ジャイロスコープの出力する角速度に基づいて運動角を検出する運動角検出手段と、加速度及び地磁気に基づいて姿勢角を検出する静止角検出手段と、静止角検出手段の検出結果が正しいかどうかを判別する判別装置と、この判別装置の判別結果と、運動角検出手段と静止角検出手段の演算結果を用いて、出力すべき姿勢角度を演算する姿勢角演算装置を備え、外部の信号発生源を使用することなく移動体の姿勢角度を検出できるようにしたことを特徴とする。
【０００７】
即ち、本発明は、水平面内で直交する軸をＸ軸、Ｙ軸とし、そのＸ軸、Ｙ軸に直交する軸をＺ軸とするとき、これら互いに直交するＸ軸、Ｙ軸、Ｚ軸が形成する基準座標系において、センサが搭載された被測定物の前記互いに直交する３軸の回りの回転角を検出する姿勢角検出装置において、互いに直交するｘ軸、ｙ軸、ｚ軸が形成するセンサ座標系のｘ軸、ｙ軸及びｚ軸の回りの角速度を検出する３個のジャイロスコープと、該ジャイロスコープの角速度に応じた出力に基づいて単位時間に移動した角度を演算する運動角演算装置と、ｘ−ｙ平面で互いに直交する２軸の加速度を検出するように配置された加速度センサと、ｘ−ｙ平面で互いに直交する２軸の地磁気を検出するように配置される地磁気センサと、該加速度センサ及び地磁気センサの出力に基づいて、Ｘ軸、Ｙ軸、Ｚ軸回りの回転角を演算する静止角演算装置と、静止角演算装置による演算結果の真偽を判別する判別装置と、該判別装置の演算結果に応じて、運動角演算装置の演算結果と静止角演算装置の演算結果から出力するべき姿勢角を演算する姿勢角演算装置とから構成される姿勢角検出装置である。
【０００８】
また、本発明は、上記姿勢角検出装置のジャイロスコープが、圧電振動ジャイロ用振動子と、その振動子をジャイロスコープとして動作させるための駆動検出回路からなり、該振動子が、非磁性材料で構成される姿勢角検出装置である。
【０００９】
また、本発明は、上記の姿勢角検出装置のジァイロスコープの出力部に、ハイパスフィルタが接続され、ハイパスフィルタの出力部は演算装置に入力され、ハイパスフィルタの出力を用いて運動角を演算する姿勢角検出装置である。
【００１０】
また、本発明は、上記ハイパスフィルタが、カットオフ周波数可変手段を有する姿勢角検出装置である。
【００１１】
また、本発明は、上記姿勢角検出装置において、２軸の加速度センサと、加速度センサの出力Ａｘ（ｎ）、Ａｙ（ｎ）から仮のロール角Ｒ（ｎ）、仮のピッチ角Ｐ（ｎ）を演算するロール角演算手段、ピッチ角演算手段と、地磁気のｘ方向成分Ｍｘ（ｎ）、ｙ方向成分Ｍｙ（ｎ）を計測する２軸の地磁気センサと、演算する単位時間で１単位時間過去の方位角演算結果Φ（ｎ−１）を記憶しておくためのバックアップメモリーと、加速度センサの出力Ａｘ（ｎ）、Ａｙ（ｎ）及びバックアップメモリーの内容Φ（ｎ−１）より、傾斜角度の北方向成分θ2 を演算する北方向成分傾斜角演算手段と、地磁気のＺ方向成分Ｍｚの符号を判定する地磁気Ｍｚ符号判別手段と、地磁気のＺ方向成分地磁気Ｍｚ絶対値演算手段と、前記地磁気Ｍｚ符号判別手段と、地磁気Ｍｚ絶対値演算手段よりＺ軸方向成分の地磁気Ｍｚを計算する地磁気Ｍｚ演算手段と、前記地磁気Ｍｘ、Ｍｙ、Ｍｚより、基準座標系での地磁気のＸ，Ｙ，Ｚ成分ＨＸ，ＨＹ，ＨＺを演算する座変換演算手段と、基準座標系における前記のＨＸ、ＨＹにより方位角Φを演算する方位演算手段から構成され、地磁気Ｍｚ演算手段により仮想的に地磁気Ｍｚを求めることにより地磁気センサ２軸により方位角Φ（ｎ）を計測する姿勢角検出装置である。
【００１２】
また、本発明は、上記姿勢角検出装置において、前記北方向成分傾斜角演算装置及び地磁気Ｍｚ符号判別手段により、北方向成分傾斜角θ2、原点時の地磁気の伏角θ0、バックアップメモリの出力をΦ（ｎ−１）、加速度センサ２軸の出力をそれぞれＡｘ（ｎ）、Ａｙ（ｎ）とする時、北方向成分傾斜角θ2は、θ2＝ｓｉｎ-1［Ａｘ（ｎ）・ｃｏｓ（−Φ（ｎ−１））＋Ａｙ（ｎ）・ｓｉｎ（−Φ（ｎ−１））］として求め、−９０度≦θ2≦θ0であれば、地磁気Ｍｚ符号はマイナス、θ0 ＜θ2 ≦９０度であれば、地磁気Ｍｚ符号はプラスと判別する姿勢角検出装置である。
【００１３】
また、本発明は、上記姿勢角検出装置において、前記判別装置により、２軸の加速度センサおよび、２軸の地磁気センサからなる静止角検出手段から求められる仮のヨー角Φ、仮のピッチ角Ｐ、仮のロール角Ｒの検出結果が、正しいかどうかの判別手段として、加速度センサの現在の出力Ａｘ（ｎ）から演算して得られた仮のロール角をＲ（ｎ）、演算する単位時間で１単位時間過去の出力Ａｘ（ｎ−１）から得られた仮のロール角演算結果をＲ（ｎ−１）とし、同様に加速度センサの現在の出力Ａｙ（ｎ）から演算して得られた仮のピッチ角をＰ（ｎ）、演算する単位時間で１単位時間過去の出力Ａｙ（ｎ−１）から得られた仮のピッチ角をＰ（ｎ−１）とすると、数１あるいは数２のうち、少なくとも一つの値が、ある値以内にあるとき、静止角検出手段からの姿勢角情報は正しいと判別する姿勢角検出装置である。
【００１４】
【数１】
【００１５】
【数２】
【００１６】
また、本発明は、上記姿勢角検出装置において、求めるべき出力をロール角α（ｎ）、ピッチ角β（ｎ）、ヨー角γ（ｎ）、運動角検出手段から求められる運動角を、仮のロール角変化△Ｘ（ｎ）、仮のピッチ角変化△Ｙ（ｎ）、仮のヨー角変化△Ｚ（ｎ）、静止角検出手段から求められる静止角を仮のロール角Ｒ（ｎ）、仮のピッチ角Ｐ（ｎ）、仮のヨー角Φ（ｎ）、誤差より大きくない一定定数をＣ1，Ｃ2，Ｃ3とすると、判別装置の判別結果により、静止角検出手段からの姿勢角情報は正しいと判別する場合、α（ｎ）＝α（ｎ−１）＋△Ｘ（ｎ）−Ｃ1、β（ｎ）＝β（ｎ−１）＋△Ｙ（ｎ）−Ｃ2、γ（ｎ）＝γ（ｎ−１）＋△Ｚ（ｎ）−Ｃ3により、また、静止角検出手段からの姿勢角情報は誤りと判別する場合は、α（ｎ）＝α（ｎ−１）＋△Ｘ（ｎ）、β（ｎ）＝β（ｎ−１）＋△Ｙ（ｎ）、γ（ｎ）＝γ（ｎ−１）＋△Ｚ（ｎ）により、姿勢角を演算する姿勢角検出装置である。
【００１７】
また、本発明は、上記姿勢角検出装置において、求めるべき出力をロール角α（ｎ）、ピッチ角β（ｎ）、ヨー角γ（ｎ）、動的な運動角検出手段から求められる運動角を、仮のロール角変化△Ｘ（ｎ）、仮のピッチ角変化△Ｙ（ｎ）、仮のヨー角変化△Ｚ（ｎ）、静的な静止角検出手段から求められる姿勢角を仮のロール角Ｒ（ｎ）、仮のピッチ角Ｐ（ｎ）、仮のヨー角Φ（ｎ）、１以下の比例定数をｋ1,ｋ2,ｋ3とすると、判別装置の判別結果により、静止角検出手段からの姿勢角情報は正しいと判別する場合は、α（ｎ）＝α（ｎ−１）＋△Ｘ（ｎ）−ｋ1［α（ｎ−１）＋△Ｘ（ｎ）−Ｒ（ｎ）］、β（ｎ）＝β（ｎ−１）＋△Ｙ（ｎ）−ｋ2［β（ｎ−１）＋△Ｙ（ｎ）−Ｐ（ｎ）］、 γ（ｎ）＝γ（ｎ−１）＋△Ｚ（ｎ）−ｋ3［γ（ｎ−１）＋△Ｚ（ｎ）−Φ（ｎ）］により、また、静止角検出手段からの姿勢角情報は誤りと判別する場合は、α（ｎ）＝α（ｎ−１）＋△Ｘ（ｎ）、β（ｎ）＝β（ｎ−１）＋△Ｙ（ｎ）、γ（ｎ）＝γ（ｎ−１）＋△Ｚ（ｎ）により、姿勢角を演算する姿勢角検出装置である。
【００１８】
【発明の実施の形態】
以下、図面によって本発明の実施の形態について説明する。
【００１９】
ここで説明するセンサ座標系から基準座標系への座標変換演算は、ヨー角γ→ピッチ角α→ロール角βの順に行うものとして説明する。
【００２０】
ここで、センサ座標系でのセンサ出力として、次の記号を用いる。
ジャイロスコープの出力：Ｊｘ（ｎ）、Ｊｙ（ｎ）、Ｊｚ（ｎ）。
加速度センサの出力：Ａｘ（ｎ）、Ａｙ（ｎ）。
地磁気センサの出力：Ｍｘ（ｎ）、Ｍｙ（ｎ）。
地磁気センサの計算値：Ｍｚ（ｎ）。
【００２１】
また、上記センサ座標系でのセンサ出力を基準座標系に座標変換して得た値として、次の記号を用いる。
ジャイロスコープの出力から得た基準座標系Ｘ、Ｙ、Ｚ軸回りの運動角：△Ｘ（ｎ）、△Ｙ（ｎ）、△Ｚ（ｎ）。
加速度センサの出力から得た基準座標系Ｘ軸回りの回転角（仮のロール角）：Ｒ（ｎ）、同じくＹ軸回りの回転角（仮のピッチ角）：Ｐ（ｎ）。
地磁気センサの出力から得たＺ軸回りの回転角（方位角或いは仮のヨー角）：Φ（ｎ）。
上記演算結果より求める最終的に出力するべき基準座標系Ｘ、Ｙ、Ｚ軸回りの回転角度ロール角α（ｎ）、ピッチ角β（ｎ）、ヨー角γ（ｎ）とする。
【００２２】
図１には、基準座標系（Ｘ−Ｙ−Ｚ系）とセンサ座標系（ｘ−ｙ−ｚ系）が一致している場合について示す。図１に示すように、水平面で直交する軸をＸ軸、Ｙ軸とし、そのＸ軸、Ｙ軸それぞれに直交する軸をＺ軸とするとき、これら互いに直交するＸ軸、Ｙ軸、Ｚ軸が形成する基準座標系において、Ｘ軸回りの回転角度をロール角α、Ｙ軸回りの回転角度をピッチ角β、Ｚ軸回りの回転角度をヨー角γと記す。
【００２３】
図１には、本発明の姿勢角検出装置におけるジャイロスコープ、加速度センサ、地磁気センサの配置を示す。図１で示すセンサ座標系において、互いに直交する３軸（ｘ軸、ｙ軸及びｚ軸）の回りの角速度を検出するための第１のジャイロスコープ５、第２のジャイロスコープ６、及び第３のジャイロスコープ７は、ｘ、ｙ、ｚ軸各軸に平行に、即ち互いに直交して、配置されている。第１の加速度センサ１０と第２の加速度センサ１１は、互いに直交する２軸、ｘ軸及びｙ軸に平行に配置されている。同様に、ヨー角を検出するための第１の地磁気センサ８と第２の地磁気センサ９は、互いに直交する２軸、ｘ軸及びｙ軸に平行に配置されている。
【００２４】
図２には、基準座標系（Ｘ−Ｙ−Ｚ系）に対して、センサ座標系（ｘ−ｙ−ｚ系）が、ずれた場合について示す。センサ座標系に記載されるべき、ジャイロスコープ、加速度センサ、地磁気センサは、省略してある。図２では、基準座標系の−Ｚ方向に重力加速度が働いており、センサ座標系のｘ軸、ｙ軸方向への分力が、Ａｘ（ｎ）、Ａｙ（ｎ）であることを示している。
【００２５】
図３は、本発明の一実施の形態における姿勢角検出装置の全体構成を示す図である。図３に示すように、本発明の姿勢角度検出装置は、図１に示すように、直交配置された３個のジャイロスコープと、そのジャイロスコープそれぞれに接続されるハイパスフィルタと、ハイパスフィルタを介したジャイロスコープの出力から単位時間あたりの移動角度（運動角）を演算する運動角演算装置３１と、図１に示すように配置された２個の加速度センサと、２個の地磁気センサと、前記２個の加速度センサから仮のロール角Ｒ、仮のピッチ角Ｐを求め、さらにその仮のロール角Ｒ、仮のピッチ角Ｐから仮のヨー角Φを求める静止角演算装置３２と、静止角演算装置３２の演算結果の真偽を判定するための判別装置３３と、運動角演算装置３１、静止角演算装置３２及び判別装置３３からの信号を処理する姿勢角演算装置３０とで構成されている。
【００２６】
最初に、運動角検出装置３１の内容を説明する。本発明の姿勢角検出装置における運動角検出装置は、圧電セラミック、圧電単結晶或いはシリコン等の非磁性材料からなる振動子を用いることが可能である。本実施例では、（株）トーキン製圧電振動セラミックジャイロを用いた場合について説明する。
【００２７】
図３に示すように、３軸のジャイロスコープ５，６，７の出力部には、それぞれ、ハイパスフィルタ３６、３７、３８が接続され、ハイパスフィルタの出力部に運動角演算装置３１が接続されている。ジャイロスコープは、小型で安価な物ほど静止しているときの出力（オフセット）のばらつき及び変動が大きいが、安定したものを用いると、姿勢検出装置が高価で大型になってしまう。
【００２８】
前記ハイパスフィルタは、前記圧電振動ジャイロのオフセットをキャンセルするもので、ハイパスフィルタを用いた後の出力を用いることで、安価で小型で高精度の姿勢角検出装置を得ることができるもので、通常０.１Ｈｚ以下の低い周波数に設定される。また、それぞれのハイパスフィルタは、カットオフ周波数可変を有している。前記低周波のハイパスフィルタは、電源投入時から安定するまで時間がかかるので、必要に応じてハイパスフィルタのカットオフ周波数を高くすることにより、安定した出力を短時間で得ることができる。カットオフ周波数を高くし安定出力を得た後は、即時にカットオフ周波数は低い状態（測定状態）に戻される。
【００２９】
運動角演算装置３１によって、姿勢角検出装置（被測定物）の回転角速度に応じた各ジャイロスコープの出力Ｊｘ（ｎ）、Ｊｙ（ｎ）、Ｊｚ（ｎ）は、座標変換を行って、演算する単位時間で１単位時間過去の出力β（ｎ−１）とから、次式の如く、基準座標系における現在の移動角度（運動角）△Ｘ（ｎ）に変換される。
【００３０】
0003763435.tif 000002
【００３１】
次に、図４の静止角演算装置の説明を行う。静止角演算装置は、先にも述べたように、姿勢角検出装置の静的動作時の姿勢角である仮のロール角Ｒ、仮のピッチ角Ｐ、仮のヨー角Φを出力するものである。
【００３２】
図１に示す加速度センサ１０、１１は、図２に示すように、それぞれ、センサ座標系でのｘ、ｙ軸上の重力加速度のｘ、ｙ成分分力Ａｘ（ｎ）、Ａｙ（ｎ）を検出するように配置されている。また、図１に示す地磁気センサ８、９は、それぞれｘ、ｙ軸上の地磁気分力Ｍｘ（ｎ）、Ｍｙ（ｎ）を検出するように配置されている。
【００３３】
加速度センサの出力Ａｘ（ｎ）、Ａｙ（ｎ）は、各々ロール角演算手段、ピッチ角演算手段にて下式により、基準座標系の水平面（ＸＹ平面）との傾斜角である仮のロール角Ｒ（ｎ）及び仮のピッチ角Ｐ（ｎ）を算出する。
【００３４】
Ｒ（ｎ）＝ｓｉｎ-1［Ａｘ（ｎ）／ｃｏｓＰ（ｎ）］
Ｐ（ｎ）＝ｓｉｎ-1Aｙ（ｎ）
【００３５】
基準座標系での仮のヨー角Φ（ｎ）は、基準座標系での地磁気成分を用いて、
Φ（ｎ）＝ｔａｎー1［ＨＸ（ｎ）／ＨＹ（ｎ）］
で表わされる。姿勢角検出装置が水平面上で回転する場合は、地磁気センサの出力Ｍｘ(n)及びＭｙ(n)が、それぞれ、ＨＸ（ｎ）及びＨＹ（ｎ）に等しく、
Φ（ｎ）＝ｔａｎー1［Ｍｘ（ｎ）／Ｍｙ（ｎ）］
が成り立つ。
【００３６】
原点時の方位角をΦ（０）とすれば、ヨー角γ（ｎ）は、
γ（ｎ）＝Φ（ｎ）−Φ（０）
となる。
【００３７】
しかし、姿勢角検出装置がロール角Ｒやピッチ角Ｐを持ち、水平でないときには、
ＨＸ（ｎ）＝ｃｏｓ［Ｐ（ｎ）］×Ｍｘ（ｎ）＋ｓｉｎ［Ｐ（ｎ）］×Ｍｚ（ｎ）、
ＨＹ（ｎ）＝ｓｉｎ［Ｒ（ｎ）］×ｓｉｎ［Ｐ（ｎ）］×Ｍｘ（ｎ）  ＋ｃｏｓ［Ｒ（ｎ）］×ｃｏｓ［Ｐ（ｎ）］×Ｍｚ（ｎ）
の関係式から、仮のロール角Ｒ（ｎ）、仮のピッチ角Ｐ（ｎ）を用いて座標変換を行い、基準座標系での地磁気成分を求めて、
Φ（ｎ）＝ｔａｎ−１［ＨＸ（ｎ）／ＨＹ（ｎ）］
に代入し、求める。
【００３８】
その際、地磁気Ｚ方向成分Ｍｚ（ｎ）が必要となる。
【００３９】
以下の方法で仮想的にＭｚを算出する。
【００４０】
まず、地磁気Ｈｔを以下の手順で求める。最初に、姿勢角検出装置（被測定物）部を９０度傾け、図１の磁気センサ１０あるいは磁気センサ１１が基準座標系のＺ軸に向くようにする。そして何らかの信号を送り、例えばキーやボタンを押すなどをし、その時のＺ軸に向いているセンサの値をＭｚ（０）としてメモリに格納する。
【００４１】
次に、姿勢角検出装置（被測定物）部を原点位置に戻し、もう一度、何らかの信号を送り、磁気センサＭｘ（ｎ）,Ｍｙ（ｎ）の値をＭｘ（０），Ｍｙ（０）としてメモリに格納する。これらＭｘ（０）,Ｍｙ（０）,Ｍｚ（０）を用いて、Ｈｔは数３から求まる。
【００４２】
【数３】
0003763435.tif 000003
【００４３】
さらに、姿勢角検出装置（被測定物）の地磁気北方向成分の傾斜角からＭｚの符号を判別する。このために、バックアップメモリーと北方向成分傾斜角演算手段を有する。
【００４４】
演算処理にて算出した、演算する単位時間で、１単位時間過去の仮のヨー角Φ(n-1)をバックアップメモリーに記憶しておき、この仮のヨー角Φ（ｎ−１）と加速度センサ出力Ａｘ（ｎ）、Ａｙ（ｎ）の値から、北方向成分傾斜角演算手段で、姿勢角検出装置（被測定物）の傾斜角度の地磁気北方向成分θ2を次式により算出する。
【００４５】
θ2＝sin-1［Ａｘ（ｎ）×cos（−Φ（ｎ−１））＋Ａｙ（ｎ）×sin（−Φ（ｎ−１））］
【００４６】
により求められ、−９０度≦θ2≦θ0であれば、地磁気Ｍｚ符号はマイナス、θ0＜θ2≦９０度であれば、地磁気Ｍｚ符号はプラスと判別する。
【００４７】
次に、本発明の姿勢角検出装置における、前記静止角検出装置で演算された仮のロール角Ｒ（ｎ）、仮のピッチ角Ｐ（ｎ）、仮のヨー角Φ（ｎ）がほぼ正しいかどうかを判別する判別装置を説明をする。
【００４８】
静止角検出手段における、Ａｘ（ｎ）、Ａｙ（ｎ）は、重力加速度のｘ，ｙ軸方向成分と運動加速度のｘ、ｙ軸方向成分との合成ベクトルとなるために、運動加速度がある場合には正しいロール角、ピッチ角にならない。そこで、Ｒ（ｎ）からＲ（ｎ−１）を引いたものが、０の近傍で、ある値以内であり、あるいはＰ（ｎ）からＰ（ｎ−１）を引いたものが、０の近傍で、ある値以内であるならば、運動加速度がない、即ち静止角検出手段から演算された仮のロール角Ｒ（ｎ）、と仮のピッチ角Ｐ（ｎ）が正しいと判別する。そうでない場合は、運動加速度があると判別し、静止角検出手段からの演算結果は正しくないとする。即ち、数１或いは数２のうち、少なくとも一つの値が、ある値以内で、ほぼ０であるとき、静止角検出手段からの姿勢角情報は正しいと判別する判別装置を提供する。
【００４９】
【数１】
【００５０】
【数２】
【００５１】
ここでは、加速度センサの出力を演算して判別する手法について述べたが、磁気センサの出力に誤差が生じ、大きく変動したとき等の場合も静止角度検出手段の出力は正しくないと判別する。
【００５２】
次に、上記判別装置の判別結果に応じて、運動角検出手段の出力と静止角検出手段の出力を演算して最終的な出力を得る姿勢角演算装置で行われる演算方法について説明する。
【００５３】
この演算の基本は、判別装置によって静止角検出手段の出力が誤っていると判別されたときには、現在の姿勢角検出結果＝１単位時間過去の姿勢角検出結果＋運動角となり、判別装置によって静止角検出手段の出力が正しいと判別されたときには、現在の姿勢角検出結果＝１単位過去の姿勢角検出結果＋運動角−補正値とし、補正値は、誤差×比例定数、或いは一定定数である。ここで、誤差＝（１単位時間過去の姿勢角検出結果＋現在の運動角）−（静止角検出手段による現在の検出結果）である。
【００５４】
式で表すと、Ｘ軸の誤差＝α（ｎ−１）＋△Ｘ（ｎ）−Ｒ（ｎ），Ｙ軸の誤差＝β（ｎ−１）＋△Ｙ（ｎ）−Ｐ（ｎ），Ｚ軸の誤差＝γ（ｎ−１）＋△Ｚ（ｎ）−Φ（ｎ）となる。
【００５５】
第１の実施例では、本発明の姿勢角検出装置の求めるべき出力をα（ｎ）、β（ｎ）、γ（ｎ）、動的な運動角検出手段から得られる運動角を△Ｘ（ｎ）、△Ｙ（ｎ）、△Ｚ（ｎ）、静的な静止角検出手段から求められる静止角をＲ（ｎ）、Ｐ（ｎ）、Φ（ｎ）とすると、
【００５６】
静止角検出手段からの姿勢角情報は正しいと判別する場合、
α（ｎ）＝α（ｎ−１）＋△Ｘ（ｎ）−Ｃ1、
β（ｎ）＝β（ｎ−１）＋△Ｙ（ｎ）−Ｃ2、
γ（ｎ）＝γ（ｎ−１）＋△Ｚ（ｎ）−Ｃ3、
ここで、Ｃ1,Ｃ2,Ｃ3は、任意に選択される誤差より大きくない定数で、符号は、誤差の符号が、プラスの時、プラス、マイナスの時、マイナスにより姿勢角を演算する。
【００５７】
また、静止角検出手段からの姿勢角情報が誤と判別する場合、
α（ｎ）＝α（ｎ−１）＋△Ｘ（ｎ）、
β（ｎ）＝β（ｎ−１）＋△Ｙ（ｎ）、
γ（ｎ）＝γ（ｎ−１）＋△Ｚ（ｎ）、
により姿勢角を演算する。
【００５８】
また、本発明の他の実施の形態として、本発明の姿勢角検出装置の求めるべき出力をα（ｎ）、β（ｎ）、γ（ｎ）、動的な運動角検出手段から得られる運動角を△Ｘ（ｎ）、△Ｙ（ｎ）、△Ｚ（ｎ）、静的な静止角検出手段から求められる静止角をＲ（ｎ）、Ｐ（ｎ）、Φ（ｎ）とすると、
【００５９】
静止角検出手段からの姿勢角情報は正しいと判別する場合、
α(ｎ)＝α(ｎ−１)＋△Ｘ(ｎ)−ｋ1［α(ｎ−１)＋△Ｘ(ｎ)−Ｒ(ｎ)］、
β(ｎ)＝β(ｎ−１)＋△Ｙ(ｎ)−ｋ2［β(ｎ−１)＋△Ｙ(ｎ)−Ｐ(ｎ)］、
γ(ｎ)＝γ(ｎ−１)＋△Ｚ(ｎ)−ｋ3［γ(ｎ−１)＋△Ｚ(ｎ)−Φ(ｎ)］、
により姿勢角を演算する。ここで、ｋ1，ｋ2，ｋ3は、任意に選択される１以下の比例定数である。
【００６０】
また、静止角検出手段からの姿勢角情報が誤と判別する場合、
α（ｎ）＝α（ｎ−１）＋△Ｘ（ｎ）、
β（ｎ）＝β（ｎ−１）＋△Ｙ（ｎ）、
γ（ｎ）＝γ（ｎ−１）＋△Ｚ（ｎ）、
により姿勢角を演算する。
【００６１】
このように、静止角検出手段の出力が正しいときのみ、誤差を補正しながら、運動角の積算で姿勢角度を求めることにより、高速な応答性と、静止時の安定性、再現性を同時に満足できるようにした姿勢角検出装置である。
【００６２】
図５は、姿勢角検出装置を適用したＨＭＤの一例示す。このＨＭＤは、目前にあるディスプレイに表示に表示された映像が頭の動きに連動して変化し、仮想空間を体験できるように設けられているもので、このＨＭＤを装着したまま右を向くと、姿勢角検出装置で計測した頭の姿勢角を映像発生装置に送信し、映像発生装置はその姿勢角にあった映像をＨＭＤに送信するので右に展開する映像が映し出され、頭の動きによって全空間の３６０度の映像を実感できるように設けられているものである。
【００６３】
図５に示すように、符号１は、ＨＭＤ本体を示し、このＨＭＤ本体１には、本発明である姿勢角検出装置（被測定物）２が信号ケーブル３で映像発生装置４に接続されている。
【００６４】
【発明の効果】
本発明による姿勢角検出装置によれば、上記のＨＭＤに適用した場合、外部信号を用いることなく、また、高速応答で累積誤差の少ない角度情報を得ることができ、高性能なＨＭＤを構成することができる。
【００６５】
本発明による姿勢角検出装置によれば、ジャイロを素子で搭載しているので、小型軽量化が可能であり、セラミック振動子を使用しているので、磁気ノイズは受けず、磁気センサと近接させても機能を損なうことがない。また、ハイパスフィルタを使用することにより、小型・低価格化、高精度化が可能である。
【００６６】
さらに、従来、姿勢角検出装置としては、加速度センサが３個ないと姿勢角の誤差が判断できず、そのため、複雑な構造になっていたが、本発明によれば、２個の加速度センサと、２個の地磁気センサと、簡単な計算だけで、誤差成分有無の判別・補正処理を行うことができ、高速応答で、高精度な姿勢角検出装置を提供できる。
【図面の簡単な説明】
【図１】本発明の姿勢角検出装置のジャイロスコープ、加速度センサ、地磁気センサの配置を示し、基準座標系（Ｘ−Ｙ−Ｚ系）とセンサ座標系（ｘ−ｙ−ｚ系）が一致している場合を示す図。
【図２】基準座標系（Ｘ−Ｙ−Ｚ系）とセンサ座標系（ｘ−ｙ−ｚ系）が一致していない場合を示す図。
【図３】本発明の姿勢角検出装置の一実施の形態の全体構成を示す図。
【図４】本発明の姿勢角検出装置のうち、静止角演算装置の構成を示すブロック図。
【図５】ＨＭＤにおける姿勢角検出装置の使用状況の説明図。
【符号の説明】
１    ディスプレイ本体
２    姿勢角検出装置（被測定物）
３    信号ケーブル
４    映像発生装置
５    （第１の）ジャイロスコープ
６    （第２の）ジャイロスコープ
７    （第３の）ジャイロスコープ
８    第１の地磁気センサ
９    第２の地磁気センサ
１０    第１の加速度センサ
１１    第２の加速度センサ
３１    運動角演算装置
３２    静止角演算装置
３３    判別装置
３０    姿勢角演算装置
３６    （第１の）ハイパスフィルタ
３７    （第２の）ハイパスフィルタ
３８    （第３の）ハイパスフィルタ
【図１】
0003763435.tif 000006
【図２】
0003763435.tif 000007
【図３】
0003763435.tif 000008
【図４】
0003763435.tif 000009
【図５】
0003763435.tif 000010