MPU6050开发 — 数据分析

如需转载请注明出处：https://blog.csdn.net/_/article/details/

上一篇文章结尾，留了一些思考问题。现在只是得到MPU6050的一些原始数据，还未做滤波处理。

接下来先讲，加速度计和陀螺仪的计算公式，然后进一步延伸出姿态滤波。

一、加速度计

（1）计算公式

参看：Arduino教程：MPU6050的数据获取、分析与处理

参看：GUIDE_D_UTILISATION_D_UN_MODULE_DE_STABILISATION_INTEGRE.pdf

先介绍一下MPU6050芯片的座标系的定义：

令芯片表面朝向自己，将其表面文字转至正确角度，此时，以芯片内部中心为原点，水平向右的为X轴，竖直向上的为Y轴，指向自己的为Z轴。见下图：

然后我们知道三轴加速度计：

三个加速度分量均以重力加速度 g 的倍数为单位，能够表示的加速度范围，即倍率可以统一设定，有4个可选倍率：±2g、±4g、±8g、±16g。上一篇分析完了，初始化MPU6050设置加速度计输出的满量程范围为± 2g，加速度计每个 LSB 的灵敏度应为 16384 LSB/g。

满量程范围± 2g和灵敏度16384 LSB/g有啥关系?

上面说了这三个加速度分量是16位的二进制补码值，且是有符号的。故而其输出范围 -32768~32767。((2^16)/2)

32767/2 = 16384 即加速度计的灵敏度。

那这个灵敏度又有啥用呢？我们拿串口调试工具，显示的一组数据来举个例子：

A X: 03702 Y: 12456 Z: 06268 G X:-00023 Y:-00059 Z: 00005

加速度计 X 轴获取原始数据位 03702，那么它对应的加速度数据是：03702/16384 = 0.23g.

g为加速度的单位，重力加速度定义为1g, 等于9.8米每平方秒。

具体的加速度公式：加速度数据 = 加速度轴原始数据/加速度灵敏度

（2）加速度计详解

我们可以把加速度计想象为一个正立方体盒子里放着一个球，如果我们把这个盒子放在没有引力场的地方，或者没有其他可能影响球的位置的场地，球就会漂浮在盒子的中间。 你可以想象这个盒子在远离任何宇宙体的外太空中，或者如果这样一个地方很难想象至少有一个宇宙飞船围绕着这个行星在一切都处于失重状态的轨道上运行。 从上面的图片可以看出，我们为每个轴分配了一对墙（我们移除了墙Y +，所以我们可以看到框内）。 想象一下，每面墙都是压力敏感的。

如果我们将盒子突然移动到左边（我们用加速度 1g = 9.8m/s^2 加速），那么球就会碰到墙壁 X-。 然后，我们测量球施加到墙上的压力，并在X轴上输出 -1g 的值。

请注意，加速度计实际上会检测到与加速度矢量方向相反的力。 这股力量通常被称为惯性力量或虚构力量。 有一点你应该从中学到，就是加速度计通过施加在其中一个墙上的力来间接测量加速度（根据我们的模型，这可能是一个弹簧或者是现实生活中的其他加速度计）。 这个力可能是由加速引起的，但是我们将在下一个例子中看到它并不总是由加速引起的。

如果我们将模型放在地球上，球就会落在Z墙上，并在底壁上施加1g的力，如下图所示：

0.71的值不是任意的，它们实际上是SQRT（1/2）的近似值。 随着我们介绍下一个加速计模型，这将变得更加清晰。
在以前的模型中，我们已经固定了引力并旋转了我们的想象框。 在最后的两个例子中，我们分析了两个不同盒子的输出，而力矢量保持不变。 虽然这对于理解加速度计如何与外力相互作用很有用，但是如果我们将坐标系固定到加速度计的轴上，并想象力矢量在我们周围旋转，那么执行计算就更为实用。

请看上面的模型，我保留了箭头的颜色，这样你就可以从以前的模型转换到新的模型。 试想一下，新模型中的每个轴都垂直于前一个模型中的各个面。 矢量R是加速度计测量的力矢量（它可以是来自上述示例的重力或惯性力，或者是两者的组合）。 Rx，Ry，Rz是R矢量在X，Y，Z轴上的投影。 请注意以下关系：

    R^2 = Rx^2 + Ry^2 + Rz^2    方程1

上面有提到，SQRT（1/2）〜0.71的值不是随机的。 如果用上面的公式来表示，在回忆我们的引力为1g后，我们可以证实：

    1^2 = (-SQRT(1/2) )^2 + 0 ^2 + (-SQRT(1/2))^2

简单地通过在方程1中代入 R = 1，Rx = -SQRT（1/2），Ry = 0，Rz = -SQRT（1/2）

