企业官网建设流程全解析

做自动驾驶决策规划领域的开发，第一次翻开Apollo planning模块源码的时候，我其实是被吓到的——模块多、依赖深、坐标变换绕，尤其看到Path Assessment、Speed Decider这些概念时，很难把它们串成一条完整的链路。后来我把目光收回到最基础的两个DP（Dynamic Programming）搜索器上，就是DpPolyPath和DpStSpeed这类实现，才慢慢理清思路：路径规划在SL坐标系里做DP搜索，速度规划在ST坐标系里做DP搜索，两个问题解耦之后，各自就变成了一个二维网格上的最优路径问题。

这篇文章就把我当时啃下来的东西整理成一份可复现的笔记：为什么Apollo要把路径和速度拆成两个DP问题、SL图和ST图上的采样和代价函数怎么设计、状态转移怎么回溯，以及配套的C++核心代码长什么样。文章最后还会聊一些调参和落地时踩过的坑，适合已经在做规划算法、或者正准备啃Apollo代码的读者参考。

1. 先解决一个关键问题：为什么路径和速度要分开规划

1.1 规划的输入输出到底是什么

在做任何实现之前，先得把坐标系说清楚。高级地图和感知模块给到规划模块的信息，是参考线（ReferenceLine）以及一堆障碍物的位置、速度、形状。参考线你可以理解成一条沿着道路方向拉出来的引导线，通常经过了平滑处理。

自车在参考线上的位置，用两个量表示：

• s：沿着参考线从起点算起的纵向距离。
• l：到参考线中心线的横向偏移，左负右正或左正右负取决于约定。

这就是SL坐标系。有了SL坐标系，二维的路径搜索就变成了一个"沿着s走，每一步选一个l"的问题。障碍物也可以投影到这个坐标系里。比如一个静态障碍物，在SL图上就是占据了一段(s区间, l区间)的矩形区域，动态障碍物则需要预测，放到后面的ST图里处理。

路径规划的输出，是一条路径：一串(s_i, l_i)点。速度规划在这条路径的基础上，输出一串(s_j, t_j)或(t_j, s_j)点，表示不同时刻车辆应该走到哪个纵向位置，进而推出速度和加速度。

1.2 联合规划的维度灾难

稍微有点算法经验的读者到这里可能会问：既然路径和速度本质上都在同一个时空中，为什么不直接做三维联合搜索，x、y、t一起规划？

答案可以很简单：计算量扛不住。假设路径方向采样50个s候选，每个s采样10个l候选，速度方向再采样50个t，每个t采样10个s位置。如果联合起来搜索，状态空间就是50×10×50×10，也就是25万个节点，再算上状态转移，量级要到几百万甚至上千万。这在10到20毫秒的规划周期内是根本不可行的。

Apollo的做法是分两步走：先固定一个路径，再做速度规划。这样做在绝大多数场景下是合理的，因为路径的形状和平滑性主要受障碍物和道路边界约束，速度曲线主要受动态障碍物、限速和舒适性约束。两者有耦合，但解耦带来的误差远小于联合搜索带来的实时性问题。

1.3 DP在Apollo里的角色

动态规划在Apollo规划模块里，并不是直接产出最终轨迹的，它承担的角色是"粗搜索"和"兜底决策"。

原因在于DP本身有一些特性：

• 采样点离散，输出结果天然带锯齿，不能直接下发控制。
• DP的代价函数只能基于有限邻域的差分近似，无法对整条曲线做高阶连续性优化。
• 但它很适合做全局贪心式的可行性搜索：只要cost函数设计合理，它能在极短时间内避开致命碰撞点，找到一个从起点到终点可行的粗糙路径。

所以Apollo的经典流程可以概括成：参考线 → Path DP（粗路径） → Path QP/spline平滑 → Speed DP（粗速度曲线） → Speed QP平滑 → 最终Trajectory。DP负责"找到可行走廊"，QP负责"在走廊里磨光滑"。理解这一点，后面看代码就不会晕。

2. 路径DP的C++实现：SL栅格上的采样、代价与回溯

2.1 路径搜索的问题建模

路径DP把路径规划抽象成这样：

• 沿参考线方向，以固定步长Δs采样出一系列s层，比如从s=0到s=40，每0.5米采一个点，得到大概80层。
• 在每一层s_i上，根据道路边界和自车宽度，在横向采样一组候选l值。比如从l=-3.0到l=3.0，步长0.5米，就是13个候选。
• 每一层的每个候选点都对应一个DP节点。我们的任务是在层层网格之间找一条从起点到终点的最小代价路径。

