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