T1 路径-速度解耦、Frenet 坐标系与 ST 图¶

所属：移动机器人规控方向 · 时空规划线（Part-T）。定位：本章是时空联合规划的**反面基线**与**基础语言**。先理解"解耦"范式为什么在多数结构化道路场景下够用、它说的是哪一套坐标语言，才能在 T2/T3 真正理解"何时必须做时空联合"——联合方法的每一个设计动机，都是在补解耦留下的洞。 文档类型：算法工程教学（理论与代码并重，text:code ≥ 60:40）。前置：Part 0 公共基础（QP / 凸优化）、v8 Ch11（Eigen 深度）、v8 Ch17（Ceres / 非线性优化直觉）。

前置自测¶

答不出 ≥ 2 题，建议先回 Part 0（凸优化与 QP）和 v8 Ch11/Ch17 补齐再来。这几道不是考记忆，是确认你具备读懂本章推导与代码的最小工具。

二次规划（Quadratic Programming, QP）的标准形式是 $\min_{x}\ \tfrac12 x^\top P x + q^\top x$，受约束 $l \le Ax \le u$。其中 $P$ 需要满足什么条件，这个问题才是**凸**的？ $P\succ 0$ 和 $P\succeq 0$ 对解的唯一性各意味着什么？
凸优化为什么在工程上如此被偏爱？一个凸问题的局部最优与全局最优是什么关系？这对"实时求解"意味着什么？
给定一条平面曲线，"弧长参数化"是什么意思？为什么用弧长 $s$ 当参数，比用时间 $t$ 当参数在**几何**上更自然？
一个二维刚体变换（旋转 $R(\theta)$ + 平移 $\mathbf t$）怎么写成矩阵形式？ $R(\theta)$ 的两列分别代表什么方向？
（开放题，凭直觉答）如果把"走哪条线"和"沿这条线走多快"两件事分开决定，相比同时决定它们，直觉上简单在哪、又可能漏掉什么？

本章目标¶

学完本章，你应该能够：

用数学语言写出**路径-速度解耦（Path-Velocity Decomposition, PVD）**：把轨迹 $\mathbf r(t)$ 拆成几何路径 $\sigma(s)$ 与速度剖面 $s(t)$，并说清这一拆为什么把决策维度从 $\mathcal O(5N)$ 的非凸问题降为两个 $\mathcal O(2N)$ 的凸问题。
推导 Frenet 坐标系 $(s, l)$ 的正变换与逆变换，并解释曲率项 $1 - \kappa_r l$ 在什么条件下导致坐标退化。
构建 SL 图（Station-Lateral，纵向-横向图），把静态障碍投影为 SL 平面上的阻挡区域，并把路径规划写成一个 piecewise-jerk QP。
构建 ST 图（Station-Time，纵向-时间图），把动态障碍的预测投影为时变阻挡，用 DP-ST 搜索产出粗粒度速度 profile。
写出 piecewise-jerk speed QP 的代价与约束，理解向心加速度对速度上界的折减。
解释 Apollo EM Planner 的 **E-step / M-step 迭代**为什么只能保证总代价单调不增、却不保证全局最优，以及它在哪类场景下失效。
判断 PVD 在哪些强交互场景下解耦失败（cut-in、无保护左转），从而理解后续时空联合方法（T2/T3）的动机。

本章知识导航¶

本章 6 个核心知识点构成一条"先拆解 → 再表示 → 后求解 → 最后认识局限"的主线：

§T1.1 PVD 是总纲与反面基线：它定义了"拆"这个动作，后 5 节都在这个拆解框架内活动。
§T1.2 Frenet 坐标系 是公共语言：SL 图和 ST 图都建立在 Frenet 坐标之上。
§T1.3 SL 图 与 §T1.4 ST 图 是 PVD 两个子问题（路径 / 速度）各自的表示空间，二者并列。
§T1.5 piecewise-jerk speed QP 是 §T1.4 的精修求解器。
§T1.6 EM 迭代 是把两个解耦子问题缝合回"准联合"的工程手段，也自然引出"为什么还需要真正的时空联合"（→ T2/T3）。

依赖路线（实线为强依赖）：

T1.1 PVD（框架 / 反面基线）
   └─→ T1.2 Frenet（公共语言）
          ├─→ T1.3 SL 图 + Path QP ─┐
          └─→ T1.4 ST 图 + DP 搜索 ─┴─→ T1.5 Speed QP ─→ T1.6 EM 迭代 ─→（T2/T3 时空联合）

本导航只给**结构**，不展开内容。建议初学者顺序读 §T1.1→§T1.6；有规划基础、只想补"语言"的读者可从 §T1.2 切入，遇到陌生术语再回 §T1.1。

前置知识桥接¶

回顾 Part 0（QP）：QP 的标准形式是 $\min_x \tfrac12 x^\top P x + q^\top x$ s.t. $l \le Ax \le u$。当 $P\succeq 0$（半正定）时问题是凸的；若 $P\succ 0$（正定）则全局最优唯一。凸性意味着"局部最优即全局最优"，求解器不会卡在次优解，这正是它能被信任地放进 10 ms 重规划循环的根本原因。本章会把路径子问题和速度子问题**都**构造成这种 QP 形式——理解了 QP 的凸性，就理解了为什么解耦之后每个子问题都能稳定、快速求解。

回顾 v8 Ch11（Eigen）：本章 QP 的 Hessian $P$ 与约束矩阵 $A$ 都是稀疏带状结构（相邻时间步才耦合），用 Eigen 的稀疏矩阵组装。回顾 v8 Ch17（Ceres）：你在那里用 Ceres 解非线性最小二乘（NLS）；本章解的是 QP——同样是优化，但 QP 的目标二次、约束线性，比一般 NLS"温顺"得多，这也是解耦带来的红利之一。

如果跳过本章会怎样¶

直接学 T3（CILQR / MINCO 时空联合优化）而没建立 PVD 这个反面基线 → 你会不理解"为什么要费劲做联合优化"，因为不知道解耦在哪里不够用。联合方法的每一个设计动机，本质上都是在补解耦的洞；没有洞，补丁就成了无源之水。
跳过 Frenet / ST 直接读 Apollo 源码 → SL 图、ST 图、STBoundary、参考线（reference line）这些概念会像天书，因为 Apollo 整个 planning 模块就是用这套语言写成的。

预计阅读时间¶

阅读方式	时间	适合谁
精读（含推导与练习）	8–10 小时	要能独立实现 Frenet 库 + Speed QP 的读者
速读（跳过 QP 矩阵细节）	3–4 小时	已有规划基础、只需建立时空规划语言体系
速查（只看公式块与速查表）	30 分钟	回头查 Frenet 变换 / QP 形式时

本章符号约定¶

