二维A*算法

MATLAB2016b可以正常运行 

二维A*算法_第1张图片

function bidirectional_ASTAR
clc;
clear;
%% 初始化界面
n = 11;   % field size n x n tiles  20*20的界面
%wallpercent = 0.3;  % this percent of field is walls   15%的界面作为阻碍物(墙)
cmap = [1 1 1; ...%  1 - white - 空地
        0 0 0; ...% 2 - black - 障碍 
        1 0 0; ...% 3 - red - 已搜索过的地方
        0 0 1; ...% 4 - blue - 下次搜索备选中心 
        0 1 0; ...% 5 - green - 起始点
        1 1 0;...% 6 - yellow -  到目 标点的路径 
       1 0 1];% 7 - -  目标点 
colormap(cmap); 
field = ones(n);
startposind =10;   %sub2ind用来将行列坐标转换为线性坐标,这里是必要的,因为如果把startposind设置成[x,y]的形式,访问field([x,y])的时候
goalposind =12;    %它并不是访问x行y列元素,而是访问线性坐标为x和y的两个元素
% field(ceil(n^2.*rand(floor(n*n*wallpercent),1) )) = Inf;
field(1:5, 7) = 2;
field(8,1:3) = 2; 
field(2:5,3:5)=2;
   field(8,10)=2;
% startposind = sub2ind([n,n],ceil(n.*rand),ceil(n.*rand));   %sub2ind用来将行列坐标转换为线性坐标,这里是必要的,因为如果把startposind设置成[x,y]的形式,访问field([x,y])的时候
%goalposind = sub2ind([n,n],ceil(n.*rand),ceil(n.*rand));    %它并不是访问x行y列元素,而是访问线性坐标为x和y的两个元素
field(startposind )=5;
field(goalposind )=7;
costchart = NaN*ones(n);      %costchart用来存储各个点的实际代价,NaN代表不是数据(不明确的操作)
costchart(startposind) = 0;     %起点的实际代价
fieldpointers = zeros(n);
fieldpointers1 = zeros(n);
    global setOpenCosts;
    global setOpenCosts1;
    global setOpen;
    global setOpen1;
    global setOpenHeuristics;
    global setOpenHeuristics1;
setOpen = (startposind); setOpenCosts = (0); setOpenHeuristics = (Inf);
setClosed = []; setClosedCosts = [];%初始化起点的open表和close表
setOpen1 = (goalposind); setOpenCosts1 = (0); setOpenHeuristics1 = (Inf);
setClosed1 = []; setClosedCosts1 = [];%初始化目标点的open表和close表

[goalpos(1) ,goalpos(2)] = ind2sub([n n],goalposind);       %获得目标点的行列坐标
[startpos(1) ,startpos(2)] = ind2sub([n n],startposind);
uicontrol('Style','pushbutton','String','RE-DO', 'FontSize',12, ...
         'Position', [10 10 60 40], 'Callback','bidirectional_ASTAR');
