JPH11211474A - 姿勢角検出装置 - Google Patents

Abstract

(57)【要約】 【課題】 使用する場所や環境の制限を緩和し、幅広く
使用できる移動体の姿勢制御・検出に用いる姿勢角検出
装置を提供すること。 【解決手段】 ３個のジャイロスコープ５，６，７と、
２個の加速度センサ１０，１１と、２個の地磁気センサ
８，９を配置し、前記３個のジャイロスコープが出力す
る角速度に基づいて運動角検出する運動角演算装置３１
と、加速度及び地磁気に基づいて姿勢角を検出する静止
角演算装置３２と、静止角検出結果の正誤を判別する判
別装置３３と、この判別装置の演算結果に応じて運動角
検演算装置と静止角演算装置の演算結果を用いて出力す
べき姿勢角度を演算する姿勢角演算装置３０を備え、外
部の信号発生源を使用することなく移動体の姿勢角度を
検出する。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、移動体の姿勢検出
や姿勢制御・バーチャルリアリティー等に使用されるヘ
ッドマウントディスプレーのうち、頭の姿勢角度を検出
するトラッカー、３Ｄゲームパッド等、３次元空間中で
被測定対象物の姿勢角度（３軸の回転角度）を検出する
のに特に適した姿勢角検出装置に関する。

【０００２】

【従来の技術】従来、バーチャルリアリティーに使用さ
れる、頭の動きを検出する姿勢角検出方法として、外部
に設置された交流磁場発生源から微弱交流磁場を発生
し、ヘッドマウントディスプレイ（以下、ＨＭＤとい
う）に配置したセンサ部で検知し、制御演算部で頭の動
きを検出する交流磁場を利用する方法や、外部に設置し
た超音波発生源からの超音波信号をＨＭＤに配置したセ
ンサ部で検知し、制御演算部で頭の動きを検出する超音
波を利用する方法がある。

【０００３】

【発明が解決しようとする課題】しかしながら、前者の
交流磁場を利用した方法では、信号発生源が微弱交流磁
場のため、ノイズをキャンセルする為にフィルタを多用
することにより応答性が低下し、頭の動きに比べて、Ｈ
ＭＤの映像の動きが遅くなり、気分を悪くしたり、酔い
を発生する場合がある。

【０００４】また、後者の超音波を利用した方法では、
他の様々な超音波信号の影響を受けての誤動作や、信号
発生源とセンサとの間の遮蔽物により信号を検出できな
くなる可能性があり、センサの前に腕や髪の毛があるだ
けで受信不能になるので、このための誤動作を防止する
ための対策を必要とする等の問題があった。

【０００５】本発明は、上記問題点を解消し、使用する
と場所や環境の制限を緩和し、幅広く使用できる移動体
の姿勢制御・検出に用いる姿勢角検出装置を提供するも
のである。

【０００６】

【課題を解決するための手段】前記課題を解決するた
め、本発明に係る姿勢角検出装置は、互いに直交する３
軸の回りの角速度を検出する第１、第２及び第３のジャ
イロスコープと、互いに直交する２軸の加速度を検出す
る第１及び第２の加速度センサと、互いに直交する２軸
の地磁気を検出する第１及び第２の地磁気センサを配置
し、前記ジャイロスコープの出力する角速度に基づいて
運動角を検出する運動角検出手段と、加速度及び地磁気
に基づいて姿勢角を検出する静止角検出手段と、静止角
検出手段の検出結果が正しいかどうかを判別する判別装
置と、この判別装置の判別結果と、運動角検出手段と静
止角検出手段の演算結果を用いて、出力すべき姿勢角度
を演算する姿勢角演算装置を備え、外部の信号発生源を
使用することなく移動体の姿勢角度を検出できるように
したことを特徴とする。

【０００７】即ち、本発明は、水平面内で直交する軸を
Ｘ軸、Ｙ軸とし、そのＸ軸、Ｙ軸に直交する軸をＺ軸と
するとき、これら互いに直交するＸ軸、Ｙ軸、Ｚ軸が形
成する基準座標系において、互いに直交する３軸の回り
の回転角を検出する姿勢角検出装置において、Ｘ軸、Ｙ
軸及びＺ軸の回りの角速度を検出する３個のジャイロス
コープと、該ジャイロスコープの角速度に応じた出力に
基づいて単位時間に移動した角度を演算する運動角演算
装置と、Ｘ−Ｙ平面で互いに直交する２軸の加速度を検
出するように配置された加速度センサと、Ｘ−Ｙ平面で
互いに直交する２軸の地磁気を検出するように配置され
る地磁気センサと、該加速度センサ及び地磁気センサの
出力に基づいて、Ｘ軸、Ｙ軸、Ｚ軸回りの回転角を演算
する静止角演算装置と、静止角演算装置による演算結果
の真偽を判別する判別装置と、該判別装置の演算結果に
応じて、運動角演算装置の演算結果と静止角演算装置の
演算結果から出力するべき姿勢角を演算する姿勢角演算
装置とから構成される姿勢角検出装置である。

【０００８】また、本発明は、上記姿勢角検出装置のジ
ャイロスコープが、圧電振動ジャイロ用振動子と、その
振動子をジャイロスコープとして動作させるための駆動
検出回路からなり、該振動子が、非磁性材料で構成され
る姿勢角検出装置である。

【０００９】また、本発明は、上記の姿勢角検出装置の
ジァイロスコープの出力部に、ハイパスフィルタが接続
され、ハイパスフィルタの出力部は演算装置に入力さ
れ、ハイパスフィルタの出力を用いて運動角を演算する
姿勢角検出装置である。

