【路径规划】基于matlab粒子群算法机器人栅格路径规划【含Matlab源码 018期】

一、粒子群算法及栅格地图简介

1 粒子群算法
1.1 引言
自然界中的鸟群和鱼群的群体行为一直是科学家的研究兴趣所在。生物学家Craig Reynolds在1987年提出了一个非常有影响的鸟群聚集模型，在他的仿真中，每一个个体都遵循：避免与邻域个体相撞：匹配邻域个体的速度；飞向鸟群中心，且整个群体飞向目标。仿真中仅利用上面三条简单的规则，就可以非常接近地模拟出鸟群飞行的现象。1990年，生物学家Frank Heppner也提出了鸟类模型，它的不同之处在于：鸟类被吸引飞到栖息地。在仿真中，一开始每一只鸟都没有特定的飞行目标，只是使用简单的规则确定自己的飞行方向和飞行速度，当有一只鸟飞到栖息地时，它周围的鸟也会跟着飞向栖息地，最终整个鸟群都会落在栖息地。
1995年，美国社会心理学家James Kennedy和电气工程师RussellEberhart共同提出了粒子群算法(ParticleS warm Optimization， PSO) ，该算法的提出是受对鸟类群体行为进行建模与仿真的研究结果的启发。他们的模型和仿真算法主要对Frank Heppner的模型进行了修正，以使粒子飞向解空间并在最优解处降落。粒子群算法一经提出，由于其算法简单，容易实现，立刻引起了进化计算领域学者们的广泛关注，形成一个研究热点。2001年出版的J.Kennedy与R.Eberhart合著的《群体智能》将群体智能的影响进一步扩大[] ，随后关于粒子群优化算法的研究报告和研究成果大量涌现，继而掀起了国内外研究热潮[2-7]。
粒子群优化算法来源于鸟类群体活动的规律性，进而利用群体智能建立一个简化的模型。它模拟鸟类的觅食行为，将求解问题的搜索空间比作鸟类的飞行空间，将每只鸟抽象成一个没有质量和体积的粒
子，用它来表征问题的一个可能解，将寻找问题最优解的过程看成鸟类寻找食物的过程，进而求解复杂的优化问题。粒子群优化算法与其他进化算法一样，也是基于“种群”和“进化”的概念，通过个体间
的协作与竞争，实现复杂空间最优解的搜索。同时，它又不像其他进化算法那样对个体进行交叉、变异、选择等进化算子操作，而是将群体中的个体看作在l维搜索空间中没有质量和体积的粒子，每个粒子以一定的速度在解空间运动，并向自身历史最佳位置P best和邻域历史最佳位置g best聚集，实现对候选解的进化。粒子群算法具有很好的生物社会背景而易于理解，由于参数少而容易实现，对非线性、多峰问题均具有较强的全局搜索能力，在科学研究与工程实践中得到了广泛关注。目前，该算法已广泛应用于函数优化、神经网络训练、模式分类、模糊控制等领域。

1.2 粒子群算法理论
1.2.1粒子群算法描述
鸟类在捕食过程中，鸟群成员可以通过个体之间的信息交流与共享获得其他成员的发现与飞行经历。在食物源零星分布并且不可预测的条件下，这种协作机制所带来的优势是决定性的，远远大于对食物
的竞争所引起的劣势。粒子群算法受鸟类捕食行为的启发并对这种行为进行模仿，将优化问题的搜索空间类比于鸟类的飞行空间，将每只鸟抽象为一个粒子，粒子无质量、无体积，用以表征问题的一个可行解，优化问题所要搜索到的最优解则等同于鸟类寻找的食物源。粒子群算法为每个粒子制定了与鸟类运动类似的简单行为规则，使整个粒子群的运动表现出与鸟类捕食相似的特性，从而可以求解复杂的优化问题。
粒子群算法的信息共享机制可以解释为一种共生合作的行为，即每个粒子都在不停地进行搜索，并且其搜索行为在不同程度上受到群体中其他个体的影响[8]，同时这些粒子还具备对所经历最佳位置的记
忆能力，即其搜索行为在受其他个体影响的同时还受到自身经验的引导。基于独特的搜索机制，粒子群算法首先生成初始种群，即在可行解空间和速度空间随机初始化粒子的速度与位置，其中粒子的位置用于表征问题的可行解，然后通过种群间粒子个体的合作与竞争来求解优化问题。
1.2.2粒子群算法建模
粒子群优化算法源自对鸟群捕食行为的研究：一群鸟在区域中随机搜索食物，所有鸟知道自己当前位置离食物多远，那么搜索的最简单有效的策略就是搜寻目前离食物最近的鸟的周围区域。粒子群算法
利用这种模型得到启示并应用于解决优化问题。在粒子群算法中，每个优化问题的潜在解都是搜索空间中的一只鸟，称之为粒子。所有的粒子都有一个由被优化的函数决定的适应度值，每个粒子还有一个速度决定它们飞翔的方向和距离。然后，粒子们就追随当前的最优粒子在解空间中搜索[9]。

