A brief introduction of A_star algorithm

0 the introductionWith the development of modern technology, there are more types of aircraft, and their applications are becoming more specialized and perfect. For example, DJI PS-X625 UAV is used for plant protection, BAOji X8 UAV is used for street scene shooting and monitoring patrol, and White Shark MIX underwater UAV is used for underwater rescue. The performance of the aircraft is mainly determined by the internal flight control system and external path planning. In terms of path problem, only by the operator in the concrete implementation of the task in the hands of the remote control of unmanned aerial vehicles to perform the corresponding work, may be psychological and technology put forward high request of the operator, in order to avoid personal error, the risk of causing the damage of aircraft, a solution is carried out on the aircraft flight path planning. The measurement accuracy of aircraft, the reasonable planning of flight path, the stability and safety of aircraft at work and other changes require more and more high requirements for the integrated control system of aircraft. Uav route planning is a problem to design the optimal route to ensure that UAV can complete specific flight tasks and avoid various obstacles and threat areas in the process of completing the tasks. Common path planning algorithms are shown in Figure 1.Figure 1 Common path planning algorithms This paper mainly studies the flight path planning of UAV in the cruise stage. Assuming that uav maintains constant altitude and speed in flight, flight path planning becomes a two-dimensional plane planning problem. In the path planning algorithm,AThe algorithm is simple and easy to implement. To improve ABased on the algorithm, A new and easy to understand improvement A is proposedAn algorithm for uav flight path planning. A traditionalIn this algorithm, the planning area is rasterized, and node expansion is limited to the intersection of grid lines, and there are usually two directions of motion at a certain Angle between the intersection of grid lines. Enlarge and refine the two sections of paths with angles infinitely, and then use the corresponding path planning points on the two sections respectively as tangent points to find the corresponding center of the circle that forms the tangent circle, then make an arc, and find the corresponding center Angle of the arc between the corresponding two tangent points, and calculate the length of the arc according to the following formulaWhere: R — radius of the tangent circle; Alpha — the central Angle corresponding to the arcs between the tangent points.

1 Overview of A* algorithm AThe algorithm is a heuristic function introduced on the basis of Dijstar algorithm, which evaluates the cost through the defined cost function, so as to determine the optimal path. AThe cost function of the algorithmWhere: f(x,y) — cost estimation of the initial state X0(X0,y0) to the target state X1(X1,y1); G (x,y) — the actual cost from initial state X0(X0,y0) to state N(x1,y1) in the state space; H (x,y) — the estimated cost of the best path from state N(x1,y1) to target state x1 (x1,y1). The essence of finding the shortest path is to find the minimum value of f(x,y), where the key to finding the shortest path in Equation (2) is to find the estimated cost h (x,y) value. Let the coefficient λ represent the optimal distance from the state N(x1,y1) to x1 (x1,y1). If λ<h(x,y), the search range is small and the optimal solution cannot be guaranteed. λ>h(x,y), the search range is large, time-consuming, but can find the optimal solution; λ=h(x,y), find the shortest path. Where h(x,y) is generally calculated by Euclidean distance (Equation (3)) or absolute distance (Equation (4)). AThe algorithm takes the starting point as the center, the center of the 8 surrounding grids as the next pre-selection, and continuously calculates the f(x,y) value of the pre-selection position, in which the f(x,y) value is the smallest as the current position, and is compared layer by layer until the target point appears at the adjacent point of the current position, and its minimum unit is shown in Figure 2.FIG. 2 Minimum unit AThe algorithm flow is as follows: 1) Create START node START, TARGET node TARGET, OPEN list, CLOSE list and CLOSE list are initially empty; 2) Add START to OPEN list; 3) Check the nodes in the OPEN list. If the list is empty, there is no line path. If it is not empty, select the node k with the minimum value of f(x,y). 4) Remove node K from OPEN and add it to CLOSE to determine whether node K is the TARGET node. If it is, the path is found; If not, continue to expand node K, generate the child node set of node K, set q as the child node set of k, calculate the corresponding f(x,y) value of all nodes Q, select the node with the lowest f(x,y) value, and put the node into the CLOSE list; 5) Jump to 3) until the algorithm obtains a viable path or exits with no solution.

