1 简介
一种基于A星算法的最短寻路方法.其包括将搜索区域划分为多个网格并将起点放入开启列表中,搜索起点所在网格相邻的网格并计算相邻的网格点与目标点的距离,采用A星算法计算从初始状态到目标状态的代价估计,通过判断起点至目标点各路径对应代价估计值是否为最小值寻找新起点,再通过判断新的起点是否为目标点完成寻路.
2 部分代码
%************************************************************************
%**************************************************************************
close all;
%**************************************************************************
%**************** Choose System Architecture ***************************
%**************************************************************************
% Here you can choose the hardware architecture implementation
% 1 . Single Robot mode
% 2 . Dual Robot Single Computer
% 3 . Dual Roboto Dual Computer
% 4 . Multi Robot Single Computer
% 5 . Multi Robot Ethernet Based
disp(' Generating Grid ... ');
co=or;
crn=s;
noOfNodes = nooc*noor;
nodmat=ones(nooc,noor,3);
nodmat(1:noOfNodes)=1:noOfNodes;
nodmat(:,:,2)=(find(nodmat(:,2,2)==1))*ones(1,noor);
nodmat(:,:,3)=ones(nooc,1)*(find(nodmat(2,:,3)==1));
nodmat(:,:,1)=flipdim((nodmat(:,:,1)),2);
nodmat(:,:,2)=flipdim((nodmat(:,:,2)),2);
nodmat(:,:,3)=flipdim((nodmat(:,:,3)),2);
noddata=reshape(nodmat,noOfNodes,3);
%rand('state', 0);
if plot_flag==1
scrsz = get(0,'ScreenSize');
h=figure(gcf);
set(h,'Position',[scrsz(3)/8 scrsz(4)/8 scrsz(3)-2*scrsz(3)/8 ...
scrsz(4)-2*scrsz(4)/8]);
clf;
hold on;
end
Astar_coor=noddata(:,2:3)*GTS;
netXloc = Astar_coor(:,1)';
netYloc = Astar_coor(:,2)';
axis([min(netXloc)-20 max(netXloc)+30 min(netYloc)-20 max(netYloc)+30])
Astar_connect = zeros(noOfNodes, noOfNodes);
Astar_coord = zeros(noOfNodes, 2);
for i = 1:noOfNodes
Astar_coord(i,1) = netXloc(i);
Astar_coord(i,2) = netYloc(i);
for j = 1:noOfNodes
distance = sqrt((netXloc(i) - netXloc(j))^2 + (netYloc(i) - netYloc(j))^2);
ll=isempty(find(o==i, 1));
lm=isempty(find(o==j, 1));
if (distance <= R && ll==1 && lm==1)
matrix(i, j) = distance; % if set to '1', Dijkstra computes Spath in terms of hops; if set to 'distance', it is the real shortest path
if i~=j % must be satisfied
Astar_connect(i, j) = 1;
else
Astar_connect(i, j) = 0;
end
if plot_flag==1
line([netXloc(i) netXloc(j)], [netYloc(i) netYloc(j)], 'color',[.65 .65 .65],'LineStyle', ':');
end
else
matrix(i, j) = inf;
Astar_connect(i, j) = 0;
end;
end;
end
for i = 1:noOfNodes
if plot_nodenum
text(netXloc(i)+20, netYloc(i), num2str(i));
end
if plot_flag==1
if i==s
plot(netXloc(i), netYloc(i),'square','MarkerSize',12,'MarkerFaceColor','g');
hold on;
end
if i==d
plot(netXloc(i), netYloc(i),'square','MarkerSize',12,'MarkerFaceColor','r');
end
if isempty(find(o==i))
plot(netXloc(i), netYloc(i),'.');
end
end
end;
% activeNodes = [];
% for i = 1:noOfNodes,
% % initialize the farthest node to be itself;
% farthestPreviousHop(i) = i; % used to compute the RTS/CTS range;
% farthestNextHop(i) = i;
% end;
Astar_coord=Astar_coord';
%Astar_connect;
%%
disp('Generating Paths ... ')
%[path, totalCost, farthestPreviousHop, farthestNextHop] = dijkstra(noOfNodes, matrix, s, d, farthestPreviousHop, farthestNextHop);
% combo = [noOfNodes s-1 d-1 R/2];
%[Astar_path, Astar_search] = Astar(Astar_coord', Astar_connect, combo); % notice, we must put Astar_coord' rather than Astar_coord
%[Astar_paths,cost_astar,astar_time] = Astarm(Astar_coord, Astar_connect, s , d);
%[Astar_path,Astar_search]=Astar(Astar_coord, Astar_connect, combo);
[Astar_path,cost_astar,astar_time,Astar_dist] =komegaA(Astar_coord, Astar_connect, s, d, 1, inf, 0);
[komega_path,cost_komega,ko_time,komega_dist]=komegaA(Astar_coord, Astar_connect, s, d, k , b, n);
%%
if disp_summary==1
Astar_path, komega_path,cost_astar,cost_komega,astar_time,ko_time, Astar_dist, komega_dist
end
if ~isempty(Astar_path)
for i = 1:(length(Astar_path)-1)
if plot_flag==1
astr=line([netXloc(Astar_path(i)) netXloc(Astar_path(i+1))], [netYloc(Astar_path(i)) netYloc(Astar_path(i+1))], 'Color','r','LineWidth', 2, 'LineStyle', '-.');
if plot_nodenum==1
text(netXloc(i), netYloc(i), num2str(i));
end
end
end;
end;
if ~isempty(komega_path)
for i = 1:(length(komega_path)-1)
if plot_flag==1
kom=line([netXloc(komega_path(i)) netXloc(komega_path(i+1))], [netYloc(komega_path(i)) netYloc(komega_path(i+1))], 'Color','g','LineWidth', 2, 'LineStyle', '-');
if plot_nodenum==1
text(netXloc(i), netYloc(i), num2str(i));
end
end
end;
end;
rest=title('Comparison Between A-Star and K-Omega');
set(rest,'Interpreter','latex');
dkstr=strcat('K-Omega(k=',num2str(k),' b=',num2str(b),' n=',num2str(n),')');
kleg=legend([astr kom],'A-Star',dkstr);hold on;
set(kleg,'Interpreter','latex');
if plot_flag == 1
hold off;
end
% Execute if K-Omega has not yet executed harware
if (n==0)
[done]=execnxt(komega_path);
end
% If Analysis mode requested, perform analysis
if AnalysisMode==1
AnalyzeKO;
end
3 仿真结果
4 参考文献
[1]邓顺平, 张艳军, & 刘会平. (2014). 一种基于威胁势场的a星路径规划算法. 科技视界(3), 2.
5 MATLAB代码与数据下载地址
见博客主页