正因为每一层只选了有限个横向偏移，问题就退化成了一个典型的网格DP搜索：从第0层逐步扩展到最后一层，每一层节点只考虑从上一层的节点转移过来。

2.2 路径代价函数的设计逻辑

在写代码前，cost函数的设计是重中之重。Apollo路径DP的代价通常由这几部分组成。

碰撞代价

这是硬约束，必须设为压倒性的大值。做法是把每个障碍物投影到SL坐标系，然后用"矩形包络+膨胀"近似。计算当前候选点(s, l)到障碍物的横向、纵向距离，如果点落在障碍物内部或者距离小于安全阈值，直接给一个极大的代价；如果不碰撞但比较近，代价随距离减小而增大。常见处理方式：

if l_center_in_obs_range 或 s_center_in_obs_range: cost = 1e6 else: cost = k / (distance_to_obs + epsilon)

参考线偏移代价

l²倍的权重。希望车辆尽量靠近参考线，避免偏离车道中心，这个项决定了路径的"贴线程度"。如果权重调太高，车会过于激进地往中心线上靠，转弯时路径会切弯甚至越线；权重太低，车可能一直在车道边缘游荡。

平滑代价

这是让路径看着"像人开的"的关键。离散路径的平滑性用差分近似衡量。假设连续三个点(s_{i-1}, l_{i-1})、(s_i, l_i)、(s_{i+1}, l_{i+1})：

• 一阶差分(l_i - l_{i-1})/Δs 对应横向变化率，太大说明路径绕着参考线来回扭。
• 二阶差分((l_{i+1} - l_i) - (l_i - l_{i-1}))/Δs² 对应曲率变化率的感觉，太大说明不连续、有折角。
• 工程上还会再加三阶jerk差分的近似。

DP状态转移时，为了计算二阶差分，必须知道前一层甚至是前两层选了什么l值。这也是DP路径实现里相对绕的地方。

2.3 状态转移与回溯

路径DP的状态转移可以形式化描述：在第i层选择候选点(l_i)时，遍历上一层所有节点。对每个上一层节点，计算"该节点的累计代价 + 从它转移到当前点的新增代价"，取最小值作为当前点的累计代价，并记录来源节点的索引。

新增代价必须包括当前点的碰撞代价、参考线偏移代价、与上一节点的平滑代价，以及和上上节点相关的三阶项。三阶项严格来说需要两层前驱信息，所以有些实现会把状态扩展成(l_{i-1}, l_i)的组合，让DP状态带记忆。为了代码可读性，我下面给出的是带前一层、前两层代价的简化写法。

从最后一层选累计代价最小的节点，不断向前回溯，得到的节点序列就是DP路径。这一步逻辑很简单，但最后得到的往往是一条锯齿明显的线，这正是需要平滑模块接手的原因。

2.4 路径DP核心代码实现

这里给出核心DP循环和回溯代码。代码不是完整工程，但把最关键的搜索部分写出来了。假设已经被投影到SL系，并用二维向量存储候选点。

