纵向决策规划框架学习

纵向规划流水线详解 (1/6): SpeedBondDecider - 初始 ST 图生成

模块一：SpeedBondDecider (第一遍运行)

这是纵向规划流水线的第一个执行单元。它的核心任务是构建一个客观、物理的“时空地图”（ST 图），为后续的“规划脑”（DP）提供决策依据。

1. 核心目标

生成一个**“空的”ST 图**。

“空的” (Empty) 是一个关键概念，它意味着这张 ST 图仅包含障碍物的物理时空边界（它在什么时间、会出现在什么位置），但不包含任何驾驶意图或决策属性（例如，我们是Follow跟车还是Overtake超车）。

2. 主要输入

SpeedBondDecider 模块为了构建 ST 图，需要两个关键的初始数据：

1. 本车规划路径 (Path):

   • 这不是笛卡尔坐标系，而是由横向模块（或上游）提供的一条一维曲线。
   • 该路径的S 轴（S-axis，即弧长）将成为 ST 图的纵坐标。S=0 代表本车当前位置，S=50 代表沿该路径前方 50 米处。

2. 障碍物预测轨迹 (Trajectory):

   • 来自感知和预测模块的数据。
   • 包含在未来 $T$ 秒内（例如 8 秒）所有障碍物的高频预测位置（3D 包围盒）。

3. 核心处理流程：“3D+T -> 1D+T” 降维

ST 图的构建是一个精密的**“降维”**过程。SpeedBondDecider 必须将复杂的 3D 物理世界投影到一个 1D 的 S 轴上：

1. 初始化 ST 图: 创建一个以 $T$（时间）为横轴，$S$（路径距离）为纵轴的二维网格。
2. 遍历时间 $T$: 以 $dT$（例如 0.1 秒）为步长，遍历规划时域内的所有时间切片。
3. 遍历障碍物: 在每一个时间切片 $t$ 上，获取该时刻所有障碍物的 3D 预测包围盒。
4. 投影计算 (核心):
   • 对于 $t$ 时刻的某个障碍物（例如“前车A”），模块会沿着S 轴（本车路径）进行扫描。
   • 它会计算“前车A”的 3D 盒子在本车 S 轴上的投影范围。
   • 例如：在 $t = 2.0$ 秒时，系统计算发现“前车A”占据了本车 S 轴上 $S=30$ 米到 $S=35$ 米的这段空间。
   • 此时，系统就得到了一个关键坐标点：在 $T=2.0$ 秒时，障碍物 A 对应的 $s_{\text{min}} = 30$ 和 $s_{\text{max}} = 35$。
5. 绘制边界: 重复步骤 4，计算出障碍物 A 在所有时间切片 $T$ 上的 $[s_{\text{min}}, s_{\text{max}}]$ 范围。将这些点连接起来，就构成了该障碍物在 ST 图上的时空边界（ST Boundary）。

4. 模块输出

• 一个“空的” ST 图:
  • 这是一个数据结构，包含了一系列障碍物的 ST 边界。
  • 它只回答一个问题：“障碍物在哪里？”
  • 它不回答：“我该怎么办？”

小结:
在第一步中，SpeedBondDecId 模块完成了一次纯粹的“环境建模”。它将 3D 物理世界成功降维，转换成了 DP 算法可以理解的 2D 时空地图。

纵向规划流水线详解 (2/6): HeuristicOptimizer - DP 寻路

模块二：HeuristicOptimizer (动态规划)

这是流水线的第二个执行单元。它扮演着“战略家”的角色，负责在模块一 (SpeedBondDecider) 构建的“时空地图”上进行智能搜索。

1. 核心目标

此模块的目标不是生成一条平滑的轨迹，而是要解决一个组合优化问题。

• 输入: 模块一 (SpeedBondDecider) 生成的“空的” ST 图（即 ST 边界）。
• 核心任务: 在这张充满“障碍物” (ST Boundaries) 的 2D 地图上，从起点 (T=0, S=0) 出发，找到一条累积代价最小的、粗略的 S-T 轨迹。
• 隐式决策: 这条轨迹的“形态”（例如是从障碍物上方还是下方绕过），就隐式地决定了后续的驾驶策略，即“跟车” (Follow) 还是“超车” (Overtake)。