【００１０】また、本発明は、上記ハイパスフィルタ
が、カットオフ周波数可変手段を有する姿勢角検出装置
である。

【００１１】また、本発明は、上記姿勢角検出装置にお
いて、２軸の加速度センサと、加速度センサの出力Ａｘ
（ｎ）、Ａｙ（ｎ）から仮のロール角Ｒ（ｎ）、仮のピ
ッチ角Ｐ（ｎ）を演算するロール角演算手段、ピッチ角
演算手段と、地磁気のｘ方向成分Ｍｘ（ｎ）、ｙ方向成
分Ｍｙ（ｎ）を計測する２軸の地磁気センサと、演算す
る単位時間で１単位時間過去の方位角演算結果Φ（ｎ−
１）を記憶しておくためのバックアップメモリーと、加
速度センサの出力Ａｘ（ｎ）、Ａｙ（ｎ）及びバックア
ップメモリーの内容Φ（ｎ−１）より、傾斜角度の北方
向成分θ₂ を演算する北方向成分傾斜角演算手段と、地
磁気のＺ方向成分Ｍｚの符号を判定する地磁気Ｍｚ符号
判別手段と、地磁気のＺ方向成分地磁気Ｍｚ絶対値演算
手段と、前記地磁気Ｍｚ符号判別手段と、地磁気Ｍｚ絶
対値演算手段よりＺ軸方向成分の地磁気Ｍｚを計算する
地磁気Ｍｚ演算手段と、前記地磁気Ｍｘ、Ｍｙ、Ｍｚよ
り、基準座標系での地磁気のＸ，Ｙ，Ｚ成分ＨＸ，Ｈ
Ｙ，ＨＺを演算する座変換演算手段と、基準座標系にお
ける前記のＨＸ、ＨＹにより方位角Φを演算する方位演
算手段から構成され、地磁気Ｍｚ演算手段により仮想的
に地磁気Ｍｚを求めることにより地磁気センサ２軸によ
り方位角Φ（ｎ）を計測する姿勢角検出装置である。

【００１２】また、本発明は、上記姿勢角検出装置にお
いて、前記北方向成分傾斜角演算装置及び地磁気Ｍｚ符
号判別手段により、北方向成分傾斜角θ₂、原点時の地
磁気の伏角θ₀、バックアップメモリの出力をΦ（ｎ−
１）、加速度センサ２軸の出力をそれぞれＡｘ（ｎ）、
Ａｙ（ｎ）とする時、北方向成分傾斜角θ₂は、θ₂＝ｓ
ｉｎ^-1［Ａｘ（ｎ）・ｃｏｓ（−Φ（ｎ−１））＋Ａｙ
（ｎ）・ｓｉｎ（−Φ（ｎ−１））］として求め、−９
０度≦θ₂≦θ₀であれば、地磁気Ｍｚ符号はマイナス、
θ₀ ＜θ₂ ≦９０度であれば、地磁気Ｍｚ符号はプラス
と判別する姿勢角検出装置である。

【００１３】また、本発明は、上記姿勢角検出装置にお
いて、前記判別装置により、２軸の加速度センサおよ
び、２軸の地磁気センサからなる静止角検出手段から求
められる仮のヨー角Φ、仮のピッチ角Ｐ、仮のロール角
Ｒの検出結果が、正しいかどうかの判別手段として、加
速度センサの現在の出力Ａｘ（ｎ）から演算して得られ
た仮のロール角をＲ（ｎ）、演算する単位時間で１単位
時間過去の出力Ａｘ（ｎ−１）から得られた仮のロール
角演算結果をＲ（ｎ−１）とし、同様に加速度センサの
現在の出力Ａｙ（ｎ）から演算して得られた仮のピッチ
角をＰ（ｎ）、演算する単位時間で１単位時間過去の出
力Ａｙ（ｎ−１）から得られた仮のピッチ角をＰ（ｎ−
１）とすると、数１あるいは数２のうち、少なくとも一
つの値が、ある値以内にあるとき、静止角検出手段から
の姿勢角情報は正しいと判別する姿勢角検出装置であ
る。

【００１４】

【数１】

【００１５】

【数２】

【００１６】また、本発明は、上記姿勢角検出装置にお
いて、求めるべき出力をロール角α（ｎ）、ピッチ角β
（ｎ）、ヨー角γ（ｎ）、運動角検出手段から求められ
る運動角を、仮のロール角変化△Ｘ（ｎ）、仮のピッチ
角変化△Ｙ（ｎ）、仮のヨー角変化△Ｚ（ｎ）、静止角
検出手段から求められる静止角を仮のロール角Ｒ
（ｎ）、仮のピッチ角Ｐ（ｎ）、仮のヨー角Φ（ｎ）、
誤差より大きくない一定定数をＣ₁，Ｃ₂，Ｃ₃とする
と、判別装置の判別結果により、静止角検出手段からの
姿勢角情報は正しいと判別する場合、α（ｎ）＝α（ｎ
−１）＋△Ｘ（ｎ）−Ｃ₁、β（ｎ）＝β（ｎ−１）＋
△Ｙ（ｎ）−Ｃ₂、γ（ｎ）＝γ（ｎ−１）＋△Ｚ
（ｎ）−Ｃ₃により、また、静止角検出手段からの姿勢
角情報は誤りと判別する場合は、α（ｎ）＝α（ｎ−
１）＋△Ｘ（ｎ）、β（ｎ）＝β（ｎ−１）＋△Ｙ
（ｎ）、γ（ｎ）＝γ（ｎ−１）＋△Ｚ（ｎ）により、
姿勢角を演算する姿勢角検出装置である。

