加速度计、陀螺仪的工作原理和数据融合

本小节主要讲解加速度传感器、陀螺仪的工作原理、姿态解算，并对两者进行数据融合。数据融合以讲解互补滤波的工作原理为主。

陀螺仪、加速度计都是惯性测量元件的一种。而 MPU-6050 传感器的内部同时集成了陀螺仪和加速度传感器两种惯性测量元件。

加速度传感器（又名加速度计）

MPU-6050 传感器内部封装了陀螺仪和加速度传感器两种惯性测量元件，而陀螺仪和加速度传感器的工作原理是不一样的。我们先从加速度传感器开始进行了解。

图片来自：starlino.com

为了便于理解，我们可以把加速度传感器想象为一个盒子模型——一个小球在一个方盒子中。

想象一下，如果把盒子模型放在真空环境中会怎么样？在真空中，一切东西都处于无重力状态（失重），这时，小球也不例外，将悬浮在盒子的中间。在上面的图中,你可以看到我们给每个轴分配了一对墙（为了观察里面的情况，我们移除了 Y+）。设想每面墙都能感测压力。如果我们突然把盒子向左移动（加速度为 $1g = 9.8m/s^2$），那么球会撞上 $X-$ 墙。然后我们检测球撞击墙面产生的压力，$X$ 轴输出值为 $-1g$。

图片来自：starlino.com

请注意加速度传感器检测到力的方向与它自身运动加速度的方向是相反的。这种力通常被称为惯性力 。在这个模型中，你能了解到加速度传感器是通过间接测量力对一个墙面的作用来测量加速度的，但在实际应用中，可能通过弹簧或其他装置来测量力。这个力可以是加速度引起的，但在下面的例子中，我们会发现它不一定是加速度引起的。

众所周知地球上的物体都会受到重力 $g = 9.8m/s^2$ 的作用。如果我们把模型放在地球上，这时圆球不再是悬浮的，而是会落在Z-墙面上并对其施加一个 $1g$ 的力，见下图：

图片来自：starlino.com

在这种情况下，盒子没有移动，但我们依然能读取到 $Z$ 轴有 $-1g$ 的值。因为球在墙面上施加的压力是由重力造成的。在理论上，它可以是不同类型的引力。例如，你可以想象盒子里的圆球是铁质的，将一个磁铁放在盒子旁边，小球就会撞上另一面墙，这时圆球受到的是磁场力。引用这个例子只是为了说明加速度传感器的本质是检测力而非加速度。只是加速度所引起的惯性力正好能被加速度传感器的检测装置所捕获。

虽然这个盒子模型并非一个 MEMS 传感器的真实构造，但它用来解决与加速度传感器相关的问题相当有效。实际上有些类似传感器中有金属小球，它们称作倾角开关，但是它们的功能更弱，只能检测设备是否在一定程度内倾斜，却不能得到倾斜的程度。

到目前为止，我们已经分析了单轴的加速度传感器输出。三轴加速度传感器的真正价值在于它们能够检测全部三个轴的惯性力。让我们回到盒子模型，并将盒子向右旋转 45 度。盒子模型依然是放在地球上。现在球会与两个面接触：$Z-$ 和 $X-$，见下图：

图片来自：starlino.com

其中，$0.71g$ 这个值是不是随意写上去的，它们实际上是 $\sqrt(1/2)$ 的近似值。这是根据勾股定理计算出来的，具体计算方法我们介绍加速度传感器的下一个模型时这一点会更清楚。

在上一个模型中，我们引入了重力并旋转了盒子。在最后的两个例子中我们分析了盒子在两种情况下的输出值，力矢量保持不变。虽然这有助于理解加速度传感器是怎么和外部力相互作用的，但如果我们将坐标系换为加速度的三个轴并想象矢量力在周围旋转，这会更方便计算。

图片来自：starlino.com

其中，

• Rx、Ry、Rz 是向量 R 分别在 X、Y、Z 轴上的投影，

• Rxz 是向量 R 在 XZ 平面上的投影；

• Ryz 是向量 R 在 YZ 平面上的投影；

• Axz 是 Rz 和 Rxz 之间的夹角；

• Ayz 是 Rz 和 Ryz 之间的夹角；

根据勾股定理，求得：

• tan(Axz) = Rx / Rz

• tan(Ayz) = Ry / Rz

那么，通过反正切函数 atan() 可得：

• Axz = atan(Rx/Rz)

