A list,
Algorithm A is A typical heuristic search algorithm, based on Dijkstra algorithm, which is 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 Equation (1). F (n)=g(n)+h(n)(1) where, 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 passage cost from the starting node to the current node, and the second part H (n) is the estimated value of the passage cost from the current node to the end. The algorithm selects the node with the smallest f(n) value as the next node on the optimal path every time it expands. In practical application, if the shortest Distance is taken as the optimization goal, H (n) is often taken as Euclidean Distance or Manhattan Distance from the current point to the destination. If h(n)=0, it means that no information about the current node and the end point is utilized, and algorithm A degenerates into uninspired Dijkstra algorithm, and the search space of the algorithm becomes larger and the search time becomes longer. A* algorithm steps 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 nodes that have been added to the optimal path tree. (1) Table P and table Q are set to empty, the starting point S is added to table P, 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 the P table is empty, the algorithm fails. Otherwise, select the node in table P with the lowest f value, denoting it as BT, and add it to table Q. Determine whether BT is the end point T, if so, go to Step (3); Otherwise, each adjacent node NT of BT is found according to network topology attributes and traffic rules, and the following steps are performed:
(1) 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) is the passage cost from BT to NT. (2) 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 table Q, and the g value calculated by Equation (3) is smaller than the original G value of NT, then the g value of NT is updated to the result of Equation (3), the parent node of NT is set as BT, and NT is moved out of table P. (4) if NT is neither in the P table nor in the Q table, then the parent node of NT is set to BT and NT is moved to the 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 the starting point S, then the optimization path can be obtained.
Part of the source code
%main, the number of robot_number currently supported2~6; clear all; close all; clear variables; clc; robot_number=3;
[map,grid1,Nrow,Ncol,sorting_table,Obstacle]=initial_input();
% Obstacle_index=max(round(rand(1.30)*length(Obstacle)),1);
% Obstacle_index=randperm(length(Obstacle),10);
Obstacle_index=[48.24.70.43.34.15.59.12.58.35.46.61.4.79.30.63.60.31.67.37.6.20.50.73.72.56.47.14.78.62]; mission_list=[]; start_node=[];for i=1:length(Obstacle_index)
mission_list=[mission_list,Obstacle(Obstacle_index(i))];
end
mission=mission_list(1:robot_number); % start_node=Obstacle(Randperm (length(Obstacle),robot_number)); start_node=[166.352.5.12.120.220.240.23.24.25.26.27.28.29.30.31.32.33.34.35.36.37.38.39.40];
for k1=1:robot_number robot_ID(k1)=robot(); robot_ID(k1).location=start_node(k1); end [scedule_table,undistributed_mission]=Schedule1(robot_ID,mission,Nrow,Ncol); % sceduLE_table Index of the first action task of the result, in the format of most cases1:length(mission), the second behavior is the car to which the task is assigned.for i=1:length(scedule_table(1,:))
end_node(scedule_table(2,i))=mission(scedule_table(1,i));
end
for k2=1:robot_number
map3=map;
for map_i = 1:robot_number
if map_i~=k2
[ia,ib]=ind2sub([Nrow,Ncol],start_node(map_i));
map3(Ncol-ib+1,ia)=2;
[ia1,ib1]=ind2sub([Nrow,Ncol],end_node(map_i));
map3(Ncol-ib1+1,ia1)=2;
end
end
[sorting_table_index]= assign_sorting_table(end_node(k2),sorting_table);
allpath_ID(k2)=allpath([Astar_method(start_node(k2),end_node(k2),0,map3),Astar_method(end_node(k2),sorting_table(sorting_table_index,1),1,map3),...
sorting_table(sorting_table_index,2:4),Astar_method(sorting_table(sorting_table_index,5),end_node(k2),1,map3)]);
end
if length(robot_ID)> 1% Check all_conflict for all two-path conflicts=nchoosek(1:length(robot_ID), 2);
k1=1; iters=length(all_conflict(:,1)) *2;
while k1<=length(all_conflict(:,1))&iters>0;
if k1==1
[wait_path1,wait_path2,result1]=Enqueue1(allpath_ID(all_conflict(k1,1)).wait_path,...
allpath_ID(all_conflict(k1,2)).wait_path,1.map);
[wait_path4,wait_path3,result2]=Enqueue1(allpath_ID(all_conflict(k1,2)).wait_path,...
allpath_ID(all_conflict(k1,1)).wait_path,1.map);
if (result1==11)&(result2==11)
disp('The first two cars can't get out of the way and need to rerouted.')
else
if (result2==11)|((result1==10)&(length(wait_path1)+length(wait_path2)<=length(wait_path3)+length(wait_path4)))
allpath_ID(all_conflict(k1,1)).wait_path=wait_path1;
allpath_ID(all_conflict(k1,2)).wait_path=wait_path2;
else
allpath_ID(all_conflict(k1,1)).wait_path=wait_path3;
allpath_ID(all_conflict(k1,2)).wait_path=wait_path4;
end
end
k1=k1+1;
else
[wait_path5,wait_path6,result3]=Enqueue1(allpath_ID(all_conflict(k1,1)).wait_path,...
if result3==10
allpath_ID(all_conflict(k1,1)).wait_path=wait_path5;
allpath_ID(all_conflict(k1,2)).wait_path=wait_path6;
k1=k1+1;
else
[wait_path6,wait_path5,result3]=Enqueue1(allpath_ID(all_conflict(k1,2)).wait_path,...
if result3==10
allpath_ID(all_conflict(k1,1)).wait_path=wait_path5;
allpath_ID(all_conflict(k1,2)).wait_path=wait_path6;
k1=1;
else
if result3==12
k1=1;
% disp('A couple of cars can't get out of the way and need to rerouted.')
end
end
end
end
iters=iters- 1;
if iters==0;
disp('Exceed maximum iteration number, need to replan path') end end % Reference program in Archive astardemo.m % stores feasible nodes, orientation1.2.3.4
function [path]=Astar_method(start_node,end_node,loading,map,stop_car)
% Nrow=20; start_node=115; shelf_node=225; end_node=143; loading=1; setOpen=[start_node]; setOpenCosts=[0]; setOpenHeuristics=[Inf]; setClosed=[]; setClosedCosts=[]; %load /Users/jindi/Desktop/map.txt
Nrow=length(map(1:)); Ncol=length(map(:,1));
% if[Nrow,Ncol] = ind2Sub ([Nrow,Ncol],end_node); % Changed the starting and ending points to no obstaclesmap(Ncol-ylabID_end+1,xlabID_end)=0;
if loading==0
for i=1:(Nrow*Ncol)
connect_node_name=[0.0.0.0]; [xlabID,ylabID]= ind2Sub ([Nrow,Ncol], I);if (xlabID<Nrow)
if (~ismember(map(Ncol-ylabID+1,xlabID+1),2.99]))
connect_node_name(1)=i+1;
end
end
if (ylabID<Ncol)
%mapThe coordinates of the grid are inversely related to the Y-axis and the number of columnsif(~ismember(map(Ncol-ylabID,xlabID),[2.99]))
connect_node_name(2)=i+Nrow;
end
end
if (xlabID>1)
if (~ismember(map(Ncol-ylabID+1,xlabID- 1),2.99]))
connect_node_name(3)=i- 1;
end
end
if (ylabID>1)
if(~ismember(map(Ncol-ylabID+2,xlabID),[2.99]))
connect_node_name(4)=i-Nrow;
end
end
connect_node_ID(i)=connect_node([connect_node_name,connect_node_name]);
end
else
for i=1:(Nrow*Ncol)
connect_node_name=[0.0.0.0]; [xlabID,ylabID]= ind2Sub ([Nrow,Ncol], I);if (xlabID<Nrow)
if(~ismember(map(Ncol-ylabID+1,xlabID+1),1.2.3.99]))
connect_node_name(1)=i+1;
end
end
if (ylabID<Ncol)
% [xlabID1,ylabID1]=ind2sub([Nrow,Nrow],i+Nrow)
if(~ismember(map(Ncol-ylabID,xlabID),[1.2.3.99]))
connect_node_name(2)=i+Nrow;
end
end
if (xlabID>1)
if(~ismember(map(Ncol-ylabID+1,xlabID- 1),1.2.3.99]))
connect_node_name(3)=i- 1;
end
end
if (ylabID>1)
if(~ismember(map(Ncol-ylabID+2,xlabID),[1.2.3.99]))
connect_node_name(4)=i-Nrow; End end % Duplicate path set for the right side first connect_node_ID(I)=connect_node([connect_node_name connect_node_name]); end endCopy the codeThird, the operation result
Fourth, note
Version: 2014 a