畢業(yè)設(shè)計(jì)基于模糊推理的自主移動機(jī)器人避障控制-資料下載頁
2025-06-28 11:09本頁面
【正文】 theta。 %計(jì)算轉(zhuǎn)向角、步長、移動后坐標(biāo) sa=fuzzylogic(dr,dc,dl,tg)。 step=STEP*(1abs(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 while(((x(n)XG)^2+(y(n)YG)^2)STEP^2) dr1=mindetected((t1theta),T1,N1,T2,N2,x(n),y(n))。 dr2=mindetectedquare((t1theta),T3,N3,x(n),y(n))。 dc1=mindetected((t2theta),T1,N1,T2,N2,x(n),y(n))。 dc2=mindetectedquare((t2theta),T3,N3,x(n),y(n))。 dl1=mindetected((t3theta),T1,N1,T2,N2,x(n),y(n))。 dl2=mindetectedquare((t3theta),T3,N3,x(n),y(n))。 %距離取最小值 dr=min(dr1,dr2)。 dc=min(dc1,dc2)。 dl=min(dl1,dl2)。 dg=atan((XGx(n))+i*(YGy(n)))*180/pitheta。 %計(jì)算轉(zhuǎn)向角、步長、移動后坐標(biāo) sa=fuzzylogic(dr,dc,dl,tg)。 step=STEP*(1abs(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。 endend%距離傳感器的模擬%對直線段障礙物邊界求最小距離function dmin=mindetected(a,T1,N1,T2,N2,x,y)dmin1=mindetected1(a,T1,N1,x,y)。dmin2=mindetected2(a,T2,N2,x,y)。dmin=min(dmin1,dmin2)。endfunction dmin1=mindetected1(a,T,N,x,y)if(N==0) dmin1=100000。else d(1)=detectedline1(a,T(1,1),T(1,2),x,y,T(1,3),T(1,4))。 dmin1=d(1)。 j=1。 for i=2:N d(i)=detectedline1(a,T(i,1),T(i,2),x,y,T(i,3),T(i,4))。 if(d(i)dmin1)j=i。 dmin1=d(j)。 end endendfunction t=detectedline1(a,b,k,x,y,minx,miny)if(tan(a)~=k) t=(b+k*xy)/(sin(a)k*cos(a))elseif(tan(a)==k) t=(b+k*xy)*100000。endif(((cos(a)*t+x)maxx)|((cos(a)*t+x)minx)|(t0)) t=100000。endfunction dmin2=mindetected2(a,T,N,x,y)if(N==0) dmin2=100000。else d(1)=detectedline2(a,T(1,1),x,y,T(1,2),T(1,3))。 dmin2=d(1)。 j=1。 for i=2:N d(i)=detectedline2(a,T(i,1),x,y,T(i,2),T(i,3))。 if(d(i)dmin2)j=i。 dmin2=d(j)。 end endendfunction t=detectedline2(a,b,x,y,minx,miny)if(tan(a)~=0) t=(bx)/cos(a)。else t=(bx)*100000。endif(((sin(a)*t+y)maxy)|((sin(a)*t+y)miny)|(t0)) t=100000。end%對二次曲線障礙物邊界求最小距離function dmin=mindetected(a,T,N,x,y)[t(1,:),d(1)]=detectedquare(a,T(1,1),T(1,2),T(1,3),T(1,4),T(1,5),x,y)。dmin=d(1)。j=1。for i=2:N [t(i,:),d(i)]=detectedquare(a,T(i,1),T(i,2),T(i,3),T(i,4),T(i,5),x,y)。 if(d(i)dmin)j=i。 dmin=d(j)。 endendfunction [t,d]=detectedquare(a,A,B,C,D,E,x,y)%二次曲線障礙物邊界探測程序c=xC。e=yD。x1=A*xos(a)^2+B*cos(a)^2。x2=(A*c*cos(a)+B*e*sin(a))。x3=A*c^2+B*e^2E。p=[x1 x2 x3]。t=roots(p)。if(t(1)=t(2)) d=t(1)。else d=t(2)。endif(imag(d)~=0|d0) d=100000。end%模糊控制器的實(shí)現(xiàn)function sa=fuzzylogic(dr,dc,dl,tg)%輸入變量模糊化[rnear,rfar]=dmembership(dr)。[ear,cfar]=dmembership(dc)。[lnear,lfar]=dmembership(dl)。[lb,ls,zo,rs,rb]=tmembership(tg)。d1=rfar*cfar*lfar。d2=rfar*cfar*lnear。d3=rfar*ear*lnear。d4=rnear*ear*lnear。d5=rfar*ear*lfar。d6=rnear*cfar*lfar。d7=rnear*ear*lfar。d8=rnear*cfar*lnear。%模糊規(guī)則庫對應(yīng)的數(shù)學(xué)實(shí)現(xiàn)（共56條規(guī)則）tlb=(d1+d5+d6+d7+d8)*lb+d4+d7*ls。 %1,16~21,26,31,32,36,共11條tls=(d1+d5+d7)*ls+(d5+d7)*zo+d7*(rs+rb)。 %2,22,23,27,33,34,35,共7條tz=(d1+d2+d6+d8)*zo+d2*lb+(d2+d8)*ls+(d6+d8)*rs+d6*rb。 %3,6,7,8,28,29,30,37,38,39,共10條trs=(d1+d2+d5)*rs+d3*(lb+ls+zo)。 %4,9,11,12,13,24,共6條trb=(d1+d2+d3+d5+d8)*rb+d3*rs。 %5,10,14,15,25,40,共6條%去模糊化，面積中心法b=[100/3 20 0 20 100/3]。 %輸出變量隸屬度函數(shù)重心向量a(1)=10*(2*tlbtlb^2)。 %對應(yīng)面積計(jì)算a(2)=20*(2*tlstls^2)。a(3)=20*(2*tztz^2)。a(4)=20*(2*trstrs^2)。a(5)=10*(2*trbtrb^2)。sa=(b*a39。)/sum(a)。 %輸出變量控制角的計(jì)算%障礙物距離隸屬度函數(shù)function [near,far]=dmembership(dmin)if((dmin=0)amp。amp。(dmin=1)) near=1。 far=0。elseif((dmin1)amp。amp。(dmin2)) near=2dmin。 far=dmin1。endend%目標(biāo)方向角隸屬度函數(shù)function[lb,ls,zo,rs,rb]=tmembership(tg)if((tg=180)amp。amp。(tr=60)) ib=1。 ls=0。 zo=0。 rs=0。 rb=0。elseif((tg60)amp。amp。(tg=30)) lb=tg/301。 ls=tg/30+2。 z0=0。 rs=0。 rb=0。elseif((tg30)amp。amp。(tg=0)) lb=0。 ls=tg/30。 zo=tg/30+1。 rs=0。 rb=0。elseif((tg0)amp。amp。(tg=30)) lb=0。 ls=0。 zo=tg/30+1。 rs=tg/30。 rb=0。elseif((tg30)amp。amp。(tg=60)) lb=0。 ls=0。 zo=0。 rs=tg/30+2。 rb=tg/301。elseif((tg60)amp。amp。(tg=180)) lb=0。 ls=0。 zo=0。 rs=0。 rb=1。end%轉(zhuǎn)向角隸屬度函數(shù)if((sa=40)amp。amp。(sa=20)) tlb=sa/201。 tls=sa/20+2。 tz=0。 trs=0。 tlb=0。elseif((sa20)amp。amp。(sa=0)) tlb=0。 tls=sa/20。 tz=sa/20+1。 trs=0。 trb=0。elseif((sa0)amp。amp。(sa=20)) tlb=0。 tls=0。 tz=sa/20+1。 trs=sa/20。 trb=0。elseif((sa20)amp。amp。(sa=40)) tlb=0。 tls=0。 tz=0。 trs=sa/20+2。 trb=sa/201。end 41
環(huán)評公示相關(guān)推薦
鄂ICP備17016276號-1