3. 坐标系与姿态表示：世界坐标系与机体坐标系、欧拉角、旋转矩阵、四元数基础
好，咱们进入第三讲。这一讲，说白了就是解决一个核心问题：传感器装在板子上，板子在天上飞，我怎么知道它朝哪个方向？
我刚开始做飞控那会儿，觉得这玩意儿特简单——不就是测个角度嘛。结果第一次试飞，无人机原地打转，差点砸到人。后来才发现，是坐标系搞混了。嗯，这里面的坑，我今天带你一个个踩一遍。
3.1 两个坐标系：世界 vs 机体
先搞清楚我们在哪。做传感器融合，你脑子里必须同时装着两个坐标系：
- 世界坐标系（World Frame）：也叫地理坐标系。通常用东北天（ENU）或北东地（NED）。我习惯用ENU，因为直观——X轴朝东，Y轴朝北，Z轴朝天。
- 机体坐标系（Body Frame）：固定在设备上。X轴朝机头，Y轴朝右翼，Z轴朝下（对无人机来说）。
你想想看，加速度计测的是机体坐标系下的加速度，但你要算位置，得转换到世界坐标系。陀螺仪测的是机体角速度，但你要控制姿态，也得知道它在世界坐标系里怎么转。
核心思想：传感器读数在机体坐标系，控制目标在世界坐标系。中间差的就是一个旋转关系。
3.2 欧拉角：俯仰、横滚、偏航
欧拉角是最直观的姿态表示法。说白了就是三个角度：
- 俯仰（Pitch, θ）：绕Y轴旋转。抬头为正，低头为负。我在做平衡车项目时，俯仰角就是命根子——稍微偏一点，车就倒了。
- 横滚（Roll, φ）：绕X轴旋转。右倾为正，左倾为负。
- 偏航（Yaw, ψ）：绕Z轴旋转。右转为正，左转为负。
旋转顺序很重要。我一般用Z-Y-X顺序（先偏航，再俯仰，最后横滚）。为什么？因为这样最符合直觉——你先转方向，再低头抬头，最后侧身。
我曾经踩过的坑：欧拉角有万向锁问题。当俯仰角接近±90°时，横滚和偏航会耦合，丢失一个自由度。做全姿态飞行器时，千万别只用欧拉角！
3.3 旋转矩阵：数学上的桥梁
旋转矩阵就是把机体坐标转到世界坐标的数学工具。它是一个3×3的正交矩阵，行列式为1。
从欧拉角到旋转矩阵的公式（Z-Y-X顺序）：
R = Rz(ψ) * Ry(θ) * Rx(φ)
其中：
Rz(ψ) = [cosψ -sinψ 0]
[sinψ cosψ 0]
[0 0 1]
Ry(θ) = [cosθ 0 sinθ]
[0 1 0 ]
[-sinθ 0 cosθ]
Rx(φ) = [1 0 0 ]
[0 cosφ -sinφ]
[0 sinφ cosφ]嗯，看着复杂，但实际用起来很简单。你只要记住：旋转矩阵乘以机体坐标向量，就得到世界坐标向量。
我的小技巧：在MCU上，别直接算三角函数。用查表法或者CORDIC算法，能省不少CPU时间。我在STM32F4上做过测试，查表比直接算sin/cos快了将近10倍。
3.4 四元数基础
四元数，说白了就是四个数：一个实部加三个虚部。写成 q = w + xi + yj + zk。
为什么用四元数？三个原因：
- 无万向锁：这是最大的优势。做全姿态估计，四元数是首选。
- 计算量小：只有乘法和加法，没有三角函数。适合MCU。
- 易于插值：做姿态平滑时，四元数球面线性插值（SLERP）比欧拉角插值自然得多。
四元数的基本运算：
// 四元数乘法（用于组合旋转）
q1 * q2 = (w1*w2 - x1*x2 - y1*y2 - z1*z2) +
(w1*x2 + x1*w2 + y1*z2 - z1*y2)i +
(w1*y2 - x1*z2 + y1*w2 + z1*x2)j +
(w1*z2 + x1*y2 - y1*x2 + z1*w2)k
// 四元数共轭（用于求逆旋转）
q* = w - xi - yj - zk
// 四元数模长
|q| = sqrt(w² + x² + y² + z²)重要提醒：四元数一定要归一化！我见过太多人因为忘记归一化，导致姿态越算越偏。每次更新完四元数，记得做一次归一化：q = q / |q|。
3.5 四元数与欧拉角的转换
实际项目中，我们经常需要在两种表示之间来回切换。比如：
- 传感器融合算法（如Mahony、Madgwick）输出四元数
- 但用户界面显示需要欧拉角
- 控制算法可能两种都需要
四元数转欧拉角（Z-Y-X顺序）：
float q0 = q.w, q1 = q.x, q2 = q.y, q3 = q.z;
float roll = atan2(2*(q0*q1 + q2*q3), 1 - 2*(q1*q1 + q2*q2));
float pitch = asin(2*(q0*q2 - q3*q1));
float yaw = atan2(2*(q0*q3 + q1*q2), 1 - 2*(q2*q2 + q3*q3));欧拉角转四元数：
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 = cr * cp * cy + sr * sp * sy;
q.x = sr * cp * cy - cr * sp * sy;
q.y = cr * sp * cy + sr * cp * sy;
q.z = cr * cp * sy - sr * sp * cy;我的经验：在MCU上，我建议全程用四元数做运算，只在最后输出时转成欧拉角。这样既避免了万向锁，又减少了三角函数调用。我在一个四轴项目中，用这个策略把姿态更新频率从200Hz提到了500Hz。
3.6 实际项目中的选择建议
| 场景 | 推荐表示法 | 原因 |
|---|---|---|
| 地面机器人（2D平面） | 欧拉角（偏航角） | 只需要一个角度，简单直观 |
| 四轴飞行器 | 四元数 | 全姿态运动，避免万向锁 |
| 手持稳定器 | 四元数+欧拉角 | 内部用四元数，显示用欧拉角 |
| IMU数据记录 | 四元数 | 便于后期处理和插值 |
好了，这一讲的内容就到这儿。坐标系和姿态表示是传感器融合的基石，搞不懂这个，后面所有算法都是空中楼阁。下一讲，咱们开始聊具体的传感器——加速度计和陀螺仪，看看它们到底是怎么工作的。