2. 轨迹规划基础

在自动驾驶系统中，规划模块承担着“决策大脑”的角色。许多初学者容易将路径规划与轨迹规划混为一谈，但二者在数学表达、求解维度与工程落地上存在本质差异。本章将从核心概念出发，厘清路径与轨迹的边界，并深入解析Frenet坐标系、SL与ST语义以及轨迹的时空表示方法。这些基础概念是理解后续优化算法（如二次规划、样条曲线、EM Planner）的基石，也是工程调试中定位问题（如“为什么车辆在换道时抖动”或“为什么规划轨迹会穿越障碍物”）的关键入口。

轨迹规划基础概念关系图 路径规划（Path） 空间维度：x, y（或s, l） 无时间属性 只回答“走哪条线” 代表：A*、Hybrid A* 输出：几何点序列 轨迹规划（Trajectory） 时空维度：s, l, t 包含速度/加速度/时间 回答“何时以多快到达” 代表：EM Planner、Lattice 输出：带时间戳的状态序列 升级 Frenet坐标系 s：沿参考线弧长 l：距参考线横向偏移 优点：道路语义清晰 简化曲线拟合与约束 笛卡尔↔Frenet可互转 SL语义（空间域） SL图：s为横轴，l为纵轴 用于静态障碍物避让 车道线约束、边界检测 输出：路径几何（l = f(s)） ST语义（时空域） ST图：t为横轴，s为纵轴 用于动态障碍物速度规划 时间窗冲突检测 输出：速度曲线（s = f(t)）

上图展示了本章核心概念之间的逻辑关系：路径规划是轨迹规划的空间子集，而Frenet坐标系为两者提供了统一的语言框架，SL与ST则分别从空间和时空两个维度对规划问题进行解耦。下面我们逐一展开。

2.1 路径规划与轨迹规划：从几何到时空的跃迁

核心原理

路径规划（Path Planning）解决的是“在哪里走”的问题，其输出是一条无碰撞的几何曲线，通常表示为一系列离散点或参数化曲线（如多项式、B样条）。路径本身不携带时间信息，也不包含速度、加速度等运动学属性。例如，A*算法在栅格地图上搜索出的最短路径，或Hybrid A*在连续空间中生成的满足车辆运动学约束的曲线，都属于路径规划范畴。

轨迹规划（Trajectory Planning）则进一步解决“何时以多快的速度到达”的问题。轨迹是路径的时间参数化，即每个路径点都关联一个期望到达时刻、速度、加速度甚至加加速度（Jerk）。轨迹规划必须满足车辆动力学约束（如最大加速度、转向速率）、舒适性约束（如Jerk限制）以及动态障碍物的时空避让要求。

判断方法

在实际工程中，判断一个模块属于路径规划还是轨迹规划，可以看其输出数据结构：

  • 若输出为 std::vector<PathPoint>，其中PathPoint仅包含x、y、heading、kappa（曲率），则为路径规划。
  • 若输出为 std::vector<TrajectoryPoint>，其中TrajectoryPoint包含x、y、heading、kappa、v、a、relative_time，则为轨迹规划。

此外，从求解维度看，路径规划在二维空间（x-y或s-l）中搜索，而轨迹规划在三维空间（s-l-t）中搜索。Apollo中，路径规划器（如PublicRoadPlanner）生成候选路径，而轨迹规划器（如LatticePlanner、EMPlanner）在候选路径基础上生成带速度信息的轨迹。

案例拆解：换道场景中的路径与轨迹

假设车辆以72km/h（20m/s）行驶在高速公路上，前方500m处有一辆慢车以15m/s行驶，自车需要向左换道超越。

  • 路径规划阶段：生成一条从当前车道中心线平滑过渡到左侧车道中心线的几何曲线。该曲线需满足曲率连续、不超出道路边界、不与静态障碍物（如护栏）碰撞。此时，路径上每个点只有位置信息，没有时间标签。
  • 轨迹规划阶段：在已生成的路径上，分配时间信息。例如，前2秒内完成横向偏移，同时纵向速度从20m/s逐渐调整到25m/s以完成超车。轨迹上每个点都包含期望到达时间、速度、加速度。若在t=3s时，左侧车道后方有一辆快速接近的车辆，轨迹规划器需要调整速度曲线，避免时空碰撞。

这个案例说明，路径规划只关心“线形是否安全”，而轨迹规划关心“按这个速度走是否安全”。一个常见的工程误区是：只做路径规划而不做轨迹规划，导致车辆在弯道中速度过快或过慢，或者无法应对动态切入的障碍物。

