– 如何结合加速度计和陀螺仪读数，以获得有关设备相对于地平面倾斜度的准确信息

在整篇文章中，我将尽量将数学保持在最低限度。如果您知道什么是正弦/余弦/正切，那么无论您使用什么平台，您都应该能够在项目中理解和使用这些想法，无论您使用什么平台 Arduino、Propeller、Basic Stamp、Atmel 芯片、Microchip PIC 等。有些人认为，为了使用 IMU 单元，您需要复杂的数学运算（复杂的 FIR 或 IIR 滤波器，例如卡尔曼滤波器、Parks-McClellan 滤波器等）。您可以研究所有这些并获得美妙但复杂的结果。我解释事物的方式只需要基本的数学。我非常相信简单。我认为一个简单的系统更容易控制和监控，此外，许多嵌入式设备没有能力和资源来实现需要矩阵计算的复杂算法。

我将以我设计的新 IMU 单元为例——Acc_Gyro加速度计 + 陀螺仪 IMU。我们将在下面的示例中使用此设备的参数。本机是一个很好的入门设备，因为它由 3 个设备组成：

– LIS331AL（数据表） – 模拟 3 轴 2G 加速度计

– LPR550AL（数据表） – 双轴（俯仰和滚动），500 度/秒陀螺仪 – LY550ALH（数据表） – 单轴（偏航）陀螺仪

（本教程中未使用最后一个设备，但当您继续实现 DCM 矩阵时，它会变得相关)

它们共同代表一个 6 自由度惯性测量单元。现在这是一个花哨的名字！然而，花哨的名字背后是一个非常有用的组合设备，我们将在下面详细介绍和解释。

第 1 部分。加速度计

为了理解这个单元，我们将从加速度计开始。在考虑加速度计时，对一个立方体形状的盒子进行成像通常很有用，里面有一个球。你可能会想象其他的东西，比如饼干或甜甜圈，但我会想象一个球：

如果我们把这个盒子放在一个没有引力场的地方，或者没有其他可能影响球位置的场——球只会漂浮在盒子的中间。你可以想象这个盒子在远离任何宇宙天体的外太空中，或者如果这样的地方很难找到，想象一下至少有一艘宇宙飞船在行星上运行，一切都处于失重状态。从上图中可以看出，我们为每个轴分配了一对墙（我们去掉了墙 Y+，这样我们就可以查看盒子内部）。想象一下，每面墙都是压力敏感的。如果我们突然将盒子向左移动（我们以加速度 1g = 9.8m/s^2 加速它），球将撞到墙上 X-。然后，我们测量球施加在墙上的压力，并在 X 轴上输出 -1g 的值。

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

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

在这种情况下，盒子没有移动，但我们仍然在 Z 轴上得到 -1g 的读数。球施加在墙上的压力是由重力引起的。从理论上讲，它可能是一种不同类型的力——例如，如果你想象我们的球是金属的，在盒子旁边放一块磁铁可以移动球，让它撞到另一面墙上。这样说只是为了证明加速度计本质上是测量力而不是加速度。恰好加速度会产生惯性力，该惯性力被加速度计的力检测机制捕获。

虽然该模型并不完全是MEMS传感器的构造方式，但它通常可用于解决与加速度计相关的问题。实际上有类似的传感器内部有金属球，它们被称为倾斜开关，但它们更原始，通常它们只能判断设备是否在某个范围内倾斜，而不是倾斜程度。

到目前为止，我们已经分析了单轴加速度计的输出，这就是单轴加速度计所能得到的全部内容。三轴加速度计的真正价值在于它们可以检测所有三个轴上的惯性力。让我们回到我们的盒子模型，让我们将盒子向右旋转 45 度。球现在将接触 2 面墙：Z- 和 X- 如下图所示：

0.71 的值不是任意的，它们实际上是 SQRT（1/2） 的近似值。当我们推出加速度计的下一个模型时，这一点将变得更加清晰。

在前面的模型中，我们固定了重力并旋转了我们的假想框。在最后 2 个示例中，我们分析了 2 个不同箱体位置的输出，而力矢量保持不变。虽然这有助于理解加速度计如何与外力相互作用，但如果我们将坐标系固定在加速度计的轴上并想象力矢量围绕我们旋转，则执行计算会更实用。

