粒子群算法路径规划 多 源码

下面是一个基于粒子群算法的路径规划的Python示例代码，可以实现多起点多终点的路径规划。

import numpy as np
import matplotlib.pyplot as plt

# 地图大小
map_size = 100

# 障碍物坐标
obstacles = [(20, 20), (30, 30), (40, 40)]

# 目标点坐标
goals = [(80, 80), (90, 90)]

# 粒子数
num_particles = 50

# 迭代次数
num_iterations = 100

# 惯性权重
w = 0.7

# 个体学习因子
c1 = 1.4

# 社会学习因子
c2 = 1.4

class Particle:
    def __init__(self):
        self.position = np.array([np.random.randint(map_size), np.random.randint(map_size)])
        self.velocity = np.zeros(2)
        self.best_position = self.position.copy()
        self.best_cost = np.inf

    def update_position(self):
        self.position += self.velocity
        if self.position[0] < 0:
            self.position[0] = 0
        elif self.position[0] >= map_size:
            self.position[0] = map_size - 1
        if self.position[1] < 0:
            self.position[1] = 0
        elif self.position[1] >= map_size:
            self.position[1] = map_size - 1

    def evaluate(self):
        cost = np.inf
        for goal in goals:
            dist = np.sqrt(np.sum((self.position - goal)**2))
            if dist < cost:
                cost = dist
        for obstacle in obstacles:
            dist = np.sqrt(np.sum((self.position - obstacle)**2))
            if dist < 1:
                cost = np.inf
                break
        if cost < self.best_cost:
            self.best_position = self.position.copy()
            self.best_cost = cost
        return cost

class PSO:
    def __init__(self):
        self.particles = [Particle() for _ in range(num_particles)]
        self.global_best_position = np.array([np.inf, np.inf])
        self.global_best_cost = np.inf

    def update(self):
        for i, particle in enumerate(self.particles):
            r1 = np.random.rand(2)
            r2 = np.random.rand(2)
            particle.velocity = w * particle.velocity + \
                                c1 * r1 * (particle.best_position - particle.position) + \
                                c2 * r2 * (self.global_best_position - particle.position)
            particle.update_position()
            cost = particle.evaluate()
            if cost < self.global_best_cost:
                self.global_best_position = particle.position.copy()
                self.global_best_cost = cost

    def run(self):
        costs = []
        for i in range(num_iterations):
            self.update()
            costs.append(self.global_best_cost)
            print(f"Iteration {i+1}: Best Cost = {self.global_best_cost:.2f}")
        return costs

if __name__ == "__main__":
    pso = PSO()
    costs = pso.run()
    plt.plot(costs)
    plt.xlabel("Iteration")
    plt.ylabel("Best Cost")
    plt.title("PSO for Path Planning")
    plt.show()

在这个示例代码中，我们使用了一个Particle类来表示每个粒子，其中包括其位置、速度以及历史最佳位置和最佳代价。在PSO类中，我们初始化了一组粒子，并在每次迭代中更新其速度和位置，并计算粒子的当前代价，然后将其与历史最佳代价进行比较。最终，我们输出了最佳代价的变化情况。