自动驾驶轨迹规划算法——Apollo Lattice Planner

自动驾驶轨迹规划算法——Apollo Lattice Planner

一、Apollo Lattice Planner 是什么？

1.1 定义与核心思想

Apollo Lattice Planner 是百度 Apollo 自动驾驶系统中的一款 基于采样搜索的实时轨迹规划器（Sampling-based Motion Planner）。
其核心思想是：在离散化的状态空间内，对终端状态进行系统采样，并通过轨迹生成模型（多项式曲线 / 样条曲线）连接当前车辆状态与候选终端状态，形成一组候选轨迹，然后基于代价函数选择最优轨迹。

“Lattice” 一词源于 格栅化状态空间（Lattice State Space） 的思想：

• 将车辆的可达状态按纵向距离、横向偏移、速度等维度进行规则离散（形成“格点”）；
• 每个格点都对应一条由轨迹生成器生成的可行曲线；
• 最终通过代价评估，从格点连接形成的轨迹集合中选取最优解。

在自动驾驶系统中，Lattice Planner 主要负责：

• 在考虑车辆动力学约束和障碍物分布的情况下，生成一组动态可行的候选轨迹；
• 根据代价函数从候选中选取最优轨迹，实现平滑、安全的行驶；
• 在动态环境中快速重新采样并选择新轨迹，确保实时避障与稳定行驶。

1.2 与其他规划器（如 EM、Hybrid A*）的区别

Lattice Planner 最大的不同点在于 先生成一组覆盖多种驾驶意图的候选轨迹，再用代价函数一次性评估选择最优解，而不是像 EM Planner 那样分路径与速度两阶段交替优化。

1.3 Apollo Lattice Planner 的典型应用场景

Lattice Planner 依靠其 快速采样–评估 的特点，在以下场景中具有良好表现：

1. 车道保持与换道
   • 可同时生成保持当前车道、向左换道、向右换道等多组候选轨迹，确保驾驶策略灵活。
2. 超车与避障
   • 在检测到前方障碍物时，生成绕行轨迹或减速跟随轨迹，根据代价函数选择更优方案。
3. 匝道汇入 / 驶出
   • 在高速公路汇入或驶出场景，通过在不同终端速度与终端横向位置采样，快速生成合适的接入轨迹。
4. 低速复杂环境导航
   • 在泊车场、园区等低速环境中，可通过高密度采样生成精细避障轨迹。
5. 实时动态避障
   • 虽然对动态障碍物处理不如 EM Planner 直接，但可在代价函数中引入动态预测信息，实现一定的动态避障能力。

二、Apollo Lattice Planner 的原理——Step by Step

下面用流水线视角把 Lattice 的一次规划周期讲清楚：每一步输入→处理→输出，并说明为什么要这么做、下一步如何用到这一步的结果。读完即可照着实现。

Step 0：准备与坐标系（Preprocess）

输入：HD Map 参考线、车辆当前状态 $(x_0,y_0,{\psi }_0,v_0,a_0)$、障碍物（静/动态）多边形与预测轨迹
处理：

1. 选定一条参考线（routing 给的那条），在其上构造 Frenet 坐标系 $(S,L)$；
2. 把车辆状态、车道边界、障碍物全部投影到 Frenet：得到 $S_0,L_0$ 和障碍物在 $(S,L)$ 的占据带。
   输出：参考线函数 $x_{\text{ref}}(s),y_{\text{ref}}(s),{\kappa }_{\text{ref}}(s)$、起点 $(S_0,L_0,v_0)$、障碍物在 $S!-!L$ 的占据区。
   为什么：Frenet 能把纵向进度与横向偏移解耦，后续采样与碰撞检测都更简单。
   交接：提供给 Step 1 做“终端状态采样”的空间边界与起点条件。

Step 1：终端状态采样（Where to go）

目标：在**规划时域 $T_{\text{plan}}$*内，枚举一批“**想去哪里、以多快的速度结束**”的*终端状态，形成“格点”（lattice）。

输入：起点 $(S_0,L_0,v_0)$、参考线限速与曲率上限、车道几何与障碍物占据区
处理：构造终端状态三元组

$q=(S_{\text{end}},L_{\text{end}},v_{\text{end}}),T\in [T_{\min },T_{\max }]$

