JP3992206B2

JP3992206B2 - Attitude angle detector

Info

Publication number: JP3992206B2
Application number: JP02653898A
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-10-17
Anticipated expiration: 2018-01-23
Also published as: JPH11211480A

Description

【０００１】
【発明の属する技術分野】
本発明は、特に、移動体の姿勢検出や姿勢制御、バーチャルリアリティ等に使用されるヘッドマウントディスプレイ（以下、ＨＭＤという）のうち頭の姿勢角を検出するトラッカー、３Ｄゲームパッド等、３次元空間中で対象物の姿勢角（３軸の回転角）の検出に用いるのに適した姿勢角検出装置に関する。
【０００２】
【従来の技術】
従来、バーチャルリアリティーに使用され、頭の動きを検出する方法として、外部に設置された交流磁場発生源から微弱交流磁場を発生し、ＨＭＤに配置したセンサ部で検知し、制御演算部で頭の動きを検出する交流磁場を利用する方法や、外部に設置された超音波発生源からの超音波信号をＨＭＤに配置したセンサ部で検知し、制御演算部で頭の動きを検出する超音波を利用する方法がある。
【０００３】
【発明が解決しようとする課題】
しかしながら、前者の交流磁場を利用した方法では、信号発生源が微弱交流磁場のため、ノイズをキャンセルするためにフィルタを多用することにより応答性が低下し、頭の動きに比べて、ＨＭＤの映像の動きが遅くなり、気分を悪くしたり、酔いを発生する場合があった。
【０００４】
また、後者の超音波を利用した方法では、他の様々な超音波信号の影響を受けての誤動作や、信号発生源とセンサとの間の遮蔽物により信号を検出できなくなる可能性があり、センサの前に腕や髪の毛があるだけで受信不能になり、誤動作してしまうなどの問題があった。
【０００５】
本発明は、上記問題点を解消し、使用する場所や環境の制限を緩和し、幅広く使用できる姿勢角検出装置を提供するものである。
【０００６】
【課題を解決するための手段】
本発明は、水平面で直交するＸ軸、Ｙ軸及び該Ｘ軸、Ｙ軸のそれぞれに直交するＺ軸回りの角速度を検出する３個のジャイロと、該ジャイロの角速度に応じた出力に基づいて、単位時間に移動した角度である運動角を演算する運動角演算装置と、少なくともＸ−Ｙ平面で互いに直交する２軸の加速度を検出するように配置され、Ｘ軸回りの回転角であるロ−ル角、Ｙ軸回りの回転角であるピッチ角を検出するための加速度センサと、少なくとも前記２軸の地磁気を検出するよう配置される地磁気センサと、該加速度センサ及び地磁気センサの出力に基づいて、ロール角、ピッチ角及びＺ軸回りの回転角であるヨー角を演算する静止角演算装置と、前記静止角演算装置による演算結果の真偽を判定演算する判別装置と、該判別装置の演算結果に応じて、運動角演算装置の演算結果から出力する姿勢角を演算する姿勢角演算装置とから構成される姿勢角検出装置であって、前記ジャイロの出力部のそれぞれに接続されるＡ／Ｄコンバータと、該Ａ／Ｄコンバータの値をハイパスフィルタ処理するＩＩＲ（infinite impulse response：無限長インパルス応答）型デジタルハイパスフィルタと、前記Ａ／Ｄコンバータからの値を基準値を境に正負を判定するための正負反転判定装置と、前記ハイパスフィルタのＩＩＲ遅延係数をリセットするＩＩＲ遅延係数リセット装置とを備え、正負判定装置の正負が逆転したときに、ＩＩＲ遅延係数リセット装置により、前記ハイパスフィルタのＩＩＲ係数をリセットする姿勢角検出装置である。
【０００７】
【発明の実施の形態】
以下、図面によって本発明の実施の形態について説明する。
【０００８】
図１は、姿勢角検出装置を適用したＨＭＤの一例を示し、このＨＭＤは、目前にあるディスプレイに表示された映像が使用者の頭の動きに連動して変化し、仮想空間を体験できるように設けられているもので、このＨＭＤを装着したまま右を向くと、姿勢角検出装置で計測した頭の姿勢角を映像発生装置に送信し、映像発生装置は、その姿勢角にあった映像をＨＭＤに送信するので、右に展開する映像が映し出され、頭の動きによって全空間の３６０度の映像を実感できるように設けられているものである。
【０００９】
図１に示すように、符号１は、ＨＭＤ本体を示し、このＨＭＤ本体１には、本発明である姿勢角検出装置２が配置され、信号ケーブル４で映像発生装置７に接続されている。
【００１０】
図２は、本発明における姿勢角変換装置の全体構成を示すブロック図である。図２に示すように、３個のジャイロの出力から単位時間あたりの移動角（運動角）を演算して求める運動角演算装置１７を備えた運動角検出手段１１と、２個の地磁気センサと２個の加速度センサから静止角を求める静止角演算装置２４を備えた静止角検出手段２１と、静止角検出手段２１で求めた姿勢角に誤差が含まれるかどうか（例えば、直線的な加速度が加わって、傾斜計としての加速度センサに傾斜以外の信号が含まれる場合）を判別する判別装置３２と、運動角演算装置１７、静止角演算装置２４及び判別装置３２からの信号を処理する姿勢角演算装置４２とで構成されている。
【００１１】
図３は、本発明の姿勢角検出装置におけるジャイロ、加速度センサ、地磁気センサの配置を示す説明図である。図３に示すように、互いに直交する３軸（水平面で直交する軸をＸ軸、Ｙ軸、そのＸ軸、Ｙ軸それぞれに直交する軸をＺ軸、Ｘ軸回りの回転角度をロール角、Ｙ軸回りの回転角度をピッチ角、Ｚ軸回りの回転角度をヨー角と記す。）の回りの角速度を検出するための第１のジャイロ１２ａ、第２のジャイロ１２ｂ及び第３のジャイロ１２ｃは、Ｘ、Ｙ、Ｚ軸各軸に平行に、即ち互いに直交して配置されている。ヨー角を検出する第１の地磁気センサ２２ａ及び第２の地磁気センサ２２ｂは、水平面で互いに直交する２軸（ここでは、Ｘ軸及びＹ軸）とで構成されている。同様に、第１の加速度センサ２３ａ及び第２の加速度センサ２３ｂは、水平面で互いに直交する２軸（ここでは、Ｘ軸及びＹ軸）に配置されている。ジャイロは、圧電セラミック、圧電単結晶、あるいは、シリコン等の非磁性材料からなる振動子を用いることができる。本実施の形態では、（株）トーキン製圧電振動セラミックジャイロを用いた。
【００１２】
また、図２に示すように、３軸のジャイロ１２ａ、１２ｂ、１２ｃの出力部には、それぞれ、Ａ／Ｄコンバータ１３ａ、１３ｂ、１３ｃ、ＩＩＲ型デジタルハイパスフィルタ１４が接続され、ハイパスフィルタ１４の出力部にハイパスフィルタを介した値より移動した角度を演算する運動角演算装置１７が接続されている。さらに、ハイパスフィルタ１４のＩＩＲ遅延係数をリセットするＩＩＲ係数リセット装置１６と、Ａ／Ｄコンバータ１３ａ、１３ｂ、１３ｃからの値をある基準値（０点）を境に正負を判定するための正負反転判定装置１５とを備える。ハイパスフィルタ１４は、正負反転判定装置の正負が逆転したときにＩＩＲ係数をリセットし、従来のハイパスフィルタによる悪影響（戻り電圧）をキャンセルすることによって、ジャイロのオフセット電圧をキャンセルすると同時に、悪影響も排除し、高精度が得られる。
【００１３】
図４は、本発明の姿勢角検出装置のうち、静止角演算装置のブロック図である。図４に示すように、加速度センサの出力Ａｘ（ｎ）、Ａｙ（ｎ）からロール角Ｒ（ｎ）、ピッチ角Ｐ（ｎ）を演算するロール角演算手段、ピッチ角演算手段と、前回のヨー角演算結果Φ（ｎ−１）を記憶しておくためのバックアップメモリーと、加速度センサの出力Ａｘ、Ａｙ及びバックアメモリーの内容Φ（ｎ−１）より傾斜角度の北方向成分Θ₂ を演算する北方向成分傾斜角演算手段と、地磁気Ｚ方向成分Ｍｚのうち符号を判定する地磁気Ｍｚ符号判別手段と、地磁気Ｍｚ絶対値演算手段と、この地磁気Ｍｚ符号判別手段と地磁気Ｍｚ絶対値演算手段より地磁気のＺ軸方向成分Ｍｚを計算する地磁気Ｍｚ演算手段と、センサ座標系において、測定或いは求めた地磁気のＸ軸方向成分Ｍｘ、Ｙ軸方向成分Ｍｙと、Ｍｚから水平座標軸であるＨｘ、Ｈｙ、Ｈｚへの変換を行う座標変換演算手段と、このＨｘ、Ｈｙにより方位を演算する方位演算手段とから構成され、地磁気Ｍｚ演算手段により仮想的に地磁気のＺ軸方向成分Ｍｚを求めることにより、２軸の地磁気センサだけでヨー角を検出する。
【００１４】
さらに、図４に示すように、北方向成分傾斜角演算装置及び地磁気Ｍｚ符号判別手段において、北方向成分傾斜角θ₂、原点時の地磁気の伏角θ₀、バックアップメモリの出力をΦ（ｎ−１）、加速度センサ２軸の出力をそれぞれＡｘ、Ａｙとすると、
θ₂ ＝sin^-1［Ax(n)・cosΦ(n-1)＋Ay(n)・sinΦ(n-1)］
により求められ、−９０度≦θ₂≦θ₀であれば、地磁気Ｍｚ符号はマイナスであり、θ₀＜θ₂≦９０度であれば、地磁気Ｍｚ符号はプラスと判別する。
【００１５】
更に詳しく説明すると、移動体が水平面（即ちＸ−Ｙ平面）で回転する時、方位角Φ（ｎ）は地磁気センサからの出力Ｍｘ（ｎ）、Ｍｙ（ｎ）により、
Φ（ｎ）＝ｔａｎ^ー1［Ｍｘ（ｎ）／Ｍｙ（ｎ）］・・・（１）
と求められ、原点時の方位をΦ₀とすれば、方位角Ｙ（ｎ）は、
Ｙ（ｎ）＝Φ（ｎ）−Φ₀ ・・・（２）
となる。
【００１６】
しかし、移動体がロール角やピッチ角を持ち、水平でない時には、そのロール角、ピッチ角を用いて座標変換を行い、絶対座標での地磁気をＭｘ（ｎ）、Ｍｙ（ｎ）を求め、式（１）、（２）を適用する。その際、地磁気のＺ軸方向成分Ｍｚ（ｎ）が必要となる。
【００１７】
地磁気Ｚ方向成分Ｍｚ（ｎ）の求め方は、先ず符号を求めるために、Φ（ｎ−１）を用いて北方向成分（北方向のピッチ角）θ₂を求める。
θ₂＝sin^-1［Ax(n)・cosΦ(n-1)+Ay(n)・sinΦ(n-1)］
このθ₂と原点時にあらかじめ地磁気の伏角θ₀を求めて置き、比較する。
θ₀=tan^-1［Mz(0)/(Mx(0)²+My(0)²)^1/2］
−９０度≦θ₂≦θ₀であれば、Ｍｚの符号はマイナスであり、θ₀＜θ₂≦９０度であれば、Ｍｚの符号はプラスである。次に、Ｍｚの絶対値は地磁気Ｈｔが分かっていれば、数１で求まる。
【００１８】
【数１】