tic
while true %ismember(A,B)返回与A同大小的矩阵,其中元素1表示A中相应位置的元素在B中也出现,0则是没有出现
   field(startposind )=5;
   field(goalposind )=7;
   image(1.5,1.5,field); 
    grid on; 
    axis image; 
    set(gca,'gridline','-','gridcolor','r','linewidth',2);
    set(gca,'xtick',1:1:12,'ytick',1:1:12);
    drawnow;
  if(max(ismember(setOpen,setOpen1)))
       break;
   end;  
   
    [~, ii] = min(setOpenCosts + setOpenHeuristics);   %从OPEN表中选择花费最低的点temp,ii是其下标(也就是标号索引)
    [~, ii1] = min(setOpenCosts1 + setOpenHeuristics1);
    field(setOpen(ii))=3;
    field(setOpen1(ii1))=6;
    [currentpos(1), currentpos(2)] = ind2sub([n n],setOpen(ii));
    [currentpos1(1), currentpos1(2)] = ind2sub([n n],setOpen1(ii1));%获得以起点扩展的当前点的行列坐标,注意currentpos(1)是行坐标,currentpos(2)是列坐标
    %% 获得当前点的邻居
    [posinds,posinds1,cost,cost1,heuristic,heuristic1]=get_neighbors(n,ii,ii1,currentpos(1),currentpos(2),currentpos1(1),currentpos1(2),goalpos(1),goalpos(2),startpos(1),startpos(2));
    
     setClosed = [setClosed; setOpen(ii)];     %将temp插入CLOSE表中
     setClosedCosts = [setClosedCosts; setOpenCosts(ii)];  %将temp的花费计入ClosedCosts
     setClosed1 = [setClosed1; setOpen1(ii1)];     %将temp插入CLOSE表中
     setClosedCosts1 = [setClosedCosts1; setOpenCosts1(ii1)];  %将temp的花费计入ClosedCosts
  
     %% 新增节点的判断
      for jj=1:length(posinds)      %对于扩展的四个方向的坐标
        if(field(posinds(jj))~=3 && field(posinds(jj))~=2 && field(posinds(jj))~=5 &&posinds(jj)~=1)
           field(posinds(jj))=4;
           if ~max([setClosed; setOpen] == posinds(jj)) %如果此点不在OPEN表和CLOSE表中
                 fieldpointers(posinds(jj)) = setOpen(ii); %将此点的方向存在对应的fieldpointers中
                 costchart(posinds(jj)) = cost(jj); %将实际代价值存入对应的costchart中
                 setOpen = [setOpen; posinds(jj)]; %将此点加入OPEN表中
                 setOpenCosts = [setOpenCosts; cost(jj)];   %更新OPEN表实际代价
                 setOpenHeuristics = [setOpenHeuristics; heuristic(jj)];    %更新OPEN表启发代价
          elseif max(setOpen == posinds(jj)) %如果此点在OPEN表中
                 I = find(setOpen == posinds(jj));   %找到此点在OPEN表中的位置
                if setOpenCosts(I) > cost(jj)  %如果在OPEN表中的此点实际代价比现在所得的大
                  costchart(setOpen(I)) = cost(jj);    %将当前的代价存入costchart中,注意此点在costchart中的坐标与其自身坐标是一致的(setOpen(I)其实就是posinds(jj)),下同fieldpointers
                  setOpenCosts(I) = cost(jj);      %更新OPEN表中的此点代价,注意此点在setOpenCosts中的坐标与在setOpen中是一致的,下同setOpenHeuristics
                  setOpenHeuristics(I) = heuristic(jj);    %更新OPEN表中的此点启发代价(窃以为这个是没有变的)
                  fieldpointers(setOpen(I)) = setOpen(ii);   %更新此点的方向   
                end
           end
         end
      end
    for jj=1:length(posinds1)      %对于扩展的四个方向的坐标
     if(field(posinds1(jj))~=6 && field(posinds1(jj))~=2 && field(posinds1(jj))~=7 && posinds1(jj)~=1)
       field(posinds1(jj))=4; 
       if ~max([setClosed1; setOpen1] == posinds1(jj))%如果此点不在OPEN表和CLOSE表中

             fieldpointers1(posinds1(jj)) = setOpen1(ii1); %将此点的方向存在对应的fieldpointers中
             costchart(posinds1(jj)) = cost1(jj); %将实际代价值存入对应的costchart中
             setOpen1 = [setOpen1; posinds1(jj)]; %将此点加入OPEN表中
             setOpenCosts1 = [setOpenCosts1; cost1(jj)];   %更新OPEN表实际代价
             setOpenHeuristics1 = [setOpenHeuristics1; heuristic1(jj)];    %更新OPEN表启发代价
       elseif max(setOpen1 == posinds1(jj)) %如果此点在OPEN表中
         I = find(setOpen1 == posinds1(jj));   %找到此点在OPEN表中的位置
          if setOpenCosts1(I) > cost1(jj)  %如果在OPEN表中的此点实际代价比现在所得的大
              costchart(setOpen1(I)) = cost1(jj);    %将当前的代价存入costchart中,注意此点在costchart中的坐标与其自身坐标是一致的(setOpen(I)其实就是posinds(jj)),下同fieldpointers
               setOpenCosts1(I) = cost1(jj);      %更新OPEN表中的此点代价,注意此点在setOpenCosts中的坐标与在setOpen中是一致的,下同setOpenHeuristics
              setOpenHeuristics1(I) = heuristic1(jj);    %更新OPEN表中的此点启发代价(窃以为这个是没有变的)
              fieldpointers1(setOpen1(I)) = setOpen1(ii1);   %更新此点的方向   
          end
       end
      end
    end 
    %% 更新open表
    update_open(ii,ii1)
    
    if (isempty(setOpen) && isempty(setOpen1))
         break; 
    end    %当OPEN表为空,代表可以经过的所有点已经查询完毕
