参考：
https://blog.csdn.net/feriman/article/details/113725937
https://blog.csdn.net/m0_73366745/article/details/137113791?spm=1001.2014.3001.5506
https://mp.weixin.qq.com/s/g1sRR6tJ8qHhFzl-vrG5fw
A*算法
import sys
import time
from typing import List
import random
from PIL import Image, ImageDraw
import matplotlib.pyplot as plt"""
Point类是数学坐标系的一个抽象的点,和Node类不是一回事
"""class Point:def __init__(self, x, y) -> None:self.x = xself.y = y# 重载“==”运算符，(x1,y1)==(x2,y2)，当且仅当x1=x2，y1=y2def __eq__(self, other) -> bool:return self.x == other.x and self.y == other.yclass Node:def __init__(self, point: Point, endpoint: Point, g: float): # 初始化中间节点的参数self.point = pointself.endpoint = endpointself.father = Noneself.g = g# h取曼哈顿距离，c=|x2-x1|+|y2-y1|self.h = (abs(endpoint.x - point.x) + abs(endpoint.y - point.y)) * 10self.f = self.g + self.hdef get_near(self, ud, rl): # 获取相邻节点near_point = Point(self.point.x + rl, self.point.y + ud)near_node = Node(near_point, self.endpoint, self.g + (10 if ud == 0 or rl == 0 else 14))return near_nodeclass AStar:def __init__(self, start: Point, end: Point, map_data): # 初始化A*算法的参数self.path = [start, end]self.closed_list = []self.open_list = []self.start = startself.end = endself.obstacle_map = map_data# 从open_list里面找到一个代价最小的节点def select_current(self) -> Node:min_f = sys.maxsizenode_temp = Nonefor node in self.open_list:if node.f < min_f:min_f = node.fnode_temp = nodereturn node_tempdef is_in_open_list(self, node: Node) -> bool: # 判断节点是否在待检测队列中return any([open_node.point == node.point for open_node in self.open_list])def is_in_closed_list(self, node: Node) -> bool: # 判断节点是否在已检测队列中return any([closed_node.point == node.point for closed_node in self.closed_list])def is_obstacle(self, node: Node) -> bool:""" 验证机器人的当前位置是否合理 """# 检查当前位置是否在环境内if node.point.x < 0 or node.point.x > len(self.obstacle_map) - 1:return Trueif node.point.y < 0 or node.point.y > len(self.obstacle_map) - 1:return True# 检查当前位置是否处于障碍物中if self.obstacle_map[node.point.x][node.point.y] != 0:return Truereturn Falsedef explore_neighbors(self, current_node: Node) -> bool:up = (0, 1) # 上down = (0, -1) # 下right = (1, 0) # 右left = (-1, 0) # 左top_right = (1, 1) # 右上top_left = (-1, 1) # 左上Bottom_right = (1, -1) # 右下Bottom_left = (-1, -1) # 左下directions = [up, down, right, left, top_right, top_left, Bottom_right, Bottom_left]for direction in directions:ud, rl = direction# current_neighbor是当前节点的邻点current_neighbor = current_node.get_near(ud, rl)# 如果检测到的节点是终点，就没必要接着往下探索了，直接退出循环，结束这个函数if current_neighbor.point == self.end:return True# 判断一下邻点是不是已经检测或者是障碍物，如果是，就跳过这个邻点if self.is_in_closed_list(current_neighbor) or self.is_obstacle(current_neighbor):continueif self.is_in_open_list(current_neighbor):previous_current_neighbor = next(open_node for open_node in self.open_list if open_node.point == current_neighbor.point)if current_neighbor.f < previous_current_neighbor.f:# 更新父节点previous_current_neighbor.father = current_node# 更新g值previous_current_neighbor.g = current_neighbor.gelse:# 对应状态3，直接入队current_neighbor.father = current_nodeself.open_list.append(current_neighbor)return Falsedef find_path(self):start_node = Node(point=self.start, endpoint=self.end, g=0)self.open_list.append(start_node)while True:# 从open_list里面取出一个代价值最小节点current_node = self.select_current()if current_node is None:return []# 取出来后，从open_list里面删除，添加到closed_list里面self.open_list.remove(current_node)self.closed_list.append(current_node)# 当current_node是终点时，explore_neighbors函数会返回一个Trueif current_node.point == self.end or self.explore_neighbors(current_node):while current_node.father is not None:self.path.insert(1, current_node.point)# 这里其实就是相当于遍历一个链表current_node = current_node.fatherreturn self.pathif __name__ == "__main__":start_point = Point(0, 0)end_point = Point(38, 3)# 设置环境地图neighbors = [[0, 1], # 上[0, -1], # 下[-1, 0], # 左[1, 0], # 右[1, 1], # 右上[1, -1], # 右下[-1, -1], # 左下[-1, 1] # 左上]map_data = [[0] * 41 for i in range(41)]for i in range(20):map_data[5][i] = 1map_data[30][i] = 2map_data[i][25] = 1for node in [[20, 20], [30, 35], [10, 33]]:map_data[node[0]][node[1]] = 3for neighbor in neighbors:map_data[node[0] + neighbor[0]][node[1] + neighbor[1]] = 3# 运行A*算法start_time = time.time()a_star = AStar(start_point, end_point, map_data)path = a_star.find_path()end_time = time.time()print("程序运行时间：", end_time - start_time, "秒", f"路径长度为{len(path) - 1}")for i in range(41):for j in range(41):if map_data[i][j] != 0:plt.plot(i, j, '.k')plt.plot(start_point.x, start_point.y, 'og')plt.plot(end_point.x, end_point.y, 'or')# plt.grid('True')plt.axis('equal')rx = [path[i].x for i in range(len(path))]ry = [path[i].y for i in range(len(path))]plt.plot(rx, ry, '-r')plt.show()蚁群算法
import random
import mathclass Ant:def __init__(self, numAnts, numCities, maxIterations):self.numAnts = numAntsself.numCities = numCitiesself.maxIterations = maxIterationsself.evaporationRate = 0.5self.alpha = 1.0self.beta = 2.0self.pheromones = []def run(self, distances):random.seed()self.initialize_pheromones()for iter in range(self.maxIterations):antPaths = []for ant in range(self.numAnts):path = self.generate_ant_path(distances)antPaths.append(path)self.update_pheromones(path)self.evaporate_pheromones()bestPath = antPaths[0]# print(f"Iteration {iter + 1}: Best Path -> {' -> '.join(map(str, bestPath))}")return bestPathdef initialize_pheromones(self):global pheromonespheromones = [[1.0] * self.numCities for _ in range(self.numCities)]def generate_ant_path(self, distances):startCity = 0path = [startCity]while len(path) < self.numCities:currentCity = path[-1]nextCity = self.choose_next_city(currentCity, path, distances)path.append(nextCity)return pathdef choose_next_city(self, currentCity, path, distances):availableCities = [city for city in range(self.numCities) if city not in path]probabilities = []totalProbability = 0.0for nextCity in availableCities:pheromone = math.pow(pheromones[currentCity][nextCity], self.alpha)distance = 1.0 / distances[currentCity][nextCity]probability = pheromone * distanceprobabilities.append(probability)totalProbability += probabilityprobabilities = [probability / totalProbability for probability in probabilities]randomValue = random.random()cumulativeProbability = 0.0for i, probability in enumerate(probabilities):cumulativeProbability += probabilityif randomValue <= cumulativeProbability:return availableCities[i]return availableCities[-1]def update_pheromones(self, path):pheromoneDeposit = 1.0for i in range(len(path) - 1):currentCity = path[i]nextCity = path[i + 1]pheromones[currentCity][nextCity] += pheromoneDepositpheromones[nextCity][currentCity] += pheromoneDepositdef evaporate_pheromones(self):for i in range(self.numCities):for j in range(self.numCities):pheromones[i][j] *= (1.0 - self.evaporationRate)if __name__ == '__main__':distances = \[[0, 2, 5, 7],[2, 0, 6, 3],[5, 6, 0, 8],[7, 3, 8, 0]]ant = Ant(5, 4, 100)best_path = ant.run(distances)print(best_path)寻找多目标点最优路径
方式一 不适用蚁群算法
import timefrom matplotlib import pyplot as pltfrom A_star.A_Star import AStar, Pointdef test_multi_node_path():start_point = Point(0, 0)end_point = Point(38, 3)# 设置环境地图neighbors = [[0, 1], # 上[0, -1], # 下[-1, 0], # 左[1, 0], # 右[1, 1], # 右上[1, -1], # 右下[-1, -1], # 左下[-1, 1] # 左上]map_data = [[0] * 41 for i in range(41)]for i in range(20):map_data[5][i] = 1map_data[30][i] = 1map_data[i][25] = 1for node in [[20, 20], [30, 35], [10, 33]]:map_data[node[0]][node[1]] = 1for neighbor in neighbors:map_data[node[0] + neighbor[0]][node[1] + neighbor[1]] = 1import randomy_stones = [[random.randint(0, 40), random.randint(0, 40)] for i in range(3)]q_stones = [[random.randint(0, 40), random.randint(0, 40)] for i in range(3)]r_stones = [[random.randint(0, 40), random.randint(0, 40)] for i in range(3)]for i in range(3):map_data[y_stones[i][0]][y_stones[i][1]] = 3for i in range(3):map_data[q_stones[i][0]][q_stones[i][1]] = 4for i in range(3):map_data[r_stones[i][0]][r_stones[i][1]] = 5start_time = time.time()dis = float("inf")path = Nonefor i in range(3):for j in range(3):for k in range(3):ystone = Point(y_stones[i][0], y_stones[i][1])qstone = Point(q_stones[j][0], q_stones[j][1])rstone = Point(r_stones[k][0], r_stones[k][1])# 计算距离import itertoolsstone_com = list(itertools.permutations([ystone, qstone, rstone], 3))for com in stone_com:p1 = AStar(start_point, com[0], map_data).find_path()p2 = AStar(com[0], com[1], map_data).find_path()p3 = AStar(com[1], com[2], map_data).find_path()# p4 = AStar(com[2], end_point, map_data).find_path()# if len(p1) > 0 and len(p2) > 0 and len(p3) > 0 and len(p4) > 0:# tmp_dis = len(p1) + len(p2) + len(p3) + len(p4)# if tmp_dis < dis:# node_list = com# dis = tmp_dis# path = p1 + p2 + p3 + p4if len(p1) > 0 and len(p2) > 0 and len(p3) > 0:tmp_dis = len(p1) + len(p2) + len(p3)if tmp_dis < dis:node_list = comdis = tmp_dispath = p1 + p2 + p3end_time = time.time()print("程序运行时间：", end_time - start_time, "秒", f"路径长度为{len(path) - 1}")for i in range(41):for j in range(41):if map_data[i][j] == 1:plt.plot(i, j, '.k')if map_data[i][j] == 3:plt.plot(i, j, '.g')if map_data[i][j] == 4:plt.plot(i, j, '.r')if map_data[i][j] == 5:plt.plot(i, j, '.b')plt.plot(start_point.x, start_point.y, 'og')plt.plot(end_point.x, end_point.y, 'or')# plt.grid('True')plt.axis('equal')rx = [path[i].x for i in range(len(path))]ry = [path[i].y for i in range(len(path))]plt.plot(rx, ry, '-r')plt.show()if __name__ == "__main__":test_multi_node_path()方式二 使用蚁群算法
import timefrom matplotlib import pyplot as pltfrom A_star.A_Star import AStar, Pointdef test_multi_node_path():start_point = Point(0, 0)end_point = Point(38, 3)# 设置环境地图neighbors = [[0, 1], # 上[0, -1], # 下[-1, 0], # 左[1, 0], # 右[1, 1], # 右上[1, -1], # 右下[-1, -1], # 左下[-1, 1] # 左上]map_data = [[0] * 41 for i in range(41)]for i in range(20):map_data[5][i] = 1map_data[30][i] = 1map_data[i][25] = 1for node in [[20, 20], [30, 35], [10, 33]]:map_data[node[0]][node[1]] = 1for neighbor in neighbors:map_data[node[0] + neighbor[0]][node[1] + neighbor[1]] = 1import randomy_stones = [[random.randint(0, 40), random.randint(0, 40)] for i in range(3)]q_stones = [[random.randint(0, 40), random.randint(0, 40)] for i in range(3)]r_stones = [[random.randint(0, 40), random.randint(0, 40)] for i in range(3)]for i in range(3):map_data[y_stones[i][0]][y_stones[i][1]] = 3for i in range(3):map_data[q_stones[i][0]][q_stones[i][1]] = 4for i in range(3):map_data[r_stones[i][0]][r_stones[i][1]] = 5start_time = time.time()dis = float("inf")path = Nonefor i in range(3):for j in range(3):for k in range(3):ystone = Point(y_stones[i][0], y_stones[i][1])qstone = Point(q_stones[j][0], q_stones[j][1])rstone = Point(r_stones[k][0], r_stones[k][1])# 计算距离nodes = [start_point, ystone, qstone, rstone]distances = [[0] * 4 for i in range(4)]paths = {}for x in range(4):for y in range(x + 1, 4, 1):p = AStar(nodes[x], nodes[y], map_data).find_path()p_len = len(p)distances[x][y] = p_lendistances[y][x] = p_lenpaths[(x, y)] = ppaths[(y, x)] = pfrom ant import Antant = Ant(5, 4, 50)best_path = ant.run(distances)tmp_dis = distances[best_path[0]][best_path[1]] + distances[best_path[1]][best_path[2]] +\distances[best_path[2]][best_path[3]]if tmp_dis < dis:dis = tmp_dispath = paths[(best_path[0], best_path[1])] + paths[(best_path[1],best_path[2])] +\paths[(best_path[2], best_path[3])]# print(best_path)end_time = time.time()print("程序运行时间：", end_time - start_time, "秒", f"路径长度为{len(path) - 1}")for i in range(41):for j in range(41):if map_data[i][j] == 1:plt.plot(i, j, '.k')if map_data[i][j] == 3:plt.plot(i, j, '.g')if map_data[i][j] == 4:plt.plot(i, j, '.r')if map_data[i][j] == 5:plt.plot(i, j, '.b')plt.plot(start_point.x, start_point.y, 'og')plt.plot(end_point.x, end_point.y, 'or')# plt.grid('True')plt.axis('equal')rx = [path[i].x for i in range(len(path))]ry = [path[i].y for i in range(len(path))]plt.plot(rx, ry, '-r')plt.show()if __name__ == "__main__":test_multi_node_path()
