ssa.asv

规则障碍物环境下的机器人路径规划matlab源码！

%function [Best,Gen,BestF,AvrF,gen,Best0,Gen0,BestF0,AvrF0,gen0]=ssa(m,n,g,op_pre,MM,d,Xlu,O,C)
clear all
m=20;   %种群规模
n=2;   %插入的点数
g=150;  %进化代数
op_pre=0.01 ;   %判断收敛的阈值
a=0.4;
%b=0.4;
%c=3;
d=20;%图形中横纵栅格数
MM=d*d; %共100个栅格
%变量范围
syms x y
Xlu=zeros(18*n+20,2);
for i=1:18*n+20
    Xlu(i,:)=[0,d];
end
%O=[1 4 14 18;8 13 8 13;14 17 2 5];%障碍物上下左右界
O=[1 4 14 18;8 13 8 13;14 17 2 5];%障碍物上下左右界
[o1,o2]=size(O);
for i=1:o1
    K(i,1)=(20-O(i,4))/(20-O(i,1));
    K(i,2)=(20-O(i,3))/(20-O(i,2));
    k(i,:)=(O(i,4)-O(i,3))/(O(i,1)-O(i,2));
    y(i,:)=k(i,:)*(x-O(i,1))+O(i,4);
end


C=[2.5 16;10.5 10.5;15.5 3.5];
% O=[1 3 7 10;2 3 10 11;3 6 2 5;7 9 6 9;15 17 12 16];
% C=[2 8.5;2.5 10.5;4.5 2.5;8 7.5;16 14];
P=initization(m,n,MM,d);
% P10=initization(10,n,MM,d);
% P20=initization(20,n,MM,d);
% P30=initization(30,n,MM,d);
% P10=ymfitness1(P10);
% P20=ymfitness1(P20);
% P30=ymfitness1(P30);
% BestF=zeros(1,g);   %各代的最优的适应值
% AvrF=zeros(1,g);    %各代适应值的平均值
% Best=[];    %最优个体
% gen=0;  %寻得最优个体的代数
% Gen=0;  %算法收敛代数
% s=20;
% for i=1:s
% [Best1,Gen1,BestF1,AvrF1,gen1,pt1]=ymSGA(m,n,g,op_pre,MM,d,Xlu,O,C,P,a,b);
% [Best0,Gen0,BestF0,AvrF0,gen0]=ymSGA0(m,n,g,op_pre,MM,d,Xlu,O,C,P);
%  
% % % Gen0,Gen,
% % % gen0,gen,Best0,Best,
% G1(i,:)=Gen1;G0(i,:)=Gen0;
% g1(i,:)=gen1;g0(i,:)=gen0;
% B1(i,:)=Best1(1,1);B0(i,:)=Best0(1,1);
% end
% %G1,G0,g1,g0,
% M1=mean(G1),M11=mean(G0),M2=mean(g1),M22=mean(g0),M3=mean(B1),M33=mean(B0),M4=std(B1,1)^2,M44=std(B0,1)^2
% % % % pt
% figure(1)
% plot(BestF0,'k-');
% hold on
% plot(BestF,'k--');
% title('各代的最优的适应值');
% xlabel('进化代数t');
% ylabel('适应值');
% legend(':算法1','：算法2');
% 
% figure(11)
% plot(BestF0,'k-');
% hold on
% plot(BestF,'k--');
% title('best fitness of each generation');
% xlabel('generation of evolution t');
% ylabel('fitness');
% legend(':algorithm1','：algorithm2');
% figure(2)
% plot(AvrF0,'k-');
% hold on
% plot(AvrF,'k--');
% title('各代适应值的平均值');
% xlabel('进化代数t');
% ylabel('平均适应值');
% legend(':算法1','：算法2');
% figure(22)
% plot(AvrF0,'k-');
% hold on
% plot(AvrF,'k--');
% title('average fitness of each generation');
% xlabel('generation of evolution t');
% ylabel('average fitness');
% legend(':algorithm1','：algorithm2');
% 
 figure(3)
