路径搜索算法——Dijkstra算法

路径搜索算法——Dijkstra算法

一、Dijkstra 算法简介

Dijkstra 算法是由荷兰计算机科学家 Edsger W. Dijkstra 于 1956 年提出的，用于单源最短路径问题——即从一个起点出发，寻找图中所有点的最短路径（常用于路径规划中）。

🚗 在自动驾驶中，Dijkstra 常用于静态全局路径规划，如在无障碍、无启发函数的高清地图上寻找最短通行路径。

二、算法核心思想

• 使用贪心策略；

• 每次从未访问节点中选择距离起点最近的一个节点进行扩展；

• 一旦某节点的最短路径被确定，就不会被更新。

  我们定义如下无向图（图中数字为边权）：

  

邻接表表示

graph = {
  A: [(B,1), (C,4)],
  B: [(A,1), (D,2), (E,5)],
  C: [(A,4), (F,1)],
  D: [(B,2), (E,1)],
  E: [(B,5), (D,1), (F,3)],
  F: [(C,1), (E,3)]
}

Dijkstra 算法执行过程（起点 A）

初始化：

队列：[(0, A)]

🔁 第 1 轮：处理 A，当前距离 0

邻居更新：

• B：0 + 1 = 1
• C：0 + 4 = 4

更新后：

队列：[(1, B), (4, C)]

🔁 第 2 轮：处理 B，当前距离 1

邻居更新：

• D：1 + 2 = 3
• E：1 + 5 = 6
• A：已访问

更新后：

队列：[(3, D), (4, C), (6, E)]

🔁 第 3 轮：处理 D，当前距离 3

邻居更新：

• E：3 + 1 = 4（比原来 6 小）→ 更新为 4

更新后：

队列：[(4, C), (4, E), (6, E)]

🔁 第 4 轮：处理 C，当前距离 4

邻居更新：

• F：4 + 1 = 5

更新后：

队列：[(4, E), (6, E), (5, F)]

🔁 第 5 轮：处理 E，当前距离 4

邻居：

• B、D：已访问
• F：4 + 3 = 7 > 当前 F=5 → 跳过

无更新。

队列：[(5, F), (6, E)]

🔁 第 6 轮：处理 F，当前距离 5

邻居：

• C、E：已访问

无更新。

✅ 最终结果（起点 A 出发的最短路径）

三、算法步骤（伪代码）

1. 初始化每个点的距离为 ∞，起点为 0
2. 创建优先队列，加入起点
3. 当队列非空：
    a. 取出距离最小的节点 u
    b. 遍历 u 的所有邻居 v：
        - 如果通过 u 到 v 的距离更短，则更新 v 的距离
        - 将 v 加入队列
4. 最终，所有点的最短路径就被记录

四、C++ 示例代码：Dijkstra 网格图最短路径

cpp复制编辑#include <iostream>
#include <vector>
#include <queue>
#include <unordered_map>
#include <utility>

using namespace std;

// 定义坐标类型
typedef pair<int, int> Point;

// 定义方向（上下左右）
const vector<Point> directions = {
    {-1, 0}, {1, 0}, {0, -1}, {0, 1}
};

// 判断点是否在地图内且为可通行（0）
bool isValid(int x, int y, const vector<vector<int>>& grid) {
    int rows = grid.size(), cols = grid[0].size();
    return x >= 0 && x < rows && y >= 0 && y < cols && grid[x][y] == 0;
}

// 哈希函数，用于 unordered_map<pair<int,int>, int>
struct pair_hash {
    size_t operator()(const Point& p) const {
        return hash<int>()(p.first) ^ (hash<int>()(p.second) << 1);
    }
};

// Dijkstra 主函数：返回从 start 到所有可达点的最短距离
unordered_map<Point, int, pair_hash>
dijkstra(const vector<vector<int>>& grid, Point start) {
    unordered_map<Point, int, pair_hash> dist;
    priority_queue<pair<int, Point>, vector<pair<int, Point>>, greater<>> pq;

    dist[start] = 0;
    pq.push({0, start});

    while (!pq.empty()) {
        auto [cost, point] = pq.top();
        pq.pop();

        for (auto [dx, dy] : directions) {
            int nx = point.first + dx;
            int ny = point.second + dy;
            Point neighbor = {nx, ny};

            if (!isValid(nx, ny, grid)) continue;

            int new_cost = cost + 1;  // 每步代价为 1
            if (!dist.count(neighbor) || new_cost < dist[neighbor]) {
                dist[neighbor] = new_cost;
                pq.push({new_cost, neighbor});
            }
        }
    }

    return dist;
}

int main() {
    // 地图示例：0 表示可通行，1 表示障碍
    vector<vector<int>> grid = {
        {0, 0, 0, 0},
        {1, 1, 0, 1},
        {0, 0, 0, 0},
        {0, 1, 1, 0},
        {0, 0, 0, 0}
    };

    Point start = {0, 0};  // 起点坐标

    // 执行 Dijkstra 算法
    auto result = dijkstra(grid, start);

    // 输出所有可达点及其最短距离
    cout << "从 (" << start.first << "," << start.second << ") 出发的最短距离：" << endl;
    for (const auto& [point, d] : result) {
        cout << "(" << point.first << "," << point.second << ") : " << d << endl;
    }

    return 0;
}

五、Python 简化实现（二维网格图）

python复制编辑import heapq

def dijkstra(grid, start):
    rows, cols = len(grid), len(grid[0])
    dist = {start: 0}
    visited = set()
    pq = [(0, start)]  # (distance, position)

    while pq:
        d, (x, y) = heapq.heappop(pq)
        if (x, y) in visited:
            continue
        visited.add((x, y))

        for dx, dy in [(-1,0), (1,0), (0,-1), (0,1)]:
            nx, ny = x + dx, y + dy
            if 0 <= nx < rows and 0 <= ny < cols and grid[nx][ny] == 0:
                new_dist = d + 1
                if (nx, ny) not in dist or new_dist < dist[(nx, ny)]:
                    dist[(nx, ny)] = new_dist
                    heapq.heappush(pq, (new_dist, (nx, ny)))

    return dist  # 返回从起点到所有可达点的最短距离

六、A* vs Dijkstra 对比

七、优缺点总结

✅ 优点：

• 简洁、稳定、易于实现；
• 不依赖启发函数，适用于所有加权图；
• 可求任意点对最短路径。

❌ 缺点：

• 搜索空间大（尤其是地图大时），效率低；
• 不考虑目标方向，盲目扩展。