企业官网建设流程全解析

深入解析Apollo Lattice Planner：自动驾驶轨迹规划的核心算法与实践

刚接触自动驾驶规划模块的时候，面对一堆名词——DWA、MPC、EM Planner、Lattice Planner——确实容易懵。直到我把Apollo的Lattice Planner源码从头到尾捋了一遍，又在仿真和实车上调了几个月的参数，才真正理解这套算法为什么能成为许多自动驾驶项目的首选局部规划方案。这篇东西不打算做教科书式的科普，我想用实际做项目踩坑的视角，把Lattice Planner的核心思路、代码结构和调参经验一次性讲透。不管你是刚入门规划算法、正在做毕设、还是工作中要从零搭建一个局部规划模块，这篇文章都会比你自己啃源码省力很多。

1. 整体设计与思路拆解：Lattice Planner的定位和工作方式

1.1 它到底在解决什么问题

自动驾驶的规划模块通常分成三层：全局规划（Routing）、行为决策（Decision）、局部规划（Planning）。Lattice Planner属于局部规划这一层，它的任务是：在已知当前车辆位置、速度、朝向，以及周围障碍物信息的前提下，从全局参考线附近生成一条从当前状态到目标状态的、安全且舒适的轨迹。

这里的关键词是“一条”。现实中车辆不能像RRT那样在空间里撒点找路，也不能像DWA那样只看速度空间，它需要一条满足运动学约束、有时间轴信息的完整轨迹——也就是说，轨迹上每个点不仅要告诉车“开到哪里”，还要告诉车“什么时候到”“到了之后速度是多少”。Lattice Planner的输出正是这样的轨迹序列，每个轨迹点都包含位置、速度、加速度、相对时间等信息，这些信息可以直接送给控制模块执行。

我习惯把Lattice Planner和另一个主流方案EM Planner做对比。EM Planner走的是“规划-决策-规划”的迭代框架，横向纵向交替优化；Lattice Planner则是“采样-评估-选择”的框架，先采样出大量候选轨迹，再用代价函数打分，选最好的那一条。两者各有优劣，但Lattice Planner的优点是结构清晰、逻辑简单、易于调试，缺点是采样数量和代价函数的设置非常影响效果。用一句话总结就是：Lattice Planner把“规划问题”变成了“搜索问题”，把“搜索问题”又变成了“排序问题”。

1.2 为什么要用Frenet坐标系

这是理解Lattice Planner的第一道坎。市面上很多讲解直接跳过坐标系变换，上来就讲采样，结果读者一直疑惑“横向和纵向到底是什么”。

Frenet坐标系是沿着参考线（通常是车道中心线或全局路径）建立的曲线坐标系。在这个坐标系下，车辆的位置用两个量表示：纵向距离s——沿着参考线方向走过的弧长；横向偏移l——偏离参考线的距离。

为什么非要用Frenet坐标系？因为在笛卡尔坐标系下，一条弯道的轨迹表达式非常复杂，横向和纵向是耦合在一起的；而在Frenet坐标系下，横向运动描述的是“车怎么从车道一侧变到另一侧”，纵向运动描述的是“车怎么沿着道路前进”，两者天然解耦。这就像坐电梯和走走廊，电梯负责上下（横向），走廊负责前进（纵向），两个方向上的运动可以分开规划，最后再合到一起。这种解耦让后面的采样和代价评估都大大简化。

当然，Frenet坐标系不是免费的午餐。它引入了参考线依赖，如果参考线本身不平滑或者参考线跳变，Frenet坐标就会出现跳跃；而且从Frenet坐标还原到笛卡尔坐标时，坐标变换公式涉及曲率的计算，曲率突变的地方容易产生数值问题。这些坑后面实操部分我会详细说。

1.3 采样和评估：Lattice Planner的宏观流程

Lattice Planner的整体思路可以分为四步：

