基于粒子群优化（Particle Swarm Optimization, PSO）算法的无人机路径的避障规划。
random: 用于生成随机数，比如初始化粒子的位置和速度。
numpy (np): 用于数学计算，如向量运算和距离计算。
matplotlib.pyplot (plt): 用于绘制图形。
mpl_toolkits.mplot3d.Axes3D 和 mpl_toolkits.mplot3d.art3d.Poly3DCollection, pathpatch_2d_to_3d: 用于在3D空间中绘制图形。
import random
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from mpl_toolkits.mplot3d.art3d import Poly3DCollection, pathpatch_2d_to_3d
class Drone:
def __init__(self, start_pos, target_pos):
self.start_pos = start_pos
self.target_pos = target_pos
class PSO:
def __init__(self, drone, nPop, MaxIt, w, c1, c2, obstacles):
self.drone = drone
self.nPop = nPop
self.MaxIt = MaxIt
self.w = w
self.c1 = c1
self.c2 = c2
self.obstacles = obstacles
self.global_best = {'position': None, 'cost': float('inf')}
self.particles = self.init_particles()
def init_particles(self):
""" 初始化粒子群 """
particles = []
for _ in range(self.nPop):
position = [(random.uniform(0, 10), random.uniform(0, 10), random.uniform(0, 10)) for _ in range(10)]
velocity = [(random.uniform(-1, 1), random.uniform(-1, 1), random.uniform(-1, 1)) for _ in range(10)]
cost = self.cost_function(position)
particles.append({
'position': position,
'velocity': velocity,
'cost': cost,
'best_position': position.copy(),
'best_cost': cost
})
if cost < self.global_best['cost']:
self.global_best['position'] = position.copy()
self.global_best['cost'] = cost
return particles
def cost_function(self, path):
""" 计算成本，即路径长度和障碍物碰撞惩罚 """
total_distance = 0
collision_penalty = 0
prev_pos = self.drone.start_pos
for pos in path:
total_distance += np.sqrt((pos[0] - prev_pos[0])**2 + (pos[1] - prev_pos[1])**2 + (pos[2] - prev_pos[2])**2)
for obstacle in self.obstacles:
if self.is_collision(pos, obstacle):
collision_penalty += 1000 高惩罚值以避免碰撞
prev_pos = pos
total_distance += np.sqrt((self.drone.target_pos[0] - prev_pos[0])**2 +
(self.drone.target_pos[1] - prev_pos[1])**2 +
(self.drone.target_pos[2] - prev_pos[2])**2)
return total_distance + collision_penalty
def is_collision(self, pos, obstacle):
""" 检查是否与障碍物碰撞 """
x, y, z = pos
center_x, center_y, center_z, radius, height = obstacle
if (x - center_x)**2 + (y - center_y)**2 <= radius**2 and center_z <= z <= center_z + height:
return True
return False
def update_velocity(self, particle):
""" 更新速度 """
new_velocity = []
for i in range(len(particle['position'])):
r1, r2 = random.random(), random.random()
cognitive = self.c1 * r1 * (np.array(particle['best_position'][i]) - np.array(particle['position'][i]))
social = self.c2 * r2 * (np.array(self.global_best['position'][i]) - np.array(particle['position'][i]))
new_velocity.append((self.w * particle['velocity'][i][0] + cognitive[0] + social[0],
self.w * particle['velocity'][i][1] + cognitive[1] + social[1],
self.w * particle['velocity'][i][2] + cognitive[2] + social[2]))
return new_velocity
def update_position(self, particle):
""" 更新位置 """
new_position = []
for i in range(len(particle['position'])):
new_x = particle['position'][i][0] + particle['velocity'][i][0]
new_y = particle['position'][i][1] + particle['velocity'][i][1]
new_z = particle['position'][i][2] + particle['velocity'][i][2]
new_position.append((new_x, new_y, new_z))
return new_position
def run(self):
""" 运行PSO """
for it in range(self.MaxIt):
for particle in self.particles:
particle['velocity'] = self.update_velocity(particle)
particle['position'] = self.update_position(particle)
cost = self.cost_function(particle['position'])
particle['cost'] = cost
if cost < particle['best_cost']:
particle['best_position'] = particle['position'].copy()
particle['best_cost'] = cost
if cost < self.global_best['cost']:
self.global_best['position'] = particle['position'].copy()
self.global_best['cost'] = cost
self.w *= 0.98 减小惯性权重
return self.global_best['position'], self.global_best['cost']
def plot_3d_paths(paths, start_pos, target_pos, obstacles):
fig = plt.figure(figsize=(10, 10))
ax = fig.add_subplot(111, projection='3d')
绘制地表
X, Y = np.meshgrid(np.linspace(0, 10, 100), np.linspace(0, 10, 100))
Z = np.zeros_like(X)
ax.plot_surface(X, Y, Z, alpha=0.3, color='lightblue')
绘制路径
colors = ['b', 'g', 'r', 'c', 'm', 'y', 'k']
for i, (best_path, best_distance) in enumerate(paths):
x_path = [start_pos[0]] + [pos[0] for pos in best_path] + [target_pos[0]]
y_path = [start_pos[1]] + [pos[1] for pos in best_path] + [target_pos[1]]
z_path = [start_pos[2]] + [pos[2] for pos in best_path] + [target_pos[2]]
ax.plot(x_path, y_path, z_path, f'{colors[i % len(colors)]}-o', label=f'Path {i+1} (Dist: {best_distance:.2f})')
绘制起始点和目标点
ax.scatter(start_pos[0], start_pos[1], start_pos[2], label='Start', color='green')
ax.scatter(target_pos[0], target_pos[1], target_pos[2], label='Target', color='red')
绘制障碍物
for obstacle in obstacles:
center_x, center_y, center_z, radius, height = obstacle
circle = plt.Circle((center_x, center_y), radius, color='gray', alpha=0.5)
ax.add_patch(circle)
pathpatch_2d_to_3d(circle, z=center_z, zdir="z")
pathpatch_2d_to_3d(circle, z=center_z + height, zdir="z")
绘制柱状体侧面
for z in np.linspace(center_z, center_z + height, 10):
circle = plt.Circle((center_x, center_y), radius, color='gray', alpha=0.5)
pathpatch_2d_to_3d(circle, z=z, zdir="z")
ax.add_patch(circle)
ax.set_xlim(-1, 12)
ax.set_ylim(-1, 12)
ax.set_zlim(-1, 12)
ax.set_xlabel('X Position')
ax.set_ylabel('Y Position')
ax.set_zlabel('Z Position')
ax.set_title('Drone Path Planning using PSO in 3D')
ax.legend()
plt.show()
if __name__ == "__main__":
start_pos = (0, 0, 0) 无人机起始位置
target_pos = (10, 10, 10) 目标位置
定义障碍物，格式为 (中心x, 中心y, 中心z, 半径, 高度)
num_obstacles = 50 增加障碍物数量
obstacles = [
(random.uniform(1, 9), random.uniform(1, 9), 0, 0.1, 10) for _ in range(num_obstacles)
]
drone = Drone(start_pos, target_pos)
num_paths = 7 路径数量
paths = []
for i in range(num_paths):
将已规划的路径作为新的障碍物
for j in range(i):
for pos in paths[j][0]:
obstacles.append((pos[0], pos[1], pos[2], 0.1, 0.1))
pso = PSO(drone, nPop=100, MaxIt=500, w=1, c1=1.5, c2=1.5, obstacles=obstacles)
best_path, best_distance = pso.run()
paths.append((best_path, best_distance))
可视化结果
plot_3d_paths(paths, start_pos, target_pos, obstacles)