2. 工程实现：基于网格的动态规划

为了让计算机能够求解，连续的 ST 空间必须被“离散化”为一个网格。

2.1 状态定义：dp[t][s]

• 网格 (Grid): ST 图被划分为一个 $M \times N$ 的网格。
  • T 轴 (横向): 被切分为 $M$ 个时间步长 $dT$ (例如 $T=8$秒, $dT=0.1$秒, $M=80$ 步)。
  • S 轴 (纵向): 被切分为 $N$ 个距离步长 $dS$ (例如 $S=120$米, $dS=0.5$米, $N=240$ 步)。
• 状态 dp[t][s]: 数组 dp[t][s] 存储一个代价值 (Cost)。
  • 物理含义: 它代表车辆从起点 (0, 0) 出发，按规则行驶，最终到达时间步 t、距离格 s 这个状态时，所需要付出的最小累积代价。

2.2 状态转移：前向迭代

DP 算法是一个“填表”的过程。它从 $T=0$ 时刻开始，按时间步前向迭代 (Forward Pass)，计算并填满整个 $dp$ 表格。

在计算 $dp[t][s]$（当前状态）时，它会回看上一个时间步 $t-1$ 的所有可能状态 $dp[t-1][s_p]$（$s_p$ 代表 $s_{\text{previous}}$）。

状态转移方程的核心思想如下：
dp[t][s] = StateCost(t, s) + min( dp[t-1][s_p] + TransitionCost(t-1, s_p -> t, s) )
(遍历所有合法的 $s_p$)

• $StateCost(t, s)$: 状态代价。代表“仅仅存在于 (t, s) 这个格子”所要付出的代价（例如，这个格子是不是在障碍物里）。
• $TransitionCost(\dots)$: 转移代价。代表“从 (t-1, s_p) 移动到 (t, s)”这个动作所付出的代价（例如，这个动作的加/减速是否过猛）。
• $min(\dots)$: DP 的核心，确保只保留累积代价最小的转移路径。

3. DP 的“大脑”：代价函数 (Cost Function)

DP 的所有“智能”都体现在代价函数的设计上。它必须平衡安全、效率、舒适三个方面。

3.1 状态代价 (StateCost) - 决定安全

这是 DP 寻路的硬约束来源。

• 障碍物代价 (Obstacle Cost):
  • 硬约束: 如果网格点 (t, s) 位于模块一生成的 ST 障碍物边界内部，则 $StateCost = \infty$ (无穷大)。
  • 效果: DP 在计算 $min(\dots)$ 时，会自动抛弃任何试图穿过障碍物的路径。这是强制 DP 绕行的核心机制。
  • 软约束 (Buffer Cost): 如果 (t, s) 只是靠近（但未进入）障碍物边界，则 $StateCost = \text{High\_Cost}$。
  • 效果: 这会鼓励 DP 轨迹尽量在“安全通道”的中央行驶，而不是“贴着”障碍物走，为后续 QP 优化留出裕度。

3.2 转移代价 (TransitionCost) - 决定效率与舒适

这是评估“驾驶动作”好坏的代价。这个“动作”是从 (t-1, s_p) 移动到 (t, s)。

1. 速度代价 (Speed Cost):

   • 动作的隐含速度: $V = (s - s_p) \times dS / dT$。
   • 代价计算: $Cost_V = w_v \times (V - V_{\text{ref}})^2$。
   • 目的: 惩罚“实际速度 $V$”与“期望参考速度 $V_{\text{ref}}$”（例如道路限速）之间的差异，鼓励车辆保持高效巡航。

2. 加速度代价 (Acceleration Cost):

   • 动作的隐含加速度: $A = (V - V_{\text{prev}}) / dT$。
   • 代价计算: $Cost_A = w_a \times A^2$。
   • 目的: 惩罚过大的加减速（$A$ 的绝对值），这是保证平顺性 (Smoothness) 的关键。