• Ayz = atan(Ry/Rz)

我们感兴趣的角度是 Axz、Ayz。请看看在上面的模型，我保留了轴的颜色，以便你的思维能更好的从上一个模型转到新的模型中。想象新模型中每个轴都分别垂直于原模型中各自的墙面。矢量 R 是加速度传感器所检测的矢量（它可能是重力或上面例子中惯性力的合成）。Rx，Ry，Rz 是矢量 R 在 X，Y，Z 轴上的投影。

根据三维空间勾股定理，可以得到下列关系：

$R^2 = R_x^2 + R_y^2 + R_z^2$（公式1）

还记得我刚才说的 $\sqrt{\frac {1}{2}}=0.71$ 吧。如果你把它们代回上式，回顾一下重力加速度是 $1g$ ，那我们就能验证：

$1^2 = (-\frac {1}{2} )^2 + 0 + (-\frac {1}{2} )^2$

回到 MPU-6050 传感器中，在上一小节我们已经分别读取到 3 个轴的加速度数据，但是我们读到的加速度数字量的单位还不是 $g(9.8m/s^2)$。最后的转换，我们还需要引入加速度传感器的灵敏度（Sensitivity），单位通常是 $LSB/g$。比方说，加速度传感器的灵敏度 $Sensitivity = 16384LSB/g$。灵敏度值可以在加速度传感器规格书中找到。要获得最后的单位为 $g$ 的加速度，我们使用下列公式计算：

$Rx = Rx / Sensitivity$

比如：

当设置 MPU-6050 加速度传感器的灵敏度为 16384LSB/g，读取 MPU-6050 的 X、Z 轴加速度数据为 1122、16674 时，则可以计算出：

$Rx = 1122LSB / 16384LSB/g = 0.068g$

$Rz = 16674LSB / 16384LSB/g = 1.018g$

$Axz = atan(0.068g / 1.018g) = 0.589rad$

注意，这时求出来的角度单位为 弧度 rad，和我们日常习惯用的单位 角度 °之间有条转换公式：

$Angle = Radian / \pi * 180$

那么，

$Axz = 0.589rad / \pi * 180 = 33.7$°

陀螺仪

对于陀螺仪，我们不会像加速度传感器一样介绍它的等价盒子模型，而是直接跳到加速度传感器的第二个模型，通过这个模型向大家介绍陀螺仪是怎么工作的。

图片来自：starlino.com

陀螺仪的每个通道检测一个轴的旋转。例如，一个 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)$

如果用度来表示角度，秒来表示时间，那这个值的单位就是 度/秒。这就是陀螺仪检测的东西。在实际运用中，陀螺仪一般都不会直接给你输出一个单位为度/秒的值（除非它是个特殊的数字陀螺仪）。在 MPU-6050 传感器中，就像读取加速度数据一样，你会得到一个经过内置 ADC 转换后得到的数字量，单位为 $LSB$。参考对加速度数据的处理，我们同样得到：

$RateAxz = (GyroX - ZeroRate) / Sensitivity$ 公式3

$RateAyz = (GyroY - ZeroRate) / Sensitivity$

其中，

GyroX，GyroY - 这两个值是陀螺仪数据，它们分别代表矢量 R 的投影在 XZ 和 YZ 平面内里的转角，也可等价的说，旋转可分解为单独绕 Y 和 X 轴的运动。

ZeroRate – 陀螺仪零偏值，换句话说，它是陀螺仪静止不动时的输出值。理论上，陀螺仪静止不动时，应该输出为 0 的值，但是受制造工艺、外部因素的影响，陀螺仪静止不动时输出一个比较小的随机的不为 0 的值，我们称之为零偏值。

Sensitivity – 陀螺仪的灵敏度，单位 LSB/(deg/s)，它的意思就是如果旋转速度增加 1°/s，陀螺仪的输出就会增加多少 LSB。灵敏度在规格书中可以查到的。

举个例子，假设我们读取到 MPU-6050 传感器的 X 轴数据：

GyroX = 12345

若此时设置陀螺仪的灵敏度为 16.4LSB/deg/s，并认为陀螺仪零偏值为 0，代入公式 3 ，得到：

$RateAxz = (12345 - 0) / 16.4LSB/deg/s = 751 deg/s$

换句话说，传感器绕 X 轴（或者说 YZ 平面内）以 751°/s 的速度旋转。请注意，因为陀螺仪输出值有正负之分，负号表示该传感器朝着反方向旋转。一份好的传感器规格书会告诉你哪个方向是正的，否则你就要自己测试出哪个旋转方向会使得输出值增加。