第一，把当前车辆状态转换到Frenet坐标系下，得到当前的(s, l, ds/dt, dl/dt)等初始状态。第二，根据驾驶场景（巡航、跟车、停车、变道等）生成纵向和横向的采样目标，横向采样目标是一组目标l值和目标时间T；纵向采样目标是一组目标s值、目标速度v或目标时间T。第三，用多项式拟合分别生成横向轨迹和纵向轨迹，再把它们合成完整的二维轨迹。第四，把每条候选轨迹做碰撞检测、曲率检查、加速度检查，然后计算代价函数，选出最优轨迹。

听起来不复杂，但魔鬼在细节里。采样怎么采才能保证轨迹多样性？代价函数各项权重怎么设才能让车既安全又不那么“愣”？多项式用什么阶次才能平衡平滑度和响应速度？这些我放在后面几节逐个展开。

2. 核心原理拆解：横向纵向解耦、轨迹生成与代价评估

2.1 横向轨迹：五次多项式与变道动作

横向规划的目标，是让车在T时间内从当前横向状态到达目标横向状态。横向状态的初始值是已知的：当前横向偏移l0、横向速度dl0/dt、横向加速度ddl0/dt²。目标值通常包括目标横向偏移l_target（比如变道目标就是相邻车道中心线的l值）、目标横向速度、目标横向加速度。为了满足这些边界条件，Lattice Planner默认使用五次多项式来拟合横向轨迹：l(t) = c0 + c1t + c2t² + c3t³ + c4t⁴ + c5*t⁵。

为什么是五次而不是三次？三次多项式只能约束位置和速度，五次多项式可以同时约束位置、速度和加速度，这样就能保证轨迹起点和终点的加速度是连续的。想想看，如果轨迹的起终点加速度不连续，控制层就得突然改变方向盘角度，坐在车里的乘客会明显感觉“顿一下”。用五次多项式，相当于告诉车辆“你不仅要从A点到B点，还要保证出发和到达时的手感和加速度是一致的”，这直接决定了乘坐体验。

采样时，横向目标会生成多组离散值。比如Apollo默认的横向采样间隔是0.5米，会采样从左侧几个车道到右侧几个车道的横向偏移值；每个横向偏移又对应一组不同时间T的轨迹，T通常从6秒到8秒、间隔1秒。横向采样密度直接影响变道的激进程度：采样间隔太大，变道选择少，可能找不到合适间隙；采样间隔太小，候选轨迹数量暴增，实时性会出问题。

2.2 纵向轨迹：从ST图到速度曲线

纵向规划解决的核心问题是“什么时候加速、什么时候减速、什么时候停车”。在Frenet坐标系下，纵向状态是s(t)，即“沿着参考线走过的距离随时间的变化”。纵向轨迹同样可以用多项式表示，但这里分两种情况。

第一种是定终点距离——车需要在T时间内走完一定距离，比如“在50米后停下”或者“在80米后到达目标位置”。这种情况下，起点纵向状态（s0, ds0/dt, dds0/dt²）和目标纵向状态（s_target, ds_target/dt, dds_target/dt²）都固定，用五次多项式拟合。

第二种是定终点速度——车需要在T时间内把速度从当前值调整到目标值，但距离不固定。比如前方车辆匀速行驶，我们想保持相同速度跟车，这时候用的是四次多项式，因为少了一个约束（终点的s不固定）。这也是Lattice Planner里“巡航”和“跟车”两种模式的差异来源。

更关键的是ST图的使用。ST图是一个以时间为横轴、纵向距离s为纵轴的二维图，障碍物在ST图上表示为被占据的矩形区域。Lattice Planner在ST图上采样纵向目标点时，会先判断哪些区域被障碍物占据，把采样点放在空白区域中。减速跟车就是“在障碍物的下方（更小的s值处）保持一个安全距离”，停车等待则是“在障碍物前方的某个s值处将速度降到零”，超车变道则是“在横向轨迹切换到旁边车道的同时，纵向轨迹从障碍物的旁边绕过去”。

在实际代码里，ST图上的障碍物信息来自感知和预测模块，预测模块给出未来几秒内每个障碍物的轨迹预测，即使障碍物不动，它在ST图上也会形成一条沿着时间轴的带状区域。这里有一个经验：ST图上的安全距离不能只看边界相等就算安全，Apollo里默认还会加上一个额外的缓冲距离，否则稍微有点感知误差，碰撞检测就会误报。