粒子群算法首先在给定的解空间中随机初始化粒子群，待优化问题的变量数决定了解空间的维数。每个粒子有了初始位置与初始速度，然后通过迭代寻优。在每一次迭代中，每个粒子通过跟踪两个“极值”来更新自己在解空间中的空间位置与飞行速度：一个极值就是单个粒子本身在迭代过程中找到的最优解粒子，这个粒子叫作个体极值：另一个极值是种群所有粒子在迭代过程中所找到的最优解粒子，这个粒子是全局极值。上述的方法叫作全局粒子群算法。如果不用种群所有粒子而只用其中一部分作为该粒子的邻居粒子，那么在所有邻居粒子中的极值就是局部极值，该方法称为局部粒子群算法。

1.2.3粒子群算法的特点
粒子群算法本质是一种随机搜索算法，它是一种新兴的智能优化技术。该算法能以较大概率收敛于全局最优解。实践证明，它适合在动态、多目标优化环境中寻优，与传统优化算法相比，具有较快的计
算速度和更好的全局搜索能力。
(1)粒子群算法是基于群智能理论的优化算法，通过群体中粒子间的合作与竞争产生的群体智能指导优化搜索。与其他算法相比，粒子群算法是一种高效的并行搜索算法。
(2)粒子群算法与遗传算法都是随机初始化种群，使用适应值来评价个体的优劣程度和进行一定的随机搜索。但粒子群算法根据自己的速度来决定搜索，没有遗传算法的交叉与变异。与进化算法相比，粒子群算法保留了基于种群的全局搜索策略，但是其采用的速度-位移模型操作简单，避免了复杂的遗传操作。
(3)由于每个粒子在算法结束时仍保持其个体极值，即粒子群算法除了可以找到问题的最优解外，还会得到若干较好的次优解，因此将粒子群算法用于调度和决策问题可以给出多种有意义的方案。
(4)粒子群算法特有的记忆使其可以动态地跟踪当前搜索情况并调整其搜索策略。另外，粒子群算法对种群的大小不敏感，即使种群数目下降时，性能下降也不是很大。

1.3 粒子群算法种类
1.3.1基本粒子群算法

1.3.2标准粒子群算法
引入研究粒子群算法经常用到的两个概念：一是“探索”，指粒子在一定程度上离开原先的搜索轨迹，向新的方向进行搜索，体现了一种向未知区域开拓的能力，类似于全局搜索；二是“开发”，指粒子在一定程度上继续在原先的搜索轨迹上进行更细一步的搜索，主要指对探索过程中所搜索到的区域进行更进一步的搜索。探索是偏离原来的寻优轨迹去寻找一个更好的解，探索能力是一个算法的全局搜索能力。开发是利用一个好的解，继续原来的寻优轨迹去搜索更好的解，它是算法的局部搜索能力。如何确定局部搜索能力和全局搜索能力的比例，对一个问题的求解过程很重要。1998年， Shi Yuhui等人提出了带有惯性权重的改进粒子群算法[10]，由于该算法能够保证较好的收敛效果，所以被默认为标准粒子群算法。其进化过程为：

