3、刚体变换与齐次坐标：旋转矩阵、平移向量、齐次坐标表示法、欧拉角与四元数
多传感器融合，说白了就是让不同传感器看到的世界「对齐」。激光雷达说「前方3米有障碍物」，摄像头说「像素坐标(500, 300)处有车」，毫米波雷达说「目标距离2.8米，速度5m/s」。这些数据怎么统一到同一个坐标系下？
答案就是刚体变换与齐次坐标。这是感知融合的基石，也是我入行时啃得最久的一块硬骨头。今天咱们就把这块骨头彻底嚼碎。
核心思想：刚体变换 = 旋转 + 平移。齐次坐标 = 用4维向量表示3维点，让变换变成矩阵乘法。
3.1 旋转矩阵：姿态的数学表达
旋转矩阵，说白了就是一个3x3的方阵。它描述了一个坐标系相对于另一个坐标系的朝向。我刚开始接触时总觉得这东西很抽象，直到我在项目里手动标定激光雷达和摄像头——你想想看，激光雷达扫出来的点云是3D的，摄像头拍到的图像是2D的，怎么对应？
答案就是旋转矩阵。
一个标准的旋转矩阵R满足两个性质：
- 正交性：RTR = I，即矩阵的逆等于它的转置
- 行列式为1：det(R) = 1
绕三个坐标轴的旋转矩阵长这样：
// 绕X轴旋转角度θ
Rx(θ) = [1, 0, 0;
0, cosθ, -sinθ;
0, sinθ, cosθ]
// 绕Y轴旋转角度θ
Ry(θ) = [cosθ, 0, sinθ;
0, 1, 0;
-sinθ, 0, cosθ]
// 绕Z轴旋转角度θ
Rz(θ) = [cosθ, -sinθ, 0;
sinθ, cosθ, 0;
0, 0, 1]我的经验：在实际项目中，我习惯用Z-Y-X顺序的欧拉角来构建旋转矩阵。为什么？因为大多数车辆的运动都是先绕Z轴转（航向角），再绕Y轴转（俯仰角），最后绕X轴转（横滚角）。这个顺序和车辆动力学天然匹配。
3.2 平移向量：位置的偏移
旋转搞定了朝向，平移解决位置。平移向量t就是一个3x1的列向量，表示两个坐标系原点之间的位移。
假设点P在坐标系A中的坐标为PA，在坐标系B中的坐标为PB，那么：
P_A = R * P_B + t这里R是B到A的旋转矩阵，t是B的原点在A中的坐标。
注意：我曾经在项目里犯过一个低级错误——把旋转和平移的顺序搞反了。正确的顺序是：先旋转，后平移。如果你先平移再旋转，结果会完全错误。嗯，那次debug花了我整整一个下午。
3.3 齐次坐标表示法：优雅的数学工具
你发现没有？上面的公式PA = R * PB + t，既有乘法又有加法，写起来麻烦，组合起来更麻烦。如果有多个传感器串联变换，公式会变得又臭又长。
齐次坐标就是来解决这个问题的。
齐次坐标的核心思想：给3D点加一个维度，变成4D向量。具体来说：
// 3D点 (x, y, z) 的齐次坐标表示为 (x, y, z, 1)
// 3D向量 (dx, dy, dz) 的齐次坐标表示为 (dx, dy, dz, 0)
// 变换矩阵变成4x4：
T = [R t;
0^T 1]
// 变换公式简化为：
P_A_homo = T * P_B_homo你看，一个矩阵乘法就搞定了旋转和平移。这就是齐次坐标的魔力。
为什么用齐次坐标？三个理由：1）统一旋转和平移为矩阵乘法；2）方便串联多个变换（矩阵连乘）；3）可以表示无穷远点（w=0时）。
3.4 欧拉角：直观但不完美
欧拉角用三个角度来描述旋转：横滚角(roll)、俯仰角(pitch)、航向角(yaw)。直观，好理解，但有个大坑——万向锁。
什么是万向锁？当俯仰角达到±90°时，横滚和航向会失去一个自由度，变成同一个轴上的旋转。我在做无人机感知时遇到过这个问题——飞机垂直向下看时，航向角和横滚角突然变得无法区分，姿态估计直接崩了。
欧拉角的另一个问题是：它不连续。在角度接近±180°时，很小的姿态变化会导致角度值跳变。这对滤波算法非常不友好。
| 表示方法 | 优点 | 缺点 |
|---|---|---|
| 欧拉角 | 直观，3个参数 | 万向锁，不连续 |
| 旋转矩阵 | 无奇异性，可组合 | 9个参数，冗余 |
| 四元数 | 无奇异性，4个参数，可插值 | 不够直观 |
3.5 四元数：工程中的最优解
四元数，说白了就是「带约束的复数」。一个四元数q = w + xi + yj + zk，满足w² + x² + y² + z² = 1。
为什么工程中都用四元数？我总结了三个原因：
- 无万向锁：这是最大的优势，姿态估计永远不会崩
- 便于插值：SLERP插值可以平滑过渡，做摄像头和IMU的时间对齐时特别好用
- 计算效率高：比旋转矩阵少5个参数，乘法运算更快
四元数和旋转矩阵的转换关系：
// 四元数 q = (w, x, y, z) 转旋转矩阵
R = [1-2y²-2z², 2xy-2zw, 2xz+2yw;
2xy+2zw, 1-2x²-2z², 2yz-2xw;
2xz-2yw, 2yz+2xw, 1-2x²-2y²]
// 旋转矩阵转四元数（取正根）
w = sqrt(1 + R[0][0] + R[1][1] + R[2][2]) / 2
x = (R[2][1] - R[1][2]) / (4w)
y = (R[0][2] - R[2][0]) / (4w)
z = (R[1][0] - R[0][1]) / (4w)我的建议：在感知融合项目中，我习惯用四元数做内部计算，用欧拉角做可视化调试。内部计算用四元数保证稳定，调试时转成欧拉角方便理解。这个习惯帮我避免了很多坑。
3.6 实战：多传感器外参标定
理论讲完了，咱们看看实际怎么用。假设我们要标定激光雷达和摄像头的外参：
- 数据采集：找一个标定板，同时采集激光点云和图像
- 特征提取：在点云中提取标定板的角点，在图像中提取对应的像素点
- 构建方程：利用3D-2D对应关系，构建PnP问题
- 求解变换：用SVD或非线性优化求解R和t
- 验证：将点云投影到图像上，检查对齐精度
我记得有一次，标定出来的结果总是差那么几厘米。排查了半天，发现是标定板的厚度没考虑进去。嗯，这种细节问题，只有踩过坑才会注意。
避坑指南：我曾经在标定时用了错误的旋转顺序，导致标定结果在特定角度下误差很大。后来我养成了一个习惯——每次标定完，都要做一次「闭环验证」：把点云投影到图像上，肉眼检查边缘对齐情况。别太相信数值结果，视觉检查往往能发现隐藏的问题。
好了，刚体变换和齐次坐标就讲到这里。这些工具是感知融合的「语法」，掌握了它们，你就能读懂传感器之间的「对话」。下一节我们会深入时间同步的话题——毕竟，空间对齐了，时间也得对齐才行。