2.3 代价函数设计：舒适度、效率与安全如何权衡

采样能生成几十甚至上百条候选轨迹，怎么选？Lattice Planner的策略是给每条轨迹打一个综合代价分，公式通常长这样：

J = w_lat * J_lat + w_lon * J_lon + w_obs * J_obs + w_dynamic * J_dynamic

其中J_lat是横向偏移代价——偏离参考线越远，代价越高，目的是让车尽量沿着车道中心线行驶；J_lon是纵向运动代价——加加速度（jerk）越大、速度偏离参考速度越多，代价越高；J_obs是碰撞代价——轨迹离障碍物越近，代价越高；J_dynamic包含法规和舒适性相关代价，比如压线代价、限速代价等。

这个权重怎么设，基本决定了车辆的性格。权重w_lat设得大，车就“画线”一样死死贴着车道中心走，变道时比较保守；权重w_lat设得小，在避障时会更灵活地绕行，但正常巡航时可能会左右飘。w_lon如果太大，车辆会过度追求平顺，前方明明有空间也不愿意加速，造成通行效率低下。

我一般在实车上按“安全优先、舒适次之、效率再次”的原则来调。一开始把w_obs设得极大，保证不撞，然后逐步调小，找到一个“不误报但又不冒险”的临界点。另外，不同场景下的权重应该是可切换的——高速巡航时横向权重高一点；城区拥堵时纵向权重低一点，允许走走停停。Apollo的Lattice Planner配置里有一个cost_weight参数块，里面每一项的含义在代码注释里写得很清楚，建议直接对着代码看。

2.4 碰撞检测与轨迹合法性检查

碰撞检测是轨迹筛选的硬门槛，不合格的轨迹直接丢掉，不进代价排序。Lattice Planner的做法是把车辆抽象成一个或多个圆形，每个圆代表一部分车体，然后检查每个圆的圆心到所有障碍物的距离是否大于圆半径。

为什么用圆而不是矩形？因为圆与圆的距离计算最简单，只要算两个圆心的欧氏距离再减去半径之和就行了。Apollo里默认用三个圆近似车体——车头、车尾、车身中间——这样既有精度又高效。但这带来一个坑：三个圆并没有完全覆盖车体的所有角落，特别是斜45度方向会有检测盲区，所以Apollo又额外提供了一个保守的包围盒膨胀参数。

轨迹合法性检查还包括：最大曲率检查——方向盘打得太死的轨迹直接废掉；最大加速度/减速度检查——超出车辆物理极限的轨迹废掉；最大jerk检查——舒适性硬上限，超了说明乘客会很难受。每次实车调试之前，我都会先在仿真环境里把所有被标记为infeasible的轨迹可视化出来，看看是被哪个条件卡掉的，这比自己瞎猜参数高效得多。

3. 实操：Apollo里的Lattice Planner是怎么跑起来的

3.1 源码架构和数据流

Apollo的代码版本一直在变，但Lattice Planner的核心文件基本保持稳定。我以比较常见的v6.0/v7.0分支为例说明。

核心代码在modules/planning/planner/lattice/lattice_planner.cc这个文件里，它对外暴露Plan方法。整个规划流程可以简化为几个阶段：读取参考线、获取当前状态、转换坐标系、生成横纵向采样、合成轨迹、评估选优。其中不少中间步骤被封装到了LatticePlanner这个类里，但也可以通过注释快速找到关键调用。

数据流方面，Lattice Planner的输入来自LocalView，里面封装了当前自车状态、障碍物信息、参考线、路由信息等，这些数据在Apollo CyberRT框架下通过PlanningComponent统一接收。输出是Planning::ADCTrajectory，这是一条带时间戳的轨迹点序列，每个点都包含x、y、z坐标、速度、加速度、相对时间以及路径点级的状态，控制模块拿到它之后做方向盘和油门的跟踪。

在刚开始读源码的时候，我建议你先不要一头扎进lattice_planner.cc里，而是先看modules/planning/common/trajectory1d/piecewise_jerk_trajectory1d.cc和polynomial_curve1d.cc这两个文件。Lattice Planner生成轨迹用的多项式工具都在这两个文件里，理解了它们，再看lattice_planner.cc会轻松很多。

