JP3696586B2

JP3696586B2 - Attitude angle detector

Info

Publication number: JP3696586B2
Application number: JP2002293532A
Authority: JP
Inventors: 和健武藤; 洋阿部
Original assignee: NEC Tokin Corp
Current assignee: Tokin Corp
Priority date: 2002-10-07
Filing date: 2002-10-07
Publication date: 2005-09-21
Anticipated expiration: 2022-10-07
Also published as: JP2004125731A

Description

【０００１】
【発明の属する技術分野】
本発明は、移動体の姿勢検出や姿勢制御、バーチャルリアリティー等に使用されるヘッドマウントディスプレーで、頭の姿勢角度を検出するトラッカー、及び３Ｄゲームパッド等、３次元空間中で被測定対象物の姿勢角度（３軸の回転角度）を検出するのに適した姿勢角検出装置に関する。
【０００２】
【従来の技術】
飛行機や船舶の姿勢計測・制御に用いられてきた姿勢角度検出装置では、精度や信頼性が重視されていたので、サイズや価格は比較的問題とならなかった。しかしながら、３Ｄゲームなど民生用途への応用では、サイズや価格が最も重視される。ジャイロ、加速度センサ、磁気センサといったセンサはもちろんのこと、すべての構成部品に関して小型品、低価格品の使用が求められている。
【０００３】
マイコンについても例外ではない。このジャイロ、加速度センサ、磁気センサを用いた姿勢角度検出装置では、センサからの出力にさまざまな演算を行って姿勢角度を求めている。この演算をマイコンで行うことにすると、浮動少数演算が可能なマイコンが必要であり、小型化、低価格化が実現できない。
【０００４】
そこで、センサからの出力値をパソコンなどのホストマシンにそのまま送り、姿勢角度を求める演算は、ホストマシンのＣＰＵで行う方法が考えられる。この方法の場合は、マイコンでは演算する必要がなく、センサ出力のデータを取り、ホストマシンと通信を行うだけの作業で済むので、小型化、低価格化が容易に実現できる。
【０００５】
現状では、通信方式はＵＳＢ通信／ロースピードデバイス／インタラプト転送が最適である。ＵＳＢ通信が最適な理由は、複数接続が容易であること、電源供給があることである。
【０００６】
【発明が解決しようとする課題】
しかしながら、上記の方法を用いる場合、通信周期はＵＳＢの規格から８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は０、磁気センサのときは１とする。
【００１７】
姿勢角度の演算方法を後述するが、ジャイロの出力は、１周期前の姿勢角度からの変化量を求めるために使用するので、その応答性が必要となる。それに対して、加速度センサや磁気センサの出力は、このジャイロによって求めた出力を補正するために使用するので、応答性は比較的必要ない。従って、加速度センサと磁気センサのデータを少なくとも交互に１回転送すれば、十分に機能する。センサの数が少ない場合は、余裕部分を用いて精度を落としたセンサのデータの通信を行うことも可能である。
【００１８】
次に、図５にパソコンの内部にある姿勢角度演算手段にて演算される姿勢角度の演算方法について説明する。ＵＳＢからは、ジャイロ５２、加速度センサ５３および磁気センサ５４によって検出された角速度（ωｘ，ωｙ，ωｚ）、加速度（Ａｘ，Ａｙ，Ａｚ）、磁気（Ｍｘ，Ｍｙ，Ｍｚ）が転送される。運動角演算装置５６ａは、ジャイロから出力される角速度（ωｘ，ωｙ，ωｚ）から単位時間に移動した姿勢角度である運動角（Δα，Δβ，Δγ）を演算し、出力する。静止角演算装置５６ｂは、加速度センサから出力される加速度（Ａｘ，Ａｙ，Ａｚ）と、磁気センサから出力される磁気（Ｍｘ，Ｍｙ，Ｍｚ）から仮の姿勢角度である静止角（φ，Ｐ，Ｒ）を演算し出力する。判別装置５７は、運動角と静止角を比較して静止角が正しいかどうかを判別し姿勢角検出装置に結果を出力する。姿勢角演算装置５６ｃは、運動角（Δα，Δβ，Δγ）、静止角（φ，Ｐ，Ｒ）と判別装置の出力から最終的に出力する姿勢角度（α，β，γ）を演算し、外部に出力する。
【００１９】
次に、運動角演算装置の内容を説明する。姿勢角度検出装置に加わる角速度に応じた各ジャイロの出力（ωｘ，ωｙ，ωｚ）は、座標変換を行って、演算する単位時間で１単位時間過去の出力α（ｎ-１）、β（ｎ-１）およびγ（ｎ-１）とから、次式の如く、基準座標系における現在の移動角度（運動角）（Δα，Δβ，Δγ）に変換される。