【００１７】また、本発明は、上記姿勢角検出装置にお
いて、求めるべき出力をロール角α（ｎ）、ピッチ角β
（ｎ）、ヨー角γ（ｎ）、動的な運動角検出手段から求
められる運動角を、仮のロール角変化△Ｘ（ｎ）、仮の
ピッチ角変化△Ｙ（ｎ）、仮のヨー角変化△Ｚ（ｎ）、
静的な静止角検出手段から求められる姿勢角を仮のロー
ル角Ｒ（ｎ）、仮のピッチ角Ｐ（ｎ）、仮のヨー角Φ
（ｎ）、１以下の比例定数をｋ₁,ｋ₂,ｋ₃とすると、判
別装置の判別結果により、静止角検出手段からの姿勢角
情報は正しいと判別する場合は、α（ｎ）＝α（ｎ−
１）＋△Ｘ（ｎ）−ｋ₁［α（ｎ−１）＋△Ｘ（ｎ）−
Ｒ（ｎ）］、β（ｎ）＝β（ｎ−１）＋△Ｙ（ｎ）−ｋ
₂［β（ｎ−１）＋△Ｙ（ｎ）−Ｐ（ｎ）］、 γ（ｎ）
＝γ（ｎ−１）＋△Ｚ（ｎ）−ｋ₃［γ（ｎ−１）＋△
Ｚ（ｎ）−Φ（ｎ）］により、また、静止角検出手段か
らの姿勢角情報は誤りと判別する場合は、α（ｎ）＝α
（ｎ−１）＋△Ｘ（ｎ）、β（ｎ）＝β（ｎ−１）＋△
Ｙ（ｎ）、γ（ｎ）＝γ（ｎ−１）＋△Ｚ（ｎ）によ
り、姿勢角を演算する姿勢角検出装置である。

【００１８】

【発明の実施の形態】以下、図面によって本発明の実施
の形態について説明する。

【００１９】ここで説明するセンサ座標系から基準座標
系への座標変換演算は、ヨー角γ→ピッチ角α→ロール
角βの順に行うものとして説明する。

【００２０】ここで、センサ座標系でのセンサ出力とし
て、次の記号を用いる。 ジャイロスコープの出力：Ｊｘ（ｎ）、Ｊｙ（ｎ）、Ｊ
ｚ（ｎ）。 加速度センサの出力：Ａｘ（ｎ）、Ａｙ（ｎ）。 地磁気センサの出力：Ｍｘ（ｎ）、Ｍｙ（ｎ）。 地磁気センサの計算値：Ｍｚ（ｎ）。

【００２１】また、上記センサ座標系でのセンサ出力を
基準座標系に座標変換して得た値として、次の記号を用
いる。 ジャイロスコープの出力から得た基準座標系Ｘ、Ｙ、Ｚ
軸回りの運動角：△Ｘ（ｎ）、△Ｙ（ｎ）、△Ｚ
（ｎ）。 加速度センサの出力から得た基準座標系Ｘ軸回りの回転
角（仮のロール角）：Ｒ（ｎ）、同じくＹ軸回りの回転
角（仮のピッチ角）：Ｐ（ｎ）。 地磁気センサの出力から得たＺ軸回りの回転角（方位角
或いは仮のヨー角）：Φ（ｎ）。 上記演算結果より求める最終的に出力するべき基準座標
系Ｘ、Ｙ、Ｚ軸回りの回転角度ロール角α（ｎ）、ピッ
チ角β（ｎ）、ヨー角γ（ｎ）とする。

【００２２】図１には、基準座標系（Ｘ−Ｙ−Ｚ系）と
センサ座標系（ｘ−ｙ−ｚ系）が一致している場合につ
いて示す。図１に示すように、水平面で直交する軸をＸ
軸、Ｙ軸とし、そのＸ軸、Ｙ軸それぞれに直交する軸を
Ｚ軸とするとき、これら互いに直交するＸ軸、Ｙ軸、Ｚ
軸が形成する基準座標系において、Ｘ軸回りの回転角度
をロール角α、Ｙ軸回りの回転角度をピッチ角β、Ｚ軸
回りの回転角度をヨー角γと記す。

【００２３】図１には、本発明の姿勢角検出装置におけ
るジャイロスコープ、加速度センサ、地磁気センサの配
置を示す。図１で示すセンサ座標系において、互いに直
交する３軸（ｘ軸、ｙ軸及びｚ軸）の回りの角速度を検
出するための第１のジャイロスコープ５、第２のジャイ
ロスコープ６、及び第３のジャイロスコープ７は、ｘ、
ｙ、ｚ軸各軸に平行に、即ち互いに直交して、配置され
ている。第１の加速度センサ１０と第２の加速度センサ
１１は、互いに直交する２軸、ｘ軸及びｙ軸に平行に配
置されている。同様に、ヨー角を検出するための第１の
地磁気センサ８と第２の地磁気センサ９は、互いに直交
する２軸、ｘ軸及びｙ軸に平行に配置されている。

【００２４】図２には、基準座標系（Ｘ−Ｙ−Ｚ系）に
対して、センサ座標系（ｘ−ｙ−ｚ系）が、ずれた場合
について示す。センサ座標系に記載されるべき、ジャイ
ロスコープ、加速度センサ、地磁気センサは、省略して
ある。図２では、基準座標系の−Ｚ方向に重力加速度が
働いており、センサ座標系のｘ軸、ｙ軸方向への分力
が、Ａｘ（ｎ）、Ａｙ（ｎ）であることを示している。

