【路径规划】二维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}")
相关推荐
专注仿真10 小时前
基于 AFSIM 的智能体训练:多智能体强化学习篇 —— QTRAN 算法实战
开发语言·python·算法·php·强化学习·afsim智能体·仿真训练
FR02EN77711 小时前
[LeetCode] 26、80.删除有序数组中的重复项 题解
数据结构·算法
2601_9679873712 小时前
Python类变量和实例变量(类属性和实例属性)
python·类变量·实例变量·类属性·实例属性
CyrusYiVPP12 小时前
新用户第二周还剩几个?pandas做留存率cohort分析,数据分析实战第6期
人工智能·python
岁月宁静21 小时前
五、《从零手撸 Agent》 — 什么是 Agent:从“会聊天”到“会做事”,差的就是一个循环
python·agent
梯度下降者1 天前
Python - 函数
python·高阶函数·可调用对象·functools·嵌套作用域
forest_091 天前
《AI 编程 Harness 实战手册》04|审批这道门:点了一百次“允许”,还剩多少作用?
人工智能·算法·prompt·agi
H.莓飛1 天前
【C++】【STL】stack_OJ题
服务器·c语言·开发语言·数据结构·c++·算法·visual studio
starzy19901 天前
深入理解Scala隐式参数:上下文流转
开发语言·python·scala