MPU6050学习

1. MPU6050 传感器原理与姿态定义

1.1 加速度计的测量原理

例如，当物体沿图中 y 轴正方向（向右）加速运动时，内部的小球由于惯性，相对于外壳向左偏移，使左侧弹簧压缩、右侧弹簧拉长。在弹性结构的测量范围内，加速度越大，小球的相对位移越大。传感器通过检测这一位移引起的电容变化，经过信号处理和标定，即可计算出沿 y 轴的加速度大小，并判断其方向。

这里应强调"向右加速运动"：如果物体只是向右匀速运动，理想情况下，小球不会因此产生持续的偏移。

1.2 传感器坐标轴与安装方向

根据圆点丝印判断方向

放置时受到万有引力作用，所以 Z 的方向为重力加速度 -g

这个叫就是 pitch 角，即俯仰角

1.3 陀螺仪的测量原理

这幅图表达的是 MEMS 振动陀螺仪的原理：让质量块持续往复振动，利用旋转时产生的科里奥利力，测出角速度。

1. 没有旋转时，质量块沿原方向振动

右图中的蓝色小球代表芯片内部的微小质量块，弹簧代表支撑它的弹性结构。

芯片通过驱动力，让质量块沿图中上下方向持续振动。芯片没有旋转时，理想情况下，质量块只会上下运动，不会产生横向振动。

2. 芯片旋转时，质量块出现横向振动

假设图中的粉色箭头表示芯片绕垂直纸面的轴旋转。

从跟着芯片一起旋转的视角来看，正在上下运动的质量块会受到科里奥利力。这个力的方向垂直于质量块的运动方向和芯片的旋转轴，因此会使质量块向左右方向偏移。

质量块向上运动和向下运动时，速度方向相反，受到的科里奥利力也随之反向。因此，质量块在持续上下振动的同时，还会产生左右往复振动。

可以直观地理解为：质量块因惯性倾向于保持原来的运动，而芯片及其支撑结构正在旋转，所以从芯片内部观察，质量块出现了横向偏移。

图中的蓝色虚线可以用来示意这种偏移，实际运动轨迹取决于两个方向振动的幅值和相位。

3. 根据横向振动，测出角速度

当质量块原有的上下振动保持稳定时，芯片旋转得越快，产生的科里奥利力越大，横向振动也越明显。

芯片通过微小的电容变化检测横向位移，再经过信号处理和标定，就能得到旋转角速度。横向振动与原有振动之间的相位关系，还能帮助判断旋转方向。

因此，陀螺仪虽然没有一个持续旋转的转子，仍然能够利用振动质量块感知自身的旋转。

4. 左图中的三个测量方向

左图中的三个输出分别表示：

输出	含义
gx	绕 X 轴旋转的角速度
gy	绕 Y 轴旋转的角速度
gz	绕 Z 轴旋转的角速度

例如，gx 表示绕 X 轴转得多快。三轴陀螺仪通过相应的振动与检测结构，测量三个方向的旋转。

整个过程可以概括为：

质量块持续振动 → 芯片发生旋转 → 产生横向科里奥利力 → 检测横向振动 → 得到角速度。

陀螺仪直接测量的是角速度；把角速度随时间累积起来，才能估计转过的角度。

1.4 欧拉角与旋转方向

三个欧拉角的定义如下：

• 偏航角（Yaw） ：绕 +Z+Z+Z 轴 旋转的角度。在此坐标系中，+Z+Z+Z 轴垂直向上，偏航角表示小车在水平面内的转向角度。

• 俯仰角（Pitch） ：绕 +X+X+X 轴 旋转的角度。在此坐标系中，+X+X+X 轴指向小车的上方/前方方向，俯仰角表示小车绕 XXX 轴前倾或后仰的角度。

• 翻滚角（Roll） ：绕 +Y+Y+Y 轴 旋转的角度。在此坐标系中，+Y+Y+Y 轴沿轮轴方向横向延伸，翻滚角表示小车绕 YYY 轴向左或向右倾斜的角度（对于两轮平衡车而言，此角度即对应车身的左右摆动/平衡状态）。

