2. 坐标系基础

做车道线定位融合，说白了就是跟坐标系打交道。我刚开始接触这个领域时，被各种坐标系绕得晕头转向。后来慢慢理清了，发现其实就四个核心坐标系，搞懂了它们之间的转换关系，整个定位融合的骨架就搭起来了。

为什么需要这么多坐标系？

你想想看，我们开车时，车辆在真实世界里运动，这是世界坐标系的事。但车辆自身的姿态、速度，得用车辆坐标系来描述。而摄像头看到的画面，是相机坐标系下的。最后显示在屏幕上，又变成了图像坐标系。每个坐标系都有自己的"主场优势"，没有哪个能包打天下。

我在项目中遇到过最典型的坑：直接把相机检测到的车道线坐标拿去跟地图匹配，结果完全对不上。为什么？因为相机看到的是像素，地图用的是经纬度，中间差了三个坐标系转换呢。

四大坐标系详解

1. 世界坐标系

世界坐标系是绝对的参考系。它固定在地球上，不随车辆移动而改变。通常我们用经纬度加海拔来表示，或者用UTM坐标系（通用横轴墨卡托投影）。

我个人习惯在自动驾驶项目中使用UTM坐标系。为什么？因为它是平面直角坐标，计算距离和角度非常方便。经纬度算个两点距离还得用球面三角公式，太麻烦了。

2. 车辆坐标系

车辆坐标系固定在车上。按照ISO 8855标准，原点在车辆后轴中心，X轴朝前，Y轴朝左，Z轴朝上。

嗯，这里要注意：不同车厂可能有自己的定义。有的把原点放在质心，有的放在前轴。我建议你在做融合之前，先确认好车辆坐标系的原点位置，否则后面所有转换都会出错。

轴	方向	典型用途
X	车辆前进方向	纵向速度、加速度
Y	车辆左侧方向	横向偏移、车道宽度
Z	车辆上方方向	高度变化、路面坡度

3. 相机坐标系

相机坐标系的原点在相机光心，Z轴指向镜头前方（即拍摄方向），X轴向右，Y轴向下。这个定义跟图像处理中的习惯一致。

我曾经犯过一个低级错误：把相机坐标系的Z轴当成了朝上方向，结果转换出来的点云全部倒立。排查了半天才发现是坐标系定义搞反了。

4. 图像坐标系

图像坐标系就是我们看到的像素坐标。原点在图像左上角，u轴向右，v轴向下。单位是像素。

这里有个容易混淆的地方：图像坐标系是二维的，而前面三个都是三维的。从三维到二维，丢失了深度信息。所以从图像坐标系反推三维坐标时，必须借助其他信息（比如地面假设、深度估计等）。

坐标系转换关系

搞清楚了四个坐标系，接下来就是它们之间的转换。我画了一张图，帮你理清脉络：

转换公式速查

这里给出最常用的转换公式。我建议你把这些公式记在笔记本上，或者做成代码库，每次用到直接调用。

世界坐标系 → 车辆坐标系

// 旋转矩阵R + 平移向量t
P_vehicle = R * P_world + t

// 其中R由车辆姿态（横滚、俯仰、偏航）决定
// t是车辆在世界坐标系中的位置

车辆坐标系 → 相机坐标系

// 外参矩阵 [R_cam | t_cam]
P_camera = R_cam * P_vehicle + t_cam

// R_cam和t_cam通过标定获得

相机坐标系 → 图像坐标系

// 内参矩阵K
// 假设相机内参：fx, fy, cx, cy
u = fx * X_cam / Z_cam + cx
v = fy * Y_cam / Z_cam + cy

// 写成矩阵形式：
// [u]   [fx  0  cx] [X_cam]
// [v] = [0  fy  cy] [Y_cam]
// [1]   [0   0   1] [Z_cam]

车道线检测中的坐标系应用

具体到车道线检测，坐标系转换的典型流程是这样的：

1. 摄像头采集图像，得到图像坐标系下的车道线像素点
2. 利用相机内参，将像素点投影到相机坐标系下的归一化平面
3. 结合地面假设（通常假设车道线在Z=0平面上），反算出相机坐标系下的三维坐标
4. 通过外参矩阵，转换到车辆坐标系
5. 最后通过车辆定位信息（GPS/IMU），转换到世界坐标系

我曾经在第三步吃过亏。地面假设在平坦路面上没问题，但遇到坡道或颠簸路面，Z=0的假设就不成立了。后来我加入了IMU的俯仰角信息，动态调整地面高度，才解决了这个问题。

常见错误与调试建议

错误类型	现象	排查方法
坐标系方向搞反	车道线投影到地图上方向相反	打印中间坐标，检查各轴方向
外参标定不准	车道线与地图车道有固定偏移	重新标定，检查标定板角点检测
内参错误	车道线变形、扭曲	用已知尺寸的物体验证投影
时间戳不同步	车道线抖动、跳变	检查各传感器时间戳，做插值对齐

嗯，最后说一句：坐标系转换看起来是基础，但基础不牢，地动山摇。我见过太多项目因为坐标系定义不一致，导致融合结果一塌糊涂。建议你在项目开始前，先把坐标系定义文档写清楚，团队统一标准，后面能省很多麻烦。