A星算法matlab代码

clc clear close 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))0 break end end %% 倒推路径 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; end Path=flipud(Path); %% 画图 [m,n]=size(map); for i=0:m plot([0 n],[i i],'-k'); hold on end for j=0:n plot([j j],[0 m],'-k'); hold on end for 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 end end plot(S(1),S(2),'.g','MarkerSize',20) hold on text(S(1),S(2),'起点') hold on plot(E(1),E(2),'.m','MarkerSize',20) hold on text(E(1),E(2),'终点') hold on plot(Path(:,1),Path(:,2),'-r','LineWidth',2)