得出：SQRT（1/2） = √0.5 ≈ 0.71

注：SQRT为平方根计算，SQRT（1/2）的意思就是0.5的平方根。

让我们继续考虑一个简单的例子，假设我们的10位ADC模块给了我们三个加速度计通道（轴）的以下值：

这里有一个简单的说明：对于8位ADC，最后的分频器是255 = 2 ^ 8 -1，对于12位ADC，最后的分频器是4095 = 2 ^ 12 -1。

将这个公式应用于所有3个通道，我们得到：

每个加速度计都有一个零g的电压水平，你可以在规格中找到它，这是对应于 0g 的电压。 为了得到有符号的电压值，我们需要计算从这个水平的转变。 假设我们的 0g 电压等级是VzeroG = 1.65V。 我们计算零g电压的电压偏移如下：

现在我们的加速度计读数以伏特为单位，它仍然不是 g（9.8 m / s ^ 2），做最后的转换我们应用加速度计的灵敏度，通常用mV/g表示。 可以说我们的灵敏度= 478.5mV/g = 0.4785V/g。 灵敏度值可以在加速度计规格中找到。 为了得到以g表示的最终力值，我们使用下面的公式：

我们当然可以将所有步骤结合在一个公式中，但是我经历了所有的步骤，清楚地说明了如何从ADC读数到用g表示的力矢量分量。

    Rx = (AdcRx * Vref / 1023 – VzeroG) / Sensitivity    方程2
    Ry = (AdcRy * Vref / 1023 – VzeroG) / Sensitivity
    Rz = (AdcRz * Vref / 1023 – VzeroG) / Sensitivity

======================================================

说明：

上面讲了好多理论，该作者是以模拟加速度计来做的介绍，最后得出的公式。

而具体到 MPU6050 加速度计的公式，就是我开头讲的：加速度数据 = 加速度轴原始数据/加速度灵敏度

======================================================

我们现在有三个组成部分来定义我们的惯性力矢量，如果该装置不受除引力以外的其他力的影响，我们可以认为这是我们的引力矢量的方向。 如果要计算设备相对于地面的倾斜度，则可以计算此向量与Z轴之间的角度。 如果您还对每个轴的倾斜方向感兴趣，则可以将这个结果分成两个分量：X和Y轴上的倾角，可以计算为重力矢量与X / Y轴之间的角度。 现在我们已经计算了Rx，Ry和Rz的值，计算这些角度比您想象的更简单。 让我们回到我们最后的加速度计模型，并做一些附加的符号：

我们感兴趣的角度是X，Y，Z轴与力矢量R之间的角度。我们将这些角度定义为Axr，Ayr，Azr。 你可以从R和Rx形成的直角三角形中注意到：

我们可以从方程1中推导出 R = SQRT（Rx ^ 2 + Ry ^ 2 + Rz ^ 2）。

我们可以使用 arccos（）函数（逆cos（）函数） 来找到我们的角度：

    Azr = arccos(Rz/R)   （重要公式）

======================================================

说明：

这里的Rx、Ry、Rz 就是用公式（加速度数据 = 加速度轴原始数据/加速度灵敏度）得出的各轴的加速度数据。

R 也可以通过方程1 导出，R = SQRT（Rx ^ 2 + Ry ^ 2 + Rz ^ 2）

最后就可以通过下面的公式：

    Azr = arccos(Rz/R) 

得到X，Y，Z轴与力矢量R之间的角度（Axr，Ayr，Azr）

举个栗子：

其中 arccos (1) = 0，arccos (0) = 90

工具：arccos 计算器     计算arccos的时候  点击左上角那个上档功能   cos就变成 arccos

======================================================

我们用了很长的篇幅来解释加速度计模型，只是想得出这些公式。 根据您的应用程序，您可能想要使用我们派生的任何中间公式。 我们也将很快介绍陀螺仪模型，我们将看到如何将加速度计和陀螺仪数据结合起来，以提供更准确的倾角估计。

但是在我们这样做之前，让我们做一些更有用的符号：

这个三元组通常称为方向余弦，它基本上代表了与我们的R向量具有相同方向的单位矢量（长度为1的矢量）。 您可以轻松验证：

    SQRT(cosX^2 + cosY^2 + cosZ^2) = 1

这是一个很好的属性，因为它使我们无法监视R向量的模数（长度）。 通常情况下，如果我们只是对惯性向量的方向感兴趣，为了简化其他计算，将其模数标准化是有意义的。

二、陀螺仪

（1）计算公式

然后我们知道三轴陀螺仪：