end
toc

%% 查找并显示路径
if(max(ismember(setOpen,setOpen1)))
    disp('已找到路径!');  %disp: Display array, disp(X)直接将矩阵显示出来,不显示其名字,如果X为string,就直接输出文字X
    b=intersect(setOpen,setOpen1);%取setClosed和setClosed1的交集
    path=b;
    %查找从起点开始搜索的路径
     while (fieldpointers(path(1)) ~= 0) 
         
         path = [fieldpointers(path(1)), path];     
     end 
     path2=b;
     %查找从终点搜索的路径
     while (fieldpointers1(path2(1)) ~= 0) 
         
         path2 = [fieldpointers1(path2(1)), path2];     
     end 
     p1=[path,flip(path2)]; %flip 颠倒矩阵
     for k = 2:length(p1) - 1 
         field(p1(k)) = 7;
         image(1.5, 1.5, field);
         grid on;
         set(gca,'gridline','-','gridcolor','r','linewidth',2);
         set(gca,'xtick',1:1:12,'ytick',1:1:12);
         axis image;
         drawnow;
     end
else
    disp('路径不存在!'); 
 end

title('基于{ \color{red}A*} 算法的路径规划 ','fontsize',16)
end







%% 更新open表
function update_open(ii,ii1)
  global   setOpenCosts
  global   setOpenCosts1
  global   setOpen
  global   setOpen1
  global setOpenHeuristics;
  global setOpenHeuristics1;
%  更新OPEN表 分为三种情况
   %从起点开始的open表
  if (ii > 1 && ii < length(setOpen))   %temp在OPEN表的中间,删除temp
    setOpen = [setOpen(1:ii-1); setOpen(ii+1:end)];
    setOpenCosts = [setOpenCosts(1:ii-1); setOpenCosts(ii+1:end)];
    setOpenHeuristics = [setOpenHeuristics(1:ii-1); setOpenHeuristics(ii+1:end)];
  elseif (ii == 1)
    setOpen = setOpen(2:end);   %temp是OPEN表的第一个元素,删除temp
    setOpenCosts = setOpenCosts(2:end);
    setOpenHeuristics = setOpenHeuristics(2:end);
  else     %temp是OPEN表的最后一个元素,删除temp
    setOpen = setOpen(1:end-1);
    setOpenCosts = setOpenCosts(1:end-1);
    setOpenHeuristics = setOpenHeuristics(1:end-1);
  end
  
  % 从终点开始的open表
   if (ii1 > 1 && ii1 < length(setOpen1))   %temp在OPEN表的中间,删除temp
    setOpen1 = [setOpen1(1:ii1-1); setOpen1(ii1+1:end)];
    setOpenCosts1 = [setOpenCosts1(1:ii1-1); setOpenCosts1(ii1+1:end)];
    setOpenHeuristics1 = [setOpenHeuristics1(1:ii1-1); setOpenHeuristics1(ii1+1:end)];
  elseif (ii1 == 1)
    setOpen1 = setOpen1(2:end);   %temp是OPEN表的第一个元素,删除temp
    setOpenCosts1 = setOpenCosts1(2:end);
    setOpenHeuristics1 = setOpenHeuristics1(2:end);
  else     %temp是OPEN表的最后一个元素,删除temp
    setOpen1 = setOpen1(1:end-1);
    setOpenCosts1 = setOpenCosts1(1:end-1);
    setOpenHeuristics1 = setOpenHeuristics1(1:end-1);
  end
  
