Dijkstra算法寻找最优路径python代码，从一点出发到达目标点的最优路径并展示

该算法为使用Dijkstra算法寻找起点到终点的最优路径，同时设置了一系列障碍点，寻找的路径需避开障碍点。以淄博市某区域的栅格影像图为例，栅格像素值不同，代表的地物不同，根据地物设置每移动一步的代价加1。
Dijkstra算法是一种经典的求解静态环境中最短路径的算法，是计算从一点到其余各点的最短路径算法，解决的是有权图中最短路径问题]。Dijkstra算法将起始节点作为当前节点，计算当前节点与相邻节点的距离，将距离最近的节点作为新的当前节点，再次计算起始节点到当前节点再到相邻节点的距离，逐层向外扩展，直到到达目标点。
具体步骤如下：
①遍历当前节点的四个相邻节点（上、下、左、右）。
②对于每个相邻节点，检查它是否在栅格地图内，且不是障碍物点。
③如果满足条件，计算从当前节点到相邻节点的新路径长度。在Dijkstra算法中，每个边的权重默认为1，因此新路径长度为当前路径长度加1。
④检查新路径长度是否小于已知的最短路径长度。如果是，则更新该相邻节点的距离信息和父节点。
⑤将新节点及其新路径长度推入优先队列，以便在下一次迭代中继续选择最短路径。
⑥这样，Dijkstra算法通过不断选择距离最短的节点，逐步更新并探索到达目标节点的最短路径。这个过程会一直进行，直到找到目标节点或者遍历完所有可能的路径。

-- coding: utf-8 --

coding=utf-8

import heapq
import numpy as np
import time
from osgeo import gdal
import matplotlib.pyplot as plt
# 导入必要的库，包括 heapq 用于优先队列，numpy 用于处理数组
# heapq 模块提供了堆队列算法的实现，用于优化 Dijkstra 算法中的优先队列操作
# numpy 用于处理多维数组，这里用于表示栅格地图

读取栅格影像

def read_raster(file_path):
    dataset = gdal.Open(file_path)
    if dataset is None:
        raise Exception("Error opening the raster file")
    band = dataset.GetRasterBand(1)
    raster_array = band.ReadAsArray()
    #这行代码调用ReadAsArray方法，该方法将栅格波段中的像素值读取为NumPy数组。
    # 这个数组raster_array包含了图像中每个像素的值，形状与影像的大小相匹配。
    return raster_array

从栅格中添加障碍物

def add_obstacles_from_raster(obstacle_value, obstacle_raster):
    obstacle_points = np.argwhere(np.isin(obstacle_raster, obstacle_value))
    return [tuple(point) for point in obstacle_points]

寻找最优路径的dijkstra算法

def dijkstra(grid_size, start, goal,raster_array):
    # Dijkstra 算法函数，用于寻找栅格地图中从起点到目标点的最短路径
    rows, cols = grid_size
    # 获取栅格地图的行数和列数
    directions = [(-1, 0), (1, 0), (0, -1), (0, 1)]
    # 定义四个方向，分别表示上、下、左、右
#priority_queue` 已经可以作为一个优先队列使用，并且可以使用 `heapq.heappop(priority_queue)` 高效地弹出具有最小距离的节点,[(1, (27, 19))]
    priority_queue = [(0, start)]
    heapq.heapify(priority_queue)
    # 初始化一个字典distance，字典用于记录从起始节点到其他节点的最短距离信息以及对应的父节点,{(28, 19): (0, None), (27, 19): (1, (28, 19))}
    distance = {start: (0, None)}
    while priority_queue:
        # 当优先队列不为空时，执行以下循环
        dist, current = heapq.heappop(priority_queue)
        # 从优先队列中弹出距离最小的节点
        if current == goal:
            # 如果当前节点是目标节点，返回最短路径
            path = [goal]
            while current != start:
                # 从目标节点回溯到起点，构建最短路径
                current = distance[current][1]
                path.append(current)
            return dist, path[::-1]

        for dx, dy in directions:
            # 遍历当前节点的四个相邻节点
            neighbor = (current[0] + dx, current[1] + dy)
            if 0 <= neighbor[0] < rows and 0 <= neighbor[1] < cols and raster_array[neighbor[0]][neighbor[1]] not in [4, 6]:
                # 如果邻居在图内且不是障碍物
                new_dist = dist + 1
#表示从当前节点到邻居节点的新路径长度。在 Dijkstra 算法中，每个边的权重（或路径的长度）通常默认为1，
                if neighbor not in distance or new_dist < distance[neighbor][0]:
                # neighbor not in distance: 检查当前邻居节点是否已经在 distance 字典中记录。如果邻居节点不在字典中，表示它是第一次被探索，
                # 因此需要更新它的距离信息和父节点。neighbor not in distance:
                #检查从当前节点经过邻居节点到达起始节点的新路径长度 new_dist 是否比已知的最短路径长度如果新的路径更短，则更新距离信息和父节点
                    distance[neighbor] = (new_dist, current)
                #从起始节点到达邻居节点的新的最短距离以及该路径上邻居节点的父节点
                    heapq.heappush(priority_queue, (new_dist, neighbor))
                  #是将一个新的节点（邻居节点）及其与起始节点的距离推入优先队列（最小堆）的操作
    # 如果遍历完所有节点仍然没有找到路径，则返回空列表
    return float('inf'), []  # 返回无穷大代表未找到路径

栅格文件的路径及设置起点、终点、障碍点，像素值为3的栅格为终点，像素值为4或6的栅格为障碍点

start_time = time.time()
file_path = r'F:\paper1\data\初始数据\allpoint1\allpoint30.tif'
raster_array = read_raster(file_path)
start_point = (28,19)
goal_value = 3  # 目标点的像素值
goal_point = tuple(np.argwhere(raster_array == goal_value)[0])
obstacle_value = [4,6]  # 障碍点的像素值
obstacles = add_obstacles_from_raster(obstacle_value, raster_array)

使用Dijkstra算法寻找路径

cost,path = dijkstra(raster_array.shape, start_point, goal_point,raster_array)
end_time = time.time()

计算总运行时间

total_runtime = end_time - start_time
#total_runtime_milliseconds = total_runtime * 1000  # 转换为毫秒
#total_runtime_microseconds = total_runtime * 1e6  # 转换为微秒
print(f"总运行时间：{total_runtime} 秒")

展示栅格

plt.imshow(raster_array, cmap='viridis', origin='lower')
plt.colorbar()
plt.rcParams['font.sans-serif'] = ['SimHei']  # 指定默认字体
plt.rcParams['axes.unicode_minus'] = False  # 解决保存图像是负号'-'显示为方块的问题

绘制障碍物

plt.scatter([o[1] for o in obstacles], [o[0] for o in obstacles], color='black', marker='s', label='障碍物')

绘制起点终点

plt.scatter(start_point[1], start_point[0], color='red', marker='o', label='起点')
plt.scatter(goal_point[1], goal_point[0], color='yellow', marker='o', label='终点')

绘制路径

path_x, path_y = zip(*path)
plt.plot(path_y, path_x, color='blue', linewidth=2, label='路径')

计算路径穿越的栅格数量

num_crossed_cells = len(path)

计算路径的拐弯个数

turns = sum(1 for i in range(1, len(path) - 1) if path[i - 1][0] != path[i + 1][0] and path[i - 1][1] != path[i + 1][1])
print(f"路径穿越的栅格数量：{num_crossed_cells}")
print(f"路径拐弯个数：{turns}")
print("路径代价值:", cost)
plt.legend()
plt.title('Dijistra算法')
plt.show()