三个角速度分量均以“度/秒”为单位，能够表示的角速度范围，即倍率可统一设定，有4个可选倍率：±250°/s， ±500°/s， ±1000°/s， ±2000°/s。上一篇分析完了，初始化MPU6050设置陀螺仪输出满量程范围为 ± 2000 °/s，陀螺仪每个 LSB 的灵敏度为 16.4 LSB/°/s。

满量程范围± 2000 °/s和灵敏度16.4 LSB/°/s有啥关系?
上面说了这三个陀螺仪分量是16位的二进制补码值，且是有符号的。故而其输出范围 -32768~32767。((2^16)/2)

32767/2000 = 16.4 即陀螺仪的灵敏度。

那这个灵敏度又有啥用呢？我们拿串口调试工具，显示的一组数据来举个例子：
A X: 03702 Y: 12456 Z: 06268 G X:-00023 Y:-00059 Z: 00005

（2）陀螺仪详解

我们不打算像加速度计那样为陀螺仪引入任何等效的箱体模型，而是直接跳到第二个加速度计模型，我们将根据这个模型显示陀螺仪测量的结果。

每个陀螺仪通道测量围绕其中一个轴的旋转。 例如一个双轴陀螺仪将测量X轴和Y轴的旋转（或者有些人会说“大约”）。 为了用数字来表示这个旋转，我们来做一些符号。 首先让我们定义：

从Rxz和Rz形成的直角三角形，使用毕达哥拉斯定理我们得到：

还要注意的是：

可以从等式1和上面的等式得出，R^2 = Rxz^2 + Ry^2

或者它可以从由R和Ryz形成的直角三角形导出  R^2 = Ryz^2 + Rx^2

在本文中，我们不打算使用这些公式，但是注意模型中所有值之间的关系是有用的。

现在我们正在接近陀螺仪的测量。 陀螺仪测量上述角度的变化率。 换句话说，它将输出一个与这些角度的变化率线性相关的值。 为了解释这个假设，我们假设我们已经在时间 t0 测量了围绕轴线 Y 的旋转角度（这将是Axz角度），并且我们将其定义为Axz0，接下来我们在稍后的时间 t1 测量该角度并且是 Axz1。 变化率计算如下：

    RateAxz = (Axz1 – Axz0) / (t1 – t0).

如果我们用度和秒来表示Axz，那么这个值将以 deg/s 表示。 这是一个陀螺仪的测量。

在实践中，陀螺仪（除非它是一个特殊的数字陀螺仪）很少会给你一个以 deg/s 表示的值。 与加速度计一样，您将得到一个ADC值，您需要使用类似于方程2的公式转换为 deg/s。 我们已经定义了加速度计的了，下面来介绍一下ADC转换陀螺仪的转换公式（我们假设我们使用的是10位ADC模块，8位ADC用1023取代1023，12位ADC用4095取代1023）。

    RateAxz = (AdcGyroXZ * Vref / 1023 – VzeroRate) / Sensitivity    方程3
    RateAyz = (AdcGyroYZ * Vref / 1023 – VzeroRate) / Sensitivity

AdcGyroXZ，AdcGyroYZ – 是从我们的adc模块中获得的，它们分别表示在YZ平面上测量XZ中R向量投影旋转的通道，这相当于分别在Y轴和X轴周围进行旋转。

Vref – 是ADC参考电压，我们将在下面的例子中使用3.3V

VzeroRate – 是零速率电压，换句话说就是陀螺仪不受任何旋转时输出的电压，对于Acc_Gyro电路板，例如1.23V（您可以在规格中找到这个值， 不要相信规格大多数陀螺仪在焊接后会受到轻微的偏移，所以使用电压表测量每个轴输出的VzeroRate，通常这个值在陀螺仪焊接后不会随时间变化，如果变化的话 – 写一个校准程序， 设备启动时，必须指示用户在启动陀螺仪时将设备保持在静止位置）。

Sensitivity – 陀螺仪的灵敏度，以mV /（deg / s）表示，通常写为mV / deg / s，基本上告诉你陀螺仪输出增加多少mV，如果将转速提高1度/秒。 Acc_Gyro板的灵敏度为2mV / deg / s或0.002V / deg / s

举一个例子，假设我们的ADC模块返回以下值：

使用上面的公式，并使用Acc_Gyro板的规格参数，我们会得到：

换句话说，设备以306度/秒的速度围绕X轴（或者我们可以说它在XZ平面内旋转）绕X轴旋转（或者我们可以说它在YZ平面内旋转），速度为 – 94度/秒。请注意，负号表示设备的旋转方向与传统的正方向相反。按照惯例，一个旋转方向是正的。一个好的陀螺仪规格表会告诉你哪个方向是正的，否则你将不得不通过对该器件的实验来找到它，并注意哪个旋转方向导致输出引脚上的电压增加。这最好用示波器来完成，因为一旦停止旋转，电压将回落到零速率水平。如果您使用的是万用表，您必须保持恒定的旋转速率至少几秒钟，并记下旋转过程中的电压，然后将其与零速率电压进行比较。如果它大于零速率电压，则意味着旋转方向为正。

