3. 路径规划基础
路径规划是端到端自动驾驶系统中承上启下的核心环节，其任务是在给定起始点、目标点以及环境感知信息的前提下，搜索出一条满足车辆运动学约束、安全约束和交通规则约束的可行驶轨迹。本章作为路径规划模块的基础章节，将系统阐述路径规划问题的数学定义、全局规划与局部规划的分工逻辑，以及工程中最常用的三种路径表示方法。理解这些基础概念，是后续学习采样算法、图搜索算法、优化算法以及模型预测控制（MPC）的前提。
3.1 路径规划问题的数学定义与核心要素
3.1.1 核心原理：从几何搜索到约束优化
路径规划问题的标准数学定义可以表述为：在状态空间 \(X\) 中，给定初始状态 \(x_{start}\) 和目标区域 \(X_{goal}\)，寻找一条连续映射 \(\tau: [0,1] \rightarrow X_{free}\)，使得 \(\tau(0)=x_{start}\)，\(\tau(1) \in X_{goal}\)，其中 \(X_{free}\) 是无碰撞的自由状态空间。在自动驾驶场景中，状态通常包含位置 \((x,y)\)、航向角 \(\theta\)、曲率 \(\kappa\) 甚至速度 \(v\) 和加速度 \(a\)。因此，路径规划不仅仅是几何上的避障，还需要满足车辆的非完整约束（如最小转弯半径）和动力学约束（如最大加速度）。
从工程实现角度，路径规划问题可以拆解为四个核心要素：环境建模（将传感器数据转换为可搜索的空间表示）、约束定义（包括车辆运动学约束、道路边界约束、障碍物膨胀约束）、目标函数设计（如路径最短、时间最优、舒适性最优）以及搜索/优化策略（采样法、图搜索法、数值优化法）。这四个要素相互耦合，决定了规划算法的性能上限。
3.1.2 判断方法：如何区分路径规划与轨迹规划
在实际工程中，初学者常混淆"路径规划"与"轨迹规划"两个概念。判断标准在于是否引入时间维度：路径规划只输出空间几何曲线（不含时间信息），而轨迹规划在路径基础上附加了速度/加速度的时间分布。例如，全局导航给出的是一条静态路径，而局部规划模块输出的则是带有期望速度的轨迹点序列。在系统架构设计中，通常将路径规划作为上游模块，轨迹规划作为下游模块，两者通过接口解耦。
| 对比维度 | 路径规划 | 轨迹规划 |
|---|---|---|
| 输出内容 | 空间几何曲线（x, y, θ） | 带时间戳的状态序列（x, y, θ, v, a） |
| 约束侧重 | 几何避障、运动学可行 | 动力学可行、时间最优、舒适性 |
| 典型算法 | A*、RRT、Hybrid A* | MPC、时间弹性带（TEB） |
| 更新频率 | 较低（全局）或中等（局部） | 高（50~100 Hz） |
3.1.3 案例拆解：十字路口左转场景的路径规划问题定义
以城市十字路口左转为例，全局规划模块基于车道级地图给出"沿当前车道行驶至路口中心，左转进入目标车道"的参考路径。该路径的数学表达为一系列离散路点，每个路点包含经纬度坐标和期望航向。局部规划模块接收该参考路径后，结合实时感知的车辆、行人、红绿灯状态，在50~100米的规划时域内重新生成一条平滑、安全、符合交规的局部路径。此时，路径规划问题的约束集包括：车辆最小转弯半径（如5.5m）、车道边界约束（左右各1.5m）、动态障碍物安全距离（纵向3m、横向0.5m）。目标函数则综合考虑路径长度、曲率变化率和与参考路径的偏差。
3.2 全局路径规划与局部路径规划的区别与协同
3.2.1 核心原理：分层规划架构
自动驾驶路径规划普遍采用"全局-局部"分层架构。全局路径规划（Global Path Planning）在宏观尺度上解决"从A到B走哪条路"的问题，其输入是高精地图（HD Map）和静态障碍物信息，输出是一条从起点到终点的拓扑路径。由于全局规划不需要考虑动态障碍物，计算频率较低（0.1~1 Hz），可以使用计算量较大的算法如A*、RRT*或Lattice Planner。局部路径规划（Local Path Planning）则在全局路径的约束下，解决"接下来3~5秒怎么走"的问题，需要实时感知动态障碍物、交通信号和路面状况，输出满足车辆运动学和动力学约束的平滑轨迹，更新频率通常为10~50 Hz。
两者的协同关系可以概括为"全局指引、局部修正"。全局路径提供参考线（Reference Line），局部规划器以参考线为基准进行横向偏移和纵向速度调整。当局部规划器发现全局路径不可行（如前方道路施工封闭）时，需要向上层反馈，触发全局路径重规划。这种分层架构的优势在于：将复杂问题分解为不同时间尺度的子问题，降低计算复杂度，同时提高系统的鲁棒性。
3.2.2 操作流程：全局与局部规划的接口设计
在实际工程中，全局规划与局部规划之间的接口设计至关重要。推荐采用以下流程：
- 全局规划层：加载高精地图，将起点和终点投影到车道级坐标系，使用A*或Hybrid A*算法搜索粗路径，然后通过样条曲线平滑生成参考线。
- 参考线适配层：将全局参考线按固定间隔（如0.5m）离散化，并计算每个离散点的航向角、曲率和累积弧长，形成局部规划器的输入数据结构。
- 局部规划层：以参考线为Frenet坐标系的S轴，将动态障碍物投影到Frenet坐标系，在S-T空间和D-T空间中进行采样或优化，生成候选轨迹集。
- 轨迹评估与选择：对候选轨迹进行碰撞检查、动力学可行性检查和舒适性评估，选择最优轨迹输出给控制模块。
3.2.3 案例拆解：高速公路匝道汇入场景
在高速公路匝道汇入场景中，全局规划给出"沿匝道行驶，在加速车道末端汇入主路"的参考路径。局部规划器需要实时感知主路车流间隙，决定是加速汇入还是减速等待。此时，局部规划器在Frenet坐标系下进行纵向规划（S方向）和横向规划（D方向）的联合优化。具体参数示例：主路车辆速度为80 km/h，匝道加速车道长度为200m，自车当前速度为60 km/h。局部规划器生成三条候选轨迹：轨迹A（加速至85 km/h，在间隙1汇入）、轨迹B（保持70 km/h，在间隙2汇入）、轨迹C（减速至50 km/h，等待更大间隙）。评估函数综合考虑汇入时间、舒适度（加速度变化率）和安全距离，最终选择轨迹B。该案例体现了全局规划提供宏观约束、局部规划进行微观决策的分工逻辑。
3.3 常见路径表示方法：离散点、多项式曲线与样条曲线
3.3.1 核心原理：三种表示方法的数学本质
路径的数学表示方法直接决定了后续平滑、优化和控制模块的算法设计。工程中最常用的三种方法各有优劣：
- 离散点序列：路径表示为一系列有序的点 \(P_i = (x_i, y_i, \theta_i)\)，点间距通常为0.1~1m。这种表示最接近传感器原始输出（如GPS轨迹、栅格地图搜索结果），存储简单、可视化直观，但点与点之间不连续，无法直接计算曲率，需要插值或拟合才能用于控制。
- 多项式曲线：使用多项式函数 \(y=f(x)\) 或参数方程 \(x(t), y(t)\) 表示路径。三次多项式（Cubic Polynomial）是最低阶的能同时约束位置和航向的曲线，五次多项式（Quintic Polynomial）还能约束曲率。多项式曲线计算高效、连续可导，适合短距离轨迹生成（如换道、泊车），但高阶多项式容易产生振荡，且全局耦合（改变一个系数影响整条曲线）。
- 样条曲线：将路径分段，每段用低阶多项式（通常为三次）表示，并在段间施加连续性约束（C0位置连续、C1切向连续、C2曲率连续）。三次样条（Cubic Spline）是最常用的形式，它兼具局部控制特性和高阶连续性，非常适合作为全局参考线或车道中心线。样条曲线的缺点是参数较多，需要求解线性方程组，且对控制点分布敏感。
3.3.2 判断方法：如何选择路径表示方法
选择路径表示方法时，可以从以下维度进行判断：
| 应用场景 | 推荐表示方法 | 关键理由 |
|---|---|---|
| 全局导航参考线 | 三次样条曲线 | 长距离、需C2连续、局部调整方便 |
| 换道/避障短轨迹 | 五次多项式 | 可同时约束位置、航向、曲率 |
| 栅格地图搜索输出 | 离散点序列 | 直接输出、无需额外转换 |
| 泊车轨迹 | 混合（离散点+多项式） | 低速场景、需精确控制 |
3.3.3 案例拆解：从离散点到样条曲线的工程转换
在典型的自动驾驶系统中，全局规划器（如Hybrid A*）输出的原始路径是离散点序列，点间距约1m。直接将该序列发送给控制模块会导致车辆转向不平顺，因此需要转换为样条曲线。具体操作流程如下：
- 数据预处理：去除离群点，对离散点进行坐标变换（经纬度转局部笛卡尔坐标）。
- 参数化：以累积弧长 \(s\) 为参数，将离散点表示为 \((x(s), y(s))\)。
- 样条拟合：使用三次样条插值，在每两个相邻点之间构造三次多项式，并保证节点处一阶导数和二阶导数连续。此时需要求解一个三对角线性方程组，计算量约为O(n)。
- 曲率计算：根据样条函数的一阶导和二阶导计算每个点的曲率 \(\kappa = \frac{x'y'' - y'x''}{(x'^2 + y'^2)^{3/2}}\)，并检查是否超过车辆最大曲率约束（如0.15 1/m）。
- 重采样：按0.2m间隔重新采样，生成平滑后的离散点序列，供局部规划器使用。
该转换过程在量产项目中通常耗时小于5ms（1000个点），满足实时性要求。转换后的样条曲线不仅保证了C2连续性，还使得曲率变化率（jerk）可控，显著提升了乘坐舒适性。
3.3.4 常见误区与总结
在路径表示方法的工程实践中，存在以下常见误区：
- 误区一：认为离散点可以直接用于控制。离散点缺乏连续性信息，直接跟踪会导致方向盘抖动。必须经过平滑处理（样条拟合或滤波）。
- 误区二：过度追求高阶多项式。七次或更高阶多项式虽然能提供更多约束，但数值稳定性差，容易产生龙格现象（Runge's Phenomenon），实际工程中很少使用超过五次的多项式。
- 误区三：忽略样条曲线的参数化方式。如果以x为参数而非弧长s，在路径接近垂直时会出现数值问题。推荐始终使用弧长参数化。
- 误区四：未考虑曲率连续性。仅保证C1连续（切向连续）的路径在高速行驶时仍会产生较大的侧向加速度突变，必须保证C2连续（曲率连续）。