本文研究基于改进粒子群算法的二维平面路径规划。粒子群算法易陷入局部最优，故采用线性变换的惯性权重系数和加速常数改进。构建地图模型，通过粒子群迭代优化路径点，经三次样条插值生成路径，结合碰撞检测计算代价，最终输出最优路径及代价曲线，验证了算法有效性。
☞☞☞AI 智能聊天, 问答助手, AI 智能搜索, 免费无限量使用 DeepSeek R1 模型☜☜☜
一键运行
路径规划是运动规划的主要研究内容之一。运动规划由路径规划和轨迹规划组成，连接起点位置和终点位置的序列点或曲线称之为路径，构成路径的策略称之为路径规划。 路径规划在很多领域都具有广泛的应用。在高新科技领域的应用有：机器人的自主无碰行动；无人机的避障突防飞行；巡航导弹躲避雷达搜索、防反弹袭击、完成突防爆破任务等。在日常生活领域的应用有：GPS导航；基于GIS系统的道路规划;城市道路网规划导航等。在决策管理领域的应用有：物流管理中的车辆问题(VRP)及类似的资源管理资源配置问题。通信技术领域的路由问题等。凡是可拓扑为点线网络的规划问题基本上都可以采用路径规划的方法解决。
粒子群算法是由Eberhart 和Kennedy于1995年提出的一种全局搜索算法，是一种模拟自然界的生物活动以及群体智能的随机搜索算法。除了考虑模拟生物的群体活动之外，融入了个体认知和社会影响，是一种群体智能算法。 粒子群算法初始化为一群随机的粒子（随机解），然后根据迭代找到最优解。每一次迭代中，粒子通过跟踪两个极值来更新自己：第1个是粒子本身所找到的最优解，这个称为个体极值；第2个是整个种群目前找到的最优解，这个称为全局极值。也可以不用整个种群，而是用其中的一部分作为粒子的邻居，称为局部极值。
PSO的算法步骤：
标准粒子群算法虽然收敛速度很快，但是容易陷入局部最优，所以在这里采用具有线性变换的惯性权重系数和加速常数的改进粒子群算法，能有效克制其出现的问题。
import numpy as npfrom scipy import interpolateimport copyimport matplotlib.pyplot as pltimport pylab as mpl
# 地图模型类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('Path Planning')
plt.scatter(model.xs, model.ys, label='starting point', marker='o', linewidths=3, color='red')
plt.scatter(model.xt, model.yt, label='finishing point', 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='decision point', marker='x', linewidths=1.5, color='blue')
plt.plot(GBest.path.xx, GBest.path.yy, label='path')
plt.legend()
plt.grid()
plt.axis('equal')
plt.show()# 画代价曲线def drawCost(BestCost):
plt.rcParams['axes.unicode_minus'] = False
plt.figure()
plt.title('Cost curve')
plt.xlabel('Iterations')
plt.ylabel('Optimal cost')
n = len(BestCost)
plt.plot(range(n), BestCost)
plt.grid()
plt.xlim((0, n + 10)) # plt.ylim(ymin=0)
plt.show()# PSO过程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] ##x部分
# 更新速度
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) # 更新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) ##y部分
# 更新速度
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) # 更新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 # 动态更新w
c1 = 0.01 * c1 + c1 # 动态更新c1
c2 = 0.01 * c2 + 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]] # x,y的边界
obstacle = [[1.5, 4.5, 1.3], [4.0, 3.0, 1.0], [1.2, 1.5, 0.8]] # 障碍圆(x,y,r)
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)第200代:cost=7.705195,决策点为(2.122464,1.363587) (2.280747,2.671492) (3.184077,4.118512)
<Figure size 640x480 with 1 Axes>
<Figure size 640x480 with 1 Axes>
以上就是基于改进粒子群算法（PSO）的二维平面路径规划研究的详细内容，更多请关注php中文网其它相关文章！
每个人都需要一台速度更快、更稳定的 PC。随着时间的推移，垃圾文件、旧注册表数据和不必要的后台进程会占用资源并降低性能。幸运的是，许多工具可以让 Windows 保持平稳运行。
Copyright 2014-2025 https://www.php.cn/ All Rights Reserved | php.cn | 湘ICP备2023035733号
804
收藏
