智能优化及其应用——课程设计（粒子群算法）

---

基于粒子群算法的路径规划应用

前言

   在上一篇文章中我记录了智能优化与应用的课程实验的内容(点击可查看)，这篇文章将继续记录课程设计的内容。
  课程设计要求在实现实验的基础上采用智能优化算法解决一个自选的更复杂、更具体的优化任务。本文继续采用粒子群算法，将其应用在一个路径规划的场景，即在一个带障碍物的二维地图上，给定起点和终点，寻找到一条最短的路径。最终实现效果如下：
该项目主要参考了该文（带matlab代码），感兴趣可查看原链接，本文主要介绍项目的主要思路以及其python详细实现。

---

以下内容主要是本人对课程设计的记录，如果有错误或不妥之处，望指正。
另：点击此处可查看完整代码进行测试

一、粒子群算法介绍

  在之前智能优化及其应用——课程实验（粒子群算法）一文中，已经介绍了粒子群算法的原理，此处仅介绍本文所采用的粒子群算法的数学建模：
  在D维的目标搜索空间中，随机初始化N个粒子组成一个群落，其中第i个粒子的位置表示为一个D维的向量，记为
$$X_i=(x_{i1},x_{i2},\cdots ,x_{iD}),i=1,2,\cdots ,N$$第i个粒子的“飞行”速度也是一个D维的向量，记为
$$V_i=(v_{i1},v_{i2},\cdots ,v_{iD}),i=1,2,\cdots ,N$$由优化函数计算每个粒子的适应度值，记为
$$F_i=fit(X_i),i=1,2,\cdots ,N$$然后开始迭代搜索，第i个粒子迄今为止搜索到的最优位置称为个体极值，记为
$$P_{Best}=(p_{i1},p_{i2},\cdots ,p_{iD}),i=1,2,\cdots ,N$$整个粒子群迄今为止搜索到的最优位置为全局极值，记为
$$G_{Best}=(g_{i1},g_{i2},\cdots ,g_{iD}),i=1,2,\cdots ,N$$每次迭代中粒子找到这两个位置后，根据以下两个式子来更新自己的速度和位置：
$$v_{ij}(t+1)=w{v}_{ij}(t)+c_1{r}_1(t)[p_{ij}(t)-x_{ij}(t)]+c_2{r}_2(t)[g_{ij}(t)-x_{ij}(t)]$$$$x_{ij}(t+1)=x_{ij}(t+1)+v_{ij}(t+1),i=1,2,\cdots ,N$$$$i=1,2,\cdots ,N;j=1,2,\cdots ,D$$  $w$为惯性系数，控制着前一速度对当前速度的影响，用于平衡算法的探索和开发能力。探索，指粒子在一定程度上离开原先的搜索轨迹，向新的方向进行搜索，体现了一种向未知区域开拓的能力，即全局搜索能力；开发，指粒子在一定程度上继续在原先的搜索轨迹上进行更细一步的搜索，主要指对探索过程中所搜索到的区域进行更进一步的搜索，即局部搜索能力。$w$可以表示在多大程度上保留原来的速度，$w$越大，全局收敛能力越强；$w$越小，局部收敛能力越强。因此算法采取动态调整的方法，在算法开始时，赋予$w$较大值，以较大的速度步长在全局范围内探测较好区域，随着搜索的进行，线性地减小w，保证粒子能够在极值点附近进行精细的搜素。动态调整的方法如下：
$$w=w_{max}-\frac{(w_{max}-w_{min})}{T_{max}}$$  $c_1$和$c_2$为学习因子，也称加速常数，分别调节向个体极值和全局极值方向飞行的加速权值，它们分别决定粒子个体经验和群体经验对粒子运行轨迹的影响，反映了粒子群之间的信息交流。；$r_1$和$r_2$为［0，1］范围内的均匀随机数，增加了粒子飞行的随机性。
  速度更新公式由三部分组成：第一部分为“惯性”部分，反映了粒子的运动“习惯”，代表粒子有维持自己先前速度的趋势；第二部分为“认知”部分，反映了粒子对自身历史经验的记忆或回忆，代表粒子有向自身历史最佳位置逼近的趋势；第三部分为“社会”部分，反映了粒子间协同合作与知识共享的群体历史经验，代表粒子有向群体历史最佳位置逼近的趋势。

二、路径规划问题

