MATLAB2016b可以正常运行 

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的花费计入ClosedCostssetClosed1 = [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)），下同fieldpointerssetOpenCosts(I) = cost(jj);      %更新OPEN表中的此点代价，注意此点在setOpenCosts中的坐标与在setOpen中是一致的，下同setOpenHeuristicssetOpenHeuristics(I) = heuristic(jj);    %更新OPEN表中的此点启发代价(窃以为这个是没有变的)fieldpointers(setOpen(I)) = setOpen(ii);   %更新此点的方向   endendendendfor 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)），下同fieldpointerssetOpenCosts1(I) = cost1(jj);      %更新OPEN表中的此点代价，注意此点在setOpenCosts中的坐标与在setOpen中是一致的，下同setOpenHeuristicssetOpenHeuristics1(I) = heuristic1(jj);    %更新OPEN表中的此点启发代价(窃以为这个是没有变的)fieldpointers1(setOpen1(I)) = setOpen1(ii1);   %更新此点的方向   endendendend %% 更新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，就直接输出文字Xb=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
elsedisp('路径不存在!'); endtitle('基于{ \color{red}A*} 算法的路径规划 ','fontsize',16)
end%% 更新open表
function update_open(ii,ii1)global   setOpenCostsglobal   setOpenCosts1global   setOpenglobal   setOpen1global setOpenHeuristics;global setOpenHeuristics1;
%  更新OPEN表 分为三种情况%从起点开始的open表if (ii > 1 && ii < length(setOpen))   %temp在OPEN表的中间，删除tempsetOpen = [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表的第一个元素，删除tempsetOpenCosts = setOpenCosts(2:end);setOpenHeuristics = setOpenHeuristics(2:end);else     %temp是OPEN表的最后一个元素，删除tempsetOpen = setOpen(1:end-1);setOpenCosts = setOpenCosts(1:end-1);setOpenHeuristics = setOpenHeuristics(1:end-1);end% 从终点开始的open表if (ii1 > 1 && ii1 < length(setOpen1))   %temp在OPEN表的中间，删除tempsetOpen1 = [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表的第一个元素，删除tempsetOpenCosts1 = setOpenCosts1(2:end);setOpenHeuristics1 = setOpenHeuristics1(2:end);else     %temp是OPEN表的最后一个元素，删除tempsetOpen1 = setOpen1(1:end-1);setOpenCosts1 = setOpenCosts1(1:end-1);setOpenHeuristics1 = setOpenHeuristics1(1:end-1);endend%% 获得当前的点的邻居
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   setOpenCostsglobal   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)为经过此方向点的真实代价endnewx1 = 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 <= npos(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;endnewx1 = currentpos1_x; newy1 = currentpos1_y+1;if newy1 <= npos1(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 <= npos(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;endnewx1 = currentpos1_x+1; newy1 = currentpos1_y;if newx1 <= npos1(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 > 0pos(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;endnewx1 = currentpos1_x-1; newy1 = currentpos1_y;if newx1 > 0pos1(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);
%      endposinds = sub2ind([n,n],pos(:,1),pos(:,2));posinds1 = sub2ind([n,n],pos1(:,1),pos1(:,2));
end%% 注释：当获取当前点的邻居向8个方向时，估计函数h(n)要采用切比雪夫距离