JP4023889B2

JP4023889B2 - Attitude angle detector

Info

Publication number: JP4023889B2
Application number: JP02653798A
Authority: JP
Inventors: 和健武藤; 洋阿部
Original assignee: NEC Tokin Corp
Current assignee: Tokin Corp
Priority date: 1998-01-23
Filing date: 1998-01-23
Publication date: 2007-12-19
Anticipated expiration: 2018-01-23
Also published as: JPH11211479A

Description

【０００１】
【発明の属する技術分野】
本発明は、移動体の姿勢検出や姿勢制御、あるいは、バーチャルリアリティー等に使用されるヘッドマウントディスプレイのうち、頭の姿勢角度を検出するトラッカー、３Ｄゲームパッド等、３次元空間中で被測定対象物の姿勢角（３軸の回転角度）を検出するのに用いる姿勢角検出装置に関する。
【０００２】
【従来の技術】
従来、バーチャルリアリティーに使用され、頭の動きを検出する姿勢角検出方法として、外部に設置された交流磁場発生源から微弱交流磁場を発生し、ヘッドマウントディスプレイ（以下、ＨＭＤという）に配置したセンサ部で検知し、制御演算部で頭の動きを検出する交流磁場を利用する方法や、外部に設置した超音波発生源からの超音波信号をＨＭＤに配置したセンサ部で検知し、制御演算部で頭の動きを検出する超音波を利用する方法がある。
【０００３】
【発明が解決しようとする課題】
しかしながら、前者の交流磁場を利用した方法では、信号発生源が微弱交流磁場のため、ノイズをキャンセルするためにフィルタを多用することにより応答性が低下し、頭の動きに比べて、ＨＭＤの映像の動きが遅くなり、気分を悪くしたり、酔いを発生した。
【０００４】
また、後者の超音波を利用した方法では、他の様々な超音波信号の影響を受けての誤動作や、信号発生源とセンサとの間の遮蔽物により信号を検出できなくなる可能性があったり、あるいは、センサの前に腕や髪の毛があるだけでも、受信不能になり、誤動作してしまうという問題があった。
【０００５】
従って、本発明は、上記問題点を解消し、使用する場所や環境の制限を緩和し、幅広く使用できる移動体の姿勢制御・検出に用いられる姿勢角検出装置を提供することである。
【０００６】
【課題を解決するための手段】
前記課題を解決するため、本発明に係る姿勢角検出装置は、互いに直交する３軸の回りの角速度を検出する第１、第２、及び第３のジャイロスコープと、上記３軸の加速度を検出する第１、第２、及び第３の加速度センサと、互いに直交する２軸の地磁気を検出する地磁気センサを配置し、前記３個のジャイロスコープの出力する角速度に基づいて単位時間あたりの移動角度（運動角）を検出する運動角検出手段と、加速度及び地磁気に基づいて静止角を検出する静止角検出手段と、静止角検出手段の検出結果が正しいかどうかを判別する判別装置と、この判別装置の判別結果と、運動角検出手段と静止角検出手段の演算結果を用いて、出力すべき姿勢角度を演算する姿勢角演算装置を備え、外部の信号発生源を使用することなく移動体の姿勢角度を検出できるようにしたことを特徴とする。
【０００７】
即ち、本発明は、平面内で直交するＸ軸、Ｙ軸に直交する軸をＺ軸とした、互いに直交する３軸の回りの回転角を検出する姿勢角検出装置において、Ｘ軸、Ｙ軸、及びＺ軸の回りの角速度を検出する３個のジャイロスコープと、該各ジャイロスコープの角速度に応じた出力に基づいて単位時間あたりの移動角度を演算する運動角演算装置と、上記３軸の加速度を検出する３個の加速度センサと、Ｘ軸及びＹ軸の２軸の地磁気を検出する２個の地磁気センサと、前記加速度センサ及び地磁気センサの出力に基づいて、前記３軸回りの静止角を演算する静止角演算装置と、該静止角演算装置による演算結果の真偽を判別する判別装置と、該判別装置の演算結果に応じて、前記運動角演算装置及び静止角演算装置の演算結果から姿勢角を演算する姿勢角演算装置とから構成される姿勢角検出装置である。
【０００８】
また、本発明は、前記ジャイロスコープが、振動子及び該振動子をジャイロスコープとして動作させるための駆動検出回路からなり、前記振動子は、非磁性材料で構成される上記の姿勢角検出装置である。
【０００９】
また本発明は、前記ジャイロスコープの出力部にハイパスフィルタを接続し、該ハイパスフィルタからの出力を用いて単位時間あたりの移動角度を演算する上記の姿勢角検出装置である。
【００１０】
また本発明は、前記ハイパスフィルタが、カットオフ周波数可変手段を有する上記の姿勢角検出装置である。
【００１１】
また、本発明は、前記３軸の加速度センサのｘ軸及びｙ軸の加速度センサからの出力Ａｘ（ｎ）、Ａｙ（ｎ）から、仮のロール角Ｒ（ｎ）、仮のピッチ角Ｐ（ｎ）を演算するロール角演算手段、ピッチ角演算手段と、地磁気のｘ方向成分Ｍｘ（ｎ）、ｙ方向成分Ｍｙ（ｎ）を計測する２軸の地磁気センサと、演算する単位時間で１単位時間過去の方位角演算結果Φ（ｎ−１）を記憶しておくためのバックアップメモリーと、加速度センサの出力Ａｘ（ｎ）、Ａｙ（ｎ）及びバックアップメモリーの内容Φ（ｎ−１）より、傾斜角度の北方向成分θ₂を演算する北方向成分傾斜角演算手段と、地磁気のＺ方向成分Ｍｚの符号を判定する地磁気Ｍｚ符号判別手段と、地磁気のＺ方向成分地磁気Ｍｚ絶対値演算手段と、前記地磁気Ｍｚ符号判別手段と、地磁気Ｍｚ絶対値演算手段よりＺ軸方向成分の地磁気Ｍｚを計算する地磁気Ｍｚ演算手段と、前記地磁気Ｍｘ、Ｍｙ、Ｍｚより、基準座標系での地磁気のＸ，Ｙ，Ｚ成分ＨＸ，ＨＹ，ＨＺを演算する座標変換演算手段と、基準座標系における前記のＨＸ、ＨＹにより方位角Φを演算する方位演算手段から構成され、地磁気Ｍｚ演算手段により仮想的に地磁気Ｍｚを求めることにより地磁気センサ２軸だけで方位角Φ（ｎ）が計測できる上記の姿勢角検出装置である。
【００１２】
また、本発明は、前記姿勢角検出装置の北方向成分傾斜角演算装置及び地磁気Ｍｚ符号判別手段において、北方向成分傾斜角θ₂、原点時の地磁気の伏角θ₀、バックアップメモリの出力をΦ（ｎ−１）、加速度センサ２軸の出力をそれぞれＡｘ（ｎ）、Ａｙ（ｎ）とする時、北方向成分傾斜角θ₂は、θ₂＝ｓｉｎ^-1［Ａｘ（ｎ）・ｃｏｓ（−Φ（ｎ−１））＋Ａｙ（ｎ）・ｓｉｎ（−Φ（ｎ−１））］により求められ、−９０度≦θ₂≦θ₀であれば、地磁気Ｍｚ符号はマイナス、θ₀＜θ₂ ≦９０度であれば、地磁気Ｍｚ符号はプラスと判別する上記の姿勢角検出装置である。
【００１３】
また、本発明は、前記３軸の加速度センサおよび２軸の地磁気センサからなる静止角検出手段から求められる、仮のヨー角Φ、仮のピッチ角Ｐ、仮のロール角Ｒの検出結果が、正しいかどうかの判別手段として、３軸の加速度センサの合成レベルが、ほぼ１Ｇであることにより、加速度センサの現在の出力Ａｘ（ｎ），Ａｙ（ｎ），Ａｚ（ｎ）が行うアルゴリスムが、数１の時、静止角検出手段からの姿勢角情報は正しいと判別する上記の姿勢角検出装置である。
【００１４】
［数１］
｜Ａ｜＝（Ａｘ＋Ａｙ＋Ａｚ）^１／２≒１
【００１５】
また、本発明は、前記姿勢角検出装置の求めるべき出力をロール角α（ｎ）、ピッチ角β（ｎ）、ヨー角γ（ｎ）、運動角検出手段から求められる運動角を△Ｘ（ｎ）、△Ｙ（ｎ）、△Ｚ（ｎ）、静止角検出手段から求められる静止角をＲ（ｎ）、Ｐ（ｎ）、Φ（ｎ）、一定定数をＣ₁，Ｃ₂，Ｃ₃とすると、判別装置の判別結果により、静止角検出手段からの姿勢角情報は正しいと判別する場合、α（ｎ）＝α（ｎ−１）＋△Ｘ（ｎ）−Ｃ₁、β（ｎ）＝β（ｎ−１）＋△Ｙ（ｎ）−Ｃ₂、γ（ｎ）＝γ（ｎ−１）＋△Ｚ（ｎ）−Ｃ₃により、また、静止角検出手段からの姿勢角情報は誤りと判別する場合は、α（ｎ）＝α（ｎ−１）＋△Ｘ（ｎ）、β（ｎ）＝β（ｎ−１）＋△Ｙ（ｎ）、γ（ｎ）＝γ（ｎ−１）＋△Ｚ（ｎ）により、姿勢角を演算する上記の姿勢角検出装置である。
【００１６】
また、本発明は、前記姿勢角検出装置の求めるべき出力をロール角α（ｎ）、ピッチ角β（ｎ）、ヨー角γ（ｎ）、運動角検出手段から求められる運動角を△Ｘ（ｎ）、△Ｙ（ｎ）、△Ｚ（ｎ）、静止角検出手段から求められる姿勢角をＲ（ｎ）、Ｐ（ｎ）、Φ（ｎ）、１以下の比例定数をｋ₁，ｋ₂，ｋ₃とすると、判別装置の判別結果により、静止角検出手段からの姿勢角情報は正しいと判別する場合は、α（ｎ）＝α（ｎ−１）＋△Ｘ（ｎ）−ｋ₁［α（ｎ−１）＋△Ｘ（ｎ）−Ｒ（ｎ）］、β（ｎ）＝β（ｎ−１）＋△Ｙ（ｎ）−ｋ₂［β（ｎ−１）＋△Ｙ（ｎ）−Ｐ（ｎ）］、 γ（ｎ）＝γ（ｎ−１）＋△Ｚ（ｎ）−ｋ₃［γ（ｎ−１）＋△Ｚ（ｎ）−Φ（ｎ）］により、また、静止角検出手段からの姿勢角情報は誤りと判別する場合は、α（ｎ）＝α（ｎ−１）＋△Ｘ（ｎ）、β（ｎ）＝β（ｎ−１）＋△Ｙ（ｎ）、γ（ｎ）＝γ（ｎ−１）＋△Ｚ（ｎ）により、姿勢角を演算する上記の姿勢角検出装置である。
【００１７】
【発明の実施の形態】
以下、図面によって本発明の実施の形態について説明する。
【００１８】
ここで説明するセンサ座標系から基準座標系への座標変換演算は、ヨー角Φ→ピッチ角Ｒ→ロール角Ｐの順に行うものとして説明する。
【００１９】
ここで、センサ座標系でのセンサ出力として、次の記号を用いる。
ジャイロスコープの出力：Ｊｘ（ｎ）、Ｊｙ（ｎ）、Ｊｚ（ｎ）。
加速度センサの出力：Ａｘ（ｎ）、Ａｙ（ｎ）、Ａｚ（ｎ）。
地磁気センサの出力：Ｍｘ（ｎ）、Ｍｙ（ｎ）。
地磁気センサの計算値：Ｍｚ（ｎ）。
【００２０】
また、上記センサ座標系でのセンサ出力を基準座標系に座標変換して得た値として、次の記号を用いる。
ジャイロスコープの出力から得た基準座標系Ｘ、Ｙ、Ｚ軸回りの運動角：△Ｘ（ｎ）、△Ｙ（ｎ）、△Ｚ（ｎ）。
加速度センサの出力から得た基準座標系Ｘ軸回りの回転角（仮のロール角）：Ｒ（ｎ）、同じく、Ｙ軸回りの回転角（仮のピッチ角）：Ｐ（ｎ）、同じく、Ｚ軸回りの回転角（仮のヨー角）：Φ_m（ｎ）。地磁気センサの出力から得たＺ軸回りの回転角（方位角或いは仮のヨー角）：Φ（ｎ）。
上記演算結果より求める最終的に出力するべき基準座標系Ｘ、Ｙ、Ｚ軸回りの回転角度をロール角α（ｎ）、ピッチ角β（ｎ）、ヨー角γ（ｎ）とする。
【００２１】
図１には、基準座標系（Ｘ−Ｙ−Ｚ系）とセンサ座標系（ｘ−ｙ−ｚ系）が一致している場合について示す。図１に示すように、水平面で直交する軸をＸ軸、Ｙ軸とし、そのＸ軸、Ｙ軸それぞれに直交する軸をＺ軸とするとき、これら互いに直交するＸ軸、Ｙ軸、Ｚ軸が形成する基準座標系において、Ｘ軸回りの回転角度をロール角α、Ｙ軸回りの回転角度をピッチ角β、Ｚ軸回りの回転角度をヨー角γと記す。
【００２２】
図１には、本発明の姿勢角検出装置におけるジャイロスコープ、加速度センサ、地磁気センサの配置を示す。図１で示すセンサ座標系において、互いに直交する３軸（ｘ軸、ｙ軸、及びｚ軸）の回りの角速度を検出するための第１のジャイロスコープ５、第２のジャイロスコープ６、及び第３のジャイロスコープ７は、ｘ、ｙ、ｚ軸各軸に平行に、即ち互いに直交して、配置されている。また、前記３軸は、第１の加速度センサ１０、第２の加速度センサ１１、及び第３の加速度センサ１２が、配置されている。更に、ｘ軸及びｙ軸には、第１の地磁気センサ８及び第２の地磁気センサ９が、それぞれｙ軸に平行に配置されている。
【００２３】
図２には、基準座標系（Ｘ−Ｙ−Ｚ系）に対して、センサ座標系（ｘ−ｙ−ｚ系）が、ずれた場合について示す。図２では、基準座標系の−Ｚ方向に重力加速度が働いており、センサ座標系のｘ軸、ｙ軸方向への分力が、Ａｘ（ｎ）、Ａｙ（ｎ）であることを示している。なお、重力加速度は、センサ座標系のｚ軸方向の力Ａｚ（ｎ）と等しい。
【００２４】
図３は、本発明の一実施の形態における姿勢角検出装置の全体構成を示す図である。図３に示すように、本発明の姿勢角度検出装置は、図１に示した直交配置された３個のジャイロスコープと、そのジャイロスコープそれぞれに接続されるハイパスフィルタと、ハイパスフィルタを介したジャイロスコープの出力から単位時間あたりの移動角度（運動角）を演算する運動角演算装置３１と、図１に示した３個の加速度センサと、２個の地磁気センサと、前記３個の加速度センサのうち、第１の加速度センサ１０から仮のロール角Ｒ、第２の加速度センサ１１から仮のピッチ角Ｐを求め、さらにその仮のロール角Ｒ、仮のピッチ角Ｐから仮のヨー角Φを求める静止角演算装置３２と、該静止角演算装置３２の演算結果の真偽を判別するための判別装置３３、及び前記運動角演算装置３１、静止角演算装置３２、判別装置３３からの信号を処理する姿勢角演算装置３０とで構成されている。
【００２５】
最初に、ジャイロスコープの内容を説明する。本発明の姿勢角検出装置におけるジャイロスコープは、圧電セラミック、圧電単結晶、或いはシリコン等の非磁性材料からなる振動子を用いることが可能である。本実施の形態では、（株）トーキン製の圧電振動セラミックジャイロを用いた。
【００２６】
図３に示すように、３軸のジャイロスコープ５，６，７の出力部には、それぞれ、ハイパスフィルタ３６、３７、３８が接続され、ハイパスフィルタの出力部に運動角演算装置３１が接続されている。なお、ジャイロスコープは、小型で安価な物ほど静止しているときの出力（オフセット）のばらつき及び変動が大きいが、これを安定化したものを用いると、姿勢検出装置が高価で大型になってしまう。
【００２７】
また、ハイパスフィルタは、前記ジャイロスコープのオフセットをキャンセルするもので、ハイパスフィルタを用いた後の出力を用いることで、安価で小型で高精度の姿勢角検出装置を得ることができ、通常０.１Ｈｚ以下の低い周波数に設定される。なお、それぞれのハイパスフィルタは、カットオフ周波数の可変手段を有している。また、前記低周波のハイパスフィルタは、電源投入時から安定するまで時間がかかるので、必要に応じてハイパスフィルタのカットオフ周波数を高くすることにより、安定した出力を短時間で得ることができる。カットオフ周波数を高くし安定出力を得た後は、即時にカットオフ周波数は低い状態（測定状態）に戻される。
【００２８】
移動体に搭載された姿勢角検出装置（被測定物）の回転角速度に応じた各ジャイロスコープの出力Ｊｘ（ｎ）、Ｊｙ（ｎ）、Ｊｚ（ｎ）は、運動角演算装置３１によって、座標変換を行って演算する単位時間で１単位時間過去の出力β（ｎ−１）から、次式の如く、基準座標系における現在の単位時間あたりの移動角度（運動角）△Ｘ（ｎ）に変換される。
【００２９】

