一. 前言

路径规划是运动规划的主要研究内容之一。运动规划由路径规划和轨迹规划组成，连接起点位置和终点位置的序列点或曲线称之为路径，构成路径的策略称之为路径规划。
路径规划在很多领域都具有广泛的应用。在高新科技领域的应用有：机器人的自主无碰行动；无人机的避障突防飞行；巡航导弹躲避雷达搜索、防反弹袭击、完成突防爆破任务等。在日常生活领域的应用有：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.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)

五. 总结

• 惯性权重系数w的心爱星变换公式可以尝试更改，以便达到更好的拟合效果。
• 加速常数C1、C2的线性变换和w的变换不同，是随着迭代次数的增加而增大的。