第三章 路径搜索基础：图搜索算法概览、Dijkstra算法原理与实现、A*算法原理与实现
各位同学，欢迎来到路径搜索的世界。说实话，这一章是整个路径规划课程的基石。你想想看，无论是无人车在十字路口选择车道，还是机器人绕过障碍物，底层都离不开这些图搜索算法。我个人习惯把这一章叫做「找路的三板斧」——图搜索基础、Dijkstra、A*。今天咱们就把它彻底吃透。
3.1 图搜索算法概览：从地图到图
先问个问题：为什么叫「图搜索」？
因为现实世界的地图，在计算机眼里就是一张图（Graph）。节点代表位置，边代表可通行的路径。我刚开始做自动驾驶时，总想着直接处理像素级的地图，后来发现那太笨了。把地图抽象成图，搜索效率能提升几个数量级。
图搜索的核心就三件事：
- 节点（Node）：比如路口、车道中心点、栅格中心
- 边（Edge）：节点之间的连接，通常带有权重（距离、时间、能耗）
- 搜索策略：怎么从起点走到终点，且代价最小
常见的图搜索算法可以分为两类：
| 类别 | 代表算法 | 特点 |
|---|---|---|
| 盲目搜索 | BFS、DFS、Dijkstra | 不利用目标信息，遍历所有可能 |
| 启发式搜索 | A*、D*、RRT | 利用启发函数引导搜索方向 |
嗯，这里要注意：盲目搜索不代表「笨」，它保证能找到最优解，只是慢。启发式搜索快，但需要好的启发函数。我在项目中遇到过，选错算法导致路径规划超时，车辆直接停在路中间——那场面，尴尬得很。
3.2 Dijkstra算法原理：稳扎稳打的贪心
Dijkstra算法，说白了就是「从起点出发，一层一层往外扩，每次选当前代价最小的节点」。它保证找到的是最短路径，前提是边的权重非负。
核心思想就一句话：维护一个距离表，每次从未访问的节点中选距离最小的，然后更新它的邻居。
伪代码长这样：
function Dijkstra(Graph, start):
dist[start] = 0
for each node v in Graph:
if v != start: dist[v] = INF
visited[v] = False
while 还有未访问的节点:
u = 未访问节点中 dist 最小的
visited[u] = True
for each neighbor v of u:
if not visited[v]:
new_dist = dist[u] + weight(u, v)
if new_dist < dist[v]:
dist[v] = new_dist
prev[v] = u
return dist, prev我曾经在高速场景下用过Dijkstra做全局路径规划。说实话，效果很稳，但就是慢。城市道路几百个节点还好，要是高精地图几万个节点，那计算时间能让你怀疑人生。
3.3 Dijkstra算法实现：手写一个最小堆版本
实际工程中，我们不会用数组找最小节点，那太慢了。用最小堆（优先队列）来优化，复杂度从O(V²)降到O((V+E)logV)。
Python实现如下：
import heapq
def dijkstra(graph, start):
# graph: 邻接表 {node: [(neighbor, weight), ...]}
dist = {node: float('inf') for node in graph}
dist[start] = 0
pq = [(0, start)] # (距离, 节点)
prev = {}
while pq:
current_dist, u = heapq.heappop(pq)
if current_dist > dist[u]:
continue # 过时的记录，跳过
for v, w in graph[u]:
new_dist = dist[u] + w
if new_dist < dist[v]:
dist[v] = new_dist
prev[v] = u
heapq.heappush(pq, (new_dist, v))
return dist, prev注意看第8行那个continue判断。我刚开始写的时候没加，结果堆里塞满了旧数据，内存直接爆了。你想想看，一个节点被更新了十几次，堆里就有十几个记录，不跳过多浪费。
3.4 A*算法原理：带方向感的Dijkstra
A*算法，说白了就是Dijkstra加了一个「指南针」。这个指南针就是启发函数h(n)，它估算从当前节点到终点的距离。
核心公式：f(n) = g(n) + h(n)
- g(n)：从起点到当前节点的实际代价（和Dijkstra一样）
- h(n)：从当前节点到终点的估计代价（启发函数）
- f(n)：总估计代价，优先选f最小的节点
为什么A*比Dijkstra快？因为h(n)引导搜索往目标方向走，而不是盲目地四周扩散。我做过对比实验：同样一张2000节点的地图，Dijkstra要遍历1800个节点，A*只遍历了300个——差距就是这么大。
但这里有个关键：h(n)必须满足可采纳性（admissible），即h(n) ≤ 真实代价。否则A*可能找不到最优解。我常用的启发函数：
| 场景 | 启发函数 | 说明 |
|---|---|---|
| 网格地图（四方向） | 曼哈顿距离 | |x1-x2| + |y1-y2| |
| 网格地图（八方向） | 切比雪夫距离 | max(|x1-x2|, |y1-y2|) |
| 任意方向 | 欧几里得距离 | √((x1-x2)² + (y1-y2)²) |
3.5 A*算法实现：从Dijkstra改两行代码
你猜怎么着？A*的代码和Dijkstra几乎一模一样，就改了两处：
- 优先队列里存的是f值，不是g值
- 需要计算启发函数h(n)
看代码：
import heapq
def heuristic(a, b):
# 曼哈顿距离
return abs(a[0] - b[0]) + abs(a[1] - b[1])
def a_star(graph, start, goal):
# graph: 邻接表，节点是(x,y)坐标
g = {node: float('inf') for node in graph}
g[start] = 0
f = {node: float('inf') for node in graph}
f[start] = heuristic(start, goal)
pq = [(f[start], start)]
prev = {}
while pq:
current_f, u = heapq.heappop(pq)
if u == goal:
break
if current_f > f[u]:
continue
for v, w in graph[u]:
new_g = g[u] + w
if new_g < g[v]:
g[v] = new_g
f[v] = new_g + heuristic(v, goal)
prev[v] = u
heapq.heappush(pq, (f[v], v))
return g, prev看到了吗？第17行计算f值，就是g值加上启发函数。其他逻辑和Dijkstra一模一样。所以我说，学会了Dijkstra，A*就是顺手的事。
3.6 实战对比：什么时候用哪个？
最后，我根据实际项目经验，给各位一个选择指南：
- 需要绝对最优解，且图不大（<1000节点） → 用Dijkstra，简单可靠
- 需要绝对最优解，图很大 → 用A*，但启发函数要精心设计
- 需要快速找到可行解，不要求最优 → 用贪心最佳优先搜索（Greedy BFS），或者RRT
- 动态环境，障碍物会变化 → 用D* Lite或A*重规划
我记得有一次做园区物流车，地图就500个节点，我偷懒用了A*。结果因为启发函数没调好，路径绕了个大弯。后来换成Dijkstra，虽然慢了0.2秒，但路径完美。所以啊，别盲目追求「高级」算法，适合场景的才是最好的。
好了，这一章的内容就到这。图搜索是路径规划的「内功」，Dijkstra和A*是两套最基础的「招式」。下一章咱们会讲更高级的算法，比如RRT和Hybrid A*，但今天这些基础打不牢，后面会越学越吃力。建议各位把代码亲手敲一遍，跑几个例子，感受一下搜索过程。