一、前言

上篇文章 完成了 MPU6050 驱动的搭建，通过 VOFA 可以实时观察 7 通道原始数据（ax/ay/az/gx/gy/gz/temperature）。但原始数据不能直接用于平衡车控制——我们需要三个欧拉角：

欧拉角	平衡车比喻	含义
Yaw（偏航角）	方向盘	绕 Z 轴旋转，车头朝向
Pitch（俯仰角）	油门/刹车	绕 X 轴旋转，前后倾斜
Roll（横滚角）	转弯侧倾	绕 Y 轴旋转，左右倾斜

本文记录从陀螺仪&加速度计原始数据 → 欧拉角解算的完整过程，核心算法是互补滤波。

硬件平台：STM32F103C8T6，MPU6500 六轴传感器，I2C1 PB8/PB9，400kHz。工程框架使用铁头山羊的 task.h 调度器。

---

二、为什么不能只用一种传感器？

2.1 纯陀螺仪积分 → 漂移

陀螺仪输出的是角速度（°/s），角度靠积分得到：

angle += gyro * dt;   // 每 5ms 累加一次

问题：陀螺仪存在零偏（bias），即使传感器完全静止，输出也不为 0。

假设零偏 = +0.5°/s，dt = 5ms：

1秒   →  +0.5°
10秒  →  +5.0°
1分钟 →  +30°      ← 歪到天上去了

零偏会被积分无限放大，这就是"静止漂移"的根源。

2.2 纯加速度计计算 → 抖动

加速度计静止时测的是重力方向，可以直接算角度：

pitch = atan2(ay, az);   // 利用重力在 Y/Z 轴的分量
roll  = atan2(ax, az);

重力永远指向地心，不会漂移。但车子一运动，加速度计读到的就不只是重力了——还混入了运动加速度：

静止：a = g（纯重力，角度准）
运动：a = g + a_motion（重力 + 刹车/加速/颠簸，角度抖）

实测中加速度计在平衡车运行时波形毛刺严重，直接用于控制会导致电机剧烈抖动。

2.3 一张表看清优劣

	陀螺仪	加速度计
高频（动态）	✅ 响应快，不受运动影响	❌ 混入运动加速度，毛刺大
低频（静态）	❌ 零偏积分 → 漂移	✅ 测重力方向，绝对不漂
适合场景	短时间内角度变化	长时间角度参考

结论：两者正好互补 → 互补滤波。

---

三、互补滤波原理

3.1 核心思想

互补滤波本质上是把陀螺仪的高通和加速度计的低通加在一起：

     陀螺仪 → 高通滤波 → 只取高频分量（动态变化）
                        ↘
                         相加 → 全频段角度
                        ↗
  加速度计 → 低通滤波 → 只取低频分量（静态参考）

频域上看：

增益 ↑
     │   \          / 加速度计
     │    \        /   （低频好）
     │     \      /
     │      \    /
     │       \  /
     │        \/       ← 交叉频率 = 1/τ
     │        /\
     │       /  \
     │      /    \
     │     /      \    陀螺仪
     │    /        \   （高频好）
     └───┴─────────┴──→ 频率
         10rad/s (≈1.6Hz)

上图直观展示了互补滤波的频率特性：低频信任加速度计（重力不漂），高频信任陀螺仪（动态跟得上）。

3.2 数学公式

// 互补滤波核心公式
angle = α × (angle + gyro × dt)          // 陀螺仪积分（高通）
      + (1-α) × accel_angle;               // 加速度计修正（低通）

// α 的含义：
//   α → 1：更信任陀螺仪 → 响应快但漂移大
//   α → 0：更信任加速度计 → 不漂但抖动大

// 滤波器时间常数：
//   τ = α / (1-α) × dt
//
//   本文：α = 0.95238, dt = 5ms
//        τ = 0.95238 / 0.04762 × 0.005 = 0.1秒
//   含义：加速度计对漂移的"拉回"作用在 100ms 内完成 63%

α 怎么选？ 用时间常数公式反推比较直观：

想要的 τ	dt=5ms 对应的 α	特点
0.05s	0.909	非常灵活，运动加速度干扰可能较大
0.1s	0.95238	平衡车常用，本文采用
0.5s	0.990	更平滑，但对快速倾斜响应稍慢