【００２５】図３は、本発明の一実施の形態における姿
勢角検出装置の全体構成を示す図である。図３に示すよ
うに、本発明の姿勢角度検出装置は、図１に示すよう
に、直交配置された３個のジャイロスコープと、そのジ
ャイロスコープそれぞれに接続されるハイパスフィルタ
と、ハイパスフィルタを介したジャイロスコープの出力
から単位時間あたりの移動角度（運動角）を演算する運
動角演算装置３１と、図１に示すように配置された２個
の加速度センサと、２個の地磁気センサと、前記２個の
加速度センサから仮のロール角Ｒ、仮のピッチ角Ｐを求
め、さらにその仮のロール角Ｒ、仮のピッチ角Ｐから仮
のヨー角Φを求める静止角演算装置３２と、静止角演算
装置３２の演算結果の真偽を判定するための判別装置３
３と、運動角演算装置３１、静止角演算装置３２及び判
別装置３３からの信号を処理する姿勢角演算装置３０と
で構成されている。

【００２６】最初に、運動角検出装置３１の内容を説明
する。本発明の姿勢角検出装置における運動角検出装置
は、圧電セラミック、圧電単結晶或いはシリコン等の非
磁性材料からなる振動子を用いることが可能である。本
実施例では、（株）トーキン製圧電振動セラミックジャ
イロを用いた場合について説明する。

【００２７】図３に示すように、３軸のジャイロスコー
プ５，６，７の出力部には、それぞれ、ハイパスフィル
タ３６、３７、３８が接続され、ハイパスフィルタの出
力部に運動角演算装置３１が接続されている。ジャイロ
スコープは、小型で安価な物ほど静止しているときの出
力（オフセット）のばらつき及び変動が大きいが、安定
したものを用いると、姿勢検出装置が高価で大型になっ
てしまう。

【００２８】前記ハイパスフィルタは、前記圧電振動ジ
ャイロのオフセットをキャンセルするもので、ハイパス
フィルタを用いた後の出力を用いることで、安価で小型
で高精度の姿勢角検出装置を得ることができるもので、
通常０.１Ｈｚ以下の低い周波数に設定される。また、
それぞれのハイパスフィルタは、カットオフ周波数可変
を有している。前記低周波のハイパスフィルタは、電源
投入時から安定するまで時間がかかるので、必要に応じ
てハイパスフィルタのカットオフ周波数を高くすること
により、安定した出力を短時間で得ることができる。カ
ットオフ周波数を高くし安定出力を得た後は、即時にカ
ットオフ周波数は低い状態（測定状態）に戻される。

【００２９】運動角演算装置３１によって、姿勢角検出
装置（被測定物）の回転角速度に応じた各ジャイロスコ
ープの出力Ｊｘ（ｎ）、Ｊｙ（ｎ）、Ｊｚ（ｎ）は、座
標変換を行って、演算する単位時間で１単位時間過去の
出力β（ｎ−１）とから、次式の如く、基準座標系にお
ける現在の移動角度（運動角）△Ｘ（ｎ）に変換され
る。

【００３０】 ΔX(n) = cos[β(n-1)]×Jx(n) + sin[β(n-1)]×Jz(n) ΔY(n) = sin[α(n-1)]×sin[β(n-1)]×Jx(n)+cos[α
(n-1)]×Jy(n)-sin[α(n-1)]×cos[β(n-1)]×Jz(n) ΔZ(n) = -cos[α(n-1)]×sin[β(n-1)]×Jx(n)+sin[α
(n-1)]×Jy(n)+cos[α(n-1)]× cos[β(n-1)]×Jz(n)

【００３１】次に、図４の静止角演算装置の説明を行
う。静止角演算装置は、先にも述べたように、姿勢角検
出装置の静的動作時の姿勢角である仮のロール角Ｒ、仮
のピッチ角Ｐ、仮のヨー角Φを出力するものである。

【００３２】図１に示す加速度センサ１０、１１は、図
２に示すように、それぞれ、センサ座標系でのｘ、ｙ軸
上の重力加速度のｘ、ｙ成分分力Ａｘ（ｎ）、Ａｙ
（ｎ）を検出するように配置されている。また、図１に
示す地磁気センサ８、９は、それぞれｘ、ｙ軸上の地磁
気分力Ｍｘ（ｎ）、Ｍｙ（ｎ）を検出するように配置さ
れている。

【００３３】加速度センサの出力Ａｘ（ｎ）、Ａｙ
（ｎ）は、各々ロール角演算手段、ピッチ角演算手段に
て下式により、基準座標系の水平面（ＸＹ平面）との傾
斜角である仮のロール角Ｒ（ｎ）及び仮のピッチ角Ｐ
（ｎ）を算出する。

【００３４】 Ｒ（ｎ）＝ｓｉｎ^-1［Ａｘ（ｎ）／ｃｏｓＰ（ｎ）］
Ｐ（ｎ）＝ｓｉｎ^-1Aｙ（ｎ）

【００３５】基準座標系での仮のヨー角Φ（ｎ）は、基
準座標系での地磁気成分を用いて、Φ（ｎ）＝ｔａｎ^ー1
［ＨＸ（ｎ）／ＨＹ（ｎ）］で表わされる。姿勢角検出
装置が水平面上で回転する場合は、地磁気センサの出力
Ｍｘ(n)及びＭｙ(n)が、それぞれ、ＨＸ（ｎ）及びＨＹ
（ｎ）に等しく、 Φ（ｎ）＝ｔａｎ^ー1［Ｍｘ（ｎ）／Ｍｙ（ｎ）］ が成り立つ。

【００３６】原点時の方位角をΦ（０）とすれば、ヨー
角γ（ｎ）は、 γ（ｎ）＝Φ（ｎ）−Φ（０） となる。

