第5章：预测与路径规划

本章导读

预测与路径规划是自动驾驶系统中连接感知与控制的关键桥梁。本章深入探讨如何预测周围交通参与者的未来行为，以及如何基于这些预测信息规划安全、高效的行驶路径。我们将重点介绍基于深度学习的预测方法，特别是图神经网络和Transformer在多智能体交互建模中的应用，以及向量化地图表示在提升预测精度方面的最新进展。

学习目标：

• 理解行为预测的核心挑战和不确定性建模方法
• 掌握多智能体交互的建模技术
• 熟悉轨迹生成的多模态预测方法
• 理解高精地图在预测中的作用
• 掌握从全局路径到局部轨迹的规划方法

5.1 行为预测基础

5.1.1 预测问题定义

自动驾驶中的预测任务是基于历史观测数据，推断周围交通参与者（车辆、行人、骑行者等）未来一段时间内的运动轨迹。典型的预测时域为3-8秒，采样频率通常为10Hz。

预测问题可以形式化为：给定过去T_h时刻的观测状态序列X = {x_{t-T_h}, ..., x_{t-1}, x_t}，预测未来T_f时刻的轨迹Y = {y_{t+1}, ..., y_{t+T_f}}。

时间轴示意：
过去观测                 当前时刻    未来预测
<---T_h--->              |          <---T_f--->
x_{t-T_h}...x_{t-1} x_t  |  y_{t+1}...y_{t+T_f}
                         |
                      预测起点

5.1.2 不确定性建模

预测的核心挑战在于未来的固有不确定性。这种不确定性主要来源于：

1. 意图不确定性（Intention Uncertainty） - 驾驶员可能有多种合理的行为选择（如变道、保持车道、转弯） - 不同意图导致完全不同的轨迹

2. 交互不确定性（Interaction Uncertainty） - 多个智能体之间的相互影响 - 社会规范和驾驶礼仪的影响

3. 感知不确定性（Perception Uncertainty） - 传感器噪声和遮挡 - 目标检测和跟踪的误差

处理不确定性的主要方法：

概率分布建模：

• 高斯混合模型（GMM）：Y ~ Σ_k π_k N(μ_k, Σ_k)
• 变分自编码器（VAE）：学习潜在空间分布
• 生成对抗网络（GAN）：隐式分布建模

多模态预测：

• 输出K个可能的轨迹假设
• 每个轨迹附带概率权重
• 覆盖不同的行为模式

5.1.3 评估指标

1. 位移误差指标 - ADE (Average Displacement Error)：平均位移误差 ADE = (1/T_f) Σ_{t=1}^{T_f} ||ŷ_t - y_t||_2

• FDE (Final Displacement Error)：终点位移误差 FDE = ||ŷ_{T_f} - y_{T_f}||_2

2. 多模态预测指标 - minADE：最佳模态的ADE minADE = min_k ADE(ŷ^k, y)

• MR (Miss Rate)：最佳预测与真值距离超过阈值的比例 MR = P(minFDE > threshold)

3. 概率评估指标 - NLL (Negative Log-Likelihood)：负对数似然 - KL散度：预测分布与真实分布的差异

Rule of Thumb：

• 城市场景：ADE < 1.0m, FDE < 2.0m (预测时域3秒)
• 高速场景：ADE < 2.0m, FDE < 4.0m (预测时域5秒)
• Top-K预测（K=6）的minADE应比单一预测降低30-50%

5.2 多智能体交互建模

5.2.1 社会力模型

社会力模型将交通参与者间的相互作用类比为物理力场，每个智能体受到目标吸引力、障碍物排斥力和社会力的共同作用。

智能体受力分析：
        目标吸引力
            ↑
            |
    邻车 ← 自车 → 邻车
    排斥    |    排斥
            ↓
        道路边界排斥

总作用力：F = F_goal + F_obstacle + F_social

虽然社会力模型物理意义明确，但难以捕捉复杂的交互模式，现已较少单独使用。

