粒子群优化路径规划

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.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)