【００３０】
次に、図４を用いて静止角演算装置の説明を行う。静止角演算装置は、先にも述べたように、姿勢角検出装置の静的動作時の姿勢角である仮のロール角Ｒ、仮のピッチ角Ｐ、仮のヨー角Φを出力するものである。
【００３１】
図１に示す加速度センサ１０、１１は、図２に示すように、それぞれ、センサ座標系でのｘ、ｙ軸上の重力加速度のｘ、ｙ成分分力Ａｘ（ｎ）、Ａｙ（ｎ）を検出するように配置されている。また、図１に示す地磁気センサ８、９は、それぞれｘ、ｙ軸上の地磁気分力Ｍｘ（ｎ）、Ｍｙ（ｎ）を検出するように配置されている。
【００３２】
加速度センサの出力Ａｘ（ｎ）、Ａｙ（ｎ）は、各々ロール角演算手段、ピッチ角演算手段にて下式により、基準座標系の水平面（ＸＹ平面）との傾斜角である仮のロール角Ｒ（ｎ）及び仮のピッチ角Ｐ（ｎ）を算出する。
【００３３】
Ｒ（ｎ）＝ｓｉｎ^-1［Ａｘ（ｎ）／ｃｏｓＰ（ｎ）］
Ｐ（ｎ）＝ｓｉｎ^-1Ａｙ（ｎ）
【００３４】
基準座標系での仮のヨー角Φ（ｎ）は、基準座標系での地磁気成分を用いて、
Φ（ｎ）＝ｔａｎ^ー1［ＨＸ（ｎ）／ＨＹ（ｎ）］
で表わされる。なお、加速度センサの出力Ａｚ（ｎ）によるヨー角Φ_m（ｎ）と前記ヨー角Φ（ｎ）を比較演算することで、高精度にヨー角を演算できる。姿勢角検出装置が水平面上で回転する場合は、地磁気センサの出力Ｍｘ（ｎ）及びＭｙ（ｎ）が、それぞれ、ＨＸ（ｎ）及びＨＹ（ｎ）に等しく、
Φ（ｎ）＝ｔａｎ^ー1［Ｍｘ（ｎ）／Ｍｙ（ｎ）］
が成り立つ。
【００３５】
原点時の方位角をΦ（０）とすれば、ヨー角γ（ｎ）は、
γ（ｎ）＝Φ（ｎ）−Φ（０）
となる。
【００３６】
しかし、姿勢角検出装置がロール角Ｒやピッチ角Ｐを持ち、水平でないときには、
ＨＸ＝ｃｏｓ［Ｐ（ｎ）］×Ｍｘ＋ｓｉｎ［Ｐ（ｎ）］×Ｍｚ、
ＨＹ＝ｓｉｎ［Ｒ（ｎ）］×ｓｉｎ［Ｐ（ｎ）］×Ｍｘ＋ｃｏｓ［Ｒ（ｎ）］
×ｃｏｓ［Ｐ（ｎ）］×Ｍｚ
の式を用いて、仮のロール角Ｒ（ｎ）、仮のピッチ角Ｐ（ｎ）を用いて座標変換を行い、基準座標系での地磁気成分を求めて、
Φ（ｎ）＝ｔａｎ^ー1［ＨＸ（ｎ）／ＨＹ（ｎ）］
に代入し、求める。
【００３７】
その際、地磁気Ｚ方向成分Ｍｚ（ｎ）が必要となる。
【００３８】
以下の方法で仮想的にＭｚを算出する。
【００３９】
まず、地磁気Ｈｔを以下の手順で求める。最初に、姿勢角検出装置（被測定物）を９０度傾け、磁気センサ１０あるいは磁気センサ１１が、基準座標系のＺ軸に向くようにする。そして、例えば、キーやボタンを押すなど、何らかの信号を送り、その時のＺ軸に向いているセンサの値をＭｚ（０）としてメモリに格納する。
【００４０】
次に、姿勢角検出装置（被測定物）を原点位置に戻し、もう一度、何らかの信号を送り、磁気センサＭｘ（ｎ），Ｍｙ（ｎ）の値をＭｘ（０），Ｍｙ（０）としてメモリに格納する。これらＭｘ（０），Ｍｙ（０），Ｍｚ（０）を用いて、Ｈｔは数２から求まる。
【００４１】
【数２】

