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

clear all; %??????????????????,?????????????????????????? %??????? Xo=[0 0];%???? k=15;%??????????? K=0;%??? m=5;%?????????????????? Po=2.5;%??????????????????????????0?????????????????? n=7;%???? a=0.5; l=0.1;%?? J=200;%?????? %???????????????????????Po????????? %end %????????? Xsum=[10 10;1 1.2;3 2.5;4 4.5;3 6;6 2;5.5 5.5;8 8.5];%?????(n+1)*2????[10 10]????????????????? Xj=Xo;%j=1??????????????Xj %***************????????????****************** 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; %?? if ((Xj(1)-Xsum(1,1))>0)&((Xj(2)-Xsum(1,2))>0)%?????????????????????????????????????? K=j;%?????????????? break; %?????j? end%?????if???????????????? end%????? K=j; Goal(K,1)=Xsum(1,1);%???????????????? Goal(K,2)=Xsum(1,2); %***********************************??????????????************************* %???? X=Goal(:,1); Y=Goal(:,2); %????Goal?????,X,Y??????x,y?????????????? x=[1 3 4 3 6 5.5 8];%???x?? y=[1.2 2.5 4.5 6 2 5.5 8.5]; plot(x,y,'o',10,10,'v',0,0,'ms',X,Y,'.r'); img =gcf; %获取当前画图的句柄 print(img, '-dpng', '-r600', './运行结果.png') %即可得到对应格式和期望dpi的图像