我有一个带有9个自由度传感器的Arduino板，必须从中确定板的俯仰，偏航和侧倾。

这是来自9自由度传感器的一组数据的示例：

加速度计（m / s）

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

陀螺仪（RPM）

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

磁力计（高斯）

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

如何根据这些数据计算俯仰，偏航和侧倾？

俯仰，滚动和偏航定义为围绕X，Y和Z轴的旋转。下面作为图片来说明定义。

在上一个项目中，我使用了ADI公司的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基本源代码。

---

参考文献：

• 飞行动力
• Beaglebone黑色ADXL345源代码
• Adxl345 Beaglebone黑色嵌入式QT演示
• Arduino IMU：来自加速度计的俯仰和横滚

因此，我在下面的较长答案中假设板子会加速，在此期间，您仍然需要能够在短时间内测量俯仰，滚动和偏航。如果所有测量板都固定不动，那么Mahendra Gunawardena的答案将非常适合您。如果这正在进入到诸如赛格威，模型飞机，多旋翼飞机或任何会四处移动的设备中，您可能需要继续阅读。这篇文章是关于如何通过称为传感器融合的方法使用所有三个传感器的。传感器融合可让您获得每个传感器的优势，并最大程度地减少每个传感器的弱点带来的影响。

传感器特性和背景

首先要了解，加速度计可以测量施加在其上的所有力，而不仅仅是重力。因此，在加速度计处于静止状态且没有任何振动的情况下，您可以使用Mahendra Gunawardena的答案所示的一些基本三角函数，完美地确定采用哪种方法。但是，由于加速度计会承受所有力，因此任何振动都会产生噪音。还应该注意的是，如果板子正在加速，则不能仅使用简单的三角函数，因为加速度计报告的力不仅是地心引力，而且是导致您加速的力。

磁力计比加速度计更简单。运动不会造成问题，但是铁和其他磁铁之类的东西最终会影响您的输出。如果引起这种干扰的源是恒定的，则不难处理，但如果这些源不是恒定的，则会产生大量的噪声，难以消除。

在这三个传感器中，陀螺仪被认为是最可靠的，并且它们在测量转速方面通常非常非常好。它不受铁源之类的因素的影响，并且加速度基本上不会影响其测量转速的能力。他们在报告设备旋转速度方面做得非常好，但是，由于您要查找绝对角度，因此必须对速度进行积分才能获得位置。这样做会将最后一次测量的误差添加到新测量的误差中，因为积分基本上是一个范围内的值之和，即使一次测量的误差每秒仅降低0.01度，在100次测量中，您的位置可以偏离1度，可以测量1000次，您可以偏离10度。如果您每秒要进行数百次测量，您可以看到这会导致问题。这通常称为陀螺漂移。

传感器融合

现在，将所有这些传感器一起使用的好处在于，您可以使用来自加速度计和磁力计的信息来抵消陀螺仪漂移。最终可以使您获得陀螺仪的精度和速度，而不会出现陀螺仪漂移的致命缺陷。

将这三个传感器的数据组合起来可以用一种以上的方法完成，我将讨论使用互补滤波器，因为它比卡尔曼滤波器和卡尔曼滤波器简单得多，将占用嵌入式系统上更多的资源。通常，互补过滤器足够好，实现起来更简单（假设您不使用预构建的库），并使您更快地处理数据。

现在进入流程。您需要做的第一步是集成陀螺仪输出，以将角速度转换为角位置。您还很有可能必须在加速度计和磁力计上应用一个低通滤波器，以处理输出中的噪声。一个简单的FIR滤波器（如下所示）在这里工作。使用某些三角函数，您可以使用加速度计找到俯仰和滚动，而通过磁力计可以找到偏航。

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

权重只是一个常数，可以根据要处理的噪声量进行调整，噪声越高，权重值将越小。现在，可以通过下面的代码行组合来自传感器的数据。

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

应当注意，数据是俯仰，滚动和偏航的矢量。您也可以只使用三个变量来执行此操作，而不是使用数组。为了进行此计算，陀螺仪提供了一个以俯仰，横滚和横摆度数表示的位置，磁力计提供了横摆角，而加速度计提供了自己的俯仰和横摆数。

如果您仍然想要更多信息，可以在Google上搜索“带有互补滤波器的传感器融合”，这方面有很多文章。

从加速器传感器数据中，您只能计算俯仰和横滚。飞思卡尔的波纹管文档提供了大量信息，说明您需要什么：

AN3461-使用三轴加速度计进行倾斜感应

根据该文件的说法，

$$\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. $\theta$
3. $\psi$

$R_{xyz}$$\psi$