Table of contents

2、Dijkstra（Uniform Cost Search）

3. Heuristic search

4. A* algorithm

4.1 Algorithm Details

4.2 A and A* algorithms

4.3 Proof of A* Algorithm

4.4 Algorithm process

3. Realization

1. Experimental requirements

2. Code implementation

4. Source code

1. Introduction

When I started learning about the algorithm, there were already many good introductory articles online . If you see the following road map with red stars ★ and purple × appearing in the article :

Then this article has a high probability of referring to the A* algorithm tutorial of Red Blob Games (click to jump) . This tutorial is a good introductory tutorial, with lively interactive diagrams and easy-to-understand descriptions of the process.

The following uses BFS , greedy BFS , and Dijkstra algorithm as the introduction to gradually introduce the A* algorithm , so that there is a preliminary overall understanding of the A* algorithm .

There are three pictures in Red Blob Games to illustrate the difference between BFS , Dijkstra , and A* algorithms. If you have a little basic knowledge, you will know the general idea after reading it:

BFS (Breadth First Search): Every direction is equally expanded , so its exploration trajectory is concentric circles circle after circle, expanding evenly circle by circle like ripples.

We know that when BFS expands, it expands all adjacent nodes of a node in turn , and the opportunity for each node to be expanded is fair , that is, the expansion opportunities of each adjacent node are the same, and the order is arbitrary. In the maze problem , choose the right, top, left, and bottom of a node (it doesn’t have to be right, left, bottom, up, down, left, or right if you want) node to add to the queue , and then take the node out of the queue in the order it was put in to continue To expand , the right, upper, left, and lower nodes of this node are also added to the queue. Then the process displayed on the maze is to search for a circle with a distance of 1 from this node, and then search for a circle with a distance of 2 and a circle with a distance of 3 ...

The specific animation effect is as follows:

As can be seen from the above, BFS is a very useful algorithm for maze problems and other graph searches with equal path weights . BFS must be able to search all reachable nodes. It is a violent exhaustive search algorithm and can find the shortest path (provided that all edge weights are equal).

Dijkstra's Algorithm (Dijkstra's Algorithm ) : Some nodes (directions, paths) will be explored first, and generally those nodes and paths with smaller costs will be explored first. Because the algorithm is used to find the shortest path , the correctness of the algorithm requires that the currently known shortest path be selected for exploration every time, so its exploration trajectory is random and uneven , just like the contour map of a ridge Same.

The difference between Dijkstra and BFS is that the edge weights of each edge of the graph are different at this time, and BFS is no longer applicable at this time.

It is still a maze problem . The value in the box indicates the cost (cost) from the starting point to the box (can be understood as distance and cost) . The weight of the adjacent square depends on the color of the square: the cost of moving to brown ■ is 1 , the cost of moving to green ■ is 5 , and gray ■ represents an impenetrable obstacle.

The picture on the left is BFS, the picture on the right is Dijkstra

In the left picture , because the cost of each block is the same , using the BFS algorithm , the algorithm will directly pass through the green area to reach the end point × ; in the right picture, because the cost of the blocks is different , using the Dijkstra algorithm , the algorithm will bypass the green area to reach the end point × .

The execution animation of the two algorithms is as follows:

Left picture is BFS, right picture is Dijkstra

The specific BFS and Dijkstra algorithm process will be introduced in detail below, here you only need to know the difference between their applications.

A* Algorithm : It prioritizes paths that "seem" closer to the goal . This algorithm is also constantly looking for the estimated "currently most potential" node, which is an uneven ridge map like Dijkstra's algorithm . But compared to the chaotic ridge trajectory of the Dijkstra algorithm , it is purposeful and directional . The trajectory expansion direction always chooses the side closer to the target. In the figure below, you can see that it stretches to the tip side , because In the eyes of the A* algorithm, it is closer to the end.

It is a modification of Dijkstra's algorithm , optimized for a single objective . Dijkstra 's algorithm can find paths to all locations. But in our pathfinding algorithm, we may only need to find a path to one location, which means some of the extra overhead in Dijkstra's algorithm is unnecessary. The A* algorithm is a point-to-point path finding algorithm, just like clicking a certain position on the small map in LOL, the system will automatically find the path and get a "white line" leading to the destination position to represent it is the shortest path.

It will use some of its known heuristic information to reasonably plan its own exploration direction, avoiding some blindness of Dijkstra.

2. Specific details

What needs to be mentioned here is that the A* algorithm discussed below is only used as a pathfinding algorithm , and the discussion is limited to the search of paths in the graph . But in fact, the A* algorithm is not only applicable to path search , it is a heuristic idea , it can also be used to solve other problems such as eight digital problems, but most of the problems can be reduced to graph theory (or tree ) , so we only need to understand the path search to understand the whole algorithm idea.

This is a solution process of an eight-number problem using the A* algorithm. If we regard each square as a node of the graph and the pointer as an edge with a weight of 1, it is a tree (a special case of a graph) with the shortest path The solution process

This is just like the BFS algorithm . BFS is not only suitable for path search, such as the brute force exhaustion commonly used in the algorithm topic , but when we learn this algorithm, we only care about the path search, because the brute force exhaustion is essentially a root The node to start the search tree from .

At the same time, for the convenience of understanding, all grid graphs are used , but in fact, the application is the same and correct in all types of graph structures . This is not difficult to understand, because the grid graph can eventually be transformed into a general graph with a node+edge weight of 1 .

We are very familiar with BFS and Dijkstra algorithm , in fact, there is no need to talk about it. Our focus is the A* algorithm . The reason for the specific development of the BFS and Dijkstra algorithm process here is that on the one hand , we hope that everyone can clearly understand how the A* algorithm is obtained; on the other hand, I personally like the code of this website too much. The pictures and interactive animations are really endless.

1、BFS（Breadth First Search）

The key to BFS thinking is to continuously expand the frontier .

The python code of BFS is as follows:

The idea is:

First create a queue (Queue, first in, first out: the nodes that are put in first are expanded first) frontier , and the frontier is used to store the nodes to be expanded, so the starting point start★ needs to be put into the frontier at the beginning ;

Then create a collection (Set, the elements of the collection are out of order and will not be repeated) reached , which is used to store the nodes that have been visited, which is what we often call visit.

