自动驾驶轨迹规划实战：Lattice Planner算法在复杂路况下的参数调优指南

在真实的自动驾驶项目里，把算法从论文搬到车上，最考验工程师的往往不是原理理解，而是那些藏在代码深处的“魔法数字”——参数。Lattice Planner以其采样和评估的框架，在学术界和工业界都备受青睐，但很多团队在初次部署时都会遇到类似的困境：仿真里跑得顺滑的轨迹，一到真实城市道路就变得要么过于保守、反应迟钝，要么过于激进、乘坐体验糟糕。这背后，几乎都是参数配置与具体场景脱节惹的祸。这篇文章，我想和你深入聊聊，如何像一位经验丰富的赛车调校师一样，去“驯服”Lattice Planner，让它能在瞬息万变的复杂路况下，输出既安全又舒适、还高效的轨迹。我们将抛开教科书式的泛泛而谈，聚焦于城市拥堵、高速合流、弯道巡航等具体场景，拆解每一个关键参数背后的物理意义和调优逻辑。

1. 理解核心：从Frenet坐标系到代价函数的设计哲学

在动手调参之前，我们必须对Lattice Planner的工作机制有更贴近工程实践的认知。它本质上是一个在Frenet坐标系下进行的“采样-评估-选择”过程。但为什么是Frenet坐标系？这不仅仅是数学上的便利。

Frenet坐标系的工程价值在于，它将复杂的二维平面规划问题，解耦成了相对独立的纵向（s方向，沿参考线）和横向（l方向，垂直参考线）运动。这极大地简化了问题。想象一下在城市道路中，参考线就是车道中心线。纵向规划主要关心“开多快”、“何时加减速”，这与前车、信号灯相关；横向规划则关心“是否变道”、“如何避开静态障碍”，这与车道线、邻车相关。解耦后，我们可以分别对两个方向进行采样和优化。

然而，这种解耦并非完美。一个常见的误区是认为纵向和横向完全独立。实际上，在高速过弯时，纵向速度会严重影响横向稳定性（离心力），这就需要通过代价函数或后处理进行耦合考虑。Lattice Planner生成候选轨迹的过程，可以概括为以下几步：

1. 横向采样：在目标时刻，横向位置l上采样若干个终点偏移量（例如，-0.5m, 0m, +0.5m，分别代表向左微调、保持车道、向右微调）。
2. 纵向采样：在目标时刻，纵向速度s_dot上采样若干个终点速度（例如，28m/s, 30m/s, 32m/s）。
3. 轨迹生成：为每一对（目标横向偏移， 目标速度）组合，利用多项式（常用四阶或五阶）生成一条从当前状态平滑过渡到目标状态的轨迹。
4. 代价评估：为每一条生成的轨迹计算一个“代价”分数。
5. 可行性过滤与选择：剔除与障碍物碰撞、超过车辆动力学极限的轨迹，然后从剩余轨迹中选择代价最低的一条。

这里最核心、也是最需要调校的部分，就是第4步的代价函数。它直接决定了算法在“安全”、“舒适”、“效率”等多个维度的权衡偏好。

一个典型的代价函数可能包含以下部分：

代价项	物理意义	调优影响
横向抖动代价 (Jerk_l)	横向加加速度的积分，衡量乘坐舒适性（“晃不晃”）。	权重调高，轨迹横向变化更柔和，但可能导致变道不果断。
纵向抖动代价 (Jerk_s)	纵向加加速度的积分，衡量加速/减速的平顺性（“蹿不蹿”）。	权重调高，加速刹车更线性，但可能对紧急情况响应变慢。
横向偏移代价 (l_offset)	轨迹终点与目标横向位置的偏差平方。	引导车辆靠近期望的横向位置（如车道中心）。
速度偏差代价 (v_offset)	轨迹终点速度与目标速度的偏差平方。	引导车辆维持期望速度，权重高则速度跟踪更紧。
时间代价 (t)	轨迹的总时间。	轻微的权重可以避免选择时间过长的轨迹，促进效率。

