1. 姿态解算概述
各位同学好，我是你们这门课的老朋友。今天咱们聊聊姿态解算——说白了，就是让机器知道自己「怎么躺」的技术。
你想想看，无人机在空中悬停，机器人走路不摔跤，手机导航告诉你该往哪转——这些背后都离不开姿态解算。我做了十几年嵌入式，可以说这玩意儿是机器人算法的「基本功」，就像练武要先扎马步一样。
1.1 什么是姿态解算
姿态解算，简单讲就是：通过传感器数据，计算出物体在三维空间中的朝向。
举个例子。你拿着手机玩《和平精英》，屏幕会跟着你转。手机怎么知道你转了多少度？靠的就是姿态解算算法。它把加速度计、陀螺仪这些传感器的原始数据，换算成你能理解的「俯仰角」「横滚角」「偏航角」。
核心公式（简化版）：
姿态 = f(加速度计, 陀螺仪, 磁力计, 时间)其中 f 就是我们后面要讲的滤波/融合算法。
我在项目中遇到过最典型的场景：给一个四足机器人做平衡控制。如果姿态算错 1 度，机器人就直接侧翻。嗯，那次调试我熬了三个通宵，最后发现是陀螺仪零偏没校准——所以你看，基础概念不扎实，后面全是坑。
1.2 IMU 传感器基础
IMU（惯性测量单元）是姿态解算的「眼睛」。它通常包含三个核心器件：
| 传感器 | 测量什么 | 优点 | 缺点 |
|---|---|---|---|
| 加速度计 | 三轴加速度（含重力） | 静态时能直接算出俯仰/横滚角 | 动态时噪声大，受振动影响 |
| 陀螺仪 | 三轴角速度 | 动态响应快，短时精度高 | 有零偏漂移，长时间积分会发散 |
| 磁力计 | 三轴地磁场强度 | 能提供绝对航向参考 | 易受环境磁场干扰 |
这里我多说一句。加速度计测的是「比力」，不是单纯的加速度。你拿着它静止不动，它测到的是 1g 的重力加速度。这个特性很重要——我们后面算俯仰角就靠它。
避坑指南： 我曾经在一个无人机项目里，直接用加速度计数据做姿态控制。结果飞机一飞起来就疯狂抖动。后来才发现，加速度计在剧烈运动时输出全是噪声。记住：加速度计只适合静态或慢速场景，动态场景必须配合陀螺仪。
1.3 姿态表示方法
有了传感器数据，怎么描述姿态？有三种主流方法：
1.3.1 欧拉角
最直观的方式。用三个角度表示：俯仰角（Pitch）、横滚角（Roll）、偏航角（Yaw）。
// 欧拉角示例（单位：度）
float pitch = 30.5f; // 抬头 30.5 度
float roll = -10.2f; // 向左倾斜 10.2 度
float yaw = 45.0f; // 朝向东北方向优点是好理解。但有个致命问题——万向锁。当俯仰角接近 ±90° 时，横滚和偏航会耦合，导致姿态丢失一个自由度。我当年做云台控制时就踩过这个坑，画面直接卡死。
1.3.2 旋转矩阵
用 3×3 矩阵表示旋转。数学上很完美，没有万向锁问题。
// 绕 Z 轴旋转 45 度的旋转矩阵
R = | cos45 -sin45 0 |
| sin45 cos45 0 |
| 0 0 1 |但缺点也很明显：计算量大。每次更新要算 9 个元素，嵌入式芯片扛不住。而且矩阵乘法容易累积误差，需要频繁正交化。
1.3.3 四元数
我个人最推荐的方法。用四个数表示旋转：一个实部 + 三个虚部。
// 四元数示例：绕 Z 轴旋转 45 度
q = [cos(22.5°), 0, 0, sin(22.5°)]
// 格式：[w, x, y, z]为什么推荐？三个原因：
- 无万向锁——随便你怎么转
- 计算量小——只有 4 个数，乘法比矩阵快 4 倍
- 插值平滑——做动画或控制时过渡自然
注意： 四元数虽然好，但理解起来有点反直觉。我刚开始学的时候，对着公式看了三天才搞明白「超复数」是什么。别急，后面我们会专门用一章讲四元数的几何意义和代码实现。
1.4 低延迟的重要性与应用场景
为什么这门课叫「低延迟姿态解算」？因为很多场景下，延迟就是生命。
举个例子。你玩穿越机（FPV 无人机），图传延迟 50ms 你还能飞，但姿态解算延迟超过 10ms，飞机就直接炸了。为什么？因为姿态是控制环路的输入，延迟会导致相位滞后，系统就会震荡。
我做过一个竞速机器人项目，要求姿态更新频率 1kHz（即每 1ms 算一次）。当时用的 STM32F4，主频 168MHz。如果算法写得不高效，一个周期要花 0.8ms，那 CPU 就干不了别的事了。所以我们必须优化到 0.2ms 以内。
低延迟姿态解算的典型应用场景：
- 无人机飞控——延迟 < 5ms，否则炸机
- 机器人平衡控制——延迟 < 10ms，否则摔倒
- AR/VR 头显——延迟 < 20ms，否则晕动症
- 手机防抖——延迟 < 30ms，否则画面模糊
核心观点： 姿态解算的延迟，决定了整个系统的实时性上限。你算法再准，如果算得慢，那就是废的。
本章知识体系
下面这张图，是我自己画的本章知识结构。你可以把它当作一个「地图」，后面每章都会对应到其中的一个模块。
好了，第一章就到这里。记住：姿态解算不是玄学，是数学 + 工程。后面我们会一步步深入，从传感器校准到滤波算法，再到代码级优化。你准备好了吗？