目录
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 : noP
Swarm.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 value
for k = 1 : noP
currentX = 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 PBEST
if Swarm.Particles(k).O < Swarm.Particles(k).PBEST.O
Swarm.Particles(k).PBEST.X = currentX;
Swarm.Particles(k).PBEST.O = Swarm.Particles(k).O;
end
% Update the GBEST
if Swarm.Particles(k).O < Swarm.GBEST.O
Swarm.GBEST.X = currentX;
Swarm.GBEST.O = Swarm.Particles(k).O;
end
end
% Update the X and V vectors
w = wMax - t .* ((wMax - wMin) / maxIter);
FirstP_D1(t) = Swarm.Particles(1).X(1);
for k = 1 : noP
Swarm.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 velocities
index1 = 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 positions
index1 = 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);
end
if dataVis == 1
outmsg = ['Iteration# ', num2str(t) , ' Swarm.GBEST.O = ' , num2str(Swarm.GBEST.O)];
disp(outmsg);
end
cgCurve(t) = Swarm.GBEST.O;
average_objective(t) = average_objective(t) / noP;
fileName = ['Resluts after iteration # ' , num2str(t)];
save( fileName)
end
GBEST = Swarm.GBEST;
if dataVis == 1
iterations = 1: maxIter;
%% Draw the landscape
figure
x = -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 );
end
end
subplot(1,5,1)
surfc(x_new , y_new , z);
title('Search landscape')
xlabel('x_1')
ylabel('x_2')
zlabel('Objective value')
shading interp
camproj perspective
box on
set(gca,'FontName','Times')
%% Visualize the cgcurve
subplot(1,5,2);
semilogy(iterations , cgCurve, 'r');
title('Convergence curve')
xlabel('Iteration#')
ylabel('Weight')
%% Visualize the average objectives
subplot(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 history
subplot(1,5,5)
hold on
for p = 1 : noP
for t = 1 : maxIter
x = position_history(p, t , 1);
y = position_history(p, t , 2);
myColor = [0+t/maxIter 0 1-t/maxIter ];
plot(x , y , '.' , 'color' , myColor );
end
end
contour(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 on
set(gcf , 'position' , [128 372 1634 259])
end2 二维环境建模
二维环境建模可以看作是对现实三维环境的投影和简化，假设我们的运动物工作在相对水平的环境，且匀速运动。建模示意图如下：
上图中，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
上图中生成的三条路径要求也是不能有冲突，路径除了起点和终点之外不能有交集。我的解决方案是为三条路径分配不同上下界，保证区域不会重合，从根本上保证三条路径不会有交集。
可以看到仿真图里有些路径经过了威胁区域，这是由粒子群算法的随机性和上下界造成的。由于种群初始化是随机生成的，而初始化种群是会影响最终的算法结果的，这种随机性有可能让算法陷入局部最优，而算法的勘探机制越好，跳出局部最优的能力也会越强，但不可避免，这也是目前群智能算法的主要弊端，但是全局搜索又是不现实的，搜索整个解空间很浪费时间。