【００１９】
地磁気Ｈｔは、以下の手順で求める。まず、センサユニット部を９０度傾け磁気センサ１あるいは磁気センサ２がＺ軸に向くようにする。そして何らかの信号を送り、例えばキーやボタンを押すなどをし、その時のＺ軸に向いているセンサの値をＭｚ（０）としてメモリに格納する。次にセンサユニット部を原点位置に戻し、もう一度何らかの信号を送り、地磁気センサＭｘ（ｎ），Ｍｙ（ｎ）の値をＭｘ（０），Ｍｙ（０）としてメモリに格納する。これらＭｘ（０），Ｍｙ（０），Ｍｚ（０）を用いてＨｔは、数２となる。
【００２０】
【数２】

【００２１】
次に、本発明の姿勢角検出装置のうち、判別装置について示す。２軸の加速度センサと２軸の地磁気センサからなる静止角検出手段から求められる仮の姿勢角が正しいかどうかを判断する判別装置は、次のとおりである。
【００２２】
即ち、２軸の加速度センサ出力から得られるＸ軸方向の加速度Ａｘ（ｎ）及びＹ軸方向の加速度Ａｙ（ｎ）より、ロール角Ｒ（ｎ）及びピッチ角Ｐ（ｎ）を次式より求める。
【００２３】
Ｒ（ｎ）＝ｓｉｎ^-1［Ａｘ（ｎ）／ｃｏｓＰ（ｎ）］
Ｐ（ｎ）＝ｓｉｎ^-1Ａｙ（ｎ）
【００２４】
ここで、Ａｘ（ｎ）、Ａｙ（ｎ）は、重力加速度の傾斜角速度のＸ、Ｙ軸方向成分との合成ベクトルなので、運動加速度がある場合には正しいロール角、ピッチ角にならない。そこで、Ｒ（ｎ）からＲ（ｎ−１）を引いたものが０の近傍であり、あるいはＰ（ｎ）からＰ（ｎ−１）を引いたものが０の近傍であるならば、運動加速度がない、即ちＲ（ｎ）、とＰ（ｎ）が正しいと判断する。そうでない場合は、運動加速度があると判断する。即ち、数３あるいは数４のうち、少なくとも一つが、ほぼ０であるとき、静止角検出手段からの姿勢角情報は正しいと判定する。
【００２５】
【数３】