As long as frontier is not empty, an element current is taken from frontier for expansion . For all current neighbors next , as long as next is not in reached (that is, it has not been reached), put next in frontier , and then put it in reached to mark it as reached.

The above BFS code does not construct a path, it just tells us how to traverse all the points on the graph. Therefore, it needs to be modified to record the path of how we got to each point.

The yellow part is the modified part:

The improved BFS code can record path information

Here came_from is replaced with reached .

came_from is a dictionary (Dictionary, key-value pair, a key corresponds to value) , came_from can not only expressthe same function as reached (judging whether a node has been reached), but also judge whether there is a key-value pair whose key is this node in came_from ; It can also record the pre-sequence node of each point, and use came_from[node i] to record, so that when the end point is found, it can follow the pre-sequence node to find the starting point.

There is another part of the modification: before it was judged whether it was in reached , if not, it was directly put into the reached set; now it is judged whether it was in came_from , if not, store came_from[the node’s neighbor next ] as the current extended current .

When each node stores the information from where it came from, the situation of the whole graph is as follows:

The indicator arrow above tells who the previous node is, so that when we BFS finds the end point, we have enough information to know how we got to the end point, and we can rebuild this path. Methods as below:

The method of finding the path according to the previous node information

Starting from the goal , following the previous nodes stored in came_from , backtracking one by one to the starting point, during which the path is continuously put into the array path , because the closer the node is to the end point, the earlier it is put in, so the last stored path is a The reverse path from the end point to the start point , and finally the entire path is reversed.

The above BFS will eventually traverse all the nodes in the graph , that is, it will know the shortest path from the starting point to all nodes (provided that the weights on the graph are equal, otherwise it is not the shortest). But in fact, we generally only need to ask for a path to a certain target node , so many processes are unnecessary. The original BFS must be terminated after all nodes have been traversed, but now we only need to traverse to the target node to stop, so we can terminate in time .

The process is as follows, once the target node is found, BFS stops expanding:

After BFS finds the end point, no matter which nodes in the frontier are not expanded at this time, it should stop immediately

The modified code is as follows, only need to add a timely termination condition:

The full version of the BFS code, looking for the shortest path to a target node and ending in time

When it is found that the node currently being expanded is the end point goal× , the code ends.

2、Dijkstra（Uniform Cost Search）

The BFS discussed above can only be used when the weight of each path on the graph is equal. If the weight of each path in the graph is not exactly the same , then BFS can be used again to traverse all nodes, but the shortest path cannot be obtained, because the shortest path The one traversed first is not necessarily the shortest.

Dijkstra's algorithm is also very simple. It starts from the starting point and continues to expand to the node with the shortest total cost until it reaches the end point ( provided that the edge weight is non-negative ). The idea of simple proof is that the currently selected node with the shortest cost, and then the cost of reaching this node through other nodes must be greater than the current cost.

Because it is necessary to find the node with the shortest total cost , the original queue (queue, first in, first out) needs to be modified to a priority queue (priority queue, elements have priority, and the element with the highest priority can be returned).

At the same time, in addition to recording the previous node came_from of this node , it is also necessary to record the current cost of reaching this node cost_so_far .

Modifications based on the BFS code :

