2. 坐标系与坐标变换

做路径规划，说白了就是回答三个问题：我在哪？我要去哪？怎么去？这三个问题，每一个都离不开坐标系。我刚开始做自动驾驶那会儿，就吃过坐标系搞混的亏——明明规划出来的路径挺漂亮，结果车一跑就偏，查了半天才发现是坐标转换的矩阵写反了。嗯，从那以后，我对坐标系这块就格外上心。

今天咱们就聊聊路径规划里最常用的三种坐标系：全局坐标系、车辆坐标系、Frenet坐标系。尤其是Frenet坐标系，它在处理道路边界约束时，简直是神器。

2.1 全局坐标系：给世界一个绝对位置

全局坐标系，也叫世界坐标系。它是个固定不动的参考系，通常用经纬度（WGS84）或者UTM投影坐标来表示。你可以把它想象成一张大地图，路上的每个点、每辆车、每个行人，都能用唯一的(x, y)坐标标出来。

在实际工程中，我们一般用UTM坐标。为什么？因为它是平面直角坐标，计算距离、角度都方便。经纬度是球面坐标，算个两点距离还得上大圆公式，太麻烦。

2.2 车辆坐标系：以车为本的视角

车辆坐标系就灵活多了。它以车辆自身为原点，通常定义如下：

• x轴：指向车辆正前方
• y轴：指向车辆左侧（也有用右侧的，看具体标准）
• z轴：垂直向上

这个坐标系最大的好处是——传感器数据天然就在这个坐标系里。激光雷达扫出来的点云，毫米波雷达检测到的障碍物，都是以车为中心报告的。你想想看，如果每次都要把传感器数据先转到全局坐标再处理，那延迟得多高？

我个人习惯在车辆坐标系里做局部路径规划，比如避障、换道。因为这时候我只关心“障碍物在我左边几米”，而不关心“障碍物在经纬度多少”。

2.3 Frenet坐标系：沿着道路走

好了，重点来了。Frenet坐标系，我愿称之为“路径规划者的瑞士军刀”。

它用两个变量描述一个点：

• s：沿着参考线的纵向距离（弧长）
• d：垂直于参考线的横向偏移（左正右负或反之）

为什么说它好用？因为道路是有边界的。在全局坐标系里，你很难直观地知道“这个点离左边界多远”。但在Frenet坐标系里，d值直接告诉你横向位置。你只要保证d值在道路左右边界对应的范围内，路径就一定不会出界。

我在项目中遇到过一个问题：用全局坐标做路径优化，结果优化出来的路径虽然平滑，但有一小段压了路沿。查了半天才发现是约束条件写得太复杂，没覆盖到那个弯道。后来换成Frenet坐标系，横向约束直接写成 d_min ≤ d ≤ d_max，简单粗暴，再也没出过问题。

2.4 坐标变换：从全局到Frenet

光有坐标系还不够，你得能在它们之间自由切换。最常用的变换有两个：

2.4.1 全局坐标 → Frenet坐标

给定一个全局点 (x, y) 和一条参考线（通常是道路中心线），求它的 (s, d)。

步骤很简单：

1. 在参考线上找到离 (x, y) 最近的点，记为 (x_r, y_r)
2. 这个最近点对应的弧长就是 s
3. 计算 (x, y) 到 (x_r, y_r) 的向量，投影到参考线的法线方向上，得到 d

代码实现大概长这样：

def global_to_frenet(x, y, ref_line):
    # ref_line: 参考线上的离散点列表 [(x0,y0), (x1,y1), ...]
    # 1. 找最近点
    min_dist = float('inf')
    nearest_idx = 0
    for i, (rx, ry) in enumerate(ref_line):
        dist = (x - rx)**2 + (y - ry)**2
        if dist < min_dist:
            min_dist = dist
            nearest_idx = i
    
    # 2. 计算s（累加弧长）
    s = 0
    for i in range(nearest_idx):
        dx = ref_line[i+1][0] - ref_line[i][0]
        dy = ref_line[i+1][1] - ref_line[i][1]
        s += math.sqrt(dx*dx + dy*dy)
    
    # 3. 计算d（横向偏移，带符号）
    rx, ry = ref_line[nearest_idx]
    # 参考线在该点的切线方向
    if nearest_idx < len(ref_line) - 1:
        dx = ref_line[nearest_idx+1][0] - rx
        dy = ref_line[nearest_idx+1][1] - ry
    else:
        dx = rx - ref_line[nearest_idx-1][0]
        dy = ry - ref_line[nearest_idx-1][1]
    
    # 法线方向（左转90度）
    nx, ny = -dy, dx
    norm = math.sqrt(nx*nx + ny*ny)
    nx, ny = nx/norm, ny/norm
    
    d = (x - rx)*nx + (y - ry)*ny
    
    return s, d

2.4.2 Frenet坐标 → 全局坐标

反过来，给定 (s, d) 和参考线，求 (x, y)。这个更简单：

1. 找到参考线上弧长为 s 的点 (x_r, y_r)
2. 计算该点的法线方向 (n_x, n_y)
3. 全局点 = 参考点 + d × 法线方向

def frenet_to_global(s, d, ref_line, cum_lengths):
    # cum_lengths: 参考线上每个点对应的累积弧长
    # 1. 找到s对应的参考点（插值）
    idx = np.searchsorted(cum_lengths, s) - 1
    idx = max(0, min(idx, len(ref_line)-2))
    
    # 线性插值
    ratio = (s - cum_lengths[idx]) / (cum_lengths[idx+1] - cum_lengths[idx])
    x_r = ref_line[idx][0] + ratio * (ref_line[idx+1][0] - ref_line[idx][0])
    y_r = ref_line[idx][1] + ratio * (ref_line[idx+1][1] - ref_line[idx][1])
    
    # 2. 法线方向
    dx = ref_line[idx+1][0] - ref_line[idx][0]
    dy = ref_line[idx+1][1] - ref_line[idx][1]
    nx, ny = -dy, dx
    norm = math.sqrt(nx*nx + ny*ny)
    nx, ny = nx/norm, ny/norm
    
    # 3. 全局坐标
    x = x_r + d * nx
    y = y_r + d * ny
    
    return x, y

2.5 边界约束下的坐标变换：实战中的坑

理论说完了，咱们聊聊实战。道路边界约束，说白了就是：你的路径不能超出左右路沿。

在Frenet坐标系下，这个约束写起来特别直观：

d_left_boundary(s) ≤ d(s) ≤ d_right_boundary(s)

其中 d_left_boundary(s) 和 d_right_boundary(s) 是沿着参考线变化的边界函数。弯道内侧的边界d值小，外侧的d值大。

我曾经踩过一个坑：边界函数直接用固定值。结果在弯道处，规划出来的路径虽然满足 |d| ≤ 3.5m，但实际已经压到路沿了。为什么？因为弯道内侧的可用宽度比直道窄！

正确的做法是：

• 从高精地图里提取道路左右边界的离散点
• 把这些点也转换到Frenet坐标系下
• 对每个s，插值出对应的 d_left 和 d_right

这样，不管道路怎么弯，你的约束都是准确的。

2.6 三种坐标系的对比

2.7 知识体系总览

下面这张图，帮你把三种坐标系的关系和变换路径理清楚：

嗯，坐标系这块内容不少，但它是路径规划的基石。你想想看，如果坐标系都搞不清楚，后面做优化、做约束，那不都是空中楼阁吗？我个人建议，刚开始接触的同学，先拿一小段直道和弯道数据，手写一遍三种坐标系的互相转换，跑通之后再用到自己的规划器里。这一步走扎实了，后面会顺很多。

可可精品资料库