3.2 关键参数配置与选择方法

Lattice Planner的参数分布在两个地方：一个是modules/planning/conf/planning_config.pb.txt，另一个是modules/planning/conf/planning.conf。前者是规划模块的整体配置，后者是场景和调试开关配置。

我挑几个影响最大的参数说：

采样时间范围和时间间隔。Apollo默认是trajectory_time_length = 8.0，trajectory_time_resolution = 0.1。如果只在低速园区场景用，可以把时间长度缩短到6秒；如果是在高速场景，建议保持8秒甚至更长，否则远处的大曲率弯道会来不及反应。

横向采样间隔。target_l_interval = 0.5，这个值越小变道越精细，但数量会增大。城区多车道场景我试过1米间隔，效果也不错，计算负担小很多。

纵向采样间隔。SAMPLE_GAP_FOR_STITCHING = 1.0（代码里是常量），影响当前轨迹拼接的连续程度。

代价函数权重系数。在LatticePlanner的配置里有一组cost_weight，包括latitude_loss、longitude_loss、obstacle_loss等。调参的时候每次只改一个系数，记录不同权重下的轨迹形状。

3.3 从零跑通一个Lattice Planner的仿真场景

很多人想跑Apollo，但被环境搭建劝退了。这里分享一条比较顺的路径：用Apollo自带的DreamView仿真，再加一个官方提供的demo地图，不需要真车和复杂硬件。

步骤大概是：

1. 按照Apollo官方文档编译环境，建议用Docker容器，编译器版本和依赖库都配好了。
2. 启动DreamView：bash scripts/apollo_neo.sh setup然后./scripts/bootstrap.sh，打开本地8000端口。
3. 在DreamView里选择合适的车辆和地图，开启“SimControl”模式，在“Module Controller”里启动Planning模块。
4. 在路由编辑里设置起点和终点，车辆会自动执行Routing → Decision → Planning → Control的完整链路。

如果跑不通，八成是依赖版本或数据文件路径问题，先看日志。Apollo的日志默认输出在/apollo/data/log/下面，规划模块的debug信息非常详细，一条轨迹从规划到执行的中间过程几乎都能在里面找到。

3.4 可视化调试：用Planning Visualizer看轨迹

规划算法光看数值日志是不够的，一定得可视化。Apollo的DreamView本身就支持在场景中显示规划的轨迹，但如果你想在离线数据上回放并逐帧分析，推荐用/apollo/scripts/planning_visualizer.sh生成的plot结果。

我自己的调试方法是：在仿真里跑一个场景，把Planning的输入输出录成离线包（Cyber Record）。然后用Python脚本读取记录，把参考线、所有候选轨迹、被选中的轨迹、障碍物边界框都画在同一张图上。这样就能直观看到“为什么选这条不选那条”，对比代价函数的输出值来定位问题。

在实车上调试时，还可以在Lattice Planner的代码里临时加一些ROS/Cyber Topic输出，把每条候选轨迹的代价项拆解发出来，方便在车上实时观察到底是哪一项权重压倒了其他项。

4. 常见问题与排查技巧实录

4.1 典型问题速查表

4.2 调参顺序的实操心得

参数不能乱调，我有一个固定的调参顺序，多次验证比较有效：先调时间长度和采样密度，再调碰撞代价，再调横向纵向代价的比值，最后才动安全和舒适相关的硬约束。这个顺序背后的逻辑是：先把搜索空间划对，再保证安全，再调驾驶风格，最后用硬约束卡边界。

如果发现车辆在正常巡航时频繁“抽风”，比如一会儿加速一会儿踩刹车，建议先检查纵向采样速度序列是否太小，导致没有中间速度候选可选，只能在加速和减速两个极端里选。解决方法是增加纵向采样速度的层数，或者把采样速度范围的上限提高一些。

如果变道成功率低，往往是横向采样时间T太小或者横向目标l的值没覆盖到目标车道中心。调试时可以临时把横向采样间隔改小到0.2米看看，如果效果变好，说明是采样密度问题；如果没变化，就去看代价函数里横向偏移的权重是否太大，把车辆“吸”在原来的车道上了。