【００４２】
さらに、Ｍｚの符号を算出するために、姿勢角検出装置（被測定物）の地磁気北方向成分の傾斜角から判断できる。このために、バックアップメモリーと北方向成分傾斜角演算手段を有する。
【００４３】
演算処理にて算出した、演算する単位時間で、１単位時間過去の仮のヨー角Φ（ｎ−１）をバックアップメモリーに記憶しておき、この仮のヨー角Φ（ｎ−１）と加速度センサ出力Ａｘ（ｎ）、Ａｙ（ｎ）の値から、北方向成分傾斜角演算手段で、姿勢角検出装置（被測定物）の傾斜角度の地磁気北方向成分θ₂を次式により算出する。
【００４４】
θ₂＝sin^-1［Ａｘ（ｎ）×cosΦ（ｎ−１）＋Ａｙ（ｎ）×sinΦ（ｎ−１）］
により求められ、−９０度≦θ₂≦θ₀であれば、地磁気Ｍｚ符号はマイナス、θ₀＜θ₂≦９０度であれば、地磁気Ｍｚ符号はプラスと判別する。
【００４５】
次に、本発明の姿勢角検出装置における、前記３軸の加速度センサの合成ベクトルが、ほぼ１Ｇであることにより行うアルゴリズム、即ち、数１の時、静止角検出装置で演算される仮のロール角Ｒ（ｎ）、仮のピッチ角Ｐ（ｎ）、仮のヨー角Φ（ｎ）が正しいかどうかを判別する判別装置３３を説明をする。
【００４６】
［数１］
｜Ａ｜＝（Ａｘ＋Ａｙ＋Ａｚ）^１／２≒１
【００４７】
判別装置３３は、静止角検出手段における、Ｘ軸方向の加速度Ａｘ（ｎ）、Ｙ軸方向の加速度Ａｙ（ｎ）より、ロール角Ｒ＝ｓｉｎ^-1（Ａｘ／ｃｏｓＰ）、及びピッチ角Ｐ＝ｓｉｎ^-1Ａｙを求める。
【００４８】
しかし、ここで、Ａｘ（ｎ），Ａｙ（ｎ）は、重力加速度の傾斜角成分と運動加速度のＸ，Ｙ軸方向成分との合成ベクトルなので、運動加速度がある場合には、正しいロール角、ピッチ角にならない。そこで、３軸の加速度センサの合成ベクトルＡの大きさを数３により求め、この値が１Ｇの近傍であれば、軸の方向の運動加速度がない、即ち、ロール角Ｒ、ピッチ角Ｐが正しいと判断し、静的モードにする。そうでない場合は、軸方向に運動加速度があると判断し、静止角検出手段からの演算結果は正しくないとし、動的モードにするものである。
【００４９】
【数３】

