python 实现A星算法

　　上代码

　　# -*- coding: utf-8 -*-

　　# @Date : 2019-12-23 20:53:33

　　# @Author : Flying Hu (1152598046@qq.com)

　　# @Link : http://www.flyinghu.cn

　　# @name : A星算法实现

　　# @Version : 0.1

　　import os

　　import math

　　from random import randint

　　import time

　　# 定义全局变量

　　gz_char = '█' # 定义默认格子字符

　　fruit_char = '★' # 定义果实显示字符

　　self_char = '●' # 定义自身显示字符

　　wall_char = '◆' # 定义墙壁显示字符

　　# 全程使用上往下方向为x正方向 (二维列表行索引增加方向)

　　# 全程使用左往右方向为y正方向 (二维列表列索引增加方向)

　　class Map2D(object):

　　'''2D地图类'''

　　def __init__(self, width=20, height=20):

　　'''初始化

　　Args:

　　width 地图宽

　　height 地图高

　　'''

　　self.width = width

　　self.height = height

　　self.char = gz_char # 地图默认字符

　　# 生成地图

　　self.map = [[self.char for col in range(

　　self.width)] for row in range(self.height)]

　　# 生成地图周围墙

　　self.wall = [[i, j] for j in [-1, self.width] for i in range(self.height)] + [

　　[j, i] for j in [-1, self.height] for i in range(-1, self.width + 1)]

　　def __getitem__(self, item):

　　'''以键方式取值'''

　　return self.map[item]

　　def __setitem__(self, key, value):

　　'''以键方式存值'''

　　self.map[key] = value

　　def show(self):

　　'''在控制台打印当前地图'''

　　for row in self.map:

　　for c in row:

　　# 使用控制台颜色控制, 区分

　　if c == self_char:

　　print('\033[1;35;44m' + c + '\033[0m', end='')

　　elif c == wall_char:

　　print('\033[0;36;41m' + c + '\033[0m', end='')

　　elif c == fruit_char:

　　print('\033[1;33;40m' + c + '\033[0m', end='')

　　else:

　　print('\033[0;37;41m' + c + '\033[0m', end='')

　　print()

　　# 重置地图, 重置后就不会留下运动轨迹

　　# self.reload()

　　def reload(self):

　　'''重置地图'''

　　self.map = [[self.char for col in range(

　　self.width)] for row in range(self.height)]

　　class AStar(object):

　　'''实现A星算法'''

　　class Node(object):

　　'''节点类'''

　　def __init__(self, x, y, parent_node=None):

　　self.x = x

　　self.y = y

　　self.parent = parent_node # 父节点

　　# F = G + H

　　# G = 从起点 A 移动到指定方格的移动代价

　　# H = 从指定的方格移动到终点 B 的估算成本。试探法。 这里使用 Manhattan 方法估算H

　　self.G = 0

　　self.H = 0

　　def __init__(self, map2D):

　　'''初始化'''

　　self.map = map2D

　　def MinF(self):

　　'''从open_list中取出F最小的节点

　　Returns:

　　minF 返回open_list中F值最小的节点

　　'''

　　# 先假设第一个为最小, 然后循环挑选出F最小的节点

　　minF = self.open_list[0]

　　for node in self.open_list:

　　if (node.G + node.H) <= (minF.G + minF.H):

　　minF = node

　　return minF

　　def in_close_list(self, x, y):

　　'''判断坐标是否在close_list中

　　Args:

　　x x坐标

　　y y坐标

　　Return:

　　node 如果坐标在close_list中, 返回该节点, 反之返回False

　　'''

　　for node in self.close_list:

　　if node.x == x and node.y == y:

　　return node

　　return False

　　def in_open_list(self, x, y):

　　'''判断坐标是否在open_list中

　　Args:

　　x x坐标

　　y y坐标

　　Return:

　　node 如果坐标在open_list中, 返回该节点, 反之返回False

　　'''

　　for node in self.close_list:

　　if node.x == x and node.y == y:

　　return node

　　return False

　　def search(self, node):

　　'''搜索周围路径

　　Args:

　　node 搜索节点

　　'''

　　# 判断改节点是否为障碍物(障碍物或者墙壁)

　　if [node.x, node.y] in self.obstacles:

　　# 如果为障碍物则忽略该路径

　　return

　　# 判断是否在close_list中

　　if self.in_close_list(node.x, node.y):

　　# 如果已经在close_list中则忽略该节点

　　return

　　# 计算G和H

　　node.G = node.parent.G + 10

　　node.H = abs(self.target_node.x - node.x) + \

　　abs(self.target_node.y - node.y) * 10

　　# 判断是否在open_list中

　　tmp = self.in_close_list(node.x, node.y)

　　if tmp:

　　# 在open_list中

　　# 比较当前F和open_list中F

