(57)【特許請求の範囲】
【請求項１】
水平面内で直交するＸ軸、Ｙ軸、及びそのＸ軸、Ｙ軸に直交するＺ軸が形成する基準座標系において、少なくともＹ軸回りの傾斜角度とＺ軸回りの方位角度を検出する姿勢角度検出装置において、少なくともＹ軸回り及びＺ軸回りの角速度を検出するジャイロと、Ｘ−Ｙ平面で互いに直交する２軸の加速度を検出する加速度センサと、Ｘ−Ｙ平面で互いに直交する２軸の地磁気を検出する磁気センサと、前記ジャイロ、加速度センサ及び磁気センサのオフセット電圧値及び検出感度を保存している記憶手段と、前記ジャイロの出力と前記記憶手段に保存されているデータに基づいて単位時間に移動した角度を演算する運動角検出装置と、前記加速度センサ及び地磁気センサの出力と、前記記憶手段に保存されているデータと、あらかじめ与えられている地磁気の俯角とに基づいて、Ｙ軸及びＺ軸回りの静止角を演算する静止角演算装置と、前記与えられている地磁気の俯角と実際の地磁気の俯角との誤差を自動的に補正する俯角自動補正装置と、静止角演算装置による演算結果の真偽を判別する判別装置と、該判別装置の判断に応じて、運動角演算装置と静止角演算装置の演算結果から出力するべき姿勢角を演算する姿勢角演算装置から構成される姿勢角度検出装置であって、前記俯角自動補正装置はＺ軸回りの姿勢角度をα、Ｚ軸回りの静止角をφ、Ｙ軸回りの姿勢角度をβ、前記静止角演算装置によって求められるＺ軸方向の地磁気の大きさＭｚとすると、（１）（φ−α）×α×β×Ｍｚの符号を計算する、（２）負の場合は与えられている地磁気の俯角を減らす、（３）正の場合は与えられている地磁気の俯角を増やす、の手順に従って、与えられている地磁気の俯角を増減することを特徴とする姿勢角度検出装置。
【請求項２】
請求項１記載の姿勢角度検出装置において、前記与えられている地磁気への俯角の増減は、一定値Ｃだけ加減することを特徴とする姿勢角度検出装置。
【請求項３】
請求項１記載の姿勢角度検出装置において、前記与えられている地磁気への俯角の増減は一定率ｋだけ加減することを特徴とする姿勢角度検出装置。
【請求項４】
請求項１ないし３のいずれか一項に記載の姿勢角度検出装置において、前記記憶手段は地磁気の俯角を保存しており、前記俯角自動補正装置によって地磁気の俯角が補正された後に記憶手段に補正されたデータを保存することを特徴とする姿勢角度検出装置。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、ジャイロ、加速度センサ、磁気センサを用いた小型の姿勢角度検出装置に関する。これら姿勢角度検出装置は、ゲームパッド、ＨＭＤ、携帯電話などに用いられる。
【０００２】
【従来の技術】
従来、飛行機や船舶の姿勢計測・制御に用いられてきた姿勢角度検出装置ではジャイロが３つ、加速度センサが３つ、磁気センサが３つという構成であった。本来は、姿勢角度検出装置が傾いたときには方位角度を求めるために３軸の磁気センサ出力が必要となる。それぞれのセンサを低価格なものを選択することによって、ある程度、低価格化ができるが、価格が最重要視されるゲームや携帯電話などの民生用途では、さらなる低価格化が求められる。特開平11-211474には、磁気センサが２軸の構成における姿勢角度の計算方法が下記のように記述されている。
【０００３】
仮想磁気Ｚ方向成分Ｍｚを以下の方法で算出する。
１）姿勢角度検出装置を９０度傾けて磁気センサＸあるいは磁気センサＹがＺ軸に向くようにする。
２）ボタンを押すなどして、そのときのＺ軸に向いているセンサの値をＭｚ（０）としてメモリに格納する。
３）姿勢角度検出装置を原点位置に戻し、もう一度ボタンを押すなどし、磁気センサＸ、Ｙの値をＭｘ（０）、Ｍｙ（０）としてメモリに格納する。
４）Ｍｘ（０）、Ｍｙ（０）、Ｍｚ（０）を用いて、地磁気の大きさＨｔを次の式１により求める。
【０００４】
Ｈｔ＝［Ｍｘ(０)2＋Ｍｙ(０)2＋Ｍｚ(０)2]1/2・・・式１
【０００５】
５）仮想磁気Ｚ方向成分Ｍｚの大きさは数１により求まる。
【０００６】
【数１】
0003797661.tif 000002
【０００７】
ここでは、原点における地磁気のＺ方向成分Ｍｚ（０）をあらかじめ求めているが、地磁気の俯角θを求めておいても地磁気の大きさＨｔを式２によって求めることができる。
【０００８】
Ｈｔ＝［Ｍｘ(０)2＋Ｍｙ(０)2]1/2／ｃｏｓθ・・・式２
【０００９】
【発明が解決しようとする課題】
しかしながら、上記の方法を用いる場合、実際に使用する環境において地磁気の俯角を測定しておく必要がある。この測定はユーザーが使用する前に行うもので、煩わしい作業がユーザーの負担になる。また、ユーザーが測定を誤ると、実際とは異なる俯角が保存されてしまうので、このデータを用いて前記の計算を行うと、仮想磁気Ｚ方向成分Ｍｚに誤差を生じる、従って最終的に方位角度αにも誤差を生じる。
【００１０】
従って、本発明は、ユーザーの煩わしい作業負担をなくしても、またはユーザーが測定を怠っても、仮想磁気Ｚ方向成分Ｍｚの誤差を自動的に補正する姿勢角度検出装置を提供することである。
【００１１】
【課題を解決するための手段】
本発明は、前記の姿勢角度検出装置の問題点の解消を図るもので、与えられた地磁気の俯角が誤っていても使用中に自動的に補正され、最終的には実際の地磁気の俯角に収束するアルゴリズムである。このアルゴリズムによって、誤った地磁気の俯角が与えられたり、途中で使用環境が変わったりしても常に実際の地磁気の俯角が得られ、精度の良い理想的な姿勢角度検出装置を提供するものである。
【００１２】
即ち、本発明は、水平面内で直交するＸ軸、Ｙ軸、及びそのＸ軸、Ｙ軸に直交するＺ軸が形成する基準座標系において、少なくともＹ軸回りの傾斜角度とＺ軸回りの方位角度を検出する姿勢角度検出装置において、少なくともＹ軸回り及びＺ軸回りの角速度を検出するジャイロと、Ｘ−Ｙ平面で互いに直交する２軸の加速度を検出する加速度センサと、Ｘ−Ｙ平面で互いに直交する２軸の地磁気を検出する磁気センサと、前記ジャイロ、加速度センサ及び磁気センサのオフセット電圧値及び検出感度を保存している記憶手段と、前記ジャイロの出力と前記記憶手段に保存されているデータに基づいて単位時間に移動した角度を演算する運動角検出装置と、前記加速度センサ及び地磁気センサの出力と、前記記憶手段に保存されているデータと、あらかじめ与えられている地磁気の俯角とに基づいて、Ｙ軸及びＺ軸回りの静止角を演算する静止角演算装置と、前記与えられている地磁気の俯角と実際の地磁気の俯角との誤差を自動的に補正する俯角自動補正装置と、静止角演算装置による演算結果の真偽を判別する判別装置と、該判別装置の判断に応じて、運動角演算装置と静止角演算装置の演算結果から出力するべき姿勢角を演算する姿勢角演算装置から構成されることを特徴とする姿勢角度検出装置である。
【００１３】
また、本発明は、前記姿勢角度検出装置において、前記俯角自動補正装置はＺ軸回りの姿勢角度をα、Ｚ軸回りの静止角をφ、Ｙ軸回りの姿勢角度をβ、前記静止角演算装置によって求められるＺ軸方向の地磁気の大きさＭｚとすると、
（１）（φ−α）×α×β×Mzの符号を計算する、
（２）負の場合は与えられている地磁気の俯角を減らす、
（３）正の場合は与えられている地磁気の俯角を増やす、
の手順に従って、与えられている地磁気の俯角を増減する姿勢角度検出装置である。
【００１４】
また、本発明は、前記姿勢角度検出装置において、前記与えられている地磁気への俯角の増減は、一定値Ｃだけ加減する姿勢角度検出装置である。
【００１５】
また、本発明は、前記姿勢角度検出装置において、前記与えられている地磁気への俯角の増減は一定率kだけ加減する姿勢角度検出装置である。
【００１６】
また、本発明は、前記姿勢角度検出装置において、前記記憶手段は地磁気の俯角を保存しており、前記俯角自動補正装置によって地磁気の俯角が補正された後に記憶手段に補正されたデータを保存する姿勢角度検出装置である。
【００１７】
【発明の実施の形態】
本発明の実施の形態による姿勢角度検出装置について、以下に説明する。なお、今回は姿勢角度検出装置の出力が２軸のもの［Ｘ軸回りの傾斜角度（ロール角）は常に０と考える］を説明するが、３軸のものについても同様に適応できる。
【００１８】
図１は、本発明の実施の形態による姿勢角度検出装置のセンサ郡の構成図であり、基準座標系（Ｘ−Ｙ−Ｚ系）とセンサ座標系（Ｘｓ−Ｙｓ−Ｚｓ系）が一致している場合について示す。図１に示すように、水平面で直交する軸をＸ軸、Ｙ軸とし、そのＸ軸、Ｙ軸それぞれに直交する軸をＺ軸とするとき、これら互いに直交するＸ軸、Ｙ軸、Ｚ軸が形成する基準座標系において、Ｙ軸回りの傾斜角度をピッチ角β、Ｚ軸回りの方位角度をヨー角αと記す。このとき、基準座標の＋Ｘ軸は北方向とする。
【００１９】
図１には、本発明の姿勢角度検出装置におけるジャイロ、加速度センサ、磁気センサの配置を示す。図１で示すセンサ座標系において、互いに直交する２軸（Ｙｓ軸及びＺｓ軸）の回りの角速度を検出するための第１のジャイロ１１、及び第２のジャイロ１２は、Ｙｓ軸及びＺｓ軸に平行に、すなわち互いに直交して、配置されている。第１の加速度センサ１３と第２の加速度センサ１４は、互いに直交する２軸、Ｘｓ軸及びＹｓ軸に平行に配置されている。同様に、第１の磁気センサ１５と第２の磁気センサ１６は、互いに直交する２軸、Ｘｓ軸及びＹｓ軸に平行に配置されている。
【００２０】
図２は、本発明の実施の形態における姿勢角度検出装置の全体構成の説明図である。ジャイロ２２、加速度センサ２３、及び磁気センサ２４からは、それぞれ角速度（ωｙ，ωｚ）、加速度（Ａｘ，Ａｙ）、磁気（Ｍｘ，Ｍｙ）が出力される。
【００２１】
運動角演算装置２６ａは、ジャイロから出力される角速度（ωｙ，ωｚ）とEEPROM２５に保存されているジャイロのオフセット電圧値、検出感度から単位時間に移動した姿勢角度である運動角（Δα，Δβ）を演算し出力する。静止角演算装置２６ｂは、加速度センサから出力される加速度（Ａｘ，Ａｙ）と、磁気センサから出力される磁気（Ｍｘ，Ｍｙ）と、EEPROM２５に保存されている加速度センサ及び磁気センサのオフセット電圧値、検出感度、地磁気の俯角から仮の姿勢角度である静止角（φ，Ｐ）を演算し出力する。このように、記憶手段としてEEPROMを利用するのが適当であるが、他の例として通常のROMも利用できる。
【００２２】
判別装置２７は、運動角と静止角を比較して静止角が正しいかどうかを判別し、姿勢角検出装置に結果を出力する。姿勢角検出装置２７ｃは、運動角（Δα，Δβ）、静止角（φ，Ｐ）と判別装置の出力から最終的に出力する姿勢角度（α，β）を演算し、外部に出力する。
【００２３】
俯角自動補正装置２８は、静止角検出装置から得られる仮の方位角度φ、仮想磁気Ｚ方向成分Ｍｚ、地磁気の俯角θと、姿勢角検出装置の出力（α，β）から地磁気の俯角θを補正し、静止角演算装置に戻す。静止角演算装置は、プログラム終了時に記憶手段に補正された地磁気の俯角θを書き込む。
【００２４】
最初に、運動角演算装置の内容を説明する。姿勢角度検出装置に加わる角速度に応じた各ジャイロの出力（ωy,ωz）は、座標変換を行って、演算する単位時間で１単位時間過去の出力β（ｎ−１）とから、次の式３、式４の如く、基準座標系における現在の移動角度（運動角）（Δα,Δβ）に変換される。
【００２５】
Δα（ｎ）＝ωｚ／ｃｏｓβ（ｎ−１）・・・・式３
【００２６】
Δβ（ｎ）＝ωｙ  ・・・・・・・式４
【００２７】
図３は、本発明の実施の形態による姿勢角度検出装置における静止角演算装置の説明図である。先の図１に示す第１の加速度センサ１３は、センサ座標系での重力加速度のＸｓ成分分力Ａｘ（ｎ）を検出するように配置されている。また、図１に示す第１の磁気センサ１５、第２の磁気センサ１６は、それぞれＸｓ、Ｙｓ軸上の地磁気分力Ｍｘ（ｎ）、Ｍｙ（ｎ）を検出するように配置されている。
【００２８】
加速度センサの出力Ａｘ（ｎ）は、ピッチ角演算手段にて式５により、基準座標系の水平面（ＸＹ平面）との傾斜角である仮のピッチ角Ｐ（ｎ）を算出する。このときロール角Ｒは、常に０と仮定する。
【００２９】
Ｐ（ｎ）＝ｓｉｎ−１Ａｘ（ｎ）   ・・・・・・・・式５
【００３０】
基準座標系での仮のヨー角φ（ｎ）は、基準座標系での地磁気成分を用いて、式６で表される。ただし、φ（０）は原点時の方位角である。
【００３１】
φ（ｎ）＝−ｔａｎ-1［ＨＹ（ｎ）／ＨＸ（ｎ）］−φ（０）・・・式６
【００３２】
姿勢角度検出装置が水平面上で回転する場合は、磁気センサの出力Ｍｘ（ｎ）及びＭｙ（ｎ）が、それぞれ、ＨＸ（ｎ）及びＨＹ（ｎ）に等しく、式７が成り立つ。
【００３３】
φ（ｎ）＝−ｔａｎ-1［Ｍｙ（ｎ）／Ｍｘ（ｎ）］−φ（０）・・・・式７
【００３４】
しかし、姿勢角度検出装置がピッチ角Pを持ち、水平でないときには、式８、式９の関係式から、仮のピッチ角Ｐ（ｎ）を用いて座標変換を行い、基準座標系での地磁気成分［ＨＸ（ｎ），ＨＹ（ｎ）とする］を求めて、式６に代入し、求める。その際、仮想磁気Ｚ方向成分Ｍｚ（ｎ）が必要となる。
【００３５】
ＨＸ＝ｃｏｓ［Ｐ（ｎ）］×Ｍｘ＋ｓｉｎ［Ｐ（ｎ）］×Ｍｚ・・・・式８
【００３６】
ＨＹ＝Ｍｙ  ・・・・・・式９
【００３７】
以下の方法で仮想的にMzを算出する。まず、地磁気の俯角θの初期値を記憶手段に保存しておく、この初期値は姿勢角度検出装置の使用場所において測定することが望ましいが、適当な値を保存しておけばよい。例えば、東京の付近では地磁気の俯角は４８度くらいなので、この値にしておいてもよい。地磁気の俯角θは、後に示す俯角自動補正装置によって随時補正される。
【００３８】
次に、地磁気Ｈｔを以下の手順で求める。姿勢角度検出装置（被測定物）を原点位置（水平）にし、何らかの信号を送り、例えば、キーやボタンを押すなどをし、磁気センサＭｘ、Ｍｙの値をＭｘ（０）、Ｍｙ（０）としてメモリに格納する。Ｍｘ（０）、Ｍｙ（０）、θを用いて、Ｈｔは式１０から求まる。
【００３９】
Ｈｔ＝［Ｍｘ(０)2＋Ｍｙ(０)2]1/2／ｃｏｓθ・・・・式１０
【００４０】
次に、姿勢角度検出装置（被測定物）の北方向成分傾斜角から仮想磁気Ｍｚの符号を判別する。静止角演算装置にて算出した、演算する単位時間で、１単位時間過去の仮のヨー角φ（ｎ−１）及び同様に姿勢角演算装置にて算出したβ（ｎ−１）をバックアップメモリに記憶しておき、この仮のヨー角φ（ｎ−１）とピッチ角β（ｎ−１）から、北方向成分傾斜角演算手段で、姿勢角度検出装置（被測定物）の北方向成分傾斜角θ２を以下の手順で求める。基準座標Ｘ−Ｚ平面とセンサ座標Ｘｓ−Ｙｓ平面とが交差する直線の方向ベクトルを（ａａ，０，ｃｃ）とすると、
ａａ＝ｓｉｎ［β（ｎ−１）］×ｃｏｓ［φ（ｎ−１）＋φ（０）］、
ｃｃ＝ｃｏｓ［β（ｎ−１）］、
となり、北方向成分傾斜角θ2は、
θ2＝ｓｉｎ-1（ａａ／ａａ2＋ｃ2)1/2、
と求められる。−９０度≦θ2≦θであれば、仮想磁気Ｍｚ符号はマイナス、θ＜θ2≦９０度であれば、仮想磁気Ｍｚ符号はプラスと判別する。
【００４１】
次に、仮想磁気Ｍｚの絶対値を求める。地磁気の絶対値Ｈｔと磁気センサの出力Ｍｘ（ｎ）、Ｍｙ（ｎ）の値から仮想磁気Ｍｚの絶対値を数１により算出する。
【００４２】
以上のように、仮想磁気Ｍｚが求まったことにより、前述したＨＸ、ＨＹが求まり、従ってφ（ｎ）を求めることができる。
【００４３】
次に、本発明の姿勢角度検出装置における、判別装置を説明する。加速度センサの出力Ａｘ、Ａｙは、重力加速度のＸｓ、Ｙｓ軸方向成分と運動加速度のＸｓ、Ｙｓ軸方向成分との合成ベクトルとなるために、運動加速度がある場合には静止角演算装置によって算出された仮のピッチ角Ｐは正しいピッチ角にはならない。また、仮のヨー角φについても磁場環境によって正しいヨー角にならない場合がある。判別装置は、静止角演算装置の出力であるφ（ｎ）、Ｐ（ｎ）が正しいかどうかを判別するものである。
【００４４】
演算する単位時間で、１単位時間に変化したφ、ＰをΔφ、ΔＰとし、これらを運動角演算装置の出力Δα、Δβと比較する。微小定数εを用いて、数２が成立するならば、それぞれφ、Ｐは正しいと判断する。
【００４５】
【数２】
0003797661.tif 000003
【００４６】
次に、上記判別装置の判別結果に応じて、運動角演算装置の出力と静止角演算装置の出力を演算して最終的な出力を得る姿勢角演算装置で行われる演算方法について説明する。判別装置においてφ、Ｐが正しいと判断された場合には、
α（ｎ）＝α（ｎ−１）＋Δα−ｋ［α（ｎ−１）＋Δα−φ（ｎ）］、
β（ｎ）＝β（ｎ−１）＋Δβ−ｋ［β（ｎ−１）＋Δβ−Ｐ（ｎ）］、
により姿勢角度を演算し、判別装置においてφ、Ｐが誤っていると判断された場合には、
α（ｎ）＝α（ｎ−１）＋Δα、β（ｎ）＝β（ｎ−１）＋Δβ、
により姿勢角度を演算する。
【００４７】
最後に、本発明における俯角自動補正装置について説明する。静止角演算装置では、あらかじめ記憶手段に保存されている地磁気の俯角θを用いて演算を行っているが、最初に保存した地磁気の俯角が誤っていた場合や、移動などによって周囲の磁場環境が変化して、地磁気の俯角が変わったときには、実際の地磁気の俯角と演算に使われる地磁気の俯角とに誤差を生じる。この誤差は、地磁気の絶対値Ｈｔ、仮想磁気Ｍｚ、基準座標系での地磁気成分ＨＸ、ＨＹ、仮のヨー角φへと順番に影響し、最終的には姿勢角演算装置の出力α（ｎ）に誤差を生じる。
【００４８】
この地磁気の俯角θの誤差による仮のヨー角φ（ｎ）の誤差は、単純な比例の関係にはならないが、その符号は（φ−α）、α、β、Ｍｚのそれぞれの符号によって反転することがわかっている。すなわち、（φ−α）×α×β×Ｍｚの符号が正の時には、φ（ｎ）の誤差はマイナス、逆に（φ−α）×α×β×Ｍｚの符号が負の時には、φ（ｎ）の誤差はプラスとなる。従って、（φ−α）×α×β×Ｍｚの符号を計算し、正であれば、φを一定量あるいは一定率増やし、負であれば、φを一定量あるいは一定率減らすことによって、自動的に正しい地磁気の俯角に収束することになる。
【００４９】
さらに、姿勢角度検出装置の動作を終了するときに補正された地磁気の俯角θを記憶手段に保存することによって、次に使用するときには初めから正しい地磁気の俯角で計算ができ、精度良く出力できるようになっている。
【００５０】
【発明の効果】
以上、本発明によれば、地磁気センサを従来の３軸から２軸に減らし、低価格化を進めるとともに、俯角自動補正装置を備えていることによって、あらかじめ記憶手段に保存されている地磁気の俯角θが誤っている場合、あるいは移動する用途に使用し、周囲の磁場環境が変化した場合でも、常に正しい地磁気の俯角に補正し、従って、誤差の少ない姿勢角度検出装置を提供できる。また、姿勢角度検出装置の出荷時に記憶手段に適当な地磁気の俯角、例えば、東京の４８度を保存しておけば、ユーザーが使用前に調整を行う必要がなく、ユーザーに負担がかからない製品が実現できる。
【図面の簡単な説明】
【図１】本発明の実施の形態による姿勢角度検出装置のセンサ群の構成図。
【図２】本発明の実施の形態による姿勢角度検出装置の全体構成に関する説明図。
【図３】本発明の実施の形態による姿勢角度検出装置における静止角演算装置の説明図。
【符号の説明】
１１    第１のジャイロ
１２    第２のジャイロ
１３    第１の加速度センサ
１４    第２の加速度センサ
１５    第１の磁気センサ
１６    第２の磁気センサ
２２ａ    ジャイロＹ
２２ｂ    ジャイロＺ
２３ａ    加速度センサＸ
２３ｂ    加速度センサＹ
２４ａ    磁気センサＸ
２４ｂ    磁気センサＹ
２５    EEPROM
２６ａ    運動角演算装置
２６ｂ    静止角演算装置
２６ｃ    姿勢角演算装置
２７    判別装置
２７ｃ    姿勢角検出装置
２８    俯角自動補正装置
２９    出力
【図１】
0003797661.tif 000004
【図２】
0003797661.tif 000005
【図３】
0003797661.tif 000006