【００３７】しかし、姿勢角検出装置がロール角Ｒやピ
ッチ角Ｐを持ち、水平でないときには、 ＨＸ＝ｃｏｓ［Ｐ（ｎ）］×Ｍｘ＋ｓｉｎ［Ｐ（ｎ）］
×Ｍｚ、 ＨＹ＝ｓｉｎ［Ｒ（ｎ）］×ｓｉｎ［Ｐ（ｎ）］×Ｍｘ
＋ｃｏｓ［Ｒ（ｎ）］×ｃｏｓ［Ｐ（ｎ）］×Ｍｚ の関係式から、仮のロール角Ｒ（ｎ）、仮のピッチ角Ｐ
（ｎ）を用いて座標変換を行い、基準座標系での地磁気
成分を求めて、 Φ（ｎ）＝ｔａｎ^ー1［ＨＸ（ｎ）／ＨＹ（ｎ）］ に代入し、求める。

【００３８】その際、地磁気Ｚ方向成分Ｍｚ（ｎ）が必
要となる。

【００３９】以下の方法で仮想的にＭｚを算出する。

【００４０】まず、地磁気Ｈｔを以下の手順で求める。
最初に、姿勢角検出装置（被測定物）部を９０度傾け、
図１の磁気センサ１０あるいは磁気センサ１１が基準座
標系のＺ軸に向くようにする。そして何らかの信号を送
り、例えばキーやボタンを押すなどをし、その時のＺ軸
に向いているセンサの値をＭｚ（０）としてメモリに格
納する。

【００４１】次に、姿勢角検出装置（被測定物）部を原
点位置に戻し、もう一度、何らかの信号を送り、磁気セ
ンサＭｘ（ｎ）,Ｍｙ（ｎ）の値をＭｘ（０），Ｍｙ
（０）としてメモリに格納する。これらＭｘ（０）,Ｍ
ｙ（０）,Ｍｚ（０）を用いて、Ｈｔは数３から求ま
る。

【００４２】

【数３】

【００４３】さらに、姿勢角検出装置（被測定物）の地
磁気北方向成分の傾斜角からＭｚの符号を判別する。こ
のために、バックアップメモリーと北方向成分傾斜角演
算手段を有する。

【００４４】演算処理にて算出した、演算する単位時間
で、１単位時間過去の仮のヨー角Φ(n-1)をバックアッ
プメモリーに記憶しておき、この仮のヨー角Φ（ｎ−
１）と加速度センサ出力Ａｘ（ｎ）、Ａｙ（ｎ）の値か
ら、北方向成分傾斜角演算手段で、姿勢角検出装置（被
測定物）の傾斜角度の地磁気北方向成分θ₂を次式によ
り算出する。

【００４５】θ₂＝sin^-1［Ａｘ（ｎ）×cosΦ（ｎ−
１）＋Ａｙ（ｎ）×sinΦ（ｎ−１）］

【００４６】により求められ、−９０度≦θ₂≦θ₀であ
れば、地磁気Ｍｚ符号はマイナス、θ₀＜θ₂≦９０度で
あれば、地磁気Ｍｚ符号はプラスと判別する。

【００４７】次に、本発明の姿勢角検出装置における、
前記静止角検出装置で演算された仮のロール角Ｒ
（ｎ）、仮のピッチ角Ｐ（ｎ）、仮のヨー角Φ（ｎ）が
ほぼ正しいかどうかを判別する判別装置を説明をする。

【００４８】静止角検出手段における、Ａｘ（ｎ）、Ａ
ｙ（ｎ）は、重力加速度のｘ，ｙ軸方向成分と運動加速
度のｘ、ｙ軸方向成分との合成ベクトルとなるために、
運動加速度がある場合には正しいロール角、ピッチ角に
ならない。そこで、Ｒ（ｎ）からＲ（ｎ−１）を引いた
ものが、０の近傍で、ある値以内であり、あるいはＰ
（ｎ）からＰ（ｎ−１）を引いたものが、０の近傍で、
ある値以内であるならば、運動加速度がない、即ち静止
角検出手段から演算された仮のロール角Ｒ（ｎ）、と仮
のピッチ角Ｐ（ｎ）が正しいと判別する。そうでない場
合は、運動加速度があると判別し、静止角検出手段から
の演算結果は正しくないとする。即ち、数１或いは数２
のうち、少なくとも一つの値が、ある値以内で、ほぼ０
であるとき、静止角検出手段からの姿勢角情報は正しい
と判別する判別装置を提供する。

【００４９】

【数１】

【００５０】

【数２】

【００５１】ここでは、加速度センサの出力を演算して
判別する手法について述べたが、磁気センサの出力に誤
差が生じ、大きく変動したとき等の場合も静止角度検出
手段の出力は正しくないと判別する。

【００５２】次に、上記判別装置の判別結果に応じて、
運動角検出手段の出力と静止角検出手段の出力を演算し
て最終的な出力を得る姿勢角演算装置で行われる演算方
法について説明する。

【００５３】この演算の基本は、判別装置によって静止
角検出手段の出力が誤っていると判別されたときには、
現在の姿勢角検出結果＝１単位時間過去の姿勢角検出結
果＋運動角となり、判別装置によって静止角検出手段の
出力が正しいと判別されたときには、現在の姿勢角検出
結果＝１単位過去の姿勢角検出結果＋運動角−補正値と
し、補正値は、誤差×比例定数、或いは一定定数であ
る。ここで、誤差＝（１単位時間過去の姿勢角検出結果
＋現在の運動角）−（静止角検出手段による現在の検出
結果）である。

【００５４】式で表すと、Ｘ軸の誤差＝α（ｎ−１）＋
△Ｘ（ｎ）−Ｒ（ｎ），Ｙ軸の誤差＝β（ｎ−１）＋△
Ｙ（ｎ）−Ｐ（ｎ），Ｚ軸の誤差＝γ（ｎ−１）＋△Ｚ
（ｎ）−Φ（ｎ）となる。

