第4章：图搜索基础——从图论到Dijkstra算法
各位同学好，我是你们的老朋友。今天我们来聊聊路径规划中最基础、也最核心的一块——图搜索。说实话，我入行那会儿，觉得图搜索不就是个数据结构题嘛，直到在AGV调度项目里被现实狠狠教育了一顿，才明白这东西有多重要。
你想想看，机器人要在地图里找路，本质上就是在图里做搜索。不管是扫地机器人还是自动驾驶，底层逻辑都离不开今天要讲的这几个算法。嗯，咱们一个一个来。
4.1 图论基础：把世界抽象成节点和边
图是什么？说白了就是一堆点（节点）和连接它们的线（边）。我习惯把节点想象成地图上的路口，边就是路口之间的路。
图分两种：有向图和无向图。有向图就像单行道，只能朝一个方向走；无向图就像双向车道，两边都能走。另外还有加权图，每条边带一个权重，比如距离、时间、能耗等。
核心概念速览：
- 节点（Vertex）：图中的基本单元，比如坐标点、路口
- 边（Edge）：连接两个节点的路径
- 权重（Weight）：边的代价，比如距离或时间
- 邻接（Adjacency）：两个节点之间有边直接相连
- 路径（Path）：从起点到终点经过的节点序列
我在做仓储机器人项目时，经常把货架位置作为节点，机器人移动的通道作为边。权重就是实际距离。这样一抽象，路径规划问题就变成了图上的最短路径问题。
4.2 深度优先搜索（DFS）：一条路走到黑
DFS的思路很简单：从起点出发，沿着一条路一直走，直到走不动了再回头。我管它叫「不撞南墙不回头」策略。
它的核心数据结构是栈（Stack）。每次访问一个节点，就把它的邻居压入栈中，然后弹出栈顶继续访问。
def dfs(graph, start, goal):
stack = [start]
visited = set()
parent = {start: None}
while stack:
node = stack.pop()
if node == goal:
return reconstruct_path(parent, start, goal)
if node not in visited:
visited.add(node)
for neighbor in graph[node]:
if neighbor not in visited:
parent[neighbor] = node
stack.append(neighbor)
return None我的经验：DFS适合搜索空间特别大的情况，比如迷宫求解。但要注意，它找到的路径不一定是最短的。我曾经在AGV调度里用DFS找路径，结果机器人绕了一大圈，被项目经理骂了一顿。
4.3 广度优先搜索（BFS）：层层推进，稳扎稳打
BFS和DFS正好相反。它像水波一样，从起点一圈一圈往外扩散。核心数据结构是队列（Queue）。
BFS有个重要特性：在无权图中，它找到的路径一定是最短的。为什么？因为它按层遍历，先访问离起点近的节点。
from collections import deque
def bfs(graph, start, goal):
queue = deque([start])
visited = set([start])
parent = {start: None}
while queue:
node = queue.popleft()
if node == goal:
return reconstruct_path(parent, start, goal)
for neighbor in graph[node]:
if neighbor not in visited:
visited.add(neighbor)
parent[neighbor] = node
queue.append(neighbor)
return None避坑指南：我曾经在室内机器人导航里用BFS，地图网格是1000x1000的。结果BFS把整个地图都遍历了一遍，内存直接爆了。记住，BFS在搜索空间大时非常吃内存，因为它要存所有已访问的节点。
4.4 Dijkstra算法：带权重的BFS
BFS只能处理无权图。但现实世界哪有那么理想？每条路的长度、拥堵程度都不一样。这时候就需要Dijkstra算法了。
Dijkstra算法的核心思想：贪心策略。每次从「未确定最短路径」的节点中，选一个距离起点最近的，然后更新它的邻居。
它用优先队列（Priority Queue）来维护待处理的节点，按当前已知的最短距离排序。
import heapq
def dijkstra(graph, start, goal):
# graph: {node: [(neighbor, weight), ...]}
pq = [(0, start)] # (距离, 节点)
dist = {start: 0}
parent = {start: None}
while pq:
current_dist, node = heapq.heappop(pq)
if node == goal:
return reconstruct_path(parent, start, goal)
if current_dist > dist.get(node, float('inf')):
continue
for neighbor, weight in graph[node]:
new_dist = current_dist + weight
if new_dist < dist.get(neighbor, float('inf')):
dist[neighbor] = new_dist
parent[neighbor] = node
heapq.heappush(pq, (new_dist, neighbor))
return None关键点：Dijkstra算法要求所有边的权重非负。如果有负权边，它会陷入死循环。我在做无人机航线规划时遇到过这个问题——有个航段的「油耗」是负的（顺风），结果算法一直绕圈。后来改用Bellman-Ford才解决。
4.5 三种算法的对比与选择
| 算法 | 数据结构 | 适用场景 | 时间复杂度 | 是否保证最短路径 |
|---|---|---|---|---|
| DFS | 栈 | 迷宫求解、拓扑排序 | O(V+E) | 否 |
| BFS | 队列 | 无权图最短路径 | O(V+E) | 是（无权图） |
| Dijkstra | 优先队列 | 加权图最短路径 | O((V+E)logV) | 是（非负权） |
我个人习惯这样选：如果地图是网格且每步代价相同，用BFS；如果地图有不同地形代价，用Dijkstra；如果只是要「找到一条路」而不在乎长短，用DFS。
4.6 知识体系总览
下面这张图是我手绘的，把今天讲的内容串起来。你一看就明白它们之间的关系了。
我的建议：初学者先手动画几个小图，手动模拟BFS和Dijkstra的执行过程。我当年就是这么学的，画了整整一沓A4纸。只有亲手推一遍，才能真正理解「松弛操作」和「贪心选择」是怎么回事。
好了，今天的内容就到这里。图搜索是路径规划的基石，后面讲A*、RRT这些高级算法时，你会发现它们都是在今天这些基础算法上做的改进。嗯，先把基础打牢，后面才能飞得起来。