【路径规划】二维Dijkstra启发式改进算法

我们使用了A*算法的启发式(曼哈顿距离)来改进Dijkstra算法的性能。当我们将邻居节点添加到优先队列时,我们使用了distance + heuristic作为优先级,这样我们可以更快地找到通往目标节点的路径。

python 复制代码
import heapq
import numpy as np


def heuristic(a, b):
    (x1, y1) = a
    (x2, y2) = b
    return abs(x1 - x2) + abs(y1 - y2)  # 使用曼哈顿距离作为启发式


def dijkstra_with_a_star_heuristic(graph, start, end):
    # 初始化距离字典,将所有节点设置为无穷大,除了起点
    distances = {position: float('infinity') for position in np.ndindex(graph.shape)}
    distances[start] = 0

    # 优先队列,存储待检查的节点和它们的距离
    pq = [(0, start)]

    while pq:
        # 弹出当前最小距离的节点
        current_distance, current_position = heapq.heappop(pq)

        # 如果已经找到更短的路径到当前节点,则跳过
        if current_distance > distances[current_position]:
            continue

        # 如果到达目标节点,返回路径(这里未实现路径重构)
        if current_position == end:
            return distances[current_position]  # 这里只返回距离,路径重构需要额外工作

        # 遍历当前节点的邻居
        for dx, dy in [(-1, 0), (1, 0), (0, -1), (0, 1)]:
            x2, y2 = current_position[0] + dx, current_position[1] + dy

            # 检查邻居是否在网格内且不是障碍物
            if 0 <= x2 < graph.shape[0] and 0 <= y2 < graph.shape[1] and graph[x2, y2] != 1:
                # 计算到达邻居节点的距离
                distance = current_distance + 1

                # 使用启发式来改进Dijkstra的选择
                priority = distance + heuristic((x2, y2), end)

                # 如果找到更短的路径到邻居节点,则更新距离并添加到优先队列中
                if priority < distances[(x2, y2)]:
                    distances[(x2, y2)] = priority
                    heapq.heappush(pq, (priority, (x2, y2)))

    # 如果没有找到路径到目标节点
    return None
if __name__ == '__main__':


    # 示例用法
    graph = np.array([
        [0, 0, 0, 1, 0],
        [1, 1, 0, 1, 0],
        [0, 0, 0, 0, 0],
        [0, 1, 1, 1, 1],
        [0, 0, 0, 0, 0]
    ])
    start = (0, 0)
    end = (4, 4)

    distance = dijkstra_with_a_star_heuristic(graph, start, end)
    print(f"The shortest distance from {start} to {end} is: {distance}")
相关推荐
仙俊红28 分钟前
LeetCode每日一题,20250914
算法·leetcode·职场和发展
酷飞飞6 小时前
Python网络与多任务编程:TCP/UDP实战指南
网络·python·tcp/ip
风中的微尘7 小时前
39.网络流入门
开发语言·网络·c++·算法
数字化顾问7 小时前
Python:OpenCV 教程——从传统视觉到深度学习:YOLOv8 与 OpenCV DNN 模块协同实现工业缺陷检测
python
西红柿维生素8 小时前
JVM相关总结
java·jvm·算法
学生信的大叔8 小时前
【Python自动化】Ubuntu24.04配置Selenium并测试
python·selenium·自动化
诗句藏于尽头9 小时前
Django模型与数据库表映射的两种方式
数据库·python·django
智数研析社9 小时前
9120 部 TMDb 高分电影数据集 | 7 列全维度指标 (评分 / 热度 / 剧情)+API 权威源 | 电影趋势分析 / 推荐系统 / NLP 建模用
大数据·人工智能·python·深度学习·数据分析·数据集·数据清洗
扯淡的闲人9 小时前
多语言编码Agent解决方案(5)-IntelliJ插件实现
开发语言·python
moxiaoran57539 小时前
Flask学习笔记(一)
后端·python·flask