import numpy as np
from scipy import interpolate
import matplotlib.pyplot as plt
import pylab as mpl
mpl.rcParams['font.sans-serif'] = ['SimHei']
# 地图模型类
class Model:
def __init__(self, start, target, bound, obstacle, n, vpr=0.1):
[self.xs, self.ys], [self.xt, self.yt] = start, target
[[self.xmin, self.xmax], [self.ymin, self.ymax]] = bound
self.vxmax, self.vxmin = vpr * (self.xmax - self.xmin), -vpr * (self.xmax - self.xmin)
self.vymax, self.vymin = vpr * (self.ymax - self.ymin), -vpr * (self.ymax - self.ymin)
self.xobs = np.array([obs[0] for obs in obstacle])
self.yobs = np.array([obs[1] for obs in obstacle])
self.robs = np.array([obs[2] for obs in obstacle])
self.n = n
def straight_path(self):
return np.linspace(self.xs, self.xt, self.n + 2)[1:-1], np.linspace(self.ys, self.yt, self.n + 2)[1:-1]
def random_path(self):
return np.random.uniform(self.xmin, self.xmax, self.n), np.random.uniform(self.ymin, self.ymax, self.n)
def random_velocity(self):
return np.random.uniform(self.vxmin, self.vxmax, self.n), np.random.uniform(self.vymin, self.vymax, self.n)
# 路径规划和碰撞检测
def plan_path(x, y, model):
XS = np.insert(np.array([model.xs, model.xt]), 1, x)
YS = np.insert(np.array([model.ys, model.yt]), 1, y)
TS = np.linspace(0, 1, model.n + 2)
tt = np.linspace(0, 1, 200) #将路径等切为200段，减少因段数较少导致部分段的欧几里得距离穿过障碍物
f1 = interpolate.UnivariateSpline(TS, XS, s=0)
f2 = interpolate.UnivariateSpline(TS, YS, s=0)
xx, yy = f1(tt), f2(tt)
dx, dy = np.diff(xx), np.diff(yy)
L = np.sum(np.sqrt(dx ** 2 + dy ** 2))
d = np.sqrt((xx[:, None] - model.xobs) ** 2 + (yy[:, None] - model.yobs) ** 2)
violation = np.sum(np.clip(1 - d / model.robs, 0, None).mean(axis=0))
return xx, yy, L, violation
# 画图函数
def draw_path(model, x, y, xx, yy, title='路径规划'):
plt.title(title)
plt.scatter(model.xs, model.ys, label='起点', marker='o', color='red')
plt.scatter(model.xt, model.yt, label='终点', marker='*', color='green')
theta = np.linspace(0, 2 * np.pi, 100)
for i in range(len(model.robs)):
plt.fill(model.xobs[i] + model.robs[i] * np.cos(theta),
model.yobs[i] + model.robs[i] * np.sin(theta), 'gray')
plt.scatter(x, y, label='决策点', marker='x', color='blue')
plt.plot(xx, yy, label='路径')
plt.legend()
plt.grid()
plt.axis('equal')
plt.show()
# PSO路径规划（替换优化策略和方式）
def pso_path_planning(model, T=200, swarm_size=100, w=0.7, c1_start=2.5, c1_end=0.5, c2_start=0.5, c2_end=2.5, clone_rate=0.1, mutation_rate=0.05):
# 初始化种群
swarm = []
for i in range(swarm_size):
if i == 0:
x, y = model.straight_path()
else:
x, y = model.random_path()
vx, vy = model.random_velocity()
xx, yy, L, violation = plan_path(x, y, model)
swarm.append({
'position': np.array([x, y]),
'velocity': np.array([vx, vy]),
'best_position': np.array([x.copy(), y.copy()]),
'cost': L * (1 + violation * 100),
'is_feasible': violation == 0
})
gbest = sorted(swarm, key=lambda p: p['cost'])[0]
# 记录全局最佳历史
best_costs_history = []
# PSO迭代
for t in range(T):
# 非线性更新c1和c2
c1 = c1_start - (c1_start - c1_end) * (1 - np.cos(np.pi * t / (2 * T)))
c2 = c2_start + (c2_end - c2_start) * (1 - np.cos(np.pi * t / (2 * T)))
# 更新粒子速度和位置
for particle in swarm:
# 更新速度和位置
r1, r2 = np.random.rand(), np.random.rand()
particle['velocity'] = w * particle['velocity'] + \
c1 * r1 * (particle['best_position'] - particle['position']) + \
c2 * r2 * (gbest['position'] - particle['position'])
# 速度限制
particle['velocity'] = np.clip(particle['velocity'],
np.array([model.vxmin, model.vymin])[:, None],
np.array([model.vxmax, model.vymax])[:, None])
# 更新位置
particle['position'] += particle['velocity']
# 位置限制
particle['position'][0] = np.clip(particle['position'][0], model.xmin, model.xmax)
particle['position'][1] = np.clip(particle['position'][1], model.ymin, model.ymax)
# 计算新路径
xx, yy, L, violation = plan_path(particle['position'][0], particle['position'][1], model)
# 更新个人最佳
new_cost = L * (1 + violation * 100)
if new_cost < particle['cost']:
particle['best_position'] = particle['position'].copy()
particle['cost'] = new_cost
particle['is_feasible'] = violation == 0
# 更新全局最佳
if particle['cost'] < gbest['cost']:
gbest = particle.copy()
# 免疫克隆和突变操作
fitness_values = np.array([particle['cost'] for particle in swarm])
sorted_indices = np.argsort(fitness_values)
c_num = int(swarm_size * clone_rate)
cloned_swarm = []
for idx in sorted_indices[:c_num]:
cloned_particle = {
'position': swarm[idx]['position'].copy(),
'velocity': swarm[idx]['velocity'].copy(),
'best_position': swarm[idx]['best_position'].copy(),
'cost': swarm[idx]['cost'],
'is_feasible': swarm[idx]['is_feasible']
}
cloned_swarm.append(cloned_particle)
for particle in cloned_swarm:
if np.random.rand() < mutation_rate:
for dim in range(2): # 对x和y两个维度进行突变
mutation_idx = np.random.randint(0, model.n)
particle['position'][dim][mutation_idx] += np.random.uniform(-0.5, 0.5)
particle['position'][dim] = np.clip(particle['position'][dim],
[model.xmin if dim == 0 else model.ymin],
[model.xmax if dim == 0 else model.ymax])
# 重新计算速度（可选）
particle['velocity'][dim][mutation_idx] = np.random.uniform(
model.vxmin if dim == 0 else model.vymin,
model.vxmax if dim == 0 else model.vymax
)
# 随机扰动
particle['position'][dim] += np.random.uniform(-0.1, 0.1, size=model.n)
particle['position'][dim] = np.clip(particle['position'][dim],
[model.xmin if dim == 0 else model.ymin],
[model.xmax if dim == 0 else model.ymax]
)
swarm.extend(cloned_swarm)
# 记录当前最佳成本
best_costs_history.append(gbest['cost'])
print(f"第{t + 1}代: cost={gbest['cost']:.2f}")
return gbest, best_costs_history
if __name__ == '__main__':
# 创建模型
start = [0.0, 0.0]
target = [5.0, 4.0]
bound = [[-10.0, 10.0], [-10.0, 10.0]]
obstacle = [[1.5, 4.5, 1.3], [4.0, 3.0, 1.0], [1.2, 1.5, 0.8]]
model = Model(start, target, bound, obstacle, 3)
# PSO路径规划
gbest, costs = pso_path_planning(model)
# 绘制结果
xx, yy, _, _ = plan_path(gbest['position'][0], gbest['position'][1], model)
draw_path(model, gbest['position'][0], gbest['position'][1], xx, yy)
plt.plot(costs)
plt.title('The shortest path varies with the number of iterations')
plt.xlabel('Number of iterations')
plt.ylabel('Shortest path')
plt.grid()
plt.show() 我提供的是改进的PSO算法在二维层面的避障，请将算法拓展到三维层面（障碍物包括一个圆柱体和两个球体），目标点设置为（400，200，300）
最新发布
本文介绍了一种通过命令提示符批量注册Windows系统中所有DLL文件的方法。只需运行特定的CMD命令，即可实现DLL文件的自动注册，提高系统维护效率。
扫一扫
631
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