【００２０】
次に、図５の静止角演算装置の説明を行う。図１に示す加速度センサ１４、１５および１６は、それぞれセンサ座標系での重力加速度のＸｓ、ＹｓおよびＺｓ軸の成分分力Ａｘ（ｎ）、Ａｙ（ｎ）およびＡｚ（ｎ）を検出するように配置されている。また、図１に示す磁気センサ１７、１８および１９は、それぞれＸｓ、ＹｓおよびＺｓ軸上の地磁気分力Ｍｘ（ｎ）、Ｍｙ（ｎ）およびＭｚ（ｎ）を検出するように配置されている。
【００２１】
加速度センサの出力Ａｘ（ｎ）、Ａｙ（ｎ）およびＡｚ（ｎ）は、ロール角演算手段およびピッチ角演算手段にて下式により、基準座標系の水平面（ＸＹ平面）との傾斜角である仮のロール角Ｒ（ｎ）および仮のピッチ角Ｐ（ｎ）を算出する。
Ｐ(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に圧縮することによって、従来の姿勢角度検出装置の問題を解消し、すなわち応答性を確保し、なおかつ精度も下げない通信アルゴリズムが得られた。このアルゴリズムによって理想的な姿勢角度検出装置が実現できた。
【図面の簡単な説明】
【図１】本発明における姿勢角度検出装置のセンサモジュール。
【図２】本発明における姿勢角度検出装置の全体構成に関する説明図。
【図３】従来におけるＵＳＢ通信データフォーマットに関する説明図。
【図４】本発明におけるＵＳＢ通信データフォーマットに関する説明図。
【図５】本発明における姿勢角度の演算方法に関する説明図。
【符号の説明】
１１第１のジャイロ
１２第２のジャイロ
１３第３のジャイロ
１４第１の加速度センサ
１５第２の加速度センサ
１６第３の加速度センサ
１７第１の磁気センサ
１８第２の磁気センサ
１９第３の磁気センサ
２１センサモジュール
２２ジャイロ
２３加速度センサ
２４磁気センサ
２５マイコン
２６パソコン
２７アプリケーション
２８姿勢角度演算手段
２９ＵＳＢコントローラ
５２ジャイロ
５３加速度センサ
５４磁気センサ
５６演算装置
５６ｃ姿勢角演算装置
５７判別装置
５９出力[0001]
BACKGROUND OF THE INVENTION
The present invention is a head-mounted display used for posture detection, posture control, virtual reality, and the like of a moving body, such as a tracker for detecting the posture angle of a head, a 3D game pad, etc. The present invention relates to a posture angle detection device suitable for detecting a posture angle (three-axis rotation angle).
[0002]
[Prior art]
In the attitude angle detection device that has been used for attitude measurement and control of airplanes and ships, accuracy and reliability have been emphasized, so size and price have not been a problem. However, in consumer applications such as 3D games, size and price are most important. Not only sensors such as gyroscopes, acceleration sensors, and magnetic sensors, but also all components are required to be small and inexpensive.
[0003]
The microcomputer is no exception. In the attitude angle detection device using the gyro, the acceleration sensor, and the magnetic sensor, the attitude angle is obtained by performing various calculations on the output from the sensor. If this calculation is performed by a microcomputer, a microcomputer capable of floating-point arithmetic is required, and miniaturization and cost reduction cannot be realized.
[0004]
Thus, a method is conceivable in which the output value from the sensor is sent as it is to a host machine such as a personal computer to calculate the attitude angle by the CPU of the host machine. In the case of this method, the microcomputer does not need to perform calculation, and only needs to take the sensor output data and communicate with the host machine, so that downsizing and cost reduction can be easily realized.
[0005]
At present, the most suitable communication method is USB communication / low speed device / interrupt transfer. The reason why USB communication is optimal is that multiple connections are easy and that there is power supply.
[0006]
[Problems to be solved by the invention]
However, when the above method is used, the communication cycle is 8 ms from the USB standard, and the amount of data that can be transferred at one time is 8 bytes. If the data to be transferred is within 8 bytes, the data is sent every 8 ms. If the data to be transferred is 8 to 16 bytes, the data is divided into 2 times, such as data 1 in the first 8 ms and data 2 in the next 8 ms. Sent. On the other hand, the sensor output value data to be transferred exceeds 8 bytes with at least 10 bits A / D × 7 = 70 bits. Therefore, if the data is sent as it is, the data is divided into two times and sent, so that one cycle becomes 16 ms and there is a problem that responsiveness cannot be secured.
[0007]
Further, if the A / D resolution is 9 bits in order to ensure responsiveness, the amount of data can be suppressed within 8 bytes, but there is a problem that the accuracy of the result of calculating the attitude angle is deteriorated.
[0008]
The present invention is a communication algorithm that solves the problems of the attitude angle detection device described above, ensures responsiveness, and does not reduce accuracy. This algorithm provides an ideal posture angle detection device.
[0009]
[Means for Solving the Problems]
The present invention relates to a gyro that detects angular velocities around three axes, an acceleration sensor that detects at least two-axis acceleration, a sensor unit that includes at least two-axis geomagnetism, and a posture angle. A calculation unit for calculating, and the sensor unit and the calculation unit are attitude angle detection devices that transfer data by remote communication, and in the data communication between the sensor unit and the calculation unit, the gyro Is output every time, and at least the data output from the acceleration sensor and the magnetic sensor are alternately transferred every other time.
[0010]
DETAILED DESCRIPTION OF THE INVENTION
Hereinafter, embodiments of the present invention will be described with reference to the drawings. In this example, the posture angle detection device has a total of nine sensors (three gyros, three acceleration sensors, and three magnetic sensors), but it can be similarly applied to seven or more devices.
[0011]
FIG. 1 shows an arrangement of a gyro, an acceleration sensor, and a magnetic sensor in the posture angle detection apparatus of the present invention. In the sensor coordinate system shown in FIG. 1, a first gyro 11, a second gyro 12, and a third gyro 13 for detecting angular velocities around three axes (Xs axis, Ys axis, and Zs axis) orthogonal to each other. Are arranged parallel to the Xs axis, the Ys axis, and the Zs axis, that is, orthogonal to each other. The first acceleration sensor 14, the second acceleration sensor 15, and the third acceleration sensor 16 are arranged in parallel to the three axes that are orthogonal to each other, the Xs axis, the Ys axis, and the Zs axis. Similarly, the first magnetic sensor 17, the second magnetic sensor 18, and the third magnetic sensor 19 are disposed in parallel to the three axes that are orthogonal to each other, the Xs axis, the Ys axis, and the Zs axis.
[0012]
FIG. 2 is a diagram showing the overall configuration of the attitude angle detection device according to one embodiment of the present invention. An angular velocity (ωx, ωy, ωz), acceleration (Ax, Ay, Az), and geomagnetism (Mx, My, Mz) are output from the gyroscope 22, the acceleration sensor 23, and the magnetic sensor 24, respectively. Each sensor output is converted into digital data by an A / D converter built in the microcomputer 25 and transferred to the personal computer 26 by USB communication which is one of remote communications. The attitude angle calculation means 28 of the personal computer 26 calculates the attitude angle of the sensor module 21 based on the A / D data of each sensor and outputs it to the application 27.
[0013]
First, a USB communication transfer method will be described. Currently, USB communication / low speed device / interrupt transfer is the most suitable communication method. USB communication is suitable for a sensor device such as the present invention because multiple connections are easy and power is supplied. A low-speed device is generally a device standard with little data, and is used for data transfer of a keyboard and a mouse. Also, high-speed device and full-speed device standards are used for printers and scanners with a large amount of data. Interrupt transfer is a transfer method used when data is sent periodically. By selecting one of these methods, the microcomputer can be reduced in size and price.
[0014]
However, when the above method is used, the communication cycle is 8 ms from the USB standard, and the amount of data that can be transferred at one time is 8 bytes. If the data to be transferred is within 8 bytes, the data is sent every 8 ms. If the data to be transferred is 8 to 16 bytes, the data is sent in two times, such as data 1 in the first 8 ms and data 2 in the next 8 ms. It is done. On the other hand, the output of each sensor is converted to digital data by an A / D converter. The resolution of this A / D converter is 10 bits.
[0015]
FIG. 4 shows a data format in the conventional communication method. Conventionally, since nine sensor outputs were transferred as they were, the data amount was 9 × 10 = 90 bits (= 12 bytes). Accordingly, the communication cycle is 16 ms, and a communication cycle sufficient for smoothing the moving image of the application cannot be obtained. Bt1 and Bt2 are button data for sending a reset signal or a signal corresponding to a mouse click. N / A is an unused area and its value is undefined.
[0016]
FIG. 4 shows a data format in the communication method of the present invention. Sensor 1 to Sensor 3 are the gyro outputs (ωx, ωy, ωz). From Sensor 4 to Sensor 6, the output (Ax, Ay, Az) of the acceleration sensor and the output (Mx, My, Mz) of the magnetic sensor are alternately transferred. Flag is a flag indicating which sensor 4 to sensor 6 is data of. For example, Flag is 0 for an acceleration sensor and 1 for a magnetic sensor.
[0017]
Although the attitude angle calculation method will be described later, the output of the gyro is used to obtain the amount of change from the attitude angle one cycle before, so that its responsiveness is required. On the other hand, since the output of the acceleration sensor or the magnetic sensor is used to correct the output obtained by the gyro, the response is relatively unnecessary. Therefore, if the data of the acceleration sensor and the magnetic sensor are transferred alternately at least once, it functions sufficiently. When the number of sensors is small, it is also possible to perform communication of sensor data with reduced accuracy using a margin.
[0018]
Next, an attitude angle calculation method calculated by the attitude angle calculation means in the personal computer will be described with reference to FIG. From the USB, the angular velocity (ωx, ωy, ωz), acceleration (Ax, Ay, Az), and magnetism (Mx, My, Mz) detected by the gyro 52, the acceleration sensor 53, and the magnetic sensor 54 are transferred. The motion angle calculation device 56a calculates and outputs motion angles (Δα, Δβ, Δγ), which are posture angles moved per unit time from angular velocities (ωx, ωy, ωz) output from the gyro. The static angle calculation device 56b is configured to generate a static angle (φ, P) that is a temporary posture angle from the acceleration (Ax, Ay, Az) output from the acceleration sensor and the magnetism (Mx, My, Mz) output from the magnetic sensor. , R) is calculated and output. The discriminating device 57 compares the motion angle and the stationary angle to discriminate whether or not the stationary angle is correct, and outputs the result to the posture angle detecting device. The posture angle calculation device 56c calculates the posture angle (α, β, γ) that is finally output from the movement angle (Δα, Δβ, Δγ), the static angle (φ, P, R) and the output of the discrimination device, Output to the outside.
[0019]
Next, the contents of the motion angle calculation device will be described. The outputs (ωx, ωy, ωz) of each gyro according to the angular velocity applied to the attitude angle detection device are output by converting the coordinates α (n−1), β (n -1) and γ (n-1) are converted into the current movement angle (movement angle) (Δα, Δβ, Δγ) in the reference coordinate system as shown in the following equation.

