第1章：坐标系与空间变换

做多传感器融合，第一个绕不开的坎儿就是坐标系。我见过不少新手，算法写得飞起，结果一跑起来数据全对不上——最后发现是坐标系搞反了。嗯，这种坑我自己也踩过。

说白了，每个传感器都有自己的“小世界”。激光雷达看的是它自己周围的点，摄像头拍的是它镜头前的像素，IMU测的是它自身的加速度。你要把这些数据凑到一起，就得先搞清楚它们各自在哪儿、朝哪儿看。

1.1 世界坐标系：大家公认的“大地图”

世界坐标系，就是所有传感器数据的最终归宿。你可以把它想象成一张大地图，所有东西的位置都用同一个尺子量。

通常我们用 W 或 world 来表示。比如 P_world 表示一个点在世界坐标系下的坐标。实际项目中，我习惯把世界坐标系的原点设在车辆启动的位置，或者地图的某个固定点。

1.2 车体坐标系：以车为中心的“局部地图”

车体坐标系，也叫车辆坐标系，通常用 B 或 body 表示。它的原点在车辆质心（或者后轴中心），x轴朝前，y轴朝左，z轴朝上——这是自动驾驶行业的标准约定。

我记得有一次做实车测试，发现IMU的数据和车辆运动方向总是差90度。查了半天，原来是IMU的安装方向定义和车体坐标系不一致。从那以后，我每次拿到新传感器，第一件事就是确认它的坐标系定义。

坐标系	原点	x轴	y轴	z轴
世界坐标系 (W)	固定点	东/北	北/东	天
车体坐标系 (B)	车辆质心	前进方向	左侧	上方
传感器坐标系 (S)	传感器光心/中心	传感器朝向	传感器左侧	传感器上方

1.3 传感器坐标系：每个传感器的“小世界”

每个传感器都有自己的坐标系。比如激光雷达，它的原点在旋转中心，x轴通常指向正前方。摄像头呢，原点在光心，z轴指向镜头前方（这个和车体坐标系不一样，容易搞混）。

你想想看，如果激光雷达装在车顶左侧，摄像头装在车顶右侧，它们看到的同一个障碍物，坐标值肯定不一样。怎么对齐？就得靠坐标变换。

1.4 欧拉角与四元数：旋转的两种“语言”

描述旋转，最直观的是欧拉角。绕x轴转叫roll（横滚），绕y轴转叫pitch（俯仰），绕z轴转叫yaw（偏航）。你开车时方向盘打的是yaw，上坡下坡是pitch，车身侧倾是roll。

但欧拉角有个大坑——万向锁。当pitch接近90度时，roll和yaw会变得无法区分。我曾经在无人机项目上吃过这个亏，飞控解算出来的姿态突然跳变，差点炸机。

四元数就没有这个问题。它用四个数 [w, x, y, z] 来表示旋转，没有奇点，插值也平滑。虽然不如欧拉角直观，但做融合算法时我几乎只用四元数。

// 欧拉角转四元数（C++示例）
Eigen::Quaterniond eulerToQuaternion(double roll, double pitch, double yaw) {
    Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());
    return yawAngle * pitchAngle * rollAngle;
}

1.5 坐标变换矩阵：把一切串起来

坐标变换矩阵，说白了就是一个4x4的矩阵，同时包含旋转和平移。用齐次坐标表示，一个点 [x, y, z, 1] 左乘变换矩阵，就得到了新坐标系下的坐标。

举个例子：激光雷达检测到一个障碍物在 P_lidar，怎么知道它在车体坐标系下的位置？

P_body = T_body_lidar * P_lidar

其中 T_body_lidar 就是激光雷达到车体的变换矩阵。这个矩阵通常通过标定得到，或者用CAD模型里的安装位置算出来。

知识体系总览

下面这张图，是我自己总结的坐标系与空间变换的核心逻辑。你顺着箭头走一遍，就能明白数据是怎么从传感器流到世界坐标系的。

避坑指南

• 坐标系方向搞反：我曾经把激光雷达的x轴和y轴搞混，结果障碍物位置全偏了45度。后来我每次标定完都会做一个简单验证——让传感器朝某个方向移动，看坐标值是否对应。
• 四元数顺序问题：不同库的四元数定义可能不同。Eigen是[w, x, y, z]，ROS也是这个顺序。但有些库是[x, y, z, w]，混用必出bug。
• 变换矩阵的逆：如果你有T_body_sensor，想求T_sensor_body，直接求逆矩阵就行。但要注意，旋转矩阵的逆等于转置，平移部分要重新计算。

---