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()