3. 加加速度代价 (Jerk Cost):

   • 动作的隐含Jerk: $Jerk = (A - A_{\text{prev}}) / dT$。
   • 代价计算: $Cost_J = w_j \times Jerk^2$。
   • 目的: G-Jerk（加速度的变化），例如从“刹车”猛地切换到“油门”。这是保证舒适性 (Comfort) 的关键。

(注: $w_v, w_a, w_j$ 是权重系数，用于平衡效率与舒适性)

4. 模块输出

当 DP 填完整个表格后：

1. 回溯 (Backtracking):
   • 算法会查看最后一行 $dp[T_{\text{max}}][\dots]$，找到总代价最低的那个 $s_{\text{final}}$。
   • 然后，它会利用在计算过程中保存的路径信息（例如一个 path[t][s] 数组，记录了它是从哪个 $s_p$ 转移来的），从 $s_{\text{final}}$ 开始反向回溯，直到 $T=0$。
2. 最终输出:
   • 一条粗略的 S-T 轨迹: 这是一个由 (t, s) 坐标点组成的折线。
   • 轨迹特点:
     • 可行性: 保证不碰撞（已绕开所有 ST 障碍物）。
     • 粗糙性: 轨迹是离散的、有棱角的（折线），不平滑。
     • 全局最优: 它是 DP 代价函数下的“战略最优解”。

小结:
在第二步中，HeuristicOptimizer (DP) 充当了“战略家”。它通过在 ST 网格上搜索，找到了一条不碰撞、代价最小的粗略 S-T 轨迹。

纵向规划流水线详解 (3/6): SpeedDecider - 决策“翻译”

模块三：SpeedDecider (决策分配器)

这是流水线的第三个执行单元，也是“第一遍规划”(Pass 1)的收尾工作。它扮演着“翻译官”的角色，负责解读DP轨迹的战略意图。

1. 核心目标

将 HeuristicOptimizer (DP) 产生的隐式路径（一条粗糙的S-T折线），“翻译”并显式地赋予 ST 图中每个障碍物一个纵向决策属性（如 Follow, Overtake）。

2. 主要输入

1. “空的” ST 图: (来自模块一) 包含所有障碍物的物理边界。
2. DP 粗略轨迹: (来自模块二) 一条S-T折线，代表DP找到的代价最低的路径。

3. 核心处理流程：“轨迹解读”

SpeedDecider 会遍历 ST 图上的每一个障碍物，并将其边界与 DP 轨迹进行对比：

1. 获取障碍物 A：例如，一个在 $T=[2, 5]$ 秒， $S=[30, 40]$ 米的“障碍物矩形”。
2. 对比 DP 轨迹：查看 DP 轨迹在 $T=[2, 5]$ 秒这个时间段内，是在该“障碍物矩形”的上方还是下方通过的。
3. 决策推导规则：
   • Overtake (超车)：如果 DP 轨迹的 $S$ 值高于障碍物的 $s_{\text{min}}$（即在 ST 图上，DP 路径从障碍物“上方”绕过），系统判定 DP 的“意图”是超车。
     • 赋值：Obstacle_A.decision = Overtake
   • Follow (跟车)：如果 DP 轨迹的 $S$ 值低于障碍物的 $s_{\text{max}}$（即在 ST 图上，DP 路径从障碍物“下方”绕过），系统判定 DP 的“意图”是减速跟车。
     • 赋值：Obstacle_A.decision = Follow

4. 特殊决策处理

除了上述二选一，SpeedDecider 还需要处理更复杂的边缘情况，以确保决策的鲁棒性：

• Stop (停止)：
  • 触发: 通常用于静态车辆（其预测轨迹为空，导致 ST 边界是一个水平长条）或高风险目标。
  • 处理: 直接赋值 Obstacle_Static.decision = Stop。这个决策会跳过后续的常规优化，直接在 QP 中生成一个到 $s_{\text{stop}}$ 位置的停车指令。
