坐标系入门：激光雷达坐标系、车体坐标系、世界坐标系
做多传感器融合，第一道坎就是坐标系。
我见过太多新手，算法写得飞起，结果一跑数据，点云全飘到天上去了。查了半天，原来是坐标系搞反了。嗯，这种坑我自己也踩过不止一次。
今天咱们就把坐标系这事儿彻底聊透。说白了，就是搞清楚三个问题：我在哪？车在哪？激光雷达看到的东西又在哪？
1. 激光雷达坐标系：传感器的“眼睛”
激光雷达坐标系，是跟着传感器走的。它定义了点云数据在空间中的位置。
我个人习惯用右手系来定义。具体来说：
- X轴：指向激光雷达的正前方
- Y轴：指向激光雷达的左侧
- Z轴：指向激光雷达的正上方
注意，不同厂家可能有差异。比如有些雷达把Y轴定义为右侧。我在项目中遇到过一台老款Velodyne，它的坐标系就跟常规不一样。所以拿到新雷达，第一件事就是查手册，确认坐标系定义。
核心要点：激光雷达坐标系的原点，通常位于雷达的几何中心或光学中心。所有点云的(x, y, z)坐标，都是相对于这个原点计算的。
2. 车体坐标系：车辆的“骨架”
车体坐标系，也叫车辆坐标系。它把车辆本身当作参考。
标准定义是这样的：
- X轴：指向车辆前进方向
- Y轴：指向车辆左侧
- Z轴：指向车辆正上方
你想想看，车体坐标系的原点一般放在后轴中心，或者车辆质心。为什么？因为车辆的运动（速度、加速度）都是围绕这个点计算的。
我的经验：实际项目中，车体坐标系的原点最好选在后轴中心。这样计算车辆运动学模型时，公式最简洁。我曾经试过用质心做原点，结果转弯时的侧偏角计算复杂得要命。
3. 世界坐标系：全局的“地图”
世界坐标系，就是大地坐标系。它不随车辆移动而改变。
通常我们用：
- X轴：指向正东
- Y轴：指向正北
- Z轴：指向天空（垂直于地面）
这就是所谓的ENU坐标系（东-北-天）。做自动驾驶定位时，GPS给出的经纬高数据，最终都要转换到这个世界坐标系里。
注意：世界坐标系的原点不是固定的。你可以选在起始点，也可以选在地图上的某个参考点。关键是整个系统要统一。我见过一个团队，不同模块用了不同的世界坐标系原点，结果融合出来的轨迹像一团乱麻。
4. 坐标系之间的转换关系
搞清楚了三个坐标系，接下来就是怎么在它们之间来回切换。说白了，就是旋转 + 平移。
4.1 激光雷达到车体坐标系
这是最常用的转换。激光雷达安装在车上，它的位置和朝向是固定的。我们需要把雷达看到的点云，转换到车体坐标系下。
公式很简单：
P_vehicle = R_lidar_to_vehicle * P_lidar + T_lidar_to_vehicle其中：
P_lidar：激光雷达坐标系下的点坐标R_lidar_to_vehicle：旋转矩阵（3x3）T_lidar_to_vehicle：平移向量（3x1）
避坑指南：我曾经在标定激光雷达时，把旋转矩阵的符号搞反了。结果点云全部翻到了车底下。后来我养成了一个习惯：标定完先拿一个已知位置的目标物验证一下。比如放一个锥桶在车正前方1米处，看转换后的点云是不是也在正前方1米。
4.2 车体到世界坐标系
车辆在移动，车体坐标系的原点也在变。我们需要知道车辆当前的位置和朝向，才能把车体坐标系下的点转换到世界坐标系。
公式类似：
P_world = R_vehicle_to_world * P_vehicle + T_vehicle_to_world这里的R_vehicle_to_world和T_vehicle_to_world，其实就是车辆的位姿（位置+朝向）。通常由GPS/IMU或者SLAM系统提供。
3. 直接转换：激光雷达到世界坐标系
实际应用中，我们经常需要一步到位：
P_world = R_vehicle_to_world * (R_lidar_to_vehicle * P_lidar + T_lidar_to_vehicle) + T_vehicle_to_world说白了，就是先转到车体，再转到世界。两步走，清晰明了。
5. 知识体系总览
为了让你更直观地理解这三个坐标系的关系，我画了一张图：
6. 实战中的坐标系管理
在实际项目中，我建议你这样做：
- 统一命名规范：所有坐标系变量名加上后缀，比如
lidar_pts、vehicle_pts、world_pts。一眼就能看出数据在哪个坐标系下。 - 写一个坐标系管理类：把所有的转换矩阵封装起来。不要到处写硬编码的旋转平移。
- 可视化验证：每次转换完，把点云画出来看看。如果点云形状扭曲或者位置不对，八成是坐标系搞错了。
小技巧：我习惯在代码里加一个断言，检查转换后的点云是否在合理范围内。比如车体坐标系下的点，Z值不应该超过5米（除非有高架桥）。一旦超出，立刻报错。这能帮你快速定位问题。
好了，坐标系这块儿就聊到这儿。记住一句话：坐标系是融合的基石，搞错了，后面全是白搭。