目录

1 粒子群算法

2 二维环境建模

 3 路径规划

3.1 初始化

3.2 适应度函数

 ​3.3 仿真结果

---

---

1 粒子群算法

对粒子群算法小结如下：粒子群算法是一种基于种群的搜索过程，其中每个个体（鸟）称为粒子，定义为在D维搜索空间中待优化问题的可行解，保存有其历史最优位置和所有粒子的最优位置的记忆，以及速度。在每一演化代，粒子的信息被组合起来调整速度关于每一维上的分量，继而被用来计算新的粒子位置。粒子在多维搜索空间中不断改变它们的状态，直到到达平衡或最优状态，或者超过了计算限制为止。问题空间的不同维度之间唯一的联系是通过目标函数引入的。很多经验证据已经显示该算法是一个非常有效的优化工具：

 

在此不对粒子群算法原理公式进行介绍说明。直接上代码：

function [ GBEST ,  cgCurve ] = PSO ( noP, maxIter,  problem, dataVis )% Define the details of the objective function
nVar = problem.nVar;
ub = problem.ub;
lb = problem.lb;
fobj = problem.fobj;% Extra variables for data visualization
average_objective = zeros(1, maxIter);
cgCurve = zeros(1, maxIter);
FirstP_D1 = zeros(1 , maxIter);
position_history = zeros(noP , maxIter , nVar );% Define the PSO's paramters
wMax = 0.9;
wMin = 0.2;
c1 = 2;
c2 = 2;
vMax = (ub - lb) .* 0.2;
vMin  = -vMax;% The PSO algorithm% Initialize the particles
for k = 1 : noPSwarm.Particles(k).X = (ub-lb) .* rand(1,nVar) + lb;Swarm.Particles(k).V = zeros(1, nVar);Swarm.Particles(k).PBEST.X = zeros(1,nVar);Swarm.Particles(k).PBEST.O = inf;Swarm.GBEST.X = zeros(1,nVar);Swarm.GBEST.O = inf;
end% Main loop
for t = 1 : maxIter% Calcualte the objective valuefor k = 1 : noPcurrentX = Swarm.Particles(k).X;position_history(k , t , : ) = currentX;Swarm.Particles(k).O = fobj(currentX);average_objective(t) =  average_objective(t)  + Swarm.Particles(k).O;% Update the PBESTif Swarm.Particles(k).O < Swarm.Particles(k).PBEST.OSwarm.Particles(k).PBEST.X = currentX;Swarm.Particles(k).PBEST.O = Swarm.Particles(k).O;end% Update the GBESTif Swarm.Particles(k).O < Swarm.GBEST.OSwarm.GBEST.X = currentX;Swarm.GBEST.O = Swarm.Particles(k).O;endend% Update the X and V vectorsw = wMax - t .* ((wMax - wMin) / maxIter);FirstP_D1(t) = Swarm.Particles(1).X(1);for k = 1 : noPSwarm.Particles(k).V = w .* Swarm.Particles(k).V + c1 .* rand(1,nVar) .* (Swarm.Particles(k).PBEST.X - Swarm.Particles(k).X) ...+ c2 .* rand(1,nVar) .* (Swarm.GBEST.X - Swarm.Particles(k).X);% Check velocitiesindex1 = find(Swarm.Particles(k).V > vMax);index2 = find(Swarm.Particles(k).V < vMin);Swarm.Particles(k).V(index1) = vMax(index1);Swarm.Particles(k).V(index2) = vMin(index2);Swarm.Particles(k).X = Swarm.Particles(k).X + Swarm.Particles(k).V;% Check positionsindex1 = find(Swarm.Particles(k).X > ub);index2 = find(Swarm.Particles(k).X < lb);Swarm.Particles(k).X(index1) = ub(index1);Swarm.Particles(k).X(index2) = lb(index2);endif dataVis == 1outmsg = ['Iteration# ', num2str(t) , ' Swarm.GBEST.O = ' , num2str(Swarm.GBEST.O)];disp(outmsg);endcgCurve(t) = Swarm.GBEST.O;average_objective(t) = average_objective(t) / noP;fileName = ['Resluts after iteration # ' , num2str(t)];save( fileName)
endGBEST = Swarm.GBEST;if dataVis == 1iterations = 1: maxIter;%% Draw the landscape figurex = -50 : 1 : 50;y = -50 : 1 : 50;[x_new , y_new] = meshgrid(x,y);for k1 = 1: size(x_new, 1)for k2 = 1 : size(x_new , 2)X = [ x_new(k1,k2) , y_new(k1, k2) ];z(k1,k2) = ObjectiveFunction( X );endendsubplot(1,5,1)surfc(x_new , y_new , z);title('Search landscape')xlabel('x_1')ylabel('x_2')zlabel('Objective value')shading interpcamproj perspectivebox onset(gca,'FontName','Times')%% Visualize the cgcurve    subplot(1,5,2);semilogy(iterations , cgCurve, 'r');title('Convergence curve')xlabel('Iteration#')ylabel('Weight')%% Visualize the average objectivessubplot(1,5,3)semilogy(iterations , average_objective , 'g')title('Average objecitves of all particles')xlabel('Iteration#')ylabel('Average objective')%% Visualize the fluctuations subplot(1,5,4)   plot(iterations , FirstP_D1, 'k');title('First dimention in first Particle')xlabel('Iteration#')ylabel('Value of the first dimension')%% Visualize the search historysubplot(1,5,5)hold onfor p = 1 : noPfor t = 1 : maxIterx = position_history(p, t , 1);y = position_history(p, t , 2);myColor = [0+t/maxIter  0 1-t/maxIter ];plot(x , y , '.' , 'color' , myColor );endendcontour(x_new , y_new , z);plot(Swarm.GBEST.X(1) , Swarm.GBEST.X(2) , 'og');xlim([lb(1) , ub(1)])ylim([lb(2) , ub(2) ])title('search history')xlabel('x')ylabel('y')box onset(gcf , 'position' , [128         372        1634         259])end