First create a priority queue frontier , put the starting point in it, and set the cost to 0 ( the smaller the priority value of PriorityQueue in python , the higher the priority , the earlier it is taken out. But I have the impression that PriorityQueue 's put([ priority, value]) the first parameter is the priority and the second parameter is the value) .

Then create came_from to record where it came from, and create cost_so_far to record the total path cost of each node currently reached.

Then continue to take out the node current with the smallest cost from the frontier to expand until it finally reaches the end of the goal .

The extended method becomes: find all adjacent nodes next of current , and calculate the new_cost of next . The calculation method is to add the current cost cost_so_far [ current ] of current to the cost graph.cost( current , next ) of this adjacent edge . If next is a node that has not been reached or new_cost is less than the known shortest path node, then add or modify the current next cost cost_so_far [ next ] , set the priority to the new cost new_cost , and add this key-value pair to the extension In the priority queue frontier , the last record node of next is current .

It is worth mentioning that the execution of the Dijkstra algorithm generally requires no negative edges , but the Dijkstra implementation code above can handle graphs with positive and negative edges , but cannot handle graphs with negative cycles .

The reason is that when a shorter path is found during expansion, it will be added to the priority queue. In the general Dijkstra algorithm, all nodes will only enter the priority queue once, but once the above code finds that a node x that reaches through other nodes has a shorter path, it will put node x into the priority queue, regardless of whether the node is expanded However, that is to give this node the opportunity to modify the shortest path again. So if the graph has negative edges and no negative cycles (meaning there is a shortest path for all nodes), the shortest path can also be found using the above code.

The renderings are as follows:

In the above picture, the cost of going to the green grid is greater than that of the brown grid . It can be seen that some brown grids will be explored first and then some green grids will be explored, that is, each time the grid with the shortest total distance is selected, it is expanding.

3. Heuristic search

The above two methods are extended in all directions . This is reasonable when our goal is to find a path to all locations or multiple locations , but if we only need a path to one location , such a time cost is not reasonable. necessary.

Our purpose is to let the expansion direction of the boundary expand towards the target position, rather than blindly expand in other directions. In order to achieve the above purpose, we need to define a heuristic function ( heuristic function ), which will be used to measure how far our current state is from the end.

The Manhattan distance is commonly used in grid graphs , which is defined as follows:

If we only use the distance calculated by the heuristic function as the priority, that is, always give priority to expanding the points closest to the end point, then the following results will be obtained:

It can be seen that with the help of the heuristic function, the end point is found faster, and this is its advantage: fast speed.

This search method is the Greedy Best First Search algorithm (Greedy Best First Search)

However, if there are obstacles, only using the calculation results of the heuristic function as the priority may not get the correct result. As follows:

It can be seen that only relying on the priority calculated by the heuristic function cannot obtain the shortest path. It abandons the advantage of ensuring correctness of the Dijstra algorithm to expand the shortest path nodes each time, so it cannot guarantee to obtain a shortest path.

Is there a way to achieve both speed and correctness? That is the A* algorithm to be introduced below.

4. A* algorithm

4.1 Algorithm Details

Dijkstra's algorithm can find the shortest path very well, but wastes unnecessary time exploring unpromising directions; only using the heuristic Greedy Best First Search algorithm (Greedy Best First Search) always chooses the most promising direction to explore , but it may not find the optimal path.

The A* algorithm uses information from both methods: the actual distance from the origin to the current location and the estimated distance from the current location to the destination .

The A* algorithm comprehensively considers the priority of each node to be expanded through the following formula :

$f(x)=g(x)+h(x)$

in:

$f(x)$ That is, $x$ the comprehensive priority of the node to be expanded, which is calculated by sum , $g(x)$ and $h(x)$ we still choose $f(x)$ the smallest node to be expanded for expansion.

$g(x)$ is $x$ the cost of the node's distance from the origin.

$h(x)$ is the estimated $x$ cost of the node from the end point .

From the above formula, it inherits the idea of Dijkstra algorithm as a whole, and always expands the shortest $f(x)$ node, which can ensure that once the end point is searched, it must be the shortest path; at the same time, $f(x)$ the calculation also takes into account the estimated distance from the end point, reducing Or avoid expanding individual unpromising nodes, so that the overall search process tends to a promising direction.

It should be noted that the above $h(x)$ selection is not arbitrary, it is the key to ensure the correctness of the final result and the speed of the search. $h(x)$ The larger the value, the faster the search speed, but it is not infinite, it has its own restrictions. If $x$ the real cost of the distance from the end point is $h^{'}(x)$ , then $h(x)$ the following requirements must be met to ensure that the optimal solution can be found, that is, it can never be greater than the real distance.

$h(x) \leq h^{'}(x)$

It can be intuitively considered that $h(x)$ it is a conservative estimate .

4.2 A and A* algorithms

One thing to note is the difference between the A algorithm and the A* algorithm. The information currently found is not clearly stated, and the definitions of the two are somewhat vague. The following are the two A* algorithms:

The first is to think that the A* algorithm is the above idea. $h^{'}(x)$ That is to say $h^{*}(x)$ , $h(x)$ the A algorithm that satisfies the following formula is the A* algorithm.

$h(x) \leq h^{*}(x)$

The second $h(x)$ is that there are often many evaluation functions in the algorithm . For example, we can use Manhattan distance, diagonal distance, or Euclidean distance, so there must be many evaluation functions $h_{1}(x)$ , $h_{2}(x)$ , $h_{3}(x)$ etc. If we further restrict the A algorithm, that is, if $g^{*}(x)>0$ and $h_{i}(x)\leq h^{*}(x)$ (that is, $h^{*}(x)$ greater than or equal to any evaluation function), then the algorithm is the A* algorithm. It can be seen that A* is the optimal A algorithm under this definition. However, in practical applications, it is often difficult for us to judge or find the optimal valuation function, so the difference between the A* algorithm and the A algorithm is not very important, and the A* algorithm is often used to express this idea.

4.3 Proof of A* Algorithm

For the idea of the above A* algorithm, I give the following simple counter-evidence idea, which may be flawed, but I hope it can help understand:

Assuming that the path found by the A* algorithm is not the shortest path, then at the end of the A* algorithm, it means that a longer path from the start point to the end point has been found . To prove the contradiction , we only need to prove that the A* algorithm will not execute smoothly on this longer path .

Let the start point be $s$ and the end point be $t$ . Let the shortest path be $T_{1}$ , and the path found by the A* algorithm be $T_{2}$ . $T_{1}$ The nodes on this path that $T_{2}$ are different from the first node on the path $a$ are, followed by $b$ , $c$ , $d$ , $e$ ... $t$ (some of these nodes may $T_{2}$ be the same as those on the path, but it doesn't matter, it is already a different path at this time). On the path $T_{2}$ , $t$ the previous node of the node is $m$ . At the same time let $h^{'}(x)$ represent the actual distance from the node $x$ to the end point $t$ . As follows:

Assume that when the A* algorithm runs to the end $m$ , it will be expanded if there is no accident , that is, the node $t$ at this time is the smallest among all the nodes to be expanded, so it will be selected . And what we want to prove is precisely this "accident", so that the A* algorithm will not choose later , and it will not choose a path longer than the shortest path at the end of the algorithm. $t$ $f(t)$ $t$ $m$ $t$

We know $h^{'}(t)=0$ ( the actual distance $t$ to $t$ itself is 0), $h(t)$ but the estimated distance from t to t must be less than $h^{'}(t)$ , that is $h(t) \leq h^{'}(t)=0$ , so $h(t)$ it is also 0 at this time. therefore:

$f(t)=g(t)+h(t)=g(t)$

It $g(t)$ represents the actual distance $s$ reached so far $t$ , that is, $T_{2}$ the length of the path. The known $T_{2}$ path length is greater than $T_{1}$ the length of the path, and $T_{1}$ the length of the path can be expressed as $g(a)+h^{'}(a)$ , so:

$g(t) \geq g(a)+h^{'}(a)$

$h(a)$ Instead, the estimated distance to $a$ arrive $t$ must be less than or equal to the actual distance $a$ to arrive , so: $t$ $h^{'}(a)$

$g(a)+h^{'}(a) \geq g(a)+h(a)$

so:

$g(t) \geq g(a)+h(a)$

Right now:

$g(t)+h(t) \geq g(a)+h(a)$

That is:

$f(t) \geq f(a)$

So we know that the node to be expanded at this time $t$ is not the minimum value, and we have smaller nodes $a$ to expand.

After the expansion $a$ , because $g(b)+h^{'}(b) \leq g(t)$ it can be launched in the same way $f(t) \geq f(b)$ , the next expansion is $b$ the node. We can analogize $T_{1}$ all the remaining nodes on the shortest path path $c$ , $d$ , $e$ ..., may wish to set $i$ , they all satisfy: $g(i)+h^{'}(i) \leq g(t)$ , and can be deduced in the same way $f(t) \geq f(i)$ .

That is, $t$ the node $f(t)$ will never be the smallest among the nodes to be expanded, and the node will not be $T_{1}$ expanded until the remaining nodes on the shortest path are expanded . $t$ When $T_{1}$ the last non- $t$ node of the shortest path is expanded, the t node is naturally expanded, and the algorithm ends at this time. We can know that at the end the path we found was $s$ exactly the opposite of what we had assumed. $t$ $T_{1}$ $T_{2}$

Therefore, if it $h(x)$ is always less than or equal to $x$ the cost from the node to the destination, the A* algorithm guarantees that it will be able to find the shortest path. When $h(x)$ the value is smaller, the algorithm will traverse more nodes, which will lead to slower algorithm. If $h(x)$ it is so large that it is exactly equal to $x$ the real cost of the node to the destination, the A* algorithm will find the best path, and the speed is very fast. Unfortunately, this is not possible in all scenarios. Because before reaching the end point, it is difficult for us to calculate exactly how far we are from the end point.

For the evaluation function $f(x)=g(x)+h(x)$ , we can find the following interesting things:

① $h(x)=0$ At that time , $f(x)=g(x)$ , indicating that it is completely based on the shortest distance among the reached nodes at this time, which is the Dijkstra algorithm.

② $g(x)=0$ At that time , it was the Greedy Best First Search algorithm (Greedy Best First Search)

4.4 Algorithm process

The following is the pseudo-code of the algorithm. Compared with the aforementioned Dijkstra algorithm process, only heuristic information is added :

The above process is similar to the Dijkstra process and will not be described here.

For the information currently searched on the Internet, the process is basically similar to the above, but the specific details and names are different. Generally speaking, open_set is the frontier of the above code . close_set is similar to the node put into cost_so_far , but the difference is that the above pseudo-code can handle graphs with negative edges and no negative loops, while general codes cannot. The following is another version of the algorithm process:

1.初始化open_set和close_set；
2.将起点加入open_set中，并设置优先级为0（优先级越小表示优先级越高）；
3.如果open_set不为空，则从open_set中选取优先级最高的节点x：
    ①如果节点x为终点，则：
        从终点开始逐步追踪parent节点，一直到达起点，返回找到的结果路径，算法结束；
    ②如果节点x不是终点，则：
        1.将节点x从open_set中删除，并加入close_set中；
        2.遍历节点x所有的邻近节点：
            ①如果邻近节点y在close_set中，则：
                跳过，选取下一个邻近节点
            ②如果邻近节点y不在open_set中，则：
                设置节点m的parent为节点x，计算节点m的优先级，将节点m加入open_set中

When implementing the code, I mainly implemented it based on the first pseudo-code.

3. Realization

1. Experimental requirements

The maze problem is a classical problem in experimental psychology. There may be several paths from the entrance to the exit of the maze, and this experiment requires finding the shortest path from the entrance to the exit.

The figure below is a schematic diagram of a 4×4 maze problem. Each position is represented by a point in the plane coordinate system. As shown in the figure, the coordinates of the entry point and the $(1,1)$ exit point are $(4,4)$ . A line connecting two points means that two locations are connected. If there is no line connected, it means no communication.

2. Code implementation

In order to solve the above-mentioned maze problem, my idea is to number each node of the above-mentioned rectangular maze, from $(1,1)$ left to right from the beginning to 0, 1, 2, 3... Another advantage of this numbering is that it can be very convenient until The node is located in which row and which column.

The adjacency list of each node records adjacent nodes, because it is an undirected edge, so an edge will be recorded twice.

The specific algorithm process is written according to the above pseudo code.

The following is the implementation code:

import numpy as np
from queue import PriorityQueue


class Map:  # 地图
    def __init__(self, width, height) -> None:
        # 迷宫的尺寸
        self.width = width
        self.height = height
        # 创建size x size 的点的邻接表
        self.neighbor = [[] for i in range(width*height)]

    # 添加边
    def addEdge(self, from_: int, to_: int):
        if (from_ not in range(self.width*self.height)) or (to_ not in range(self.width*self.height)):
            return 0
        self.neighbor[from_].append(to_)
        self.neighbor[to_].append(from_)
        return 1

    # 由序号获得该点在迷宫的x、y坐标
    def get_x_y(self, num: int):
        if num not in range(self.width*self.height):
            return -1, -1
        x = num % self.width
        y = num // self.width
        return x, y


class Astar:  # A*寻路算法
    def __init__(self, _map: Map, start: int, end: int) -> None:
        # 地图
        self.run_map = _map
        # 起点和终点
        self.start = start
        self.end = end
        # open集
        self.open_set = PriorityQueue()
        # cost_so_far表示到达某个节点的代价，也可相当于close集使用
        self.cost_so_far = dict()
        # 每个节点的前序节点
        self.came_from = dict()

        # 将起点放入,优先级设为0，无所谓设置多少，因为总是第一个被取出
        self.open_set.put((0, start))
        self.came_from[start] = -1
        self.cost_so_far[start] = 0

    # h函数计算,即启发式信息
    def heuristic(self, a, b):
        x1, y1 = self.run_map.get_x_y(a)
        x2, y2 = self.run_map.get_x_y(b)
        return abs(x1-x2) + abs(y1-y2)

    # 运行A*寻路算法，如果没找到路径返回0，找到返回1
    def find_way(self):
        # open表不为空
        while not self.open_set.empty():
            # 从优先队列中取出代价最短的节点作为当前遍历的节点，类型为(priority,node)
            current = self.open_set.get()
            # 找到终点
            if current[1] == self.end:
                break
            # 遍历邻接节点
            for next in self.run_map.neighbor[current[1]]:
                # 新的代价
                new_cost = self.cost_so_far[current[1]]+1
                # 没有到达过的点 或 比原本已经到达过的点的代价更小
                if (next not in self.cost_so_far) or (new_cost < self.cost_so_far[next]):
                    self.cost_so_far[next] = new_cost
                    priority = new_cost+self.heuristic(next, self.end)
                    self.open_set.put((priority, next))
                    self.came_from[next] = current[1]

        if self.end not in self.cost_so_far:
            return 0
        return 1

    def show_way(self):
        # 记录路径经过的节点
        result = []
        current = self.end
        # 不断寻找前序节点
        while self.came_from[current] != -1:
            result.append(current)
            current = self.came_from[current]
        # 加上起点
        result.append(current)
        # 翻转路径
        result.reverse()
        print(result)


# 初始化迷宫
theMap = Map(4, 4)
# 添加边
theMap.addEdge(0, 1)
theMap.addEdge(1, 2)
theMap.addEdge(2, 6)
theMap.addEdge(3, 7)
theMap.addEdge(4, 5)
theMap.addEdge(5, 6)
theMap.addEdge(6, 7)
theMap.addEdge(4, 8)
theMap.addEdge(5, 9)
theMap.addEdge(7, 11)
theMap.addEdge(8, 9)
theMap.addEdge(9, 10)
theMap.addEdge(10, 11)
theMap.addEdge(8, 12)
theMap.addEdge(10, 14)
theMap.addEdge(12, 13)
theMap.addEdge(13, 14)
theMap.addEdge(14, 15)
# A* 算法寻路
theAstar = Astar(theMap, 0, 15)
theAstar.find_way()
theAstar.show_way()

After running, the following results are obtained:

[0, 1, 2, 6, 7, 11, 10, 14, 15]

That is, the path on the graph is:

The above is the main body of the code. In order to better visualize the results, I use python's matploblib library for visualization.

The matploblib library is generally used to visualize data charts. My idea is to use its circle-drawing function Circle to draw nodes, draw rectangle function Rectangle to draw edges, and then use the ion() function of plt (matplotlib.pyplot) to open interactions and draw dynamic graphs , representing each stage in the lookup. The details are as follows:

import numpy as np
from queue import PriorityQueue
import matplotlib.pyplot as plt
import matplotlib.patches as mpathes
import random

# 画布
fig, ax = plt.subplots()


class Map:  # 地图
    def __init__(self, width, height) -> None:
        # 迷宫的尺寸
        self.width = width
        self.height = height
        # 创建size x size 的点的邻接表
        self.neighbor = [[] for i in range(width*height)]

    def addEdge(self, from_: int, to_: int):    # 添加边
        if (from_ not in range(self.width*self.height)) or (to_ not in range(self.width*self.height)):
            return 0
        self.neighbor[from_].append(to_)
        self.neighbor[to_].append(from_)
        return 1

    def get_x_y(self, num: int):    # 由序号获得该点在迷宫的x、y坐标
        if num not in range(self.width*self.height):
            return -1, -1
        x = num % self.width
        y = num // self.width
        return x, y

    def drawCircle(self, num, color):    # 绘制圆形
        x, y = self.get_x_y(num)
        thePoint = mpathes.Circle(np.array([x+1, y+1]), 0.1, color=color)
        # 声明全局变量
        global ax
        ax.add_patch(thePoint)

    def drawEdge(self, from_, to_, color):    # 绘制边
        # 转化为(x,y)
        x1, y1 = self.get_x_y(from_)
        x2, y2 = self.get_x_y(to_)
        # 整体向右下方移动一个单位
        x1, y1 = x1+1, y1+1
        x2, y2 = x2+1, y2+1
        # 绘长方形代表边
        offset = 0.05
        global ax
        if from_-to_ == 1:  # ← 方向的边
            rect = mpathes.Rectangle(
                np.array([x2-offset, y2-offset]), 1+2*offset, 2*offset, color=color)
            ax.add_patch(rect)
        elif from_-to_ == -1:  # → 方向的边
            rect = mpathes.Rectangle(
                np.array([x1-offset, y1-offset]), 1+2*offset, 2*offset, color=color)
            ax.add_patch(rect)
        elif from_-to_ == self.width:  # ↑ 方向的边
            rect = mpathes.Rectangle(
                np.array([x2-offset, y2-offset]), 2*offset, 1+2*offset, color=color)
            ax.add_patch(rect)
        else:  # ↓ 方向的边
            rect = mpathes.Rectangle(
                np.array([x1-offset, y1-offset]), 2*offset, 1+2*offset, color=color)
            ax.add_patch(rect)

    def initMap(self):    # 绘制初始的迷宫
        # 先绘制边
        for i in range(self.width*self.height):
            for next in self.neighbor[i]:
                self.drawEdge(i, next, '#afeeee')

        # 再绘制点
        for i in range(self.width*self.height):
            self.drawCircle(i, '#87cefa')


class Astar:  # A*寻路算法
    def __init__(self, _map: Map, start: int, end: int) -> None:
        # 地图
        self.run_map = _map
        # 起点和终点
        self.start = start
        self.end = end
        # open集
        self.open_set = PriorityQueue()
        # cost_so_far表示到达某个节点的代价，也可相当于close集使用
        self.cost_so_far = dict()
        # 每个节点的前序节点
        self.came_from = dict()

        # 将起点放入,优先级设为0，无所谓设置多少，因为总是第一个被取出
        self.open_set.put((0, start))
        self.came_from[start] = -1
        self.cost_so_far[start] = 0

        # 标识起点和终点
        self.run_map.drawCircle(start, '#ff8099')
        self.run_map.drawCircle(end, '#ff4d40')

    def heuristic(self, a, b):    # h函数计算,即启发式信息
        x1, y1 = self.run_map.get_x_y(a)
        x2, y2 = self.run_map.get_x_y(b)
        return abs(x1-x2) + abs(y1-y2)

    def find_way(self):    # 运行A*寻路算法，如果没找到路径返回0，找到返回1
        while not self.open_set.empty():  # open表不为空
            # 从优先队列中取出代价最短的节点作为当前遍历的节点，类型为(priority,node)
            current = self.open_set.get()

            # 展示A*算法的执行过程
            if current[1] != self.start:
                # 当前节点的前序
                pre = self.came_from[current[1]]
                # 可视化
                self.run_map.drawEdge(pre, current[1], '#fffdd0')
                if pre != self.start:
                    self.run_map.drawCircle(pre, '#99ff4d')
                else:  # 起点不改色
                    self.run_map.drawCircle(pre, '#ff8099')
                if current[1] != self.end:
                    self.run_map.drawCircle(current[1], '#99ff4d')
                else:
                    self.run_map.drawCircle(current[1], '#ff4d40')
                # 显示当前状态
                plt.show()
                plt.pause(0.5)

            # 找到终点
            if current[1] == self.end:
                break
            # 遍历邻接节点
            for next in self.run_map.neighbor[current[1]]:
                # 新的代价
                new_cost = self.cost_so_far[current[1]]+1
                # 没有到达过的点 或 比原本已经到达过的点的代价更小
                if (next not in self.cost_so_far) or (new_cost < self.cost_so_far[next]):
                    self.cost_so_far[next] = new_cost
                    priority = new_cost+self.heuristic(next, self.end)
                    self.open_set.put((priority, next))
                    self.came_from[next] = current[1]

    def show_way(self):  # 显示最短路径
        # 记录路径经过的节点
        result = []
        current = self.end

        if current not in self.cost_so_far:
            return

        # 不断寻找前序节点
        while self.came_from[current] != -1:
            result.append(current)
            current = self.came_from[current]
        # 加上起点
        result.append(current)
        # 翻转路径
        result.reverse()
        # 生成路径
        for point in result:
            if point != self.start:  # 不是起点
                # 当前节点的前序
                pre = self.came_from[point]
                # 可视化
                self.run_map.drawEdge(pre, point, '#ff2f76')
                if pre == self.start:  # 起点颜色
                    self.run_map.drawCircle(pre, '#ff8099')
                elif point == self.end:  # 终点颜色
                    self.run_map.drawCircle(point, '#ff4d40')
                # 显示当前状态
                plt.show()
                plt.pause(0.1)

    def get_cost(self):  # 返回最短路径
        if self.end not in self.cost_so_far:
            return -1
        return self.cost_so_far[self.end]


# 初始化迷宫
theMap = Map(4, 4)

# 设置迷宫显示的一些参数
plt.xlim(0, theMap.width+1)
plt.ylim(0, theMap.height+1)
# 将x轴的位置设置在顶部
ax.xaxis.set_ticks_position('top')
# y轴反向
ax.invert_yaxis()
# 等距
plt.axis('equal')
# 不显示背景的网格线
plt.grid(False)
# 允许动态
plt.ion()
# 添加边
theMap.addEdge(0, 1)
theMap.addEdge(1, 2)
theMap.addEdge(2, 6)
theMap.addEdge(3, 7)
theMap.addEdge(4, 5)
theMap.addEdge(5, 6)
theMap.addEdge(6, 7)
theMap.addEdge(4, 8)
theMap.addEdge(5, 9)
theMap.addEdge(7, 11)
theMap.addEdge(8, 9)
theMap.addEdge(9, 10)
theMap.addEdge(10, 11)
theMap.addEdge(8, 12)
theMap.addEdge(10, 14)
theMap.addEdge(12, 13)
theMap.addEdge(13, 14)
theMap.addEdge(14, 15)

# 初始化迷宫
theMap.initMap()

# A* 算法寻路
theAstar = Astar(theMap, 0, 15)
theAstar.find_way()
theAstar.show_way()

# 输出最短路径长度
theCost = theAstar.get_cost()
if theCost == -1:
    print("不存在该路径！")
else:
    print("从起点到终点的最短路径长度为: ", theCost)

# 关闭交互，展示结果
plt.ioff()
plt.show()

The running effect is as follows:

The output is as follows:

从起点到终点的最短路径长度为:  8

Test for a slightly larger graph (6x6):

# 初始化迷宫
theMap = Map(6, 6)

# 设置迷宫显示的一些参数
plt.xlim(0, theMap.width+1)
plt.ylim(0, theMap.height+1)
# 将x轴的位置设置在顶部
ax.xaxis.set_ticks_position('top')
# y轴反向
ax.invert_yaxis()
# 等距
plt.axis('equal')
# 不显示背景的网格线
plt.grid(False)
# 允许动态
plt.ion()

# 添加边
theMap.addEdge(0, 1)
theMap.addEdge(1, 2)
theMap.addEdge(2, 3)
theMap.addEdge(3, 4)
theMap.addEdge(4, 5)
theMap.addEdge(1, 7)
theMap.addEdge(3, 9)
theMap.addEdge(4, 10)
theMap.addEdge(5, 11)
theMap.addEdge(6, 7)
theMap.addEdge(8, 9)
theMap.addEdge(6, 12)
theMap.addEdge(7, 13)
theMap.addEdge(8, 14)
theMap.addEdge(10, 16)
theMap.addEdge(11, 17)
theMap.addEdge(12, 13)
theMap.addEdge(13, 14)
theMap.addEdge(15, 16)
theMap.addEdge(16, 17)
theMap.addEdge(14, 20)
theMap.addEdge(15, 21)
theMap.addEdge(16, 22)
theMap.addEdge(17, 23)
theMap.addEdge(18, 19)
theMap.addEdge(19, 20)
theMap.addEdge(20, 21)
theMap.addEdge(22, 23)
theMap.addEdge(18, 24)
theMap.addEdge(19, 25)
theMap.addEdge(20, 26)
theMap.addEdge(22, 28)
theMap.addEdge(26, 27)
theMap.addEdge(27, 28)
theMap.addEdge(24, 30)
theMap.addEdge(27, 33)
theMap.addEdge(29, 35)
theMap.addEdge(30, 31)
theMap.addEdge(31, 32)
theMap.addEdge(33, 34)
theMap.addEdge(34, 35)

# 初始化迷宫
theMap.initMap()

# A* 算法寻路
theAstar = Astar(theMap, 0, 35)
theAstar.find_way()
theAstar.show_way()

# 输出最短路径长度
theCost = theAstar.get_cost()
if theCost == -1:
    print("不存在该路径！")
else:
    print("从起点到终点的最短路径长度为: ", theCost)

# 关闭交互，展示结果
plt.ioff()
plt.show()

operation result:

Output result:

从起点到终点的最短路径长度为:  10

It can be seen that the operation result is correct.

But we found that every time we input a new graph, we have to input a lot of edges, which is very inconvenient to debug for more complex graphs. Is there a way to let the program generate a maze randomly after we set the size of the maze?

To do this, we can write a function that randomly generates a maze.

The random generation method I used is a simple deep search method. The maze in the initial state has no edges, only an array of nodes of the specified size. Starting from the starting point, explore four directions in turn (the order of exploration in the four directions is random), if the adjacent point in this direction has not been explored, then generate an edge and advance to this point at the same time. For this point, continue to repeat the above process until all points are explored, and the algorithm terminates.

    # 寻找
    def search(self, current: int):
        # 四个方向的顺序
        sequence = [i for i in range(4)]
        # 打乱顺序
        random.shuffle(sequence)
        # 依次选择四个方向
        for i in sequence:
            # 要探索的位置
            x = self.direction[i]+current

            # 跨了一行
            if (current % self.width == self.width-1 and self.direction[i] == 1) or (current % self.width == 0 and self.direction[i] == -1):
                continue

            # 要探索的位置没有超出范围 且 该位置没有被探索过
            if 0 <= x < self.width*self.height and self.visited[x] == 0:
                self.addEdge(current, x)
                self.visited[x] = 1
                self.search(x)


    def randomCreateMap(self, start, k):  # 随机生成迷宫
        # 标识每个节点是否被探索过
        self.visited = np.zeros(self.width*self.height)
        self.visited[start] = 1
        # 四个方向,分别代表上、下、左、右
        self.direction = {0: -self.width,
                          1: self.width,
                          2: -1,
                          3: 1}
        # 从起点开始
        self.search(start)

The following are randomly generated 10x10, 20x20, 30x25 mazes:

It can be seen that the generated maze works well and can meet the basic needs. But because the algorithm for generating the maze uses a deep search, there is only one path from the start point to the end point. This seems to be inexplicable for us to find the shortest path, because once the end point is found, it must be the shortest path. Therefore, we add complexity to the maze, that is, randomly add k edges in the maze, so that there are multiple paths in the graph.


    # 随机添加k条边
    def randomAddEdges(self, k):
        # 循环k次(可能不止k次)
        for i in range(k):
            node = random.randint(0, self.width*self.height)
            # 随机添加一个方向
            sequence = [i for i in range(4)]
            random.shuffle(sequence)
            isPick = 0
            for d in sequence:
                # 跨了一行,不存在该方向的边
                if (node % self.width == self.width-1 and self.direction[d] == 1) or (node % self.width == 0 and self.direction[d] == -1):
                    continue
                x = self.direction[d]+node
                # 该边存在
                if x in self.neighbor[node]:
                    continue
                # 该边不存在
                self.addEdge(node, x)
                isPick = 1
            # 重新添加一条边,即重新循环一次
            if isPick == 0:
                if i == 0:  # 第一次
                    i = 0
                else:
                    i -= 1

The generated maze is as follows:

It can be seen that there are many redundant paths, so that there is more than one path from the start point to the end point.

Apply the A* algorithm to a randomly generated maze:

The output is as follows:

从起点到终点的最短路径长度为:  18

The output is as follows:

从起点到终点的最短路径长度为:  28

The output is as follows:

从起点到终点的最短路径长度为:  50

4. Source code

import numpy as np
from queue import PriorityQueue
import matplotlib.pyplot as plt
import matplotlib.patches as mpathes
import random

# 画布
fig, ax = plt.subplots()


class Map:  # 地图
    def __init__(self, width, height) -> None:
        # 迷宫的尺寸
        self.width = width
        self.height = height
        # 创建size x size 的点的邻接表
        self.neighbor = [[] for i in range(width*height)]

    def addEdge(self, from_: int, to_: int):    # 添加边
        if (from_ not in range(self.width*self.height)) or (to_ not in range(self.width*self.height)):
            return 0
        self.neighbor[from_].append(to_)
        self.neighbor[to_].append(from_)
        return 1

    def get_x_y(self, num: int):    # 由序号获得该点在迷宫的x、y坐标
        if num not in range(self.width*self.height):
            return -1, -1
        x = num % self.width
        y = num // self.width
        return x, y

    def drawCircle(self, num, color):    # 绘制圆形
        x, y = self.get_x_y(num)
        thePoint = mpathes.Circle(np.array([x+1, y+1]), 0.1, color=color)
        # 声明全局变量
        global ax
        ax.add_patch(thePoint)

    def drawEdge(self, from_, to_, color):    # 绘制边
        # 转化为(x,y)
        x1, y1 = self.get_x_y(from_)
        x2, y2 = self.get_x_y(to_)
        # 整体向右下方移动一个单位
        x1, y1 = x1+1, y1+1
        x2, y2 = x2+1, y2+1
        # 绘长方形代表边
        offset = 0.05
        global ax
        if from_-to_ == 1:  # ← 方向的边
            rect = mpathes.Rectangle(
                np.array([x2-offset, y2-offset]), 1+2*offset, 2*offset, color=color)
            ax.add_patch(rect)
        elif from_-to_ == -1:  # → 方向的边
            rect = mpathes.Rectangle(
                np.array([x1-offset, y1-offset]), 1+2*offset, 2*offset, color=color)
            ax.add_patch(rect)
        elif from_-to_ == self.width:  # ↑ 方向的边
            rect = mpathes.Rectangle(
                np.array([x2-offset, y2-offset]), 2*offset, 1+2*offset, color=color)
            ax.add_patch(rect)
        else:  # ↓ 方向的边
            rect = mpathes.Rectangle(
                np.array([x1-offset, y1-offset]), 2*offset, 1+2*offset, color=color)
            ax.add_patch(rect)

    def initMap(self):    # 绘制初始的迷宫
        # 先绘制边
        for i in range(self.width*self.height):
            for next in self.neighbor[i]:
                self.drawEdge(i, next, '#afeeee')

        # 再绘制点
        for i in range(self.width*self.height):
            self.drawCircle(i, '#87cefa')

    # 寻找
    def search(self, current: int):
        # 四个方向的顺序
        sequence = [i for i in range(4)]
        # 打乱顺序
        random.shuffle(sequence)
        # 依次选择四个方向
        for i in sequence:
            # 要探索的位置
            x = self.direction[i]+current

            # 跨了一行
            if (current % self.width == self.width-1 and self.direction[i] == 1) or (current % self.width == 0 and self.direction[i] == -1):
                continue

            # 要探索的位置没有超出范围 且 该位置没有被探索过
            if 0 <= x < self.width*self.height and self.visited[x] == 0:
                self.addEdge(current, x)
                self.visited[x] = 1
                self.search(x)

    # 随机添加k条边
    def randomAddEdges(self, k):
        # 循环k次(可能不止k次)
        for i in range(k):
            node = random.randint(0, self.width*self.height)
            # 随机添加一个方向
            sequence = [i for i in range(4)]
            random.shuffle(sequence)
            isPick = 0
            for d in sequence:
                # 跨了一行,不存在该方向的边
                if (node % self.width == self.width-1 and self.direction[d] == 1) or (node % self.width == 0 and self.direction[d] == -1):
                    continue
                x = self.direction[d]+node
                # 该边存在
                if x in self.neighbor[node]:
                    continue
                # 该边不存在
                self.addEdge(node, x)
                isPick = 1
            # 重新添加一条边,即重新循环一次
            if isPick == 0:
                if i == 0:  # 第一次
                    i = 0
                else:
                    i -= 1

    def randomCreateMap(self, start, k):  # 随机生成迷宫
        # 标识每个节点是否被探索过
        self.visited = np.zeros(self.width*self.height)
        self.visited[start] = 1
        # 四个方向,分别代表上、下、左、右
        self.direction = {0: -self.width,
                          1: self.width,
                          2: -1,
                          3: 1}
        # 从起点开始
        self.search(start)
        # 随机添加k条边，使得迷宫尽可能出现多条到达终点的路径
        self.randomAddEdges(k)


class Astar:  # A*寻路算法
    def __init__(self, _map: Map, start: int, end: int) -> None:
        # 地图
        self.run_map = _map
        # 起点和终点
        self.start = start
        self.end = end
        # open集
        self.open_set = PriorityQueue()
        # cost_so_far表示到达某个节点的代价，也可相当于close集使用
        self.cost_so_far = dict()
        # 每个节点的前序节点
        self.came_from = dict()

        # 将起点放入,优先级设为0，无所谓设置多少，因为总是第一个被取出
        self.open_set.put((0, start))
        self.came_from[start] = -1
        self.cost_so_far[start] = 0

        # 标识起点和终点
        self.run_map.drawCircle(start, '#ff8099')
        self.run_map.drawCircle(end, '#ff4d40')

    def heuristic(self, a, b):    # h函数计算,即启发式信息
        x1, y1 = self.run_map.get_x_y(a)
        x2, y2 = self.run_map.get_x_y(b)
        return abs(x1-x2) + abs(y1-y2)

    def find_way(self):    # 运行A*寻路算法，如果没找到路径返回0，找到返回1
        while not self.open_set.empty():  # open表不为空
            # 从优先队列中取出代价最短的节点作为当前遍历的节点，类型为(priority,node)
            current = self.open_set.get()

            # 展示A*算法的执行过程
            if current[1] != self.start:
                # 当前节点的前序
                pre = self.came_from[current[1]]
                # 可视化
                self.run_map.drawEdge(pre, current[1], '#fffdd0')
                if pre != self.start:
                    self.run_map.drawCircle(pre, '#99ff4d')
                else:  # 起点不改色
                    self.run_map.drawCircle(pre, '#ff8099')
                if current[1] != self.end:
                    self.run_map.drawCircle(current[1], '#99ff4d')
                else:
                    self.run_map.drawCircle(current[1], '#ff4d40')
                # 显示当前状态
                plt.show()
                plt.pause(0.01)

            # 找到终点
            if current[1] == self.end:
                break
            # 遍历邻接节点
            for next in self.run_map.neighbor[current[1]]:
                # 新的代价
                new_cost = self.cost_so_far[current[1]]+1
                # 没有到达过的点 或 比原本已经到达过的点的代价更小
                if (next not in self.cost_so_far) or (new_cost < self.cost_so_far[next]):
                    self.cost_so_far[next] = new_cost
                    priority = new_cost+self.heuristic(next, self.end)
                    self.open_set.put((priority, next))
                    self.came_from[next] = current[1]

    def show_way(self):  # 显示最短路径
        # 记录路径经过的节点
        result = []
        current = self.end

        if current not in self.cost_so_far:
            return

        # 不断寻找前序节点
        while self.came_from[current] != -1:
            result.append(current)
            current = self.came_from[current]
        # 加上起点
        result.append(current)
        # 翻转路径
        result.reverse()
        # 生成路径
        for point in result:
            if point != self.start:  # 不是起点
                # 当前节点的前序
                pre = self.came_from[point]
                # 可视化
                self.run_map.drawEdge(pre, point, '#ff2f76')
                if pre == self.start:  # 起点颜色
                    self.run_map.drawCircle(pre, '#ff8099')
                elif point == self.end:  # 终点颜色
                    self.run_map.drawCircle(point, '#ff4d40')
                # 显示当前状态
                plt.show()
                plt.pause(0.005)

    def get_cost(self):  # 返回最短路径
        if self.end not in self.cost_so_far:
            return -1
        return self.cost_so_far[self.end]


# 初始化迷宫，设置宽度和高度
theMap = Map(20, 20)

# 设置迷宫显示的一些参数
plt.xlim(0, theMap.width+1)
plt.ylim(0, theMap.height+1)
# 将x轴的位置设置在顶部
ax.xaxis.set_ticks_position('top')
# y轴反向
ax.invert_yaxis()
# 等距
plt.axis('equal')
# 不显示背景的网格线
plt.grid(False)
# 允许动态
plt.ion()

# 随机添加边，生成迷宫，第一个参数为起点；第二个参数为额外随机生成的边，可以表示为图的复杂程度
theMap.randomCreateMap(0, 20)

# 初始化迷宫
theMap.initMap()

# A* 算法寻路
theAstar = Astar(theMap, 0, 399)  # 设置起点和终点
theAstar.find_way()  # 寻路
theAstar.show_way()  # 显示最短路径

# 输出最短路径长度
theCost = theAstar.get_cost()
if theCost == -1:
    print("不存在该路径！")
else:
    print("从起点到终点的最短路径长度为: ", theCost)

# 关闭交互，展示结果
plt.ioff()
plt.show()

A* algorithm to solve the maze problem (algorithm explanation and proof, python implementation and visualization)

1. Introduction

2. Specific details

1、BFS（Breadth First Search）

2、Dijkstra（Uniform Cost Search）

3. Heuristic search

4. A* algorithm

4.1 Algorithm Details

4.2 A and A* algorithms

4.3 Proof of A* Algorithm

4.4 Algorithm process

3. Realization

1. Experimental requirements

2. Code implementation

4. Source code

Guess you like