夜雨聆风
上一篇讲了Apollo规划怎么选场景、怎么拉参考线、怎么把车扔进Frenet坐标系。这篇聊核心：拿到参考线之后，轨迹到底怎么算出来。
Apollo 6.0的LANE_FOLLOW场景里跑着两个规划器：EM Planner和Lattice Planner。一个迭代优化，一个采样搜索，思路完全不同，但都在100毫秒内交出答案。
EM Planner：猜一轮，磨一轮
EM的名字来自期望最大化（Expectation-Maximization），但跟统计学习那个EM不是一回事。Apollo的EM是把轨迹规划拆成路径和速度两个子问题，交替求解直到收敛。
为什么拆？三维SLT空间直接优化是非凸的。你站在路口，左边绕还是右边绕，这是非凸决策；先走还是先让，也是非凸决策。非凸空间里QP（二次规划）这种凸优化工具直接歇菜。所以Apollo的做法是：先用DP（动态规划）在离散网格上猜一个粗解，把非凸空间切成凸域，再让QP在凸域里精磨。
E-Step：在SL图上画弯路
E-Step解决的是"车往哪偏"。输入是Frenet坐标系下的SL图，纵轴s是沿参考线的弧长，横轴l是横向偏移。
第一步：SL投影。 障碍物投影到SL平面上变成占据区。注意，只有静态障碍物和低速/对向动态障碍物才会投影进SL图。高速动态障碍物不在这一步处理——后面M-Step的ST图才是它的主场。
第二步：DP粗搜索。 在s方向等间距采样，每个s位置沿l方向取若干采样点，用DP搜索一条代价最小的粗路径。代价函数三项：偏离参考线+靠近障碍物+平滑性（航向角/曲率/曲率变化率）。DP搜出来的路径是折线，但已经做了关键决策——从障碍物左边绕还是右边绕。
第三步：建可行走廊。 以DP路径为骨架，向两侧扩展出凸的安全区间（Feasible Tunnel）。这个走廊是凸的，为QP准备。
第四步：QP样条平滑。 路径用五次多项式样条表示，分段拼接。目标函数四项加权：平滑度（l'²+l''²+l'''²积分）+参考线贴近+障碍物距离+DP路径引导。约束包括：起点终点边界条件（l, l', l''已知）、段间连续性（C0/C1/C2）、安全边界（不出走廊）、动力学可行性（曲率限制）。
QP求解器用的OSQP，上一轮的结果做热启动，平均3毫秒解完。
M-Step：在ST图上赶时间
M-Step解决的是"车跑多快"。输入是ST图，横轴t是时间，纵轴s是沿路径的位置。
第一步：ST投影。 障碍物投影到ST平面上变成占据区。这次静态、低速、高速、对向障碍物全都进来了。高速障碍物在ST图上是一条斜线——它随着时间在s方向移动。
第二步：DP粗搜索。 在t方向等间距采样，每个时刻取s方向若干点，DP搜索一条粗速度曲线。代价函数：参考速度跟踪+S''²加速度+S'''²加加速度+障碍物代价。
第三步：建ST隧道。 以DP速度曲线为骨架，扩展凸的安全区间。
第四步：QP样条平滑。 同样五次多项式样条，额外加单调性约束（车只能往前开，s(t)必须递增）。
E和M怎么迭代
关键来了。E-Step优化路径时，用的是上一轮M-Step的速度曲线来判断动态障碍物的交互位置。M-Step优化速度时，用的是本轮E-Step刚算出的路径。
举个论文里的例子：对向来车。第一轮E-Step，用默认速度估出在s=40m处跟对向车交汇，于是路径往右偏。M-Step发现按新路径，实际交汇点变成了s=30m。第二轮E-Step把交汇点更新到s=30m，路径偏移量调整。M-Step再更新速度。一般2-4轮就收敛。
Apollo论文给出的复杂度：n个障碍物，M条候选路径，N条候选速度，O(n(M+N))。实测在Nuvo-6108GC+GTX1080平台上，平均不到100毫秒。截至2018年5月，这个算法已经跑了3380小时、约68000公里的闭环测试，加上仿真环境超10万小时、百万公里。
Lattice Planner：撒网捞鱼
EM Planner是"先决策再优化"，Lattice Planner是"撒网再挑"。
核心思路：在规划时域T内，对终端状态做离散采样，每个终端状态用五次多项式曲线连到起点，生成一条候选轨迹，最后按代价函数挑最好的。
终端状态采样
终端状态三元组(s_end, l_end, v_end)，配上到达时间T。
纵向s_end：根据当前速度、最大加速度和规划时域，每隔10-20m取一个点 横向l_end：当前车道中心、左右相邻车道中心，±0.5m微调 速度v_end：按限速和曲率约束取低/中/高三档 时间T：每个(s_end, l_end, v_end)配1-2个候选到达时间
快速过滤：l_end越界的扔、终点跟障碍物重叠的扔、所需纵向加速度超|a_max|的扔。
轨迹生成
每个终端状态，纵向和横向各用一条五次多项式连接起点和终点。六个边界条件定六个系数（s₀, ṡ₀, s̈₀, s_end, ṡ_end, s̈_end），解析解直接出。
代价评估
每条候选轨迹算一个总代价：平滑性（加加速度积分）+参考线偏移+障碍物距离+速度偏离。挑代价最小的。
Lattice Planner的优势是天然多模态——同时生成保持车道、换道、跟随、超车多种策略的候选轨迹，一次性比较。代价是采样密度有限，轨迹不一定全局最优。
两个规划器怎么选
Apollo 6.0里，EM Planner是LANE_FOLLOW场景的默认规划器，Lattice Planner作为备选。
实际选择看场景。动态障碍物多、需要精细交互的城区用EM Planner。结构化道路、动态环境简单的场景用Lattice Planner更快。
争论点
路径-速度解耦到底亏了多少？ EM Planner把3D SLT优化拆成2D SL和2D ST。论文承认，解耦后对动态障碍物可能次优——因为路径优化时看不到速度，速度优化时路径已经固定了。迭代能缓解但不能根治。直接3D优化（比如某些时空联合规划方案）理论上更优，但搜索空间爆炸，实时性没保证。这是一个安全性和通过性的权衡：严格约束保安全但降低通过性，放松约束提高通过性但牺牲安全。EM Planner用轻决策代替重决策，不做强规则，而是让优化器自己找平衡。
DP+QP两阶段是不是多此一举？ DP单独用，受采样网格限制，路径粗糙；QP单独用，没有凸域就没法启动。两阶段配合才能既保证全局方向对，又保证局部质量好。但这引入了一个风险：DP如果搜错了凸域（比如选了不该选的绕行方向），QP再怎么磨也是错的。这也是为什么Apollo在DP代价函数里做了大量工程调参。
下一篇聊控制模块——MPC/LQR/PID三套控制器怎么选型，对比PX4的四层串级PID。
