9自由度のセンサーを備えたArduinoボードがあり、そこからボードのピッチ、ヨー、ロールを決定する必要があります。

9-DOFセンサーからのデータセットの例を次に示します。

加速度計（m / s）

• = -5,85$\text{Acc}_{X}$
• = 1,46$\text{Acc}_{Y}$
• = 17,98$\text{Acc}_{Z}$

ジャイロスコープ（RPM）

• = 35,14$\text{Gyr}_{X}$
• = -40,22$\text{Gyr}_{Y}$
• = -9,86$\text{Gyr}_{Z}$

磁力計（ガウス）

• = 0,18$\text{Mag}_{X}$
• = -0,04$\text{Mag}_{Y}$
• = -0,15$\text{Mag}_{Z}$

これらのデータからピッチ、ヨー、ロールを計算するにはどうすればよいですか？

ピッチ、ロール、ヨーは、X、Y、Z軸を中心とした回転として定義されます。以下に、定義を説明する図として示します。

前のプロジェクトでは、アナログデバイスのADXL345加速度計を使用してロールとピッチを計算しました。以下は、ロールとピッチの計算に使用される式です。ソースコードの一部を公開して使用できるようにしました。

accelerationX = (signed int)(((signed int)rawData_X) * 3.9);
accelerationY = (signed int)(((signed int)rawData_Y) * 3.9);
accelerationZ = (signed int)(((signed int)rawData_Z) * 3.9);
pitch = 180 * atan (accelerationX/sqrt(accelerationY*accelerationY + accelerationZ*accelerationZ))/M_PI;
roll = 180 * atan (accelerationY/sqrt(accelerationX*accelerationX + accelerationZ*accelerationZ))/M_PI;

完全なソースコードはこちらにあります。

上記の定義に基づく

yaw = 180 * atan (accelerationZ/sqrt(accelerationX*accelerationX + accelerationZ*accelerationZ))/M_PI;

注：M_PI = 3.14159265358979323846は、math.hで定義された定数です

以下は、Arduinoの基本ソースコードを含む参考資料です。

---

参照：

• 飛行力学
• ビーグルボーンブラックADXL345ソースコード
• Adxl345 Beaglebone Black Embedded QTデモ
• Arduino IMU：加速度計のピッチとロール

したがって、以下の私のより長い答えは、ボードが加速を受けると仮定しており、この間、ピッチ、ロール、ヨーを短時間で測定できる必要があります。ボードがすべての測定に対して静止している場合、Mahendra Gunawardenaの答えは完璧に機能します。これがセグウェイやモデルプレーン、マルチローター、または動き回る何かのようなデバイスに入っている場合は、読み続けることができます。この投稿では、センサーフュージョンと呼ばれる方法を使用して、3つのセンサーすべてを使用する方法について説明します。センサーフュージョンを使用すると、各センサーの長所を取得し、各センサーの弱点の影響を最小限に抑えることができます。

センサーの特性と背景

まず、加速度計は重力だけでなく、それに加えられるすべての力を測定することを理解してください。したがって、振動のない静止位置に加速度計がある完璧な世界では、Mahendra Gunawardenaの答えが示すように、いくつかの基本的な三角法を使用して、どちらの方向に進むかを完全に決定できます。ただし、加速度計はすべての力を拾うため、振動があるとノイズが発生します。また、ボードが加速している場合、加速度計が報告する力は地球の重力だけでなく、加速させる力でもあるため、単純な三角法を使用することはできません。

磁力計は、加速度計よりも簡単です。動きはそれで問題を引き起こしませんが、鉄や他の磁石のようなものはあなたの出力に影響を及ぼすことになります。この干渉の原因となるソースが一定の場合、対処するのは難しくありませんが、これらのソースが一定でない場合、除去するのに問題のある大量のノイズが発生します。

3つのセンサーのうち、ジャイロスコープは最も信頼性が高く、通常は回転速度の測定に非常に優れています。それは鉄源のようなものの影響を受けず、加速度は基本的に回転速度を測定する能力に影響を与えません。彼らは、デバイスが回転している速度を報告する非常に良い仕事をしますが、絶対角度を探しているので、位置を得るために速度を統合する必要があります。これにより、1回の測定の誤差が100回の測定で1秒あたり0.01度しかずれていない場合でも、積分は基本的に範囲全体の値の合計であるため、最後の測定の誤差が新しい測定の誤差に追加されます1度、1000回の測定でオフにでき、10度オフにできます。1秒間に何百もの測定を行っている場合、これにより問題が発生することがわかります。これは一般にジャイロドリフトと呼ばれます。

センサーフュージョン

これらすべてのセンサーを連携させることの利点は、加速度計と磁力計からの情報を使用してジャイロドリフトを相殺できることです。これにより、ジャイロドリフトの致命的な欠陥なしに、ジャイロの精度と速度を提供できます。

これら3つのセンサーからのデータを組み合わせる方法は1つ以上ありますが、補完的なフィルターの使用について説明します。これは、カルマンフィルターとカルマンフィルターよりもはるかに単純であるため、組み込みシステムでより多くのリソースを消費します。多くの場合、補完的なフィルタで十分であり、実装が簡単で（事前に構築されたライブラリを使用しない場合）、データをより速く処理できます。

次にプロセスに移ります。必要な最初の手順は、ジャイロスコープの出力を統合して角速度を角位置に変換することです。また、出力のノイズに対処するために、加速度計と磁力計にローパスフィルターを適用する必要があります。以下に示すような単純なFIRフィルターがここで機能します。いくつかの三角法を使用すると、加速度計でピッチとロールを、磁力計でヨーを見つけることができます。

filteredData = (1-weight)*filteredData + weight*newData

重みは単なる定数であり、処理するノイズの量に応じて調整できます。ノイズが大きいほど、重み値は小さくなります。センサーからのデータを結合するには、次のコード行を使用します。

fusedData = (1-weight)*gyroData + weight*accelMagData

データはピッチ、ロール、ヨーのベクトルであることに注意してください。必要に応じて、配列の代わりに3つの変数を使用してこれを行うこともできます。この計算では、ジャイロはピッチ、ロール、ヨーの位置を度単位で提供し、磁力計はヨーの角度を提供し、加速度計はピッチとロールの独自の数値を提供します。

さらに情報が必要な場合は、「補完フィルターとのセンサーフュージョン」をグーグルで検索できます。これに関する記事はたくさんあります。

アクセルセンサーデータから、ピッチとロールのみを計算できます。フリースケールの以下のドキュメントには、必要な情報が豊富に説明されています。

AN3461-3軸加速度計を使用した傾斜検知

ドキュメントのことわざに基づいて、

$$\tan \phi_{xyz} = \frac{G_{py}}{G_{pz}}$$

$$\tan \theta_{xyz} = \frac{-G_{px}}{G_{py}\sin \phi + G_{pz}\cos \phi} = \frac{-G_{px}}{\sqrt{G_{py}^2 + G_{pz}^2}}$$

これは次と同等です：

roll = atan2(accelerationY, accelerationZ)

pitch = atan2(-accelerationX, sqrt(accelerationY*accelerationY + accelerationZ*accelerationZ))

もちろん、特定の順序（Rxyz）で回転が発生している場合にのみ、結果がこれになります。

1. $\phi$
2. 角度 Y軸の周りにピッチ$\theta$
3. 角度よるz軸周りのヨー$\psi$

$R_{xyz}$$\psi$