======================================================

说明：

上面讲的又是该作者是以双轴模拟陀螺仪来做的介绍，最后得出的公式。

而具体到 MPU6050 它是三轴陀螺仪其公式就是我开头讲的：陀螺仪数据 = 陀螺仪轴原始数据/加速度灵敏度

======================================================

三、结合加速度计和陀螺仪数据

使用组合了加速度计和陀螺仪的组合IMU设备的第一步是对齐它们的坐标系。 最简单的方法是选择加速度计坐标系作为参考坐标系。 大多数加速计数据表将显示相对于物理芯片或设备图像的X，Y，Z轴的方向。 例如，这里是Acc_Gyro板的规格中所示的X，Y，Z轴的方向：

    RateAxz = InvertAxz *（AdcGyroXZ * Vref / 1023 – VzeroRate）/Sensitivity，其中InvertAxz是1或-1

对于RateAyz，可以通过围绕X轴旋转设备来完成相同的测试，并且可以确定哪个陀螺仪输出对应于RateAyz，以及是否需要反转。 一旦你有了InvertAyz的值，你应该使用下面的公式来计算RateAyz：

    RateAyz = InvertAyz * (AdcGyroYZ * Vref / 1023 – VzeroRate) / Sensitivity

如果你想在Acc_Gyro板上做这些测试，你会得到如下结果：

陀螺仪不是没有噪音的，但是因为它测量旋转，所以对线性机械运动（加速度计所受到的噪音类型）不那么敏感，但是陀螺仪还有其他类型的问题，例如漂移（不回到零速率值当旋转停止时）。尽管如此，通过对来自加速度计和陀螺仪的数据进行平均，我们可以获得比通过单独使用加速度计数据获得的当前设备倾角更好的估计。

在接下来的步骤中，我将介绍一种受卡尔曼滤波器中的一些想法启发的算法，然而它在嵌入式设备上实现起来要简单得多。在此之前，让我们先看看我们想要算法的计算。那么，引力矢量的方向是R = [Rx，Ry，Rz]，我们可以从中得出其他值，如Axr，Ayr，Azr或cosX，cosy，cosZ，它们会给我们一个关于我们设备的倾向的想法相对于地平面，我们在第一部分讨论了这些值之间的关系。有人可能会说 – 我们不是已经有了第1部分公式2中的Rx，Ry，Rz值吗？是的，但请记住，这些值仅来自加速度计数据，所以如果你直接在你的应用程序中使用它们，你可能会得到比应用程序能够容忍的更多的噪声。为了避免进一步的混淆，我们重新定义加速度计测量如下：

Racc – 是由加速度计测量的惯性力矢量，由以下部件（X，Y，Z轴上的投影）组成：

到目前为止，我们有一组测量值，我们可以从加速度计ADC值纯粹获得。 我们将这组数据称为“向量”，我们将使用下面的符号。

    Racc = [RxAcc,RyAcc,RzAcc]

    |Racc| = SQRT(RxAcc^2 +RyAcc^2 + RzAcc^2)

不过要确定的是更新这个向量是有意义的，如下所示：

    Racc(标准化) = [RxAcc/|Racc| , RyAcc/|Racc| , RzAcc/|Racc|].

    Rest = [RxEst,RyEst,RzEst]

这将是我们的算法的输出，这些是基于陀螺仪数据和基于过去估计的数据的校正值。

我们将通过相信我们的加速度计并分配：

接下来，我们将在T秒的相同时间间隔内进行常规测量，我们将获得新的测量结果，我们将定义为Racc（1），Racc（2），Racc（3）等等。 我们还会在每个时间间隔Rest（1），Rest（2），Rest（3）等等上发布新的估计值。

假设我们在步骤n。 我们有两套我们想要使用的已知值：

在计算Rest（n）之前，我们介绍一个新的测量值，我们可以从陀螺仪中获得一个以前的估计值。
我们将它称为Rgyro，它也是一个由3个部分组成的向量：

    Rgyro = [RxGyro,RyGyro,RzGyro]

我们将一次计算这个向量一个组件。 我们将从RxGyro开始。

首先观察我们的陀螺仪模型中的以下关系，从由Rz和Rxz组成的直角三角形，我们可以推导出：

    tan(Axz) = Rx/Rz => Axz = atan2(Rx,Rz)

