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