1. Lattice Planner是什么为什么项目里需要它1.1 一个经典场景看懂Lattice Planner在解决什么问题先说一个我实际跟车调试时经常遇到的场景高速巡航限速120本车100跟在慢车后面右侧车道空着。正常人会怎么开先看一眼右后视镜确认没车然后打灯、稳着方向盘、踩一脚油门平滑地并过去。整个过程大概持续3到5秒横向位移3米多一点纵向速度变化不大。这个行为交给自动驾驶规划算法其实就是Lattice Planner最典型的用武之地。Lattice Planner这个名字直译是“晶格/网格规划器”在自动驾驶规划领域通常被翻译为“横纵向解耦采样规划算法”。它的核心思路一句话就能讲明白把车辆在道路上的运动拆成“沿路走了多远”和“偏离路中心多少”两个独立维度分别在两个维度上采样生成一堆候选轨迹然后设计一个代价函数把每条轨迹的安全、舒适、效率量化成分数最后挑分数最低的那条执行。听起来很简单但实际工程里想把它做稳、做准、做出合规的驾驶风格里面有不少细节值得写下来。这篇文章主要面向两类读者。一类是刚接触自动驾驶规划控制算法的学生或初级工程师想搞清楚Lattice Planner的原理和代码结构另一类是已经在做规划模块落地、要处理实车问题的人想看看别人在参数标定、代价函数设计、问题排查上踩过哪些坑。我会尽量按“原理拆解—实操流程—踩坑实录”的顺序来讲能直接参考复现的东西都放在前面。1.2 这套方案在项目里有哪些取舍我不止一次在技术讨论群里看到有人问现在都上深度学习、端到端了Lattice Planner是不是过时了说实话任何工具都有它的适用边界Lattice Planner最大的优点是它把“横纵解耦”和“采样评价”这两个思想做到了极致在结构化道路场景下非常稳定、可解释、易调试。这恰恰是量产项目最看重的三个特性。它和当前主流的其他方案相比各有各的赛道方案核心思路适用场景常见问题Lattice PlannerFrenet坐标系下横纵向解耦采样代价函数选优高速、城市结构化道路依赖采样密度非全局最优EM Planner横纵向交替优化基于动态规划二次规划复杂城市道路、弯道多实现复杂迭代容易振荡直接MPC统一优化在预测时域内把路径和速度作为一个优化问题求解终点状态明确、约束复杂求解慢标定难度大图搜索轨迹优化全局搜索出粗略路径再局部轨迹优化平滑开放区域、泊车、越野全局与局部衔接容易断裂一句话总结我的取舍经验如果你的场景是车道线清晰、道路结构相对固定的公路上Lattice Planner是性价比很高的一种选择。它不像MPC那样求解器参数一大堆也不像EM Planner那样横纵向要反复迭代代价函数调整后逻辑一目了然测试人员和老板问你“为什么这帧轨迹选了这条”你能直接对着采样图片把理由说清楚。想快速把规控系统跑通、后续稳定迭代它够用且够稳。2. 核心原理拆解从Frenet坐标系到轨迹生成2.1 Frenet坐标系为什么是Lattice的地基Lattice Planner的一切都建立在Frenet坐标系之上。这里先花点篇幅把坐标系讲透因为很多代码跑飞、轨迹奇形怪状的问题根子都在坐标转换上。Frenet坐标系用一对正交轴描述车辆在道路中的位置纵向距离s表示车辆沿参考线方向行驶了多少米横向距离d表示车辆偏离参考线的距离向左一般为正、向右为负。参考线可以是车道中心线、道路中心线也可以是高精地图给出的一条全局路线投影。笛卡尔坐标系下的一个点(x, y)要先投影到参考线上找到距离它最近的那个参考点该点对应的弧长就是s加上垂直于参考线方向的偏移就是d。为什么非得在Frenet坐标系里做规划举个反例你就明白了在笛卡尔坐标系下一条笔直变道轨迹和一条弯道中变道轨迹的x、y表达式差别巨大横纵两个方向完全耦合在一起。一旦把坐标换到Frenet坐标系车辆的换道行为在d方向上就是从0平滑过渡到3.5米纵向s方向依然保持正常跟车行驶横纵向问题瞬间解耦各自单独规划再合成算法复杂度大幅下降。这里有一个工程细节需要重点注意投影不是简单求一下最近点就完事了。参考线的长度和采样密度要足够高否则车辆运动时s和d会发生跳变参考线如果局部曲率突变投影出来的d方向定义会跟着扭曲直接导致后面代价函数和碰撞检测失效。我见过实车日志里定位稍微抖一下Frenet投影结果来回跳最终输出给控制的方向盘转角也跟着抖。后面在排查章节我会专门展开。2.2 横向规划五次多项式让变道平顺横向维度的规划目标是生成一条从当前横向状态到目标横向状态的光滑曲线。这里的“状态”包含三个量横向位移d、横向速度d对时间的一阶导、横向加速度d对时间的二阶导。常用的做法是用五次多项式拟合横向位移随时间的变化d(t) a0 a1·t a2·t² a3·t³ a4·t⁴ a5·t⁵为什么偏偏是五次因为五次多项式包含6个未知系数刚好匹配6个边界条件起点位置、起点速度、起点加速度、终点位置、终点速度、终点加速度。六条约束解六个未知数解唯一且能保证首尾两端的位置、速度、加速度都连续。如果只用三次多项式只能约束到起点终点的一阶导端到端加速度会跳变控制模块跟着就会产生冲击感乘客能明显feel到“咯噔”一下。七次多项式当然也能用但边界条件个数不变时反而会出现多余自由度没有实际收益还多一份计算量。在实际工程里横向终点状态可以分两种情况处理。如果终点横向位置固定比如换道到邻车道中心线那d(T)是明确的目标直接把六个边界条件扔进去算系数。另一种是巡航场景下目标横向位置不固定比如车道内微调这时候如果把d(T)定死在某个值轨迹会显得很僵硬更合理的做法是对终点横向位置也做采样或者让d(T)成为一个受代价惩罚的量让算法自己平衡“偏离中心的代价”和“平滑换道的收益”。2.3 纵向规划跟车、巡航、停车三种状态怎么采样纵向维度规划的核心是生成s(t)曲线也就是车辆沿道路走了多远、走得多快。虽然也是多项式拟合但纵向比横向要复杂一些因为纵向必须对接实际的驾驶行为前方有车就跟着没车就巡航遇红灯就要停。先说巡航场景。目标就是维持某个期望速度v_ref终端位置可以表示为s(T) s0 v_ref·T 0.5·a·T²。这里的采样变量一般是T完成这段纵向运动的时间和v_ref期望巡航速度。T采样范围我常用4到8秒v_ref则按当前限速和道路曲率来定。跟车场景比巡航麻烦。纵向终端位置不能自己拍脑袋得看前车在T时刻会跑到哪里。这里牵扯到对前车运动状态的预测最简单的模型是假设前车保持当前速度和加速度不变然后推算T秒后它的位置。预测不准是跟车规划里最常见的故障源前车观测带一点噪声目标s(T)跟着抖动生成的轨迹就会忽快忽慢。我的习惯是在进规划器之前先给前车的状态估计加一个低通滤波把噪声抖动的尖峰削掉。停车场景相对简单终端状态s(T)是停车点位置终端速度必须设置为0终端加速度一般也设为0这样才能保证刹停那一刻乘客不会往前栽。但这里藏着一个容易被经验不足的工程师忽略的问题如果起始车速不低、T又选得不够长五次多项式会硬生生拉出一条加速度很大的急刹曲线舒适性直接崩掉。处理办法是在代价函数里对纵向加速度做惩罚同时前处理时检测最小停车距离必要时主动放宽容许停车位置的范围让轨迹可以提前一点刹住。纵向多项式我一般用四次或五次。四次多项式可以约束位置、速度、加速度的起点状态和位置、速度的终点状态适合跟车五次多项式进一步约束终点加速度适合停车这类对终端状态要求严格的场景。2.4 轨迹合成从s(t)、d(t)还原成可执行的路径横向轨迹d(t)和纵向轨迹s(t)分别生成之后并没有直接结束。控制器最终要执行的是笛卡尔坐标系下的一系列路径点每个点包含x、y、朝向角θ、曲率κ、速度v、加速度a。所以需要把Frenet坐标系下的s(t)、d(t)合成回笛卡尔坐标。这个合成过程的公式推导在各类教材里都有我在这里只点几个工程上容易出问题的位置。第一步对每一个时间戳t由s(t)找到参考线上对应的点r(s)这一步要求在参考线上能做高效的插值查找第二步根据d(t)和参考线切向量法向量重新计算笛卡尔坐标点同时要根据d(t)的一阶导二阶导推导出真实的航向角和曲率。特别提醒一点在线速度求解时有一个公式长这样v sqrt( (s_dot - d·θr_dot)² d_dot² )这个式子里出现了参考线朝向角的变化率θr_dot它和参考线曲率直接相关。如果参考线曲率在某个位置突然变大同时d的符号和数值又不太稳定计算出来的速度会出现明显振荡对应到实车上就是方向盘和油门踏板一起抖。我的处理方案是两条路同时走上游尽可能平滑参考线比如用二次B样条或者滑动平均滤波下游对计算出的曲率做限幅曲率变化率也做滤波。两层都做实车表现会稳很多。3. 实操过程与核心环节实现3.1 我常用的一个线程框架Lattice Planner在工程代码里通常不是一个独立的进程而是规划模块中的一个核心子模块。我习惯把它放在一个固定频率的规划线程里输入是定位、感知、高精地图的融合信息输出是一条轨迹点序列交给下游控制模块跟踪。代码框架大致长这样def lattice_plan(current_state_frenet, reference_line, obstacles, static_map): # 1. 状态预处理笛卡尔转Frenet平滑当前状态 s0, d0, ds0, dd0 to_frenet(current_state_cartesian, reference_line) # 2. 生成候选轨迹集横向采样 纵向采样 candidates [] for target_d in lateral_samples: for target_s in longitudinal_samples: # 横向五次多项式拟合 d(t) traj_d quintic_poly(d0, d0_dot, d0_ddot, target_d, 0.0, 0.0, T) # 纵向多项式拟合 s(t) traj_s quintic_poly(s0, s0_dot, s0_ddot, target_s, target_s_dot, target_s_ddot, T) # 合成笛卡尔轨迹 cartesian_traj composite_to_cartesian(traj_s, traj_d, reference_line) candidates.append(cartesian_traj) # 3. 合法性过滤碰撞、边界、交规、动力学约束 valid_trajs [traj for traj in candidates if is_safe_and_feasible(traj, obstacles)] # 4. 代价评估选出最优 best_traj min(valid_trajs, keycost_function) return best_traj这个框架看起来不复杂但工程落地的魔鬼全在细节里。比如candidates列表的大小直接决定了整个循环能不能在规定的周期内跑完。如果横向采样10个点、纵向采样20个点理论上一帧就是200条轨迹每条轨迹上再均匀取20到30个点做碰撞检测计算量就起来了。就算单条轨迹检测用包围盒粗筛整帧也要控制在5毫秒到20毫秒之间才能给控制模块留出足够的余量。所以我在框架里做了一个分层过滤的设计先用最粗的包络判断排除明显撞车的轨迹再用逐采样点的精细检测确认剩余轨迹合法。粗筛阶段甚至可以把纵向采样和横向采样组合出来的重复轨迹提前去重减少无谓的计算。3.2 参数化的采样流程T、dt、横向宽度怎么定采样参数怎么定直接决定Lattice Planner表现好不好。我贴一组经过实车和仿真验证的第一版参数新手可以拿来当初始值再调参数第一版推荐值调参方向说明单次采样周期T6~8秒弯道、城市复杂场景取8高速巡航取4~5时间分辨率dt0.1~0.2秒输出轨迹点20~80个0.1更精细但更耗算力横向采样范围-3.5m ~ 3.5m超过车道半宽容易跑出边界设太多无意义横向采样步长0.5m想区分“贴左一点”和“居中”时缩小到0.2m横向速度采样0, ±0.5, ±1 m/s覆盖平稳、加速、减速变道纵向巡航速度采样0 ~ 限速值步长2m/s起步阶段步长可以大些但高速要细终点加速度0或接近0保证控制端不出现加速度跳变这里我把选择逻辑讲透一点T为什么要取6到8秒因为中高速变道本身就需要3到5秒横向五次多项式要在3秒内完成3米的横移并满足舒适性即使允许一定侧向加速度也需要给轨迹留出缓冲时间。T太短轨迹为了赶到终点会强行拉高横向加速度T太长轨迹在前几秒几乎看不出变化容易给人一种算法“不干活”的感觉。横向采样范围为什么是正负3.5米一般车道宽度3.5米到3.75米车宽1.8米左右车体中心能安全通过的横向范围也就是从-1.75米到1.75米但采样范围要覆盖到-3.5到3.5是给前车超宽、路边有障碍物等场景留出表达空间。实际代价函数会把居中位置的代价压得最低所以正常情况下最终选中的轨迹不会跑得太偏。关于总候选轨迹条数我的经验是控制在100到300条之间。低于100条可选的轨迹多样性不够碰上复杂障碍物组合容易一条可行解都找不到高于300条计算量开始明显增加而且很多轨迹之间差别极小代价函数的分辨率反而会被平均掉。3.3 代价值函数设计与权重调参心得候选轨迹生成以后要靠代价函数来分高下。代价函数这么设计是一个经验题也是Lattice Planner项目中最容易扯皮的部分因为“舒适”“效率”“安全”这几个词的权重本质上是对驾驶风格的选择。我常用的代价函数长这样J_total w1 · J_jerk_lateral w2 · J_offset_lateral w3 · J_jerk_longitudinal w4 · J_time w5 · J_speed_compliance w6 · J_collision_buffer逐项拆开说J_jerk_lateral横向加加速度jerk的绝对值在时间上的积分再除以采样周期T做平均是舒适性最主要指标。换道时jerk过大会让人感觉车被“拽”过去这一项权重稍微给高一点我的常用初始值是1.0左右。J_offset_lateral横向偏移代价即整条轨迹距离车道中心线的平均距离。用来保证正常情况下车辆居中行驶避免贴线跑。权重给太高变道会变得犹豫给太低车辆会频繁贴边我通常从0.3开始调。J_jerk_longitudinal纵向加加速度代价反映车辆变速的平顺性。频繁加速刹车就是这项权重不足的典型表现。J_time到达终点需要的时间。时间代价的作用是限制“磨蹭”——如果不惩罚时间算法可能选一条慢悠悠开8秒才完成变道的轨迹影响通行效率。但权重给太高又会让变道很激进这里需要跟安全性权衡。J_speed_compliance速度限制代价比如超速、弯道没有提前减速都会让这一项增大。J_collision_buffer车辆轨迹与障碍物的距离代价越近越大用来保持安全距离。调参最大的一个坑是各项之间没有做归一化就开始调权重。比如横向jerk的量级可能动不动就上千而偏移代价量级只有几十权重直接设成一样编辑器里看起来你在“公平”地加权实际上jerk项完全主导了结果。正确做法是先统计每种代价在正常轨迹上的量级分布然后分别归一化再给权重。归一化之后w1和w2的调整才有意义。另一个常见问题是横纵向代价权重相差太大。横向调得特别平顺纵向却忽快忽慢开起来像抽风。我在实车调试时发现纵向权重给横向的1.5倍到2倍左右整体驾驶风格会平衡很多。当然这个值因场景而异城市拥堵路和高速巡航的比例肯定不一样。3.4 碰撞检测和约束检查碰撞检测是合法性过滤中最关键的一道关卡。最实用的做法是在候选轨迹上等间隔取20到30个采样点在每个采样点上把自车近似成一个矩形或一组圆然后跟障碍物做相交检测。用圆近似自车时一般画3个圆车头、车身中部、车尾半径根据车宽车长标定。三个圆接起来会覆盖车辆实际轮廓但四个角上会稍微多出一块导致某些理论上能通过的窄缝被误杀。矩形近似精度更高但相交测试的计算量也要高一些。折中办法是粗筛用三圆细筛再上矩形两道关卡跑下来既能保证时效又能保证安全。除了碰撞约束检查还要覆盖这几项横向加速度不能超过轮胎摩擦极限一般0.6g左右雨天要更保守纵向加速度、减速度在设定范围内乘用车舒适减速度约3m/s²紧急制动可以到6~8m/s²加加速度jerk做限幅横向一般不超过2m/s³纵向不超过2~3m/s³轨迹上每个点的曲率不得超过车辆最小转弯半径对应的上限速度不得超过限速和弯道安全速度的低值工程上我把这组检查做成一个可插拔的合法性链每个检查项单独可开关。这样在做回归测试时我能把某一项的误杀情况单独暴露出来而不是纠缠在一个大而全的is_safe函数里。4. 常见问题与排查技巧实录4.1 问题速查表做Lattice Planner踩过坑之后我整理过一张问题速查表很多新来的同事照着这张表排查效率比我当年盲试高出一大截常见现象可能原因排查思路变道完成后车头贴着车道线横向偏移代价权重过低终点d采样偏外拉高横向偏移权重收敛目标横向位置变道不果断长时间跟在慢车后时间代价权重偏低加大J_time权重缩小T采样上限某些场景完全找不到可行轨迹采样范围过窄、步长过粗、碰撞圆/矩形过保守打印空格曲线确认采样覆盖放大碰撞外扩半径前先确认检测算法本身弯道里出弯速度仍然偏高纵向采样未考虑弯道限速增加曲率-速度映射对弯道降速做硬约束高速巡航时轨迹左右摆参考线不平滑、定位抖动、投影s跳变平滑参考线增加定位状态缓冲跟车时车速忽快忽慢前车状态噪声大、预测模型不稳前车观测低通滤波缩小预测窗口这张速查表的价值在于绝大多数Lattice Planner的“玄学问题”本质都落在采样覆盖不够、代价权重失衡、输入状态不干净这三类里。先思考属于哪一类再动手改绝不建议一上来就调权重。4.2 从实车日志里看到的几个“鬼故事”第一个故事和横向jerk的归一化有关。当时我们的车在环路上正常巡航前车慢算法也检测到了变道机会但始终不执行变道。看日志发现候选轨迹里有变道轨迹但它们的总代价总是比保道轨迹高出一截。再把各分项代价打印出来才发现横向jerk项因为T比较短计算出的平均jerk值被放大得很离谱直接压过了时间代价和变道收益。问题根源不是权重不对而是归一化里分母T没处理好。修正之后变道行为立刻恢复正常。第二个故事和停车舒适性有关。车载测试员反馈车辆刹停瞬间人会明显前倾。查日志发现纵向规划把停车点设得非常靠前从10m/s速度进、终端速度设为0五次多项式为了保证准时刹停中途把减速度拉到了接近极限。处理方式是给纵向轨迹增加了减速度过滤项同时把停车需求的规划窗口从4秒扩展到8秒让算法有更多空间提前缓慢减速。第三个故事的根源是定位抖动。车在高架桥下行驶时定位偶发跳变自车的Frenet起点状态跟着跳导致每一帧轨迹的起点都不一样输出到控制层就是方向盘突然抖一下。定位的问题一时改不完我的临时方案是在规划线程内部维护一个状态缓冲对s、d、s_dot、d_dot做一阶低通变化率超过阈值就丢弃这一帧规划结果沿用上一帧输出。这个方案从补丁角度解决了实车安全问题也让我理解了一个道理再好的规划算法也要对输入噪声有足够的鲁棒性设计。4.3 一个调参与排查的好习惯排查这类问题强烈建议在调试阶段输出“完整决策日志”。日志里除了最终选中的轨迹还要把每条候选轨迹的分项代价、被拒原因、采样参数全部记录下来。具体可以把每帧候选轨迹画到Rviz或Matplotlib里用颜色深浅标注代价高低。当实车表现异常时对照着决策日志和轨迹图通常十分钟就能定位到问题。我自己在系统里加过一个统计接口每100个规划周期统计一次候选轨迹总数、合法轨迹数、最终选中的轨迹对应的横向/纵向索引。如果合法轨迹数长期偏低说明采样范围或碰撞检测过保守如果一直选到同一个区域的轨迹说明代价权重让解空间偏置了。这套统计比每次拍脑袋调参高效得多。5. 从代码原型走向项目落地的几点体会最后聊几句个人经验。Lattice Planner的公式推导不难网上资料一抓一大把难的是把它做成一套在实车上稳定跑几个月的系统。我的体会可以浓缩成三点。第一先把采样和五次多项式拟合做扎实再碰代价。不要一上来就追求复杂代价函数先让候选轨迹生成正确、碰撞检测可靠后面调参才有意义。我自己带人的时候第一步都是让新人写一个横向变道轨迹生成器输出的d(t)曲线要首尾平滑、加速度连续能在仿真里跑通再进纵向、合成、代价。第二代价函数的设计要对驾驶场景有预判。同样的权重组合高速巡航、城市拥堵、停车入位三个场景的表现完全不同。成熟的工程做法不是用一套权重打天下而是按场景配置多组参数在规划时根据场景标签动态切换。接缝处做平滑过渡避免权重突变导致轨迹跳变。第三调试工具链的建设比算法本身更影响项目周期。如果一个参数异常要翻三个模块才能确认调试效率一定低反过来日志完整、可视化充分、统计接口齐全很多“诡异问题”一查就水落石出。Lattice Planner这类算法的问题从来不会只出现在一个环节花时间把工具链搭好是最值回票价的投资。如果你正准备在自己的项目里引入Lattice Planner或者已经在跟横向jerk、变道犹豫这类问题搏斗希望上面这些思路能给你一些参考。踩过几次坑之后你会明白Lattice Planner的边界不在算法本身而在你对场景的理解和调试的耐心。