Диссертация (1025035), страница 20

Просмтор этого файла доступен только зарегистрированным пользователям. Но у нас супер быстрая регистрация: достаточно только электронной почты!

Текст из файла (страница 20)

2. Программы Matlab алгоритма отслеживания запланированногомаршрутаfunction xxx=circlex(u)X0=u(1);Y0=u(2);Z0=u(3);vx=u(4);vy=u(5);vz=u(6);kk=u(7);vec2=[vx,vy,vz];num=size(xx,2);ddd=sqrt((xx-X0).^2+(yy-Y0).^2+(zz-Z0).^2);mind=min(ddd);for j=1:numif ddd(j)>sqrt(mind*5+1)&&j>=kkbreakendendV=sqrt(vx^2+vy^2+vz^2);vec1=[xx(j)-X0,yy(j)-Y0,zz(j)-Z0];angle=anglevec(vec1(1),vec1(2),vec1(3),vec2(1),vec2(2),vec2(3));dis=sqrt((xx(j)-X0)^2+(yy(j)-Y0)^2+(zz(j)-Z0)^2);xreal=xx(j);yreal=yy(j);zreal=zz(j);ac=2*V^2*sin(angle)/dis;V1=vec1(1);V2=vec1(2);V3=vec1(3);x=(1/(V1^2*vy^2 + V1^2*vz^2 - 2*V1*V2*vx*vy - 2*V1*V3*vx*vz + V2^2*vx^2 + ...V2^2*vz^2 - 2*V2*V3*vy*vz + V3^2*vx^2 + V3^2*vy^2))^(1/2)*(V2*vz - V3*vy);y= -(1/(V1^2*vy^2 + V1^2*vz^2 - 2*V1*V2*vx*vy - 2*V1*V3*vx*vz + V2^2*vx^2 + ...V2^2*vz^2 - 2*V2*V3*vy*vz + V3^2*vx^2 + V3^2*vy^2))^(1/2)*(V1*vz - V3*vx);z=(1/(V1^2*vy^2 + V1^2*vz^2 - 2*V1*V2*vx*vy - 2*V1*V3*vx*vz + V2^2*vx^2 + ...V2^2*vz^2 - 2*V2*V3*vy*vz + V3^2*vx^2 + V3^2*vy^2))^(1/2)*(V1*vy - V2*vx);ax1=(ac/(vx^2*y^2 + vx^2*z^2 - 2*vx*vy*x*y - 2*vx*vz*x*z + vy^2*x^2 + ...vy^2*z^2 - 2*vy*vz*y*z + vz^2*x^2 + vz^2*y^2))^(1/2)*(vz*y - vy*z);ax2=-(ac/(vx^2*y^2 + vx^2*z^2 - 2*vx*vy*x*y - 2*vx*vz*x*z + vy^2*x^2 + ...vy^2*z^2 - 2*vy*vz*y*z + vz^2*x^2 + vz^2*y^2))^(1/2)*(vz*y - vy*z);ay1=-(ac/(vx^2*y^2 + vx^2*z^2 - 2*vx*vy*x*y - 2*vx*vz*x*z + vy^2*x^2 + ...vy^2*z^2 - 2*vy*vz*y*z + vz^2*x^2 + vz^2*y^2))^(1/2)*(vz*x - vx*z);ay2=(ac/(vx^2*y^2 + vx^2*z^2 - 2*vx*vy*x*y - 2*vx*vz*x*z + vy^2*x^2 + ...vy^2*z^2 - 2*vy*vz*y*z + vz^2*x^2 + vz^2*y^2))^(1/2)*(vz*x - vx*z);az1=(ac/(vx^2*y^2 + vx^2*z^2 - 2*vx*vy*x*y - 2*vx*vz*x*z + vy^2*x^2 + ...vy^2*z^2 - 2*vy*vz*y*z + vz^2*x^2 + vz^2*y^2))^(1/2)*(vy*x - vx*y);az2=-(ac/(vx^2*y^2 + vx^2*z^2 - 2*vx*vy*x*y - 2*vx*vz*x*z + vy^2*x^2 + ...vy^2*z^2 - 2*vy*vz*y*z + vz^2*x^2 + vz^2*y^2))^(1/2)*(vy*x - vx*y);angle1=anglevec(ax1,ay1,az1,(xreal-X0),(yreal-Y0),(zreal-Z0));angle2=anglevec(ax2,ay2,az2,(xreal-X0),(yreal-Y0),(zreal-Z0));if angle1<angle2ax=ax1;ay=ay1;az=az1;elseax=ax2;ay=ay2;az=az2;endxxx=[ax;ay;az;mind;j];П.

