Anyone can help me? I change the 2 dimension to 3 dimension for path planning system

1 次查看（过去 30 天）

eduard tulus 2020-9-4

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/588826-anyone-can-help-me-i-change-the-2-dimension-to-3-dimension-for-path-planning-system

dh=.4; 
X=-7:dh:5;
Y=-7:dh:5;
Z=-7:dh:5;
[X,Y,Z]=meshgrid(X,Y,Z);
Z=X+Y;
x0=[0;4;5]; %koordinat mulai
xT=[0;-4;-5]; %koordinat finish
 
Kr=30; % nilai yang mengendalikan bentuk permukaan di sekitar permukaan
Vr=Kr./sqrt((X-x0(1)).^2+(Y-x0(2)).^2+(Z-x0(3)).^2); % rumus dan proses menghitung gaya tarik pada posisi awal
 
[fxr,fyr,fzr]=gradient(Vr,dh);
Ka=20; % nilai yang mengendalikan bentuk permukaan di sekitar permukaan
Va=.5.*Ka.*((X-xT(1)).^2+(Y-xT(2)).^2+(Z-xT(3)).^2); % rumus dan perhitungan  gaya tolak pada posisi awal
 
[fxa,fya,fza]=gradient(Va,dh);
 %untuk medan objek yang berdekatan
 cx1=1;
 cy1=0;
cz1=2;
cx2=-1;
cy2=0;
cz2=-2;
untuk medan objek yang berjauhan
cx1=3;
cy1=-1;
cz1=-1;
cx2=-1;
cy2=0;
cz2=1;
rad=2+.1;
th=(0:.1*dh:2).*pi;
xo=rad.*cos(th);
yo=rad.*sin(th);
zo=rad.*cos(th); 
% 
xo1=xo-cx1;
yo1=yo-cy1;
zo1=zo-cz1;
xo2=xo-cx2;
yo2=yo-cy2;
z02=zo-cz2;
% 
Vo1=[];
Vo2=[];
Vo=zeros(size(Vr));
 
Ko=7; % nilai bentuk permukaan
 
for i=1:length(th)  %Proses perhitungan gaya tolak menolak pada hambatan
 Vo1=Ko./sqrt((X-xo1(i).^2+(Y-yo1(i)).^2 +(Z-zo1(i)).^2)); % proses perhitungan gaya tolak menolak di sekitar hambatan 1
 Vo2=Ko./sqrt((X-xo2(i).^2+(Y-yo2(i)).^2 +(Z-zo2(i)).^2));
 Vo=Vo+Vo1+Vo2;% Total perhitungan gaya tolak menolak disekitar hambatan 1 dan 2
% end
% 
[fxo,fyo,fzo]=gradient(Vo);%Gaya buatan
% 
 fX=-fxr-fxa; %  gaya total pada sumbu x tanpa fo
 fY=-fyr-fya; %  gaya total pada sumbu y tanpa fo
 fZ=-fzr-fza;  % gaya total pada sumbu z tanpa fo
  quiver(X,Y,Z,fX,fY,fZ,'k')
 
 title('Potential Field of Area')
 xlabel('X-Axis')
 ylabel('Y-Axis')
 zlabel('Z-Axis')
 hold on
 plot(x0(1),x0(2),x0(3),'ro',xT(1),xT(2),xT(3),'ro')
 fill(xo1,yo1,zo1,'b',xo2,yo2,zo2,'b')
 plot(xo1,yo1,zo1,'b',xo2,yo2,zo2,'b')
 hold off
% 
 ss=2;
 k=2;
 
 xp=[];
 yp=[];
 zp=[];
 xp(2)=x0(1);
 yp(2)=xo(2);
 zp(2)=xo(3);
 ix=[];
 iy=[];
 iz=[];
 jx=[];
 jy=[];
 jz=[];
 fxx=0;
 fyy=0;
 fzz=0;
 
  while ss<=1
  Pw=sqrt((X-xp(k)).^2)+(Y-yp(k).^2+(Z-zp(k).^2));
 xw(k)=min(min(Pw));
 [iix,iiy,iiz]=find(Pw==xw(k));
 ix(k)=iix(1);
 iy(k)=iiy(1);
 iz(k)=iiz(1);
 
 fx1=fX(ix(k),iy(k),iz(k));
 fy1=fY(ix(k),iy(k),iz(k));
 fz1=fZ(ix(k),iy(k),iz(k));
 fxx(k)=fx1./norm(fX);
 fyy(k)=fy1./norm(fY);
      
 ff(k,:)=[fX(ix(k),iy(k),iz(k)),fY(ix(k),iy(k),iz(k)),fZ(ix(k),iy(k),iz(k))];
 
 xp(k+1)=xp(k)+dh*(fxx(k));
 yp(k+1)=yp(k)+dh*(fyy(k));
 zp(k+1)=zp(k)+dh*(fzz(k));
      
      if(sqrt((xp(k+1)-xT(1)).^2+(yp(k+1)-xT(2)).^2+(zp(k+1)-xT(3)).^2)<=0.4)
          ss=0;
     end
      k=k+1;
 
  
  hold 
 plot((xp),(yp),(zp),'r')
  
 figure
surf(X,Y,Z,(Vr)+(Va)) %tidak ada Vo
 title ('Perkiraan Lintasan 3D')