(57)【特許請求の範囲】
【請求項１】
３軸の回りの角速度を検出するジャイロと、少なくとも２軸の加速度を検出する加速度センサと、少なくとも２軸の地磁気を検出する磁気センサとから構成されるセンサ部と、姿勢角度を算出するための演算手段とを備え、前記センサ部と前記演算手段とは遠隔通信によってデータを転送する姿勢角度検出装置であって、前記センサ部と前記演算手段とのデータの通信において、前記ジャイロから出力されるデータは毎回転送され、少なくとも前記加速度センサおよび前記磁気センサから出力されるデータは１回おきに交互に転送されることを特徴とする姿勢角度検出装置。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、移動体の姿勢検出や姿勢制御、バーチャルリアリティー等に使用されるヘッドマウントディスプレーで、頭の姿勢角度を検出するトラッカー、及び３Ｄゲームパッド等、３次元空間中で被測定対象物の姿勢角度（３軸の回転角度）を検出するのに適した姿勢角検出装置に関する。
【０００２】
【従来の技術】
飛行機や船舶の姿勢計測・制御に用いられてきた姿勢角度検出装置では、精度や信頼性が重視されていたので、サイズや価格は比較的問題とならなかった。しかしながら、３Ｄゲームなど民生用途への応用では、サイズや価格が最も重視される。ジャイロ、加速度センサ、磁気センサといったセンサはもちろんのこと、すべての構成部品に関して小型品、低価格品の使用が求められている。
【０００３】
マイコンについても例外ではない。このジャイロ、加速度センサ、磁気センサを用いた姿勢角度検出装置では、センサからの出力にさまざまな演算を行って姿勢角度を求めている。この演算をマイコンで行うことにすると、浮動少数演算が可能なマイコンが必要であり、小型化、低価格化が実現できない。
【０００４】
そこで、センサからの出力値をパソコンなどのホストマシンにそのまま送り、姿勢角度を求める演算は、ホストマシンのＣＰＵで行う方法が考えられる。この方法の場合は、マイコンでは演算する必要がなく、センサ出力のデータを取り、ホストマシンと通信を行うだけの作業で済むので、小型化、低価格化が容易に実現できる。
【０００５】
現状では、通信方式はＵＳＢ通信／ロースピードデバイス／インタラプト転送が最適である。ＵＳＢ通信が最適な理由は、複数接続が容易であること、電源供給があることである。
【０００６】
【発明が解決しようとする課題】
しかしながら、上記の方法を用いる場合、通信周期はＵＳＢの規格から８msであり、一度に転送できるデータ量は８byteである。転送するデータが８byte以内であれば８ms毎にそのデータが送られ、転送するデータが８〜１６byteであれば最初の８msにデータ１、次の８msにデータ２というように、２回に分けて送られる。一方、転送するセンサ出力値のデータは、少なくとも１０bitＡ／Ｄ×７つ＝７０bitで８byteを超える。従って、このままではデータが2回に分けられて送られるために、１周期が１６msになってしまい応答性が確保できないという問題がある。
【０００７】
また、応答性を確保するために、Ａ／Ｄ分解能を９bitにするとデータ量は８byte以内に抑えることができるが、姿勢角度を演算した結果の精度が悪くなってしまうという問題があった。
【０００８】
本発明は、前記の姿勢角度検出装置の問題点の解消を図るもので、応答性を確保し、なおかつ精度も下げない通信アルゴリズムである。このアルゴリズムによって、理想的な姿勢角度検出装置を提供するものである。
【０００９】
【課題を解決するための手段】
本発明は、３軸の回りの角速度を検出するジャイロと、少なくとも２軸の加速度を検出する加速度センサと、少なくとも２軸の地磁気を検出する磁気センサとから構成されるセンサ部と、姿勢角度を算出するための演算手段とを備え、前記センサ部と前記演算手段とは遠隔通信によってデータを転送する姿勢角度検出装置であって、前記センサ部と前記演算手段とのデータの通信において、前記ジャイロから出力されるデータは毎回転送され、少なくとも前記加速度センサおよび前記磁気センサから出力されるデータは１回おきに交互に転送されることを特徴とする姿勢角度検出装置である。
【００１０】
【発明の実施の形態】
以下、本発明の実施の形態について図面を参照して説明する。なお、今回は姿勢角度検出装置の全センサ数が９つのもの（ジャイロ３つ、加速度センサ３つ、磁気センサ３つ）を説明するが、７つ以上のものについては、同様に適応できる。
【００１１】
図１には、本発明の姿勢角度検出装置におけるジャイロ、加速度センサ、磁気センサの配置を示す。図１で示すセンサ座標系において、互いに直交する３軸（Ｘｓ軸、Ｙｓ軸およびＺｓ軸）の回りの角速度を検出するための第１のジャイロ１１、第２のジャイロ１２および第３のジャイロ１３はＸｓ軸、Ｙｓ軸およびＺｓ軸に平行に、すなわち互いに直交して、配置されている。第１の加速度センサ１４、第２の加速度センサ１５および第３の加速度センサ１６は、互いに直交する３軸、Ｘｓ軸、Ｙｓ軸およびＺｓ軸に平行に配置されている。同様に、第１の磁気センサ１７、第２の磁気センサ１８および第３の磁気センサ１９は、互いに直交する３軸、Ｘｓ軸、Ｙｓ軸およびＺｓ軸に平行に配置されている。
【００１２】
図２は、本発明の一実施の形態における姿勢角度検出装置の全体構成を示す図である。ジャイロ２２、加速度センサ２３および磁気センサ２４からは、それぞれ角速度（ωｘ，ωｙ，ωｚ）、加速度（Ａｘ，Ａｙ，Ａｚ）、地磁気（Ｍｘ，Ｍｙ，Ｍｚ）が出力される。各センサ出力は、マイコン２５に内蔵されたＡ／Ｄコンバータによりデジタルデータに変換され、遠隔通信の一つであるＵＳＢ通信によってパソコン２６に転送される。パソコン２６の姿勢角度演算手段２８は、各センサのＡ／Ｄデータを元にセンサモジュール２１の姿勢角度を演算し、アプリケーション２７に出力する。
【００１３】
最初に、ＵＳＢ通信の転送方法について説明する。通信方式としては、現状ではＵＳＢ通信／ロースピードデバイス／インタラプト転送が最適である。ＵＳＢ通信は、複数接続が容易であり、また電源供給があるので、本発明のようなセンサデバイスに最適である。ロースピードデバイスは、一般にデータの少ないデバイスの規格で、キーボードやマウスのデータ転送に使われる。また、ハイスピードデバイスやフルスピードデバイスの規格は、データ量の多いプリンタやスキャナなどに使われる。インタラプト転送は、周期的にデータを送るときに使用する転送方法である。これらの方式を選択することによって、マイコンの小型・低価格化が実現できる。
【００１４】
しかしながら、上記の方法を用いる場合、通信周期はＵＳＢの規格から８ｍｓであり、一度に転送できるデータ量は８ｂｙｔｅである。転送するデータが８ｂｙｔｅ以内であれば８ｍｓ毎にそのデータが送られ、転送するデータが８〜１６ｂｙｔｅであれば最初の８ｍｓにデータ１、次の８ｍｓにデータ２というように２回に分けて送られる。一方、各センサの出力はＡ／Ｄコンバータによってデジタルデータになるが、このＡ／Ｄコンバータの分解能は１０bitである。
【００１５】
図４に、従来の通信方法におけるデータフォーマットを示す。従来は、９つあるセンサ出力をそのまま転送していたために、９×１０＝９０bit（＝１２byte）のデータ量になっていた。従って、通信周期は１６ｍｓとなり、アプリケーションの動画がなめらかになるのに十分な通信周期が得られなかった。Ｂｔ１，Ｂｔ２は、リセット信号やマウスのクリックに相当する信号を送るためのボタンデータである。Ｎ／Ａは未使用の領域で値は不定である。
【００１６】
図４に、本発明の通信方法におけるデータフォーマットを示す。Sensor1からSensor３は、ジャイロの出力（ωｘ，ωｙ，ωｚ）である。Sensor４からSensor６は、加速度センサの出力（Ａｘ，Ａｙ，Ａｚ）と磁気センサの出力（Ｍｘ，Ｍｙ，Ｍｚ）が交互に転送される。Flagは、Sensor４からSensor６が、どちらのセンサのデータなのかを示すフラグである。たとえば、加速度センサのときはFlagは０、磁気センサのときは１とする。
【００１７】
姿勢角度の演算方法を後述するが、ジャイロの出力は、１周期前の姿勢角度からの変化量を求めるために使用するので、その応答性が必要となる。それに対して、加速度センサや磁気センサの出力は、このジャイロによって求めた出力を補正するために使用するので、応答性は比較的必要ない。従って、加速度センサと磁気センサのデータを少なくとも交互に１回転送すれば、十分に機能する。センサの数が少ない場合は、余裕部分を用いて精度を落としたセンサのデータの通信を行うことも可能である。
【００１８】
次に、図５にパソコンの内部にある姿勢角度演算手段にて演算される姿勢角度の演算方法について説明する。ＵＳＢからは、ジャイロ５２、加速度センサ５３および磁気センサ５４によって検出された角速度（ωｘ，ωｙ，ωｚ）、加速度（Ａｘ，Ａｙ，Ａｚ）、磁気（Ｍｘ，Ｍｙ，Ｍｚ）が転送される。運動角演算装置５６ａは、ジャイロから出力される角速度（ωｘ，ωｙ，ωｚ）から単位時間に移動した姿勢角度である運動角（Δα，Δβ，Δγ）を演算し、出力する。静止角演算装置５６ｂは、加速度センサから出力される加速度（Ａｘ，Ａｙ，Ａｚ）と、磁気センサから出力される磁気（Ｍｘ，Ｍｙ，Ｍｚ）から仮の姿勢角度である静止角（φ，Ｐ，Ｒ）を演算し出力する。判別装置５７は、運動角と静止角を比較して静止角が正しいかどうかを判別し姿勢角検出装置に結果を出力する。姿勢角演算装置５６ｃは、運動角（Δα，Δβ，Δγ）、静止角（φ，Ｐ，Ｒ）と判別装置の出力から最終的に出力する姿勢角度（α，β，γ）を演算し、外部に出力する。
【００１９】
次に、運動角演算装置の内容を説明する。姿勢角度検出装置に加わる角速度に応じた各ジャイロの出力（ωｘ，ωｙ，ωｚ）は、座標変換を行って、演算する単位時間で１単位時間過去の出力α（ｎ-１）、β（ｎ-１）およびγ（ｎ-１）とから、次式の如く、基準座標系における現在の移動角度（運動角）（Δα，Δβ，Δγ）に変換される。
0003696586.tif 000002
【００２０】
次に、図５の静止角演算装置の説明を行う。図１に示す加速度センサ１４、１５および１６は、それぞれセンサ座標系での重力加速度のＸｓ、ＹｓおよびＺｓ軸の成分分力Ａｘ（ｎ）、Ａｙ（ｎ）およびＡｚ（ｎ）を検出するように配置されている。また、図１に示す磁気センサ１７、１８および１９は、それぞれＸｓ、ＹｓおよびＺｓ軸上の地磁気分力Ｍｘ（ｎ）、Ｍｙ（ｎ）およびＭｚ（ｎ）を検出するように配置されている。
【００２１】
加速度センサの出力Ａｘ（ｎ）、Ａｙ（ｎ）およびＡｚ（ｎ）は、ロール角演算手段およびピッチ角演算手段にて下式により、基準座標系の水平面（ＸＹ平面）との傾斜角である仮のロール角Ｒ（ｎ）および仮のピッチ角Ｐ（ｎ）を算出する。
Ｐ(n)＝sin−１Ax(n)
Ｒ(n)＝tan−１[−Ay(n)／−Az(n)]
【００２２】
基準座標系での仮のヨー角φ（ｎ）を求めるために、基準座標系での地磁気成分ＨＸ，ＨＹを地磁気センサの出力Ｍｘ（ｎ）、Ｍｙ（ｎ）およびＭｚ（ｎ）と仮のピッチ角Ｐ（ｎ）、仮のロール角Ｒ（ｎ）を用いて次式のごとく座標変換を行う。
HX(n)=Mx・cosＰ(n)＋[My・sinＲ(n)＋Mz・cosＲ(n)]・sinＰ(n)
HY(n)＝My・cosＲ(n)−Mz・sinＲ(n)
【００２３】
基準座標系での仮のヨー角φ(n)は、
φ(n)＝−tan−１[HY(n)／HX(n)]−φ(0)
で表される。ただし、φ(0)は原点時の方位角である。
【００２４】
次に、本発明の姿勢角度検出装置における、判別装置を説明する。加速度センサの出力Ａｘ、ＡｙおよびＡｚは重力加速度のＸｓ、ＹｓおよびＺｓ軸方向成分と運動加速度のＸｓ、ＹｓおよびＺｓ軸方向成分との合成ベクトルとなるために、運動加速度がある場合には、静止角演算装置によって算出された仮のピッチ角Ｐは正しいピッチ角にはならない。また、仮のヨー角φについても磁場環境によって正しいヨー角にならない場合がある。判別装置は、静止角演算装置の出力であるφ（ｎ）、Ｐ（ｎ）、Ｒ（ｎ）が正しいかどうかを判別するものである。
【００２５】
運動加速度が加わっていない状態では、Ａｘ、ＡｙおよびＡｚの合成ベクトルは１Ｇとなるので微小定数εを用いて、
｜(Ａｘ２＋Ａｙ２＋Ａｚ２)１／２−１｜＜ε
が成立するならば、それぞれφ、Ｐ、Ｒは正しいと判断する。
【００２６】
次に、上記判別装置の判別結果に応じて、運動角演算装置の出力と静止角演算装置の出力を演算して最終的な出力を得る姿勢角演算装置で行われる演算方法について説明する。
判別装置においてφ、Ｐ、Ｒが正しいと判断された場合には、
α(n)＝α(n-1)＋Δα−k[α(n-1)＋Δα−φ(n)]、
β(n)＝β(n-1)＋Δβ−k[β(n-1)＋Δβ−Ｐ(n)]、
γ(n)＝γ(n-1)＋Δγ−k[γ(n-1)＋Δγ−Ｒ(n)]、
により姿勢角度を演算し、判別装置においてφ、Ｐ、Ｒが誤っていると判断された場合には、
α(n)＝α(n-1)＋Δα、
β(n)＝β(n-1)＋Δβ、
γ(n)＝γ(n-1)＋Δγ、
により姿勢角度を演算する。
【００２７】
【発明の効果】
以上説明したように、本発明によれば、センサのデータを応答性の必要なものと不必要なものとに分け、実際に転送するデータを８byteに圧縮することによって、従来の姿勢角度検出装置の問題を解消し、すなわち応答性を確保し、なおかつ精度も下げない通信アルゴリズムが得られた。このアルゴリズムによって理想的な姿勢角度検出装置が実現できた。
【図面の簡単な説明】
【図１】本発明における姿勢角度検出装置のセンサモジュール。
【図２】本発明における姿勢角度検出装置の全体構成に関する説明図。
【図３】従来におけるＵＳＢ通信データフォーマットに関する説明図。
【図４】本発明におけるＵＳＢ通信データフォーマットに関する説明図。
【図５】本発明における姿勢角度の演算方法に関する説明図。
【符号の説明】
１１    第１のジャイロ
１２    第２のジャイロ
１３    第３のジャイロ
１４    第１の加速度センサ
１５    第２の加速度センサ
１６    第３の加速度センサ
１７    第１の磁気センサ
１８    第２の磁気センサ
１９    第３の磁気センサ
２１    センサモジュール
２２    ジャイロ
２３    加速度センサ
２４    磁気センサ
２５    マイコン
２６    パソコン
２７    アプリケーション
２８    姿勢角度演算手段
２９    ＵＳＢコントローラ
５２    ジャイロ
５３    加速度センサ
５４    磁気センサ
５６    演算装置
５６ｃ    姿勢角演算装置
５７    判別装置
５９    出力
【図１】
0003696586.tif 000003
【図２】
0003696586.tif 000004
【図３】
0003696586.tif 000005
【図４】
0003696586.tif 000006
【図５】
0003696586.tif 000007