bizhang.m

一个用MATLAB实现的单机器人避障程序。效果很好。

close all;
figure;
axis([0,17,0,17]);
box off;
hold on;
set(gca,'xtick',[0,2,15],'ytick',[0,15]);
set(gcf,'Position',[463 218 304 288]);
plot([0,15,15],[15,15,0],'k');
text(5,16,'墙壁');
text(15.3,7,'墙');
text(15.3,9,'壁');
rectangle('Position',[1.3,5,1.3,5],'FaceColor','M');
rectangle('Position',[4,1.3,7,2],'FaceColor','G','Curvature',[1,1]);
x1=5+cos(linspace(pi/2,pi*1.5,100));y1=2.3+sin(linspace(pi/2,pi*1.5,100));
% From 萝卜驿站: http://luobo.yculblog.com/
% email: [e-mail]
x2=10+cos(linspace(-pi/2,pi/2,100));y2=2.3+sin(linspace(-pi/2,pi/2,100));
fill([x1,x2],[y1,y2],'G');
F=fill(6+2*cos(linspace(0,pi*2,100)),7+2*sin(linspace(0,pi*2,100)),'Y');
rectangle('Position',[6,12,6,1.5],'FaceColor','B');
F1=fill([11,14,14.8,10.2],[4,4,7.5,7.5],'w');
set(F1,'FaceColor',[1,0.3,0.3]);
x=2;y=0;
z=x+y*i;
A=[pi/2,1.1,0.6,0.3,0.2,0.4,0.9,1.6,1.7,1.5,...
       0.5,0,-0.4,-1.4,-2.7,-3,-3.6,-3.6,-3.5,...
        -3.6,-4,-2,-1.5,-1.7,-1.8,-1.5];
for k=1:length(A);
   z1=z+1.5*exp(i*A(k));
   za=z1-1.2*exp(i*A(k))+0.3*exp(i*[A(k)-pi/2]);
    zb=z1-1.2*exp(i*A(k))+0.3*exp(i*[A(k)+pi/2]);
    fill(real([za,z1,zb]),imag([za,z1,zb]),'k');
    real([za,z1,zb])
   plot([z,z1],'k');
    z=z1;
end