版本说明：
V1.0版本为只有固定上下左右四个方向进行规划路线。
V2.0版本对V1.0做出了提升，现在可以填入任意方向（至多8个方向）进行路线规划。
V3.0版本删除了sub和index的互相转换，减少了算法复杂度。优化了代码，增加可重复使用性。发现并改正了total_costs计算错误。
V3.0使用方法：
(1)
from 文件名 import A_star
a_star = A_star(matrix=[], step=1000, way=[“R”, “L”, “D”, “U”, “RU”, “RD”, “LU”, “LD”], wall=0)
road = a_star.run(start_point=[],end_point=[]) #可重复使用
或者直接
from 文件名 import A_star
road = A_star(matrix=[], step=1000, way=[“R”, “L”, “D”, “U”, “RU”, “RD”, “LU”, “LD”], wall=0).run(start_point=[],end_point=[])
输出为二位点坐标数组eg:[[10, 10], [9, 9], [8, 8], [7, 7], [6, 6], [5, 5], [4, 4], [3, 3], [2, 2], [1, 1], [0, 0]]
(2)参数说明：
start_point 为起始坐标.
end_point为终点坐标.
matrix为地图/迷宫地图，bool类型.
wall为matrix里True/False哪个为墙，False为墙写0，True为墙写1.
weight为权值（不用特别调整）.
Corner_amend为拐角优化，1开启，0关闭.
step表示运行一定循环后退出程序，防止因为地图过大无限运行。代码默认无限循环即step=float(“inf”)，推荐加入限制条件，减少特定程序时间.
way表示可以走的方向，eg：[“R”, “L”, “D”, “U”, “RU”, “RD”, “LU”, “LD”]。需要大写（因为没写upper操作）.
说明：
该代码为matlab改编成的python代码并经过我的相关优化，有一定参考性，在pycharm debug调试中你可以测试该算法逻辑，希望对你有帮助。
待更新：
无
如果代码有错误请和我联系，我会及时改正。
V3.0版本代码
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 |
import numpy as np from math import sqrt class A_star: def __init__(self, matrix, weights=1, corner_amend=1, step=float("inf"), way=["R", "L", "D", "U", "RU", "RD", "LU", "LD"], wall=0): self.matrix = matrix self.weights = weights self.corner_amend = corner_amend self.matrix_length = len(self.matrix[0]) self.matrix_width = len(self.matrix) self.step = step # 当你知道你的 way 配置保证问题的时候下面三行可进行更改，虽然没有加快多少速度 self.way = list(map(lambda x: x.upper(), way)) if len(set(self.way + ["R", "L", "D", "U", "RU", "RD", "LU", "LD"])) != 8: exit("Error way") self.wall = wall self.field = np.array(np.copy(self.matrix), dtype=float) for i in range(self.matrix_width): for j in range(self.matrix_length): if self.field[i][j] == self.wall: self.field[i][j] = float("inf") def run(self, start_point, end_point): self.fieldpointers = np.array(np.copy(self.matrix), dtype=str) self.start_point = start_point self.end_point = end_point if int(self.matrix[self.start_point[0]][self.start_point[1]]) == self.wall or int(self.matrix[self.end_point[0]][self.end_point[1]] == self.wall): exit("start or end is wall") self.fieldpointers[self.start_point[0]][self.start_point[1]] = "S" self.fieldpointers[self.end_point[0]][self.end_point[1]] = "G" return self.a_star() def a_star(self): setopen = [self.start_point] setopencosts = [0] setopenheuristics = [float("inf")] setclosed = [] setclosedcosts = [] movementdirections = self.way while self.end_point not in setopen and self.step: self.step -= 1 total_costs = list(np.array(setopencosts) + self.weights * np.array(setopenheuristics)) temp = np.min(total_costs) ii = total_costs.index(temp) if setopen[ii] != self.start_point and self.corner_amend == 1: new_ii = self.Path_optimization(temp, ii, setopen, setopencosts, setopenheuristics) ii = new_ii [costs, heuristics, posinds] = self.findFValue(setopen[ii], setopencosts[ii]) setclosed = setclosed + [setopen[ii]] setclosedcosts = setclosedcosts + [setopencosts[ii]] setopen.pop(ii) setopencosts.pop(ii) setopenheuristics.pop(ii) for jj in range(len(posinds)): if float("Inf") != costs[jj]: if not posinds[jj] in setclosed + setopen: self.fieldpointers[posinds[jj][0]][posinds[jj][1]] = movementdirections[jj] setopen = setopen + [posinds[jj]] setopencosts = setopencosts + [costs[jj]] setopenheuristics = setopenheuristics + [heuristics[jj]] elif posinds[jj] in setopen: position = setopen.index(posinds[jj]) if setopencosts[position] > costs[jj]: setopencosts[position] = costs[jj] setopenheuristics[position] = heuristics[jj] self.fieldpointers[setopen[position][0]][setopen[position][1]] = movementdirections[jj] else: position = setclosed.index(posinds[jj]) if setclosedcosts[position] > costs[jj]: setclosedcosts[position] = costs[jj] self.fieldpointers[setclosed[position][0]][setclosed[position][1]] = movementdirections[jj] if not setopen: exit("Can't") if self.end_point in setopen: rod = self.findWayBack(self.end_point) return rod else: exit("Can't") def Path_optimization(self, temp, ii, setOpen, setOpenCosts, setOpenHeuristics): [row, col] = setOpen[ii] _temp = self.fieldpointers[row][col] if "L" in _temp: col -= 1 elif "R" in _temp: col += 1 if "U" in _temp: row -= 1 elif "D" in _temp: row += 1 if [row, col] == self.start_point: new_ii = ii else: _temp = self.fieldpointers[row][col] [row2, col2] = [row, col] if "L" in _temp: col2 += 1 elif "R" in _temp: col2 -= 1 if "U" in _temp: row2 += 1 elif "D" in _temp: row2 -= 1 if 0 <= row2 <= self.matrix_width and 0 <= col2 <= self.matrix_length: new_ii = ii else: if self.fieldpointers[setOpen[ii][0]][setOpen[ii][1]] == self.fieldpointers[row][col]: new_ii = ii elif [row2, col2] in setOpen: untext_ii = setOpen.index([row2, col2]) now_cost = setOpenCosts[untext_ii] + self.weights * setOpenHeuristics[untext_ii] if temp == now_cost: new_ii = untext_ii else: new_ii = ii else: new_ii = ii return new_ii def findFValue(self, currentpos, costsofar): cost = [] heuristic = [] posinds = [] for way in self.way: if "D" in way: x = currentpos[0] - 1 elif "U" in way: x = currentpos[0] + 1 else: x = currentpos[0] if "R" in way: y = currentpos[1] - 1 elif "L" in way: y = currentpos[1] + 1 else: y = currentpos[1] if 0 <= y <= self.matrix_length - 1 and 0 <= x <= self.matrix_width - 1: posinds.append([x, y]) heuristic.append(sqrt((self.end_point[1] - y) ** 2 + (self.end_point[0] - x) ** 2)) cost.append(costsofar + self.field[x][y]) else: posinds.append([0, 0]) heuristic.append(float("inf")) cost.append(float("inf")) return [cost, heuristic, posinds] def findWayBack(self, goal): road = [goal] [x, y] = goal while self.fieldpointers[x][y] != "S": temp = self.fieldpointers[x][y] if "L" in temp: y -= 1 if "R" in temp: y += 1 if "U" in temp: x -= 1 if "D" in temp: x += 1 road.append([x, y]) return road if __name__ == "__main__": print(A_star(matrix=[[True for i in range(1000)] for j in range(1000)]).run(start_point=[0, 0], end_point=[999, 999])) |
A*算法可视化参考代码
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 |
from PIL import Image import numpy as np import os import imageio # 删除文件夹下原有图片 list_dir = os.listdir() for i in range(len(list_dir)): if "visualization" in list_dir[i] and ".jpg" in list_dir[i]: os.remove(list_dir[i]) # 合成 picture 文件夹下图片成jpg， fps为帧率， between为多少张图片选一张（防止图片过多） def output_gif(fps, between): image_path = os.listdir("./picture") gif_image = [] i = 0 while 1: if "picture{}.jpg".format(i) in image_path: gif_image.append(imageio.imread("./picture/picture{}.jpg".format(i))) else: break i += between imageio.mimsave("test.gif", gif_image, fps=fps) # location 为原图片， road为A*算法找到的路，如果要输出A*算法每步的图片，建议把im = Image.open(location)设置成全局变量，加快运行速度 def visualize(location, road): filenames = os.listdir() count = 0 if "visualization0.jpg" in filenames: for filename in filenames: if "visualization" in filename and ".jpg" in filename and "Edge" not in filename: now = int(filename.split(".")[-2][13:]) count = count if count > now else now + 1 name = "visualization{}.jpg".format(count) im = Image.open(location) for i in road: pos = (i[1], i[0]) im.putpixel(pos, (255, 0, 0)) im.save(name) |
下面为历史版本代码：
Read More
浙公网安备 33010902002953号