文章目录1. 初始化参数2. 构建地图3. A*算法搜索路径4. 路径优化5. 效果图6. 下载链接

可以在这里看

画仆：A星算法详解(个人认为最详细,最通俗易懂的一个版本)zhuanlan.zhihu.com

在此主要解释下代码。

1. 初始化参数

主要参数:

地图大小起始点和目标点坐标clcclear allm = 30;n = 30;Spoint = [3 3]; %起始点坐标Epoint = [29 22]; %目标点坐标2. 构建地图

-inf表示不可到达的障碍物点

%%构建地图for i = 1:m+2if i == 1for j = 1:n+2Matrix(i,j) = -inf;endelseif i == m+2for j = 1:n+2Matrix(i,j) = -inf;endelsefor j = 1:n+2if ((j == 1)|(j == n+2))Matrix(i,j) = -inf;elseMatrix(i,j) = inf;endendendend%%障碍for j=2:10Matrix(5,j)=-inf;for j=2:15Matrix(24,j)=-inf;for j=9:24%for j=6:24Matrix(10,j)=-inf;for j=20:31Matrix(15,j)=-inf;for j=5:20Matrix(20,j)=-inf;for j=18:27Matrix(28,j)=-inf;for i=2:6Matrix(i,18)=-inf;for i=17:20Matrix(i,5)=-inf;for i=23:25Matrix(i,20)=-inf;for i=13:17Matrix(i,13)=-inf;endendendendendendendendendend%end% 显示地图%subplot(2,2,1);h1 = plot(Spoint(1),Spoint(2),'gO');hold onh2 = plot(Epoint(1),Epoint(2),'rO');3. A*算法搜索路径%%寻路Matrix(Spoint(1),Spoint(2))=0;Matrix(Epoint(1),Epoint(2))=inf;G=Matrix;F=Matrix;openlist=Matrix;closelist=Matrix;parentx=Matrix;parenty=Matrix;openlist(Spoint(1),Spoint(2)) =0;%closelist(Epoint(1),Epoint(2))=inf;for i = 1:n+2for j = 1:m+2k = Matrix(i,j);if(k == -inf)%subplot(2,2,1);h3 = plot(i,j,'k.');% elseif(k == inf) % show green feasible point% %subplot(2,2,1);% plot(i,j,'gh');% else% %subplot(2,2,1);% plot(i,j,'gh');endhold onendendaxis([0 m+3 0 n+3]);%subplot(2,2,1);plot(Epoint(1),Epoint(2),'b+');%subplot(2,2,1);plot(Spoint(1),Spoint(2),'b+');while(1)num=inf;for p=1:m+2for q=1:n+2if(openlist(p,q)==0&&closelist(p,q)~=1)Outpoint=[p,q];if(F(p,q)>=0&&num>F(p,q))num=F(p,q);Nextpoint=[p,q];endendendendcloselist(Nextpoint(1),Nextpoint(2))=1;for i = 1:3for j = 1:3k = G(Nextpoint(1)-2+i,Nextpoint(2)-2+j);if(i==2&&j==2|closelist(Nextpoint(1)-2+i,Nextpoint(2)-2+j)==1)continue;elseif (k == -inf)G(Nextpoint(1)-2+i,Nextpoint(2)-2+j) = G(Nextpoint(1)-2+i,Nextpoint(2)-2+j);closelist(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=1;elseif (k == inf)distance=((i-2)^2+(j-2)^2)^0.5;G(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=G(Nextpoint(1),Nextpoint(2))+distance;openlist(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=0;% H=((Nextpoint(1)-2+i-Epoint(1))^2+(Nextpoint(2)-2+j-Epoint(2))^2)^0.5;%欧几里德距离启发函数H_diagonal=min(abs(Nextpoint(1)-2+i-Epoint(1)),abs(Nextpoint(2)-2+j-Epoint(2)));%比较复杂的对角线启发函数H_straight=abs(Nextpoint(1)-2+i-Epoint(1))+abs(Nextpoint(2)-2+j-Epoint(2));H=sqrt(2)*H_diagonal+(H_straight-2*H_diagonal);% H=max(abs(Nextpoint(1)-2+i-Epoint(1)),abs(Nextpoint(2)-2+j-Epoint(2)));%比较简单的对角线函数F(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=G(Nextpoint(1)-2+i,Nextpoint(2)-2+j)+H;parentx(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=Nextpoint(1);parenty(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=Nextpoint(2);else distance=((i-2)^2+(j-2)^2)^0.5;if(k>(distance+G(Nextpoint(1),Nextpoint(2))))k=distance+G(Nextpoint(1),Nextpoint(2));% H=((Nextpoint(1)-2+i-Epoint(1))^2+(Nextpoint(2)-2+j-Epoint(2))^2)^0.5; %欧几里德距离启发函数H_diagonal=min(abs(Nextpoint(1)-2+i-Epoint(1)),abs(Nextpoint(2)-2+j-Epoint(2)));%比较复杂的对角线启发函数H_straight=abs(Nextpoint(1)-2+i-Epoint(1))+abs(Nextpoint(2)-2+j-Epoint(2));H=sqrt(2)*10*H_diagonal+10*(H_straight-2*H_diagonal);% H=max(abs(Nextpoint(1)-2+i-Epoint(1)),abs(Nextpoint(2)-2+j-Epoint(2)));%比较简单的对角线函数F(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=k+H;parentx(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=Nextpoint(1);parenty(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=Nextpoint(2);endendif(((Nextpoint(1)-2+i)==Epoint(1)&&(Nextpoint(2)-2+j)==Epoint(2))|num==inf)parentx(Epoint(1),Epoint(2))=Nextpoint(1);parenty(Epoint(1),Epoint(2))=Nextpoint(2);break;endendif(((Nextpoint(1)-2+i)==Epoint(1)&&(Nextpoint(2)-2+j)==Epoint(2))|num==inf)parentx(Epoint(1),Epoint(2))=Nextpoint(1);parenty(Epoint(1),Epoint(2))=Nextpoint(2);break;endendif(((Nextpoint(1)-2+i)==Epoint(1)&&(Nextpoint(2)-2+j)==Epoint(2))|num==inf)parentx(Epoint(1),Epoint(2))=Nextpoint(1);parenty(Epoint(1),Epoint(2))=Nextpoint(2);break;endendP=[];s=1;while(1)if(num==inf)break;end%subplot(2,2,1);h4 = plot(Epoint(1),Epoint(2),'b+');P(s,:)=Epoint;s=s+1;% pause(1);xx=Epoint(1);Epoint(1)=parentx(Epoint(1),Epoint(2));Epoint(2)=parenty(xx,Epoint(2));if(parentx(Epoint(1),Epoint(2))==Spoint(1)&&parenty(Epoint(1),Epoint(2))==Spoint(2))%subplot(2,2,1);plot(Epoint(1),Epoint(2),'b+');P(s,:)=Epoint;break;endendP(s+1,:)=Spoint;legend([h1,h2,h3,h4],'起始点','目标点','障碍物','航迹点');count=0;for i=2:12for j=2:12if(G(i,j)~=inf&&G(i,j)~=-inf)count=count+1;endendendcount4. 路径优化%将得到的折现曲线拟合成光滑的曲线P=P';a=[];b=[];a=P(1,:);b=P(2,:);figure%subplot(2,2,3);plot(a,b);axis([0,n+3,0,n+3]);values = spcrv([[a(1) a a(end)];[b(1) b b(end)]],3);figure%subplot(2,2,4);plot(values(1,:),values(2,:),'r');axis([0,m+3,0,m+3]);5. 效果图

A*路径

优化后路径

6. 下载链接

直接复制到matlab即可使用，或者也可以点击下载

如果觉得有用可以点赞收藏哦~~~