知识点拆解｜自动驾驶轨迹规划基础

🧭 引言：为什么需要轨迹规划？

自动驾驶系统分为感知、预测、规划、控制四大模块。轨迹规划位于预测之后、控制之前，其任务是在给定环境约束下，生成一条从起点到终点的安全、舒适、可行的路径和时间序列。

轨迹规划与路径规划的关键区别在于：路径只关心几何形状（x, y），轨迹关心的是"什么时间到达什么位置"（x, y, t），包含速度和加速度信息。

📐 Frenet坐标系：把复杂问题变简单

笛卡尔坐标系的困境

在笛卡尔坐标系下描述车辆沿道路行驶非常困难：道路弯曲时，车辆的 x-y 坐标变化剧烈，约束表达复杂。

Frenet坐标系的妙用

Frenet坐标系以道路中心线为参考，将车辆位置分解为两个正交分量：

• 纵向坐标 s：沿参考线的弧长
• 横向坐标 d：到参考线的垂直偏移

关键公式：将笛卡尔坐标 (x, y) 投影到参考线上得到投影点 (x_r, y_r)，然后：

• s = 投影点处的累积弧长
• d = 符号距离（向左为正/向右为负）

速度与加速度的分解：

在Frenet框架下，纵向速度 $\dot{s}$ 和横向速度 $\dot{d}$ 相互解耦，可以独立优化。这是轨迹规划中最核心的简化：将二维轨迹规划降为两个一维规划（s 方向和 d 方向）。

🛤️ 轨迹表示方法

1. Waypoint（离散路径点）

最直观的表示：直接输出一系列离散点。

优点：简单直接，计算快。缺点：不连续，需要插值，存储量大。

2. 多项式曲线 (Polynomial)

用多项式 $f(t) = a₀ + a₁t + a₂t² + a₃t³ + a₄t⁴ + a₅t⁵$ 表示轨迹。五次多项式可以保证位置、速度、加速度连续。

边界约束：已知起点状态 (p₀, v₀, a₀) 和终点状态 (p₁, v₁, a₁)，求解 6 个系数。

3. B样条曲线 (B-Spline)

用多个控制点加权得到平滑曲线，具有局部控制性：移动一个控制点只影响局部形状。

🗂️ 规划算法三大流派

1. 基于搜索的方法

代表算法：A*, Dijkstra, Hybrid A*, State Lattice

核心思路：在状态空间（如网格或晶格）中搜索最优路径。Hybrid A 是自动驾驶中最常用的搜索算法*，它结合了连续状态空间和离散网格，保证路径的运动学可行性。

2. 基于优化的方法

代表算法：曲线优化 (Optimization-based), MPC (Model Predictive Control)

核心思路：将规划建模为带约束的优化问题。

特点：可显式处理复杂约束，但计算量大，且非凸问题易陷入局部最优。

3. 基于学习的方法

代表算法：IL (Imitation Learning), RL (Reinforcement Learning), Implicit Model

核心思路：用神经网络直接从感知输入映射到轨迹。

⚡ 碰撞检测：轨迹安全的底线

主流方法

1. 包围盒检测 (Bounding Box)：用矩形/圆形包围车辆，检测是否重叠。计算快但保守。
2. 分离轴定理 (SAT)：精确检测两个凸多边形是否相交，常用于激光雷达点云的障碍物检测。
3. SDF (Signed Distance Function)：构建场景的符号距离场，快速查询任意点到障碍物的距离。

碰撞约束的优化表达

在优化框架中，碰撞约束通常表达为：

# 伪代码：SDF 碰撞约束
for each state in trajectory:
    distance = sdf_map.query(state.x, state.y)
    constraint: distance >= safety_margin

关键技术：

• 凸分解 (Convex Decomposition)：将非凸障碍物拆分为多个凸体
• DCC (Dual Convex Convex)：用对偶形式加速碰撞检测计算

🌊 轨迹平滑：让乘客不晕车

为什么需要平滑？

不平滑的轨迹会导致：

• 乘客舒适度下降（急加速/急转向）
• 车辆跟踪困难
• 轮胎磨损加剧

平滑指标

平滑方法

1. 后处理平滑：生成轨迹后用滤波器平滑

   • 移动平均滤波：简单但会缩短轨迹
   • Savitzky-Golay滤波：保形性好

2. 优化目标内置平滑

   • 在代价函数中加入 Jerk 项：$J_{smooth} = \int \dddot{x}^2 dt$
   • 用 B-Spline 的曲率连续性保证

3. Minimum Jerk 轨迹：理论上最平滑的轨迹，$J = \int \dddot{x}^2 dt$ 最小

🧩 实践链路：从感知到控制的轨迹生成

一条完整的轨迹规划pipeline：

1. 环境感知 ⮕ 预测模块 提供障碍物轨迹
2. 行为决策 ⮕ 选择宏观行为（直行/换道/停车）
3. 轨迹生成 ⮕ 在Frenet空间生成多条候选轨迹
4. 轨迹评估 ⮕ 按代价函数选取最优
5. 碰撞检查 ⮕ 剔除不安全轨迹
6. 轨迹输出 ⮕ 送给控制模块跟踪

💭 个人思考

轨迹规划是自动驾驶中"理想与现实碰撞最激烈"的模块。学术界喜欢用优化方法求解全局最优解，工业界却偏爱搜索+后处理平滑的实用组合。核心矛盾在于：

1. 实时性 vs 最优性：MPC 可以每 50ms 重新规划，但只能看到有限时域
2. 确定性 vs 不确定性：传统规划假设环境完全确定，但实际存在大量不确定性
3. 规划 vs 控制：规划层输出的平滑轨迹，控制层往往跟踪不上 — 这需要规划时预留控制余量

未来的方向是端到端规划和交互式规划：让规划器不仅考虑障碍物当前状态，还要主动预测障碍物对自己规划的响应。这需要将博弈论思想引入轨迹规划，是当前学术界最活跃的方向之一。