atan2可能是你以前从未使用过的函数，它和atan类似，不同的是它返回（-PI，PI）范围内的值，而不是由atan返回的（-PI / 2，PI / 2） 2个参数，而不是一个。 它允许我们将Rx，Rz的两个值转换为360度全方位角度（-PI到PI）。 你可以在这里阅读关于atan2的更多信息。

所以知道RxEst（n-1）和RzEst（n-1）我们可以找到：

我们可以找到相同的方式：

    Ayz(n) = Ayz(n-1) + RateAyz(n) * T

好的，现在我们有Axz（n）和Ayz（n）。 我们从哪里去扣除RxGyro / RyGyro？ 从方程 1，我们可以写出矢量Rgyro的长度如下：

    |Rgyro| = SQRT(RxGyro^2 + RyGyro^2 + RzGyro^2)

另外，因为我们对Racc向量进行了归一化处理，所以我们可以假设它的长度是1，并且在旋转之后它没有改变，所以写出来是相对安全的：

    |Rgyro| = 1

让我们在下面的计算中采用一个临时的简短表示法：

    x =RxGyro , y=RyGyro, z=RzGyro

使用上面的关系，我们可以写：

    x = x / 1 = x / SQRT(x^2+y^2+z^2)

用SQRT（x^2+z^2）来划分分数的分子和分母，

    x = ( x / SQRT(x^2 + z^2) ) / SQRT( (x^2 + y^2 + z^2) / (x^2 + z^2) )

注意 x / SQRT(x^2 + z^2) = sin(Axz)，所以：

    x = sin(Axz) / SQRT (1 + y^2 / (x^2 + z^2) )

现在将SQRT内部的分子和分母相乘z^2

    x = sin(Axz) / SQRT (1 + y^2  * z ^2 / (z^2 * (x^2 + z^2)) )

注意 z / SQRT(x^2 + z^2) = cos(Axz) and y / z = tan(Ayz)，所以最后：

    x = sin(Axz) / SQRT (1 + cos(Axz)^2 * tan(Ayz)^2 )

回到我们的符号，我们得到：

    RxGyro = sin(Axz(n)) / SQRT (1 + cos(Axz(n))^2 * tan(Ayz(n))^2 )

同样的方式，我们发现

    RyGyro = sin(Ayz(n)) / SQRT (1 + cos(Ayz(n))^2 * tan(Axz(n))^2 )

注意：可以进一步简化这个公式。 通过将sin（Axz（n））的两部分除以得到：

现在加减  cos(Axz(n))^2/sin(Axz(n))^2   = cot(Axz(n))^2 

RxGyro =  1  / SQRT (1/ sin(Axz(n))^2  –  cos(Axz(n))^2/sin(Axz(n))^2   + cot(Axz(n))^2  * sin(Ayz(n))^2  / cos(Ayz(n))^2  + cot(Axz(n))^2 )

并通过分组条款1和2，然后3和4我们得到：

RxGyro =  1  / SQRT (1  +   cot(Axz(n))^2 * sec(Ayz(n))^2 ),      where  cot(x) = 1 / tan(x)  and  sec(x) = 1 / cos(x)

该公式仅使用2个三角函数，并且可以在计算上更简单。 如果你有Mathematica程序，你可以验证它

通过评估 FullSimplify [Sin [A] ^ 2 /（1 + Cos [A] ^ 2 * Tan [B] ^ 2）]

现在，我们终于可以找到：

    RzGyro  =  Sign(RzGyro)*SQRT(1 – RxGyro^2 – RyGyro^2).

当 RzGyro> = 0 时符号（RzGyro）= 1，当 RzGyro <0 时符号（RzGyro）= -1。

估计这个简单的方法是采取：

我们使用哪个值来计算更新的估计值Rest（n）？ 你可能猜到我们会同时使用这两个。 我们将使用加权平均值，以便：

    Rest(n) = (Racc + Rgyro * wGyro ) / (1 + wGyro)

在上面的公式中，wGyro告诉我们相比于我们的加速度计，我们相信我们的陀螺仪有多少。 这个值可以通过实验选择，通常在5到20之间的值将会产生好的结果。
该算法与卡尔曼滤波器的主要区别在于该权重相对固定，而在卡尔曼滤波器中，根据加速度计读数的测量噪声永久更新权重。 卡尔曼滤波器专注于为您提供“最好的”理论结果，而这种算法可以为您的实际应用提供“足够好”的结果。 您可以实现一个算法，根据您测量的噪音因素来调整wGyro，但是对于大多数应用程序，固定值将会很好地工作。
我们离得到最新的估计值只有一步之遥：
    RxEst(n) = (RxAcc + RxGyro * wGyro ) / (1 + wGyro)
    RyEst(n) = (RyAcc + RyGyro * wGyro ) / (1 + wGyro)
    RzEst(n) = (RzAcc + RzGyro * wGyro ) / (1 + wGyro)
