全局路由与行为规划

本页覆盖"从地图到驾驶策略"的上层规划，包括路网搜索算法、行为决策建模与复杂场景策略。

1. 路网图模型

1.1 数据结构

路网在计算机中表示为有向加权图 $G = (V, E)$：

节点 $V$：路口、路段端点、变道连接点
边 $E$：道路段（单向），携带权重

边权重来源：

权重分量	说明
物理距离	道路几何长度（km）
行驶时间	基于历史/实时速度
道路等级惩罚	优先高速，避免村道
限行约束	货车限行、奇偶号限行
实时交通	拥堵系数，动态更新
安全偏好	避开高事故率路段

1.2 实时交通融合

云端路线规划需要融合实时交通数据：

路网基础图（静态）
      +
实时交通流速（动态，每 5 min 更新）
      +
事件信息（施工、封路、事故，秒级更新）
      =
综合代价图（用于查询）

2. 路由搜索算法

2.1 Dijkstra 算法

原理： 从起点逐步扩展最短路径前沿，保证找到全局最优解。

# 伪代码
def dijkstra(G, start, end):
    dist = {v: inf for v in G.nodes}
    dist[start] = 0
    pq = PriorityQueue()
    pq.push((0, start))

    while not pq.empty():
        d, u = pq.pop()
        if u == end: break
        for v, w in G.neighbors(u):
            if dist[u] + w < dist[v]:
                dist[v] = dist[u] + w
                pq.push((dist[v], v))

    return reconstruct_path(dist, end)

时间复杂度： $O((V + E) \log V)$（使用优先队列）
适用： 静态代价图，中等规模路网（城市级别可用）

2.2 A* 算法

A* 在 Dijkstra 基础上引入启发函数 $h(n)$，优先扩展更接近终点的节点：

\[f(n) = g(n) + h(n)\]

其中 $g(n)$ 为从起点到 $n$ 的已知最短代价，$h(n)$ 为从 $n$ 到终点的估计代价。

常用启发函数：

启发函数	公式	适用场景
欧氏距离	$h = \sqrt{\Delta x^2 + \Delta y^2}$	坐标图，无方向限制
曼哈顿距离	$h =	\Delta x
直线时间	$h = \text{distance} / v_{\max}$	时间代价路由

可接受性条件

$h(n)$ 必须不大于真实最优代价（"可接受启发式"），否则 A* 不保证找到最优解。

2.3 Contraction Hierarchies（CH）

CH 通过预处理将路网分层，实现超大规模路网（全国）的毫秒级查询：

预处理阶段（离线，耗时 1–10 分钟）：

按"重要性"排序节点（度数、路径中间节点频率等）
逐步"收缩"最不重要的节点，添加快捷边（Shortcut）
构建分层图

查询阶段（在线，< 1 ms）：

从起点向上（重要性升高方向）搜索
从终点向上搜索
在顶层中间节点合并

工程实践

车载路由通常使用 CH 或类似加速结构（Road Hierarchy、Arc Flags）。大规模路网的 Dijkstra 查询时延可能超过 1 秒，不满足实时需求。

3. 行为规划

行为规划决定车辆在局部交通交互中的"驾驶意图"，输出结构化驾驶行为（跟车/变道/让行/停车/通行）。

3.1 关键输入

┌──────────────────────────────────┐
│  地图语义（车道、限速、停止线）   │
│  感知输出（障碍物状态与轨迹）     │
│  预测输出（他车未来轨迹分布）     │
│  自车状态（位置、速度、能力）     │
│  全局路由（目标车道序列）         │
└──────────────────┬───────────────┘
                   ↓
              行为规划器
                   ↓
┌──────────────────────────────────┐
│  驾驶行为决策（枚举或连续空间）   │
│  目标车道 / 目标速度 / 行为模式  │
└──────────────────────────────────┘

3.2 有限状态机（FSM）

FSM 是最直观的行为规划实现方式：

       ┌──── keep_lane_cond ────┐
       │                        │
[KEEP_LANE] ──change_left──> [CHANGE_LEFT]
       │                        │
       └──change_right──> [CHANGE_RIGHT]
       │
       ├── stop_cond ──> [STOP_AT_LINE]
       │
       └── yield_cond ──> [YIELD]