• Yield (避让)：
  • 触发: Yield 是 Follow 决策的一种演变或升级。
  • 处理: 当 Follow 决策被赋予后，模块会进一步检查。如果发现跟车时间过长（例如超过了预设的 MaxT 阈值，如2秒），或者跟车距离过近，系统会将决策从 Follow 升级为 Yield。
  • 含义: Yield 通常意味着需要采取更保守的减速策略，或者为可能的停车做准备。
• Ignore (忽略)：
  • 触发: 对于那些虽然在 ST 图上，但距离本车极远（例如 S > 100米），或者横向距离很远（D > 5米）的障碍物。
  • 处理: 赋予 Ignore 决策。这些障碍物将不会在后续的 QP 优化中产生任何约束。

5. 模块输出

• 一个“带决策属性”的 ST 图:
  • 这不是一张新图，而是对模块一 ST 图的**“原地修改” (In-place Modification)**。
  • 现在，ST 图数据结构中的每一个障碍物，除了有 $[s_{\text{min}}, s_{\text{max}}]$ 边界信息，还多了一个**Decision 标签**（Follow, Overtake, Stop 等）。

小结:
在第三步中，“翻译官” (SpeedDecider) 成功地将 DP 的数学最优路径，转化为了人类可以理解的、明确的驾驶意图。

流水线状态:
至此，“第一遍规划” (Pass 1) 已全部完成。我们有了一张带决策的 ST 图。

纵向规划流水线详解 (4/6): SpeedFinalDecider - 决策驱动的约束精化

模块四：SpeedFinalDecider (约束精化器)

这是流水线的第四个执行单元，也是“第二遍规划”(Pass 2)的起点。它扮演着“安全工程师”的角色，负责利用 Pass 1 的决策，生成 QP 优化器最终必须严格遵守的硬约束。

1. 核心目标

将 SpeedDecider (模块三) 产生的驾驶意图（Follow, Overtake 等决策），转化为数学上严格、且带有安全裕度的 S-T 边界，供 QP 求解器使用。

2. 主要输入

1. “带决策属性”的 ST 图: (来自模块三) ST 图中每个障碍物都已被标记了 Follow, Overtake, Stop 或 Ignore。
2. 本车状态与安全参数: 例如本车车长、配置的安全跟车模型（如 C-HW, Constant Headway time-based following）、期望的超车裕度等。

3. 核心处理流程：“决策驱动的扩图 (Expansion)”

SpeedFinalDecider 会重新遍历 ST 图上的每一个障碍物，并根据其决策标签，对其 ST 边界 (来自模块一) 进行“精修”和“扩图”：

3.1 Follow (跟车) 决策处理

• 原始边界: 模块一生成的 ST 边界是一个“硬壳”，代表障碍物的物理位置 $[s_{\text{min}}, s_{\text{max}}]$。
• 决策含义: 既然我们要 Follow（在它上方通过），QP 优化器只需要关心它的下边界 $s_{\text{min}}$（即障碍物在 S 轴上的“尾部”）。
• 扩图 (Expansion): 我们不能让 QP 规划一条“贴着”前车 $s_{\text{min}}$ 的轨迹。
  • 精修: 模块会丢弃上边界 $s_{\text{max}}$（因为我们不关心它的车头），只保留下边界 $s_{\text{min}}$。
  • 扩图: 在 $s_{\text{min}}$ 的基础上减去一个“安全跟车距离”（例如，根据安全车头时距 C-HW 计算出的 10 米）。
• 最终约束: 生成一个新的、QP 必须遵守的下边界硬约束：$S_t \ge (s_{\text{min}, t} - \text{SafeFollowDistance})$。

3.2 Overtake (超车) 决策处理

• 原始边界: $[s_{\text{min}}, s_{\text{max}}]$。
• 决策含义: 既然我们要 Overtake（在它下方通过），QP 优化器只需要关心它的上边界 $s_{\text{max}}$（即障碍物在 S 轴上的“头部”）。
• 扩图 (Expansion):
  • 精修: 模块会丢弃下边界 $s_{\text{min}}$，只保留上边界 $s_{\text{max}}$。
  • 扩图: 在 $s_{\text{max}}$ 的基础上加上一个“安全超车裕度”（例如 1 米），确保我们不会“擦着”车头过去。
