坐标系基础：世界坐标系、车体坐标系、激光雷达坐标系、坐标变换与齐次矩阵

说实话，坐标系这个东西，刚入行时我觉得太简单了——不就是三个轴嘛。直到我在实际项目中把车体坐标系和激光雷达坐标系搞反了，点云拼接出来一辆车变成了两辆...嗯，从那以后我再也不敢小看坐标系了。

这一章，咱们就把坐标系这件事彻底聊透。你想想看，激光雷达装在不同位置，数据怎么对齐？车在运动，每一帧点云怎么拼到一起？这些问题的答案，都藏在坐标系里。

1. 四种坐标系，各司其职

在激光雷达SLAM中，我们打交道最多的就是这四个坐标系。我习惯把它们分成两组：固定组和运动组。

这里有个坑，我踩过好几次——车体坐标系的原点到底在哪？不同厂家定义不一样。有的用后轴中心，有的用车辆几何中心，还有的用GPS天线位置。做标定时一定要确认清楚，不然你算出来的位置全是偏的。

2. 世界坐标系：一切测量的基准

世界坐标系，说白了就是那个「绝对参考系」。在室外场景，我们通常用UTM坐标系或者经纬度+高程。室内场景呢？一般把第一帧激光雷达的位置设为原点。

我个人习惯在项目开始时，先确定三件事：

• X轴指向哪？ 通常是东向或车辆前进方向
• Z轴是朝上还是朝下？ 多数用朝上（右手系）
• 单位是米还是毫米？ 统一用米，避免单位换算错误

3. 车体坐标系：车辆自己的小世界

车体坐标系跟着车走。车转弯，它也跟着转。这个坐标系最常用的定义是：

• X轴：车辆前进方向
• Y轴：车辆左侧（左手系）或右侧（右手系）
• Z轴：垂直向上

为什么要有车体坐标系？因为控制车辆时，你关心的是「我左边有没有障碍物」，而不是「障碍物在世界坐标系的哪个位置」。说白了，车体坐标系是给控制算法用的。

4. 激光雷达坐标系：数据的源头

激光雷达坐标系的原点就在雷达的光学中心。不同雷达的坐标系定义可能不一样：

• Velodyne/Hesai：Z轴向上，X轴指向雷达前方
• Ouster：Z轴向上，X轴指向线缆接口方向
• Livox：Z轴向上，X轴指向镜头方向

这里要特别注意：激光雷达的安装角度。我记得有一次，客户把雷达装歪了5度，点云里的地面都是斜的。后来我们加了一个外参标定步骤，才把这个问题解决。

5. 坐标变换：从雷达到世界

坐标变换的核心，就是把一个点从一个坐标系挪到另一个坐标系。公式其实很简单：

// 从激光雷达到车体
P_car = R_lidar_to_car * P_lidar + T_lidar_to_car

// 从车体到世界
P_world = R_car_to_world * P_car + T_car_to_world

// 合起来：从激光雷达到世界
P_world = R_car_to_world * (R_lidar_to_car * P_lidar + T_lidar_to_car) + T_car_to_world

你想想看，每一帧点云都要做这个变换。如果一秒钟来10帧，每帧有10万个点，那就是每秒100万次变换。所以效率很重要。

6. 齐次矩阵：把变换写成乘法

上面那个公式写起来太长了。有没有更简洁的方式？有，就是齐次矩阵。

齐次矩阵把旋转和平移合并成一个4x4的矩阵：

| R11 R12 R13 Tx |
| R21 R22 R23 Ty |
| R31 R32 R33 Tz |
|  0   0   0   1  |

用齐次矩阵，坐标变换就变成了一个矩阵乘法：

P_world_homo = T_car_to_world * T_lidar_to_car * P_lidar_homo

这里P_lidar_homo是点云坐标加上一个1，变成4维向量。为什么加1？为了把平移也塞进矩阵里。这个技巧，说白了就是「用维度换简洁」。

7. 知识体系总览

下面这张图，是我自己总结的坐标系变换流程。每次做项目前，我都会先画一遍这个图，确保思路清晰。

8. 避坑指南：我踩过的那些坑

做坐标系变换，最容易出问题的就是这几个地方：

1. 旋转顺序搞错：绕X、Y、Z轴的旋转顺序不同，结果完全不同。我习惯用Z-Y-X顺序（即偏航-俯仰-横滚）。
2. 左手系和右手系混用：有些传感器用左手系，有些用右手系。转换时一定要统一。
3. 外参标定不准确：我曾经用尺子量安装位置，结果差了2厘米，点云拼接出来全是重影。后来改用标定板+优化算法，精度才上去。
4. 忘记更新车体位姿：车在运动，车体坐标系到世界坐标系的变换矩阵每一帧都在变。忘了更新，点云就会飘。

好了，坐标系这部分就聊到这。你把这些概念理清了，后面的点云配准、地图构建才能顺利推进。记住一句话：坐标系是SLAM的骨架，骨架歪了，肉长得再好也没用。

可可精品资料库