2. 坐标系与标定：世界坐标系、车辆坐标系、相机坐标系、激光雷达坐标系之间的转换，以及联合标定方法

说实话，坐标系与标定这块，是自动驾驶感知里最基础但也最容易翻车的地方。我见过不少新手，算法调得飞起，结果因为标定参数差了一点点，整个感知结果全偏了。嗯，咱们今天就把这事儿彻底捋清楚。

2.1 四大坐标系，你得心里有数

自动驾驶里，我们天天跟四个坐标系打交道。说白了，它们就是不同视角下的“参考系”。

• 世界坐标系：一个固定的全局坐标系。比如以某个路口中心为原点，X轴指向东，Y轴指向北，Z轴朝天。所有物体的真实位置，最终都要映射到这个坐标系里。
• 车辆坐标系：以车为中心。通常原点在车辆后轴中心，X轴朝前，Y轴朝左，Z轴朝上。我习惯叫它“车身坐标系”，因为所有传感器数据最终都要统一到这里。
• 相机坐标系：以相机光心为原点，Z轴沿着光轴朝前，X轴朝右，Y轴朝下。注意，相机坐标系是右手系，但图像坐标系是左手系，这个转换容易搞混。
• 激光雷达坐标系：以激光雷达的旋转中心为原点。不同厂家的定义略有差异，但通常X轴朝前，Y轴朝左，Z轴朝上。

2.2 坐标系转换，其实就三步

我在项目中遇到过最头疼的事，就是不同传感器数据对不上。后来发现，问题出在转换顺序上。你想想看，从激光雷达到世界坐标系，中间必须经过车辆坐标系。

举个例子，把激光雷达点云转换到世界坐标系：

// 伪代码示例
// 第一步：激光雷达坐标系 -> 车辆坐标系
P_vehicle = R_lidar_to_vehicle * P_lidar + T_lidar_to_vehicle

// 第二步：车辆坐标系 -> 世界坐标系
P_world = R_vehicle_to_world * P_vehicle + T_vehicle_to_world

// 合并一步到位
P_world = R_vehicle_to_world * (R_lidar_to_vehicle * P_lidar + T_lidar_to_vehicle) + T_vehicle_to_world

为什么会这样？因为车辆坐标系是“中间人”。所有传感器都先统一到车体，再跟全局地图对齐。我个人习惯把这一步叫做“归一化”，说白了就是让所有数据说同一种语言。

2.3 联合标定，最磨人的环节

联合标定，就是求传感器之间的外参（R和T）。我做过激光雷达和相机的联合标定，也做过相机和相机的。这里重点说激光雷达+相机，因为这是最常见的组合。

2.3.1 标定原理

核心思路：找一个共同目标，比如棋盘格。激光雷达能扫到棋盘格的边缘，相机能拍到棋盘格的角点。然后通过匹配这些特征点，解出R和T。

2.3.2 标定流程

1. 数据采集：让车停在平坦地面，在车前不同距离、不同角度摆放标定板。我建议至少采集10-15组数据，覆盖视野的各个区域。
2. 特征提取：从激光雷达点云中提取棋盘格边缘点，从图像中提取角点。注意，激光雷达的点云稀疏，边缘提取需要做滤波。
3. 匹配与优化：用PnP算法或非线性优化，最小化重投影误差。说白了，就是让激光雷达点投影到图像上后，跟图像里的角点尽量重合。

// 重投影误差计算示例
// 激光雷达点 P_lidar 投影到图像坐标系
P_camera = R * P_lidar + T
p_image = K * P_camera  // K是相机内参

// 误差 = 投影点 - 真实图像角点
error = p_image - p_detected

// 优化目标：最小化所有点的误差平方和

2.4 标定结果的验证

标定完别急着用，先验证。我常用的方法：

• 可视化验证：把激光雷达点云投影到图像上，看看边缘是否对齐。如果出现“重影”，说明标定参数有问题。
• 距离验证：在车前放一个已知尺寸的物体，比如一个1米高的锥桶。用标定后的参数反算物体距离，跟实际距离对比。误差超过5厘米，就得重新标。
• 重复性验证：换几个位置重新标定，看看结果是否一致。如果每次标出来的R和T都不一样，说明数据采集有问题。

2.5 一些实用建议

嗯，这里要注意，标定不是一劳永逸的事。车辆行驶过程中，传感器可能会因为震动而移位。我建议每三个月重新标定一次，或者每次车辆发生碰撞后必须重新标定。

另外，不同传感器的坐标系定义可能不同。比如有些激光雷达的Z轴是朝上的，有些是朝下的。拿到传感器手册后，第一件事就是确认坐标系定义，别想当然。

最后，推荐一个开源工具：lidar_camera_calibration（ROS包）。我项目里一直在用，效果不错。但记住，工具只是辅助，理解原理才是根本。