3、坐标系与姿态表示：欧拉角、旋转矩阵、四元数
各位同学，咱们今天聊点硬核的。做可穿戴设备，尤其是做运动追踪、姿态解算，你绕不开一个核心问题：怎么描述一个物体在三维空间里“转”了？
说白了，就是坐标系和姿态表示。我当年刚入行时，被欧拉角的万向锁坑过不止一次。后来做手环的9轴融合算法，又跟四元数死磕了两个月。今天我把这些经验掰开揉碎讲给你听。
3.1 坐标系：你得先有个“参照物”
描述姿态，第一步是定义坐标系。可穿戴设备里，我们通常用两套：
- 世界坐标系（全局坐标系）：固定在地面上，比如东北天（ENU）或北东地（NED）。你的设备最终输出的姿态，都是相对于这个系的。
- 载体坐标系（本体坐标系）：固定在设备上。比如手环的屏幕朝上为Z轴，表带方向为X轴。
嗯，这里要注意：所有传感器（加速度计、陀螺仪、磁力计）的原始数据，都是在载体坐标系下测量的。 你要做的，就是把这些数据转换到世界坐标系下，才能算出“人走了几步”、“手臂抬了多高”。
COORD_WORLD 和 COORD_BODY，避免搞混。曾经有一次，我把磁力计数据当成世界坐标系下的值直接用了，结果航向角漂了30度，查了两天才发现是坐标系没对齐。
3.2 欧拉角：直观，但别太依赖它
欧拉角是大家最容易理解的姿态表示法。它用三个角度来描述旋转：偏航（Yaw）、俯仰（Pitch）、横滚（Roll）。你可以想象成飞机在空中转：先转机头方向（Yaw），再抬头低头（Pitch），最后机身侧倾（Roll）。
但欧拉角有个致命问题——万向锁（Gimbal Lock）。当俯仰角达到 ±90° 时，偏航和横滚会失去一个自由度，导致姿态解算失效。
欧拉角的转换公式（Z-Y-X顺序）：
// 从旋转矩阵 R 提取欧拉角
pitch = asin(-R[2][0])
yaw = atan2(R[1][0], R[0][0])
roll = atan2(R[2][1], R[2][2])3.3 旋转矩阵：数学上完美，但计算量大
旋转矩阵是一个 3x3 的正交矩阵，它能把一个向量从一个坐标系旋转到另一个坐标系。它的优点是：没有奇点，可以连续旋转。
但缺点也很明显：9个元素，冗余度高。你想想看，每次更新姿态都要做 3x3 矩阵乘法，在低功耗MCU上跑，CPU开销不小。
旋转矩阵的性质：
- 正交性：
R * R^T = I - 行列式为 +1（右手系）
- 逆矩阵等于转置矩阵：
R^(-1) = R^T
我个人习惯，只在需要做坐标变换时用旋转矩阵。比如把加速度计数据从载体系转到世界系：
// 载体坐标系下的加速度
float acc_body[3] = {ax, ay, az};
// 旋转矩阵 R (由当前姿态得到)
float acc_world[3];
for(int i=0; i<3; i++) {
acc_world[i] = R[i][0]*acc_body[0] + R[i][1]*acc_body[1] + R[i][2]*acc_body[2];
}3.4 四元数：嵌入式工程师的“瑞士军刀”
四元数，说白了就是一个复数在三维空间的推广。它用四个数表示旋转：q = w + xi + yj + zk，其中 w 是实部，(x, y, z) 是虚部。
为什么我推荐用四元数？三个理由：
- 无万向锁：随便你怎么转，不会死锁。
- 计算量小：更新姿态只需要4个数的乘法，比旋转矩阵的9个数省多了。
- 插值平滑：做姿态融合时，球面线性插值（SLERP）非常自然。
四元数表示旋转的公式：
// 绕单位向量 (nx, ny, nz) 旋转角度 theta
q.w = cos(theta/2)
q.x = nx * sin(theta/2)
q.y = ny * sin(theta/2)
q.z = nz * sin(theta/2)q1 * q2 和 q2 * q1 结果不同。我刚开始写代码时，经常搞混旋转顺序，导致姿态乱跳。记住：先发生的旋转在右边，后发生的在左边。
3.5 它们之间的转换：一张表搞定
实际开发中，你需要在三种表示之间来回切换。比如：
- 从陀螺仪积分得到四元数增量
- 从加速度计/磁力计算出欧拉角，再转成四元数做融合
- 最终输出给蓝牙时，转成欧拉角方便上位机显示
下面是我常用的转换公式，你直接拿去用：
| 转换方向 | 公式/方法 |
|---|---|
| 欧拉角 → 四元数 | q = q_yaw(yaw) * q_pitch(pitch) * q_roll(roll) |
| 四元数 → 旋转矩阵 | R[0][0] = 1-2(y²+z²), R[0][1] = 2(xy-wz), ... |
| 旋转矩阵 → 四元数 | w = 0.5*sqrt(1+R[0][0]+R[1][1]+R[2][2]) |
| 四元数 → 欧拉角 | pitch = asin(2(w*y - z*x)), yaw = atan2(...) |
代码实现（C语言，四元数转欧拉角）：
void quat_to_euler(float q[4], float *yaw, float *pitch, float *roll) {
float w = q[0], x = q[1], y = q[2], z = q[3];
// 俯仰角
float sinp = 2.0f * (w * y - z * x);
if (fabs(sinp) >= 1.0f) {
*pitch = copysign(M_PI/2, sinp); // 处理万向锁
} else {
*pitch = asin(sinp);
}
// 偏航角
*yaw = atan2f(2.0f * (w * z + x * y), 1.0f - 2.0f * (y*y + z*z));
// 横滚角
*roll = atan2f(2.0f * (w * x + y * z), 1.0f - 2.0f * (x*x + y*y));
}atan2f 而不是 atan2，单精度浮点运算快很多。另外，fabs 判断万向锁时，别用 == 1.0，浮点比较要留余量，比如 > 0.9999f。
3.6 实战中的选择建议
最后，给你一个我自己的决策树：
- 做姿态解算（IMU融合）：用四元数。Mahony、Madgwick算法都是基于四元数的。
- 做坐标变换（向量旋转）：用旋转矩阵。虽然计算量大点，但逻辑清晰。
- 做用户界面显示：用欧拉角。人类能看懂，但记得在±90°附近做保护。
嗯，今天就先聊到这儿。下一章我们讲传感器标定，那可是个“坑”更多的活儿。到时候我拿几个真实案例给你看，保证让你少走弯路。