【００５０】
ここでは、加速度センサの出力を演算して判別する手法について述べたが、磁気センサの出力に誤差が生じ、大きく変動したとき等の場合も静止角検出手段の出力は正しくないと判別する。
【００５１】
次に、上記判別装置３３の判別結果に応じて、運動角検出手段の出力及び静止角検出手段の出力を演算する運動角演算装置３１と静止角演算装置３２から最終的な出力を得る姿勢角演算装置３０で行われる演算方法について説明する。
【００５２】
この演算の基本は、判別装置３３によって静止角検出手段の出力が誤っていると判別されたときには、現在の姿勢角検出結果は、１単位時間過去の姿勢角検出結果と運動角を加算したものとなり、判別装置３３によって静止角検出手段の出力が正しいと判別されたときには、現在の姿勢角検出結果は、１単位過去の姿勢角検出結果と運動角を加算したものから補正値を減算したものである。なお、補正値は、誤差と比例定数を乗算したもの、或いは一定定数である。ここで、誤差は、１単位時間過去の姿勢角検出結果に現在の運動角を加算したものから静止角検出手段による現在の検出結果を減算したものである。
【００５３】
これを式で表すと、本実施の形態において、本発明の姿勢角検出装置の求めるべき出力を、α（ｎ）、β（ｎ）、γ（ｎ）、運動角検出手段から得られる運動角を△Ｘ（ｎ）、△Ｙ（ｎ）、△Ｚ（ｎ）、静止角検出手段から求められる静止角をＲ（ｎ）、Ｐ（ｎ）、Φ（ｎ）とすると、
【００５４】
静止角検出手段からの姿勢角情報は正しいと判別する場合、
α（ｎ）＝α（ｎ−１）＋△Ｘ（ｎ）−Ｃ₁、
β（ｎ）＝β（ｎ−１）＋△Ｙ（ｎ）−Ｃ₂、
γ（ｎ）＝γ（ｎ−１）＋△Ｚ（ｎ）−Ｃ₃、
ここで、Ｃ₁，Ｃ₂，Ｃ3は、任意に選択される誤差より大きくない定数で、符号は、誤差の符号が、プラスの時、プラス、マイナスの時、マイナスにより姿勢角を演算する。
【００５５】
また、静止角検出手段からの姿勢角情報が誤と判別する場合、
α（ｎ）＝α（ｎ−１）＋△Ｘ（ｎ）、
β（ｎ）＝β（ｎ−１）＋△Ｙ（ｎ）、
γ（ｎ）＝γ（ｎ−１）＋△Ｚ（ｎ）、
により姿勢角を演算する。
【００５６】
また、本発明の他の実施の形態として、本発明の姿勢角検出装置の求めるべき出力をα（ｎ）、β（ｎ）、γ（ｎ）、運動角検出手段から得られる運動角を△Ｘ（ｎ）、△Ｙ（ｎ）、△Ｚ（ｎ）、静止角検出手段から求められる静止角をＲ（ｎ）、Ｐ（ｎ）、Φ（ｎ）とすると、
【００５７】
静止角検出手段からの姿勢角情報は正しいと判別する場合、
α(ｎ)＝α(ｎ−１)＋△Ｘ(ｎ)−ｋ₁［α(ｎ−１)＋△Ｘ(ｎ)−Ｒ(ｎ)］、
β(ｎ)＝β(ｎ−１)＋△Ｙ(ｎ)−ｋ₂［β(ｎ−１)＋△Ｙ(ｎ)−Ｐ(ｎ)］、
γ(ｎ)＝γ(ｎ−１)＋△Ｚ(ｎ)−ｋ₃［γ(ｎ−１)＋△Ｚ(ｎ)−Φ(ｎ)］、
により姿勢角を演算する。ここで、ｋ₁，ｋ₂，ｋ₃は、任意に選択させる１以下の比例定数である。
【００５８】
また、静止角検出手段からの姿勢角情報が誤と判別する場合、
α（ｎ）＝α（ｎ−１）＋△Ｘ（ｎ）、
β（ｎ）＝β（ｎ−１）＋△Ｙ（ｎ）、
γ（ｎ）＝γ（ｎ−１）＋△Ｚ（ｎ）、
により姿勢角を演算する。
【００５９】
このように、静止角検出手段の出力が正しいときのみ、誤差を補正しながら、運動角の積算で姿勢角を求めることにより、高速な応答性と、静止時の安定性、再現性を同時に満足できるようにした姿勢角検出装置である。
【００６０】
図５は、本発明の実施の形態の姿勢角検出装置を適用したＨＭＤの一例を示す。このＨＭＤは、目前にあるディスプレイ１に表示された映像が、頭の動きに連動して変化し、仮想空間を体験できるように設けられているもので、このＨＭＤを装着したまま右を向くと、姿勢角検出装置２で計測した頭の姿勢角を信号ケーブル３で映像発生装置４に送信し、映像発生装置４は、その姿勢角にあった映像をＨＭＤに送信するので、右に展開する映像が映し出され、頭の動きによって全空間の３６０度の映像を実感できるように設けられているものである。
【００６１】
【発明の効果】
以上、説明したように、本発明による姿勢角検出装置をＨＭＤに適用した場合、外部信号を用いることなく、また、高速応答で累積誤差のない角度情報が得られる高性能なＨＭＤを構成することができる。
【００６２】
また、本発明による姿勢角検出装置によれば、ジャイロスコープを素子で搭載しているので、小型軽量化が可能であり、セラミック振動子を使用しているので、磁気ノイズは受けず、磁気センサと近接させても、機能を損なうことがない。また、ハイパスフィルタを使用することにより、小型・低価格化、高精度化が可能である。
【００６３】
さらに、本発明によれば、３個の加速度センサと、２個の地磁気センサにより、簡単な計算だけで、誤差成分有無の判別・補正処理を高精度で行うことができ、高速応答で、高精度な姿勢角検出装置を提供できる。
【図面の簡単な説明】
【図１】本発明の姿勢角検出装置のジャイロスコープ、加速度センサ、地磁気センサの配置を示し、基準座標系（Ｘ−Ｙ−Ｚ系）とセンサ座標系（ｘ−ｙ−ｚ系）が一致している場合を示す図。
【図２】基準座標系（Ｘ−Ｙ−Ｚ系）とセンサ座標系（ｘ−ｙ−ｚ系）が一致していない座標系を示す図。
【図３】本発明の姿勢角検出装置の構成を示す図。
【図４】本発明の姿勢角検出装置のうち、静止角演算装置の構成を示すブロック図。
【図５】ＨＭＤにおける姿勢角検出装置の使用状況の説明図。
【符号の説明】
１ディスプレイ
２姿勢角検出装置（被測定物）
３信号ケーブル
４映像発生装置
５（第１の）ジャイロスコープ
６（第２の）ジャイロスコープ
７（第３の）ジャイロスコープ
８（第１の）地磁気センサ
９（第２の）地磁気センサ
１０（第１の）加速度センサ
１１（第２の）加速度センサ
１２（第３の）加速度センサ
３０姿勢角演算装置
３１運動角演算装置
３２静止角演算装置
３３判別装置
３６（第１の）ハイパスフィルタ
３７（第２の）ハイパスフィルタ
３８（第３の）ハイパスフィルタ[0001]
BACKGROUND OF THE INVENTION
The present invention relates to a target to be measured in a three-dimensional space, such as a tracker for detecting the posture angle of a head, a 3D game pad, etc., among head mounted displays used for posture detection, posture control, or virtual reality of a moving body. The present invention relates to a posture angle detection device used for detecting a posture angle (three-axis rotation angle) of an object.
[0002]
[Prior art]
Conventionally, as a posture angle detection method used for virtual reality to detect head movement, a sensor that generates a weak alternating magnetic field from an external alternating magnetic field generation source and places it on a head mounted display (hereinafter referred to as HMD) The method of using an alternating magnetic field to detect the head movement in the control unit and the control calculation unit to detect the ultrasonic signal from the ultrasonic source installed outside in the sensor unit arranged in the HMD There is a method using ultrasonic waves to detect head movement.
[0003]
[Problems to be solved by the invention]
However, in the former method using an alternating magnetic field, since the signal generation source is a weak alternating magnetic field, the responsiveness is lowered by using many filters to cancel noise, and the image of the HMD is compared with the movement of the head. The movement slowed down, made me feel sick and caused sickness.
[0004]
Also, in the latter method using ultrasonic waves, there is a possibility that signals cannot be detected due to malfunctions due to the influence of various other ultrasonic signals, or due to shielding between the signal generation source and the sensor. Alternatively, there is a problem that even if there is an arm or hair in front of the sensor, reception becomes impossible and malfunction occurs.
[0005]
Accordingly, an object of the present invention is to provide a posture angle detection device used for posture control / detection of a mobile body that can solve the above-mentioned problems, relax restrictions on the place of use and environment, and can be widely used.
[0006]
[Means for Solving the Problems]
In order to solve the above problems, an attitude angle detection device according to the present invention detects first, second, and third gyroscopes that detect angular velocities about three axes orthogonal to each other, and detects the accelerations of the three axes. The first, second, and third acceleration sensors and a geomagnetic sensor that detects biaxial geomagnetism orthogonal to each other are arranged, and the movement angle per unit time based on the angular velocity output by the three gyroscopes A motion angle detection means for detecting (motion angle), a static angle detection means for detecting a static angle based on acceleration and geomagnetism, a determination device for determining whether the detection result of the static angle detection means is correct, and this determination A posture angle calculation device that calculates the posture angle to be output using the device discrimination results and the calculation results of the motion angle detection means and the stationary angle detection means, and without using an external signal generation source Appearance Characterized in that to be able to detect the angle.
[0007]
That is, the present invention relates to an X-axis, Y-axis in an attitude angle detection device that detects rotation angles about three axes orthogonal to each other, with the X axis orthogonal to the plane and the axis orthogonal to the Y axis as the Z axis. , And three gyroscopes that detect angular velocities around the Z axis, a motion angle calculation device that calculates a movement angle per unit time based on an output corresponding to the angular velocity of each gyroscope, Three acceleration sensors that detect acceleration, two geomagnetic sensors that detect biaxial geomagnetism of the X axis and Y axis, and a static angle around the three axes based on the outputs of the acceleration sensor and geomagnetic sensor A static angle calculation device that calculates the true value of the calculation result by the static angle calculation device, and the calculation results of the motion angle calculation device and the static angle calculation device according to the calculation result of the determination device Figure to calculate posture angle from A posture angle detecting device comprising an angular computing device.
[0008]
According to the present invention, the gyroscope includes a vibrator and a drive detection circuit for operating the vibrator as a gyroscope, and the vibrator is the posture angle detection device described above configured of a nonmagnetic material. is there.
[0009]
The present invention is also the above attitude angle detection device in which a high-pass filter is connected to the output unit of the gyroscope, and a movement angle per unit time is calculated using an output from the high-pass filter.
[0010]
The present invention is also the above attitude angle detection device in which the high-pass filter has a cutoff frequency varying means.
[0011]
Further, according to the present invention, from the outputs Ax (n) and Ay (n) from the x-axis and y-axis acceleration sensors of the three-axis acceleration sensor, a temporary roll angle R (n) and a temporary pitch angle P ( n) roll angle calculation means, pitch angle calculation means, two-axis geomagnetic sensor that measures the x-direction component Mx (n) and y-direction component My (n) of geomagnetism, and one unit in the unit time to calculate From the backup memory for storing the past azimuth calculation results Φ (n−1), the acceleration sensor outputs Ax (n), Ay (n), and the contents Φ (n−1) of the backup memory, North direction component inclination angle calculating means for calculating the north direction component θ ₂ of the inclination angle, geomagnetism Mz code determining means for determining the sign of the geomagnetic Z direction component Mz, geomagnetic Z direction component geomagnetism Mz absolute value calculating means, The geomagnetic Mz code discrimination means; The geomagnetism Mz computing means for calculating the geomagnetism Mz of the Z-axis direction component from the geomagnetism Mz absolute value computing means, and the geomagnetism X, Y, Z components HX, HY, HZ in the reference coordinate system from the geomagnetism Mx, My, Mz. The coordinate transformation calculation means for calculating the azimuth angle and the azimuth calculation means for calculating the azimuth angle Φ by the HX and HY in the reference coordinate system. It is said attitude | position angle detection apparatus which can measure azimuth angle (phi) (n) only by.
[0012]
Further, according to the present invention, in the north direction component inclination angle calculation device and the geomagnetic Mz code discrimination means of the attitude angle detection device, the north direction component inclination angle θ ₂ , the geomagnetic dip angle θ ₀ at the origin, and the output of the backup memory are Φ (N-1), where the acceleration sensor biaxial outputs are Ax (n) and Ay (n), respectively, the north direction component inclination angle θ ₂ is θ ₂ = sin ⁻¹ [Ax (n) · cos ( −Φ (n−1)) + Ay (n) · sin (−Φ (n−1))], and −90 degrees ≦ θ ₂ ≦ θ ₀ , the geomagnetic Mz code is negative, θ ₀ < If θ ₂ ≦ 90 degrees, the attitude angle detection device described above determines that the geomagnetic Mz code is positive.
[0013]
Further, in the present invention, detection results of the temporary yaw angle Φ, the temporary pitch angle P, and the temporary roll angle R, which are obtained from the static angle detection means including the triaxial acceleration sensor and the biaxial geomagnetic sensor, As a means for determining whether or not it is correct, an algorithm performed by the current outputs Ax (n), Ay (n), and Az (n) of the acceleration sensor when the combined level of the three-axis acceleration sensor is approximately 1 G, In Equation 1, the posture angle detection device described above determines that the posture angle information from the stationary angle detection means is correct.
[0014]
[Equation 1]
| A | = (Ax + Ay + Az) ^1/2 ≈1
[0015]
In the present invention, the output to be obtained by the posture angle detection device is the roll angle α (n), the pitch angle β (n), the yaw angle γ (n), and the motion angle obtained from the motion angle detection means is ΔX ( n), ΔY (n), ΔZ (n), the static angles obtained from the static angle detecting means are R (n), P (n), Φ (n), and constant constants are C ₁ , C ₂ , C ₃ and when, the discrimination result of the discriminating device, to determine the attitude angle information from the static angle detecting means correct, α (n) = α ( n-1) + △ X (n) -C 1, β ( n) = β (n−1) + ΔY (n) −C ₂ , γ (n) = γ (n−1) + ΔZ (n) −C ₃ , and the posture from the stationary angle detection means When it is determined that the angle information is an error, α (n) = α (n−1) + ΔX (n), β (n) = β (n−1) + ΔY (n), γ (n) = Calculate the posture angle by γ (n-1) + ΔZ (n) It is the attitude angle detection device.
[0016]
In the present invention, the output to be obtained by the posture angle detection device is the roll angle α (n), the pitch angle β (n), the yaw angle γ (n), and the motion angle obtained from the motion angle detection means is ΔX ( n), ΔY (n), ΔZ (n), the posture angle obtained from the stationary angle detection means are R (n), P (n), Φ (n), and proportional constants of 1 or less are k ₁ , k ₂ and k ₃ , α (n) = α (n−1) + ΔX (n) −k when it is determined that the posture angle information from the stationary angle detection means is correct based on the determination result of the determination device. ₁ [α (n−1) + ΔX (n) −R (n)], β (n) = β (n−1) + ΔY (n) −k ₂ [β (n−1) + Δ Y (n) −P (n)], γ (n) = γ (n−1) + ΔZ (n) −k ₃ [γ (n−1) + ΔZ (n) −Φ (n)] If the attitude angle information from the stationary angle detection means is determined to be incorrect, n) = α (n−1) + ΔX (n), β (n) = β (n−1) + ΔY (n), γ (n) = γ (n−1) + ΔZ (n ) To calculate the posture angle.
[0017]
DETAILED DESCRIPTION OF THE INVENTION
Hereinafter, embodiments of the present invention will be described with reference to the drawings.
[0018]
The coordinate conversion calculation from the sensor coordinate system to the reference coordinate system described here will be described as being performed in the order of yaw angle Φ → pitch angle R → roll angle P.
[0019]
Here, the following symbols are used as sensor outputs in the sensor coordinate system.
Gyroscope output: Jx (n), Jy (n), Jz (n).
Output of acceleration sensor: Ax (n), Ay (n), Az (n).
Geomagnetic sensor output: Mx (n), My (n).
Calculated value of geomagnetic sensor: Mz (n).
[0020]
Further, the following symbols are used as values obtained by converting the sensor output in the sensor coordinate system into the reference coordinate system.
Movement angles around the reference coordinate system X, Y, Z axis obtained from the output of the gyroscope: ΔX (n), ΔY (n), ΔZ (n).
Rotation angle around the reference coordinate system X axis (temporary roll angle) obtained from the output of the acceleration sensor: R (n), similarly Rotation angle around the Y axis (temporary pitch angle): P (n), Rotational angle around the Z axis (provisional yaw angle): Φ _m (n). Rotation angle (azimuth angle or provisional yaw angle) around the Z axis obtained from the output of the geomagnetic sensor: Φ (n).
The rotation angles around the reference coordinate system X, Y, and Z axes, which are finally obtained from the calculation results, are defined as a roll angle α (n), a pitch angle β (n), and a yaw angle γ (n).
[0021]
FIG. 1 shows a case where the reference coordinate system (XYZ system) and the sensor coordinate system (xyz system) match. As shown in FIG. 1, when an axis orthogonal to the horizontal plane is an X axis and a Y axis, and an axis orthogonal to each of the X axis and the Y axis is a Z axis, these X axis, Y axis, and Z axis orthogonal to each other In the reference coordinate system formed by the above, the rotation angle around the X axis is denoted as roll angle α, the rotation angle around the Y axis as pitch angle β, and the rotation angle around the Z axis as yaw angle γ.
[0022]
FIG. 1 shows an arrangement of a gyroscope, an acceleration sensor, and a geomagnetic sensor in the posture angle detection device of the present invention. In the sensor coordinate system shown in FIG. 1, a first gyroscope 5, a second gyroscope 6, and a first gyroscope 6 for detecting angular velocities around three axes (x axis, y axis, and z axis) orthogonal to each other. The three gyroscopes 7 are arranged in parallel to the x, y, and z axes, that is, orthogonal to each other. Further, the first acceleration sensor 10, the second acceleration sensor 11, and the third acceleration sensor 12 are arranged on the three axes. Further, a first geomagnetic sensor 8 and a second geomagnetic sensor 9 are arranged in parallel to the y-axis on the x-axis and the y-axis, respectively.
[0023]
FIG. 2 shows a case where the sensor coordinate system (xyz system) is deviated from the reference coordinate system (XYZ system). FIG. 2 shows that gravitational acceleration acts in the −Z direction of the reference coordinate system, and the component forces in the x-axis and y-axis directions of the sensor coordinate system are Ax (n) and Ay (n). Yes. The gravitational acceleration is equal to the force Az (n) in the z-axis direction of the sensor coordinate system.
[0024]
FIG. 3 is a diagram showing an overall configuration of the attitude angle detection device according to the embodiment of the present invention. As shown in FIG. 3, the attitude angle detection device of the present invention includes three orthogonally arranged gyroscopes shown in FIG. 1, a high-pass filter connected to each of the gyroscopes, and a gyro via the high-pass filter. A motion angle calculation device 31 that calculates a movement angle (movement angle) per unit time from the output of the scope, the three acceleration sensors shown in FIG. 1, the two geomagnetic sensors, and the three acceleration sensors Of these, the provisional roll angle R is obtained from the first acceleration sensor 10, the provisional pitch angle P is obtained from the second acceleration sensor 11, and the provisional yaw angle Φ is obtained from the provisional roll angle R and provisional pitch angle P. From the static angle calculation device 32 to be obtained, the determination device 33 for determining the authenticity of the calculation result of the static angle calculation device 32, the motion angle calculation device 31, the static angle calculation device 32, and the determination device 33 It is composed of a posture angle computing device 30 for processing the signal.
[0025]
First, the contents of the gyroscope will be described. The gyroscope in the attitude angle detection device of the present invention can use a vibrator made of a non-magnetic material such as piezoelectric ceramic, piezoelectric single crystal, or silicon. In the present embodiment, a piezoelectric vibration ceramic gyro manufactured by Tokin Corporation was used.
[0026]
As shown in FIG. 3, high-