#include <vector> #include <limits> #include <algorithm> struct DpPathPoint { double s; // 纵向位置 double l; // 横向偏移 double cost; // 到当前点的累计最小代价 int prev_idx; // 最优前驱在上一层的下标 }; // 计算当前点(s,l)与上一层前驱之间新增的代价 // 这里用一阶+二阶差分近似平滑代价，三阶jerk可作为扩展项 double ComputeAdditiveCost(double s_cur, double l_cur, double s_prev, double l_prev, double s_prev2, double l_prev2, double ref_l, double obs_cost) { double ds = std::max(s_cur - s_prev, 1e-6); double first_diff = (l_cur - l_prev) / ds; // 近似横向变化率 double second_diff = (l_cur - 2.0 * l_prev + l_prev2) / (ds * ds); // 近似曲率变化 double smooth_cost = w_first * first_diff * first_diff + w_second * second_diff * second_diff; double ref_cost = w_ref * (l_cur - ref_l) * (l_cur - ref_l); return smooth_cost + ref_cost + obs_cost; } std::vector<DpPathPoint> DpPathSearch( const std::vector<double>& s_samples, const std::vector<std::vector<double>>& l_candidates, const std::vector<std::vector<double>>& obstacle_costs, // 已经算好的每个候选点碰撞代价 double w_first, double w_second, double w_ref) { const int n = s_samples.size(); std::vector<std::vector<DpPathPoint>> dp(n); // 初始化第0层 for (int j = 0; j < static_cast<int>(l_candidates[0].size()); ++j) { double ref_cost = w_ref * l_candidates[0][j] * l_candidates[0][j]; dp[0].push_back({s_samples[0], l_candidates[0][j], ref_cost + obstacle_costs[0][j], -1}); } // 从第1层开始逐层扩展 for (int i = 1; i < n; ++i) { int prev_size = static_cast<int>(dp[i - 1].size()); for (int j = 0; j < static_cast<int>(l_candidates[i].size()); ++j) { double best_cost = std::numeric_limits<double>::infinity(); int best_prev = -1; double l_cur = l_candidates[i][j]; double s_cur = s_samples[i]; for (int k = 0; k < prev_size; ++k) { double s_prev = dp[i - 1][k].s; double l_prev = dp[i - 1][k].l; double l_prev2 = l_prev; // 第一层时prev2没有实际值，用prev代替 double s_prev2 = s_prev; if (i >= 2) { // 严格来说应该查dp[i-2]来确定prev2，这里需要额外记录 // 下面这个示例为了简洁，省略了跨层的prev2存储 } double additive = ComputeAdditiveCost( s_cur, l_cur, s_prev, l_prev, s_prev2, l_prev2, 0.0, obstacle_costs[i][j]); double total = dp[i - 1][k].cost + additive; if (total < best_cost) { best_cost = total; best_prev = k; } } dp[i].push_back({s_cur, l_cur, best_cost, best_prev}); } } // 从最后第n-1层选最优节点，回溯完整路径 std::vector<DpPathPoint> path_reversed; int last_layer_best = 0; for (int j = 1; j < static_cast<int>(dp[n - 1].size()); ++j) { if (dp[n - 1][j].cost < dp[n - 1][last_layer_best].cost) { last_layer_best = j; } } int idx = last_layer_best; for (int i = n - 1; i >= 0; --i) { path_reversed.push_back(dp[i][idx]); idx = (i > 0) ? dp[i][idx].prev_idx : -1; } std::reverse(path_reversed.begin(), path_reversed.end()); return path_reversed; }

这段代码有一个明显简化：没有真正记录前驱的前驱（prev_prev），所以在计算二阶导时严格逻辑上有缺口。实际工程中要么在DpPathPoint里多存一个prev_prev_idx，要么把DP状态扩展为"上一层的节点是组合状态"，要么用另外的数组缓存历史路径。不要直接照搬这段就上线，但核心的逐层取最小、记录backpointer、回溯的逻辑是完整的。

另外我要强调：obstacle_costs这个二维矩阵，是DP开始前就算好的。每个候选点都预先算一个基于障碍物距离的碰撞代价，避免在多层循环里反复做投影和距离计算。这也是Apollo代码里会花很多时间做预处理的原因——把"反复重复的计算"提前到循环之前，能让DP主循环快不少。

3. 速度DP的C++实现：在ST图上搜索可执行的速度曲线

3.1 速度规划到底在解决什么

路径已经定下来后，车辆沿路径行驶的快慢是速度规划管的事。输入包括：路径的几何信息、自车当前速度、加速度、当前时刻、障碍物的预测轨迹、限速、交通规则（停车线、人行横道）等。

速度规划用时间t和纵向距离s组成ST图。横轴是时间，纵轴是沿路径的纵向距离。任何一条速度曲线，在ST图上都是一条从(t=0, s=0)出发的单调曲线，因为车不能后退。

障碍物在ST图上的投影方式比较关键：

• 静态障碍物：如果障碍物正好挡在路径前方s_obs处，那么在ST图上它是一条竖直的阻塞带，横向覆盖时间轴的所有t位置，但纵向占据s_obs附近的一段区域。车辆要么在到达s_obs前停车，要么提前变道绕开。
• 动态障碍物：根据障碍物预测轨迹，在某个时间段内它可能占据某个s区间，于是在ST图上形成一个多边形区域。自车的速度曲线要避开这些区域，同时保持舒适性。

速度DP要做的就是：在ST网格上找一条从起点到未来某个终点的低代价路径。节点之间横向（时间步）的移动距离对应平均速度，两次速度变化对应加速度，三次对应jerk。

3.2 ST图上的采样方式

速度DP的采样一般有两种选择：以时间为横轴、以纵向位置s作为每层候选变量；或者以时间为横轴、以速度v作为每层候选变量。

