第四章：全局路径规划（一）——图搜索基础
各位同学，今天我们来聊聊全局路径规划里最基础、也最核心的一块——图搜索算法。说实话，我当年刚入行时，觉得路径规划不就是找条路嘛，有啥难的？直到第一次在实车上跑，地图一加载，路网一复杂，Dijkstra直接卡死，我才意识到——嗯，这里面的门道比想象中深得多。
4.1 为什么需要图搜索？
自动驾驶的全局路径规划，说白了就是回答一个问题：从A点到B点，走哪条路最合适？
你想想看，城市路网就是一张巨大的图。路口是节点，道路是边。每条边都有权重——可能是距离、时间、或者拥堵程度。我们要做的，就是在这张图上找到一条最优路径。
我个人习惯把这个问题拆成两步：
- 建图：把真实路网抽象成带权图
- 搜索：在图上找到最优路径
今天重点讲搜索。两个最经典的算法：Dijkstra和A*。
4.2 Dijkstra算法原理
Dijkstra算法，我愿称之为「老实人算法」。它不耍花招，就是老老实实地从起点开始，一层一层往外扩展，直到找到终点。
4.2.1 核心思想
Dijkstra维护一个「最短距离表」，记录从起点到每个节点的当前最短距离。每次从未处理的节点中，挑一个距离最小的，然后更新它的邻居。
伪代码长这样：
function Dijkstra(Graph, start):
dist[start] = 0
for each node v in Graph:
if v != start:
dist[v] = INFINITY
add v to unvisited_set
while unvisited_set is not empty:
u = node with smallest dist in unvisited_set
remove u from unvisited_set
for each neighbor v of u:
alt = dist[u] + weight(u, v)
if alt < dist[v]:
dist[v] = alt
prev[v] = u
return dist, prev这里有个关键点：为什么每次要选距离最小的节点？因为Dijkstra保证，一旦某个节点被从unvisited_set中移除，它的最短距离就已经确定了。这就是贪心策略的正确性保证。
重要性质：Dijkstra要求所有边的权重非负。如果有负权边，算法会失效。我在项目中遇到过有人用Dijkstra处理带负权的地图，结果路径完全不对——嗯，这是个经典坑。
4.2.2 时间复杂度
如果用朴素数组实现，复杂度是O(V²)。V是节点数。对于城市路网，V可能上百万，O(V²)根本跑不动。
我建议用优先队列（最小堆）优化，复杂度降到O((V+E)logV)。E是边数。实际路网中E≈2V（每个路口连2-4条路），所以近似O(VlogV)。
| 实现方式 | 时间复杂度 | 适用场景 |
|---|---|---|
| 朴素数组 | O(V²) | 小图（V < 1000） |
| 二叉堆 | O((V+E)logV) | 中等规模 |
| 斐波那契堆 | O(VlogV+E) | 理论最优，实现复杂 |
我的经验：工程上二叉堆就够用了。斐波那契堆常数大，实际跑起来不一定快。别为了理论最优牺牲工程稳定性。
4.3 A*算法原理
Dijkstra有个问题：它太「老实」了。它会往所有方向扩展，哪怕终点就在起点正后方。A*算法就是来解决这个问题的。
4.3.1 启发式函数
A*在Dijkstra的基础上加了一个启发式函数h(n)，用来估计从节点n到终点的距离。总代价变成：
f(n) = g(n) + h(n)
- g(n)：从起点到n的实际代价（和Dijkstra一样）
- h(n)：从n到终点的估计代价
算法每次选f(n)最小的节点扩展。这样，它会优先往「看起来离终点近」的方向走。
伪代码：
function AStar(Graph, start, goal):
open_set = {start}
g[start] = 0
f[start] = h(start, goal)
while open_set is not empty:
current = node with smallest f in open_set
if current == goal:
return reconstruct_path(prev, current)
open_set.remove(current)
closed_set.add(current)
for each neighbor v of current:
if v in closed_set:
continue
tentative_g = g[current] + weight(current, v)
if v not in open_set or tentative_g < g[v]:
g[v] = tentative_g
f[v] = g[v] + h(v, goal)
prev[v] = current
if v not in open_set:
open_set.add(v)
return failure4.3.2 启发式函数设计
启发式函数的设计直接决定A*的表现。我总结了几种常见选择：
| 启发式函数 | 公式 | 特点 |
|---|---|---|
| 曼哈顿距离 | |dx| + |dy| | 适用于网格地图，允许四方向移动 |
| 欧几里得距离 | √(dx² + dy²) | 适用于任意方向移动，但计算稍慢 |
| 对角线距离 | max(|dx|, |dy|) | 适用于允许对角线移动的网格 |
关键约束：h(n)必须是可采纳的（admissible），即h(n) ≤ 实际从n到终点的真实代价。否则A*可能找不到最优解。我曾经在项目中用了不可采纳的启发式，结果路径绕了一大圈——排查了半天才发现是h(n)设大了。
4.3.3 Dijkstra vs A*
两者本质区别：
- Dijkstra：f(n) = g(n)，只考虑已走的路
- A*：f(n) = g(n) + h(n)，兼顾已走和未走
如果h(n)=0，A*退化为Dijkstra。如果h(n)恰好等于真实距离，A*直接沿着最优路径走，一步不浪费。实际中h(n)通常介于两者之间。
我的建议：如果地图很大且终点明确，用A*。如果地图小或者需要全图信息（比如计算所有点到某点的距离），用Dijkstra。没有银弹，看场景选。
4.4 在路网中的实际应用
理论讲完了，聊聊落地。真实路网和教科书上的图，差距有多大？
4.4.1 路网建模
真实路网不是简单的网格。路口有红绿灯、有转向限制、有单行道。我一般这样建模：
- 节点：路口 + 道路上的关键点（曲率变化点）
- 边：连接两个节点的路段，权重可以是距离、预计通行时间
- 转向代价：左转、右转、直行的代价不同，需要额外处理
举个例子，一个十字路口有4个入口、4个出口，理论上16种转向组合。但实际中左转可能等红灯，右转可能不用等。这些都要编码到图里。
4.4.2 性能优化
城市路网动辄几十万节点，直接跑A*也慢。我常用的优化手段：
- 分层规划：先在高层次（快速路、主干道）规划，再在低层次（具体车道）细化
- 路网预处理：提前计算一些关键节点之间的最短路径，存起来
- 双向搜索：从起点和终点同时搜索，相遇即停止
避坑指南：我曾经在双向A*中踩过一个坑——两个方向的启发式函数不一致，导致搜索空间重叠严重，反而比单向还慢。后来统一用同一个启发式函数，效果才好起来。
4.4.3 动态路网
真实路况是动态的。堵车、封路、事故都会改变边的权重。怎么办？
- 重规划：定期重新运行A*，更新路径
- 增量式搜索：只更新受影响的部分，比如D* Lite算法
- 混合策略：全局用A*，局部用D* Lite微调
我个人倾向第三种。全局路径变化不频繁，用A*就够了。局部遇到突发情况，用增量搜索快速调整。
4.5 小结
这一章我们聊了：
- Dijkstra算法：老实人算法，保证最优，但效率一般
- A*算法：加了个启发式函数，效率大幅提升
- 启发式函数设计：曼哈顿、欧几里得、对角线，各有适用场景
- 路网应用：建模、优化、动态处理，全是实战经验
下一章我们会深入A*的变种，比如Hybrid A*——它考虑了车辆的运动学约束，是自动驾驶路径规划里的重头戏。到时候我会分享一个我在高速场景下用Hybrid A*的案例，挺有意思的。
今天就到这里。有问题随时交流。
公众号:蓝海资料掘金营，微信deep3321