A list,

Algorithm A algorithm is A typical heuristic search algorithm, based on Dijkstra algorithm, widely used in game maps and the real world to find the shortest path between two points. The most important thing of Algorithm A is to maintain A heuristic evaluation function, as shown in Formula (1). F (n)=g(n)+h(n)(1) where, f(n) is the heuristic function corresponding to each node searched by the algorithm. It consists of two parts, the first part g(n) is the actual traffic cost from the initial node to the current node, and the second part H (n) is the estimated traffic cost from the current node to the end point. Every time the algorithm is expanded, the node with the smallest f(n) value is selected as the next node on the optimal path. In practical application, if the shortest Distance is taken as the optimization target, h(n) is often taken as Euclidean Distance or Manhattan Distance from the current point to the end point. If h(n)=0, it means that no information of the current node and endpoint is used, and algorithm A degenerates into uninspired Dijkstra algorithm, and the algorithm search space becomes larger and the search time becomes longer. A* algorithm steps are as follows, the algorithm maintains two sets: P table and Q table. The P table stores nodes that have been searched but have not been added to the optimal path tree. The Q table maintains those nodes that have been added to the optimal path tree. (1) The P and Q tables are empty. Add the starting point S to the P table, and its G value is set to 0, the parent node is empty, and the G value of other nodes in the road network is set to infinity. (2) If table P is empty, the algorithm fails. Otherwise, select the node with the smallest f value in table P, call it BT, and add it to table Q. Determine whether BT is the endpoint T. If so, go to step (3). Otherwise, according to road network topology attributes and traffic rules, find each adjacent node NT of BT and perform the following steps:

(1) Calculate NT heuristic value F (NT)=g(NT)+h(NT)(2) g(NT)= G (BT)+cost(BT, NT)(3) where cost(BT, NT) is the cost of BT to NT. ② If NT is in the P table and the g value calculated by Formula (3) is smaller than the original G value of NT, then the G value of NT is updated to the result of Formula (3) and the parent node of NT is set as BT. ③ If NT is in the Q table and the g value calculated by Formula (3) is smaller than the original G value of NT, then the G value of NT is updated to the result of Formula (3), the parent node of NT is set as BT, and NT is moved out of the P table. ④ If NT is neither in P table nor Q table, the parent node of NT is set to BT and NT is moved to P table. ⑤ Go to Step (2). (3) Trace back from the end point T, find the parent node in turn, and add it to the optimization path until it reaches the starting point S, then the optimization path can be obtained.

Ii. Source code

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% A* ALGORITHM Demo
% Interactive A* search demo
% 0426 -- 2005.
%   Copyright 2009- 2010. The MathWorks, Inc.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%DEFINE THE 2-D MAP ARRAY
% 
% n=input('Please insert the obstacle numbers');
% 
% for i=1:n
% 
%     ab=input('Please insert the obstacle numbers') % lazy (I,:) clear all close all CLC tic % data1=load('newMoun300.dat');
l = 100; % Length of terrain data, that is, length of Y-axis data w =150; 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 mapstep =5; % grid size nl = l/mapstep; nw = w/mapstep; The maximum nw %30
OPEN_COUNT=0;
CLOSED_COUNT=0;
MAP=2*(ones(nw,nl));
for z=1:1:3
% data(nl,nw);
z
p0.x = 20;
p0.y = 60;
p0.z = 1754; Xval =p0.x/mapstep; yval=p0.y/mapstep; MAP(p0.x/mapstep,p0.y/mapstep)=1;
% xval=p0.x;
% yval=p0.y;
% MAP(p0.x,p0.y)=1; xStart=xval; %Starting Position Start point1yStart=yval; %Starting Position p1.x =120;

p1.y = 60;
p1.z = 1342; % destination xTarget=floor(p1.x/mapstep);
yTarget=floor(p1.y/mapstep);
% zTarget=floor(p1.z);
% 
MAP(p1.x/mapstep,p1.y/mapstep)=0;
% xTarget=floor(p1.x);
% yTarget=floor(p1.y);
% % zTarget=floor(p1.z);
% 
% MAP(p1.x,p1.y)=0; % maxpoint = (p1.x-p0.x)/mapstep; % Maximum number of points CLCswitch z
       case 1MAP=dataprocess(MAP); % % dataprocess(nw,nl,mapstep); % Terrain data dataprocess; % Process terrain data % I =1
%  for i1 = 10:1:14
%                 for j1 = 10:1:12
%                    MAP(i1,j1)=- 1;
% 
%                 end
%                end
%               for i1 = 6:1:10
%                 for j1 = 16:1:18
%                    MAP(i1,j1)=- 1;
% 
%                 end
%               end
%               for i1 = 14:1:18
%                 for j1 = 16:1:18
%                    MAP(i1,j1)=- 1;
% 
%                 end
%               end
%              continue;
       case 2% dataprocess1(nw,nl,mapstep); % Handle obstacles1Terrain data MAP= dataProcess1 (MAP); % Process terrain data % I =2
% for i1 = 10:1:14
%                 for j1 = 10:1:12
%                    MAP(i1,j1)=- 1;
% 
%                 end
%                end
%               for i1 = 6:1:10
%                 for j1 = 16:1:18
%                    MAP(i1,j1)=- 1;
% 
%                 end
%               end
%               for i1 = 14:1:18
%                 for j1 = 16:1:18
%                    MAP(i1,j1)=- 1;
% 
%                 end
%               end
%                 for i1 = 9:1:11
%                 for j1 = 13:1:15
%                    MAP(i1,j1)=- 1;
% 
%                 end
%                 end
%                 continue;
              
  case 3% dataprocess2(nw,nl,mapstep); % Handle obstacles2Terrain data MAP= dataProcess2 (MAP); % Process terrain data % I =3
