2. 刚体变换与李群李代数：三维空间刚体运动表示

各位同学，欢迎来到《感知融合与多传感器标定实战》的第一章。今天咱们聊点硬核的——三维空间里的刚体运动。说白了，就是搞清楚一个物体在三维空间里怎么转、怎么动。你想想看，自动驾驶里，车在跑、激光雷达在转、摄像头在晃，这些运动怎么用数学描述？嗯，这就是本章要解决的问题。

2.1 旋转矩阵：最直观的旋转表示

旋转矩阵，我习惯叫它“旋转的身份证”。一个3x3的矩阵，就能唯一确定一个旋转。它的性质很漂亮：行列式为+1，且是正交矩阵（转置等于逆）。

我在项目中遇到过一个问题：用旋转矩阵做连续旋转时，数值误差会累积，导致矩阵不再正交。这时候就得做“正交化”处理。嗯，这里要注意，千万别直接硬算，要用SVD分解来修正。

核心性质：

  • R ∈ SO(3)，即特殊正交群
  • RTR = I，det(R) = +1
  • 逆矩阵 = 转置矩阵，求逆就是求转置
// 旋转矩阵示例：绕Z轴旋转30度
double theta = 30.0 * M_PI / 180.0;
Eigen::Matrix3d R;
R << cos(theta), -sin(theta), 0,
      sin(theta),  cos(theta), 0,
      0,           0,          1;

2.2 旋转向量：紧凑的旋转表示

旋转矩阵有9个参数，但自由度只有3个。说白了，太冗余了。旋转向量就聪明多了——用一个三维向量表示旋转：方向代表旋转轴，模长代表旋转角度。

为什么需要它？我记得在做IMU预积分时，旋转向量比矩阵更省内存，而且插值也更方便。但要注意，旋转向量在角度为0时存在奇异性，这时候用四元数更安全。

个人经验：旋转向量到旋转矩阵的转换，用罗德里格斯公式。我建议你手推一遍这个公式，对理解指数映射很有帮助。

2.3 欧拉角：最符合直觉的表示

欧拉角就是“绕三个轴依次旋转”。比如无人机常用的偏航-俯仰-滚转（Yaw-Pitch-Roll）。

但欧拉角有个大坑——万向锁。我曾经在调试一个机械臂时，就因为欧拉角导致某个姿态下自由度丢失，差点把电机烧了。所以，在自动驾驶里，我几乎不用欧拉角做内部计算，只用来做可视化展示。

表示方法 优点 缺点
旋转矩阵 无奇异性，易组合 冗余，不紧凑
旋转向量 紧凑，直观 有奇异性
欧拉角 最符合直觉 万向锁，不唯一
四元数 无奇异性，易插值 不直观，需归一化

2.4 四元数：工程中的首选

四元数，说白了就是“带约束的复数”。一个单位四元数q = [w, x, y, z]，满足w²+x²+y²+z²=1。它没有奇异性，插值平滑，是SLAM和自动驾驶里的标配。

我个人习惯用Hamilton四元数（w在前）。注意，Eigen和ROS用的是这种约定，但有些库用JPL约定（w在后）。搞混了的话，旋转方向会完全相反。我曾经因为这个bug排查了整整两天...

// 四元数示例：绕Z轴旋转30度
Eigen::Quaterniond q;
q = Eigen::AngleAxisd(theta, Eigen::Vector3d::UnitZ());
// 或者直接构造
q.w() = cos(theta/2);
q.x() = 0;
q.y() = 0;
q.z() = sin(theta/2);

2.5 李群SO(3)与李代数so(3)

好了，前面都是热身。现在进入正题——李群与李代数。你可能会问：为什么要学这个？

因为优化！在传感器标定中，我们需要优化旋转矩阵。但旋转矩阵有约束（正交且行列式为1），直接优化很难。李代数so(3)是三维向量，没有约束，可以随便优化。优化完再映射回SO(3)就行。

核心关系：

  • SO(3)：旋转矩阵的集合，是群
  • so(3)：三维向量的集合，是李代数
  • 指数映射：so(3) → SO(3)，即旋转向量 → 旋转矩阵
  • 对数映射：SO(3) → so(3)，即旋转矩阵 → 旋转向量

指数映射的公式就是罗德里格斯公式。我建议你记住这个：exp(φ^) = I + sinθ/θ * φ^ + (1-cosθ)/θ² * (φ^)²。其中φ是旋转向量，θ是它的模长。

2.6 SE(3)与se(3)：带上平移

SE(3)是刚体变换的集合，包含旋转和平移。它的李代数se(3)是六维向量：前三维是平移，后三维是旋转。

在实际标定中，我们经常用SE(3)来表示传感器之间的外参。比如激光雷达和摄像头之间的变换矩阵T = [R | t]。优化时，我们就在se(3)上做扰动，然后通过指数映射更新T。

避坑指南：我曾经在优化外参时，直接对旋转矩阵做加法更新，结果矩阵很快就不正交了。后来改用李代数扰动模型，问题迎刃而解。记住：永远不要在流形上做加法，要在切空间上做加法。

2.7 指数映射与对数映射的实现

最后，我们看看代码里怎么实现。Eigen库已经封装好了，但理解原理很重要。

// 指数映射：so(3) → SO(3)
Eigen::Vector3d phi(0.1, 0.2, 0.3);  // 旋转向量
Eigen::Matrix3d R = Eigen::AngleAxisd(phi.norm(), phi.normalized()).toRotationMatrix();

// 对数映射：SO(3) → so(3)
Eigen::AngleAxisd angle_axis(R);
Eigen::Vector3d phi_back = angle_axis.angle() * angle_axis.axis();

嗯，这里要注意：当旋转角度接近0时，对数映射的数值稳定性会变差。我一般会加一个阈值判断，如果角度小于1e-6，直接返回零向量。

个人建议：刚开始学李群李代数时，别被数学公式吓到。你只需要记住三件事：

  1. so(3)是旋转向量，可以自由优化
  2. 指数映射把旋转向量变成旋转矩阵
  3. 优化时用李代数，更新后用指数映射回到李群

就这么简单。剩下的细节，用到的时候再查。

好了，这一章的内容就到这里。下一章我们会讲多传感器的时间同步，那又是一个大坑。各位先把刚体变换练熟，后面标定的时候会轻松很多。

← 上一章 📖 返回目录 下一章 →