注意：代价函数的设计没有银弹。城市道路可能更看重舒适性（高Jerk权重），而高速场景可能更看重效率和对目标速度的跟踪（高v_offset权重）。这构成了我们参数调优的基调。

2. 场景化调优：关键参数的具体配置策略

参数调优绝不能脱离场景空谈。下面我们针对几种典型的复杂路况，分析核心参数的调整策略。假设我们有一个基础的参数集作为起点：

// 基础参数示例
float D_ROAD_W = 1.0; // 横向采样间隔 (米)
float MINT = 2.0;     // 最小规划时间 (秒)
float MAXT = 5.0;     // 最大规划时间 (秒)
float DT = 0.2;       // 时间分辨率 (秒)
float TARGET_SPEED = 10.0; // 目标速度 (米/秒)
float D_T_S = 2.0;    // 速度采样间隔 (米/秒)

// 代价函数权重
float KJ = 0.1;  // Jerk权重
float KT = 0.1;  // 时间权重
float KD = 1.0;  // 偏移权重
float KLAT = 1.0; // 横向总权重
float KLON = 1.0; // 纵向总权重

2.1 城市拥堵跟车场景

场景特点：车流密集，速度低（<50km/h），启停频繁，与前车距离近，对舒适性要求极高。

• 横向采样 (D_ROAD_W)：此场景下变道需求少，主要进行车道内微调以保持居中。可以将D_ROAD_W设置为较小的值，例如 0.2米，让采样点更密集，从而能生成更精细的横向调整轨迹，避免在车道内“画龙”。同时，横向偏移范围可以缩小，比如只采样[-0.3, 0, +0.3]米。
• 时间范围 (MINT, MAXT)：跟车时需要对前车动作做出快速反应。规划时间不宜过长，因为远处交通状态不确定性大。建议MINT=1.5, MAXT=3.0。较短的时间窗能让规划器更关注近期，做出更及时的加减速决策。
• 目标速度与采样 (TARGET_SPEED, D_T_S)：目标速度应设置为自适应巡航控制（ACC）模块给出的期望速度。速度采样间隔D_T_S可以适当减小，例如 0.5米/秒，在低速域生成更多速度选项，便于实现平缓的跟车。
• 代价函数权重：
  • 大幅提高纵向抖动代价权重 (KJ for Jerk_s)。这是提升舒适性的关键，让加速和刹车无比柔和。
  • 适度提高横向抖动代价权重，减少不必要的方向盘微动。
  • 降低速度偏差代价权重 (KD for v_offset)。在拥堵中，精确跟踪某个速度值不如平稳跟车重要，允许速度有一定误差可以换来更平滑的加速度曲线。
  • 可以引入一项 “距离代价”，惩罚轨迹点上与前方动态障碍物过近的状态，作为安全性的强化。

2.2 高速公路巡航与合流场景

场景特点：车速高（>80km/h），车道线清晰，但可能遇到慢车需要超车，或者需要汇入主路。

• 横向采样 (D_ROAD_W)：需要考虑变道动作。采样间隔可以保持默认的 1.0米，但采样范围需要覆盖到相邻车道，例如[-3.5, 0, +3.5]米（假设车道宽3.5米）。为了生成平滑的变道轨迹，可能需要使用更高阶的多项式（如五阶）来连接横向状态。
• 时间范围 (MINT, MAXT)：高速下需要看得更远以提前规划。建议MINT=3.0, MAXT=6.0甚至更长。更长的规划视野有助于提前发起舒适的变道，而不是急打方向。
• 目标速度与采样 (TARGET_SPEED, D_T_S)：目标速度设为道路限速或更高（在安全超车时）。速度采样间隔D_T_S可以设为 1.0米/秒，因为高速下速度分辨率无需像低速那样精细。
• 代价函数权重：
  • 提高速度偏差代价权重。高速巡航时，维持稳定、接近限速的速度是首要效率目标。
  • 平衡横向与纵向抖动权重。高速下突然的横向移动（高横向Jerk）或急加减速（高纵向Jerk）都会带来危险和不适，两者权重都应维持在适中偏高的水平。
  • 引入“曲率代价”或“离心力代价”。在生成轨迹后计算每个轨迹点的曲率，惩罚曲率过大（即转弯过急）的轨迹，这对于高速弯道安全至关重要。可以将其整合进横向代价中。

