第二章 地图与坐标系：高精地图概念、世界坐标系与车辆坐标系转换、Frenet坐标系详解
说实话，做自动驾驶这几年，我踩过最大的坑，往往不是算法本身，而是坐标系搞混了。
你想想看，一辆车在路上跑，传感器看到的是障碍物相对于车的位置，高精地图告诉你的是车道线在世界上的绝对坐标，而规划模块需要知道的是「车沿着路走了多远」。这三个东西如果不统一，后果就是——车不知道自己该往哪拐。
嗯，这一章我们就来彻底理清这些概念。
2.1 高精地图：不只是「地图」
普通导航地图告诉你「前方500米右转」。高精地图告诉你「右转车道宽度3.5米，曲率半径200米，停止线距离路口12.3米」。说白了，高精地图是给车看的，不是给人看的。
高精地图的核心要素：
- 车道级拓扑：每条车道的连接关系、变道可行性
- 几何信息：车道中心线坐标、宽度、曲率、坡度
- 语义信息：红绿灯位置、停止线、限速标志、人行横道
- 动态层：施工区域、临时交通管制（需要实时更新）
我在项目中遇到过一个问题：高精地图里车道中心线的采样点间距是1米，但车辆在高速上以120km/h行驶时，1米的采样间隔会导致控制点过于密集，反而引起路径抖动。后来我改成根据车速动态调整采样间隔——低速密采，高速疏采。这个经验后来成了我们团队的标配做法。
2.2 世界坐标系与车辆坐标系
这两个坐标系，是自动驾驶里最基础、也最容易搞混的东西。
2.2.1 世界坐标系（World Coordinate System）
世界坐标系是一个固定的参考系。通常我们使用UTM坐标系（通用横轴墨卡托投影），把地球表面投影到平面上。单位是米，原点在某个固定的UTM带中心。
高精地图里的所有元素——车道线、红绿灯、路沿——都是用世界坐标存储的。GPS/IMU给出的定位结果，也是世界坐标。
2.2.2 车辆坐标系（Vehicle Coordinate System）
车辆坐标系是跟着车动的。一般定义：
- x轴：车头方向
- y轴：车身左侧（注意，有些标准定义右侧为正，一定要确认好）
- z轴：车顶方向
- 原点：后轴中心（我习惯用这个点，因为车辆运动学模型基于后轴）
注意：我曾经因为车辆坐标系定义不一致，导致传感器融合模块和规划模块对「前方障碍物」的理解差了180度。传感器团队用ISO标准（x向前，y向左），规划团队用ROS标准（x向前，y向右）。结果车看到左侧有障碍物，规划模块以为右侧有障碍物，直接往左打了一把方向……嗯，那次测试差点出事。
所以，团队内部必须统一坐标系定义，并且在代码里用常量明确标注。
2.2.3 坐标转换：从世界到车辆
转换公式其实很简单，就是一个旋转加平移：
// 世界坐标 -> 车辆坐标
// 已知：车辆在世界坐标系下的位置 (x_v, y_v) 和航向角 yaw
// 输入：某点的世界坐标 (x_w, y_w)
// 输出：该点在车辆坐标系下的坐标 (x_c, y_c)
double dx = x_w - x_v;
double dy = y_w - y_v;
double cos_yaw = cos(yaw);
double sin_yaw = sin(yaw);
double x_c = dx * cos_yaw + dy * sin_yaw;
double y_c = -dx * sin_yaw + dy * cos_yaw;反过来，从车辆坐标转世界坐标，就是把旋转矩阵转置一下：
// 车辆坐标 -> 世界坐标
double x_w = x_c * cos_yaw - y_c * sin_yaw + x_v;
double y_w = x_c * sin_yaw + y_c * cos_yaw + y_v;我个人习惯把这些转换函数封装成一个工具类，并且加上单元测试。因为坐标转换一旦写错，整个系统都会跑偏，而且很难排查。
2.3 Frenet坐标系：为什么需要它？
好，现在问题来了。世界坐标系和车辆坐标系虽然好用，但它们都不适合做路径规划。
为什么？
你想想看，在弯道上，用世界坐标描述一条路径，x和y都在变化，曲率也在变，规划起来很麻烦。而Frenet坐标系，说白了就是「沿着路走」的坐标系。
2.3.1 Frenet坐标系的定义
Frenet坐标系有两个轴：
- s轴（纵向）：沿着参考线（通常是车道中心线）的弧长距离
- d轴（横向）：垂直于参考线的偏移距离，左侧为正
这样一来，车辆在路上的状态就变成了 (s, d, s_dot, d_dot, s_ddot, d_ddot)。
s描述「走了多远」，d描述「偏离中心线多少」。是不是直观多了？
Frenet坐标系的最大优势：
- 纵向规划和横向规划可以解耦——你只管管好油门刹车（s方向），方向盘（d方向）交给另一个模块
- 道路约束天然满足——只要d保持在车道宽度范围内，车就不会压线
- 曲率变化被「吸收」到参考线里——规划器不用关心弯道有多急
2.3.2 世界坐标 ↔ Frenet坐标转换
这个转换稍微复杂一点，核心是找到参考线上距离当前点最近的点（匹配点）。
// 世界坐标 -> Frenet坐标（简化版）
// 输入：车辆世界坐标 (x, y)，参考线点集 ref_line[]
// 输出：Frenet坐标 (s, d)
// 1. 找到参考线上距离 (x, y) 最近的点
int nearest_idx = findNearestPoint(x, y, ref_line);
double s = ref_line[nearest_idx].s; // 累积弧长
// 2. 计算横向偏移
double dx = x - ref_line[nearest_idx].x;
double dy = y - ref_line[nearest_idx].y;
// 参考线在该点的法向量
double nx = -sin(ref_line[nearest_idx].theta);
double ny = cos(ref_line[nearest_idx].theta);
double d = dx * nx + dy * ny; // 正负表示左右
return {s, d};避坑指南：我曾经在匹配最近点时，直接用欧氏距离暴力搜索。结果参考线有5000个点，每帧都要搜索，CPU直接拉满。后来改用KD树做最近邻搜索，速度提升了两个数量级。嗯，这个优化值得做。
2.3.3 Frenet坐标系下的路径规划思路
在Frenet坐标系下做路径规划，基本套路是这样的：
- 纵向规划：给定当前s和速度，规划未来几秒的s(t)曲线——说白了就是「什么时候加速，什么时候刹车」
- 横向规划：给定当前d和横向速度，规划未来几秒的d(s)曲线——也就是「什么时候变道，什么时候回正」
- 合成轨迹：把s(t)和d(s)合起来，再转回世界坐标，就得到了一条完整的轨迹
这个框架的好处是，你可以分别优化纵向和横向的目标。比如：
- 纵向目标：保持安全跟车距离、限速内行驶、平缓加减速
- 横向目标：保持在车道中心、变道时平滑过渡、避让障碍物
我见过很多新手一上来就在世界坐标系里做规划，结果弯道上的轨迹要么切弯太狠，要么出弯太晚。换成Frenet坐标系后，这些问题迎刃而解。
2.4 三种坐标系的关系总结
| 坐标系 | 用途 | 优点 | 缺点 |
|---|---|---|---|
| 世界坐标系 | 高精地图存储、全局定位 | 绝对参考，不随时间变化 | 不直观，弯道规划困难 |
| 车辆坐标系 | 传感器感知、障碍物相对位置 | 实时性好，与传感器对齐 | 随车移动，不适合长期规划 |
| Frenet坐标系 | 路径规划、行为决策 | 纵向横向解耦，道路约束天然满足 | 依赖参考线质量，匹配计算有开销 |
实际工程中，这三个坐标系会频繁转换。我的建议是：
- 感知模块输出用车辆坐标系
- 定位模块输出用世界坐标系
- 规划模块内部用Frenet坐标系
- 控制模块输入用世界坐标系（因为控制指令需要绝对位置）
嗯，这一章的内容就到这里。坐标系这个东西，说起来简单，但真正写代码的时候，稍不注意就会翻车。下一章我们会深入Frenet坐标系下的轨迹生成算法，到时候会用到今天讲的所有内容。