4、坐标系与变换：世界坐标系、车体坐标系、传感器坐标系、刚体变换与欧拉角、四元数
做自动驾驶融合导航，说白了就是跟各种坐标系打交道。我刚开始入行那会儿，最头疼的就是坐标系搞混，一个旋转矩阵写反了，整个定位结果直接飞到了马路对面。嗯，今天咱们就把这块硬骨头啃下来。
4.1 为什么需要这么多坐标系？
你想想看，激光雷达看到的是它自己周围的点云，IMU测量的是车体的加速度和角速度，相机拍的是像素平面上的图像。这些数据如果不统一到一个参考系下，根本没法融合。
我个人习惯把坐标系分成三个层级：
- 世界坐标系：全局的、固定的参考系。比如UTM坐标系、经纬度坐标系。它是所有数据的最终归宿。
- 车体坐标系：固定在车辆上的坐标系。通常原点在车辆后轴中心，x轴朝前，y轴朝左，z轴朝上。
- 传感器坐标系：每个传感器自己的小天地。激光雷达有LiDAR坐标系，相机有相机坐标系，IMU有IMU坐标系。
我在项目中遇到过最典型的坑：标定完外参后，忘记把激光雷达的点云从传感器坐标系变换到车体坐标系，结果建出来的地图全是歪的。排查了整整两天才找到问题。
4.2 刚体变换：旋转 + 平移
坐标系之间的变换，本质上就是刚体变换。说白了，就是把一个点从一个坐标系挪到另一个坐标系。公式很简单：
P_w = R * P_s + t其中：
- P_w 是点在世界坐标系下的坐标
- P_s 是点在传感器坐标系下的坐标
- R 是旋转矩阵（3x3）
- t 是平移向量（3x1）
这里有个关键点：旋转矩阵是正交矩阵，它的逆等于它的转置。这个性质在实际代码中非常有用，求逆变换时直接转置就行，省去了求逆矩阵的麻烦。
重要提醒：旋转矩阵的乘法不满足交换律。R1 * R2 和 R2 * R1 通常不一样。写代码时一定要注意旋转顺序。
4.3 欧拉角：直观但容易踩坑
欧拉角用三个角度来描述旋转：横滚角（roll）、俯仰角（pitch）、偏航角（yaw）。听起来很直观对吧？但实际用起来问题不少。
我曾经在项目里用欧拉角做插值，结果出现了万向锁——在俯仰角接近90度时，横滚和偏航变得无法区分，定位直接崩了。从那以后，我内部计算一律用四元数，只在可视化或者给用户看的时候才转成欧拉角。
常见的欧拉角顺序有：
| 顺序 | 说明 | 常见应用 |
|---|---|---|
| ZYX | 先绕z轴转yaw，再绕y轴转pitch，最后绕x轴转roll | 无人机、车辆姿态 |
| ZYZ | 先绕z轴转，再绕y轴转，再绕z轴转 | 机械臂运动学 |
| XYZ | 固定轴顺序 | 某些SLAM系统 |
避坑指南：不同库对欧拉角的定义可能不同。比如ROS和Eigen的默认顺序就不一样。我曾经因为没注意这个，把IMU数据喂给滤波器，结果姿态估计全反了。建议在代码开头明确注释清楚你用的是哪种顺序。
4.4 四元数：SLAM的标配
四元数听起来高大上，其实就是一个四维向量 q = [w, x, y, z]，满足 w² + x² + y² + z² = 1。它没有万向锁问题，插值平滑，计算效率高。现在主流的SLAM系统，比如ORB-SLAM、LOAM、LIO-SAM，内部全是用四元数。
我个人习惯用四元数做旋转的链式乘法。比如从传感器到车体，再从车体到世界，直接乘起来就行：
q_world = q_body_to_world * q_sensor_to_body * q_sensor注意乘法顺序：在Hamilton四元数约定下，先发生的旋转在右边。这个顺序跟旋转矩阵正好相反，刚开始很容易搞混。
四元数转旋转矩阵的公式：
R = [[1-2(y²+z²), 2(xy-wz), 2(xz+wy)],
[2(xy+wz), 1-2(x²+z²), 2(yz-wx)],
[2(xz-wy), 2(yz+wx), 1-2(x²+y²)]]小技巧：调试时可以用Eigen或Sophus库自动帮你做转换。手写的话，建议写个单元测试，用已知的旋转验证一下。我每次写新的变换代码，都会先跑一遍随机旋转测试，确保正反变换能对得上。
4.5 坐标系变换的工程实践
在实际的融合导航系统中，坐标系变换通常封装成TF树。每个坐标系是一个节点，节点之间的变换是边。这样做的好处是：
- 新增传感器时，只需要在TF树里加一条边
- 查询任意两个坐标系之间的变换，TF树会自动帮你计算
- 支持动态变换（比如IMU坐标系相对于车体坐标系是固定的，但GPS天线相对于车体坐标系可能随车辆载重变化）
我记得在做一个多传感器融合项目时，车上装了3个激光雷达、2个相机、1个IMU、1个GPS。如果手动维护每对传感器之间的变换，代码会变得极其混乱。后来改用TF树管理，整个系统清晰多了。
下面这张图展示了典型的自动驾驶坐标系变换关系：
从这张图可以看出，所有传感器数据最终都要变换到世界坐标系下才能进行融合。中间经过车体坐标系作为中转，这样设计的好处是：如果某个传感器的外参变了，只需要更新它到车体的变换，不影响其他传感器。
4.6 代码实现要点
最后分享几个我在实际代码中常用的技巧：
// C++ 示例：使用Eigen进行坐标系变换
#include <Eigen/Dense>
#include <Eigen/Geometry>
// 定义四元数（w, x, y, z）
Eigen::Quaterniond q_sensor_to_body(0.707, 0.0, 0.707, 0.0);
Eigen::Vector3d t_sensor_to_body(0.5, 0.0, 0.3);
// 传感器坐标系下的点
Eigen::Vector3d p_sensor(1.0, 2.0, 3.0);
// 变换到车体坐标系
Eigen::Vector3d p_body = q_sensor_to_body * p_sensor + t_sensor_to_body;
// 再变换到世界坐标系
Eigen::Quaterniond q_body_to_world(1.0, 0.0, 0.0, 0.0);
Eigen::Vector3d t_body_to_world(100.0, 200.0, 0.0);
Eigen::Vector3d p_world = q_body_to_world * p_body + t_body_to_world;个人建议：写变换代码时，一定要在注释里写明坐标系名称和变换方向。比如 // 将点从front_lidar坐标系变换到base_link坐标系。这样半年后回来看代码，还能一眼看懂。
好了，坐标系与变换这块就讲到这里。记住一句话：坐标系是融合导航的基石，变换搞清楚了，后面的融合算法才能站得住脚。
公众号:蓝海资料掘金营，微信deep3321