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