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 Baseddisp( Generating Grid ... );coor;crns;noOfNodes nooc*noor;nodmatones(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);noddatareshape(nodmat,noOfNodes,3);%rand(state, 0);if plot_flag1scrsz get(0,ScreenSize);hfigure(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;endAstar_coornoddata(:,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:noOfNodesAstar_coord(i,1) netXloc(i);Astar_coord(i,2) netYloc(i);for j 1:noOfNodesdistance sqrt((netXloc(i) - netXloc(j))^2 (netYloc(i) - netYloc(j))^2);llisempty(find(oi, 1));lmisempty(find(oj, 1));if (distance R ll1 lm1)matrix(i, j) distance; % if set to 1, Dijkstra computes Spath in terms of hops; if set to distance, it is the real shortest pathif i~j % must be satisfiedAstar_connect(i, j) 1;elseAstar_connect(i, j) 0;endif plot_flag1line([netXloc(i) netXloc(j)], [netYloc(i) netYloc(j)], color,[.65 .65 .65],LineStyle, :);endelsematrix(i, j) inf;Astar_connect(i, j) 0;end;end;endfor i 1:noOfNodesif plot_nodenumtext(netXloc(i)20, netYloc(i), num2str(i));endif plot_flag1if isplot(netXloc(i), netYloc(i),square,MarkerSize,12,MarkerFaceColor,g);hold on;endif idplot(netXloc(i), netYloc(i),square,MarkerSize,12,MarkerFaceColor,r);endif isempty(find(oi))plot(netXloc(i), netYloc(i),.);endendend;% 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_coordAstar_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_summary1Astar_path, komega_path,cost_astar,cost_komega,astar_time,ko_time, Astar_dist, komega_distendif ~isempty(Astar_path)for i 1:(length(Astar_path)-1)if plot_flag1astrline([netXloc(Astar_path(i)) netXloc(Astar_path(i1))], [netYloc(Astar_path(i)) netYloc(Astar_path(i1))], Color,r,LineWidth, 2, LineStyle, -.);if plot_nodenum1text(netXloc(i), netYloc(i), num2str(i));endendend;end;if ~isempty(komega_path)for i 1:(length(komega_path)-1)if plot_flag1komline([netXloc(komega_path(i)) netXloc(komega_path(i1))], [netYloc(komega_path(i)) netYloc(komega_path(i1))], Color,g,LineWidth, 2, LineStyle, -);if plot_nodenum1text(netXloc(i), netYloc(i), num2str(i));endendend;end;resttitle(Comparison Between A-Star and K-Omega);set(rest,Interpreter,latex);dkstrstrcat(K-Omega(k,num2str(k), b,num2str(b), n,num2str(n),));kleglegend([astr kom],A-Star,dkstr);hold on;set(kleg,Interpreter,latex);if plot_flag 1hold off;end% Execute if K-Omega has not yet executed harwareif (n0)[done]execnxt(komega_path);end% If Analysis mode requested, perform analysisif AnalysisMode1AnalyzeKO;end3 仿真结果4 参考文献[1]邓顺平, 张艳军, 刘会平. (2014). 一种基于威胁势场的a星路径规划算法. 科技视界(3), 2. 【路径规划】基于A星算法和改进A星算法求解路径规划问题matlab代码
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 Baseddisp( Generating Grid ... );coor;crns;noOfNodes nooc*noor;nodmatones(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);noddatareshape(nodmat,noOfNodes,3);%rand(state, 0);if plot_flag1scrsz get(0,ScreenSize);hfigure(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;endAstar_coornoddata(:,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:noOfNodesAstar_coord(i,1) netXloc(i);Astar_coord(i,2) netYloc(i);for j 1:noOfNodesdistance sqrt((netXloc(i) - netXloc(j))^2 (netYloc(i) - netYloc(j))^2);llisempty(find(oi, 1));lmisempty(find(oj, 1));if (distance R ll1 lm1)matrix(i, j) distance; % if set to 1, Dijkstra computes Spath in terms of hops; if set to distance, it is the real shortest pathif i~j % must be satisfiedAstar_connect(i, j) 1;elseAstar_connect(i, j) 0;endif plot_flag1line([netXloc(i) netXloc(j)], [netYloc(i) netYloc(j)], color,[.65 .65 .65],LineStyle, :);endelsematrix(i, j) inf;Astar_connect(i, j) 0;end;end;endfor i 1:noOfNodesif plot_nodenumtext(netXloc(i)20, netYloc(i), num2str(i));endif plot_flag1if isplot(netXloc(i), netYloc(i),square,MarkerSize,12,MarkerFaceColor,g);hold on;endif idplot(netXloc(i), netYloc(i),square,MarkerSize,12,MarkerFaceColor,r);endif isempty(find(oi))plot(netXloc(i), netYloc(i),.);endendend;% 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_coordAstar_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_summary1Astar_path, komega_path,cost_astar,cost_komega,astar_time,ko_time, Astar_dist, komega_distendif ~isempty(Astar_path)for i 1:(length(Astar_path)-1)if plot_flag1astrline([netXloc(Astar_path(i)) netXloc(Astar_path(i1))], [netYloc(Astar_path(i)) netYloc(Astar_path(i1))], Color,r,LineWidth, 2, LineStyle, -.);if plot_nodenum1text(netXloc(i), netYloc(i), num2str(i));endendend;end;if ~isempty(komega_path)for i 1:(length(komega_path)-1)if plot_flag1komline([netXloc(komega_path(i)) netXloc(komega_path(i1))], [netYloc(komega_path(i)) netYloc(komega_path(i1))], Color,g,LineWidth, 2, LineStyle, -);if plot_nodenum1text(netXloc(i), netYloc(i), num2str(i));endendend;end;resttitle(Comparison Between A-Star and K-Omega);set(rest,Interpreter,latex);dkstrstrcat(K-Omega(k,num2str(k), b,num2str(b), n,num2str(n),));kleglegend([astr kom],A-Star,dkstr);hold on;set(kleg,Interpreter,latex);if plot_flag 1hold off;end% Execute if K-Omega has not yet executed harwareif (n0)[done]execnxt(komega_path);end% If Analysis mode requested, perform analysisif AnalysisMode1AnalyzeKO;end3 仿真结果4 参考文献[1]邓顺平, 张艳军, 刘会平. (2014). 一种基于威胁势场的a星路径规划算法. 科技视界(3), 2.