• 纵向采样 $S_{\text{end}}$: 依据 $v_0,a_{\max },T_{\text{plan}}$ 取若干值（如每 10–20m 一个）。
• 横向采样 $L_{\text{end}}$: 取当前车道中心、相邻车道中心及±偏移（如 ±0.5m 微调）。
• 速度采样 $v_{\text{end}}$: 依据限速与曲率约束 $v_{\max }(s)=\min !\left(v_{\text{limit}},\sqrt{a_{y,\max }/|\kappa |}\right)$ 取低/中/高多个档。
• 时间采样 $T$: 给每个 $(S_{\text{end}},L_{\text{end}},v_{\text{end}})$ 配 1–2 个候选到达时刻（加速/匀速/减速情形）。

快速可行性过滤：

• $L_{\text{end}}$ 不得越界（车道边/硬路肩）；
• 终点邻域与障碍物重叠的直接丢弃；
• 若要在 $T$ 内达到 $S_{\text{end}},v_{\text{end}}$ 必需的纵向加速度超过 $|a_{\max }|$，丢弃。

输出：一组候选终端状态 $\mathcal{Q}=(S_e,L_e,v_e,T)$​。
为什么：把“意图空间”离散化，一次性覆盖保持车道/换道/跟随/超车等多种策略。
交接：每个 $q\in \mathcal{Q}$​ 会在 Step 2 被连接成一条实际轨迹。

Step 2：轨迹生成（Connect start → each terminal）

目标：为每个 $q$，生成一条从当前状态到终端状态、时域对齐、动力学可行的曲线。

输入：起点边界条件 $(S_0,{\dot{S}}_0=v_0,{\ddot{S}}_0=a_0)$、$(L_0,{\dot{L}}_0,{\ddot{L}}_0)$，终点 $q=(S_e,L_e,v_e,T)$
处理：纵向与横向各用五次多项式（满足位置/速度/加速度三对边界）：