Apollo系列实现更常见的是以s作为候选变量：在每个采样时间t_i，生成若干候选s位置。相邻两层的s差除以Δt就是速度。如果想直接约束速度区间，可以先根据限速和当前约束算出一个s区间范围，再在这个范围内线性采样。比如t_i时刻可选的s在[s_min_i, s_max_i]之间，按0.5米步长取多个候选点。

ST图上的DP状态转移与路径DP几乎一样：对每一层(t_i)的每个候选s_j，遍历上一层所有候选点，计算加速度、jerk、障碍物代价，取累计代价最小的作为当前节点的值，记录前驱索引。

3.3 速度DP代价函数设计

速度DP的代价函数比路径DP更讲究，因为它直接决定了乘客的舒适性，也是控制模块能否稳定执行的凭据。

加速度代价

(i层的s - i-1层的s)/Δt是当前段平均速度。相邻两段平均速度的差再除以Δt就是加速度。加速度大，推背感强，悬架压缩大，乘员容易晕车。所以代价函数里通常加上加速度的平方项。

jerk代价

加速度的变化率，也就是相邻两段加速度差除以Δt。这个量对乘坐体验影响很大，急刹急加减速会让乘客尤其不舒服。Apollo速度DP里jerk本身也是重要item，但注意DP的jerk是用差分近似得到的，精度有限，所以DP找的是"一个大致的、舒适的节奏"，精确的jerk最小化仍然交给后面的QP平滑。

限速和规则代价

如果当前s对应的路径点有限速，而候选点速度超过限速，则给高代价。如果前方有停车线，在停车线对应的s位置附近强制要求速度降到0，这就相当于给ST图加了一个特殊障碍物区域。

跟车距离代价

对于前车，我们希望在ST图上不仅有碰撞区域，还有安全缓冲距离。比如前车轨迹在ST图上占了一条带状区域，我们可以人为扩展这条带，把"太接近但不碰撞"的情况也赋予中等代价，让DP倾向于留出足够距离。这个距离通常跟自车速度和前车速度有关系，跟车越近，代价增长越快。

3.4 速度DP核心代码实现

下面给一个ST图DP搜速的简化实现。重点在状态转移和回溯逻辑，前面路径DP里讲过的部分就不再重复展开。

#include <vector> #include <limits> #include <algorithm> struct DpSpeedPoint { double t; double s; double cost; int prev_idx; }; // 计算从(t_prev, s_prev)到(t_cur, s_cur)再到(t_prev2, s_prev2)产生的代价 double SpeedAdditiveCost(double t_cur, double s_cur, double t_prev, double s_prev, double t_prev2, double s_prev2, double obstacle_cost, double speed_limit) { double dt = std::max(t_cur - t_prev, 1e-3); double v1 = (s_prev - s_prev2) / std::max(t_prev - t_prev2, 1e-3); // 上一段平均速度 double v2 = (s_cur - s_prev) / dt; // 当前段平均速度 double acc = (v2 - v1) / dt; double jerk = 0.0; // jerk需要三个连续时段才能计算，工程上可用前两段加速度的差近似 double acc_cost = w_acc * acc * acc; double jerk_cost = w_jerk * jerk * jerk; double limit_cost = speed_limit > 0.0 ? w_limit * std::max(0.0, v2 - speed_limit) * std::max(0.0, v2 - speed_limit) : 0.0; return acc_cost + jerk_cost + limit_cost + obstacle_cost; } std::vector<DpSpeedPoint> DpSpeedSearch( const std::vector<double>& t_samples, const std::vector<std::vector<double>>& s_candidates, const std::vector<std::vector<double>>& obstacle_costs, // ST栅格碰撞代价 const std::vector<double>& speed_limits) { // 每秒限速 const int n = t_samples.size(); std::vector<std::vector<DpSpeedPoint>> dp(n); // 第0层：t=0, s=0，且要求与当前速度v_init匹配 for (int j = 0; j < static_cast<int>(s_candidates[0].size()); ++j) { double init_cost = obstacle_costs[0][j]; dp[0].push_back({t_samples[0], s_candidates[0][j], init_cost, -1}); } for (int i = 1; i < n; ++i) { for (int j = 0; j < static_cast<int>(s_candidates[i].size()); ++j) { double best_cost = std::numeric_limits<double>::infinity(); int best_prev = -1; double s_cur = s_candidates[i][j]; double t_cur = t_samples[i]; for (int k = 0; k < static_cast<int>(dp[i - 1].size()); ++k) { double s_prev = dp[i - 1][k].s; double t_prev = dp[i - 1][k].t; // 简化：jerk项需要更早两层，这里对i>=2时从dp[i-2]取前驱 // 实现时建议在DpSpeedPoint里额外存储prev_prev_idx double s_prev2 = s_prev; double t_prev2 = t_prev; if (i >= 2) { // 从dp[i-2]和dp[i-1][k].prev_idx获取真实的前两段 } double additive = SpeedAdditiveCost( t_cur, s_cur, t_prev, s_prev, t_prev2, s_prev2, obstacle_costs[i][j], speed_limits[i]); double total = dp[i - 1][k].cost + additive; if (total < best_cost) { best_cost = total; best_prev = k; } } dp[i].push_back({t_cur, s_cur, best_cost, best_prev}); } } std::vector<DpSpeedPoint> result_reversed; int best_idx = 0; for (int j = 1; j < static_cast<int>(dp[n - 1].size()); ++j) { if (dp[n - 1][j].cost < dp[n - 1][best_idx].cost) { best_idx = j; } } int idx = best_idx; for (int i = n - 1; i >= 0; --i) { result_reversed.push_back(dp[i][idx]); idx = (i > 0) ? dp[i][idx].prev_idx : -1; } std::reverse(result_reversed.begin(), result_reversed.end()); return result_reversed; }

