3、Lattice Planner详解：Frenet坐标系、横向与纵向轨迹采样、代价函数设计、轨迹选择与发布

各位同学，今天我们来啃一块硬骨头——Lattice Planner。

说实话，这个算法在工业界用得非常多。我最早接触它是在2018年，当时我们团队要做一个高速场景下的换道决策。传统的DWA和A*在结构化道路上表现还行，但一遇到曲率变化大的弯道，轨迹就变得很“拧巴”。后来我翻到了MIT的一篇论文，才真正理解了Frenet坐标系的价值。

说白了，Lattice Planner的核心思想就一句话：把三维时空的规划问题，拆成两个一维问题来处理。一个管横向（左右偏移），一个管纵向（前后速度）。这样拆开之后，计算量小了很多，而且轨迹质量反而更高。

3.1 Frenet坐标系：为什么非它不可？

我们先聊聊坐标系。Cartesian坐标系（就是咱们常见的x-y坐标系）在直道上用着挺顺手，但一遇到弯道就麻烦了。你想想看，一条弯曲的道路，用x-y描述车道中心线，得用一堆多项式去拟合，而且横向偏移和纵向距离是耦合的。

Frenet坐标系就不一样了。它把道路中心线当作参考线，用两个变量来描述车辆状态：

• s：沿着参考线走的弧长（纵向）
• d：偏离参考线的距离（横向，左正右负）

这样一来，横向控制和纵向控制就解耦了。我在项目中遇到过一个问题：用Cartesian坐标系做换道规划，车辆在弯道里总是“画龙”。后来换成Frenet坐标系，同样的控制参数，轨迹平滑了很多。原因很简单——Frenet坐标系天然贴合道路几何。

纵向状态：[s, s_dot, s_ddot]  (位置、速度、加速度)
横向状态：[d, d_dot, d_ddot]  (位置、速度、加速度)

嗯，这里要注意一点：Frenet坐标系依赖参考线的质量。如果参考线本身不平滑，那后面生成的轨迹也会出问题。我建议在构建参考线时，至少做一次三次样条插值，保证曲率连续。

3.2 横向轨迹采样：怎么生成候选路径？

横向轨迹，说白了就是车辆怎么从当前车道变到目标车道。Lattice Planner的做法是：在终点状态空间里采样，然后反推轨迹。

具体来说，我们会在目标车道的横向位置d_target附近，采样一组终点状态：

• d_end：在目标车道中心线附近，比如 -0.5m, 0m, 0.5m
• d_dot_end：横向速度，通常设为0（平稳切入）
• d_ddot_end：横向加速度，通常设为0

然后，我们用五次多项式来拟合从当前状态到终点状态的轨迹。为什么是五次多项式？因为它能同时约束位置、速度、加速度——六个边界条件，刚好对应五次多项式的六个系数。

3.3 纵向轨迹采样：速度怎么规划？

纵向轨迹解决的是“什么时候到”的问题。我们需要考虑前方障碍物的运动状态、交通信号灯、限速等因素。

纵向采样的思路和横向类似：在终点时间T和终点速度v_end上采样。常见的采样策略包括：

• 巡航模式：保持当前速度，终点速度 = 当前速度
• 跟车模式：终点速度 = 前车速度，终点位置 = 前车位置 - 安全距离
• 停车模式：终点速度 = 0，终点位置 = 停止线

对于每种模式，我们会采样不同的T值（比如4秒、6秒、8秒），然后用四次或五次多项式生成纵向轨迹。

3.4 代价函数设计：怎么选出最优轨迹？

采样生成了几十上百条轨迹，怎么挑出最好的一条？这就需要设计代价函数了。

我一般把代价函数分成三部分：

实际项目中，权重需要根据场景动态调整。比如在高速上，时间代价的权重可以低一些，安全距离的权重高一些；在城区拥堵路段，舒适度的权重可以适当提高。

def cost_function(trajectory, obstacles):
    cost = 0.0
    # 横向偏移代价
    cost += w_lat * sum(trajectory.d_deviation ** 2)
    # 纵向加速度代价
    cost += w_acc * sum(trajectory.acceleration ** 2)
    # jerk代价
    cost += w_jerk * sum(trajectory.jerk ** 2)
    # 时间代价
    cost += w_time * trajectory.total_time
    # 障碍物距离代价
    min_dist = min_distance_to_obstacles(trajectory, obstacles)
    if min_dist < SAFE_DISTANCE:
        cost += w_obs * (SAFE_DISTANCE - min_dist) ** 2
    return cost

3.5 轨迹选择与发布：最后一步

计算完所有轨迹的代价后，我们选代价最小的那条。但这里有个坑：代价最小的轨迹不一定是最安全的。

我遇到过一种情况：有一条轨迹代价很低，因为它离障碍物很远，但它的横向加速度很大，车辆需要猛打方向。这种轨迹虽然“安全”，但会让乘客很不舒服。所以我在选择轨迹时，会加一个“可行性检查”：

• 最大横向加速度是否超过阈值（比如 2 m/s²）
• 最大纵向jerk是否超过阈值（比如 5 m/s³）
• 轨迹是否与障碍物发生碰撞（哪怕代价很低）

只有通过所有检查的轨迹，才会被发布给下游的控制模块。

3.6 知识体系总览

下面这张图总结了Lattice Planner的核心流程。你可以看到，从Frenet坐标系开始，到横向纵向采样，再到代价评估和轨迹发布，每一步都环环相扣。

好了，以上就是Lattice Planner的核心内容。从Frenet坐标系到轨迹发布，每一步都有它的设计哲学。我个人觉得，这个算法的精髓在于“解耦”——把复杂问题拆成简单问题，再组合起来。你在实际项目中遇到问题时，也可以试试这个思路。