Two, some source code

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%DEFINE THE 2-D MAP ARRAY
MAX_X=10;
MAX_Y=10;
MAX_VAL=10;
%This array stores the coordinates of the map and the 
%Objects in each coordinate
MAP=2*(ones(MAX_X,MAX_Y));

% Obtain Obstacle, Target and Robot Position
% Initialize the MAP with input values
% Obstacle=- 1,Target = 0,Robot=1,Space=2
j=0;
x_val = 1;
y_val = 1;
axis([1 MAX_X+1 1 MAX_Y+1])
grid on;
hold on;
n=0; %Number of Obstacles % BEGIN Interactive Obstacle, Target,Start Location selection
pause(1);
h=msgbox('Please Select the Target using the Left Mouse button');
uiwait(h,5);
if ishandle(h) = =1
    delete(h);
end
xlabel('Please Select the Target using the Left Mouse button'.'Color'.'black');
but=0;
while (but ~= 1) %Repeat until the Left button is not clicked
    [xval,yval,but]=ginput(1);
end
xval=floor(xval);
yval=floor(yval); xTarget=xval; %X Coordinate of the Target yTarget=yval; %Y Coordinate of the Target

MAP(xval,yval)=0; %Initialize MAP with location of the target
plot(xval+. 5,yval+. 5.'gd');
text(xval+1,yval+. 5.'Target')

pause(2);
h=msgbox('Select Obstacles using the Left Mouse button,to select the last obstacle use the Right button');
  xlabel('Select Obstacles using the Left Mouse button,to select the last obstacle use the Right button'.'Color'.'blue');
uiwait(h,10);
if ishandle(h) = =1
    delete(h);
end
while but == 1
    [xval,yval,but] = ginput(1);
    xval=floor(xval);
    yval=floor(yval);
    MAP(xval,yval)=- 1; %Put on the closed list as well
    plot(xval+. 5,yval+. 5.'ro');
 end%End of While loop
 
pause(1);

h=msgbox('Please Select the Vehicle initial position using the Left Mouse button');
uiwait(h,5);
if ishandle(h) = =1
    delete(h);
end
xlabel('Please Select the Vehicle initial position '.'Color'.'black');
but=0;
while (but ~= 1) %Repeat until the Left button is not clicked
    [xval,yval,but]=ginput(1);
    xval=floor(xval);
    yval=floor(yval); end xStart=xval; %Starting Position yStart=yval; %Starting Position
MAP(xval,yval)=1;
 plot(xval+. 5,yval+. 5.'bo');
%End of obstacle-Target pickup

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%LISTS USED FOR ALGORITHM
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%OPEN LIST STRUCTURE
%--------------------------------------------------------------------------
%IS ON LIST 1/0 |X val |Y val |Parent X val |Parent Y val |h(n) |g(n)|f(n)|
%--------------------------------------------------------------------------
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:MAX_X
    for j=1:MAX_Y
        if(MAP(i,j) == - 1)
            CLOSED(k,1)=i; 
            CLOSED(k,2)=j; 
            k=k+1;
        end
    end