　　if (node.G + node.H) < (tmp.G + tmp.H):

　　# 如果判断该路径F值是否比open_list中存在的相同坐标的F值更小

　　tmp = node

　　else:

　　# 不在open_list中, 向open_list中添加

　　self.open_list.append(node)

　　def start(self, current_position, target_positiion, obstacles):

　　'''计算A星最优路径

　　Args:

　　current_position 当前位置坐标

　　target_positiion 目标位置坐标

　　obstacles 障碍物坐标列表

　　Returns:

　　path_list 如果存在最优路径返回A星最优路径, 反正返回None

　　'''

　　# 目标节点

　　self.target_node = AStar.Node(*target_positiion)

　　# 当前节点

　　self.current_node = AStar.Node(*current_position)

　　# 开启表, 添加当前节点到open_list中

　　self.open_list = [self.current_node]

　　# 关闭表

　　self.close_list = []

　　# 障碍物坐标集 (真实障碍物 + 墙壁)

　　self.obstacles = obstacles + self.map.wall

　　# 开启A星计算循环

　　while True:

　　# 判断是否到达终点, 判断目标节点坐标是否在close_list中

　　tmp = self.in_close_list(self.target_node.x, self.target_node.y)

　　if tmp:

　　# 返回路线

　　path_list = [[tmp.x, tmp.y]]

　　# 逆推路线

　　while tmp.parent:

　　tmp = tmp.parent

　　path_list.append([tmp.x, tmp.y])

　　# 反转列表

　　path_list.reverse()

　　return path_list

　　if not self.open_list:

　　# 如果open_list为空, 表示无路可走

　　return None

　　# 从open_list中选出F最小路径节点

　　minF = self.MinF()

　　# 将当前选出节点添加到close_list中, 并从open_list中移除

　　self.close_list.append(minF)

　　self.open_list.remove(minF)

　　# 搜索路径并将当前节点作为父节点 根据固定顺序搜索 (固定就行, 顺序任意)

　　self.search(AStar.Node(minF.x - 1, minF.y, minF))

　　self.search(AStar.Node(minF.x, minF.y - 1, minF))

　　self.search(AStar.Node(minF.x + 1, minF.y, minF))

　　self.search(AStar.Node(minF.x, minF.y + 1, minF))

　　# 存在计算时长比较久的清空, 打印提示信息

　　print('\r叼毛, 正在规划路径...', end='')

　　def sub_start(self, current_position, target_positiion, obstacles, num=20):

　　'''限定次数判断是否有路可走'''

　　# 目标节点

　　self.target_node = AStar.Node(*target_positiion)

　　# 当前节点

　　self.current_node = AStar.Node(*current_position)

　　# 开启表, 添加当前节点到open_list中

　　self.open_list = [self.current_node]

　　# 关闭表

　　self.close_list = []

　　# 障碍物坐标集 (真实障碍物 + 墙壁)

　　self.obstacles = obstacles + self.map.wall

　　# 开启A星计算循环

　　for i in range(num):

　　# 判断是否到达终点, 判断目标节点坐标是否在close_list中

　　tmp = self.in_close_list(self.target_node.x, self.target_node.y)

　　if tmp:

　　# 返回路线

　　path_list = [[tmp.x, tmp.y]]

　　# 逆推路线

　　while tmp.parent:

　　tmp = tmp.parent

　　path_list.append([tmp.x, tmp.y])

　　# 反转列表

　　path_list.reverse()

　　return True

　　if not self.open_list:

　　# 如果open_list为空, 表示无路可走

　　return False

　　# 从open_list中选出F最小路径节点

　　minF = self.MinF()

　　# 将当前选出节点添加到close_list中, 并从open_list中移除

　　self.close_list.append(minF)

　　self.open_list.remove(minF)

　　# 搜索路径并将当前节点作为父节点 根据固定顺序搜索 (固定就行, 顺序任意)

　　self.search(AStar.Node(minF.x - 1, minF.y, minF))

　　self.search(AStar.Node(minF.x, minF.y - 1, minF))

　　self.search(AStar.Node(minF.x + 1, minF.y, minF))

　　self.search(AStar.Node(minF.x, minF.y + 1, minF))

　　else:

　　return True

　　class Game(object):

　　'''game类'''

　　def __init__(self, map2D, obs_num=None):

　　'''初始化

　　Args:

　　map2D 2D地图

　　obs_num 初始化障碍物数量

　　'''

　　self.map = map2D

　　self.height = self.map.height

　　self.width = self.map.width

　　# 随机一个初始化运动方向

　　self.direction = randint(0, 3)

　　# 根据地图大小计算障碍物数量

　　# self.obs_num = int(math.sqrt(self.height * self.width))

　　self.obs_num = obs_num if obs_num else int(

　　math.sqrt(self.height * self.width))