现在让我们再次归一化这个向量：

    R = SQRT(RxEst(n) ^2 + RyEst(n)^2 +  RzEst(n)^2 )

注意：要实际执行和测试这个算法，请阅读本文：

Arduino code for IMU Guide algorithm. Using a 5DOF IMU (accelerometer and gyroscope combo)

======================================================

说明：

这里一大堆的公式，看的我头大。不过加速度计和陀螺仪为什么结合这个问题倒是搞清楚了。

但是还是有问题啊，这个卡尔曼滤波程序怎么写？？

======================================================

四、姿态融合

参看：MPU6050姿态融合

（下面内容是以STM32开发板来讲的，而我现在使用的C52单片机，有不符合我开发的地方，可做了解）

首先要明确，MPU6050 是一款姿态传感器，使用它就是为了得到待测物体（如四轴、平衡小车） x、y、z 轴的倾角（俯仰角 Pitch、滚转角 Roll、偏航角 Yaw） 。我们通过 I2C 读取到 MPU6050 的六个数据（三轴加速度 AD 值、三轴角速度 AD 值）经过姿态融合后就可以得到 Pitch、Roll、Yaw 角。
本帖主要介绍三种姿态融合算法：四元数法 、一阶互补算法和卡尔曼滤波算法。

（1）四元数法

可通过下面的算法，可以把六个数据转化成四元数（q0、q1、q2、q3），然后四元数转化成欧拉角（P、R、Y 角）。

#include 
  
    #include "stm32f10x.h" //--------------------------------------------------------------------------------------------------- // 变量定义 #define Kp 100.0f // 比例增益支配率收敛到加速度计/磁强计 #define Ki 0.002f // 积分增益支配率的陀螺仪偏见的衔接 #define halfT 0.001f // 采样周期的一半 float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // 四元数的元素，代表估计方向 float exInt = 0, eyInt = 0, ezInt = 0; // 按比例缩小积分误差 float Yaw,Pitch,Roll; //偏航角，俯仰角，翻滚角 void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) { float norm; float vx, vy, vz; float ex, ey, ez; // 测量正常化 norm = sqrt(ax*ax + ay*ay + az*az); ax = ax / norm; //单位化 ay = ay / norm; az = az / norm; // 估计方向的重力 vx = 2*(q1*q3 - q0*q2); vy = 2*(q0*q1 + q2*q3); vz = q0*q0 - q1*q1 - q2*q2 + q3*q3; // 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和 ex = (ay*vz - az*vy); ey = (az*vx - ax*vz); ez = (ax*vy - ay*vx); // 积分误差比例积分增益 exInt = exInt + ex*Ki; eyInt = eyInt + ey*Ki; ezInt = ezInt + ez*Ki; // 调整后的陀螺仪测量 gx = gx + Kp*ex + exInt; gy = gy + Kp*ey + eyInt; gz = gz + Kp*ez + ezInt; // 整合四元数率和正常化 q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT; q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT; q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT; q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT; // 正常化四元 norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); q0 = q0 / norm; q1 = q1 / norm; q2 = q2 / norm; q3 = q3 / norm; Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch ,转换为度数 Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // rollv //Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //此处没有价值，注掉 } 
  

说明：

这里 IMUupdate 函数的形参就是我们获取的加速度计和陀螺仪的原始数据，将其带入不就行了吗？

说明这个四元数法在C52单片机上也能写一写的。

不过要注意的是，四元数算法输出的是三个量 Pitch、Roll 和 Yaw，运算量很大。而像平衡小车这样的例子只需要一个角（Pitch 或 Roll ）就可以满足工作要求，个人觉得做平衡小车最好不用四元数法。

（2）一阶互补算法

MPU6050 可以输出三轴的加速度和角速度。通过加速度和角速度都可以得到 Pitch 和 Roll 角（加速度不能得到 Yaw 角），就是说有两组 Pitch、Roll 角，到底应该选哪组呢？别急，先分析一下。MPU6050 的加速度计和陀螺仪各有优缺点，三轴的加速度值没有累积误差，且通过算 tan()  可以得到倾角，但是它包含的噪声太多（因为待测物运动时会产生加速度，电机运行时振动会产生加速度等），不能直接使用；陀螺仪对外界振动影响小，精度高，通过对角速度积分可以得到倾角，但是会产生累积误差。所以，不能单独使用 MPU6050 的加速度计或陀螺仪来得到倾角，需要互补。一阶互补算法的思想就是给加速度和陀螺仪不同的权值，把它们结合到一起，进行修正。得到 Pitch 角的程序如下：