2 二维环境建模

二维环境建模可以看作是对现实三维环境的投影和简化，假设我们的运动物工作在相对水平的环境，且匀速运动。建模示意图如下：

上图中，S,T分别是起点和终点，圆圈区域是我们路径规划需要尽量避开的区域（亦称为威胁），其主要参数为威胁中心，威胁半径和威胁等级（威胁等级并没有在示意图显示），以黄色节点为waypoint的线条是可行的一条规划路径。

通过设计复杂程度不一样的二维环境，来测试算法在不同苛刻程度下的环境的效果。在此用了三个二维环境来进行实验：

%% case 1
radius = [10 10 8 12 9];
threat_lv = [2 10 1 2 3];
threat_center = [45,52;12,40;32,68;36,26;55,80];%% case 2
radius = [10 10 8 12 9 10 10];
threat_lv = [2 10 1 2 5 2 4];
threat_center = [52,52;32,40;12,48;36,26;63,56;50,42;30,70];%% case 3
radius = [10 18 10 12 10 11];
threat_lv = [5 5 5 5 5 5];
threat_center = [30,20;50,15;65,55;50,80;75,90;50,36];

 3 路径规划

 由于起点S和终点T的连线并不与X轴平行，为了方便计算我们先将坐标轴旋转，将线段ST所在的直线作为新的X轴，S作为新坐标系的原点。

旋转矩阵：

路径规划算法设计：我们将新的X轴ST等分为D+1份，意味着路径包含D个黄色节点，D个黄色节点在新坐标系下的X坐标是已知的，而Y坐标是需要通过灰狼算法求解的。因此灰狼算法中的每个可行解都是D维的（即包含D个黄色节点的Y坐标）。

3.1 初始化

粒子群算法需要设置几个初始参数，如种群数量N（随机生成多少个可行解） 、迭代次数、解的维数D，以及上下界。

种群数量N可根据实际问题复杂程度或搜索空间大小来设置，显然N越大不一定越好，因为会增加算法时间复杂度。

最大迭代次数不宜过小，可通过多次实验整定。

上下界(lb,ub)限定了算法在整个过程中生成的解的范围，如果我们不设定上下界，那么最后生成的路径可能非常的离谱，虽然它没有经过威胁区域，但覆盖的范围可能会非常大，不满足实际要求，加上下界可以限定路径的大概区域。

3.2 适应度函数

 这里我们将路径成本定义为威胁代价的累加，带价越低意味着适应度越好，只有当路径经过了威胁区域时，我们才计算该段掉在威胁区域的路径代价：

 

如上图所示，威胁代价最终变成五个采样点的威胁代价的加权和。

3.3 仿真结果

case1

case2

case3

上图中生成的三条路径要求也是不能有冲突，路径除了起点和终点之外不能有交集。我的解决方案是为三条路径分配不同上下界，保证区域不会重合，从根本上保证三条路径不会有交集。

可以看到仿真图里有些路径经过了威胁区域，这是由粒子群算法的随机性和上下界造成的。由于种群初始化是随机生成的，而初始化种群是会影响最终的算法结果的，这种随机性有可能让算法陷入局部最优，而算法的勘探机制越好，跳出局部最优的能力也会越强，但不可避免，这也是目前群智能算法的主要弊端，但是全局搜索又是不现实的，搜索整个解空间很浪费时间。

---