风险提示：在Apollo中，如果仅使用路径规划结果直接控制车辆，由于缺少时间维度，无法预测未来时刻的车辆位置，也无法进行动态避障。这会导致在汇入、十字路口等场景中出现碰撞风险。因此，量产系统中必须使用完整的轨迹规划模块。

总结复盘

维度路径规划轨迹规划
输出维度空间几何（x,y,heading,kappa）时空状态（x,y,heading,kappa,v,a,t）
约束类型几何约束、静态障碍物动力学约束、动态障碍物、时间窗
典型算法A*, Hybrid A*, 样条拟合Lattice, EM Planner, 二次规划
调试关注点曲率突变、边界越界速度抖动、加速度超限、碰撞时间

2.2 Frenet坐标系：让规划问题“道路化”

核心原理

Frenet坐标系是一种以参考线（通常是道路中心线或车道中心线）为基准的曲线坐标系。它由两个正交方向组成：s方向（纵向，沿参考线的弧长）和l方向（横向，垂直于参考线的偏移距离）。在Frenet坐标系中，车辆位置表示为(s, l)，其中s表示车辆在参考线上的投影点对应的弧长，l表示车辆到投影点的有向距离（左侧为正，右侧为负）。

Frenet坐标系的核心优势在于：它将车辆运动分解为纵向运动（沿s方向）和横向运动（沿l方向），使得道路边界、车道线等约束可以简单地表示为l的上下界。例如，车道宽度为3.5m，则l的约束为[-1.75, 1.75]（以车道中心线为0）。这种解耦大大简化了优化问题的建模。

坐标转换

实际应用中，车辆状态通常由笛卡尔坐标（x, y, heading, curvature）表示，而规划算法在Frenet坐标系中工作，因此需要频繁进行坐标转换。Apollo中提供了CartesianFrenetConverter工具类，核心转换公式如下：

// 笛卡尔转Frenet
s = s0 + ∫(cos(θ - θr) / (1 - κr * l)) ds
l = (x - xr) * sin(θr) - (y - yr) * cos(θr)