速度DP的结果就是一系列(t_i, s_i)点。我们拿相邻两点的s差除以Δt，就能得到速度；再对速度序列做差分，得到加速度。这个速度曲线同样带有离散步子，比如第i层到第i+1层之间可能有速度突变，所以也必须交给QP平滑器做二次优化。

这里还有一个细节要提醒：ST图DP和路径DP不一样的地方在于，纵向s是严格单调的。也就是说，候选s_j必须大于上一层选中的s_prev，否则车辆就是倒车了。在遍历上一层时，可以直接跳过s_prev >= s_cur的节点，或者初始化时就把负向速度候选剔除。这个小问题不注意，很容易在特殊场景（比如停车再起步）里搜出回退路径。

4. DP结果只有一半是好用的：为什么必须接平滑和约束

4.1 DP路径的锯齿效应与QP平滑

如果你真把DP搜出来的路径打出来看，大概率会看到一条折线：明明障碍物是圆的，路径却是一段段直角的折线，拐点处还有很小的棱角。原因很简单：

• 横向候选点都是离散的，比如l只能取±0.5的倍数。
• 纵向s步长再小，也无法让l在层与层之间连续变化。
• 代价函数里差分近似和高阶项也只是"尽量"抑制突变，但不可能让曲线C2连续。

控制和规划领域常说的"平滑"，本质上是在满足约束的前提下找一条曲率连续、曲率变化率有界的曲线。DP给不出这种曲线，于是Apollo里会用二次规划（QP）或者样条拟合做再做一轮优化：把DP路径当作初始解或约束走廊，优化目标是让路径到DP路径的距离不要太大，同时让曲率变化尽可能小。这样既保住了避障的可行性，又让下游控制模块能跟着走。

4.2 速度曲线的平滑与加速度约束

速度DP输出的(s, t)序列同样需要平滑。如果直接把相邻两点的差当速度下发，那么速度的差分会有很大的噪声，尤其当DP为了躲避ST障碍物时选择了非常硬性的绕行路径，速度曲线可能出现阶跃。

QP速度平滑器做的事情是：在DP结果附近寻找一条时间连续的速度曲线，要求：

• 起点速度、加速度与当前自车状态一致。
• 全程满足加速度上下限（比如[-3, 2] m/s²）。
• 全程满足jerk上限（比如[−2, 2] m/s³）。
• 尽量贴近DP给的可行解，避免被优化后的曲线拉进障碍物区。

一条严格满足这些约束的曲线，才能作为最终的Trajectory下发到底盘/控制模块。

4.3 DP和QP的边界划分

很多新手最大的困惑是：DP已经选了路径/速度，QP又改动它，最后会不会改坏？

实际问题里，DP的定位是"找到安全走廊和粗略形状"，QP的定位是"在走廊内优化车辆动力学可行性"。DP为了避免碰撞，会倾向于绕得很远，而QP在平滑时会把路径往参考线方向拉回来一点，只要不碰撞就行。这两级优化天然互补：DP保证"不错"（不撞、可行、有大方向），QP保证"更优"（平滑、舒适、可控）。

