ymmm.m

规则障碍物环境下的机器人路径规划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
    %画障碍物

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
 pt10 =[ 1    45;
   133   400;
     1    25;
     1    25;
     1    25];
   pt20 =[133   400;
     1    41;
     1    25;
     1    25;
     1    25];
     pt30 =[1    45;
     1    25;
     1    25;
     1    25;
     1    25;
     1    25];
% pt10 =[1   328;
%      1    42;
%      1   111;
%      1   111];
% pt20 =[ 1    66;
%      1    66;
%      1    66;
%      1    66;
%      1    66]
% pt30 =[133   400;
%      1   111;
%      1   111;
%      1   111];


% %      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
% 
% 
% %