第四章：经典路径规划算法（上）——Dijkstra与A*的实战心法
各位同学，今天我们来聊聊路径规划里最经典的两个算法：Dijkstra和A*。说实话，这两个算法我在本科时就学过，但真正理解它们的精髓，还是在做了几个机器人项目之后。你想想看，一个扫地机器人要绕过你家客厅的茶几，一个机械臂要抓取流水线上的零件——背后都离不开这些算法。
4.1 从图论说起：为什么路径规划需要图？
在讲算法之前，我们先统一一下语言。路径规划问题，说白了就是：给定起点和终点，找一条代价最小的路径。这里的「代价」可以是距离、时间、能耗，甚至是你自定义的任意指标。
我们通常用图（Graph）来表示环境。图由节点（Node）和边（Edge）组成。节点代表位置，边代表连通关系，边上可以带权重（Weight）表示代价。
核心概念速览
- 节点：机器人可以停留的位置
- 边：节点之间的可行路径
- 权重：通过这条边的代价（距离/时间/能量）
- 路径：从起点到终点的一系列节点
我在做仓储机器人项目时，一开始直接把仓库地图转成栅格图，每个格子就是一个节点。结果发现路径规划慢得离谱——因为节点太多了。后来我改用拓扑图，只保留货架间的关键通道，速度提升了10倍不止。嗯，这里要记住：图的构建方式直接影响算法效率。
4.2 Dijkstra算法：最朴素的「地毯式搜索」
Dijkstra算法是荷兰计算机科学家Edsger Dijkstra在1956年提出的。它的思路非常直观：从起点开始，一层层向外扩展，直到找到终点。就像你在黑暗中用手电筒照路，一步步照亮周围的环境。
4.2.1 算法核心思想
Dijkstra维护两个关键数据结构：
- dist[]：记录从起点到每个节点的当前最短距离
- visited[]：标记节点是否已确定最短路径
算法流程其实就三步：
- 初始化：起点dist=0，其他节点dist=∞
- 每次从未访问节点中选dist最小的，标记为已访问
- 更新该节点所有邻居的dist值
重复步骤2和3，直到所有节点都被访问，或者找到终点。
我的小经验：Dijkstra算法保证找到最短路径，但代价是「慢」。因为它要探索所有可能的方向，像个无头苍蝇一样到处乱撞。在大型地图上，这效率简直让人抓狂。
4.2.2 代码实现（Python）
import heapq
def dijkstra(graph, start, end):
# graph: 邻接表 {node: [(neighbor, weight), ...]}
# 初始化
dist = {node: float('inf') for node in graph}
dist[start] = 0
prev = {node: None for node in graph} # 记录前驱节点
# 优先队列：(当前距离, 节点)
pq = [(0, start)]
visited = set()
while pq:
current_dist, current = heapq.heappop(pq)
if current in visited:
continue
visited.add(current)
# 找到终点，提前退出
if current == end:
break
# 更新邻居
for neighbor, weight in graph[current]:
if neighbor in visited:
continue
new_dist = current_dist + weight
if new_dist < dist[neighbor]:
dist[neighbor] = new_dist
prev[neighbor] = current
heapq.heappush(pq, (new_dist, neighbor))
# 回溯路径
path = []
node = end
while node is not None:
path.append(node)
node = prev[node]
path.reverse()
return path, dist[end]避坑指南：我曾经在项目中直接用列表来选最小dist节点，结果地图一大（几千个节点），每次选最小都要遍历整个列表，复杂度O(n²)。后来改用优先队列（heapq），复杂度降到O((V+E)logV)，速度提升肉眼可见。记住：Dijkstra一定要用优先队列实现。
4.3 A*算法：给搜索装上「指南针」
Dijkstra的问题在于它太「盲目」了。它不知道终点在哪边，只能向所有方向均匀扩展。A*算法则聪明得多——它用启发式函数（Heuristic Function）给搜索指明了方向。
4.3.1 启发式函数：A*的灵魂
A*的核心公式很简单：
f(n) = g(n) + h(n)
- g(n)：从起点到节点n的实际代价（和Dijkstra一样）
- h(n)：从节点n到终点的估计代价（启发式函数）
- f(n)：综合评估值，A*优先扩展f值最小的节点
你想想看，h(n)就像个指南针，告诉算法「往这个方向走更有可能到达终点」。这样算法就不会在错误的方向上浪费太多时间。
常用的启发式函数
| 地图类型 | 启发式函数 | 说明 |
|---|---|---|
| 网格地图（四方向） | 曼哈顿距离 | |x1-x2| + |y1-y2| |
| 网格地图（八方向） | 切比雪夫距离 | max(|x1-x2|, |y1-y2|) |
| 任意地图 | 欧几里得距离 | √((x1-x2)² + (y1-y2)²) |
4.3.2 A*的「可采纳性」
这里有个关键点：h(n)必须小于等于实际代价，A*才能保证找到最短路径。这叫「可采纳性」（Admissibility）。
为什么会这样？因为如果h(n)高估了距离，算法可能会错过真正的最短路径。举个例子，你从北京去上海，h(n)估计是1000公里，实际只有1200公里，那没问题。但如果h(n)估计是2000公里，算法可能就绕远路了。
我的经验：在机器人项目中，我通常用欧几里得距离作为h(n)。它虽然简单，但满足可采纳性，而且计算快。有一次我尝试用更复杂的启发式函数，结果计算开销反而拖慢了整体速度。有时候，简单就是最好的。
4.3.3 代码实现（Python）
import heapq
import math
def heuristic(a, b):
# 欧几里得距离
return math.sqrt((a[0]-b[0])**2 + (a[1]-b[1])**2)
def a_star(graph, start, end, positions):
# positions: 节点坐标字典 {node: (x, y)}
open_set = [(0, start)]
g_score = {node: float('inf') for node in graph}
g_score[start] = 0
f_score = {node: float('inf') for node in graph}
f_score[start] = heuristic(positions[start], positions[end])
prev = {node: None for node in graph}
while open_set:
current_f, current = heapq.heappop(open_set)
if current == end:
# 回溯路径
path = []
node = end
while node is not None:
path.append(node)
node = prev[node]
path.reverse()
return path, g_score[end]
for neighbor, weight in graph[current]:
tentative_g = g_score[current] + weight
if tentative_g < g_score[neighbor]:
prev[neighbor] = current
g_score[neighbor] = tentative_g
f_score[neighbor] = tentative_g + heuristic(positions[neighbor], positions[end])
heapq.heappush(open_set, (f_score[neighbor], neighbor))
return None, float('inf') # 无路径4.4 Dijkstra vs A*：什么时候用哪个？
很多同学会问：「既然A*更快，那为什么还要学Dijkstra？」
这个问题问得好。我个人的经验是：
- Dijkstra：适合所有边权重都为正的场景，而且你必须找到最短路径。比如在自动驾驶中，安全第一，多花点时间计算也值得。
- A*：适合有明确目标方向的场景，而且你对实时性有要求。比如游戏中的NPC寻路，玩家可不想等几秒钟才看到角色动起来。
对比总结
| 特性 | Dijkstra | A* |
|---|---|---|
| 保证最短路径 | ✅ 是 | ✅ 是（h(n)可采纳时） |
| 搜索方向 | 均匀向外 | 朝向目标 |
| 时间复杂度 | O((V+E)logV) | O((V+E)logV)，但实际更快 |
| 适用场景 | 小地图、需要精确解 | 大地图、实时性要求高 |
4.5 实战中的坑与技巧
最后分享几个我在项目中踩过的坑：
- 地图太大怎么办？ 别直接用原始分辨率。先做地图压缩，或者用分层规划——高层用粗粒度地图找大致方向，低层用细粒度地图做局部优化。
- 动态障碍物怎么处理？ 每次重新规划太慢。我一般用D* Lite算法（后面会讲），它能在局部修改路径，不用全盘重算。
- 路径不平滑怎么办？ A*出来的路径都是折线。机器人走起来会一顿一顿的。我通常会在路径规划后加一个平滑步骤，比如用B样条曲线拟合。
曾经踩过的坑：有一次我在A*中用了曼哈顿距离作为启发式函数，但地图是八方向移动的。结果h(n)严重低估了实际距离，算法退化成Dijkstra，慢得不行。后来改成切比雪夫距离，速度立刻上来了。记住：启发式函数一定要和移动方式匹配。
4.6 本章小结
好了，这一章我们讲了Dijkstra和A*两个经典算法。说白了，Dijkstra是「老实人」，一步一个脚印；A*是「聪明人」，懂得走捷径。在实际项目中，我建议你先用A*试试，如果发现路径不是最优的，再检查一下启发式函数是否可采纳。
下一章我们会继续讲更高级的路径规划算法，比如RRT和D*。但在此之前，我建议你把这两个算法的代码亲手跑一遍。只有亲手写过，才能真正理解它们的区别。