机器人导航路径规划算法入门（6）Dijkstra（迪杰斯特拉）算法深入解析

文章目录

- 一、核心思想
- 二、为什么要求边权不能为负数？
- [三、两种实现方式 & 复杂度对比](#三、两种实现方式 & 复杂度对比)
- - [1. 朴素版（数组找最小距离）](#1. 朴素版（数组找最小距离）)
  - [2. 优先队列优化版（最小堆，工程最常用）](#2. 优先队列优化版（最小堆，工程最常用）)
- [四、Dijkstra vs A* vs D*Lite（机器人场景对比）](#四、Dijkstra vs A* vs D*Lite（机器人场景对比）)
- [五、C++ 最简优先队列实现（邻接表版本，带前驱数组+路径回溯）](#五、C++ 最简优先队列实现（邻接表版本，带前驱数组+路径回溯）)
- 六、四方向栅格地图版本
- 七、常见坑点（工程踩坑）
- 八、视频演示搜索过程

我们前面介绍了BFS宽度优先算法，本质是按照距离逐层遍历，可以用来解决无权图最短路径查找，但是在有权图里，逐层查找，找到的路径就未必是最短路径了，因为可能会有其他比较绕的路径，但是累加起来的边距离更小。举一个通俗的例子，我到重庆比较近，假设重庆没有飞机场，只能坐高铁花费5小时。另外一个方案就是，我虽然离成都远，但是我可以坐飞机到，只要1小时，然后再从成都动车1小时到重庆。我不能说我从起点出发，看到直连重庆，就认为是最短路径，我还得继续看看离我稍远的邻居成都，通过成都走到重庆的代价是不是更短，这意味着，如果我有n个邻居，我到n个邻居的距离都得算下，然后都得进入候选路径里，因为搞不好，最短路径就是从某个邻居抵达。从这个n个邻居里选出离起点最近的u，此刻我们能下的结论是，从起点出发到u的路径，直连肯定是最短的，因为它本身就是最小了，再从起点的其他邻居走到u就更远了。用数学论证如下：

dist(n)表示起点s到邻居n的直接距离（不通过其他节点绕到）。

dist(u) =min(dist(n)) < dist(n)。

从其他节点绕到u的距离dist_around(u) = dist(n1) + dist(n2) + ... 必定是大于dist(u)的。所以从起点抵达u的最短路径已确定。

如何继续确定其他节点的最短路径呢。也是套用一样的方法。

假设此刻候选池里已经有(n1,d1) ( n2,d2) (n3,d3)...，这些都是s的邻居。

其中dx表示他们到起点s的距离（还不能确定是最短，只是当下走一步直接得到的）。

查看u的邻居，起点s通过u走到u的邻居的距离都可以算出来，也放入候选池。

由于u的邻居可能已经在候选池里，所以放入候选池后，候选池可能长这样：

(n1,d1) (n2,d2) (n3,d3) (n1,d1'') (n2,d2'') (n4,d4)

由于u是s最近的邻居，通过s的其他邻居走到剩余未处理邻居的路径必定是比从u走过来大的，所以候选池里最短的路径就是该点的最短路径，这样我们又确定了起点到某个邻居最短距离，接下来又是把该邻居的邻居放入候选池，如此往复，直到抵达目标。

迪杰斯特拉（Dijkstra）：单源最短路径算法 ，求解从一个起点到图中其余所有节点的最短距离，仅适用于边权 ≥ 0 的图。

机器人路径规划（割草机、AGV）里最常用的基础算法，A*、D*Lite 都是在它之上做启发式改进。

一、核心思想

维护两组顶点：
- 已确定集合 S：起点到该点的最短路径已经找到，不再更新
- 未确定集合 U：最短距离只是临时估计值，还可能被更小的值更新
新增：前驱数组 parent\[\] ，用来记录路径信息。parent[v] = u 含义：到达v的最短路径上，v的前一个节点是u。初始全部置为 -1（无前置节点），起点 parent $start$ = -1。
初始：起点距离=0，其余所有节点距离 = ∞（无穷大）
循环：
1. 从 U 选出当前距离最小 的节点 u
2. 将 u 移入 S（它的最短路径敲定）
3. 遍历 u 的所有邻接节点 v，做松弛操作 relax
  
  如果 dist[v] > dist[u] + weight(u→v)
  
  则更新：dist[v] = dist[u] + weight(u→v)
  
  含义：经过 u 中转到 v，比原来直接到 v 更近，就更新 v 的最短距离。
  
  ✅ 松弛成功时同步更新 parent[v] = u：代表v的最短路径前驱更新为u。
直到 U 为空，或者目标点进入S（提前终止）
路径回溯：算法结束后，从终点反向沿着 parent 追溯直到起点，再反转序列得到完整路径。

二、为什么要求边权不能为负数？

如果存在负权边：

即使节点u被选进S（标记最短），后续可能出现一条负权路径，重新得到一条更短到u的路径。

Dijkstra 一旦把点加入S就不再更新，负权会破坏这个不变量。

负权图要用 Bellman-Ford / SPFA；有负权环不能求最短路径。

三、两种实现方式 & 复杂度对比

设顶点数 V V V，边数 E E E

1. 朴素版（数组找最小距离）

每次遍历全部V个点找最小值

时间复杂度：$O(V^2)
✅ 适合稠密图（节点不多，边很多，栅格地图小场景）

伪代码：

复制代码

dist[] ← INF
parent[] ← -1
dist[start] = 0
visited[] ← false

for i from 1 to V:
    u = 未访问节点中 dist 最小的点
    visited[u] = true
    for each v ∈ adj[u]:
        if dist[v] > dist[u] + w(u,v):
            dist[v] = dist[u] + w(u,v)
            parent[v] = u      // 更新前驱节点

2. 优先队列优化版（最小堆，工程最常用）

用小根堆保存 (临时距离, 节点)，省去每次全图扫描。

堆里会残留旧的、更大距离的节点记录，取出时判断：如果堆中记录距离 > dist $u$ ，直接跳过。

时间复杂度：$O(E \log V)
✅ 适合稀疏图，机器人栅格地图基本都用这个版本

伪代码：

复制代码

dist[] ← INF
parent[] ← -1
dist[start] = 0
min_heap ← [(0, start)]

while heap not empty:
    (d, u) = heappop(min_heap)
    if d > dist[u]: continue  // 旧的无效记录，跳过
    for each v, w in adj[u]:
        if dist[v] > d + w:
            dist[v] = d + w
            parent[v] = u      // 更新前驱节点
            heappush(min_heap, (dist[v], v))

路径回溯伪代码：
复制代码
path = []
cur = goal
while cur != -1:
path.push_back(cur)
cur = parent[cur]
reverse(path.begin(), path.end())
path 即为起点到终点完整路径；若 dist $goal$ ==INF，代表终点不可达。

四、Dijkstra vs A* vs D*Lite（机器人场景对比）

算法	特点	适用场景
Dijkstra	无启发，保证全局最优；搜索范围大；parent数组回溯路径	静态小地图，验证基准路径
A*	正向启发，最优；静态地图；同样用parent回溯	环境不变，预先规划路径
D* Lite	反向搜索，动态重规划；维护代价图，不使用简单parent数组	机器人运动中发现新障碍物（割草机实时避障）

Dijkstra：代价函数 (f(n)=g(n))，只看从起点走到当前点的真实代价，保证全局最优，搜索范围大。
A*：(f(n)=g(n)+h(n))，增加启发函数预估到终点距离，有方向搜索，更快；启发函数满足可采纳性时，同样保证最优。

Dijkstra 本质就是启发值 h=0 的 A * 算法。

五、C++ 最简优先队列实现（邻接表版本，带前驱数组+路径回溯）

cpp 复制代码

pair<vector<int>, vector<int>> dijkstra(int n, vector<vector<PII>>& adj, int start)
{
    vector<int> dist(n, INT_MAX); // dist[i]起点到i最短距离，初始无穷大
    vector<int> parent(n, -1);    // parent[i] = 到达i的上一个节点；-1表示无先驱（起点）
    dist[start] = 0;              // 起点到自己距离=0
    // 小根堆：pair(距离d,节点u)，greater 使得堆顶是最小d
    priority_queue<PII, vector<PII>, greater<PII>> heap;
    heap.emplace(0, start);       // 起点入堆

    while (!heap.empty())
    {
        auto [d, u] = heap.top(); // 取出堆顶：当前记录的距离d，节点u
        heap.pop();

        // ==========【核心】替代visited数组的判断 ==========
        if (d > dist[u]) continue;
        /*
        含义：堆里面存了旧的、过期的记录！
        比如：u之前已经找到更短距离dist[u]=2，堆里还留着一条旧记录(d=10,u)
        现在把这条旧记录弹出来一看：d > dist[u]，直接跳过，不处理。
        👉 这就是为什么这个版本**不需要visited数组**。
        */

        // 遍历u所有邻边 u -> v，权重w
        for (auto [v, w] : adj[u])
        {
            // 松弛操作：如果走u中转去v更近
            if (dist[v] > d + w)
            {
                dist[v] = d + w;       // 更新v最短距离
                parent[v] = u;         // 记录：到v的上一站是u
                heap.emplace(dist[v], v); // 把新的更优状态压入堆
            }
        }
    }
    return {dist, parent};
}

// 路径回溯函数：从终点根据parent数组还原路径
vector<int> reconstruct_path(int goal, vector<int>& parent) {
    vector<int> path;
    int cur = goal;
    while(cur != -1)
    {
        path.push_back(cur);
        cur = parent[cur];
    }
    reverse(path.begin(), path.end());
    return path;
}

int main() {
    int n = 5;
    vector<vector<PII>> adj(n);
    adj[0].emplace_back(1, 2);
    adj[0].emplace_back(2, 4);
    adj[1].emplace_back(2, 1);
    adj[1].emplace_back(3, 7);
    adj[2].emplace_back(4, 3);
    adj[3].emplace_back(4, 1);

    auto [dist, parent] = dijkstra(n, adj, 0);
    vector<int> path = reconstruct_path(4, parent);

    cout << "Path: ";
    for(auto node : path) cout << node << " ";
    return 0;
}

六、四方向栅格地图版本