//一阶互补滤波 float K1 =0.1; // 对加速度计取值的权重 float dt=0.001;//注意：dt的取值为滤波器采样时间 float angle; angle_ax=atan(ax/az)*57.3; //加速度得到的角度 gy=(float)gyo[1]/7510.0; //陀螺仪得到的角速度 Pitch = yijiehubu(angle_ax,gy); float yijiehubu(float angle_m, float gyro_m)//采集后计算的角度和角加速度 { angle = K1 * angle_m + (1-K1) * (angle + gyro_m * dt); return angle; }

互补算法只能得到一个倾角，这在平衡车项目中够用了，而在四轴飞行器设计中还需要 Roll 和 Yaw，就需要两个 互补算法，我是这样写的，注意变量不要搞混：

//一阶互补滤波 float K1 =0.1; // 对加速度计取值的权重 float dt=0.001;//注意：dt的取值为滤波器采样时间 float angle_P,angle_R; float yijiehubu_P(float angle_m, float gyro_m)//采集后计算的角度和角加速度 { angle_P = K1 * angle_m + (1-K1) * (angle_P + gyro_m * dt); return angle_P; } float yijiehubu_R(float angle_m, float gyro_m)//采集后计算的角度和角加速度 { angle_R = K1 * angle_m + (1-K1) * (angle_R + gyro_m * dt); return angle_R; }

单靠 MPU6050 无法准确得到 Yaw 角，需要和地磁传感器结合使用。

（3）卡尔曼滤波

其实卡尔曼滤波和一阶互补有些相似，输入也是一样的。在此给出具体程序，和一阶互补算法一样，每次卡尔曼滤波只能得到一个方向的角度。

#include 
  
    #include "stm32f10x.h" #include "Kalman_Filter.h" //卡尔曼滤波参数与函数 float dt=0.001;//注意：dt的取值为kalman滤波器采样时间 float angle, angle_dot;//角度和角速度 float P[2][2] = { 
   { 1, 0 }, { 0, 1 }}; float Pdot[4] ={ 0,0,0,0}; float Q_angle=0.001, Q_gyro=0.005; //角度数据置信度,角速度数据置信度 float R_angle=0.5 ,C_0 = 1; float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1; //卡尔曼滤波 float Kalman_Filter(float angle_m, float gyro_m)//angleAx 和 gyroGy { angle+=(gyro_m-q_bias) * dt; angle_err = angle_m - angle; Pdot[0]=Q_angle - P[0][1] - P[1][0]; Pdot[1]=- P[1][1]; Pdot[2]=- P[1][1]; Pdot[3]=Q_gyro; P[0][0] += Pdot[0] * dt; P[0][1] += Pdot[1] * dt; P[1][0] += Pdot[2] * dt; P[1][1] += Pdot[3] * dt; PCt_0 = C_0 * P[0][0]; PCt_1 = C_0 * P[1][0]; E = R_angle + C_0 * PCt_0; K_0 = PCt_0 / E; K_1 = PCt_1 / E; t_0 = PCt_0; t_1 = C_0 * P[0][1]; P[0][0] -= K_0 * t_0; P[0][1] -= K_0 * t_1; P[1][0] -= K_1 * t_0; P[1][1] -= K_1 * t_1; angle += K_0 * angle_err; //最优角度 q_bias += K_1 * angle_err; angle_dot = gyro_m-q_bias;//最优角速度 return angle； } 
  

总结：

三种融合算法都能够输出姿态角（Pitch 和 Roll ），一次四元数法可以输出 P、R、Y 三个倾角，计算量比较大。一阶互补和卡尔曼滤波每次只能输出一个轴的姿态角。

再有这个一阶互补和卡尔曼滤波里的采集后计算的角度和角加速度，上面我们已经讲了，然后再带入到 Kalman_Filter 函数里，那么在C52单片机上实现也不成问题的。

（4）三种滤波比较源代码

参看：MPU6050数据采集及其意义和滤波（一阶互补滤波、二阶互补滤波、卡尔曼滤波）