请看一下上面的模型，我保留了轴的颜色，这样你就可以从以前的模型过渡到新的模型。试想一下，新模型中的每个轴都垂直于先前模型中盒子的相应面。矢量 R 是加速度计正在测量的力矢量（它可以是上面示例中的重力或惯性力，也可以是两者的组合）。Rx、Ry、Rz 是 R 向量在 X、Y、Z 轴上的投影。请注意以下关系：

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

这基本上相当于 3D 中的勾股定理。

请记住，刚才我告诉过你，SQRT（1/2） ~ 0.71 的值不是随机的。如果你把它们代入上面的公式，在回想一下我们的引力是 1 g 之后，我们可以验证：

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） 即可

经过漫长的理论序言，我们越来越接近现实生活中的加速度计。值 Rx、Ry、Rz 实际上与现实生活中的加速度计将输出的值呈线性关系，可用于执行各种计算。

在我们到达那里之前，让我们先谈谈加速度计将这些信息传递给我们的方式。大多数加速度计将分为两类：数字加速度计和模拟加速度计。数字加速度计将使用 I2C 、SPI 或 USART 等串行协议为您提供信息，而模拟加速度计将输出预定义范围内的电压电平，您必须使用 ADC（模数转换器）模块将其转换为数字值。我不会详细介绍ADC的工作原理，部分原因是它是一个如此广泛的主题，部分原因是它因平台而异。一些微控制器将具有内置ADC模块，其中一些需要外部组件才能执行ADC转换。无论您使用哪种类型的ADC模块，最终都会得到一个在一定范围内的值。例如，一个 10 位 ADC 模块将输出一个 0..1023 范围内的值，请注意 1023 = 2^10 -1。12 位 ADC 模块将输出 0..4095 范围内的值，请注意 4095 = 2^12-1。

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

AdcRx = 586 AdcRy = 630

AdcRz = 561

每个ADC模块都有一个基准电压，在我们的示例中假设它是3.3V。要将 10 位 adc 值转换为电压，我们使用以下公式：

伏特Rx = AdcRx * Vref / 1023

这里需要快速说明：对于 8 位 ADC，最后一个分频器为 255 = 2 ^ 8 -1，对于 12 位 ADC，最后一个分频器为 4095 = 2^12 -1。

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

伏特Rx = 586 * 3.3V / 1023 =~ 1.89V （我们将所有结果四舍五入到小数点后 2 位）

伏特 = 630 * 3.3V / 1023 =~ 2.03V 伏特 = 561 * 3.3V / 1023 =~ 1.81V

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

DeltaVoltsRx = 1.89V – 1.65V = 0.24V DeltaVoltsRy = 2.03V – 1.65V = 0.38V DeltaVoltsRz = 1.81V – 1.65V = 0.16V

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

Rx = DeltaVoltsRx / 灵敏度

Rx = 0.24V / 0.4785V/g =~ 0.5g Ry = 0.38V / 0.4785V/g =~ 0.79g Rz = 0.16V / 0.4785V/g =~ 0.33g

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

Rx = （AdcRx * Vref / 1023 – VzeroG） / 灵敏度 （方程 2） Ry = （AdcRy * Vref / 1023 – VzeroG） / 灵敏度

Rz = （AdcRz * Vref / 1023 – VzeroG）

/ 灵敏度

我们现在有定义惯性力矢量的所有 3 个分量，如果设备不受重力以外的其他力的影响，我们可以假设这是我们的重力矢量的方向。如果要计算设备相对于地面的倾斜度，可以计算此矢量与 Z 轴之间的角度。如果您也对每轴的倾斜方向感兴趣，您可以将此结果分为 2 个分量：X 轴和 Y 轴上的倾角，可以计算为重力矢量和 X / Y 轴之间的角度。计算这些角度比您想象的要简单，现在我们已经计算了 Rx、Ry 和 Rz 的值。让我们回到上一个加速度计模型，做一些额外的符号：

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

cos（Axr） = Rx / R ，类似地：

cos（Ayr） = Ry / R

cos（Azr） = Rz / R

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

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

Axr = arccos（Rx/R） Ayr = arccos（Ry/R） Azr = arccos（Rz/R）

我们已经走了很长一段路来解释加速度计模型，只是为了得出这些公式。根据您的应用，您可能希望使用我们派生的任何中间公式。我们还将很快介绍陀螺仪模型，我们将看到如何结合加速度计和陀螺仪数据来提供更准确的倾角估计。

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