4.3 从仿真到实车过渡的注意事项

仿真环境里表现再好，到实车也会遇到很多问题。首先是延时问题，仿真里感知和控制都是理想化的，实车有通信延迟和控制误差，所以Lattice Planner输出的轨迹必须与控制模块做跟踪反馈。如果车辆出现“画龙”现象，先检查控制模块的横向跟踪增益，再回来看规划轨迹是否过于频繁切换。

第二个是参考线的稳定性。实车的参考线通常来自高精地图或者实时感知的车道线拟合，当参考线出现切换或跳变时，Frenet坐标也会跟着跳，这会让Lattice Planner生成不连续的轨迹。遇到这类问题，我通常会在规划模块前加一个参考线平滑和滤波的环节，让输出的参考线更稳定。

第三个是时间同步问题。Lattice Planner的轨迹是带时间戳的，但如果规划模块输出的时间戳与传感器时间基准不一致，控制模块按时间戳插值时会出错。建议在实车集成时专门检查一遍时间同步配置。

4.4 一个调参实例：低速园区场景的避障摇摆问题

之前做一个低速园区项目，车速在10km/h以下，车辆在遇到行人时经常出现一个奇怪的现象：明明已经减速停下来让人先走，行人一旦稍微移动一点，车辆又立刻重新规划轨迹，往前蹭一步再停，整个过程很像“犹豫”。通过可视化发现，纵向采样生成的候选轨迹中，停车轨迹和低速前进轨迹的代价在某些帧非常接近，打分时微小波动就导致选择结果在两条轨迹之间来回跳。

这个问题的根因是纵向采样速度间隔太大——低速区间里可选速度只有0和2m/s，没有中间值。把采样速度间隔从1.0m/s改成0.3m/s之后，车辆在低速区间有了更多平滑的速度选择，就不再忽走忽停了。另外，我发现对紧急程度不同的场景设置不同的代价上限也很有用——当车辆已经检测到行人很近时，把“低速跟车”这一类的候选轨迹的代价压低，让车辆更倾向于停下来等待，减少“试探性”起步。

5. 个人经验总结与扩展思路

5.1 走完一遍Lattice Planner后的深刻体会

我在做Lattice Planner相关项目时最大的感受是：这套算法确实比很多现代学习类规划方法“老”，但它在工程上的稳定性和可解释性依然很强。其实所有规划算法最终解决的都只有两个问题——安全和舒适，Lattice Planner通过采样和代价函数把这两个问题转化成了人人都看得懂的数学表达，这本身就是巨大的工程价值。

对于新手，我建议不要一上来就改代码调参。先用默认参数跑通仿真，再手动在DreamView里设置不同的起点和终点，观察车辆大致表现。然后读三份代码文件：polynomial_curve1d.cc理解多项式生成，lattice_planner.cc理解整体流程，trajectory_evaluator.cc理解代价计算。这三份文件吃透了，Lattice Planner的基本功就算扎实了。

5.2 进阶方向：多车道交互场景和预测融合

Lattice Planner的一个天然短板是它倾向于“反应式”规划——即根据当前感知到的环境做出反应，但对其他车辆意图的建模比较弱。如果目标是高密度城市道路上的交互场景，建议把Lattice Planner和其他策略层模块结合起来，比如在采样前先做一次行为预测，根据预测结果动态调整采样范围和代价权重。

另一个进阶方向是让Lattice Planner的输出和其他规划器做融合。有些项目会把Lattice Planner作为纵向安全规划器，另一些项目则把Lattice Planner用于结构化道路场景，再配一个Frenet优化器用于弯道。这种混合架构的思路很实用，特别适合量产车规级系统。

后在我调试过程中，还发现了一个小技巧：在做完一条轨迹的所有代价评估之后，对选中的最优轨迹再做一次“温和的平滑后处理”（在安全档位内做一次插值），能明显减少控制层的抖动。代价函数选出的轨迹已经是“最优”的，但它不一定是最平滑的，加一步轻量平滑是低成本高收益的操作，非常适合工程落地。