ymmm.asv

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

clear all
d=20;
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.85;
    y0=coordinate(2)-0.85;
    text(x0,y0,{k},'fontsize',9); 
    end
    %画障碍物
X111=[1 3 3 3 1 1];
  Y111=[7 7 9 11 9 7];
fill(X111,Y111,'k');hold on
  X21=[7 9 9 8 7 7];
  Y21=[6 6 7 9 7 6];
fill(X21,Y21,'k');hold on
  X31=[3 6 6 5 3 3];
  Y31=[2 2 4 5 4 2];
fill(X31,Y31,'k');hold on
X11=[15 17 16 15];
Y11=[12 12 16 12];
fill(X11,Y11,'k');hold on
X12=[1 3 3 2 2 1 1];
Y12=[7 7 11 11 10 10 7];
line(X12,Y12,'color','k','linewidth',3);hold on

X8=[3 6 6 3 3];
Y8=[2 2 5 5 2];
line(X8,Y8,'color','k','linewidth',3);hold on
X9=[7 9 9 7 7];
Y9=[6 6 9 9 6];
line(X9,Y9,'color','k','linewidth',3);hold on
X13=[15 17 17 15 15];
Y13=[12 12 16 16 12];
line(X13,Y13,'color','k','linewidth',3);hold on
% pt10 =[1     6;
%      1     2;
%    165   400;
%    165   400;
%    103   165;
%    103   165];
%    pt20 =[1   266;
%    217   400;
%      1    82;
%      1    82;
%      1    82;
%      1    82;
%      1    82;
%      1    82;
%      1    82;
%    145   400;
%      1    83;
%      1    83;
%      1    83];
%      pt30 =[6   306;
%      1   205;
%      1    82;
%      1    82;
%    145   400;
%      1   103;
%      1   103];
pt10 =

     1   328
     1    42
     1   111
     1   111
% pt20 =[1     6;
%      1     6;
%      1     6;
%      1    41;
%      1    41;
%      1    41;
%      1    41];
%      pt30 =[1   307;
%    226   400;
%    226   400;
%      1    21;
%      1    21];


% %      pt10 =[1     6;
% %    217   338];
% % pt20 =[375   400;
% %      1   124];
% % pt30 =[1    28;
% %      1    82;
% %      1    82;
% %      1    82];
    PT10=xutubiao(pt10,d);
     PT20=xutubiao(pt20,d);
     PT30=xutubiao(pt30,d);
    [a10,b10]=size(PT10);
    for i=1:a10
    for j=2:2:b10
    X10(i,j/2)=PT10(i,j); 
    Y10(i,j/2)=PT10(i,j+1);
    end
    end
    for i=1:a10
        line(X10(i,:),Y10(i,:),'linestyle','-','color','k','linewidth',3);
        hold on
    end
    [a20,b20]=size(PT20);
    for i=1:a20
    for j=2:2:b20
    X20(i,j/2)=PT20(i,j); 
    Y20(i,j/2)=PT20(i,j+1);
    end
    end
    for i=1:a20
        line(X20(i,:),Y20(i,:),'linestyle','--','color','k','linewidth',3);
        hold on
    end
    [a30,b30]=size(PT30);
    for i=1:a30
    for j=2:2:b30
    X30(i,j/2)=PT30(i,j); 
    Y30(i,j/2)=PT30(i,j+1);
    end
    end
    for i=1:a30
        line(X30(i,:),Y30(i,:),'linestyle','-.','color','k','linewidth',3);
        hold on
    end
% pt2 =[217   400;
%      6   217;
%      1     2;
%      1     2];
% pt3 =[6   217;
%      1     6;
%      6   237;
%      6   237;
%      1     2;
%      1     2];
% pt4 =[217   278;
%      1     6];
% pt5 =[1    31;
%      6   217];
% 
% PT2=xutubiao(pt2,d);
% PT3=xutubiao(pt3,d);
% PT4=xutubiao(pt4,d);
% PT5=xutubiao(pt5,d);
% [a2,b2]=size(PT2);
%     for i=1:a2
%     for j=2:2:b2
%     X2(i,j/2)=PT2(i,j); 
%     Y2(i,j/2)=PT2(i,j+1);
%     end
%     end
%     for i=1:a2
%         line(X2(i,:),Y2(i,:),'linestyle','-','color','k','marker','o','linewidth',1.5);
%         hold on
%     end
%     [a3,b3]=size(PT3);
%     for i=1:a3
%     for j=2:2:b3
%     X3(i,j/2)=PT3(i,j); 
%     Y3(i,j/2)=PT3(i,j+1);
%     end
%     end
%     for i=1:a3
%         line(X3(i,:),Y3(i,:),'linestyle','-','color','k','marker','*','linewidth',1.5);
%         hold on
%     end
%     [a4,b4]=size(PT4);
%     for i=1:a4
%     for j=2:2:b4
%     X4(i,j/2)=PT4(i,j); 
%     Y4(i,j/2)=PT4(i,j+1);
%     end
%     end
%     for i=1:a4
%         line(X4(i,:),Y4(i,:),'linestyle','-','color','k','marker','^','linewidth',1.5);
%         hold on
%     end
%     [a5,b5]=size(PT5);
%     for i=1:a5
%     for j=2:2:b5
%     X5(i,j/2)=PT5(i,j); 
%     Y5(i,j/2)=PT5(i,j+1);
%     end
%     end
%     for i=1:a5
%         line(X5(i,:),Y5(i,:),'linestyle','-','color','k','marker','s','linewidth',1.5);
%         hold on
%     end
% 
% 
% %