end
CLOSED_COUNT=size(CLOSED,1);
%set the starting node as the first node
xNode=xval;
yNode=yval;
OPEN_COUNT=1;
path_cost=0;
goal_distance=distance(xNode,yNode,xTarget,yTarget);
OPEN(OPEN_COUNT,:)=insert_open(xNode,yNode,xNode,yNode,path_cost,goal_distance,goal_distance);
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,MAX_X,MAX_Y);
 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)); %#ok<*SAGROW>
            if OPEN(j,8)== exp_array(i,5)
                %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
    if flag == 0
        OPEN_COUNT = OPEN_COUNT+1;
        OPEN(OPEN_COUNT,:)=insert_open(exp_array(i,1),exp_array(i,2),xNode,yNode,exp_array(i,3),exp_array(i,4),exp_array(i,5)); end; %End of insertnew element into the OPEN listend; %End of ifor
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %END OF WHILE LOOP
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %Find out the node with the smallest fn 
  index_min_node = min_fn(OPEN,OPEN_COUNT,xTarget,yTarget);
  if (index_min_node ~= - 1)    
   %Set xNode and yNode to the node with minimum fn
   xNode=OPEN(index_min_node,2);
   yNode=OPEN(index_min_node,3);
   path_cost=OPEN(index_min_node,6); %Update the cost of reaching the parent node %Move the Node tolist CLOSED
  CLOSED_COUNT=CLOSED_COUNT+1;
  CLOSED(CLOSED_COUNT,1)=xNode;
  CLOSED(CLOSED_COUNT,2)=yNode;
  OPEN(index_min_node,1) =0;
  else
      %No path exists to the Target!!
      NoPath=0; %Exits the loop! end; %End of index_min_node check end; %End of While Loop %Once algorithm has run The optimal path is generated by starting of at the %last node(if it is the target node) and then identifying its parent node
%until it reaches the start node.This is the optimal path

i=size(CLOSED,1);
Optimal_path=[];
xval=CLOSED(i,1);
yval=CLOSED(i,2);
i=1;
Optimal_path(i,1)=xval;
Optimal_path(i,2)=yval;
i=i+1;

if ( (xval == xTarget) && (yval == yTarget))
    inode=0;
   %Traverse OPEN and determine the parent nodes
   parent_x=OPEN(node_index(OPEN,xval,yval),4); %node_index returns the index of the node parent_y=OPEN(node_index(OPEN,xval,yval),5);
   
   while( parent_x ~= xStart || parent_y ~= yStart)
     
           %Get the grandparents:-)
           inode=node_index(OPEN,parent_x,parent_y);
           parent_x=OPEN(inode,4); %node_index returns the index of the node parent_y=OPEN(inode,5);
           i=i+1;
    end;
 j=size(Optimal_path,1);
 %Plot the Optimal Path!
 p=plot(Optimal_path(j,1) +. 5,Optimal_path(j,2) +. 5.'bo');
 j=j- 1;
 for i=j:- 1:1
  pause(25.);
  set(p,'XData',Optimal_path(i,1) +. 5.'YData',Optimal_path(i,2) +. 5);
 drawnow ;
 end;
 plot(Optimal_path(:,1) +. 5,Optimal_path(:,2) +. 5);
else
 pause(1);
 h=msgbox('Sorry, No path exists to the Target! '.'warn');
 uiwait(h,5);
end

Copy the code

3. Operation results

Matlab version and references

1 matlab version 2014A

[1] Yang Baoyang, YU Jizhou, Yang Shan. Intelligent Optimization Algorithm and Its MATLAB Example (2nd Edition) [M]. Publishing House of Electronics Industry, 2016. [2] ZHANG Yan, WU Shuigen. MATLAB Optimization Algorithm source code [M]. Tsinghua University Press, 2017. [3] WU Qian, LUO Jinbiao, GU Xiaoqun, ZENG Qing. [4] Deng Ye, Jiang Xiangju. Optimization Algorithm of UAV 3D Trajectory Planning Based on Improved PSO [J]. Journal of Ordnance Equipment Engineering, 201,42(08). [5] Yunhong Ma, Heng Zhang, Legrong Qi, Jianliang He. Trajectory Planning Algorithm of quadrotor UAV based on improved Artificial Potential Field Method [J]. Sensors and Microsystems, 201,40(07) [5] 3d uav path planning based on improved A* algorithm [J]. Electronics optics & control, 2019,26(10) [6] jiao Yang. Research on 3d path planning of uav based on improved ant colony algorithm [J]. Ship electronic engineering, 2019,39(03)