【００２６】
【数４】

【００２７】
上記、２軸の加速度センサから求められる判定結果をもとに、運動角検出手段及び静止角検出手段から求められる運動角及び静止角から、姿勢角演算装置により、本発明による姿勢角検出装置が求めるべき姿勢角を演算する。
【００２８】
次に、本発明の姿勢角検出装置のうち、姿勢角演算装置について示す。本発明の姿勢角検出装置の求めるべき出力をα（ｎ）、β（ｎ）、γ（ｎ）、動的な運動角検出手段から得られる運動角を△Ｘ（ｎ）、△Ｙ（ｎ）、△Ｚ（ｎ）、静的な静止角検出手段から求められる静止角をＲ（ｎ）、Ｐ（ｎ）、Φ（ｎ）とすると、静止角検出手段からの姿勢角情報は正しいと判断する場合、
α（ｎ）＝α（ｎ−１）＋△Ｘ（ｎ）−Ｃ
β（ｎ）＝β（ｎ−１）＋△Ｙ（ｎ）−Ｃ
γ（ｎ）＝γ（ｎ−１）＋△Ｚ（ｎ）−Ｃ
ここで、Ｃは任意に選択される誤差より大きくない定数で、符号は、誤差の符号がプラスの時、プラスであり、マイナスの時、マイナスである。
【００２９】
また、静止角検出手段からの姿勢角情報が誤と判断する場合、
α（ｎ）＝α（ｎ−１）＋△Ｘ（ｎ）
β（ｎ）＝β（ｎ−１）＋△Ｙ（ｎ）
γ（ｎ）＝γ（ｎ−１）＋△Ｚ（ｎ）
により、姿勢角を演算し、求める。
【００３０】
また、本発明の他の実施の形態として、静止角検出手段からの姿勢角情報は正しいと判断する場合、
α(ｎ)＝α(ｎ−１)＋△Ｘ(ｎ)−ｋ［α(ｎ−１)＋△Ｘ(ｎ)−Ｒ(ｎ)］
β(ｎ)＝β(ｎ−１)＋△Ｙ(ｎ)−ｋ［β(ｎ−１)＋△Ｙ(ｎ)−Ｐ(ｎ)］
γ(ｎ)＝γ(ｎ−１)＋△Ｚ(ｎ)−ｋ［γ(ｎ−１)＋△Ｚ(ｎ)−Φ(ｎ)］
により、姿勢角を演算する。ここで、ｋは任意に選択させる１以下の比例定数である。
【００３１】
また、静止角検出手段からの姿勢角情報が誤と判断する場合、
α（ｎ）＝α（ｎ−１）＋△Ｘ（ｎ）
β（ｎ）＝β（ｎ−１）＋△Ｙ（ｎ）
γ（ｎ）＝γ（ｎ−１）＋△Ｚ（ｎ）
により、姿勢角を演算し、求める。
【００３２】
従って、本実施の形態のように、本発明の姿勢角検出装置をＨＭＤに適用した場合、外部信号を用いることなく、高速応答で累積誤差のない角度情報を得ることができ、高性能である。
【００３３】
ジャイロの出力部にハイパスフィルタを挿入することにより、ドリフトの影響を除くことができるが、信号も影響を受けてしまい、正しい出力にならない［図５（ｃ）］。これは、コンデンサの戻り電圧によるもので、特に正だけ、あるいは負だけの出力が続いた時などに顕著に表れる。本発明のように、ジャイロの出力部のそれぞれに接続されるＡ／Ｄコンバータと、このＡ／Ｄコンバータの値をハイパスフィルタ処理するＩＩＲ型デジタルハイパスフィルタと、Ａ／Ｄコンバータからの値を基準値を境に正負を判定するための正負反転判定装置と、ハイパスフィルタのＩＩＲ遅延係数をリセットするＩＩＲ遅延係数リセット装置とを設けることによって、この影響をなくすことができる。フィルタの出力において、正負が反転した時にバッファをリセットする処理により、戻り電圧がなくなり、正しい出力が得られる［図５（ｄ）］。
【００３４】
なお、本発明による姿勢角検出装置において、ジャイロを素子で搭載しているので、小型軽量化が可能であり、セラミック振動子を使用しているので、磁気ノイズは受けず、磁気センサと近接させても、機能を損なうことがない。ＩＩＲ型デジタルハイパスフィルターを使用することにより、高精度化が可能である。
【００３５】
また、本実施の形態では、地磁気センサ、加速度センサが、それぞれ２個の例を示したが、少なくとも、どちらか一方が３個でもよい。小型化という点では、本実施の形態が最も好ましい。
【００３６】
【発明の効果】
本発明によれば、使用する場所や環境の制限を緩和し、幅広く使用できる姿勢角検出装置を提供することができた。
【図面の簡単な説明】
【図１】本発明のＨＭＤにおける姿勢角検出装置の使用状況の説明図。
【図２】本発明の姿勢角変換装置の全体構成を示すブロック図。
【図３】本発明の姿勢角検出装置におけるジャイロ、加速度センサ、地磁気センサの配置を示す説明図。
【図４】本発明の姿勢角検出装置における静止角演算装置の構成を示すブロック図。
【図５】運動角の出力波形を示す図。図５（ａ）は、ジャイロの入力波形を示す図。図５（ｂ）は、ジャイロの出力波形を示す図。図５（ｃ）は、ハイパスフィルタの出力波形を示す図。図５（ｄ）は、ＩＩＲ型デジタルハイパスフィルタの出力波形を示す図。
【符号の説明】
１ＨＭＤ本体
２姿勢角検出装置
４信号ケーブル
７映像発生装置
１１運動角検出手段
１２ａ第１のジャイロ
１２ｂ第２のジャイロ
１２ｃ第３のジャイロ
１３ａ（第１の）Ａ／Ｄコンバータ
１３ｂ（第２の）Ａ／Ｄコンバータ
１３ｃ（第３の）Ａ／Ｄコンバータ
１４（ＩＩＲ型デジタル）ハイパスフィルタ
１５正負反転判定装置
１６ＩＩＲ係数リセット装置
１７運動角演算装置
２１静止角検出手段
２２ａ第１の地磁気センサ
２２ｂ第２の地磁気センサ
２３ａ第１の加速度センサ
２３ｂ第２の加速度センサ
２４静止角演算装置
３２判別装置
４２姿勢角演算装置[0001]
BACKGROUND OF THE INVENTION
The present invention particularly relates to a tracker for detecting the posture angle of a head of a head mounted display (hereinafter referred to as HMD) used for posture detection, posture control, virtual reality, etc. of a moving body, a 3D game pad, etc. The present invention relates to a posture angle detection apparatus suitable for use in detecting a posture angle (triaxial rotation angle) of an object.
[0002]
[Prior art]
Conventionally, as a method for detecting head movement, which is used for virtual reality, a weak alternating magnetic field is generated from an alternating magnetic field generating source installed outside, detected by a sensor unit arranged in the HMD, and detected by a control calculation unit. A method that uses an alternating magnetic field to detect movement and an ultrasonic signal from an ultrasonic source installed outside is detected by a sensor unit arranged in the HMD, and an ultrasonic wave that detects head movement is detected by a control calculation unit. There are ways to use it.
[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 of the child slowed down, and sometimes felt sick and caused sickness.
[0004]
In addition, in the latter method using ultrasonic waves, there is a possibility that signals cannot be detected due to malfunctions due to the influence of other various ultrasonic signals, or shielding between the signal generation source and the sensor, There was a problem that reception could not be performed if there was an arm or hair just in front of the sensor, causing malfunction.
[0005]
The present invention provides a posture angle detection device that solves the above-described problems, relaxes restrictions on the place of use and environment, and can be used widely.
[0006]
[Means for Solving the Problems]
The present invention is based on three gyros that detect an X-axis, a Y-axis that are orthogonal to each other in a horizontal plane, and an angular velocity around the Z-axis that is orthogonal to each of the X-axis and the Y-axis, and an output corresponding to the angular velocity of the gyro. A motion angle computing device that computes a motion angle that is an angle moved per unit time, and at least a biaxial acceleration that is orthogonal to each other on the XY plane, and a rotation angle around the X axis. An acceleration sensor for detecting a pitch angle that is a rotation angle around the Y-axis, a geomagnetic sensor arranged to detect at least the biaxial geomagnetism, and outputs of the acceleration sensor and the geomagnetic sensor A static angle calculation device that calculates a roll angle, a pitch angle, and a yaw angle that is a rotation angle around the Z axis, a determination device that determines whether the calculation result by the static angle calculation device is true, and a determination device In the calculation result An attitude angle detection device comprising an attitude angle calculation device for calculating an attitude angle output from a calculation result of the motion angle calculation device, and an A / D converter connected to each of the output units of the gyro In addition, an IIR (infinite impulse response) type digital high-pass filter that performs high-pass filtering on the value of the A / D converter, and a value from the A / D converter to determine whether the value is positive or negative And an IIR delay coefficient reset device that resets the IIR delay coefficient of the high-pass filter, and when the positive / negative determination device reverses, the IIR delay coefficient reset device causes the IIR coefficient of the high-pass filter to be reversed. Is a posture angle detection device for resetting
[0007]
DETAILED DESCRIPTION OF THE INVENTION
Hereinafter, embodiments of the present invention will be described with reference to the drawings.
[0008]
FIG. 1 shows an example of an HMD to which an attitude angle detection device is applied. In this HMD, an image displayed on a display in front changes in accordance with the movement of a user's head so that a virtual space can be experienced. If the head is turned right with the HMD attached, the posture angle of the head measured by the posture angle detection device is transmitted to the image generation device, and the image generation device Is transmitted to the HMD, so that a video to be developed to the right is projected, and 360 ° video of the entire space can be realized by the movement of the head.
[0009]
As shown in FIG. 1, reference numeral 1 denotes an HMD main body. An attitude angle detection device 2 according to the present invention is arranged in the HMD main body 1 and connected to a video generation device 7 by a signal cable 4.
[0010]
FIG. 2 is a block diagram showing the overall configuration of the posture angle conversion apparatus according to the present invention. As shown in FIG. 2, a motion angle detection means 11 having a motion angle calculation unit 17 that calculates a movement angle (motion angle) per unit time from outputs of three gyros, two geomagnetic sensors, The stationary angle detecting means 21 provided with the stationary angle calculating device 24 for obtaining the stationary angle from the two acceleration sensors, and whether the posture angle obtained by the stationary angle detecting means 21 includes an error (for example, linear acceleration is In addition, when the acceleration sensor as the inclinometer includes a signal other than the inclination), the attitude angle for processing the signals from the motion angle calculation device 17, the stationary angle calculation device 24, and the determination device 32 And an arithmetic unit 42.
[0011]
FIG. 3 is an explanatory diagram showing the arrangement of the gyro, the acceleration sensor, and the geomagnetic sensor in the posture angle detection device of the present invention. As shown in FIG. 3, three axes orthogonal to each other (the X-axis and Y-axis are orthogonal to each other in the horizontal plane, the Z-axis is the axis orthogonal to each of the X-axis and Y-axis, the rotation angle around the X-axis is the roll angle, The first gyro 12a, the second gyro 12b, and the third gyro 12c for detecting angular velocities around the Y axis are referred to as a pitch angle and a rotation angle around the Z axis as a yaw angle. , X, Y, and Z axes are arranged in parallel to each other, that is, orthogonal to each other. The first geomagnetic sensor 22a and the second geomagnetic sensor 22b that detect the yaw angle are composed of two axes (here, the X axis and the Y axis) that are orthogonal to each other on a horizontal plane. Similarly, the first acceleration sensor 23a and the second acceleration sensor 23b are arranged on two axes (here, the X axis and the Y axis) orthogonal to each other on a horizontal plane. As the gyro, a vibrator made of a non-magnetic material such as a piezoelectric ceramic, a piezoelectric single crystal, or silicon can be used. In the present embodiment, a piezoelectric vibration ceramic gyro manufactured by Tokin Corporation was used.
[0012]
Further, as shown in FIG. 2, A /

D converters

13a, 13b, 13c, and an IIR digital high-pass filter 14 are connected to the output portions of the three-

axis gyros

12a, 12b, and 12c, respectively. A motion angle calculation device 17 is connected to the output unit for calculating the angle moved from the value via the high-pass filter. Further, the IIR coefficient reset device 16 that resets the IIR delay coefficient of the high-pass filter 14 and the positive / negative inversion for determining whether the value from the A /

D converters

13a, 13b, and 13c is positive or negative with a certain reference value (0 point) as a boundary. And a determination device 15. The high-pass filter 14 resets the IIR coefficient when the polarity of the positive / negative inversion determination device is reversed, and cancels the adverse effect (return voltage) due to the conventional high-pass filter, thereby canceling the gyro offset voltage and eliminating the adverse effect at the same time. High accuracy can be obtained.
[0013]
FIG. 4 is a block diagram of a static angle calculation device in the posture angle detection device of the present invention. As shown in FIG. 4, the roll angle calculation means, the pitch angle calculation means for calculating the roll angle R (n) and the pitch angle P (n) from the outputs Ax (n) and Ay (n) of the acceleration sensor, Calculates north component Θ ₂ of tilt angle from backup memory for storing yaw angle calculation result Φ (n−1), acceleration sensor outputs Ax and Ay, and content Φ (n−1) of backup memory A north direction component inclination angle calculating means, a geomagnetic Mz code determining means for determining a sign of the geomagnetic Z direction component Mz, a geomagnetic Mz absolute value calculating means, and a geomagnetic Mz code determining means and a geomagnetic Mz absolute value calculating means. Geomagnetism Mz calculating means for calculating the Z-axis direction component Mz of the geomagnetism, the X-axis direction component Mx, the Y-axis direction component My of the geomagnetism measured or obtained in the sensor coordinate system, and the horizontal coordinate axis H from Mz , Hy, Hz, a coordinate transformation calculation means, and an azimuth calculation means for calculating the azimuth by the Hx, Hy, and the geomagnetism Mz calculation means virtually obtains the Z-axis direction component Mz of the geomagnetism. Thus, the yaw angle is detected only by the biaxial geomagnetic sensor.
[0014]
Further, as shown in FIG. 4, in the north direction component inclination angle calculation device and the geomagnetic Mz code discriminating means, the north direction component inclination angle θ ₂ , the geomagnetic depression angle θ ₀ at the origin, and the output of the backup memory are Φ (n− 1) When the outputs of the acceleration sensor two axes are Ax and Ay, respectively,
θ ₂ = sin ^-1 [Ax (n) · cosΦ (n-1) + Ay (n) · sinΦ (n-1)]
If −90 degrees ≦ θ ₂ ≦ θ ₀ , the geomagnetic Mz code is negative, and if θ ₀ <θ ₂ ≦ 90 degrees, the geomagnetic Mz code is determined to be positive.
[0015]
More specifically, when the moving body rotates in the horizontal plane (that is, the XY plane), the azimuth angle Φ (n) is determined by outputs Mx (n) and My (n) from the geomagnetic sensor.
Φ (n) = tan ⁻¹ [Mx (n) / My (n)] (1)
If the azimuth at the origin is Φ ₀ , the azimuth angle Y (n) is
Y (n) = Φ (n) −Φ ₀ (2)
It becomes.
[0016]
However, when the moving body has a roll angle and a pitch angle and is not horizontal, coordinate conversion is performed using the roll angle and the pitch angle, and geomagnetism in absolute coordinates is obtained as Mx (n) and My (n). Apply (1) and (2). At that time, a geomagnetic Z-axis direction component Mz (n) is required.
[0017]
In order to obtain the geomagnetic Z direction component Mz (n), first, in order to obtain the sign, the north direction component (pitch angle in the north direction) θ ₂ is obtained using Φ (n−1).
θ ₂ = sin ^-1 [Ax (n) · cosΦ (n-1) + Ay (n) · sinΦ (n-1)]
This θ ₂ and the geomagnetic dip angle θ ₀ are obtained in advance at the origin and compared.
θ ₀ = tan ^-1 [Mz (0) / (Mx (0) ² + My (0) ² ) ^1/2 ]
If −90 degrees ≦ θ ₂ ≦ θ ₀ , the sign of Mz is negative, and if θ ₀ <θ ₂ ≦ 90 degrees, the sign of Mz is positive. Next, if the geomagnetism Ht is known, the absolute value of Mz can be obtained by Equation 1.
[0018]
[Expression 1]

[0019]
The geomagnetism Ht is obtained by the following procedure. First, the sensor unit is tilted 90 degrees so that the magnetic sensor 1 or the magnetic sensor 2 faces the Z axis. Then, a certain signal is sent, for example, a key or button is pressed, and the value of the sensor facing the Z-axis at that time is stored in the memory as Mz (0). Next, the sensor unit unit is returned to the origin position, and some signal is sent again, and the values of the geomagnetic sensors Mx (n) and My (n) are stored in the memory as Mx (0) and My (0). Using these Mx (0), My (0), and Mz (0), Ht is expressed by Equation 2.
[0020]
[Expression 2]

[0021]
Next, among the attitude angle detection devices of the present invention, a discrimination device will be described. A discriminating apparatus that determines whether or not the temporary posture angle obtained from the static angle detection means including the biaxial acceleration sensor and the biaxial geomagnetic sensor is correct is as follows.
[0022]
That is, the roll angle R (n) and the pitch angle P (n) are obtained from the following equations from the X-axis acceleration Ax (n) and the Y-axis acceleration Ay (n) obtained from the biaxial acceleration sensor output. .
[0023]
R (n) = sin ⁻¹ [Ax (n) / cosP (n)]
P (n) = sin ⁻¹ Ay (n)
[0024]
Here, Ax (n) and Ay (n) are combined vectors with the X and Y axis direction components of the gravitational acceleration tilt angular velocity, and therefore, when there is motion acceleration, the correct roll angle and pitch angle are not obtained. Therefore, if R (n) minus R (n-1) is in the vicinity of 0, or P (n) minus P (n-1) is in the vicinity of 0, the motion It is determined that there is no acceleration, that is, R (n) and P (n) are correct. Otherwise, it is determined that there is motion acceleration. That is, when at least one of Equation 3 or Equation 4 is approximately 0, it is determined that the posture angle information from the stationary angle detection means is correct.
[0025]
[Equation 3]

[0026]
[Expression 4]

[0027]
Based on the determination result obtained from the biaxial acceleration sensor described above, the posture angle detection device according to the present invention is obtained from the movement angle and stationary angle obtained from the movement angle detection means and the stationary angle detection means by the posture angle calculation device. Calculate the posture angle to be obtained.
[0028]
Next, among the posture angle detection devices of the present invention, a posture angle calculation device will be described. Α (n), β (n), γ (n) are outputs to be obtained by the posture angle detection device of the present invention, and the motion angles obtained from the dynamic motion angle detection means are ΔX (n), ΔY (n ), ΔZ (n), and R (n), P (n), and Φ (n) as the static angles obtained from the static static angle detecting means, the posture angle information from the static angle detecting means is correct. When judging
α (n) = α (n−1) + ΔX (n) −C
β (n) = β (n−1) + ΔY (n) −C
γ (n) = γ (n−1) + ΔZ (n) −C
Here, C is a constant not larger than an arbitrarily selected error, and the sign is positive when the sign of the error is plus, and is minus when the sign of the error is minus.
[0029]
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 and obtain the attitude angle.
[0030]
As another embodiment of the present invention, when it is determined 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 is a proportionality constant of 1 or less that is arbitrarily selected.
[0031]
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 and obtain the attitude angle.
[0032]
Therefore, when the attitude angle detection device of the present invention is applied to an HMD as in this embodiment, angle information without a cumulative error can be obtained without using an external signal, and high performance is achieved. .
[0033]
By inserting a high-pass filter in the output part of the gyro, the influence of drift can be eliminated, but the signal is also affected and the output is not correct [FIG. 5 (c)]. This is due to the return voltage of the capacitor, and is particularly noticeable when a positive or negative output continues. As in the present invention, an A / D converter connected to each of the output units of the gyro, an IIR digital high-pass filter that performs high-pass filtering on the value of the A / D converter, and a value from the A / D converter as a reference This effect can be eliminated by providing a positive / negative inversion determination device for determining positive / negative with respect to the value and an IIR delay coefficient reset device for resetting the IIR delay coefficient of the high-pass filter. In the output of the filter, the process of resetting the buffer when the polarity is inverted causes the return voltage to disappear and a correct output is obtained [FIG. 5 (d)].
[0034]
In the attitude angle detection device according to the present invention, since the gyro is mounted as an element, it is possible to reduce the size and weight, and since a ceramic vibrator is used, it is not affected by magnetic noise and is brought close to the magnetic sensor. However, the function is not impaired. High accuracy can be achieved by using an IIR type digital high-pass filter.
[0035]
In this embodiment, two examples of the geomagnetic sensor and the acceleration sensor are shown, but at least one of them may be three. This embodiment is most preferable in terms of downsizing.
[0036]
【The invention's effect】
According to the present invention, it is possible to provide a posture angle detection device that relaxes restrictions on the place of use and environment and can be used widely.
[Brief description of the drawings]
FIG. 1 is an explanatory diagram of a usage state of an attitude angle detection device in an HMD of the present invention.
FIG. 2 is a block diagram showing the overall configuration of the posture angle conversion apparatus of the present invention.
FIG. 3 is an explanatory diagram showing an arrangement of a gyro, an acceleration sensor, and a geomagnetic sensor in the posture angle detection device of 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 a diagram showing an output waveform of an exercise angle. FIG. 5A shows a gyro input waveform. FIG. 5B shows a gyro output waveform. FIG.5 (c) is a figure which shows the output waveform of a high pass filter. FIG. 5D shows an output waveform of the IIR digital high-pass filter.
[Explanation of symbols]
DESCRIPTION OF SYMBOLS 1 HMD main body 2 Attitude angle detection apparatus 4 Signal cable 7 Image | video generator 11 Motion angle detection means 12a 1st gyro 12b 2nd gyro 12c 3rd gyro 13a (1st) A / D converter 13b (2nd ) A / D converter 13c (third) A / D converter 14 (IIR type digital) high-pass filter 15 positive / negative inversion determining device 16 IIR coefficient resetting device 17 motion angle calculating device 21 static angle detecting means 22a first geomagnetic sensor 22b 2nd geomagnetic sensor 23a 1st acceleration sensor 23b 2nd acceleration sensor 24 Stationary angle calculating device 32 Discriminating device 42 Attitude angle calculating device

Claims

Based on three gyros that detect the angular velocity around the X-axis, Y-axis, and the X-axis and Y-axis that are orthogonal to each other in the horizontal plane, and the output corresponding to the angular velocity of the gyro, in unit time A movement angle calculation device that calculates a movement angle that is a moved angle, and a roll angle that is a rotation angle about the X axis, arranged to detect at least two axes of acceleration that are orthogonal to each other in the XY plane, An acceleration sensor for detecting a pitch angle that is a rotation angle about the Y axis, a geomagnetic sensor arranged to detect at least the biaxial geomagnetism, and a roll angle based on outputs of the acceleration sensor and the geomagnetic sensor A static angle calculation device that calculates a pitch angle and a yaw angle that is a rotation angle around the Z axis, a determination device that determines whether the calculation result by the static angle calculation device is true or false, and a calculation result of the determination device Luck An attitude angle detection device comprising an attitude angle calculation device that calculates an attitude angle output from a calculation result of an angle calculation device, comprising: an A / D converter connected to each of the output units of the gyro; IIR type digital high-pass filter that performs high-pass filter processing on the value of the A / D converter, a positive / negative inversion determination device for determining whether the value from the A / D converter is a reference value, and the IIR delay coefficient of the high-pass filter And an IIR delay coefficient reset device for resetting the attitude, and when the positive / negative of the positive / negative determination device reverses, the IIR delay coefficient reset device resets the IIR coefficient of the high-pass filter.