在式(6.7)中，第一部分表示粒子先前的速度，用于保证算法的全局收敛性能；第二部分、第三部分则使算法具有局部收敛能力。可以看出，式(6.7)中惯性权重w表示在多大程度上保留原来的速度：W
较大，则全局收敛能力较强，局部收敛能力较弱；w较小，则局部收敛能力较强，全局收敛能力较弱。
当w=1时，式(6.7)与式(6.5)完全一样，表明带惯性权重的粒子群算法是基本粒子群算法的扩展。实验结果表明：w在0.8~1.2之间时，粒子群算法有更快的收敛速度；而当w>1.2时，算法则容易陷入局部极值。
另外，在搜索过程中可以对w进行动态调整：在算法开始时，可给w赋予较大正值，随着搜索的进行，可以线性地使w逐渐减小，这样可以保证在算法开始时，各粒子能够以较大的速度步长在全局范围内探
测到较好的区域；而在搜索后期，较小的w值则保证粒子能够在极值点周围做精细的搜索，从而使算法有较大的概率向全局最优解位置收敛。对w进行调整，可以权衡全局搜索和局部搜索能力。目前，采用较多的动态惯性权重值是Shi提出的线性递减权值策略，其表达式如下：

1.3.3压缩因子粒子群算法
Clerc等人提出利用约束因子来控制系统行为的最终收敛[11] ，该方法可以有效搜索不同的区域，并且能得到高质量的解。压缩因子法的速度更新公式为：

实验结果表明：与使用惯性权重的粒子群优化算法相比，使用具
有约束因子的粒子群算法具有更快的收敛速度。
1.3.4离散粒子群算法
基本的粒子群算法是在连续域中搜索函数极值的有力工具。继基本粒子群算法之后， Kennedy和Eberhart又提出了一种离散二进制版的粒子群算法[12]。在此离散粒子群方法中，将离散问题空间映射到连续粒子运动空间，并适当修改粒子群算法来求解，在计算上仍保留经典粒子群算法速度-位置更新运算规则。粒子在状态空间的取值和变化只限于0和1两个值，而速度的每一维vi y代表位置每一位xi取值为1的可能性。因此，在连续粒子群中的vij更新公式依然保持不变，但是P best和：best只在[0， 1] 内取值。其位置更新等式表示如下：

