目录
前言
上篇文章讲的是用OpenCV画简单图形以及绘制栅格地图。这篇文章在栅格地图中实现A*算法（静态图）。
一、A*算法的基本原理
A*算法是一种在图形平面上，有多个节点的路径中，寻找一条从起始点（source）到目标点（goal）的最短遍历路径的算法。它属于启发式搜索算法（Heuristic Search Algorithm），因为它使用启发式方法来计算图中的节点，从而减少实际计算的节点数量。
A*算法的基本原理包括以下几个关键点：
-
启发式函数（Heuristic Function）：启发式函数用来估计从当前节点到目标节点的距离。这个函数是问题特定的，并且通常是问题解决的关键。一个好的启发式函数可以显著提高搜索效率。
-
成本函数（Cost Function）：A*算法使用一个成本函数来计算从起始点到当前节点的实际路径成本。这个成本通常是通过累加从起始点到当前节点的边的权重来计算的。
-
A*算法使用一个路径优劣评价公式为：
f(n):从起始点到节点n的总估计成本
g(n):从起始点到当前节点n的实际路径成本。
h(n):从当前节点n到目标节点的估计成本，即启发式函数的值。
4.开放列表（Open List）：这是一个优先队列，用于存储待探索的节点。节点按照F值的大小进行排序，F值最小的节点最先被探索。
5.关闭列表（Closed List）：这是一个已探索节点的集合，用于避免重复探索同一个节点。
6.搜索过程：
- 从开放列表（Open List）中取出F值最小的节点，称为当前节点。
- 如果当前节点是目标节点，则搜索结束，找到了最短路径。
- 否则，将当前节点的所有邻居节点加入开放列表，并计算它们的G、H和F值。
- 将当前节点加入关闭列表，表示已经探索过。
7.路径回溯：一旦找到目标节点，算法会从目标节点开始回溯，直到起始节点，构建出完整的路径。
A*算法的效率和准确性很大程度上依赖于启发式函数的选择。如果启发式函数总是低估实际成本，那么A算法可以保证找到最短路径，并且是最优的。
A*寻路算法详解 #A星 #启发式搜索_哔哩哔哩_bilibili
关于A*算法的原理讲解，我推荐B站上这个up主的视频，真的非常简洁清晰（相对来说），多看几遍肯定有所收获。
二、A*算法代码实现（Python）
1.栅格地图的构建
1.1导入模块
import cv2
import numpy as np
import matplotlib.pyplot as plt
import time
前面两行在上篇文章《用OpenCV画简单图形以及绘制栅格地图》中讲过，想了解的可以去看一下。
import matplotlib.pyplot as plt： 导入了matplotlib库中的pyplot模块，并给它起了一个别名plt。matplotlib.pyplot是一个用于创建静态、交互式和动画可视化的模块。通过as plt，你可以在代码中使用plt来调用pyplot模块中的函数，例如plt.plot()用于绘制图表，plt.show()用于显示图表等。
import time：导入了Python标准库中的time模块，这个模块通常用于需要时间测量或延迟执行任务的场景。time模块提供了各种与时间相关的函数，例如time.sleep()用于暂停程序执行指定的时间（秒），time.time()返回当前时间的时间戳等。
1.2 场景布置
#画出画布，方框
# 定义图像的大小
img_size = 401
grid_size = 20
# 创建白色背景的图像
img = np.ones((img_size, img_size, 3), np.uint8) * 255
#设置起点和终点
"""#可以采用键盘输入
print('请输入起始点横坐标（0到19）：')
sx = input()
print('请输入起始点纵坐标（0到19）：')
sy = input()
print('请输入目标点横坐标（0到19）：')
gx = input()
print('请输入目标点纵坐标（0到19）：')
gy = input()
"""
sx=0
sy=0
gx=19
gy=19
cv2.rectangle(img,(20*int(sx),20*int(sy)),(20+20*int(sx),20+20*int(sy)),(0,0,255),-1) #起点
cv2.rectangle(img,(20*int(gx),20*int(gy)),(20+20*int(gx),20+20*int(gy)),(255,255,0),-1)#终点
#增加黑色障碍物
cv2.rectangle(img,(60,60),(120,80),(0,0,0),-1)
cv2.rectangle(img,(180,80),(220,260),(0,0,0),-1)
cv2.rectangle(img,(240,220),(280,260),(0,0,0),-1)
cv2.rectangle(img,(60,180),(240,200),(0,0,0),-1)
cv2.rectangle(img,(280,260),(400,280),(0,0,0),-1)
# 绘制网格
for x in range(0, img_size, grid_size):
for y in range(0, img_size, grid_size):
# 绘制每个方框的边框
cv2.rectangle(img, (x, y), (x + grid_size, y + grid_size), (0, 0, 0), 1)
#显示图像
cv2.imshow('rectangle',img)
cv2.waitKey(0)
#将图像以图片的形式保存下来
# 保存图像的文件路径
save_path = 'a_star_tupian.png'
# 保存图像
cv2.imwrite(save_path, img)
print(f"图像已保存至：{save_path}")PS：1、 这里起点和终点的坐标既可以采用键盘输入的方式，也可以提前设置。分别用红色和蓝色的方块表示起点和终点。
2、将图像用png格式的图片的形式保存下来，方便多次的读取和调用。注意保存地址要和代码文件一致。
# 读取栅格地图图像
input_path = 'a_star_tupian.png'
image = cv2.imread(input_path, cv2.IMREAD_GRAYSCALE) #image是Numpy数组形式
block_size = 20
num_block_x = image.shape[1] // block_size #列数/宽度--x
num_block_y = image.shape[0] // block_size #行数/高度--y
#初始化一个新的布尔数组来表示每个小块
block_grid = np.zeros((num_block_y, num_block_x), dtype=np.uint8) #np.zeros数组，将所有元素初始化为零
#遍历是否每个小块都是黑色
for by in range(num_block_y): #遍历行数
for bx in range(num_block_x): #遍历列数
# 计算小块在图像中的索引范围
y_start = by * block_size #block_size = 20
y_end = y_start + block_size
x_start = bx * block_size
x_end = x_start + block_size
# 检查小块内的所有像素是否都满足条件（例如全部为0）
block = image[y_start:y_end, x_start:x_end] #image是Numpy数组形式
is_block_black = np.all(block == 0) #如果所有的block都是0，那么得到的赋值是True
# 将结果赋值给 block_grid
block_grid[by, bx] = is_block_black #新生成的数组里面对应的点（dy,dx）的值是右边的值
# 打印新的栅格表示
print("栅格小块表示：")
print(block_grid)这里我们将保存的图片识别出来，0表示障碍物，1表示空的。一个方格有20*20=400个像素，为了简化，我们将400个像素的值用一个方块来表示。当400个像素全是0时表示该方格的值为True,然后，这个
True值会被赋给block_grid数组中对应的位置。在Python中，布尔值True和False分别对应于整数1和0。所以：
- 如果小块是黑色（所有像素都为0），
block_grid[by, bx]会被赋值为1。- 如果小块不是黑色（至少有一个像素不为0），
block_grid[by, bx]会被赋值为0。
2. A*算法代码
2.1 函数和类的调用
open_list=[]
class Position:
x = 0
y = 0
def __init__(self, X, Y):
self.x = X
self.y = Y
def __eq__(self, other): #等价性比较 __eq__
if other and self:
return self.x == other.x and self.y == other.y #比较两个点是否在相同的位置。
else:
return not (self or other)
#Position 类用于表示二维空间中的点
class Msg:
G = 0
H = 0
IsUnk = 1 #此处替代close_list功能，判断某点是否被搜索过
parent = None # Position
def __init__(self):
self.G = 0
self.H = 0
self.IsUnk = 1 #后续中会把探索过的点的IsUnk改为0
def GetF(self):
return self.G+self.H
#代价函数初始化 返回的是F代价
class Board:
mapx=[ ] #二维列表，用于存储地图上每个单元格的状态信息。每个单元格由 Msg 类的实例表示。
def __init__(self,mapp,target_position):
for i in range(len(mapp)):
self.mapx.append([])
for j in range(len(mapp[0])):
self.mapx[i].append(Msg())
self.mapx[i][j].IsUnk = 1 - mapp[i][j]
#如果mapp[i][j]=0说明可通行，IsUnk=1,表示未探索；反之说明是障碍物，已经探索
#曼哈顿方法计算H值
self.mapx[i][j].H = 10*(abs(target_position.x-i) + abs(target_position.y-j))
def GetMsg(self,pos):
return self.mapx[pos.x][pos.y]
def IsInBoard(i,j):
if i>=0 and i < len(mapp) and j>=0 and j<len(mapp[i]) and mapp[i][j] == 0:
return 1
else:
return 0这里用IsUnk代替close_list功能，判断某点是否被搜索过。
def SearchPath(mapp,current_pos,target_position):
start_time = time.time()
board = Board(mapp,target_position) #地图棋盘对象
board.GetMsg(current_pos).IsUnk = 0 #未探索点
open_list.append(current_pos) #把该点放入openlist中
while open_list != []:
#取出第一个位置（F最小，判定最优）
current_position = open_list[0]
open_list.remove(current_position)
if current_position == target_position:
print("成功找到解")
global tmp #内存储Position
while current_position != None:
tmp.append(current_position)
current_position = board.GetMsg(current_position).parent #回溯父节点
tmp.reverse() #反转列表，从而得到从起点到终点的正确顺序。
for i in tmp:
print(str(i.__dict__))
end_time = time.time() #计时
print('搜索用时：',end_time - start_time)
return
#将下一步可到达的位置加入open_list，并检查记录的最短路径G是否需要更新，记录最短路径经过的上一个点
for i in [current_position.x-1, current_position.x+1]:
for j in [current_position.y-1, current_position.y+1]:#分别在水平和垂直方向上遍历当前节点的相邻节点
if IsInBoard(i,j): #检查节点是否在棋盘内
new_G = board.GetMsg(current_position).G + 14
if board.mapx[i][j].IsUnk: #检查节点是否未被检查
board.mapx[i][j].IsUnk = 0 #状态改成已经被检查
open_list.append(Position(i,j)) #将周围待检查的点放入openlist中
board.mapx[i][j].parent = current_position #将他们的当前点设为父节点
board.mapx[i][j].G = new_G #更新周围待检查节点的G值
if board.mapx[i][j].G > new_G: #如果未遍历且G值大于从当前点到改点的代价值，则需要更新
board.mapx[i][j].parent = current_position #将他们的父节点设为当前点
board.mapx[i][j].G = new_G
#上下
j = current_position.y
for i in [current_position.x-1, current_position.x+1]:
if IsInBoard(i,j):
new_G = board.GetMsg(current_position).G + 10
if board.mapx[i][j].IsUnk:
board.mapx[i][j].IsUnk = 0 #把未遍历的点的状态改成遍历
open_list.append(Position(i,j))#放入openlsit中
board.mapx[i][j].parent = current_position #把当前点设为父节点
board.mapx[i][j].G = new_G
if board.mapx[i][j].G > new_G:
board.mapx[i][j].parent = current_position
board.mapx[i][j].G = new_G
#左右
i = current_position.x
for j in [current_position.y-1, current_position.y+1]:
if IsInBoard(i,j):
new_G = board.GetMsg(current_position).G + 10
if board.mapx[i][j].IsUnk:
board.mapx[i][j].IsUnk = 0
open_list.append(Position(i,j))
board.mapx[i][j].parent = current_position
board.mapx[i][j].G = new_G
if board.mapx[i][j].G > new_G:
board.mapx[i][j].parent = current_position
board.mapx[i][j].G = new_G
#open_list.sort(key=searchKey(board))
#对open_list里的内容按F的大小排序，从小到大
open_list.sort(key=lambda elem:board.GetMsg(elem).GetF()) #将各个待检查的节点都放入openlist中进行排序这段代码实现了A*算法的核心逻辑，通过维护开放列表和已探索节点，逐步探索地图，直到找到目标位置或开放列表为空（表示无法到达目标位置）。注释解释了该段代码的含义。
2.2 图像的处理
colored_map = np.zeros((mapp.shape[0], mapp.shape[1], 3), dtype=np.uint8)
colored_map[mapp == 1] = [0, 0, 0] # 障碍物 - 黑色
colored_map[mapp == 0] = [255, 255, 255] # 空白处 - 白色
colored_map[mapp == 3] = [0,255,127] # 路径 - 嫩绿色分别用黑色、白色和嫩绿色代表障碍物、空白处和算法的路径。
# 创建一个图形和子图
fig, ax = plt.subplots()
# 显示彩色图像
ax.imshow(colored_map)
#设置网格的宽度
ax.set_xticks(np.arange(0.5, mapp.shape[0], 1)) # 包括0和mapp.shape[0]-0.5
ax.set_yticks(np.arange(0.5, mapp.shape[1], 1))#range只生成整数，np.arange可以生成浮点数
# 添加网格线，alpha 设置网格线的透明度
ax.grid(which='major', axis='both', linestyle='-', linewidth=1, color='k', alpha=0.5)这段代码是使用Matplotlib库来创建和显示一个图像，并且为图像添加网格线。
fig, ax = plt.subplots(): 创建了一个图形（fig）和一个子图（ax）。在Matplotlib中，一个图形可以包含多个子图，但这里只创建了一个。
ax.imshow(colored_map): 使用子图对象ax来显示一个名为colored_map的彩色图像。imshow函数是Matplotlib中用于显示图像的函数。
ax.set_xticks(np.arange(0.5, mapp.shape[0], 1)): 设置了x轴上的刻度位置。np.arange函数生成从0.5开始到mapp.shape[0]结束（不包括结束值）的浮点数序列，步长为1。这样设置的目的是让网格线正好位于图像的像素边界上。
ax.set_yticks(np.arange(0.5, mapp.shape[1], 1)): 设置了y轴上的刻度位置。
ax.grid(which='major', axis='both', linestyle='-', linewidth=1, color='k', alpha=0.5): 添加了主要网格线。参数which='major'指定只添加主要网格线，axis='both'表示在x轴和y轴都添加网格线。linestyle='-'设置网格线为实线，linewidth=1设置网格线的宽度为1，color='k'设置网格线的颜色为黑色，alpha=0.5设置网格线的透明度为0.5（半透明）。
完整代码如下：
import cv2
import numpy as np
import matplotlib.pyplot as plt
import time
#场景布置--------------------------------------------------------------------------------------------------
#画出画布，方框
# 定义图像的大小
img_size = 401
grid_size = 20
# 创建白色背景的图像
img = np.ones((img_size, img_size, 3), np.uint8) * 255
#设置起点和终点
"""#可以采用键盘输入
print('请输入起始点横坐标（0到19）：')
sx = input()
print('请输入起始点纵坐标（0到19）：')
sy = input()
print('请输入目标点横坐标（0到19）：')
gx = input()
print('请输入目标点纵坐标（0到19）：')
gy = input()
"""
sx=0
sy=0
gx=19
gy=19
cv2.rectangle(img,(20*int(sx),20*int(sy)),(20+20*int(sx),20+20*int(sy)),(0,0,255),-1) #起点
cv2.rectangle(img,(20*int(gx),20*int(gy)),(20+20*int(gx),20+20*int(gy)),(255,255,0),-1)#终点
#增加黑色障碍物
cv2.rectangle(img,(60,60),(120,80),(0,0,0),-1)
cv2.rectangle(img,(180,80),(220,260),(0,0,0),-1)
cv2.rectangle(img,(240,220),(280,260),(0,0,0),-1)
cv2.rectangle(img,(60,180),(240,200),(0,0,0),-1)
cv2.rectangle(img,(280,260),(400,280),(0,0,0),-1)
# 绘制网格
for x in range(0, img_size, grid_size):
for y in range(0, img_size, grid_size):
# 绘制每个方框的边框
cv2.rectangle(img, (x, y), (x + grid_size, y + grid_size), (0, 0, 0), 1)
#显示图像
cv2.imshow('rectangle',img)
cv2.waitKey(0)
#将图像以图片的形式保存下来
# 保存图像的文件路径
save_path = 'a_star_tupian.png'
# 保存图像
cv2.imwrite(save_path, img)
print(f"图像已保存至：{save_path}")
# 读取栅格地图图像
input_path = 'a_star_tupian.png'
image = cv2.imread(input_path, cv2.IMREAD_GRAYSCALE) #image是Numpy数组形式
block_size = 20
num_block_x = image.shape[1] // block_size #列数/宽度--x
num_block_y = image.shape[0] // block_size #行数/高度--y
#初始化一个新的布尔数组来表示每个小块
block_grid = np.zeros((num_block_y, num_block_x), dtype=np.uint8) #np.zeros数组，将所有元素初始化为零
#遍历是否每个小块都是黑色
for by in range(num_block_y): #遍历行数
for bx in range(num_block_x): #遍历列数
# 计算小块在图像中的索引范围
y_start = by * block_size #block_size = 20
y_end = y_start + block_size
x_start = bx * block_size
x_end = x_start + block_size
# 检查小块内的所有像素是否都满足条件（例如全部为0）
block = image[y_start:y_end, x_start:x_end] #image是Numpy数组形式
is_block_black = np.all(block == 0) #如果所有的block都是0，那么得到的赋值是True
# 将结果赋值给 block_grid
block_grid[by, bx] = is_block_black #新生成的数组里面对应的点（dy,dx）的值是右边的值
# 打印新的栅格表示
print("栅格小块表示：")
print(block_grid)
#场景布置--------------------------------------------------------------------------------------------------
open_list=[]
class Position:
x = 0
y = 0
def __init__(self, X, Y):
self.x = X
self.y = Y
def __eq__(self, other): #等价性比较 __eq__
if other and self:
return self.x == other.x and self.y == other.y #比较两个点是否在相同的位置。
else:
return not (self or other)
#Position 类用于表示二维空间中的点
class Msg:
G = 0
H = 0
IsUnk = 1 #此处替代close_list功能，判断某点是否被搜索过
parent = None # Position
def __init__(self):
self.G = 0
self.H = 0
self.IsUnk = 1 #后续中会把探索过的点的IsUnk改为0
def GetF(self):
return self.G+self.H
#代价函数初始化 返回的是F代价
class Board:
mapx=[ ] #二维列表，用于存储地图上每个单元格的状态信息。每个单元格由 Msg 类的实例表示。
def __init__(self,mapp,target_position):
for i in range(len(mapp)):
self.mapx.append([])
for j in range(len(mapp[0])):
self.mapx[i].append(Msg())
self.mapx[i][j].IsUnk = 1 - mapp[i][j]
#如果mapp[i][j]=0说明可通行，IsUnk=1,表示未探索；反之说明是障碍物，已经探索
#曼哈顿方法计算H值
self.mapx[i][j].H = 10*(abs(target_position.x-i) + abs(target_position.y-j))
def GetMsg(self,pos):
return self.mapx[pos.x][pos.y]
def IsInBoard(i,j):
if i>=0 and i < len(mapp) and j>=0 and j<len(mapp[i]) and mapp[i][j] == 0:
return 1
else:
return 0
def SearchPath(mapp,current_pos,target_position):
start_time = time.time()
board = Board(mapp,target_position) #地图棋盘对象
board.GetMsg(current_pos).IsUnk = 0 #未探索点
open_list.append(current_pos) #把该点放入openlist中
while open_list != []:
#取出第一个位置（F最小，判定最优）
current_position = open_list[0]
open_list.remove(current_position)
if current_position == target_position:
print("成功找到解")
global tmp #内存储Position
while current_position != None:
tmp.append(current_position)
current_position = board.GetMsg(current_position).parent #回溯父节点
tmp.reverse() #反转列表，从而得到从起点到终点的正确顺序。
for i in tmp:
print(str(i.__dict__))
end_time = time.time() #计时
print('搜索用时：',end_time - start_time)
return
#将下一步可到达的位置加入open_list，并检查记录的最短路径G是否需要更新，记录最短路径经过的上一个点
for i in [current_position.x-1, current_position.x+1]:
for j in [current_position.y-1, current_position.y+1]:#分别在水平和垂直方向上遍历当前节点的相邻节点
if IsInBoard(i,j): #检查节点是否在棋盘内
new_G = board.GetMsg(current_position).G + 14
if board.mapx[i][j].IsUnk: #检查节点是否未被检查
board.mapx[i][j].IsUnk = 0 #状态改成已经被检查
open_list.append(Position(i,j)) #将周围待检查的点放入openlist中
board.mapx[i][j].parent = current_position #将他们的当前点设为父节点
board.mapx[i][j].G = new_G #更新周围待检查节点的G值
if board.mapx[i][j].G > new_G: #如果未遍历且G值大于从当前点到改点的代价值，则需要更新
board.mapx[i][j].parent = current_position #将他们的父节点设为当前点
board.mapx[i][j].G = new_G
#上下
j = current_position.y
for i in [current_position.x-1, current_position.x+1]:
if IsInBoard(i,j):
new_G = board.GetMsg(current_position).G + 10
if board.mapx[i][j].IsUnk:
board.mapx[i][j].IsUnk = 0 #把未遍历的点的状态改成遍历
open_list.append(Position(i,j))#放入openlsit中
board.mapx[i][j].parent = current_position #把当前点设为父节点
board.mapx[i][j].G = new_G
if board.mapx[i][j].G > new_G:
board.mapx[i][j].parent = current_position
board.mapx[i][j].G = new_G
#左右
i = current_position.x
for j in [current_position.y-1, current_position.y+1]:
if IsInBoard(i,j):
new_G = board.GetMsg(current_position).G + 10
if board.mapx[i][j].IsUnk:
board.mapx[i][j].IsUnk = 0
open_list.append(Position(i,j))
board.mapx[i][j].parent = current_position
board.mapx[i][j].G = new_G
if board.mapx[i][j].G > new_G:
board.mapx[i][j].parent = current_position
board.mapx[i][j].G = new_G
#open_list.sort(key=searchKey(board))
#对open_list里的内容按F的大小排序，从小到大
open_list.sort(key=lambda elem:board.GetMsg(elem).GetF()) #将各个待检查的节点都放入openlist中进行排序
#搜索完成后画图-----------------------------------------------------
if __name__ == '__main__':
global tmp
tmp = []
#定义初始状态
mapp = block_grid
start_position = Position(int(sy), int(sx))
target_position = Position(int(gy), int(gx))
SearchPath(mapp,start_position,target_position)
for i in range(len(tmp)):
mapp[tmp[i].x][tmp[i].y]=3
#转换颜色
colored_map = np.zeros((mapp.shape[0], mapp.shape[1], 3), dtype=np.uint8)
colored_map[mapp == 1] = [0, 0, 0] # 障碍物 - 黑色
colored_map[mapp == 0] = [255, 255, 255] # 空白处 - 白色
colored_map[mapp == 3] = [0,255,127] # 路径 - 嫩绿色
# 创建一个图形和子图
fig, ax = plt.subplots()
# 显示彩色图像
ax.imshow(colored_map)
#设置网格的宽度
ax.set_xticks(np.arange(0.5, mapp.shape[0], 1)) # 包括0和mapp.shape[0]-0.5
ax.set_yticks(np.arange(0.5, mapp.shape[1], 1))#range只生成整数，np.arange可以生成浮点数
# 添加网格线，alpha 设置网格线的透明度
ax.grid(which='major', axis='both', linestyle='-', linewidth=1, color='k', alpha=0.5)
# 隐藏坐标轴
# ax.axis('off')
#----------修改点3---------------------------------------
start_position = (int(sx), int(sy)) # 起点坐标
target_position = (int(gx), int(gy)) # 终点坐标
# 起点和终点的像素位置，这里简单地将坐标乘以一个缩放因子（例如 10），根据实际图像尺寸调整
start_x, start_y = start_position
target_x, target_y = target_position
plt.scatter([start_x * 1], [start_y * 1], c='red', s=50) # 起点
plt.scatter([target_x * 1], [target_y * 1], c='blue', s=50) # 终点
plt.show()
三、结果展示（静态结果）
下面是代码结果展示，障碍物和起点可以自己修改。
PS:关于A*算法的动态展示以及基于A*算法的全覆盖路径规划我会分别在下篇和下下篇文章展示，有兴趣的小伙伴欢迎前来！
（个人觉得本代码也有需要改进的地方，比如关闭列表我是用的 “IsUnk = 1和0”表示是否被探索过，而不是用的closed list（虽然也能跑）。欢迎各位大佬前来指点！ ）
2776
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
