【学习】A星寻路python可视化
一、算法原理
1.前言
作为启发式搜索中最容易理解的算法，A星算法其实就是A的最优解。
寻路问题一般来讲，需要将地图划分如上图所示的格子地图，格子越小，精度就越高，能够简化搜索区域。对于每个格子，其大体上都有两种状态，分别是可以通过（walk）和不能通过（stop），最后计算出从起始点到终点需要走过哪些方格，就找到了路径。
2.搜索过程
将搜索区域再进行简化，视为一组二维节点，查找当前节点的相邻节点，依照下述规则进行搜索。
- 从起点A开始搜索，并把其加入openlist中。openlist中存放待扩展节点，此时只有起点A。
- 查看与点A相邻的所有节点，根据判别语句排除掉障碍、边界等无法到达的区域，将其中可达的节点放入openlist，并将A节点设置为他们的father节点。
- 将A从openlist中移出，置入closelist中。closelist中存放的是扩展完毕的节点。
- 随后从openlist中根据f值，挑选下一个需要扩展的点。
3.求f及排序
对于启发式搜索算法来说，通常用f表示该节点向目标节点前进一步的希望程度，对于A星算法来说$f=g+h$ ，其中g表示从起点 A 移动到指定方格的移动代价，h表示从指定的方格移动到终点 B 的估算成本。
举个例子：
设定方格为单位大小，则对角线长度为根号2，为了方便计算用1.4代替，故该点的g=1.4，对于h来说，计算方法有很多，这里采用Manhattan作演示,h = 3+1 = 4。
f = g + h = 1.4 + 4 = 5.4
将openlist中节点的f进行排序，拥有最小f的节点就是估计的最优节点
4.继续搜索
- 将openlist中f最小的节点移出，放入closelist中
- 检查所有与其相邻的方格，将状态为walk的节点置入openlist中，并设置好father
- 若某个节点已经存在openlist中，则比较g，若经由当前节点的g小，则更新那个节点father、f和g
二、核心代码实现
def FindPath(startRow,startCol,endRow,endCol):
global mapRow,mapCol,nodes,openlist,closelist
# 判断是否在地图范围内
if startRow < 0 or startRow >= mapRow or startCol < 0 or startCol >= mapCol or endRow < 0 or endRow >= mapRow or endCol < 0 or endCol >= mapCol:
print("开始或结束点在地图范围外！")
# 获取起点终点位置格子对象
startNode = nodes[startRow,startCol]
endNode = nodes[endRow,endCol]
# 判断是不是障碍
if startNode.type == 'stop' or endNode.type == 'stop':
print("开始或结束点为障碍！")
return
# 清空openlist和closelist
openlist.clear()
closelist.clear()
# 把起点放入closelist
closelist.append(startNode)
while True:
# 从起点开始找寻周围的节点，并放入openlist中
# 左上
FindNearlyToOpenList(startNode.row - 1, startNode.column - 1, 1.4, startNode, endNode)
# 上
FindNearlyToOpenList(startNode.row - 1, startNode.column, 1.0, startNode, endNode)
# 右上
FindNearlyToOpenList(startNode.row - 1, startNode.column + 1, 1.4, startNode, endNode)
# 左
FindNearlyToOpenList(startNode.row, startNode.column - 1, 1.0, startNode, endNode)
# 右
FindNearlyToOpenList(startNode.row, startNode.column + 1, 1.0, startNode, endNode)
# 左下
FindNearlyToOpenList(startNode.row + 1, startNode.column - 1, 1.4, startNode, endNode)
# 下
FindNearlyToOpenList(startNode.row + 1, startNode.column, 1.0, startNode, endNode)
# 右下
FindNearlyToOpenList(startNode.row + 1, startNode.column + 1, 1.4, startNode, endNode)
# time.sleep(0.04)
# 将f值显示在window上
ShowF()
# time.sleep(1)
# 如果openlist为空了还未找到终点，则证明死路
if len(openlist) == 0:
print("死路一条！")
return
# 选出openlist中，寻路消耗最小的结点
openlist = sorted(openlist,key=lambda x:x.f)
# 放入closelist
closelist.append(openlist[0])
# 最小的点作为新的起点
startNode = openlist[0]
openlist.pop(0)
# 如果已经搜索到了终点,则得到最终结果
if startNode == endNode:
path = []
path.append(endNode)
while endNode.father != None:
path.append(endNode.father)
endNode = endNode.father
# 反转
path.reverse()
return path
def FindNearlyToOpenList(nodeRow,nodeCol,g,fatherNode,endNode):
global mapRow,mapCol,nodes,openlist,closelist,findMethod
# 边界判断
if nodeRow < 0 or nodeRow >= mapRow or nodeCol < 0 or nodeCol >= mapCol:
return
node = nodes[nodeRow, nodeCol]
# 判断其是否是障碍或者已经存在closelist之中
if (node == None) or (node.type == 'stop') or (node in closelist):
return
# 如果当前节点已经存在openlist中
if node in openlist:
newg = fatherNode.g + g
if newg > node.g:
return
# 记录父节点
node.father = fatherNode
# 计算g值
node.g = fatherNode.g + g
# h，这里用曼哈顿计算，常用的还有（欧式距离、对角线估价)
if findMethod == "Manhattan":
node.h = abs(endNode.row - node.row) + abs(endNode.column - node.column)
elif findMethod == "Euclidean":
node.h = ((endNode.row - node.row)**2 + (endNode.column - node.column)**2)**0.5
# f
node.f = node.g + node.h
# 放入openlist
openlist.append(node)
三、全部代码及可视化
import numpy as np
import random
import matplotlib.pyplot as plt
import tkinter as tk
import sys
import os
import time
# 地图宽高
mapRow = 0
mapCol = 0
# 初始化openlist与closelist
openlist = []
closelist = []
# 初始化格子容器
nodes = {}
# 当前功能
menuNode = -1
# 按钮字典
buttonNodes = {}
# 搜索起点,搜索终点坐标
startRow, startCol, endRow, endCol = 0, 0, 0, 0
# 按步执行参数(暂无用)
ispause = False
# 当前搜索方法,默认曼哈顿（Manhattan ），欧氏距离（Euclidean）
findMethod = "Manhattan"
# h的功能按钮
methodButton1, methodButton2 = 0, 0
# 定义格子类
class AStarNode:
def __init__(self):
# 格子坐标
self.row = -1
self.column = -1
# 寻路消耗
self.f = 0
# 距离起点距离
self.g = 0
# 距离终点距离
self.h = 0
self.type = 'walk'
# 声明父格子
self.father = None
# 状态码 0为初始，1为走过
self.value = 0
# 设置node坐标
def setPos(self,row,column):
self.row = row
self.column = column
# 设置node状态，walk可走，stop不可
def setType(self,type):
self.type = type
# 测试格子矩阵
def ShowNodes(w,h):
global mapRow,mapCol,nodes,openlist,closelist
for i in range(w):
for j in range(h):
print(nodes[i,j].type, end='\t')
print()
# 控制台打印路径，用于测试
def ShowResultMap(w, h, pathlist):
global nodes
for p in pathlist:
nodes[p.row,p.column].type = "gogo"
for i in range(w):
for j in range(h):
print(nodes[i,j].type, end='\t')
print()
# 初始化nodes（新）
def InitMap(w,h):
# 初始化地图大小
global mapRow, mapCol, nodes, openlist, closelist
mapRow = w
mapCol = h
# 生成格子矩阵
for i in range(w):
for j in range(h):
nodes[i,j] = AStarNode()
# 初始化每个格子的位置
nodes[i, j].setPos(i, j)
def FindPath(startRow,startCol,endRow,endCol):
global mapRow,mapCol,nodes,openlist,closelist
# 判断是否在地图范围内
if startRow < 0 or startRow >= mapRow or startCol < 0 or startCol >= mapCol or endRow < 0 or endRow >= mapRow or endCol < 0 or endCol >= mapCol:
print("开始或结束点在地图范围外！")
# 获取起点终点位置格子对象
startNode = nodes[startRow,startCol]
endNode = nodes[endRow,endCol]
# 判断是不是障碍
if startNode.type == 'stop' or endNode.type == 'stop':
print("开始或结束点为障碍！")
return
# 清空openlist和closelist
openlist.clear()
closelist.clear()
# 把起点放入closelist
closelist.append(startNode)
while True:
# 从起点开始找寻周围的节点，并放入openlist中
# 左上
FindNearlyToOpenList(startNode.row - 1, startNode.column - 1, 1.4, startNode, endNode)
# 上
FindNearlyToOpenList(startNode.row - 1, startNode.column, 1.0, startNode, endNode)
# 右上
FindNearlyToOpenList(startNode.row - 1, startNode.column + 1, 1.4, startNode, endNode)
# 左
FindNearlyToOpenList(startNode.row, startNode.column - 1, 1.0, startNode, endNode)
# 右
FindNearlyToOpenList(startNode.row, startNode.column + 1, 1.0, startNode, endNode)
# 左下
FindNearlyToOpenList(startNode.row + 1, startNode.column - 1, 1.4, startNode, endNode)
# 下
FindNearlyToOpenList(startNode.row + 1, startNode.column, 1.0, startNode, endNode)
# 右下
FindNearlyToOpenList(startNode.row + 1, startNode.column + 1, 1.4, startNode, endNode)
# time.sleep(0.04)
# 将f值显示在window上
ShowF()
# time.sleep(1)
# 如果openlist为空了还未找到终点，则证明死路
if len(openlist) == 0:
print("死路一条！")
return
# 选出openlist中，寻路消耗最小的结点
openlist = sorted(openlist,key=lambda x:x.f)
# 放入closelist
closelist.append(openlist[0])
# 最小的点作为新的起点
startNode = openlist[0]
openlist.pop(0)
# 如果已经搜索到了终点,则得到最终结果
if startNode == endNode:
path = []
path.append(endNode)
while endNode.father != None:
path.append(endNode.father)
endNode = endNode.father
# 反转
path.reverse()
return path
def FindNearlyToOpenList(nodeRow,nodeCol,g,fatherNode,endNode):
global mapRow,mapCol,nodes,openlist,closelist,findMethod
# 边界判断
if nodeRow < 0 or nodeRow >= mapRow or nodeCol < 0 or nodeCol >= mapCol:
return
node = nodes[nodeRow, nodeCol]
# 判断其是否是障碍或者已经存在closelist之中
if (node == None) or (node.type == 'stop') or (node in closelist):
return
# 如果当前节点已经存在openlist中
if node in openlist:
newg = fatherNode.g + g
if newg > node.g:
return
# 记录父节点
node.father = fatherNode
# 计算g值
node.g = fatherNode.g + g
# h，这里用曼哈顿计算，常用的还有（欧式距离、对角线估价)
if findMethod == "Manhattan":
node.h = abs(endNode.row - node.row) + abs(endNode.column - node.column)
elif findMethod == "Euclidean":
node.h = ((endNode.row - node.row)**2 + (endNode.column - node.column)**2)**0.5
# f
node.f = node.g + node.h
# 放入openlist
openlist.append(node)
# 检测当前是否可以寻路
def CheckMap():
# 有起点和终点，则可以寻路
global nodes
checkStart = False
checkEnd = False
for bt in buttonNodes.values():
if bt['bg'] == 'green':
checkStart = True
if bt['bg'] == 'red':
checkEnd = True
if checkEnd and checkStart:
return True
def ShowF():
global openlist, closelist,buttonNodes
for node in openlist:
buttonNodes[node.row, node.column]['text'] = str(round(node.f, 3))
for node in closelist:
buttonNodes[node.row, node.column]['text'] = str(round(node.f, 3))
def ChangeMethod(method,temp):
global findMethod,methodButton1,methodButton2
findMethod = method
if temp == 1:
methodButton1['bg'] = 'DeepPink'
methodButton2['bg'] = '#6495ED'
elif temp == 2:
methodButton2['bg'] = 'DeepPink'
methodButton1['bg'] = '#6495ED'
# 清除f和颜色
def ClearMap():
global openlist, closelist, buttonNodes, nodes, mapRow, mapCol
# 清除按钮上的f和颜色
for btKey in buttonNodes.keys():
buttonNodes[btKey]['bg'] = 'white'
buttonNodes[btKey]['text'] = ''
# 初始化nodes
for i in range(mapRow):
for j in range(mapCol):
nodes[i,j] = AStarNode()
# 默认均为walk
nodes[i,j].setType('walk')
# 初始化每个格子的位置
nodes[i, j].setPos(i, j)
def MenuClick(i):
global mapRow,mapCol,nodes,menuNode,buttonNodes,startRow,startCol,endRow,endCol,findMethod
# 0 开始
# 1 设置障碍
# 2 设置起点
# 3 设置终点
# 4 按步显示结果
# 5 重置
if i == 0 and CheckMap():
menuNode = 0
pathlist = FindPath(startRow, startCol, endRow, endCol)
# 画出路径
if pathlist is not None:
for p in pathlist:
if buttonNodes[p.row, p.column]['bg'] == 'white':
buttonNodes[p.row, p.column]['bg'] = 'Orange'
# print(nodes[5,1].f)
elif i == 1:
menuNode = 1
elif i == 2:
menuNode = 2
elif i == 3:
menuNode = 3
# elif i == 4:
# menuNode = 4
elif i == 4:
menuNode = 4
ClearMap()
# 重启程序
# python = sys.executable
# os.execl(python, python, *sys.argv)
def CreateGUI():
global mapRow,mapCol,nodes,buttonNodes,methodButton1,methodButton2
window = tk.Tk()
window.title('my window')
window.geometry('1920x1027')
tk.Button(window,text='开始',width=11,height=5,bg='#6495ED',command=lambda : MenuClick(i = 0)).grid(row=0,column=0,padx=0,pady=0)
tk.Button(window,text='障碍',width=11,height=5,bg='#6495ED',command=lambda : MenuClick(i = 1)).grid(row=0,column=1,padx=0,pady=0)
tk.Button(window,text='起点',width=11,height=5,bg='#6495ED',command=lambda : MenuClick(i = 2)).grid(row=0,column=2,padx=0,pady=0)
tk.Button(window,text='终点',width=11,height=5,bg='#6495ED',command=lambda : MenuClick(i = 3)).grid(row=0,column=3,padx=0,pady=0)
# tk.Button(window,text='下一步',width=11,height=5,bg='#6495ED',command=lambda : MenuClick(i = 4)).grid(row=0,column=4,padx=0,pady=0)
tk.Button(window,text='重置',width=11,height=5,bg='#6495ED',command=lambda : MenuClick(i = 4)).grid(row=0,column=4,padx=0,pady=0)
methodButton1 = tk.Button(window, text='欧氏距离', width=11, height=5, bg='#6495ED', command=lambda: ChangeMethod(method="Euclidean", temp=1))
methodButton1.grid(row=0, column=5, padx=0, pady=0)
methodButton2 = tk.Button(window, text='曼哈顿距离', width=11, height=5, bg='DeepPink', command=lambda: ChangeMethod(method="Manhattan", temp=2))
methodButton2.grid(row=0,column=6,padx=0,pady=0)
for i in range(mapRow):
# width +1加9像素
# height +1加20像素
for j in range(mapCol):
buttonNodes[i,j] = tk.Button(window,text='',width=11,height=5,bg='white',command=lambda i=i,j=j: SetButton(i,j))
buttonNodes[i,j].grid(row=1+i,column=j,padx=0,pady=0)
# if pathlist is not None:
# for p in pathlist:
# buttonNodes[p.row,p.column]['bg'] = 'red'
window.mainloop()
def SetButton(i,j):
global menuNode,buttonNodes,mapRow,mapCol,nodes,startRow,startCol,endRow,endCol
if menuNode == -1:
return
elif menuNode == 1:
# 如果设置过障碍,则清除
if buttonNodes[i,j]['bg'] == 'black':
nodes[i,j].type = 'walk'
buttonNodes[i,j]['bg'] = 'white'
else:
nodes[i,j].type = 'stop'
buttonNodes[i,j]['bg'] = 'black'
elif menuNode == 2:
# 如果设置过起点,则清除
for buttonKey,buttonValue in buttonNodes.items():
if buttonValue['bg'] == 'green':
buttonNodes[buttonKey]['bg'] = 'white'
break
buttonNodes[i,j]['bg'] = 'green'
startRow,startCol = i,j
nodes[i,j].type = 'walk'
elif menuNode == 3:
# 如果设置过终点,则清除
for buttonKey,buttonValue in buttonNodes.items():
if buttonValue['bg'] == 'red':
buttonNodes[buttonKey]['bg'] = 'white'
buttonNodes[i,j]['bg'] = 'red'
endRow,endCol = i,j
nodes[i,j].type = 'walk'
InitMap(8, 17)
ShowNodes(8, 17)
# pathlist = FindPath(1, 0, 5, 6)
print("_________________________________")
CreateGUI()
