第1章：全局路径规划：Dijkstra算法原理与实现，从图搜索到最短路径的推导

1.1 为什么我们需要全局路径规划？

做机器人这么多年，我经常被问到：「你让机器人动起来不就行了吗？为什么还要搞什么全局规划？」

嗯，这个问题其实挺有意思的。你想想看，如果让一个机器人在房间里乱撞，它也能到达目的地——只要时间够长。但实际项目中，没人能接受这种「碰运气」的方式。

全局路径规划，说白了就是给机器人画一张「地图导航图」。它告诉机器人：从A点到B点，哪条路最省时间、最安全。我最早做AGV（自动导引车）项目时，就吃过亏——只做了局部避障，结果机器人经常在仓库里绕远路，被客户吐槽「这机器人是不是路痴」。

所以，全局规划是机器人的「大脑」，局部规划是「小脑」。两者缺一不可。

1.2 Dijkstra算法：图搜索的基石

说到全局路径规划，Dijkstra算法是绕不开的。它是Edsger Dijkstra在1956年提出的，至今仍是很多路径规划算法的基础。

我个人习惯把Dijkstra算法理解成「水波扩散」——就像往平静的水面扔一颗石子，波纹会一圈一圈向外扩散，最先到达某个点的波纹，就是最短路径。

1.3 算法原理：一步步拆解

咱们先别急着看代码。我带你手推一遍Dijkstra的逻辑，这样你才能真正理解它。

1.3.1 基本概念

在讲算法之前，先明确几个术语：

• 节点（Node）：地图中的位置点，比如路口、房间中心
• 边（Edge）：连接两个节点的路径，有长度（权重）
• 距离（Distance）：从起点到某个节点的当前最短距离
• 前驱节点（Predecessor）：记录最短路径中，到达该节点的上一个节点

1.3.2 算法步骤

我习惯用「三步走」来记忆Dijkstra：

1. 初始化：起点距离设为0，其他节点距离设为无穷大。所有节点标记为「未访问」。
2. 选择：从未访问节点中，选出距离最小的节点，标记为「已访问」。
3. 松弛：检查该节点的所有邻居，如果通过当前节点能获得更短距离，就更新邻居的距离和前驱节点。

重复步骤2和3，直到所有节点都被访问，或者到达目标点。

1.4 图解Dijkstra：一个简单例子

光说理论太枯燥。咱们看一个具体例子。

假设有这样一个图：

节点：A（起点）, B, C, D, E（终点）
边（权重）：
A-B: 4, A-C: 2
B-C: 1, B-D: 5
C-D: 8, C-E: 10
D-E: 2

咱们手动跑一遍Dijkstra：

步骤	当前节点	A距离	B距离	C距离	D距离	E距离	已访问集合
0	-	0	∞	∞	∞	∞	{}
1	A	0	4	2	∞	∞	{A}
2	C	0	3	2	10	12	{A, C}
3	B	0	3	2	8	12	{A, C, B}
4	D	0	3	2	8	10	{A, C, B, D}
5	E	0	3	2	8	10	{A, C, B, D, E}

最终最短路径：A → C → B → D → E，总距离10。

你可能会问：「为什么不是A → C → E？那条路是12啊。」没错，Dijkstra就是通过这种「贪心」策略，保证每次选的都是当前最优。

1.5 核心逻辑流程图

下面这张图是我自己画的，把Dijkstra的整个流程串起来了。你看完应该能有个整体印象。

1.6 C++实现：从理论到代码

好了，理论讲完了。咱们来看看代码怎么写。我习惯用C++写路径规划，因为ROS里用的就是C++。

#include <iostream>
#include <vector>
#include <queue>
#include <limits>
#include <algorithm>

using namespace std;

// 定义图的邻接表表示
// 每个元素是 pair<邻居节点, 边权重>
typedef pair<int, int> Edge;
typedef vector<vector<Edge>> Graph;