end

%% 获得当前的点的邻居
function [posinds,posinds1,cost,cost1,heuristic,heuristic1]=get_neighbors(n,ii,ii1,currentpos_x,currentpos_y,currentpos1_x,currentpos1_y,goalpos_x,goalpos_y,startpos_x,startpos_y)
  global   setOpenCosts
  global   setOpenCosts1
cost = Inf*ones(4,1); heuristic = Inf*ones(4,1); pos = ones(4,2);  
    cost1 = Inf*ones(4,1); heuristic1 = Inf*ones(4,1); pos1 = ones(4,2); 
    %  左侧查询
    newx = currentpos_x ; newy = currentpos_y-1; 
    if newy > 0  %如果没有到边界
      pos(1,:) = [newx newy];   %获得新的坐标
      heuristic(1) = abs(goalpos_x-newx) + abs(goalpos_y-newy);    %heuristic(1)为启发函数计算的距离代价
      % heuristic(1) = sqrt((goalpos(1)-newx)^2 + (goalpos(2)-newy)^2);    %heuristic(1)为启发函数计算的距离代价
      cost(1) = setOpenCosts(ii) +  1;   %costsofar为之前花费的代价,field(newy,newx)为环境威胁代价,cost(1)为经过此方向点的真实代价
    end
     newx1 = currentpos1_x ; newy1= currentpos1_y-1; 
     if newy1 > 0  %如果没有到边界
       pos1(1,:) = [newx1 newy1];   %获得新的坐标
       heuristic1(1) = abs(startpos_x-newx1) + abs(startpos_y-newy1);    %heuristic(1)为启发函数计算的距离代价
       % heuristic(1) = sqrt((goalpos(1)-newx)^2 + (goalpos(2)-newy)^2);    %heuristic(1)为启发函数计算的距离代价
       cost1(1) = setOpenCosts1(ii1) +  1;   %costsofar为之前花费的代价,field(newy,newx)为环境威胁代价,cost(1)为经过此方向点的真实代价
     end
    %  向右查询
    newx = currentpos_x; newy = currentpos_y+1;
    if newy <= n
      pos(2,:) = [newx newy];
     heuristic(2) = abs(goalpos_x-newx) + abs(goalpos_y-newy);  %heuristic(1)为启发函数计算的距离代价
     % heuristic(2) = sqrt((goalpos(1)-newx)^2 + (goalpos(2)-newy)^2); 
      cost(2) =setOpenCosts(ii) +  1;
    end
     newx1 = currentpos1_x; newy1 = currentpos1_y+1;
     if newy1 <= n
       pos1(2,:) = [newx1 newy1];
      heuristic1(2) = abs(startpos_x-newx1) + abs(startpos_y-newy1);  %heuristic(1)为启发函数计算的距离代价
      % heuristic(2) = sqrt((goalpos(1)-newx)^2 + (goalpos(2)-newy)^2); 
      cost1(2) =setOpenCosts1(ii1) +  1;
     end
    %  向上查询
    newx = currentpos_x+1; newy = currentpos_y;
    if newx <= n
      pos(3,:) = [newx newy];
      heuristic(3) = abs(goalpos_x-newx) + abs(goalpos_y-newy);    %heuristic(1)为启发函数计算的距离代价
    %  heuristic(3) = sqrt((goalpos(1)-newx)^2 + (goalpos(2)-newy)^2); 
      cost(3) =setOpenCosts(ii) + 1;
    end
     newx1 = currentpos1_x+1; newy1 = currentpos1_y;
     if newx1 <= n
       pos1(3,:) = [newx1 newy1];
       heuristic1(3) = abs(startpos_x-newx1) + abs(startpos_y-newy1);    %heuristic(1)为启发函数计算的距离代价
     %  heuristic(3) = sqrt((goalpos(1)-newx)^2 + (goalpos(2)-newy)^2); 
       cost1(3) =setOpenCosts1(ii1) + 1;
     end
    %   向下查询
    newx = currentpos_x-1; newy = currentpos_y;
    if newx > 0
      pos(4,:) = [newx newy];
      heuristic(4) = abs(goalpos_x-newx) + abs(goalpos_y-newy);    %heuristic(1)为启发函数计算的距离代价
  %heuristic(4) = sqrt((goalpos(1)-newx)^2 + (goalpos(2)-newy)^2); 
      cost(4) = setOpenCosts(ii) + 1;
    end
     newx1 = currentpos1_x-1; newy1 = currentpos1_y;
     if newx1 > 0
       pos1(4,:) = [newx1 newy1];
       heuristic1(4) = abs(startpos_x-newx1) + abs(startpos_y-newy1);    %heuristic(1)为启发函数计算的距离代价
   %heuristic(4) = sqrt((goalpos(1)-newx)^2 + (goalpos(2)-newy)^2); 
       cost1(4) = setOpenCosts1(ii1) + 1;
     end