　　# 初始化起点

　　self.current = [

　　randint(int(1/4 * (self.height - 1)),

　　int(3/4 * (self.height - 1))),

　　randint(int(1/4 * (self.width - 1)),

　　int(3/4 * (self.width - 1)))

　　]郑州妇科医院 http://www.hnzzkd.com/

　　# 生成果实目标

　　self.gen_fruit()

　　# 生成障碍物

　　self.gen_obs()

　　def gen_fruit(self):

　　'''生成果实'''

　　while True:

　　# 随机生成果实坐标

　　self.fruit = [randint(0, self.height - 1),

　　randint(0, self.width - 1)]

　　# 避免果实和起点坐标重合

　　if self.fruit != self.current:

　　break

　　def gen_obs(self):

　　'''生成障碍物'''

　　self.obs_list = []

　　for i in range(self.obs_num):

　　while True:

　　tmp = [randint(0, self.height - 1), randint(0, self.width - 1)]

　　# 避免障碍物和起点或者果实重合

　　if tmp != self.current and tmp != self.fruit:

　　self.obs_list.append(tmp)

　　break

　　def move(self):

　　'''根据移动方向移动

　　0, 1, 2, 3分别对应上, 左, 下, 右

　　'''

　　if self.direction == 0:

　　self.current = [self.current[0] - 1, self.current[1]]

　　elif self.direction == 1:

　　self.current = [self.current[0], self.current[1] - 1]

　　elif self.direction == 2:

　　self.current = [self.current[0] + 1, self.current[1]]

　　else:

　　self.current = [self.current[0], self.current[1] + 1]

　　def cls(self):

　　'''清空控制台输出'''

　　os.system('cls')

　　def load(self):

　　'''加载果实和障碍物'''

　　# 加载障碍物

　　for row, col in self.obs_list:

　　self.map[row][col] = wall_char

　　# 加载果实和当前点

　　row, col = self.current

　　self.map[row][col] = self_char

　　row, col = self.fruit

　　self.map[row][col] = fruit_char

　　def start(self):

　　'''开始循环'''

　　# 开启A星

　　g = self.a_star()

　　# 进入循环

　　while True:

　　# 清空控制台输出

　　self.cls()

　　# 加载

　　self.load()

　　# 打印显示

　　self.map.show()

　　# 判断是否吃到果实

　　if self.current == self.fruit:

　　# 吃到果实

　　# 重置地图

　　self.map.reload()

　　# 重新生成果实, 重新生成障碍物

　　self.gen_fruit()

　　self.gen_obs()

　　self.map.reload()

　　if next(g) is False:

　　# 表示无路可走

　　# 重置地图

　　self.map.reload()

　　continue

　　# 移动

　　self.move()

　　# 控制移动速度

　　time.sleep(0.3)

　　def a_star(self):

　　'''接入A*算法寻路

　　使用python生成器方式实现在循环中一次一次改变方向

　　'''

　　# 创建对象

　　a = AStar(self.map)

　　while True:

　　# 提前加载显示地图, 提前显示可以人工判断是否真的无路可走

　　# 清空控制台输出

　　self.cls()

　　# 加载

　　self.load()

　　# 打印显示

　　self.map.show()

　　# 先反向判断, 在限定次数中是否得出无路可走, 添加改策略一定程度减少计算时间很长的正向计算

　　if a.sub_start(self.fruit, self.current, self.obs_list, 30) is False:

　　# 表示无路可走

　　input('无路可走, 回车刷新地图')

　　self.map.reload()

　　self.gen_fruit()

　　self.gen_obs()

　　# 返回无路可走信号

　　yield False

　　continue

　　path_list = a.start(self.current, self.fruit, self.obs_list)

　　if not path_list:

　　# 表示无路可走

　　# 提前加载显示地图, 提前显示可以人工判断是否真的无路可走

　　# 清空控制台输出

　　# self.cls()

　　# # 加载

　　# self.load()

　　# # 打印显示

　　# self.map.show()

　　input('无路可走, 回车刷新地图')

　　self.map.reload()

　　self.gen_fruit()

　　self.gen_obs()

　　# 返回无路可走信号

　　yield False

　　continue

　　# 遍历启动后路径, 对比得出行走方向

　　for path in path_list[1:]:

　　if path[0] > self.current[0]:

　　self.direction = 2

　　elif path[0] < self.current[0]:

　　self.direction = 0

　　elif path[1] > self.current[1]:

　　self.direction = 3

　　else:

　　self.direction = 1

　　yield

　　if __name__ == "__main__":

　　# 初始化控制台大小

　　os.system("mode con cols=80 lines=80")

　　# 创建地图

　　map2D = Map2D(width=20, height=20)

　　# 新建, 指定障碍物

　　game = Game(map2D, 150)

　　# 打开

　　game.start()