【００５５】第１の実施例では、本発明の姿勢角検出装
置の求めるべき出力をα（ｎ）、β（ｎ）、γ（ｎ）、
動的な運動角検出手段から得られる運動角を△Ｘ
（ｎ）、△Ｙ（ｎ）、△Ｚ（ｎ）、静的な静止角検出手
段から求められる静止角をＲ（ｎ）、Ｐ（ｎ）、Φ
（ｎ）とすると、

【００５６】静止角検出手段からの姿勢角情報は正しい
と判別する場合、 α（ｎ）＝α（ｎ−１）＋△Ｘ（ｎ）−Ｃ₁、 β（ｎ）＝β（ｎ−１）＋△Ｙ（ｎ）−Ｃ₂、 γ（ｎ）＝γ（ｎ−１）＋△Ｚ（ｎ）−Ｃ₃、 ここで、Ｃ₁,Ｃ₂,Ｃ₃は、任意に選択される誤差より大
きくない定数で、符号は、誤差の符号が、プラスの時、
プラス、マイナスの時、マイナスにより姿勢角を演算す
る。

【００５７】また、静止角検出手段からの姿勢角情報が
誤と判別する場合、 α（ｎ）＝α（ｎ−１）＋△Ｘ（ｎ）、 β（ｎ）＝β（ｎ−１）＋△Ｙ（ｎ）、 γ（ｎ）＝γ（ｎ−１）＋△Ｚ（ｎ）、 により姿勢角を演算する。

【００５８】また、本発明の他の実施の形態として、本
発明の姿勢角検出装置の求めるべき出力をα（ｎ）、β
（ｎ）、γ（ｎ）、動的な運動角検出手段から得られる
運動角を△Ｘ（ｎ）、△Ｙ（ｎ）、△Ｚ（ｎ）、静的な
静止角検出手段から求められる静止角をＲ（ｎ）、Ｐ
（ｎ）、Φ（ｎ）とすると、

【００５９】静止角検出手段からの姿勢角情報は正しい
と判別する場合、 α(ｎ)＝α(ｎ−１)＋△Ｘ(ｎ)−ｋ₁［α(ｎ−１)＋△
Ｘ(ｎ)−Ｒ(ｎ)］、 β(ｎ)＝β(ｎ−１)＋△Ｙ(ｎ)−ｋ₂［β(ｎ−１)＋△
Ｙ(ｎ)−Ｐ(ｎ)］、 γ(ｎ)＝γ(ｎ−１)＋△Ｚ(ｎ)−ｋ₃［γ(ｎ−１)＋△
Ｚ(ｎ)−Φ(ｎ)］、 により姿勢角を演算する。ここで、ｋ₁，ｋ₂，ｋ₃は、
任意に選択される１以下の比例定数である。

【００６０】また、静止角検出手段からの姿勢角情報が
誤と判別する場合、 α（ｎ）＝α（ｎ−１）＋△Ｘ（ｎ）、 β（ｎ）＝β（ｎ−１）＋△Ｙ（ｎ）、 γ（ｎ）＝γ（ｎ−１）＋△Ｚ（ｎ）、 により姿勢角を演算する。

【００６１】このように、静止角検出手段の出力が正し
いときのみ、誤差を補正しながら、運動角の積算で姿勢
角度を求めることにより、高速な応答性と、静止時の安
定性、再現性を同時に満足できるようにした姿勢角検出
装置である。

【００６２】図５は、姿勢角検出装置を適用したＨＭＤ
の一例示す。このＨＭＤは、目前にあるディスプレイに
表示に表示された映像が頭の動きに連動して変化し、仮
想空間を体験できるように設けられているもので、この
ＨＭＤを装着したまま右を向くと、姿勢角検出装置で計
測した頭の姿勢角を映像発生装置に送信し、映像発生装
置はその姿勢角にあった映像をＨＭＤに送信するので右
に展開する映像が映し出され、頭の動きによって全空間
の３６０度の映像を実感できるように設けられているも
のである。

【００６３】図５に示すように、符号１は、ＨＭＤ本体
を示し、このＨＭＤ本体１には、本発明である姿勢角検
出装置（被測定物）２が信号ケーブル３で映像発生装置
４に接続されている。

【００６４】

【発明の効果】本発明による姿勢角検出装置によれば、
上記のＨＭＤに適用した場合、外部信号を用いることな
く、また、高速応答で累積誤差の少ない角度情報を得る
ことができ、高性能なＨＭＤを構成することができる。

【００６５】本発明による姿勢角検出装置によれば、ジ
ャイロを素子で搭載しているので、小型軽量化が可能で
あり、セラミック振動子を使用しているので、磁気ノイ
ズは受けず、磁気センサと近接させても機能を損なうこ
とがない。また、ハイパスフィルタを使用することによ
り、小型・低価格化、高精度化が可能である。

【００６６】さらに、従来、姿勢角検出装置としては、
加速度センサが３個ないと姿勢角の誤差が判断できず、
そのため、複雑な構造になっていたが、本発明によれ
ば、２個の加速度センサと、２個の地磁気センサと、簡
単な計算だけで、誤差成分有無の判別・補正処理を行う
ことができ、高速応答で、高精度な姿勢角検出装置を提
供できる。

【図面の簡単な説明】

【図１】本発明の姿勢角検出装置のジャイロスコープ、
加速度センサ、地磁気センサの配置を示し、基準座標系
（Ｘ−Ｙ−Ｚ系）とセンサ座標系（ｘ−ｙ−ｚ系）が一
致している場合を示す図。