cosX = cos（Axr） = Rx / R cosY = cos（Ayr） = Ry / R cosZ =

cos（Azr） = Rz / R

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

SQRT（cosX^2 + cosY^2 + cosZ^2） = 1

这是一个很好的属性，因为它使我们免于监控 R 向量的模量（长度）。很多时候，如果我们只对惯性矢量的方向感兴趣，那么归一化它的模量以简化其他计算是有意义的。

第 2 部分。陀螺仪

我们不会像加速度计那样为陀螺仪引入任何等效的盒式模型，而是直接跳到第二个加速度计模型，我们将展示陀螺仪根据这个模型测量什么。

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

Rxz – 是惯性力矢量 R 在 XZ 平面上的投影 Ryz – 是惯性力矢量 R 在 YZ 平面

上的投影

从 Rxz 和 Rz 形成的直角三角形中，使用勾股定理，我们得到：

Rxz^2 = Rx^2 + Rz^2 ，同样：

Ryz^2 = Ry^2 + Rz^2

另请注意：

R^2 = Rxz^2 + Ry^2 ，这可以从方程 1 及以上方程推导，也可以推导自 R 和 Ryz

R^2 = Ryz^2 + Rx^2 形成的直角三角形

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

取而代之的是，我们将定义 Z 轴和 Rxz、Ryz 向量之间的角度，如下所示：

Axz – 是 Rxz（R 在 XZ 平面上的投影）和 Z 轴之间的角度 Ayz – 是 Ryz（R 在 YZ 平面上的投影）和 Z 轴

之间的角度

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

RateAxz = （Axz1 – Axz0） / （t1 – t0）。

如果我们用度来表示 Axz，用秒来表示时间，那么这个值将用 deg/s 来表示。这就是陀螺仪测量的内容。

在实践中，陀螺仪（除非是特殊的数字陀螺仪）很少会给你一个以度/秒表示的值。与加速度计相同，您将获得一个 ADC 值，您需要使用类似于我们为加速度计定义的方程 2 的公式将其转换为度/秒。让我们介绍陀螺仪的ADC到度/秒的转换公式（我们假设我们使用的是10位ADC模块，对于8位ADC，将1023替换为255，对于12位ADC，将1023替换为4095）。

RateAxz = （AdcGyroXZ * Vref / 1023 – VzeroRate） / 灵敏度方程 3

RateAyz = （AdcGyroYZ * Vref / 1023 – VzeroRate） / 灵敏度

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

Vref – 是ADC基准电压，我们将在下面的示例中使用3.3V

VzeroRate – 是零速率电压，换句话说，陀螺仪在不受任何旋转时输出的电压，例如Acc_Gyro板为 1.23V（您可以在规格中找到此值 - 但不要相信规格，大多数陀螺仪在焊接后会受到轻微的偏移，因此请使用电压表测量每个轴输出的 VzeroRate， 通常，一旦陀螺仪被焊接，这个值就不会随着时间的推移而改变，如果它发生变化 - 在设备启动之前编写一个校准程序来测量它，必须指示用户在启动时将设备保持在静止位置，以便陀螺仪进行校准）。

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

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

AdcGyroXZ = 571

AdcGyroXZ = 323

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

RateAxz = （571 * 3.3V / 1023 – 1.23V） / （ 0.002V/deg/s） =~ 306 deg/s RateAyz = （323 * 3.3V / 1023 – 1.23V） / （ 0.002V/deg/s） =~ -94 deg/s

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

第 3 部分。把它们放在一起。结合加速度计和陀螺仪数据。

如果您正在阅读本文，您可能已经购买或计划购买 IMU 设备，或者您可能计划从单独的加速度计和陀螺仪设备构建一个。

注意： 有关此算法的实际实现和测试，请阅读本文：

http://starlino.com/imu_kalman_arduino.html

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

接下来的步骤是：

– 识别对应于 RateAxz 的陀螺仪输出，上面讨论的 RateAyz 值。

– 确定这些输出是否需要反转，因为陀螺仪相对于加速度计的物理位置

不要假设如果陀螺仪的输出标记为 X 或 Y，它将对应于加速度计坐标系中的任何轴，即使此输出是 IMU 单元的一部分。最好的方法是测试它。

