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

%Main

clc
clear


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Grid and path parameters

%Grid size [y,x,z] and resolution
sizeE=[80 80 20];
d_grid=1;

%Starting point
x0=5;
y0=7;
z0=12;

%Arrival point
xend=68;
yend=66;
zend=6;

%Number of points with low elevation around start and end point area 
n_low=3;

%A* or Theta* cost weights
kg=1;
kh=1.25;
ke=sqrt((xend-x0)^2+(yend-y0)^2+(zend-z0)^2);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Map definition

%Average flight altitude
h=max(z0,zend);

%Points coordinates in [y,x,z] format
P0=[y0 x0 z0];
Pend=[yend xend zend];

%Generate map
[E,E_safe,E3d,E3d_safe]=grid_3D_safe_zone(sizeE,d_grid,h,P0,Pend,n_low);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Path generation

%Store gains in vector
K=[kg kh ke];

%Measure path computation time
tic

%Generate path (chose one)
% [path,n_points]=a_star_3D(K,E3d_safe,x0,y0,z0,xend,yend,zend,sizeE);
% [path,n_points]=theta_star_3D(K,E3d_safe,x0,y0,z0,xend,yend,zend,sizeE);
[path,n_points]=lazy_theta_star_3D(K,E3d_safe,x0,y0,z0,xend,yend,zend,sizeE);

T_path=toc;


%Path waypoints partial distance

%Initialize
path_distance=zeros(n_points,1);

for i=2:n_points 
	path_distance(i)=path_distance(i- 1) +sqrt((path(i,2)-path(i- 1.2)) ^2+(path(i,1)-path(i- 1.1)) ^2+(path(i,3)-path(i- 1.3)) ^2);      
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Plot

%Map grid
x_grid=1:d_grid:sizeE(2);
y_grid=1:d_grid:sizeE(1);

%Path on safe map
figure(1)
surf(x_grid(2:end- 1),y_grid(2:end- 1),E_safe(2:end- 1.2:end- 1))
hold on
plot3(x0,y0,z0,'gs')
plot3(xend,yend,zend,'rd')
plot3(path(:,2),path(:,1),path(:,3),'yx')
plot3(path(:,2),path(:,1),path(:,3),'w')
axis tight
axis equal
view(0.90);
colorbar

%Path on standard map
figure(2)
surf(x_grid(2:end- 1),y_grid(2:end- 1),E(2:end- 1.2:end- 1))
hold on
plot3(x0,y0,z0,'gs')
plot3(xend,yend,zend,'rd')
plot3(path(:,2),path(:,1),path(:,3),'yx')
plot3(path(:,2),path(:,1),path(:,3),'w')
axis tight
axis equal
view(0.90);
colorbar

%Path height profile
figure(3)
plot(path_distance,path(:,3))
grid on
xlabel('Path distance')
ylabel('Path height')
%A star algorithm


function [path,n_points]=a_star_3D(K,E3d_safe,x0_old,y0_old,z0_old,xend_old,yend_old,zend_old,sizeE)



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Cost weights

kg=K(1);
kh=K(2);
ke=K(3);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Algorithm


%If start and end points are not integer, they are rounded for the path genertion. Use of floor and ceil so they are different even if very close
x0=floor(x0_old);
y0=floor(y0_old);
z0=floor(z0_old);
xend=ceil(xend_old);
yend=ceil(yend_old);
zend=ceil(zend_old);


%Initialize

%Size of environment matrix
y_size=sizeE(1);
x_size=sizeE(2);
z_size=sizeE(3);


%Node from which the good neighbour is reached
came_fromx=zeros(sizeE);
came_fromy=zeros(sizeE);
came_fromz=zeros(sizeE);
came_fromx(y0,x0,z0)=x0;
came_fromy(y0,x0,z0)=y0;
came_fromz(y0,x0,z0)=z0;

%Nodes already evaluated
closed=[];

%Nodes to be evaluated
open=[y0,x0,z0];

%Cost of moving from start point to the node
G=Inf*ones(sizeE);
G(y0,x0,z0)=0;

%Initial total cost
F=Inf*ones(sizeE);
F(y0,x0,z0)=sqrt((xend-x0)^2+(yend-y0)^2+(zend-z0)^2);

%Initialize
exit_path=0;

%While open is not empy
while isempty(open)= =0 && exit_path==0
    
    %Find node with minimum f in open
    
    %Initialize
    f_open=zeros(size(open,1),1);
    
    %Evaluate f for the nodes in open
    for i=1:size(open,1)
        f_open(i,:)=F(open(i,1),open(i,2),open(i,3));
    end
    
    %Find the index location in open for the node with minimum f
    [~,i_f_open_min]=min(f_open);
    
    %Location of node with minimum f in open
    ycurrent=open(i_f_open_min,1);
    xcurrent=open(i_f_open_min,2);
    zcurrent=open(i_f_open_min,3);
    
    point_current=[ycurrent, xcurrent, zcurrent];
    
    
    %Check if the arrival node is reached
    if xcurrent==xend && ycurrent==yend && zcurrent==zend
        
        %Arrival node reached, exit and generate path
        exit_path=1;
        
    else
        
        %Add the node to closed
        closed(size(closed,1) +1,:)=point_current;
        
        %Remove the node from open
        i_open_keep=horzcat(1:i_f_open_min- 1,i_f_open_min+1:size(open,1));
        open=open(i_open_keep,:);
        
        %Check neighbour nodes
        for i=- 1:1
            for j=- 1:1
                for k=- 1:1                    
                    
                    %If the neighbour node is within grid
                    if xcurrent+i>0 && ycurrent+j>0 && zcurrent+k>0 && xcurrent+i<=x_size && ycurrent+j<=y_size && zcurrent+k<=z_size
                        
                        %If the neighbour node does not belong to open and to closed

                        check_open=max(sum([ycurrent+j==open(:,1) xcurrent+i==open(:,2) zcurrent+k==open(:,3)].2));
                        check_closed=max(sum([ycurrent+j==closed(:,1) xcurrent+i==closed(:,2) zcurrent+k==closed(:,3)].2));
                        
                        if isempty(check_open)= =1
                            check_open=0;
                        end
                        
                        if isempty(check_closed)= =1
                            check_closed=0;
                        end

                        if check_open<3 && check_closed<3                            
                            
                            
                            %Add the neighbour node to open
                            open(size(open,1) +1, :)=[ycurrent+j, xcurrent+i, zcurrent+k];
                            
                            %Evaluate the distance from start to the current node + from the current node to the neighbour node
                            g_try=G(ycurrent,xcurrent,zcurrent)+sqrt(i^2+j^2+k^2);
                            
                            %If this distance is smaller than the neighbour distance
                            if g_try<G(ycurrent+j,xcurrent+i,zcurrent+k)
                                
                                %We are on the good path, save information
                                
                                %Record from which node the good neighbour is reached
                                came_fromy(ycurrent+j,xcurrent+i,zcurrent+k)=ycurrent;
                                came_fromx(ycurrent+j,xcurrent+i,zcurrent+k)=xcurrent;
                                came_fromz(ycurrent+j,xcurrent+i,zcurrent+k)=zcurrent;
                                


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)