3. 刚体变换与齐次坐标：旋转矩阵、平移向量、齐次坐标矩阵、欧拉角与四元数

刚体变换，说白了就是物体在三维空间里「挪个地方」——不拉伸、不扭曲，只是换个位置和朝向。激光雷达每次扫描，得到的点云都在传感器自己的坐标系里。要想把这些点放到车体坐标系、或者全局地图里，就必须搞懂刚体变换。

我个人习惯把刚体变换拆成两部分看：旋转和平移。旋转决定朝向，平移决定位置。两者合在一起，就是一个完整的刚体变换。

旋转矩阵是个 3×3 的正交矩阵，行列式为 +1。它的每一列，代表旋转后的坐标轴在原坐标系下的方向向量。

举个例子，绕 Z 轴旋转 θ 角，旋转矩阵长这样：

R_z(θ) = [
  [cosθ, -sinθ, 0],
  [sinθ,  cosθ, 0],
  [0,      0,    1]
]

绕 X 轴和 Y 轴的类似，只是把 sin/cos 放到对应位置。我刚开始做标定时，经常把符号搞反——顺时针和逆时针混了。后来养成习惯：右手法则，拇指指向旋转轴正方向，四指弯曲方向就是正角度方向。

我的小技巧： 写旋转矩阵前，先在纸上画个坐标系，标出旋转方向。写代码时再对着检查一遍，能省很多调试时间。

平移向量 t = [t_x, t_y, t_z]^T，表示从源坐标系原点到目标坐标系原点的位移。注意方向——是「从 A 到 B」还是「B 在 A 中的坐标」？我见过不少同事在这上面栽跟头。

旋转加平移，完整的刚体变换公式：

p' = R * p + t

其中 p 是源坐标系下的点，p' 是目标坐标系下的点。R 是旋转矩阵，t 是平移向量。

避坑指南： 我曾经在项目里把平移方向搞反了，结果点云全部偏到马路对面。排查了整整一下午才发现——t 向量写成了负值。记住：公式是 p' = R*p + t，不是 p' = R*p - t。

每次都要写 R*p + t，有点啰嗦。而且旋转和平移是分开的，不方便连续变换。齐次坐标就是来解决这个问题的。

把三维点 p = [x, y, z]^T 扩展成四维齐次坐标 [x, y, z, 1]^T。然后构造一个 4×4 的齐次变换矩阵：

T = [
  [R_11, R_12, R_13, t_x],
  [R_21, R_22, R_23, t_y],
  [R_31, R_32, R_33, t_z],
  [0,    0,    0,    1  ]
]

这样，一次矩阵乘法就完成了旋转和平移：

p'_hom = T * p_hom

连续变换就更方便了。比如从激光雷达到车体，再到全局地图：

T_map_lidar = T_map_vehicle * T_vehicle_lidar

矩阵乘法顺序很重要——先右后左。我习惯从右往左读：先做激光雷达到车体，再做车体到地图。

  核心要点： 齐次变换矩阵的逆矩阵，就是反向变换。计算逆矩阵时，R 部分转置，t 部分取 -R^T * t。不用真的去求 4×4 矩阵的逆，那样太慢了。

欧拉角用三个角度描述旋转：绕 X 轴（roll）、绕 Y 轴（pitch）、绕 Z 轴（yaw）。直观好理解，但坑不少。

最大的问题是万向锁——当 pitch 接近 ±90° 时，roll 和 yaw 的旋转轴会重合，丢失一个自由度。我在做无人机标定时遇到过，飞机俯仰到 90° 时，航向角突然乱跳。

另一个问题是旋转顺序。常见的顺序有 ZYX、ZYZ、XYZ 等。不同顺序得到的欧拉角完全不同。ROS 里默认是 ZYX（先绕 Z，再绕 Y，最后绕 X）。

避坑指南： 我曾经在两个模块之间传递欧拉角，一个用 ZYX，一个用 XYZ，结果标定出来的外参完全不对。后来我强制团队：所有欧拉角必须标注旋转顺序，否则一律用四元数。

四元数 q = [w, x, y, z]^T，其中 w 是实部，x、y、z 是虚部。它没有万向锁问题，插值平滑，计算效率高。

单位四元数表示旋转：

q = [cos(θ/2), sin(θ/2)*u_x, sin(θ/2)*u_y, sin(θ/2)*u_z]^T

其中 u = [u_x, u_y, u_z]^T 是旋转轴单位向量，θ 是旋转角度。

用四元数旋转一个点 p：

p' = q * p * q_conjugate

这里 p 要写成纯四元数 [0, x, y, z]^T。q_conjugate 是 q 的共轭 [w, -x, -y, -z]^T。

我的习惯： 在代码里统一用四元数做内部计算，只在用户界面或日志输出时转成欧拉角。这样既避免了万向锁，又方便调试时直观理解。

下面这张图梳理了刚体变换的核心概念和它们之间的关系：

说了这么多，到底该用哪种？我根据实际项目经验，给几个建议：

  一句话总结： 内部计算用四元数，坐标转换用齐次矩阵，人机交互用欧拉角。各取所长，别死磕一种。