第4章运动规划基础

4、运动规划基础：路径规划与轨迹规划的区别、Frenet坐标系

大家好，欢迎来到运动规划基础这一讲。

说实话，很多新手刚接触自动驾驶时，容易把路径规划和轨迹规划混为一谈。我当年刚入行时也犯过这个错，直到第一次实车测试，车在弯道里“画龙”……嗯，那次教训挺深刻的。今天我们就彻底把这两个概念掰扯清楚，再聊聊那个让规划变得简单的坐标系——Frenet坐标系。

先问一个问题：你开车从A点到B点，需要知道什么？

第一，你得知道走哪条路。第二，你得知道每个时刻油门踩多少、方向盘打多少。

路径规划解决的就是第一个问题——“走哪条路”。它只关心空间上的几何形状，不关心时间。比如，一条路径就是一系列的点 (x, y) 连成的线，它告诉你“从这里到那里，经过这些位置”。

轨迹规划解决的是第二个问题——“怎么走”。它不仅要给出路径，还要给出每个时刻的速度、加速度，甚至方向盘转角。轨迹是带时间戳的路径，即 (x, y, t, v, a) 的序列。

  一句话总结：路径是“空间上的线”，轨迹是“时间上的线”。路径只关心几何，轨迹关心运动学。

我在项目中遇到过这样一个场景：路径规划给出了一条完美的避障路径，但轨迹规划没跟上，导致车辆在避障时急刹车，乘客差点撞到前挡风玻璃。你看，路径再漂亮，没有好的轨迹来执行，体验就是零。

我的习惯：在实际工程中，我通常先跑路径规划，得到一个粗路径，然后用轨迹规划在这个路径上做“速度规划”。这样分工明确，调试起来也方便。

好了，现在问题来了：路径和轨迹都是在笛卡尔坐标系（x, y）下描述的。但你想过没有，在弯道上规划时，用笛卡尔坐标有多痛苦？

举个例子：一条弯曲的道路，你在笛卡尔坐标系下描述“沿着车道中心线行驶”，需要写一堆复杂的曲线方程。而且，判断车辆是否偏离车道中心，还得算点到曲线的距离，麻烦得很。

Frenet坐标系就是来解决这个问题的。它把问题从“二维平面”降维到“一维道路”上。

Frenet坐标系以道路中心线为参考线，定义了两个轴：

这样一来，任何车辆的位置都可以用 (s, d) 来表示。是不是简单多了？

  核心思想：把“沿着路走”的问题，变成“沿着一条直线（s轴）走，同时控制左右偏移（d轴）”的问题。

我记得第一次在项目中用Frenet坐标系时，感觉就像打开了新世界的大门。以前在笛卡尔坐标系下写避障逻辑，要考虑各种几何变换，代码又臭又长。换成Frenet后，逻辑清晰得像白纸黑字。

原因有三点，我一个个说：

解耦纵向和横向控制：在Frenet坐标系下，你可以独立规划“纵向速度”（s方向）和“横向偏移”（d方向）。比如，超车时，你只需要在d方向做一个“S形”偏移，同时在s方向保持速度。这两个问题互不干扰。
简化约束表达：道路边界、车道线在Frenet坐标系下就是简单的常数。比如“车道宽度3.5米”，在d轴上就是“d ∈ [-1.75, 1.75]”。不用再算复杂的几何距离了。
自然适配道路形状：不管道路是直道、弯道还是S弯，Frenet坐标系下的参考线始终是“拉直”的。你规划出来的轨迹天然就是沿着道路的。

注意：Frenet坐标系虽然好用，但它依赖参考线。如果参考线质量不好（比如有抖动、不连续），那规划出来的轨迹也会出问题。我曾经因为参考线采样不均匀，导致车辆在弯道里来回摆动……后来加了一个参考线平滑模块才解决。

实际控制车辆时，最终还是要用笛卡尔坐标。所以我们需要一个转换公式。

假设参考线上某点的切向角为 θ_r，曲率为 κ_r，那么从 (s, d) 到 (x, y) 的转换是：

x = x_r(s) - d * sin(θ_r(s))
y = y_r(s) + d * cos(θ_r(s))

其中 (x_r, y_r) 是参考线上对应 s 处的点。

反过来，从 (x, y) 到 (s, d) 的转换稍微复杂一点，需要找到参考线上距离当前点最近的点，然后计算 s 和 d。这个“最近点投影”是Frenet坐标系里最耗时的操作，我建议用二分查找或者KD树来加速。

避坑指南：我曾经在投影时没处理好“曲率突变”的情况，导致d值跳变，轨迹出现尖点。后来我加了一个“投影一致性检查”，确保相邻帧的s值是单调递增的。这个细节很重要，大家写代码时一定要留意。

这一讲我们聊了两个核心概念：

下一讲，我们会深入Frenet坐标系下的轨迹生成算法，聊聊怎么用多项式生成平滑的换道轨迹。到时候我会分享一个我在实车上踩过的坑——关于“jerk最小化”的误解。嗯，敬请期待。