路径规划 粒子群算法python

粒子群算法（Particle Swarm Optimization, PSO）是一种基于群体智能的优化算法，用于求解优化问题，其中每个解被称为一个粒子。在路径规划中，PSO可以用于寻找最优路径。

下面是一个使用Python实现粒子群算法解决路径规划问题的示例代码：

import numpy as np

# 定义问题
start_point = (0, 0)  # 起点坐标
end_point = (10, 10)  # 终点坐标
obstacles = [(2, 2), (3, 3), (5, 6)]  # 障碍物坐标

# 定义粒子群算法的参数
num_particles = 50  # 粒子数量
max_iter = 100  # 最大迭代次数
w = 0.5  # 惯性权重
c1 = 2.0  # 自我认知参数
c2 = 2.0  # 社会经验参数

# 定义目标函数
def fitness_function(position):
    # 计算位置position的适应度值，表示到达终点的距离
    distance = np.sqrt((position[0] - end_point[0]) ** 2 + (position[1] - end_point[1]) ** 2)
    return distance

# 初始化粒子群
particles = np.zeros((num_particles, 2))  # 粒子位置矩阵
velocities = np.zeros((num_particles,2))  # 粒子速度矩阵
best_positions = np.zeros((num_particles, 2))  # 粒子的最佳位置矩阵
best_fitness_values = np.ones(num_particles) * np.inf  # 粒子的最佳适应度值
# 初始化全局最佳位置和适应度值
global_best_position = np.zeros(2)
global_best_fitness_value = np.inf

# 迭代寻找最优解
for iteration in range(max_iter):
    for i in range(num_particles):
        # 更新速度
        velocities[i] = w * velocities[i] + c1 * np.random.rand() * (best_positions[i] - particles[i]) + \
                        c2 * np.random.rand() * (global_best_position - particles[i])
        # 更新位置
        particles[i] += velocities[i]
        
        # 判断位置是否越界
        particles[i][0] = max(min(particles[i][0], end_point[0]), start_point[0])
        particles[i][1] = max(min(particles[i][1], end_point[1]), start_point[1])
        
        # 判断位置是否撞到障碍物
        for obstacle in obstacles:
            if particles[i][0] == obstacle[0] and particles[i][1] == obstacle[1]:
                particles[i] = best_positions[i]  # 返回到粒子的最佳位置
        
        # 计算适应度值
        fitness_value = fitness_function(particles[i])
        
        # 更新粒子的最佳位置和适应度值
        if fitness_value < best_fitness_values[i]:
            best_positions[i] = particles[i]
            best_fitness_values[i] = fitness_value
        
        # 更新全局最佳位置和适应度值
        if fitness_value < global_best_fitness_value:
            global_best_position = particles[i]
            global_best_fitness_value = fitness_value

# 输出结果
print("最优路径：", global_best_position)
print("最优适应度值：", global_best_fitness_value)

以上代码实现了使用粒子群算法进行路径规划的基本过程。你可以根据具体的问题进行修改和扩展。注意，以上代码仅为示例，可能需要根据实际情况进行调整和优化。