MPU6050 姿态解算学习笔记
本笔记面向零基础读者，从物理直觉出发，逐步理解 MPU6050 的姿态解算原理。
第一章：基础概念
1.1 什么是姿态？
姿态描述的是物体在三维空间中的朝向。我们用三个角度来描述它，最直观的类比是飞机：
| 角度 | 英文 | 飞机动作 | 生活感受 |
|---|---|---|---|
| 横滚角 | Roll | 飞机左右倾斜，像在做桶滚 | 摩托车过弯时身体倾斜 |
| 俯仰角 | Pitch | 飞机抬头或低头 | 过山车爬坡或俯冲 |
| 偏航角 | Yaw | 飞机左转或右转（机头指向变化） | 汽车转弯时方向盘转动 |
抬头 (+Pitch)
↑
|
左转 ←----[ 飞机 ]----→ 右转
(+Yaw) | (-Yaw)
↓
低头 (-Pitch)
左倾 (←) Roll (+) 右倾 (→) Roll (-)
1.2 MPU6050 能测什么？
MPU6050 是一颗 6 轴传感器，内部集成了两种传感器：
加速度计（Accelerometer）—— 3 轴
测量线性加速度，包括重力加速度。
| 输出 | 含义 | 单位 |
|---|---|---|
| ax | X 轴方向的加速度 | g（重力加速度，1g ≈ 9.8 m/s²） |
| ay | Y 轴方向的加速度 | g |
| az | Z 轴方向的加速度 | g |
静止平放时：ax ≈ 0, ay ≈ 0, az ≈ 1g（重力全部落在 Z 轴）
陀螺仪（Gyroscope）—— 3 轴
测量角速度，即旋转的快慢。
| 输出 | 含义 | 单位 |
|---|---|---|
| gx | 绕 X 轴旋转的角速度 | °/s（度每秒） |
| gy | 绕 Y 轴旋转的角速度 | °/s |
| gz | 绕 Z 轴旋转的角速度 | °/s |
静止时：gx ≈ 0, gy ≈ 0, gz ≈ 0（没有旋转）
原始值与物理值
传感器输出的是 16 位原始值（-32768 ~ 32767），需要根据量程换算为物理值：
物理值 = 原始值 / 灵敏度
| 传感器 | 量程示例 | 灵敏度 |
|---|---|---|
| 加速度计 | ±2g | 16384 LSB/g |
| 加速度计 | ±16g | 2048 LSB/g |
| 陀螺仪 | ±250°/s | 131 LSB/(°/s) |
| 陀螺仪 | ±2000°/s | 16.4 LSB/(°/s) |
通信方式
MPU6050 通过 I2C 与 MCU 通信：
- 默认地址：0x68（AD0 接地）或 0x69（AD0 接高）
- 数据寄存器从 0x3B 开始，连续 14 字节包含全部传感器数据
1.3 坐标系约定
理解坐标系是正确解算姿态的前提。MPU6050 使用右手坐标系。
右手定则
伸出右手，拇指、食指、中指互相垂直：
- 拇指 → X 轴正方向
- 食指 → Y 轴正方向
- 中指 → Z 轴正方向
MPU6050 芯片坐标系
以芯片正面朝上、标识文字正向为参考：
(芯片顶视图)
+Y
↑
|
|
+X ←---●---→ -X
|
|
↓
-Y
+Z 向上（朝向你）
-Z 向下（朝向桌面）
● = MPU6050 芯片中心
旋转正方向
使用右手螺旋定则：大拇指指向轴的正方向，四指弯曲方向即为该轴旋转的正方向。
| 旋转 | 轴 | 正方向描述 |
|---|---|---|
| Roll | X 轴 | 从 +X 方向看，逆时针旋转为正 |
| Pitch | Y 轴 | 从 +Y 方向看，逆时针旋转为正 |
| Yaw | Z 轴 | 从 +Z 方向看（俯视），逆时针旋转为正 |
第二章：加速度计解算
2.1 目标
利用加速度计输出的 ax, ay, az 三个值，计算出传感器的 Roll（横滚角） 和 Pitch（俯仰角）。
2.2 常见误区：加速度计测的是"运动"吗？
初学者看到"加速度计"这个名字，很容易产生一个误解：
“加速度计测的是物体运动的加速度，静止时应该没有读数。”
但实际上，当你把 MPU6050 静止平放在桌上时，读数是：
- ax ≈ 0, ay ≈ 0, az ≈ 1g
明明没动，为什么 Z 轴有读数？
这说明加速度计测的不只是运动，它还能感知重力。要理解这一点，我们需要换一个角度看待加速度计。
2.3 物理模型：盒子与小球
不要把加速度计看作一个复杂的电子芯片，请把它想象成一个物理机械装置：
- 装置：一个封闭的小盒子（传感器本身）。
- 核心：盒子中间悬浮着一个有质量的小球（质量块）。
- 测量原理：
- 盒子内壁有压力感应。
- 小球压在哪个方向的墙壁上，那个轴就有读数。
- 读数的大小 = 小球压在墙壁上的力的大小。
现在再想想"静止平放"的情况：
- 盒子静止，小球受重力作用，死死压在盒底
- 盒底就是 Z 轴方向 → 所以 az = 1g
核心认知：加速度计本质上是一个测力计，它测的是小球对墙壁的压力。
2.4 静态解算：利用重力算角度
当我们想计算姿态（欧拉角）时，我们利用的是地球重力（g）。
前提：物体处于静止或匀速直线运动状态。
现象：
- 平放时：重力垂直向下，小球死死压在盒底（Z轴）。
- 读数：Z = 1g, X = 0, Y = 0 → 角度 = 0°
- 翻滚（Roll）时：我们将盒子转动，重力的方向相对于盒子变了，小球滚向了侧壁（Y轴）。
- 读数：Y 轴读数变大，Z 轴读数变小。
- 解算逻辑：
- 传感器检测到：“现在重力分量在 Y 轴上有 0.7g，在 Z 轴上有 0.7g。”
- 结论：“既然重力分配变了，那盒子肯定是歪了 45 度。”
本质：加速度计解算角度，就是测量重力在各个轴上的投影，然后反推倾斜角度。
解算公式
Roll = atan2(ay, az)
Pitch = atan2(-ax, sqrt(ay² + az²))
代码实现
void mpu6050_calc_attitude_accel(mpu6050_t *data)
{
data->roll = atan2f(data->ay, data->az) * RAD_TO_DEG;
data->pitch = atan2f(-data->ax, sqrtf(data->ay * data->ay + data->az * data->az)) * RAD_TO_DEG;
}
2.5 现实挑战：加速度计是怎么"被骗"的？
这是理解加速度计局限性的最关键一步。
我们通常认为加速度是描述"运动快慢"的，但在传感器内部，"运动"和"重力"表现得一模一样。
场景对比
A. 真的转动了（Tilt）
- 动作：把盒子向后仰 90 度（竖起来）。
- 物理过程：重力拉着小球，把它压在后壁上。
- 传感器视角：检测到后壁受压。
- 解算结果：90 度（正确）。
实测数据：缓慢倾斜传感器，Roll/Pitch 平稳变化，与实际角度一致。
B. 猛地向前加速（Acceleration）
- 动作：盒子保持水平（0 度），猛踩油门向前冲（平移）。
- 物理过程：由于惯性，小球不愿意动，它被甩向了后壁。
- 传感器视角：检测到后壁受压。
- 解算结果：90 度（错误！明明是平移，却算出了角度）。
实测数据：突然晃动传感器，角度数据剧烈跳变，完全不可信。
2.6 小结
- 加速度计其实是"测力计"：它测量的不是纯粹的运动，而是合力（重力 + 惯性力）。
- 解算的前提假设：
- 我们假设物体是静止的（忽略惯性力）。
- 一旦物体产生平移变速运动，惯性力就会混入，破坏这个假设。
- 传感器无法区分：到底是"歪了"还是"加速了"。
- 实际表现：
- 静止时，角度非常准。
- 一晃动，角度数据就会剧烈抖动。
| 优势 | 局限 |
|---|---|
| 静止时精准 | 动态时抖动 |
| 无累积漂移 | 无法测量 Yaw |
| 算法简单 | 对振动敏感 |
第三章：一阶互补滤波
3.1 为什么不能只靠加速度计？
正如第二章所述，加速度计在动态下会被骗。
- 例子：假设你正在做一个平衡小车，电机突然启动。
- 加速度计：“惯性力把小球甩到后壁了！我认为车子倒了 45 度！”（其实车子还是直的，只是在加速）
- 后果：如果只听加速度计的，控制器会因为错误的判断而做出错误的补偿。
3.2 陀螺仪：弥补加速度计的"冲动"
陀螺仪测的是角速度（°/s）。它不关心重力，也不关心惯性，它只关心：“我现在转得有多快？”
物理直觉：盲人的步伐
想象你闭着眼睛走在一段楼梯上：
- 加速度计（触觉定位）：就像你每走一步都用手摸一下台阶（测重力方向）。虽然能确定你在哪，但如果有人推你一下，你的手可能摸错位置（振动干扰）。
- 陀螺仪（空间感觉）：就像你内耳的平衡觉。即使闭上眼，你也能感觉到"我刚才向左转了 30 度"。
陀螺仪的计算依据：积分
$$角度_{当前} = 角度_{上一时刻} + 角速度 \times dt$$
如果传感器原本在 0°，陀螺仪测到当前正以 10°/s 的速度旋转，过了 0.01 秒，陀螺仪会告诉你：“我现在在 0.1° 的位置”。
陀螺仪的优缺点
| 优点 | 缺点 |
|---|---|
| 不受平移加速度干扰 | 零点漂移（积分误差累积） |
| 短期内平滑精准 | 长期运行会"跑偏" |
| 响应速度快 | 需要准确的 dt |
3.3 互补滤波原理
现在我们把两者的特性放在一起看：
| 传感器 | 长期表现 | 短期（动态）表现 | 弱点 |
|---|---|---|---|
| 加速度计 | 准（重力永远向下） | 差（怕振动和加速） | 抖动严重 |
| 陀螺仪 | 差（累积漂移） | 准（响应快且平滑） | 随时间跑偏 |
核心思想：用陀螺仪提供短期精度，用加速度计提供长期参考。
公式
angle = α * (angle + gyro * dt) + (1 - α) * accel_angle
- 左边（α = 0.98）：98% 信任陀螺仪的"空间感觉"。它说转了 0.1°，我们就加上 0.1°。保证数据平滑，不会因为振动而乱跳。
- 右边（1-α = 0.02）：2% 信任加速度计。虽然它容易受干扰，但它指的方向（重力）长期来看永远是对的。这 2% 的权重像一只微弱的手，不停地把陀螺仪跑偏的角度往重力方向"拽"回来。
3.4 代码实现
#define CF_ALPHA 0.98f
void mpu6050_calc_attitude_cf(mpu6050_t *data, float dt)
{
float accel_roll, accel_pitch;
/* 加速度计角度（作为长期参考） */
accel_roll = atan2f(data->ay, data->az) * RAD_TO_DEG;
accel_pitch = atan2f(-data->ax, sqrtf(data->ay * data->ay + data->az * data->az)) * RAD_TO_DEG;
/* 互补滤波 */
data->roll = CF_ALPHA * (data->roll + data->gx * dt) + (1 - CF_ALPHA) * accel_roll;
data->pitch = CF_ALPHA * (data->pitch + data->gy * dt) + (1 - CF_ALPHA) * accel_pitch;
}
调用时传入采样周期：
mpu6050_calc_attitude_cf(&mpu_data, 0.01f); // dt = 10ms
3.5 局限性
一阶互补滤波虽然简单有效，但存在以下局限：
欧拉角的固有缺陷
| 问题 | 说明 |
|---|---|
| 万向节死锁 | 当 Pitch = ±90° 时，Roll 和 Yaw 轴重合，丢失一个自由度 |
| Pitch ≈ 90° 时数值奇异 | Roll = atan2(ay, az)，此时 az ≈ 0，分母趋近于零，ay 微小变化导致 Roll 剧烈跳变(如下图) |
算法本身的局限
| 问题 | 说明 |
|---|---|
| α 值固定 | 无法根据运动状态自适应调整权重 |
| 仍无法解算 Yaw | 加速度计无法感知绕 Z 轴旋转 |
| 自由落体失效 | ax = ay = az ≈ 0 时，加速度计无参考 |
| 振动环境 | 持续振动会污染加速度计的长期均值 |
解决方向
要彻底解决这些问题，需要：
- 四元数：避免万向节死锁和奇异点
- Mahony / Madgwick 算法：更优雅的融合方式
- 磁力计：解算 Yaw
3.6 小结
| 角色 | 传感器 | 职责 |
|---|---|---|
| 主力军 | 陀螺仪 | 提供平滑、实时的角度变化 |
| 教官 | 加速度计 | 长期纠正陀螺仪的漂移 |
一阶互补滤波是性价比最高的入门方案：
- 相比纯加速度计：动态不再抖动
- 相比纯陀螺仪：长期不再漂移
- 代码简单，资源占用低
但对于全姿态、高精度场景，需要进阶到四元数方案。