1. 自动驾驶规划算法概述
在自动驾驶技术快速发展的今天,路径规划算法作为核心模块之一,直接决定了车辆能否安全、舒适地行驶。作为一名在自动驾驶领域工作多年的工程师,我经常被问到:Hybrid A*、Lattice Planner和EM Planner这三种经典算法究竟有什么区别?它们各自适合什么场景?今天我就结合自己的工程实践经验,为大家深入解析这三种算法的原理、特点和应用。
这三种算法之所以重要,是因为它们都在解决自动驾驶规划中的核心问题:如何在复杂环境中生成既满足车辆运动学约束,又能安全避障,同时保证乘坐舒适性的轨迹。与传统的A*、Dijkstra等算法相比,它们更注重"车辆能实际执行"这一关键需求。
需要模型API调用? 免费领10W Token,多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. Hybrid A*算法详解
2.1 为什么需要Hybrid A*
传统A*算法在二维栅格地图中表现优异,但它存在一个致命缺陷:它规划出的路径往往不符合车辆的运动学约束。想象一下,你的车突然需要直角转弯或者原地掉头,这在实际中是不可能实现的。
Hybrid A正是为了解决这一问题而诞生的。它将车辆的运动学特性融入搜索过程,确保生成的路径是车辆能够实际执行的。我在参与自动泊车项目时,就深刻体会到了Hybrid A的价值——它能在狭小的空间内规划出包含前进、后退的复杂路径。
2.2 核心原理与实现
Hybrid A的核心改进在于状态表示和节点扩展方式。不同于传统A的(x,y)二维状态,Hybrid A*使用(x,y,θ)三维状态,加入了车辆航向角信息。在节点扩展时,它会基于车辆运动学模型生成后继状态。
常用的车辆模型是简化自行车模型:
code复制ẋ = v * cosθ
ẏ = v * sinθ
θ̇ = v/L * tanδ
其中v是车速,L是轴距,δ是前轮转角。这个模型很好地描述了车辆的基本运动特性。
在实际实现中,我们会预设几种典型的控制动作组合:
- 前进+左转
- 前进+直行
- 前进+右转
- 后退+左转
- 后退+直行
- 后退+右转
每个动作组合都会生成一小段符合车辆运动学的轨迹,作为搜索的边。
2.3 代价函数设计
Hybrid A的代价函数比传统A复杂得多,通常包含多个惩罚项:
code复制g(n) = L + C_reverse + C_switch + C_κ + C_obs
其中:
- L:路径长度代价
- C_reverse:倒车惩罚
- C_switch:前进/倒车切换惩罚
- C_κ:曲率惩罚
- C_obs:靠近障碍物惩罚
这种设计使得规划器不仅考虑路径长度,还会倾向于:
- 减少倒车次数
- 避免频繁切换前进/后退
- 选择转弯平缓的路径
- 与障碍物保持安全距离
2.4 启发函数的重要性
Hybrid A*的搜索效率高度依赖启发函数。常用的启发函数有两类:
-
考虑车辆运动学但忽略障碍物的启发,如Dubins或Reeds-Shepp距离。这类启发能准确估计不考虑障碍时车辆到达目标的最短路径长度。
-
考虑障碍物但忽略车辆运动学的启发,如在二维栅格地图上预先计算的障碍物距离场。
在工程实践中,我们通常同时使用这两种启发,取其最大值作为最终启发值。这种组合方式能有效平衡搜索效率和路径质量。
2.5 解析扩展技术
为了提高搜索效率,Hybrid A*引入了"解析扩展"技术:当节点接近目标时,直接尝试用Reeds-Shepp曲线连接当前状态和目标状态。如果这条曲线无碰撞,就可以立即结束搜索。
这个技巧特别适合泊车场景。在实际项目中,我们通过合理设置解析扩展的触发距离,将规划时间缩短了30%以上。
2.6 优缺点分析
优点:
- 直接考虑车辆运动学约束
- 能处理前进/后退混合路径
- 适合复杂低速场景(如泊车、窄路掉头)
缺点:
- 状态空间大,计算成本高
- 生成的路径可能不够平滑
- 对角度离散化间隔敏感
在实际系统中,我们通常会将Hybrid A与后续平滑优化结合使用。Hybrid A负责找到可行解,优化算法再对路径进行平滑处理。
3. Lattice Planner算法解析
3.1 基本概念与原理
Lattice Planner的核心思想是"用预定义的运动原语拼接轨迹"。想象你有一盒积木,每块积木代表车辆能执行的一个基本动作(如直行、左转、右转等),规划就是选择合适的积木拼接成完整路径。
与Hybrid A*不同,Lattice Planner不是在线计算车辆运动,而是预先计算好一组"运动原语"(Motion Primitive)。每个原语都是一小段满足车辆运动学约束的轨迹,连接两个离散状态。
3.2 状态表示方法
Lattice Planner的状态表示比Hybrid A*更丰富,通常包括:
code复制(x, y, θ, κ) 或 (x, y, θ, κ, v)
其中κ是曲率,v是速度。加入这些额外状态量可以让规划结果更符合车辆控制需求。
在结构化道路场景中,我们更常使用Frenet坐标系表示状态:
code复制(s, l, θ, κ, v)
其中s是沿参考线的纵向位置,l是横向偏移。这种表示方式更贴合道路结构,便于处理车道保持、变道等场景。
3.3 运动原语设计
运动原语是Lattice Planner的关键,其设计需要考虑:
- 覆盖性:能组合出各种需要的轨迹
- 可执行性:每段原语都满足车辆动力学约束
- 效率:原语数量要适中,避免搜索空间爆炸
常见的原语生成方法包括:
- 基于车辆模型的数值积分
- 求解两点边值问题
- 离线优化生成
在实际项目中,我们会根据场景特点设计不同的原语库。例如,高速公路场景需要更多直线加速和缓弯原语,而泊车场景则需要更灵活的小半径转向原语。
3.4 规划流程
完整的Lattice Planner流程包括:
- 状态空间离散化:确定如何采样状态
- 原语库生成/加载:准备可用的运动原语
- 碰撞检测:检查每条原语是否与障碍物冲突
- 图搜索:在原语构成的图中寻找最优路径
- 轨迹后处理:平滑和优化拼接后的轨迹
在结构化道路中,我们通常会在Frenet坐标系下进行规划,将问题分解为纵向(s)和横向(l)两个维度分别处理,这大大降低了问题复杂度。
3.5 代价函数设计
Lattice Planner的代价函数通常包括:
- 路径长度代价
- 曲率/转向代价
- 障碍物距离代价
- 偏离参考线代价
- 舒适性代价(如加速度、jerk等)
在Frenet坐标系下,我们可以更自然地设计针对道路场景的代价项,如:
- 车道保持代价
- 变道代价
- 跟车距离代价
3.6 优缺点分析
优点:
- 轨迹天然满足车辆约束
- 工程可解释性强
- 适合结构化道路场景
- 便于融入道路规则和交通约束
缺点:
- 原语库设计复杂
- 高维状态空间搜索成本高
- 对场景结构依赖较强
4. EM Planner深度解析
4.1 框架概述
EM Planner是Apollo开源框架中的核心规划算法,它采用"路径-速度解耦"的思想,将复杂的轨迹规划问题分解为路径规划和速度规划两个子问题,分别求解后再合并。
这种解耦设计基于一个重要观察:在结构化道路中,车辆的横向运动(避障、变道)和纵向运动(跟车、停车)可以相对独立地考虑。
4.2 Frenet坐标系的应用
EM Planner在Frenet坐标系下工作,将车辆状态表示为(s,l),其中:
- s:沿参考线的纵向位置
- l:相对于参考线的横向偏移
这种表示方式使我们可以:
- 将障碍物投影到SL坐标系
- 独立优化横向和纵向运动
- 自然地融入道路约束
4.3 路径规划实现
路径规划分为两个阶段:
- DP路径搜索:在SL空间进行动态规划,确定绕过障碍物的粗略路径
- QP路径优化:在DP结果的走廊内进行二次规划,生成平滑路径
DP阶段主要解决"从左边还是右边绕障"这类离散决策问题,而QP阶段则负责生成精确的l=f(s)函数,保证路径的连续性和舒适性。
4.4 速度规划实现
速度规划同样采用DP+QP的两阶段方法:
- DP速度搜索:在ST空间进行动态规划,处理跟车、停车等决策
- QP速度优化:生成平滑的s(t)曲线,满足加速度等约束
ST图是速度规划的关键工具,横轴是时间,纵轴是纵向距离。障碍物在ST图中表现为禁区,规划器需要找到一条从当前位置到目标位置的无碰撞轨迹。
4.5 代价函数设计
EM Planner的路径代价函数通常包括:
- 平滑项(惩罚曲率变化)
- 障碍物项(惩罚靠近障碍物)
- 引导项(惩罚偏离参考线)
速度代价函数则考虑:
- 舒适性(加速度、jerk)
- 安全性(与障碍物的时空距离)
- 效率(行驶时间)
4.6 优缺点分析
优点:
- 解耦设计降低问题复杂度
- 适合结构化道路场景
- 决策与优化分离,结果更可靠
- 便于融入交通规则
缺点:
- 依赖参考线质量
- 路径速度解耦是一种近似
- 系统实现复杂度高
5. 三种算法对比与应用
5.1 本质区别
- Hybrid A*:带运动学约束的启发式搜索
- Lattice Planner:基于运动原语的图搜索
- EM Planner:路径-速度解耦的优化框架
5.2 状态表示对比
| 算法 | 典型状态表示 |
|---|---|
| Hybrid A* | (x, y, θ) |
| Lattice | (x, y, θ, κ)或(s, l, θ, κ) |
| EM Planner | (s, l)和(s, t) |
5.3 适用场景建议
- Hybrid A*:泊车、窄路掉头等低速复杂场景
- Lattice Planner:结构化道路的车道保持、变道
- EM Planner:城市道路、高速公路等结构化场景
在实际工程中,我们往往会组合使用这些算法。例如:
- 用Hybrid A*处理泊车场景
- 用Lattice生成候选轨迹
- 用EM Planner框架进行路径速度解耦优化
6. 工程实践中的经验分享
6.1 Hybrid A*调优技巧
-
角度离散化:通常将航向角离散为5°-10°间隔。间隔太小会导致搜索空间爆炸,太大则影响路径质量。
-
解析扩展触发:建议设置为距离目标3-5米时触发解析扩展,既能提高效率又不损失太多路径质量。
-
代价权重调整:根据场景调整各项代价权重。例如在泊车场景,可以适当提高倒车惩罚,减少频繁换挡。
6.2 Lattice Planner实现要点
-
原语库设计:建议针对不同场景准备不同的原语库。高速公路场景需要更多直线和大半径转弯原语,而城市道路则需要更灵活的小半径转向原语。
-
状态采样:在Frenet坐标系下,纵向s的采样间隔通常为1-2米,横向l的采样间隔为0.2-0.5米。
-
多分辨率搜索:可以先粗粒度搜索找到大致方向,再在局部区域进行细粒度搜索,平衡效率和质量。
6.3 EM Planner调试建议
-
参考线生成:确保参考线平滑且贴合道路中心,这对后续规划质量至关重要。
-
DP网格设置:SL空间的网格分辨率通常为0.5m×0.1m,ST空间为0.5m×0.3s。
-
QP权重调整:平滑项权重需要仔细调整,过小会导致路径抖动,过大会使车辆对障碍物反应迟钝。
7. 常见问题与解决方案
7.1 规划结果震荡问题
现象:连续几帧规划结果差异很大,导致车辆抖动。
解决方案:
- 在代价函数中加入与上一帧结果的相似性惩罚
- 对规划结果进行低通滤波
- 增加QP中的平滑项权重
7.2 狭窄通道通过困难
现象:在狭窄通道中规划失败率高。
解决方案:
- 调整障碍物距离代价函数,允许更靠近障碍物
- 在Hybrid A*中启用更精细的角度离散化
- 使用更长的Reeds-Shepp曲线进行解析扩展
7.3 复杂场景规划超时
现象:在复杂场景中规划时间超过预算。
解决方案:
- 采用多分辨率搜索策略
- 限制最大搜索节点数
- 对非关键区域降低规划精度要求
8. 算法选择指南
在实际项目中选择规划算法时,建议考虑以下因素:
-
场景特点:
- 结构化道路优先考虑EM Planner或Lattice
- 非结构化场景考虑Hybrid A*
-
计算资源:
- 资源有限时选择计算量较小的算法
- 允许复杂计算时可考虑多算法融合
-
车辆特性:
- 大型车辆需要更严格的运动学约束
- 高速场景需要更长的规划视野
-
功能需求:
- 需要精细速度控制时选择EM Planner
- 简单路径生成可考虑Hybrid A*
在我参与的自动驾驶项目中,城市道路场景通常采用EM Planner框架,融合Lattice的轨迹生成思想;而自动泊车模块则主要依赖Hybrid A*算法。这种组合在实践中表现出了良好的平衡性。