本文 α=0.95238 配合 5ms 调度周期，时间常数 100ms，是平衡车场景的实践经验值。

---

四、atan vs atan2：为什么必须选后者

加速度计算角度需要用反正切函数。C 标准库提供了两个：

	atan(y/x)	atan2(y, x)
输入	一个比值	y 和 x 各传各的
返回值范围	[-90°, +90°]	[-180°, +180°]
x=0	除零崩溃	返回 ±90°，安全
象限判断	❌ 分不清	✅ 根据 y/x 符号判断

实际陷阱：

atan(1/1)  = 45°   ┐
atan(-1/-1) = 45°   ┘  明明一三象限，结果一样！

平衡车的俯仰角完全可能超过 ±90°（翻车时），atan 范围不够且象限混乱。在 STM32 上用 atan2f()（单精度版本），不要用 atan。

课程 PPT"5.4【MPU6050】解算欧拉角（上）"专门有一页对比 y=arctan(x) 和 y=atan2(x) 的取值范围，结论就是 atan2。

---

五、轴方向与取反问题

5.1 芯片坐标系

MPU6050 芯片的坐标系如下（右手定则）：

        Z ↑ (上)
          |
          |
          |
          +--------→ Y (芯片标注方向)
         /
        /
       ↙
      X

5.2 平衡车上的实际安装

芯片平焊在 PCB 上，PCB 水平安装在车体上。此时：

• Z 轴 = 垂直于 PCB 面，指向天空
• X 轴 = 车体左右方向
• Y 轴 = 车体前后方向

5.3 Roll 为什么取反？

代码里这句是关键：

float roll_g = roll - gy * 0.05f;   // ← 注意是减号！
float pitch_g = pitch + gx * 0.05f; // ← 这个是加号

原因是 芯片 Y 轴的实际物理方向与右手定则规定的正旋转方向相反。

当车体向右倾倒（右侧低、左侧高）：

从车头方向看，这是顺时针旋转。芯片的 Y 轴指向车前方，绕 Y 轴顺时针 → 右手定则说 gy 应该为正。芯片确实输出正的 gy。

但绕 Y 轴的顺时针旋转在直觉上应该是"向右倒 → Roll 角减小"。而纯积分 roll += gy * dt 得到的是 Roll 增大。

所以需要取反：roll -= gy * dt，让"向右倒"对应"Roll 减小"。

一句话：atan2(ax, az) 给出的角度方向和 gy 积分的方向刚好反着，取反就对齐了。

验证方法：向右倾斜传感器，看 VOFA 中 Roll 波形是否负向走。如果是，代码正确。

---

六、完整代码实现

6.1 app_mpu6050.h 新增声明

// —— 欧拉角获取 ——
float App_MPU6050_GetYaw(void);
float App_MPU6050_GetPitch(void);
float App_MPU6050_GetRoll(void);

6.2 app_mpu6050.c 新增变量

#include "math.h"      // atan2f, M_PI
#include "task.h"       // PERIODIC 宏

static float yaw, pitch, roll;   // 欧拉角，单位：度

6.3 App_MPU6050_Proc() — 核心互补滤波

void App_MPU6050_Proc(void)
{
    PERIODIC(5);                     // 每 5ms 执行一次
    App_MPU6050_Update();            // 读取最新传感器数据

    // ─── 1. 陀螺仪纯积分 ───
    float yaw_g  = yaw + gz * 0.005f;    // Yaw：绕Z轴
    float pitch_g = pitch + gx * 0.005f; // Pitch：绕X轴（车前后倾）
    float roll_g  = roll - gy * 0.005f;  // Roll：绕Y轴（车左右倾）
    //                          ↑ 减号：芯片Y轴方向与右手定则反号

    // ─── 2. 加速度计计算 ───
    float pitch_a = atan2f(ay, az) / M_PI * 180.0f;
    float roll_a  = atan2f(ax, az) / M_PI * 180.0f;
    // 注意：pitch_a 用 ay，roll_a 用 ax
    // 因为芯片放置时 X=左右, Y=前后

    // ─── 3. 互补滤波融合 ───
    float alpha = 0.95238f;              // τ = 100ms @ 5ms周期
    yaw   = yaw_g;                        // Yaw 无法用加速度计修正
    pitch = alpha * pitch_g + (1.0f - alpha) * pitch_a;
    roll  = alpha * roll_g  + (1.0f - alpha) * roll_a;
}

