（4-3） 轨迹规划算法和优化：Dijkstra算法

4.3  Dijkstra算法

Dijkstra算法是一种用于找到图中单源最短路径的贪婪算法，其基本思想是从起始节点开始，逐步扩展离起始节点最近的节点，直到到达目标节点或者遍历完所有节点。

4.3.1  Dijkstra算法介绍

Dijkstra算法是一种用于求解图中单源最短路径的贪婪算法，该算法以荷兰计算机科学家 Edsger Dijkstra 的名字命名。Dijkstra算法是一种典型的单源最短路径算法，它的时间复杂度取决于具体的实现方式，一般在稠密图上可能表现较差，但在稀疏图上效果较好。

1. 基本概念

1. 图：Dijkstra算法适用于有向或无向图，图中的节点表示路径上的位置，边表示连接这些位置的路径，每条边上有一个非负权重。
2. 节点：图中的每个点称为节点（或顶点），代表路径的一个位置。
3. 边：图中的每条连接两个节点的线称为边，边上带有权重，表示从一个节点到另一个节点的代价。
4. 距离：距离是指从一个节点到另一个节点的路径上边的权重之和，用于衡量路径的代价。
5. 单源最短路径：Dijkstra算法解决的问题是从图中的一个起始节点到其他所有节点的最短路径。

2. 实现步骤

（1）初始化：将起始节点的距离设置为0，其他节点的距离设置为无穷大。

（2）贪婪选择：选择当前距离最短的节点，将其标记为已访问。

（3）更新距离：对于所有与当前节点相邻且未被访问的节点，更新它们的距离，如果通过当前节点到达它们的路径比已知路径短。

（4）循环：重复执行以上步骤，直到所有节点都被访问。

3. 输出结果

Dijkstra算法输出的结果包括从起始节点到每个其他节点的最短路径距离和路径信息，这些信息可以用于导航、网络路由和其他需要找到最短路径的应用中。

4.3.2  Dijkstra算法实战

假设我们有一个简单的图表示城市之间的道路，我们想要找到从起点城市到目标城市的最短路径，此时可以考虑使用Dijkstra算法实现。在下面的这个例子中有四个城市（节点），它们之间有一些道路（边），每条道路都有一个权重表示距离。通过Dijkstra算法，我们可以找到从起点城市到目标城市的最短路径。

实例4-2：使用Dijkstra算法找出到目标城市的最短路径（源码路径：codes\4\gui\dij.py）

实例文件dij.py的具体实现代码如下所示。

import heapq

class Node:
    def __init__(self, name):
        self.name = name
        self.distance = float('inf')
        self.neighbors = []
        self.parent = None

    def add_neighbor(self, neighbor, weight):
        self.neighbors.append((neighbor, weight))

    def __lt__(self, other):
        return self.distance < other.distance


def dijkstra(start, goal):
    start.distance = 0
    open_list = [(0, start)]

    while open_list:
        current_distance, current_node = heapq.heappop(open_list)

        if current_node == goal:
            path = []
            while current_node:
                path.append(current_node.name)
                current_node = current_node.parent
            return path[::-1]  # 反转路径，使其从起点到终点

        for neighbor, weight in current_node.neighbors:
            distance = current_node.distance + weight

            if distance < neighbor.distance:
                neighbor.distance = distance
                neighbor.parent = current_node
                heapq.heappush(open_list, (distance, neighbor))

    return None  # 无法到达目标节点

# 创建节点
city_a = Node("City A")
city_b = Node("City B")
city_c = Node("City C")
city_d = Node("City D")

# 连接节点并指定权重
city_a.add_neighbor(city_b, 1)
city_a.add_neighbor(city_c, 4)
city_b.add_neighbor(city_c, 2)
city_b.add_neighbor(city_d, 5)
city_c.add_neighbor(city_d, 1)

# 设置起点和目标
start_city = city_a
goal_city = city_d

# 执行Dijkstra算法
path = dijkstra(start_city, goal_city)

if path:
    print("最短路径:", path)
else:
    print("无法找到路径")

上述代码的实现流程如下所示：

1. 首先，定义类Node表示图中的节点，每个节点有一个名称、距离属性、邻居列表和一个指向上一个节点的指针。
2. 然后，定义dijkstra 函数，该函数使用 Dijkstra 算法来找到从起点到目标的最短路径。在此函数中，使用优先队列（heapq）来按距离从小到大的顺序处理节点。
3. 接着，初始化起点的距离为0，将起点加入优先队列。在循环中，我们弹出队列中距离最小的节点，然后更新它的邻居节点的距离，将它们加入队列。这个过程一直持续，直到目标节点被弹出队列。
4. 最后，从目标节点开始，沿着每个节点的指针回溯，构建最短路径。最终，我们得到了从起点到目标的最短路径。执行后会输出：

最短路径: ['City A', 'City B', 'City C', 'City D']