pass filters

36, 37, and 38 are connected to the output portions of the three-

axis gyroscopes

5, 6, and 7, respectively, and the motion angle calculation device 31 is connected to the output portion of the high-pass filter. ing. Note that the gyroscope is smaller and cheaper, and the output (offset) varies and fluctuates more greatly when it is stationary. However, using a stabilized gyroscope makes the attitude detection device expensive and large. End up.
[0027]
The high-pass filter cancels the offset of the gyroscope. By using the output after using the high-pass filter, an inexpensive, small, and highly accurate attitude angle detection device can be obtained. It is set to a low frequency of 1 Hz or less. Each high-pass filter has a means for varying the cutoff frequency. In addition, since the low-frequency high-pass filter takes time to stabilize after the power is turned on, a stable output can be obtained in a short time by increasing the cutoff frequency of the high-pass filter as necessary. After the cutoff frequency is increased and a stable output is obtained, the cutoff frequency is immediately returned to a low state (measurement state).
[0028]
The outputs Jx (n), Jy (n), and Jz (n) of the respective gyroscopes according to the rotational angular velocity of the posture angle detection device (measurement object) mounted on the moving body are coordinated by the motion angle calculation device 31. From the output β (n−1) that is one unit time in the unit time calculated by conversion, the current movement angle (movement angle) ΔX (n) per unit time in the reference coordinate system is expressed as follows: Converted.
[0029]