• 最终约束: 生成一个新的、QP 必须遵守的上边界硬约束：$S_t \le (s_{\text{max}, t} + \text{SafeOvertakeMargin})$。

3.3 Stop (停止) 决策处理

• 处理: Stop 决策会生成一个静态的、上边界约束。
• 例如: 如果静态障碍物在 $S=50$ 米处，约束将是 $S_t \le (50 - \text{SafeStopDistance})$。

3.4 Ignore (忽略) 决策处理

• 处理: 被标记为 Ignore 的障碍物，不会在此模块中生成任何 S-T 约束。它将被 QP 求解器完全无视。

3.5 特殊约束注入 (例如：换道调速)

• 场景: 正如会议中所讨论，如果车辆正在换道，此模块还负责注入“换道调速”约束。
• 处理: 它会“观察”目标车道上的车辆，并可能将其投影到本车的 ST 图中，生成一个（通常是 Follow）约束，以确保本车在汇入时与目标车道的车流速度相匹配。

4. 模块输出

• 最终的 QP 约束集:
  • 这是一个精简且安全的 S-T 边界列表。
  • 它不再是“硬壳”矩形，而是 QP 优化器需要遵守的一系列 S 上边界 ($S \le \dots$) 和 S 下边界 ($S \ge \dots$)。
  • 这就是 QP 求解器 (QP Optimizer) 的核心硬约束。

小结:
在第四步中，SpeedFinalDecider 充当了“安全工程师”。它将“战略家”(DP) 的意图，转化为了“专业车手”(QP) 必须严格遵守的、带有安全裕度的最终赛道边界。

纵向规划流水线详解 (5/6): QP Optimizer - 轨迹平滑与求解

模块五：QP Optimizer (二次规划优化器)

这是流水线的第五个执行单元，也是“第二遍规划”(Pass 2)的核心。它扮演着“专业车手”的角色，负责在“硬约束”下求解出“最平滑”的轨迹。

1. 核心目标

在满足所有安全和动力学硬约束的前提下，求解一个连续优化问题，生成一条在数学上最平滑、最舒适的 S-T 轨迹。

• 对比 DP (模块二): DP 解决的是“走哪条路”（组合问题）；QP 解决的是“怎么把这条路走得最好”（连续问题）。

2. 主要输入

QP 优化器接收两类输入：硬约束（必须遵守）和软约束（尽量满足）。

1. 硬约束 (Hard Constraints) - 必须遵守

   • ST 边界约束: (来自模块四)
     • $S_t \le S_{\text{upper\_bound}, t}$ (例如，不能超过前方障碍物的尾部减去安全距离)
     • $S_t \ge S_{\text{lower\_bound}, t}$ (例如，不能低于后方障碍物的头部加上安全裕度)
   • 速度约束 (V): (来自地图和车辆)
     • $V_t \le V_{\text{speed\_limit}}$ (道路限速)
     • $V_t \ge 0$ (不能倒车)
   • 加速度约束 (A): (来自车辆动力学)
     • $A_{\text{min}} \le A_t \le A_{\text{max}}$ (例如，最大刹车 $-5.0 \text{m/s}^2$，最大油门 $2.0 \text{m/s}^2$)
   • 加加速度约束 (Jerk): (来自舒适性要求)
     • $Jerk_{\text{min}} \le Jerk_t \le Jerk_{\text{max}}$ (例如，Jerk 变化率不能过大，防止顿挫)

2. 软约束 / 参考线 (Soft Constraints / Reference Line) - 尽量满足

   • DP 粗略轨迹: (来自模块二)
     • $S_{\text{ref}, t}$ (DP 规划的 S 轨迹)
     • $V_{\text{ref}, t}$ (DP 规划的 V 轨迹)
   • 作用: 这条 DP 轨迹是 QP 的“引导线”。QP 会努力“贴近”这条轨迹，但如果为了贴近它而会导致不平滑（Jerk过大），QP 会优先选择平滑。

3. 核心处理流程：求解二次规划问题