vector<int> dijkstra(const Graph& graph, int start, int goal) {
    int n = graph.size();
    
    // 距离数组，初始化为无穷大
    vector<int> dist(n, numeric_limits<int>::max());
    // 前驱节点数组，用于回溯路径
    vector<int> prev(n, -1);
    // 访问标记
    vector<bool> visited(n, false);
    
    // 优先队列：存储 (距离, 节点)
    // 小顶堆，距离小的优先
    priority_queue<Edge, vector<Edge>, greater<Edge>> pq;
    
    // 初始化起点
    dist[start] = 0;
    pq.push({0, start});
    
    while (!pq.empty()) {
        // 取出距离最小的节点
        int u = pq.top().second;
        int d = pq.top().first;
        pq.pop();
        
        // 如果已经访问过，跳过
        if (visited[u]) continue;
        visited[u] = true;
        
        // 如果到达目标点，提前结束
        if (u == goal) break;
        
        // 松弛操作：检查所有邻居
        for (const auto& edge : graph[u]) {
            int v = edge.first;
            int weight = edge.second;
            
            if (!visited[v] && dist[u] + weight < dist[v]) {
                dist[v] = dist[u] + weight;
                prev[v] = u;
                pq.push({dist[v], v});
            }
        }
    }
    
    // 回溯路径
    vector<int> path;
    if (dist[goal] == numeric_limits<int>::max()) {
        // 无法到达目标点
        return path;
    }
    
    for (int v = goal; v != -1; v = prev[v]) {
        path.push_back(v);
    }
    reverse(path.begin(), path.end());
    
    return path;
}

int main() {
    // 构建图：6个节点 (0-5)
    Graph graph(6);
    
    // 添加边 (无向图)
    graph[0].push_back({1, 4});
    graph[0].push_back({2, 2});
    graph[1].push_back({0, 4});
    graph[1].push_back({2, 1});
    graph[1].push_back({3, 5});
    graph[2].push_back({0, 2});
    graph[2].push_back({1, 1});
    graph[2].push_back({3, 8});
    graph[2].push_back({4, 10});
    graph[3].push_back({1, 5});
    graph[3].push_back({2, 8});
    graph[3].push_back({4, 2});
    graph[4].push_back({2, 10});
    graph[4].push_back({3, 2});
    
    // 从节点0到节点4的最短路径
    vector<int> path = dijkstra(graph, 0, 4);
    
    cout << "最短路径: ";
    for (int node : path) {
        cout << node << " ";
    }
    cout << endl;
    
    return 0;
}

1.7 我在项目中踩过的坑

讲到这里，我得分享几个实战中遇到的坑。这些经验可是花钱买来的。

1.8 Dijkstra的优缺点

没有完美的算法。Dijkstra也有它的局限性。

优点	缺点
保证找到最短路径（最优性）	搜索范围大，效率较低
实现简单，易于调试	需要知道全局地图信息
适用于静态环境	动态环境需要重新规划
理论基础扎实，扩展性强	无法处理负权边

1.9 什么时候用Dijkstra？

我个人总结了几条经验：

• 地图不大（< 1万节点）：直接用Dijkstra，简单可靠
• 需要最优路径：比如AGV搬运，多走一米都是浪费
• 静态环境：仓库、工厂等固定场景
• 作为其他算法的基础：比如A*算法就是在Dijkstra基础上加了启发式函数

如果地图很大，或者需要实时响应，我建议改用A*算法。下一章我们会详细讲A*，它比Dijkstra快得多。

1.10 小结

Dijkstra算法是全局路径规划的基石。它虽然简单，但「最短路径」这个保证，在很多场景下是刚需。

记住三个关键点：

• 贪心策略：每次选距离最小的节点
• 松弛操作：不断更新邻居的最短距离
• 非负权重：这是算法的前提条件

我在实际项目中，80%的路径规划需求用Dijkstra或A*就能解决。剩下的20%，才需要RRT、PRM这些更高级的算法。所以，把Dijkstra吃透，你就已经掌握了路径规划的半壁江山。

---

公众号:蓝海资料掘金营，微信deep3321