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
%% use A * search to demonstrate %% parameter Settings % define 2d map frame MAX_X =10;
MAX_Y = 10; MAP = zeros(MAX_X,MAX_Y) % This array stores MAP coordinates and objects in each coordinate % get obstacles, target and robot positions % initialize MAP % obstacles = with input values- 1, target =1The starting point, =1, the blank space =0
j=0;
x_val = 1;
y_val = 1;
axis([1 MAX_X+1 1 MAX_Y+1[) % grid on; % join grid hold on; % with the new graph n=0; % Number of obstaclesfor BB = 1:3%% Determine the target point1); H =msgbox h=msgbox h=msgbox'Please use the left mouse button to select target'); % uiwait(h,5);
if ishandle(h) = =1
delete(h);
end
xlabel('Please use the left mouse button to select target'.'Color'.'black');
but=0; % this place can be set to1In that case, unify the target pointwhile (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 Target coordinate yTarget=yval; % Y Target coordinates MAP(xval,yval)=1; % initialize the destination location in the map plot(xval+. 5,yval+. 5.'gd');
text(xval+1,yval+. 5.'Target point') %% Identify obstacles2);
h=msgbox('Use the left mouse button to select the obstacle, use the right mouse button to select the last obstacle');
xlabel('Use the left mouse button to select the obstacle, use the right mouse button to select the last obstacle'.'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 %% determine the starting point1);
h=msgbox('Please use the left mouse button to select the initial position');
uiwait(h,5);
if ishandle(h) = =1
delete(h);
end
xlabel('Please select initial location'.'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; % yStart=yval; MAP(xval,yval)=1;
plot(xval+. 5,yval+. 5.'bo'); % End of obstacle-Target pickup %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %LISTS USED FOR ALGORITHM used in the ALGORITHM, a list of % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % OPEN LIST STRUCTURE open LIST STRUCTURE % -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- % IS ON the LIST1/0|X val |Y val |Parent X val |Parent Y val |h(n) |g(n)|f(n)| %-------------------------------------------------------------------------- OPEN=[]; % % CLOSED LIST STRUCTURE enclosed LIST STRUCTURE -- -- -- -- -- -- -- -- -- -- -- -- -- -- % X val val | Y | % -- -- -- -- -- -- -- -- -- -- -- -- -- -- % CLOSED = zeros (MAX_VAL,2);
CLOSED=[];
%Put all obstacles on the Closed listPut all obstacles in the close list k=1; % Dummy counter
for 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);
%setThe starting node as the first node sets the starting node to the first node. 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 succeeded NODES UPDATE LIST turn on THE succeeded NODES %OPEN LIST FORMAT %-------------------------------------------------------------------------- %IS ON LIST1/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 tolistCLOSED Moves a node to the list 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; After the % algorithm runs, the optimal path is generated by starting from the last node (if it is the target node) and then identifying the parent node until it reaches the start node. This is the best 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 andParent_x =OPEN(node_index(OPEN,xval,yval),4); % node_index returns the index of the node node_index Parent_y =OPEN(node_index(OPEN,xval,yval),5);
while( parent_x ~= xStart || parent_y ~= yStart)
Optimal_path(i,1) = parent_x;
Optimal_path(i,2) = parent_y;
%Get the grandparents:-)
inode=node_index(OPEN,parent_x,parent_y);
parent_x=OPEN(inode,4); % node_index returns the index of the node node_index Returns the node index parent_y=OPEN(inode,5);
i=i+1;
end;
j=size(Optimal_path,1); % Plot the Optimal Path! % Draw the best path! p=plot(Optimal_path(j,1) +. 5,Optimal_path(j,2) +. 5.'bo');
j=j- 1;
for i=j:- 1:1
h=msgbox('Sorry, no path to target found! '.'warn');
uiwait(h,5);
end
Optimal_path
[m,n]=size(Optimal_path);
for ii=1:m;
% for jj=1:1
a=Optimal_path(ii,1)
b=Optimal_path(ii,2)
MAP(a,b)=1
c=a+0.5;
d=b+0.5;
% Optimal_path(i,j)=1;
% end
plot(c,d,'*k'); hold on; end % MAP(Optimal_path(2.1),Optimal_path(2.2=))1
MAP1=rot90(MAP)
number = size(Optimal_path,1)
if (BB==1)
Optimal_path1 = Optimal_path
hold on;
else if (BB==2)
Optimal_path2 = Optimal_path
hold on;
else
Optimal_path3 = Optimal_path
end
end
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.