做自动驾驶规划控制有段时间的朋友大概率都翻过Apollo的代码。源码仓库里规划模块那一大坨第一次打开的时候是真容易劝退尤其是lattice_planner这个目录看起来文件不多但里面又是Trajectory1d又是LatticeTrajectory1d类跟类之间的关系绕得人头疼。我最早接触Lattice Planner是在一个园区低速接驳项目上那时候团队里没人真正啃过Apollo源码大家在EM Planner和Lattice之间纠结了很久最后还是选了Lattice原因很简单——它思路直白采样、选优、碰撞检测每一步都能单独拎出来看出了问题也好定位。等真正把代码一行行理完、又把参数调到能在实车上稳定跑起来之后我对这套算法的评价是它未必是效果上限最高的规划器但绝对是最适合拿来学习轨迹规划、也最适合在限定场景里快速落地的方案之一。这篇内容适合几类人刚入门自动驾驶规划想知道轨迹规划到底在解决什么问题的人已经在用Apollo但主要调参、没深入看过Lattice实现的人或者你在自研规划模块想找一套能抄作业的采样代价函数框架。下面我会把Lattice Planner从思想到实现再到实车调参的完整链路拆开讲里面的参数和踩坑记录都来自实际项目你可以直接把结论拿去用。1. 轨迹规划为什么不能靠“打点连线”解决先搞清楚一个容易被忽略的问题全局导航给出的路径和车真正要走的轨迹根本是两码事。导航路径是一条几何线它不包含“什么时候到”“以多快速度经过”这些信息更不关心这条线上某个位置会不会撞上旁边突然窜出来的行人。轨迹规划要输出的是一条带时间戳的序列——每个时刻车在哪儿、朝向是多少、速度是多少、加速度是多少。这条序列必须满足车辆运动学约束比如前轮转角有限制横摆角速度不能无限大必须满足舒适性约束也就是加速度变化率不能太夸张还必须满足安全性约束在动态障碍物的包围下不能撞。1.1 路径规划与轨迹规划的分工边界在Apollo的架构里routing模块负责全局路径planning模块负责局部轨迹。Lattice Planner属于planning的一部分它接收上游的参考线ReferenceLine、障碍物信息、定位和底盘状态然后在参考线附近搜索一条满足约束的局部轨迹。这里有一个很关键的点Lattice是“局部”规划器它的视野有限通常只看前方几十到一百多米视野之外靠全局路径兜底所以它不需要、也不可能做出长距离的战术决策。它要做的是“在接下来几秒内把车从当前状态安全、舒适地挪到某个目标状态”。这个定位决定了Lattice的很多设计取舍。比如它不太擅长处理需要长距离预判的复杂博弈场景但在结构化道路、低速园区、限定区域的场景里它的简单直接反而是优势。1.2 为什么选Lattice而不是EM PlannerApollo里面还有一套更出名的EM Planner它的核心思路是“EM迭代动态规划/二次规划”把轨迹规划拆成路径规划和速度规划两个步骤用动态规划提供粗解再用二次规划平滑。EM Planner的上限更高能处理更复杂的场景但代价是代码复杂参数多调起来非常痛苦。Lattice的思路不一样它先把问题简化——横向和纵向解耦然后在Frenet坐标系下分别采样生成一堆候选轨迹最后用代价函数打分选出最优。整个过程没有迭代优化没有动态规划逻辑非常线性出了问题很容易追溯到是采样没采到、还是代价函数权重不对、还是碰撞检测误报。团队做技术选型的时候如果追求快速落地、场景可控、团队技术水平还在爬坡期Lattice是性价比很高的选择。如果要做开放道路、复杂交互场景EM或者基于学习的方法才是正路但学习曲线也陡得多。2. Lattice Planner的思想拆解横向和纵向为什么能分开Lattice最核心的思想是解耦。车辆运动本身是横纵耦合的转向会影响纵向速度加速也会改变横向响应。但如果把问题放到Frenet坐标系里事情就变得清爽很多。Frenet坐标系是沿着参考线建立的纵向用s表示沿参考线方向的距离横向用l表示偏离参考线的距离。在这个坐标系里一条道路可以被想象成一个“拉直的管道”车辆在这个管道里的运动被拆成“顺着管道走”和“在管道内左右偏”两个独立维度。Lattice Planner干的事就是在s-t和l-s两个平面里分别采样目标状态各自生成一条一维轨迹再把这两条一维轨迹合并成二维轨迹。2.1 Frenet坐标系怎么理解打个比方参考线像一条河道车辆像一条船。船在河道里的位置可以用“离河口多远”对应s和“离左岸/右岸多远”对应l两个数字来描述。船既可以往前走也可以左右调整航向但这两个动作在“河道坐标系”里是相对独立的。Frenet坐标系最大的好处是把“沿路走”和“靠左靠右”解开了道路的弯曲程度被“拉直”了很多计算在数学上就简单了。这一点特别重要因为如果直接在笛卡尔坐标系下做采样和多项式拟合道路一转弯轨迹就很难描述代价函数也很难写。而在Frenet坐标系里一条弯道的横向偏移计算和直道几乎没有区别。2.2 横向轨迹与纵向轨迹的解耦逻辑横向轨迹描述的是l随时间或随s的变化曲线它决定了车在车道里的横向位置——是从车道中心走还是往左变道还是往右靠边。纵向轨迹描述的是s随时间的变化曲线它决定了车的速度轮廓——是匀速巡航、加速超车、还是减速停车。这两个维度在生成的时候是独立的但最后合并的时候要互相校验。比如我打算变道到左侧车道采样终态的时候在横向上生成一条从当前车道中心到左侧车道中心的轨迹在纵向上生成一条带目标速度的跟车轨迹。合并之后才能检查这条“边变道边加速”的轨迹会不会撞车。如果碰撞检测不通过就换一组横纵向组合继续试。2.3 采样不是撒点而是撒“目标状态”“撒点采样”听起来像是乱试但Lattice的采样其实非常讲究。它不是在(x, y)空间里随便撒点而是在(s, l, t, v, a)这个状态空间里针对每一类驾驶意图生成一组“终点状态候选”。比如巡航场景纵向采样会以不同的目标速度、不同的到达时间生成终点跟车场景会以不同的跟车距离生成终点停车场景会以不同的减速度、停车位置生成终点。横向采样则围绕当前车道的中心线生成一组不同横向偏移的目标位置比如0米居中、±1米轻微靠边、±2米接近车道边界附近等。每个终点状态都是一组目标值然后用多项式拟合出一条从当前状态到目标状态的平滑轨迹。通过控制终点状态的分布密度和范围就能控制轨迹搜索的覆盖范围——采得越密覆盖越全计算量也越大。3. 实操拆解Apollo Lattice Planner的关键环节看源码的时候建议按这条主线走先看LatticePlanner::Plan它把整个流程串起来然后依次看LatticeTrajectory1d、Trajectory1dGenerator、LatticeEvaluator这几个核心类。下面把每个环节的关键细节和参数选择逻辑都过一遍。3.1 输入处理参考线和障碍物怎么进LatticeLattice Planner不是直接吃全局导航路径的。上游会先把全局路径投影到车道内部、做平滑生成一条局部参考线。参考线的质量直接影响Lattice的上限——如果参考线本身有锯齿或者曲率跳变后面生成的所有轨迹都会继承这些问题。所以实际项目中reference_line_provider的平滑参数很重要smoother的w_curvature、w_smoothness这些权重不要用默认值怼到复杂弯道上得根据场景调。障碍物信息进来的时候会被投影到Frenet坐标系下变成一个或多个ST图里的障碍物矩形。这个过程看着简单其实坑不少障碍物形状投影到ST图时要考虑自车在换道过程中是否会扫过障碍物的“膨胀区域”低速场景下的行人、自行车轨迹预测不准ST图里的矩形位置就可能偏。这里需要给障碍物地图加一层安全缓冲实测下来加上缓冲和没加缓冲实车表现差异非常大。3.2 横向采样从l0到l±4.5米的一组候选在典型配置里横向采样的目标偏移集合大概是这样的保持当前车道l 0微调l ±1.0米靠边/让行倾向l ±2.0米贴近车道线附近l ±3.0米变道目标车道中心附近l ±4.5米这组数字不是拍脑袋定的。车道宽度一般在3.5米左右车道中心线到车道边界的距离约1.75米如果车辆宽度取2米那车辆边界离车道线差不多还有0.75米余量。l±3.0米已经非常贴近边界除非场景需要一般不作为首选目标。±4.5米通常对应相邻车道中心线两边各3.5米车道用于变道场景。横向轨迹用五次多项式来拟合从当前l、l、l到目标l、l0、l0。五次多项式保证了轨迹在起点和终点的位置、速度、加速度都连续这一点直接决定了车辆的横向控制会不会抖动。有些论文会用三次多项式但三次只能保证位置和速度连续加速度会跳变实车上会感觉一顿一顿的。3.3 纵向采样速度、距离、时间三个维度的组合纵向采样的设计比横向更复杂因为纵向意图非常多。常见配置是把纵向采样分成几类巡航目标速度v 0.5、1.0、1.5倍当前限速时间t 4.0、6.0、8.0秒终点距离Δs由速度和时间的乘积决定跟车根据前车速度和距离采样多个目标跟车距离通常等于前车速度乘一个时间间隔如1.2s、1.8s、2.4s车头时距再加最小停车间距停车目标速度v0目标位置在前方停止线或者障碍物前0.5~1.0米时间采样覆盖4~8秒。纵向轨迹的拟合通常对位移使用四次多项式或者五阶多项式这样能同时约束s、v、a在起终点连续。这里有个细节我觉得值得单独说纵向在时间上的采样密度直接决定了计算量。如果把时间轴从4秒到8秒每0.5秒采一个点那么每个目标距离又会对应一批候选轨迹最后数量和横向候选一做笛卡尔积总轨迹数量可能大几百条。每条都要做碰撞检测和代价计算算力压力就在这儿。3.4 轨迹生成之后合并、碰撞检测、代价评估横向候选和纵向候选两两组合就会生成“一条候选纵向轨迹×一条候选横向轨迹”的二维轨迹。在Apollo里这一步会得到DiscretizedPath和对应的速度曲线。先做粗碰撞检测用自车的包络矩形沿轨迹扫一遍看是否与障碍物的膨胀矩形相交然后做细碰撞检测把轨迹离散成若干个点在每个点用车体矩形或者圆形与障碍物计算距离。只有通过碰撞检测的轨迹才能进代价评估。代价函数是Lattice的“灵魂”它决定了在无碰撞的轨迹里选哪一条。Apollo的LatticeEvaluator会统计一系列分数舒适性代价轨迹的加加速度jerk积分值越大越不舒适效率代价到达目标的时间、平均速度与期望速度的偏差横向偏移代价l偏离参考线越大扣分越多终点朝向代价轨迹终点航向与参考线切向的夹角靠右行驶倾向一些版本在右侧通行的场景里对偏左的轨迹加额外惩罚。这些代价不是简单相加每项的权重决定了车辆的行为风格。比如把效率权重调高车会更激进把舒适性权重调高车会更温柔但可能压住后车。实车调参的时候这些权重是体感差别最直接的地方。3.5 代价函数权重一套可落地的初始配置我基于Apollo的默认权重和几个落地项目的经验给出一套可以当起点的权重配置代价项默认参考值调高后效果注意点jerk积分1.0车更“柔”启动/停车更缓太高会让车变肉、容易被加塞横向偏移1.0~2.0更贴车道中心/更保守不配合变道场景会导致变道延迟目标速度偏差5.0~10.0更愿意追限速太高会让车顶着前车屁股走终点朝向偏差1.0更早回正方向在匝道、弯道要降低停车距离偏差10.0停车更准高到一定程度会牺牲舒适性这套配置在园区低速场景最高时速30km/h以下表现比较稳。上了快速路60~80km/h需要把jerk权重降一点、效率权重升一点否则后车会闪灯。4. 碰撞检测与ST图Lattice的杀手锏和隐藏的坑Lattice是采样式规划器它能不能保证安全完全取决于“采样是否覆盖了安全解”。如果在一个场景里所有采样出的轨迹都会碰撞那Lattice就无解了它不会像EM那样通过优化硬生生“挤”出一条安全路径。所以在设计采样范围时要确保覆盖足够多的安全驾驶行为。这看着有点“笨”但反过来也带来了好处——因为每一组采样意图都很明确所以你能直接看出算法为什么选了某条轨迹调试很方便。4.1 碰撞检测的两种主流方式矩形扫掠与圆形近似Apollo碰撞检测里比较核心的是把自车离散成若干个覆盖圆也有的版本用矩形包络然后沿轨迹逐点检测圆或矩形与障碍物边界的距离。用圆形近似车辆的优点是计算快、无方向性缺点是四个角上会有虚检明明车头角已经擦到障碍物了但圆和障碍物还有距离这会增加“看起来能过其实过不了”的情况。用矩形包络更精确但计算量更大。实际项目中建议两步走先用粗碰撞检测把所有明显撞上的轨迹筛掉再用细碰撞检测对剩下的轨迹逐点检查。这样能把单帧规划耗时控制在几十毫秒以内不至于把算力全耗在碰撞检测上。4.2 ST图如何辅助速度规划Lattice虽然没有显式维护一个ST图优化模块但它的纵向采样结果可以理解为是在隐式地对ST图做离散搜索。ST图的横轴是时间t纵轴是纵向距离s障碍物在ST图里是一块块“禁入区域”。如果前方有一辆慢车ST图里就会出现一个随时间向右上方移动的障碍物区域Lattice纵向采样生成的各种速度曲线本质上就是在尝试绕过这块区域——要么减速让行要么加速超过。这里有个实操要点ST图里的障碍物速度怎么定会显著影响效果。如果直接用当前速度外推障碍物一旦急刹ST图里的禁入区域会失真采出的轨迹可能离真实障碍物位置太近。我见过一个项目里把所有动态障碍物在ST图里的矩形都加宽了时间轴方向上的膨胀量效果立竿见影车离行人、电动车远远的。4.3 容易被忽略的坑采样分辨率与计算量Lattice的轨迹数量可以粗略估算假设横向候选有11条纵向每个意图下候选有20条总候选就是220条。每条轨迹做一次全路程碰撞检测比如100米、0.1米一个点1000个点再算一次代价函数要积分jerk等单帧计算量就在几十毫秒这个量级。如果参考线再长一点、采样再密一点算力就会爆。更隐蔽的坑是采样点太疏可能导致某个意图下所有轨迹都被碰撞检测拦掉车就会在正常场景下频繁“无解”。比如在穿过一个窄门或者绕桩场景横向偏移采样只有0、±1.0、±2.0而绕桩需要偏移2.3米那这组采样里就没有一条能安全通过。遇到这种情况唯一的办法是加采样点或者调整参考线让门桩在参考线上的投影更居中。这个问题的排查思路是先看日志里是不是频繁出现“无可行轨迹”再逐步调高横向采样的范围检查能通过的轨迹起点偏移。5. 常见问题与排查技巧实录做Lattice落地有几个问题几乎每个项目都会遇到。我把它们整理成一张速查表后面再展开说几个我印象最深的排查过程。现象可能原因排查思路与解法频繁报“无可行轨迹”采样范围不够、碰撞检测过于保守扩大横向偏移集合、降低障碍物膨胀系数、检查参考线是否被障碍物切断车在弯道里抖动参考线平滑不足、横向目标点超出车道检查参考线平滑权重、把横向采样范围限制到车道边界内、降低代价函数里l变化的惩罚变道动作太慢、犹豫变道意图没有对应合适的纵向轨迹检查变道的目标车速是否过低、增加Δs较大的一组纵向候选、提高效率代价权重停车不够准总是过冲或提前停停车采样终点距离设置不合理增加0.5米间隔的多组停车距离候选、把停车距离代价项的权重调高高速场景下舒适性差jerk权重太低、速度采样间隔太大调高jerk积分代价、加密速度采样如0.1m/s间隔单帧计算耗时100ms轨迹候选过多、碰撞检测过细减少纵向时间采样点、粗碰撞检测前置、把自车圆形数量减少但适当加大半径5.1 实例一窄路绕障场景频繁无解做园区项目的时候有一段路两边停满了车中间留出来的通道特别窄。车开着开着就停在原地不动日志里全是“No feasible trajectory”。一开始以为是障碍物检测太灵敏把膨胀半径调小了但没解决。后来把横向采样的候选打印出来才发现最靠边的候选只有l±2.0米而通道的有效宽度要求车至少偏移2.3米才能不碰上旁边的车。把横向采样集合扩展成0, ±1.0, ±1.5, ±2.0, ±2.5, ±3.0同时增加了靠边场景对应的纵向低速候选问题就消失了。这个排查让我长了个记性先确认采样范围是否覆盖了当前场景的解空间再动代价参数。代价函数只能排序不能无中生有采样范围没覆盖安全解再怎么调权重都没用。5.2 实例二连续弯道上车身姿态来回摆另一个项目里车在连续S弯里会出现方向盘反复修正、车身姿态来回摆的情况。看了轨迹之后发现横向轨迹在弯道里被反复规划成“先偏左再偏右再偏左”。根因是参考线在弯道处的平滑度不够导致相邻两帧的参考线横向偏移基准发生了跳变Lattice基于跳变的参考算出的最优轨迹也就不一样。解决方法是两级处理一是把参考线平滑的曲率权重调大让连续弯道上的参考线更“顺”二是在代价函数里增加了一个相邻帧横向偏移变化率的惩罚项强迫两次规划的横向轨迹不要差太多。严谨一点的做法是用“上一次最优轨迹的横向偏移”作为参考基准之一让新的轨迹不要突然跳变。这一项加进去之后实车明显稳了很多。5.3 实例三变道场景忽快忽慢、犹豫不决变道的时候车经常出现“加速超过旁边车又减速缩回去”的诡异行为。分析下来是纵向采样里超车和跟车两类意图的候选轨迹分数接近超车轨迹因为速度高效率分数好但舒适性差一点跟车轨迹因为稳舒适性好但效率分数低。两者差距非常小于是算法在帧间来回切换表现为车一会儿激进一会儿保守。这类问题没有标准答案实际做法是给“变道意图”加上一个状态机当变道指令已经下发就只允许搜索变道相关的轨迹集合同时把代价函数里效率项的权重临时调高避免中途放弃。等变道完成再恢复到正常权重。这其实是在Lattice外围做了一层简单的决策逻辑但非常有效。5.4 独门技巧离线回放调参我强烈建议所有人在上车之前先做“离线回放”。把实车采集的/planning、/localization、/perception话题数据录成bag然后在离线环境里重放把规划结果和实际行驶轨迹叠加在一起看。这一步能做两件事第一快速重现问题不用在车上反复试第二调参后立刻看效果不用一趟一趟跑场地。我们团队后来形成了一条习惯任何参数修改先在离线数据上跑一遍对比验证有效再上实车。整个Lattice调参时间大概缩短了三分之二。6. Lattice Planner的上限与适用边界讲到这里还是要泼一盆冷水Lattice Planner不是万能的。它的采样式思想决定了它在高维复杂场景里会力不从心。6.1 什么时候该用Lattice什么时候该换方案Lattice非常适合结构化程度高的场景园区接驳、封闭停车场、高速巡航、城市快速路。在这些场景里道路结构清晰、驾驶意图明确、障碍物类型相对简单Lattice的效率和可靠性都很好。但如果你要做开放道路、复杂城区、大量行人非机动车混行或者需要做高度博弈的变道/汇入决策Lattice的结构就绷不住了。不是说完全不能跑而是你为了让它跑通会在外围堆越来越多的规则和状态机最后整个系统变得又复杂又脆弱。这时候EM Planner的迭代优化思路甚至基于学习的算法可能更合适。但它们对团队工程能力的要求也高很多。很多团队其实是在Lattice外面包一层行为决策用“场景分类规则限制”的方式把复杂场景拆成一个个简单场景每个简单场景再交给Lattice去算。这算是工业界最务实的做法。6.2 往后的扩展方向从Lattice到 learning-based planner在Lattice框架上做扩展比较现实的路径是把采样过程oracle化——用学习模型预测更可能安全的终态而不是靠固定网格或者用强化学习调整代价函数权重让行为风格能自适应场景。这些方向都有学术界在推进但距离工程成熟还有距离。对大多数自动驾驶团队来说把Lattice吃透、调好、再加上一层稳定的决策状态机已经能覆盖非常多的量产项目需求了。结尾我在实际项目里把Lattice Planner从看懂到调好前后折腾了快两个月。回头看真正卡住我的不是代码读不懂而是“采样范围没覆盖安全解”和“代价函数权重和场景不匹配”这两个问题反复出现。如果你也在啃这套算法我的建议是先别急着调参先把每一帧规划里的采样轨迹、碰撞检测结果、代价分数三项数据打印出来盯着看几帧你对这套算法的理解会立刻上一个台阶。等你把横向偏移集合、纵向时间采样密度、代价权重这三组参数和你自己的场景对齐了这辆车开着顺手了你才算真正把Lattice Planner变成了自己的工具。