1. 坐标系基础：刚体运动与旋转矩阵、欧拉角、四元数、齐次坐标与变换矩阵
大家好，我是你们的老朋友。今天咱们来聊聊多传感器融合里最基础、也最绕不开的话题——坐标系。
说实话，我入行头两年，踩的最多的坑就是坐标系搞混了。有一次做激光雷达和相机的联合标定，折腾了两周才发现是旋转矩阵的符号写反了。嗯，从那以后，我对这部分内容就格外上心。
你想想看，一个机器人身上可能同时装着IMU、相机、激光雷达、GPS。每个传感器都有自己的坐标系。怎么让它们「说同一种语言」？这就是我们今天要解决的问题。
1.1 刚体运动：说白了就是「不形变」
刚体运动，顾名思义，物体在运动过程中保持形状不变。你推一把桌子，桌子整体移动了，但桌腿和桌面之间的相对位置没变——这就是刚体运动。
在数学上，刚体运动可以分解为两部分：旋转 + 平移。旋转描述的是朝向变化，平移描述的是位置变化。
核心公式：
p' = R · p + t
其中 p 是原始点坐标，R 是旋转矩阵，t 是平移向量，p' 是变换后的点。
我个人习惯把旋转和平移分开处理。为什么？因为旋转涉及的东西比平移复杂得多。平移就是三个数（x, y, z），简单明了。旋转嘛……咱们下面慢慢聊。
1.2 旋转矩阵：最直观，但最啰嗦
旋转矩阵是一个 3×3 的正交矩阵，行列式为 +1。它把三维空间中的一个向量，旋转到另一个方向。
举个例子，绕 Z 轴旋转 θ 角度的旋转矩阵长这样：
R_z(θ) = [cosθ -sinθ 0]
[sinθ cosθ 0]
[ 0 0 1]绕 X 轴和 Y 轴的类似，只是把 sin/cos 放到对应的位置。
旋转矩阵的优点：
- 没有奇异性——任何旋转都能表示
- 组合方便——两个旋转矩阵相乘就是复合旋转
- 数学性质好——可逆，逆矩阵就是转置
缺点也很明显：
- 9 个参数，但只有 3 个自由度，冗余严重
- 必须满足正交约束，数值计算中容易漂移
- 不直观——你看着一堆 sin/cos，很难想象出旋转后的样子
我的经验：在代码里，我一般只用旋转矩阵做中间计算。比如把多个旋转合成一个，或者把旋转应用到大量点上。但让我手写一个旋转矩阵？我宁愿用四元数或者欧拉角生成，再转成矩阵。
1.3 欧拉角：直观但暗藏杀机
欧拉角用三个角度来描述旋转：偏航（Yaw）、俯仰（Pitch）、横滚（Roll）。
你想想看，飞机转弯时先转方向（Yaw），再抬头（Pitch），最后侧倾（Roll）。是不是很符合直觉？
但欧拉角有个著名的坑——万向锁（Gimbal Lock）。
我曾经踩过的坑：有一次做无人机姿态估计，用欧拉角做插值。飞到某个角度时，Yaw 和 Roll 突然「打架」，一个转 180°，另一个也转 180°，结果无人机直接翻了个跟头。后来查了半天，发现是 Pitch 接近 90° 时触发了万向锁。
万向锁的本质是：当第二个旋转轴的角度为 ±90° 时，第一个和第三个旋转轴变得共线，丢失了一个自由度。
欧拉角的优缺点总结：
| 优点 | 缺点 |
|---|---|
| 直观，三个角度容易理解 | 存在万向锁问题 |
| 参数少，存储方便 | 旋转顺序依赖（ZYX、XYZ 等不同顺序结果不同） |
| 适合人机交互 | 插值不自然，不能直接做平滑过渡 |
我个人建议：欧拉角只用来做「显示」和「人工输入」。内部计算，还是用四元数或者旋转矩阵更靠谱。
1.4 四元数：SLAM 界的「真香」选择
四元数是什么？说白了就是一个四维的复数扩展：q = w + xi + yj + zk，其中 w 是实部，x、y、z 是虚部，满足 i² = j² = k² = ijk = -1。
听起来有点抽象？没关系，你只需要记住：单位四元数可以表示三维空间中的任意旋转。
一个旋转轴为 u（单位向量）、旋转角度为 θ 的旋转，对应的四元数是：
q = [cos(θ/2), u·sin(θ/2)]
= [w, x, y, z]四元数的好处：
- 没有万向锁——这是最大的优势
- 插值平滑——可以用 Slerp（球面线性插值）做平滑过渡
- 计算高效——比旋转矩阵少 3 个参数，乘法也更快
- 数值稳定——归一化就能保持单位性，不会漂移
我个人的习惯：在 SLAM 系统里，状态向量中的旋转部分我全部用四元数表示。优化的时候再用四元数的扰动模型（左乘或右乘），避免过参数化带来的问题。
四元数乘法也很有意思。两个四元数 q₁ 和 q₂ 相乘，表示先做 q₁ 的旋转，再做 q₂ 的旋转：
q = q₂ ⊗ q₁注意顺序！和矩阵一样，四元数乘法也不可交换。
1.5 齐次坐标与变换矩阵：把旋转和平移装进一个「盒子」
前面我们说了，刚体运动是 p' = R·p + t。但如果你有一连串的变换要做，比如从相机坐标系到机器人坐标系，再到世界坐标系，每次都写一个 R·p + t 会很麻烦。
齐次坐标就是来解决这个问题的。它把一个三维点 (x, y, z) 扩展成四维 (x, y, z, 1)，然后旋转和平移就可以合并成一个 4×4 的变换矩阵：
T = [R t]
[0 1]
p' = T · p这样一来，多个变换就可以直接矩阵相乘：T_total = T₃ · T₂ · T₁。
齐次坐标的好处：
- 统一了旋转和平移——一个矩阵搞定所有
- 支持链式变换——矩阵乘法天然支持复合
- 逆变换简单——求逆矩阵即可
一个小技巧：在代码里，我习惯把变换矩阵写成 Eigen::Isometry3d 或者 Sophus::SE3。这些库会自动处理齐次坐标的细节，你只需要关心旋转和平移的部分。
1.6 四种表示方式的对比
| 表示方式 | 参数数量 | 奇异性 | 插值 | 计算效率 | 直观性 |
|---|---|---|---|---|---|
| 旋转矩阵 | 9 | 无 | 困难 | 中等 | 低 |
| 欧拉角 | 3 | 有（万向锁） | 不自然 | 高 | 高 |
| 四元数 | 4 | 无 | 平滑（Slerp） | 高 | 中 |
| 齐次变换矩阵 | 16 | 无 | 困难 | 低 | 中 |
看到这个表格，你可能会问：那我到底该用哪个？
我的建议是：
- 做优化、滤波、SLAM 后端 → 用四元数（或者李代数，那是后话了）
- 做坐标变换链、TF 树 → 用齐次变换矩阵
- 给人看、手动调参 → 用欧拉角（但小心万向锁）
- 做中间计算、合成旋转 → 用旋转矩阵
嗯，这就是坐标系基础的全部内容了。说白了，刚体运动描述就是这四种工具，各有各的脾气。你在项目里用多了，自然就知道什么时候该拿哪把扳手。
可可精品资料库