刚体运动学基础：旋转矩阵、平移向量、齐次坐标与SE(3)变换

各位同学，今天我们聊聊刚体运动学。说实话，这是整个自车运动补偿的数学地基。你想想看，一辆车在路上跑，它怎么转、怎么动，我们怎么用数学去描述它？

我个人习惯把刚体运动拆成两件事：旋转和平移。旋转用矩阵，平移用向量，合起来就是SE(3)变换。嗯，就是这么简单。

旋转矩阵：别被名字吓到

旋转矩阵，说白了就是一个3x3的方阵。它描述了一个坐标系相对于另一个坐标系的朝向。比如，车身坐标系相对于世界坐标系怎么转的。

我记得刚入行那会儿，总觉得旋转矩阵很抽象。后来在项目里做激光雷达和相机的联合标定，天天跟它打交道，慢慢就熟了。

旋转矩阵有几个关键性质：

• 正交性：R^TR = I，矩阵的逆等于它的转置
• 行列式为+1：保证是纯旋转，没有缩放或镜像
• 三个自由度：虽然9个元素，但只有3个是独立的

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

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

为什么是Z轴？因为自动驾驶里，车身的Z轴通常指向上方，绕Z轴旋转就是车在水平面上的转向。

平移向量：简单但容易出错

平移向量就简单多了。一个3x1的向量，表示两个坐标系原点之间的位移。

比如，激光雷达装在车顶，它的坐标系原点相对于车身坐标系原点的偏移，就是一个平移向量。

我曾经在项目里犯过一个低级错误：平移向量的单位搞混了。激光雷达给的数据是毫米，车身坐标系用的是米，结果算出来的位置差了1000倍。嗯，排查了半天才发现。

齐次坐标：为什么需要它？

你想想看，旋转用矩阵乘法，平移用向量加法。如果先旋转再平移，就得写两步：

p' = R * p + t

这写法没问题，但如果你要做连续变换，比如从激光雷达到车身，再从车身到世界，就得嵌套好几层。代码写起来又长又容易错。

齐次坐标就是来解决这个问题的。它把一个3D点(x, y, z)扩展成4D向量(x, y, z, 1)。然后旋转和平移就可以合并成一个4x4的矩阵：

T = [
  [R, t],
  [0, 1]
]

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

p'_hom = T * p_hom

说白了，齐次坐标就是把「旋转+平移」打包成一个操作。代码里写起来特别清爽。

SE(3)变换：刚体运动的完整描述

SE(3)是Special Euclidean Group的缩写，翻译过来就是「特殊欧几里得群」。别被「群」这个数学概念吓到，你只需要知道：SE(3)就是所有刚体变换的集合。

一个SE(3)变换矩阵T，由两部分组成：

• 左上角3x3：旋转矩阵R
• 右上角3x1：平移向量t
• 最后一行固定为[0, 0, 0, 1]

为什么叫「特殊」？因为旋转矩阵的行列式必须是+1，不能是-1（-1代表镜像，现实中刚体不能镜像）。

我在做多传感器融合时，经常需要把不同传感器的数据统一到同一个坐标系下。每个传感器都有自己的SE(3)变换参数，标定就是求这些参数。

Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.rotate(R);
T.pretranslate(t);

变换的复合：链式法则

实际项目中，很少只有一个变换。比如：

1. 激光雷达点云 → 车身坐标系：T_lidar_to_vehicle
2. 车身坐标系 → 世界坐标系：T_vehicle_to_world
3. 最终点云在世界坐标系下：T_world = T_vehicle_to_world * T_lidar_to_vehicle

注意顺序！矩阵乘法不满足交换律。先乘哪个后乘哪个，取决于你的变换链是怎么定义的。

我建议在代码里统一用「左乘」或者「右乘」的约定，别混着用。我个人习惯用左乘：新变换加在最左边。

逆变换：从世界回到局部

有时候我们需要反过来，从世界坐标系变换回车身坐标系。这时候就需要求SE(3)的逆。

SE(3)的逆有简洁的公式：

T_inv = [
  [R^T,  -R^T * t],
  [0,        1    ]
]

你看，旋转部分直接转置，平移部分要乘上负号再转置。这个公式我建议你记下来，手写代码时经常用到。

总结一下

刚体运动学这块，说白了就是三件事：

• 旋转用旋转矩阵R，记住正交性和行列式为+1
• 平移用平移向量t，注意单位统一
• 合起来用SE(3)变换矩阵T，齐次坐标让计算更简洁

下一章我们会聊李代数，那是SE(3)的「导数」版本，用来做优化和插值。嗯，到时候再细说。

对了，如果你在项目里遇到变换矩阵搞不清楚的情况，可以试试画个坐标系图，把每个变换标出来。我每次做标定都这么干，思路会清晰很多。