QP 优化的本质是求解一个带约束的最小二乘问题。

3.1 优化变量

求解器需要求解的未知数是未来一段时间内（例如 $T=8$秒, $dT=0.1$秒）所有 $M=80$ 个时间点上的状态序列：

(注：实际求解中，S, V, A 之间通过运动学公式 $S_t = S_{t-1} + V_{t-1}dT + \dots$ 相互关联)

3.2 目标函数 (Cost Function) - 软约束

QP 求解器的目标是找到一组 $X$，使得这个总代价函数 $J$ 最小。
正如会议中提到的，这个函数是各项代价的平方和（这就是“二次”规划的来源）：

• $w_s(S_t - S_{\text{ref}, t})^2$: S 路径代价。惩罚轨迹偏离 DP 参考线的距离。
• $w_v(V_t - V_{\text{ref}, t})^2$: V 速度代价。惩罚速度偏离 DP 参考速度。
• $w_a(A_t)^2$: 加速度代价。一个关键的平滑项，迫使求解器找到 $A$ 尽量接近 0 的解，避免不必要的加减速。
• $w_j(Jerk_t)^2$: Jerk 代价。最关键的舒适性项，迫使求解器找到 $A$ 变化率尽量为 0 的解，消除顿挫感。

(注: $w_s, w_v, w_a, w_j$ 是权重系数，用于平衡“跟随参考”与“保持平滑”之间的关系)

3.3 求解

求解器（如 OSQP, qpOASES）会接收上述的“目标函数”和“硬约束”，在毫秒级时间内计算出一个唯一的最优解 $X^*$。

3.4 健壮性处理 (Failsafe)

• 如果无解?: 理论上，如果“硬约束”设置得过于严苛（例如，SpeedFinalDecider 给出的 $S_{upper}$ 和 $S_{lower}$ 边界在某一刻重叠了），QP 可能会求解失败 (Infeasible)。
• 处理: 如会议中所述，系统会有一个 Failsafe 机制。
  1. 尝试放宽 (Expand): 第一次无解时，系统可能会尝试略微放宽硬约束（例如，将安全裕度缩小 10%），然后再次求解。
  2. 使用 DP 降级: 如果再次求解失败，系统将放弃 QP，转而使用（不平滑但安全的）DP 轨迹 (模块二) 作为本帧的输出，并触发紧急减速。

4. 模块输出

• 最终 S-T 轨迹 (Optimal Trajectory):
  • 这是一个平滑、连续、舒适且绝对安全（严格遵守所有硬约束）的 S-T 轨迹。
  • 它以一系列高频 (t, s, v, a, jerk) 坐标点的形式存储。

小结:
在第五步中，“专业车手”(QP Optimizer) 登场。它在“安全工程师”(SpeedFinalDecider) 划定的“最终赛道”上，以“战略家”(HeuristicOptimizer) 的粗略轨迹为“引导线”，跑出了那条最平滑、最舒适的“冠军路线”。

纵向规划流水线详解 (6/6): 轨迹输出与 MPC 闭环

模块六：轨迹发布器 (Trajectory Publisher)

这是流水线的第六个、也是最后一个执行单元。它不进行复杂的计算，而是充当“规划大脑”和“车辆神经系统”（控制模块）之间的信使。

1. 核心目标

将 QP Optimizer (模块五) 生成的完整轨迹，以正确的格式、在正确的时间，发送给车辆控制模块（Control）。

2. 主要输入

• 最优 S-T 轨迹: (来自模块五)
  • 这是一个轨迹点序列 (List)，包含了未来 $T$ 秒（例如 $T=8$ 秒）的完整规划。
  • 数据结构: List<TrajectoryPoint>
  • 每个 TrajectoryPoint 包含:
    • $t$: 时间戳 (e.g., 0.1s, 0.2s, ...)
    • $s$: 路径距离
    • $v$: 速度
    • $a$: 加速度
    • $jerk$: 加加速度

3. 核心处理流程：“模型预测控制” (MPC) 思想

这是一个极其关键的概念，也是自动驾驶规划的核心思想。

