Lattice Planner在自动驾驶圈子里的热度一直不低，尤其在做Apollo相关项目或参加智能车竞赛时，它几乎是必绕不开的规划算法。很多新手一上来就啃Apollo源码，被里面的Frenet坐标、多项式拟合、ST图、代价函数搞得晕头转向，最后只能对着代码发呆。这篇文章我想把Lattice Planner的完整逻辑拆开揉碎讲清楚，不管你是刚接触轨迹规划的学生，还是已经在工程里踩过坑的开发者，都能从里面找到自己需要的东西。
我会从算法思想、数学原理、实操流程、源码结构到调参经验一条线捋出来。我尽量用大白话解释那些看似高深的概念，但凡能用类比说清楚的地方，绝不放公式吓人。同时也把我在实际工程里踩过的坑、排查过的问题一并写出来，这部分是文档里找不到的，能帮你少走很多弯路。
1. Lattice Planner是什么——先把问题域框清楚
很多入门资料一上来就抛Frenet公式、多项式求解，结果读者连Lattice Planner到底在解决什么问题都没搞明白。所以在深入算法之前，我觉得有必要先把规划模块的家底捋一遍。
1.1 规划分层架构里它处于哪一环
自动驾驶的软件链路大体可以从上到下分成几层：感知层负责看路，定位层负责找自己，决策规划层负责决定怎么走，控制层负责执行转向和加减速。Lattice Planner就在决策规划层里面。
决策规划层内部一般还会继续分。最上游是全局路径规划，它管的是从A点到B点的宏观路线，输出通常是一条静态的参考线，不考虑路上的动态车辆和障碍。到了局部规划这一级，算法才真正开始忙活：它要在参考线附近，结合当前车辆状态、周围动态障碍物、交通规则和道路边界，实时生成一条未来几秒内可执行的平滑轨迹。
Lattice Planner就是局部规划里的一个经典方案，它的输入是车当前的位置、速度、朝向，再加上参考线、周围障碍物的预测轨迹，输出是一条满足动力学约束、安全无碰撞且能让乘客坐得舒服的轨迹。你可以把它理解成：全局导航已经告诉你“下一个路口右转”，Lattice Planner负责算出“接下来3秒钟方向盘怎么打、油门怎么踩、踩多深”。
1.2 Lattice在轨迹规划流派里的独特位置
轨迹规划算法大致有几条路线。早期用得比较多的是基于采样的方法，比如RRT系列，靠随机撒点找可行路径，好处是能处理高维空间，坏处是轨迹质量随机性大，而且通常不考虑时间维度，不太适合高速场景。还有基于优化的方法，比如Apollo后来推出的基于二次规划或非线性规划的分段多项式轨迹优化，把轨迹生成问题转成带约束的优化问题，质量很高，但对算力和初始解比较敏感。
Lattice Planner走的是另一条路：采样加评估。它不做反复的随机探索，而是在一个经过坐标系变换的空间里，按照固定的模式生成一批候选轨迹，然后用代价函数给候选轨迹排序，挑一条最好的。这种处事方式非常像考试前把所有题型都刷一遍：我不确定哪道题会考，但我把所有可能性都准备一遍，最后挑一个最稳的答案。
Lattice本身的英文原意是“晶格、点阵”，引申到轨迹规划里就是“在状态空间里撒出一片有规律的点阵，再基于点阵构造候选轨迹”。它最大的优势是结构简单、逻辑直观、轨迹可变性强，而且每个候选轨迹都是显式的，调试时可以直接可视化看到算法在“想什么”。这也是为什么很多教学和竞赛项目都拿它当入门算法——不是因为它最简单，而是因为它最好理解、最能讲清楚。
1.3 为什么从Apollo入手学Lattice最合适
肯定有人会问，市面上轨迹规划算法那么多，为什么要专门跟Apollo死磕？我个人的看法是：Apollo的代码质量在工业级自动驾驶开源项目里确实属于第一梯队，尤其是Lattice Planner的实现非常完整，它把工程里该考虑的模块都考虑到了——坐标系转换、采样、碰撞检测、代价评估、预测接口、参数配置，一应俱全。
更重要的是，Apollo把Lattice Planner完整地嵌入到了整个planning模块里，你可以在仿真环境里看到它和其他模块的交互。这一点是非常有价值的：很多算法书拿一个孤立函数让你跑，但Apollo让你看到真实系统里算法是怎么运转的，上下游的数据是怎么流的。理解了Apollo里的Lattice，你就理解了工业级规划系统的基本骨架，以后再去看其他家的实现，底子就扎实了。
2. Frenet坐标系与横纵向解耦——Lattice的核心思想
Lattice Planner最核心的思想，不是采样，也不是代价函数，而是“横纵向解耦”。而要让这种解耦成立，背后的数学工具就是Frenet坐标系。
2.1 从笛卡尔到Frenet：换一把尺子看世界
平时我们用笛卡尔坐标系（x，y）描述位置，在全局地图上很直观。但在道路场景里，笛卡尔坐标有一个尴尬的地方：道路是弯曲的，你很难直接从(x，y)判断车离车道中心线偏了多少，也很难判断车走了多长距离。
Frenet坐标系换了一把尺子。它不直接用x、y，而是定义在参考线上：用s表示车辆沿着参考线方向行驶的弧长，用d表示车辆偏离参考线的横向距离。在这个坐标系下，任意一个点的位置就变成了一对(s，d)，s告诉你跑到哪了，d告诉你偏了多少。
这个转换带来的好处是颠覆性的。你在Frenet坐标系里看自动驾驶问题，横向上需要关心的是“怎么从当前位置平滑地偏移到目标横向位置”，纵向上需要关心的是“怎么沿着道路加速、减速、保持车距”。横纵两个问题解耦开了，复杂度从二维联合规划降成了两个独立的一维问题。
我第一次接触这个坐标系的时候，脑子里最大障碍就是：车明明走的是曲线，怎么把s和d拆开算就对了？后来看了一个类比，才终于懂了：想象把一条弯曲道路像拉链一样拉直，拉直之后车的运动，纵向就是沿拉链方向跑，横向就是垂直于拉链方向左右移动。你分别规划纵向和横向，最后再映射回弯曲的真实道路，就是Lattice的做法。
2.2 横纵向解耦：把二维路径问题拆成两个一维问题
解耦之后，横向规划和纵向规划各干各的，但最终要合成一条完整轨迹。这里需要搞清楚一件事：横向和纵向不是完全独立的，它们通过时间关联在一起。横向轨迹告诉你在某一时刻应该在哪条车道上，纵向轨迹告诉你在这一时刻该走到哪个位置，两者按时间轴对齐，就可合成车辆在笛卡尔坐标系下的完整轨迹。
具体到Apollo的实现里，整个规划过程大概分这几步：先把障碍物投影到Frenet坐标系，把当前状态和采样目标点也转换到Frenet坐标系，然后分别生成横向轨迹和纵向轨迹，两两组合成完整轨迹，再对每一条完整轨迹做碰撞检测和代价评估。
这里我想多提一句，为什么要费劲做这个解耦，直接在笛卡尔坐标下同时规划横纵向不行吗？表面上看当然可以，但实际工程中，道路的几何约束、车道边界、限速、障碍物位置全都和“沿路方向”以及“垂直道路方向”强相关。在Frenet坐标系下，这些约束的表达会简化和规则化很多。你设一个横向避障目标，只需要说“目标横向偏移1.5米”，而不是“目标点坐标是(x+3，y-2)”。这种标准化对工程开发和调试有巨大价值。
2.3 轨迹生成的多项式表达：五次多项式为什么够用
解耦完成后，下一步就是怎么在横纵两个方向上生成一条具体轨迹。在Lattice算法里，横向轨迹和纵向轨迹都是用多项式函数来表达的，横向用五次多项式，纵向用四次多项式。这个选择背后是有讲究的。
先看横向。横向轨迹描述了d随时间的变化：d(t)。我们需要边界条件，也就是起点的横向位置、横向速度和横向加速度，以及终点的横向位置、横向速度和横向加速度。从起点到终点，每个状态需要3个约束，一共6个约束，恰好对应五次多项式的6个系数。这就是为什么横向要用五次多项式。
纵向轨迹同理。纵向轨迹描述s随时间的变化：s(t)。工程实践中纵向目标通常只约束位置、速度和加速度三项，纵深方向速度是采样得到的，比如你决定终点速度是某某值，那么起点有3个约束，终点也有3个约束，加起来也是6个约束，但Apollo里纵向常用四次多项式，是因为它把目标加速度做了简化处理，或者干脆不约束终点加速度，这样5个系数对应5个约束就够了。
用多项式的好处也很直观：多项式天然光滑，它的导数（速度）和二阶导（加速度）也是连续的，不会出现速度或加速度突变的情况。你想想，如果轨迹上加速度有跳变，那乘客体验就是被猛地推一下或拽一下，这在自动驾驶中是不能接受的。五次多项式足够简单，又能满足平滑性要求，所以成了Lattice这类采样算法的默认选择。
3. 采样、评估与选择——候选轨迹的完整生命周期
Lattice Planner的流程如果浓缩成一句话，就是：采样一批轨迹，给它们打分，选一条最好的，发出去。这句话听起来简单，但每一步展开都有很多细节和工程考量。
3.1 采样策略：生成一批“说得过去”的轨迹
在Frenet坐标系下，横向轨迹采样围绕“横向终点状态”展开，纵向轨迹采样围绕“纵向终点状态”展开。采样其实就是设定一组候选目标状态，然后根据当前状态和目标状态，反解多项式系数。
举个例子，横向采样时，算法会生成一组目标横向偏移量。比如当前车在车道中心线左边0.5米，采样目标可能是：维持当前偏移不变、回到车道中心、偏移到隔壁车道中心、偏移到隔壁车道左侧/右侧边界等。每个目标加上一组横向速度（通常归零），就构成一个横向目标状态。
纵向采样则更多样。一种是定距离采样，也就是在接下来的某段弧长距离上，设定车辆要达到的目标速度；另一种是定时间采样，比如在3秒、5秒、8秒后，目标速度和当前速度的关系如何。Apollo里常见的做法是：把纵向目标拆成“保持巡航”、“匀速跟车”、“减速停车”几种语义模式，每种模式对应一组目标状态。
当你有了N个横向候选状态和M个纵向候选状态，把它们两两组合，再和当前车辆状态一起代入多项式求解，就能得到N乘以M条候选轨迹。这一批轨迹覆盖了“往前走、往左并、往右并、加速、减速、停车”等各种可能性。如果采样的密度够高，最终选出的轨迹大概率就是当前场景下最优的解。
采样密度是一个需要权衡的参数。采样间隔太大会漏掉关键轨迹，比如该避障的时候没有合适的偏移量可选；采样间隔太小会导致候选轨迹数量爆炸，实时性崩掉。实际工程里，往往不是单纯加密采样，而是根据场景自适应调整，比如检测到前方有障碍物，就在障碍物附近多采一些横向偏移。
3.2 代价函数设计：怎么给轨迹打分
有了候选轨迹，下一步就是评估。评估的最终手段是一个代价函数，它把轨迹的各种“好坏”指标量化成一个数字，数字越小代表轨迹越优。这里的好与坏，要从安全性、舒适性、效率、规则遵从多个维度来定义。
Apollo里Lattice的代价函数主要包括这么几类：
第一类是贴合参考线的代价。车辆偏离参考线太远，一方面容易压线，另一方面也对驾驶习惯造成冲击，所以越贴近参考线代价越低。这个代价通常基于横向偏移量d的平方来计算。
第二类是纵向速度代价。轨迹的终点速度如果和期望速度（比如道路限速、巡航设定速度）差距越大，代价越高。这保证算法不会选一条慢悠悠开或者超速的轨迹。
第三类是加速度代价和加加速度代价。加加速度就是加速度的变化率，它直接影响乘客舒适度。代价函数会惩罚过大的加速度值和加加速度值，尤其会在起步、刹停这些场景里起作用。
第四类是碰撞风险代价。即使轨迹没有直接撞上障碍物，如果距离障碍物太近，也会被加惩罚项。这个设计很巧妙，它让算法在条件允许时主动远离障碍物，而不是贴着边儿擦过去。
每一类代价都会设置权重系数。这些权重是调参的核心对象。权重设得不同，车辆行为风格就完全不同——如果参考线代价权重很高，车辆会非常“粘”车道线，变道犹豫；如果速度代价权重高，车辆会更激进地加速追赶目标车速；如果舒适性代价权重高，车辆会变得特别“肉”，起步刹车都慢吞吞的。调这些权重没有一个万能答案，完全看车辆的产品定位和场景需求。
3.3 碰撞检测与约束检查：不合格直接淘汰
代价函数负责给候选轨迹排名，但排名之前，算法要先做一轮硬性筛查，把不满足安全约束的轨迹直接淘汰。这是工程实现的底线逻辑，代价再小，撞车了也不能用。
碰撞检测的基本思路是：把车辆沿轨迹展开成一个时间序列的矩形框，每个时刻取车辆在轨迹上的位姿，和障碍物的预测位置比对，判断矩形是否相交。Apollo里这一块是基于边界框的几何检测实现的，同时会考虑一个安全距离的膨胀系数，保证车辆不是零距离贴边通过。
除了碰撞，还要检查轨迹是否超出道路边界、是否越过可行驶区域。这部分依赖高精地图或道路边界信息。如果轨迹上一小段冲出道路边界，直接就淘汰。
还有一个关键约束是动力学可行性。轨迹不仅要几何上无碰撞，还要让车能真正跟得上。横向加速度过大、纵向减速度超出轮胎附着极限、速度方向变化太剧烈，这些都会违反车辆动力学约束。Apollo里会检查轨迹的曲率和加速度是否超出车辆允许范围，超出就淘汰。
4. 动态场景下的ST图与SL图——Lattice如何应对周围车辆
如果路上一辆车都没有，Lattice采样的任务会轻松很多——选一条平滑、快速、靠车道中心的轨迹就完事了。但现实道路是动态的，周围车辆会加速、减速、变道，算法必须有能力感知并应对这些动态变化。这里就要引出ST图和SL图这两个工具。
4.1 ST图是怎么表示动态障碍物的
ST图是一个以时间为横轴、弧长s为纵轴的二维图。在这个图里，每个动态障碍物被描述成一片禁止进入的区域。
举个例子，前方同一车道有一辆慢车，预测它在未来5秒会保持30km/h匀速行驶。在ST图里，它会形成一条向上倾斜的斜杠区域，因为这个障碍物的s坐标随时间不断增大。如果本车目前速度更快，跟它的相对距离会越来越近，那么在ST图上，这个障碍物的区域就威胁到本车期望的s(t)轨迹。Lattice的纵向采样就要避开这片区域——要么选择让车降速跟随，让s(t)曲线贴着障碍物区域下沿走；要么选择提前变道绕过去，但这要横向规划配合。
ST图的厉害之处在于，它把“动态避障”这个看起来很复杂的问题，变成了“在二维图里找一条不穿过禁区的路径”这个直观问题。纵向轨迹的多项式曲线画到ST图上，如果曲线某一时刻穿过了某个障碍物的区域，那这条轨迹在时间维度上就是有碰撞风险的，直接淘汰。
Apollo里有一个专门的类来处理这个映射关系，叫STBoundaryMapper，它负责把预测模块输出的障碍物轨迹转换成ST图中的边界区域。工程实现上，障碍物会被离散成多个采样点，每个采样点对应一个时间和一个s位置，再把这些点连成边界。这个过程看似简单，但障碍物的预测不确定性、采样分辨率、时间窗口长度都会直接影响边界质量。
4.2 SL图在静态避障里的作用
ST图处理动态障碍物，SL图则是处理静态障碍物和道路边界的主力工具。SL图的横轴是弧长s，纵轴是横向偏移d，相当于把整条参考线拉直摊平，形成一个俯视图，所有静态障碍物都投影到这个图上。
对于静态障碍物，在SL图上看到的是一片固定的禁止区域。本车轨迹的横向偏移函数d(s)如果和这片区域相交，说明会撞上障碍物。SL图天然适合做横向避障规划：算法可以在SL图上看到障碍物占据的横向范围，然后选择合适的横向偏移区间去绕开它，比如向左偏移1米通过，或者向右偏移2米绕行。
Lattice在生成候选轨迹时，横向采样目标很大程度上就是依据SL图来设定的。如果下一个弧长区间内出现了一个静态障碍物，算法会让横向采样点分布在障碍物两侧，并对无法顺利通过障碍物的横向目标直接淘汰。
这里有一个工程细节值得提：SL图里的障碍物边界需要提供一定的安全余量，不能刚好卡着障碍物的边缘。Apollo的SL边界生成会对障碍物的外形做膨胀处理，这样即使车辆控制有微小偏差，也不会真的剐蹭到障碍物。这个膨胀系数需要结合车辆宽度和定位精度来标定，定得太小有风险，定得太大又会让车辆绕行幅度过大，影响通行效率。
4.3 实际调度策略：从候选轨迹里挑一条“最优且可执行”的
理论上讲，Lattice的流程是：横向和纵向分别采样，组合成候选轨迹，ST图和SL图参与碰撞检查，代价函数给所有通过检查的轨迹排序，选代价最小的输出。但实际工程里，还有几个额外的调度策略要处理。
第一个是参考线切换的衔接问题。在Apollo里，当车辆需要变道时，参考线会从当前车道平滑切换到目标车道。Lattice Planner在参考线切换过程中，如果横向采样目标设置不当，很容易产生来回蛇形摆动。所以工程上一般会设置一个变道意图状态，只有当决策层明确输出了变道指令后，纵向采样才会在目标车道方向增加对应的横向偏移采样。
第二个是轨迹的平滑后处理。Lattice生成的轨迹，多项式本身是平滑的，但两段轨迹在切换点（比如每一轮规划的起点拼接处）可能有不连续的风险。Apollo在输出最终轨迹前，会做一轮平滑或至少检查起点状态是否和上一帧一致，如果起点状态都接不上，控制层执行起来会非常不舒服。
第三是重规划时机。自动驾驶规划不是只做一次，而是以10Hz甚至更高的频率持续运行。Lattice每轮重新采样、重新评估、重新选优，输出给控制层的轨迹只覆盖未来几秒。这种周期性重规划的好处是能及时应对环境变化，但也带来一个问题：如果每轮选出的最优轨迹变化太剧烈，车辆动作就会不连贯。所以Apollo在代价比较和选优时，往往会偏向选择和上一帧轨迹更接近的候选，避免输出突变。这种“时间一致性”的考量，是采样类算法在工程落地时不可或缺的一环。
5. Apollo Lattice Planner的代码结构与工程落地要点
如果你已经读完前面的原理部分，再去看代码会轻松很多。Apollo的Lattice Planner源码主要集中在modules/planning/lattice目录下，我看过不少开源轨迹规划代码，Apollo这套组织方式比较清晰，值得花时间好好读。
5.1 源码目录与核心类梳理
打开modules/planning/lattice，你首先会看到几个关键目录和文件。其中lattice_planner.h和lattice_planner.cpp是整个插件的入口，它继承自Planner类，实现Plan方法。这个Plan方法就是外部调度器调用局部规划器的统一接口。
再往下一层，你会发现几个核心功能模块。trajectory_generation目录负责生成候选轨迹，里面包含横向轨迹、纵向轨迹以及轨迹1D类的定义。横向轨迹主要用了PolynomialX1d这类多项式类，纵向轨迹同理，只不过次数不同。
prediction_querier目录是查询预测信息的接口。它负责把预测模块给出的障碍物轨迹投影到参考线上，并输出ST边界。这个过程本身不复杂，但它做了一件事很关键——把障碍物信息和Frenet坐标系建立了绑定关系，后续所有碰撞检测和代价评估都用这个统一坐标下的数据。
behavior目录里是行为状态相关的逻辑，比如判断当前是跟车状态、停车状态还是变道状态。Lattice不是孤立工作的，它需要知道决策层给它下发的目标——是跟上前车，还是保持车道，还是准备变道。behavior目录就是用来解析这些语义信息，并反馈给采样策略的。
最值得佩服的是，Apollo把采样逻辑也做成了相对独立的模块。你可以在代码里清楚看到一个循环：把所有可能的横向目标状态和纵向目标状态两两配对，逐个求解多项式、生成轨迹、做碰撞检测、计算代价。整个思路和我们在前几章讲的一模一样，代码即文档，这一点对于学习算法的人来说非常友好。
5.2 关键参数与调参经验
Lattice Planner的行为风格很大程度由参数决定。在Apollo的配置文件里，你可以找到lattice_planner的配置项。这里面有几个参数对性能影响特别大，我逐个说一下。
第一个是采样时间窗口，它决定未来多长时间被规划覆盖。时间窗口太短，车辆只能看到眼前一点距离，遇到突发情况反应空间不够；时间窗口太长，计算量增大，而且远期的预测可信度低，候选轨迹意义不大。Apollo默认一般设置在6到8秒左右，具体看车速和场景复杂度。
第二个是纵向采样间隔和横向采样间隔。横向采样间隔决定了变道避障时车道内的偏移分辨率，纵向采样间隔决定了速度序列的空间分辨率。我之前在仿真里遇到过一个情况：横向采样间隔设得太大，导致车辆在绕行静态障碍物时，总是找不到合适的偏移量，要么偏太多压线，要么偏太少剐蹭，后来把间隔缩小到0.2米左右，问题就解决了。
第三个是代价权重系数。你可以在配置里看到避障代价、舒适性代价、效率代价等各自的权重。调试这些权重最有效的方式是在sim_control里反复跑同一个场景，观察轨迹曲线的形状，然后调整权重看行为差异。比如你希望车辆更早提速通过路口，可以把纵向效率代价的权重调大；如果希望车辆更稳健地跟车，可以把舒适性和碰撞风险的权重调大。
调参这条路上，我的核心建议是：一次只调一个参数，并且每次修改都要记录效果。轨迹规划的系统耦合性很强，两个参数经常互相影响，如果一次改好几处，出了问题根本定位不到是哪个参数引起的。我在实际项目里习惯做一张表格，记录每次参数修改的前后行为差异，积累到一定量之后，你会对每个参数的作用形成很强的直觉。
5.3 工程坑位记录：标定、坐标系、离散化带来的问题
读完代码、调完参数，你还得在工程里应对各种意想不到的问题。我把我在实际使用中遇到过的坑整理了一下，希望能帮你避开。
第一个坑是坐标系转换精度问题。Frenet坐标和笛卡尔坐标的转换依赖参考线，而参考线本身是高精地图坐标下的离散点序列。如果地图点密度不够，参考线的曲率计算波动会很大，导致横向位移d计算出现抖动，最终轨迹也会跟着抖。解决办法是在预处理阶段对参考线做平滑或插值，保证曲率连续性。
第二个坑是横向加速度的计算误差。Lattice在评估轨迹动力学可行性时，需要根据轨迹曲率计算横向加速度。这个值对参考线质量非常敏感，参考线上一个微小的噪声放大到横向加速度上，可能就触发动力学约束的误判，导致车辆在明明是安全的情况下被强制减速。遇到这种问题，别急着调参，先检查参考线质量。
第三个坑是时间离散化带来的碰撞检测盲区。碰撞检测是按时间步长逐帧检测的，如果步长太大，高速运动场景下有可能跳过碰撞点，出现“轨迹看着安全其实撞了”的情况。Apollo里一般会把步长取得足够小，但太小会拖慢计算速度。你需要根据最高车速和障碍物相对速度，算一下最极端情况下能否保证检测精度。
还有一个经常被忽略的问题：障碍物的预测轨迹不准确。Lattice依赖ST图来避让动态障碍物，而ST边界来自预测模块。如果预测给出的轨迹偏乐观，ST边界可能画得太窄，导致候选轨迹贴着真实障碍物边缘通过，非常危险。工程上通常会给ST边界额外加一层安全余量，宁可略微保守，也要保证安全。
6. 常见问题与排查技巧实录
这一节我想换个方式，不做理论讲解，而是分享一些实打实的排查经验。这些案例都是我在仿真和实车调试中真实遇到过的，虽然具体环境不同，但排查思路是通用的。
6.1 跟车场景抖动排查
有段时间我在仿真里跑Lattice，发现车辆在跟随前车时，速度曲线一直抖动，油门忽大忽小，体感非常差。第一反应是代价权重调的问题，但我反复调了舒适性权重，抖动还是存在。
后来我拉出ST图一看，发现问题出在纵向采样上：前车速度稍微波动，ST边界就跟着变，Lattice每一轮重新规划时，为了避开新边界，选出的轨迹终点速度也在频繁跳变。换句话说，不是轨迹质量差，而是每轮选出的最优轨迹变化太快，导致控制层一直在追赶新目标。
解决办法是在代价函数中增加上一帧轨迹的相似度惩罚，或者对纵向目标速度做低通滤波，让目标速度变化更平缓。这一招效果立竿见影，抖动基本消除。
6.2 换道失败从头到尾检查一遍
有一次车辆在高速场景下需要向左变道超车，但Lattice迟迟不输出变道轨迹，一直保持在本车道跟车。我先看决策层，确认变道意图已经下发；再看参考线，确认目标车道参考线已经切换；然后看SL图，确认目标车道没有静态障碍物占位。
最后发现，问题出在横向采样目标上。因为车速较快，横向采样的目标横向速度设成了0，理论上没有问题。但如果采样目标里没有覆盖目标车道中心线对应的横向偏移量，算法就只能选“维持当前车道”的轨迹。加上变道目标的横向采样点之后，变道就自然发生了。
这个小问题让我印象很深，因为排查链条特别典型：决策没问题、环境没问题、参考线没问题，最后发现只是采样空间设计不完整。采样类的算法，候选集合的完备性太重要了——如果你的采样空间里根本没有那条该走的路，代价函数再合理也选不出来。
6.3 参数调优的实操路子
最后聊一聊调参的整体方法。很多人一上来就动代价权重，其实是不太推荐的做法。我自己的习惯是“先安全后舒适再效率”的顺序。
第一阶段先把碰撞检测、道路边界、动力学约束这些硬性逻辑检查好，保证轨迹不会撞车、不会出界、车能跑出来。这一阶段不追求最优，只求不出安全问题。
第二阶段调舒适性。重点看加速度、加加速度曲线是否平滑，横向偏移变化是否均匀。如果车辆动作太突兀，先回到轨迹生成环节，看看是不是采样间隔太大导致候选轨迹太少，而不是急着改权重。
第三阶段才调效率。在安全和舒适都有保障的前提下，通过调整速度代价权重、采样时间窗口等参数，让车辆更高效地到达目的地。
这套顺序的核心思想是：先把“能不能用”的问题解决掉，再谈“好不好用”。因为效率指标最容易被看到，也最容易被过度优化，但安全和舒适层面的问题，一旦形成习惯性行为，后面要改回来会非常痛苦。
写在最后
我在把Apollo Lattice Planner彻底吃透之后，最大的感受是：这个算法之所以经典，不是因为它的每一步都最高级，而是因为它在“可解释性”和“工程可落地性”之间找到了一个极好的平衡点。你可以在白板上跟人讲清楚它的每一个环节，也可以直接在代码里看到它的每一个细节。对于想进入自动驾驶规划领域的人来说，它是一块非常扎实的跳板。
学到后面你会发现，无论是Apollo后来推出的基于优化的规划方案，还是其他厂商自研的规划器，很多核心思路都能追溯到Lattice时代的积累：坐标系的选取、目标的解耦、安全约束的建模、代价函数的加权。这些思想不会过时。
最后再分享一个小技巧：调试Lattice时一定要多用可视化工具，把ST图、SL图、候选轨迹、代价分数全部画出来看。我调试的速度，有一半是可视化工具帮我提起来的。数据看不到，问题就猜不出来；问题猜不出来，就只能瞎调参数，浪费时间。让每条轨迹的“出生”和“死亡”原因都明明白白摊在眼前，调起参来会顺手得多。