我在实际调试里的体会是：DP的代价权重决定了QP的初始解位置，DP把路径选得太贴右边护栏，QP再怎么平滑也难救回来。所以调参顺序通常是先让DP搜出来的路径肉眼可见地合理，再调QP的优化系数。这个顺序不能反，否则QP永远在跟一个不合理的初值较劲。

5. 落地过程中的几个真坑和调试心得

5.1 采样分辨率：不是越细越好

路径DP的纵向步长Δs我见过有人直接取0.1米，理由是"这样路径更精细"。看起来很美，但注意：步长越小，层数越多，每一层的候选点之间间距也越近，DP的总节点数是层数乘以每层候选数，复杂度随步长细化线性增长。

更关键的是，步长过小会让平滑代价失真。相邻两层l几乎一样，一阶差分被分母的Δs放大了很多倍，导致代价函数对横向抖动极其敏感，DP会倾向于把路径掰得笔直，反而不利绕障。

我自己的经验值：路径DP的Δs取0.5到1.0米比较合理，横向候选步长取0.5米，横向范围根据车道宽动态调整。速度DP的时间步长Δt取0.1到0.2秒，规划视界6到8秒即可。时间步长太小，jerk的差分噪声很大；太大，速度曲线又太糙，对障碍物动态变化反应滞后。

5.2 代价权重的调参原则

权重调整是规划算法落地时最消耗时间的工作。这块我给一个比较有效的调参顺序：

• 先把碰撞代价调到"绝对不可违反"级别。就像前面代码里写的1e6这种，只要真的碰撞，任何代价都比不过它。
• 再调参考线偏移权重，让DP路径基本贴得住中心线。
• 最后调平滑权重。平滑权重过低，路径会像在网格里爬似的，控制模块会疯狂抖方向盘；平滑权重过高，车辆在遇到需要急绕障的场景时会显得很呆，宁可减速也不变道。

调参有个小技巧：把DP每一层的代价矩阵输出成文本或热力图，用脚本可视化。你很快就能看出来"哪里代价凹陷、哪里被高高隆起的地形挡住"。如果某一段路径贴着障碍物走，点开热力图看碰撞代价那一项，大概率就是权重没拉开。

5.3 动态障碍物投影的坑

速度DP里最容易出问题的是动态障碍物的ST投影。有些实现直接把动态障碍物在未来的位置当成静态障碍物投影，这其实是不对的，因为没有考虑自车的运动状态，会导致ST图上出现不可能避开的"假墙"。

正确的做法是依据障碍物预测轨迹，将障碍物在t时刻的s区间投影到ST图，同时记得考虑障碍物本身的形状和自车半个车长加半个车宽的包络。如果障碍物正在减速或静止，这个区域会随时间往后延伸；如果障碍物加速离开，它占据的s区间可能快速移出前方规划范围。总之，投影错了，后续所有DP速度搜索都是在跟空气斗智斗勇。

5.4 一个特别容易忽略的问题：前驱索引的合法性

DP回溯在二维网格里看着简单，但工程实现里有个常见bug：在计算当前节点最优前驱时，只比较了cost，没有检查前驱节点本身是否有效。比如路径DP里有些候选点因为碰撞代价过高根本不该被扩展，但它可能仍然以极小概率成为某一层的"局部最优"，然后被后续回溯选中，导致最终路径穿过障碍物。

稳妥做法是在扩展前驱时跳过cost已经超过某个阈值的节点，或者在初始化时把碰撞区域的候选点直接从候选集合里剔除，而不是寄希望于1e6的cost能拦住所有路径。我实际碰到过因为int溢出导致1e6的碰撞代价在累加过程中变成负数的诡异问题，后来把所有代价累加改成double，并把碰撞代价上限统一收敛，这个问题才彻底消失。

5.5 从DP到实车需要补的测试

代码写完不代表这块工作结束了。我自己的流程里，DP模块必须有三个层次的验证：

• 单层单测：固定输入障碍物配置，验证DP路径是否在期望的栅格走廊里。
• 场景重放：拿之前的log重放，对比DP中间结果是否合理，尤其关注"变道绕障""路口停车""跟车减速"三类场景。
• 整车在环：DP只是规划大流程里的一个环节，最终下发轨迹要验证控制模块跟不跟得住。如果跟不住，大概率是平滑环节的契合问题，而不是DP本身的问题。

跑完一轮之后我个人最大的体会是：DP规划器的代码量其实不大，真正的复杂度全在代价函数设计、参数调试和障碍物预处理上。把这三个地方想明白，SL/ST上的DP路径规划和速度规划就能真正在自己手里转起来，而不是只在文档里被一笔带过。