规划与轨迹生成（控制接口）

决策说“先低速探出，条件满足后汇入”。这句话离方向盘还差一整章。

路径不是轨迹

先分清两个词：

• 路径回答“车从空间的哪里走”，是一条线；
• 轨迹回答“每个时刻车在哪里、速度多少、加速度多少”，是一条带时间戳的线。

决策交下来的“探出后汇入”连路径都不是，只是意图加一组约束。底层控制要的是轨迹，而且是一条避开障碍、守住交规、车辆物理上做得出来、乘客坐着不难受的轨迹。

把意图变成轨迹，经典做法有三条路线：

这一章依次讲这三条路线，再讲它们共同的失效边界怎么把我们逼向第四条路，最后说清规划的终点：它到底把什么交给底层控制。

搜索：A* 为什么开不了车

最容易想到的办法：把地图打成格子，用 A* 搜一条最短路。问题马上出现：A* 不管车辆运动学。它搜出来的折线可能要求车原地转向 90 度，而真实车辆的前轮转角有上限、转弯有最小半径。路径几何上存在，物理上开不出来。

Hybrid A* 的“Hybrid”就是来修这个的：

• 节点扩展用连续的车辆运动学模型：从当前状态，按几个离散的前轮转角各往前滚一小段，长出来的每个后继都是车真能到达的状态；
• 代价计算仍用离散栅格；
• 提前终止：随时试着用 Dubins 或 Reeds-Shepp 曲线直接连到目标，没有碰撞就收工。这两类曲线是“转弯半径受限时，连接两个位姿的最短曲线”，Dubins 只能前进，最优解是 6 种组合之一；Reeds-Shepp 允许倒车，扩到 48 种；
• 两个启发函数：一个只管运动学、不管障碍，一个只管障碍、不管运动学。

为什么要两个？因为它们各有一类盲区：

• 只管运动学的那个，知道目标的朝向，但看不见障碍，一遇到死路就会把代价严重算低；
• 只管障碍的那个，知道哪里是死路，但不管朝向，朝向对不上时同样会把代价严重算低。

两个各补对方的短板，所以要一起用。两者都不会高估代价，原论文取两者中较大的那个。

Hybrid A* 擅长非结构化场景：停车场、掉头、狭窄空间。结构化道路上，有更快的路线。

采样：一条定理撑起一个流派

结构化道路的第一步是换坐标系。Frenét 坐标沿参考线展开，把运动拆成纵向位移 s 和横向偏移 l。高速时，横向的动作幅度比纵向小得多，这么一拆，“沿路开”就变成了两个近似独立的一维问题。低速时两者幅度相当，硬拆会丢掉运动学约束，这时要把横向偏移写成纵向位移 s 的函数，再沿 s 采样。

然后是整个采样流派的理论支柱，一条五次多项式定理：让 jerk（加加速度，衡量舒适性的核心指标）平方积分最小的最优轨迹，是五次多项式。这条定理的实际价值是降维：本来“找最优轨迹”是在无穷维的函数空间里做优化，现在变成了——采样一批终止状态，每个终止状态配一条五次多项式，逐条算代价，挑最好的。可枚举、可并行，每条都自带平滑性。

纵向的终止状态，按工况定义得很直白：

• 跟车：目标位置 = 前车位置 − 安全距离，安全距离 = 固定间距 + 时距 × 前车车速；
• 变道：取目标车道前后两车的中点；
• 停车：目标速度恒为零。

横向、纵向各自采样，再组合起来，检查碰撞，检查速度、加速度和曲率有没有超限，选联合代价最低的一条。

这里有一道缝值得记下：横向和纵向分开采样，合成轨迹的曲率不一定符合车辆约束。组合后逐条检查、超限就丢，是事后筛，不是事前保证。这是解耦架构的裂缝之一，后面还会裂得更大。

优化：DP 画圈，QP 填线

第三条路线以百度 Apollo 的 EM Planner 为代表，结构是解耦 + 两段式：路径和速度分开规划，每一边都先做动态规划（DP），再做二次规划（QP）。名字里的“EM”指交替的两步：E 步把障碍物投影到一个坐标系里，M 步在这个坐标系里优化。

这里有一个容易看错的架构要点。光看“DP Path”这个名字，会以为 DP 的输出只是“一条粗糙路径”，其实 DP 真正的产物是三件套：

• 一个粗解；
• 一条可行走廊（Feasible Tunnel）；
• 一组离散决策，比如从障碍物左边绕还是右边绕。

SL 空间里的 DP 在做“左绕还是右绕”的选择，同时圈出一条凸的可行域；QP 随后在这条凸走廊里求平滑解。一句话概括：DP 生成凸空间，QP 在凸空间里求解。DP 负责在非凸的世界里做离散选择，QP 负责在凸空间里精细求解。

