深入解析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.0trajectory_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地图不需要真车和复杂硬件。步骤大概是按照Apollo官方文档编译环境建议用Docker容器编译器版本和依赖库都配好了。启动DreamViewbash scripts/apollo_neo.sh setup然后./scripts/bootstrap.sh打开本地8000端口。在DreamView里选择合适的车辆和地图开启“SimControl”模式在“Module Controller”里启动Planning模块。在路由编辑里设置起点和终点车辆会自动执行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 典型问题速查表问题现象可能原因排查方向规划的轨迹抖动左右变道频繁横向采样间隔太小或代价函数权重不合理调大横向采样间隔适当增加横向偏移代价权重车辆在静止障碍物前刹不住纵向采样距离不够或ST图安全距离过小增加trajectory_time_length检查ST图上障碍物边界膨胀系数轨迹曲率过大车辆报错参考线本身有折点或采样时间太短检查参考线平滑模块是否开启适当增大采样时间范围匝道或弯道中频繁压线Frenet坐标转换时曲率项处理有误差检查参考线曲率计算确认坐标变换公式中曲率半径没有漏项规划耗时过高CPU吃满采样点数量太多或碰撞检测过于频繁减小采样密度或优化碰撞检测中对障碍物遍历的逻辑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优化器用于弯道。这种混合架构的思路很实用特别适合量产车规级系统。后在我调试过程中还发现了一个小技巧在做完一条轨迹的所有代价评估之后对选中的最优轨迹再做一次“温和的平滑后处理”在安全档位内做一次插值能明显减少控制层的抖动。代价函数选出的轨迹已经是“最优”的但它不一定是最平滑的加一步轻量平滑是低成本高收益的操作非常适合工程落地。