文档介绍：附录设计过程和程序清单
参考程序如下：
function[x,y]=obstacleavoidance(Tl,Nl,T2,N2,T3,N3,X0,Y0,XG,YG,ST
EP)
hold on
x ⑴=X0;
y(D=Y0;
tl=pi/4;
t2=pi/2;
t3=3*pi/4;
theta=O;
n=l;
if((N3==0)&&((Nl+N2)==0))
error
%只有A类障碍物的情况
else if((N3==0)&&((Nl+N2)〜=0))
while(((x(n)-XG)A2+(y(n)-YG)A2)>STEPA2)
dr=mindetected((tl-theta),Tl,Nl,T2,N2,x(n),y(n));
dc=mindetected((t2-theta),T 1 ,N 1 ,T2,N2,x(n),y (n));
dl=mindetected((t3-theta),Tl,Nl,T2,N2,x(n),y(n));
tg=atan((XG-x(n))+i*(YG-y(n)))*180/pi-theta;
sa=fuzzylogic(dr,dc,dl,tg);
step=STER*(l-abs(sa)/100);
sa=sa*pi/180;
x(n+1 )=x(n)+step* sin(sa);
y(n+l)=y(n)+step*cos(sa);
theta=theta+sa;
n=n+l;
end
%只有B类障碍物的情况
else if(((Nl+N2)==0)&&(N3〜=0))
while (((x(n)-XG)A2+(y(n)-YG)A2)>STEPA2)
dr=mindetectedquare((tl-theta),T3,N3,x(n),y(n));
dc=mindetectedquare((t2-theta),T3,N3,x(n),y(n));
dl=mindetectedquare((t3-theta),T3,N3,x(n),y(n));
tg=atan((XG-x(n))+i*(YG-y(n)))*180/pi-theta;
sa=fuzzylogic(dr,dc,dl,tg);
step=STER*(l-abs(sa)/100);
sa=sa*pi/180;
x(n+l)=x(n)+step*sin(sa);
y(n+l)=y(n)+step*cos(sa);
theta=theta+sa;
n=n+l;
%A、B类障碍物都有的情况
else
while (((x(n)-XG)A2+(y(n)-YG)A2)>STEPA2)
dr 1 =mindetected((tl-theta),T1 ,N 1 ,T2,N2,x(n),y (n));
dr2=mindetectedquare((tl -theta),T3 ,N3 ,x(n),y(n));
de 1 =mindetected((t2-theta),T 1 ,N 1 ,T2,N2,x(n),y (n));
dc2=mindetectedquare((t2-theta),T3,N3,x(n),y(n));
dll=mindetected((t3-theta),Tl,Nl,T2,N2,x(n),y(n));
dl2=mindetectedquare((t3-theta),T3,N3,x(n),y(n));
dr=min(drl,dr2);
dc=min(dcl,dc2);
dl=min(dll,dl2);
tg=atan((XG-x(n))+i*(YG-y(n)))*180/pi-theta;
sa=fuzzylogic(dr,dc,dl,tg);
step=STER*(l-abs(sa)/100);
sa=sa*pi/180;
x(n+l)=x(n)+step*sin(sa);
y(n+l)=y(n)+step*cos(sa);
theta=theta+sa;
n=n+l;
%距离传感器的模拟
function dmin=mindetected(a,Tl,Nl,T2,N2,x,y)
dmin 1 = mindetected 1 (a,T l,Nl,x,y);
dmin2= mindetected2(a,T2,N2,x,y);
end
function dmin 1 ^mindetected 1 (a,T,N,x,y)
if(N==0)
dminl=100000;
else
d( 1 )=detectedline 1 (a,T( 1,1),T( l,2),x,y,T( 1,3),T( 1,4