axis([0,d,0,d])
    for i=1:d
        for j=1:d
                x1=j-1;y1=d-i;
                x2=j;y2=d-i;
                x3=j;y3=d-i+1;
                x4=j-1;y4=d-i+1;
                fill([x1,x2,x3,x4],[y1,y2,y3,y4],[1 1 1]);
                hold on
        end
    end
    hold on
    %画栅格
%     for k=1:d*d  
%     coordinate=ymadjp1(k,d);
%     x0=coordinate(1)-0.5;
%     y0=coordinate(2)-0.5;
%     text(x0,y0,{k}); 
%     end
    %画障碍物
    %画障碍物
X1=[1 4 4 1 1];
  Y1=[14 14 18 18 14];
fill(X1,Y1,'k');hold on
  X2=[8 13 13 8 8];
  Y2=[8 8 13 13 8];
fill(X2,Y2,'k');hold on
  X3=[14 17 17 14 14];
  Y3=[2 2 5 5 2];
fill(X3,Y3,'k');hold on
for j=0:d
    for l=0:d
    for w=1:o1
    if j<=d&l<=d&j>=0&l>=0
        a=(d-l)/(d-j);
        if a<K(w,2)&a>K(w,1)
            if l-k(w,:)*(j-O(w,1))-O(w,4)>=0
            b=0.8;
             plot(j,l,'rs');
            else
              b=0.4;
            plot(j,l,'b*');    
            end
        end
        
    end 
    end
    if a<K(1,1)|
    end
end
            
    


% 
%  %画出最优路径
% 
%   [b1,b2]=size(Best0);
% for i=2:2:b2
%     if Best0(1,i)==0
%         break
%     else
%     X4(i/2)=Best0(1,i); 
%     Y4(i/2)=Best0(1,i+1);
%     end
% end
% line(X4,Y4,'linestyle','-','color','k');
% hold on
% [b11,b21]=size(Best);
% for j=2:2:b21 
%     if Best(1,j)==0
%         break
%     end
%     X5(j/2)=Best(1,j); 
%     Y5(j/2)=Best(1,j+1);
%    
% end
% line(X5,Y5,'linestyle','--','color','k');
% hold off