[0030]
Next, the static angle calculation device will be described with reference to FIG. As described above, the static angle calculation device outputs a temporary roll angle R, a temporary pitch angle P, and a temporary yaw angle Φ that are posture angles during static operation of the posture angle detection device. is there.
[0031]
As shown in FIG. 2, the

acceleration sensors

10 and 11 shown in FIG. 1 respectively express the x and y component component forces Ax (n) and Ay (n) of the gravitational acceleration on the x and y axes in the sensor coordinate system. Arranged to detect. Further, the geomagnetic sensors 8 and 9 shown in FIG. 1 are arranged to detect geomagnetic component forces Mx (n) and My (n) on the x and y axes, respectively.
[0032]
Outputs Ax (n) and Ay (n) of the acceleration sensor are provisional roll angles that are inclination angles with respect to the horizontal plane (XY plane) of the reference coordinate system by the following formulas in the roll angle calculation means and the pitch angle calculation means, respectively. R (n) and provisional pitch angle P (n) are calculated.
[0033]
R (n) = sin ⁻¹ [Ax (n) / cosP (n)]
P (n) = sin ⁻¹ Ay (n)
[0034]
The provisional yaw angle Φ (n) in the reference coordinate system is calculated using the geomagnetic component in the reference coordinate system.
Φ (n) = tan ^-1 [HX (n) / HY (n)]
It is represented by The yaw angle can be calculated with high accuracy by comparing the yaw angle Φ _m (n) based on the output Az (n) of the acceleration sensor and the yaw angle Φ (n). When the attitude angle detection device rotates on a horizontal plane, the outputs Mx (n) and My (n) of the geomagnetic sensor are equal to HX (n) and HY (n), respectively.
Φ (n) = tan ⁻¹ [Mx (n) / My (n)]
Holds.
[0035]
If the azimuth angle at the origin is Φ (0), the yaw angle γ (n) is
γ (n) = Φ (n) −Φ (0)
It becomes.
[0036]
However, when the posture angle detection device has a roll angle R or a pitch angle P and is not horizontal,
HX = cos [P (n)] × Mx + sin [P (n)] × Mz,
HY = sin [R (n)] × sin [P (n)] × Mx + cos [R (n)]
X cos [P (n)] x Mz
Using the above formula, coordinate conversion is performed using the temporary roll angle R (n) and the temporary pitch angle P (n), and the geomagnetic component in the reference coordinate system is obtained.
Φ (n) = tan ^-1 [HX (n) / HY (n)]
Substitute into and find.
[0037]
At that time, the geomagnetic Z direction component Mz (n) is required.
[0038]
Mz is virtually calculated by the following method.
[0039]
First, the geomagnetism Ht is obtained by the following procedure. First, the attitude angle detection device (measurement object) is tilted 90 degrees so that the magnetic sensor 10 or the magnetic sensor 11 faces the Z axis of the reference coordinate system. Then, for example, a signal such as pressing a key or a button is sent, and the value of the sensor facing the Z-axis at that time is stored in the memory as Mz (0).
[0040]
Next, the attitude angle detection device (measurement object) is returned to the origin position, and some signal is sent again, and the values of the magnetic sensors Mx (n) and My (n) are stored as Mx (0) and My (0). To store. Using these Mx (0), My (0), and Mz (0), Ht is obtained from Equation 2.
[0041]
[Expression 2]