这是一个示例序列，用于确定陀螺仪的哪个输出对应于上面讨论的 RateAxz 值。

– 从将设备置于水平位置开始。加速度计的 X 和 Y 输出都将输出零 g 电压（例如，对于Acc_Gyro板，这是 1.65V）——

接下来开始绕 Y 轴旋转设备，另一种说法是您在 XZ 平面上旋转设备，以便 X 和 Z 加速度计输出发生变化，Y 输出保持不变。

– 在以恒定速度旋转设备时，注意陀螺仪输出的变化，其他陀螺仪输出应保持恒定

– 绕 Y 轴旋转期间变化的陀螺仪输出（在 XZ 平面上旋转）将提供 AdcGyroXZ 的输入值，我们从中计算 RateAxz

– 最后一步是确保旋转方向与我们的模型相对应， 在某些情况下，由于陀螺仪相对于加速度计

的物理位置，您可能需要反转 RateAxz 值 – 再次执行上述测试，绕 Y 轴旋转设备，这次监控加速度计的 X 输出（在我们的模型中为 AdcRx）。如果 AdcRx 增长（从水平位置旋转的前 90 度），则 AdcGyroXZ 应减小。这是因为我们正在监测引力矢量，当设备沿一个方向旋转时，矢量将沿相反方向旋转（相对于我们正在使用的设备协调系统）。因此，否则您需要反转 RateAxz ，您可以通过在方程 3 中引入符号因子来实现这一点，如下所示：

RateAxz = InvertAxz * （AdcGyroXZ * Vref / 1023 – VzeroRate） / 灵敏度 ，其中 InvertAxz 为 1 或 -1

可以对 RateAyz 进行相同的测试，通过绕 X 轴旋转设备，您可以确定哪个陀螺仪输出对应于 RateAyz，以及是否需要反转。获得 InvertAyz 的值后，应使用以下公式来计算 RateAyz：

RateAyz = InvertAyz * （AdcGyroYZ * Vref / 1023 – VzeroRate） / 灵敏度

如果您在Acc_Gyro板上进行这些测试，您将获得以下结果：

– RateAxz 的输出引脚为 GX4 且 InvertAxz = 1

– RateAyz 的输出引脚为 GY4 且 InvertAyz = 1

从现在开始，我们将认为您已经设置了 IMU，以便您可以计算 Axr、Ayr、Azr（定义第 1 部分）的正确值。加速度计）和 RateAxz、RateAyz（定义见第 2 部分。陀螺仪）。接下来，我们将分析这些值之间的关系，这些值有助于更准确地估计设备相对于接地层的倾斜度。

此时您可能会问自己，如果加速度计模型已经为我们提供了 Axr、Ayr、Azr 的倾角，我们为什么要为陀螺仪数据而烦恼？答案很简单：加速度计数据并不总是 100% 可信。有几个原因，请记住，加速度计测量惯性力，这种力可以由重力引起（理想情况下仅由重力引起），但也可能是由设备的加速度（运动）引起的。因此，即使加速度计处于相对稳定的状态，它对振动和机械噪声仍然非常敏感。这就是为什么大多数IMU系统使用陀螺仪来消除任何加速度计错误的主要原因。但是这是如何做到的呢？陀螺仪没有噪音吗？

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

在接下来的步骤中，我将介绍一种算法，该算法的灵感来自卡尔曼滤波中使用的一些想法，但是到目前为止，它在嵌入式设备上实现起来更简单，更容易实现。在此之前，让我们先看看我们希望算法计算什么。好吧，它是引力矢量的方向 R = [Rx，Ry，Rz]，我们可以从中推导出其他值，如 Axr、Ayr、Azr 或 cosX、cosY、cosZ，这将使我们了解我们的设备相对于地平面的倾斜度，我们将在第 1 部分中讨论这些值之间的关系。有人可能会说——我们不是已经从第 1 部分的方程 2 中获得了这些值 Rx、Ry 、Rz 吗？嗯，是的，但请记住，这些值仅来自加速度计数据，因此，如果您要直接在应用程序中使用它们，则可能会得到超出应用程序所能承受的噪声。为了避免进一步的混淆，让我们重新定义加速度计测量值，如下所示：

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

RxAcc = （AdcRx * Vref / 1023 – VzeroG） / 灵敏度 RyAcc = （