5.2.2 图神经网络方法

图神经网络（GNN）通过构建智能体交互图，显式建模智能体间的关系。

图构建：

• 节点：每个交通参与者
• 边：智能体间的潜在交互关系
• 特征：位置、速度、类型、历史轨迹

消息传递机制：

节点更新过程：
h_i^{l+1} = σ(W_self · h_i^l + Σ_{j∈N(i)} W_msg · m_{j→i}^l)

其中：

- h_i^l: 节点i在第l层的特征
- m_{j→i}: 从节点j到i的消息
- N(i): 节点i的邻居集合

典型架构：

• VectorNet：将地图和智能体统一表示为向量图
• LaneGCN：基于车道拓扑的图卷积网络
• TPCN：时序概率图网络

5.2.3 Transformer交互建模

Transformer通过自注意力机制隐式学习智能体间的交互关系，无需预定义图结构。

多智能体注意力：

注意力计算：
Attention(Q,K,V) = softmax(QK^T/√d_k)V

Q: 查询智能体特征
K: 所有智能体的键特征
V: 所有智能体的值特征

时空建模：

• 空间注意力：建模同一时刻不同智能体的关系
• 时间注意力：建模同一智能体不同时刻的演化
• 交叉注意力：智能体与地图元素的关系

代表性工作：

• Scene Transformer：统一的场景表示和预测
• Wayformer：多模态运动预测
• MTR (Motion Transformer)：目标导向的轨迹预测

Rule of Thumb：

• 注意力头数：8-16个
• 隐藏维度：256-512
• Transformer层数：4-6层
• 有效注意力范围：50-100米

5.3 轨迹生成技术

5.3.1 基于规则的方法

传统方法通过预定义的运动模型生成轨迹：

恒速模型（CV）： x_{t+1} = x_t + v_t · Δt

恒加速模型（CA）： x_{t+1} = x_t + v_t · Δt + 0.5 · a_t · Δt² v_{t+1} = v_t + a_t · Δt

自行车模型（Bicycle Model）： 考虑车辆运动学约束，适用于低速场景。

这些方法计算简单但预测精度有限，通常作为基线或与学习方法结合使用。

5.3.2 学习型轨迹生成

1. 回归方法 直接预测未来轨迹点的坐标：

• MLP回归器：简单但表达能力有限
• CNN编码器-解码器：捕获空间结构
• RNN/LSTM：建模时序依赖

2. 生成模型 - VAE-based：学习轨迹的潜在表示 编码器：q(z|x,y) 解码器：p(y|x,z)

• GAN-based：通过对抗训练生成真实轨迹 生成器：G(x,z) → y 判别器：D(x,y) → [0,1]

• Diffusion Models：最新方法，逐步去噪生成轨迹 前向过程：q(y_t|y_{t-1}) = N(y_t; √(1-β_t)y_{t-1}, β_t I) 反向过程：p_θ(y_{t-1}|y_t)

3. 目标条件生成 给定目标点，生成到达该目标的轨迹：

• TNT：预测目标候选点，然后生成轨迹
• DenseTNT：密集目标采样
• PRIME：优先级目标采样

5.3.3 多模态轨迹预测

现实驾驶场景存在多种合理的未来可能，多模态预测旨在覆盖这些可能性。

模态生成策略：

1. 锚点方法（Anchor-based） - 预定义K个轨迹原型（如直行、左转、右转） - 对每个原型进行精调 - 优点：可解释性强 - 缺点：灵活性受限

2. 目标采样（Goal Sampling） - 在可行驶区域采样多个目标点 - 为每个目标生成轨迹 - 优点：覆盖度好 - 缺点：计算开销大

3. 潜变量采样（Latent Sampling） - 从学习的潜在空间采样 - 解码为不同的轨迹模态 - 优点：自动学习模态 - 缺点：可解释性差

模态选择与评分：

• 为每个模态分配概率分数
• 基于场景上下文选择最可能的模态
• 运行时可根据置信度阈值动态调整输出模态数

