3. 坐标变换与空间描述：刚体运动、旋转矩阵、四元数、欧拉角、齐次变换矩阵
各位同学，欢迎来到第三章。这一章，我们聊聊传感器融合里最基础、也最绕不开的话题——坐标变换与空间描述。
说实话，我刚开始做多传感器融合的时候，觉得这玩意儿不就是矩阵乘来乘去嘛，有啥难的？结果第一次把IMU和视觉数据对齐，发现姿态怎么都对不上，折腾了两天才找到原因——欧拉角的万向锁问题。嗯，从那以后，我再也不敢小看坐标变换了。
你想想看，一个IMU装在无人机中心，一个摄像头装在机头，一个激光雷达装在机腹。它们各自看世界，坐标系都不一样。你要把数据融合到一起，就得先搞清楚：怎么描述一个物体在空间里的位置和姿态？怎么在不同坐标系之间来回切换？
这就是本章要解决的核心问题。
核心要点：刚体运动 = 位置（平移）+ 姿态（旋转）。描述姿态有三种主流方式：旋转矩阵、欧拉角、四元数。齐次变换矩阵把平移和旋转统一成一个4×4矩阵，方便连续变换。
3.1 刚体运动：位置 + 姿态
先问个问题：什么叫刚体？说白了，就是一个物体在运动过程中，内部任意两点之间的距离保持不变。比如你手里的手机、无人机机身、机器人手臂——这些都可以近似看作刚体。
刚体在三维空间里的运动，可以拆成两部分：
- 平移（位置）：刚体上某一点（通常取质心或原点）在空间中的坐标变化。用三维向量 t = [x, y, z]^T 表示。
- 旋转（姿态）：刚体绕自身坐标轴的转动。描述起来就比平移复杂多了。
我记得有个学生问我：老师，为什么不能直接用三个角度描述姿态，非要搞那么多方法？我的回答是：因为每种方法都有它的脾气。下面我们一个一个来看。
3.2 旋转矩阵：最直观，但最啰嗦
旋转矩阵是一个3×3的正交矩阵，行列式为+1。它描述了一个坐标系相对于另一个坐标系的旋转关系。
假设有一个点 p 在坐标系 {A} 中的坐标为 p_A，在坐标系 {B} 中的坐标为 p_B。如果 {B} 相对于 {A} 的旋转矩阵是 R_AB，那么：
p_A = R_AB * p_B旋转矩阵的每一列，其实就是 {B} 坐标系的基向量在 {A} 坐标系下的投影。这个理解很重要，我在做视觉-惯性标定时，就是靠这个物理意义来检查标定结果是否合理。
旋转矩阵有9个元素，但自由度只有3。说白了，它冗余了。好处是直观、容易组合（矩阵乘法），坏处是计算量大，而且数值误差会让它不再是正交矩阵——需要不断重新正交化。
我的习惯：在需要多次连续旋转、或者要跟平移统一处理时，我用旋转矩阵。比如机器人运动学正解，用旋转矩阵链式乘法非常自然。但在做姿态估计、滤波时，我更喜欢四元数。
3.3 欧拉角：直观，但小心万向锁
欧拉角用三个角度来描述旋转：绕X轴（滚转roll）、绕Y轴（俯仰pitch）、绕Z轴（偏航yaw）。顺序很重要！常见的顺序有ZYX、ZYZ等。
举个例子，无人机姿态通常用roll-pitch-yaw（ZYX顺序）：
- 偏航角 ψ：绕Z轴旋转
- 俯仰角 θ：绕Y轴旋转
- 滚转角 φ：绕X轴旋转
欧拉角最大的优点是直观。你跟飞手说“偏航30度”，他马上能理解。但缺点也很致命——万向锁。
⚠️ 我曾经踩过的坑：在做无人机姿态控制时，俯仰角接近±90度时，偏航和滚转变得不可区分，控制直接炸了。这就是万向锁——中间那个轴（俯仰）转到±90度时，内外两个轴共面，丢失了一个自由度。
所以，如果你用欧拉角做姿态估计或控制，一定要限制俯仰角范围，或者干脆换四元数。
3.4 四元数：无奇点，适合插值
四元数是个好东西。它用四个数表示旋转：q = [w, x, y, z]^T，其中w是实部，[x, y, z]是虚部，且满足 w² + x² + y² + z² = 1（单位四元数）。
为什么用四元数？三个理由：
- 无万向锁：这是最大的优势。任何姿态都能用四元数唯一表示。
- 插值平滑：球面线性插值（SLERP）可以在两个姿态之间生成平滑过渡。做动画、做轨迹规划时特别好用。
- 计算效率高：比旋转矩阵少一个维度，组合旋转用四元数乘法也比矩阵乘法快。
四元数旋转公式（点 p 绕单位四元数 q 旋转）：
p' = q * p * q_conj其中 p 是纯四元数 [0, x, y, z]^T，q_conj 是 q 的共轭。
我个人习惯在卡尔曼滤波的状态向量里用四元数表示姿态。但要注意——四元数必须归一化，否则滤波会发散。我见过有人忘了这步，结果姿态越跑越偏。
3.5 齐次变换矩阵：把平移和旋转装进一个矩阵
好了，现在我们有旋转（用矩阵或四元数），有平移（用向量）。能不能把它们统一起来？
能。齐次变换矩阵就是一个4×4的矩阵：
T = [ R t ]
[ 0 1 ]其中 R 是3×3旋转矩阵，t 是3×1平移向量，最后一行是 [0, 0, 0, 1]。
用齐次坐标，一个点 p 的齐次表示为 [x, y, z, 1]^T。变换就是简单的矩阵乘法：
p_A = T_AB * p_B连续变换可以链式相乘：
T_AC = T_AB * T_BC这个太方便了。我在做多传感器外参标定时，就是把每个传感器相对于车体的变换矩阵写出来，然后任意两个传感器之间的变换就是矩阵乘法的结果。
实用技巧：齐次变换矩阵的逆也很好求——不需要真的求4×4矩阵的逆。如果 T = [R t; 0 1]，那么 T^{-1} = [R^T -R^T * t; 0 1]。这个在代码里直接写，比调用通用求逆函数快得多。
3.6 三种姿态表示方法的对比
| 特性 | 旋转矩阵 | 欧拉角 | 四元数 |
|---|---|---|---|
| 参数数量 | 9（冗余） | 3（最小） | 4（约束1个） |
| 直观性 | 中等 | 高 | 低 |
| 万向锁 | 无 | 有 | 无 |
| 组合旋转 | 矩阵乘法 | 复杂 | 四元数乘法 |
| 插值 | 困难 | 困难 | SLERP平滑 |
| 数值稳定性 | 需重新正交化 | 一般 | 需归一化 |
| 常用场景 | 机器人运动学 | 人机交互、飞行控制 | 姿态估计、滤波、动画 |
3.7 实战：坐标系变换的代码骨架
下面给出一段C++风格的伪代码，展示如何用齐次变换矩阵做坐标系变换。实际项目中，我建议用Eigen或Sophus库，但理解底层逻辑更重要。
// 定义旋转矩阵（绕Z轴旋转30度）
double theta = 30.0 * M_PI / 180.0;
Matrix3d R;
R << cos(theta), -sin(theta), 0,
sin(theta), cos(theta), 0,
0, 0, 1;
// 定义平移向量
Vector3d t(1.0, 2.0, 0.5);
// 构建齐次变换矩阵
Matrix4d T = Matrix4d::Identity();
T.block<3,3>(0,0) = R;
T.block<3,1>(0,3) = t;
// 点p在坐标系{B}中的坐标
Vector4d p_B(0.5, -0.3, 1.2, 1.0);
// 变换到坐标系{A}
Vector4d p_A = T * p_B;
// 求逆变换（快速方法）
Matrix4d T_inv = Matrix4d::Identity();
T_inv.block<3,3>(0,0) = R.transpose();
T_inv.block<3,1>(0,3) = -R.transpose() * t;避坑指南：写代码时，一定要搞清楚变换的方向。T_AB 表示从 {B} 到 {A} 的变换，还是从 {A} 到 {B}？我建议统一约定：T_AB 表示将 {B} 中的点变换到 {A} 中。这样链式乘法 T_AC = T_AB * T_BC 就非常自然。
3.8 本章小结
这一章我们聊了刚体运动的两种成分——平移和旋转。旋转有三种主流描述方式：
- 旋转矩阵：直观但冗余，适合链式变换
- 欧拉角：人机友好，但要小心万向锁
- 四元数：无奇点、适合插值和滤波，我的首选
齐次变换矩阵把平移和旋转统一成4×4矩阵，是传感器融合中坐标对齐的利器。记住：坐标系变换不是数学游戏，而是让不同传感器的数据在同一个参考系下对话。