---

2. 陀螺仪计算欧拉角

2.1 角速度积分与角度递推

这张图说明的是：已知上一时刻的角度，再根据陀螺仪测得的角速度，计算这一小段时间转过的角度，得到新角度。 这是角速度积分，还没有进行互补滤波。

1. 图中的计算过程

已知：

• t0 时刻偏航角为 10°。
• 绕 Z 轴的角速度 gz 为 30°/s。
• 两次更新间隔为 5 ms，也就是 0.005 s。

假设这段时间内角速度保持不变，且转动方向使偏航角增加：

这段时间转过的角度：
30 × 0.005 = 0.15°

t1 时刻偏航角：
10 + 0.15 = 10.15°

右侧曲线中，横向距离是时间间隔，纵向增加量是这段时间转过的角度，曲线斜率表示角速度。

2. 下一次从哪里继续算？

从这次得到的 10.15° 继续。

如果下一段 5 ms 内仍然是 30°/s：

t0：10.00°
t1：10.15°
t2：10.30°
t3：10.45°

这就是图中"当前角度＝上一时刻角度＋本次转角"的含义。

最开始的 10° 是题目给定的初始条件，不是陀螺仪测出来的。实际使用 MPU6050 时，如果没有其他方向参考，通常将初始偏航角设为 0°，记录相对于启动方向的转动。

3. 为什么这样计算会产生漂移？

假设设备实际没有转动，但陀螺仪存在零偏，一直输出 0.1°/s：

每隔 5 ms，错误地增加：
0.1 × 0.005 = 0.0005°

单次很小，但不断累积：

10 秒后：多出约 1°
60 秒后：多出约 6°

算法无法仅凭这个读数判断它是实际转动还是传感器误差，所以会把误差也累加到角度中。

2.2 递推公式中的正负号与采样间隔

加号还是减号，取决于"角度增大的方向"与"陀螺仪正角速度的方向"是否一致。 同向就加，反向就减。

图中的黑色箭头表示角度的正方向，粉色箭头表示陀螺仪角速度的正方向。按照图中采用的定义：

角度	对应角速度	正方向关系	更新方式
yaw，偏航角	gz	同向	加上 gz × 时间间隔
pitch，俯仰角	gx	同向	加上 gx × 时间间隔
roll，横滚角	gy	反向	减去 gy × 时间间隔

△t 越小越精确，而 t 取决于采样率，MPU6050最大采样率是200Hz，所以是 5ms

---

3. 加速度计计算欧拉角

3.1 重力参考与可测量的姿态角

图中的蓝色 XYZ 坐标轴固定在传感器上，会随着车身一起转动；粉色箭头 G 表示重力方向，在当地始终近似竖直向下。

当平衡车发生俯仰或横滚时，传感器坐标轴与重力方向之间的夹角会改变，重力在各轴上的分量也随之变化。因此，在设备静止或运动加速度较小的条件下，可以利用加速度计的三轴读数估计重力方向，进而计算俯仰角和横滚角。

但加速度计不能仅凭重力确定偏航角。平衡车直立时，Z 轴近似竖直，偏航运动相当于绕竖直方向转动。无论车头朝向哪个方向，重力仍然沿竖直方向（即偏航角的旋转平面与G垂直），在传感器各轴上的分量保持不变，所以加速度计无法区分这些不同的朝向。

因此，偏航角无法测出的原因，并不是"夹角太小"，而是绕重力方向旋转不会改变重力在传感器坐标系中的分量，测量数据中没有这部分朝向信息。即使设备存在倾斜，仅凭重力这个单一参考，也不能确定完整的三维姿态。

所以，加速度计通常用于估计俯仰角和横滚角，并辅助纠正陀螺仪在这两个角度上的漂移；偏航角则需要通过陀螺仪积分跟踪，如果要限制其长期漂移，还需要磁力计或其他外部方向参考。

