A星算法matlab代码
资源内容介绍
A星算法matlab代码 clcclearclose all%% 设置地图map=[0 0 0 0 0;0 0 1 1 0;1 0 0 1 0;0 1 0 1 0;0 0 0 1 0;0 0 0 0 0];%地图S=[0.5 0.5]; %起点E=[4.5 5.5]; %终点G=inf*ones(size(map));%实际代价P=cell(size(map)); %根节点B=zeros(size(map)); %关闭K=zeros(size(map)); %开放Limit=cal(map);%% 处理起点J_S=Change(map,S,1);K(J_S(1),J_S(2))=1;G(J_S(1),J_S(2))=0;%% 循环寻路while 1 [chx,chy]=find(K==1); if isempty(chx) disp('未找到路径') break end W=[]; for i=1:size(chx,1) W=[W,G(chx(i),chy(i))]; end [~,index]=min(W); index=index(1); chW=[chx(index),chy(index)]; J_chW=Change(map,chW,-1); DK=[J_chW(1)-1,J_chW(2); J_chW(1)-1,J_chW(2)-1; J_chW(1)-1,J_chW(2)+1; J_chW(1)+1,J_chW(2); J_chW(1)+1,J_chW(2)-1; J_chW(1)+1,J_chW(2)+1; J_chW(1),J_chW(2)-1; J_chW(1),J_chW(2)+1]; Ju=zeros(1,size(DK,1)); for i=1:size(DK,1) if judge(DK(i,:),Limit,map,B)==1 Ju(i)=1; J_DK=Change(map,DK(i,:),1); K(J_DK(1),J_DK(2))=1; if norm(chW-DK(i,:))+G(chW(1),chW(2))<G(J_DK(1),J_DK(2)) G(J_DK(1),J_DK(2))=norm(chW-DK(i,:))+G(chW(1),chW(2)); P(J_DK(1),J_DK(2))={chW}; end end end B(chW(1),chW(2))=1; K(chW(1),chW(2))=0; G(chW(1),chW(2))=inf; if sum(Ju)~=0 DK=DK(Ju==1,:); Jp=DK-repmat(E,size(DK,1),1); SJP=sum(Jp'); end if sum(SJP==0)>0 break endend%% 倒推路径Pe=Change(map,E,1);Ps=Change(map,S,1);Pn=Pe;Path=E;while sum(abs(Pn-Ps))~=0 CH_p=P{Pn(1),Pn(2)}; Path=[Path;Change(map,CH_p,-1)]; Pn=CH_p;endPath=flipud(Path);%% 画图[m,n]=size(map);for i=0:m plot([0 n],[i i],'-k'); hold onendfor j=0:n plot([j j],[0 m],'-k'); hold onendfor i=1:m for j=1:n if map(i,j)==1 fill([j-0.5-0.5 j-0.5+0.5 j-0.5+0.5 j-0.5-0.5],[m-i+0.5-0.5 m-i+0.5-0.5 m-i+0.5+0.5 m-i+0.5+0.5],'k') hold on end endendplot(S(1),S(2),'.g','MarkerSize',20)hold ontext(S(1),S(2),'起点')hold onplot(E(1),E(2),'.m','MarkerSize',20)hold ontext(E(1),E(2),'终点')hold onplot(Path(:,1),Path(:,2),'-r','LineWidth',2)