2. 参考路径生成

各位同学，今天我们来聊聊参考路径生成。这是整个路径跟踪策略的起点，也是我当年入行时踩坑最多的地方。

你想想看，一辆自动驾驶车，它怎么知道该往哪走？靠的就是参考路径。说白了，就是给车画一条「理想轨迹」。但这条轨迹不是随便画的，它要满足几个条件：一是全局可达，二是平滑可跟踪，三是离散可计算。

我个人习惯把参考路径生成拆成三个步骤：全局规划 → 平滑插值 → 离散采样。下面我们一个一个说。

2.1 全局路径规划基础

全局路径规划，就是在地图上找一条从起点到终点的可行路径。嗯，这里要注意，它不关心车辆动力学，只关心「能不能走通」。

常用的算法有 Dijkstra 和 A*。我个人更偏爱 A*，因为它加了启发式搜索，效率高不少。我在项目中遇到过一个问题：用 Dijkstra 跑 1km 的地图，耗时 200ms，换成 A* 后直接降到 30ms。你想想看，这差距在高速场景下意味着什么？

但这里有个坑——全局规划出来的路径，往往是折线。直接让车去跟踪折线？那方向盘会疯狂抖动，乘客直接晕车。所以我们需要第二步：平滑与插值。

2.2 路径平滑与插值

平滑的目的，就是把折线变成一条曲率连续的曲线。常用的工具有两种：样条曲线和贝塞尔曲线。

2.2.1 样条曲线（Spline）

样条曲线，说白了就是用多段低次多项式拼接成一条光滑曲线。最常用的是三次样条（Cubic Spline），它保证了一阶导和二阶导连续。

我曾经在高速测试中，直接用三次样条插值路径，结果发现曲率突变点还是会让车辆横向加速度超限。后来我改用五次样条，效果就好多了。为什么？因为五次样条能保证三阶导连续，加速度变化更平缓。

// 三次样条插值示例（Python 伪代码）
def cubic_spline_interpolate(waypoints):
    # waypoints: [(x0,y0), (x1,y1), ..., (xn,yn)]
    n = len(waypoints)
    # 构建三对角矩阵求解系数
    # 输出每段的多项式系数 a,b,c,d
    return coefficients  # 每段: f(x) = a + b*x + c*x^2 + d*x^3

2.2.2 贝塞尔曲线（Bezier Curve）

贝塞尔曲线是另一种思路。它通过控制点来「拉」出一条曲线。优点是直观，拖动控制点就能改变形状。缺点是全局性——移动一个控制点，整条曲线都会变。

我记得在做一个泊车路径规划时，用了三阶贝塞尔曲线。因为泊车路径短，控制点少，调起来很方便。但在高速场景下，路径长、控制点多，贝塞尔曲线就不太合适了——你改一个点，远端路径也跟着变，这谁受得了？

2.3 路径离散化与采样

平滑完的曲线是连续的数学表达式，但控制器没法直接吃连续函数。它需要离散的点——这就是路径离散化与采样要做的事。

常见的采样方式有两种：

• 等距采样： 沿着路径每隔固定距离（比如 0.5m）取一个点。简单，但弯道处可能丢失细节。
• 等时间采样： 按照车辆以参考速度行驶，每隔固定时间（比如 0.1s）取一个点。更符合控制需求，但计算稍复杂。

离散化后的路径，通常存储为一个数组，每个元素包含：

struct PathPoint {
    double x;           // 全局 x 坐标
    double y;           // 全局 y 坐标
    double theta;       // 航向角（弧度）
    double kappa;       // 曲率（1/m）
    double s;           // 弧长（从起点算起）
};

嗯，这里要注意，曲率 kappa 一定要算准。它是后续横向控制的关键输入。我习惯用三点法估算曲率：取当前点、前一个点、后一个点，用外接圆半径的倒数来算。