1、姿态估计基础：什么是姿态？欧拉角与四元数入门，为什么需要滤波？
各位同学，欢迎来到《互补滤波姿态估计算法实战》的第一讲。
我是你们这门课的老朋友。做了十几年嵌入式算法，我最大的感触就是：姿态解算，是所有运动控制、导航、AR/VR设备的灵魂。你想想看，一个四轴飞行器如果不知道自己“头朝哪”，它怎么悬停？一个手机如果不知道自己是横着还是竖着，屏幕怎么翻转？
所以，咱们今天先把地基打牢。这一章，我们不写一行代码，但我会把姿态的“根”给你讲透。
1.1 到底什么是“姿态”？
说白了，姿态就是描述一个物体在三维空间里“怎么摆放”的。
举个例子：你手里拿着一部手机。
- 位置：手机在桌子上，还是在口袋里？这是坐标问题。
- 姿态：手机屏幕是朝上还是朝下？摄像头是朝东还是朝西？这是角度问题。
在工程上，我们通常用一个坐标系来描述姿态。最常见的是：
- 地理坐标系（NED）：北-东-地。想象你站在操场上，X轴指北，Y轴指东，Z轴指向地心。
- 机体坐标系（Body）：固定在物体上。比如飞机的机头方向是X，右翼是Y，垂直向下是Z。
姿态解算，就是求这两个坐标系之间的旋转关系。嗯，这里要注意，这个旋转关系不是唯一的，它可以用好几种数学工具来表达。
核心概念：姿态 = 从“地理坐标系”到“机体坐标系”的旋转映射。
1.2 欧拉角：最直观，但有个大坑
欧拉角，大家应该不陌生。它就是三个角度：横滚（Roll）、俯仰（Pitch）、偏航（Yaw）。
我个人习惯用“飞机比喻”来理解：
- Roll（Φ）：飞机绕机头轴线旋转，就像你侧身翻滚。
- Pitch（θ）：飞机抬头或低头。
- Yaw（ψ）：飞机水平转向，就像你原地转圈。
欧拉角的好处是物理意义极其清晰。你一看 Roll=30°，就知道飞机向右倾斜了30度。调试的时候，我经常直接打印欧拉角看波形，非常直观。
但是，欧拉角有一个致命缺陷——万向锁（Gimbal Lock）。
我在项目中遇到过这么一件事：有一次做云台控制，当俯仰角接近±90°时，横滚和偏航突然“打架”了，云台瞬间乱转。为什么会这样？因为当 Pitch=90° 时，Roll 和 Yaw 的旋转轴重合了，系统丢失了一个自由度。
所以，欧拉角不适合做全姿态的插值和连续旋转。它只适合用来给人看，或者在小角度变化时使用。
避坑指南：我曾经因为没处理好万向锁，导致一个无人机在倒飞时姿态解算崩溃。记住，只要你的物体可能经历大角度运动（比如翻转），就别用欧拉角做内部运算。
1.3 四元数：数学上完美，但难理解
既然欧拉角有坑，那用什么？答案是四元数。
四元数听起来玄乎，其实就是一个超复数：q = w + xi + yj + zk。其中 w 是实部，x、y、z 是虚部，且满足 i² = j² = k² = ijk = -1。
你不需要深究它的数学推导，只需要记住它的物理意义：
- 四元数描述的是绕一个单位向量 (x, y, z) 旋转 θ 角度。
- 其中 w = cos(θ/2)，(x, y, z) = sin(θ/2) * 旋转轴方向。
四元数最大的优点是：没有奇点，可以平滑插值，计算效率高。在嵌入式里，我们几乎都用四元数做内部运算。
举个例子，把欧拉角转成四元数：
// 假设 Roll=30°, Pitch=20°, Yaw=10°
// 先转弧度，再计算
float cy = cos(yaw * 0.5);
float sy = sin(yaw * 0.5);
float cp = cos(pitch * 0.5);
float sp = sin(pitch * 0.5);
float cr = cos(roll * 0.5);
float sr = sin(roll * 0.5);
q.w = cy * cp * cr + sy * sp * sr;
q.x = cy * cp * sr - sy * sp * cr;
q.y = sy * cp * sr + cy * sp * cr;
q.z = sy * cp * cr - cy * sp * sr;我的小技巧：调试时，我习惯把四元数转成欧拉角再打印，因为人脑对角度更敏感。但内部运算，永远用四元数。
1.4 为什么需要滤波？传感器都是“骗子”
好，现在我们知道姿态可以用四元数表示。那怎么得到它？靠传感器。
常用的传感器有三个：
| 传感器 | 测量什么 | 优点 | 缺点 |
|---|---|---|---|
| 陀螺仪 | 角速度 | 动态响应快，高频准 | 有零偏漂移，积分会发散 |
| 加速度计 | 加速度（含重力） | 长期稳定，能测倾角 | 易受振动干扰，不能测偏航 |
| 磁力计 | 地磁场 | 能测偏航角 | 易受铁磁干扰，精度差 |
你想想看：
- 只用陀螺仪？积分几分钟，角度就飘到天边去了。
- 只用加速度计？稍微一震动，角度就乱跳。
- 只用磁力计？在室内或者靠近金属，数据完全不可信。
所以，我们需要滤波。
滤波的本质，就是取长补短：
- 陀螺仪提供短期精度（高频好，但长期漂移）。
- 加速度计和磁力计提供长期参考（低频好，但短期噪声大）。
互补滤波，就是用一个低通滤波器处理加速度计/磁力计，用一个高通滤波器处理陀螺仪，然后把它们加起来。说白了，就是相信陀螺仪的动态变化，但用加速度计和磁力计来“拉”住它，不让它飘走。
一句话总结：没有滤波的姿态解算，就像闭着眼睛走路——刚开始还行，走几步就偏了。
1.5 本章知识体系：一张图看懂
下面这张图，是我手绘的本章核心逻辑。你可以把它当作整个课程的“导航图”。
这张图你看懂了吗？从“姿态是什么”出发，我们分了三路：数学表示、传感器、滤波需求。最终汇合到互补滤波这个核心方法上。后面的章节，我们会一步步把这个框架填满。
课后思考：如果你只用加速度计，能不能算出完整的姿态（Roll、Pitch、Yaw）？为什么？想明白这个问题，你就理解了为什么需要磁力计。
公众号:蓝海资料掘金营，微信deep3321