状态转移条件示例（变道）：

目标车道安全间隙 > 阈值
变道收益 > 变道代价（舒适性惩罚）
自车在可变道区域
无路口/停止线冲突

FSM 局限：

状态数量随场景复杂度指数增长
边界条件和优先级冲突难以穷举
调试困难，新场景需要重新设计状态图

3.3 行为树（BT）

行为树通过模块化节点组合复杂策略：

根节点（选择节点）
├── 紧急停车检查（条件节点）→ [STOP]
├── 路口行为子树（序列节点）
│   ├── 路口检测（条件）
│   ├── 计算冲突区（动作）
│   └── 选择通行/让行（选择节点）
│       ├── 可通行（条件）→ [GO_STRAIGHT]
│       └── 让行（动作）→ [YIELD]
└── 常规驾驶子树
    ├── 变道评估（条件）→ [CHANGE_LANE]
    └── 跟车（动作）→ [FOLLOW]

BT 优势：

模块化，子树可复用
运行时可调试（可视化当前激活路径）
增加新场景只需添加新分支

3.4 基于学习的方法

方法	原理	适用场景	挑战
强化学习（RL）	通过环境交互学习策略	复杂交互场景	仿真与现实差距，安全约束难嵌入
模仿学习（IL）	从人类驾驶数据学习	常规工况泛化	复合误差，长尾覆盖不足
MCTS	树搜索 + 策略/价值估计	博弈场景（路口博弈）	计算量大

4. 路口与并线关键策略

4.1 无保护左转

步骤：
1. 进入路口等待区
2. 构建"冲突区"（与对向来车的交叉时空间隔）
3. 评估对向车辆间隙（Gap Acceptance）
   - 如果预计穿越时间 T_cross < 对向车辆 TTC：执行左转
   - 否则：等待下一个间隙
4. 执行轨迹，持续监控交互车辆

间隙接受模型：

\[\text{Accept Gap} = \begin{cases} 1 & \text{if } T_{\text{gap}} \geq T_{\text{critical}} \\ 0 & \text{otherwise} \end{cases}\]

$T_{\text{critical}}$ 通常在 4–6 秒，可根据车速、交叉角度和车辆动力学调整。

4.2 匝道汇入

策略框架：

估计主路车辆速度与位置分布
计算可用间隙窗口
决策："礼让减速"或"加速主动汇入"
生成汇入轨迹，保证最小 TTC > 3 s

4.3 施工绕行

施工区域的临时交通控制要素（锥桶、临时标志、施工人员指挥）优先级高于静态地图：

感知识别施工区域边界和引导方向
地图切换为"实时感知主导"模式
收紧行驶速度和变道策略

5. 常见失效与兜底

失效原因	现象	兜底策略
预测不稳定	目标策略频繁切换（抖动）	引入迟滞（Hysteresis）和最短保持时间
规则冲突	同时触发变道和让行	设置规则优先级（安全 > 效率 > 舒适）
决策超时	规划器计算超时未输出	上一周期安全策略延续，触发告警
交互车辆博弈僵局	双方相互等待，长时间静止	超时后触发"主动通行"或"呼叫远程协助"

6. 评估指标

6.1 安全指标

指标	说明
最小 TTC	与障碍物的最小碰撞时间
最小时距（THW）	跟车场景最小时间间距
冲突点暴露时长	在高风险区域停留的时间总量

6.2 效率指标

指标	说明
平均旅行速度	全程平均行驶速度（含等待）
不必要停车次数	非路口/信号灯导致的停车
额外行程时间	与最优路线的时间差

6.3 成功率指标

指标	说明
路口通行成功率	在合理时间内完成路口通行的比例
变道完成率	触发变道后成功完成的比例
任务完成率	不依赖人工干预完成全程的比例

启发函数	公式	适用场景
欧氏距离	\(h = \sqrt{\Delta x^2 + \Delta y^2}\)	坐标图，无方向限制
曼哈顿距离	$h =	\Delta x
直线时间	\(h = \text{distance} / v_{\max}\)	时间代价路由