在这里插入图片描述

3.2 加速度计读数与重力方向的关系

重力向下，读数却为正，是因为加速度计测量的是"比力"，不是直接测量重力向量。 可以结合图中的小球和弹簧理解。

3.2.1 无重力环境中的向上加速

假设在没有重力的环境中，让传感器外壳向上加速：

• 外壳向上运动；
• 小球由于惯性，相对于外壳向下偏移；
• 弹性结构对小球产生向上的力，带着它加速；
• 传感器把这种偏移标定为向上的正加速度。

所以，小球向下偏移，对应的读数可以是正值，读数方向并不是小球偏移方向。

3.2.2 地面静止时的支撑作用

现在把传感器放在地面，Z 轴朝上：

• 重力把内部小球向下拉；
• 小球相对外壳向下偏移；
• 弹性结构产生向上的支撑力，平衡重力。

这种偏移与"无重力环境中，外壳向上加速"的情况类似，因此加速度计输出：

az ≈ +9.8 m/s²

虽然传感器整体没有向上加速，它仍测到了支撑作用对应的比力。

3.3 倾斜角计算与 atan2 函数

第一张图利用加速度在 Y、Z 轴上的分量计算倾斜角；第二张图说明：只计算两者的比值会丢失方向信息，所以要用 atan2(ay, az)，而不是 atan(ay / az)。

3.3.1 加速度分量与倾斜角的关系

按照第一张图的定义，车身绕 X 轴前后倾斜，这个角度叫 pitch。

设备静止或运动加速度较小时，加速度计测到的向上比力近似为固定大小的重力参考。车身倾斜后，这个参考方向在传感器轴上分解成：

• ay：沿 Y 轴的分量，对应图中三角形的"对边"；
• az：沿 Z 轴的分量，对应"邻边"。

因此：

tan(pitch) = ay / az

例如：

ay = 4.9
az ≈ 8.49

ay / az ≈ 0.577
对应的倾斜角约为 30°

这里沿用图中的轴向和角度正方向，其他安装方式可能需要调整符号。

3.3.2 atan 的象限局限

atan() 只接收一个数，也就是已经除完的比值：

atan(ay / az)

两个分量相除后，无法知道它们原来分别是正还是负。

例如，下面两种情况的实际方向完全不同：

ay	az	ay / az	实际角度
+1	+1	+1	+45°
−1	−1	+1	−135°

但两次调用都是：

atan(1)

结果都会是 45°。

这就是第二张图中，45° 与 −135° 的正切值都为 1 的含义。正切值相同，不代表方向相同；它们可以相差 180°。

atan() 返回的角度范围只有 −90° 到 +90° 之间，无法单独区分四个象限。

3.3.3 atan2 的象限判断与参数顺序

atan2() 分别接收两个分量：

atan2(ay, az)

它既看分量的比例，也看每个分量各自的正负号，因此能够判断方向所在的象限：

ay	az	atan2(ay, az) 的角度
+1	+1	+45°
+1	−1	+135°
−1	−1	−135°
−1	+1	−45°

它能覆盖约 −180° 到 +180° 的方向范围。

注意参数顺序：

atan2(对边分量, 邻边分量)

本图对应：
atan2(ay, az)

函数名中的 y、x 表示数学平面的纵向、横向分量，不要求必须传入传感器的 ay、ax 。本图就是把 ay 当作纵向分量、az 当作横向分量。

3.3.4 分量为零时的处理

设备倾斜到 90° 时，可能出现：

ay > 0
az = 0

使用 ay / az 会遇到除零问题，而：

atan2(ay, 0)

可以直接得到 +90° ；ay < 0 时得到 −90°。

但如果 ay、az 都为零，这两个分量就无法确定方向，即使函数返回了一个数，也不能把它当成有效倾斜角。

3.3.5 弧度与角度转换

C 数学函数返回的是弧度，要转换成度：

#include <math.h>

double pitch_deg = atan2(ay, az) * 180.0
                   / 3.14159265358979323846;