// 示例：在轨迹评估函数中增加曲率代价计算
float calc_curvature_cost(const FrenetPath& path) {
    float max_k = 0.0;
    for (size_t i = 0; i < path.x.size(); ++i) {
        // 简化计算曲率: k = |(x'y'' - y'x'')| / ( (x'^2 + y'^2)^(3/2) )
        float dx = path.s_d[i] * cos(path.yaw[i]) - path.d_d[i] * sin(path.yaw[i]); // 近似
        float dy = path.s_d[i] * sin(path.yaw[i]) + path.d_d[i] * cos(path.yaw[i]);
        float ddx = path.s_dd[i] * cos(path.yaw[i]) - path.d_dd[i] * sin(path.yaw[i]); // 更精确计算需考虑Frenet转换
        float ddy = path.s_dd[i] * sin(path.yaw[i]) + path.d_dd[i] * cos(path.yaw[i]);
        float k = std::abs(dx * ddy - dy * ddx) / std::pow(dx*dx + dy*dy, 1.5);
        if (k > max_k) max_k = k;
    }
    return K_CURV * max_k * max_k; // K_CURV 为曲率代价权重
}

2.3 复杂弯道与路口场景

场景特点：参考线曲率变化大，需要同时管理纵向速度和横向位置，防止侧滑或驶出车道。

• 横向采样 (D_ROAD_W)：弯道中，采样应更保守。采样范围应缩小，紧密围绕参考线，例如[-0.5, 0, +0.5]米。间隔D_ROAD_W也可减小至 0.5米，以生成更贴合弯道的轨迹。
• 时间范围 (MINT, MAXT)：取决于弯道大小。急弯需要更短的规划时间以快速调整，缓弯则可以长一些。一种自适应策略是根据参考线曲率动态调整MAXT：曲率越大，MAXT越小。
• 目标速度：目标速度不应是定值！ 必须根据弯道曲率进行动态调整。一个简单的映射是：target_speed = min(road_speed_limit, sqrt(lateral_accel_limit / |curvature|))，其中lateral_accel_limit是设定的横向加速度上限（如3.0 m/s²）。
• 代价函数权重：
  • 显著提高横向偏移代价权重 (KD for l_offset)。强制车辆紧紧跟随弯道参考线，减少偏离风险。
  • 强烈耦合纵向与横向代价。如前所述，高速过弯时，纵向速度过高会导致横向代价剧增（因为需要更大的离心力，体现为更高的横向加速度/加加速度）。在实践中，除了曲率代价，还可以在总代价中增加一项 “横向加速度代价”，直接惩罚过高的v² * curvature。

3. 进阶技巧：超越静态参数的动态与自适应策略

当基本参数在不同场景下手动切换显得笨拙时，我们需要更智能的策略。

3.1 参数的自适应调整

基于实时环境感知输出，动态调整采样范围和代价权重。

• 基于交通流密度调整采样：当感知模块检测到周围车辆密度高时，自动缩小横向采样范围，减少无效采样，聚焦于车道保持。
• 基于风险调整代价权重：当预测模块判断有高风险切入（如旁边车道车辆快速靠近）时，瞬间提高“与障碍物距离代价”的权重，使规划器产出更保守的轨迹。
• 基于舒适度模式的调整：为乘客提供“运动”、“舒适”、“经济”等模式。这些模式本质上就是不同预设的参数包：“运动”模式降低Jerk权重、提高效率权重；“舒适”模式则相反。

3.2 采样策略的优化

基础的均匀采样在复杂场景下效率低下，可以采用非均匀采样或分层采样。

