1. Introduction
Algorithm
A Algorithm A is a typical heuristic search algorithm based on Dijkstra's algorithm. It is widely used in game maps and the real world to find the shortest path between two points. The main thing of the A algorithm is to maintain a heuristic evaluation function, as shown in equation (1). f(n)=g(n)+h(n)(1) Among them, f(n) is the corresponding heuristic function when the algorithm searches for each node. It consists of two parts, the first part g(n) is the actual travel cost from the starting node to the current node, and the second part h(n) is the estimated value of the travel cost from the current node to the end point. Each time the algorithm expands, the node with the smallest f(n) value is selected as the next node on the optimal path. In practical applications, if the shortest distance is taken as the optimization goal, h(n) is often taken as the Euclidean distance or Manhattan distance from the current point to the end point. If h(n)=0, it means that no information about the current node and end point is used, and the A algorithm degenerates to the non-heuristic Dijkstra algorithm. The algorithm search space becomes larger and the search time becomes longer.
The steps of the A* algorithm are as follows. The algorithm maintains two sets: P list and Q list. The P table stores those 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) Set the P table and Q table to be empty, add the starting point S to the P table, set its g value to 0, the parent node is empty, and set the g value of other nodes in the road network to infinity.
(2) If the P list is empty, the algorithm fails. Otherwise, select the node with the smallest f value in the P list, record it as BT, and add it to the Q list. Judge whether BT is the end point T, if yes, go to step (3); otherwise, find each adjacent node NT of BT according to the topological attributes of the road network and traffic rules, and perform the following steps:
①Calculate the heuristic value of
NT f(NT)=g(NT)+h(NT)(2)
g(NT)=g(BT)+cost(BT, NT)(3)
where cost(BT, NT) It is the cost of passing 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, update the g value of NT to the result of formula (3), and set the parent node of NT 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, update the g value of NT to the result of formula (3), set the parent node of NT to BT, and Move NT to the P list.
④ If NT is neither in the P list nor in the Q list, set the parent node of NT to BT, and move NT to the P list.
⑤Go to step (2) to continue execution.
(3) Backtrack from the end point T, find the parent node in turn, and add it to the optimized path until the starting point S, then the optimized path can be obtained.
Second, the source code
clc;
clear all;
[datax,datay,dataz]=loadmap('map.xyz');
startpoint=[datax(1,30),datay(30,1),dataz(30,30)+0.1];%起点
endpoint=[datax(1,390),datay(390,1),dataz(390,390)+0.1];%终点
q=endpoint-startpoint;
q=q/norm(q);
direction=q;
position=startpoint;
limit=[pi/6,pi/6];
dataz=dataz.*2.5;
% r_posi=[300,320];
% R=3;
% dataz=radar_pos(r_posi,R,datax,datay,dataz);
% r_posi1=[100,120];R1=3;
r_posi2=[200,150];R2=4;
r_posi3=[300,320];R3=5;
r_posi4=[150,320];R4=3.5;
r_posi5=[200,80];R5=4.2;
% dataz=radar_pos(r_posi1,R1,datax,datay,dataz);
dataz=radar_pos(r_posi2,R2,datax,datay,dataz);
dataz=radar_pos(r_posi3,R3,datax,datay,dataz);
dataz=radar_pos(r_posi4,R5,datax,datay,dataz);
dataz=radar_pos(r_posi5,R5,datax,datay,dataz);
lest=sqrt(sum((position-endpoint).^2));
h=0.5;
k=0;
while lest>h
k=k+1
route(k,:)=position;
[open_list,dir_list]=openlist(position,direction,h,limit);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fate=zeros(25,3);
for i=1:25
xipt=open_list(i,1);
yipt=open_list(i,2);
[zopt,xind,yind]= mappoint(xipt,yipt,datax,datay,dataz);
if(zopt>open_list(i,3))
fate(i,:)=[1,1,1];
open_list(i,:)=[0,0,0];
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
G=k*h;
H=H_func(open_list,endpoint);
[mini,ind]=min(H);
lest=sqrt(sum((position-endpoint).^2));
F=G+mini;
position=open_list(ind,:);
direction=dir_list(ind,:);
end
x=route(:,1);
y=route(:,2);
z=route(:,3)+21;
figure(1)
mesh(datax,datay,dataz+20);
hold on;
% view(0,10)
plot3(x,y,z,'r--*');
axis equal;
3. Remarks
Add QQ912100926 for complete code or writing.
Review >>>>>>
[Path Planning] Particle Swarm Optimization Algorithm for 3D UAV Path Planning [Matlab 012]
[Path Planning] Genetic Algorithm for Open Vehicle Path Planning for Multiple Logistics Centers [Matlab 013]
[Path planning] Robot grid path planning based on particle swarm algorithm [Matlab 014]
[Path planning] Ant colony algorithm to solve the shortest path [Matlab 015]
[Path planning] Immune algorithm for logistics center location selection [Matlab 016]
[Path planning] Three-dimensional path planning for drones
based on artificial bee colony [Matlab 017] [Path planning] Genetic algorithm for robot path planning based on grid map [Matlab 018]
[Path planning] Ant colony algorithm for multiple drones Attack scheduling [Matlab 019]
[Path planning] Genetic algorithm based on grid map robot optimal path planning [Matlab 020]
[Path planning] Genetic algorithm for multi-UAV cooperative target allocation modeling considering the allocation order [ Matlab 021 issue]
[path planning] ant colony algorithm multi-center vrp problem [Matlab 022]
[path planning] ant colony algorithm to solve multi-center VRP with time window [Matlab 023 issue]
[path planning] many genetic algorithms Central VRP solution [Matlab 024]
[Path planning] Simulated annealing to solve the VRP problem [Matlab 025]
[Path planning] A star grid path planning [Matlab 026]
[Path planning] Based on a two-way cross factor Optimizing particle swarm grid map path planning [Matlab 027]
[Path Planning] [TSP] Ant Colony Algorithm for Solving TSP Problem with GUI [Matlab 028]
[Path Planning] Ant Colony Algorithm for Raster Map Path Planning [Matlab 029]
[Path Planning] Genetic Algorithm for Traveling Salesman TSP [ Matlab Issue 030]
[Path Planning] Traveling Salesman TSP Problem of Simulated Annealing Algorithm [Matlab 031]
[Path Planning] Smart Car Path Planning of Ant Colony Algorithm [Matlab 032]
[Path Planning] Huawei Cup: UAV based on matlab Optimized application in emergency rescue and disaster relief [Matlab 033]
[Path planning] MATLAB's minimum cost and maximum flow calculation problem [Matlab 034]