【路径规划-二维路径规划】基于人工势场求解机器人路径规划问题5附matlab代码 上传.zip

clear all; x=[1 3 4 7 6 5.5 8 9.5];%???x?? y=[1.5 2.2 4.5 6 2 6 7.8 7]; plot(x,y,'O',10,10,'v',0,0,'ms'); hold on %??????????????????,?????????????????????????? %??????? Xo=[0 0];%???? k=20;%??????????? K=0;%??? m=5;%?????????????????? Po=2;%??????????????????????????0?????????????????? n=8;%???? a=0.5; l=0.05;%?? J=500;%?????? %???????????????????????Po????????? %end %????????? Xsum=[10 10;1 1.5;3 2.2;4 4.5;7 6;6 2;5.5 6;8 7.8;9.5 7];%?????(n+1)*2????[10 10]????????????????? Xj=Xo;%j=1??????????????Xj [x,y]=meshgrid(-1:0.5:12,-1:0.5:12); z=0.5*k./(sqrt((x-10).^2+(y-10).^2+0.09))-0.5*m*(1./(sqrt((x-1).^2+(y-1.5).^2+0.09))-1/3.5).^2-0.5*m*(1./(sqrt((x-3).^2+(y-2.2).^2+0.09))-1/3.5).^2 ... -0.5*m*(1./(sqrt((x-4).^2+(y-4.5).^2+0.09))-1/3).^2-0.5*m*(1./(sqrt((x-7).^2+(y-6).^2+0.09))-1/3).^2-0.5*m*(1./(sqrt((x-6).^2+(y-2).^2+0.09))-1/3.5).^2 ... -0.5*m*(1./(sqrt((x-5.5).^2+(y-6).^2+0.09))-1/4).^2-0.5*m*(1./(sqrt((x-8).^2+(y-7.8).^2+0.09))-1/3).^2-0.5*m*(1./(sqrt((x-9.5).^2+(y-7).^2+0.09))-1/3).^2; % contour(x,y,z,[-50:20:1000]); [C,h]=contour(x,y,z,[-80:10:300]); set(h,'ShowText','on','TextStep',get(h,'LevelStep')*2) colormap cool [px,py]=gradient(z);%????x,y?????????? quiver(x,y,px,py,'k') %??????????? p=sqrt(px.^2+py.^2); %%?? t=1; M(t)=getframe; t=t+1; %***************????????????****************** for j=1:J%???? Goal(j,1)=Xj(1);%Goal??????????????????????????? Goal(j,2)=Xj(2); %???????? Theta=compute_angle(Xj,Xsum,n);%Theta??????????????????X???????????????????????????????? %???????? Angle=Theta(1);%Theta?1???????????????????? angle_at=Theta(1);%???????????????????angle_at [Fatx,Faty]=compute_Attract(Xj,Xsum,k,Angle,0,Po,n);%???????????x,y????????? for i=1:n angle_re(i)=Theta(i+1);%?????????????????n??????n???? end %???????? [Frerxx,Freryy,Fataxx,Fatayy]=compute_repulsion(Xj,Xsum,m,angle_at,angle_re,n,Po,a);%??????x,y???????? %????????????????????j??????????????????????????????????????????????? Fsumyj=Faty+Freryy+Fatayy;%y????? Fsumxj=Fatx+Frerxx+Fataxx;%x????? Position_angle(j)=atan(Fsumyj/Fsumxj);%???x???????? %????????? Xnext(1)=Xj(1)+l*cos(Position_angle(j)); Xnext(2)=Xj(2)+l*sin(Position_angle(j)); %????????????? Xj=Xnext; X=Goal(:,1); Y=Goal(:,2); plot(X,Y,'.r'); M(t)=getframe;t=t+1; %?? if ((Xj(1)-Xsum(1,1))>0)&((Xj(2)-Xsum(1,2))>0)%?????????????????????????????????????? K=j;%?????????????? break; %?????j? end%?????if???????????????? end%?????