#include "Wire.h" #include "I2Cdev.h" #include "MPU6050.h" #include "Timer.h"//时间操作系统头文件 本程序用作timeChange时间采集并处理一次数据 Timer t;//时间类 float timeChange=20;//滤波法采样时间间隔毫秒 float dt=timeChange*0.001;//注意：dt的取值为滤波器采样时间 // 陀螺仪 float angleAx,gyroGy;//计算后的角度（与x轴夹角）和角速度 MPU6050 accelgyro;//陀螺仪类 int16_t ax, ay, az, gx, gy, gz;//陀螺仪原始数据 3个加速度+3个角速度 //一阶滤波 float K1 =0.05; // 对加速度计取值的权重 //float dt=20*0.001;//注意：dt的取值为滤波器采样时间 float angle1;//一阶滤波角度输出 //二阶滤波 float K2 =0.2; // 对加速度计取值的权重 float x1,x2,y1;//运算中间变量 //float dt=20*0.001;//注意：dt的取值为滤波器采样时间 float angle2;//er阶滤波角度输出 //卡尔曼滤波参数与函数 float angle, angle_dot;//角度和角速度 float angle_0, angle_dot_0;//采集来的角度和角速度 //float dt=20*0.001;//注意：dt的取值为kalman滤波器采样时间 //一下为运算中间变量 float P[2][2] = { 
  { 1, 0 }, { 0, 1 }}; float Pdot[4] ={ 0,0,0,0}; float Q_angle=0.001, Q_gyro=0.005; //角度数据置信度,角速度数据置信度 float R_angle=0.5 ,C_0 = 1; float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1; void setup() { Wire.begin();//初始化 Serial.begin(9600);//初始化 accelgyro.initialize();//初始化 int tickEvent1=t.every(timeChange, getangle);//本语句执行以后timeChange毫秒执行回调函数getangle int tickEvent2=t.every(50, printout) ;//本语句执行以后50毫秒执行回调函数printout，串口输出 } void loop() { t.update();//时间操作系统运行 } void printout() { Serial.print(angleAx);Serial.print(','); Serial.print(angle1);Serial.print(','); Serial.print(angle2);Serial.print(','); // Serial.print(gx/131.00);Serial.print(','); Serial.println(angle);//Serial.print(','); // Serial.println(Output); } void getangle() { accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);//读取原始6个数据 angleAx=atan2(ax,az)*180/PI;//计算与x轴夹角 gyroGy=-gy/131.00;//计算角速度 Yijielvbo(angleAx,gyroGy);//一阶滤波 Erjielvbo(angleAx,gyroGy);//二阶滤波 Kalman_Filter(angleAx,gyroGy); //卡尔曼滤波 } void Yijielvbo(float angle_m, float gyro_m) { angle1 = K1 * angle_m+ (1-K1) * (angle1 + gyro_m * dt); } void Erjielvbo(float angle_m,float gyro_m) { x1=(angle_m-angle2)*(1-K2)*(1-K2); y1=y1+x1*dt; x2=y1+2*(1-K2)*(angle_m-angle2)+gyro_m; angle2=angle2+ x2*dt; } void Kalman_Filter(double angle_m,double gyro_m) { angle+=(gyro_m-q_bias) * dt; angle_err = angle_m - angle; Pdot[0]=Q_angle - P[0][1] - P[1][0]; Pdot[1]=- P[1][1]; Pdot[2]=- P[1][1]; Pdot[3]=Q_gyro; P[0][0] += Pdot[0] * dt; P[0][1] += Pdot[1] * dt; P[1][0] += Pdot[2] * dt; P[1][1] += Pdot[3] * dt; PCt_0 = C_0 * P[0][0]; PCt_1 = C_0 * P[1][0]; E = R_angle + C_0 * PCt_0; K_0 = PCt_0 / E; K_1 = PCt_1 / E; t_0 = PCt_0; t_1 = C_0 * P[0][1]; P[0][0] -= K_0 * t_0; P[0][1] -= K_0 * t_1; P[1][0] -= K_1 * t_0; P[1][1] -= K_1 * t_1; angle += K_0 * angle_err; //最优角度 q_bias += K_1 * angle_err; angle_dot = gyro_m-q_bias;//最优角速度 }

五、欧拉角

参看：姿态角 — 维基百科

参看：姿态角（Euler角）：yaw pitch roll

上面提到的俯仰角 Pitch、滚转角 Roll、偏航角 Yaw、欧拉角等等都是什么呢？

（1）俯仰角θ（pitch）

机体坐标系X轴与水平面的夹角。当X轴的正半轴位于过坐标原点的水平面之上（抬头）时，俯仰角为正，否则为负。

（2）偏航角ψ（yaw）：

机体坐标系xb轴在水平面上投影与地面坐标系xg轴（在水平面上，指向目标为正）之间的夹角，由xg轴逆时针转至机体xb的投影线时，偏航角为正，即机头右偏航为正，反之为负。

（3）滚转角Φ（roll）

机体坐标系zb轴与通过机体xb轴的铅垂面间的夹角，机体向右滚为正，反之为负。

六、上位机

参看：谈一谈 MPU6050 姿态融合

参看：第44章 MPU6050传感器—姿态检测

通过上位机，可以为调试带来方便。

上位机下载：匿名地面站.zip

上位机使用：

如需转载请注明出处：https://blog.csdn.net/_/article/details/