[0042]
Furthermore, in order to calculate the sign of Mz, it can be determined from the inclination angle of the geomagnetic north direction component of the attitude angle detection device (measurement object). For this purpose, a backup memory and a north component tilt angle calculation means are provided.
[0043]
The temporary yaw angle Φ (n−1) of one unit time in the unit time calculated by the calculation process is stored in the backup memory, and the temporary yaw angle Φ (n−1) and acceleration are stored. From the values of the sensor outputs Ax (n) and Ay (n), the north direction component inclination angle calculation means calculates the geomagnetic north direction component θ ₂ of the inclination angle of the attitude angle detection device (measurement object) by the following equation.
[0044]
θ ₂ = sin ⁻¹ [Ax (n) × cosΦ (n−1) + Ay (n) × sinΦ (n−1)]
If −90 degrees ≦ θ ₂ ≦ θ ₀ , the geomagnetic Mz code is determined to be negative, and if θ ₀ <θ ₂ ≦ 90 degrees, the geomagnetic Mz code is determined to be positive.
[0045]
Next, in the posture angle detection device of the present invention, an algorithm performed when the combined vector of the three-axis acceleration sensor is approximately 1G, that is, a temporary roll calculated by the static angle detection device when the number 1 is given. A determination device 33 for determining whether the angle R (n), the temporary pitch angle P (n), and the temporary yaw angle Φ (n) are correct will be described.
[0046]
[Equation 1]
| A | = (Ax + Ay + Az) ^1/2 ≈1
[0047]
Based on the acceleration Ax (n) in the X-axis direction and the acceleration Ay (n) in the Y-axis direction, the discriminating device 33 determines the roll angle R = sin ⁻¹ (Ax / cosP) and the pitch angle P = Find sin ⁻¹ Ay.
[0048]
However, here, Ax (n) and Ay (n) are combined vectors of the inclination angle component of gravitational acceleration and the X and Y-axis direction components of motion acceleration, and therefore when there is motion acceleration, the correct roll angle, The pitch angle is not reached. Therefore, the magnitude of the combined vector A of the triaxial acceleration sensor is obtained by Equation 3, and if this value is close to 1G, there is no motion acceleration in the axial direction, that is, the roll angle R and the pitch angle P are correct. Judgment is made and a static mode is set. Otherwise, it is determined that there is motion acceleration in the axial direction, the calculation result from the stationary angle detection means is not correct, and the dynamic mode is set.
[0049]
[Equation 3]