Rule of Thumb：

• 城市场景：6-10个模态
• 高速场景：3-5个模态
• 模态间最小距离：> 0.5m（避免冗余）
• Top-1准确率：> 40%
• Top-5覆盖率：> 80%

5.4 地图表示与编码

5.4.1 高精地图结构

高精地图为预测提供关键的结构化先验信息：

地图要素：

道路拓扑结构：
├── 车道线（Lane Lines）
│   ├── 类型：实线、虚线、双黄线
│   └── 属性：可变道、禁止变道
├── 车道中心线（Lane Centerlines）
│   ├── 连接关系：前驱、后继、左右邻居
│   └── 属性：限速、转向类型
├── 交通标志（Traffic Signs）
│   ├── 停止线、人行横道
│   └── 交通信号灯
└── 道路边界（Road Boundaries）
    ├── 路缘、护栏
    └── 可行驶区域

坐标系统：

• 全局坐标系：WGS84、UTM
• 局部坐标系：车辆坐标系、Frenet坐标系

5.4.2 向量化地图表示

向量化表示将地图元素编码为向量序列，便于神经网络处理。

多段线表示（Polyline）： 每条车道线表示为点序列： L = {p_1, p_2, ..., p_n}，其中p_i = (x_i, y_i, θ_i, w_i)

向量编码：

单个向量特征：
v = [Δx, Δy, θ, ||v||, type_encoding]

其中：

- Δx, Δy: 向量起止点位移
- θ: 向量方向
- ||v||: 向量长度
- type_encoding: 类型编码（one-hot）

层级聚合：

1. 点级特征：单个地图点的属性
2. 线段级特征：聚合多个点形成线段特征
3. 车道级特征：聚合线段形成完整车道
4. 局部地图特征：聚合邻近车道

5.4.3 神经地图编码

1. CNN编码 将地图栅格化为BEV图像，使用CNN提取特征：

• 分辨率：0.1-0.5 m/pixel
• 覆盖范围：100×100 m
• 通道：不同地图元素类型

2. GNN编码 构建地图拓扑图：

• 节点：车道段
• 边：连接关系（前后、左右）
• 消息传递：聚合邻居车道信息

3. Transformer编码

地图注意力：
MapAttn = MultiHeadAttn(Q_agent, K_map, V_map)

Q_agent: 智能体查询
K_map, V_map: 地图元素的键值

4. 隐式神经表示 使用神经网络隐式编码地图：

• Neural Fields：连续空间表示
• 查询任意位置的地图特征
• 无需显式存储地图

Rule of Thumb：

• 地图编码范围：半径50-100m
• 向量化采样间隔：1-2m
• 特征维度：64-128维
• 更新频率：静态地图10Hz，动态信息（信号灯）1Hz

5.5 路径规划算法

5.5.1 全局路径规划

全局规划在道路网络层面生成从起点到终点的参考路径。

基于图搜索的方法：

Dijkstra算法：

• 保证最短路径
• 时间复杂度：O(V²)或O(E log V)使用优先队列

A*算法：

• 启发式搜索：f(n) = g(n) + h(n)
• g(n)：起点到n的实际代价
• h(n)：n到终点的启发式估计

A*搜索过程：
起点 S ──→ 扩展节点 ──→ ... ──→ 目标 G
     ↓        ↓              ↓
  开放列表  评估f值      选择最小f值

分层规划：

• 高层：道路级规划（选择道路序列）
• 中层：车道级规划（选择车道序列）
• 底层：轨迹级规划（生成可执行轨迹）

5.5.2 局部路径优化

局部规划生成满足动力学约束的平滑轨迹。

基于采样的方法：

RRT (Rapidly-exploring Random Tree)：

算法流程：

1. 从初始状态开始
2. 随机采样目标点
3. 找到树中最近节点
4. 向目标扩展新节点
5. 碰撞检测
6. 重复直到达到目标