AdcRy * Vref / 1023 – VzeroG） / 灵敏度

RzAcc = （AdcRz * Vref / 1023 – VzeroG） / 灵敏度

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

Racc = [RxAcc，RyAcc，RzAcc]

因为 Racc 的这些组件可以从加速度计数据中获得，我们可以将其视为我们算法的输入。

请注意，由于 Racc 测量的是引力，因此如果您假设该矢量的长度等于或接近 1g，则正确定义如下。

Racc|= SQRT（RxAcc^2 +RyAcc^2 + RzAcc^2），

但是，为了确保更新此向量有意义，如下所示：

Racc（归一化） = [RxAcc/|Racc|， RyAcc/|Racc|， RzAcc/|Racc|]。

这将确保归一化 Racc 向量的长度始终为 1。

接下来，我们将引入一个新的向量，我们将它称为

其余 = [RxEst，RyEst，RzEst]

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

以下是我们的算法将要执行的操作：

– 加速度计告诉我们：“您现在处于 Racc 位置” – 我们说“谢谢，但让我检查一下”

，

– 然后用陀螺仪数据以及过去的 Rest 数据更正此信息，我们输出一个新的估计矢量 Rest。 – 我们认为 Rest 是我们关于设备当前位置的“最佳选择”。

让我们看看如何让它发挥作用。

我们将通过信任我们的加速度计并分配以下命令来开始我们的序列：

休息 （0） = Racc（0）

顺便说一句，记住 Rest 和 Racc 是向量，所以上面的方程式只是写出 3 组方程式的简单方法，避免重复：

RxEst（0） = RxAcc（0） RyEst（0） = RyAcc（0）

RzEst（0） = RzAcc（0）

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

假设我们处于第 n 步。我们有两组已知的值，我们想要使用：

Rest（n-1） – 我们之前的估计，其中 Rest（0） = Racc（0） Racc（n）

– 我们当前的加速度计测量值

在计算 Rest（n） 之前，让我们引入一个新的测量值，我们可以从陀螺仪和之前的估计中获得该值。

我们称它为 Rgyro ，它也是一个由 3 个组件组成的向量：

陀螺仪 = [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） 我们可以找到：

Axz（n-1） = atan2（ RxEst（n-1） ， RzEst（n-1） ）。

请记住，陀螺仪测量的是 Axz 角的变化率。因此，我们可以按如下方式估计新角度 Axz（n）：

Axz（n） = Axz（n-1） + RateAxz（n） * T

请记住，RateAxz 可以从我们的陀螺仪 ADC 读数中获得。更精确的公式可以使用计算如下的平均旋转率：

RateAxzAvg = ( RateAxz(n) + RateAxz(n-1) ) / 2

Axz(n) = Axz(n-1) + RateAxzAvg * T

同样，我们可以找到：

Ayz（n） = Ayz（n-1） + RateAyz（n） * T

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

陀螺|= SQRT（RxGyro^2 + RyGyro^2 + RzGyro^2）

另外，由于我们对 Racc 向量进行了归一化，我们可以假设它的长度为 1，并且在旋转后没有变化，因此编写起来相对安全：

陀螺|= 1

让我们采用一个临时的较短的符号来计算以下计算：

x =Rx陀螺仪，y=Ry陀螺仪，z=Rz陀螺仪

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

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） 和 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）），得到：

RxGyro = 1 / SQRT （1/ sin（Axz（n））^2 + cos（Axz（n））^2 / sin（Axz（n））^2 * tan（Ayz（n））^2 ） RxGyro = 1 / SQRT （1/ sin（Axz（n））^2 + cot（Axz（n））^2 * sin（Ayz（n））^2 / cos（Ayz（n））^2 ） 现在加减 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 ），其中 cot（x） = 1 / tan（x） 和 sec（x） = 1 / cos（x）

该公式仅使用 2 个三角函数，可以计算更便宜。如果您有 Mathematica 程序，您可以通过评估 FullSimplify [Sin[A]^2/ （ 1 + Cos[A]^2 * Tan[B]^2）] 来验证它

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

RzGyro = 符号（RzGyro）*SQRT（1 – RxGyro^2 – RyGyro^2）。

其中 Sign（RzGyro） = 1 当 RzGyro>=0 时，Sign（RzGyro） = -1 当 RzGyro