一、参考路径生成：从全局规划到局部跟踪的桥梁

大家好，我是你们的老朋友。今天咱们聊聊参考路径生成这个话题。

说实话，我刚入行那会儿，觉得路径规划就是画条线，跟踪就是沿着线走。后来在实车上栽了跟头才发现——路径生成的质量，直接决定了跟踪系统能不能跑起来。你想想看，一条锯齿状的路径，再牛的控制器也救不回来。

1.1 全局路径规划：到底在规划什么？

全局路径规划，说白了就是回答一个问题：从A点到B点，怎么走？

但这里有个坑——我们规划的是一条几何路径，而不是一条轨迹。路径只关心空间位置（x, y），不关心时间。轨迹才关心速度、加速度这些时间属性。

我在项目里见过不少新人，上来就把路径当轨迹用，结果跟踪的时候发现速度控制一塌糊涂。嗯，这里要分清楚。

常见的全局路径规划算法有这些：

Dijkstra：经典的最短路径算法，适合静态环境
A*：带启发式搜索，比Dijkstra快很多
RRT：快速随机搜索树，适合高维空间
Hybrid A*：考虑车辆运动学约束的A*，我特别喜欢用这个

不过，不管用哪种算法，最终输出的都是一系列离散的路径点：

# 全局路径规划输出的原始路径点
path_points = [
    (0.0, 0.0),
    (2.5, 1.2),
    (5.0, 3.8),
    (7.8, 5.0),
    (10.0, 5.0)
]

你看，这些点之间距离不均匀，有的近有的远。直接拿给跟踪控制器用？那肯定不行。控制器会像喝醉了一样左右摇摆。

1.2 路径离散化与插值：把粗糙的路径变平滑

为什么要做插值？原因很简单——原始路径点太稀疏了。

我曾经在一个项目中，全局规划器只给了每10米一个点。车辆在高速行驶时，两个点之间的路径完全靠控制器自己猜。结果呢？车辆在弯道里直接冲出去了。那次教训让我明白：插值不是锦上添花，而是刚需。

线性插值：简单但够用

线性插值是最基础的方法。在两个点之间画一条直线，然后等间距取点。

def linear_interpolate(p1, p2, num_points):
    """线性插值：在p1和p2之间生成num_points个点"""
    x1, y1 = p1
    x2, y2 = p2
    points = []
    for i in range(num_points):
        t = i / (num_points - 1)
        x = x1 + t * (x2 - x1)
        y = y1 + t * (y2 - y1)
        points.append((x, y))
    return points

# 示例：在(0,0)和(10,5)之间插值出10个点
interpolated = linear_interpolate((0,0), (10,5), 10)
print(interpolated)

线性插值的优点是快，缺点也明显——在弯道处会丢失曲率信息。你想想看，如果原始路径点刚好在弯道的两端，线性插值会直接把弯道拉直。这显然不行。

避坑指南：我曾经在一个园区低速项目中用了线性插值，结果车辆在直角弯处总是切内线，差点撞上路沿。后来换成样条插值才解决问题。所以，有弯道的地方，别用线性插值。

样条插值：弯道救星

样条插值，尤其是三次样条，能保证路径的一阶和二阶导数连续。这意味着什么？路径是光滑的，曲率是连续的。车辆转向时不会出现突变。

import numpy as np
from scipy.interpolate import CubicSpline

# 原始路径点
x = np.array([0.0, 2.5, 5.0, 7.8, 10.0])
y = np.array([0.0, 1.2, 3.8, 5.0, 5.0])

# 创建三次样条插值
cs = CubicSpline(x, y)

# 在更密集的x坐标上求值
x_dense = np.linspace(0, 10, 50)
y_dense = cs(x_dense)

# 现在(x_dense, y_dense)就是平滑的路径点了

我个人习惯用三次样条，因为它能在平滑性和计算效率之间取得很好的平衡。五次样条更平滑，但计算量也更大，一般用在高速场景。

小技巧：如果你发现样条插值在路径端点处出现震荡（这叫龙格现象），可以试试在两端添加虚拟点来约束边界条件。我在做泊车路径时经常用这招。

1.3 路径坐标系转换：从全局到局部

路径生成好了，但控制器需要的是相对于路径的偏差，而不是全局坐标。这就涉及到坐标系转换。

常用的坐标系有三种：

坐标系	说明	用途
全局坐标系	固定的大地坐标系（如UTM）	路径规划、地图构建
车辆坐标系	以车辆质心为原点，前进方向为x轴	传感器数据、控制指令
Frenet坐标系	沿路径的纵向距离s和横向偏移d	路径跟踪、行为规划

Frenet坐标系是我最常用的。它把问题简化成两个独立的一维问题：沿着路径走多远（s），偏离路径多远（d）。

转换的核心思路是：找到车辆当前位置在路径上的最近投影点。

def global_to_frenet(vehicle_pos, path_points):
    """
    将车辆全局坐标转换为Frenet坐标
    vehicle_pos: (x, y) 车辆全局位置
    path_points: [(x0,y0), (x1,y1), ...] 路径点列表
    返回: (s, d) 纵向距离和横向偏移
    """
    # 找到最近路径点
    min_dist = float('inf')
    nearest_idx = 0
    for i, pt in enumerate(path_points):
        dist = np.sqrt((vehicle_pos[0] - pt[0])**2 + 
                       (vehicle_pos[1] - pt[1])**2)
        if dist < min_dist:
            min_dist = dist
            nearest_idx = i
    
    # 计算纵向距离s（累积路径长度）
    s = 0
    for i in range(nearest_idx):
        dx = path_points[i+1][0] - path_points[i][0]
        dy = path_points[i+1][1] - path_points[i][1]
        s += np.sqrt(dx**2 + dy**2)
    
    # 横向偏移d（带符号，左正右负）
    # 这里简化处理，实际需要计算投影点到路径的法向距离
    d = min_dist  # 简化：用最近距离近似
    
    return s, d

  核心要点：Frenet坐标系转换的精度，取决于路径的离散密度和投影算法的准确性。我建议路径点间距不要超过0.5米，否则投影误差会累积。

知识体系总览

下面这张图，是我自己总结的参考路径生成的知识结构。你看一眼就能明白各个模块之间的关系：

从这张图你能看到，整个流程是串行的：全局规划输出粗糙路径 → 插值变成平滑路径 → 坐标系转换供控制器使用。每一步都影响最终效果。

  我的经验之谈：很多团队花大量精力调控制器参数，却忽略了路径质量。其实，一条好的参考路径，能解决80%的跟踪问题。剩下的20%才是控制器的活儿。

好了，这一章的内容就到这里。路径生成是基础中的基础，但也是最容易被忽视的环节。希望你能从我的踩坑经历中有所收获。

可可精品资料库

📚 课程目录

一、参考路径生成：从全局规划到局部跟踪的桥梁

1.1 全局路径规划：到底在规划什么？

1.2 路径离散化与插值：把粗糙的路径变平滑

线性插值：简单但够用

样条插值：弯道救星

1.3 路径坐标系转换：从全局到局部

知识体系总览