2. 坐标系与变换：世界坐标系、车辆坐标系、传感器坐标系及其相互转换

做轨迹预测，说白了就是回答一个问题：「这个障碍物接下来会去哪儿？」

但这个问题有个前提——你得先搞清楚「它在哪儿」。而「在哪儿」这三个字，其实是个相对概念。你站在车上看它，和站在地图上看它，位置是完全不同的。这就是坐标系存在的意义。

我个人习惯把坐标系比作「语言的方言」。你说「前面三米」，这个「前面」是以你为参考的。但如果你想让另一辆车理解，就得翻译成大家都懂的「世界坐标」。嗯，今天我们就来聊聊这几门「方言」怎么互相翻译。

2.1 三大坐标系：各司其职

在自动驾驶系统中，我们主要跟三个坐标系打交道：

• 世界坐标系：全局地图的参考系。通常用经纬度+海拔，或者UTM坐标。它是「绝对位置」。
• 车辆坐标系：以自车为中心。x轴朝前，y轴朝左，z轴朝上。它是「相对位置」。
• 传感器坐标系：以传感器（相机、LiDAR、雷达）为中心。每个传感器都有自己的小世界。

你想想看，障碍物的轨迹最终要在世界坐标系下表达，但原始数据却来自传感器。这中间就得做两次变换：传感器→车辆→世界。

2.2 传感器坐标系 → 车辆坐标系

这是第一步。每个传感器安装在车上的位置和朝向都不同。比如前向相机装在挡风玻璃后面，LiDAR装在车顶。

假设LiDAR相对于车辆后轴中心的位置是 (x_offset, y_offset, z_offset)，朝向偏了 yaw_offset。那么一个LiDAR点 (x_l, y_l, z_l) 转换到车辆坐标系就是：

// 伪代码：LiDAR点到车辆坐标系
x_v = x_l * cos(yaw_offset) - y_l * sin(yaw_offset) + x_offset
y_v = x_l * sin(yaw_offset) + y_l * cos(yaw_offset) + y_offset
z_v = z_l + z_offset  // 通常z轴方向一致，直接加

我在项目中遇到过一个问题：某次标定数据里yaw_offset写反了符号，结果所有障碍物都偏了15度。嗯，这种低级错误排查起来特别费时间。所以我建议你每次做完变换，都拿一个已知位置的静态物体验证一下。

2.3 车辆坐标系 → 世界坐标系

这一步需要知道自车当前在世界坐标系下的位姿。通常来自GPS+IMU的组合导航系统。

假设自车在世界坐标系下的位置是 (ego_x, ego_y, ego_z)，朝向是 ego_yaw。那么车辆坐标系下的点 (x_v, y_v, z_v) 转换到世界坐标系：

// 伪代码：车辆坐标系到世界坐标系
x_w = x_v * cos(ego_yaw) - y_v * sin(ego_yaw) + ego_x
y_w = x_v * sin(ego_yaw) + y_v * cos(ego_yaw) + ego_y
z_w = z_v + ego_z

你会发现，这个公式和传感器→车辆的公式长得一模一样。没错，因为都是刚体变换，数学本质是一样的。

我曾经在实车测试时发现轨迹预测结果在地图上「漂移」。排查了半天，发现是GPS的ego_yaw更新频率太低，导致车辆转弯时坐标变换滞后。后来我加了一个IMU航迹推算做插值，问题就解决了。

2.4 逆变换：世界坐标系 → 车辆坐标系

有时候我们需要反过来做。比如高精地图上的车道线，要投影到车辆坐标系下才能做碰撞检测。

逆变换其实就是把上面的公式反过来：

// 伪代码：世界坐标系到车辆坐标系
dx = x_w - ego_x
dy = y_w - ego_y
x_v = dx * cos(-ego_yaw) - dy * sin(-ego_yaw)
y_v = dx * sin(-ego_yaw) + dy * cos(-ego_yaw)
z_v = z_w - ego_z

注意旋转角度取了负号。因为「从A到B的旋转」的逆，就是「从B到A的旋转」。这个道理很简单，但我在代码里见过有人直接复制粘贴正变换公式，忘了取负号——结果地图上的车道线全跑到车后面去了。

2.5 齐次坐标与变换矩阵

上面用三角函数写公式，看着有点啰嗦。实际工程中，我们通常用齐次坐标和变换矩阵来统一表达。

一个4x4的变换矩阵长这样：

| R(3x3)  t(3x1) |
| 0 0 0    1     |

其中R是旋转矩阵，t是平移向量。一个点 (x, y, z) 写成齐次坐标 (x, y, z, 1)，左乘变换矩阵就得到变换后的点。

这样做的好处是：多次变换可以合并成一个矩阵。比如传感器→世界，就是先乘传感器→车辆的矩阵，再乘车辆→世界的矩阵。两个矩阵相乘，得到一个总矩阵，后面直接用这个总矩阵就行。

2.6 坐标系变换的常见坑

做这行久了，我总结出几个容易踩的坑：

• 旋转顺序：有的系统用RPY（Roll-Pitch-Yaw），有的用YPR。顺序不同，结果天差地别。我建议统一用YPR，因为车辆运动主要是偏航角变化。
• 角度单位：弧度还是角度？我见过有人把度当弧度传进去，结果障碍物飞到天上了。嗯，血的教训。
• 左手系 vs 右手系：相机坐标系通常是左手系（z朝前），而车辆坐标系是右手系（x朝前）。转换时别忘了翻转一个轴。
• 时间戳对齐：传感器数据和自车位姿数据的时间戳必须对齐。差个10毫秒，在高速场景下就是几十厘米的误差。

2.7 知识体系总览

下面这张图总结了坐标系变换的核心逻辑。你可以把它当作一个「翻译流程」来看：

说白了，坐标系变换就是自动驾驶的「翻译官」。没有它，传感器看到的世界和地图上的世界就是两码事。我个人建议你在项目初期就把变换模块写好、测好，后面所有依赖它的模块（检测、跟踪、预测、规划）都会省心很多。

嗯，今天就聊到这儿。坐标系变换这块，理解了原理，剩下的就是细心。标定参数别写反，时间戳别对错，基本上就不会出大问题。