3. 刚体旋转基础：旋转的物理意义、坐标系定义与DCM-四元数转换
各位同学，欢迎来到第三章。这一章，我们聊聊刚体旋转的基础。
说实话，很多做惯导的工程师，最后栽跟头的地方，往往不是卡尔曼滤波有多复杂，而是最基础的旋转概念没搞透。我自己刚入行那会儿，就吃过这个亏——坐标系搞反了，结果解算出来的姿态角，飞机明明是抬头，数据显示却在俯冲。嗯，那场面，挺尴尬的。
3.1 旋转的物理意义：到底什么是“转”？
我们说的“旋转”，不是数学游戏。它描述的是一个刚体，在三维空间里，绕某个轴转了多少角度。
你想想看，一个物体从A姿态到B姿态，中间发生了什么？
- 旋转轴：绕哪根线转？
- 旋转角：转了多少度？
- 旋转顺序：先绕X还是先绕Y？
这三个要素，缺一不可。我见过不少新手，只给角度不给顺序，结果代码跑出来天差地别。记住：旋转顺序不是约定俗成的，是必须明确指定的。
核心理解：旋转的本质，是坐标变换。不是物体在动，而是我们描述物体的“参考系”在变。
3.2 坐标系定义：导航系与机体系
做姿态解算，你手里至少要有两把“尺子”。
3.2.1 导航坐标系（n系）
导航系，也叫“世界系”。它是不动的，或者说是相对地球固定的。我们常用的是“北-东-地”（NED）或者“东-北-天”（ENU）。
- NED：X轴指北，Y轴指东，Z轴指向地心。我个人习惯用NED，因为和大多数飞控的默认设置一致。
- ENU：X轴指东，Y轴指北，Z轴指天。GPS输出的速度通常就是ENU。
这里有个坑：千万别混用。我曾经在项目里，把NED的加速度计数据和ENU的GPS数据直接融合，结果位置解算直接飞了。嗯，查了两天才发现是坐标系没对齐。
3.2.2 机体坐标系（b系）
机体系，是固定在飞行器上的。原点在重心，X轴指向机头，Y轴指向右翼，Z轴指向下（符合右手定则）。
你想想看，IMU（惯性测量单元）输出的加速度和角速度，都是在机体系下的。而我们想要知道的俯仰、横滚、航向，是相对于导航系的。所以，所有姿态解算的核心，就是求这两个坐标系之间的旋转关系。
个人经验：在代码里，我建议用枚举类型把坐标系定义死。比如 typedef enum {NED, ENU, BODY} CoordFrame_t;。这样每次调用函数，都强制传入坐标系参数，能避免很多低级错误。
3.3 方向余弦矩阵（DCM）与四元数的转换
好了，现在我们知道需要描述两个坐标系之间的旋转。怎么描述？有两种主流方式：DCM和四元数。
3.3.1 方向余弦矩阵（DCM）
DCM，说白了就是一个3x3的矩阵。它的每一列，是机体系的一个轴在导航系下的投影。
比如，DCM的第一列，就是机体系X轴在导航系下的方向余弦。所以叫“方向余弦矩阵”。
优点：直观。你一眼就能看出每个轴指向哪。
缺点：有9个参数，冗余。而且随着时间推移，矩阵会慢慢“不正交”，需要不断修正。我在早期项目里用过DCM，每次更新完都要做正交化，挺麻烦的。
3.3.2 四元数
四元数，是一个超复数。形式是 q = w + xi + yj + zk，其中 w 是实部，x, y, z 是虚部。
它用4个参数，就描述了三维旋转。没有冗余，不会出现“非正交”的问题。而且，四元数乘法比矩阵乘法快得多——这对嵌入式平台来说，太重要了。
为什么用四元数？ 因为它在计算效率和数值稳定性上，完胜DCM。现在的MEMS IMU姿态解算，几乎清一色用四元数。
3.3.3 DCM与四元数的转换
虽然我们用四元数做解算，但最终输出给飞控或者显示器的，往往是欧拉角。而欧拉角从DCM提取最方便。所以，四元数 -> DCM -> 欧拉角 这条链路，你必须烂熟于心。
下面给出转换公式。记住，这些公式我建议你直接写成函数，不要每次手算。
四元数转DCM：
// 输入：四元数 q = [w, x, y, z]
// 输出：DCM 矩阵 C (3x3)
// 公式来源：S. L. Altmann, "Rotations, Quaternions, and Double Groups"
C[0][0] = 1 - 2*(y*y + z*z);
C[0][1] = 2*(x*y - w*z);
C[0][2] = 2*(x*z + w*y);
C[1][0] = 2*(x*y + w*z);
C[1][1] = 1 - 2*(x*x + z*z);
C[1][2] = 2*(y*z - w*x);
C[2][0] = 2*(x*z - w*y);
C[2][1] = 2*(y*z + w*x);
C[2][2] = 1 - 2*(x*x + y*y);DCM转四元数：
// 输入：DCM 矩阵 C (3x3)
// 输出：四元数 q = [w, x, y, z]
// 注意：需要保证矩阵是正交的，否则结果会有误差
float trace = C[0][0] + C[1][1] + C[2][2];
if (trace > 0) {
float S = 0.5f / sqrtf(trace + 1.0f);
w = 0.25f / S;
x = (C[2][1] - C[1][2]) * S;
y = (C[0][2] - C[2][0]) * S;
z = (C[1][0] - C[0][1]) * S;
} else {
// 处理特殊情况，比如 trace <= 0 时，需要取最大对角元
// 这里省略分支代码，实际项目中必须完整实现
}避坑指南：我曾经在DCM转四元数时，没处理 trace <= 0 的情况。结果在特定姿态下，四元数突然跳变，导致姿态解算发散。记住：这个分支不是可选的，是必须的。
3.4 知识体系结构图
下面这张图，是我自己总结的。它把本章的核心逻辑串起来了。你仔细看一遍，应该能明白：旋转的物理意义 -> 坐标系定义 -> 数学表达（DCM/四元数） -> 相互转换，这条线是贯通的。
这张图里，从上到下，就是我们从物理世界到数学表达，再到工程实现的完整路径。你每次写代码前，不妨先看看这张图，确认自己当前在哪个环节。
3.5 本章小结
这一章，我们搞清楚了：
- 旋转不是玄学，是轴、角、顺序的组合。
- 导航系和机体系，是两把尺子，不能混用。
- DCM直观但冗余，四元数高效且稳定。
- 两者之间的转换，是姿态解算的“桥梁”，必须写对。
嗯，基础打牢了，后面讲四元数更新、姿态融合，你才能跟得上。记住：地基不稳，楼盖得再高也是危楼。