融合加速度传感器和陀螺仪的数据

惯性测量元件包括多种传感器，比如倾角仪、加速度计、陀螺仪、罗盘等传感器。每种惯性测量元件的性能都不尽相同，对比分析表格如下表所示：

测量值	优点	缺点
倾角仪	角度	静态性能好，精度高，无累积误差	动态响应速度慢
加速度传感器	加速度	静态性能好，精度高，更新频率快	存在累积漂移误差，动态响应慢
陀螺仪	角速度	动态响应好，频带宽，更新频率快	存在温漂和零漂
罗盘	角度	静态性能好，精度高，无漂移误差	动态响应速度慢，受环境磁场影响大

倾角仪测量与重力方向相关的姿态角，输出频率低，实时性较差，而且输出信号容易受噪声污染。对相频特性进行分析显示，倾角仪相位存在滞后。

加速度传感器测量与惯性有关的加速度，包括旋转、重力和线性加速度，对测量数据进行积分可以得到线性速度，二次积分可以得到线位移，但积分产生的漂移误差将随时间累积而无限制地增长导致积分后得到的数据不准确。加速度传感器通过三角函数运算可获得倾角值，但是输出信号容易受噪声污染。当加速度传感器垂直于俯仰角安装时，通过反正切函数运算可以作为倾角仪使用进行360°的全方位测量，并且不会产生线性化误差。

陀螺仪测量瞬时旋转角速度，由于温度变化、摩擦力、不稳定力矩等因素，会产生漂移误差。对陀螺仪测量数据进行积分可以得到与垂直方向相关的倾角信息，动态响应快，但漂移误差将随时间累积而无限增长。陀螺仪具有足够的带宽，动态性能好，静态输出受漂移误差影响较大。

罗盘通过测量相对于地磁场的方向，提供绝对姿态角信息。由于室内存在电源线和钢结构导致磁场产生畸变，罗盘一般不适于室内工作。

在上面的加速度传感器部分，我们似乎只要读取到加速度数据，通过反正切函数 atan() 就能求出我们想要的角度，再对此信号进行微分便可以获得角速度。但是在实际运用环境中，比如两轮自平衡小车，由于小车本身的摆动所产生的加速度会产生很大的干扰信号，它叠加在上述加速度数据量上使得该数据无法准确反映小车的倾角。

陀螺仪可以用来测量物体的旋转角速度。由于陀螺仪输出的是小车的角速度，不会受到小车运动的影响，因此该信号中噪声很小。小车的倾角又是通过对角速度积分而得，这可进一步平滑信号，从而使得角度信号更加平稳。因此，小车控制所需要的角度和角速度可以使用陀螺仪所得到的数据量。

由于从陀螺仪角速度获得角度信息，需要经过积分运算。如果角速度信号存在微小的偏差和漂移，经过积分运算之后，变化形成积累误差。这个误差会随着时间延长逐步增加，最终导致电路饱和，无法形成正确的角度信号。

如何消除这个累计误差呢？

一种简单的方法就是使用数据融合算法。通过上面的加速度传感器获得的角度信息对此进行校正。通过对比积分所得到的的角度与重力加速度所得到的的角度，使用它们之间的偏差改变陀螺仪输出，从而积分的角度逐步跟踪到加速度传感器所得到的的角度。

数据融合算法有很多，目前流行使用的有两种：

• 互补滤波（complementary filter）

• 卡尔曼滤波（kalman filter）

萌新们可能会纳闷：我这里要学的是数据融合算法，为啥扔给我两个滤波算法？为啥不是学“互补融合”、“卡尔曼融合”？

要知道，数据滤波是去除噪声还原真实数据的一种数据处理技术。任何的数据融合都是为了将多种途径的数据中的噪声滤波，以达到尽可能接近真实值的目的。从这个角度理解，将多种数据融合只是表象，滤除了信号中的噪声才是本质。

互补滤波和卡尔曼滤波都是能去除加速度传感器和陀螺仪的噪声，取得精准的姿态数据。其中，互补滤波简单易懂，适合萌新们初学入门使用。

互补滤波（complementary filter）

2007年，MIT 大神 Shane Colton 发表了经典论文《The Balance Filter》，里面提出了一种对加速度传感器与陀螺仪进行数据融合的有效方法——互补滤波。互补滤波包括低通滤波（滤除或衰减短期加速度传感器波动），以及高通滤波（消除漂移对陀螺仪的影响）。

