首先用栅格法描述机器人工作环境,在此基础上,将机器人路径表示为粒子位置的二进制编码,并以路径长度为适应值,产生初始种群后,再对粒子位置和速度进行更新,经过多次迭代,即可获得从起始点到目标点的一条全局最优路径.
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_shange
for j=1:num_shange
if sign(i,j)==1
y=[i-1,i-1,i,i];
x=[j-1,j,j,j-1];
h=fill(x,y,'k');
set(h,'facealpha',0.5)
end
s=(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_shange
plot([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:index1
Q1=[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 on
end
title('粒子群算法-随机路线');
title('粒子群算法-随机路线');
figure(2)
hold on
for i=1:num_shange
for j=1:num_shange
if sign(i,j)==1
y=[i-1,i-1,i,i];
x=[j-1,j,j,j-1];
h=fill(x,y,'k');
set(h,'facealpha',0.5)
end
s=(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_shange
plot([0 num_shange],[i-1 i-1],'k-');
plot([i i],[0 num_shange],'k-');%画网格线
end
for i=2:index1
Q1=[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:PopSize
Velocity(:,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 < maxgen
w=w_max-(w_max-w_min)*gen/maxgen;
%迭代次数递增
gen = gen +1
%更新全局极值点位置,这里指路径
for i=1:PopSize
GlobalBest(:,i) = Group(:,index);
end
for i = 1:PopSize % 更新各路径总距离
EachPathDis(i) = PathDistance(Group(:,i)',E,num_shange);
end
IsChange = 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:index1
Q1=[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:PopSize
p_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:index3
Q1=[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:PopSize
p_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:index2
Q1=[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_shange
for j=1:num_shange
if sign(i,j)==1
y=[i-1,i-1,i,i];
x=[j-1,j,j,j-1];
h=fill(x,y,'k');
set(h,'facealpha',0.5)
end
s=(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_shange
plot([0 num_shange],[i-1 i-1],'k-');
plot([i i],[0 num_shange],'k-');%画网格线
end
for i=2:index1
Q1=[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('最佳个体适应度值变化趋势');
- 1.
- 2.
- 3.
- 4.
- 5.
- 6.
- 7.
- 8.
- 9.
- 10.
- 11.
- 12.
- 13.
- 14.
- 15.
- 16.
- 17.
- 18.
- 19.
- 20.
- 21.
- 22.
- 23.
- 24.
- 25.
- 26.
- 27.
- 28.
- 29.
- 30.
- 31.
- 32.
- 33.
- 34.
- 35.
- 36.
- 37.
- 38.
- 39.
- 40.
- 41.
- 42.
- 43.
- 44.
- 45.
- 46.
- 47.
- 48.
- 49.
- 50.
- 51.
- 52.
- 53.
- 54.
- 55.
- 56.
- 57.
- 58.
- 59.
- 60.
- 61.
- 62.
- 63.
- 64.
- 65.
- 66.
- 67.
- 68.
- 69.
- 70.
- 71.
- 72.
- 73.
- 74.
- 75.
- 76.
- 77.
- 78.
- 79.
- 80.
- 81.
- 82.
- 83.
- 84.
- 85.
- 86.
- 87.
- 88.
- 89.
- 90.
- 91.
- 92.
- 93.
- 94.
- 95.
- 96.
- 97.
- 98.
- 99.
- 100.
- 101.
- 102.
- 103.
- 104.
- 105.
- 106.
- 107.
- 108.
- 109.
- 110.
- 111.
- 112.
- 113.
- 114.
- 115.
- 116.
- 117.
- 118.
- 119.
- 120.
- 121.
- 122.
- 123.
- 124.
- 125.
- 126.
- 127.
- 128.
- 129.
- 130.
- 131.
- 132.
- 133.
- 134.
- 135.
- 136.
- 137.
- 138.
- 139.
- 140.
- 141.
- 142.
- 143.
- 144.
- 145.
- 146.
- 147.
- 148.
- 149.
- 150.
- 151.
- 152.
- 153.
- 154.
- 155.
- 156.
- 157.
- 158.
- 159.
- 160.
- 161.
- 162.
- 163.
- 164.
- 165.
- 166.
- 167.
- 168.
- 169.
- 170.
- 171.
- 172.
- 173.
- 174.
- 175.
- 176.
- 177.
- 178.
- 179.
- 180.
- 181.
- 182.
- 183.
- 184.
- 185.
- 186.
- 187.
- 188.
- 189.
- 190.
- 191.
- 192.
- 193.
- 194.
- 195.
- 196.
- 197.
- 198.
- 199.
- 200.
- 201.
- 202.
- 203.
- 204.
- 205.
- 206.
- 207.
- 208.
- 209.
- 210.
- 211.
- 212.
- 213.
- 214.
- 215.
- 216.
- 217.
- 218.
- 219.
- 220.
- 221.
- 222.
- 223.
- 224.
- 225.
- 226.
- 227.
- 228.
- 229.
- 230.
- 231.
- 232.
- 233.
- 234.
- 235.
- 236.
- 237.
- 238.