速度侧的舞台是 ST 图：横轴是时间，纵轴是沿路的纵向位移。他车的预测轨迹在这张图上投出一条条斜的占用带，自车的速度曲线要从带子的缝隙里穿过去：从障碍带下方过是让行，从上方过是超车。所谓“速度决策”，就是选择从每条带子的哪一侧通过；DP 选侧并圈出可行域，QP 把折线磨成平滑曲线。

解耦的失效边界，正好落在我们的路口

EM Planner 这类 DP + QP 解耦方法，有一个明确的短板：需要横向、纵向联合动作的场景搞不定，例如窄道会车。在那种场景里，你走哪条线取决于你什么时候到，什么时候到又取决于你走哪条线。先定路径、再配速度的架构，天生给不出答案。

回到我们的汇入口，这正是一个强耦合场景：从哪个位置切进主路（横向），取决于切入车和后车让出的间隙什么时候出现（时间）；反过来，你选的切入点又会改变通过的时机。先画线再配速的规划器，在这里会反复给出“路径可行，但时机冲突”的答案。

解法是不再解耦：时空联合规划。

把 s-l 平面沿时间轴拉成三维的 s-l-t 空间，规划直接在这个空间里做。这个空间有一个漂亮的几何直觉：

• 静止的障碍物是一根竖直的柱：位置不随时间变；
• 运动的障碍物是一片斜面：位置随时间平移。

规划就是在柱和斜面之间找一条三维曲线。空间上的绕行和时间上的避让，第一次变成了同一个问题的两个投影。

代价也很直接：搜索的动作空间从“转角”变成了“转角 × 加速度”，维度乘了上去。所以时空联合的工程化版本，都在想办法把空间砍小，其中最成体系的是 SSC。

SSC：先修走廊，再铺轨迹

SSC（时空语义走廊）接在上一章的 MPDM 后面：MPDM 选出语义行为，并前向仿真出一串期望状态；SSC 沿这串状态在 s-l-t 空间里修一条走廊，再在走廊里优化轨迹。上一章的 EPSILON，运动规划层走的也是这条路。决策和规划，在这里合龙。

修走廊分四步：

每个仿真状态投进 s-l-t，就是一颗种子。相邻两颗种子围成一个小立方体，再向各个方向胀开。胀的时候碰到两类东西要处理：

• 障碍物：静态障碍、他车的预测占用、红灯占住的那段时空。碰到就停，那个方向不许再胀；
• 语义边界：某条交通约束开始或停止生效的位置，比如限速区的起点。走廊不会在这里停下，而是在边界处断开：下一个立方体只许沿 s 正向接着长，并把这一段的限速挂上去。

“语义”二字的含义就在这里：走廊不是纯几何的空腔，障碍物挡住它，交通规则把它切成段，每一段带着自己的约束。所以走廊里的轨迹天然避开预测占用、不闯红灯，每一段都满足该段的速度约束。SSC 的演示里，正好有一个“道路施工导致拥堵时汇入”的场景，和我们的路口几乎一样。

轨迹优化用分段贝塞尔曲线加 QP，其中藏着一个很优雅的技巧：贝塞尔曲线有凸包性质，整条曲线必然落在它的控制点围成的凸包里。于是“轨迹上无穷多个点都要在走廊内”这个没法直接写的约束，变成了“有限个控制点在走廊内”的线性约束。无穷变有限，QP 才解得动。

规划交付什么：控制接口

走到这里，汇入口的规划是这样落地的：

1. 决策给出“探出”策略后，规划器在 s-l-t 空间里检查切入车和后车的占用；
2. 生成一条低速越过停止线、任何时刻都能在可见距离内刹停的探出轨迹；
3. 确认间隙后，再接一段汇入曲线；雨后路面湿滑，目标加速度取得更低，制动余量留得更大；
4. 每个规划周期重算一次，预测一更新，旧轨迹就作废。

最终交给底层控制的，是一份格式严格的交付物：

• 带时间戳的参考轨迹：每个点含位置、朝向、曲率、速度、加速度；
• 目标速度与控制约束；
• 轨迹的有效期，以及触发重规划的条件；
• 外加一份双方必须对齐的“合同”：坐标系是哪个、时间基准是什么、曲率怎么定义。

接口对不齐，一条完美的轨迹会被底层执行成一次“画龙”。方向盘和制动力怎么产生，是控制的事，这条主线也止步于这个接口。

至此，模块化链路走通了：像素进来，轨迹出去。下一章换一个问题：当这条链路上的结构化接口一层层丢掉信息，每个模块各自优化、却没法联合训练，端到端拿什么重组这一切？

本章速查