[0020]
Next, the static angle calculation device in FIG. 5 will be described. The

acceleration sensors

14, 15 and 16 shown in FIG. 1 detect component component forces Ax (n), Ay (n) and Az (n) of the Xs, Ys and Zs axes of gravity acceleration in the sensor coordinate system, respectively. Is arranged. Further, the

magnetic sensors

17, 18 and 19 shown in FIG. 1 are arranged to detect geomagnetic component forces Mx (n), My (n) and Mz (n) on the Xs, Ys and Zs axes, respectively. .
[0021]
The outputs Ax (n), Ay (n) and Az (n) of the acceleration sensor are inclination angles with respect to the horizontal plane (XY plane) of the reference coordinate system by the following formulas in the roll angle calculation means and the pitch angle calculation means. A temporary roll angle R (n) and a temporary pitch angle P (n) are calculated.
P (n) = sin ⁻¹ Ax (n)
R (n) = tan ⁻¹ [−Ay (n) / − Az (n)]
[0022]
In order to obtain the provisional yaw angle φ (n) in the reference coordinate system, the geomagnetic components HX and HY in the reference coordinate system are assumed to be the temporary outputs Mx (n), My (n) and Mz (n) of the geomagnetic sensor. Using the pitch angle P (n) and the temporary roll angle R (n), coordinate conversion is performed as in the following equation.
HX (n) = Mx.cosP (n) + [My.sinR (n) + Mz.cosR (n)]. SinP (n)
HY (n) = My · cosR (n) −Mz · sinR (n)
[0023]
The temporary yaw angle φ (n) in the reference coordinate system is
φ (n) = − tan ⁻¹ [HY (n) / HX (n)] − φ (0)
It is represented by Where φ (0) is the azimuth angle at the origin.
[0024]
Next, a determination device in the posture angle detection device of the present invention will be described. Since the outputs Ax, Ay, and Az of the acceleration sensor are combined vectors of the Xs, Ys, and Zs axis direction components of the gravitational acceleration and the Xs, Ys, and Zs axis direction components of the motion acceleration, The temporary pitch angle P calculated by the static angle calculation device is not a correct pitch angle. Also, the temporary yaw angle φ may not be a correct yaw angle depending on the magnetic field environment. The discriminating device discriminates whether or not φ (n), P (n), and R (n), which are outputs of the static angle calculation device, are correct.
[0025]
In the state where motion acceleration is not applied, the combined vector of Ax, Ay and Az is 1G.
| (Ax ² + Ay ² + Az ² ) ^1/2 -1 | <ε
Is satisfied, φ, P, and R are determined to be correct.
[0026]
Next, a calculation method performed by the attitude angle calculation device that calculates the output of the motion angle calculation device and the output of the static angle calculation device according to the determination result of the determination device to obtain the final output will be described.
If the discriminator determines that φ, P, and R are correct,
α (n) = α (n-1) + Δα−k [α (n−1) + Δα−φ (n)],
β (n) = β (n−1) + Δβ−k [β (n−1) + Δβ−P (n)],
γ (n) = γ (n−1) + Δγ−k [γ (n−1) + Δγ−R (n)],
The posture angle is calculated by the following, and if the discriminating device determines that φ, P, and R are incorrect,
α (n) = α (n-1) + Δα,
β (n) = β (n-1) + Δβ,
γ (n) = γ (n−1) + Δγ,
To calculate the attitude angle.
[0027]
【The invention's effect】
As described above, according to the present invention, the sensor data is divided into those that require responsiveness and those that are unnecessary, and the data to be actually transferred is compressed to 8 bytes. Thus, a communication algorithm that eliminates the above problem, ie, secures responsiveness and does not reduce accuracy is obtained. With this algorithm, an ideal attitude angle detector was realized.
[Brief description of the drawings]
FIG. 1 shows a sensor module of a posture angle detection device according to the present invention.
FIG. 2 is an explanatory diagram relating to the overall configuration of a posture angle detection device according to the present invention.
FIG. 3 is an explanatory diagram related to a conventional USB communication data format.
FIG. 4 is an explanatory diagram relating to a USB communication data format in the present invention.
FIG. 5 is an explanatory diagram relating to an attitude angle calculation method according to the present invention.
[Explanation of symbols]
11 1st gyro 12 2nd gyro 13 3rd gyro 14 1st acceleration sensor 15 2nd acceleration sensor 16 3rd acceleration sensor 17 1st magnetic sensor 18 2nd magnetic sensor 19 3rd magnetism Sensor 21 Sensor module 22 Gyro 23 Acceleration sensor 24 Magnetic sensor 25 Microcomputer 26 Personal computer 27 Application 28 Attitude angle calculation means 29 USB controller 52 Gyro 53 Acceleration sensor 54 Magnetic sensor 56 Calculation device 56c Attitude angle calculation device 57 Discrimination device 59 Output

Claims

A sensor unit including a gyro that detects angular velocities around three axes, an acceleration sensor that detects at least two-axis acceleration, a magnetic sensor that detects at least two-axis geomagnetism, and a posture angle calculation Computation means, wherein the sensor unit and the computation means are attitude angle detection devices that transfer data by remote communication, and are output from the gyro in data communication between the sensor unit and the computation means Data is transferred every time, and at least data output from the acceleration sensor and the magnetic sensor are alternately transferred every other time.