1.4 粒子群算法流程
粒子群算法基于“种群”和“进化”的概念，通过个体间的协作与竞争，实现复杂空间最优解的搜索[13]，其流程如下：
(1)初始化粒子群，包括群体规模N，每个粒子的位置x；和速度Vio
(2) 计算每个粒子的适应度值fit[i] 。
(3) 对每个粒子，用它的适应度值fit[门和个体极值P best(i)比较。如果fit[i] <P best(i) ，则用fit[i] 替换掉P best(i) 。
(4) 对每个粒子，用它的适应度值fit[i] 和全局极值g best比较。如果fit[i] < 8 best，则用fit[i] 替换g best。
(5)迭代更新粒子的速度v；和位置xj。
(6)进行边界条件处理。
(7)判断算法终止条件是否满足：若是，则结束算法并输出优化结果；否则返回步骤(2)。
粒子群算法的运算流程如图6.1所示。

1.5 关键参数说明
在粒子群优化算法中，控制参数的选择能够影响算法的性能和效率；如何选择合适的控制参数使算法性能最佳，是一个复杂的优化问题。在实际的优化问题中，通常根据使用者的经验来选取控制参数。
粒子群算法的控制参数主要包括：粒子种群规模N，惯性权重w，加速系数c和c，最大速度Via x，停止准则，邻域结构的设定，边界条件处理策略等[14]，
粒子种群规模N
粒子种群大小的选择视具体问题而定，但是一般设置粒子数为20~50。对于大部分的问题10个粒子，已经可以取得很好的结果：不过对于比较难的问题或者特定类型的问题，粒子的数量可以取到100或
200。另外，粒子数目越大，算法搜索的空间范围就越大，也就更容易发现全局最优解；当然，算法运行的时间也越长。
惯性权重w
惯性权重w是标准粒子群算法中非常重要的控制参数，可以用来控制算法的开发和探索能力。惯性权重的大小表示了对粒子当前速度继承的多少。当惯性权重值较大时，全局寻优能力较强，局部寻优能力
较弱：当惯性权重值较小时，全局寻优能力较弱，局部寻优能力较强。惯性权重的选择通常有固定权重和时变权重。固定权重就是选择常数作为惯性权重值，在进化过程中其值保持不变，一般取值为
[0.8，1.2]：时变权重则是设定某一变化区间，在进化过程中按照某种方式逐步减小惯性权重。时变权重的选择包括变化范围和递减率。固定的惯性权重可以使粒子保持相同的探索和开发能力，而时变权重可以使粒子在进化的不同阶段拥有不同的探索和开发能力。
加速常数c1和c2
加速常数c和c 2分别调节向P best和g best方向飞行的最大步长，它们分别决定粒子个体经验和群体经验对粒子运行轨迹的影响，反映粒子群之间的信息交流。如果cr=c2=0，则粒子将以当前的飞行速度飞到边界。此时，粒子仅能搜索有限的区域，所以难以找到最优解。如果q=0，则为“社会”模型，粒子缺乏认知能力，而只有群体经验，它的收敛速度较快，但容易陷入局部最优；如果oy=0，则为“认知”模
型，没有社会的共享信息，个体之间没有信息的交互，所以找到最优解的概率较小，一个规模为D的群体等价于运行了N个各行其是的粒子。因此一般设置c1=C2，通常可以取c1=cg=1.5。这样，个体经验和群体经验就有了同样重要的影响力，使得最后的最优解更精确。
粒子的最大速度vmax
粒子的速度在空间中的每一维上都有一个最大速度限制值vd max，用来对粒子的速度进行钳制，使速度控制在范围[-Vimax， +va max] 内，这决定问题空间搜索的力度，该值一般由用户自己设定。Vmax是一个非常重要的参数，如果该值太大，则粒子们也许会飞过优秀区域：而如果该值太小，则粒子们可能无法对局部最优区域以外的区域进行充分的探测。它们可能会陷入局部最优，而无法移动足够远的距离而跳出局部最优，达到空间中更佳的位置。研究者指出，设定Vmax和调整惯性权重的作用是等效的，所以!max一般用于对种群的初始化进行设定，即将vmax设定为每维变量的变化范围，而不再对最大速度进行细致的选择和调节。
停止准则
最大迭代次数、计算精度或最优解的最大停滞步数▲t(或可以接受的满意解)通常认为是停止准则，即算法的终止条件。根据具体的优化问题，停止准则的设定需同时兼顾算法的求解时间、优化质量和
搜索效率等多方面性能。
邻域结构的设定
全局版本的粒子群算法将整个群体作为粒子的邻域，具有收敛速度快的优点，但有时算法会陷入局部最优。局部版本的粒子群算法将位置相近的个体作为粒子的邻域，收敛速度较慢，不易陷入局部最优
值。实际应用中，可先采用全局粒子群算法寻找最优解的方向，即得到大致的结果，然后采用局部粒子群算法在最优点附近进行精细搜索。
边界条件处理
当某一维或若干维的位置或速度超过设定值时，采用边界条件处理策略可将粒子的位置限制在可行搜索空间内，这样能避免种群的膨胀与发散，也能避免粒子大范围地盲目搜索，从而提高了搜索效率。
具体的方法有很多种，比如通过设置最大位置限制Xmax和最大速度限制Vmax，当超过最大位置或最大速度时，在范围内随机产生一个数值代替，或者将其设置为最大值，即边界吸收。

2 栅格地图
2.1 栅格法应用背景
路径规划时首先要获取环境信息，建立环境地图，合理的环境表示有利于建立规划方法和选择合适的搜索算法，最终实现较少的时间开销而规划出较为满意的路径。一般使用栅格法在静态环境下建立环境地图。
2.2 栅格法实质
将AGV的工作环境进行单元分割，将其用大小相等的方块表示出来，这样栅格大小的选取是影响规划算法性能的一个很重要的因素。栅格较小的话，由栅格地图所表示的环境信息将会非常清晰，但由于需要存储较多的信息，会增大存储开销，同时干扰信号也会随之增加，规划速度会相应降低，实时性得不到保证；反之，由于信息存储量少，抗干扰能力有所增强，规划速随之增快，但环境信息划分会变得较为模糊，不利于有效路径的规划。在描述环境信息时障碍物所在区域在栅格地图中呈现为黑色，地图矩阵中标为1，可自由通行区域在栅格地图中呈现为白色，地图矩阵中标为0。路径规划的目的就是在建立好的环境地图中找到一条最优的可通行路径，所以使用栅格法建立环境地图时，栅格大小的合理设定非常关键。
2.3 10乘10的静态环境地图

10乘10的静态环境地图代码

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%建立环境地图%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function DrawMap(map)
n = size(map);
step = 1;
a = 0 : step :n(1);
b = 0 : step :n(2);
figure(1)
axis([0 n(2) 0 n(1)]); %设置地图横纵尺寸
set(gca,'xtick',b,'ytick',a,'GridLineStyle','-',...
'xGrid','on','yGrid','on');
hold on
r = 1;
for(i=1:n(1))         %设置障碍物的左下角点的x,y坐标for(j=1:n(2))if(map(i,j)==1)p(r,1)=j-1;p(r,2)=i-1;fill([p(r,1) p(r,1) + step p(r,1) + step p(r,1)],...[p(r,2) p(r,2) p(r,2) + step p(r,2) + step ],'k');r=r+1;hold onendend
end%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%栅格数字标识%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x_text = 1:1:n(1)*n(2); %产生所需数值.
for i = 1:1:n(1)*n(2)[row,col] = ind2sub([n(2),n(1)],i);text(row-0.9,col-0.5,num2str(x_text(i)),'FontSize',8,'Color','0.7 0.7 0.7');
end
hold on
axis square

建立环境矩阵，1代表黑色栅格，0代表白色栅格，调用以上程序，即可得到上述环境地图。

map=[0 0 0 1 0 0 1 0 0 0;1 0 0 0 0 1 1 0 0 0;0 0 1 0 0 0 1 1 0 0;0 0 0 0 0 0 0 0 0 0;0 0 0 0 0 1 0 0 1 0;1 0 0 0 0 1 1 0 0 0;0 0 0 1 0 0 0 0 0 0;1 1 1 0 0 0 1 0 0 0;0 0 0 0 0 1 1 0 0 0;0 0 0 0 0 1 1 0 0 0;];DrawMap(map);         %得到环境地图

2.4 栅格地图中障碍栅格处路径约束
移动体栅格环境中多采用八方向的移动方式，此移动方式在完全可通行区域不存在运行安全问题，当
移动体周围存在障碍栅格时此移动方式可能会发生与障碍物栅格的碰撞问题，为解决此问题加入约束
条件，当在分别与障碍物栅格水平方向和垂直方向的可行栅格两栅格之间通行时，禁止移动体采用对
角式移动方式。

约束条件的加入，实质是改变栅格地图的邻接矩阵，将障碍栅格(数字为“1”的矩阵元素)的对角栅格
设为不可达，即将对角栅格的距离值改为无穷大。其实现MATLAB代码如下：
代码：

%约束移动体在障碍栅格对角运动
%通过优化邻接矩阵实现
%%%%%%%%%%%%%%%%%% 约束移动体移动方式 %%%%%%%%%%%%%%%%%
function W=OPW(map,W)
% map 地图矩阵  % W 邻接矩阵
n = size(map);
num = n(1)*n(2);
for(j=1:n(1))for(z=1:n(2))if(map(j,z)==1)if(j==1)                  %若障碍物在第一行if(z==1)               %若障碍物为第一行的第一个W(j+1,j+n(2)*j)=Inf;W(j+n(2)*j,j+1)=Inf;elseif(z==n(2))         %若障碍物为第一行的最后一个W(n(2)-1,n(2)+n(1)*j)=Inf;W(n(2)+n(1)*j,n(2)-1)=Inf;else                %若障碍物为第一行的其他W(z-1,z+j*n(2))=Inf;W(z+j*n(2),z-1)=Inf;W(z+1,z+j*n(2))=Inf;W(z+j*n(2),z+1)=Inf;endendendif(j==n(1))               %若障碍物在最后一行if(z==1)               %若障碍物为最后一行的第一个W(z+n(2)*(j-2),z+n(2)*(j-1)+1)=Inf;W(z+n(2)*(j-1)+1,z+n(2)*(j-2))=Inf;elseif(z==n(2))            %若障碍物为最后一行的最后一个W(n(1)*n(2)-1,(n(1)-1)*n(2))=Inf;W((n(1)-1)*n(2),n(1)*n(2)-1)=Inf;else                   %若障碍物为最后一行的其他W((j-2)*n(2)+z,(j-1)*n(2)+z-1)=Inf;W((j-1)*n(2)+z-1,(j-2)*n(2)+z)=Inf;W((j-2)*n(2)+z,(j-1)*n(2)+z+1)=Inf;W((j-1)*n(2)+z+1,(j-2)*n(2)+z)=Inf;endendendif(z==1)              if(j~=1&&j~=n(1))       %若障碍物在第一列非边缘位置 W(z+(j-2)*n(2),z+1+(j-1)*n(2))=Inf;W(z+1+(j-1)*n(2),z+(j-2)*n(2))=Inf;W(z+1+(j-1)*n(2),z+j*n(2))=Inf;W(z+j*n(2),z+1+(j-1)*n(2))=Inf;endendif(z==n(2))if(j~=1&&j~=n(1))         %若障碍物在最后一列非边缘位置 W((j+1)*n(2),j*n(2)-1)=Inf;W(j*n(2)-1,(j+1)*n(2))=Inf;W(j*n(2)-1,(j-1)*n(2))=Inf;W((j-1)*n(2),j*n(2)-1)=Inf;endendif(j~=1&&j~=n(1)&&z~=1&&z~=n(2))   %若障碍物在非边缘位置W(z+(j-1)*n(2)-1,z+j*n(2))=Inf;W(z+j*n(2),z+(j-1)*n(2)-1)=Inf;W(z+j*n(2),z+(j-1)*n(2)+1)=Inf;W(z+(j-1)*n(2)+1,z+j*n(2))=Inf;W(z+(j-1)*n(2)-1,z+(j-2)*n(2))=Inf;W(z+(j-2)*n(2),z+(j-1)*n(2)-1)=Inf;W(z+(j-2)*n(2),z+(j-1)*n(2)+1)=Inf;W(z+(j-1)*n(2)+1,z+(j-2)*n(2))=Inf;endendendend
end

2.5 栅格法案例
下面以Djkstra算法为例，其实现如下：

map=[0 0 0 1 0 0 1 0 0 0;1 0 0 0 0 1 1 0 0 0;0 0 1 0 0 0 1 1 0 0;0 0 0 0 0 0 0 0 0 0;0 0 0 0 0 1 0 0 1 0;1 0 0 0 0 1 1 0 0 0;0 0 0 1 0 0 0 0 0 0;1 1 1 0 0 0 1 0 0 0;0 0 0 0 0 1 1 0 0 0;0 0 0 0 0 1 1 0 0 0;];%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%建立环境矩阵map%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DrawMap(map); %得到环境地图
W=G2D(map);   %得到环境地图的邻接矩阵
W(W==0)=Inf;  %邻接矩阵数值处理
W=OPW(map,W); %优化邻接矩阵
[distance,path]=dijkstra(W,1,100);%设置起始栅格，得到最短路径距离以及栅格路径
[x,y]=Get_xy(distance,path,map);   %得到栅格相应的x，y坐标
Plot(distance,x,y);   %画出路径

运行结果如下：

其中函数程序：
DrawMap(map) 详见建立栅格地图
W=G2D(map) ；详见建立邻接矩阵
[distance， path] =dijkstra(W， 1， 100) 详见Djk stra算法
[x， y] =Get_xy(distance， path， map) ；
Plot(distance， x， y) ；

二、部分源代码

clc;
close all
clear
load('data4.mat')
figure(1)%画障碍图
hold on
S=(S_coo(2)-0.5)*num_shange+(S_coo(1)+0.5);%起点对应的编号
E=(E_coo(2)-0.5)*num_shange+(E_coo(1)+0.5);%终点对应的编号
for i=1:num_shangefor j=1:num_shangeif sign(i,j)==1y=[i-1,i-1,i,i];x=[j-1,j,j,j-1];h=fill(x,y,'k');set(h,'facealpha',0.5)ends=(num2str((i-1)*num_shange+j));%text(j-0.95,i-0.5,s,'fontsize',6) end
end
axis([0 num_shange 0 num_shange])%限制图的边界
plot(S_coo(2),S_coo(1), 'p','markersize', 10,'markerfacecolor','b','MarkerEdgeColor', 'm')%画起点
plot(E_coo(2),E_coo(1),'o','markersize', 10,'markerfacecolor','g','MarkerEdgeColor', 'c')%画终点
set(gca,'YDir','reverse');%图像翻转
for i=1:num_shangeplot([0 num_shange],[i-1 i-1],'k-');plot([i i],[0 num_shange],'k-');%画网格线
end
PopSize=20;%种群大小
OldBestFitness=0;%旧的最优适应度值
gen=0;%迭代次数
maxgen =20;%最大迭代次数

c1=0.5;%认知系数
c2=0.7;%社会学习系数
w=0.96;%惯性系数
%%
%初始化路径
w_min=0.5;
w_max=1;
Group=ones(num_point,PopSize);  %种群初始化
%最优解
route=Group(:,end)';
index1=find(route==E);
route_lin=route(1:index1);
for i=2:index1Q1=[mod(route_lin(i-1)-1,num_shange)+1-0.5,ceil(route_lin(i-1)/num_shange)-0.5];Q2=[mod(route_lin(i)-1,num_shange)+1-0.5,ceil(route_lin(i)/num_shange)-0.5];plot([Q1(1),Q2(1)],[Q1(2),Q2(2)],'b-.','LineWidth',3);hold onend
title('粒子群算法-随机路线');

title('粒子群算法-随机路线');
figure(2)
hold on
for i=1:num_shangefor j=1:num_shangeif sign(i,j)==1y=[i-1,i-1,i,i];x=[j-1,j,j,j-1];h=fill(x,y,'k');set(h,'facealpha',0.5)ends=(num2str((i-1)*num_shange+j));text(j-0.95,i-0.5,s,'fontsize',6) end
end
axis([0 num_shange 0 num_shange])%限制图的边界
plot(S_coo(2),S_coo(1), 'p','markersize', 10,'markerfacecolor','b','MarkerEdgeColor', 'm')%画起点
plot(E_coo(2),E_coo(1),'o','markersize', 10,'markerfacecolor','g','MarkerEdgeColor', 'c')%画终点
set(gca,'YDir','reverse');%图像翻转
for i=1:num_shangeplot([0 num_shange],[i-1 i-1],'k-');plot([i i],[0 num_shange],'k-');%画网格线
end
for i=2:index1Q1=[mod(route_lin(i-1)-1,num_shange)+1-0.5,ceil(route_lin(i-1)/num_shange)-0.5];Q2=[mod(route_lin(i)-1,num_shange)+1-0.5,ceil(route_lin(i)/num_shange)-0.5];plot([Q1(1),Q2(1)],[Q1(2),Q2(2)],'b-.','LineWidth',3)
end
%初始化粒子速度（即交换序）
Velocity =zeros(num_point,PopSize);
for i=1:PopSizeVelocity(:,i)=round(rand(1,num_point)'*num_point/10); %round取整
end

%计算每个个体对应路径的距离
for i=1:PopSize   EachPathDis(i) = PathDistance(Group(:,i)',E,num_shange);
end

IndivdualBest=Group;%记录各粒子的个体极值点位置,即个体找到的最短路径
IndivdualBestFitness=EachPathDis;%记录最佳适应度值,即个体找到的最短路径的长度
[GlobalBestFitness,index]=min(EachPathDis);%找出全局最优值和相应序号
%寻优
while gen < maxgenw=w_max-(w_max-w_min)*gen/maxgen;%迭代次数递增gen = gen +1%更新全局极值点位置,这里指路径for i=1:PopSize   GlobalBest(:,i) = Group(:,index);endfor i = 1:PopSize    % 更新各路径总距离EachPathDis(i) = PathDistance(Group(:,i)',E,num_shange);endIsChange = EachPathDis<IndivdualBestFitness;%更新后的距离优于更新前的，记录序号IndivdualBest(:, find(IsChange)) = Group(:, find(IsChange));%更新个体最佳路径IndivdualBestFitness = IndivdualBestFitness.*( ~IsChange) + EachPathDis.*IsChange;%更新个体最佳路径距离[GlobalBestFitness, index] = min(IndivdualBestFitness);%更新全局最佳路径,记录相应的序号
if GlobalBestFitness~=OldBestFitness %比较更新前和更新后的适应度值;OldBestFitness=GlobalBestFitness;%不相等时更新适应度值best_route=IndivdualBest(:,index)';end   BestFitness(gen) =GlobalBestFitness;%每一代的最优适应度
end
%最优解
index1=find(best_route==E);
route_lin=best_route(1:index1);
for i=2:index1Q1=[mod(route_lin(i-1)-1,num_shange)+1-0.5,ceil(route_lin(i-1)/num_shange)-0.5];Q2=[mod(route_lin(i)-1,num_shange)+1-0.5,ceil(route_lin(i)/num_shange)-0.5];plot([Q1(1),Q2(1)],[Q1(2),Q2(2)],'r','LineWidth',3)
end
for i=1:PopSizep_lin=randperm(num_point)';%随机生成1*400不重复的行向量%% 将起点编号放在首位index=find(p_lin==S);lin=p_lin(1);p_lin(1)=p_lin(index);p_lin(index)=lin;Group(:,i)=p_lin;%%将每个个体进行合理化处理[Group(:,i),flag]=deal_fun(Group(:,i),num_point,liantong_point,E,num_shange);while flag==1%如处理不成功，则初始化个体，重新处理p_lin=randperm(num_point)';index=find(p_lin==S);lin=p_lin(1);p_lin(1)=p_lin(index);p_lin(index)=lin;Group(:,i)=p_lin;[Group(:,i),flag]=deal_fun(Group(:,i),num_point,liantong_point,E,num_shange);       end
end

%最优解
route=Group(:,end)';
index3=find(route==E);
route_lin1=route(1:index3);
for i=2:index3Q1=[mod(route_lin1(i-1)-1,num_shange)+1-0.5,ceil(route_lin1(i-1)/num_shange)-0.5];Q2=[mod(route_lin1(i)-1,num_shange)+1-0.5,ceil(route_lin1(i)/num_shange)-0.5];plot([Q1(1),Q2(1)],[Q1(2),Q2(2)],'c-.','LineWidth',3);hold on
end
for i=1:PopSizep_lin=randperm(num_point)';%随机生成1*400不重复的行向量%% 将起点编号放在首位index=find(p_lin==S);lin=p_lin(1);p_lin(1)=p_lin(index);p_lin(index)=lin;Group(:,i)=p_lin;%%将每个个体进行合理化处理[Group(:,i),flag]=deal_fun(Group(:,i),num_point,liantong_point,E,num_shange);while flag==1%如处理不成功，则初始化个体，重新处理p_lin=randperm(num_point)';index=find(p_lin==S);lin=p_lin(1);p_lin(1)=p_lin(index);p_lin(index)=lin;Group(:,i)=p_lin;[Group(:,i),flag]=deal_fun(Group(:,i),num_point,liantong_point,E,num_shange);       end
end

%最优解
route=Group(:,end)';
index2=find(route==E);
route_lin2=route(1:index2);
for i=2:index2Q1=[mod(route_lin2(i-1)-1,num_shange)+1-0.5,ceil(route_lin2(i-1)/num_shange)-0.5];Q2=[mod(route_lin2(i)-1,num_shange)+1-0.5,ceil(route_lin2(i)/num_shange)-0.5];plot([Q1(1),Q2(1)],[Q1(2),Q2(2)],'m-.','LineWidth',3);hold on
end
title('粒子群算法-对比路线');
figure(3)
hold on
for i=1:num_shangefor j=1:num_shangeif sign(i,j)==1y=[i-1,i-1,i,i];x=[j-1,j,j,j-1];h=fill(x,y,'k');set(h,'facealpha',0.5)ends=(num2str((i-1)*num_shange+j));text(j-0.95,i-0.5,s,'fontsize',6) end
end
axis([0 num_shange 0 num_shange])%限制图的边界
plot(S_coo(2),S_coo(1), 'p','markersize', 10,'markerfacecolor','b','MarkerEdgeColor', 'm')%画起点
plot(E_coo(2),E_coo(1),'o','markersize', 10,'markerfacecolor','g','MarkerEdgeColor', 'c')%画终点
set(gca,'YDir','reverse');%图像翻转
for i=1:num_shangeplot([0 num_shange],[i-1 i-1],'k-');plot([i i],[0 num_shange],'k-');%画网格线
end
for i=2:index1Q1=[mod(route_lin(i-1)-1,num_shange)+1-0.5,ceil(route_lin(i-1)/num_shange)-0.5];Q2=[mod(route_lin(i)-1,num_shange)+1-0.5,ceil(route_lin(i)/num_shange)-0.5];plot([Q1(1),Q2(1)],[Q1(2),Q2(2)],'r','LineWidth',3)
end
title('粒子群算法-最优路线');


%进化曲线
figure(4);
plot(BestFitness);
xlabel('迭代次数')
ylabel('适应度值')
grid on;
title('进化曲线');
disp('粒子群算法-最优路线方案：')
disp(num2str(route_lin))
disp(['起点到终点的距离:',num2str(BestFitness(end))]);
figure(5);
plot(BestFitness*100);
xlabel('迭代次数')
ylabel('适应度值')
grid on;
title('最佳个体适应度值变化趋势');

三、运行结果

四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1] 包子阳,余继周,杨杉.智能优化算法及其MATLAB实例（第2版）[M].电子工业出版社，2016.
[2]张岩,吴水根.MATLAB优化算法源代码[M].清华大学出版社，2017.