智能算法+A星算法+六边形栅格

clc clear close all load('data.mat') model1=[0.1 0.5 1 0.8 1 1]; model2=[0.8 0.1 1 0.5 1 1]; w=[0.34 0.33 0.33];%总评价指数修正系数 select_n=2;%选择A或B棋子 if select_n==1 model=model1; else model=model2; end fai=0.8;%深海路径指数 figure(1) hold on axis([h_x(1,1)-1.1*l_six h_x(1,end)+1.1*l_six h_y(1,1)-1.1*l_six h_y(end,2)+1.1*l_six]) axis off %% 设置图的大小和位置 set(gcf,'position',[200,10,650*col/row,750]); yanse={'b','b','y','b','g','k'};%海域颜色 face_num=[1,0.6,1,0.2,1,0.8];%海域透明度 for i=1:row for j=1:col x=h_x(i,j)+l_six*cos(alphy); y=h_y(i,j)+l_six*sin(alphy); h=fill(x,y,yanse{h_data(i,j)}); set(h,'facealpha',face_num(h_data(i,j))) if threaten_data(i,j)==1 h=fill(x,y,'r','edgealpha',0); set(h,'facealpha',0.6) end text(h_x(i,j)-l_six/2.7,h_y(i,j)+l_six*0.6,[num2str(i),'-',num2str(j)],'FontSize',90/row) end end %% 坐标轴反转 set(gca,'YDir','reverse'); %% 获取图上坐标点 disp('请选择起点！') [A(1),A(2)]=ginput(1); x1=h_x(:); y1=h_y(:); temp=[x1 y1]; dis=((A(1)-x1).^2+(A(2)-y1).^2).^0.5; [~,s_node]=min(dis);%起点 S_coo=[x1(s_node),y1(s_node)]; plot(S_coo(1),S_coo(2), 'p','markersize',8,'markerfacecolor','r','MarkerEdgeColor', 'm') drawnow [x_temp,y_temp]=ind2sub([row,col],s_node); if model(h_data(x_temp,y_temp))==1 disp('起点选择有误，请重新选择！') return end disp('请选择终点！') [B(1),B(2)]=ginput(1); dis=((B(1)-x1).^2+(B(2)-y1).^2).^0.5; [~,e_node]=min(dis);%终点 E_coo=[x1(e_node),y1(e_node)]; plot(E_coo(1),E_coo(2), 'o','markersize',6,'markerfacecolor','r','MarkerEdgeColor', 'c') drawnow [x_temp,y_temp]=ind2sub([row,col],e_node); if model(h_data(x_temp,y_temp))==1 disp('终点选择有误，请重新选择！') return end gn1=inf*ones(1,node_num); gn1(s_node)=0; gn2=inf*ones(1,node_num); gn2(e_node)=0; hn1=((E_coo(1)-x1).^2+(E_coo(2)-y1).^2).^0.5; hn1=hn1';%每个节点到终点的估计值 hn2=((S_coo(1)-x1).^2+(S_coo(2)-y1).^2).^0.5; hn2=hn2';%每个节点到终点的估计值 fn1=gn1+hn1; fn2=gn2+hn2; fieldpointers1 =zeros(1,node_num); %fieldpointers用来存储各个点的来源方向 fieldpointers2 =zeros(1,node_num); %fieldpointers用来存储各个点的来源方向 %% 初始化起点的open表和close表 [~,now_point1]=min(fn1); [~,now_point2]=min(fn2); opened1 =now_point1; closing1 =zeros(1,node_num); opening1=zeros(1,node_num); opened2 =now_point2; closing2 =zeros(1,node_num); opening2=zeros(1,node_num); tic %% 未找到终点循环 while true % if now_point1==e_node % break; % end [x_now,y_now]=ind2sub([row,col],now_point1); opening1(now_point1)=0; closing1(now_point1)=1; next_node=find(ConnectMatrix(now_point1,:)); for i=1:length(next_node) [x_temp,y_temp]=ind2sub([row,col],next_node(i)); if model(h_data(x_temp,y_temp))==1 continue end if closing1(next_node(i))==0 if opening1(next_node(i))==0 fieldpointers1(next_node(i))=now_point1; if h_data(x_now,y_now)==1||h_data(x_temp,x_temp)==1 gn1(next_node(i))=gn1(now_point1)+fai*ConnectMatrix(now_point1,next_node(i)); else gn1(next_node(i))=gn1(now_point1)+ConnectMatrix(now_point1,next_node(i)); end opening1(next_node(i))=1; plot(x1(next_node(i)),y1(next_node(i)), 'o','markersize',6,'markerfacecolor','r','MarkerEdgeColor', 'c') drawnow else if h_data(x_now,y_now)==1||h_data(x_temp,y_temp)==1 dis_temp=gn1(now_point1)+fai*ConnectMatrix(now_point1,next_node(i)); else dis_temp=gn1(now_point1)+ConnectMatrix(now_point1,next_node(i)); end if dis_temp<gn1(next_node(i)) fieldpointers1(next_node(i))=now_point1; gn1(next_node(i))=dis_temp; end end end end opened1=find(opening1==1); if isempty(opened1) disp('路径寻找失败！'); return end hn1=((x1(now_point2)-x1).^2+(y1(now_point2)-y1).^2).^0.5; hn1=hn1';%每个节点到终点的估计值 fn1=gn1+hn1; [~,ind]=min(fn1(opened1)); now_point1=opened1(ind); %% 计算反向探索 next_node=find(ConnectMatrix(now_point2,:)); if ~isempty(find(ismember(next_node,now_point1)==1)) break end [x_now,y_now]=ind2sub([row,col],now_point2); opening2(now_point2)=0; closing2(now_point2)=1; for i=1:length(next_node) [x_temp,y_temp]=ind2sub([row,col],next_node(i)); if model(h_data(x_temp,y_temp))==1 continue end if closing2(next_node(i))==0 if opening2(next_node(i))==0 fieldpointers2(next_node(i))=now_point2; if h_data(x_now,y_now)==1||h_data(x_temp,x_temp)==1 gn2(next_node(i))=gn2(now_point2)+fai*ConnectMatrix(now_point2,next_node(i)); else gn2(next_node(i))=gn2(now_point2)+ConnectMatrix(now_point2,next_node(i)); end opening2(next_node(i))=1; plot(x1(next_node(i)),y1(next_node(i)), 'o','markersize',6,'markerfacecolor','r','MarkerEdgeColor', 'c') drawnow else if h_data(x_now,y_now)==1||h_data(x_temp,y_temp)==1 dis_temp=gn2(now_point2)+fai*ConnectMatrix(now_point2,next_node(i)); else dis_temp=gn2(now_point2)+ConnectMatrix(now_point2,next_node(i)); end if dis_temp<gn2(next_node(i)) fieldpointers2(next_node(i))=now_point2; gn2(next_node(i))=dis_temp; end end end end opened2=find(opening2==1); if isempty(opened2) disp('路径寻找失败！'); return end hn2=((x1(now_point1)-x1).^2+(y1(now_point1)-y1).^2).^0.5; hn2=hn2';%每个节点到终点的估计值 fn2=gn2+hn2; [~,ind]=min(fn2(opened2)); now_point2=opened2(ind); next_node=find(ConnectMatrix(now_point2,:)); if ~isempty(find(ismember(next_node,now_point1)==1)) break end end toc %% 显示路线运动过程 posind1 = now_point1; while (fieldpointers1(posind1(1)) ~= 0) posind1 = [fieldpointers1(posind1(1)), posind1]; end posind2 = now_point2; while (fieldpointers2(posind2(end)) ~= 0) posind2 = [posind2,fieldpointers2(posind2(end))]; end posind=[posind1,posind2]; hd1=line('Color','b','Marker','s','MarkerSize',10);%用来设置运动点颜色/形状/大小 route=S_coo; for i=2:length(posind) Q1=[x1(posind(i-1)),y1(posind(i-1))]; Q2=[x1(posind(i)),y1(posind(i))]; route=[route;Q2]; plot([Q1(1),Q2(1)],[Q1(2),Q2(2)],'r','LineWidth',3) set(hd1,'xdata',Q2(1),'ydata',Q2(2)); %用来给运动点设置运动位置的 % pause(0.3) % drawnow; end [re1,re2,re3,re]=re_fun(posind,x1,y1,model,row,col,h_data,threaten_data,fai,w); disp('双向A星算法结果：') disp(['路径指数',num2str(re1)]) disp(['装备指数',num2str(re2)]) disp(['威胁指数',num2str(re3)]) disp(['总评价指数',num2str(re)])