Lattice Planner： 在状态空间预定义的格点上搜索：

• 横向采样：车道偏移
• 纵向采样：速度、加速度
• 时间采样：到达时间

基于优化的方法：

二次规划（QP）：

min J = w_ref||τ - τ_ref||² + w_smooth||Δ²τ||² + w_acc||Δ³τ||²
s.t. 动力学约束、避障约束、舒适性约束

非线性优化（NLP）： 处理非凸约束：

• 序列二次规划（SQP）
• 内点法（Interior Point）
• ADMM（交替方向乘子法）

5.5.3 实时性考虑

计算预算分配：

• 感知：30-50ms
• 预测：20-30ms
• 规划：20-30ms
• 控制：10ms
• 总延迟：< 100ms

加速策略：

1. 分层递进 - 粗规划：快速生成参考路径（5ms） - 精规划：局部优化（15ms） - 紧急响应：反应式避障（5ms）

2. 并行计算 - GPU加速：批量轨迹评估 - 多线程：独立模块并行 - 流水线：不同时间戳并行处理

3. 增量更新 - 重用上一帧规划结果 - 局部地图增量更新 - 轨迹热启动

4. 早停机制 - 设置迭代上限 - 次优解接受阈值 - 任意时间算法

Rule of Thumb：

• 规划频率：10-20 Hz
• 规划时域：5-8秒
• 轨迹点数：50-80个
• 重规划触发：偏差 > 0.5m 或新障碍物

5.6 最新技术进展

5.6.1 端到端预测与规划

将预测和规划联合优化，避免级联误差：

联合建模优势：

• 预测考虑自车规划意图
• 规划利用预测不确定性
• 端到端可微分训练

代表性方法：

• PiP (Prediction in Planning)：规划条件下的预测
• ILQR-DQN：结合模型预测控制和深度Q网络
• Contingency Planning：考虑多种预测场景的鲁棒规划

5.6.2 大模型在预测中的应用

预训练模型：

• 在大规模驾驶数据上预训练
• 迁移到特定场景微调
• 零样本泛化能力

GPT-style自回归预测：

轨迹token化：
轨迹 → [x₁,y₁,x₂,y₂,...] → [token₁,token₂,...]
                ↓
            Transformer
                ↓
        预测下一个token

优势：

• 长期依赖建模
• 多任务学习（预测+意图识别+风险评估）
• 少样本学习能力

挑战：

• 计算开销大
• 实时性要求高
• 安全性验证困难

5.6.3 社会兼容性预测

预测不仅要准确，还要符合社会规范：

社会力场建模：

• 学习隐式社会规则
• 礼让行为预测
• 文化差异适应

交互式预测：

• 博弈论建模
• 逆强化学习推断他人目标
• 协商与合作行为

Rule of Thumb：

• 大模型参数量：100M-1B for 车载部署
• 预训练数据量：> 100M 轨迹片段
• 微调数据量：10K-100K 场景特定样本

5.7 本章小结

本章系统介绍了自动驾驶中的预测与路径规划技术。核心要点包括：

预测技术演进：

• 从基于规则到深度学习
• 从单一预测到多模态预测
• 从独立预测到交互式预测

关键技术突破：

1. 图神经网络：显式建模多智能体交互，特别适合结构化场景
2. Transformer架构：通过注意力机制捕获复杂交互模式，成为当前主流
3. 向量化地图：统一表示地图和智能体，提升预测精度30-50%
4. 多模态预测：覆盖多种可能的未来，关键指标minADE < 1.0m@3s

规划算法体系：

• 全局规划：道路级别的路径搜索（A*、Dijkstra）
• 局部规划：满足动力学约束的轨迹生成（Lattice、优化方法）
• 实时性保证：分层规划、并行计算、增量更新

前沿发展方向：

• 端到端联合预测与规划，避免级联误差
• 大模型提升长尾场景泛化能力
• 社会兼容性建模，生成人类友好的驾驶行为

工程实践要点：