1.设计思路

  移动机器人路径规划是根据给定的地图信息，规划一条从起始位置到达目标位置的最优的无碰撞路径。本项目主要研究粒子群算法在路径规划算法上的应用，将问题进行简化处理，如上面的结果图所示，给定地图设定为二维平面，同时地图上的障碍物设计为二维平面上的圆形，移动机器人为二维平面上的质点，给定起始坐标和终点坐标，路径则为两点之间的一条点序列，路径长度即为路径点序列上前后两点距离之和。路径规划的目的即寻找一条与障碍物无碰撞的，路径长度尽可能小的点序列。

2.问题建模

  地图为整个二维平面$S(x,y)$，存在$k$个障碍物圆，用圆心坐标和圆半径表示：$$Obstracl{e}_i=(o{x}_i,o{y}_i,o{r}_i),i=1,2,\cdots ,k$$移动机器人的起点和终点分别为：
$$Start=(x_s,y_s),Target=(x_t,y_t)$$  从计算量的角度考虑，选择起点与终点间的$n$个点作为路径的决策变量，即一个的粒子的维度为$2n$(包括$n$个决策点的$x,y$)，具有对应的位置和速度：
$$Position=((x_1,y_1),(x_2,y_2),\cdots ,(x_n,y_n))$$$$Velocity=((v{x}_1,v{y}_1),(v{x}_2,v{y}_2),\cdots ,(v{x}_n,v{y}_n))$$  为了使路径更平滑，起点，决策点和终点组成的点序列进行三次样条插值，得到一条点序列长度为m的完整平滑的路径：
$$X=(x_1,x_2,\cdots ,x_m)$$$$Y=(y_1,y_2,\cdots ,y_m)$$
路径长度为：
$$L=\sum _{i=1}^{m-1}\sqrt{((x_{(i+1)}-x_i{)}^2+(y_{(i+1)}-y_i{)}^2)}$$  为了让机器人在行进中对障碍进行合理地避让，对路径序列上的每一点进行碰撞检测，即该点是否在障碍圆中，判断路径是否可行，并在代价函数中引入碰撞惩罚机制：
$$d_{ij}=\sqrt{((x_i-o{x}_j{)}^2+(y_i-o{y}_j{)}^2)},i=1,2,\cdots ,m;j=1,2,\cdots ,k$$$$violation=\sum _{j=1}^{k}mean(max(1-\frac{d_{ij}}{o{r}_j},0))$$$$Cost=L\ast (1+100\ast violation)$$最终将整个路径规划转化为一个单目标约束优化问题：
$$mi{n}_{(S(x,y))}Cost(Position)或mi{n}_{(S(x,y))}Cost(X,Y)$$

三、python编程设计

1.模型建立

  首先，创建问题的模型，主程序中给定移动机器人起点(0.0,0.0)，终点(4.0,6.0)，地图中的障碍圆3个，以及决策变量点(x,y)的边界约束bound，确定决策变量的个数为3个，即每条路径为起点，3个决策点和终点通过三次样条插值得到，以上参数可以根据需要修改

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)

  模型类的定义如下：

# 地图模型类
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

  在创建模型时，需根据决策变量点的边界，计算决策变量点对应的速度的边界。速度边界值越大，则粒子容易飞过最优解区域，速度边界值越小，则粒子可能陷入局部最优区域无法飞出。本项目中，速度边界取值如下$$v_{max}=0.1(x_{max}-x_{min}),v_{min}=-v_{max}$$  模型类设计了3个函数分别用于初始化粒子位置和速度，第一个函数初始化的粒子代表起点到终点的直线路径，第二和第三个函数分别是在边界范围内随机生成位置和速度。

    #起点到终点直线路径中间点
    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]

2.粒子设计

  粒子类的定义如下，每个粒子包含当前位置，当前速度，当前位置对应生成的路径和自身历史最优情况记录。

# 粒子类
class Particle():
    def __init__(self):
        self.position = Position()
        self.velocity = Velocity()
        self.path = Path()
        self.best = Best()

粒子中的位置对象和速度对象用于保存决策点和决策点对应的速度

# 位置类
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

3.粒子群初始化

  给定粒子群算法的参数，包括迭代次数，种群大小，w的最大值和最小值，c1和c2，开始粒子群算法。（该段代码在主程序中）

##粒子群参数
    T = 200
    size = 100
    wmax = 0.9
    wmin = 0.4
    c1 = 1.5
    c2 = 1.5
    PSO(T, size, wmax, wmin, c1, c2, model)

首先初始化一个size大的粒子种群，第一个粒子的位置初始化为起点到终点的直线决策点，当该直线未经过障碍物时，一定为最优路径。其他粒子的位置随机生成，所有粒子的速度也随机生成。粒子再根据自身的位置生成自身的路径，然后更新局部最优和全局最优，此时所有粒子的局部最优均为自身，全局最优为所有粒子路径的代价最小且路径可行的粒子。

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

