企业官网建设流程全解析

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 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, key=cost_function) return best_traj

这个框架看起来不复杂，但工程落地的魔鬼全在细节里。比如candidates列表的大小，直接决定了整个循环能不能在规定的周期内跑完。如果横向采样10个点、纵向采样20个点，理论上一帧就是200条轨迹，每条轨迹上再均匀取20到30个点做碰撞检测，计算量就起来了。就算单条轨迹检测用包围盒粗筛，整帧也要控制在5毫秒到20毫秒之间，才能给控制模块留出足够的余量。

所以我在框架里做了一个分层过滤的设计：先用最粗的包络判断排除明显撞车的轨迹，再用逐采样点的精细检测确认剩余轨迹合法。粗筛阶段甚至可以把纵向采样和横向采样组合出来的重复轨迹提前去重，减少无谓的计算。

3.2 参数化的采样流程：T、dt、横向宽度怎么定

采样参数怎么定，直接决定Lattice Planner表现好不好。我贴一组经过实车和仿真验证的第一版参数，新手可以拿来当初始值再调：

这里我把选择逻辑讲透一点：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踩过坑之后，我整理过一张问题速查表，很多新来的同事照着这张表排查，效率比我当年盲试高出一大截：

这张速查表的价值在于：绝大多数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的边界不在算法本身，而在你对场景的理解和调试的耐心。