二维A*算法
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的花费计入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)要采用切比雪夫距离