// Frenet转笛卡尔
x = xr - l * sin(θr)
y = yr + l * cos(θr)
θ = arctan( (l' * cos(θr)) / (1 - κr * l) ) + θr

其中，下标r表示参考线上的投影点，κr为参考线曲率，θr为参考线航向角。需要注意的是，当参考线曲率较大或车辆横向偏移较大时，转换误差会增大，因此需要定期重新投影。

实战场景：弯道中的横向控制

在高速弯道中，如果使用笛卡尔坐标直接规划，道路边界约束会变成复杂的非线性不等式。而在Frenet坐标系中，道路边界约束简化为l_min ≤ l ≤ l_max，同时曲率约束可以表示为κ = (l'' - (κr + l * κr^2) * tan(θ - θr)) / ...。这使得优化求解器能够高效处理。

一个典型的工程问题是：当车辆从直道进入弯道时，参考线的曲率发生突变，导致Frenet坐标下的l方向出现跳变。解决方法是使用平滑的参考线（如经过平滑处理的道路中心线），并在转换时采用迭代投影算法（如牛顿法）保证投影点收敛。

经验提醒：在Apollo中，参考线提供器（ReferenceLineProvider）会定期平滑参考线，并缓存投影点信息。调试时，如果发现轨迹在弯道入口处出现横向抖动，优先检查参考线曲率是否连续，以及投影点是否发生跳变。

2.3 SL与ST语义：空间与时空的解耦表达

SL语义（空间域）

SL图（Station-Lateral）以s为横轴、l为纵轴，将道路展开为二维平面。SL图是路径规划的核心工具，它直观地展示了车辆沿道路纵向位置与横向偏移的关系。在SL图中，静态障碍物被投影为占据一定s和l范围的矩形区域，道路边界表现为l的上下包络线。路径规划的目标就是在SL图中寻找一条从起点(s0, l0)到终点(s1, l1)的曲线，满足避障和边界约束。

SL语义的关键优势在于：它将三维空间问题降维为二维问题，使得动态规划（DP）或样条拟合等算法可以高效求解。Apollo的EM Planner在SL阶段使用动态规划生成粗略路径，再用二次规划进行平滑。

ST语义（时空域）

ST图（Station-Time）以t为横轴、s为纵轴，描述车辆纵向位置随时间的变化。ST图是速度规划的核心工具。动态障碍物的轨迹被投影为ST图中的障碍区域（通常为平行四边形或梯形），表示在特定时间段内，该s区间被障碍物占据。速度规划的目标是生成一条s(t)曲线，避开所有障碍区域，同时满足速度、加速度限制。

ST语义将“何时能走”的问题从空间避障中解耦出来。例如，前方车辆在t=2s到t=4s之间位于s=100m到s=120m区间，则自车的s(t)曲线必须避开这个时空窗口，可以选择减速等待（s曲线在t=2s时低于100m）或提前变道（在SL阶段解决）。

对比与协同

维度SL图ST图
坐标轴s（横）、l（纵）t（横）、s（纵）
主要用途路径几何规划速度/时间规划
障碍物类型静态障碍物、低速动态障碍物（近似静态）动态障碍物（含速度信息）
输出l = f(s) 路径曲线s = f(t) 速度曲线
典型算法DP + QPDP + QP 或 样条拟合

在Apollo的EM Planner中，SL与ST是串行执行的：先通过SL规划确定路径几何，再在固定路径上通过ST规划确定速度曲线。这种解耦策略大幅降低了求解复杂度，但也带来一个固有限制——当动态障碍物与路径强耦合时（如障碍物横跨多个车道），SL阶段无法准确建模，需要依赖更高层的决策模块（如换道决策）来切换参考线。

2.4 轨迹的时空表示：从离散点到连续函数

核心原理

一条完整的轨迹在数学上可以表示为时间t的函数：状态向量 = [s(t), l(t), v_s(t), v_l(t), a_s(t), a_l(t)]。在实际工程中，轨迹通常以离散点序列存储，每个点包含时间戳、位置、速度、加速度等信息。Apollo的DiscretizedTrajectory类就是这种表示的典型实现，它内部维护一个std::vector<TrajectoryPoint>，并提供插值查询接口。

轨迹的时空表示需要满足以下性质：

  • 时间一致性：相邻点的时间间隔应均匀（如0.1s），便于控制模块按固定周期采样。
  • 运动学连续性：位置、速度、加速度在相邻点之间应连续，避免突变导致控制抖动。
  • 可执行性：轨迹上的每个点都应满足车辆动力学约束（如最大转向角、最大加速度）。

轨迹评估指标

在轨迹生成后，需要从多个维度评估其质量：

  1. 安全性：与所有障碍物的最小距离是否大于安全阈值（如0.5m）。
  2. 舒适性：最大Jerk（加加速度）是否小于舒适阈值（如2m/s³）。
  3. 效率：到达目的地的时间或平均速度是否满足任务要求。
  4. 可行性：轨迹是否满足车辆运动学约束（如曲率不超过最大转向曲率）。

案例拆解：十字路口左转轨迹

假设车辆在十字路口需要左转，路口宽度为30m，自车以10m/s减速进入路口。轨迹规划器需要生成一条从当前车道到目标车道的时空轨迹。

  • SL阶段：在SL图中，规划出一条平滑的左转路径，s从0增长到约50m（含转弯半径），l从0变化到-3.5m（进入目标车道）。
  • ST阶段：在ST图中，检测到对向直行车辆在t=3s时通过路口中心，因此自车的s(t)曲线需要在t=3s时避开冲突区域，选择减速至5m/s等待，待对向车辆通过后再加速。
  • 轨迹合成：将SL路径与ST速度曲线合成，得到完整的时空轨迹。每个轨迹点包含时间戳、笛卡尔坐标、速度、加速度。

在实车调试中，一个常见问题是：轨迹在转弯处出现速度波动，原因是ST规划中速度曲线与SL路径的曲率耦合不足。解决方法是引入曲率-速度耦合约束，即在曲率较大的区域限制最大速度。

复盘要点：轨迹的时空表示不仅是数据结构问题，更是规划算法的核心抽象。理解SL与ST的解耦逻辑，有助于快速定位问题是出在路径几何（SL）还是速度分配（ST）。在Apollo中，可以通过可视化工具分别显示SL图和ST图，观察障碍物投影和规划曲线，从而高效调试。

常见误区与总结

  • 误区一：将路径规划等同于轨迹规划。忽略时间维度会导致动态避障失效。
  • 误区二：在笛卡尔坐标下直接施加道路边界约束。这会导致非线性优化求解困难，应转换到Frenet坐标系。
  • 误区三：SL与ST完全独立。实际上，路径的曲率会影响速度规划的上限，需要迭代或耦合优化。
  • 误区四：轨迹点越密越好。过密的点会增加计算和通信开销，且对控制模块无显著收益，通常0.1s间隔即可。
← 上一章 📖 返回目录 下一章 →