3. ПрограммыMatlab152алгоритмапланированияглобальногомаршрутаfunction s=OAC(u)Cx=u(1);Cx=u(2);Cx=u(3);Gx=u(4);Gx=u(5);Gx=u(6);CCoor1=[]; CCoor2=[];CCoor3=[];for i=1:size(coordinates,2)CCoor1=[CCoor1,coordinates{1,i}];CCoor2=[CCoor2,coordinates{2,i}];CCoor3=[CCoor3,coordinates{3,i}];endCCoor=[Cx,CCoor1;Cy,CCoor2;Cz,CCoor3];CCC=[];for ff=1:size(CCoor,2)if CCoor(1,ff)<0 || CCoor(2,ff)<0CCC=[CCC,ff];endendCCoor(:,CCC)=[];numb=find(CCoor(1,:)==pointsG(1) & CCoor(2,:)==pointsG(2) & CCoor(3,:)==pointsG(3));K=1;M=1;S=1;E=numb;Alpha=1 Beta=100;Zeta=20;Cya=5;Rho=0.95; Q=10;mm=size(CCoor,2);Tau=ones(mm,mm);N=size(CCoor,2);Ex=CCoor(1,E);Ey=CCoor(2,E);Ez=CCoor(3,E);Eta=zeros(1,N);Gama=zeros(1,N);Haya=zeros(1,N);for i=1:Nix=CCoor(1,i); iy=CCoor(2,i);iz=CCoor(3,i);if i~=EEta(1,i)=1/((ix-Ex)^2+(iy-Ey)^2+(iz-Ez)^2)^0.5;elseEta(1,i)=1000;Gama(1,i)=0.1;Haya(1,i)=0.1;endendGama(1,1)=1;Haya(1,1)=1;ROUTES=cell(K,M);PL=zeros(K,M);for k=1:Kfor m=1:MW=S;DD=G2D2(CCoor,Rx,Ry,H,W,CCx,CCy);Path=S;PLkm=0;TABUkm=ones(1,N);TABUkm(S)=0;DW=DD(1,:); DW1=find(DW<inf);for j=1:length(DW1)if TABUkm(DW1(j))==0DW(j)=inf;endendLJD=find(DW<inf);Len_LJD=length(LJD);while W~=E && Len_LJD>=1PP=zeros(1,Len_LJD); Haya(1,i)=1/((ix-Cx)^2+(iy-Cy)^2)^0.5;for i=1:Len_LJDHaya=1/((CCoor(1,W)-CCoor(1,LJD(i)))^2+(CCoor(2,W)...-CCoor(2,LJD(i)))^2+(CCoor(3,W)-CCoor(3,LJD(i)))^2);Gama=anglevec(CCoor(1,LJD(i))-CCoor(1,W),CCoor(2,LJD(i))...153-CCoor(2,W),CCoor(3,LJD(i))-CCoor(3,W),CCoor(1,E)...-CCoor(1,W),CCoor(2,E)-CCoor(2,W),CCoor(3,E)-CCoor(3,W));PP(i)=(Tau(W,LJD(i))^Alpha)*(Eta(LJD(i))^Beta)*Gama...*Zeta*Haya^Cya;endPP=PP/(sum(PP));Pcum=cumsum(PP);Select=find(Pcum>=rand);to_visit=LJD(Select(1));Path=[Path,to_visit];PLkm=PLkm+DD(1,to_visit);W=to_visit;DD=G2D2(CCoor,Rx,Ry,H,W,CCx,CCy);for kk=1:Nif TABUkm(kk)==0DD(1,kk)=inf;endendTABUkm(W)=0;DW=DD(1,:);DW1=find(DW<inf);for j=1:length(DW1)if TABUkm(DW1(j))==0DW(j)=inf;endendLJD=find(DW<inf);Len_LJD=length(LJD);endROUTES{k,m}=Path;if Path(end)==EPL(k,m)=PLkm;elsePL(k,m)=inf;endendDelta_Tau=zeros(N,N);for m=1:Mif PL(k,m)<infROUT=ROUTES{k,m};TS=length(ROUT)-1;PL_km=PL(k,m);for s=1:TSxxx=ROUT(s);yyy=ROUT(s+1);Delta_Tau(xxx,yyy)=Delta_Tau(xxx,yyy)+Q/PL_km;Delta_Tau(yyy,xxx)=Delta_Tau(yyy,xxx)+Q/PL_km;endendendTau=(1-Rho).*Tau+Rho*Delta_Tau;endplotif=1;if plotif==1ROUT=ROUTES{K,M};LENROUT=length(ROUT);Rxxx=ROUT;Ryyy=ROUT;Rzzz=ROUT;for ii=1:LENROUTRxxx(ii)=CCoor(1,ROUT(ii));Ryyy(ii)=CCoor(2,ROUT(ii));Rzzz(ii)=CCoor(3,ROUT(ii));endends=[Rxxx,Ryyy,Rzzz];154155156157158.

Характеристики

Список файлов диссертации