%  s=20;
% for i=1:s
% [Best10,Gen10,BestF10,AvrF10,gen10,pt10]=ymSGA(m,n,g,op_pre,MM,d,Xlu,O,C,P,a,0.1);
% [Best20,Gen20,BestF20,AvrF20,gen20,pt20]=ymSGA(m,n,g,op_pre,MM,d,Xlu,O,C,P,a,0.4);
% [Best30,Gen30,BestF30,AvrF30,gen30,pt30]=ymSGA(m,n,g,op_pre,MM,d,Xlu,O,C,P,a,0.7);
% % Best10,Gen10,gen10,pt10
% % Best20,Gen20,gen20,pt20
% % Best30,Gen30,gen30,pt30
% G10(i,:)=Gen10;
% G20(i,:)=Gen20;
% G30(i,:)=Gen30;
% g10(i,:)=gen10;
% g20(i,:)=gen20;
% g30(i,:)=gen30;
% B10(i,:)=Best10(1,1);
% B20(i,:)=Best20(1,1);
% B30(i,:)=Best30(1,1);
% end
% mean(G10),mean(G20),mean(G30),mean(g10),mean(g20),mean(g30),mean(B10),mean(B20),mean(B30),std(B10,1)^2,std(B20,1)^2,std(B30,1)^2,
% % figure(1)
% plot(BestF10,'k-');
% hold on
% plot(BestF20,'k--');
% hold on
% plot(BestF30,'k-.')
% title('各代的最优的适应值');
% xlabel('进化代数t');
% ylabel('适应值');
% legend(':种群规模10',':种群规模20',':种群规模30');
% figure(11)
% plot(BestF10,'k-');
% hold on
% plot(BestF20,'k--');
% hold on
% plot(BestF30,'k-.')
% title('best fitness of each generation');
% xlabel('generation of evolution t');
% ylabel('fitness');
% legend(':popsize10',':popsize20',':popsize30');
% figure(2)
% plot(AvrF10,'k-');
% hold on
% plot(AvrF20,'k--');
% hold on
% plot(AvrF30,'k-.');
% title('每代的平均适应值');
% xlabel('进化代数t');
% ylabel('平均适应值');
% legend(':种群规模10',':种群规模20',':种群规模30');
%  figure(22)
% plot(AvrF10,'k-');
% hold on
% plot(AvrF20,'k--');
% hold on
% plot(AvrF30,'k-.');
% title('average fitness of each generation');
% xlabel('generation of evolution t');
% ylabel('average fitness');
% legend(':popsize10',':popsize20',':popsize30');
%  figure(3)
% axis([0,d,0,d])
%     for i=1:d
%         for j=1:d
%                 x1=j-1;y1=d-i;
%                 x2=j;y2=d-i;
%                 x3=j;y3=d-i+1;
%                 x4=j-1;y4=d-i+1;
%                 fill([x1,x2,x3,x4],[y1,y2,y3,y4],[1,1,1]);
%                 hold on
%         end
%     end
%     hold on
%     画栅格
%     for k=1:d*d  
%     coordinate=ymadjp1(k,d);
%     x0=coordinate(1)-0.84;
%     y0=coordinate(2)-0.84;
%     text(x0,y0,{k},'fontsize',9); 
%     end
%     %画障碍物
%     %画障碍物
% X1=[1 4 4 1 1];
%   Y1=[14 14 18 18 14];
% fill(X1,Y1,'k');hold on
%   X2=[8 13 13 8 8];
%   Y2=[8 8 13 13 8];
% fill(X2,Y2,'k');hold on
%   X3=[14 17 17 14 14];
%   Y3=[2 2 5 5 2];
% fill(X3,Y3,'k');hold on
%  画出最优路径
% 
%   [b10,b11]=size(Best10);
% for i=2:2:b11
%     if Best10(1,i)==0
%         break
%     else
%     X4(i/2)=Best10(1,i); 
%     Y4(i/2)=Best10(1,i+1);
%     end
% end
% line(X4,Y4,'linestyle','-','color','k');
% hold on
% [b20,b21]=size(Best20);
% for j=2:2:b21 
%     if Best20(1,j)==0
%         break
%     end
%     X5(j/2)=Best20(1,j); 
%     Y5(j/2)=Best20(1,j+1);
% end
% line(X5,Y5,'linestyle','--','color','k');
% hold on
% [b30,b31]=size(Best30);
% for j=2:2:b31 
%     if Best30(1,j)==0
%         break
%     end
%     X6(j/2)=Best30(1,j); 
%     Y6(j/2)=Best30(1,j+1);
% end
% line(X6,Y6,'linestyle','-.','color','k');

