Astar路径规划算法复现-python实现

# -*- coding: utf-8 -*-
"""
Created on Fri May 24 09:04:23 2024


"""

import os
import sys
import math
import heapq
import matplotlib.pyplot as plt
import time

'''
传统A*算法
'''


class Astar:
    '''
    AStar set the cost + heuristics as the priority
    AStar将成本+启发式设置为优先级
    '''

    def __init__(self, s_start, s_goal, heuristic_type, xI, xG):
        self.s_start = s_start
        self.s_goal = s_goal
        self.heuristic_type = heuristic_type
        self.u_set = [(-1, 0), (0, 1), (1, 0), (0, -1)]

        # self.obs = self.obs_map()  # 障碍物位置
        self.Open = []  # 优先级序列，open集合
        self.Closed = []  # 相邻点集合，访问visited序列
        self.parent = dict()  # 相邻父节点
        self.g = dict()  # 成本
        self.x_range = 51  # 设置背景大小
        self.y_range = 51

        self.xI, self.xG = xI, xG
        self.obs = self.obs_map()

    def animation(self, path_l, visited_l, name, path_color='g'):

        # 绘制地图基础元素
        obs_x = [x[0] for x in self.obs]
        obs_y = [x[1] for x in self.obs]
        plt.plot(self.xI[0], self.xI[1], "bs")  # 起点
        plt.plot(self.xG[0], self.xG[1], "gs")  # 终点
        plt.plot(obs_x, obs_y, "sk")  # 障碍物
        plt.title(name)
        plt.axis("equal")

        # 移除起点和终点于visited_l列表中，避免它们被标记为已访问点
        visited_l = [node for node in visited_l if node != self.xI and node