6.4 关键细节说明

PERIODIC(5) 的作用：

// 等价于下面三行代码，保证此函数每 5ms 只执行一次
static uint32_t next_time = 0;
if (GetTick() < next_time) return;
// ... 执行逻辑 ...
next_time += 5;

dt = 5ms 为什么硬编码而不实测？

因为 PERIODIC(5) 已经保证了精确的 5ms 周期，dt 就是常数，无需每次用 GetUs() 去量。

M_PI 从哪来？

#include <math.h>
// math.h 中有 #define M_PI 3.14159265358979323846
// atan2f 返回弧度，/ M_PI * 180.0f 转为角度

---

七、VOFA 测试验证

7.1 测试代码

void MPU6050_EularAngleTest(void)
{
    App_MPU6050_Init();
    App_USART2_Init();

    while (1)
    {
        App_MPU6050_Proc();    // 5ms 更新一次欧拉角
        USART2_Proc();         // 10ms 发一次数据到 VOFA
    }
}

static void USART2_Proc(void)
{
    PERIODIC(10);  // 每 10ms 发送一次（100Hz 足够观察）

    float ax = App_MPU6050_GetAx();
    float ay = App_MPU6050_GetAy();
    float az = App_MPU6050_GetAz();
    float gx = App_MPU6050_GetGx();
    float gy = App_MPU6050_GetGy();
    float gz = App_MPU6050_GetGz();
    float yaw   = App_MPU6050_GetYaw();
    float pitch = App_MPU6050_GetPitch();
    float roll  = App_MPU6050_GetRoll();
    float temp  = App_MPU6050_GetTemperature();

    // 10 通道 VOFA 数据
    My_USART_Printf(USART2, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
        ax, ay, az, gx, gy, gz, temp, yaw, pitch, roll);
}

为什么数据更新（5ms）和串口发送（10ms）用不同频率？

• 互补滤波需要高频运行 → 5ms（200Hz），积分精度更高
• 串口发送速率有限，10ms（100Hz）足够 VOFA 画图，不丢帧
• 这是"快计算、慢外设"的常见模式

7.2 验证步骤

1. 传感器水平放置，观察 VOFA：
   • pitch ≈ 0°，roll ≈ 0°（加速度计初始化时已对准重力）
   • gyro 三轴 ≈ 0°/s（静止状态）
   • yaw 缓慢漂移（正常，无法用重力修正）
2. 前后倾斜传感器（模拟车点头/后仰）：
   • pitch 正负变化，响应灵敏且不抖动
   • 静止后 pitch 稳定在倾斜角度，不漂移
3. 左右倾斜传感器（模拟车侧翻）：
   • roll 正负变化
   • 有加速度计修正，长期稳定
4. 快速晃动摇摆：
   • 波形平滑无毛刺（互补滤波的功劳）
   • 静止刹那立即回正（加速度计的低频修正生效）

---

八、总结

核心要点

#	要点	说明
1	陀螺仪积分会漂移	零偏被 dt 无限放大，1 分钟歪 30°不是梦
2	加速度计不漂但抖	运动加速度混入，高频毛刺严重
3	互补滤波取长补短	陀螺仪管动态 + 加速度计拽静态，α=0.95238
4	用 atan2 不用 atan	象限正确、范围 [-180°,+180°]、x=0 不炸
5	Roll 要取反	芯片 Y 轴方向与右手定则反号
6	PERIODIC 保证精确 dt	定时器宏简化代码，无需手动计时

程序架构

App_MPU6050_Init()     // 初始化 I2C + MPU6050 寄存器
         ↓
while(1) {
    App_MPU6050_Proc() // 每 5ms：读传感器 → 互补滤波 → 更新欧拉角
    USART2_Proc()      // 每 10ms：发送 10 通道数据到 VOFA
}

下一步

有了稳定可靠的欧拉角（Pitch/Roll），下一步就是PID 控制——用 Pitch 角度偏差驱动电机，让平衡车站起来。

---

如果这篇文章对你有帮助，欢迎点赞收藏 👍