• 横向非均匀采样：在车道中心附近采样密集，在车道边缘采样稀疏。这既保证了居中行驶的精细控制，又覆盖了变道的可能性。
• 纵向速度引导采样：不要均匀采样[v_min, v_max]。可以根据前车相对速度、交通灯状态，在期望速度附近进行密集采样，在极端速度区间进行稀疏采样。这能更快地找到最优解。

3.3 代价函数的工程化改进

基础的二次型代价函数有时不够灵活。

• 使用非线性代价函数：对于碰撞距离这类关键安全项，可以使用指数型或分段函数。例如，距离障碍物小于5米时，代价呈指数上升，这比平方项更能提供“安全边际”。
• 引入软约束作为代价：将一些硬约束（如加速度限制）转化为高权重的代价项，而不是直接过滤。这样在极端情况下（如紧急避障），算法仍能产生“可接受”的轨迹（虽然超出舒适限值，但物理可行），而不是无解。

4. 调试与验证：构建你的参数调优工作流

调参不是玄学，需要一个系统化的工程闭环。

4.1 仿真环境的构建

你需要一个能反映目标场景的仿真环境。不仅仅是简单的直线弯道，而应包含：

• 高保真车辆动力学模型：确保规划的轨迹车辆能实际执行，避免出现“规划可行，控制翻车”的情况。
• 丰富的场景库：涵盖本节提到的所有典型场景及其组合（如弯道跟车、路口汇入等）。
• 可量化的评价指标：除了是否碰撞，还要定义一系列量化指标，用于AB测试不同参数集。
  • 舒适性指标：纵向/横向Jerk的均方根（RMS）。
  • 安全性指标：与最近障碍物的最小距离（TTC， Time to Collision）。
  • 效率指标：平均速度、行程时间。
  • 稳定性指标：方向盘转角变化率、轨迹选择跳变频率。

4.2 数据驱动的调优流程

1. 基线测试：用一套默认参数在场景库中运行，记录各项指标作为基线。
2. 单参数敏感度分析：固定其他参数，系统性地改变一个参数（如KJ），观察指标变化。这能帮你理解每个参数的“影响力”。
3. 场景分组调优：将场景库按类型分组。为“城市拥堵”组找出一套最优参数，为“高速巡航”组找出另一套。记录下每组参数。
4. 设计场景识别器：在实车或仿真中，开发一个轻量级模块，实时判断当前属于哪种场景（如基于车速、道路曲率、周围车辆密度）。
5. 在线参数切换：根据场景识别器的输出，动态加载对应的最优参数集。
6. 回归测试与迭代：每次参数更新后，在全场景库中进行回归测试，确保新参数没有在其他场景中引起性能衰退。

4.3 实车测试的注意事项

仿真过关后，才能进行小范围实车测试。

• 从简单到复杂：先在封闭场地直线行驶测试，再到简单弯道，最后进入真实交通流。
• 记录所有中间数据：不仅记录最终执行的轨迹，还要记录所有候选轨迹及其代价。这为分析规划器的“决策过程”提供了宝贵数据。当你发现车辆做出了一个令人费解的选择时，回放这些数据，看看是不是因为某个代价项设置不合理，导致一条“看起来”更差的轨迹分数更低。
• 关注边缘案例：特意去测试那些“勉强能过”的场景。例如，刚好能通过的间隙，刚好能刹停的距离。观察在这些临界状态下，参数微调是否会导致截然不同的规划结果。

参数调优是自动驾驶算法工程化的深水区，它没有终点，只有持续的迭代和优化。最深刻的体会是，永远不要指望找到一套“放之四海而皆准”的完美参数。真正的能力，是构建一个能够根据场景、车辆状态甚至用户偏好，进行智能、平滑参数适配的系统。当你看到车辆在暴雨的高速上稳健巡航，在拥堵的环岛中流畅穿梭，你会觉得，那些与无数个参数和代价函数项搏斗的日夜，都是值得的。