下面把全章反复出现的符号集中列出，便于回查。横向偏移 $l$ 一律取**左正右负**；除特别说明外，撇号 $(')$ 表示对弧长 $s$ 求导、点号 $(\dot{\ })$ 表示对时间 $t$ 求导——这正是 PVD"路径看 $s$、速度看 $t$"的记号体现。

符号	含义	首见
$s$	纵向弧长 / station（沿参考线的进度）	§T1.2
$l$	横向偏移 / lateral（左正右负）	§T1.2
$l',\ l''$	$\mathrm dl/\mathrm ds,\ \mathrm d^2l/\mathrm ds^2$（横向一、二阶导）	§T1.2
$\theta_r,\ \kappa_r$	投影点处参考线的切向朝向、曲率	§T1.2
$\theta$	车辆朝向	§T1.2
$\kappa_{\text{path}}$	车辆实际路径的曲率（$\approx\kappa_r + l''$）	§T1.2
$v$	车速	§T1.2
$a,\ j$	纵向加速度、jerk（$\mathrm d^3 s/\mathrm d t^3$）	§T1.5
$a_{\text{lat}},\ a_{\text{lat,max}}$	横向（向心）加速度及其上限	§T1.5
$v_{\text{limit}}(s)$	速度上界（含向心折减 $\sqrt{a_{\text{lat,max}}/\lvert\kappa\rvert}$）	§T1.5
$a^\kappa_{\max}$	车辆最大曲率（转向极限）	§T1.3
$s_{\text{lb}}(t),\ s_{\text{ub}}(t)$	ST 走廊下 / 上界（来自 §T1.4 的 DP 决策）	§T1.4
$\Delta s,\ \Delta t$	路径 / 速度问题的离散步长	§T1.3 / §T1.5
$N,\ N_t,\ N_s$	离散点数 / ST 时间层数 / $s$ 方向格数	§T1.3 / §T1.4
$w_\bullet$	各代价项权重（$w_l, w_{l''}, w_j, \dots$）	§T1.3
$\mathcal O_{\text{static}},\ \mathcal O_{\text{dynamic}}$	静态 / 动态障碍集合	§T1.1
$\xi$	软约束的松弛变量	§T1.5
`JointCost`	路径 + 速度的联合代价（EM 收敛判据）	§T1.6

怎么读这章：第一次读建议按 §T1.1 → §T1.7 顺序走，每节先读开头"这一节解决什么问题"的引言、再看理论推导、最后动手把代码与练习过一遍。 §T1.3 与 §T1.5 的 QP 矩阵细节较重，速读时可先跳过、用到时再回看——§T1.7 的完整管线会把所有零件重新串一遍，是检验"是否真的连起来了"的试金石。每节末尾的陷阱分编程 / 概念 / 思维三类，建议都读：很多坑不是不会写，而是想当然。

§T1.1 路径-速度解耦（PVD）的数学形式化 ⭐⭐¶

这一节解决什么问题：为什么实践中几乎所有经典道路规划器，都不直接优化完整的时空轨迹，而是先"画线"（path）再"定速度"（speed）？这个拆解的代价和收益分别是什么？

动机：完整轨迹规划问题有多难¶

先看最朴素、最"诚实"的提法：一辆车要在动态交通里从当前状态走到目标，我们想直接求出最优的、带时间参数的轨迹 $\mathbf r(t) = \big(x(t),\, y(t)\big)$：

\[ \min_{\mathbf r(\cdot)}\ J(\mathbf r) \quad\text{s.t.}\quad \underbrace{\mathbf r(0)=\mathbf r_0,\ \dot{\mathbf r}(0)=\mathbf v_0}_{\text{initial}},\quad \underbrace{\mathbf r(t)\notin \mathcal O(t),\ \forall t}_{\text{collision-free}},\quad \underbrace{\lVert\dot{\mathbf r}\rVert\le v_{\max},\ \lVert\ddot{\mathbf r}\rVert\le a_{\max}}_{\text{dynamics}} \]

逐项读懂这个式子：$J(\mathbf r)$ 是代价泛函（平滑性、靠近目标、远离障碍等的加权）；初值约束保证轨迹接得上当前车辆状态； $\mathcal O(t)$ 是**随时间变化**的障碍占据集合——动态障碍在不同时刻占据不同位置，所以它带 $t$；动力学约束限制速度和加速度的物理上界。注意这里 $\mathbf r$ 是一个**函数**（无穷维对象），求解前必须先离散化。

把它离散成 $N$ 步，每步的决策状态至少是 $(x, y, \theta, v, a)$ 五维（还没算曲率 $\kappa$ 或 jerk），整条轨迹就是一个 $\sim 5N$ 维的决策向量。更糟的是约束的**几何性质**：避障约束 $\mathbf r(t)\notin\mathcal O(t)$ 在配置空间里是**非凸**的——障碍物把可行域挖成了带洞、不连通的形状（"从障碍左边绕"和"从右边绕"是两个互不相连的可行区域）。

把两件事叠加起来，结论就清楚了：

本质洞察：直接优化完整时空轨迹，难点不在"维度高"，而在"高维 × 非凸"的乘积。高维但凸（如一个 1000 维 QP）现代求解器几毫秒就能解；低维但非凸（如二维带洞可行域）也能暴力搜。真正棘手的是两者相乘——既没有多项式时间的全局最优算法，局部方法又极易卡在"既不左也不右"的鞍点上。 PVD 的全部价值，就是把这个乘积拆开。

如果不这样做会怎样（反事实）¶

假设我们头铁，坚持直接优化这个 $5N$ 维非凸问题。会遇到三个具体后果：

维度灾难压垮实时性。 $N=40$ 就是 200 维决策变量。通用非线性求解器（如 IPOPT）单次迭代要分解约 $200\times200$ 的 KKT 矩阵，且非凸下必须多次随机重启以躲局部最优 → 单次规划耗时从数百毫秒到秒级，远超 10 ms 的重规划预算。车都开过去了，轨迹还没算完。
非凸陷阱产生危险轨迹。避障约束的非凸性，让求解器极易停在"绕左"与"绕右"两个吸引盆之间的鞍点，输出一条**既不充分左也不充分右**的轨迹——数值上"收敛"了，物理上正对着障碍开。
耦合放大噪声。位置和速度挤在同一个优化里，障碍预测的一点点抖动会**同时**扰动路径与速度，输出在时间上前后不连贯（这一帧决定绕左、下一帧又减速让行）。

这三条不是理论臆测，而是 1980 年代以来研究者反复撞到的墙——正是这堵墙催生了 PVD。

历史：PVD 从哪来¶

路径-速度解耦的形式化，最早由 Kamal Kant 与 Steven Zucker 在 1986 年提出（Toward Efficient Trajectory Planning: The Path-Velocity Decomposition，IJRR 5(3):72–89）。他们把轨迹规划问题（Trajectory Planning Problem, TPP）拆成两个子问题，记作

\[\text{TPP} \;\Rightarrow\; \text{PPP} + \text{VPP}\]

其中 PPP（Path Planning Problem）负责躲静态障碍、回答"走哪"，VPP（Velocity Planning Problem）负责沿已定路径躲动态障碍、回答"何时走到路径上哪一点"。那个 $\Rightarrow$ 是**有条件**的——它依赖一个假设：障碍独立于自车运动，不会主动"追"自车。更关键的是：他们已经把 VPP 放进**路径-时间空间**（path-time space），把"时间"显式当作额外一维，并将其化为该空间里的图搜索。

这一笔很重要，它纠正了一个常见误解：

本质洞察：ST 图（path-time / station-time 图）不是 Apollo 的发明，而是 1986 年 Kant–Zucker 就给出的构造。 Apollo 做的是把它工业化（HD 地图投影、DP-ST 搜索、QP 精修），而不是发明它。理解这一点，你读 §T1.4 的 ST 图时就不会把它当作某个工程团队的"奇技淫巧"，而会看到一条四十年的连续脉络。

这条脉络的后续我们会逐段展开：Werling 等（ICRA 2010）把 PVD 升级为 Frenet 坐标系下的末端状态采样（§T1.2 展开），Apollo（Fan 等 2018）再把它工业化为 SL+ST 图的 DP+QP 管线（§T1.3–T1.6 展开）。

PVD 成立的前提：什么时候可以放心解耦¶

Kant–Zucker 那个 $\Rightarrow$ 是有条件的，把条件讲清楚，才知道这一章学的方法什么时候靠得住、什么时候会翻车。核心前提有三条：

障碍独立于自车（不"追"自车）。 VPP 把动态障碍的预测当成**外生**的、不随自车决策改变的时变占据。这对大多数交通参与者近似成立（前车按自己的意图走），但对**强交互**对象（会因你减速而加塞的车、博弈型对手）不成立——障碍的轨迹依赖你的轨迹，"独立"假设破裂。这类场景属于 Part-G（博弈式规划）的疆域。
路径与速度弱耦合。解耦默认"先定的几何路径不会让速度无解"。弯道的向心折减（§T1.5）是一处**单向**耦合（路径限速度），尚可在解耦框架内用速度上界吸收；但当"改路径能显著改善速度可行性"时（cut-in、窄道会车），耦合变双向，顺序求解会落到次优 / 不可行（§T1.1 概念误区、§T1.4 跨章综合题）。
存在合理的参考线。 Frenet / SL / ST 这套表示都依赖一条参考线（车道中心线）。结构化道路天然有；但停车场、路口内部等**非结构化**场景没有清晰参考线，整套解耦语言失效，要改用笛卡尔 / freespace 规划（§T1.2 退化讨论）。

把这三条当成一张"适用性检查表"：

前提	满足时（本章方法适用）	不满足时该去哪
障碍独立于自车	常规交通流	强交互 / 博弈 → Part-G
路径-速度弱耦合	多数结构化道路	强耦合 → §T1.6 EM 缓解，或 T2/T3 联合
有合理参考线	车道内行驶	非结构化场景 → 笛卡尔 / freespace

思维提醒：很多"规划器在某场景诡异失败"的根因，不是代码 bug，而是**这三条前提里有一条悄悄破裂了**。调试时先问一句"我还在 PVD 的适用域里吗"，往往比逐行看代码更快定位问题。

理论：PVD 的两个子问题¶

PVD 把上面那个 $5N$ 维非凸问题，换成两个低维凸子问题。

子问题 1 —— 路径规划（在静态 / 准静态环境中，求几何路径 $\sigma(s)$）：

\[ \min_{\sigma(\cdot)}\ J_{\text{path}}(\sigma) \quad\text{s.t.}\quad \sigma(0)=\mathbf r_0,\quad \sigma \notin \mathcal O_{\text{static}},\quad \lvert\kappa(\sigma)\rvert \le \kappa_{\max} \]

这里 $\sigma$ 以**弧长** $s$ 为参数（几何对象，不含时间），代价 $J_{\text{path}}$ 关注平滑与居中，约束含静态避障与曲率上界 $\kappa_{\max}$（车辆最小转弯半径的倒数）。

子问题 2 —— 速度规划（在已知路径上，求速度剖面 $s(t)$）：

\[ \min_{s(\cdot)}\ J_{\text{speed}}(s) \quad\text{s.t.}\quad s(0)=0,\ \dot s(0)=v_0,\quad \text{collision-free},\quad 0\le \dot s\le v_{\max},\ \lvert\ddot s\rvert\le a_{\max} \]

$s(t)$ 描述"在时刻 $t$ 已沿路径走到弧长 $s$ 处"，它把动态避障（什么时候到哪）和舒适性（加速度、jerk）都收进来。

维度与可解性对比（系统性地列清楚，而非笼统说"更简单"）：

维度	完整时空问题	PVD 拆解后
决策变量规模	$\sim 5N$（如 $N{=}40 \Rightarrow 200$ 维）	path $\sim 2N$ + speed $\sim 2M$（如 $80 + 80$）
凸性	非凸（避障可行域带洞、不连通）	path 可整理为凸 QP，speed 可整理为凸 QP
全局最优	无多项式算法，易卡鞍点	每个凸子问题有唯一全局最优
实时性	数百 ms～秒级	每个子问题 OSQP 毫秒级
信息耦合	静态地图与动态预测混在一起	path 吃静态地图，speed 吃动态预测

对同一个解耦动作，给两种互补的理解角度：

几何视角：PVD 把一条二维轨迹曲线，分解为"形状（path，画在地面上的线）"加"沿形状的进度（speed，什么时候走到线上哪一点）"两个解耦自由度。
信息视角：path 子问题只吃慢变的静态/准静态地图，speed 子问题吃快变的动态障碍预测——于是可以把两者放到**不同的求解频率**上（地图变得慢，路径就能算得久一点；预测变得快，速度就得刷得勤一点）。

最后这一点直接连到工程：

正因为 path 子问题只依赖静态/准静态环境，Apollo 工程上才能用相对长的周期算路径、用更短周期更新速度——这不是随意分频，而是 PVD 解耦的直接红利。把这个红利记住，你在 §T1.6 看 EM 迭代时就会明白：当 path 和 speed 必须互相喂数据时，这个红利就开始打折。

为了让"两阶段"具体可感，下面给出解耦管线的骨架（注意它只是**结构示意**，每个子模块的真正实现分别在 §T1.3 与 §T1.5）：

Step 1 — 为什么这样组织代码：两阶段必须串成清晰的数据流（path 的输出是 speed 的输入），且每阶段的求解器各自独立、可单独替换和测试——这正是 PVD 在软件架构上的体现。

// 解耦规划管线的结构骨架(教学示意,非完整实现)
struct PlanningResult {
  std::vector<PathPoint>  path;   // 几何路径 σ(s):一串 (s, l) 或笛卡尔点
  std::vector<SpeedPoint> speed;  // 速度剖面 s(t):一串 (t, s, v, a)
};

PlanningResult PvdPlan(const ReferenceLine& ref_line,
                       const std::vector<Obstacle>& obstacles) {
  // 阶段 1:路径规划——只看静态/准静态障碍,在 SL 图上求 l(s)
  //        (实现见 §T1.3 的 piecewise-jerk path QP)
  PathData path = PlanPathInSL(ref_line, FilterStatic(obstacles));

  // 阶段 2:速度规划——在【已定路径】上看动态障碍,在 ST 图上求 s(t)
  //        (实现见 §T1.4 的 DP-ST 搜索 + §T1.5 的 speed QP)
  SpeedData speed = PlanSpeedInST(path, FilterDynamic(obstacles));

  return {path.points(), speed.points()};  // 串起来即时空轨迹
}

代码里两件事值得记住：（1）PlanPathInSL 只收到 FilterStatic 过滤后的静态障碍，动态障碍此刻被挡在门外——这就是解耦；（2）PlanSpeedInST 拿到的是**已经定好的** path，它不能再改路径，只能在这条路径上调速度。第二点正是 PVD 的命门：如果这条路径选错了（比如贴着一个即将切入的车），速度阶段无论怎么算都救不回来。

⚠️ 常见陷阱¶

💡 概念误区：把 PVD 理解成"先规划路径、再规划速度，两步彼此独立、各算一次就完事" - 错误想法：path 和 speed 是两个独立模块，顺序跑一遍即可，互不影响。 - 现象 / 后果：在 cut-in（旁车切入）等强耦合场景，先定的路径让速度无解——为了不撞必须急刹，但路径已经选了贴近障碍的线，规划器频繁报失败或输出生硬的急动作。 - 根本原因：PVD 的"解耦"是**建模假设**，不是物理事实。当路径与速度强耦合时，顺序求解会落进联合问题的次优甚至不可行区域。 - 正确做法：弱耦合场景用纯解耦；强耦合场景要么用 EM 迭代（§T1.6）让两阶段互相喂几轮数据，要么直接上时空联合（T2/T3）。判据见本节末"PVD 失败条件"。

🧠 思维陷阱：认为"维度降低 = 近似 = 结果一定更差" - 错误想法：解耦丢掉了信息，所以解耦解一定不如联合解，联合恒优。 - 实际情况：在弱耦合场景，解耦的两个**凸**子问题各自求到全局最优，合起来就是联合问题的（近）全局最优；而联合的**非凸**问题反而可能卡在局部最优——此时解耦解**更好**，不是更差。 - 正确思维：解耦与联合的优劣取决于耦合强度，而非"联合天然更优"。面对一个场景，先问"这里路径和速度耦合多强"，再决定用哪种。

💡 概念误区：在 SL（路径）阶段把动态障碍也当静态处理就完事 - 错误想法：动态障碍取当前时刻位置投影到 SL 图，路径绕开它即可。 - 现象 / 后果：路径绕开了障碍**此刻**的位置，但障碍移动后，自车反而和它"撞个正着"——因为路径阶段没有时间维，看不到障碍会动。 - 根本原因：SL 图是**准静态快照**，没有时间轴；动态交互天然属于速度维度。 - 正确做法：动态障碍的交互交给 ST 图 / 速度阶段处理（§T1.4）； SL 阶段对动态障碍至多做保守的"当前+短预测包络"占位，不指望它解决动态避让。

练习¶

[推导] 写出完整时空轨迹问题离散化后的决策维度，以及 PVD 拆解后两个子问题各自的维度；取 $N=40$ 给出具体数字对比。然后用一两句话论证：为什么"两个凸问题"相比"一个非凸问题"在实时性上是**质变**（可解 vs 不可解）而非量变（快一点 vs 慢一点）。
[设计] 给定场景"前车急刹 + 相邻车道有并行车辆"，判断它属于强耦合还是弱耦合，并具体说明：纯 PVD 会在哪一步、以什么形式失败？ EM 迭代（§T1.6）能否救？什么情况下必须上时空联合？
[跨章预告思考，学完 T3 再回看] PVD 的速度子问题在 $(t, s)$ 平面上规划，机械臂的 TOPP-RA（时间最优路径参数化）则在 $(s, \dot s^2)$ 相平面上规划。先记下这个对应关系——两者都是"路径已定、只优化沿路径的运动"的 PVD 实例，T3 会用它来打通"自动驾驶速度规划"与"机械臂时间最优"两套看似无关的方法。

§T1.2 Frenet 坐标系详解 ⭐⭐¶

这一节解决什么问题：车道是弯的，直接在笛卡尔系 $(x, y)$ 里描述"在车道里偏左偏右多少、沿车道走了多远"非常别扭。 Frenet 坐标系把弯曲车道"拉直"，让横纵向解耦变得自然——它是 §T1.3 SL 图与 §T1.4 ST 图共同的坐标语言。

动机：弯道上，笛卡尔坐标无法自然表达"偏离车道多少"¶

在弯道上问一个最基本的问题："这辆车偏离车道中心线多少？" 笛卡尔坐标 $(x, y)$ 答不上来——因为"偏离量"是相对于一条**弯曲**参考线定义的，而笛卡尔坐标的两个轴是固定的全局方向。你需要一套"贴着参考线走"的坐标：一个数表示"沿车道走了多远"，另一个数表示"离车道中心偏了多少"。这正是 Frenet 坐标 $(s, l)$。

用一个有边界的类比建立直觉：

可以把 Frenet 坐标想成给弯曲车道铺一条**橡皮尺**：尺子沿车道中心线弯曲，$s$ 是尺子上的刻度（纵向进度），$l$ 是垂直于尺子的偏移（横向，左正右负）。 像的地方：和直角坐标一样，用两个数就能定位平面上一点。 不像的地方：这把尺子在急弯处会被"挤皱"——法向射线在凹侧会交汇，刻度不再均匀甚至重叠。所以**不要**把 Frenet 的 $(s, l)$ 当成全局笛卡尔那样处处线性、处处一一对应。这个"挤皱"就是后面要讲的坐标退化。

如果不这样做会怎样（反事实）¶

假设不用 Frenet，硬在笛卡尔系里对弯道做横纵向解耦：（1）"横向偏移"的定义会随曲率不断变化，没有统一基准；（2）把障碍"投影"到这套坐标需要做复杂的最近点搜索，且映射高度非线性；（3）最终 SL 图、ST 图这类"以纵向进度为横轴、以横向/时间为纵轴"的规则表示根本建不起来。一句话：没有 Frenet，整条 Apollo 式 SL/ST 管线无从谈起。

历史：Werling 与 Frenet 末端状态采样¶

Frenet–Serret 标架来自 19 世纪微分几何，本是描述空间曲线的"活动标架"（在曲线每一点定义切向、法向、副法向）。把它系统地用于自动驾驶轨迹生成，公认的奠基工作是 Werling、Ziegler、Kammel、Thrun 的 Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenét Frame（ICRA 2010，Anchorage，pp. 987–993）。他们在 $(s, l)$ 下对横纵向分别做末端状态采样 + 解析多项式连接，$(s, l)$ 从此成为之后 SL/ST 图的坐标基础。

辨析（论文版本易混点）：Werling 团队还有一篇常被一并引用的 Optimal Trajectories for Time-Critical Street Scenarios Using Discretized Terminal Manifolds（IJRR 2012, 31(3):346–359）。它与上面的 ICRA 2010 是**两篇不同论文**（标题不同、作者序不同，IJRR 版无 Thrun、多 Gröll），不是同一篇的"会议版/期刊版"。引用时不要把二者合并成一条。

理论：定义、正逆变换与退化¶

定义。给定参考线 $\gamma(s)$（以弧长 $s$ 参数化，通常是车道中心线），平面上任一点 $P$ 的 Frenet 坐标为：

$s$：$P$ 在 $\gamma$ 上**最近投影点**的弧长参数（纵向进度）；
$l$：$P$ 到该投影点的**有符号距离**（约定左正右负，横向偏移）。

正变换（Frenet → Cartesian）。设参考线在弧长 $s$ 处的点为 $\big(x_r(s), y_r(s)\big)$、切向朝向为 $\theta_r(s)$。参考线在该点的单位法向是 $\mathbf n = (-\sin\theta_r,\ \cos\theta_r)$（把切向 $(\cos\theta_r, \sin\theta_r)$ 逆时针转 90°）。沿法向偏移 $l$，即得 $P$：

\[ \boxed{\,x = x_r(s) - l\,\sin\theta_r(s),\qquad y = y_r(s) + l\,\cos\theta_r(s)\,} \]

逐步看这两式怎么来的：从参考点 $(x_r, y_r)$ 出发，沿单位法向 $(-\sin\theta_r, \cos\theta_r)$ 走 $l$ 个单位，$x$ 分量加 $l\cdot(-\sin\theta_r)$、$y$ 分量加 $l\cdot(\cos\theta_r)$，就是上式。 $l>0$ 偏向法向（左侧），$l<0$ 偏向右侧。

逆变换（Cartesian → Frenet）。给定 $P=(x_P, y_P)$，要找它在 $\gamma$ 上的最近投影点。最近点 $s^\*$ 满足**投影正交条件**：连线 $P-\gamma(s^\*)$ 垂直于参考线切向，即

\[ \big(P - \gamma(s^\*)\big)\cdot \gamma'(s^\*) = 0 \]

这是关于 $s$ 的一维求根问题。工程上不解析求解，而是在参考线离散点上找最近点做初值、再做几步局部牛顿/割线迭代。求得 $s^\*$ 后，$l$ 就是带符号距离 $\,\pm\lVert P-\gamma(s^\*)\rVert$，符号由 $P$ 在切向的左右侧决定（用叉积符号判定）。

一阶运动学关系（退化的关键来源）。把 $P$ 换成随时间运动的车辆，对正变换关系微分、整理可得纵向进度的速率：

\[ \dot s = \frac{v\,\cos(\theta - \theta_r)}{\,1 - \kappa_r\, l\,} \]

其中 $v$ 是车速、$\theta$ 是车辆朝向、$\theta_r$ 与 $\kappa_r$ 是投影点处参考线的朝向与曲率。逐项理解：分子 $v\cos(\theta-\theta_r)$ 是车速在参考线切向上的投影（"沿车道方向的速度"）；分母 $1-\kappa_r l$ 是曲率引入的**几何放缩因子**——当车辆偏离参考线 $l$ 后，它实际走过的弧长进度被曲率拉伸或压缩了。

直觉：在内弯（$\kappa_r>0$）且偏向内侧（这里按符号约定对应 $l$ 使 $\kappa_r l>0$）时，分母 $<1$，同样的切向速度对应更大的 $\dot s$（内圈半径小、转得"快"）；在外侧则相反。

把 $\dot s$ 推出来（别停在"整理可得"）。对正变换 $x = x_r - l\sin\theta_r,\ y = y_r + l\cos\theta_r$ 关于时间求导，用参考线弧长参数化的三个事实 $\dot x_r=\dot s\cos\theta_r$、$\dot y_r=\dot s\sin\theta_r$、$\dot\theta_r=\kappa_r\dot s$：

\[ \dot x = \dot s\cos\theta_r\,(1-\kappa_r l) - \dot l\sin\theta_r,\qquad \dot y = \dot s\sin\theta_r\,(1-\kappa_r l) + \dot l\cos\theta_r \]

把车辆速度向量 $(\dot x,\dot y)=(v\cos\theta,\ v\sin\theta)$ 分别投影到参考线**切向** $(\cos\theta_r,\sin\theta_r)$ 与**法向** $(-\sin\theta_r,\cos\theta_r)$（投影即点乘），交叉项相消，得两条干净关系：

\[ v\cos(\theta-\theta_r) = \dot s\,(1-\kappa_r l)\ \Rightarrow\ \boxed{\ \dot s = \frac{v\cos(\theta-\theta_r)}{1-\kappa_r l}\ }, \qquad \dot l = v\sin(\theta-\theta_r) \]

第一条就是上面那个 $\dot s$；第二条 $\dot l = v\sin(\theta-\theta_r)$ 是横向速率。两者相比，得**横向几何关系**：

\[ l' = \frac{\mathrm d l}{\mathrm d s} = \frac{\dot l}{\dot s} = (1-\kappa_r l)\,\tan(\theta-\theta_r) \]

这条 $l'$ 关系把"航向偏差 $\theta-\theta_r$"与"横向斜率 $l'$"换算起来——Werling 2010 的横向采样正是在 $l(s)$ 这个量上做多项式拟合。

补上 §T1.3 欠的那一步：车辆路径曲率为什么 $\approx\kappa_r + l''$。把 $l'$ 再对 $s$ 求导、代回曲率定义，可得车辆实际轨迹在笛卡尔下的曲率 $\kappa_{\text{path}}$ 关于 $(\kappa_r, l, l', l'')$ 的完整表达式（形式较繁，见 Werling 2010）。但在**小航向偏差、小偏移**的常见工况下，它线性化为一个极简形式：

\[\kappa_{\text{path}} \approx \kappa_r + l''\]

——车辆路径的曲率 ≈ 参考线曲率加上横向二阶导。这正是 §T1.3 ddl_bounds 的理论根据：车辆能提供的总曲率受最大转向限制，$|\kappa_{\text{path}}|\le a^\kappa_{\max}$，代入上式即 $|\kappa_r + l''|\le a^\kappa_{\max}$，移项就得到 §T1.3 那个**减去 $\kappa_r$** 的界 $l''\in[-a^\kappa_{\max}-\kappa_r,\ a^\kappa_{\max}-\kappa_r]$。当时只让你"记住要减 $\kappa$"，到这里它的来历就清楚了——这也是为什么 Frenet 这套坐标语言值得先建：路径优化的约束，是直接从坐标的微分几何里长出来的。

坐标退化。当 $\kappa_r l \to 1$ 时，分母 $\to 0$，$\dot s \to \infty$，变换**奇异**。几何含义很清楚：偏移 $l$ 达到曲率半径 $1/\kappa_r$ 时，参考线两侧的法向射线在曲率中心交汇，多个笛卡尔点映到同一个 $(s, l)$ 附近，一一对应关系破裂。

Frenet 坐标的可用区有两条线——严格的数学条件和更保守的工程经验阈值：

\[ \underbrace{|\kappa_r\, l| < 1}_{\text{strict}};\qquad \underbrace{|\kappa_r\, l| < 0.3}_{\text{empirical}} \]

前者是 Frenet 变换不退化的严格界，后者是工程上留足裕度的经验取法。

超过经验阈值（大曲率匝道、停车场、路口内部等无清晰车道线场景），就应改用笛卡尔坐标规划（如 Apollo Open Space、Autoware 的 freespace planner）。

本质洞察：Frenet 坐标系的本质不是"换一个坐标系"，而是把**参考线的曲率信息一次性吸收进坐标定义**，从而让横纵向在新坐标里近似解耦。代价就是这次吸收在 $|\kappa_r l|\to 1$ 时失效——曲率越大、偏移越大，这套"拉直"越不可信。 Frenet 不是免费的几何魔法，而是一笔有息贷款：道路越直，利息越低。

代码实现（为什么 → 正确 → 错误 → 对比）¶

Frenet 变换是规划热路径上的高频操作（每帧要对自车和几十个障碍反复做），实现质量直接影响实时性。这里代码角色是**实现并验证**上面的变换公式。

Step 1 — 为什么这样写：参考线用"离散点 + 每点预存累积弧长、朝向、曲率"的结构存储；变换用 Eigen 固定大小向量、避免每次调用堆分配。原因和 v8 Ch11 一致——固定大小矩阵栈分配、可被编译器向量化，比动态分配快一个数量级，这在每帧上千次的调用里是决定性的。

Step 2 — 正确写法：

#include <Eigen/Dense>
#include <cmath>

struct ReferencePoint {
  double x, y;      // 参考线上该点的笛卡尔坐标
  double theta;     // 该点切向朝向 θ_r
  double kappa;     // 该点曲率 κ_r
  double s;         // 该点的累积弧长(从参考线起点算起)
};

// 正变换 Frenet → Cartesian:已知参考点 rp 与横向偏移 l,求笛卡尔坐标
Eigen::Vector2d FrenetToCartesian(const ReferencePoint& rp, double l) {
  const double cos_t = std::cos(rp.theta);
  const double sin_t = std::sin(rp.theta);
  // 沿单位法向 (-sinθ_r, cosθ_r) 偏移 l:
  //   x = x_r - l·sinθ_r ; y = y_r + l·cosθ_r
  return Eigen::Vector2d(rp.x - l * sin_t,
                         rp.y + l * cos_t);
}

Step 3 — 错误写法并解释为什么错：

// ❌ 错误 1:把横向偏移 l 误加到【切向】而非法向
Eigen::Vector2d Wrong_TangentOffset(const ReferencePoint& rp, double l) {
  const double cos_t = std::cos(rp.theta);
  const double sin_t = std::sin(rp.theta);
  return Eigen::Vector2d(rp.x + l * cos_t,   // 这是沿切向(cosθ,sinθ)移动 l
                         rp.y + l * sin_t);  // ——变成了"前后平移"而非"左右偏移"!
  // 现象:障碍/轨迹整体沿车道方向前后挪了 l,在可视化里一眼看出错位。
  // 根因:切向 (cosθ,sinθ) 与法向 (-sinθ,cosθ) 搞混。横向偏移必须用法向。
}

// ❌ 错误 2:逆变换里直接取"最近的离散参考点",不做投影
//    后果:s/l 带量化误差,且该误差在弯道处被 1/(1-κl) 放大,
//          导致 §T1.3 的 SL 边界算错、QP 约束失真。必须做局部最近点投影。

Step 4 — 多种实现方式对比：

方式	最近点投影做法	精度	性能	适用
A 离散点 + 线性插值	在离散参考点中找 argmin，相邻两点间线性插值定位	中	高	教学、原型
B 样条拟合 + 解析投影	参考线拟合为样条，解投影正交方程	高	中	工业（Apollo reference line）
C 现成库	调用成熟 Frenet 工具的投影接口	高	视实现	快速集成

教学建议先实现 A 把流程跑通、用解析曲线验证正确性，再在工程里换成 B。

一份能跑的 Frenet 正逆变换（无外部依赖）¶

把上面的"方案 A + 解析曲线验证"落成一份**可直接编译运行**的代码——用一段已知半径的圆弧当参考线，验证"笛卡尔点 → Frenet → 笛卡尔"能还原回原点（正逆互逆）。只用 C++ 标准库，g++ -std=c++17 即可编译；它贴合无人机/自驾的部署语言（Apollo 的 reference line 投影正是 C++）。

// Frenet ↔ 笛卡尔 正逆变换的完整可运行实现(教学版)
// 验证:笛卡尔点 → Frenet (s,l) → 笛卡尔,应还原回原点(正逆互逆)
#include <vector>
#include <cmath>
#include <cstdio>
#include <limits>

struct Pt { double x, y; };
// 参考线采样点:坐标 + 累积弧长 s + 切向角 theta
struct RefPoint { double x, y, s, theta; };

// 用半径 R 的圆弧生成参考线(圆心在 (0,R),起点在原点)
std::vector<RefPoint> MakeArcRef(double R, double s_max, double ds) {
  std::vector<RefPoint> ref;
  for (double s = 0; s <= s_max; s += ds) {
    double phi = s / R;                        // 圆弧:弧长 s 对应圆心角 phi
    ref.push_back({R*std::sin(phi), R - R*std::cos(phi), s, phi});  // theta = phi
  }
  return ref;
}

// 笛卡尔 → Frenet:找最近参考点,投影出 (s, l)
void CartesianToFrenet(const Pt& q, const std::vector<RefPoint>& ref,
                       double* s_out, double* l_out) {
  int best = 0; double best_d2 = std::numeric_limits<double>::infinity();
  for (int i = 0; i < (int)ref.size(); ++i) {     // 线性扫描找最近点(工程用 KD-tree)
    double dx = q.x-ref[i].x, dy = q.y-ref[i].y, d2 = dx*dx+dy*dy;
    if (d2 < best_d2) { best_d2 = d2; best = i; }
  }
  const RefPoint& r = ref[best];
  double dx = q.x-r.x, dy = q.y-r.y;
  // l = 偏移向量在【法向】的投影;法向 = 切向逆时针转 90° = (-sinθ, cosθ);左正右负
  *l_out = -std::sin(r.theta)*dx + std::cos(r.theta)*dy;
  *s_out = r.s;
}

// Frenet → 笛卡尔:沿参考点法向偏移 l
Pt FrenetToCartesian(double s, double l, const std::vector<RefPoint>& ref) {
  int best = 0; double best_ds = std::numeric_limits<double>::infinity();
  for (int i = 0; i < (int)ref.size(); ++i) {     // 找弧长最接近 s 的参考点
    double d = std::fabs(ref[i].s - s);
    if (d < best_ds) { best_ds = d; best = i; }
  }
  const RefPoint& r = ref[best];
  return {r.x + (-std::sin(r.theta))*l, r.y + (std::cos(r.theta))*l};  // 沿法向偏移 l
}

int main() {
  auto ref = MakeArcRef(/*R=*/20.0, /*s_max=*/30.0, /*ds=*/0.05);
  Pt q{8.0, 3.5};                                 // 取参考线左侧约 1.5m 的一个点
  double s, l;
  CartesianToFrenet(q, ref, &s, &l);
  std::printf("笛卡尔 (%.3f, %.3f) -> Frenet (s=%.3f, l=%.3f)\n", q.x, q.y, s, l);
  Pt q2 = FrenetToCartesian(s, l, ref);           // 逆变换回去
  std::printf("Frenet (s=%.3f, l=%.3f) -> 笛卡尔 (%.3f, %.3f)\n", s, l, q2.x, q2.y);
  double err = std::hypot(q.x-q2.x, q.y-q2.y);
  std::printf("正逆变换还原误差 = %.4f m  %s\n", err,
              err < 0.05 ? "(< 离散步长,正逆互逆 OK)" : "(误差偏大)");
  return 0;
}
// 运行输出:
//   笛卡尔 (8.000, 3.500) -> Frenet (s=9.050, l=1.663)
//   Frenet (s=9.050, l=1.663) -> 笛卡尔 (8.017, 3.508)
//   正逆变换还原误差 = 0.0192 m  (< 离散步长,正逆互逆 OK)

跑完你会看到：一个笛卡尔点变换到 Frenet 的 $(s,l)$、再变回去，落回了几乎同一位置（误差 $0.019\,\text{m}$，小于参考线离散步长 $0.05\,\text{m}$，正是离散化的固有精度）。这就把 §T1.2 "正逆变换互逆"从公式变成了亲眼可验的事实。代码里两处和理论的对应：l 用法向 $(-\sin\theta,\cos\theta)$ 投影——这正是"横向偏移沿法向、左正右负"的定义；逆变换沿同一法向偏移——正逆用的是同一个法向，互逆才成立（陷阱区会讲一个把 $l$ 误加到切向的典型错误，对照这里就一目了然）。把离散步长 ds 改小，会看到还原误差随之变小——离散越细、投影越准。

这份教学实现用"线性扫描找全局最近点"，简单但藏着两个工程上必须正视的问题，正好借它点破。其一是**最近点可能不唯一或会突变**：当车辆位于参考线曲率中心附近、或参考线有急弯时，到参考线的"最近点"可能有多个候选，或随车辆微动而从一处跳到另一处——表现为 $s$ 值的突然跳变，下游基于 $s$ 的速度规划会因此抖动。工程实现（如 Apollo 的 reference line）会维护上一帧的投影点、在其邻域内局部搜索，而非每帧全局重找，既快又避免跳变。其二是**全局扫描的复杂度**：线性扫描是 $O(N)$（$N$ 为参考线点数），参考线长、点密时不划算；工程上用 KD-tree 或基于弧长的索引把它降到对数级，或用上述"局部搜索"直接做到近似 $O(1)$。还有一个更隐蔽的点：这份实现把 $l$ 取为"偏移向量在法向的投影"，隐含假设了**车辆离参考线不太远**（$|\kappa_r l| < 1$，即 §T1.2 那个退化条件）；一旦偏移大到接近曲率半径，法向投影本身就不再良定义，$(s,l)$ 表示开始失真——这正是前面强调过的"Frenet 可用区"在代码层面的体现。教学实现先不管这些、把主干跑通即可，但你心里要清楚：从这份 50 行的玩具到 Apollo 那套工业级投影，差距正是在"最近点的稳定性、效率、可用区边界"这三件事上。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：逆变换的最近点搜索用"全局线性扫描"，且每帧从头扫 - 错误做法：每帧、对每个查询点，都遍历整条参考线找最近点（$\mathcal O(M)$ per query）。 - 现象：参考线 500 点 × 几十个障碍 × 每帧若干次 → 投影函数吃掉热路径大半时间，规划频率上不去。 - 根本原因：没利用"车和障碍在相邻帧的投影点几乎不动"这一时间连续性，每帧白白重算。 - 正确做法：缓存上一帧的最近点索引，本帧只在其邻域做局部窗口搜索（$\mathcal O(\text{window})$），或用 kd-tree 把单次查询降到 $\mathcal O(\log M)$。自检：用 profiler 看 FrenetProjection 的占比，正常应远低于 QP 求解耗时。

💡 概念误区：认为 Frenet 变换处处精确、可无脑使用 - 错误想法：$(s, l)$ 与 $(x, y)$ 一一对应、随便用。 - 实际情况：仅在 $|\kappa_r l| < 1$ 时一一对应；大曲率 + 大偏移时退化，$1-\kappa_r l \to 0$ 使 $\dot s$ 与诸多 Frenet 量数值爆炸。 - 为什么重要：停车场、路口内部、急匝道等无清晰车道线/大曲率场景，强行用 Frenet 会得到发散的离谱结果。 - 正确做法：运行时监控 $\kappa_r\cdot l$，超过经验阈值（如 0.3）就告警并切换到笛卡尔规划。

🧠 思维陷阱：认为"参考线随便给一条就行，反正只是个参考" - 错误想法：参考线只是辅助基准，光不光滑无所谓。 - 实际情况：参考线不光滑（曲率 $\kappa_r$ 跳变）会让 Frenet 量——尤其横向导数 $l'$ 和 $\kappa_r$——剧烈抖动，直接把 §T1.3 的 QP 喂成病态问题；参考线的质量决定了整张 SL/ST 图的质量。 - 正确思维：参考线必须**先平滑**（Apollo 专门有 reference line smoother 这一前置步骤）。这是典型的"垃圾进、垃圾出"——再好的 QP 也救不了一条锯齿状的参考线。

练习¶

[实现] 实现 FrenetToCartesian 与 CartesianToFrenet（后者含局部最近点的牛顿/割线迭代）。用一条**已知解析解的圆弧**参考线（半径 $R$、圆心已知）验证：任取若干 $(s, l)$ 正变换到 $(x, y)$ 再逆变换回来，往返误差应 $< 10^{-6}$。
[调试] 给出一段有 bug 的 CartesianToFrenet：它用全局 argmin 找最近离散点、但未处理参考线在 U 形弯处"一个查询点对应多个近似最近段"的多解情况。请定位：为什么车辆经过 U 形弯顶点时 $s$ 会突然跳变？并给出修复（提示：用上一帧 $s$ 约束本帧搜索窗口）。
[推导] 推导 $\dot s = \dfrac{v\cos(\theta-\theta_r)}{1-\kappa_r l}$ 中分母 $1-\kappa_r l$ 的来源（从正变换关系对时间求导入手）。然后代入数值：参考线曲率半径 $1/\kappa_r = 10\,\text{m}$、车辆横向偏移 $l = 2\,\text{m}$，计算 $\dot s$ 相对 $v\cos(\theta-\theta_r)$ 被放大了多少倍？这对"高速 + 大曲率匝道"的规划意味着什么风险？

§T1.3 SL 图与 piecewise-jerk 路径 QP ⭐⭐⭐¶

这一节解决什么问题：有了 Frenet 坐标（§T1.2），怎么把"在车道里选一条避开静态障碍、平滑、居中的路径"变成一个能在 5 ms 内求出全局最优的凸 QP？这是 PVD 路径子问题（§T1.1）的具体落地，也是你读 Apollo piecewise_jerk_path_optimizer 源码的钥匙。

动机：从"画一条线"到"一个可求解的优化问题"¶

PVD 的路径子问题要在静态环境里求几何路径。在 Frenet 系下，路径就是横向偏移关于纵向进度的函数 $l(s)$——"沿车道走到弧长 $s$ 处时，偏离中心线多少"。难点在于：$l(s)$ 是一个函数（无穷维对象），静态障碍把某些 $(s, l)$ 区域挡死，我们要在剩下的"自由走廊"里挑一条最平滑、最居中的 $l(s)$。怎么把这个**无穷维、带障碍**的问题，压成计算机能在毫秒级解出全局最优的形式？答案分两步：先把"画线"问题搬到一张 SL 图**上，再把它整理成一个**凸 QP。

如果不这样做会怎样（反事实）¶

假设我们不走"离散化 + 凸 QP"这条路：

用通用非线性求解器（IPOPT）直接解 → 如果代价或约束没刻意整理成凸的，求解器会卡在局部最优，且单次求解几十到上百毫秒，撑不住高频重规划。
用纯采样（Werling 2010 的末端状态采样） → 在简单场景很优雅，但采样点无法覆盖整个走廊，复杂的窄走廊里经常**采不到可行解**——这正是采样法相比优化法的短板，也是 Apollo 选择 QP 路线的原因之一。
不离散化、停留在函数空间 → 根本无法交给数值求解器。

历史：从 spline-QP 到 piecewise-jerk¶

Apollo EM Planner（Fan 等 2018）最初用 spline-QP：把 $l(s)$ 表示成分段多项式（样条），优化样条系数。后来 Apollo（约 v2.0 起）改用 piecewise-jerk 表示——不再优化样条系数，而是直接以每个 station 上的状态 $[l, l', l'']$ 为决策变量，相邻 station 之间假设 jerk（$l'''$）为常数来做积分。换的理由很实在：piecewise-jerk 的 QP 是**稀疏带状**的（只有相邻 station 耦合）、状态有**直接物理意义**（偏移 / 斜率 / 曲率）、box 约束好加、求解更快也更好调。本节实现的就是这个现行版本。

提醒一个版本-代码差异（已在事实注册表登记）：§T1.5 里你会看到的 "piecewise-jerk" 一词，指的是 Apollo 当前代码的做法，不是 2018 EM 论文里写的 spline-QP。引用时别把 piecewise-jerk 算到 Fan 2018 头上。

本质洞察：piecewise-jerk 把"找一条曲线"重写成"找一串满足积分约束的状态"，换来的最大好处是 QP 矩阵的**稀疏带状结构**——只有相邻 station 的变量耦合，Hessian 与约束矩阵都是带状的。 OSQP 这类吃稀疏性的求解器因此能毫秒级解几百维问题。换句话说，piecewise-jerk 的"快"不是求解器的魔法，而是**问题结构被刻意设计成了稀疏的**。

理论：SL 图构建与 QP 形式化¶

SL 图构建（四步）：

从 HD map 取参考线（必须已平滑，见 §T1.2 思维陷阱），沿弧长离散成 station 序列 $s_0, s_1, \dots, s_{N-1}$，间距 $\Delta s$。
每个静态障碍投影到 Frenet → 占据某段 $[s_a, s_b]\times[l_c, l_d]$ → 折算成每个 station 上的横向上下界 $[\,l_{\text{lb}}(s_i),\ l_{\text{ub}}(s_i)\,]$（障碍与道路边界共同决定）。
动态障碍此刻只做保守占位（§T1.1 概念误区：动态交互留给 §T1.4 的 ST 图）。
路径 $l(s)$ = 在这些逐 station 横向边界内、从起点状态到终点的一条光滑曲线。

对 piecewise-jerk 的状态表示，给两个互补视角：

代数视角：$[l_i, l'_i, l''_i]$ 是一串受积分等式约束的数，QP 在这些约束下最小化平滑代价。
控制 / 状态空间视角：把 $l'''$（jerk）当成"控制输入"，$l, l', l''$ 当成被它积分的"状态"，于是这就是一个以**弧长 $s$ 当'时间'**的离散最优控制问题，piecewise-jerk 即"假设控制 jerk 在每段内恒定"的一阶保持。理解这个视角后你会发现：它和 §T1.5 的速度 QP、乃至一般 MPC，是同一套数学的不同实例。

决策变量：每个 station $i$ 上的 $[l_i, l'_i, l''_i]$，共 $3N$ 个。其中 $l'_i = \mathrm{d}l/\mathrm{d}s$，$l''_i = \mathrm{d}^2 l/\mathrm{d}s^2$。

连续性等式约束（piecewise-constant-jerk 积分）。相邻 station 间设 $l'''\equiv \text{jerk}_i$，由匀 jerk 运动学（对 $s$ 的泰勒展开）得：

\[l''_{i+1} = l''_i + \text{jerk}_i\,\Delta s$$ $$l'_{i+1} = l'_i + l''_i\,\Delta s + \tfrac12\,\text{jerk}_i\,\Delta s^2$$ $$l_{i+1} = l_i + l'_i\,\Delta s + \tfrac12 l''_i\,\Delta s^2 + \tfrac16\,\text{jerk}_i\,\Delta s^3\]

由第一式解出 $\text{jerk}_i = (l''_{i+1}-l''_i)/\Delta s$，代回后两式，即得**只含状态、不含 jerk** 的两条等式约束，把相邻三元组 $[l_i,l'_i,l''_i]$ 与 $[l_{i+1},l'_{i+1},l''_{i+1}]$ 线性地锁在一起。这两条等式就是 QP 里的等式约束行。

代价函数：

\[ J = \sum_{i}\Big[\,w_l\,(l_i - l_{\text{ref},i})^2 + w_{l'}\,l_i'^2 + w_{l''}\,l_i''^2\,\Big] \;+\; w_{l'''}\sum_{i}\Big(\frac{l''_{i+1}-l''_i}{\Delta s}\Big)^2 \]

逐项读：$w_l$ 把路径拉向参考线（居中或跟随预设参考路径）； $w_{l'}$ 抑制横向斜率（别斜着冲出车道）； $w_{l''}$ 抑制横向曲率（弯得别太急）； $w_{l'''}$ 抑制 jerk（更平滑）。这些项全是平方和，所以 Hessian 半正定——QP 是凸的。

约束（box bounds）：

横向位置：$l_{\text{lb}}(s_i)\le l_i\le l_{\text{ub}}(s_i)$（障碍 + 道路边界）。
横向曲率（来自车辆转向能力 + 参考线曲率）。 Apollo 的 piecewise_jerk_path 把 $l''$（代码里叫 ddl）的界设为

\[ l''_i \in \big[\,-a^{\kappa}_{\max} - \kappa(s_i),\ \ a^{\kappa}_{\max} - \kappa(s_i)\,\big], \qquad a^{\kappa}_{\max} = \frac{\tan(\delta_{\max}/\text{steer\_ratio})}{\text{wheelbase}} \]

为什么要**减去参考线曲率 $\kappa(s_i)$**？因为车辆实际路径的曲率近似等于"参考线曲率 $\kappa$ 加上横向二阶导贡献 $l''$"（小偏移近似），而车辆能提供的总曲率被最大转向角 $\delta_{\max}$ 限死在 $\pm a^{\kappa}_{\max}$（注意 $a^{\kappa}_{\max}$ 名字里有 acc 但量纲是曲率 = 1/转弯半径），所以留给 $l''$ 的范围要扣掉参考线本身占掉的那部分 $\kappa$。这是个论文不会写、只在代码里现身的细节，弯道场景必须处理对。 - 边界条件：$l_0, l'_0, l''_0$ 锁定为起点状态（接当前车辆）；终点按需给软或硬约束。

凸性回收：代价是平方和 → Hessian $P\succeq 0$；约束全是线性（等式 + box）→ 这是个**凸 QP**，OSQP 毫秒级给出全局最优。这正好兑现了章首"前置知识桥接"里的承诺：解耦之后，路径子问题落进了最温顺的那类优化。

一个最小数值算例（N=3，把矩阵摊开看）¶

抽象地说"组装一个 QP"说一百遍，不如把一个最小例子的矩阵摊开看一遍。取 $N=3$ 个 station、$\Delta s=1\,\text{m}$，决策向量 9 维：

\[x = [\,l_0,\, l_0',\, l_0'',\ \ l_1,\, l_1',\, l_1'',\ \ l_2,\, l_2',\, l_2''\,]^\top\]

**起点**固定 $l_0=l_0'=l_0''=0$（接当前车辆，用等式或把上下界都设成 0 实现）。设一个小障碍要求 station 1 处向右让出，即 $l_1\le -0.5$。权重取 $w_l=w_{l'}=w_{l''}=w_{l'''}=1$。

连续性等式（消去 jerk 后的两条状态关系，§T1.3 理论里推过，这里代入 $\Delta s=1$ 的简洁形式）：

\[l'_{i+1} = l'_i + \tfrac12\big(l''_i + l''_{i+1}\big),\qquad l_{i+1} = l_i + l'_i + \tfrac13 l''_i + \tfrac16 l''_{i+1}\]

两个间隙（$0\!\to\!1$、$1\!\to\!2$）共 4 条等式，组成 $A_{\text{eq}}\,x = \mathbf 0$。例如间隙 $0\!\to\!1$ 的 $l$ 关系那一行（其余变量系数为 0）：

\[1\cdot l_1 \;-\;1\cdot l_0 \;-\;1\cdot l_0' \;-\;\tfrac13\, l_0'' \;-\;\tfrac16\, l_1'' \;=\; 0\]

不等式 / box：$l_1\le -0.5$（障碍）；各 $l''_i\in[-a^\kappa_{\max}-\kappa,\ a^\kappa_{\max}-\kappa]$（直道段 $\kappa\approx 0$、取 $a^\kappa_{\max}=0.2$ 时约 $[-0.2, 0.2]$）。

代价 Hessian $P$ 的结构：平方和代价让 $P$ 呈"按 $[l, l', l'']$ 分组的对角"主体，唯一的非对角耦合来自 jerk 项 $\big((l''_{i+1}-l''_i)/\Delta s\big)^2$——它只把**相邻** station 的 $l''$ 绑在一起。把 9 维 $P$ 的稀疏模式画出来（X=分组对角项，x=jerk 引入的相邻 $l''$ 耦合）：

         l0 l0' l0'' l1 l1' l1'' l2 l2' l2''
   l0  [  X                                 ]
   l0' [      X                             ]
   l0''[          X         x               ]   x: jerk 把 l0'' 与 l1'' 耦合
   l1  [             X                      ]
   l1' [                X                   ]
   l1''[          x         X         x     ]   三对角: 仅相邻 l'' 互联
   l2  [                       X            ]
   l2' [                          X         ]
   l2''[                    x         X     ]

仅看 $l''$ 子块：曲率项 $w_{l''}\sum (l''_i)^2$ 贡献单位阵 $I$，jerk 项 $w_{l'''}\sum_{\text{gap}}(l''_{i+1}-l''_i)^2$ 贡献一个"一阶差分 Gram 矩阵" $L$，合起来（$\Delta s=1$、权重为 1，差一个由 $\tfrac12 x^\top P x$ 约定带来的整体 $2\times$ 因子，这里只看结构本质）：

\[ P_{l''}\ \propto\ I + L = \begin{bmatrix}1&&\\&1&\\&&1\end{bmatrix} + \begin{bmatrix}1&-1&\\-1&2&-1\\&-1&1\end{bmatrix} = \begin{bmatrix}2&-1&0\\-1&3&-1\\0&-1&2\end{bmatrix} \]

这就是 §T1.3 "本质洞察"里说的**稀疏带状**的来历——它不是抽象口号，就是这个三对角。 OSQP 吃的正是这种结构，几百维也毫秒级。 别忘了：交给 OSQP 1.0 时只填这个 $P$ 的上三角（§T1.3 编程陷阱）。

解的样子：QP 会让 $l_1$ 恰好压到约 $-0.5$（障碍约束 active），$l_0, l_2$ 附近回到 0，整条 $l(s)$ 是一条平滑的小"右凸"——既满足障碍约束，又因 jerk 项而不显突兀。把 $w_{l'''}$ 调到 0 再解一遍，你会看到 $l(s)$ 在 station 1 出现更尖的折角：这就是 jerk 项在"磨平"路径。把这个 9 维例子手推一遍，再去读 Apollo 的 piecewise_jerk_path，几百维的组装就只是同一件事的放大。

QP 权重怎么调（一点工程手感）¶

刚才把 $w_{l'''}$ 调到 0、看折角变尖的那个小实验，已经是调参的一个缩影——权重怎么设，直接决定路径的脾气。 $w_l, w_{l'}, w_{l''}, w_{l'''}$ 之间是**相对**关系——只有比值有意义，整体同乘一个常数不改变最优解。常见调参手感：先把 $w_l$（贴参考线）定为基准 1，再调其余。 $w_{l''}$（曲率 / 舒适）调大，路径会变得"懒得拐"，绕障更迟缓、更贴近直线，太大则可能贴不够障碍裕度； $w_{l'''}$（jerk）调大，横向动作更平滑、转向更柔，太大则响应迟钝、避障不够利落。障碍与边界是**硬约束**（box），不靠权重控制——权重只在"已经可行的解集合"内部挑舒适度。

落地顺序建议：先保证可行（约束写对、上下界不打架），再调 $w_{l''}/w_{l'''}$ 找"平滑 vs 灵活"的平衡，最后微调 $w_l$ 控制贴线程度。一个实用自检：把某个权重翻 10 倍重解一遍，看 $l(s)$ 往哪个方向变，就知道这个权重到底"管什么"。 speed QP 的 $w_s, w_v, w_a, w_j$ 同理（贴进度 / 贴速度 / 抑加速度 / 抑 jerk），调法一致。

思维提醒：别指望权重解决"决策"问题——绕左还是绕右、让还是抢，是 §T1.4 DP 的**离散决策**，不是连续权重能调出来的。权重只在 DP 选定的同伦类**内部**塑形。见过太多人把"避障方向不对"当成调权重的问题，结果怎么调都不对——因为那根本是上游决策错了。

代码实现（为什么 → 正确 → 错误 → 对比）¶

Step 1 — 为什么这样写：用 $[l, l', l'']$ 状态表示 + piecewise-constant-jerk 积分，是为了让 QP 稀疏带状、约束 box 化、状态可解释（见上文本质洞察）。求解器选 OSQP——ADMM 一阶法、利用稀疏性、可编译为无依赖嵌入式库，适合实时控制。

Step 2 — 正确写法。先看状态布局与一条连续性等式约束怎么落成线性行（下面是**教学实现**，非 Apollo 源码原文）：

// 决策向量 x 布局:每个 station 三个状态,按 [l_0, dl_0, ddl_0, l_1, dl_1, ddl_1, ...] 排列
// 第 i 个 station 的三个变量下标:
inline int IdxL  (int i) { return 3 * i + 0; }  // l_i
inline int IdxDL (int i) { return 3 * i + 1; }  // l'_i  (dl/ds)
inline int IdxDDL(int i) { return 3 * i + 2; }  // l''_i (d²l/ds²)

// 连续性等式之一(消去 jerk 后的 l 关系):
//   l_{i+1} - l_i - l'_i·Δs - ½·l''_i·Δs² - ⅙·(l''_{i+1}-l''_i)·Δs² = 0
// 写成稀疏行 A_row·x = 0(只触碰相邻两 station 的 6 个变量,故矩阵带状)
void AddLContinuityRow(SparseTriplets* A, int row, int i, double ds) {
  const double ds2 = ds * ds;
  A->emplace_back(row, IdxL(i + 1),   1.0);            // +l_{i+1}
  A->emplace_back(row, IdxL(i),      -1.0);            // -l_i
  A->emplace_back(row, IdxDL(i),     -ds);             // -l'_i·Δs
  A->emplace_back(row, IdxDDL(i),    -0.5 * ds2 + (ds2 / 6.0)); // l''_i 的合并系数
  A->emplace_back(row, IdxDDL(i + 1), -ds2 / 6.0);     // l''_{i+1} 项(来自 jerk 代入)
  // 该行的上下界都设为 0(等式约束):lb[row]=ub[row]=0
}

横向曲率上下界，注意那个**减去 $\kappa$** 的细节（来源：Apollo piecewise_jerk_path 的 ddl_bounds 逻辑）：

// l''(ddl) 的逐 station 上下界:留给 l'' 的曲率裕度 = 车辆最大曲率 ∓ 参考线曲率
// a_kappa_max:车辆最大转向对应的路径曲率(名为 acc,量纲实为 1/m)
const double a_kappa_max =
    std::tan(veh.max_steer_angle / veh.steer_ratio) / veh.wheel_base;
for (int i = 0; i < N; ++i) {
  const double kappa_r = ref_line.NearestPoint(s[i]).kappa;   // 参考线在 s_i 处的曲率
  ddl_lb[i] = -a_kappa_max - kappa_r;   // 减 κ:扣掉参考线本身占用的曲率
  ddl_ub[i] =  a_kappa_max - kappa_r;
}

最后把代价 Hessian、约束、上下界交给 OSQP（注意 OSQP 1.0 的 $P$ 只读上三角）：

// 代价 J = ½ xᵀP x + qᵀx。P 由各平方项组装,半正定。
// ⚠️ OSQP 1.0 约定:P 必须以【上三角】CSC 提供(只填 i<=j 的元素);0.6.x 接口不同。
OSQPSolver* solver = nullptr;
osqp_setup(&solver, P_upper_csc, q.data(), A_csc,
           l_bounds.data(), u_bounds.data(), m, n, settings);  // m 约束数, n=3N
osqp_solve(solver);                       // ADMM 迭代,通常 < 5 ms
// 解 solver->solution->x 即 [l_i, l'_i, l''_i] 序列 → 重建 l(s)

Step 3 — 错误写法并解释为什么错：

// ❌ 错误 1:连续性只用一阶 Euler,漏掉 ½Δs² 和 ⅙Δs³ 的高阶项
//   l_{i+1} = l_i + l'_i·Δs;  l'_{i+1} = l'_i + l''_i·Δs;   // 缺高阶项!
//   现象:解出的 l(s) 与其声称的 l'、l'' 自相矛盾,在曲率约束下要么不可行、要么抖动。
//   根因:把"匀 jerk 三次积分"退化成了一次积分,状态间不再自洽。

// ❌ 错误 2:ddl 上下界忘了减参考线曲率 κ
   ddl_lb[i] = -a_kappa_max;  ddl_ub[i] = a_kappa_max;       // 漏了 -κ_r
//   现象:直道没事,弯道上路径要么算不出(可行域被错误收紧)、要么过保守贴不住车道。
//   根因:没扣掉参考线本身占用的曲率,把"总曲率上限"误当成"l'' 的上限"。

// ❌ 错误 3:把完整对称 P 喂给 OSQP 1.0
//   现象:数值不对或直接报错。根因:OSQP 1.0 只读 P 的上三角,重复计入下三角=权重翻倍。
//   正确:只填上三角。自检:用一个有解析解的两点小例子核对。

Step 4 — 多种实现方式对比：

维度	spline-QP（EM 2018）	piecewise-jerk QP（Apollo 现行）
决策变量	分段多项式系数	每 station 的 $[l, l', l'']$ 状态
矩阵结构	较密（系数跨段耦合）	稀疏带状（仅相邻 station 耦合）
物理可解释性	弱（系数无直观含义）	强（偏移 / 斜率 / 曲率）
box 约束	不直接（需经基函数换算）	直接（状态即被约束量）
调参与求解	相对难调	更快、更好调
Apollo 现状	已被替换	当前路径优化器所用

⚠️ 常见陷阱¶

⚠️ 编程陷阱：把完整对称 Hessian $P$ 直接喂给 OSQP 1.0 - 错误做法：按数学习惯组装完整对称 $P$（上下三角都填）后调 osqp_setup。 - 现象：结果数值不对，或求解器报参数错误；二次项实际被算成了两倍权重。 - 根本原因：OSQP 1.0 的 C 接口约定 $P$ 以**上三角** CSC 提供（只读 $i\le j$ 的元素）； 0.6.x 接口与此不同，跨版本迁移代码极易踩这条。 - 正确做法：只填上三角；明确锁定并在注释里写清所用 OSQP 版本。自检：用一个有解析解的两点小 QP 核对求解器输出。

💡 概念误区：以为 path QP 里的 "jerk" 就是乘客感受到的那个 jerk - 错误想法：调大 $w_{l'''}$ 就能让乘坐更舒适（减小冲击感）。 - 实际情况：这里的 jerk 是 $\mathrm{d}^3 l/\mathrm{d}s^3$——横向偏移对**弧长**的三阶导，是路径的**空间平滑度**，不是对**时间**的 jerk。乘客感受到的时间 jerk 由速度规划（§T1.5）决定。 - 为什么重要：把两者混为一谈，会在错误的模块里调错误的旋钮——舒适度问题在 path QP 里怎么调都收效甚微。 - 正确做法：空间平滑找 path QP（$w_{l'''}$），时间舒适找 speed QP（§T1.5 的 jerk 项）。

🧠 思维陷阱：认为权重 $w$ 越大越好、可以随便拍 - 错误想法：想要平滑就把 $w_{l''}, w_{l'''}$ 拉满，想居中就把 $w_l$ 拉满。 - 实际情况：这是一组**此消彼长**的权衡——$w_l$ 过大→紧贴参考线但可能贴上障碍； $w_{l''}/w_{l'''}$ 过大→路径超平滑但"反应迟钝"，该避让时让不开。 - 正确思维：权重的**相对比例**比绝对数值重要；按场景（高速求平滑、拥堵求灵活）平衡，且应在典型场景集上回归调参，而非单场景拍脑袋。

练习¶

[实现] 实现一个最简 piecewise-jerk 路径 QP：输入逐 station 的 $[l_{\text{lb}}, l_{\text{ub}}]$ 与起点状态 $[l_0, l'_0, l''_0]$，组装连续性等式 + box 约束 + 平滑代价，用 OSQP 求解并重建 $l(s)$。对比开启与关闭 $w_{l'''}$ 项时路径的平滑度差异。
[调试] 给定一段把连续性约束写成一阶 Euler（漏掉 $\tfrac12\Delta s^2$、$\tfrac16\Delta s^3$）的代码，解释为什么求得的 $l(s)$ 与它声称的 $l', l''$ 自相矛盾、在曲率约束下不可行；并改回完整三次积分。
[推导] 从匀 jerk 运动学推导三条连续性等式，并消去 jerk 得到只含状态的等式形式。然后论证 ddl_bounds 为什么要减去参考线曲率 $\kappa(s_i)$（提示：车辆路径总曲率 $\approx \kappa + l''$ 的小偏移近似，总曲率受最大转向限制）。

§T1.4 ST 图与 DP-ST 速度搜索 ⭐⭐¶

这一节解决什么问题：路径已经定了（§T1.3），现在要决定"什么时候走到路径上的哪一点"——即速度剖面 $s(t)$。动态障碍是在**时间**上和我们交互的（一个横穿的行人只在某个时间窗里挡住路径），所以速度规划需要一张把"时间"显式画出来的图：ST 图。本节讲它怎么建、为什么求解要"先 DP 再 QP"。

动机：速度规划要回答的是"何时到哪"¶

§T1.3 给出的是一条画在地面上的几何路径，它没有时间。可真实交通里，能不能走、走多快，取决于动态障碍**在什么时刻**占据路径的哪一段。把这件事画清楚的工具就是 ST 图：横轴是时间 $t$，纵轴是沿**那条固定路径**的弧长 $s$。自车的速度计划就是这张图上一条曲线 $s(t)$——"到时刻 $t$ 我已经沿路径走到了 $s$ 处"。

回到 §T1.1 的历史线：Kant–Zucker 1986 早就把速度规划（VPP）放进 **path-time 空间**并化为图搜索。本节的 ST 图 + DP-ST 搜索，正是那个四十年前的想法，在 HD 地图 + 动态障碍预测下的现代工程形态。它不是新发明，是老思想的工业落地。

用一个有边界的类比建立直觉：

可以把 ST 图当成物理里的**时空图（spacetime diagram）：横轴时间、纵轴位置（这里是沿路径的弧长 $s$）。每个动态障碍画出一条"世界线管道"——它在各时刻占据的 $s$ 区间；自车的 $s(t)$ 就是自己的世界线。" 不碰撞" = 自车世界线不穿过任何障碍的管道。 **像的地方：都在 (时间, 位置) 平面上用曲线描述运动，"曲线不相交"即"不碰撞"。 不像的地方：物理时空图里粒子可前可后、时空近乎对称；这里 $s$ 被强制**单调不减**（车不能沿路径倒退），时间也只能前进——所以自车世界线只能是一条朝右上方单调延伸的曲线。别把相对论时空图的那些自由度套到这里。

如果不这样做会怎样（反事实）¶

假设我们不建 ST 图、不先做 DP，而是直接对 $s(t)$ 列一个 QP 求解：

你会立刻撞上一个结构性障碍：每个动态障碍在 ST 平面上挖出一块阻挡区域，自车曲线要么从它**上方**穿过（抢行 overtake），要么从**下方**穿过（让行 yield）。这两类路线**互不连通**——可行域是非凸的、分裂的。
直接上凸 QP 会怎样？ QP 只能在一个**连通**的凸区域里找最优；面对"上方 or 下方"这种分裂选择，它无法自己决定该钻哪个洞，给它一个错误的初值就卡死在错误的一侧。

这正是为什么速度规划要分两段：先用 DP 在 ST 图上做"让不让"的离散决策，把非凸问题切成一个连通的凸走廊，再交给 QP 精修（§T1.5）。

理论：ST 图构建与 DP-ST 搜索¶

ST 图构建：

路径已固定（§T1.3）。把每个动态障碍的**预测轨迹**投影到 $(t, s)$ 平面：对每个时刻 $t$，问"障碍此刻占据了自车路径上哪一段 $s$？" ——得到一块随时间变化的阻挡区域。
阻挡区域的形状反映障碍运动：匀速 → 平行四边形（占据的 $s$ 带随 $t$ 线性平移）；横穿且匀速 → 近似**矩形**（固定 $s$ 段在固定时间窗内被占）；加 / 减速 → 梯形。这块区域在 Apollo 里就是 STBoundary 数据结构。
自车速度剖面 $s(t)$ 是从 $(t{=}0, s{=}s_0)$ 出发、**单调不减**的曲线。
关键决策：$s(t)$ 从每个阻挡上方过（overtake）还是下方过（yield）。

本质洞察：速度规划真正的难点不是"让多少"，而是"让不让"。每个动态障碍把 ST 平面切成"从上方过（抢行）"和"从下方过（让行）"两个**互不连通**的可行类——这是离散决策，非凸的根源就在这里。 DP-ST 搜索本质上不是在求最优速度曲线，而是在替每个障碍做出 yield/overtake 决策、从而把非凸问题切成一个 QP 能精修的凸走廊。这也解释了 Apollo "DP 构造凸空间 → QP 精修"范式为什么必须是两段、而非一段。

DP-ST 搜索：把 ST 平面用 $(\Delta t, \Delta s)$ 离散成网格，节点 $(t_i, s_j)$ 表示"自车在时刻 $t_i$ 处于弧长 $s_j$"。做前向动态规划（本质是 Dijkstra）：

边：从 $(t_i, s_j)$ 到 $(t_{i+1}, s_k)$，表示在 $\Delta t$ 内从 $s_j$ 走到 $s_k$。
边代价：加速度 + jerk + 偏离期望速度 + 离阻挡过近的惩罚。
约束：$s$ 单调不减（$s_k\ge s_j$）、$v=(s_k-s_j)/\Delta t\in[0, v_{\max}]$、$a\in[a_{\min}, a_{\max}]$。
碰撞检测：若 $(t_i, s_j)$ 落在任一阻挡区域内部，则该节点不可达（代价 $+\infty$）。

DP 产出一条粗粒度速度 profile，它有两个用途：（a）给出对每个障碍的 yield/overtake 决策；（b）作为 §T1.5 QP 的 warm start 与 ST 上下界 $[s_{\text{lb}}(t), s_{\text{ub}}(t)]$。第二点是关键——DP 的决策把"该钻哪个洞"固定下来后，QP 面对的就是一个连通的凸走廊了。

边代价怎么设计（把上面那句"加速度 + jerk + 偏离期望速度 + 惩罚"展开）。一条从 $(t_i, s_j)$ 到 $(t_{i+1}, s_k)$ 的边，代价常取

\[ \text{EdgeCost} = w_a\,a^2 + w_j\,j^2 + w_v\,(v - v_{\text{ref}})^2 + w_{\text{obs}}\cdot \phi_{\text{dist}} \]

其中 $v=(s_k-s_j)/\Delta t$、$a$、$j$ 由相邻段差分估计，$\phi_{\text{dist}}$ 是离阻挡越近惩罚越大的项。为什么 DP 边代价里要放这些"舒适性"项，而不是只判可行/不可行？因为同一个 yield 决策下可能有很多条可行折线，DP 要在**选决策的同时**粗略偏好平滑、接近期望速度的那条——否则它可能选出一条"虽让行但急刹到底再猛加速"的极端可行解，让后续 QP 的精修起点很差。 DP 不求精确，但要求"决策对、起点不离谱"。

DP 复杂度。节点数为 $N_t\times N_s$，每个节点向不超过 $N_s$ 个 $s_k$ 连边，故时间复杂度 $\mathcal O(N_t\cdot N_s^2)$。注意它对 $s$ 方向的分辨率 $N_s$ 是**平方**敏感的——这正是 §T1.4 网格分辨率陷阱的代价根源：$N_s$ 翻倍、计算量翻四倍，而 $N_s$ 太小又会漏掉障碍间的窄缝。

多视角：把 DP-ST 和你熟悉的最短路算法对照。像**的地方：都是在图上求最小代价路径，本质是 Dijkstra/Bellman。 **不像**的地方：ST 图里时间单向流动（$t_i\to t_{i+1}$）、$s$ 单调不减，整张图是**分层有向无环图（DAG）——所以不需要 Dijkstra 的优先队列，只要按时间层 $t_0, t_1, \dots$ 顺序前向递推即可，比一般 Dijkstra 更简单。若想进一步加速，可像 A* 那样用"到目标的乐观代价估计"剪掉明显劣的节点。理解了这层 DAG 结构，你就知道 DP-ST 为什么能做到线性于层数。

本质洞察（补充）：DP 这一段的真正产物不是那条阶梯状 $s(t)$，而是**一组离散决策 + 一条凸走廊**。把它想成"先决定剧情走向（让谁、抢谁），再让 QP 把镜头拍平滑"。这就是 Apollo 把速度规划拆成 DP + QP 两段、而不是一个大优化的全部理由。

一个 ST-DP 的最小算例（接 §T1.7 跟车场景）¶

形式上，DP 在 ST 网格上求解的是这样一个累积代价递推（Bellman 方程）：

\[ J^*(t_i, s_k) = \min_{s_j}\Big[\,J^*(t_{i-1}, s_j) + c_{\text{edge}}(s_j \to s_k)\,\Big] \]

其中边代价 $c_{\text{edge}}$ 把"从上一层的 $s_j$ 走到这一层的 $s_k$"打包成一个数：隐含速度 $(s_k-s_j)/\Delta t$ 是否越限、这条边是否扫过 ST 障碍（扫过则代价记为 $+\infty$，等于禁行）、以及与期望巡航速度的偏离。逐层从 $t_0$ 推到末层 $t_{N_t}$、记下每个节点的最优前驱，再从末层最小代价节点回溯，就得到那条世界线。拿具体数字走一遍：前车 $t{=}0$ 在 $s{=}30$、匀速 $6\,\text{m/s}$，自车 $v_0=8$。把 ST 网格离散为 $\Delta t=0.5\,\text{s}$、$t\in\{0, 0.5, 1.0, 1.5, 2.0\}$（5 层），$s$ 每 $2\,\text{m}$ 一格。前车阻挡带（含约 $4\,\text{m}$ 膨胀）在 $t$ 时刻占据 $s\in[\,30+6t-2,\ 30+6t+L\,]$，即一条以 $6\,\text{m/s}$ 向上平移的斜带。

自车从 $(t{=}0, s{=}0)$ 出发做前向 DP，逐层挑累积代价最小的边：

若维持 $8\,\text{m/s}$：$s$ 每层 $+4\,\text{m}$（$8\times 0.5$），到 $t{=}2$ 时 $s{=}16$，离前车（此刻在 $s{\approx}42$）还远——近处不冲突；但把时域拉长到追上点，这条"维持 8"的边会在某层撞进阻挡带，代价 $+\infty$（不可行）。
DP 因此在更早的层就偏好减速边：让 $s(t)$ 的斜率降到约 $6\,\text{m/s}$，贴着阻挡带下沿走——这一串"减速"低代价边连起来，就是 yield 决策。
输出：决策 = yield；走廊 $s_{\text{ub}}(t)=$ 阻挡带下沿，$s_{\text{lb}}(t)=$ 不倒退下限。这条走廊交给 §T1.5 的 speed QP 精修。

注意网格分辨率的影响：若 $\Delta s$ 取太粗（如 $5\,\text{m}$），DP 可能"看不见"前车与某条边的薄接触、误判其可行——这正是 §T1.4 分辨率陷阱的具体长相。把这个 5 层小网格在纸上画出来、手动填一遍每个节点的最小累积代价，DP-ST 就从"一个名词"变成了"一套你会算的递推"。

代码实现（为什么 → 正确 → 错误 → 对比）¶

Step 1 — 为什么这样写：yield/overtake 是离散、组合、非凸的决策，网格 + Dijkstra 能自然地把"上方过"和"下方过"两条路线都探一遍，挑出可达且代价最小的那条。 DP 在这里是**做决策**的工具，不是求精确解的工具（精确交给 QP）。

Step 2 — 正确写法（教学实现）：

// ST 网格 + 前向 DP:求每个节点的最小到达代价,回溯得到粗速度 profile
std::vector<SpeedPoint> DpStSearch(const std::vector<STBoundary>& obs,
                                   double s0, double v0, const STGrid& g) {
  // dp[ti][sj]=到达 (t_i, s_j) 的最小累计代价;back 记录前驱 s 下标用于回溯
  Grid2D<double> dp(g.Nt, g.Ns, +INF);
  Grid2D<int>    back(g.Nt, g.Ns, -1);
  dp(0, g.IdxS(s0)) = 0.0;

  for (int ti = 0; ti + 1 < g.Nt; ++ti) {
    for (int sj = 0; sj < g.Ns; ++sj) {
      if (dp(ti, sj) == INF) continue;
      for (int sk = sj; sk < g.Ns; ++sk) {              // sk>=sj:s 单调不减(不能倒退)
        const double v = (g.S(sk) - g.S(sj)) / g.dt;    // 该段平均速度
        if (v > g.v_max) break;                          // v 随 sk 增而增,越界即可 break
        const double a = (v - g.PrevV(ti, sj)) / g.dt;   // 估计加速度
        if (a < g.a_min || a > g.a_max) continue;        // 加速度约束
        if (InAnyBoundary(obs, g.T(ti + 1), g.S(sk)))    // 落在阻挡内 → 不可达
          continue;
        const double c = dp(ti, sj) + EdgeCost(v, a, /*jerk, 偏离期望速度*/);
        if (c < dp(ti + 1, sk)) { dp(ti + 1, sk) = c; back(ti + 1, sk) = sj; }
      }
    }
  }
  return Backtrack(dp, back, g);   // 回溯最小代价终点 → 粗速度 profile + yield/overtake 决策
}

注意 if (v > g.v_max) break; 用的是 break 而非 continue：因为 sk 递增时 $v$ 单调增，一旦越界，更大的 sk 必然也越界，直接跳出内层即可——这是利用单调性的小优化。

Step 3 — 错误写法并解释为什么错：

// ❌ 错误 1:允许 s 递减(把 sk 从 0 开始,而非从 sj 开始)
   for (int sk = 0; sk < g.Ns; ++sk) { ... }        // sk<sj 意味着 s 倒退!
//   现象:DP 可能选出"自车沿路径往回开"来绕开阻挡——物理上荒谬,且 §T1.5 QP 接不住。
//   根因:漏掉了弧长单调性约束 sk >= sj。

// ❌ 错误 2:碰撞检测只判几何中心点,不按尺寸+裕度膨胀阻挡区域
   if (PointInPolygon(t, s)) ...                     // 只看中心点
//   现象:s(t) 贴着阻挡边缘"擦过",可视化上"没进去",实车却剐蹭。
//   根因:没把自车长度、障碍尺寸、安全裕度膨胀进 STBoundary。

// ❌ 错误 3:网格过粗(Δs 太大)
//   现象:两个相邻障碍之间本有一条窄缝可钻,但网格采样跨过了它 → DP 判无解或被迫绕远。
//   根因:DP 的可行性受网格分辨率限制。Δt/Δs 要在"分辨窄缝"与"计算量"间权衡。

Step 4 — 多种实现方式对比（为什么是"DP→QP 两段"而非别的）：

路线	能否处理 yield/overtake 决策	精度	实时性	备注
直接凸 QP	否（非凸决策无法自决）	高（在给定凸走廊内）	快	需外部先给定决策才可用
纯采样速度曲线	是（采样覆盖两侧）	受采样密度限	中	复杂场景覆盖不全
DP-ST → QP（Apollo）	是（DP 做决策）	DP 粗 + QP 精	快	两段互补，本章采用

⚠️ 常见陷阱¶

⚠️ 编程陷阱：ST 碰撞检测忘了按"自车 + 障碍尺寸 + 安全裕度"膨胀阻挡区域 - 错误做法：把障碍预测轨迹直接投影成一条线 / 一个点，碰撞检测只判自车质心是否落在其中。 - 现象：DP 选出的 $s(t)$ 贴着阻挡边缘"恰好不碰"，可视化上没穿过去，实车却发生剐蹭或惊险擦行。 - 根本原因：阻挡区域应是"障碍占据 + 自车半长 + 安全裕度"的闵可夫斯基膨胀，只判点等于把车和障碍都当成质点。 - 正确做法：构建 STBoundary 时按自车纵向半长、障碍尺寸、纵向安全时距统一膨胀。自检：可视化时把膨胀后的阻挡和 $s(t)$ 一起画，确认有可见间隙。

💡 概念误区：以为 DP-ST 输出的就是最终精确速度曲线 - 错误想法：DP 既然给出了 $s(t)$，直接拿去跟踪就行。 - 实际情况：DP 的结果是**网格量化的粗解**，加速度 / jerk 都是阶梯状、不平滑，不能直接执行。它的真正职责是（a）定下每个障碍的 yield/overtake 决策，（b）给 §T1.5 的 QP 提供 warm start 和 ST 上下界。 - 为什么重要：把粗 DP 解当终解去跟踪，会得到颠簸、超 jerk 的速度，乘坐极差。 - 正确做法：DP 定决策 + 给走廊，QP（§T1.5）在走廊内精修出平滑的 $s(t)$。

🧠 思维陷阱：把 yield/overtake 当成一个连续的"让多少"问题 - 错误想法：让行和抢行是同一条曲线的连续调节，调个参数就能从让滑到抢。 - 实际情况：从阻挡上方过和从下方过是 ST 平面上两个**互不连通**的可行类（homotopy 类），中间隔着阻挡区域，没法连续过渡——这是离散决策。 - 正确思维：先认清非凸性的来源是这个离散决策，才能理解"为什么必须先 DP（选类）再 QP（类内精修）"，而不是指望一个优化器一步到位。

练习¶

[实现] 手写一个 ST 图构建器 + DP-ST 搜索：给定一条固定路径和 3 个匀速动态障碍（投影成平行四边形 / 矩形），在 $(\Delta t, \Delta s)$ 网格上做前向 DP，输出粗速度 profile，并在 ST 图上可视化它对每个障碍做出的 yield / overtake 决策。
[调试] 给定一段允许 $s$ 递减（内层 sk 从 0 开始）的 DP 代码，解释它为什么会产出"自车沿路径倒退"的荒谬解；再给一个网格过粗、漏掉两障碍间窄缝的例子，分析"分辨率 vs 可行性 vs 计算量"的三方权衡。
[跨章综合题 · 综合 §T1.1 + §T1.3 + §T1.4] 给定一个 cut-in（旁车切入）场景：（a）用 §T1.3 在 SL 图上规划一条几何路径；（b）用本节的 ST 图 + DP 规划速度；（c）指出在哪一步会暴露出 §T1.1 所说的"路径与速度强耦合导致顺序解耦失败"（提示：先定的路径贴近切入车，使速度阶段在 ST 图上**两侧都不可行**）。这个失败正是 §T1.6 EM 迭代要缓解的问题——把三节的概念串成一条因果链。

§T1.5 piecewise-jerk speed QP ⭐⭐⭐¶

这一节解决什么问题：§T1.4 的 DP 已经替每个动态障碍做出了 yield/overtake 决策、给出了一条连通的 ST 走廊 $[s_{\text{lb}}(t), s_{\text{ub}}(t)]$，但 DP 解是网格量化的、阶梯状的。本节用一个凸 QP 在这条走廊里把它**精修**成平滑、舒适、动力学可行的速度剖面 $s(t)$。这是 Apollo "DP 构造凸空间 → QP 精修"范式的最后一段。

动机：把 DP 的粗解精修成能执行的平滑速度¶

回顾 §T1.3：我们用 piecewise-jerk QP 求过路径——以每个 station 的 $[l, l', l'']$ 为状态、假设段内 jerk 恒定来积分、最小化平滑代价。本节是**同一台机器换一个维度**：把自变量从弧长 $s$ 换成时间 $t$，状态从横向 $[l, l', l'']$ 换成纵向 $[s, v, a]$，约束来源从"道路 + 障碍横向边界"换成"§T1.4 DP 给的 ST 走廊"。机器一样，物理含义不同。

为什么不直接用 §T1.4 DP 的输出去跟踪？因为它是网格量化的粗解：加速度、jerk 都是阶梯状，乘坐颠簸、甚至违反 jerk 上限（§T1.4 概念误区）。我们需要在 DP 选定的走廊里，求一条**真正平滑**的 $s(t)$。

如果不这样做会怎样（反事实）¶

直接执行 DP 粗解 → 加速度阶跃、jerk 无界，车开起来一顿一顿，舒适性极差。
抛开 DP 走廊、对 $s(t)$ 重新上一个无约束优化 → 又掉回 §T1.4 讲的非凸陷阱：没有走廊约束，优化器不知道该从障碍上方还是下方过。
所以正确姿势只能是：继承 DP 的决策与走廊，在凸走廊内做凸 QP 精修。

理论：speed QP 的状态、代价与约束¶

决策变量：每个时间步 $i=0,\dots,N$ 上的 $[s_i, v_i, a_i]$。其中 $v_i=\dot s_i$（速度）、$a_i=\ddot s_i$（加速度）。

连续性等式约束（piecewise-constant-jerk 积分，与 §T1.3 同构，但对时间 $t$）。设段内 $\dddot s\equiv j_i$：

\[a_{i+1} = a_i + j_i\,\Delta t$$ $$v_{i+1} = v_i + a_i\,\Delta t + \tfrac12 j_i\,\Delta t^2$$ $$s_{i+1} = s_i + v_i\,\Delta t + \tfrac12 a_i\,\Delta t^2 + \tfrac16 j_i\,\Delta t^3\]

由第一式 $j_i=(a_{i+1}-a_i)/\Delta t$ 代回，得只含状态的两条等式约束。

代价函数：

\[ J = w_s\sum_i (s_i - s_{\text{ref},i})^2 + w_v\sum_i (v_i - v_{\text{ref},i})^2 + w_a\sum_i a_i^2 + w_j\sum_i\Big(\frac{a_{i+1}-a_i}{\Delta t}\Big)^2 \]

逐项：$w_s$ 拉向期望进度（如跟随 DP 的 profile 或巡航目标）； $w_v$ 拉向期望速度（限速 / 巡航速度）； $w_a$ 抑制加速度； $w_j$ 抑制 jerk。

这里的 jerk $\dfrac{a_{i+1}-a_i}{\Delta t}\approx \dddot s = \mathrm{d}^3 s/\mathrm{d}t^3$ 是对**时间**的三阶导——正是乘客身体感受到的那个 jerk。请对照 §T1.3 的概念误区：那里的 jerk 是 $\mathrm{d}^3 l/\mathrm{d}s^3$（路径的空间平滑度），这里才是时间 jerk（乘坐舒适度）。同一套 piecewise-jerk 机器，跑在不同自变量上，调的是完全不同的"舒适"。这个区分到这一节算彻底闭合了。

约束：

ST 走廊（来自 §T1.4 DP 的决策）：$s_{\text{lb}}(t_i)\le s_i\le s_{\text{ub}}(t_i)$。 yield 把上界压低（停在阻挡下方），overtake 把下界抬高（冲到阻挡上方）。
单调性：$s_{i+1}\ge s_i$（车不沿路径倒退）。
速度上界（含向心加速度折减）：$0\le v_i\le v_{\text{limit}}(s_i)$，其中

\[ v_{\text{limit}}(s) = \min\Big(v_{\max},\ \sqrt{a_{\text{lat,max}}\,/\,|\kappa(s)|}\Big) \]

这一项很关键：横向（向心）加速度 $a_{\text{lat}} = v^2\,\kappa$，要它不超 $a_{\text{lat,max}}$，速度就得满足 $v\le\sqrt{a_{\text{lat,max}}/|\kappa|}$。 注意 $\kappa(s)$ 来自 §T1.3 已经定死的路径——弯越急，速度被压得越低。 - 加速度 / jerk 约束：$a_{\min}\le a_i\le a_{\max}$，$|j_i|\le j_{\max}$。

凸性兑现：代价平方和 → $P\succeq 0$；约束全线性（等式 + box）→ 凸 QP，OSQP 在 10 ms 内给全局最优。

数值算例（用 §T1.7 的弯道场景）¶

借 §T1.7 的数把抽象约束落到具体数字：弯道 $\kappa=0.04$、$a_{\text{lat,max}}=2$、$\Delta t=0.2\,\text{s}$、自车 $v_0=8$、前车 $6\,\text{m/s}$。

向心折减先算清楚：

\[v_{\text{limit}}^{\text{curve}} = \sqrt{a_{\text{lat,max}}/|\kappa|} = \sqrt{2/0.04} = \sqrt{50}\approx 7.07\,\text{m/s}\]

也就是说，哪怕限速是 12，进这段弯也不能超 7.07，否则横向加速度 $a_{\text{lat}}=v^2\kappa$ 破 2。

连续性一行长什么样（$\Delta t=0.2 \Rightarrow \Delta t^2=0.04$，代入 §T1.5 的两条状态关系）：

\[v_{i+1} = v_i + 0.1\,(a_i + a_{i+1}),\qquad s_{i+1} = s_i + 0.2\,v_i + 0.0133\,a_i + 0.0067\,a_{i+1}\]

（系数来自 $\tfrac12\Delta t = 0.1$、$\tfrac13\Delta t^2 \approx 0.0133$、$\tfrac16\Delta t^2 \approx 0.0067$。）这些就是 QP 等式约束矩阵里那几行的数。

走廊与速度界叠加：前车 yield 把 $s_{\text{ub}}(t)$ 压在前车后方（保持安全时距），把 $v$ 拉向 6；向心界把 $v$ 顶在 7.07 以下。本例 $6 < 7.07$ → 跟车约束更紧，弯道里稳定在约 $6\,\text{m/s}$。

反事实核对（确认你抓住了耦合）：若**没有**前车，QP 会想加速到接近限速 12，但 $v_{\text{limit}}^{\text{curve}}=7.07$ 这个 box 上界把它截在 7.07——此时向心界成了 binding 约束。再把 $a_{\text{lat,max}}$ 从 2 调到 4，$v_{\text{limit}}$ 升到 $\sqrt{100}=10$，过弯就能更快——这条 $v\le\sqrt{a_{\text{lat,max}}/\kappa}$ 的关系，正是你给整车调"过弯激进度"的旋钮。

jerk 平滑的效果：§T1.4 DP 给的粗解可能是"8 → 6 一步到位"的阶跃（jerk 无穷大）； speed QP 在 $|j|\le j_{\max}=4$ 约束下，把这步减速摊到若干个 $\Delta t$ 上，加速度平滑过渡（如约 $-2\,\text{m/s}^2$ 持续约 1 s），乘客感受到的是一次平稳刹车而非顿挫。这一步"磨平"，正是 QP 相对 DP 的全部价值所在。

硬约束不可行时怎么办：软化与松弛¶

前面一直默认 ST 走廊里总能解出一条平滑速度。但实车上并不总是如此——走廊偶尔会"无解"——比如前车急刹使 $s_{\text{ub}}(t)$ 压得过低、与"起点速度 + 减速度上限"冲突。若全用硬约束，QP 直接报 infeasible，规划器这一周期**哑火**，在高速上是安全事故。工程上的标准做法是把部分约束**软化**：引入非负松弛变量 $\xi_i\ge 0$，把硬约束 $s_i\le s_{\text{ub}}(t_i)$ 改写成

\[s_i \le s_{\text{ub}}(t_i) + \xi_i,\qquad \xi_i\ge 0\]

并在代价里加一项大权重惩罚 $w_\xi\sum_i \xi_i^2$（$w_\xi$ 取得很大）。

这样：能满足时 $\xi=0$（与硬约束等价）；实在满足不了时也给出"尽量少越界"的解，而非彻底失败——规划器永远有输出，下游控制不会断流。

哪些该软、哪些必须硬？经验法则：**安全相关**的约束（碰撞边界、速度上限）尽量硬、或用很大的 $w_\xi$ 重罚软化； **舒适 / 偏好**相关的（贴近期望速度、贴近参考线）本就以代价项形式存在、天然是软的。把碰撞约束随手软化会埋安全隐患，把舒适约束做成硬约束又会频繁不可行——分清这条边界，是把规划器从"demo 能跑"推进到"实车不哑火"的关键一步。

多视角（与 MPC 的关系）：若你学过模型预测控制，会发现这里的 speed QP 其实就是一个**单步 MPC 问题**——状态 $[s, v, a]$、控制量 jerk、有限时域、软硬约束混合、滚动求解。整条规控本质上是"每个周期解一个带约束的最优控制问题"，piecewise-jerk QP 只是它在速度维度的一个具体实例。像：都是有限时域带约束最优控制。不像：这里目标二次、约束线性（凸 QP），而一般 MPC 的模型可非线性。把这个对应记在心里，T3 学 iLQR / MPC 时就是故地重游，而非从零开始。

本质洞察：speed QP 之所以"温顺"，是因为 §T1.4 的 DP 已经吞掉了非凸的 yield/overtake 决策，留给 QP 的 $[s_{\text{lb}}, s_{\text{ub}}]$ 走廊是**连通的凸区域**。两段式分工的全部回报在这里兑现：DP 吞非凸（选类），QP 独享凸性（精修）。硬要把两段合成一段，你要么丢掉决策能力（纯 QP 不会自己选边），要么丢掉凸性与速度（纯非凸搜索又慢又易陷局部最优）。

代码实现（为什么 → 正确 → 错误 → 对比）¶

Step 1 — 为什么这样写：状态空间 + piecewise-constant-jerk 的理由与 §T1.3 相同（稀疏带状、状态可解释、box 约束）。关键差异是：约束里的 ST 走廊来自 DP（保证凸），速度上界要按路径曲率折减。

Step 2 — 正确写法（教学实现）。连续性等式（对时间）：

// 速度 QP 决策向量:每步 [s_i, v_i, a_i],按 [s_0,v_0,a_0, s_1,v_1,a_1, ...] 排列
inline int IdxS(int i){ return 3*i + 0; }
inline int IdxV(int i){ return 3*i + 1; }
inline int IdxA(int i){ return 3*i + 2; }

// 连续性等式之一(消去 jerk 后的 s 关系,匀 jerk 三次积分):
//   s_{i+1} - s_i - v_i·Δt - ½·a_i·Δt² - ⅙·(a_{i+1}-a_i)·Δt² = 0
void AddSContinuityRow(SparseTriplets* A, int row, int i, double dt){
  const double dt2 = dt * dt;
  A->emplace_back(row, IdxS(i + 1),  1.0);
  A->emplace_back(row, IdxS(i),     -1.0);
  A->emplace_back(row, IdxV(i),     -dt);
  A->emplace_back(row, IdxA(i),     -0.5 * dt2 + dt2 / 6.0);  // a_i 合并系数
  A->emplace_back(row, IdxA(i + 1), -dt2 / 6.0);              // a_{i+1}(来自 jerk 代入)
  // 该行上下界都设为 0(等式约束)
}

向心加速度折减的速度上界（把路径曲率耦进速度）：

// 向心加速度约束 → 速度上界:弯道处 v 必须降,否则横向加速度超限
//   a_lat = v²·κ ≤ a_lat_max  ⟹  v ≤ sqrt(a_lat_max / |κ|)
// κ 来自【已定路径】(§T1.3):这是 path → speed 的单向耦合。
double SpeedLimitAt(double kappa_at_s, double a_lat_max, double v_max){
  if (std::abs(kappa_at_s) < 1e-6) return v_max;             // 近直线:不折减
  return std::min(v_max, std::sqrt(a_lat_max / std::abs(kappa_at_s)));
}
// 逐步:v_ub[i] = SpeedLimitAt(κ(s_i)),作为 box 上界。
// 说明:s_i 是决策变量,这里用 DP 的 s 预估对应的 κ 来【预计算】v_ub——
//       这是 QP(线性)对"曲率-速度耦合"的近似;精确处理见 Step 4 的非线性优化器。

走廊约束来自 DP，并交 OSQP 求解（$P$ 上三角，见 §T1.3 编程陷阱，不再重复）：

// ST 走廊 box 约束:来自 §T1.4 DP 的 yield/overtake 决策(连通凸区域)
for (int i = 0; i < N; ++i) {
  s_lb[i] = dp_corridor.Lower(t[i]);   // yield → 压低 s 上界(留在阻挡下方)
  s_ub[i] = dp_corridor.Upper(t[i]);   // overtake → 抬高 s 下界(冲到阻挡上方)
}
// 组装 P(上三角)、q、A(连续性等式 + 走廊/速度/加速度 box)、l、u,交 OSQP:
osqp_setup(&solver, P_upper_csc, q.data(), A_csc, l.data(), u.data(), m, n, settings);
osqp_solve(solver);    // 通常 < 10 ms;解 [s_i,v_i,a_i] → 平滑速度剖面 s(t)

Step 3 — 错误写法并解释为什么错：

// ❌ 错误 1:把 DP 的离散 s 当【硬等式】,要求 QP 逐点复现
   s_lb[i] = s_ub[i] = dp_s[i];        // 锁死 = DP 多粗,精修后就多粗
//   现象:QP 要么不可行(DP 解本身违反 jerk/连续性),要么输出仍是阶梯状——精修白做。
//   根因:DP 给的是【决策 + 走廊】,不是要被逐点复现的轨迹。应给区间,不给点。

// ❌ 错误 2:忘了向心折减,速度只用 v_max 限
   v_ub[i] = v_max;                    // 漏了 sqrt(a_lat_max/|κ|)
//   现象:自车以 v_max 冲进急弯,横向加速度爆表 → 甩尾 / 乘客被甩出去。
//   根因:没把【已定路径的曲率】折减进速度上界。

// ❌ 错误 3:把加速度 a 当"控制"、不建模也不惩罚 jerk
//   现象:相邻步加速度可阶跃 → jerk 无界 → 顿挫。
//   根因:piecewise-jerk 的控制是 jerk(三阶),不是 a;漏了 jerk 项 = 漏了平滑的来源。

Step 4 — 多种实现方式对比（已验证 Apollo 同时存在这两个优化器）：

维度	piecewise-jerk speed QP（线性）	piecewise-jerk nonlinear speed optimizer（Apollo `PiecewiseJerkSpeedNonlinearOptimizer`）
曲率-速度耦合 $v\le v_{\text{limit}}(s)$	近似：用 DP 的 $s$ 预算 $v_{\text{limit}}$ 当 box 界	精确：作为非线性约束直接处理
求解器	OSQP（凸 QP，ms 级）	IPOPT（NLP，较慢）
何时用	实时主路径、绝大多数场景	需要更精确曲率-速度协调（如高速大曲率匝道）时

⚠️ 常见陷阱¶

⚠️ 编程陷阱：把 DP 的离散 $s$ 当硬等式约束喂给 QP - 错误做法：直接令 $s_{\text{lb}}[i]=s_{\text{ub}}[i]=$ DP 解的 $s_i$，要求 QP 精确复现 DP 轨迹。 - 现象：QP 频繁报不可行（DP 的阶梯解本就违反 jerk / 连续性约束），或即便可行也输出和 DP 一样的阶梯状——精修完全失效。 - 根本原因：DP 给的是"决策 + 走廊"，不是一条要被逐点复现的轨迹。把走廊压成一条线，等于扔掉了 QP 的全部精修自由度。 - 正确做法：用 $[s_{\text{lb}}, s_{\text{ub}}]$ 区间**做 box 约束 + 用 DP 解做 warm start。 **自检：精修后加速度 / jerk 应明显比 DP 解平滑。

💡 概念误区：以为速度只受 $v_{\max}$ 限制，忘了向心加速度折减 - 错误想法：速度上界就是限速 $v_{\max}$，弯道直道一个样。 - 实际情况：弯道处必须满足 $v\le\sqrt{a_{\text{lat,max}}/|\kappa(s)|}$，否则横向加速度 $a_{\text{lat}}=v^2\kappa$ 超限——车会甩、乘客会被甩。曲率越大、上限越低。 - 为什么重要：漏了这一项，规划器会让车以限速冲进急弯，仿真里看着达标，实车直接失稳。 - 正确做法：按已定路径的曲率剖面逐点折减速度上界（$v_{\text{limit}}(s)$），或用非线性优化器精确处理耦合。

🧠 思维陷阱：以为到了速度阶段就和路径彻底无关、解耦是无害的 - 错误想法：路径定完就翻篇了，速度规划是独立的一维问题。 - 实际情况：速度上界 $v_{\text{limit}}$ 由 §T1.3 的路径曲率 $\kappa$ 决定——这是 path → speed 的**单向**耦合，解耦"漏"出来的一条线。更彻底的耦合是**双向**的：现实里有时"把路径修直一点就能开快一点"才是全局最优，但纯 PVD 的顺序结构无法让速度反过来改路径。 - 正确思维：解耦不是无害的，它默认了"路径不该为速度让步"。这道裂缝正是 §T1.6 EM 迭代要缝合、T2/T3 时空联合要彻底打通的地方。看到 $v_{\text{limit}}$ 依赖 $\kappa$，你就摸到了解耦的边界。

练习¶

[实现] 实现 piecewise-jerk speed QP：输入 §T1.4 DP 给的 ST 走廊 $[s_{\text{lb}}, s_{\text{ub}}]$、起点 $[s_0, v_0, a_0]$、按路径曲率折减的 $v_{\text{limit}}$，用 OSQP 求 $s(t)/v(t)/a(t)$。做两组对比：（a）开 / 关 jerk 项时加速度的平滑度；（b）有 / 无向心折减时的过弯速度。
[调试] 给定一段把 DP 的 $s$ 当硬等式约束（$s_{\text{lb}}=s_{\text{ub}}=$ DP 解）的代码，解释它为什么经常报不可行、或输出和 DP 一样阶梯状；改成走廊区间约束 + warm start，并说明精修后应观察到什么变化。
[推导 + 设计] 从横向加速度 $a_{\text{lat}}=v^2\kappa$ 推导速度上界 $v\le\sqrt{a_{\text{lat,max}}/|\kappa(s)|}$。然后讨论：这个上界依赖路径曲率，属于 path → speed 的单向耦合；如果允许"为了开得更快而把路径修直"（双向耦合），纯 PVD 的顺序结构还能处理吗？这把你引向 §T1.6——请带着这个问题读下一节。

§T1.6 EM 迭代：解耦中的"准联合" ⭐⭐⭐¶

这一节解决什么问题：§T1.5 末尾埋下的伏笔——纯顺序解耦（path → speed 各算一次）处理不了双向耦合（cut-in 等场景路径与速度互相牵制）。 EM 迭代是工程上的折中：不放弃解耦框架，但让 path 和 speed 互相喂几轮数据，逼近联合解。本节讲清它是什么、为什么只能"单调不增、不保证全局最优"、以及它在哪儿失效、何时该转向 T2/T3。

动机：在不放弃凸子问题的前提下，缝合解耦的裂缝¶

回顾这一章的因果链：§T1.1 指出强耦合场景解耦会失败； §T1.4 的 cut-in 练习里我们看到先定的路径让速度在 ST 图上**两侧都不可行**； §T1.5 末尾点明 $v_{\text{limit}}$ 对路径曲率的依赖只是单向耦合，真正的双向耦合解耦给不了。 EM 迭代回答的是：能不能不上重武器（时空联合），只靠多跑几轮解耦，就把大部分耦合补回来？

如果不这样做会怎样（反事实）¶

纯一次性 path → speed → 在强耦合场景输出次优甚至不可行（cut-in 两侧都不可行时，速度阶段直接失败）。
直接上时空联合（T2/T3） → 能拿到联合最优，但要解非凸问题，计算更重、工程更复杂。对**弱到中等**耦合的绝大多数场景，这是杀鸡用牛刀。
EM 迭代正是这两者之间的中间档：仍解凸子问题（快、稳），但靠交替迭代逼近联合。

历史与命名辨析¶

Apollo EM Planner（Fan 等 2018）的 "EM" 借喻自统计学的 Expectation-Maximization 算法——用 E-step / M-step 交替来描述"路径估计 ↔ 速度估计"的轮流优化。

辨析（一个常见误解的预防）：这里的 "EM" 是**借喻**，不是统计意义上的 EM 算法。统计 EM 在 E-step 求隐变量的期望、M-step 最大化期望似然；这里没有隐变量、没有概率期望，是**确定性的交替优化**。下文会看到它的真名是"块坐标下降"。 Fan 2018 用 EM 这个名字，是因为它的交替结构形似 EM，而非数学等价。

理论：E-step / M-step 与收敛性¶

迭代结构：

E-step（估计动态障碍投影 → 优化路径）：用上一轮的速度 profile，估计动态障碍在 SL 图上的（准静态）投影，然后在 SL 图上做 §T1.3 的路径 DP + QP，得到新路径。
M-step（用新路径更新投影 → 优化速度）：用新路径更新 ST 图中障碍的投影，然后在 ST 图上做 §T1.4 + §T1.5 的速度 DP + QP，得到新速度。
重复 2–3 轮。

给两个互补视角，这是理解 EM 迭代性质的关键：

优化视角（它的真名）：EM 迭代就是**块坐标下降（block coordinate descent）——把联合决策变量分成"路径块"和"速度块"，轮流固定一块、优化另一块。坐标下降的经典性质直接套用：每步在各自凸子问题内求最优 → 总代价**单调不增；收敛到一个**坐标驻点**（沿任一坐标方向都无法再降）；但在非凸的联合问题上**不保证全局最优**。
概率视角（命名来源）：形似统计 EM 的 E/M 交替。 **像**的地方：都是"固定一部分、优化另一部分"的交替结构。 **不像**的地方：这里没有 E-step 的概率期望，是确定性优化。别把统计 EM 的收敛理论（如似然单调上升）硬套过来——这里该用的是坐标下降理论。

把视野再放大一点：这种"把变量分块、轮流固定一块优化另一块"的思路，是一整个**交替优化（alternating optimization）家族**的成员。最朴素的一档是 Gauss–Seidel 式坐标下降——本节的 EM 迭代就是它；更讲究的有 ADMM（交替方向乘子法），它在分块之上引入对偶变量与一致性约束，能处理子问题间的耦合约束、收敛性也更可控。把 EM 认成这个家族里"最轻量的一档"，它的长处（实现简单、每轮只解一个凸子问题）和短板（只在原始变量上交替、对强耦合无能为力）就都有了出处——而要啃下强耦合，往往就得换更重的成员，或干脆转向 T2/T3 的联合优化。

收敛性的数学保证与局限：

\[ J(\text{path}^{(k+1)}, \text{speed}^{(k)}) \le J(\text{path}^{(k)}, \text{speed}^{(k)}),\qquad J(\text{path}^{(k+1)}, \text{speed}^{(k+1)}) \le J(\text{path}^{(k+1)}, \text{speed}^{(k)}) \]

每个 E-step 和 M-step 都在各自凸子问题内取到最优，所以总代价沿迭代**单调不增**。但"单调不增"只能保证收敛到坐标驻点，不等于全局最优——联合可行域非凸时，坐标下降可能停在一个"沿路径方向和沿速度方向都改不动、但联合方向还能更好"的次优点。

坐标下降的收敛性：一个二维直觉¶

为什么块坐标下降（= EM 迭代）能保证单调不增、却不保证全局最优？用一个二维例子就能看穿，不必动用一般性证明。

设要最小化联合变量 $(p, q)$ 的某代价 $J(p, q)$（$p$ 代表"路径决策"，$q$ 代表"速度决策"）。坐标下降轮流做：固定 $q$ 优化 $p$（E-step）、固定 $p$ 优化 $q$（M-step）。

为什么单调不增：每一步都是"在另一坐标固定下取最小"，所以 $J$ 在这一步只会降或不变。一连串"只降不升"叠起来，自然单调不增；又因代价有下界（非负），单调有界数列**必收敛**。
为什么不保证全局：坐标下降每步只沿**坐标轴方向**（纯 $p$ 或纯 $q$）移动。想象 $J$ 的等高线是一个沿 $45^\circ$ 方向拉长的"斜山谷"：从谷壁某点出发，沿 $p$ 轴走到该行最低、再沿 $q$ 轴走到该列最低……你会在谷壁上走出一串越来越小的台阶，收敛到谷壁上某点——但那点沿斜对角（同时改 $p$ 和 $q$）还能继续下降。" 坐标方向都改不动"不等于"真到了谷底"。这就是坐标驻点 $\ne$ 全局最优。

把这个二维图景搬回规划：$(p, q)$ 的"斜山谷"对应路径与速度的**强耦合**——最优改进需要同时动路径和速度，而 EM 一次只能动一个。于是它停在次优坐标驻点；耦合越强（山谷越斜），停得越早、离全局越远。

震荡又是怎么发生的：上面是"卡住不动"，更糟的是"反复横跳"。当存在两个相互竞争的局部解（"绕左 + 减速" vs "绕右 + 加速"），E-step 把路径推向左、M-step 发现左路径下该减速、下一轮 E-step 又因这个减速而觉得右更好……$(p, q)$ 在两个吸引盆之间来回跳，2–3 轮预算内不收敛。这正是必须**硬限轮数**的原因，也是"该转 T2/T3"的最直接信号。

一个两轮 EM 的数值轨迹（弱耦合 vs 强耦合）¶

把上面的文字变成你能在日志里认出的形状。下面的 JointCost 是**示意数值**，重点看趋势。

弱耦合（§T1.7 的"绕抛锚车 + 跟前车"，两者几乎不牵制）： - 轮 0：速度取巡航初值； E-step 出路径（向右让 $0.6\,\text{m}$）； M-step 出速度（跟车降到 $6$）。 JointCost ≈ 100。 - 轮 1：E-step 用新速度重算路径，几乎不变； M-step 速度也几乎不变。 JointCost ≈ 99。降幅已小于阈值 → 早停。 2 轮收敛。

强耦合（cut-in，前车正切入自车道）： - 轮 0：E-step 路径仍走原车道（切入车此刻还偏）； M-step 速度发现前方将被占、急减。 JointCost ≈ 200。 - 轮 1：E-step 用"急减"的速度重算，发现可以更早右避、让出空间 → 路径右移； M-step 速度因路径让开而不必那么急减。 JointCost ≈ 150。 - 轮 2：E-step 又因"不必急减"而觉得右移过头、想收回……JointCost ≈ 155（回升了！）。在两个解之间震荡，2–3 轮没收敛。

判据落地：JointCost 单调下降且降幅逐轮收窄 → 收敛（弱耦合，取最后一轮）；出现回升、或降幅不收窄 → 震荡（强耦合），硬限轮数后取迄今最优解，并打上"建议 T2/T3"的标记。这串数字是示意的，但它把 §T1.6 那句"单调不增 vs 震荡"从抽象判断变成了你在监控里一眼能认的曲线形状。

多视角：这套 path ⇄ speed 交替，从三个角度看是三件不同的事，合起来才看全。 统计 EM 的借喻视角：它借了 EM "E 步/M 步交替"的名字和形式——固定一块、优化另一块、轮流来。像的地方是"交替结构"；不像的地方是它没有概率模型、没有似然，"E/M"纯属类比（前面"辨析"已强调），别套用 EM 的收敛定理。 块坐标下降的视角：剥掉 EM 的外衣，它的真名是块坐标下降（block coordinate descent）——把决策变量分成"路径块"和"速度块"，固定一块对另一块做凸优化。这才是它收敛性该用的理论：保证单调不增、收敛到坐标驻点，但不保证全局最优。 二维优化的视角：把路径、速度抽象成两个标量轴，联合代价是这个二维平面上的一个曲面。EM 就是"先沿路径轴走到谷底、再沿速度轴走到谷底"地交替下降。在弱耦合（曲面像正圆碗）时几步到底；强耦合（曲面是斜放的窄山谷）时会在谷壁间来回横跳、收敛极慢——这正是上面数值轨迹里"震荡"的几何根源。三视角不矛盾：借喻视角解释它的名字从哪来、块坐标视角给它正确的理论、二维视角给它收敛快慢的几何直觉。只记名字会误用 EM 定理，只记理论会缺乏直觉——三个一起才既不误用、又有画面。

本质洞察：EM 迭代不是"把解耦变成联合"，而是"用多轮解耦逼近联合"。它换来的是：每轮仍解凸子问题（快、稳、有全局最优）；代价是整体只收敛到坐标驻点，且耦合很强时，2–3 轮的实时预算内可能**来不及收敛、甚至在两个局部解之间震荡**。它是"解耦的可解性"与"联合的最优性"之间的一个**中间档位**，不是免费的联合替代品。

代码实现（为什么 → 正确 → 错误 → 对比）¶

Step 1 — 为什么这样写：EM 是在 §T1.3 / §T1.4 / §T1.5 之上加一层**外循环**，每轮把另一块的最新结果当作固定输入。必须硬限轮数（实时预算）并监控代价单调性（早停 + 发散保护）。

Step 2 — 正确写法（教学实现）：

// EM 迭代外循环:交替优化路径(E-step)与速度(M-step),逼近联合解
PlanningResult EmPlan(const ReferenceLine& ref, const std::vector<Obstacle>& obs,
                      int max_iter = 3, double cost_eps = 1e-2) {
  SpeedData speed = WarmStartSpeed(ref);   // 初值:用上周期速度或简单巡航
  PathData  path;
  double prev_cost = std::numeric_limits<double>::infinity();

  for (int k = 0; k < max_iter; ++k) {     // ⚠️ 必须硬限轮数:实时预算 + 防震荡
    // E-step:用当前 speed 估计动态障碍投影 → SL 图路径 DP+QP(§T1.3)
    path  = PlanPathInSL(ref, ProjectObstacles(obs, speed));
    // M-step:用新 path 更新障碍 ST 投影 → ST 图速度 DP(§T1.4)+QP(§T1.5)
    speed = PlanSpeedInST(path, ProjectObstacles(obs, path));

    const double cost = JointCost(path, speed);   // 在【联合】代价上评估这一轮
    if (prev_cost - cost < cost_eps) break;        // 代价基本不降 → 早停(已近驻点)
    prev_cost = cost;                              // 记录用于下一轮单调性检查
  }
  return {path.points(), speed.points()};
}

Step 3 — 错误写法并解释为什么错：

// ❌ 错误 1:不限迭代轮数,等"收敛"再停
   while (!converged) { E_step(); M_step(); }      // 强耦合下可能永不收敛
//   现象:cut-in 等场景里 E/M 在两个局部解间震荡,while 死循环 → 规划超时,车失控。
//   根因:坐标下降在非凸联合下不保证收敛到单点;实时系统必须硬限轮数。

// ❌ 错误 2:不在【联合】代价上评估,只看子问题代价
   if (PathCost(path) < eps) break;                // 只看 path 块的代价
//   现象:path 块降了但 speed 块升了,总代价没降甚至升,却误判"收敛"。
//   根因:坐标下降的单调性是对【联合】代价而言,必须用 JointCost 判早停。

// ❌ 错误 3:每轮把 warm start 清零
//   现象:每个 E/M 都从零冷启动,QP 迭代次数翻倍,2-3 轮预算根本跑不完。
//   根因:相邻轮的解很接近,应把上一轮结果当 warm start 传下去。

Step 4 — 多种实现方式对比（三档，由轻到重）：

方案	最优性	实时性	收敛保证	适用耦合强度
一次性解耦（path→speed）	最低	最快	无迭代	弱耦合
EM 迭代（2–3 轮）	中（坐标驻点）	快	总代价单调不增、不保证全局	弱~中等
时空联合（T2/T3）	最高（联合最优）	最重	视方法（非凸，可能局部最优）	强耦合

⚠️ 常见陷阱¶

⚠️ 编程陷阱：EM 外循环不限轮数，"等收敛再停" - 错误做法：用 while (!converged) 跑到收敛为止。 - 现象：强耦合（cut-in）场景里 E/M 在两个局部解之间震荡，循环永不退出 → 规划周期超时，控制断流，车失控。 - 根本原因：坐标下降在非凸联合问题上**不保证收敛到单点**；实时系统不能把"是否收敛"当循环条件。 - 正确做法：硬限 2–3 轮（实时预算）+ 在**联合**代价上设早停阈值。自检：日志记录每轮 JointCost，确认单调不增且轮数受控。

💡 概念误区：把 Apollo 的 "EM" 当成统计学的 EM 算法 - 错误想法：套用 EM 算法的隐变量、期望步、似然单调上升等理论来分析它。 - 实际情况：这里的 EM 是**借喻**——是确定性的块坐标下降，没有隐变量、没有 E-step 期望。该套用的是坐标下降理论（单调下降、坐标驻点），不是统计 EM 理论。 - 为什么重要：用错理论框架会得出错误的收敛性预期（比如误以为它像统计 EM 那样有良好收敛保证）。 - 正确做法：把它当块坐标下降分析——这才解释了它"单调不增但不保证全局"的真实性质。

🧠 思维陷阱：以为 EM 迭代等价于时空联合，跑几轮就能拿到联合最优 - 错误想法：EM 迭代会收敛到和联合优化一样的解，所以不需要 T2/T3。 - 实际情况：EM 是坐标下降，强耦合下会停在次优坐标驻点、甚至震荡；它**逼近**联合，不**等于**联合。 - 正确思维：EM 是中间档——弱到中等耦合够用，强耦合必须上 T2/T3。判据：若 2–3 轮内 JointCost 仍明显下降或在震荡，说明耦合超出了 EM 的能力范围。

练习¶

[实现] 在 §T1.3 的路径 + §T1.4/§T1.5 的速度之上，加一层 2–3 轮的 EM 外循环；每轮记录 JointCost，用一个弱耦合场景验证它**单调不增**，并观察通常 2 轮即近收敛。
[设计 / 分析] 构造一个强耦合场景（如 cut-in），观察 EM 在 2–3 轮内是收敛还是震荡；给出"何时该放弃 EM、转向 T2/T3 时空联合"的可操作判据（提示：JointCost 的轮间下降量 / 是否在两个解间来回跳）。
[理论] 论证：为什么块坐标下降在每个凸子问题上能保证总代价单调不增，却在非凸的联合可行域上不保证全局最优？用一个二维非凸等高线 + 坐标轮换搜索停在"轴向都改不动但斜向还能降"的反例说明（草稿纸上画图完成）。

§T1.7 完整管线实战：一个场景从参考线到时空轨迹 ⭐⭐¶

这一节解决什么问题：前六节把零件一个个讲清楚了，但"零件会用"不等于"整机会跑"。本节拿一个**带具体数字**的场景，从原始 HD 地图点出发（顺带补上 §T1.2 欠下的参考线平滑），一路走完 Frenet → SL/path QP → ST/DP → speed QP → EM，把整条数据流走一遍。这也是本章累积项目的起点。

场景设定（带数字，便于追踪）¶

道路：参考线前 40 m 直、后段左转弯（半径 $R=25\,\text{m}$，即 $\kappa=0.04\,\text{m}^{-1}$）；车道宽 3.5 m。横向偏移 $l$ 左正右负。
自车：起点 $s_0=0$、$l_0=0$、$v_0=8\,\text{m/s}$、$a_0=0$。限速 $v_{\max}=12\,\text{m/s}$，$a\in[-3, 2]\,\text{m/s}^2$，$j_{\max}=4\,\text{m/s}^3$，横向加速度上限 $a_{\text{lat,max}}=2\,\text{m/s}^2$。
静态障碍：左侧抛锚车，占 $l\in[1.0, 2.0]$、$s\in[15, 25]$。
动态障碍：同车道前车，$t=0$ 时在 $s=30$、匀速 $v_{\text{lead}}=6\,\text{m/s}$。

这些数都是**为讲解构造的**，不是某次真实求解的输出；目的是让每一步的因果看得见、量得出。

Step 0：参考线平滑（补上 §T1.2 的前置）¶

HD 地图给的车道中心线通常是离散采样点，带噪、曲率不连续。 直接拿它当参考线，会让所有下游 Frenet 量（尤其 $l'$、$\kappa$）抖动，把 §T1.3/§T1.5 的 QP 喂成病态问题（这正是 §T1.2 的思维陷阱）。所以管线第 0 步是**参考线平滑**：在"贴近原始点"和"曲率平滑"之间求折中。

一种常见做法是离散点平滑 QP——决策变量是每个参考点的微调量 $(\delta x_i, \delta y_i)$，代价为

\[ \min_{\{\delta\}}\ w_{\text{ref}}\sum_i \lVert \delta_i\rVert^2 \;+\; w_{\text{smooth}}\sum_i \lVert p_{i-1} - 2p_i + p_{i+1}\rVert^2 \]

第一项把点拉回原始位置（别改太多），第二项（二阶差分）压平曲率突变。这又是一个凸 QP（OSQP 可解），输出一条曲率连续的参考线。 Apollo 的 reference_line 模块就有这类平滑器（离散点 / 螺旋线 / QP 多种）。

这一步与本章主线同构——又是一个"平方和代价 + 线性约束"的凸 QP。你会发现 SL 路径、ST 速度、参考线平滑，本质都是同一类问题。这种"到处都是凸 QP"的体感，本身就是这条技术路线的特征。

本场景平滑后的曲率剖面：$\kappa(s)\approx 0$（$s<38$）→ 在 $s\in[38,42]$ 平滑过渡 → $\kappa\approx 0.04$（$s\ge 42$）。注意平滑把"直道突变到弯道"抹成了一段过渡，避免 $\kappa$ 阶跃。

Step 1：Frenet 投影（§T1.2）¶

把自车与两个障碍投影到 $(s, l)$：

对象	$s$ 范围	$l$ 范围	说明
自车起点	$0$	$0$	$v_0=8$
抛锚车（静态）	$[15, 25]$	$[1.0, 2.0]$	投到 SL 图当横向阻挡
前车（动态，$t{=}0$）	$\approx 30$	$\approx 0$	随 $t$ 在 ST 图里上移（§T1.3 中先准静态占位）

检查退化（§T1.2）：弯道段 $\kappa l$ 在 $l$ 不超 1 m 时约 $0.04$，远小于 0.3 阈值 → Frenet 在本场景安全可用。

Step 2：SL 图 + piecewise-jerk 路径 QP（§T1.3）¶

抛锚车占 $l\in[1.0,2.0]$、$s\in[15,25]$。折算成逐 station 的横向上界：在 $s\in[15,25]$ 段，把 $l_{\text{ub}}$ 从车道左边界（约 +1.75）压到 $+0.8$（留出自车半宽 + 裕度），其余段保持车道边界。下界 $l_{\text{lb}}$ 仍为右边界 $-1.75$。

把这些 box 上下界 + 起点状态 + 平滑代价喂给 §T1.3 的路径 QP，解出 $l(s)$：路径在 $s\in[10,30]$ 平滑地向右（负 $l$）让出约 $-0.6\,\text{m}$、绕过抛锚车，再平滑回中线。

这里嵌一个**渐隐示例**（规范推荐用于复杂步骤）：

Phase 1（完全展示）：$l_{\text{ub}}$ 的构造 = $\min(l_{\text{lane}},\ l_{\text{obs,lower}} - w_{\text{ego}} - d_{\text{margin}})$，其中 $l_{\text{lane}}$ 是车道左边界、$w_{\text{ego}}$ 是自车半宽、$d_{\text{margin}}$ 是安全裕度。本例 $= \min(1.75,\ 1.0 - 0.9 - 0\ )$ ⇒ 障碍段被压到约 $+0.1$ 还是 $+0.8$？请注意符号与裕度的取法。
Phase 2（留给你填）：$l''$（ddl）的上下界怎么写？回忆 §T1.3 的 ddl_bounds——它等于 ______ 减去参考线曲率 ______。在弯道段（$\kappa=0.04$）和直道段（$\kappa\approx 0$），这个界有何不同？（答案藏在 §T1.3 那段代码里。）

Step 3：ST 图 + DP-ST 搜索（§T1.4）¶

路径定了，前车（$t{=}0$ 在 $s{=}30$、$6\,\text{m/s}$）投影到 ST 平面：一条从 $(t{=}0, s{\approx}30)$ 出发、斜率 $6\,\text{m/s}$ 的阻挡带（匀速 → 平行四边形），按车长 + 时距膨胀。

自车 $8\,\text{m/s}$ 会追上 $6\,\text{m/s}$ 的前车，同车道无法安全超车 → DP 在 ST 图上选择 yield（从阻挡下方过）：粗速度 profile 显示自车减速到约 $6\,\text{m/s}$ 跟车。 DP 输出 → 一条 ST 走廊 $[s_{\text{lb}}, s_{\text{ub}}]$（上界被前车压住），交给 Step 4。

Step 4：speed QP（§T1.5）¶

在 DP 给的走廊里精修。两处约束同时生效：

跟车（来自 ST 走廊）：$s_{\text{ub}}(t)$ 把自车压在前车之后 → 速度收敛到约 $6\,\text{m/s}$。
向心折减（来自路径曲率）：弯道段 $\kappa=0.04$ → $v_{\text{limit}}=\sqrt{a_{\text{lat,max}}/\kappa}=\sqrt{2/0.04}\approx 7.07\,\text{m/s}$。

本例两者叠加：跟车把速度压到 6（比 7.07 更紧），所以最终弯道里以约 $6\,\text{m/s}$ 平稳通过，加速度 / jerk 被 QP 磨平。 反事实：若没有前车，自车会想以接近限速进弯，此时 $v_{\text{limit}}=7.07$ 就成了**binding** 约束——这正是 §T1.5 概念误区警告的"忘了向心折减就甩尾"的场景。

Step 5：要不要 EM 迭代（§T1.6）¶

本场景里路径（绕抛锚车）和速度（跟前车）弱耦合——绕障与跟车几乎不互相牵制，一次性 path→speed 就够，EM 加一轮 JointCost 几乎不降。

但把场景改成**前车正在切入自车道**（cut-in），就变强耦合：先定的路径若贴近切入车，速度阶段在 ST 图上可能两侧都不可行（§T1.4 跨章综合题）。此时跑 2–3 轮 EM——E-step 让路径更早右避、M-step 重算速度——JointCost 单调下降逼近联合解；若 2–3 轮仍震荡，则该上 T2/T3（§T1.6 判据）。

整条管线的数据流¶

HD 地图离散点
   │  Step 0:参考线平滑 QP(凸)
   ▼
平滑参考线 γ(s)、κ(s)
   │  Step 1:Frenet 投影(自车 + 障碍 → (s,l))
   ▼
SL 图(静态障碍横向边界) ──Step 2: 路径 piecewise-jerk QP(凸)──▶ 路径 l(s)
                                                                  │
   ┌──────────────────────────────────────────────────────────────┘
   ▼
ST 图(动态障碍时变阻挡) ──Step 3: DP-ST 搜索(选 yield/overtake)──▶ ST 走廊 [s_lb,s_ub]
                                                                  │
   ┌──────────────────────────────────────────────────────────────┘
   ▼
Step 4: 速度 piecewise-jerk QP(凸, 含向心折减)──▶ 速度 s(t)
   │
   ▼
(强耦合?) Step 5: EM 外循环 2-3 轮 ── 否 ──▶ 时空轨迹 (l(s), s(t)) 串成 r(t)
                                  └─ 仍震荡 ──▶ 转 T2/T3 时空联合

整合后的伪代码（把六节的入口串起来）：

Trajectory PlanOneCycle(const RawWaypoints& raw, const Obstacles& obs,
                        const VehicleState& ego) {
  ReferenceLine ref = SmoothReferenceLine(raw);          // Step 0(§T1.2 补充)
  FrenetState  fs   = ToFrenet(ego, ref);                // Step 1(§T1.2)

  // 弱耦合:一次性解耦即可;强耦合:外面再套 EmPlan 的 2-3 轮(§T1.6)
  PathData  path  = PlanPathInSL(ref, FilterStatic(obs));     // Step 2(§T1.3)
  SpeedData speed = PlanSpeedInST(path, FilterDynamic(obs));  // Step 3+4(§T1.4/§T1.5)

  return Combine(path, speed);   // (l(s), s(t)) → 笛卡尔时空轨迹 r(t)
}

把这个 PlanOneCycle 当作**本章累积项目的主干**：后面每学一节就给它补实一个模块（Frenet 库 → SL/path QP → ST/DP → speed QP → EM 外循环），学完整章你就有了一个能跑的最小时空规划器。详见章末"累积项目"。

各阶段会怎么失效，又怎么兜底¶

把管线跑通只是第一步；真正上车，得知道每一环在什么情况下会失效、失效了怎么降级——否则一个环节报错就整车失控。逐段过一遍这条"失效—兜底"清单，也是对全章知识的一次反向复习。 Step 0 参考线平滑：若上游地图点稀疏或带噪，平滑 QP 可能给出曲率剧烈的参考线，污染下游所有 Frenet 量。兜底：对参考线曲率设上限、平滑失败时退回上一帧的参考线。 Step 1 Frenet 投影：车辆离参考线过远（$|\kappa_r l|$ 接近 1）时投影失真、$s$ 可能跳变（§T1.2 那两个工程坑）。兜底：维护上帧投影点做局部搜索避免跳变；偏离过大时触发重新选参考线或报警。 Step 2 路径 QP：障碍把可行横向区间挤空时 QP 不可行。兜底：放宽非关键的软约束（§T1.3）、或缩短规划范围只保证近端可行。 Step 3 DP-ST 搜索：动态障碍预测把 ST 图填满、找不到无碰撞世界线时返回失败。兜底：触发减速/停车决策，而非硬解。 Step 4 speed QP：走廊无解时，靠 §T1.5 的松弛变量软化，输出"尽量少越界"的解而非哑火——这是高速行车安全的关键。 Step 5 EM 迭代：强耦合时不收敛、来回震荡（§T1.6）。兜底：硬限轮数、取迄今最优解，并打上"建议升级到 T2/T3 时空联合"的标记。 贯穿全程的总兜底：任何一步彻底失败，规划器都不应输出带碰撞的轨迹，而应回退到一个保守的安全行为（沿当前车道减速直至停车），同时上报。"宁可保守停车、绝不输出危险轨迹"——这是所有规划系统压在功能之上的安全底线，也是从"算法能跑"到"产品敢上路"之间最重要的一课。这条清单里每一环的失效，恰好都指向本章某一节的局限；而其中"强耦合该升级"的多处提示，正是下一阶段 T2/T3 时空联合规划要回答的。

本章常见误解汇总¶

把全章散落的陷阱与误区收拢成一张表，便于回头自查（每条都附出处，可回到对应小节看四要素分析）：

误解	实际情况	出处
PVD 是"先路径、后速度"两步彼此独立	解耦是建模假设，强耦合下顺序求解会次优甚至不可行	§T1.1
维度降低 = 近似 = 一定更差	弱耦合下解耦解（两个凸子问题各取全局最优）可能优于联合非凸的局部最优	§T1.1
Frenet 坐标处处精确、可无脑用	仅 $\lvert\kappa_r l\rvert<1$ 时一一对应；大曲率 + 大偏移退化	§T1.2
参考线随便给一条就行	不光滑参考线 → Frenet 量抖动 → QP 病态；必须先平滑	§T1.2 / §T1.7
path QP 里的 jerk 就是乘客感受的 jerk	那是 $\mathrm{d}^3 l/\mathrm{d}s^3$（空间平滑度）；时间 jerk 在 speed QP	§T1.3 / §T1.5
$l''$（ddl）的界 = 车辆最大曲率	要减去参考线曲率 $\kappa(s_i)$	§T1.3
OSQP 的 $P$ 随便填完整对称矩阵	OSQP 1.0 只读上三角，填全等于权重翻倍	§T1.3
速度只受限速 $v_{\max}$ 约束	弯道受向心折减 $v\le\sqrt{a_{\text{lat,max}}/\lvert\kappa\rvert}$	§T1.5
DP-ST 的输出就是最终速度曲线	是网格粗解：定 yield/overtake 决策 + 给走廊 + warm start；精修靠 QP	§T1.4 / §T1.5
yield / overtake 是连续可调的选择	离散 homotopy 决策（非凸性的根源）	§T1.4
Apollo 的 "EM" 就是统计学 EM 算法	借喻；实为块坐标下降，无概率期望步	§T1.6
EM 迭代等价于时空联合	坐标下降，强耦合下次优 / 震荡；联合最优要 T2/T3	§T1.6

本章小结¶

术语速查表¶

术语	英文	一句话定义
路径-速度解耦	Path-Velocity Decomposition (PVD)	把轨迹拆成几何路径 $\sigma(s)$ + 速度剖面 $s(t)$ 两个子问题
Frenet 坐标系	Frenet frame	贴参考线的坐标 $(s, l)$：纵向弧长 + 横向有符号偏移
纵向 / 横向	station $s$ / lateral $l$	沿参考线进度 / 垂直参考线偏移（左正右负）
SL 图	Station-Lateral graph	$(s, l)$ 平面，静态障碍投影为横向阻挡，用于路径规划
ST 图	Station-Time graph	$(t, s)$ 平面，动态障碍投影为时变阻挡，用于速度规划
时变阻挡	STBoundary	动态障碍在 ST 平面的占据区域（平行四边形 / 矩形 / 梯形）
分段 jerk	piecewise-jerk	以状态 $[\cdot,\cdot',\cdot'']$ 为变量、段内 jerk 恒定的状态空间表示
DP-ST 搜索	DP-ST search	ST 网格上的前向 DP，做 yield/overtake 决策、出粗速度
让行 / 抢行	yield / overtake	从 ST 阻挡下方 / 上方通过——两个不连通的可行类
同伦类	homotopy class	拓扑上不可连续过渡的解类（如绕障左/右、让/抢）
块坐标下降	block coordinate descent	把变量分块、轮流固定一块优化另一块；EM 迭代的真名
向心折减	centripetal speed limit	$v\le\sqrt{a_{\text{lat,max}}/\lvert\kappa\rvert}$，弯道压速度
参考线平滑	reference line smoothing	把 HD 地图离散点平滑成曲率连续的参考线（一个凸 QP）
热启动	warm start	用上一轮 / 上一周期的解做求解器初值，加速收敛
坐标退化	coordinate degeneracy	$\kappa_r l\to 1$ 时 Frenet 变换奇异

知识点总表¶

知识点	核心	难度	依赖	对应代码模块
§T1.1 PVD	用结构换可解性：非凸 → 两个凸	⭐⭐	—	`PvdPlan` 骨架
§T1.2 Frenet	吸收曲率、横纵解耦；退化条件	⭐⭐	T1.1	`FrenetConverter`
§T1.3 SL + path QP	piecewise-jerk 路径、ddl 减 κ、OSQP	⭐⭐⭐	T1.2	`SLPathOptimizer`
§T1.4 ST + DP	yield/overtake 离散决策、化非凸为凸	⭐⭐	T1.2	`STGraphBuilder` + `DpStSearch`
§T1.5 speed QP	piecewise-jerk 速度、向心折减、走廊精修	⭐⭐⭐	T1.3, T1.4	`SpeedOptimizer`
§T1.6 EM 迭代	块坐标下降、单调不增、不保证全局	⭐⭐⭐	T1.3, T1.5	`EmPlanner`
§T1.7 完整管线	端到端串联 + 参考线平滑	⭐⭐	全部	`PlanOneCycle`

累积项目：本章新增模块（T 线）¶

本章是时空规划线的第一章，累积项目从这里起步：实现一个能跑通 §T1.7 场景的最小时空规划器，主干就是 §T1.7 的 PlanOneCycle。每学完一节补实一个模块：

模块	职责	来自	验收标准
`FrenetConverter`	$(s,l)\leftrightarrow(x,y)$ 正逆变换 + 局部最近点投影	§T1.2	圆弧参考线往返误差 $<10^{-6}$；U 弯不跳变
`ReferenceLineSmoother`	离散点平滑 QP	§T1.7	输出 $\kappa(s)$ 连续、贴近原始点
`SLPathOptimizer`	piecewise-jerk 路径 QP（OSQP）	§T1.3	绕开静态障碍；`ddl_bounds` 含减 $\kappa$；$P$ 上三角
`STGraphBuilder`	动态障碍投影为 `STBoundary`	§T1.4	阻挡按自车 + 障碍尺寸 + 裕度膨胀
`DpStSearch`	前向 DP-ST	§T1.4	输出 yield/overtake 决策 + 走廊；$s$ 单调不减
`SpeedOptimizer`	piecewise-jerk 速度 QP（OSQP）	§T1.5	弯道按向心折减降速；走廊内精修平滑
`EmPlanner`	2–3 轮 E/M 外循环	§T1.6	`JointCost` 单调不增；硬限轮数；warm start 传递
`PlanOneCycle`	串联全部	§T1.7	在 §T1.7 场景端到端跑通并可视化

建议的测试与可视化：（1）在 SL 图上画路径 + 静态障碍膨胀框，确认有可见间隙；（2）在 ST 图上画 $s(t)$ + 动态障碍阻挡带，确认 yield/overtake 决策正确；（3）画 $v(t)$ 与 $\kappa(s)$ 对照，确认弯道降速；（4）对 cut-in 场景记录每轮 JointCost，确认单调下降。 推荐对照 AtsushiSakai/PythonRobotics 中的 Frenet / 速度规划示例（其星标 / 协议见"版本速查"，使用前请自行核实）。

实现时可照下面的接口骨架推进——每学完一节补实一个，最后用 PlanOneCycle 串起来：

// ── T 线累积项目:最小时空规划器的模块接口 ──
// 学完每节,把对应模块从声明补成实现;PlanOneCycle 负责把它们串起来。

struct FrenetState { double s, l, dl, ddl; };   // (s, l, l', l'')
struct PathPoint   { double s, l, dl, ddl; };
struct SpeedPoint  { double t, s, v, a; };
struct StCorridor  { /* s_lb(t), s_ub(t):DP 给出的连通凸走廊 */ };

// §T1.2:正逆变换 + 局部投影
FrenetState ToFrenet(const VehicleState& ego, const ReferenceLine& ref);
CartPoint   FrenetToCartesian(const ReferencePoint& rp, double l);

// §T1.7:参考线平滑(凸 QP)
ReferenceLine SmoothReferenceLine(const RawWaypoints& raw);

// §T1.3:SL 路径 piecewise-jerk QP → l(s)
std::vector<PathPoint> PlanPathInSL(const ReferenceLine& ref,
                                    const std::vector<Obstacle>& static_obs);

// §T1.4:ST 图 + DP-ST → yield/overtake 决策 + 走廊
StCorridor DpStSearch(const std::vector<Obstacle>& dynamic_obs,
                      const std::vector<PathPoint>& path);

// §T1.5:速度 piecewise-jerk QP(走廊内精修,含向心折减) → s(t)
std::vector<SpeedPoint> PlanSpeedInST(const std::vector<PathPoint>& path,
                                      const StCorridor& corridor);

// §T1.6:EM 外循环(硬限轮数,JointCost 早停)
Trajectory EmPlan(const ReferenceLine& ref, const std::vector<Obstacle>& obs,
                  int max_iter = 3);

// §T1.7:把以上串成一个规划周期
Trajectory PlanOneCycle(const RawWaypoints& raw, const Obstacles& obs,
                        const VehicleState& ego);

如何评价一条时空轨迹的好坏（评价指标）¶

写出规划器只是第一步，得能**量化**地说"这条轨迹好不好"，才能调参、做回归测试、和别的方法比。常用指标分四类：

安全：全程自车与所有障碍的最小距离 $d_{\min}$、最小碰撞时间（TTC, time-to-collision）。这是硬指标——任何时刻 $d_{\min}\le 0$ 直接判失败。
舒适：横向加速度峰值 $\max\lvert a_{\text{lat}}\rvert$、纵向加速度峰值、jerk 的均方根（RMS）。这几项直接对应乘客体感，也正是 §T1.3 / §T1.5 那些 jerk 惩罚项要压的东西。
效率：平均速度相对限速的比值、到达目标的用时、是否有不必要的急刹或绕行。
可行性 / 一致性：约束被违反的次数与幅度（理想为 0）、相邻规划周期之间轨迹的跳变量（衡量时间一致性，跳变大会让下游控制难跟）。

工程上常把这些打成一个**回归测试集**：一批固定场景 + 每个场景的指标阈值，每次改代码自动跑，指标变差就报警。把"规划器对不对"从"看可视化拍脑袋"变成"指标过不过线"，是从玩具走向产品的分水岭。

思维提醒：这些指标之间常**互相拉扯**——更安全（离障碍更远）往往更不舒适（绕得更急）或更低效（绕得更远）。所谓"调好一个规划器"，本质是在这组指标间选一个符合产品定位的平衡点，而不是把每个指标单独做到极致。

常见问答（FAQ）¶

Q：既然解耦是近似，为什么不直接做时空联合、甚至端到端？ A：解耦换来的是**可解性与实时性**——两个凸子问题，毫秒级、有全局最优。时空联合（T2/T3）更优但更重；端到端（UniAD / VAD 等）是另一条技术路线，可解释性与安全验证仍在演进。工业界主流仍是"以解耦为骨架、在强耦合处用 EM 或局部联合来补"，本章讲的正是这套骨架。

Q：Frenet 坐标一定比笛卡尔好吗？ A：不是。 Frenet 在**结构化道路**（有清晰参考线、曲率不极端）上让横纵解耦、问题变简单；但在停车场、路口内部等**非结构化**场景会退化（§T1.2），那里笛卡尔 / freespace 反而是对的。选坐标系看场景，不看流行。

Q：DP 和 QP 能不能合并成一步？ A：不建议。 DP 负责**离散决策**（让 / 抢、绕左 / 绕右），QP 负责**连续精修**。决策是非凸的根源、QP 是凸的；合并要么丢掉决策能力、要么丢掉凸性（§T1.4 本质洞察）。两段式正是为了让各自待在自己擅长的数学结构里。

Q：piecewise-jerk 里的"jerk"到底是对谁求导？ A：看维度。路径 QP（§T1.3）里是 $\mathrm d^3 l/\mathrm d s^3$（横向偏移对弧长，管路径的空间平滑度）；速度 QP（§T1.5）里是 $\mathrm d^3 s/\mathrm d t^3$（纵向进度对时间，才是乘客感受的那个 jerk）。同名不同物，这是本章最容易混的一处。

Q：学完这章，我能直接上手改 Apollo 吗？ A：能读懂、能对照、能改局部参数与约束，但要注意版本差异（spline-QP → piecewise-jerk、OSQP 0.6.x → 1.0、ddl_bounds 减 $\kappa$ 等，见"版本速查"）。把本章的 PlanOneCycle 累积项目先自己写一遍、再去读源码，会顺很多。

Q：为什么路径用弧长 $s$ 当自变量、速度用时间 $t$，不统一成一个？ A：因为两个子问题问的是不同的事。路径问"空间上走哪条线"，最自然的自变量是沿线弧长 $s$（与时间无关）；速度问"何时走到线上某点"，自变量必须是时间 $t$。 PVD 的全部精神就是把"空间"和"时间"拆开、各管一段（§T1.1），自变量不同正是这种拆分的直接体现。到 T2/T3 做时空联合时，$s$ 和 $t$ 才会被放回同一个问题里。

Q：ST 图横轴是时间、纵轴是 $s$，为什么不画成 $x$-$y$ 的真实轨迹？ A：因为速度规划关心的是"沿已定路径的进度随时间如何变"，而不是平面位置。 $s$-$t$ 图把"何时到达路径上哪一点"一目了然地摊开，动态障碍变成时变阻挡带、让 / 抢变成从带子下方 / 上方绕（§T1.4）。真实 $x$-$y$ 轨迹是路径 $l(s)$ 与速度 $s(t)$ 合成后的结果，不是规划这一步直接面对的对象。

Q：练习总让我"手推一遍"，工程上又不手算，有必要吗？ A：有。手推小算例（N=3 的 QP 矩阵、5 层的 DP 网格）不是为了让你以后手算，而是为了让你对"这套机器在算什么"建立**底层直觉**——出 bug 时能猜到大概错在哪、调参时知道该改哪个量、读源码时几百维的组装在你眼里只是小例子的放大。会调库和懂原理之间，差的就是这几次手推。

延伸阅读（按主题与难度）¶

奠基论文：Kant & Zucker, Toward Efficient Trajectory Planning: The Path-Velocity Decomposition, IJRR 1986, 5(3):72–89（★ 必读，PVD 与 ST 图的源头）。
Frenet 轨迹生成：Werling 等, Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenét Frame, ICRA 2010, pp. 987–993（★★）；注意与同组 …Discretized Terminal Manifolds, IJRR 2012, 31(3):346–359 是**两篇不同论文**（见 §T1.2 辨析）。
工业实现：Fan 等, Baidu Apollo EM Motion Planner, arXiv:1807.08048（★★，注意它用的是 spline-QP，非现行 piecewise-jerk）； Apollo modules/planning 源码（★★★，对照本章读 piecewise_jerk_path / piecewise_jerk_speed）。
框架对照：Autoware 的 planning 模块（★★，ROS 2 原生的另一套实现，T4 会精读）。
求解器：OSQP 论文与文档（★★，理解 ADMM 一阶法与 1.0 / 0.6.x 的 API 差异）。
指向后续：解耦的失效与时空联合 → 本书 T2（走廊类）、T3（CILQR / MINCO 优化类）。

本章与后续章节的关系¶

本章概念	在后续如何深化 / 被替代
PVD 解耦范式	T2 / T3 打破解耦，做时空联合优化（本章是它们的反面基线）
Frenet 坐标系	贯穿整个 Part-T；T4 在 Apollo / Autoware 工程语境再现
ST 图 + DP→QP	T2 的走廊（corridor）类方法在此基础上深化
piecewise-jerk QP	T3 的 MINCO / 优化类方法的前身与对照
速度规划（$(t,s)$ 空间）	T3 与机械臂 TOPP-RA（$(s,\dot s^2)$ 空间）打通为同一范式
EM / 块坐标下降	T3 与真正的联合优化（CILQR 等）做收敛性与最优性对比
动态障碍的预测投影	Part-U 处理预测的不确定性（本章把预测当确定值）
单车 ST 协调	Part-Multi 推广为多智能体时空协调（§T1.4 的多体版）

🔧 故障排查手册¶

按"症状 → 可能原因 → 排查 → 修复"组织，覆盖实现本章时最常撞到的 5 类问题。

1. QP 求解返回 infeasible（OSQP 报 primal infeasible） - 可能原因：约束自相矛盾——常见于横向 / ST 走廊上下界交叉（$l_{\text{lb}}>l_{\text{ub}}$），或把 DP 离散解当硬等式（§T1.5 编程陷阱）。 - 排查：逐 station 打印 $[\text{lb}, \text{ub}]$，找 $\text{lb}>\text{ub}$ 的位置；检查起点状态是否落在边界内。 - 修复：放松冲突边界 / 用走廊区间而非等式 / 对软约束加松弛变量。

2. 路径在弯道处抖动或曲率超限 - 可能原因：参考线未平滑（$\kappa$ 阶跃，§T1.2 思维陷阱）；或 ddl_bounds 忘了减 $\kappa$（§T1.3）。 - 排查：单独画 $\kappa(s)$，看是否有跳变；检查 ddl 界是否随 $\kappa$ 变化。 - 修复：先跑 ReferenceLineSmoother； ddl_bounds 改为 $\pm a^\kappa_{\max}-\kappa(s_i)$。

3. 速度规划让车以高速冲进急弯（仿真过、实车甩） - 可能原因：漏了向心加速度折减（§T1.5 概念误区），速度上界只用了 $v_{\max}$。 - 排查：在弯道段打印 $v_i$ 与 $\sqrt{a_{\text{lat,max}}/\lvert\kappa\rvert}$ 对比。 - 修复：把 $v_{\text{limit}}(s)$ 按路径曲率折减接进速度 box 上界；或上非线性优化器精确处理耦合。

4. EM 迭代不收敛 / 规划周期超时 - 可能原因：外循环用"等收敛再停"而非硬限轮数（§T1.6 编程陷阱）；强耦合场景本就该上 T2/T3。 - 排查：日志记录每轮 JointCost，看是单调下降、还是在两个值间来回跳。 - 修复：硬限 2–3 轮 + 联合代价早停；若仍震荡，判定为强耦合、转时空联合。

5. Frenet 逆变换在 U 形弯处 $s$ 突然跳变 - 可能原因：最近点用全局 argmin、未处理多解 / 端点（§T1.2 调试练习）。 - 排查：可视化查询点的候选最近点，看是否在弯顶附近有两个近似等距段。 - 修复：用上一帧 $s$ 约束本帧搜索窗口（局部投影）；必要时上 kd-tree。

6. 跟车距离忽远忽近 / 整体一顿一顿 - 可能原因：相邻规划周期的 ST 走廊或 DP 决策跳变（时间一致性差）；或速度 QP 未做 warm start。 - 排查：记录连续几帧的 $s(t)$，看周期间是否大幅跳变；检查 STBoundary 的膨胀是否逐帧一致。 - 修复：用上一周期解做 warm start；对 yield/overtake 决策加迟滞 / 平滑，避免在边界反复横跳。

7. OSQP 结果数值不稳定 / 偶发不收敛 - 可能原因：$P$ 矩阵条件数差（权重量级悬殊）；或约束尺度差异大（米、弧度、秒混在一起未归一）。 - 排查：打印 $P$ 的权重比值与各约束的数量级；对比开 / 关 OSQP 内置 scaling 的表现。 - 修复：把权重控制在合理的量级比内、对变量与约束做无量纲化；必要时调 OSQP 的 eps_abs / eps_rel 与 max_iter。

API 速查表¶

接口	作用	关键点
`FrenetToCartesian(rp, l)`	$(s,l)\to(x,y)$	沿法向 $(-\sin\theta_r,\cos\theta_r)$ 偏移 $l$
`CartesianToFrenet(p, ref)`	$(x,y)\to(s,l)$	局部最近点投影 + 带符号距离
`SmoothReferenceLine(raw)`	平滑参考线	凸 QP，输出 $\kappa$ 连续
`AddSContinuityRow(...)`	速度连续性等式	匀 jerk 三次积分，含 $\tfrac12\Delta t^2,\tfrac16\Delta t^3$
`SpeedLimitAt(kappa,...)`	向心折减速度上界	$\min(v_{\max},\sqrt{a_{\text{lat}}/\lvert\kappa\rvert})$
`DpStSearch(obs,...)`	ST 前向 DP	$s$ 单调；出 yield/overtake + 走廊
`EmPlan(ref,obs,max_iter)`	EM 外循环	硬限轮数；`JointCost` 早停
`osqp_setup(...)`	建 QP	$P$ 上三角（OSQP 1.0）；声明版本

研究实践建议（分层）¶

入门（先跑起来）：只实现 FrenetConverter + 单个 SLPathOptimizer，用一条解析参考线验证；对照 PythonRobotics 的 Frenet 示例理解数据流。
进阶（对照工业代码）：把本章的 PlanOneCycle 跑通，逐模块对照 Apollo planning 源码，重点看 piecewise_jerk_* 的约束组装与本章的异同；亲手实现 2–3 轮 EM。
研究（找问题）：量化"解耦失效边界"——构造一组耦合强度递增的场景，测 EM 在第几轮震荡 / 与时空联合（T2/T3）的代价差，给出"何时必须联合"的可操作判据。这是一个能直接接续到 T2/T3 的开放研究问题。
复现 + 对比：复现 Werling 2010 的 Frenet 末端采样法，与本章的 SL-QP 在同一组场景上比轨迹质量与耗时，亲手体会"采样 vs 优化"两条路线的取舍（呼应 §T1.2）。
消融实验：在你的 PlanOneCycle 上做消融——去掉 jerk 项、去掉向心折减、把 DP 网格调粗，分别看评价指标（见"评价指标"一节）怎么变。这能把本章每个设计选择的"为什么"，从读来的文字变成你自己测出来的数字。

版本信息速查¶

项目	状态 / 版本	备注
Apollo 路径 / 速度优化器	piecewise-jerk（现行）	EM 论文（2018）用的是 spline-QP；引用别混
Apollo `ddl_bounds`	$\pm a^\kappa_{\max}-\kappa(s_i)$	减参考线曲率，论文不写、代码里有
OSQP	0.6.x（多数教程） / 1.0.x（2025）	1.0 的 $P$ 须上三角，C API 与 0.6.x 不兼容
Kant & Zucker	IJRR 1986, 5(3):72–89	"Toward"（非 Towards）；卷期 5(3) 非 5(1)
Werling	ICRA 2010 / IJRR 2012	两篇不同论文（见 §T1.2 辨析）
本章代码	教学实现	为讲解简化，非任何框架源码原文

拓展练习（综合 / 项目级）¶

这些题跨多个小节，适合在做完累积项目后回头挑战，每道都把本章的若干块拼在一起用：

[综合实现] 在 §T1.7 的 cut-in 场景上，对比"一次性解耦"与"2 轮 EM"两种管线的输出，画出各自的 $l(s)$、$s(t)$ 与逐轮 JointCost，定量说明 EM 在这个强耦合场景里改善了什么、代价是多少耗时。
[设计] 给你的规划器接一套"评价指标"（见"评价指标"一节）自动打分，并搭一个含 5 个场景（直道跟车、绕静态障碍、弯道限速、cut-in、无前车自由流）的回归测试集，每个场景定一组阈值，跑通"改代码即自动评分"的闭环。
[推导 + 工程] 推导参考线平滑 QP 的代价与约束（§T1.7 Step 0），实现它，并验证：用平滑前 vs 平滑后的参考线分别跑路径 QP，对比下游 $l(s)$ 与 $\kappa$ 的抖动差异。
[复现] 取 Apollo planning 仓库里 piecewise-jerk path / speed 的一段约束组装代码，逐行对照本章 §T1.3 / §T1.5 的公式，列出"代码里有、本章没细讲"的工程细节（如数值缩放、边界裕度的具体取法），整理成一页对照笔记。
[开放] 在你的实现里制造一个"前提破裂"的场景（会主动加塞的强交互车），观察解耦管线如何失败，并写一段分析：这个失败该用 §T1.6 的 EM 缓解，还是必须等 T2/T3 或 Part-G？你的判据是什么？

结语：你现在站在哪里¶

学完这一章，你手里有了一套完整的解耦式时空规划骨架：用 PVD 把非凸的轨迹问题拆成路径与速度两段（§T1.1），用 Frenet 坐标把弯曲道路"拉直"、让横纵解耦（§T1.2），在 SL 图上用 piecewise-jerk QP 求路径（§T1.3），在 ST 图上用 DP 做让 / 抢决策、再用 QP 精修速度（§T1.4–§T1.5），最后用 EM 迭代在弱耦合处缝合两段（§T1.6），并亲手把它们串成一个能跑的 PlanOneCycle（§T1.7）。这套"先解耦、再用 DP 构造凸空间、最后 QP 精修"的范式，就是当今工业级规划器（以 Apollo 为代表）的主干。

更要紧的是，你也看清了这套骨架的**边界**：它依赖障碍独立、路径-速度弱耦合、有合理参考线这三条前提（§T1.1）。任何一条破裂，就该把目光投向后面的章节——强耦合去 T2/T3 的时空联合，强交互去 Part-G 的博弈式规划，预测的不确定性去 Part-U。解耦不是终点，而是一把让你看清"何时它够用、何时不够"的尺子。带着这把尺子，我们进入 T2。

对象	\(s\) 范围	\(l\) 范围	说明
自车起点	\(0\)	\(0\)	\(v_0=8\)
抛锚车（静态）	\([15, 25]\)	\([1.0, 2.0]\)	投到 SL 图当横向阻挡
前车（动态，\(t{=}0\)）	\(\approx 30\)	\(\approx 0\)	随 \(t\) 在 ST 图里上移（§T1.3 中先准静态占位）

符号	含义	首见
\(s\)	纵向弧长 / station（沿参考线的进度）	§T1.2
\(l\)	横向偏移 / lateral（左正右负）	§T1.2
\(l',\ l''\)	\(\mathrm dl/\mathrm ds,\ \mathrm d^2l/\mathrm ds^2\)（横向一、二阶导）	§T1.2
\(\theta_r,\ \kappa_r\)	投影点处参考线的切向朝向、曲率	§T1.2
\(\theta\)	车辆朝向	§T1.2
\(\kappa_{\text{path}}\)	车辆实际路径的曲率（\(\approx\kappa_r + l''\)）	§T1.2
\(v\)	车速	§T1.2
\(a,\ j\)	纵向加速度、jerk（\(\mathrm d^3 s/\mathrm d t^3\)）	§T1.5
\(a_{\text{lat}},\ a_{\text{lat,max}}\)	横向（向心）加速度及其上限	§T1.5
\(v_{\text{limit}}(s)\)	速度上界（含向心折减 \(\sqrt{a_{\text{lat,max}}/\lvert\kappa\rvert}\)）	§T1.5
\(a^\kappa_{\max}\)	车辆最大曲率（转向极限）	§T1.3
\(s_{\text{lb}}(t),\ s_{\text{ub}}(t)\)	ST 走廊下 / 上界（来自 §T1.4 的 DP 决策）	§T1.4
\(\Delta s,\ \Delta t\)	路径 / 速度问题的离散步长	§T1.3 / §T1.5
\(N,\ N_t,\ N_s\)	离散点数 / ST 时间层数 / \(s\) 方向格数	§T1.3 / §T1.4
\(w_\bullet\)	各代价项权重（\(w_l, w_{l''}, w_j, \dots\)）	§T1.3
\(\mathcal O_{\text{static}},\ \mathcal O_{\text{dynamic}}\)	静态 / 动态障碍集合	§T1.1
\(\xi\)	软约束的松弛变量	§T1.5
`JointCost`	路径 + 速度的联合代价（EM 收敛判据）	§T1.6

维度	完整时空问题	PVD 拆解后
决策变量规模	\(\sim 5N\)（如 \(N{=}40 \Rightarrow 200\) 维）	path \(\sim 2N\) + speed \(\sim 2M\)（如 \(80 + 80\)）
凸性	非凸（避障可行域带洞、不连通）	path 可整理为凸 QP，speed 可整理为凸 QP
全局最优	无多项式算法，易卡鞍点	每个凸子问题有唯一全局最优
实时性	数百 ms～秒级	每个子问题 OSQP 毫秒级
信息耦合	静态地图与动态预测混在一起	path 吃静态地图，speed 吃动态预测

维度	spline-QP（EM 2018）	piecewise-jerk QP（Apollo 现行）
决策变量	分段多项式系数	每 station 的 \([l, l', l'']\) 状态
矩阵结构	较密（系数跨段耦合）	稀疏带状（仅相邻 station 耦合）
物理可解释性	弱（系数无直观含义）	强（偏移 / 斜率 / 曲率）
box 约束	不直接（需经基函数换算）	直接（状态即被约束量）
调参与求解	相对难调	更快、更好调
Apollo 现状	已被替换	当前路径优化器所用

维度	piecewise-jerk speed QP（线性）	piecewise-jerk nonlinear speed optimizer（Apollo `PiecewiseJerkSpeedNonlinearOptimizer`）
曲率-速度耦合 \(v\le v_{\text{limit}}(s)\)	近似：用 DP 的 \(s\) 预算 \(v_{\text{limit}}\) 当 box 界	精确：作为非线性约束直接处理
求解器	OSQP（凸 QP，ms 级）	IPOPT（NLP，较慢）
何时用	实时主路径、绝大多数场景	需要更精确曲率-速度协调（如高速大曲率匝道）时

术语	英文	一句话定义
路径-速度解耦	Path-Velocity Decomposition (PVD)	把轨迹拆成几何路径 \(\sigma(s)\) + 速度剖面 \(s(t)\) 两个子问题
Frenet 坐标系	Frenet frame	贴参考线的坐标 \((s, l)\)：纵向弧长 + 横向有符号偏移
纵向 / 横向	station \(s\) / lateral \(l\)	沿参考线进度 / 垂直参考线偏移（左正右负）
SL 图	Station-Lateral graph	\((s, l)\) 平面，静态障碍投影为横向阻挡，用于路径规划
ST 图	Station-Time graph	\((t, s)\) 平面，动态障碍投影为时变阻挡，用于速度规划
时变阻挡	STBoundary	动态障碍在 ST 平面的占据区域（平行四边形 / 矩形 / 梯形）
分段 jerk	piecewise-jerk	以状态 \([\cdot,\cdot',\cdot'']\) 为变量、段内 jerk 恒定的状态空间表示
DP-ST 搜索	DP-ST search	ST 网格上的前向 DP，做 yield/overtake 决策、出粗速度
让行 / 抢行	yield / overtake	从 ST 阻挡下方 / 上方通过——两个不连通的可行类
同伦类	homotopy class	拓扑上不可连续过渡的解类（如绕障左/右、让/抢）
块坐标下降	block coordinate descent	把变量分块、轮流固定一块优化另一块；EM 迭代的真名
向心折减	centripetal speed limit	\(v\le\sqrt{a_{\text{lat,max}}/\lvert\kappa\rvert}\)，弯道压速度
参考线平滑	reference line smoothing	把 HD 地图离散点平滑成曲率连续的参考线（一个凸 QP）
热启动	warm start	用上一轮 / 上一周期的解做求解器初值，加速收敛
坐标退化	coordinate degeneracy	\(\kappa_r l\to 1\) 时 Frenet 变换奇异

模块	职责	来自	验收标准
`FrenetConverter`	\((s,l)\leftrightarrow(x,y)\) 正逆变换 + 局部最近点投影	§T1.2	圆弧参考线往返误差 \(<10^{-6}\)；U 弯不跳变
`ReferenceLineSmoother`	离散点平滑 QP	§T1.7	输出 \(\kappa(s)\) 连续、贴近原始点
`SLPathOptimizer`	piecewise-jerk 路径 QP（OSQP）	§T1.3	绕开静态障碍；`ddl_bounds` 含减 \(\kappa\)；\(P\) 上三角
`STGraphBuilder`	动态障碍投影为 `STBoundary`	§T1.4	阻挡按自车 + 障碍尺寸 + 裕度膨胀
`DpStSearch`	前向 DP-ST	§T1.4	输出 yield/overtake 决策 + 走廊；\(s\) 单调不减
`SpeedOptimizer`	piecewise-jerk 速度 QP（OSQP）	§T1.5	弯道按向心折减降速；走廊内精修平滑
`EmPlanner`	2–3 轮 E/M 外循环	§T1.6	`JointCost` 单调不增；硬限轮数；warm start 传递
`PlanOneCycle`	串联全部	§T1.7	在 §T1.7 场景端到端跑通并可视化

接口	作用	关键点
`FrenetToCartesian(rp, l)`	\((s,l)\to(x,y)\)	沿法向 \((-\sin\theta_r,\cos\theta_r)\) 偏移 \(l\)
`CartesianToFrenet(p, ref)`	\((x,y)\to(s,l)\)	局部最近点投影 + 带符号距离
`SmoothReferenceLine(raw)`	平滑参考线	凸 QP，输出 \(\kappa\) 连续
`AddSContinuityRow(...)`	速度连续性等式	匀 jerk 三次积分，含 \(\tfrac12\Delta t^2,\tfrac16\Delta t^3\)
`SpeedLimitAt(kappa,...)`	向心折减速度上界	\(\min(v_{\max},\sqrt{a_{\text{lat}}/\lvert\kappa\rvert})\)
`DpStSearch(obs,...)`	ST 前向 DP	\(s\) 单调；出 yield/overtake + 走廊
`EmPlan(ref,obs,max_iter)`	EM 外循环	硬限轮数；`JointCost` 早停
`osqp_setup(...)`	建 QP	\(P\) 上三角（OSQP 1.0）；声明版本