自动驾驶的“大脑”如何思考?深解路径与轨迹规划算法

导语：
    如果说感知是自动驾驶的“眼睛”，控制是“手脚”，那么规划（Planning）就是真正的“大脑”。 当车辆面对复杂的十字路口、突然窜出的行人或拥堵的车流时，它如何在毫秒级时间内决定“往哪走”和“走多快”？今天，我们将拆解路径规划与轨迹规划的底层逻辑，并揭秘百度 Apollo、Tesla 等巨头是如何将这些数学公式转化为安全驾驶的。

01. 概念辨析：Path vs. Trajectory

在很多人的印象中，“规划”就是画一条线。但在自动驾驶工程中，路径（Path）和轨迹（Trajectory）有着本质的区别。

简单总结：

轨迹 = 路径 + 速度剖面（时间律）
先定下“走哪条线”，再决定“在这条线上什么时候踩油门、什么时候刹车”，这就是经典的解耦规划思路。

02. 算法江湖：主流规划流派大比拼

在将理论落地之前，我们先看看学术界和工业界常用的几类核心算法。

1. 基于搜索的算法 (Search-Based)

• • 代表：A*, Dijkstra, Hybrid A*
• • 原理：将地图网格化，像走迷宫一样寻找最短路径。
• • 适用：低速、结构化环境。
• • 高光时刻：Hybrid A* 是自动泊车的王者，它考虑了车辆的“非完整约束”（车不能横着走），能规划出完美的倒车入库路径。

2. 基于采样的算法 (Sampling-Based)

• • 代表：RRT (快速扩展随机树), RRT*
• • 原理：在空间中随机撒点，连接成树，逐渐逼近目标。
• • 适用：高维空间、复杂障碍物环境（如机械臂）。
• • 缺点：生成的路径通常曲折不平滑，需要后期优化。

3. 基于优化的算法 (Optimization-Based)

• • 代表：QP (二次规划), NLP (非线性规划)
• • 原理：将规划问题转化为数学方程：min(代价函数) subject to 约束条件。
• • 适用：生成平滑、符合动力学的轨迹。
• • 优势：计算效率高，易于处理软硬约束，是目前车企的主流选择。

4. 端到端学习 (End-to-End Learning)

• • 代表：Tesla FSD v12, UniAD
• • 原理：输入图像/点云，神经网络直接输出轨迹或控制指令。
• • 趋势：数据驱动，拟人化程度高，但可解释性仍是挑战。

03. 工业界实战：百度 Apollo 的“解题思路”

百度 Apollo 作为开源自动驾驶的标杆，其 Planning 模块展示了如何将上述理论工程化。Apollo 的核心策略是：分层架构 + 场景化管理 + 路径速度解耦优化。

架构：Scenario -> Stage -> Task

Apollo 不再是一个巨大的单体算法，而是根据场景切换策略：

• • Lane Follow（车道保持）：用 EM Planner。
• • Valet Parking（代客泊车）：用 Open Space Planner (Hybrid A*)。
• • Intersection（路口通行）：用专门的路口决策逻辑。

核心解密：EM Planner (Expectation-Maximization)

在高速和城市快速路场景中，Apollo 广泛使用 EM Planner。它的精髓在于“迭代解耦”：

第一步：路径优化 (Path Optimizer)

• • 算法：Piecewise Jerk Path Optimization (分段加加速度优化)。
• • 坐标系：Frenet 坐标系（以参考线为基准，分解为纵向 s 和横向 l）。
• • 做法：将静态障碍物投影到 Frenet 系，形成横向禁区；构建 QP (二次规划) 问题，最小化横向偏移、曲率和曲率变化率。最终输出一条几何上平滑且避开静态障碍物的路径 l(s)。

第二步：速度优化 (Speed Optimizer)

• • 算法：DP (动态规划) + QP (二次规划)。
• • 工具：ST 图 (Space-Time Graph)。横轴是距离 S，纵轴是时间 T。动态障碍物在 ST 图上表现为“红色碰撞块”。
• • 做法：
1. 1. DP 粗搜：在离散化的 ST 网格中，寻找一条无碰撞的粗略速度曲线（解决“能不能过”的问题）。
2. 2. QP 精修：以 DP 结果为初值，求解平滑的速度剖面 v(t)，保证加速度和加加速度（Jerk）在舒适范围内（解决“坐得舒不舒服”的问题）。

第三步：合成与评估
将优化后的路径 l(s) 和速度 v(s) 结合，生成最终的 (x, y, t, v, a) 轨迹，并通过代价函数评估选出最优解发送给 Control 模块。

Apollo 的工程智慧：
为什么不直接联合优化？因为联合优化是非凸的，计算太慢。通过 DP 处理非凸约束（避障） + QP 处理凸优化（平滑），Apollo 在实时性和最优性之间找到了完美的平衡点。

04. 他山之石：其他车企的规划策略

Tesla：端到端的神经网络
Tesla 的 FSD v12 彻底摒弃了传统的 C++ 规则代码。其核心是 Occupancy Network (占用网络) 加上 Planning Net。神经网络直接从视频流中提取特征，预测周围物体的未来轨迹，并直接回归出自车的轨迹点。优势在于极强的泛化能力，能处理从未见过的长尾场景，但黑盒模型的安全性验证极其困难。

Waymo：混合专家系统与行为策略
Waymo 依然坚持模块化，但更加精细化。其核心基于高精地图的语义理解和强大的预测模块。对于每个场景，系统会生成数百条候选轨迹，通过模拟仿真评估每条轨迹的安全性、效率和舒适度，最后选出最佳的一条。其特点是极度保守和安全，强调可解释性和形式化验证。

传统车企 (BBA等)：稳健的 MPC 控制
大多数 L2+/L3 级量产车采用 MPC (模型预测控制) 进行局部规划。其核心是滚动时域优化，只看未来几秒并不断重新规划。优势在于对车辆动力学模型利用充分，跟踪精度高，非常适合高速巡航和 ACC 自适应巡航。

05. 未来趋势：规划算法的演进方向

1. 从“解耦”走向“联合”
随着车载算力的大幅提升（如 Orin X, Thor），越来越多的方案开始尝试时空联合优化，甚至直接使用非线性 MPC 同时处理路径和速度，以获得全局最优解。

2. 大模型赋能 (LLM for Planning)
利用大语言模型的推理能力，处理复杂的交通博弈（如无保护左转时的意图判断），让规划更具“社交属性”和拟人化。

3. 世界模型 (World Models)
自动驾驶系统通过世界模型预测未来的多种可能性，并在“想象”中进行规划推演，从而在真正执行前筛选出最安全的决策。

结语

路径与轨迹规划，是数学之美与工程之巧的完美结合。
       Apollo 告诉我们：巧妙的解耦和迭代可以解决复杂的工程难题。
       Tesla 告诉我们：数据规模可以暴力破解长尾问题。
       Waymo 告诉我们：安全和冗余永远是第一位的。

没有绝对完美的算法，只有最适合当前场景与算力约束的方案。未来的自动驾驶，必将是规则、优化与深度学习的深度融合。