做自动驾驶决策规划领域的开发第一次翻开Apollo planning模块源码的时候我其实是被吓到的——模块多、依赖深、坐标变换绕尤其看到Path Assessment、Speed Decider这些概念时很难把它们串成一条完整的链路。后来我把目光收回到最基础的两个DPDynamic 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层比如从s0到s40每0.5米采一个点得到大概80层。在每一层s_i上根据道路边界和自车宽度在横向采样一组候选l值。比如从l-3.0到l3.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_{i1}, l_{i1})一阶差分(l_i - l_{i-1})/Δs 对应横向变化率太大说明路径绕着参考线来回扭。二阶差分((l_{i1} - 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::vectorDpPathPoint DpPathSearch( const std::vectordouble s_samples, const std::vectorstd::vectordouble l_candidates, const std::vectorstd::vectordouble obstacle_costs, // 已经算好的每个候选点碰撞代价 double w_first, double w_second, double w_ref) { const int n s_samples.size(); std::vectorstd::vectorDpPathPoint dp(n); // 初始化第0层 for (int j 0; j static_castint(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_castint(dp[i - 1].size()); for (int j 0; j static_castint(l_candidates[i].size()); j) { double best_cost std::numeric_limitsdouble::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::vectorDpPathPoint path_reversed; int last_layer_best 0; for (int j 1; j static_castint(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图上都是一条从(t0, s0)出发的单调曲线因为车不能后退。障碍物在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::vectorDpSpeedPoint DpSpeedSearch( const std::vectordouble t_samples, const std::vectorstd::vectordouble s_candidates, const std::vectorstd::vectordouble obstacle_costs, // ST栅格碰撞代价 const std::vectordouble speed_limits) { // 每秒限速 const int n t_samples.size(); std::vectorstd::vectorDpSpeedPoint dp(n); // 第0层t0, s0且要求与当前速度v_init匹配 for (int j 0; j static_castint(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_castint(s_candidates[i].size()); j) { double best_cost std::numeric_limitsdouble::infinity(); int best_prev -1; double s_cur s_candidates[i][j]; double t_cur t_samples[i]; for (int k 0; k static_castint(dp[i - 1].size()); k) { double s_prev dp[i - 1][k].s; double t_prev dp[i - 1][k].t; // 简化jerk项需要更早两层这里对i2时从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::vectorDpSpeedPoint result_reversed; int best_idx 0; for (int j 1; j static_castint(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层到第i1层之间可能有速度突变所以也必须交给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路径规划和速度规划就能真正在自己手里转起来而不是只在文档里被一笔带过。