3.4 俯仰角和横滚角的轴向对应

俯仰角和横滚角对应哪个轴，取决于传感器的安装方向和车身坐标定义。 按前面图片的定义，X 轴沿车轮轴线，Y 轴沿车身前后方向，Z 轴朝上，因此：

角度	表示的动作	图中对应的旋转轴
俯仰角 pitch	车身前后倾斜	X 轴
横滚角 roll，也称翻滚角	车身左右倾斜	Y 轴
偏航角 yaw	车头左右转向	直立时的 Z 轴

3.4.1 俯仰角与 X 轴

想象把一根杆沿两轮之间的轴线穿过车身。车身绕这根杆转动时，就会向前或向后倾斜，这就是俯仰。

图中这根轴线被定义为 X 轴，所以 pitch 是绕 X 轴的转动。

只发生这种转动时，Y、Z 轴会改变方向，而 X 轴方向不变。因此，静止时用于估计姿态的重力参考主要在 Y、Z 两轴之间重新分配：

车身直立：
ay ≈ 0，az ≈ 9.8

车身前后倾斜：
ay 发生变化，az 也发生变化

按图中的正方向定义，可以计算：

pitch = atan2(ay, az);

绕 X 轴的角度用 ay、az 计算，是因为转动发生在与 X 轴垂直的 YZ 平面内。

3.4.2 横滚角与 Y 轴

再想象一根杆沿车身前后方向穿过车身。车身绕它转动时，左侧升高、右侧降低，或反过来，这就是横滚。

图中前后方向定义为 Y 轴，所以 roll 是绕 Y 轴的转动。

只发生这种转动时，重力参考主要在 X、Z 两轴之间重新分配。因此，要利用 ax、az 估计该倾斜角。

若沿用前图中"roll 正方向与 gy 正方向相反"的约定，单轴情况下可写成：

roll = atan2(ax, az);

如果将 roll 的正方向定义为与 gy 正方向一致，则相应写成：

roll = atan2(-ax, az);

符号必须与角度正方向一致；不能只记住函数中的轴名。

---

4. 互补滤波

4.1 漂移、噪声与融合的必要性

图中表达的是：陀螺仪和加速度计都能提供姿态估计所需的信息，但各有不足，需要融合使用。 要注意，加速度计依靠重力方向只能估计俯仰角和横滚角，不能单独确定偏航角。

4.1.1 陀螺仪的积分漂移

陀螺仪测量的是角速度。把角速度随时间累积起来，就能估计设备相对于初始姿态转过的角度。

但陀螺仪存在零偏：设备明明没有转动，输出也可能不是零。零偏还可能随着温度等条件变化。

例如，设备静止时，陀螺仪始终多测出每秒 0.1° 的角速度。如果不修正，累计一分钟后，计算出的角度就会偏离约 6°。

漂移就是：实际姿态没有这样变化，估计的角度却随着时间逐渐偏离。 因为角度需要积分得到，微小的角速度偏差也会不断累积。随机噪声同样会影响积分结果。

因此，陀螺仪适合跟踪短时间内的快速转动，但长期仅靠它估计姿态，误差容易增大。

4.1.2 加速度计的噪声与运动干扰

设备静止或运动较缓慢时，可以根据重力在三个轴上的分量，判断设备的倾斜方向，估计俯仰角和横滚角。

噪声是测量值中不需要的随机波动。 例如，设备保持不动，传感器输出仍会在真实值附近上下跳动。这些波动可能来自传感器内部电路等因素，会使计算出的角度抖动。

另外，加速度计不仅感受到重力，也会受到设备平移加速度和振动的影响。走路、晃动或突然移动执法仪时，如果把全部测量值都当成重力，就可能把"设备正在加速"误判为"设备发生倾斜"。

运动加速度是实际运动信号，不全是随机噪声；但对利用重力估计姿态来说，它是干扰。

因此，加速度计在静止或缓慢运动时能提供长期倾斜参考，运动剧烈时角度估计容易失真。

