【路径规划】二维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}")
相关推荐
荏苒追寻2 分钟前
Python 爬虫——爬虫基础
python
脑洞代码4 分钟前
20250909的学习笔记
算法
Christo34 分钟前
TFS-2003《A Contribution to Convergence Theory of Fuzzy c-Means and Derivatives》
人工智能·算法·机器学习
wdfk_prog5 分钟前
Python脚本深度解析:实现基于YMODEM的单片机固件自动化升级
python·单片机·自动化
黑菜钟12 分钟前
代码随想录第七天|● 454.四数相加II ● 383. 赎金信 ● 15. 三数之和 18.四数之和
c++·算法·leetcode
Yingjun Mo24 分钟前
1. 统计推断-ALMOND收敛性分析
人工智能·算法·机器学习
海梨花33 分钟前
CSP认证练习题目推荐 (1)
算法·深度优先·csp
天上的光1 小时前
大模型——剪枝、量化、蒸馏、二值化
算法·机器学习·剪枝
萧鼎1 小时前
深入解析 Python 的 pytun 库:虚拟网络接口与隧道技术实战指南
服务器·网络·python
西猫雷婶1 小时前
pytorch基本运算-分离计算
人工智能·pytorch·python·深度学习·神经网络·机器学习