• 预测时延 < 30ms，规划时延 < 30ms
• 多模态数量与场景复杂度匹配（城市6-10个，高速3-5个）
• 地图编码范围50-100m，更新频率10Hz

记住：预测的目的不是完美预知未来，而是为规划提供足够的信息以做出安全决策。多模态预测+鲁棒规划是当前最可靠的技术路线。

5.8 练习题

基础题

习题5.1：解释为什么自动驾驶中需要多模态轨迹预测而不是单一最优预测？请举例说明。

习题5.2：比较GNN和Transformer在多智能体交互建模中的优缺点。

习题5.3：设计一个简单的轨迹预测评估实验，包括数据集、评估指标和基线方法。

for each test_sample:
    history = sample[-2s:0s]
    ground_truth = sample[0s:6s]
    predictions = model(history)  # K个模态

    # 计算指标
    errors = [ADE(pred, gt) for pred in predictions]
    minADE = min(errors)

report mean(minADE), mean(minFDE), MR

挑战题

习题5.4：如何设计一个能够处理罕见场景（如紧急车辆、道路施工）的预测系统？讨论数据收集、模型架构和评估方法。

分层架构：
├── 场景识别模块：检测是否为罕见场景
├── 通用预测器：处理常规场景
└── 专家模型集合：
    ├── 紧急车辆专家
    ├── 施工区域专家
    └── 异常行为专家

习题5.5：设计一个预测-规划联合优化框架，解释如何处理两个模块之间的耦合和反馈。

输入场景 → 共享编码器 → [预测分支]
                       ↓     ↓
                  [规划分支] ←
                       ↓
                  联合输出层

# 伪代码
scene_encoding = encoder(observations)

# 预测其他智能体
others_pred = predictor(scene_encoding)

# 条件规划：基于预测结果
ego_plan = planner(scene_encoding, others_pred)

# 反应式预测：考虑自车计划
others_react = reactive_predictor(others_pred, ego_plan)

L_total = λ₁L_pred + λ₂L_plan + λ₃L_interact + λ₄L_safety

其中：
L_pred: 预测准确性损失
L_plan: 规划模仿损失
L_interact: 交互一致性损失
L_safety: 安全约束违反惩罚

博弈论框架：

- 自车效用：U_ego = -cost(plan|predictions)
- 他车效用：U_other = -cost(reaction|plan)
- 纳什均衡：∂U_ego/∂plan = 0, ∂U_other/∂pred = 0

# 保持模块化用于调试
if joint_mode:
    output = joint_forward(input)
else:
    pred = predictor(input)
    plan = planner(input, pred)

习题5.6：分析基于Transformer的预测模型在实际部署中的计算瓶颈，提出三种优化方案。

实现策略：

1. 局部注意力：只关注50m内智能体
   复杂度：O(N×k×d)，k<<N

2. 图引导注意力：基于道路拓扑
   - 同车道：强注意力
   - 邻车道：中等注意力
   - 远距离：弱/无注意力

3. 动态稀疏：基于相关性阈值
   if attention_score < threshold:
       mask out

压缩技术栈：

1. 知识蒸馏：
   Teacher (1B参数) → Student (100M参数)

   - 软标签蒸馏
   - 特征对齐
   - 注意力迁移

2. 量化：
   - INT8量化：4×加速
   - 混合精度：关键层FP16，其他INT8
   - 量化感知训练(QAT)

3. 剪枝：
   - 结构化剪枝：删除整个注意力头
   - 非结构化：稀疏权重矩阵
   - 渐进式剪枝：训练时逐步稀疏化

硬件优化：

1. Flash Attention：
   - 融合kernel减少内存访问
   - 分块计算优化cache使用
   - 2-3×加速，内存减少10×

2. TensorRT优化：
   - 层融合：QKV投影合并
   - 动态批处理
   - FP16/INT8自动混合精度

3. 专用加速器：
   - NPU/TPU部署
   - 自定义ASIC（如Tesla D1）
   - FPGA流水线实现

