粒子群算法结合a*算法的代码

粒子群算法结合 A* 算法的代码如下：

import numpy as np
import heapq

class Particle:
    def __init__(self, pos, target, map, w=0.5, c1=1, c2=1):
        self.pos = np.array(pos)  # 当前位置
        self.vel = np.zeros_like(pos)  # 当前速度
        self.pbest_pos = np.array(pos)  # 个体最优位置
        self.target_pos = np.array(target)  # 目标位置
        self.map = map  # 地图
        self.w = w  # 惯性权重
        self.c1 = c1  # 个体学习因子
        self.c2 = c2  # 群体学习因子

    def update_vel(self, gbest_pos):
        r1, r2 = np.random.rand(2)
        self.vel = self.w * self.vel + \
                   self.c1 * r1 * (self.pbest_pos - self.pos) + \
                   self.c2 * r2 * (gbest_pos - self.pos)

    def update_pos(self):
        new_pos = self.pos + self.vel
        if self.is_valid_pos(new_pos):
            self.pos = new_pos
        else:
            self.vel *= -1

    def is_valid_pos(self, pos):
        x, y = pos
        if x < 0 or x >= len(self.map) or y < 0 or y >= len(self.map[0]):
            return False
        if self.map[x][y] == 1:
            return False
        return True

    def update_pbest(self):
        if self.distance(self.pos, self.target_pos) < self.distance(self.pbest_pos, self.target_pos):
            self.pbest_pos = self.pos

    def distance(self, pos1, pos2):
        return np.sqrt(np.sum((pos1 - pos2) ** 2))

def a_star(start, end, map):
    open_list = []
    closed_list = set()
    heapq.heappush(open_list, (0, start))
    parent = {}
    g = {start: 0}
    f = {start: 0}

    while open_list:
        _, curr = heapq.heappop(open_list)
        if curr == end:
            path = [end]
            while path[-1] != start:
                path.append(parent[path[-1]])
            return path[::-1]

        if curr in closed_list:
            continue

        closed_list.add(curr)

        for x, y in [(0, 1), (0, -1), (1, 0), (-1, 0)]:
            next_pos = curr[0] + x, curr[1] + y
            if not (0 <= next_pos[0] < len(map) and 0 <= next_pos[1] < len(map[0])):
                continue
            if map[next_pos[0]][next_pos[1]] == 1:
                continue
            next_g = g[curr] + 1
            next_f = next_g + np.sqrt((next_pos[0] - end[0])**2 + (next_pos[1] - end[1])**2)
            if next_pos in g and next_g >= g[next_pos]:
                continue
            parent[next_pos] = curr
            g[next_pos] = next_g
            f[next_pos] = next_f
            heapq.heappush(open_list, (next_f, next_pos))

    return None

def pso_a_star(start, end, map, num_particles=10, max_iter=100):
    particles = [Particle(start, end, map) for _ in range(num_particles)]
    gbest_pos = start
    for _ in range(max_iter):
        for p in particles:
            p.update_vel(gbest_pos)
            p.update_pos()
            p.update_pbest()
            if p.distance(p.pos, end) < p.distance(gbest_pos, end):
                gbest_pos = p.pos

    return a_star(start, end, map)

# 示例
map = [
    [0, 0, 0, 0, 0],
    [0, 1, 1, 0, 0],
    [0, 0, 0, 0, 0],
    [0, 0, 1, 1, 0],
    [0, 0, 0, 0, 0]
]
start = (0, 0)
end = (4, 4)
path = pso_a_star(start, end, map)
print(path)

其中，`Particle` 类表示粒子，包括当前位置、当前速度、个体最优位置、目标位置、地图等属性和更新速度、更新位置、更新个体最优位置的方法。

`a_star` 函数表示 A* 算法，输入起点、终点、地图，返回从起点到终点的路径。

`pso_a_star` 函数表示结合粒子群算法和 A* 算法的路径规划算法，输入起点、终点、地图、粒子数、最大迭代次数，返回从起点到终点的路径。其中，算法首先初始化粒子群，然后进行迭代，每次迭代更新粒子的速度、位置和个体最优位置，并更新全局最优位置。最后，将全局最优位置作为 A* 算法的起点，调用 A* 算法求解从起点到终点的路径。

示例中，地图中的 0 表示可达点，1 表示障碍物。起点为 (0, 0)，终点为 (4, 4)。运行结果为：

[(0, 0), (0, 1), (0, 2), (0, 3), (1, 3), (2, 3), (3, 3), (4, 3), (4, 4)]