4.1.3 两类测量的互补关系

图中的互补滤波，就是一种融合方法：

• 短时间的姿态变化主要跟随陀螺仪，保留快速转动的响应。
• 利用加速度计提供的重力方向，缓慢修正俯仰和横滚的漂移，同时减弱加速度计高频波动的影响。
• 运动加速度较大时，应降低对加速度计修正的信任程度。

这种融合可以让姿态估计既能及时跟随运动，又减少长期偏差。

这三个问题正好把互补滤波的完整过程补齐了。下面仍以俯仰角为例，假设设备启动时可以先保持静止。

4.2 初始融合角度与零偏估计

通常先用加速度计估计初始倾斜角。

启动时，可以这样做：

1. 让设备静止一小段时间，采集多组加速度和角速度。
2. 对加速度数据取平均，减小随机波动。
3. 根据重力在各轴上的分量，计算初始俯仰角和横滚角。
4. 将这个角度保存为初始融合角度。
5. 同时对静止时的陀螺仪数据取平均，估计零偏，后续读取时减去它。

例如，加速度计估计设备俯仰角为 30°，就从 30° 开始融合。

陀螺仪只能测出转动速度，不能告诉你启动时已经倾斜了多少度。 所以需要初始角度参考。如果启动时正在剧烈运动，加速度计也可能估错，初始化就需要额外处理。

偏航角无法靠重力确定，只有 MPU6050 时，通常把初始偏航角设为 0°，表示相对于启动方向的转角。

4.3 加速度计修正陀螺仪的依据

因为两者的误差特点不同：

传感器	怎样估计角度	主要问题
陀螺仪	从初始角度开始，累积每次转动量	微小零偏不断积分，形成长期漂移
加速度计	根据当前重力方向估计倾斜	受噪声、振动和运动加速度干扰

静止或缓慢运动时，加速度计提供一个不依赖历史积分的参考，因此能纠正陀螺仪逐渐积累的偏差。

但它不是每次都正确，所以只让它做少量修正。快速变化主要跟随陀螺仪，避免输出追随加速度计的短时波动。

例如：

陀螺仪预测：32°
加速度计估计：30°

算法不知道谁绝对正确，
但按照"短时信陀螺仪、长期信重力参考"的原则，
让融合角度从 32° 稍微向 30° 靠近。

因此，修正依据是传感器的误差规律和当前可信度，不是因为加速度计永远比陀螺仪准确。

4.4 修正幅度、收敛与限制

要分清每次修正多少 和最终接近哪里。

每次修正多少，由权重决定。

假设每次修正差值的 2%：

两者角度差	本次修正量
10°	0.2°
2°	0.04°
0.1°	0.002°

差值越小，修正越小。这个比例与采样周期一起决定纠偏速度；不是所有系统都应该用 2%。

基础互补滤波通常没有"误差小于某个值就停止"的界限。 它每次都更新，差值趋近于零时，修正量也趋近于零。

如果设备静止、加速度计一直估计为 30°、陀螺仪没有残余零偏，那么融合角度会逐渐接近 30°。真实情况下有噪声和残余误差，结果通常在附近波动，不会严格停在一个数上。

还可以另外设置两类限制：

• 可信度限制：检测到明显运动加速度时，降低或暂停加速度计修正，避免被错误的重力估计带偏。
• 单次修正幅度限制：限制一次允许调整多少，避免异常数据造成突变。

这些属于进一步的工程处理，并不是基础互补滤波必然自带的规则。最终误差能到多少，需要结合标定和实际测试确定。

整个过程是：

启动静止采样
    ↓
加速度计给初始倾斜角，陀螺仪估计零偏
    ↓
陀螺仪预测本次新角度
    ↓
加速度计提供重力倾斜参考
    ↓
按权重纠正一部分差值
    ↓
保存融合结果，继续下一次更新

4.5 互补滤波公式与执行过程