%     %   左上查询
%          newx = currentpos(1)-1; newy = currentpos(2)-1;
%         if (newy >0 && newx>0)
%            pos1(1,:) = [newx newy];
%            heuristic1(1)=abs(goalpos(1)-newx) + abs(goalpos(2)-newy);    %heuristic(1)为启发函数计算的距离代价
%            cost1(1) = setOpenCosts(ii)  + sqrt(2)*field(newx,newy);
%        end
%   %    右上查询
%      newx = currentpos(1)-1; newy = currentpos(2)+1;
%      if (newy <= n && newx>0)
%         pos1(2,:) = [newx newy];
%         heuristic1(2) = abs(goalpos(1)-newx) + abs(goalpos(2)-newy);    %heuristic(1)为启发函数计算的距离代价
%         cost1(2) = setOpenCosts(ii)  +sqrt(2)*field(newx,newy);
%      end
%  %  左下查询
%      newx = currentpos(1)+1; newy = currentpos(2)-1;
%      if (newy > 0 && newx<=n)
%         pos1(3,:) = [newx newy];
%         heuristic1(3) = abs(goalpos(1)-newx) + abs(goalpos(2)-newy);    %heuristic(1)为启发函数计算的距离代价
%         cost1(3) = setOpenCosts(ii)  + sqrt(2)*field(newx,newy);
%      end
%     %  右下查询
%      newx = currentpos(1)+1; newy = currentpos(2)+1;
%      if (newx <= n && newy<=n)
%         pos1(4,:) = [newx newy];    
%         heuristic1(4) = abs(goalpos(1)-newx) + abs(goalpos(2)-newy);    %heuristic(1)为启发函数计算的距离代价
%         cost1(4) = setOpenCosts(ii)  + sqrt(2)*field(newx,newy);
%      end
     posinds = sub2ind([n,n],pos(:,1),pos(:,2));
     posinds1 = sub2ind([n,n],pos1(:,1),pos1(:,2));
end



%% 注释:当获取当前点的邻居向8个方向时,估计函数h(n)要采用切比雪夫距离

 

你可能感兴趣的:(多无人机集群编队,算法,前端,javascript)