4.粒子群迭代更新

  粒子群开始迭代，根据公式更新粒子速度和位置，同时根据边界条件对速度和位置进行约束，即超出边界的值约束为边界。此外，如果更新后的位置超出边界，可认为速度更新位置的方向不正确，将速度方向调整为反向。

#开始迭代
    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)

每次迭代后展示最优信息和地图情况，并且动态更新w，直到迭代完成，最后展示最终代价曲线。

#展示信息
        print('第%d代:cost=%f,决策点为'%(t+1,GBest.path.cost),end='')
        GBest.position.display()
        BestCost.append(GBest.path.cost)
        w = w - (wmax - wmin)/T      #动态更新w
        plt.cla()
        drawPath(model,GBest)
        plt.pause(0.01)
    plt.ioff()
    drawCost(BestCost)

四、结果分析

  运行程序，开始迭代，地图可视化如下，起点为(0.0,0.0),终点为(4.0,6.0)，决策点设为3个，路径为地图上起点开始经过三个决策点到达终点且不碰撞障碍圆的曲线。地图上为这一代的全局最优路径，程序将动态展示路径变化过程。

每一代的最优代价和决策点位置信息输出如下：

迭代200次后，最终得到的最优路径如下：

最优代价即最短路径长为7.45，路径平滑地经过了三个决策点和绕过了三个障碍物，以最小的代价从起点到达终点。完整路径序列保存在全局最优对象中。

最终得到代价曲线如下，第50代时代价已经减小到7.5，粒子群已经搜索到全局最优附近（开发），然后在该位置处进行局部搜索（探索），最终收敛在最优位置。

由于粒子群算法中采取了一定的随机策略，每次运行的迭代情况不一样，但最终基本上可以找到大致相同的最优路径。改变地图模型中的障碍物、起点终点等相关参数，仍可使用该程序进行路径规划。

---

总结

  本项目将移动机器人的路径规划问题转化为一个单目标约束的优化问题，将粒子群算法应用在该问题的求解上，从程序运行的结果看，粒子群算法能以极大的概率快速高效地寻找到代价最小的最优解。因此本文为移动机器人在路径规划上的具体应用提供了一种思路。
  粒子群算法本质上是一种随机搜索算法，其采用的速度-位置模型操作简单，避免了复杂的遗传操作，通过粒子的个体经验和群体经验指导优化搜索，具有快速的计算速度和好的全局搜索策略。粒子群算法最重要的部分在于速度更新公式，包括了粒子的惯性部分，认知部分和社会部分，主要采用惯性系数w和加速常数c1，c2平衡。 本项目采用了惯性系数的动态更新方法，从实验结果看出，迭代初期，w较大，粒子的探索能力强，在全局范围内寻找最优解，最优路径变化幅度大；迭代后期，w教小，粒子的开发能力强，在已找到的最优解附近小幅度调整路径。
  在课程实验中，主要研究适用于解决普通优化问题的粒子群算法，而在课程设计中，则研究用于解决特定优化问题，相较于前者，在编程的过程中与问题相结合，更偏向于具体化，详细化。
  当然，项目还是存在着一些不足，一方面是简化后的路径规划问题规模较小，模型并不复杂，且没有精确的全局最优解进行结果验证；另一方面是由于粒子群算法的随机性，有一定的概率粒子群陷入局部最优的情况，在本项目所用模型中，可能出现代价差距在0.1左右的局部最优解。因此后续另一方面可以考虑设计更复杂的模型检测算法的性能和实用性，另一方面可以考虑对粒子群算法进行改进，比如调整算法参数、改变算法的拓扑结构，以及与遗传算法相结合等方向，降低算法陷入局部最优的概率。

编程时遇到的问题记录

1. python的数组、对象直接赋值并不会创建新数组、对象，而是共用相同地址。最开始我的局部最优和全局最优都是直接赋值更新的，导致粒子更新时局部最优和全局最优也直接跟着改变，而没有保存正确的值。采用deepcopy()则可解决。
2. python的列表中数的类型虽然不用定义，但实际上根据数的格式已经确定了类型，一开始定义点的坐标时，整数坐标我没有写成小数格式，导致三次样条插值时出现了问题。
3. python实现动态展示图片时，需要从阻塞模式切换到交互模式，即调用代码plt.ion()
   以上其实是一些比较基础的内容，主要是我粗心和基础不扎实导致，阻碍了我较多时间，以后必须加强训练。