设计九 基模糊推理的自主移动机器人的避障控制.docVIP

附录 设计过程和程序清单 参考程序如下： function[x,y]=obstacleavoidance(T1,N1,T2,N2,T3,N3,X0,Y0,XG,YG,STEP) hold on x(1)=X0; y(1)=Y0; t1=pi/4; t2=pi/2; t3=3*pi/4; theta=0; n=1; if((N3==0)((N1+N2)==0)) error %只有A类障碍物的情况 else if((N3==0)((N1+N2)~=0)) while(((x(n)-XG)^2+(y(n)-YG)^2)STEP^2) dr=mindetected((t1-theta),T1,N1,T2,N2,x(n),y(n)); dc=mindetected((t2-theta),T1,N1,T2,N2,x(n),y(n)); dl=mindetected((t3-theta),T1,N1,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*(1-abs(sa)/100); sa=sa*pi/180; x(n+1)=x(n)+step*sin(sa); y(n+1)=y(n)+step*cos(sa); theta=theta+sa; n=n+1; end %只有B类障碍物的情况 else if(((N1+N2)==0)(N3~=0)) while (((x(n)-XG)^2+(y(n)-YG)^2)STEP^2) dr=mindetectedquare((t1-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*(1-abs(sa)/100); sa=sa*pi/180; x(n+1)=x(n)+step*sin(sa); y(n+1)=y(n)+step*cos(sa); theta=theta+sa; n=n+1; end %A、B类障碍物都有的情况 else while (((x(n)-XG)^2+(y(n)-YG)^2)STEP^2) dr1=mindetected((t1-theta),T1,N1,T2,N2,x(n),y(n)); dr2=mindetectedquare((t1-theta),T3,N3,x(n),y(n)); dc1=mindetected((t2-theta),T1,N1,T2,N2,x(n),y(n)); dc2=mindetectedquare((t2-theta),T3,N3,x(n),y(n)); dl1=mindetected((t3-theta),T1,N1,T2,N2,x(n),y(n)); dl2=mindetectedquare((t3-theta),T3,N3,x(n),y(n)); dr=min(dr1,dr2); dc=min(dc1,dc2); dl=min(dl1,dl2); tg=atan((XG-x(n))+i*(YG-y(n)))*180/pi-theta; sa=fuzzylogic(dr,dc,dl,tg); step=STER*(1-abs(sa)/100); sa=sa*pi/180; x(n+1)=x(n)+step*sin(sa); y(n+1)=y(n)+step*cos(sa); theta=theta+sa; n=n+1; end end %距离传感器的模拟 function dmin=mindetected(a,T1,N1,T2,N2,x,y) dmin1= mindetected1(a,T1,N1,x,y); dmin2= mindetected2(a,T2,N2,x,y); end function dmin1=mindete