1. Introduction
Suppose someone wants to move from point A to point B separated by a wall. As shown in the figure below, the green is the start point A, the red is the end point B, and the blue square is the wall in the middle.
You first notice that the search area is divided into square grids by us. Like this, simplifying the search area is the first step in pathfinding. This method simplifies the search area into a two-dimensional array. Each element of the array is a square of the grid, and the squares are marked as passable and unpassable. The path is described as the set of squares we pass from A to B. Once the path is found, our people walk from the center of one square to another until they reach the destination.
These midpoints are called "nodes." When you read other pathfinding materials, you will often see people discussing nodes. Why not describe them as squares? Because it is possible that your path is divided into other structures that are not squares. They can be rectangular, hexagonal, or any other shape. Nodes can be placed anywhere in the shape-they can be in the center, or along the border, or whatever. We use this system, anyway, because it is the simplest.
Starting the search
As we did with the grid in the figure above, once the search area is converted into easy-to-handle nodes, the next step is to guide a search to find the shortest path. In the A* pathfinding algorithm, we start from point A, check the adjacent squares, and expand outward until we find the target.
We do the following operations to start the search:
1. Start from point A, and save it as a pending point in an "open list". The open list is like a shopping list. Although there is only one element in the list now, there will be more in the future. Your path may pass through the squares it contains, or it may not. Basically, this is a list of squares to be checked.
2. Look for all reachable or passable squares around the starting point, and skip over the squares with walls, water, or other inaccessible terrain. Reachable or passable squares are added to the open list. Save point A as the "parent square" for all these squares. When we want to describe the path, the information of the parent grid is very important. Its specific purpose will be explained later.
3. Delete point A from the open list, add it to a "close list", and save all the squares that do not need to be checked again in the list.
At this point, you should form the structure shown in the figure. In the picture, the dark green square is the center of your starting square. It is stroked in light blue to indicate that it has been added to the closed list. All adjacent cells are now in the open list, and they are stroked in light green. Each square has a gray pointer pointing back to their parent square, which is the starting square.
Next, we choose to turn on the adjacent square in the list and roughly repeat the previous process, as follows. But which square should we choose? The one with the lowest F value.
Path scoring
The key to choosing which square to pass on the path is the following equation:
F = G + H
Here:
- G = The cost of moving from the starting point A to the specified square on the grid along the generated path.
- H = The estimated cost of moving from that square on the grid to the end point B. This is often called heuristic and may confuse you a bit. The reason for this is because it is just a guess. There is no way to know the length of the path in advance, because there may be various obstacles (walls, water, etc.) on the road. Although this article only provides a method for calculating H, you can find many other methods on the Internet.
Our path is generated by repeatedly traversing the open list and selecting the square with the lowest F value. The article will describe this process in more detail. First, let's take a closer look at how to calculate this equation.
As mentioned above, G represents the cost of moving from the starting point to the current point along the path. In this example, we set the cost of horizontal or vertical movement to 10, and the cost of diagonal direction to 14. We take these values because the distance along the diagonal is the square root of 2 (don't be afraid), or about 1.414 times the cost of moving horizontally or vertically. For simplicity, we approximate 10 and 14. The ratio is basically correct, and we avoid root operations and decimals. This is not just because we are afraid of trouble or dislike mathematics. Using such integers is also faster for computers. You will soon find that if you do not use these simplified methods, pathfinding will become very slow.
Since we are calculating the G value of a certain square along a specific path, the method of evaluation is to take the G value of its parent node, and then according to whether it is diagonal or right angle direction (non-diagonal) relative to the parent , Increase by 14 and 10 respectively. (G value of a square = G value of the parent node + 10 or +14) In the example, the demand for this method will become more, because we have obtained more than one square from the starting square.
The H value can be estimated in different ways. The method we use here is called the Manhattan method, which calculates the sum of the number of horizontal and vertical squares from the current grid to the destination grid, ignoring the diagonal direction. Then multiply the result by 10. This is called the Manhattan method because it looks like counting the number of blocks in the city from one place to another, where you cannot cross the blocks diagonally. It is very important that we ignore all obstacles. This is an estimate of the remaining distance, not an actual value, which is why this method is called a heuristic.
The value of F is the sum of G and H. The results of the first search can be seen in the chart below. The scores of F, G and H are written in each square. As indicated by the square immediately to the right of the starting grid, F is printed in the upper left corner, G in the lower left corner, and H in the lower right corner.
Now let's look at these squares. The square for writing letters, G = 10. This is because it only deviates from the starting grid by one grid spacing in the horizontal direction. The squares immediately above, below and to the left of the starting square have G values equal to 10. The G value of the diagonal square is 14.
The H value is obtained by solving the Manhattan distance to the red target grid (|x2-x1|+|y2-y1|)*10, which only moves in the horizontal and vertical directions, and ignores the wall in the middle. In this way, the square to the right of the starting point is 3 squares away from the red square, and the H value is 30. The square above this square has a distance of 4 squares (remember, it can only be moved in the horizontal and vertical directions), and the H value is 40. You should probably know how to calculate the H value of other squares~.
The F value of each grid is simply obtained by adding G and H.
Keep searching
To continue searching, we simply select the square with the lowest F value from the open list. Then, do the following processing to the selected square:
4. Remove it from the open list and add it to the closed list.
5. Check all adjacent grids. Skip those that are already in the closed list or are not passable (walls, water terrain, or other impassable terrain), and add them to the open list if they are not already there. Take the selected square as the parent node of the new square.
6. If an adjacent grid is already in the open list, check whether the current path is better. In other words, check whether the G value will be lower if we reach it using the new path. If not, then do nothing.
On the other hand, if the new G value is lower, change the parent node of the adjacent square to the currently selected square (in the above graph, change the direction of the arrow to point to this square). Finally, recalculate the values of F and G. If this does not seem clear enough, you can look at the diagram below.
Okay, let's see how it works. In our initial 9 squares, after the starting point is switched to the closed list, there are 8 squares left in the open list. Here, the one with the lowest F value is the grid immediately to the right of the starting grid, and its F value is 40. So we choose this square as the next square to be processed. In the image that immediately follows, it is highlighted in blue.
First, we take it out of the open list and put it in the closed list (this is why it is highlighted in blue). Then we check the adjacent grid. Oh, the three grids on the right are walls, so we skip them. The grid on the left is the starting grid. It is in the closed list, so we skip it too.
The other 4 grids are already in the open list, so we check the G value to determine whether the path is better if we get there through this grid. Let's look at the grid below the selected grid. Its G value is 14. If we move from the current grid to there, the G value will be equal to 20 (the G value to reach the current grid is 10, and moving to the upper grid will increase the G value by 10). Because the G value of 20 is greater than 14, this is not a better path. If you look at the picture, you can understand. Instead of moving one square horizontally and then one square vertically, it is as simple as moving one square diagonally.
When we repeat this process for the 4 adjacent grids that already exist in the open list, we find that none of the paths can be improved by using the current grid, so we do not make any changes. Now that we have checked all adjacent squares, we can move to the next square.
So we searched the open list, now there are only 7 grids in it, and we still choose the one with the lowest F value. Interestingly, this time, there are two grids with 54 values. How do we choose? This is not troublesome. In terms of speed, it will be faster to select the last grid added to the list. This leads to the preference of using newly found grids first when approaching the target in the pathfinding process. But it doesn't matter. (Different treatment of the same value causes different versions of the A* algorithm to find different paths of equal length.)
Then we select the grid at the bottom right of the starting grid, as shown in the figure.
This time, when we checked the adjacent grid, we found a wall on the right, so we skipped it. The top box has also been skipped. We also skipped the grid under the wall. why? Because you can't reach that grid directly without crossing the corner. You really need to go down first and then reach that square, and walk through the corner step by step. (Note: The rule of crossing corners is optional. It depends on how your nodes are placed.)
In this way, the other 5 grids are left. The other two grids below the current grid are not currently in the open list, so we add them and designate the current grid as their parent node. Of the remaining 3 grids, two are already in the closed list (the starting grid, and the grid above the current grid, highlighted in blue in the table), so we skip them. The last grid, on the left side of the current grid, will be checked to see if the G value is lower through this path. Don't worry, we are ready to check the next grid in the open list.
We repeat this process until the target cell is added to the closed list (annotation), as you can see in the figure below.
Note that the parent node of the grid below the starting grid is different from the previous one. Previously, its G value was 28, and it pointed to the upper right grid. Now its G value is 20, pointing to the grid above it. This happens somewhere in the pathfinding process, when the new path is applied, the G value is checked to become low-so the parent node is reassigned, and the G and F values are recalculated. Although this change is not important in this example, in many cases, this change can lead to huge changes in the pathfinding results.
So, how do we determine this path? It's very simple. Start from the red target grid and move towards the parent node in the direction of the arrow. This will eventually lead you back to the starting grid, this is your path! It should look like in the picture. Moving from the starting grid A to the target grid B simply moves from the midpoint of each grid (node) to the next one along the path until you reach the target point. It's that simple.
Here is the code for the next JAVA version:
A star algorithm steps:
1. The starting point is first added to the open list
2. If there is a node in the open list, take out the first node, that is, the node with the smallest F value, and
judge whether this node is the target point, if it is found, jump out;
get the node in eight directions according to this node, and find G , H, F value;
judge whether each node can pass in the map, if it fails, add it to the closed list, and jump out;
judge whether each node is in the closed list, and jump out if it is;
judge whether each node is in the open list If it is in, update the G value, F value, and its parent node; if not, add it to the open list, calculate G value, H value, F value, and add its node.
3. Delete this node from the open list and add it to the closed list;
4. Sort the open list according to the node with the smallest F value, and the smallest F value is the first;
5. Repeat steps 2, 3, 4 until the target point is in the open list, that is, it is found; the target point is not in the open list, and the open list is empty, that is, it is not found
Second, the source code
%% A* demo
clc;
clear;
% pause(3);
%% init
n=30;
starNum=1;
% starNum = randi(n*n,[1,1]);
goalNum=172;
goalNum = randi(n*n,[1,1]);
banper=0.25;
%% map init
figure('name','A*','NumberTitle','off','MenuBar','none');
global point
for ii=1:n*n
point(ii).num = ii;
point(ii).father=[];
point(ii).Gcost=[];
point(ii).Hcost=[];
end
%% banper
banList=[randi(n*n,[1,floor(banper*n*n)])];
load banList
banList(find(banList==goalNum))=[];
for jj = 1:length(banList)
if banList(jj)~=goalNum || banList(jj)~=starNum
point(banList(jj)).Gcost = Inf;
end
end
point(starNum).Gcost=0;
point(starNum).father = point(starNum).num;
point(starNum).Hcost=getHcost(point(starNum),point(goalNum),n);
%% A*core
openList = [];
closeList = [];
closeListNum=[];
openListNum=[];
openList = [openList,point(starNum)];
while length(openList)
% opneList
costList = getCost(openList,point(goalNum),n);
currentPoint = openList(find(costList==min(costList),1));
openList(find(min(costList)==costList,1))=[];
closeList = [closeList,currentPoint];
neighbourNum = getNeighbour(currentPoint,n);
closeListNum = cat(1,closeList.num);
openListNum = cat(1,openList.num);
for ii = 1:length(neighbourNum)
if neighbourNum(ii)==point(goalNum).num
point(neighbourNum(ii)).father = currentPoint.num;
point(goalNum).father = currentPoint.num;
disp('ok')
routPlot(goalNum,n);
return;
end
log1=0;
try
tmp=point(neighbourNum(ii)).Gcost;
if tmp ==inf
log1 = 1;
end
catch
log1=0;
end
if log1 || ismember(neighbourNum(ii),closeListNum)
continue;
elseif (ismember(neighbourNum(ii),openListNum))
oldGcost = getGcost(point(neighbourNum(ii)),n);
father = point(neighbourNum(ii)).father;
point(neighbourNum(ii)).father = currentPoint.num;
newGcost = getGcost(point(neighbourNum(ii)),n);
if newGcost>oldGcost
point(neighbourNum(ii)).father = father;
else
point(neighbourNum(ii)).Gcost = newGcost;
end
continue;
elseif ~ismember(neighbourNum(ii),closeListNum)
point(neighbourNum(ii)).father = currentPoint.num;
point(neighbourNum(ii)).Gcost = getGcost(point(neighbourNum(ii)),n);
point(neighbourNum(ii)).Hcost = getHcost(point(neighbourNum(ii)),point(goalNum),n);
openList = [openList,point(neighbourNum(ii))];
end
end
closeListNum = cat(1,closeList.num);
openListNum = cat(1,openList.num);
pause(0.1);
mydrawnow(starNum,goalNum,banList,closeListNum,openListNum,n);
end
Three, running results
Four, remarks
Complete code or writing to add QQ912100926 past review
>>>>>>
[Path planning] Particle swarm optimization algorithm for three-dimensional UAV path planning [Matlab 012]
[Path planning] Genetic algorithm for open vehicles in multiple logistics centers Path planning [Matlab 013]
[Path planning] Particle swarm algorithm for robot grid path planning [Matlab 014]
[Path planning] Ant colony algorithm for solving the shortest path [Matlab 015]
[Path planning] Immune algorithm for logistics center Site selection [Matlab 016]
[Path planning] Three-dimensional path planning of drones with artificial bee colony [Matlab 017]
[Path planning] Genetic algorithm based on grid map robot path planning [Matlab 018]
[Path planning] Ant Swarm algorithm for multiple drone attack scheduling [Matlab 019]
[Path planning] Genetic algorithm for optimal path planning of robots based on grid map [Matlab 020]
[Path planning] Genetic algorithm for multiple unmanned systems considering the order of distribution [Matlab 021 Issue]
[Path Planning]
Multi-center VRP Problem of Ant Colony Algorithm [Matlab 022] [Path Planning] Ant Colony Algorithm Solving Multi-center VRP with Time Window [Matlab 023 Issue]
[ Path planning] Multi-center VRP solution based on genetic algorithm [Matlab 024]
[Path planning] Simulated annealing solution to VRP problem [Matlab 025]