目录
[3.1 车应该走哪条路？](#3.1 车应该走哪条路？)
[3.2 车应该以什么速度走？](#3.2 车应该以什么速度走？)
[四、Hybrid A*：会"开车"的 A*](#四、Hybrid A*：会“开车”的 A*)
[4.1 为什么普通 A* 不够用？](#4.1 为什么普通 A* 不够用？)
[4.2 Hybrid A* 的核心思想](#4.2 Hybrid A* 的核心思想)
[4.3 Hybrid A* 的车辆模型](#4.3 Hybrid A* 的车辆模型)
[4.4 Hybrid A* 的代价函数](#4.4 Hybrid A* 的代价函数)
[4.5 启发函数为什么重要？](#4.5 启发函数为什么重要？)
[4.6 解析扩展 Analytic Expansion](#4.6 解析扩展 Analytic Expansion)
[4.7 Hybrid A* 的优缺点](#4.7 Hybrid A* 的优缺点)
[4.8 Hybrid A* 算法运行期望结果：](#4.8 Hybrid A* 算法运行期望结果：)
[五、Lattice Planner：用"动作积木"拼轨迹](#五、Lattice Planner：用“动作积木”拼轨迹)
[5.1 什么是 Lattice？](#5.1 什么是 Lattice？)
[5.2 Lattice 的核心思想](#5.2 Lattice 的核心思想)
[5.3 Lattice 的状态怎么表示？](#5.3 Lattice 的状态怎么表示？)
[5.4 什么是 Motion Primitive？](#5.4 什么是 Motion Primitive？)
[5.5 Lattice Planner 的整体流程](#5.5 Lattice Planner 的整体流程)
[5.6 Frenet 坐标系是什么？](#5.6 Frenet 坐标系是什么？)
[5.6.1 为什么要用 Frenet 坐标？](#5.6.1 为什么要用 Frenet 坐标？)
[5.6.2 Frenet 坐标怎么表示？](#5.6.2 Frenet 坐标怎么表示？)
[5.6.3 Frenet 坐标和笛卡尔坐标的区别](#5.6.3 Frenet 坐标和笛卡尔坐标的区别)
[5.7 Frenet 坐标系和 Lattice Planner 有什么关系？](#5.7 Frenet 坐标系和 Lattice Planner 有什么关系？)
[5.7.1 先说结论](#5.7.1 先说结论)
[5.7.2 为什么它们经常一起出现？](#5.7.2 为什么它们经常一起出现？)
[5.8 Lattice Planner 的代价函数怎么设计？](#5.8 Lattice Planner 的代价函数怎么设计？)
[5.9 Lattice Planner 的优点](#5.9 Lattice Planner 的优点)
[5.10 Lattice Planner 的缺点](#5.10 Lattice Planner 的缺点)
[1）Motion primitive 库设计复杂](#1）Motion primitive 库设计复杂)
[5.11 Lattice 适用于哪些场景？](#5.11 Lattice 适用于哪些场景？)
[5.12 一句话总结 Lattice Planner](#5.12 一句话总结 Lattice Planner)
[六、EM Planner：工程界非常经典的"路径-速度解耦"方案](#六、EM Planner：工程界非常经典的“路径-速度解耦”方案)
[6.1 EM Planner 到底是什么？](#6.1 EM Planner 到底是什么？)
[6.2 为什么要用 Frenet 坐标？](#6.2 为什么要用 Frenet 坐标？)
[6.3 EM Planner 的总体流程](#6.3 EM Planner 的总体流程)
[第三步：路径 DP 粗搜索](#第三步：路径 DP 粗搜索)
[第四步：路径 QP 精优化](#第四步：路径 QP 精优化)
[第五步：速度 DP 粗搜索](#第五步：速度 DP 粗搜索)
[第六步：速度 QP 精优化](#第六步：速度 QP 精优化)
[6.4 为什么是 DP + QP？](#6.4 为什么是 DP + QP？)
[1）DP 解决什么？](#1）DP 解决什么？)
[2）QP 解决什么？](#2）QP 解决什么？)
[6.5 EM Planner 里的路径代价函数](#6.5 EM Planner 里的路径代价函数)
[6.6 SL 与 ST 是什么？](#6.6 SL 与 ST 是什么？)
[1）SL 图](#1）SL 图)
[2）ST 图](#2）ST 图)
[6.7 EM Planner 的优缺点](#6.7 EM Planner 的优缺点)
[7.1 从本质上看](#7.1 从本质上看)
[7.2 从状态表示看](#7.2 从状态表示看)
[7.3 从输出结果看](#7.3 从输出结果看)
[7.4 从适用场景看](#7.4 从适用场景看)
[8.1 Hybrid A* 和 Lattice 都考虑车辆约束，它们一样吗？](#8.1 Hybrid A* 和 Lattice 都考虑车辆约束，它们一样吗？)
[8.2 为什么工程里经常不是只用一种算法？](#8.2 为什么工程里经常不是只用一种算法？)
一、为什么一定要理解这三个算法？
在自动驾驶、移动机器人、AGV、无人车路径规划里，很多初学者一开始会接触 A*、Dijkstra、RRT 这些名字，但真正进入工程系统后，很快就会遇到三个更"经典且实用"的概念：
- Hybrid A*
- Lattice Planner
- EM Planner
它们之所以重要，是因为它们都在解决同一个核心问题：
如何让车在复杂环境中，既安全避障，又满足车辆自身运动约束，还能输出尽量平滑、可跟踪、舒适的轨迹。
普通图搜索方法虽然能求出一条路，但那条路未必"车能开"；纯优化方法虽然结果很平滑，但又容易陷入局部最优或者对初始化特别敏感。因此，经典自动驾驶规划算法的发展，本质上就是在下面几件事之间做平衡：
- 可行性
- 实时性
- 平滑性
- 安全性
- 工程可落地性
Dolgov 的 Hybrid A* 来源于 DARPA Urban Challenge 的经典实践，State Lattice 则是将运动原语和图搜索结合起来的一条重要路线，Apollo 的 EM Planner 面向量产级自动驾驶系统。它们分别代表了三种不同的规划思想。
二、先用一句话理解三者
初学者最怕一上来全是概念，所以先记住这三句：
Hybrid A* ：
把普通 A* 变成"考虑车辆朝向和转弯约束"的 A*。Lattice Planner ：
先准备一堆车能够执行的小动作，再从这些动作里拼出一条轨迹。EM Planner ：
把"从哪里绕过去"和"用什么速度通过"拆开，先粗搜再精修。
如果你先把这个总印象建立起来，后面再看公式和流程就不会乱。三者分别对应的核心思想是：Hybrid A* 偏带约束搜索 ，Lattice 偏运动原语图搜索 ，EM Planner 偏搜索与优化结合的工程框架。
三、规划问题到底在求什么？
在讲具体算法前，先明确"规划"到底在求什么。
对于一辆自动驾驶车辆或移动机器人，规划模块通常要回答两个问题：
3.1 车应该走哪条路？
这对应的是****路径规划 Path Planning。
输出通常更偏几何意义，比如一条二维曲线或参考线。
3.2 车应该以什么速度走？
这对应的是速度规划 Speed Planning。
输出通常更偏时序意义，比如什么时候加速、减速、跟车、停车。
如果把两者再进一步组合起来，最终真正需要的是轨迹规划 Trajectory Planning，也就是：
它表示车辆在每个时刻的位置、姿态、速度等状态。
这也是为什么很多经典算法并不是单纯"找一条线"，而是在做"路径 + 速度 + 约束"的综合求解。**Apollo EM Planner 就是典型代表，它显式把路径和速度拆开处理；**而 Hybrid A* 与 Lattice 更常先解决几何可行路径，再配合控制或速度模块形成完整轨迹。
四、Hybrid A*：会"开车"的 A*
4.1 为什么普通 A* 不够用？
普通 A* 非常经典，核心评价函数是：
其中：
- g(n)：起点到当前节点的实际代价
- h(n)：当前节点到终点的启发式估计代价
普通 A* 非常适合在二维栅格地图里求最短路径 ，但问题是：
车辆不是一个可以横着平移的点。
一辆真实车辆通常有这些特性：
- 它有朝向 θ
- 它不能原地横移
- 它转弯半径有限
- 它可能需要前进/倒车切换
- 它的轨迹要尽量平滑
所以普通 A* 找到的折线路径，往往在几何上可达，但在车辆运动学上并不可执行。Dolgov 等人在经典论文中明确指出，普通离散搜索容易产生不平滑且不满足非完整约束的路径，而 Hybrid A* 正是为了解决这一问题。
4.2 Hybrid A* 的核心思想
Hybrid A* 的关键点是：
搜索框架还是 A*，
但节点扩展方式不再是简单的网格邻接，而是按车辆运动学模型生成后继状态。Hybrid A*规划的路径考虑了车辆的运动学约束，即满足了车辆的最大曲率约束。
它不再只把状态写成二维位置：
(x,y)
而是通常写成：
(x,y,θ)
这里：
- x,y：车辆位置
- θ：车辆航向角
这样搜索时，每个节点都不只是"走到某个格子"，而是"车辆以某个姿态到达了某个位置"。
Dolgov 的论文中将其描述为：在离散网格单元上关联连续 3D 车辆状态，从而在保留 A* 搜索效率的同时，得到可驾驶而不是单纯折线的路径。
4.3 Hybrid A* 的车辆模型
Hybrid A* 扩展节点时，常用简化自行车模型。其连续运动学模型通常写为：
其中：
- v：车辆速度
- L：轴距
- δ：前轮转角
这组方程的含义非常直观：
- 车前进方向由航向角 θ 决定
- 转得快不快与速度和转角有关
- 轴距越长，同样转角下转弯越慢
Hybrid A* 就是在节点扩展时，对若干个控制动作进行短时积分，例如：
- 前进 + 左转
- 前进 + 直行
- 前进 + 右转
- 倒车 + 左转
- 倒车 + 直行
- 倒车 + 右转
于是生成的后继节点天然带有车辆运动学约束。
4.4 Hybrid A* 的代价函数
虽然基础形式还是
但在工程里，g(n)往往不只是路径长度，还会加入很多惩罚项，比如：
可以理解为：
- L：路径长度代价
- Creverse：倒车惩罚
- Cswitch：前进/倒车切换惩罚
- Cκ：曲率惩罚
- Cobs：靠近障碍物惩罚
这样一来，规划器不会只盯着"最短"，还会偏向于：
- 少倒车
- 少换挡
- 少急转弯
- 离障碍物更安全
这也是 Hybrid A* 比普通 A* 更像"工程规划器"的地方。
4.5 启发函数为什么重要？
A* 的搜索效率极大程度依赖启发函数 h(n)。
Hybrid A* 常见两类启发：
（1）忽略障碍物，但考虑车辆运动学
例如用 Dubins 或 Reeds-Shepp 距离近似到目标的最短可行路径长度。
（2）考虑障碍物，但忽略车辆朝向
例如在二维占据栅格上先做一次普通最短路传播，得到 obstacle-aware 的距离估计。
Dolgov 论文中明确提到其搜索由两个启发式共同引导：
一个是"non-holonomic without obstacles"，也就是考虑车辆非完整约束但忽略障碍；
另一个是基于二维障碍地图的启发，用于引导搜索避开 U 形障碍和死胡同。
这也是面试里很常见的一问：
为什么 Hybrid A* 不直接用欧氏距离？
因为欧氏距离只知道"多远"，不知道"车怎么过去"。
4.6 解析扩展 Analytic Expansion
Hybrid A* 还有一个非常经典的技巧，叫解析扩展。
意思是：
当当前节点已经比较接近目标时，不再一步一步扩展，而是直接尝试用一条解析可行曲线将当前状态连接到目标状态。
常见就是 Reeds-Shepp 曲线。
如果这条解析连接无碰撞，就可以直接结束搜索。
这样做的优点是：
- 提高搜索速度
- 提升终点连接精度
- 更容易得到完整可行解
Dolgov 论文在 "Analytic Expansions" 一节中明确描述了这一过程，并指出用 Reeds-Shepp 模型增强搜索可提升效率。
4.7 Hybrid A* 的优缺点
（1）优点
它能直接处理车辆朝向、最小转弯半径和前进/倒车问题，所以非常适合：
- 自动泊车
- 窄路掉头
- 园区低速绕障
- 非结构化低速场景
Dolgov 论文报告，其算法用于 DARPA Urban Challenge 中的 Junior 车辆，典型全周期重规划时间可做到 50--300ms，且能处理停车场、掉头等复杂任务。
（2）缺点
它的问题也很明显：
- 状态维数比普通 A* 大很多
- 角度离散、步长设置会强烈影响结果
- 路径虽然可行，但往往还需要再平滑
所以很多实际系统会采用：
Hybrid A* + 平滑优化
这也是它经典而不过时的原因：它非常擅长先给你找出一条"肯定能走"的路。
4.8 Hybrid A* 算法运行期望结果：
Hybrid A* + ReedShepp曲线规划效果：
五、Lattice Planner：用"动作积木"拼轨迹
5.1 什么是 Lattice？
Lattice 的中文通常翻译为"格"或"晶格"，但在路径规划里，它并不是普通的二维网格，而是更准确的 状态格（State Lattice）。
它的核心思想可以概括成一句话：
把车辆可达状态离散成图，再用一组满足运动约束的可执行轨迹把这些状态连接起来。
所以，Lattice Planner 不是简单地在地图上"找一条线"，而是在状态空间 里找一串可以真正开出来的动作。Pivtoraiko 与 Kelly 的经典工作正是用 motion primitives 构成状态格，把受微分约束的连续规划问题转成图搜索问题。
5.2 Lattice 的核心思想
Lattice 的关键不是"节点很多"，而是：
边本身就是车能执行的一小段轨迹。
这和普通 A* 很不一样。
普通 A* 的边更像是"格点和格点之间的连接关系"；
而 Lattice 的边，是提前设计好的可行运动原语。
你可以把它理解成一盒"动作积木"：
- 直行一小段
- 小曲率左转
- 小曲率右转
- 大曲率左转
- 大曲率右转
- 倒车弧线
- 换道轨迹片段
规划时，系统不是临时从零造轨迹，而是：
- 先准备好一批合法动作
- 再从这些动作里挑出一串最优组合
- 最后把它们拼成完整轨迹
这就是为什么 Lattice 很强调可执行性。因为它从一开始就考虑了车辆约束，而不是先找一条几何路径，再想办法修。Pivtoraiko 的论文也强调，state lattice 的优势就在于把 mobility constraints 直接编码进 primitives 和图结构中
5.3 Lattice 的状态怎么表示？
Lattice 常见状态可以写成：
或者再进一步写成：
其中 κ 表示曲率。
如果考虑更加平滑和更容易控制的轨迹，加入曲率会更合理，因为车辆控制往往和曲率、曲率变化率密切相关。
再进一步，还可以加入速度、时间等变量，例如：
当状态维度越来越高时，Lattice 的表达能力会增强，但搜索复杂度也会迅速上升。CMU 的 conformal spatiotemporal lattice 就是在道路环境下，把静态状态格进一步扩展到速度与时间维度中，以适应结构化道路和动态交通参与者
5.4 什么是 Motion Primitive？
这是 Lattice 最重要的关键词。
Motion Primitive = 运动原语 = 可执行小动作模板
它本质上是一小段满足车辆运动学约束的轨迹 。
这段轨迹从一个离散状态出发，经过固定弧长、固定时间，或者满足某个终端约束后，到达另一个离散状态。
说得更直白一点：
Motion Primitive 就是"提前算好的合法驾驶动作"。
搜索时直接拿来拼，而不是现场从零推导。
这些原语一般有几种生成方式：
- 基于车辆模型做数值积分
- 求解边值问题
- 用数值优化离线生成
- 根据对称性和分辨率完备性设计控制集合
Pivtoraiko 的研究里还特别强调一件事：
原语库不能太大，也不能太小。
- 太小：可表达的轨迹不够，容易找不到解
- 太大：每个节点分支数暴涨，搜索会变慢
所以 motion primitive 库设计，其实是 Lattice Planner 里最核心、也最工程化的一步。
5.5 Lattice Planner 的整体流程
Lattice Planner 的流程可以概括为下面五步。
第一步：离散状态空间
先确定状态变量要怎么离散，例如：
- 位置怎么采样
- 航向角怎么分桶
- 曲率要不要离散
- 速度、时间要不要纳入状态
这一步决定了状态格的分辨率。
分辨率越高，表达越精细；但图也会越大。
第二步：生成或加载运动原语库
给每个状态定义一组允许的后继动作。
这些动作本质上就是从当前状态出发可以走的轨迹模板。
第三步：碰撞检测
对每条原语对应的轨迹进行检查，判断是否与障碍物、道路边界或其他约束冲突。
注意，这里检测的不是一条直线，而是一整段轨迹。
所以通常需要沿轨迹采样多个点进行碰撞检测。
第四步：图搜索
在状态格上运行搜索算法，例如：
- A*
- Dijkstra
- D* Lite
搜索的目标，是找到一串总代价最小的 motion primitives。
也就是说，最终得到的是一串"动作序列"，而不只是几个离散点。
第五步：轨迹细化
拼出来的路径通常已经具备可执行性，但在工程系统中，往往还会进一步：
- 做平滑
- 匹配速度曲线
- 转成控制器更容易跟踪的形式
这样更利于实际跟踪控制。
5.6 Frenet 坐标系是什么？
这一节非常重要，因为很多人学 Lattice Planner 时，最容易在这里混淆。
5.6.1 为什么要用 Frenet 坐标？
在普通笛卡尔坐标系中，车辆位置通常写成：
但在自动驾驶道路场景里，车辆大多数时候不是在"任意平面"上运动，而是沿着道路参考线附近前进。
这时如果还一直用全局 x,y 来做规划，会有两个问题：
- 很难直接表达"沿车道前进了多少"
- 很难直接表达"离车道中心偏了多少"
于是就引入了 Frenet 坐标系 。
它是以一条参考线为基准建立的局部坐标系，把车辆状态拆成：
- 沿参考线方向的纵向位置
- 相对参考线的横向偏移
这样一来，很多道路规划问题就会变得更自然。Apollo 的工程实现里大量使用了参考线下的 SL 坐标与 Frenet 表达，路径优化和时空约束也都是围绕它展开的。
5.6.2 Frenet 坐标怎么表示？
Frenet 坐标最常见写成：
其中：
- s：沿参考线的弧长坐标，也可以理解为"往前走了多远"
- l：相对参考线的横向偏移，也就是"偏离中心线多少"
如果考虑速度和动态信息，还常见写成：
或者进一步加入加速度项。
这里可以这样直观理解：
- s 负责回答：车沿路走到哪了？
- l 负责回答：车偏到哪条横向位置了？
所以在结构化道路中：
- 保持车道：就是让 l 维持在较小范围
- 变道：就是让 l 从当前车道中心逐渐过渡到目标车道中心
- 跟车/超车：主要体现在 s 和时间维度上的变化
这也是 Frenet 坐标特别适合道路规划的原因。
5.6.3 Frenet 坐标和笛卡尔坐标的区别
两者可以简单对比如下：
笛卡尔坐标系：
更适合描述"车在全局地图上的绝对位置"。
Frenet 坐标系：
更适合描述"车相对道路参考线的位置关系"。
所以如果场景是：
- 停车场
- 非结构化空旷区域
- 任意平面绕障
那么直接用 (x,y,θ) 的 state lattice 会更自然。
如果场景是：
- 车道级道路
- 高速公路
- 城市结构化车道
那么 Frenet 坐标通常更好用，因为它天然贴合道路几何。CMU 在 conformal spatiotemporal lattice 中也强调，传统 lattice 更适合停车场等非结构化场景，而在 on-road driving 中，需要让 lattice conform to road structure。
5.7 Frenet 坐标系和 Lattice Planner 有什么关系？
这也是最容易被问到的问题。
5.7.1 先说结论
Frenet 不是 Lattice，本身也不是一种规划算法。
Frenet 是一种坐标表达方式。
Lattice 是一种规划框架。
两者不是同一个层级的概念。
5.7.2 为什么它们经常一起出现？
因为在自动驾驶道路场景中，Lattice Planner 往往会放到 Frenet 坐标下去实现。
原因很简单：
- 在 Frenet 坐标下，车道保持、变道、跟车、绕障都更容易表示
- 候选轨迹的横向和纵向行为可以更方便地分解
- 相对道路参考线的约束更容易编码进代价函数
所以很多工程实现会变成这样：
- 先在 Frenet 坐标系下采样候选状态
- 生成横向轨迹和纵向轨迹
- 把它们组合成候选轨迹
- 再根据碰撞、舒适性、参考速度等指标筛选最优解
Apollo 的 Lattice Planner 就非常典型：其工程实现更偏向 Frenet 下的轨迹采样 + 组合 + 打分，而不是最原始 textbook 风格的"规则状态格 + 原语边"图示。
5.8 Lattice Planner 的代价函数怎么设计？
Lattice Planner 很适合做工程化代价设计。
因为每条原语、每条候选轨迹都可以单独计算代价。
常见形式可以写成：
其中可能包括：
- 路径长度代价
- 曲率或转向代价
- 障碍物安全距离代价
- 偏离车道中心代价
- 舒适性代价
如果是在 Frenet 坐标下，还可以更自然地设计：
- 纵向目标代价
- 横向偏移代价
- 换道代价
- 跟车安全代价
- 时空冲突代价
所以 Lattice Planner 的一个很大优势，就是：
它不仅能找到路，还能通过代价函数体现"这条路开得好不好"。
Apollo 相关资料里，路径和速度的候选解会围绕参考线、障碍物、舒适性和目标行为做综合评分，这正是 Lattice 工程化优势的体现。
5.9 Lattice Planner 的优点
1）可执行性强
因为边本身就是满足约束的轨迹，所以不像普通网格搜索那样容易出现"路径连得上，但车开不出来"的问题。
2）很适合车辆约束建模
最小转弯半径、曲率限制、倒车约束、速度约束等，都可以自然融入状态和原语设计中。
3）工程可解释性强
原语库、代价函数、碰撞约束都比较直观，调参逻辑也相对清晰。
4）适合道路场景扩展
结合 Frenet 坐标后，Lattice 很适合做：
- 车道保持
- 变道
- 跟车
- 超车
- 障碍物绕行
5.10 Lattice Planner 的缺点
1）Motion primitive 库设计复杂
这是 Lattice 最难的部分。
原语太少，表达能力不足；原语太多，计算复杂度升高。
2）状态维度一高，搜索会很重
如果把位置、航向、曲率、速度、时间都纳入状态，状态空间会迅速膨胀。
3）非常依赖场景结构和工程设计
Lattice 不是那种"套个模板就一定好用"的方法。
它往往很强，但也很吃原语设计、代价设计和场景建模质量。
5.11 Lattice 适用于哪些场景？
Lattice Planner 很适合下面这些问题：
- 停车场低速机动
- 窄路掉头
- 结构化道路中的车道保持
- 变道与绕障
- 高速公路驾驶行为规划
如果是非结构化环境 ，更常见的是笛卡尔坐标下的 state lattice；
如果是结构化道路环境，更常见的是结合 Frenet 坐标的 lattice 风格规划。
5.12 一句话总结 Lattice Planner
Lattice Planner 的本质不是在地图上随便找一条线，而是：
在离散状态空间中，用一组满足车辆约束的运动原语搜索一条最优、可执行的轨迹。
而 Frenet 坐标系的作用则是：
把道路场景下的规划问题，变成"沿参考线前进 + 横向偏移控制"的形式，从而更适合车道级自动驾驶。
所以在自动驾驶里，常见的其实不是"Lattice 和 Frenet 二选一"，而是：
在 Frenet 坐标下实现 Lattice 风格的轨迹采样与搜索。
六、EM Planner：工程界非常经典的"路径-速度解耦"方案
6.1 EM Planner 到底是什么？
EM Planner 最经典的来源之一是 Apollo 的 Baidu Apollo EM Motion Planner 论文。论文中明确说明，该规划器在车道级轨迹生成中，基于 Frenet 坐标系迭代求解路径与速度优化问题，并采用动态规划 + 样条二次规划的组合方式来兼顾障碍决策、交通规则与平滑性。
所以 EM Planner 不是单个"公式算法"，而更像一个规划框架。
它的核心思想是：
- 先把复杂的运动规划问题投影到参考线上
- 再把问题拆成路径和速度两个子问题
- 每个子问题先做离散粗决策，再做连续精优化
- 反复迭代直到得到可行又平滑的结果
6.2 为什么要用 Frenet 坐标？
普通笛卡尔坐标下，直接规划车辆轨迹会同时耦合：
- 前进距离
- 横向偏移
- 速度变化
- 动态障碍交互
问题维度很高。
于是 Apollo 采用参考线坐标系，也就是 Frenet 坐标。
在这个坐标里，一个位置可以写成：
- s：沿参考线的纵向弧长
- l：相对参考线的横向偏移
于是路径问题可以表示为：
而速度问题可以表示为：
这样一来，"横向绕障"和"纵向调速"就被拆开了。Apollo 论文明确指出，其 lane-level trajectory generator 在 Frenet frame 中迭代求解路径和速度优化。
6.3 EM Planner 的总体流程
EM Planner 的典型流程可以概括成下面六步。
第一步：构建参考线
根据地图和导航结果，先得到一条参考线。
第二步：障碍物投影
把静态和动态障碍物投影到 Frenet 空间中。
Apollo 论文中分别讨论了 SL 投影与 ST 投影。
第三步：路径 DP 粗搜索
在 s−l 空间里先做动态规划，粗略决定：
- 从障碍物左边绕还是右边绕
- 大体走廊在哪里
第四步：路径 QP 精优化
在 DP 给出的可行走廊中，再做二次规划，得到更平滑的路径 l=f(s)。
第五步：速度 DP 粗搜索
在 s−t 空间中做动态规划，处理：
- 跟车
- 超车
- 让行
- 停车
- 障碍物通行时序
第六步：速度 QP 精优化
在可行 ST 区域内生成平滑的 s(t)，并满足速度、加速度、jerk 等约束。
Apollo 论文的摘要中直接指出：该系统在路径与速度优化中采用 dynamic programming 与 spline-based quadratic programming 的组合框架。
6.4 为什么是 DP + QP？
这点特别重要，也是面试高频问题。
1）DP 解决什么？
DP 擅长处理离散非凸决策 。
比如：
- 障碍物从左边绕还是右边绕
- 当前是跟车还是减速让行
- 哪一条走廊更安全
这些问题往往不适合直接做成光滑凸优化。
2）QP 解决什么？
QP 擅长在一个确定的可行区域内，求一条平滑、连续、舒适的曲线。
因此：
- DP 决定"大的决策方向"
- QP 决定"具体怎么走得更顺"
Apollo 论文中，路径优化被分成 dynamic-programming-based path decision 和 spline-based path planning，两者配合完成从粗决策到连续优化的过程。
6.5 EM Planner 里的路径代价函数
Apollo 论文给出的一个典型形式是：
也就是总路径代价由三部分组成：
- 平滑项
- 障碍物代价
- 引导项/车道引导代价
其中平滑项又可写成：
这几个导数项非常好理解：
- f′(s)：与航向偏差相关
- f′′(s)：与曲率相关
- f′′′(s)：与曲率变化率相关
也就是说，EM Planner 不只是要让你"绕过去"，还要让这条路：
- 不要左右摇摆太厉害
- 不要曲率突变
- 不要方向盘变化太激烈
Apollo 论文在路径 DP/QP 部分明确给出了这一类平滑代价。
6.6 SL 与 ST 是什么？
这两个图是理解 EM Planner 的钥匙。
1）SL 图
横轴是 s，纵轴是 l。
它主要回答的问题是：
我在路径上往前走时，横向该如何偏移？
也就是决定"从左绕还是从右绕"。
Apollo 论文 Figure 4 展示了一个 oncoming traffic 的 SL projection 示例：障碍物被投影到 Frenet 平面后，形成需要规避的区域。
2）ST 图
横轴是时间 t，纵轴是纵向距离 s。
它主要回答的问题是：
我应该什么时候走到哪里？
也就是决定：
- 减速还是加速
- 先让还是先过
- 何时与动态障碍产生冲突
Apollo 论文 Figure 5 展示了 cut-in obstacle 和后车在 ST 图中的示意，剩余区域即为速度规划的可行空间。
6.7 EM Planner 的优缺点
1）优点
EM Planner 的优势非常明显：
- 特别适合道路结构清晰的自动驾驶场景
- 容易融入车道线、交通规则、参考线信息
- 路径和速度分解后，实时性更容易做
- 有 DP 和 QP 两阶段，工程上稳定性较好
Apollo 论文明确将其定位为面向工业级 L4 自动驾驶的实时规划系统，并强调安全、舒适与可扩展性。
2）缺点
它也有明显局限：
- 很依赖参考线质量
- 路径和速度解耦本身是一种近似
- 系统实现复杂度很高
- 在非常非结构化场景里不一定最优
所以 EM Planner 往往更适合：
- 高速道路
- 城市道路
- 车道级驾驶
- 规则相对明确的结构化环境
七、三者到底有什么区别？
这一节是博客里最关键的总结部分。
7.1 从本质上看
Hybrid A* 的本质是：
带车辆运动学约束的启发式搜索。
它最核心的是"搜"。
Lattice的本质是：
基于状态格与运动原语的图搜索。
它最核心的是"预定义动作，再搜索"。
EM Planner的本质是：
路径-速度解耦下的搜索与优化结合框架。
它最核心的是"分解问题，再粗搜和精修结合"。
这些定位与三类代表性文献的描述是一致的。
7.2 从状态表示看
Hybrid A* 常见状态是：
Lattice 常见状态是：
EM Planner 更常使用 Frenet 坐标：
以及时间扩展：
也就是说：
- Hybrid A* 强调车辆姿态搜索
- Lattice 强调状态与动作结构化表示
- EM Planner 强调参考线下的路径/速度拆解
7.3 从输出结果看
Hybrid A* 输出的通常是一条可行几何路径，后续常要继续平滑。
Lattice 输出的是由原语拼接成的可执行路径或轨迹。
EM Planner 则更直接面向带时间分配的时空轨迹。
7.4 从适用场景看
Hybrid A* 更适合：
- 自动泊车
- 低速狭窄空间
- 需要强可达性的场景
Lattice 更适合：
- 结构化道路
- 车道级路径生成
- 需要显式考虑运动原语的规划
EM Planner 更适合：
- 城市道路自动驾驶
- 高速/城区车道级规划
- 需要路径和速度联合处理的工业系统
八、初学者最容易混淆的几个点
8.1 Hybrid A* 和 Lattice 都考虑车辆约束，它们一样吗？
不一样。
Hybrid A* 是在线扩展控制动作 ，更像"边搜边生孩子"。
Lattice 是提前准备好动作模板，更像"先备积木再拼图"。
前者更灵活，后者更结构化。
8.2 为什么工程里经常不是只用一种算法？
因为单一方法通常只能兼顾一部分需求。
- 纯搜索：可行性强，但可能不够平滑
- 纯优化：平滑性强，但容易非凸
- 纯采样/原语：结构清楚，但设计复杂
所以工程里经常出现：
- Hybrid A* + smoothing
- Lattice + optimization
- DP + QP
- 搜索 + 预测 + 优化
Apollo EM Planner 就是这种"工程融合思维"的典型代表。
九、总结
Hybrid A*、Lattice 和 EM Planner 都是自动驾驶中非常经典的规划方法，但三者代表了三条不同思路。
- **Hybrid A***是从传统图搜索演化而来，通过把车辆朝向和运动学模型加入搜索过程，使得结果更接近真实车辆可执行路径，尤其适合泊车和低速复杂场景。
- Lattice则通过构建状态格和运动原语，把可执行动作提前离散化，再在这张图上进行搜索，因此它天然具有较好的可行性和结构性，尤其适合结构化道路和工程化系统。
- EM Planner 更偏向工业级规划框架，它依托 Frenet 坐标，将路径和速度拆开处理，并通过 DP 做粗决策、QP 做精优化，最终生成兼顾安全、舒适与实时性的轨迹。Apollo 的实践表明，这种方法在自动驾驶量产系统中具有很高的工程价值。
如果只用一句话概括三者的区别，那就是：
Hybrid A* 在"搜一条车能开的路"；
Lattice 在"拼一条车能执行的路"；
EM Planner 在"决定怎么绕、何时过，并把结果优化得更平滑"。