% s=20;
% for i=1:s
% [Best2,Gen2,BestF2,AvrF2,gen2,pt2]=ymSGA(m,n,g,op_pre,MM,d,Xlu,O,C,P,0.2,b);
% [Best3,Gen3,BestF3,AvrF3,gen3,pt3]=ymSGA(m,n,g,op_pre,MM,d,Xlu,O,C,P,0.3,b);
% [Best4,Gen4,BestF4,AvrF4,gen4,pt4]=ymSGA(m,n,g,op_pre,MM,d,Xlu,O,C,P,0.4,b);
% [Best5,Gen5,BestF5,AvrF5,gen5,pt5]=ymSGA(m,n,g,op_pre,MM,d,Xlu,O,C,P,0.5,b);
% Best2,Gen2,gen2,pt2
% % Best3,Gen3,gen3,pt3
% % Best4,Gen4,gen4,pt4
% % Best5,Gen5,gen5,pt5
% G2(i,:)=Gen2;G3(i,:)=Gen3;G4(i,:)=Gen4;G5(i,:)=Gen5;
% g2(i,:)=gen2;g3(i,:)=gen3;g4(i,:)=gen4;g5(i,:)=gen5;
% B2(i,:)=Best2(1,1);B3(i,:)=Best3(1,1);B4(i,:)=Best4(1,1);B5(i,:)=Best5(1,1);
% end
% mean(G2),mean(G3),mean(G4),mean(G5),
% mean(g2),mean(g3),mean(g4),mean(g5),
% mean(B2),mean(B3),mean(B4),mean(B5),std(B2,1)^2,std(B3,1)^2,std(B4,1)^2,std(B5,1)^2
% figure(1)
% hold on
% plot(BestF2,'-ko');
% hold on
% plot(BestF3,'-k*');
% hold on
% plot(BestF4,'-k^');
% hold on
% plot(BestF5,'-ks');
% hold on
% title('各代的最优的适应值');
% xlabel('进化代数t');
% ylabel('适应值');
% legend(':知识提取比例0.2',':知识提取比例0.3',':知识提取比例0.4',':知识提取比例0.5');
% figure(11)
% plot(BestF2,'-ko');
% hold on
% plot(BestF3,'-k*');
% hold on
% plot(BestF4,'-k^');
% hold on
% plot(BestF5,'-ks');
% hold on
% title('best fitness of each generation');
% xlabel('generation of evolution t');
% ylabel('fitness');
% legend(': proportion of extraction0.2',': proportion of extraction0.3',': proportion of extraction0.4',': proportion of extraction0.5');
% hold on
% figure(2)
% hold on
% plot(AvrF2,'-ko');
% hold on
% plot(AvrF3,'-k*');
% hold on
% plot(AvrF4,'-k^');
% hold on
% plot(AvrF5,'-ks');
% title('各代的平均适应值');
% xlabel('进化代数t');
% ylabel('平均适应值');
% legend(':知识提取比例0.2',':知识提取比例0.3',':知识提取比例0.4',':知识提取比例0.5');
% figure(22)
% plot(AvrF2,'-ko');
% hold on
% plot(AvrF3,'-k*');
% hold on
% plot(AvrF4,'-k^');
% hold on
% plot(AvrF5,'-ks');
% title('average fitness of each generation');
% xlabel('generation of evolution t');
% ylabel('average fitness');
% legend(':知识提取比例0.2',':知识提取比例0.3',':知识提取比例0.4',':知识提取比例0.5');
% hold on
%  figure(3)
% axis([0,d,0,d])
%     for i=1:d
%         for j=1:d
%                 x1=j-1;y1=d-i;
%                 x2=j;y2=d-i;
%                 x3=j;y3=d-i+1;
%                 x4=j-1;y4=d-i+1;
%                 fill([x1,x2,x3,x4],[y1,y2,y3,y4],[1,1,1]);
%                 hold on
%         end
%     end
%     hold on
%     %画栅格
% %     for k=1:d*d  
% %     coordinate=ymadjp1(k,d);
% %     x0=coordinate(1)-0.5;
% %     y0=coordinate(2)-0.5;
% %     text(x0,y0,{k}); 
% %     end
%     %画障碍物
% X1=[1 4 4 1 1];
%   Y1=[14 14 18 18 14];
% fill(X1,Y1,'k');hold on
%   X2=[8 13 13 8 8];
%   Y2=[8 8 13 13 8];
% fill(X2,Y2,'k');hold on
%   X3=[14 17 17 14 14];
%   Y3=[2 2 5 5 2];
% fill(X3,Y3,'k');hold on
%  %画出最优路径
% 
%   [b20,b21]=size(Best2);
% for i=2:2:b21
%     if Best2(1,i)==0
%         break
%     else
%     X4(i/2)=Best2(1,i); 
%     Y4(i/2)=Best2(1,i+1);
%     end
% end
% line(X4,Y4,'linestyle','-','color','k','marker','o');
% hold on
% [b30,b31]=size(Best3);
% for j=2:2:b31 
%     if Best3(1,j)==0
%         break
%     end
%     X5(j/2)=Best3(1,j); 
%     Y5(j/2)=Best3(1,j+1);
% end
% line(X5,Y5,'linestyle','-','color','k','marker','*');
% hold on
% [b40,b41]=size(Best4);
% for j=2:2:b41 
%     if Best4(1,j)==0
%         break
%     end
%     X6(j/2)=Best4(1,j); 
%     Y6(j/2)=Best4(1,j+1);
% end
% line(X6,Y6,'linestyle','-','color','k','marker','^');
% hold on
% [b50,b51]=size(Best5);
% for j=2:2:b51 
%     if Best5(1,j)==0
%         break
%     end
%     X7(j/2)=Best5(1,j); 
%     Y7(j/2)=Best5(1,j+1);
% end
% line(X7,Y7,'linestyle','-','color','k','marker','s');
% hold off