这张图说明：陀螺仪估计的角度比较平滑，但长期会漂移；加速度计估计的倾斜角容易波动，却能提供重力方向参考。互补滤波把两者结合起来，跟踪快速转动，同时缓慢纠正漂移。

图中横轴可以理解为时间，纵轴为角度：

• 黑色虚线：真实角度，实际程序通常不知道它，只是图中的对照。
• 黄色曲线：仅靠陀螺仪积分的角度，逐渐偏离真实值。
• 粉色曲线：加速度计估计的角度，在真实值附近波动。运动加速度较大时，也可能明显偏离真实值。

4.5.1 参数含义

用普通文本写出图中的关系：

Pitch = α × Pitch_g + (1 − α) × Pitch_a

Roll  = α × Roll_g  + (1 − α) × Roll_a

Yaw = Yaw_g

参数	含义
Pitch	本次融合后的俯仰角
Roll	本次融合后的横滚角
Yaw	偏航角
Pitch_g、Roll_g	利用陀螺仪，基于上一次融合结果预测出的本次角度
Pitch_a、Roll_a	根据加速度计当前的重力参考计算出的倾斜角
Yaw_g	通过陀螺仪估计的偏航角
α	陀螺仪预测角度的权重，取值在 0 到 1 之间
1 − α	加速度计角度的权重

下标 g 表示陀螺仪，a 表示加速度计。两部分参与融合时，必须使用相同的角度单位、轴向和正方向定义。

4.5.2 每次更新的执行步骤

以俯仰角为例。

第一步：得到初始角度。

启动时保持静止，用多组加速度数据估计初始俯仰角，同时估计陀螺仪零偏。假设初始融合角度为 30°。

第二步：用陀螺仪预测新角度。

Pitch_g = 上一次融合俯仰角
          + 本次测得的俯仰角度增量

角度增量由校正后的角速度和时间间隔计算。这里按图中的单轴运动理解；一般三维运动还需要姿态转换。

第三步：用加速度计估计当前倾斜角。

根据三轴数据和采用的坐标定义，得到 Pitch_a。

第四步：按权重融合，并保存结果。

Pitch = α × Pitch_g + (1 − α) × Pitch_a

下一次从这个融合后的 Pitch 继续预测，而不是从另一条一直未修正的陀螺仪积分曲线继续。 这是持续纠正漂移的关键。

4.6 权重与修正效果示例

图中示例采用：

α = 0.95238
1 − α = 0.04762

也就是本次结果主要跟随陀螺仪预测，再用加速度计做少量修正。

假设：

Pitch_g = 32°
Pitch_a = 30°

那么：

Pitch = 0.95238 × 32 + 0.04762 × 30
      ≈ 31.90476°

这也可以理解成：

陀螺仪预测：32°
加速度计参考：30°
两者差值：−2°

只修正差值的 4.762%
→ 向下修正约 0.09524°
→ 输出约 31.90476°

如果下一次设备没有转动，就从 31.90476° 开始，再向加速度计参考靠近一点。

4.7 加速度计瞬时干扰的抑制

假设设备仍在 30°，陀螺仪预测为 30°，但加速度计因短暂干扰跳到 40°：

Pitch = 0.95238 × 30 + 0.04762 × 40
      = 30.4762°

输出只变化约 0.4762°，不会直接跳到 40°。但如果干扰持续存在，融合结果仍可能被带偏，因此剧烈运动时需要降低对加速度计的信任。

4.8 权重选择与偏航角的限制

• α 较大：纠偏较慢，加速度计波动对结果影响较小。
• α 较小：更快靠近加速度计参考，也更容易受其干扰。

0.95238 不是固定标准，选择还要结合采样间隔和希望的纠偏速度。基础滤波通常每次都修正，没有固定的"修正到某个角度就停止"的界限；差值变小，修正量自然变小。

Yaw 没有加速度计融合项，是因为重力不能提供偏航方向参考。 因此，仅靠 MPU6050，这个融合过程可以限制俯仰和横滚漂移，但不能消除偏航角的长期漂移。