【図２】基準座標系（Ｘ−Ｙ−Ｚ系）とセンサ座標系
（ｘ−ｙ−ｚ系）が一致していない場合を示す図。

【図３】本発明の姿勢角検出装置の一実施の形態の全体
構成を示す図。

【図４】本発明の姿勢角検出装置のうち、静止角演算装
置の構成を示すブロック図。

【図５】ＨＭＤにおける姿勢角検出装置の使用状況の説
明図。

【符号の説明】

１ ディスプレイ本体 ２ 姿勢角検出装置（被測定物） ３ 信号ケーブル ４ 映像発生装置 ５ （第１の）ジャイロスコープ ６ （第２の）ジャイロスコープ ７ （第３の）ジャイロスコープ ８ 第１の地磁気センサ ９ 第２の地磁気センサ １０ 第１の加速度センサ １１ 第２の加速度センサ ３１ 運動角演算装置 ３２ 静止角演算装置 ３３ 判別装置 ３０ 姿勢角演算装置 ３６ （第１の）ハイパスフィルタ ３７ （第２の）ハイパスフィルタ ３８ （第３の）ハイパスフィルタ

───────────────────────────────────────────────────── フロントページの続き (51)Int.Cl.⁶ 識別記号 ＦＩ Ｇ０６Ｆ 3/033 ３１０ Ｇ０６Ｆ 3/033 ３１０Ｙ Ｈ０４Ｎ 5/64 ５１１ Ｈ０４Ｎ 5/64 ５１１Ａ (72)発明者 山本 直治 宮城県仙台市太白区郡山６丁目７番１号 株式会社トーキン内

Claims (9)

【特許請求の範囲】
1. 【請求項１】 水平面内で直交する軸をＸ軸、Ｙ軸と
   し、そのＸ軸、Ｙ軸に直交する軸をＺ軸とするとき、こ
   れら互いに直交するＸ軸、Ｙ軸、Ｚ軸が形成する基準座
   標系において、互いに直交する３軸の回りの回転角を検
   出する姿勢角検出装置において、Ｘ軸、Ｙ軸及びＺ軸の
   回りの角速度を検出する３個のジャイロスコープと、該
   ジャイロスコープの角速度に応じた出力に基づいて単位
   時間に移動した角度を演算する運動角演算装置と、Ｘ−
   Ｙ平面で互いに直交する２軸の加速度を検出するように
   配置された加速度センサと、Ｘ−Ｙ平面で互いに直交す
   る２軸の地磁気を検出するように配置される地磁気セン
   サと、該加速度センサ及び地磁気センサの出力に基づい
   て、Ｘ軸、Ｙ軸、Ｚ軸回りの回転角を演算する静止角演
   算装置と、静止角演算装置による演算結果の真偽を判別
   する判別装置と、該判別装置の演算結果に応じて、運動
   角演算装置の演算結果と静止角演算装置の演算結果から
   出力するべき姿勢角を演算する姿勢角演算装置とから構
   成されることを特徴とする姿勢角検出装置。
2. 【請求項２】 請求項１記載の姿勢角検出装置のジャイ
   ロスコープが、圧電振動ジャイロ用振動子と、その振動
   子をジャイロスコープとして動作させるための駆動検出
   回路からなり、該振動子が、非磁性材料で構成されるこ
   とを特徴とする姿勢角検出装置。
3. 【請求項３】 請求項１または２記載の姿勢角検出装置
   のジァイロスコープの出力部に、ハイパスフィルタが接
   続され、ハイパスフィルタの出力部は演算装置に入力さ
   れ、ハイパスフィルタの出力を用いて運動角を演算する
   ことを特徴とする姿勢角検出装置。
4. 【請求項４】 請求項３記載のハイパスフィルタは、カ
   ットオフ周波数可変手段を有することを特徴とする姿勢
   角検出装置。
5. 【請求項５】 請求項１乃至請求項４のいずれかに記載
   の姿勢角検出装置において、２軸の加速度センサと、加
   速度センサの出力Ａｘ（ｎ）、Ａｙ（ｎ）から仮のロー
   ル角Ｒ（ｎ）、仮のピッチ角Ｐ（ｎ）を演算するロール
   角演算手段、ピッチ角演算手段と、地磁気のｘ方向成分
   Ｍｘ（ｎ）、ｙ方向成分Ｍｙ（ｎ）を計測する２軸の地
   磁気センサと、演算する単位時間で１単位時間過去の方
   位角演算結果Φ（ｎ−１）を記憶しておくためのバック
   アップメモリーと、加速度センサの出力Ａｘ（ｎ）、Ａ
   ｙ（ｎ）及びバックアップメモリーの内容Φ（ｎ−１）
   より、傾斜角度の北方向成分θ₂を演算する北方向成分
   傾斜角演算手段と、地磁気のＺ方向成分Ｍｚの符号を判
   定する地磁気Ｍｚ符号判別手段と、地磁気のＺ方向成分
   地磁気Ｍｚ絶対値演算手段と、前記地磁気Ｍｚ符号判別
   手段と、地磁気Ｍｚ絶対値演算手段よりＺ軸方向成分の
   地磁気Ｍｚを計算する地磁気Ｍｚ演算手段と、前記地磁
   気Ｍｘ、Ｍｙ、Ｍｚより、基準座標系での地磁気のＸ，
   Ｙ，Ｚ成分ＨＸ，ＨＹ，ＨＺを演算する座標変換演算手
   段と、基準座標系における前記のＨＸ、ＨＹにより方位
   角Φを演算する方位演算手段から構成され、地磁気Ｍｚ
   演算手段により仮想的に地磁気Ｍｚを求めることにより
   地磁気センサ２軸により方位角Φ（ｎ）を計測すること
   を特徴とする姿勢角検出装置。
