第三章：图搜索基础——从图论到Dijkstra

各位同学，今天我们来聊聊路径规划里最基础、也最核心的一块——图搜索。说实话，我刚开始做机器人时，觉得这东西太理论了，不就是画几个圈圈连几条线吗？直到第一次在真实场景里跑算法，地图一复杂，机器人直接原地转圈……嗯，从那以后我再也不敢小看图搜索了。

这一章，我会带大家从图论的基本概念讲起，然后手撸DFS、BFS，最后重点攻克Dijkstra算法。你想想看，这些算法搞懂了，后面A*、RRT*什么的，理解起来就轻松多了。

3.1 图论基础：节点、边、权重

图是什么？说白了，就是一堆节点（Node）和连接它们的边（Edge）。节点可以代表城市、路口、或者机器人的位姿；边代表它们之间的连接关系。

我个人习惯把图分成两类：

  • 无向图：边没有方向，A能到B，B也能到A。比如城市间的公路。
  • 有向图：边有方向，A→B可以，但B→A不一定行。比如单行道。

权重（Weight）就更关键了。它可以是距离、时间、能耗，甚至风险值。我在项目中遇到过一个问题：用距离做权重，机器人总爱走大路，结果堵车堵到没电。后来我把时间作为权重，路径才合理起来。

核心概念速查表：
术语 英文 我的理解
节点 Node / Vertex 地图上的一个点，比如路口坐标
边 Edge 连接两个节点的路径段
权重 Weight 走这条边的代价，越大越不划算
邻接 Adjacency 两个节点之间有边直接相连

这里我画了一张图，帮你把知识体系串起来：

图搜索知识体系 图搜索 图论基础 节点·边·权重 DFS 深度优先 BFS 广度优先 Dijkstra 最短路径·带权 DFS/BFS是无权图搜索，Dijkstra是带权图搜索 三者层层递进，理解顺序很重要

3.2 深度优先搜索（DFS）

DFS的思路很简单：一条路走到黑，撞了南墙再回头。它用栈（Stack）来实现，或者用递归——本质上也是栈。

我刚开始学的时候，总觉得DFS不如BFS实用。后来做拓扑排序、检测环路时才发现，DFS在某些场景下简直无敌。

我的经验： 如果你要判断一个图里有没有环，DFS比BFS快得多。我曾经在调试SLAM后端时，就用DFS快速检测了因子图里有没有形成环路。

来看代码：

def dfs(graph, start, visited=None):
    if visited is None:
        visited = set()
    visited.add(start)
    print(f"访问节点: {start}")
    
    for neighbor in graph[start]:
        if neighbor not in visited:
            dfs(graph, neighbor, visited)
    return visited

# 示例图（邻接表）
graph = {
    'A': ['B', 'C'],
    'B': ['A', 'D', 'E'],
    'C': ['A', 'F'],
    'D': ['B'],
    'E': ['B', 'F'],
    'F': ['C', 'E']
}

dfs(graph, 'A')

输出顺序是 A → B → D → E → F → C。看到了吗？它先往深处扎，直到没有新节点才回头。

注意： 递归深度太深会导致栈溢出。我遇到过真实案例——地图有上万个节点，递归DFS直接崩了。后来改成显式栈才搞定。

3.3 广度优先搜索（BFS）

BFS和DFS正好相反。它一层一层地往外扩，像水波一样。它用队列（Queue）实现，先进先出。

BFS有个重要特性：在无权图中，它找到的路径一定是最短的（按边数算）。这一点在机器人导航里非常实用。

我记得有一次做仓库机器人调度，要求机器人走最少的路口数到达目标。BFS直接搞定，代码简单，效果还好。

from collections import deque

def bfs(graph, start):
    visited = set()
    queue = deque([start])
    visited.add(start)
    
    while queue:
        node = queue.popleft()
        print(f"访问节点: {node}")
        
        for neighbor in graph[node]:
            if neighbor not in visited:
                visited.add(neighbor)
                queue.append(neighbor)

bfs(graph, 'A')

输出顺序是 A → B → C → D → E → F。和DFS对比一下，是不是完全不一样？BFS先把A的邻居B、C都访问了，再往下走。

DFS vs BFS 对比：
  • DFS：适合找所有解、拓扑排序、检测环路
  • BFS：适合找最短路径（无权图）、层次遍历
  • DFS用栈（递归），BFS用队列
  • DFS可能陷入无限深，BFS可能占用大量内存

3.4 Dijkstra算法原理与实现

好了，重头戏来了。DFS和BFS都不考虑边的权重，但现实世界哪有这么简单？从A到B，走高速和走小路，时间能一样吗？

Dijkstra算法就是来解决这个问题的。它能在带权图中找到从起点到所有其他节点的最短路径。

核心思想其实就一句话：每次选当前代价最小的节点，然后更新它的邻居。说白了，就是贪心策略——每次都走当前看起来最划算的路。

我当年第一次手写Dijkstra时，犯了个低级错误：忘记用优先队列，每次手动找最小节点。结果地图一复杂，算法跑得比乌龟还慢。后来改用heapq，速度直接起飞。

import heapq

def dijkstra(graph, start):
    # graph: 邻接表，格式 {node: [(neighbor, weight), ...]}
    distances = {node: float('inf') for node in graph}
    distances[start] = 0
    pq = [(0, start)]  # (距离, 节点)
    visited = set()
    
    while pq:
        current_dist, current = heapq.heappop(pq)
        
        if current in visited:
            continue
        visited.add(current)
        
        for neighbor, weight in graph[current]:
            distance = current_dist + weight
            if distance < distances[neighbor]:
                distances[neighbor] = distance
                heapq.heappush(pq, (distance, neighbor))
    
    return distances

# 示例：城市间道路（带权重）
graph = {
    'A': [('B', 4), ('C', 2)],
    'B': [('A', 4), ('C', 1), ('D', 5)],
    'C': [('A', 2), ('B', 1), ('D', 8), ('E', 10)],
    'D': [('B', 5), ('C', 8), ('E', 2)],
    'E': [('C', 10), ('D', 2)]
}

result = dijkstra(graph, 'A')
for node, dist in result.items():
    print(f"从A到{node}的最短距离: {dist}")
避坑指南： 我曾经在项目中直接用Dijkstra处理动态障碍物，结果路径频繁重规划，机器人抖得像筛子。后来加了平滑处理和代价缓存，才稳定下来。记住：Dijkstra假设权重不变，动态环境要配合其他策略。

Dijkstra的时间复杂度是O((V+E)logV)，其中V是节点数，E是边数。用优先队列后，效率还不错。但如果地图特别大（比如几万个节点），就要考虑A*算法了——那是下一章的内容。

重要限制： Dijkstra不能处理负权边！如果有负权边，要用Bellman-Ford算法。我在做路径规划时遇到过这种情况：某些路段下坡，权重设成负值，结果Dijkstra直接算错。后来统一做了权重归一化才解决。

小结

这一章我们走完了图搜索的入门三部曲：DFS（一条路走到黑）、BFS（层层向外扩）、Dijkstra（带权最短路径）。

我个人建议你动手跑一遍代码，把图换一换，看看结果怎么变。纸上得来终觉浅，绝知此事要躬行——这话放在算法学习上，再合适不过了。

可可精品资料库

← 上一章 📖 返回目录 下一章 →