[0050]
Here, although the method of calculating and determining the output of the acceleration sensor has been described, it is determined that the output of the stationary angle detection means is not correct even when an error occurs in the output of the magnetic sensor and the output is greatly changed.
[0051]
Next, the attitude angle which obtains the final output from the motion angle calculation device 31 and the static angle calculation device 32 which calculate the output of the motion angle detection means and the output of the static angle detection means according to the determination result of the determination device 33 A calculation method performed by the calculation device 30 will be described.
[0052]
The basis of this calculation is that when the discriminating device 33 determines that the output of the stationary angle detection means is incorrect, the current posture angle detection result is the sum of the posture angle detection result and the exercise angle of one unit time in the past. When the output of the stationary angle detection means is determined to be correct by the determination device 33, the current posture angle detection result is obtained by subtracting the correction value from the addition of the posture angle detection result and the exercise angle of one unit past. It is. The correction value is obtained by multiplying the error by a proportional constant or a constant constant. Here, the error is obtained by subtracting the current detection result by the stationary angle detection means from the addition of the current motion angle to the posture angle detection result of one unit time in the past.
[0053]
When this is expressed by an equation, in the present embodiment, the output to be obtained by the attitude angle detection device of the present invention is α (n), β (n), γ (n), the motion angle obtained from the motion angle detection means. Is ΔX (n), ΔY (n), ΔZ (n), and the static angles obtained from the static angle detecting means are R (n), P (n), and Φ (n),
[0054]
When determining that the posture angle information from the stationary angle detection means is correct,
α (n) = α (n−1) + ΔX (n) −C ₁ ,
β (n) = β (n−1) + ΔY (n) −C ₂ ,
γ (n) = γ (n−1) + ΔZ (n) −C ₃ ,
Here, C ₁ , C ₂ , and C 3 are constants that are not larger than an arbitrarily selected error, and the sign calculates the attitude angle by minus when the sign of error is plus or minus when plus.
[0055]
Also, if it is determined that the posture angle information from the stationary angle detection means is incorrect,
α (n) = α (n−1) + ΔX (n),
β (n) = β (n−1) + ΔY (n),
γ (n) = γ (n−1) + ΔZ (n),
To calculate the attitude angle.
[0056]
As another embodiment of the present invention, α (n), β (n), γ (n) are outputs to be obtained by the attitude angle detection device of the present invention, and the motion angle obtained from the motion angle detection means is Δ. X (n), ΔY (n), ΔZ (n), and the static angles obtained from the static angle detecting means are R (n), P (n), and Φ (n).
[0057]
When determining that the posture angle information from the stationary angle detection means is correct,
α (n) = α (n−1) + ΔX (n) −k ₁ [α (n−1) + ΔX (n) −R (n)],
β (n) = β (n−1) + ΔY (n) −k ₂ [β (n−1) + ΔY (n) −P (n)],
γ (n) = γ (n−1) + ΔZ (n) −k ₃ [γ (n−1) + ΔZ (n) −Φ (n)],
To calculate the attitude angle. Here, k ₁ , k ₂ , and k ₃ are proportional constants of 1 or less that are arbitrarily selected.
[0058]
Also, if it is determined that the posture angle information from the stationary angle detection means is incorrect,
α (n) = α (n−1) + ΔX (n),
β (n) = β (n−1) + ΔY (n),
γ (n) = γ (n−1) + ΔZ (n),
To calculate the attitude angle.
[0059]
In this way, only when the output of the stationary angle detection means is correct, the posture angle is obtained by accumulating the motion angle while correcting the error, thereby simultaneously satisfying high-speed response, stability at rest, and reproducibility. This is an attitude angle detection device that can be used.
[0060]
FIG. 5 shows an example of an HMD to which the posture angle detection device according to the embodiment of the present invention is applied. This HMD is provided so that the image displayed on the display 1 in front of you changes in conjunction with the movement of the head so that you can experience the virtual space. If you turn right while wearing this HMD, Then, the posture angle of the head measured by the posture angle detection device 2 is transmitted to the video generation device 4 through the signal cable 3, and the video generation device 4 transmits the video corresponding to the posture angle to the HMD, so that it is expanded to the right. The video is projected, and is provided so that a 360-degree video of the entire space can be realized by the movement of the head.
[0061]
【The invention's effect】
As described above, when the attitude angle detection device according to the present invention is applied to an HMD, a high-performance HMD capable of obtaining angular information without a cumulative error without using an external signal and having a high-speed response is configured. Can do.
[0062]
Further, according to the attitude angle detection device of the present invention, since the gyroscope is mounted as an element, it is possible to reduce the size and weight, and since the ceramic vibrator is used, the magnetic sensor is not affected, and the magnetic sensor Even if they are close to each other, the function is not impaired. In addition, by using a high-pass filter, it is possible to reduce the size, price, and accuracy.
[0063]
Furthermore, according to the present invention, the three acceleration sensors and the two geomagnetic sensors can perform the determination / correction processing of the presence / absence of the error component with high accuracy by a simple calculation, with a high-speed response, An accurate attitude angle detection device can be provided.
[Brief description of the drawings]
FIG. 1 shows an arrangement of a gyroscope, an acceleration sensor, and a geomagnetic sensor of an attitude angle detection device according to the present invention, and a reference coordinate system (XYZ system) and a sensor coordinate system (xyz system) are one. The figure which shows the case where it has done.
FIG. 2 is a diagram illustrating a coordinate system in which a reference coordinate system (XYZ system) and a sensor coordinate system (xyz system) do not match.
FIG. 3 is a diagram showing a configuration of an attitude angle detection device according to the present invention.
FIG. 4 is a block diagram showing a configuration of a static angle calculation device in the posture angle detection device of the present invention.
FIG. 5 is an explanatory diagram of a usage state of a posture angle detection device in an HMD.
[Explanation of symbols]
1 Display 2 Attitude angle detector (Measurement object)
3 Signal Cable 4 Image Generator 5 (First) Gyroscope 6 (Second) Gyroscope 7 (Third) Gyroscope 8 (First) Geomagnetic Sensor 9 (Second) Geomagnetic Sensor 10 (First (1) acceleration sensor 11 (second) acceleration sensor 12 (third) acceleration sensor 30 attitude angle calculation device 31 motion angle calculation device 32 stationary angle calculation device 33 discrimination device 36 (first) high-pass filter 37 (first 2) high-pass filter 38 (third) high-pass filter

Claims

In an attitude angle detection device that detects rotation angles around three axes orthogonal to each other, with the X axis orthogonal to the plane and the axis orthogonal to the Y axis as the Z axis, the X axis, Y axis, and Z axis rotation Three gyroscopes for detecting the angular velocity of each, a motion angle calculation device for calculating a movement angle per unit time based on an output corresponding to the angular velocity of each gyroscope, and three for detecting the above three-axis acceleration Acceleration sensor, two geomagnetic sensors for detecting X-axis and Y-axis geomagnetism, and a static angle calculation for calculating a static angle around the three axes based on outputs of the acceleration sensor and the geomagnetic sensor A posture angle is calculated from the calculation results of the motion angle calculation device and the static angle calculation device in accordance with the calculation result of the determination device. Attitude angle calculation device Attitude-angle detecting apparatus characterized by being configured.

2. The posture angle detection according to claim 1, wherein the gyroscope includes a vibrator and a drive detection circuit for operating the vibrator as a gyroscope, and the vibrator is made of a nonmagnetic material. apparatus.

The posture angle detection device according to claim 1, wherein a high-pass filter is connected to an output unit of the gyroscope, and a movement angle per unit time is calculated using an output from the high-pass filter.

The posture angle detection device according to claim 3, wherein the high-pass filter includes a cutoff frequency varying unit.

Roll for calculating a temporary roll angle R (n) and a temporary pitch angle P (n) from the outputs Ax (n) and Ay (n) from the x-axis and y-axis acceleration sensors of the three-axis acceleration sensor. Angle calculation means, pitch angle calculation means, two-axis geomagnetic sensor for measuring the x-direction component Mx (n) and y-direction component My (n) of geomagnetism, and azimuth calculation of one unit time in the unit time to be calculated From the backup memory for storing the result Φ (n-1), the acceleration sensor outputs Ax (n), Ay (n) and the content Φ (n-1) of the backup memory, the north component of the inclination angle North direction component inclination angle calculating means for calculating θ ₂ , geomagnetic Mz code determining means for determining the sign of the geomagnetic Z direction component Mz, geomagnetic Z direction component geomagnetic Mz absolute value calculating means, and the geomagnetic Mz code determining Means and geomagnetic Mz absolute A geomagnetism Mz calculation means for calculating the geomagnetism Mz of the Z-axis direction component from the calculation means, and a coordinate for calculating the X, Y, Z components HX, HY, HZ of the geomagnetism in the reference coordinate system from the geomagnetism Mx, My, Mz. It is composed of transformation calculation means and azimuth calculation means for calculating the azimuth angle Φ by the above-mentioned HX and HY in the reference coordinate system, and the azimuth angle is obtained only by two geomagnetic sensor axes by virtually obtaining the geomagnetism Mz by the geomagnetism Mz calculation means. The posture angle detection device according to claim 1, wherein Φ (n) can be measured.

In the north direction component inclination angle calculation means and the geomagnetic Mz code determination means of the attitude angle detection device, the north direction component inclination angle θ ₂ , the geomagnetic dip angle θ ₀ at the origin, and the output of the backup memory are Φ (n−1), When the outputs of the two axes of the acceleration sensor are Ax (n) and Ay (n), the north direction component inclination angle θ ₂ is θ ₂ = sin ⁻¹ [Ax (n) · cos (−Φ (n−1) )) + Ay (n) · sin (−Φ (n−1))], and if −90 degrees ≦ θ ₂ ≦ θ ₀ , the geomagnetic Mz sign is negative and θ ₀ <θ ₂ ≦ 90 degrees. 6. The posture angle detection device according to claim 1, wherein if there is, the geomagnetic Mz code is determined to be positive.

Determining means for determining whether or not the detection results of the temporary yaw angle Φ, the temporary pitch angle P, and the temporary roll angle R obtained from the static angle detecting means including the triaxial acceleration sensor and the biaxial geomagnetic sensor are correct. As the synthesis level of the triaxial acceleration sensor is approximately 1G, the algorithm performed by the current output Ax (n), Ay (n), Az (n) of the acceleration sensor is stationary when 7. The posture angle detection apparatus according to claim 1, wherein the posture angle information from the angle detection means is determined to be correct.

The output to be obtained by the posture angle detection device is the roll angle α (n), the pitch angle β (n), the yaw angle γ (n), and the movement angle obtained from the movement angle detection means is ΔX (n), ΔY ( n), ΔZ (n), R (n), P (n), Φ (n), and constant constants C ₁ , C ₂ , C ₃ determined from the static angle detection means, and a discriminating device If it is determined that the posture angle information from the stationary angle detection means is correct, α (n) = α (n−1) + ΔX (n) −C ₁ , β (n) = β (n -1) + △ Y (n) -C 2, the γ (n) = γ (n -1) + △ Z (n) -C 3, also the attitude angle information from the angle of repose detecting means determines an error Α (n) = α (n−1) + ΔX (n), β (n) = β (n−1) + ΔY (n), γ (n) = γ (n−1) ) + ΔZ (n) to calculate the posture angle Attitude-angle detecting apparatus according to any one of claims 1 to 7.

The output to be obtained by the posture angle detection device is the roll angle α (n), the pitch angle β (n), the yaw angle γ (n), and the motion angle obtained from the motion angle detection means is ΔX (n), ΔY (N), ΔZ (n), posture angles obtained from the static angle detection means are R (n), P (n), Φ (n), and proportional constants of 1 or less are k ₁ , k ₂ , k ₃ Then, when it is determined that the posture angle information from the stationary angle detection means is correct based on the determination result of the determination device, α (n) = α (n−1) + ΔX (n) −k ₁ [α (n −1) + ΔX (n) −R (n)], β (n) = β (n−1) + ΔY (n) −k ₂ [β (n−1) + ΔY (n) − P (n)], γ (n) = γ (n−1) + ΔZ (n) −k ₃ [γ (n−1) + ΔZ (n) −Φ (n)] When it is determined that the posture angle information from the angle detection means is an error, α (n) = α (n 1) Posture angle is calculated by + ΔX (n), β (n) = β (n−1) + ΔY (n), γ (n) = γ (n−1) + ΔZ (n) The posture angle detecting device according to claim 1, wherein