3. 坐标系统与空间变换：世界坐标系、车身坐标系、传感器坐标系、欧拉角与四元数、坐标变换矩阵推导

各位同学，今天我们来聊聊多传感器融合里最基础、也最容易踩坑的一个话题——坐标系统与空间变换。

说实话，我刚开始做融合的时候，觉得坐标变换不就是矩阵乘来乘去嘛，有什么难的？直到有一次，我把激光雷达的点云投影到了图像上，结果发现所有障碍物都飘到了天上……嗯，从那以后，我再也不敢小看坐标系这件事了。

3.1 为什么需要多个坐标系？

你想想看，一辆自动驾驶车上，装了激光雷达、摄像头、毫米波雷达、GPS/IMU。每个传感器都有自己的“视角”。

• 激光雷达说：障碍物在我前方5米，偏左30度
• 摄像头说：障碍物在图像的第320列、第240行
• GPS说：障碍物在北纬39.9度、东经116.4度

这些数据如果不统一到一个坐标系下，根本没法融合。说白了，坐标系变换就是给所有传感器一个“共同语言”。

3.2 四大坐标系详解

3.2.1 世界坐标系（World Frame）

世界坐标系是绝对参考系。在自动驾驶里，通常用UTM坐标系或者经纬高坐标系。它的原点在地球上的某个固定点，X轴指向东，Y轴指向北，Z轴指向天。

我个人习惯用UTM坐标系做全局规划，因为它是平面直角坐标系，计算距离和角度都很方便。但要注意，UTM在不同带区之间切换时会有跳变，跨区域行驶时得小心处理。

3.2.2 车身坐标系（Vehicle/Body Frame）

车身坐标系固定在车辆上。通常定义：

• 原点：车辆后轴中心（或者质心，看具体需求）
• X轴：车辆前进方向
• Y轴：车辆左侧（或者右侧，不同标准有差异）
• Z轴：垂直向上

我在项目中遇到过一个问题：不同供应商对车身坐标系的定义不一样。有的把原点放在前保险杠中心，有的放在后轴。如果不提前对齐，融合出来的结果就是一团糟。

3.2.3 传感器坐标系（Sensor Frame）

每个传感器都有自己的坐标系：

• 相机坐标系： 原点在光心，Z轴指向镜头前方，X轴向右，Y轴向下（图像坐标系）
• 激光雷达坐标系： 原点在激光头旋转中心，通常X轴向前，Y轴向左，Z轴向上
• 毫米波雷达坐标系： 原点在雷达天线中心，X轴指向雷达波束方向

这些传感器的安装位置和朝向都不同，所以我们需要一个统一的变换流程。

3.3 坐标变换的数学工具

3.3.1 欧拉角

欧拉角用三个角度来描述旋转：横滚角（Roll）、俯仰角（Pitch）、偏航角（Yaw）。

说白了，就是绕X轴转、绕Y轴转、绕Z轴转。但这里有个大坑——旋转顺序！

我记得有一次，我用欧拉角做IMU和相机的标定，结果怎么都对不上。后来发现，我用的旋转顺序是ZYX，而IMU驱动里默认的是XYZ。顺序一错，结果天差地别。

3.3.2 四元数

欧拉角有个致命问题——万向锁。当俯仰角接近±90度时，横滚和偏航会耦合在一起，丢失一个自由度。

四元数就没有这个问题。它用四个数来表示旋转：q = [w, x, y, z]，其中w是实部，x、y、z是虚部。

我个人更偏爱四元数，尤其是在做插值和滤波的时候。但四元数不太直观，你很难直接看出它代表什么姿态。所以调试的时候，我经常把四元数转成欧拉角来看。

3.4 坐标变换矩阵推导

好了，重头戏来了。我们来看看怎么把一个点从传感器坐标系变换到世界坐标系。

假设传感器坐标系下有一个点 P_s = [x_s, y_s, z_s]^T，我们想把它变换到世界坐标系 P_w。

变换公式很简单：

P_w = R * P_s + T

其中：

• R 是3x3的旋转矩阵
• T 是3x1的平移向量

旋转矩阵R可以从欧拉角或四元数推导出来。以ZYX顺序为例：

R = R_z(yaw) * R_y(pitch) * R_x(roll)

其中：
R_x(roll)  = [[1, 0, 0],
              [0, cos(roll), -sin(roll)],
              [0, sin(roll), cos(roll)]]

R_y(pitch) = [[cos(pitch), 0, sin(pitch)],
              [0, 1, 0],
              [-sin(pitch), 0, cos(pitch)]]

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

注意顺序！是先绕X轴转，再绕Y轴转，最后绕Z轴转。这个顺序不能搞反。

如果使用四元数，旋转矩阵可以写成：

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²)]]

其中 q = [w, x, y, z] 是单位四元数。

3.5 实战中的变换链

在实际系统中，我们通常维护一个变换树（Transform Tree）。比如：

world -> vehicle -> sensor

每个节点之间的变换关系用TF（Transform）表示。ROS里的TF库就是干这个的。

我建议在代码里这样组织：

// 定义变换
struct Transform {
    Eigen::Vector3d translation;  // 平移
    Eigen::Quaterniond rotation;  // 旋转（四元数）
};

// 变换链
Transform T_world_to_vehicle;  // 世界到车身
Transform T_vehicle_to_lidar;  // 车身到激光雷达
Transform T_vehicle_to_camera; // 车身到相机

// 点云从激光雷达坐标系变换到世界坐标系
Eigen::Vector3d point_in_lidar = ...;
Eigen::Vector3d point_in_world = 
    T_world_to_vehicle * (T_vehicle_to_lidar * point_in_lidar);

3.6 知识体系总览

下面这张图展示了本章的核心逻辑：

3.7 总结与建议

坐标变换说难不难，说简单也不简单。我见过太多项目因为坐标系问题翻车了。这里给大家几个建议：

1. 项目一开始就定义好坐标系规范，包括原点位置、轴向、旋转顺序。写进文档里，让所有人都遵守。
2. 用四元数做计算，用欧拉角做调试。四元数没有万向锁问题，适合做插值和滤波。欧拉角直观，适合人眼观察。
3. 写单元测试验证变换矩阵。我习惯写一个简单的测试：把一个已知点从A坐标系变换到B坐标系，再变换回来，看结果是否一致。
4. 可视化调试。把坐标系画出来，看看各个传感器的位置和朝向是否正确。有时候一眼就能看出问题。

好了，这一章的内容就到这里。坐标变换是融合的基础，基础不牢，地动山摇。希望大家在实际项目中能少踩坑，多出活。

---