3. 坐标系与刚体变换：世界坐标系、机体坐标系、传感器坐标系、欧拉角与四元数、李群与李代数基础
各位同学，咱们今天聊点硬核的。坐标系与刚体变换，说白了就是解决一个核心问题：如何描述一个物体在空间中的位置和朝向。
我刚开始做多传感器融合那会儿，觉得这玩意儿不就是几个坐标轴嘛，有啥好学的？结果第一次做IMU+视觉融合，坐标系没对齐，跑出来的轨迹直接飞到了外太空。嗯，从那以后，我再也不敢小看这一章了。
3.1 世界坐标系与机体坐标系
先说说最基础的两个坐标系。
世界坐标系，也叫全局坐标系。你可以把它想象成一个绝对参考系。通常我们定义它为东北天坐标系（ENU），或者北东地坐标系（NED）。
- ENU：X轴指向东，Y轴指向北，Z轴指向天
- NED：X轴指向北，Y轴指向东，Z轴指向地
我个人习惯用ENU，因为Z轴朝上比较符合直觉。你想想看，我们平时说“高度”，不就是往上走吗？
机体坐标系，就是固定在运动物体上的坐标系。比如无人机、机器人、汽车。它的原点在物体的质心，X轴通常指向前进方向，Y轴指向右侧，Z轴指向下方（右手系）。
关键点：世界坐标系是静止的，机体坐标系是运动的。我们做融合，本质上就是在不断估计机体坐标系相对于世界坐标系的变换。
3.2 传感器坐标系
每个传感器都有自己的坐标系。IMU、相机、激光雷达、GPS，它们各自的坐标系定义都不一样。
| 传感器 | 坐标系定义 | 常见问题 |
|---|---|---|
| IMU | X前、Y右、Z下（右手系） | 加速度计和陀螺仪的轴可能不对齐 |
| 相机 | Z前、X右、Y下（光心为原点） | 图像坐标系和相机坐标系需要转换 |
| 激光雷达 | X前、Y左、Z上（常见） | 不同厂家定义不同，需仔细看手册 |
| GPS | 经纬高（WGS84） | 需要投影到平面坐标系 |
我在项目中遇到过最坑的事：IMU和相机之间的外参标定没做好，导致视觉SLAM的位姿估计和IMU的积分结果差了十万八千里。所以，传感器坐标系之间的变换矩阵，一定要标定准确。
避坑指南：我曾经因为激光雷达的坐标系定义和IMU不一致，花了整整两天排查问题。后来发现，只要在配置文件中把旋转矩阵写对，一切就正常了。所以，拿到传感器第一件事：看手册，确认坐标系定义。
3.3 欧拉角与四元数
描述旋转，最直观的方式就是欧拉角。但也是最容易出问题的方式。
欧拉角：用三个角度表示旋转，比如偏航角（Yaw）、俯仰角（Pitch）、横滚角（Roll）。
- Yaw：绕Z轴旋转
- Pitch：绕Y轴旋转
- Roll：绕X轴旋转
欧拉角有个致命问题：万向锁。当Pitch接近±90°时，Yaw和Roll会变得无法区分。说白了，就是丢失了一个自由度。
我建议在工程中尽量少用欧拉角做内部计算，只用于人机交互的显示。
四元数：用四个数表示旋转，没有万向锁问题。形式是 q = w + xi + yj + zk，其中 w 是实部，x、y、z 是虚部。
四元数的好处：
- 无万向锁
- 插值平滑（球面线性插值）
- 计算效率高
坏处：不直观。你很难从四个数里看出物体朝哪个方向。
我的习惯：内部计算全用四元数，只在输出给用户看的时候转成欧拉角。这样既避免了万向锁，又方便人理解。
3.4 李群与李代数基础
这一节可能有点抽象，但非常重要。尤其是做SLAM后端优化的时候，李群和李代数是绕不开的。
李群：连续光滑的群。在三维空间中，旋转矩阵的集合构成了特殊正交群 SO(3)，刚体变换的集合构成了特殊欧几里得群 SE(3)。
李代数：李群在单位元处的切空间。说白了，就是李群的“导数”。
为什么要引入李代数？因为旋转矩阵本身有约束（正交且行列式为1），直接对它做优化很麻烦。而李代数是一个向量空间，没有约束，可以自由地做加法、求导。
举个例子：
- SO(3) 对应的李代数是 so(3)，是一个三维向量
- SE(3) 对应的李代数是 se(3)，是一个六维向量
它们之间通过指数映射和对数映射相互转换。
// 伪代码示例：指数映射
// 将李代数 so(3) 映射到李群 SO(3)
Eigen::Matrix3d R = Eigen::AngleAxisd(theta, axis).toRotationMatrix();
// 或者用 Sophus 库
Sophus::SO3d R = Sophus::SO3d::exp(so3_vector);核心思想：在优化过程中，我们在李代数上做增量更新，然后通过指数映射得到新的旋转矩阵。这样既保证了旋转矩阵的约束，又方便了计算。
我记得第一次接触李群李代数时，觉得这东西太抽象了。后来在写图优化代码时，发现用李代数做扰动模型求导，比直接对旋转矩阵求导简单太多了。你想想看，一个三维向量求导，和一个九维矩阵求导，哪个更爽？
3.5 知识体系总览
为了让大家更直观地理解本章的知识结构，我画了一张图：
这张图把本章的核心逻辑串起来了。从坐标系定义开始，到旋转表示方法，再到李群李代数的优化工具，最后落到多传感器融合的应用上。
好了，这一章的内容就到这里。坐标系和刚体变换是融合的基石，一定要打好基础。下一章我们会聊传感器的时间同步问题，那也是个大坑。
公众号:蓝海资料掘金营，微信deep3321