6. 【請求項６】 請求項５記載の姿勢角検出装置におい
   て、前記北方向成分傾斜角演算装置及び地磁気Ｍｚ符号
   判別手段により、北方向成分傾斜角θ₂、原点時の地磁
   気の伏角θ₀、バックアップメモリの出力をΦ（ｎ−
   １）、加速度センサ２軸の出力をそれぞれＡｘ（ｎ）、
   Ａｙ（ｎ）とする時、北方向成分傾斜角θ₂は、θ₂＝ｓ
   ｉｎ^-1［Ａｘ（ｎ）・ｃｏｓ（−Φ（ｎ−１））＋Ａｙ
   （ｎ）・ｓｉｎ（−Φ（ｎ−１））］として求め、−９
   ０度≦θ₂≦θ₀であれば、地磁気Ｍｚ符号はマイナス、
   θ₀＜θ₂≦９０度であれば、地磁気Ｍｚ符号はプラスと
   判別することを特徴とする姿勢角検出装置。
7. 【請求項７】 請求項１乃至６の姿勢角検出装置におい
   て、前記判別装置により、２軸の加速度センサおよび、
   ２軸の地磁気センサからなる静止角検出手段から求めら
   れる仮のヨー角Φ・仮のピッチ角Ｐ・仮のロール角Ｒの
   検出結果が、正しいかどうかの判別手段として、加速度
   センサの現在の出力Ａｘ（ｎ）から演算して得られた仮
   のロール角をＲ（ｎ）、演算する単位時間で１単位時間
   過去の出力Ａｘ（ｎ−１）から得られた仮のロール角演
   算結果をＲ（ｎ−１）とし、同様に加速度センサの現在
   の出力Ａｙ（ｎ）から演算して得られた仮のピッチ角を
   Ｐ（ｎ）、演算する単位時間で１単位時間過去の出力Ａ
   ｙ（ｎ−１）から得られた仮のピッチ角をＰ（ｎ−１）
   とすると、数１あるいは数２のうち、少なくとも一つの
   値が、ある値以内にあるとき、静止角検出手段からの姿
   勢角情報は正しいと判別することを特徴とする姿勢角検
   出装置。 【数１】 【数２】
8. 【請求項８】 請求項１乃至請求項７の姿勢角検出装置
   において、求めるべき出力をロール角α（ｎ）、ピッチ
   角β（ｎ）、ヨー角γ（ｎ）、運動角検出手段から求め
   られる運動角を、仮のロール角変化△Ｘ（ｎ）、仮のピ
   ッチ角変化△Ｙ（ｎ）、仮のヨー角変化△Ｚ（ｎ）、静
   止角検出手段から求められる静止角を、仮のロール角Ｒ
   （ｎ）、仮のピッチ角Ｐ（ｎ）、仮のヨー角Φ（ｎ）、
   誤差より大きくない一定定数をＣ₁，Ｃ₂，Ｃ₃とする
   と、判別装置の判別結果により、静止角検出手段からの
   姿勢角情報は正しいと判別する場合、α（ｎ）＝α（ｎ
   −１）＋△Ｘ（ｎ）−Ｃ₁、β（ｎ）＝β（ｎ−１）＋
   △Ｙ（ｎ）−Ｃ₂、γ（ｎ）＝γ（ｎ−１）＋△Ｚ
   （ｎ）−Ｃ₃により、また、静止角検出手段からの姿勢
   角情報は誤りと判別する場合は、α（ｎ）＝α（ｎ−
   １）＋△Ｘ（ｎ）、β（ｎ）＝β（ｎ−１）＋△Ｙ
   （ｎ）、γ（ｎ）＝γ（ｎ−１）＋△Ｚ（ｎ）により、
   姿勢角を演算することを特徴とする姿勢角検出装置。
9. 【請求項９】 請求項１乃至請求項８の姿勢角検出装置
   において、求めるべき出力をロール角α（ｎ）、ピッチ
   角β（ｎ）、ヨー角γ（ｎ）、動的な運動角検出手段か
   ら求められる運動角を、仮のロール角変化△Ｘ（ｎ）、
   仮のピッチ角変化△Ｙ（ｎ）、仮のヨー角変化△Ｚ
   （ｎ）、静的な静止角検出手段から求められる姿勢角
   を、仮のロール角Ｒ（ｎ）、仮のピッチ角Ｐ（ｎ）、仮
   のヨー角Φ（ｎ）、１以下の比例定数をｋ₁,ｋ₂,ｋ₃と
   すると、判別装置の判別結果により、静止角検出手段か
   らの姿勢角情報は正しいと判別する場合は、α（ｎ）＝
   α（ｎ−１）＋△Ｘ（ｎ）−ｋ₁［α（ｎ−１）＋△Ｘ
   （ｎ）−Ｒ（ｎ）］、β（ｎ）＝β（ｎ−１）＋△Ｙ
   （ｎ）−ｋ₂［β（ｎ−１）＋△Ｙ（ｎ）−Ｐ
   （ｎ）］、γ（ｎ）＝γ（ｎ−１）＋△Ｚ（ｎ）−ｋ₃
   ［γ（ｎ−１）＋△Ｚ（ｎ）−Φ（ｎ）］により、ま
   た、静止角検出手段からの姿勢角情報は誤りと判別する
   場合は、α（ｎ）＝α（ｎ−１）＋△Ｘ（ｎ）、β
   （ｎ）＝β（ｎ−１）＋△Ｙ（ｎ）、γ（ｎ）＝γ（ｎ
   −１）＋△Ｚ（ｎ）により、姿勢角を演算することを特
   徴とする姿勢角検出装置。