分级处理策略：
Level 1 (1ms)：关键智能体快速预测
Level 2 (10ms)：完整场景预测
Level 3 (100ms)：长期战略预测

时序优化：

- 帧间增量计算
- KV cache复用
- 异步预测更新

5.9 常见陷阱与错误（Gotchas）

数据相关陷阱

1. 坐标系混淆 - 错误：混用全局坐标和车辆坐标 - 后果：预测结果错位，特别是在旋转后 - 解决：始终明确坐标系，进行必要转换 - 检查：可视化预测轨迹，确认对齐

2. 时间戳不对齐 - 错误：不同传感器数据时间戳不同步 - 后果：基于过时信息预测，精度下降30%+ - 解决：时间同步，考虑传感器延迟 - 最佳实践：使用统一时钟源，补偿已知延迟

3. 数据泄露 - 错误：训练时使用了未来信息 - 症状：训练精度极高，测试性能差 - 示例：使用t+1时刻的地图信息预测t时刻 - 预防：严格的时序分割，因果masking

模型相关陷阱

4. 模态坍缩 - 现象：多模态预测退化为单一模态 - 原因：损失函数设计不当，如简单MSE - 解决：使用winner-takes-all损失，或GMM/VAE - 诊断：计算模态多样性指标

5. 过拟合静态模式 - 错误：模型记忆训练集的静态模式 - 表现：新场景泛化差，如新路口 - 解决：数据增强，旋转/平移不变性 - 验证：留出整个地理区域做测试

6. 忽视稀有事件 - 问题：99%准确率，但1%的失败是致命的 - 示例：倒车、U-turn、紧急制动 - 对策：过采样稀有事件，分层建模 - 评估：分别统计稀有事件性能

工程实施陷阱

7. 级联误差放大 - 链条：感知误差→预测偏差→规划失败 - 量化：每层10%误差，三层后27%误差 - 缓解：端到端训练，不确定性传播 - 监控：各模块误差归因分析

8. 实时性假设过于乐观 - 错误：只测试单帧延迟，忽视系统延迟 - 现实：排队、调度、通信开销可达50ms+ - 测试：端到端延迟测试，最坏情况分析 - 优化：预测未来状态补偿延迟

9. 地图依赖过强 - 问题：高精地图缺失或过时时失效 - 场景：临时施工、新道路 - 解决：mapless方法作为降级方案 - 比例：地图特征权重<50%

评估相关陷阱

10. 评估指标与安全脱节 - 问题：ADE很低但产生危险行为 - 示例：平均准确但关键时刻失败 - 改进：增加安全相关指标（碰撞率、TTC） - 权重：安全指标权重>准确性指标

11. 过度依赖公开数据集 - 局限：数据集偏差（地域、天气、时间） - 风险：过拟合benchmark，实际场景失败 - 对策：自建测试集，分布外(OOD)测试 - 标准：私有测试集性能为主要指标

12. 忽视长尾场景 - 统计：90%性能提升来自常见场景 - 现实：10%长尾场景导致90%事故 - 平衡：专门的长尾测试集 - 指标：worst-case性能保证

调试技巧

快速诊断流程：

1. 可视化预测与真值对比
2. 检查输入数据质量（NaN、异常值）
3. 分析错误案例的共同特征
4. 逐层检查中间输出
5. 对比不同模型的失败案例

常用调试工具：

• TensorBoard：训练曲线、注意力权重可视化
• Weights & Biases：实验跟踪、异常检测
• 自定义可视化：BEV视图、3D轨迹
• 单元测试：坐标转换、损失函数

性能优化检查清单：

• [ ] 批处理是否充分利用
• [ ] 是否有冗余计算可以缓存
• [ ] 注意力范围是否过大
• [ ] 是否可以降低输出频率
• [ ] 模型精度是否过剩

记住：大多数bug来自数据处理和坐标转换，而非模型架构。先检查简单的错误，再怀疑复杂的算法问题。