% s=20;
% for i=1:20
% [Best,Gen,BestF,AvrF,gen,pt]=ymSGA(m,n,g,op_pre,MM,d,Xlu,O,C,P,a,0.2);
% [Best0,Gen0,BestF0,AvrF0,gen0,pt0]=ymSGA(m,n,g,op_pre,MM,d,Xlu,O,C,P,a,0.4);
%  
% % Gen0,Gen,
% % gen0,gen,Best0,Best,
% G(i,:)=Gen;G0(i,:)=Gen0;
% g(i,:)=gen;g0(i,:)=gen0;
% B(i,:)=Best(1,1);B0(i,:)=Best0(1,1);
% end
% mean(G),mean(G0),mean(g),mean(g0),mean(B),mean(B0),
%  pt,pt0
% figure(1)
% plot(BestF0);
% hold on
% plot(BestF,'--');
% title('各代的最优的适应值');
% xlabel('进化代数t');
% ylabel('适应值');
% legend(':算法1','：算法2');
% figure(2)
% plot(AvrF0);
% hold on
% plot(AvrF,'--');
% title('各代适应值的平均值');
% xlabel('进化代数t');
% ylabel('平均适应值');
% legend(':算法1','：算法2');
%  figure(3)
% axis([0,d,0,d])
%     for i=1:d
%         for j=1:d
%                 x1=j-1;y1=d-i;
%                 x2=j;y2=d-i;
%                 x3=j;y3=d-i+1;
%                 x4=j-1;y4=d-i+1;
%                 fill([x1,x2,x3,x4],[y1,y2,y3,y4],[1,1,1]);
%                 hold on
%         end
%     end
%     hold on
% %     %画栅格
% %     for k=1:d*d  
% %     coordinate=ymadjp1(k,d);
% %     x0=coordinate(1)-0.5;
% %     y0=coordinate(2)-0.5;
% %     text(x0,y0,{k}); 
% %     end
%     %画障碍物
% X1=[1 3 3 3 1 1];
%   Y1=[7 7 9 11 9 7];
% fill(X1,Y1,[1,0,1]);hold on
%   X2=[7 9 9 8 7 7];
%   Y2=[6 6 7 9 7 6];
% fill(X2,Y2,[1,0,1]);hold on
%   X3=[3 6 6 5 3 3];
%   Y3=[2 2 4 5 4 2];
% fill(X3,Y3,[1,0,1]);hold on
% X11=[15 17 16 15];
% Y11=[12 12 16 12];
% fill(X11,Y11,[1,0,1]);hold on
% X12=[1 3 3 2 2 1 1];
% Y12=[7 7 11 11 10 10 7];
% line(X12,Y12,'linestyle','--');hold on
% 
% X8=[3 6 6 3 3];
% Y8=[2 2 5 5 2];
% line(X8,Y8,'linestyle','--');hold on
% X9=[7 9 9 7 7];
% Y9=[6 6 9 9 6];
% line(X9,Y9,'linestyle','--');hold on
% X10=[15 17 17 15 15];
% Y10=[12 12 16 16 12];
% line(X10,Y10,'linestyle','--');hold on
% 
% 
% 
%  %画出最优路径
% 
%   [b1,b2]=size(Best0);
% for i=2:2:b2
%     if Best0(1,i)==0
%         break
%     else
%     X4(i/2)=Best0(1,i); 
%     Y4(i/2)=Best0(1,i+1);
%     end
% end
% line(X4,Y4);
% hold on
% [b11,b21]=size(Best);
% for j=2:2:b21 
%     if Best(1,j)==0
%         break
%     end
%     X5(j/2)=Best(1,j); 
%     Y5(j/2)=Best(1,j+1);
%    
% end
% line(X5,Y5,'linestyle','--');
% hold off
%