劃】基于A星算法和改進(jìn)A星算法求解路徑規(guī)劃問(wèn)題matlab代碼)
1 簡(jiǎn)介一種基于A星算法的最短尋路方法.其包括將搜索區(qū)域劃分為多個(gè)網(wǎng)格并將起點(diǎn)放入開(kāi)啟列表中,搜索起點(diǎn)所在網(wǎng)格相鄰的網(wǎng)格并計(jì)算相鄰的網(wǎng)格點(diǎn)與目標(biāo)點(diǎn)的距離,采用A星算法計(jì)算從初始狀態(tài)到目標(biāo)狀態(tài)的代價(jià)估計(jì),通過(guò)判斷起點(diǎn)至目標(biāo)點(diǎn)各路徑對(duì)應(yīng)代價(jià)估計(jì)值是否為最小值尋找新起點(diǎn),再通過(guò)判斷新的起點(diǎn)是否為目標(biāo)點(diǎn)完成尋路.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 仿真結(jié)果4 參考文獻(xiàn)[1]鄧順平, 張艷軍, 劉會(huì)平. (2014). 一種基于威脅勢(shì)場(chǎng)的a星路徑規(guī)劃算法. 科技視界(3), 2. 【路徑規(guī)劃】基于A星算法和改進(jìn)A星算法求解路徑規(guī)劃問(wèn)題matlab代碼
1 簡(jiǎn)介一種基于A星算法的最短尋路方法.其包括將搜索區(qū)域劃分為多個(gè)網(wǎng)格并將起點(diǎn)放入開(kāi)啟列表中,搜索起點(diǎn)所在網(wǎng)格相鄰的網(wǎng)格并計(jì)算相鄰的網(wǎng)格點(diǎn)與目標(biāo)點(diǎn)的距離,采用A星算法計(jì)算從初始狀態(tài)到目標(biāo)狀態(tài)的代價(jià)估計(jì),通過(guò)判斷起點(diǎn)至目標(biāo)點(diǎn)各路徑對(duì)應(yīng)代價(jià)估計(jì)值是否為最小值尋找新起點(diǎn),再通過(guò)判斷新的起點(diǎn)是否為目標(biāo)點(diǎn)完成尋路.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 仿真結(jié)果4 參考文獻(xiàn)[1]鄧順平, 張艷軍, 劉會(huì)平. (2014). 一種基于威脅勢(shì)場(chǎng)的a星路徑規(guī)劃算法. 科技視界(3), 2.