第四章：全局路径规划基础：图搜索算法概述、Dijkstra算法原理与实现
4.1 为什么我们需要“图”来思考？
做机器人控制这些年，我越来越觉得，路径规划本质上就是个“找路”的问题。你想想看，机器人从A点到B点，中间有障碍物、有地形限制、有运动约束——这不就是我们小时候玩的迷宫游戏吗？
但现实世界比迷宫复杂得多。我们得用一种数学工具来描述这个“世界”。这个工具就是——图（Graph）。
图由两部分组成：节点（Node）和边（Edge）。节点代表位置，边代表连接关系。比如，一个房间的四个角落是节点，门和走廊就是边。每条边还可以带一个权重，代表距离、时间或者能耗。
我个人习惯把图想象成一张“交通网”。节点是路口，边是道路，权重就是路长。路径规划，说白了就是在这张网上找到一条从起点到终点的最优路径。
4.2 图搜索算法家族概览
图搜索算法有很多，但万变不离其宗。我根据自己多年的项目经验，把它们分成两大类：
- 盲目搜索（Uninformed Search）： 不知道目标在哪，只能一步步试探。比如BFS（广度优先）、DFS（深度优先）。
- 启发式搜索（Informed Search）： 知道目标的大致方向，利用“启发函数”来引导搜索。比如A*算法、Dijkstra算法（其实Dijkstra是盲目搜索，但它是A*的基础）。
嗯，这里要注意：Dijkstra算法虽然被归类为盲目搜索，但它利用了“代价”信息，所以比BFS聪明得多。它知道每条路有多长，但不知道目标在哪边。
下面这张图，是我自己画的，能帮你快速理解这些算法的关系：
4.3 Dijkstra算法：最经典的“最短路径”算法
Dijkstra算法，由荷兰计算机科学家Edsger Dijkstra在1956年提出。说实话，这个算法已经快70岁了，但至今仍是很多路径规划系统的核心。为什么？因为它保证能找到最短路径，只要所有边的权重都是非负数。
我在项目中遇到过很多次，明明有更快的A*算法，但最后还是用Dijkstra。为什么？因为有些场景下，我们无法准确估计“启发函数”，A*可能会找到次优解。而Dijkstra虽然慢一点，但结果绝对可靠。
4.3.1 算法核心思想
Dijkstra算法的思路其实很简单，就一句话：每次从“未访问”的节点中，选一个距离起点最近的，然后更新它邻居的距离。
你可以想象成：你手里有一张地图，上面标着每个城市到起点的距离。一开始，除了起点是0，其他都是无穷大。然后你从起点出发，每到一个新城市，就看看能不能通过这个城市，让去其他城市的路更短。如果能，就更新地图上的距离。
这个过程，我称之为“贪心式的松弛”。贪心，是因为每次都选最近的节点；松弛，是因为不断尝试缩短距离。
4.3.2 算法步骤详解
我们一步步来。假设有一个图，节点编号0到N-1，起点是0。
- 初始化：
- 创建一个数组
dist[]，dist[0]=0，其他为无穷大。 - 创建一个数组
visited[]，全部为false。 - 创建一个优先队列
pq，初始放入 (0, 0) 表示 (距离, 节点)。
- 创建一个数组
- 循环： 当优先队列不为空时：
- 从
pq中取出距离最小的节点u。 - 如果
visited[u]为true，跳过（说明已经处理过）。 - 标记
visited[u] = true。 - 遍历
u的所有邻居v： - 计算新距离
newDist = dist[u] + weight(u, v)。 - 如果
newDist < dist[v]，更新dist[v] = newDist，并将(newDist, v)放入pq。
- 从
- 结束： 当所有节点都被访问过，或者优先队列为空时，
dist[]中存储的就是从起点到每个节点的最短距离。
4.3.3 代码实现（Python）
下面是我常用的一个实现。我习惯用 heapq 来实现优先队列，简洁高效。
import heapq
def dijkstra(graph, start):
"""
graph: 邻接表，graph[u] = [(v, weight), ...]
start: 起点节点
返回: dist 数组，dist[i] 表示从 start 到 i 的最短距离
"""
n = len(graph)
INF = float('inf')
dist = [INF] * n
dist[start] = 0
visited = [False] * n
# 优先队列，元素为 (距离, 节点)
pq = [(0, start)]
while pq:
d, u = heapq.heappop(pq)
# 如果已经访问过，跳过
if visited[u]:
continue
visited[u] = True
# 遍历邻居
for v, w in graph[u]:
if not visited[v]:
new_dist = dist[u] + w
if new_dist < dist[v]:
dist[v] = new_dist
heapq.heappush(pq, (new_dist, v))
return dist
# 示例用法
if __name__ == "__main__":
# 一个简单的图：0-1(4), 0-2(2), 1-2(1), 1-3(5), 2-3(8)
graph = [
[(1, 4), (2, 2)], # 节点0
[(0, 4), (2, 1), (3, 5)], # 节点1
[(0, 2), (1, 1), (3, 8)], # 节点2
[(1, 5), (2, 8)] # 节点3
]
distances = dijkstra(graph, 0)
print(f"从节点0到各节点的最短距离: {distances}")
# 输出: [0, 3, 2, 8]
# 解释: 0->2->1 是3，0->2 是2，0->2->1->3 是84.3.4 算法复杂度分析
这个实现的时间复杂度是 O((V+E)logV)。其中V是节点数，E是边数。为什么？因为每个节点最多入队一次（被visited标记后不再入队），每条边最多被检查一次。每次入队和出队操作都是O(logV)。
空间复杂度是 O(V+E)，主要存储邻接表和dist数组。
| 指标 | 值 | 说明 |
|---|---|---|
| 时间复杂度 | O((V+E)logV) | 使用优先队列实现 |
| 空间复杂度 | O(V+E) | 邻接表 + dist数组 |
| 适用场景 | 非负权图 | 不能有负权边 |
| 保证最优解 | 是 | 只要权重非负 |
4.4 从Dijkstra到实际应用
你可能会问：Dijkstra算法在机器人上怎么用？我举个例子。
几年前，我做AGV（自动导引车）的调度系统。工厂地面画了很多二维码，每个二维码就是一个节点。AGV从一个二维码移动到另一个二维码，需要时间。这个时间就是边的权重。
我们直接用Dijkstra计算最短时间路径。但有个问题：地图很大，有上千个节点。每次重新规划路径，Dijkstra都要跑一遍，太慢了。
后来我们做了优化：分层Dijkstra。先把地图分成几个区域，区域内部用Dijkstra，区域之间用另一层Dijkstra。这样，每次规划只需要在局部区域跑Dijkstra，速度提升了好几倍。
4.5 本章小结
这一章，我们聊了图搜索算法的基础，重点讲了Dijkstra算法。说白了，Dijkstra就是“每次选最近的，然后更新邻居”。它简单、可靠，是很多高级路径规划算法的基石。
我个人建议，如果你刚开始学路径规划，先把Dijkstra吃透。把代码写熟，把原理搞懂。后面学A*、D*、RRT等算法时，你会发现它们都有Dijkstra的影子。
嗯，最后提醒一句：永远不要在生产环境中用没有经过测试的路径规划算法。我曾经因为一个边界条件没处理好，导致AGV在工厂里原地打转，被老板骂了一下午。从那以后，我写路径规划代码，一定会先写单元测试，把各种边界情况都覆盖到。
可可精品资料库