%  for i1 = 10:1:14
%                 for j1 = 10:1:12
%                    MAP(i1,j1)=- 1;
% 
%                 end
%                end
%               for i1 = 6:1:10
%                 for j1 = 16:1:18
%                    MAP(i1,j1)=- 1;
% 
%                 end
%               end
%               for i1 = 14:1:18
%                 for j1 = 16:1:18
%                    MAP(i1,j1)=- 1;
% 
%                 end
%               end
%                 for i1 = 19:1:21
%                 for j1 = 11:1:13
%                    MAP(i1,j1)=- 1;
% 
%                 end
%                 end
%               continue;
      
   end

%   for i1 = 10:1:14
%                 for j1 = 10:1:12
%                    MAP(i1,j1)=- 1;
% 
%                 end
%                end
%               for i1 = 6:1:10
%                 for j1 = 16:1:18
%                    MAP(i1,j1)=- 1;
% 
%                 end
%               end
%               for i1 = 14:1:18
%                 for j1 = 16:1:18
%                    MAP(i1,j1)=- 1;
% 
%                 end
%               end
%             

%  for i1 = 10:1:14
%                 for j1 = 10:1:12
%                    MAP(i1,j1)=- 1;
% 
%                 end
%                end
%               for i1 = 6:1:10
%                 for j1 = 16:1:18
%                    MAP(i1,j1)=- 1;
% 
%                 end
%               end
%               for i1 = 14:1:18
%                 for j1 = 16:1:18
%                    MAP(i1,j1)=- 1;
% 
%                 end
%               end
%                 for i1 = 9:1:11
%                 for j1 = 13:1:15
%                    MAP(i1,j1)=- 1;
% 
%                 end
%                 end
              
%  for i1 = 10:1:14
%                 for j1 = 10:1:12
%                    MAP(i1,j1)=- 1;
% 
%                 end
%                end
%               for i1 = 6:1:10
%                 for j1 = 16:1:18
%                    MAP(i1,j1)=- 1;
% 
%                 end
%               end
%               for i1 = 14:1:18
%                 for j1 = 16:1:18
%                    MAP(i1,j1)=- 1;
% 
%                 end
%               end
%                 for i1 = 19:1:21
%                 for j1 = 11:1:13
%                    MAP(i1,j1)=- 1;
% 
%                 end
%               end





OPEN=[];
%CLOSED LIST STRUCTURE
%--------------
%X val | Y val |
%--------------
% CLOSED=zeros(MAX_VAL,2);
CLOSED=[];

%Put all obstacles on the Closed list
k=1; %Dummy counterfor i=1:nw
    for j=1:nl
        if(MAP(i,j) == - 1)
            CLOSED(k,1)=i;
            CLOSED(k,2)=j;
            k=k+1; CLOSED_COUNT=size(closed,1);
%setSet the starting node as the first node to xval; yNode=yval; OPEN_COUNT=1;
path_cost=0; %h goal_distance=distance(xNode,yNode,xTarget,yTarget); OPEN(OPEN_COUNT,:)=insert_open(xNode,yNode,xNode,yNode,path_cost,goal_distance,goal_distance); % inserts the starting point into the open table, and sets the first element of the open table to0, into the close table. * When placed in close, the first element in open is set0
OPEN(OPEN_COUNT,1) =0;
CLOSED_COUNT=CLOSED_COUNT+1;
CLOSED(CLOSED_COUNT,1)=xNode;
CLOSED(CLOSED_COUNT,2)=yNode;
NoPath=1; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % START ALGORITHM % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % %while((xNode ~= xTarget || yNode ~= yTarget) && NoPath == 1)
%  plot(xNode+. 5,yNode+. 5.'go');
 exp_array=expand_array(xNode,yNode,path_cost,xTarget,yTarget,CLOSED,nw,nl);
 exp_count=size(exp_array,1);
 %UPDATE LIST OPEN WITH THE SUCCESSOR NODES
 %OPEN LIST FORMAT
 %--------------------------------------------------------------------------
 %IS ON LIST 1/0 |X val |Y val |Parent X val |Parent Y val |h(n) |g(n)|f(n)|
 %--------------------------------------------------------------------------
 %EXPANDED ARRAY FORMAT
 %--------------------------------
 %|X val |Y val ||h(n) |g(n)|f(n)|
 %--------------------------------
 for i=1:exp_count
    flag=0;
    for j=1:OPEN_COUNT
        if(exp_array(i,1) == OPEN(j,2) && exp_array(i,2) == OPEN(j,3) )
            OPEN(j,8)=min(OPEN(j,8),exp_array(i,5));
            if OPEN(j,8)== exp_array(i,5%UPDATE PARENTS,gn,hn open (j, n, n, n, n); %UPDATE PARENTS,gn,hn open (j,4)=xNode;
                OPEN(j,5)=yNode;
                OPEN(j,6)=exp_array(i,3);
                OPEN(j,7)=exp_array(i,4); end; %End of minimum fn check flag=1; end; %End of node check %if flag == 1
%             break; end; %End of jfor
Copy the code

3. Operation results

Fourth, note

Version: 2014 a