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换一把尺子看世界平时我们用笛卡尔坐标系xy描述位置在全局地图上很直观。但在道路场景里笛卡尔坐标有一个尴尬的地方道路是弯曲的你很难直接从(xy)判断车离车道中心线偏了多少也很难判断车走了多长距离。Frenet坐标系换了一把尺子。它不直接用x、y而是定义在参考线上用s表示车辆沿着参考线方向行驶的弧长用d表示车辆偏离参考线的横向距离。在这个坐标系下任意一个点的位置就变成了一对(sd)s告诉你跑到哪了d告诉你偏了多少。这个转换带来的好处是颠覆性的。你在Frenet坐标系里看自动驾驶问题横向上需要关心的是“怎么从当前位置平滑地偏移到目标横向位置”纵向上需要关心的是“怎么沿着道路加速、减速、保持车距”。横纵两个问题解耦开了复杂度从二维联合规划降成了两个独立的一维问题。我第一次接触这个坐标系的时候脑子里最大障碍就是车明明走的是曲线怎么把s和d拆开算就对了后来看了一个类比才终于懂了想象把一条弯曲道路像拉链一样拉直拉直之后车的运动纵向就是沿拉链方向跑横向就是垂直于拉链方向左右移动。你分别规划纵向和横向最后再映射回弯曲的真实道路就是Lattice的做法。2.2 横纵向解耦把二维路径问题拆成两个一维问题解耦之后横向规划和纵向规划各干各的但最终要合成一条完整轨迹。这里需要搞清楚一件事横向和纵向不是完全独立的它们通过时间关联在一起。横向轨迹告诉你在某一时刻应该在哪条车道上纵向轨迹告诉你在这一时刻该走到哪个位置两者按时间轴对齐就可合成车辆在笛卡尔坐标系下的完整轨迹。具体到Apollo的实现里整个规划过程大概分这几步先把障碍物投影到Frenet坐标系把当前状态和采样目标点也转换到Frenet坐标系然后分别生成横向轨迹和纵向轨迹两两组合成完整轨迹再对每一条完整轨迹做碰撞检测和代价评估。这里我想多提一句为什么要费劲做这个解耦直接在笛卡尔坐标下同时规划横纵向不行吗表面上看当然可以但实际工程中道路的几何约束、车道边界、限速、障碍物位置全都和“沿路方向”以及“垂直道路方向”强相关。在Frenet坐标系下这些约束的表达会简化和规则化很多。你设一个横向避障目标只需要说“目标横向偏移1.5米”而不是“目标点坐标是(x3y-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图、候选轨迹、代价分数全部画出来看。我调试的速度有一半是可视化工具帮我提起来的。数据看不到问题就猜不出来问题猜不出来就只能瞎调参数浪费时间。让每条轨迹的“出生”和“死亡”原因都明明白白摊在眼前调起参来会顺手得多。 
长河君 · 2026/9/29 19:33:16 · 阅读 1.2k Lattice Planner算法解析：从Frenet坐标到Apollo工程实践
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换一把尺子看世界平时我们用笛卡尔坐标系xy描述位置在全局地图上很直观。但在道路场景里笛卡尔坐标有一个尴尬的地方道路是弯曲的你很难直接从(xy)判断车离车道中心线偏了多少也很难判断车走了多长距离。Frenet坐标系换了一把尺子。它不直接用x、y而是定义在参考线上用s表示车辆沿着参考线方向行驶的弧长用d表示车辆偏离参考线的横向距离。在这个坐标系下任意一个点的位置就变成了一对(sd)s告诉你跑到哪了d告诉你偏了多少。这个转换带来的好处是颠覆性的。你在Frenet坐标系里看自动驾驶问题横向上需要关心的是“怎么从当前位置平滑地偏移到目标横向位置”纵向上需要关心的是“怎么沿着道路加速、减速、保持车距”。横纵两个问题解耦开了复杂度从二维联合规划降成了两个独立的一维问题。我第一次接触这个坐标系的时候脑子里最大障碍就是车明明走的是曲线怎么把s和d拆开算就对了后来看了一个类比才终于懂了想象把一条弯曲道路像拉链一样拉直拉直之后车的运动纵向就是沿拉链方向跑横向就是垂直于拉链方向左右移动。你分别规划纵向和横向最后再映射回弯曲的真实道路就是Lattice的做法。2.2 横纵向解耦把二维路径问题拆成两个一维问题解耦之后横向规划和纵向规划各干各的但最终要合成一条完整轨迹。这里需要搞清楚一件事横向和纵向不是完全独立的它们通过时间关联在一起。横向轨迹告诉你在某一时刻应该在哪条车道上纵向轨迹告诉你在这一时刻该走到哪个位置两者按时间轴对齐就可合成车辆在笛卡尔坐标系下的完整轨迹。具体到Apollo的实现里整个规划过程大概分这几步先把障碍物投影到Frenet坐标系把当前状态和采样目标点也转换到Frenet坐标系然后分别生成横向轨迹和纵向轨迹两两组合成完整轨迹再对每一条完整轨迹做碰撞检测和代价评估。这里我想多提一句为什么要费劲做这个解耦直接在笛卡尔坐标下同时规划横纵向不行吗表面上看当然可以但实际工程中道路的几何约束、车道边界、限速、障碍物位置全都和“沿路方向”以及“垂直道路方向”强相关。在Frenet坐标系下这些约束的表达会简化和规则化很多。你设一个横向避障目标只需要说“目标横向偏移1.5米”而不是“目标点坐标是(x3y-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图、候选轨迹、代价分数全部画出来看。我调试的速度有一半是可视化工具帮我提起来的。数据看不到问题就猜不出来问题猜不出来就只能瞎调参数浪费时间。让每条轨迹的“出生”和“死亡”原因都明明白白摊在眼前调起参来会顺手得多。