一个问题： 既然我们辛辛苦苦算出了未来 8 秒的完美轨迹，我们会把这 8 秒的轨迹全部发给控制模块去执行吗？

答案是：绝对不会。 我们只发送这个轨迹的第一个点。

这就是“模型预测控制”（Model Predictive Control, MPC）的“滚动优化” (Receding Horizon) 思想，也是整个流水线闭环运行的关键：

1. 规划 (Plan): 在 $T=0$ 时刻，我们运行模块一到五，生成了一条 8 秒的完整轨迹（Optimal Trajectory）。

   • 为什么需要 8 秒？ 因为我们必须看得足够远（例如看到 5 秒后的红灯），才能做出当前（$T=0$ 时刻）正确的决策（例如开始轻微减速）。

2. 执行 (Execute):

   • 系统从这条 8 秒轨迹中，只提取第一个时间步（例如 $T=0.1$ 秒）的规划点：TrajectoryPoint(t=0.1, s=..., v=..., a=..., jerk=...)。

3. 发布 (Publish):

   • 系统将这个单独的 TrajectoryPoint 打包成一个 SpeedData 或 ControlCommand 消息。
   • 发送：这个消息被发送给 Control 模块（车辆的底层执行器）。
   • Control 模块的唯一任务就是：“在接下来的 0.1 秒内，努力让车辆的实际状态（$v, a$）与这个目标点一致。”

4. 丢弃 (Discard):

   • 刚才那条 8 秒轨迹的剩余 7.9 秒，被立即丢弃。

5. 循环 (Loop):

   • 0.1 秒后，车辆到达了新的位置，传感器（雷达、摄像头）重新扫描了整个世界。
   • 此时，T=0 时刻的“前车A”可能已经加速了，“前车B”可能突然刹车了。
   • 整个纵向规划流水线从模块一 (SpeedBondDecider) 开始，全部重新运行一次。
   • 它会基于这个全新的世界状态，再次计算出一条全新的 8 秒轨迹，然后重复步骤 2、3、4。

这个“规划 -> 执行第一步 -> 丢弃 -> 重新规划”的循环，以每秒 10 次（10Hz）的频率不断重复，确保了车辆既有长远的战略眼光（DP/QP），又能对瞬息万变的环境做出毫秒级的快速反应。

4. 模块输出

• SpeedData (控制指令):
  • 内容: 包含 (v, a, jerk) 的单点目标。
  • 目标: 车辆 Control 模块（最终通向 CAN 总线，控制油门和刹车）。

纵向规划流水线总结

至此，我们完整地走完了纵向规划的全部 6 个模块：

1. SpeedBondDecider (环境建模)

   • 工作: 将 3D 物理世界“降维”到 2D 的“时空地图”(ST图)。
   • 输出: “空的” ST 边界。

2. HeuristicOptimizer (DP 战略)

   • 工作: 在 ST 地图上，通过“填表”找到一条代价最低的粗略S-T 轨迹。
   • 输出: 一条S-T折线 (DP 轨迹)。

3. SpeedDecider (决策翻译)

   • 工作: “读取” DP 轨迹，将其“翻译”为 Follow, Overtake, Stop 等明确决策。
   • 输出: “带决策”的 ST 图。

4. SpeedFinalDecider (安全精修)

   • 工作: “画第二遍图”。利用“决策”，对 ST 边界进行安全裕度扩图。
   • 输出: 最终的 QP 硬约束（S上/下边界）。

5. QP Optimizer (平滑执行)

   • 工作: 在“硬约束”下，以 DP 轨迹为“软约束”参考，求解最平滑、最舒适的 8 秒 S-T 轨迹。
   • 输出: 最终的最优 S-T 轨迹序列。

6. Publisher (闭环控制)

   • 工作: 提取最优轨迹的第一个点，发送给控制模块。
   • 输出: SpeedData 控制指令。

这个“建模 -> 战略 -> 翻译 -> 安全 -> 执行 -> 控制”的流水线，在每个 100 毫秒的周期内不断循环，构成了自动驾驶纵向控制的坚实基础。