在加速度传感器部分，我们通过反正切函数求出了角度，这里用 Acc 表示；在陀螺仪部分，我们得到了角速度，这里用 Gyro表示。

互补滤波的核心公式：

$Angle = 0.98 * (Angle + Gyro * dt) + 0.02 * Acc$

其中，

• Angle 为经过互补滤波后得到的角度

• Gyro 为陀螺仪部分得到的角速度

• Acc 为加速度传感器部分通过反正切函数 atan() 再转换单位后的角度

• dt 为滤波器的运行周期

• 0.98 和 0.02 为加权系数 $\alpha$ 和 $(1 - \alpha)$

使用 c 语言把互补滤波封装成函数：

// a = tau / (tau + dt)  
// acc = 加速度传感器数据 
// gyro = 陀螺仪数据 
// dt = 运行周期       
float angle;
float a;
float ComplementaryFliter(float acc, float gyro, float dt) 
{
    a = 0.98;  
    angle = a * (angle + gyro * dt) + (1 - a) * (acc);  
    return angle;  
}

在互补滤波公式中，像陀螺仪角速度、加速度角度这些数据可以直接从陀螺仪、加速度传感器中读取得到。可是，加权系数是怎么得来的呢？

在《The Balance Filter》中提到关于 加权系数的求解公式。在这里，先设滤波器的加权系数为 $\alpha$，时间常数为为 $\tau$，运行周期为 $dt$，那么公式为：

$\alpha = \tau / (\tau + dt)$

运行周期 $dt$ 通常都是我们设定的。比如在程序里，我们打算让互补滤波器在 $1s(=1000ms)$ 中运行 200 次，那么$dt = 1000ms / 200hz = 5ms$。

时间常数 $\tau$ 的取值是由我们根据系统的实际滤波需求调整的，每个不同的系统的 $\tau$ 值都不一定相同。时间常数 $\tau$ 是“信任”陀螺仪和“信任”加速度传感器的边界值。若时间常数 $\tau$ 取值越大，则更加“信任”陀螺仪积分，但跟随加速度传感器的速度会变慢。若时间常数 $\tau$ 取值越小，则更加“信任”加速度传感器，但同时引入加速度传感器中更多的噪声。通常，互补滤波器是为了得到更“纯”的融合角度，必须要“信任”陀螺仪积分多些，以削弱加速度传感器中噪声的影响。

比如，互补滤波器放在 $10ms (= 0.01s)$ 周期中循环运行，时间常数 $\tau = 0.49$，那么，此时加权系数为：

$\alpha = \tau / (\tau + dt) = 0.49 / (0.49 + 0.01) = 0.98$

函数的入口参数是加速度传感器数据、陀螺仪数据和运行周期。经过计算后，函数返回融合角度。

为了更进一步了解加权系数的作用，我们留意两个极值情况：

• 当 $a = 1$ 时，那么滤波器变成 $angle = 1 * (Angle + Gyro * dt) + (1 - 1) * Acc = Angle + Gyro * dt$，失去了加速度传感器的修正效果，变成了一个纯粹的陀螺仪积分器，此时互补滤波器完全信任陀螺仪。

• 当 $a = 0$ 时，那么滤波器变成 $angle = 0 * (Angle + Gyro * dt) + (1 - 0) * Acc = Acc$，此时陀螺仪完全失去了作用，融合角度直接等于加速度传感器数值，此时互补滤波器完全信任加速度传感器。

由此可见，加权系数 $\alpha$ 值的大小决定了互补滤波器是信任陀螺仪还是信任加速度传感器多一些，也直接决定了互补滤波器的效果。

当然，互补滤波器也存在缺陷：

• 初始化时不能及时跟随实际角度。这种情况，跟融合角度变量没有正确初始化和互补滤波器的时间常数取值有关。时间常数约大，陀螺仪积分比重越大，跟随加速度传感器的速度约慢，这意味着初始化时融合角度可能是不精准的。

• 陀螺仪的零漂问题。理论上，陀螺仪在静止的时候，读出来的数值应该为 0，但实际上往往不为 0，这个值被称为零偏值。陀螺仪每次上电后的零偏值是随机的。互补滤波器并不能有效地过滤掉陀螺仪的零漂。

下一小节，我们将教你把互补滤波实际应用到实际项目中去。