改进粒子群算法二维平面路径规划
- 一. 前言
- 二. 模型介绍
- 三. 算法改进
- 四. 执行代码
- 五. 总结
一. 前言
路径规划是运动规划的主要研究内容之一。运动规划由路径规划和轨迹规划组成,连接起点位置和终点位置的序列点或曲线称之为路径,构成路径的策略称之为路径规划。
路径规划在很多领域都具有广泛的应用。在高新科技领域的应用有:机器人的自主无碰行动;无人机的避障突防飞行;巡航导弹躲避雷达搜索、防反弹袭击、完成突防爆破任务等。在日常生活领域的应用有:GPS导航;基于GIS系统的道路规划;城市道路网规划导航等。在决策管理领域的应用有:物流管理中的车辆问题(VRP)及类似的资源管理资源配置问题。通信技术领域的路由问题等。凡是可拓扑为点线网络的规划问题基本上都可以采用路径规划的方法解决。
二. 模型介绍
- 粒子群算法是由Eberhart 和Kennedy于1995年提出的一种全局搜索算法,是一种模拟自然界的生物活动以及群体智能的随机搜索算法。除了考虑模拟生物的群体活动之外,融入了个体认知和社会影响,是一种群体智能算法。
粒子群算法初始化为一群随机的粒子(随机解),然后根据迭代找到最优解。每一次迭代中,粒子通过跟踪两个极值来更新自己:第1个是粒子本身所找到的最优解,这个称为个体极值;第2个是整个种群目前找到的最优解,这个称为全局极值。也可以不用整个种群,而是用其中的一部分作为粒子的邻居,称为局部极值。
PSO的算法步骤:
(1)初始化所有粒子、初始化它们的速度和位置,并且将粒子的历史最优pBest设为当前位置,而群体中最优的粒子作为当前的gBest。
(2)在每一次迭代中,计算各个粒子的适应度函数值。
(3)如果该粒子当前的适应度函数值比历史最优值好,那么历史最优将会被当前位置所替代。
(4)如果该粒子的历史最优比全局最优好,全局最优将会被粒子的历史最优所替代。
(5)对每个粒子i的第n维的速度和位置分别按照给定公式进行更新。
(6)如果未满足结束条件,则转到(2),否则输出gBest并结束。
三. 算法改进
标准粒子群算法虽然收敛速度很快,但是容易陷入局部最优,所以在这里采用具有线性变换的惯性权重系数和加速常数的改进粒子群算法,能有效克制其出现的问题。
四. 执行代码
import numpy as np
from scipy import interpolate
import copy
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] = start
[self.xt, self.yt] = target
[[self.xmin, self.xmax], [self.ymin, self.ymax]] = bound
self.vxmax = vpr * (self.xmax - self.xmin)
self.vxmin = - self.vxmax
self.vymax = vpr * (self.ymax - self.ymin)
self.vymin = - self.vymax
self.nobs = len(obstacle)
self.xobs = [obs[0] for obs in obstacle]
self.yobs = [obs[1] for obs in obstacle]
self.robs = [obs[2] for obs in obstacle]
self.n = n
def Straight_path(self):
xn = np.linspace(self.xs, self.xt, self.n + 2)[1:-1]
yn = np.linspace(self.ys, self.yt, self.n + 2)[1:-1]
return [xn, yn]
def Random_path(self):
xn = np.random.uniform(self.xmin, self.xmax, self.n)
yn = np.random.uniform(self.ymin, self.ymax, self.n)
return [xn, yn]
def Random_velocity(self):
vxn = np.random.uniform(self.vxmin, self.vxmax, self.n)
vyn = np.random.uniform(self.vymin, self.vymax, self.n)
return [vxn, vyn]
class Position():
def __init__(self):
self.x = []
self.y = []
def display(self):
n = len(self.x)
for i in range(n):
print('(%f,%f) ' % (self.x[i], self.y[i]), end='')
print('\n')
class Velocity():
def __init__(self):
self.x = []
self.y = []
class Path():
def __init__(self):
self.xx = []
self.yy = []
self.L = []
self.violation = np.inf
self.isfeasible = False
self.cost = np.inf
def plan(self, position, model):
XS = np.insert(np.array([model.xs, model.xt]), 1, position.x)
YS = np.insert(np.array([model.ys, model.yt]), 1, position.y)
TS = np.linspace(0, 1, model.n + 2)
tt = np.linspace(0, 1, 100)
f1 = interpolate.UnivariateSpline(TS, XS, s=0)
xx = f1(tt)
f2 = interpolate.UnivariateSpline(TS, YS, s=0)
yy = f2(tt)
dx = np.diff(xx)
dy = np.diff(yy)
L = np.sum(np.sqrt(dx ** 2 + dy ** 2))
violation = 0
for i in range(model.nobs):
d = np.sqrt((xx - model.xobs[i]) ** 2 + (yy - model.yobs[i]) ** 2)
v = np.maximum(1 - np.array(d) / model.robs[i], 0)
violation = violation + np.mean(v)
self.xx = xx
self.yy = yy
self.L = L
self.violation = violation
self.isfeasible = (violation == 0)
self.cost = L * (1 + violation * 100)
class Best():
def __init__(self):
self.position = Position()
self.path = Path()
pass
class Particle():
def __init__(self):
self.position = Position()
self.velocity = Velocity()
self.path = Path()
self.best = Best()
def drawPath(model, GBest):
plt.rcParams['axes.unicode_minus'] = False
plt.title('路径规划')
plt.scatter(model.xs, model.ys, label='起点', marker='o', linewidths=3, color='red')
plt.scatter(model.xt, model.yt, label='终点', marker='*', linewidths=3, color='green')
theta = np.linspace(0, 2 * np.pi, 100)
for i in range(model.nobs):
plt.fill(model.xobs[i] + model.robs[i] * np.cos(theta), model.yobs[i] + model.robs[i] * np.sin(theta), 'gray')
plt.scatter(GBest.position.x, GBest.position.y, label='决策点', marker='x', linewidths=1.5, color='blue')
plt.plot(GBest.path.xx, GBest.path.yy, label='路径')
plt.legend()
plt.grid()
plt.axis('equal')
plt.show()
def drawCost(BestCost):
plt.rcParams['axes.unicode_minus'] = False
plt.figure()
plt.title('代价曲线')
plt.xlabel('代数')
plt.ylabel('最优代价')
n = len(BestCost)
plt.plot(range(n), BestCost)
plt.grid()
plt.xlim((0, n + 10))
plt.show()
def PSO(T, size, wmax, wmin, c1, c2, model):
plt.ion()
plt.figure(1)
GBest = Best()
BestCost = []
Swarm = []
for i in range(size):
p = Particle()
if i:
[p.position.x, p.position.y] = model.Random_path()
else:
[p.position.x, p.position.y] = model.Straight_path()
[p.velocity.x, p.velocity.y] = model.Random_velocity()
p.path.plan(p.position, model)
p.best.position = copy.deepcopy(p.position)
p.best.path = copy.deepcopy(p.path)
if p.best.path.isfeasible and (p.best.path.cost < GBest.path.cost):
GBest = copy.deepcopy(p.best)
Swarm.append(p)
BestCost.append(GBest.path.cost)
w = wmax
for t in range(T):
for i in range(size):
p = Swarm[i]
p.velocity.x = w * p.velocity.x + \
c1 * np.random.rand() * (p.best.position.x - p.position.x) \
+ c2 * np.random.rand() * (GBest.position.x - p.position.x)
p.velocity.x = np.minimum(model.vxmax, p.velocity.x)
p.velocity.x = np.maximum(model.vxmin, p.velocity.x)
p.position.x = p.position.x + p.velocity.x
outofrange = (p.position.x < model.xmin) | (p.position.x > model.xmax)
p.velocity.x[outofrange] = -p.velocity.x[outofrange]
p.position.x = np.minimum(model.xmax, p.position.x)
p.position.x = np.maximum(model.xmin, p.position.x)
p.velocity.y = w * p.velocity.y + \
c1 * np.random.rand() * (p.best.position.y - p.position.y) \
+ c2 * np.random.rand() * (GBest.position.y - p.position.y)
p.velocity.y = np.minimum(model.vymax, p.velocity.y)
p.velocity.y = np.maximum(model.vymin, p.velocity.y)
p.position.y = p.position.y + p.velocity.y
outofrange = (p.position.y < model.ymin) | (p.position.y > model.ymax)
p.velocity.y[outofrange] = -p.velocity.y[outofrange]
p.position.y = np.minimum(model.ymax, p.position.y)
p.position.y = np.maximum(model.ymin, p.position.y)
p.path.plan(p.position, model)
if p.path.cost < p.best.path.cost:
p.best.position = copy.deepcopy(p.position)
p.best.path = copy.deepcopy(p.path)
if p.best.path.isfeasible and (p.best.path.cost < GBest.path.cost):
GBest = copy.deepcopy(p.best)
print('第%d代:cost=%f,决策点为' % (t + 1, GBest.path.cost), end='')
GBest.position.display()
BestCost.append(GBest.path.cost)
w = w - (wmax - wmin) / T
c1 = 0.01 * c1 + c1
c2 = 0.01 * c2 + c2
plt.cla()
drawPath(model, GBest)
plt.pause(0.01)
plt.ioff()
drawCost(BestCost)
if __name__ == '__main__':
start = [0.0, 0.0]
target = [4.0, 6.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]]
n = 3
model = Model(start, target, bound, obstacle, n, 0.4)
T = 200
size = 100
wmax = 0.9
wmin = 0.4
c1 = 1.5
c2 = 1.5
PSO(T, size, wmax, wmin, c1, c2, model)
五. 总结
- 惯性权重系数w的心爱星变换公式可以尝试更改,以便达到更好的拟合效果。
- 加速常数C1、C2的线性变换和w的变换不同,是随着迭代次数的增加而增大的。
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)