4. Dijkstra算法：算法原理、优先队列优化、在栅格地图上的仿真实现
Dijkstra算法，说实话，是我个人在路径规划领域接触的第一个真正意义上的「全局搜索」算法。记得刚入行那会儿，我还在用BFS做简单寻路，结果在带权重的栅格地图上碰了一鼻子灰——明明看起来很近的路，走起来却绕了一大圈。后来导师丢给我一句话：「去把Dijkstra搞明白，你就知道什么叫最优了。」
好，咱们今天就把这个算法彻底聊透。我会从原理讲起，再聊工程上的优化，最后带你看一个栅格地图上的仿真实现。
4.1 算法核心思想：贪心 + 松弛
Dijkstra算法的本质，说白了就是一句话：从起点开始，每次选一个当前已知「距离最近」且没处理过的节点，然后尝试用它去更新邻居的距离。
这个过程，专业术语叫「松弛操作」。你想想看，就像你手里有一张地图，你每到一个新路口，就看看能不能通过这个路口让去其他地方的路变得更短。能，就更新；不能，就拉倒。
核心三要素：
- dist[] 数组：记录从起点到每个节点的当前最短距离
- visited[] 数组：标记哪些节点已经找到了最短路径
- 优先队列：每次快速取出「当前距离最小」的节点
嗯，这里要注意：Dijkstra要求所有边的权重必须是非负的。为什么？因为一旦有负权边，你之前认为「已经最短」的路径，可能被后面一条负权边给「反超」了。我在项目中遇到过这种坑——用Dijkstra跑一个带负权回路的图，结果程序死循环了。后来改用Bellman-Ford才解决。
4.2 算法步骤详解
咱们一步步来，我习惯用伪代码先理清逻辑：
function Dijkstra(Graph, source):
dist[source] = 0
for each vertex v in Graph:
if v != source:
dist[v] = INF
add v to priority queue Q with priority dist[v]
while Q is not empty:
u = Q.extract_min() // 取出当前距离最小的节点
for each neighbor v of u:
alt = dist[u] + weight(u, v)
if alt < dist[v]:
dist[v] = alt
Q.decrease_key(v, alt) // 更新优先队列中的优先级
return dist[]你看，逻辑其实很简洁。但工程实现上，有几个细节值得聊一聊。
4.3 优先队列优化：从 O(V²) 到 O((V+E)logV)
如果不做优化，每次找「当前距离最小节点」都要遍历整个dist数组，复杂度是O(V²)。这在节点数一多（比如栅格地图1000×1000，那就是100万个节点）时，根本跑不动。
优化的思路很直接：用最小堆（优先队列）来维护待处理节点。每次取堆顶元素，复杂度降为O(logV)。
我个人习惯用C++的std::priority_queue配合pair<int, int>来实现。但要注意一个坑：标准库的优先队列不支持decrease_key操作。怎么办？
我的做法是：直接push新的pair进去，旧的不管它。取出时判断一下，如果当前节点的dist值已经比队列里记录的大，就跳过。这叫「惰性删除」，工程上非常实用。
避坑指南：我曾经在嵌入式平台上用Dijkstra，堆的实现用了STL的priority_queue，结果发现内存占用比预期大很多。后来改用std::set模拟堆，既支持decrease_key，又节省了内存。具体场景要具体分析。
4.4 栅格地图上的仿真实现
好，理论聊完了，咱们上点干货。下面是一个在栅格地图上实现Dijkstra的Python示例。地图用二维数组表示，0是空地，1是障碍物。
import heapq
import numpy as np
def dijkstra_grid(grid, start, goal):
rows, cols = grid.shape
# 4方向移动：上、下、左、右
dirs = [(-1,0), (1,0), (0,-1), (0,1)]
# 初始化距离矩阵
dist = np.full((rows, cols), np.inf)
dist[start] = 0
# 优先队列：(距离, 行, 列)
pq = [(0, start[0], start[1])]
# 记录路径
parent = {}
while pq:
d, r, c = heapq.heappop(pq)
# 惰性删除：如果当前距离比记录的大，跳过
if d > dist[r, c]:
continue
# 到达终点
if (r, c) == goal:
break
# 遍历邻居
for dr, dc in dirs:
nr, nc = r + dr, c + dc
# 边界检查 & 障碍物检查
if 0 <= nr < rows and 0 <= nc < cols and grid[nr, nc] == 0:
new_dist = d + 1 # 每步代价为1
if new_dist < dist[nr, nc]:
dist[nr, nc] = new_dist
parent[(nr, nc)] = (r, c)
heapq.heappush(pq, (new_dist, nr, nc))
# 回溯路径
path = []
if (goal) in parent or goal == start:
cur = goal
while cur != start:
path.append(cur)
cur = parent[cur]
path.append(start)
path.reverse()
return path, dist注意：上面这个实现假设每步代价为1。如果你的栅格地图中不同区域移动代价不同（比如草地、沙地），只需要把new_dist = d + 1改成new_dist = d + cost[nr][nc]即可。我在做农业机器人项目时，就遇到过不同地形代价不同的问题，Dijkstra处理起来非常自然。
4.5 知识体系与核心逻辑
下面这张图，是我自己总结的Dijkstra算法知识体系。你看一眼，就能把整个章节串起来。
4.6 几个值得注意的工程细节
最后，分享几个我在实际项目中踩过的坑：
- 地图太大怎么办？ 如果栅格地图超过1000×1000，Dijkstra的搜索空间会非常大。我建议先做地图降采样，或者改用A*算法（带启发式）。
- 路径不平滑怎么办？ Dijkstra出来的路径是栅格级的，会有锯齿。我习惯在后面加一个路径平滑步骤，比如用B样条曲线拟合。
- 动态障碍物怎么处理？ Dijkstra是静态算法。如果环境变化，需要重新规划。我在项目中用过D* Lite来做动态重规划，效果不错。
一个小技巧：如果你用Python做仿真，可以用heapq库实现优先队列。它比你自己手写堆要稳定得多。我早期犯过傻，自己写了个二叉堆，结果在边界条件上出了bug，调试了一整天。
好了，Dijkstra算法就聊到这儿。它的思想其实很简单——贪心地扩展最短路径边界。但正是这个简单的思想，构成了后续A*、D*等更高级算法的基础。你把它吃透了，后面的路就好走了。
公众号:蓝海资料掘金营，微信deep3321