$\begin{array}{rl}S(t)& =\sum _{i=0}^5{a}_i{t}^i,\{\begin{array}{l}S(0)=S_0,\dot{S}(0)=v_0,\ddot{S}(0)=a_0\\ S(T)=S_e,\dot{S}(T)=v_e,\ddot{S}(T)=0\end{array}\\ L(t)& =\sum _{i=0}^5{b}_i{t}^i,\{\begin{array}{l}L(0)=L_0,\dot{L}(0)={\dot{L}}_0,\ddot{L}(0)={\ddot{L}}_0\\ L(T)=L_e,\dot{L}(T)=0,\ddot{L}(T)=0\end{array}\end{array}$

线性方程组一次解出 $a_i,b_i$。得到 Frenet 轨迹 $\gamma (t)=(S(t),L(t))$。
随后做 Frenet→笛卡尔 转换得到 $(x(t),y(t))$，并计算 $v(t),a(t),\kappa (t)$。

硬约束快速筛（fail-fast）：

• $|a(t)|\le a_{\max }$、$|j(t)|\le j_{\max }$；
• $|\kappa (t)|\le {\kappa }_{\max }$、$|a_y(t)|=v^2|\kappa |\le a_{y,\max }$；
• 轨迹包络不与车道边界相交。
  不满足者直接淘汰，减少后续代价评估负担。

输出：候选轨迹集 $\mathcal{G}={\gamma }_k(t)$（已通过硬约束）。
为什么：分解生成使求解快、稳、可并行；以 $T$​ 对齐避免“纵横不同步”。
交接：把可行轨迹交给 Step 3 计算打分。

Step 3：碰撞检测与安全评估（Safe or not）

目标：判明每条候选轨迹是否与障碍物发生碰撞或过近。

输入：${\gamma }_k(t)$、车辆矩形包络（长 $L_{\text{veh}}$、宽 $W_{\text{veh}}$）、障碍物（含预测）在时间上的形状/占据区
处理：

• 以 $\Delta t$ 采样 $t=0!\to !T$，得到车辆姿态 $(x,y,\psi )$；
• 构造车辆矩形多边形（可外扩安全边距 $d_{\text{safe}}$）；
• 与静态障碍物多边形做重叠测试（SAT/多边形相交）；
• 对动态障碍物，取其在时刻 $t$ 的预测多边形，与车辆包络相交检测；
• 记录最近距离 $d_{\min }(t)$，作为安全代价的输入。
  输出：
• 可行性标志（是否碰撞）；
• 最近距离曲线 $d_{\min }(t)$（供代价函数使用）。
  为什么：把**“能不能走”与“走得安不安全”**分开：先剔除碰撞，再量化接近程度。
  交接：把可行轨迹及其 $d_{\min }(t)$​ 交给 Step 4 计算综合代价。

Step 4：代价评估（Score every feasible trajectory）

目标：用一套可加权、可调参的指标，给每条可行轨迹打分，平衡安全/舒适/效率/规则。

输入：${\gamma }_k(t)$ 的 $L,\dot{L},\ddot{L},v,a,j,d_{\min }(t)$、参考速度 $v_{\text{des}}(s)$、目标进度 $S_{\text{goal}}$ 等
处理：离散求和近似积分，常用代价：

$\begin{array}{rl}{C}_{\text{lat}}& =\sum _t\left({\alpha }_0{L}^2+{\alpha }_1{\dot{L}}^2+{\alpha }_2{\ddot{L}}^2\right)\Delta t\\ C_{\text{lon}}& =\sum _t\left({\beta }_0(v-v_{\text{des}}{)}^2+{\beta }_1{a}^2+{\beta }_2{j}^2\right)\Delta t\\ C_{\text{obs}}& =\sum _t\varphi \left(d_{\min }(t)\right),\varphi (d)=\{\begin{array}{ll}+\infty ,& d\le 0\\ \exp (-d/\tau ),& d>0\end{array}\\ C_{\text{eff}}& ={\lambda }_1|S(T)-S_{\text{goal}}|+{\lambda }_2\max (0,v_{\text{des}}-v_{\text{avg}})\\ C_{\text{rule}}& =\text{车道变更惩罚、实线跨越、限速超标等软惩罚}\end{array}$

归一化与加权：对各项做 min-max 或基于期望量纲的归一化，再加权求和

$C_{\text{total}}=w_{\text{lat}}{C}_{\text{lat}}+w_{\text{lon}}{C}_{\text{lon}}+w_{\text{obs}}{C}_{\text{obs}}+w_{\text{eff}}{C}_{\text{eff}}+w_{\text{rule}}{C}_{\text{rule}}.$

输出：每条候选轨迹的总分 $C_{\text{total}}({\gamma }_k)$。
为什么：安全 > 舒适 > 效率的层级可通过权重体现；同时保留对业务规则的软约束。
交接：把打分结果交给 Step 5 做选择与收敛策略。

Step 5：最优选择与防抖（Pick one & make it stable）

目标：选出当前周期的最优轨迹，同时避免周期间“抖动”（频繁换道/意图跳变）。

输入：$C_{\text{total}}({\gamma }_k)$、上周期已执行轨迹片段 ${\gamma }_{\text{prev}}$
处理：

1. 选 ${\gamma }^{\ast }=\arg \min C_{\text{total}}$；
2. 与上周期保持一致性：
   • 若 ${\gamma }^{\ast }$ 与 ${\gamma }_{\text{prev}}$ 代价差很小，优先延续 ${\gamma }_{\text{prev}}$（滞后/黏滞策略）；
   • 对换道引入迟滞阈值（进入阈值 < 退出阈值），减少反复横跳；
   • 对输出加终端意图锁定时间（例如换道一旦开始，至少执行 $t_{\text{lock}}$）。
3. 失败回退（fallback）：若无任何可行轨迹（极端拥堵/感知瞬断），降级为原地/沿参考线安全制动的保底轨迹。

输出：稳定的单条执行轨迹 ${\Gamma }^{\ast }(t)$​。
为什么：实车上“又换回去了”的体验很差，滞后与锁定能显著提升连贯性。
交接：把执行轨迹交给控制器，并把末端状态作为下一周期的warm-start。

Step 6：发布与滚动执行（Publish & roll）

输入：${\Gamma }^{\ast }(t)$（当前周期 3–8s 时域，多为 50–100 Hz 采样点）
处理：

• 发布以 $(x(t),y(t),v(t),a(t),\psi (t))$ 形式的时序轨迹；
• 控制器（MPC/Stanley/PID）在高频回路跟踪；
• 记录已执行前缀，剩余后缀与环境状态一起喂回 Step 0，进入下一周期。
  输出：车控可直接跟踪的时间标定轨迹。
  为什么：Lattice 是滚动规划，每 80–150ms 重算一次，以应对环境变化。
  交接：形成闭环，新的感知/预测与上周期轨迹共同决定下周期采样的范围与密度（可对“上周期最优”的邻域加密）。

把“如何串起来”再压缩成一句话

Step 0 定基准线与 Frenet；Step 1 选一堆“终点假设”；Step 2 把每个终点用五次多项式连接成整条轨迹；Step 3 先剔除碰撞，再量化安全距离；Step 4 统一打分（安全/舒适/效率/规则）；Step 5 选最优并加防抖/回退；Step 6 发布并滚动到下一周期。
每一步都把自己的结构化结果（格点、曲线、距离、分数、单条轨迹）交给下一步继续加工，直到得到一条可执行且稳定的轨迹。

三、 关键技术细节实现手册

3.1 五次多项式系数求解矩阵（Quintic Polynomial Coefficients）

在 Lattice Planner 中，纵向 $S(t)$ 与横向 $L(t)$ 都用 五次多项式：

$p(t)=a_0+a_{1}t+a_2{t}^2+a_3{t}^3+a_4{t}^4+a_5{t}^5$

• 已知边界条件（位置、速度、加速度）：

$\{\begin{array}{l}p(0)=p_0,\dot{p}(0)=v_0,\ddot{p}(0)=a_0\\ p(T)=p_T,\dot{p}(T)=v_T,\ddot{p}(T)=a_T\end{array}$

• 矩阵形式求解：
  设

$\mathbf{x}=[a_0,a_1,a_2,a_3,a_4,a_5{]}^{\top }$

则前 3 个系数直接由起点条件确定：

$a_0=p_0,a_1=v_0,a_2=a_0/2$

剩余 3 个系数由终点条件解出：

$\left[\begin{array}{ccc}{T}^3& T^4& T^5\\ 3T^2& 4T^3& 5T^4\\ 6T& 12T^2& 20T^3\end{array}\right]\left[\begin{array}{c}{a}_3\\ a_4\\ a_5\end{array}\right]=\left[\begin{array}{c}{p}_T-(a_0+a_{1}T+a_2{T}^2)\\ v_T-(a_1+2a_{2}T)\\ a_T-(2a_2)\end{array}\right]$

数值实现可直接用 numpy.linalg.solve 求解。

3.2 碰撞检测几何模型（Collision Checking Geometry）

为了快速判断轨迹是否与障碍物冲突，常用简化矩形模型：

• 车辆包络：
  • 长度 $L_{\text{car}}$
  • 宽度 $W_{\text{car}}$
  • 在 $(x_c,y_c,\theta )$ 下，车辆四个顶点坐标为：

${\mathbf{p}}_i=\mathbf{R}(\theta )\cdot {\mathbf{p}}_i^{\text{body}}+[x_c,y_c{]}^{\top }$

其中 $\mathbf{R}(\theta )$ 为旋转矩阵，${\mathbf{p}}_i^{\text{body}}$ 为车体坐标系下顶点位置。

• 障碍物表示：
  • 静态障碍物：已知多边形/矩形
  • 动态障碍物：已知轨迹 $O(t)$ + 包络
• 检测方法：
  1. AABB 快速排除（Axis-Aligned Bounding Box）
  2. SAT 分离轴定理（支持凸多边形）
  3. 若采用圆形近似，直接比较中心距离：

$\Vert {\mathbf{p}}_{\text{car}}-{\mathbf{p}}_{\text{obs}}\Vert <r_{\text{car}}+r_{\text{obs}}$

3.3 代价函数归一化（Cost Normalization）

为了在不同物理量（时间、加速度、横向偏移等）之间进行加权，先对每类代价做归一化：

$C_{\text{norm}}=\frac{C-C_{\min }}{C_{\max }-C_{\min }+ϵ}$

• 典型代价项：
  1. 舒适性代价：加速度平方和、加加速度平方和
  2. 安全代价：最小障碍物距离的倒数
  3. 参考线偏移：末端 $|L_{\text{end}}-L_{\text{ref}}|$
• 归一化后加权求和：

$C_{\text{total}}=w_s{C}_s+w_c{C}_c+w_l{C}_l$

其中权重 $w_i$ 从经验或自动调参获得。

3.4 参数默认值表